Real-Time Obstacle Avoidance for a Swarm of Autonomous Mobile Robots

In this paper, we propose a computational trajectory generation algorithm for swarm mobile robots using local information in a dynamic environment. The algorithm plans a reference path based on constrained convex nonlinear optimization which avoids both static and dynamic obstacles. This algorithm is combined with one-step-ahead predictive control for a swarm of mobile robots to track the generated paths and reach the goals without collision. The numerical simulations and experimental results demonstrate the effectiveness of the proposed free-collision path planning algorithm.


Introduction
Motions planning of multiple mobile robots has been the subject of considerable research effort over recent years (see the references [10,25,31] and the references therein).In fact, a set of mobile robots can perform some tasks more efficiently and robustly than an individual robot.Moreover, the ability of mobile robots to navigate safely and avoid static and dynamic obstacles is necessary for many real world applications.Collision-free Paths Planning Methods (PPMs) require, in dynamic environments, dynamic collision avoidance algorithms.PPMs are more difficult to use in dynamic as opposed to static environments.We refer the reader to [12] for various complex algorithms proposed to solve this problem.In addition to their applications to mobile robot navigation, collision-free PPMs have been used successfully in fairly varied areas such as animation and graphics, intelligent transportation, and aerospace engineering.Collision-free PPMs are heavily based on a collision detection system dealing with the problem of checking whether or not two objects overlap in space.
Many methods have been proposed in the literature dealing with obstacle avoidance for robotics (see for instance the excellent book [17] and the references therein).The well known methods among them are the so-called Reactive Approaches, which have been used successfully for many years.They have been proposed for path generation for a single mobile robot (see for instance [12]).They are also known as local planning algorithms (LPA) [17].During the last two decades, many authors extended some of them to multiple mobile robots (see for instance [3]).Among these approaches, we find the potential field approach [15,16], the vector filed histogram [4,10], the curvature method [27], and the dynamic window approach [11].The main advantage of reactive methods is the fact that they are suitable for real-time navigation in unknown environments.These approaches use only sensor information (local navigation algorithm).However, the major drawbacks of these methods are: 1-the local minima where the robots are trapped; 2-the smoothness of the robot motion is not ensured.
In global planning algorithms, the information process of the environment is acquired partially before starting to use PPMs.Some of them have been developed and applied to mobile robot navigation in indoor or outdoor partially-known environments.A partially-known environment means that the position of walls or the topology of the scene (halls, walls and corridors) are known beforehand.We state and discuss in the following lines some existing papers [1,14,18,24] that have studied situations with partially-known environments.In [1], the authors aimed to develop a control algorithm to command and manage a swarm of mobile robots for transshipment in harbors, airports and marshalling yards.They have designed an environment model of the scene where the algorithm has been implemented.The model contains a topological and geometrical representation of the environment where walls, routes and crossing are composed of cells.The known or unknown (static and dynamic) obstacles have also been taken into account online by the proposed scheme for avoidance capabilities.In [14], the authors have designed an autonomous mobile robot to clean floors for building maintenance where the environment is known a priori.The authors in [24] have realized a semi-autonomous navigation module for a robot wheelchair.The user of this wheelchair can execute the local manoeuvres algorithm in a narrow and cluttered space, as well as in a cluttered wide area.A climbing mobile robot has been designed in [18] for automatic wall inspections.The most important features to be inspected by the proposed mobile robot [18] are the rate of collision, detection of leaks, cracks and crumbling parts.These kinds of inspections are useful especially for the structure of buildings and in general for the safety of the environment.The topology of the space to be inspected is known in advance and the robot is able to navigate along different directions of the wall while avoiding different obstacles due to the structure of the building.
Furthermore, many authors have used collision-free PPMs to control multiple robots to maintain a given formation during the movement (see for instance [2,7,19].In [19], the authors combined a collision-free PPM with a back-stepping controller and applied it to a dynamic model of a multiple robot navigation function to a team of holonomic and nonholonomic mobile robots.However, they assumed that the environment is known and stationary.The work in [2] proposed an obstacle avoidance algorithm based on fast speed generation in dynamic environments.If the generated speed does not belong to a required interval, then a set of acceleration (or deceleration) of robot's movement is provided.These adjustable accelerations represent the collision-free trajectories for the mobile robot.The algorithm has high computational complexity.In [7], graph theory is used to generate the collision-free path for each robot.
In our present work, an online path planning algorithm for multiple mobile robots is obtained using convex optimization with inequality constraints.The path of each robot is planned based on the position of static and dynamic obstacles in the neighborhood.Each robot considers the other robots as moving obstacles.Note that the proposed algorithm can be used for centralized control systems or decentralized control systems (distributed control systems).Knowing that centralized control systems solve a single optimization algorithm with constraints for the entire team, this will require significant computation according to the size of the team.For the distributed control of multiple mobile robots, each robot's behaviour is affected by its neighbour's actions.The proposed algorithm performs an online path planning with global convergence and is applied to nonholonomic wheeled mobile robots.Our main aim in this work is to remove the major drawbacks of the LPA (local planning algorithms), namely, the local minima problem and the smoothness of robot motion.Using a convex constrained minimization problem, we do not encounter the local minima problem, and the special structure of our non-overlapping scheme allows us to prove mathematically the smoothness of the robot motion.
The paper is organized as follows.Section 2 is devoted to the mathematical framework, namely, the description of the non-overlapping scheme.In Section 3, we prove the smoothness of the robot motion.Section 4 contains an application to swarm mobile robots of the abstract scheme described in Section 3. The numerical simulations and experimental results are gathered in Section 5. A brief conclusion ends the paper.

Collision-free path planning
In this section we present an algorithm allowing us to handle contacts between wheeled mobile robots and obstacles.For more applications of this algorithm to real life problems we refer to [20,21].
Consider N 1 mobile robots (or agents) and N 2 obstacles (static and/or mobile) with arbitrary forms.Robots and obstacles are immersed in rigid disks with different radii The safety distance is denoted by δ i > 0 as Figure 1 shows.
In the case of circular mobile robots (or obstacles), the safety distance is: δ i = R i − r i , where r i is the radius of the mobile robot (or obstacle).Note that a robot or an obstacle i having a general shape (see Figure 1-b) is first immersed  in a disk with radius r i and this disk is then immersed in a safety disk with radius R i .So, the safety distance in this case is also given by δ The purpose is to simulate the motion of the mobile robots inside a given domain Ω in R 2 during an interval time [0, T] and subject to the static and/or dynamic obstacles.
To do that, we consider the configuration vector q = (q 1 , q 2 , • • • , q N ) ∈ R 2N (N ∈ N) where q i is the centre of the safety disk i with coordinates x q i y q i .To ensure the avoidance of collision of robots and obstacles, we define the admissible configuration set: where is the distance between the safety disks i and j.From a mathematical point of view, collision avoidance between safety disks i and j at time t means the non-negativity of the distance D ij (t).Thus, starting from an admissible configuration q(t) at time t, we want to ensure the admissibility of the configuration vector q(t + h) after a small value of unit of time h > 0. For this aim, we define the set of admissible velocities ensuring the non-overlapping of safety disks during all the time interval [0, T]: 2) where ∇D ij (q) is the gradient of the distance function D ij (q).Indeed, if we are given an admissible configuration Using the first order Taylor expansion, we can write By taking the real velocity q(t n ) at that time t n belonging to C h (q n ), we can write and so by (2.3) we obtain Let U(q) be the desired velocity of all mobile robots and obstacles.For the static obstacle the desired velocity is always assumed to be zero.To avoid the overlapping of all the safety disks which ensures the free-collision of robots and obstacles, we propose the following scheme: Given an admissible configuration q n ∈ Q at time t n , the next configuration q n+1 will be taken as follows: where W n+1 is the solution of the convex constrained optimization: . The scheme (2.4) can be interpreted as follows: W n+1 is the closed direction to the desired velocity ϑ(q n ) in the set of admissible velocities C h (q n ) ensuring the avoidance of collision of robots and obstacles.
The minimization problem P n can be solved by any suitable solver for the computation of the next direction.In our present work we use the previous scheme to generate free-collision paths to move mobile robots from a starting position to admissible configurations until the robots reach all their targets without collision.

