Swarm Robot Control for Human Services and Moving Rehabilitation by Sensor Fusion

A current trend in robotics is fusing different types of sensors having different characteristics to improve the performance of a robot system and also benefit from the reduced cost of sensors. One type of robot that requires sensor fusion for its application is the service robot. To achieve better performance, several service robots are preferred to work together, and, hence, this paper concentrates on swarm service robots. Swarm service mobile robots operating within a fixed area need to cope with dynamic changes in the environment, and they must also be capable of avoiding dynamic and static obstacles. This study applies sensor fusion and swarm concept for service mobile robots in human services and rehabilitation environment. The swarm robots follow the humanmoving trajectory to provide support to humanmoving and perform several tasks required in their living environment. This study applies a reference control and proportional-integral (PI) control for the obstacle avoidance function. Various computer simulations are performed to verify the effectiveness of the proposed method.


Introduction
A current trend in robotics is fusing different types of sensors having different characteristics to improve the performance of the robot system and also benefit from the reduced cost of sensors.Sensor fusion is a combination of sensory data that has inherent redundancy and may provide robust recognition of a robot's working environment [1][2][3].
A service robot, which operates either semi-or fully autonomously to perform services useful to human wellbeing (excluding manufacturing operations [4][5][6][7][8][9]), requires sensor fusion to enable it to recognize the environment because it is expected to work in dynamic environments such as human living environment.
One of the applications of service robots is rehabilitation environment.The primary objectives of the rehabilitation robots are to either fully or partially perform tasks that benefit the disabled people and support a rehabilitee's manipulative function [10][11][12].Conventional, rehabilitation programs relied heavily on the experience and manual manipulation of the rehabilitator.Because rehabilitation must be conducted carefully and the number of rehabilitee continues to increase, a well-designed service robot may prove effective in providing the support required for careful rehabilitation.
Although most service robots developed thus far are based on a single robot application, to provide better service, multiple service robots working together to create a swarm robot team are preferred, particularly for human services and rehabilitation purposes.The swarm robot may not only support the rehabilitee's movement but also conduct various tasks such as collecting or transporting certain objects during rehabilitation.
A swarm service mobile robot operating in the human living environment needs to cope with its dynamic changes.Hence, the fundamental function of the robot is to avoid static and dynamic obstacles.Particularly, mobile robots in the swarm team must maintain their velocity and avoid collisions with other swarm mates [13][14][15][16][17][18][19][20].Existing studies have employed proximity [14] and vision sensors [17,21] for swarm robot obstacle avoidance and the radio frequency identification (RFID) for localization and navigation purposes [21][22][23][24].
This study presents the collision avoidance control for swarm robots moving in a dynamic environment of moving obstacles.A method is presented for sensor fusion to achieve motion in the dynamic environment.The proposed method combines the information obtained by several proximity sensors, an image sensor, and localization sensors (RFID system).This study applies a leader-follower formation in which the swarm robot team has a leader robot that follows the rehabilitee, while the other robots follow the leader robot.The robot controllers comprise a reference and PI controller.The reference controller generates a robot motion trajectory by referring to sensor information in real-time, and the PI controller makes the robots follow the generated motion trajectory.Various simulation results, which assume the presence of several static and dynamic obstacles in the human living environment, demonstrate the effectiveness of the proposed design.

Mobile Robots Dynamics
This study considers a typical two-wheeled differential-drive mobile robot for swarm robots as shown in Figure 1.Notations for Figure 1 and the following equations are given at the end of this section.
The dynamics of the mobile robot are given by The translational and angular velocity and acceleration of the mobile robot are represented by the following equations: The motor dynamics of the right and left wheels are given by Substituting ( 1)-( 3) into (4), we obtain the state equations as The notations used in Figure 1 and (1)-( 5) are given as follows: : moment of inertia of the robots around the center of gravity : mass of the robots : damping coefficient   ,   : torque and force applied to the robot to follow the rehabilitee or the leader robot : heading angle of the robots φ , V: angular and translational velocities of the robots φ , V : angular and translational acceleration of the robots   ,   : driving forces for the right and left wheels : half width of the robots   ,   : angles of the right and left wheels θ  , θ  : angular velocity of the right and left wheels θ  , θ  : angular acceleration of the right and left wheels : angle between the proximity sensors   : distance data retrieved from proximity sensors and a vision sensor ( = 0, 1, 2, 3) : moment of inertia of motors , , : index for right, left, and time, respectively   ,   : input voltage to the left and right wheels : driving gain of the motor : wheels radius.

