# Design of sTetro: A Modular, Reconfigurable, and Autonomous Staircase Cleaning Robot

Authors: Muhammad Ilyas; Shi Yuyao; Rajesh Elara Mohan; Manojkumar Devarassu; Manivannan Kalimuthu
Journal: Journal of Sensors
 (2018)

---

## Abstract

The mechanical, electrical, and autonomy aspects of designing a novel, modular, and reconfigurable cleaning robot, dubbed assTetro (stair Tetro), are presented. The developed robotic platform uses a vertical conveyor mechanism to reconfigure itself and is capable of navigating over flat surfaces as well as staircases, thus significantly extending the automated cleaning capabilities as compared to conventional home cleaning robots. The mechanical design and system architecture are introduced first, followed by a detailed description of system modelling and controller design efforts in sTetro. An autonomy algorithm is also proposed for self-reconfiguration, locomotion, and autonomous navigation of sTetro in the controlled environment, for example, in homes/offices with a flat floor and a straight staircase. A staircase recognition algorithm is presented to distinguish between the surrounding environment and the stairs. The misalignment detection technique of the robot with a front staircase riser is also given, and a feedback from the IMU sensor for misalignment corrective measures is provided. The experiments performed with the sTetro robot demonstrated the efficacy and validity of the developed system models, control, and autonomy approaches.


---

## Body

## 1. Introduction

Due to a faster pace of life in most of the developed world, floor cleaning is often seen as a dull, dirty, laborious, time-consuming, and tedious job (Figure1(a)) giving rise to the development of robotic products for handling the cleaning task autonomously. Such robotic platforms have given their vast potential by improving productivity in cleaning jobs in domestic and commercial settings and witnessed a steep rise over the last two decades [1]. It is estimated that between 2015 and 2018, about 25.2 million USD robotic cleaning units would be sold worldwide [2].Figure 1
(a) Manual staircase cleaning. (b) Conventional home cleaning robots.
(a)
(b)A number of successful products such as iRoomba, Neato XV-11, Samsung Powerbot, Bobsweep bobi, Miele scout, Moneual RYDIS, and Infinuvo CleanMate exist in the marketplace today [3]. These robots are generally characterized by nonmodular/fixed circular morphology capable of autonomously mapping its environment using on-board sensors and navigating around the defined floor space to clean the smooth surface environment efficiently. The limitation of these conventional robots is that they are applicable on smooth floor-like surfaces only and are unable to clean staircases which are an essential part of most of homes/offices as shown in Figure 1(b).Numerous research literature deal with different aspects of floor cleaning robots such as the mechanism design [3–5], autonomy [6, 7], human-robot interaction studies [8, 9], multirobot teams [10, 11], and benchmarking strategies [12, 13]. Even though there exists such literature demonstrating the benefits of floor cleaning robots, the conventional floor cleaning robots suffer from serious performance issues that curtain their full potential dexterousness. One major factor attributing to their performance loss is their inability to access staircases that form the integral part of almost every built infrastructure. Considering the crucial role staircases play and their permanent presence even after the advent of lifts and escalators virtually in every multistory building, the subject of cleaning staircases has received little attention from roboticists. One viable approach to overcome this bottleneck is to design next-generation cleaning robots that are able to reconfigure themselves between floor and staircase cleaning modes, thereby maximizing their dexterous task performance. A number of design mechanisms towards realizing staircase climbing robots have been proposed and validated [14–18]. However, these robots target mostly search, rescue, and security applications using design principles and mechanisms that are not optimal for cleaning tasks. A very limited research effort has been made towards the design of staircase cleaning robots. An autonomous cleaning robot is proposed in [19] capable of accessing both flat floors and staircases. This robot consists of a rectangular-shaped body frame with L-shaped legs on both sides of the frame and moves forward and climbs stairs by rotating the body so that the top and bottom sides of it may be reversed using L-shaped legs. Megalingam et al. in [20] present various design considerations for vertical climbing robots to be used in staircase cleaning. The work describes a case study with the design of one such robot that can function in a 3D space. However, these robotic platforms that target staircase cleaning are complex and suffer from serious design and performance issues.ThesTetro (a staircase cleaning robot) is a novel modular reconfigurable cleaning robot, which uses a vertical conveyor belt mechanism and is capable of navigating over flat surfaces (e.g., floor) as well as climbing staircases, thus significantly extending the cleaning capabilities of home cleaning robots. The working principle concept of sTetro is borrowed from the Tetris Tiling Theory [21]. The sTetro’s body consists of three cuboids connected with two sliders attached to each side of the central cuboid. The hollow space inside each parallelepiped block encloses the electronic units, vacuum/suction tools, and sweeping mechanism. The blocks have a modular design, which allows reusing many pieces in the system. An intelligent application allows the user to control the robot to start and stop, navigate to follow a defined path, and change the operational configuration, as desired. The reconfiguration mechanism allows the sTetro robot to efficiently clean both the floor and the stairs at one time without user intervention, which is almost impossible in presently available home cleaning robots in the market.In this paper, the mechanical design and system architecture ofsTetro are introduced, followed by a detailed description of system modelling and controller design efforts. An autonomous motion algorithm is also proposed and validated with real robot data. To keep a robot aligned with the front riser of the staircase, an IMU sensor is used for heading change tracking and a feedback signal is a feedback to control for misalignment correction, during left/right movement and climbing the stair steps. In fully autonomous mode operation, the robot must be able to find/recognize the staircase. Many algorithms exist which try to recognize stairs with ultrasonic sensors [22], RGBD camera [23–25], stereo camera [26], and/or lidar [27]. These algorithms are complex and use already existing techniques for recognition. Camera-based recognition techniques suffer from lighting conditions as well. In this work, we have made use of a 2D lidar fixed in a vertical configuration to distinguish between the staircase and the surrounding environment. A staircase detection algorithm is developed using 2D lidar scans, which are matched with precaptured reference scan data.Experiments performed with thesTetro robot demonstrated the viability of the original design concepts, system model autonomy, and control approaches.


## 2. sTetro: System Architecture

### 2.1. Mechanical Design

In Figure2(a), the proposed prototype of the staircase cleaning robot is shown and a typical operation environment of sTetro is shown in Figure 2(b). A desktop 3D printer was used to print the entire robot’s mechanical parts with polylactic acid (PLA) material. With many prototyping methods available, the Fused Deposition Melting (FDM) method was used in the development of the sTetro robot which has an elastic modulus of 3.5 GPa and a density value of 1.3 g/cm3.Figure 2
(a)sTetro images. (b) Typical working environment of sTetro (stairs with treads and risers).
(a)
(b)Figure3 presents the bottom view and an exploded view of the sTetro robot. The sTetro platform has three segments/blocks with two metal sliders attached to each side of the middle block. The components in each block are distributed accordingly to maintain the block’s weight which ensures the centre of gravity (CoG) of the robot during its locomotion and self-reconfiguration.Figure 3
(a) Bottom view. (b) Exploded view of sTetro.
(a)
(b)