Smoothness of the Robots' Motion
The main characterizations of the algorithm defined in the previous section, which distinguish it from other methods, are: 1-the smoothness of the motion of each robot, that is, the trajectory generated by the algorithm is continuous, and 2-no collision between the safety disks of robots and obstacles.Indeed, if we are given a time horizon T, we fix N ∈ N and we divide the interval time I := [0, T] into N subintervals with equal length h = T N .Clearly h → 0 as N → +∞.Define t n := nh, I n := [t n , t n+1 ), ∀n = 0, • • • , N − 1 and I N := {T}.First we specialize the above algorithm generating the velocities W n+1 , to the present situation as follows: , where, Here Proj S (x) stands for the metric projection of the point x ∈ S on the closed set S, that is, u = Proj S (x) provided that u ∈ S and u − x = inf s∈S s − x .
The algorithm (P n ) is well defined due to the closedness and convexity of the set C h (q n ).Using the sequence of points of configurations (q n ) 0≤n≤N−1 , we define a sequence of affine functions (q N ) N≥1 passing through those points as follows: Clearly, q N is defined over and d dt q N (t) = W n+1 almost everywhere over I.We have to prove the uniform convergence of the sequence q N as N → ∞ to some continuous trajectory q which satisfies the non-overlapping of the disks over all the time interval I. To do that, we define the two following functions θ N and ρ N as follows: θ N (t) = t n and ρ N (t) = t n+1 for all t ∈ I n .In addition, we define the set Observing that K(q n ) = q n + hC h (q n ), we can check by simple computations that (P n ) is equivalent to which can be rewritten as In order to prove the convergence of the sequence of functions (q N ) N , we rewrite the previous equality in terms of the normal cone in the sense of convex analysis, that is, with q N (0 Here N(S; u) is the normal cone associated with the closed convex set S at u ∈ S, defined in terms of the projection as follows: The above inclusion 3.2 is called the Sweeping Process with Perturbation introduced and studied by Moreau [22] and extended in different ways by many authors (see for instance [5,6,9] and the references therein).Using the techniques developed in [6,9], the author in [30] proved in Theorem 4.5, the uniform convergence of the sequence q N satisfying (3.2) to a unique absolutely continuous function q such that with q(0) ∈ Q 0 , q(t) ∈ K(q(t)), ∀t ∈ I.The author in [30] also proved in Proposition 4.7 that q(t) ∈ Q 0 , ∀t ∈ I.This proves that the limit trajectory q satisfies our requests, that is, q is continuous and is admissible in Q 0 during the time interval I, which means that the motion of the robots will be smooth and with collision-free trajectories.

Application to Swarm Mobile Robots
In this section we present a real life application of the numerical scheme (2.4) described previously to path tracking of the mobile robots.

Description of Wheeled Mobile Control (WMR)
The mobile robot used in this work is a simple unicycle mobile robot type shown in Figure 3.
The kinematic model for i th WMR is (see for instance [23,26]): where xi ȳi are the coordinates of the robot's reference point in the Cartesian plane, the angle θ i is the orientation of the robot with respect to the positive x-axis, v i and w i are the linear and angular velocities.A configuration of the robot i is given by: 1 where S 1 is the unit circle in the plane.
The commands of the i th differential drive mobile robot are the velocities of the right and the left wheels , rather than the linear speed v i and the steering angle ω i .The relations between these velocities are: and , where L is the distance between two wheels.The kinematic model (4.1) of the WMR becomes: 2) The kinematic model presents the nonholonomic constraint which is given by: ẋi sin θ i − ẏi cos θ i = 0.
This constraint means that the robot path is tangent to the main axis of the robot.
It is known that in order to design a trajectory tracking controller via feedback a coordinates transformation is used to overcome the noncontrollability of the nonlinear model (4.1).This transformation is given by (see for instance [26]):  with the effective radius of (r i + d) and δ i is the safety distance of the robot (see the Figure 4).
The derivatives of the new model of the WMR are: The nonlinear model of the mobile robot can be written under matrix form as: where In this work, we consider the problem of tracking a reference trajectory given by the equation: for the i th mobile robot.The reference trajectory q i,re f = x q i,re f y q i,re f is deduced from the collision-free path planning discussed previously and θ i,re f is computed by: θ i,re f = tan −1 ( ).