Controller Design
This study applies a reference and PI controller for collision avoidance as shown in Figure 2. The reference controller generates a reference trajectory for the leader and follower robots, and the PI controller makes the robots follow the reference trajectory of the leader.

Reference Controller Design.
The environmental information required to create the reference trajectory is provided by fusing multiple sensors: a Kinect sensor, four proximity sensors, and an RFID system.We assume that the RFID system indicates the position of the rehabilitee.The RFID tag attached to the rehabilitee is read by the RFID reader that is attached to the leader, and this helps the leader in tracking and following the rehabilitee by identifying his/her position.
The RFID tag is also attached to the leader whose signal is read by the follower, so that it also can track and follow the leader.
Human position is the goal position for the robots.In this study, human position is assumed to be given from the RFID system, and robots follow the human trajectory as the reference trajectory.
When there is no obstacle on the way from the initial position to the goal position, the input torque and force for the robot in (1) and ( 2),   and   , are given as follows: where φ  is the desired rotational acceleration and V  is the desired translational acceleration.They are obtained from the reference (human) trajectory.
The control algorithm for the leader and the follower is the same.The existence of obstacles creates virtual torques and forces ( 1 ,  2 ,  1 , and  2 ) to the dynamics of mobile robots.
Therefore, in the environments where the presence of several static and dynamic obstacles is assumed, we consider the following reference model to generate the reference trajectory: where   ,  V : virtual damping coefficient and   ,   : virtual torque and force to avoid collision ( = 1, 2).Although damping coefficients are not considered in (1) and ( 2) because they are normally small,   and  V are included in (7) to ensure the stable motion of the robot.
The reference trajectories are created by adding virtual torques and forces ( 1 ,  2 ,  1 , and  2 ) to the dynamics of mobile robots in (7).The virtual torques and forces are calculated by considering the fused data input from the Kinect sensor, proximity sensors, and the RFID system, as shown in Figure 2.
Torques  1 and  2 are designed for collision avoidance and ensuring that the robots move parallel to the virtual wall of the passages, respectively.The force  1 provides the deceleration effect based on the distance of robots to the obstacles, and  2 provides the deceleration effect based on the approaching speed of dynamic obstacles.These four parameters are used to adjust the magnitude of the virtual external force/torque and are calculated as shown below where sgn(V): sign function of the translational velocity of the robots,   ,   : adjusted constants to provide the effective collision avoidance ( = 1, 2), ḋ : time derivative of the distance   to obstacles, which corresponds to the approaching robot's speed to the obstacles, and (  ): shape function that represents the relationship between the virtual external force and distance to the obstacles; the shape function is 0 when   ≥  max and is 1 when   = 0.
The shape function of the distance in Figure 3 is designed as follows:

𝑠 (𝑑
where , : design parameters for defining the shape function of the virtual external force and  max : maximum distance that can be measured by the sensor.The virtual external force/torque increases as the distance of the obstacle decreases.The virtual external force will be zero when the distance of the robot from the obstacle is greater than  max , which is a design parameter according to the sensor specification.

PI Controller Design and Dynamics for Simulation.
The PI controller is employed to make the robots follow the desired trajectory.In this study, the PI controller is employed by calculating the difference between the desired and actual velocities.Consider The reference velocities ( θ  , θ  ) for the right and left wheels are calculated by where θ  , θ  : reference velocities for the right and left wheels, V  , φ  : desired translational and angular velocity,   : proportional gain, and   : integral gain.
The PI control system design for simulation is presented in Appendix A.

Stability Analysisy.
This study considers the case where the robot is situated inside four walls space as shown in Figure 4; the objective of this arrangement is to investigate the effect of the walls to the robot in order to confirm the stability of the robot system.The distance from the sensors to the walls in Figure 4 is given by where , : distance from the mobile robot to the wall,   ,   : centre position of the robot, and   : robot orientation.The shape function in ( 9) is approximated as follows: where   and   are constants for stability analysis and given by where   and   are constants for stability analysis and given by In a similar manner, we have where  <  <  max and  <  <  max are assumed.
Considering the dynamics in (1) and related equations, we have where V 0 is the velocity of the robot.Substituting ( 14), ( 17), (19), and ( 20) into ( 21) results in By notating that we have the following dynamics from (22): From Figure 4, we have where   is the orientation of robot in Figure 4 and considered to have small magnitude to analyse the stability in a typical linear manner.
Notating that  = [  φ    ]  , we have where   is a matrix derived from (24) as follows: The determinant det( −   ) is given by Because   is positive, if the conditions below are satisfied, then the system in ( 26) is stable [25].Next, we consider the translational motion of mobile robots.Considering (2) and related equations, we have min ( ḋ , 0)) .