### 2.2. Electrical Design

ThesTetro robot consists of six DC motors; among them, four (M1–M4) are Worm Gear DC motors, and two (M5, M6) are Pololu DC motor. It also has four high torque servo motors (Herkulex 0201: SM1 to SM4). The motors M1 to M4 operate at 11.1 V and draw an average current of 800 mA with power rating 1600 mAh to deliver a nominal torque of 3 kg cm. The Pololu DC motor (M5, M6) has more torque of about 14.5 kg cm and has a gear ratio of 172 : 1. Four Omni-directional wheels are used in sTetro, with a diameter of 48 mm, to navigate the robot in planar directions (X, Y plane). Two of these Omni wheels (forward wheel) are driven by Pololu motors (M5, M6), and the side Omni wheels are driven by Worm Gear motors (M2, M3). In the 1st and 3rd blocks, DC motors M1 and M4 are installed to control the motion of monowheels (H-wheels), and Herkulex servo motors, SM3 and SM4, are used to control the direction of the 1st and 3rd block mono wheels. At the 2nd block, two servo motors (Herkulex: SM1, SM2) are attached to lift all three blocks using a vertical conveyor belt mechanism (Figure 3(b)). Six long bump sensors are attached on the left/right sides of the robot to detect side boundaries of the stair, and two time of flight (ToF) sensors are attached on the bottom of block-1 and block-2 to detect touch down on tread. A 3rd ToF sensor is attached in front of block-3 to detect front riser touching during reconfiguration. The more detailed internal views of sTetro are given in Figure 3.The mechanical and electrical parts/components used to construct this robot are listed in Tables1 and 2, respectively, for clarity and easy understanding of sTetro model.Table 1
Mechanical components with specifications.
Component
Specs.
Remarks
Block-1 and block-3Block-2
L=147 mm, W=172 mm, H=390 mm, weight = 3 kgL=135 mm, W=172 mm, H=430 mm, weight = 2 kg
Total weight = 7 kg
4 × Omni-directional wheels
Dia = 48 mm
2 × conveyor belt mechanism
L=220 mm, W=36 mm
2 × contact switch
SPDT
6 × bumper (long)/RB-Nex-31Table 2
Electrical components with specifications.
Component/model
Specs.
Remarks
4 × DC motors/WGM34
No load RPM: 100; stall torque: 3.5 kg cm; rated voltage: 12 VDC; no load current: 0.08 A max
2× Pololu DC motors/172 : 1 gear ratio
25D × 56 L mm; no load RPM: 34 RPM; stall torque: 14.5 kg cm; rated voltage: 6 VDC; stall current: 2400 MA max
4 × servo motors (SM1 and SM2)/Herkulex Drs-0201
Operating angle: 320°; stall torque: 24 kg cm; rated voltage: 7.4 VDC
1 × microcontroller
Arduino MEGA 2560
1 × Adafruit DC motor shield
L293D motor shield
1 × Bluetooth module
HC-06, 2.4 GHz
4 × batteries (BT1~BT4):BT1, BT2 (block-2); BT3 (block-1), BT4 (block-3),
BT1: 11.1 V Li-po with 1600 mAh (to power 6 DC motors and Arduino)
BT2: 7.4 V Li-Po with 2100 mAh (to power Herkulex: SM1, SM2)
BT3: 7.4 V Li-Po with 2100 mAh (Ttzo power Herkulex: SM3)
BT4: 7.4 V Li-Po with 2100 mAh (to power Herkulex: SM4)
1 × IMU
MPU-6050 6 DoF motion sensor
3 × VL6180X, proximity sensor
Dia: 4.8 × 2.8 × 1.0 mm; range: 0 to 100 mm; 2.6 to 3.0 V; 1.7 mA
1 × solid-state 2D lidar/LS02A (proposed)
FOV: 86°; distance accuracy: 1.5%; distance range: 0.1–4 m; scan freq.: 10 Hz; angular resolution: 1°/0.5°


## 2.1. Mechanical Design

In Figure2(a), the proposed prototype of the staircase cleaning robot is shown and a typical operation environment of sTetro is shown in Figure 2(b). A desktop 3D printer was used to print the entire robot’s mechanical parts with polylactic acid (PLA) material. With many prototyping methods available, the Fused Deposition Melting (FDM) method was used in the development of the sTetro robot which has an elastic modulus of 3.5 GPa and a density value of 1.3 g/cm3.Figure 2
(a)sTetro images. (b) Typical working environment of sTetro (stairs with treads and risers).
(a)
(b)Figure3 presents the bottom view and an exploded view of the sTetro robot. The sTetro platform has three segments/blocks with two metal sliders attached to each side of the middle block. The components in each block are distributed accordingly to maintain the block’s weight which ensures the centre of gravity (CoG) of the robot during its locomotion and self-reconfiguration.Figure 3
(a) Bottom view. (b) Exploded view of sTetro.
(a)
(b)


## 2.2. Electrical Design

ThesTetro robot consists of six DC motors; among them, four (M1–M4) are Worm Gear DC motors, and two (M5, M6) are Pololu DC motor. It also has four high torque servo motors (Herkulex 0201: SM1 to SM4). The motors M1 to M4 operate at 11.1 V and draw an average current of 800 mA with power rating 1600 mAh to deliver a nominal torque of 3 kg cm. The Pololu DC motor (M5, M6) has more torque of about 14.5 kg cm and has a gear ratio of 172 : 1. Four Omni-directional wheels are used in sTetro, with a diameter of 48 mm, to navigate the robot in planar directions (X, Y plane). Two of these Omni wheels (forward wheel) are driven by Pololu motors (M5, M6), and the side Omni wheels are driven by Worm Gear motors (M2, M3). In the 1st and 3rd blocks, DC motors M1 and M4 are installed to control the motion of monowheels (H-wheels), and Herkulex servo motors, SM3 and SM4, are used to control the direction of the 1st and 3rd block mono wheels. At the 2nd block, two servo motors (Herkulex: SM1, SM2) are attached to lift all three blocks using a vertical conveyor belt mechanism (Figure 3(b)). Six long bump sensors are attached on the left/right sides of the robot to detect side boundaries of the stair, and two time of flight (ToF) sensors are attached on the bottom of block-1 and block-2 to detect touch down on tread. A 3rd ToF sensor is attached in front of block-3 to detect front riser touching during reconfiguration. The more detailed internal views of sTetro are given in Figure 3.The mechanical and electrical parts/components used to construct this robot are listed in Tables1 and 2, respectively, for clarity and easy understanding of sTetro model.Table 1
Mechanical components with specifications.
Component
Specs.
Remarks
Block-1 and block-3Block-2
L=147 mm, W=172 mm, H=390 mm, weight = 3 kgL=135 mm, W=172 mm, H=430 mm, weight = 2 kg
Total weight = 7 kg
4 × Omni-directional wheels
Dia = 48 mm
2 × conveyor belt mechanism
L=220 mm, W=36 mm
2 × contact switch
SPDT
6 × bumper (long)/RB-Nex-31Table 2
Electrical components with specifications.
Component/model
Specs.
Remarks
4 × DC motors/WGM34
No load RPM: 100; stall torque: 3.5 kg cm; rated voltage: 12 VDC; no load current: 0.08 A max
2× Pololu DC motors/172 : 1 gear ratio
25D × 56 L mm; no load RPM: 34 RPM; stall torque: 14.5 kg cm; rated voltage: 6 VDC; stall current: 2400 MA max
4 × servo motors (SM1 and SM2)/Herkulex Drs-0201
Operating angle: 320°; stall torque: 24 kg cm; rated voltage: 7.4 VDC
1 × microcontroller
Arduino MEGA 2560
1 × Adafruit DC motor shield
L293D motor shield
1 × Bluetooth module
HC-06, 2.4 GHz
4 × batteries (BT1~BT4):BT1, BT2 (block-2); BT3 (block-1), BT4 (block-3),
BT1: 11.1 V Li-po with 1600 mAh (to power 6 DC motors and Arduino)
BT2: 7.4 V Li-Po with 2100 mAh (to power Herkulex: SM1, SM2)
BT3: 7.4 V Li-Po with 2100 mAh (Ttzo power Herkulex: SM3)
BT4: 7.4 V Li-Po with 2100 mAh (to power Herkulex: SM4)
1 × IMU
MPU-6050 6 DoF motion sensor
3 × VL6180X, proximity sensor
Dia: 4.8 × 2.8 × 1.0 mm; range: 0 to 100 mm; 2.6 to 3.0 V; 1.7 mA
1 × solid-state 2D lidar/LS02A (proposed)
FOV: 86°; distance accuracy: 1.5%; distance range: 0.1–4 m; scan freq.: 10 Hz; angular resolution: 1°/0.5°