Description of Tracking Control Method (TCM) ([13]).
The tracking control problem is to find for the i th robot a suitable control vector U i (Z i ; t) to drive the mobile robot (4.4) to track precisely the desired trajectory (4.5).
The goal of the tracking control problem of the i th mobile robot is to minimize the cost function given by: where e i (t) = Z i (t) − Z i,re f (t) is the tracking error, Q is a positive definite matrix, R is a positive semi-definite matrix and h is the prediction horizon.The desired trajectory is specified by a smooth vector function Z i,re f (t) for t ∈ [0, T].Note that T is the final time needed for all the robots to reach all the targets.The problem comprises elaborating a control law U i (Z i , t) that improves tracking accuracy at the step t + h, where h > 0 is a prediction horizon, such that Z i (t + h) tracks Z i,re f (t + h).That is, the tracking error is defined by: A simple and effective way to predict the influence of U i (t) on e i (t + h) is to write its first order Taylor expansion as follows: In order to find the current control U i (t) that improves tracking error at the next step and to avoid the computational burden, the expression of the above-predicted tracking error is used in the objective function (4.6).Therefore, the unique control signal U * i that minimizes J i , obtained by setting ∂Ji ∂U i = 0, is given by the following equation Tracking performance : For simplicity, let R = 0, equation (4.8) becomes: and we can show easily that the mobile robot model with the feedback control (4.9) leads to: The error dynamics (4.10) is linear and time-invariant.Thus, the proposed controller that minimizes the one step ahead predicted tracking error naturally leads to a special case of input/state linearization.The advantage of this controller with regard to the linearization method is a clear physical meaning of maximum and minimum when saturation occurs.Note that the tracking error dynamic (4.10) is stable for any h > 0.