Simulation Results
Computer simulations were performed to verify the effectiveness of the proposed method.Figure 5 shows the initial condition of the human living environment in which several static and dynamic objects exist.It includes tables low enough for the rehabilitee to step over, although these tables are considered to be static obstacles for robots.The passing humans are considered to be the dynamic obstacles that have to be avoided by the robots.This simulation applies the Kinect sensor to enable the robot to "see" static and dynamic obstacles.Figure 6 shows the simulation result of the Kinect sensor application.The simulation was conducted by taking the distance data input from real Kinect sensor detecting approaching human.The distance data is also compared with that from the proximity sensors.The application of Kinect sensor in Figure 6 shows that when the passing human approaches the robots and the distance from the robot to the human is smaller than the allowed distance, the robots stop when the dwelling period reaches approximately 1500 s.
Figure 7 shows the computer simulation screenshot, and Figure 8 shows the simulation results that demonstrate the effectiveness of the proposed system.Figure 7(a) shows the rehabilitee moving between tables.Figure 7(b) shows the environment in which the rehabilitee moves randomly.
In Figure 8, the green, red, and blue lines are trajectories of the rehabilitee, leader, and follower, respectively.In Figure 8(a), when the rehabilitee walks between tables, some ripples occur for obstacle avoidances.Figure 8(b) shows the result in random setup, in which more ripples are shown.In all these cases, the swarm robots successfully follow the rehabilitee while avoiding obstacles.

Conclusion
This study presents the design of a collision avoidance control system for swarm robots moving in an environment that includes moving obstacles.The swarm robots follow the rehabilitee to provide support in performing his/her tasks in a dynamic environment.This study applies a reference and PI controller.The reference controller creates the reference trajectory for the PI controller based on the fused sensor information obtained from the Kinect, proximity sensors and RFID system.The obstacle avoidance trajectory is generated by the reference controller, and the stability of the overall system is analytically verified.Various computer simulations are performed to verify the effectiveness of the proposed method.The rehabilitee was successfully followed by the swarm robots in all situations.

A. PI Control System Design for Simulation
We apply the PI controller to (4), and the following dynamics are obtained: where θ  and θ  are reference velocities for right and left wheels.
where   is the integral of the tracking error given by  Then, we have Defining the state vector by  = [V φ     ]  , input vector by  = [V  φ  ]  , and output vector by  = [    ]  in (A.8), we employed the following linear dynamics for simulation: where (A.10)

B. Computer Simulation Screenshot and Simulation Results
Figure 9 shows several environment setups used in this study.Figures 9(a) and 9(b) show the environments without obstacles in which the rehabilitee moves around the rooms and the robots follow him/her.Figure 9(c) shows the rehabilitee moving between tables.Figures 9(d) and 9(e) show environments in which the rehabilitee steps over the tables and robots avoid them while still following him/her.Figures 9(f), 9(g), and 9(h) show the environment in which the rehabilitee moves randomly.
Figure 10 shows simulation results of complete environment setup in Figure 10 where the green, red, and blue lines are trajectories of the rehabilitee, leader, and follower, respectively.Figures 10(a

Figure 2 :
Figure 2: Block diagram of proposed control system.

Figure 3 :
Figure 3: Shape function of the distance.

Figure 6 :
Figure 6: Simulation result of the Kinect sensor application.

Figure 7 :
Figure 7: Computer simulation screenshot assuming several static and dynamic obstacles in the human living environment.

Figure 8 :
Figure 8: Computer simulation results of generated trajectories.
Figure9shows several environment setups used in this study.Figures9(a) and 9(b) show the environments without obstacles in which the rehabilitee moves around the rooms and the robots follow him/her.Figure9(c) shows the rehabilitee moving between tables.Figures9(d) and 9(e) show environments in which the rehabilitee steps over the tables and robots avoid them while still following him/her.Figures 9(f), 9(g), and 9(h) show the environment in which the rehabilitee moves randomly.Figure10shows simulation results of complete environment setup in Figure10where the green, red, and blue lines are trajectories of the rehabilitee, leader, and follower, respectively.Figures10(a) and 10(b) show the setup where

Figure 9 :
Figure 9: Environment setups in this study to verify the effectiveness.