## 3. Locomotion and Reconfiguration Control of sTetro

The sequence of sensing and control actions enables thesTetro robot to reconfigure itself and move forward, left/right wards and upwards. In the previous prototype [28], motion and reconfiguration actions were controlled by a human operator by sending control commands through a remote control, but in the current version, we have automated most of these actions. Different sensors are installed to accomplish the self-reconfiguration and locomotion task. To make the locomotion and self-reconfiguration of sTetro automatic, simple, low-cost, low-weight, and power-efficient sensors have been used in this project. As it is a slow-moving wheeled robot to physically interact/negotiate with its environment (e.g., walls/stairs and human), the contact sensors are used in the design due to the robot’s slow motion and low momentum. Moreover, physical contact sensors are unaffected with environmental conditions, for example, light conditions and target material type, and are more accurate than electronic sensors. Contact sensors react only when they have physical touch with the surrounding environment and have a zero limiting range. Six long bump sensors (BM1~6) are installed on the left and right sides of the robot to detect the side boundaries of the staircase. A contact switch (SW0) is installed in the front of block-1. Two touch sensors (SW2 and SW3) of the SPDT type are fixed on the front face of block-2 to detect the front riser and smooth sliding motion against the front riser. Two ToF1 and ToF2 sensors are fixed in the bottom of each block-1 and block-2 to detect the touchdown of these blocks on the tread of the stair. The third ToF3 sensor is attached in the front of block-3 to detect the front riser. The navigation sensors installed on sTetro are shown in Figure 4.Figure 4
Navigation sensors installed onsTetro.When the Herkulex-1 (SM1) actuates clockwise, it initiates the upward motion of block-1, and when SM1 actuates in the counter clock direction, it enables the downward motion of block-1. Similarly, when Herkulex-2 (SM2) actuates in the clockwise/counterclockwise direction, it enables the upward/downward motion of the 3rd block. When SM1 actuates in a clockwise direction and SM2 actuates in the counterclockwise direction, the upward and downward motions of the block-2 are achieved, respectively.An Arduino MEGA 2560-based microcontroller and an Adafruit DC motor shield are used in the sTetro robot to achieve its locomotion and reconfiguration. This unit is placed inside the middle block. An IMU (PMU-6050) is placed in block-2 as well to detect the heading angle of the robot and to be kept aligned with the front riser of the staircase. The microcontroller controls all the 6 × DC motors by sending PWM values through the DC motor shield. The unit also controls the 4 × servo motors to control its position and angle during locomotion. The Adafruit DC motor shield has 4 channels, MD1–MD4, which are used to address each DC motor. The entire robot is powered using a 3 × 7.4 V (2100 mAh) and 1 × 11.1 V lithium polymer (1600 mAh) battery power. A voltage regulator is used to distribute power to two servo motors. Additionally, a proportional-integrative-derivative (PID) controller has been implemented to control the position and angle of the robot with feedback from the IMU sensor. To control the locomotion and self-reconfiguration modes of the developed robot, we have designed an Android application using Android Studio in Java. A Bluetooth module (HC-06) with a band frequency of 2.4 GHz is used to communicate with the robot. The system architecture of the sTetro robot is illustrated in Figure5.Figure 5
System architecture ofsTetro.


## 4. sTetro: System Modeling

### 4.1. Torque of Conveyor Motor

The modelling of the vertical conveyor system that forms the core of thesTetro robot was the critical component system design. The analysis is done on the relationship between the torque required and the mass of each block. As shown in Figure 6(a), the forward locomotion requires the vertical motion of the blocks which is driven by the motor marked as the red dot. The torque of the conveyor motor and the mass of each block determine whether it is feasible for the conveyor motor, and the mass of each block determines whether it is feasible for the robot to reconfigure itself and complete the staircase climbing locomotion. The weights of each block are represented by m1, m2, and m3. The black blocks in the middle are the motors, and the red geometry is the position of the conveyor. Figure 6(b) shows the schematic diagram and the parameter used in the conveyor torque analysis. In Figure 6(b), point A refers to the position of the motor, W1 and F1,req. are the weight and the force required to lift up block-1 while LW1 and Lf refer to their corresponding moment, and N11, N12, N13, and N14 are the normal forces exerted on block-1 while d refers to the distance between the mounting points.Figure 6
Parameters used in conveyor torque analysis.
(a)
(b)The dynamic model ofsTetro is formulated below. The summation of moments at point A on the linear guide which carries block-1 (m1) is given as
(1)∑MA=0,W1LW1+Fm1,req.Lf1−N11+N12+N13+N14d2=0,N11+N12+N13+N14=2W1LW1+2Fm1,req.Lf1d,∑Fy=Fm1,req.−W1−Ff1=m1y¨,Fm1,req.−W1−μN11+N12+N13+N14=m1y¨,Fm1,req.−W1−μ2W1LW1+2Fm1,req.Lf1d=m1y¨,Fm1,req.=m1y¨d+W1d+2μLW1d−2μLf1.Hence, simplifying the equations above gives out(2)∑MA=Fm2,req.Lf+N21+N22+N23+N240.5d−0.5W2LW2=0,N21+N22+N23+N24=W2LW2−2Fm2,req.Lf2d,∑Fy=Fm2,req.−0.5W2−Ff2=0.5m2y¨,Fm2,req.−0.5W2−μN21+N22+N23+N24=0.5m2y¨,Fm2,req.−0.5W2−μW2LW2−2Fm2,req.Lfd=0.5m2y¨,Fm2,req.=0.5m2y¨d+W20.5d+μLW2d+2μLf.