Simulation Results
This section presents the results of the numerical simulations using MATLAB to show the application of the collision-free path planning algorithm described in Section 2 with a one-step-ahead predictive controller.To verify the effectiveness of the proposed algorithm, we have set up a simulation with two robots and different static obstacles.The robots are represented as small filled disks (Rob. 1 as black and Rob. 2 as blue).
The objective is to drive each mobile robot (Rob. 1 and Rob. 2) towards its corresponding targets (Targ.1 and Targ.2) without collision with the obstacles and the other robot.Each robot uses the reference trajectory induced from the collision-free path planning algorithm (2.4) and generates the control vector U i to be applied to the robot i using the one-step-ahead predictive control.
We have two different situations in our numerical simulations.The first one is the case of obstacles included in safety disks and without walls.The second one is the case of obstacles as walls.

Case1: Without walls
The initial configuration q 0 is q 0 = (q 0 1 , q 0 2 , q 0 obs1 , q 0 obs2 , q 0 obs3 ), where q 0 1 = 0 0 is the initial position of Rob.1; is the initial position of Rob.2; is the position of the obstacle Obs.1; is the position of the obstacle Obs.2; is the position of the obstacle Obs.3; The position of the targets are: is the position of the target Targ.1; is the position of the target Targ.2.
The radii used in the simulations are: r 1 = r 2 = 0.2; r obs1 = r obs2 = 0.7; r obs3 = 1.2; δ 1 = δ 2 = 0.05; d = 0.05.The first graph in Figure 5 shows the tracking performance of the proposed algorithm in mid-path.Two robots, which are represented by two small disks, are going towards their target and avoid the first obstacle in their path.
The second graph in Figure 5 shows the tracking performance of the proposed algorithm.Two robots reach the targets without colliding neither with the last obstacle nor with each other.Indeed, at the end of the path, Rob. 1 encounters two obstacles: static obstacle which is the third obstacle and the dynamic obstacle which is Rob. 2. Additionally, the robot Rob. 2 meets two obstacles: a static obstacle which is the third obstacle and a dynamic obstacle which is Rob. 1.The figure shows clearly how robots Rob. 1 and Rob. 2 avoid each other and how they avoid the third obstacle during their way to the targets respectively.
The three graphs in Figure 6 show the case of two mobile robots with two static obstacles.These two mobile robots meet at the mid-path and the graphs show clearly how they avoid collision.Note that Rob. 1 has slowed down its speed letting Rob. 2 follow its path towards Targ.2.

Case2: Walls as obstacles
The environment in this simulation contains two parallel walls as Figure 7 shows.The initial configuration q 0 is , where is the initial position of Rob.1; is the initial position of Rob. 2.
The position of the targets are: q targ 1 = 90 90 and q targ 2 = 10 10 The radii used in the simulations are: Each robot reaches its corresponding target without collision neither with the walls and nor with the other robot (see Figure 7).We also have to point out the smoothness of the motion of both robots due to our proposed algorithm.