### 4.2. Centre of Gravity (CoG) Analysis

The purpose of CoG analysis is to verify the stability of the robot when not all wheels are in contact with the ground during ascending and descending staircases. This section will deal with the stability calculation of the robot under the typical static scenarios. Figure7(a) shows parameters used in the calculation and the free-body diagram (FBD) when block ① is lifted up. The sTetro robot consists of three blocks which are labelled as ①, ②, and ③. As the normal forces exerted on the robot are on the x-z plane, they are labelled in the bottom diagram of Figure 7(a). During the calculation, the whole unit is assumed to be static and stable. The stability is verified by identifying the direction of the normal forces.Figure 7
(a) Schematic of FBD ofsTetro robot. (b) Control scheme.
(a)
(b)According to the FBD, the force and moment equation of the whole unit can be expressed as(3)∑F→=0,∴m1+m2+m3g=∑i=18Ni,∑M→=0.Similarly, the equations of block ①, ②, and ③ are written below. For block ①, parametera is an unknown length depending on the height of block ①.
(4)∑F→x=0,∴Fr1,2=Fr2,1,∑F→y=0,∴m1g=Vr2,1,∑M→=0,∴m1gL2=mr2,1=3Fr2,1a4−3Fr1,2a4.For block ②,(5)∑F→=0,∴Fr1,2+Fr2,3=Fr2,1+Fr3,2,∑F→y=0,∴m2g+Vr1,2−Vr3,2=∑i=58Ni,Vr1,2=Vr2,1,∑M→=0,∴0.5m2gL−N5L−0.5N6+N8L−0.5Vr3,2L=Mr3,2−Mr2,1,Mr3,2=3Fr3,2H4−3Fr2,3H4.Similarly, for block ③,(6)∑F→x=0,∴Fr2,3=Fr3,2,∑F→y=0,∴m3g+Vr2,3=∑i=14Ni,∑M→=0,∴m3gL2−N1L−N2+N4L2=−Mr2,3,Mr2,3=Mr3,2.For blocks ② and ③ as a whole,(7)∑F→x=0,∴Fr1,2=Fr2,1,∑F→y=0,∴m2+m3g+Vr1,2=∑i=18Ni,∑M→=0,∴m1gL2+Vr1,2L+Mr1,2+N1L+N2+N4L2=m3gL2+N6+N8L2+N7L.For the robot to be stable, the constraints are set to beNi≥0,i=1,2,3,4,5,6,7,8. The extreme case is when all the normal forces are equal to zero; that is, m1g=4m2g+m3g. By substituting the dimensions from the design, the constraints are fulfilled. Therefore, the whole robot is in the stable condition.


### 4.3. PID Control Approach

The implementation of a PID controller in this system is necessary as there are always multiple driving and driven wheels working simultaneously. Any asynchronization of three blocks will cause it to deviate from the original orientation. The mathematical modelling and simulated response were done in [28]. The block diagram of the control system is shown in Figure 7(b). The overall transfer function of the system is given as (complete derivation of the transfer function is given in [28])
(8)ΦsE2s−E1s=ry2KbR+KKbJwR/bR+KKbs+1s.Substitute the constants into the equation,(9)ΦsE2s−E1s=0.030.32×0.955b1.82×10−5×6.324+0.955×6.29×1.707×10−4/6.26s+1s.By usingSISOTOOL in MATLAB, the simulation of the step and impulse response are shown in the figures below with corresponding bode diagram and values of gains. The optimized transfer function of the controller is found as
(10)Gcs=0.057728s+20.78+161.63s.Therefore, the proportional gain is 20.78, the integral gain is 0.057728, and the derivative gain is 161.63. Under the control of the above controller, Figures8 and 9 show the step and impulse response of this system.Figure 8
Step response of the system with bode plot.Figure 9
Impulse response of the system with bode plot.The simulated response of the system to unit step input shows that the final value approaches one with zero steady-state error. Similarly, the impulse response of the system simulates the ability of handling impulse disturbance from the surrounding because of the implementation of the integration term. Both of the poles of controlled systems are placed at the turning point on the magnitude bode diagram to reduce the settling time.


## 4.1. Torque of Conveyor Motor

The modelling of the vertical conveyor system that forms the core of thesTetro robot was the critical component system design. The analysis is done on the relationship between the torque required and the mass of each block. As shown in Figure 6(a), the forward locomotion requires the vertical motion of the blocks which is driven by the motor marked as the red dot. The torque of the conveyor motor and the mass of each block determine whether it is feasible for the conveyor motor, and the mass of each block determines whether it is feasible for the robot to reconfigure itself and complete the staircase climbing locomotion. The weights of each block are represented by m1, m2, and m3. The black blocks in the middle are the motors, and the red geometry is the position of the conveyor. Figure 6(b) shows the schematic diagram and the parameter used in the conveyor torque analysis. In Figure 6(b), point A refers to the position of the motor, W1 and F1,req. are the weight and the force required to lift up block-1 while LW1 and Lf refer to their corresponding moment, and N11, N12, N13, and N14 are the normal forces exerted on block-1 while d refers to the distance between the mounting points.Figure 6
Parameters used in conveyor torque analysis.
(a)
(b)The dynamic model ofsTetro is formulated below. The summation of moments at point A on the linear guide which carries block-1 (m1) is given as
(1)∑MA=0,W1LW1+Fm1,req.Lf1−N11+N12+N13+N14d2=0,N11+N12+N13+N14=2W1LW1+2Fm1,req.Lf1d,∑Fy=Fm1,req.−W1−Ff1=m1y¨,Fm1,req.−W1−μN11+N12+N13+N14=m1y¨,Fm1,req.−W1−μ2W1LW1+2Fm1,req.Lf1d=m1y¨,Fm1,req.=m1y¨d+W1d+2μLW1d−2μLf1.Hence, simplifying the equations above gives out(2)∑MA=Fm2,req.Lf+N21+N22+N23+N240.5d−0.5W2LW2=0,N21+N22+N23+N24=W2LW2−2Fm2,req.Lf2d,∑Fy=Fm2,req.−0.5W2−Ff2=0.5m2y¨,Fm2,req.−0.5W2−μN21+N22+N23+N24=0.5m2y¨,Fm2,req.−0.5W2−μW2LW2−2Fm2,req.Lfd=0.5m2y¨,Fm2,req.=0.5m2y¨d+W20.5d+μLW2d+2μLf.


## 4.2. Centre of Gravity (CoG) Analysis

The purpose of CoG analysis is to verify the stability of the robot when not all wheels are in contact with the ground during ascending and descending staircases. This section will deal with the stability calculation of the robot under the typical static scenarios. Figure7(a) shows parameters used in the calculation and the free-body diagram (FBD) when block ① is lifted up. The sTetro robot consists of three blocks which are labelled as ①, ②, and ③. As the normal forces exerted on the robot are on the x-z plane, they are labelled in the bottom diagram of Figure 7(a). During the calculation, the whole unit is assumed to be static and stable. The stability is verified by identifying the direction of the normal forces.Figure 7
(a) Schematic of FBD ofsTetro robot. (b) Control scheme.
(a)
(b)According to the FBD, the force and moment equation of the whole unit can be expressed as(3)∑F→=0,∴m1+m2+m3g=∑i=18Ni,∑M→=0.Similarly, the equations of block ①, ②, and ③ are written below. For block ①, parametera is an unknown length depending on the height of block ①.
(4)∑F→x=0,∴Fr1,2=Fr2,1,∑F→y=0,∴m1g=Vr2,1,∑M→=0,∴m1gL2=mr2,1=3Fr2,1a4−3Fr1,2a4.For block ②,(5)∑F→=0,∴Fr1,2+Fr2,3=Fr2,1+Fr3,2,∑F→y=0,∴m2g+Vr1,2−Vr3,2=∑i=58Ni,Vr1,2=Vr2,1,∑M→=0,∴0.5m2gL−N5L−0.5N6+N8L−0.5Vr3,2L=Mr3,2−Mr2,1,Mr3,2=3Fr3,2H4−3Fr2,3H4.Similarly, for block ③,(6)∑F→x=0,∴Fr2,3=Fr3,2,∑F→y=0,∴m3g+Vr2,3=∑i=14Ni,∑M→=0,∴m3gL2−N1L−N2+N4L2=−Mr2,3,Mr2,3=Mr3,2.For blocks ② and ③ as a whole,(7)∑F→x=0,∴Fr1,2=Fr2,1,∑F→y=0,∴m2+m3g+Vr1,2=∑i=18Ni,∑M→=0,∴m1gL2+Vr1,2L+Mr1,2+N1L+N2+N4L2=m3gL2+N6+N8L2+N7L.For the robot to be stable, the constraints are set to beNi≥0,i=1,2,3,4,5,6,7,8. The extreme case is when all the normal forces are equal to zero; that is, m1g=4m2g+m3g. By substituting the dimensions from the design, the constraints are fulfilled. Therefore, the whole robot is in the stable condition.


## 4.3. PID Control Approach

The implementation of a PID controller in this system is necessary as there are always multiple driving and driven wheels working simultaneously. Any asynchronization of three blocks will cause it to deviate from the original orientation. The mathematical modelling and simulated response were done in [28]. The block diagram of the control system is shown in Figure 7(b). The overall transfer function of the system is given as (complete derivation of the transfer function is given in [28])
(8)ΦsE2s−E1s=ry2KbR+KKbJwR/bR+KKbs+1s.Substitute the constants into the equation,(9)ΦsE2s−E1s=0.030.32×0.955b1.82×10−5×6.324+0.955×6.29×1.707×10−4/6.26s+1s.By usingSISOTOOL in MATLAB, the simulation of the step and impulse response are shown in the figures below with corresponding bode diagram and values of gains. The optimized transfer function of the controller is found as
(10)Gcs=0.057728s+20.78+161.63s.Therefore, the proportional gain is 20.78, the integral gain is 0.057728, and the derivative gain is 161.63. Under the control of the above controller, Figures8 and 9 show the step and impulse response of this system.Figure 8
Step response of the system with bode plot.Figure 9
Impulse response of the system with bode plot.The simulated response of the system to unit step input shows that the final value approaches one with zero steady-state error. Similarly, the impulse response of the system simulates the ability of handling impulse disturbance from the surrounding because of the implementation of the integration term. Both of the poles of controlled systems are placed at the turning point on the magnitude bode diagram to reduce the settling time.


## 5. Autonomy Algorithm ofsTetro Robot

To make the locomotion and self-reconfiguration of sTetro automatic, simple, low-cost, low-weight, and power-efficient sensors have been used in this project. A time of flight (ToF) lidar is used used for distance measurement to obstacle/stair, and a MEMS IMU is used for misalignment detection from the front riser of staircase. In addition, two ToFs are used in the bottom of block-1 and block-2 to detect touching down. In order to achieve autonomy insTetro, the following sensors/components are used:
(i)
1 × contact switch (SW0) in the front side of block-1 (to detect the front riser of the stair)
(ii)
2 × contact switches (SW1 and SW2) at both sides of block-2 (to detect the front riser of the stair and for the smooth motion against riser)
(iii)
6 × side bumpers (B1–6) at both sides of block-1~block-3 (to detect the end of sides of the staircase)
(iv)
2 × ToF sensors (ToF1 and ToF2) at the bottom of block-1and block-2 (to detect touchdown on tread)
(v)
1 × ToF sensor (ToF3) in the front side of block-3 (to detect front riser of the stair)
(vi)
1 × IMU fitted in block-2 (to detect misalignment of the robot)
(vii)
1 × 2D lidar for staircase recognition


### 5.1. Automatic Staircase Recognition

A 2D lidar is used, in vertical scanning configuration, to capture the staircase structure scans in the working environment. As in this application, the staircase has a unique feature as compared to surrounding walls and home features (table, chairs, beds, etc.); hence, by exploiting this distinguishing shape of the staircase, we have developed a simple algorithm to initialize the staircase cleaning operation bysTetro.Theautomated staircase recognition algorithm starts with a capture of a reference scan (SR) with the lidar installed on sTetro in the vertical direction (Figure 10(a)). Due to this particular placement of the lidar, the staircase structure will appear as steps (riser and tread) when lidar ranges are obtained in a scan. While capturing the reference scan, the sTetro/lidar is facing (almost perfectly) to the staircase, and thus, the scan range obtained looks like a staircase pattern. After getting and storing SR, we place sTetro in any arbitrary orientation in front of the stairs, and the robot starts taking rotations to scan its environment. At each angular position, the measured scan (SM) is compared, by calculating the Euclidean norm (also known as the square root of the sum of squares of differences), with already stored reference scan and the one which gives the minimum Euclidean distance (Di) is assumed to be the closest angular position with the reference orientation (facing staircase).
(11)Di=∑iSR−SMi2,where Di is the Euclidean norm.Figure 10
(a) 2D lidar installed onsTetro. (b) Range data with an outlier rejection filter effect.
(a)
(b)Before comparing the two scans, we also have designed a simple outlier filter to reject the spurious points. The technique used is simple. In the range data, the difference between consecutive lidar points is calculated; if this difference is larger than some threshold value, that particular point in the scan is considered a spurious point and is replaced with the mean value of the previous point and this spurious point. This enables smoother data points in a particular scan and avoids any overestimate of the Euclidean norm value, as shown in Figure10(b). A pseudocode of this algorithm is provided in Pseudocode 1 below.Pseudocode 1: Staircase Recognition.
Automatic Staircase Recognition Algorithm insTetro:
Capture reference scan (SR);         // by placing robot in front of stairs such that lidar rays
//are perpendicular to the stairs.
OutLierFilter(SR);               // apply OutLierFilter() to reject any superiors points.
SaveSR;
While (Euclidean Norm is Not Min.)
TURN in-place;// rotate CCW
Capture measured scan (SMi);
OutLierFilter(SMi);
Di ← CalcEuclideanNorm(SR,SMi);  //calculate Euclidean Norm between ref. scan and
//measured scan.
StoreDi;
If (Di<Di−1)
Continue TURN in-place;// continue rotating till Euclidean dist. is min.
end if
endWhile
STOP Turning.