Experimental Results
In this subsection, we focus on the implementation of our algorithm to the motion of three mobile robots.The experiments have been conducted using TOSHIBA laptops (Qosmio) with Intel Core i7 processors.MATLAB software has been used with a Bluetooth communication system.The platform used in this work is Khepera III 8)(made by K-TEAM [32]), which is a differential drive mobile robot with a circular shape 12 cm in diameter.It senses its environment using five ultrasonic sensors and 11 IR sensors (see Figure 9).In our lab experiments, we used only the eight IR sensors corresponding to labels 0 to 7 in Figure 9.Each IR sensor has a range of measurements from 5 mm to 200 mm.To compute the distance between  the robot i and the existing obstacles, we first read all the sensors for robot i, IR s i s = 0, . . ., 7, and we take the maximum over all the sensors, IR i := max{IR s i , s = 0, . . ., 7}.
If IR i is greater than a given threshold, we deduce the presence of at least one obstacle.For a detected obstacle j we associate a reading number IR i,j which is the reading number of the sensor detecting that obstacle.
Once the values IR i,j are collected, we convert them to distances via the function f (IR i,j ) = α IR i,j +β , for IR i,j ∈ [100, 4000], where α = 10 5 , β = 595, and f (IR i,j ) is the distance measured in millimetres.We point out that the function f is the simplest function approximating various data collected during the experimental measurements of distances and reading numbers.
The implementation of the algorithm is conducted on three different laptops which are connected to three Khepera robots via Bluetooth.The proposed navigation algorithm needs, for its implementation, the (x, y)-position relative to the world frame of all obstacles (both mobile and static).

We denote by
x obs j y obs j the (x, y)-position for the obstacle j.Using the well-known homogeneous transformation matrix (see for instance [26]), we derive the values of x obs j y obs j from the following formulas: Here H(θ i , x i , y i ) is the homogeneous transformation matrix given by where θ i is the orientation of Robot i, x i y i is the (x, y)-position of Robot i, r is the radius of the Robot i, d is the distance between the center of the axis connecting the two wheels and the center of the robot (see Figure 10), and α i,j is the angle between the sagittal axis of robot i and the line passing through sensor j.
The implemented algorithm used in our lab experiments is described as follows: Algorithm: Real-time obstacle avoidance for Robot i 1. Input constant data: 1. Set the direction to the target as: .  function to estimate the position of these surrounding obstacles.Therefore, the time processing of the algorithm increases to 1.8 seconds instead of 0.3233 seconds.We point out that the processing time for our algorithm in all the implementations is relatively high which is due to the use of MATLAB under Windows 8, which is known for its slowness in the implementation relative to other softwares such as C ++ .

Conclusion
This paper presents a collision-free path planning using nonlinear optimization combined with one-step-ahead predictive controller.The proposed collision-free path planning algorithm is an online path planning with a global minimum.The simulations and experimental results showed the effectiveness of this algorithm, namely, overcoming the local minima problem and smoothness of robot motion.Indeed, it was desired (in both simulations and lab experiment) to control the mobile robots smoothly to their desired targets without collisions between them and avoiding the static obstacles.
The proposed algorithm can be used efficiently in many applications where the environment is partially known.For instance, in building for maintenance and inspection, hospitals and warehouses for material handling.In this case, the direction or velocity of each mobile robot can be determined precisely in a given region.The algorithm can provide the optimal velocity to move the mobile robot from the current position to the next free position without colliding with walls or with any other obstacles (static or dynamic).For a completely unknown environment, the sensors are used to estimate the position of the obstacles which will be inserted in our algorithm to determine the next free position in the scene.Therefore, the optimal velocity can be calculated using the proposed scheme to navigate the mobile robot to the next position from the current one.
In the simulations, good tracking performances and navigation without collision have been obtained in the presence of the obstacles.
However, in this case the positions of obstacles are known exactly and we therefore have no need for a localization algorithm.For the experimental part, to perform the proposed algorithm, localization and position estimation of obstacles algorithms have been included.The tracking performances and navigation of the wheeled mobile robot, in an unknown environment with static and dynamic obstacles, are good and robust against the uncertainties induced by the infrared sensors.

Figure 1 .
Figure 1.Mobile robot representation inside the safety disk

Figure 2 .
Figure 2. Obstacle representation inside the safety disk

) with d = 0 .
These coordinates represent the Cartesian coordinates of a point located along the axis of the WMR at a distance |d| from the center of the line connecting the contact-point of the wheels with the ground.Hence, the center C xi ȳi of the mobile robot is shifted along the sagittal axel by d units.The new (effective) center of WMR

Figure 4 .
Figure 4.The effective center and the effective radius of the safety disk is Px i y i with the effective radius of (r i + d) and δ i is the

Figure 5 .
Figure 5. Mid-path planning and obstacle avoidance of two mobile robots

Figure 6 .
Figure 6.Navigation performance of two-WBR: 1-meeting of two mobile robots at the mid-path.2-Obstacle avoidance performance.3-Arriving at the targets.

Figure 7 .
Figure 7. Tracking performance of two mobile robots with wall obstacles

Figure 10 .
Figure 10.Detection of Obstacle j by Robot i