### 5.2. Misalignment Correction Technique

During operation, thesTetro robot must be aligned with the staircase riser so that it can cover all tread area for cleaning purposes. If some misalignment occurs uncorrected, the robot may fall back from the narrow tread’s area. In order to detect misalignment with the front riser, an attitude and heading reference system (AHRS) has been used in this work. The AHRS consists of inertial IMU sensor (3 × gyros, 3 × accelerometers) on three orthogonal axes that provide attitude information (roll, pitch, and yaw/heading) for any moving platform on which the sensor is installed. When the robot touches the riser in the first step and is aligned with the staircase, the yaw angle at that position is taken as the reference heading, and any subsequent changes from this reference heading angle are taken as misalignments (with some tolerance/threshold). The AHRS algorithm is able to detect such misalignments very reliably and gives feedback to the control system for corrective measures.For thesTetro to move and reconfigure itself autonomously, the high-level description (pseudocode) of the autonomy algorithm is given in Pseudocode 2 below:Pseudocode 2: Autonomy Algorithm in sTetro.
Pseudocode of Autonomy Algorithm in sTetro:
// Initialization Start
While (staircase Not detected)      // using staircase recognition algorithm.
TURN in-place;// rotate CCW
endWhile
STOP Turning;
Initialization ends
//Step#1;
If (ToF0_range< 1500mm)      // continue staircase climbing until ToF0 > 1.5 m.
// Forward motion.
While (ToF0_range > 5mm)        // move fwd until ToF0 measure < 5 mm.
MOVE FWD;
endWhile
STOP Moving fwd;
LIFT Block-1 to ~18 cm;// determined by mechanical lock.
While (SW1_OFF or SW2_OFF)     // move fwd until SW1 or SW2 is ON.
MOVE FWD;
endWhile
STOP Moving fwd;
While (ToF1_Range_Down < 1)      // drop Block_1 until ToF1 senses < 1cm range.
DROP Block-1;
endWhile
STOP Dropping Block-1;
TURN Herkulex (H-) wheels to left 90° CCW;
// Leftwards motion.
While (BM_1 ~ 3 Not Touch)        // move leftwards until BM_1 ~ 3 touches left wall.
START Moving leftwards;
If (Heading >3°)// misalignment detection & correction.
MOVE H-wheels CW to achieve robot alignment;
elseif (Heading > -3°)
MOVE H-wheels CCW to achieve robot alignment;
endif
endWhile
STOP moving leftwards;
// Rightwards motion.
While (BM 4 ~ 6 Not Touch)       // move rightwards until BM_4~6 touches right wall.
START Moving rightwards;
If (Heading > 3°)// misalignment detection & correction.
MOVE H-wheels CCW to achieve robot alignment;
elseif (Heading > -3°)
MOVE H-wheels CW to achieve robot alignment;
endif
endWhile
STOP moving rightwards;
MOVE leftwards for 2 ~ 3 sec.// this movement is to keep robot little away from right wall
// of stairs, to prevent rubbing with right side-wall, before stepping to next staircase.
Turn H-wheels 90° CW.
LIFT Block-2 upwards (~18cm)// determined by mechanical lock.
While (ToF3 >0.5cm)            // move fwd until ToF3 measures< 0.5cm.
MOVE FWD;
endWhile
STOP Moving fwd;
While (ToF2_Range_Down < 0.5)      // drop Block_2 until ToF2 senses < 0.5cm range.
DROP Block-2;
endWhile
STOP Dropping Block-2;
LIFT Block-3 upwards (~18cm);// determined by mechanical lock.
Go to Step#1;
endIf
STOP sTetro;               // finished climbing up the staircase.


## 5.1. Automatic Staircase Recognition

A 2D lidar is used, in vertical scanning configuration, to capture the staircase structure scans in the working environment. As in this application, the staircase has a unique feature as compared to surrounding walls and home features (table, chairs, beds, etc.); hence, by exploiting this distinguishing shape of the staircase, we have developed a simple algorithm to initialize the staircase cleaning operation bysTetro.Theautomated staircase recognition algorithm starts with a capture of a reference scan (SR) with the lidar installed on sTetro in the vertical direction (Figure 10(a)). Due to this particular placement of the lidar, the staircase structure will appear as steps (riser and tread) when lidar ranges are obtained in a scan. While capturing the reference scan, the sTetro/lidar is facing (almost perfectly) to the staircase, and thus, the scan range obtained looks like a staircase pattern. After getting and storing SR, we place sTetro in any arbitrary orientation in front of the stairs, and the robot starts taking rotations to scan its environment. At each angular position, the measured scan (SM) is compared, by calculating the Euclidean norm (also known as the square root of the sum of squares of differences), with already stored reference scan and the one which gives the minimum Euclidean distance (Di) is assumed to be the closest angular position with the reference orientation (facing staircase).
(11)Di=∑iSR−SMi2,where Di is the Euclidean norm.Figure 10
(a) 2D lidar installed onsTetro. (b) Range data with an outlier rejection filter effect.
(a)
(b)Before comparing the two scans, we also have designed a simple outlier filter to reject the spurious points. The technique used is simple. In the range data, the difference between consecutive lidar points is calculated; if this difference is larger than some threshold value, that particular point in the scan is considered a spurious point and is replaced with the mean value of the previous point and this spurious point. This enables smoother data points in a particular scan and avoids any overestimate of the Euclidean norm value, as shown in Figure10(b). A pseudocode of this algorithm is provided in Pseudocode 1 below.Pseudocode 1: Staircase Recognition.
Automatic Staircase Recognition Algorithm insTetro:
Capture reference scan (SR);         // by placing robot in front of stairs such that lidar rays
//are perpendicular to the stairs.
OutLierFilter(SR);               // apply OutLierFilter() to reject any superiors points.
SaveSR;
While (Euclidean Norm is Not Min.)
TURN in-place;// rotate CCW
Capture measured scan (SMi);
OutLierFilter(SMi);
Di ← CalcEuclideanNorm(SR,SMi);  //calculate Euclidean Norm between ref. scan and
//measured scan.
StoreDi;
If (Di<Di−1)
Continue TURN in-place;// continue rotating till Euclidean dist. is min.
end if
endWhile
STOP Turning.


## 5.2. Misalignment Correction Technique

During operation, thesTetro robot must be aligned with the staircase riser so that it can cover all tread area for cleaning purposes. If some misalignment occurs uncorrected, the robot may fall back from the narrow tread’s area. In order to detect misalignment with the front riser, an attitude and heading reference system (AHRS) has been used in this work. The AHRS consists of inertial IMU sensor (3 × gyros, 3 × accelerometers) on three orthogonal axes that provide attitude information (roll, pitch, and yaw/heading) for any moving platform on which the sensor is installed. When the robot touches the riser in the first step and is aligned with the staircase, the yaw angle at that position is taken as the reference heading, and any subsequent changes from this reference heading angle are taken as misalignments (with some tolerance/threshold). The AHRS algorithm is able to detect such misalignments very reliably and gives feedback to the control system for corrective measures.For thesTetro to move and reconfigure itself autonomously, the high-level description (pseudocode) of the autonomy algorithm is given in Pseudocode 2 below:Pseudocode 2: Autonomy Algorithm in sTetro.
Pseudocode of Autonomy Algorithm in sTetro:
// Initialization Start
While (staircase Not detected)      // using staircase recognition algorithm.
TURN in-place;// rotate CCW
endWhile
STOP Turning;
Initialization ends
//Step#1;
If (ToF0_range< 1500mm)      // continue staircase climbing until ToF0 > 1.5 m.
// Forward motion.
While (ToF0_range > 5mm)        // move fwd until ToF0 measure < 5 mm.
MOVE FWD;
endWhile
STOP Moving fwd;
LIFT Block-1 to ~18 cm;// determined by mechanical lock.
While (SW1_OFF or SW2_OFF)     // move fwd until SW1 or SW2 is ON.
MOVE FWD;
endWhile
STOP Moving fwd;
While (ToF1_Range_Down < 1)      // drop Block_1 until ToF1 senses < 1cm range.
DROP Block-1;
endWhile
STOP Dropping Block-1;
TURN Herkulex (H-) wheels to left 90° CCW;
// Leftwards motion.
While (BM_1 ~ 3 Not Touch)        // move leftwards until BM_1 ~ 3 touches left wall.
START Moving leftwards;
If (Heading >3°)// misalignment detection & correction.
MOVE H-wheels CW to achieve robot alignment;
elseif (Heading > -3°)
MOVE H-wheels CCW to achieve robot alignment;
endif
endWhile
STOP moving leftwards;
// Rightwards motion.
While (BM 4 ~ 6 Not Touch)       // move rightwards until BM_4~6 touches right wall.
START Moving rightwards;
If (Heading > 3°)// misalignment detection & correction.
MOVE H-wheels CCW to achieve robot alignment;
elseif (Heading > -3°)
MOVE H-wheels CW to achieve robot alignment;
endif
endWhile
STOP moving rightwards;
MOVE leftwards for 2 ~ 3 sec.// this movement is to keep robot little away from right wall
// of stairs, to prevent rubbing with right side-wall, before stepping to next staircase.
Turn H-wheels 90° CW.
LIFT Block-2 upwards (~18cm)// determined by mechanical lock.
While (ToF3 >0.5cm)            // move fwd until ToF3 measures< 0.5cm.
MOVE FWD;
endWhile
STOP Moving fwd;
While (ToF2_Range_Down < 0.5)      // drop Block_2 until ToF2 senses < 0.5cm range.
DROP Block-2;
endWhile
STOP Dropping Block-2;
LIFT Block-3 upwards (~18cm);// determined by mechanical lock.
Go to Step#1;
endIf
STOP sTetro;               // finished climbing up the staircase.


## 6. Experimental Results

Experiments are performed with thesTetro platform to demonstrate the efficacy and validity of the developed algorithms of the control system, the torque generated by the conveyor motor as well as autonomous mode operation. This section presents the experimental results in detail.


### 6.1. PID Controller Experiments

The first set of experiments focused on testing the response of the robot during the side-way motion without any disturbance. The unit is switched on and commanded to move in the direction that is perpendicular to its body as shown in Figure11(a). Initially, the accelerometer is calibrated as the zero degree. Although all motors belong to the same model, the levelling issue arising from the three blocks resulted in the slippage in wheels and thereby deviation in the direction of motion. The values of three gains used in the experiments are based on the theoretical analysis. Figure 11(b) shows the time plot of the angle it deviates (φ) during the side-way motion without disturbance. The plot shows that it tends to deviate towards the positive side. The suspected reason is the wear-off of the motor shafts which are 3D printed in PLA material. In addition, the weight of the robot could have contributed to the loose connection between the shaft and the motor, resulting in further deformation of the wheel and deviation in the motion.Figure 11
(a) Schematic diagram for deviation process. (b) Time plot of angle under natural response.
(a)
(b)The second set of the experiment involved the study of the step response during the side-way motion. After the calibration, the robot is placed with a nonzero initial angle and commanded to move sideways. The expected angle is set as zero as a step input to the system. Figure12(a) shows the time plot of its deviation angle. Lastly, the experiment involved the study of the impulse response during the side-way motion. Instead of placing the stopper in front, the impulse response is simulated by kicking block-1 back to create an abrupt change in the deviation angle. Figure 12(b) shows the time plot of the angle under impulse disturbance.Figure 12
(a) Step response. (b) Impulse response of the system.
(a)
(b)The plot of the step and impulse responses of this system coincide with the simulation with exceptions to settling time, peak time, and the peak value. The exceptions are mainly attributed to differences in motor specifications and input conditions between real and simulation settings. However, the trends in both plots are the same indicating that the PID controller is able to regulate the movement under unit step and impulse inputs. The simulation and experiments performed clearly indicate that the gain value obtained from the system modelling for thesTetro platform is usable in regulating the sideway motion of the unit.


### 6.2. Torque Experiment on the Conveyor Motor

This section presents our efforts to experimentally validate the estimated torque for the conveyor motor using the model in Section4.1 to ensure stable operation. The torque of the servo motor is not measured directly. Instead, it is approximated by the current drawn by the concerned conveyor motor. An ACS714 current sensor is integrated onboard the sTetro robot and used to measure the current draw by the motor. Figure 13 shows the current versus time plot during the lifting of block-1.Figure 13
Current versus time plot during lifting of block-1.The figure above presents the change in torque for the concerned servo motor. The values of the current vary around 0.3 A which falls within the acceptable working range for the servo motor. Based on the manufacturing specification of Herkulex drs-0201 motor used, the torque generated by the servo during this process is approximately 1.13 kgf cm while lifting block-1 which coincides with the theoretical value. This validates the theoretical predictions for the torque needed for the conveyor motor.


### 6.3. Staircase Detection Experiment

For the smooth transition from the flat surface to the staircase, a staircase recognition algorithm is tested in this work. In Figure10(a), the experimental setup is shown for the stair recognition test. The sTetro robot fixed with the 2D lidar in the vertical position is put in facing stairs, and a reference scan is taken. After that, the robot can be put in any arbitrary orientation and it starts scanning its environment by turning the counterclockwise direction and scan matching algorithm is activated. In Figure 14(a), the range scan matching is given for one scan shot. Staircases are clearly detected when the lidar is nearly facing the stairs. However, if the robot is not facing the stairs, but facing, for example, a wall, then the measured scan gives a one big step in range data, as shown in Figure 14(b), which is clearly distinguishable from the staircase pattern.Figure 14
(a) Reference and measured scan matching for staircase. (b) Wall detection in measured scan.
(a)
(b)In Figure15, the plot of Euclidean norm versus scan index is given. As the robot starts turning at any arbitrary angular position but approaches the angular position at which the reference scan was taken, the Euclidean norm gives the minimum value. This angular position declared that the robot has recognized stairs and is now facing the staircase. After recognizing the stairs, the sTetro starts approaching the staircase to start the cleaning operation.Figure 15
(a) Euclidean distance plot for each scan matching outcome. (b) Heading angle of sTeto.
(a)
(b)Figure15(b) gives the heading angle change during the left and right motion of the sTetro. As the robot starts from a heading angle of about −161.4°, the left/right motion affects the heading/misalignment angle within a tolerance of ±5°. Beyond this limit, the controller activates for corrective measures as described in the autonomy algorithm.


## 6.1. PID Controller Experiments

The first set of experiments focused on testing the response of the robot during the side-way motion without any disturbance. The unit is switched on and commanded to move in the direction that is perpendicular to its body as shown in Figure11(a). Initially, the accelerometer is calibrated as the zero degree. Although all motors belong to the same model, the levelling issue arising from the three blocks resulted in the slippage in wheels and thereby deviation in the direction of motion. The values of three gains used in the experiments are based on the theoretical analysis. Figure 11(b) shows the time plot of the angle it deviates (φ) during the side-way motion without disturbance. The plot shows that it tends to deviate towards the positive side. The suspected reason is the wear-off of the motor shafts which are 3D printed in PLA material. In addition, the weight of the robot could have contributed to the loose connection between the shaft and the motor, resulting in further deformation of the wheel and deviation in the motion.Figure 11
(a) Schematic diagram for deviation process. (b) Time plot of angle under natural response.
(a)
(b)The second set of the experiment involved the study of the step response during the side-way motion. After the calibration, the robot is placed with a nonzero initial angle and commanded to move sideways. The expected angle is set as zero as a step input to the system. Figure12(a) shows the time plot of its deviation angle. Lastly, the experiment involved the study of the impulse response during the side-way motion. Instead of placing the stopper in front, the impulse response is simulated by kicking block-1 back to create an abrupt change in the deviation angle. Figure 12(b) shows the time plot of the angle under impulse disturbance.Figure 12
(a) Step response. (b) Impulse response of the system.
(a)
(b)The plot of the step and impulse responses of this system coincide with the simulation with exceptions to settling time, peak time, and the peak value. The exceptions are mainly attributed to differences in motor specifications and input conditions between real and simulation settings. However, the trends in both plots are the same indicating that the PID controller is able to regulate the movement under unit step and impulse inputs. The simulation and experiments performed clearly indicate that the gain value obtained from the system modelling for thesTetro platform is usable in regulating the sideway motion of the unit.


## 6.2. Torque Experiment on the Conveyor Motor

This section presents our efforts to experimentally validate the estimated torque for the conveyor motor using the model in Section4.1 to ensure stable operation. The torque of the servo motor is not measured directly. Instead, it is approximated by the current drawn by the concerned conveyor motor. An ACS714 current sensor is integrated onboard the sTetro robot and used to measure the current draw by the motor. Figure 13 shows the current versus time plot during the lifting of block-1.Figure 13
Current versus time plot during lifting of block-1.The figure above presents the change in torque for the concerned servo motor. The values of the current vary around 0.3 A which falls within the acceptable working range for the servo motor. Based on the manufacturing specification of Herkulex drs-0201 motor used, the torque generated by the servo during this process is approximately 1.13 kgf cm while lifting block-1 which coincides with the theoretical value. This validates the theoretical predictions for the torque needed for the conveyor motor.


## 6.3. Staircase Detection Experiment

For the smooth transition from the flat surface to the staircase, a staircase recognition algorithm is tested in this work. In Figure10(a), the experimental setup is shown for the stair recognition test. The sTetro robot fixed with the 2D lidar in the vertical position is put in facing stairs, and a reference scan is taken. After that, the robot can be put in any arbitrary orientation and it starts scanning its environment by turning the counterclockwise direction and scan matching algorithm is activated. In Figure 14(a), the range scan matching is given for one scan shot. Staircases are clearly detected when the lidar is nearly facing the stairs. However, if the robot is not facing the stairs, but facing, for example, a wall, then the measured scan gives a one big step in range data, as shown in Figure 14(b), which is clearly distinguishable from the staircase pattern.Figure 14
(a) Reference and measured scan matching for staircase. (b) Wall detection in measured scan.
(a)
(b)In Figure15, the plot of Euclidean norm versus scan index is given. As the robot starts turning at any arbitrary angular position but approaches the angular position at which the reference scan was taken, the Euclidean norm gives the minimum value. This angular position declared that the robot has recognized stairs and is now facing the staircase. After recognizing the stairs, the sTetro starts approaching the staircase to start the cleaning operation.Figure 15
(a) Euclidean distance plot for each scan matching outcome. (b) Heading angle of sTeto.
(a)
(b)Figure15(b) gives the heading angle change during the left and right motion of the sTetro. As the robot starts from a heading angle of about −161.4°, the left/right motion affects the heading/misalignment angle within a tolerance of ±5°. Beyond this limit, the controller activates for corrective measures as described in the autonomy algorithm.


## 7. Conclusion

In this paper, we presented the design and testing of a novel modular reconfigurable cleaning robot,sTetro, that uses vertical conveyor mechanism as a basis. We put forward system models for the estimation of torque for the conveyor motor and centre of gravity analysis towards platform stability. We also describe in detail the design of a PID controller to ensure stable motion of the sTetro robot.The initialization step, which comprises of staircase recognition during operation, is also tested and shows staircase recognition results are very faithful. Moreover, for smooth autonomous operation, the alignment of the robot with the front riser of the staircase is also detected and results are promising to detect and correct misalignment, if any, using single MEMS IMU. To achieve full autonomy, an algorithm is developed which uses inputs from simple, low-cost, low-power sensors and ensures smooth autonomous staircase cleaning operation, by self-reconfiguration and forward/left/right motion. The integration of contact sensors, ToF sensors, 2D lidar, and an IMU ensures autonomous navigation over a wide range of flat floors and staircases as well, without human intervention. The experiments performed with the real robot demonstrated the efficacy and validity of the developed system models and control approaches.Future research work onsTetro will focus on the following: (1) incorporation of UWB/vision sensor network for localization and autodocking with charging station; (2) integration of a suspension system with a force sensor for adaptation to multiterrain and steadier performance; (3) development of algorithms that ensure optimal path planning outcomes that maximize area coverage while minimizing computational and energy needs; and (4) development of additional features to improve power management issues.



---

*Source: 8190802-2018-07-24.xml*