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

Abstract

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.

Keywords

Obstacle Avoidance Smooth Motion Swarm Mobile Robots Sweeping Process Predictive Control

1. 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.

2. 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₁ mobile robots (or agents) and N₂ obstacles (static and/or mobile) with arbitrary forms. Robots and obstacles are immersed in rigid disks with different radii R_i, i = 1, ···, N₁ + N₂. The safety distance is denoted by δ_i > 0 as Figure 1 shows.

Figure 1.

Mobile robot representation inside the safety disk

Figure 2.

Obstacle representation inside the safety disk

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 δ_i = R_i − r_i.

The purpose is to simulate the motion of the mobile robots inside a given domain Ω in ℝ² 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₁, q₂, ···, q_N) ∊ ℝ^2N (N ∊ ℕ) where q_i is the centre of the safety disk i with coordinates ( $(\begin{matrix} x_{q_{i}} \\ y_{q_{i}} \end{matrix})$ ). To ensure the avoidance of collision of robots and obstacles, we define the admissible configuration set:

Q = {q ∊ ℝ^{2 N} : D_{i j} (q) \geq 0, \forall i, j},

(2.1)

where D_ij(q) = ‖q_i − q_j‖ − (R_i + R_j) 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]:

C_{h} (q) = {v ∊ ℝ^{2 N} : D_{i j} (q) + h \nabla D_{i j} (q) \cdot v \geq 0, \forall i < j},

(2.2)

where ∇D_ij(q) is the gradient of the distance function D_ij(q). Indeed, if we are given an admissible configuration q_n at time t_n ∊ [0, T], that is, q_n := q(t_n) ∊ Q. The next configuration q_n+1 after h unit of time is q_n+1 = q(t_n + h). Using the first order Taylor expansion, we can write

D_{i j} (q (t_{n} + h)) = D_{i j} (q (t_{n})) + h \nabla D_{i j} (q (t_{n})) \cdot \dot{q} (t_{n}) .

(2.3)

By taking the real velocity $\dot{q} (t_{n})$ at that time t_n belonging to C_h(q_n), we can write

D_{i j} (q_{n}) + h \nabla D_{i j} (q_{n}) \cdot \dot{q} (t_{n}) \geq 0,

and so by (2.3) we obtain D_ij(q(t_n + h)) ≥ 0, that is, q(t_n + h) ∊ Q.

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:

q_{n + 1} = q_{n} + h W^{n + 1},

(2.4)

where Wⁿ⁺¹ is the solution of the convex constrained optimization:

(P_{n}) {\begin{cases} Min {‖ V - ϑ (q_{n}) ‖}^{2} \\ subject t o V ∊ C_{h} (q_{n}), \end{cases}

that is, ‖Wⁿ⁺¹ − ϑ(q_n)‖ = d_Ch(qn)(ϑ(q_n)). The scheme (2.4) can be interpreted as follows: Wⁿ⁺¹ 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.

3. 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 ∊ ℕ and we divide the interval time I := [0, T] into N subintervals with equal length $h = \frac{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ⁿ⁺¹, to the present situation as follows:

$q_{0} ∊ Q_{0}, \forall n = 0, \dots N - 1, q_{n + 1} : = q_{n} + h W^{n + 1}$ , where,

(P_{n}) W^{n + 1} : = P r o j_{C_{h} (q_{n})} (ϑ (q_{n})) .

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:

q^{N} (t) : = q_{n} + (t - t_{n}) W^{n + 1}, \forall t ∊ I_{n} .

Clearly, q^N is defined over I, $q^{N} (t_{n}) = q_{n} = \lim_{t \to t_{n}} q^{N} (t)$ and $\frac{d}{d t} 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

K (q) = {\tilde{q} ∊ ℝ^{2 N} : D_{i j} (q) + \nabla D_{i j} (q) \cdot (\tilde{q} - q) \geq 0, \forall i < j} .

(3.1)

Observing that K(q_n) = q_n + hC_h (q_n), we can check by simple computations that (P_n) is equivalent to

q_{n + 1} = P r o j_{K (q_{n})} (q_{n} + h ϑ (q_{n}))

which can be rewritten as

q^{N} (ρ_{N} (t)) = P r o j_{K (q^{N} (θ_{N} (t)))} (q^{N} (θ_{N} (t)) + h ϑ (q^{N} (θ_{N} (t)))), \forall t ∊ I .

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,

\frac{d}{d t} q^{N} (t) ∊ - N (K (q^{N} (θ_{N} (t))); q^{N} (ρ_{N} (t))) + ϑ (q^{N} (θ_{N} (t))), a . e . t ∊ I,

(3.2)

with q^N(0) = q₀ ∊ K(q₀) = Q₀, q^N(ρ_N(t)) ∊ K(q^N(θ_N (t))),∀t ∊ I. 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:

N (S; u) = {ξ : \forall α > 0 such t h a t u = p r o j_{S} (u + α ξ)} .

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

\frac{d}{d t} q (t) ∊ - N (K (q (t)); q (t)) + ϑ (q (t)), a . e . t ∊ I

(3.3)

with q(0) ∊ Q₀, q(t) ∊ K(q(t)), ∀t ∊ I. The author in [30] also proved in Proposition 4.7 that q(t) ∊ Q₀, ∀t ∊ I. This proves that the limit trajectory q satisfies our requests, that is, q is continuous and is admissible in Q₀ during the time interval I, which means that the motion of the robots will be smooth and with collision-free trajectories.

4. 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.

4.1 Description of Wheeled Mobile Control (WMR)

The mobile robot used in this work is a simple unicycle mobile robot type shown in Figure 3.

Figure 3.

Wheeled mobile robot

The kinematic model for i^th WMR is (see for instance[23, 26]):

{\begin{array}{l} {\dot{\overline{x}}}_{i} = v_{i} \cos (θ_{i}) \\ {\dot{\overline{y}}}_{i} = v_{i} \sin (θ_{i}) \\ {\dot{θ}}_{i} = w_{i}, \end{array}

(4.1)

where $(\begin{matrix} {\bar{x}}_{i} \\ {\bar{y}}_{i} \end{matrix})$ 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: $Z_{i} = (\begin{matrix} {\bar{x}}_{i} \\ {\bar{y}}_{i} \\ θ_{i} \end{matrix}) ∊ ℝ^{2} \times S^{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 $(\begin{matrix} v_{i}^{L} \\ v_{i}^{R} \end{matrix})$ , rather than the linear speed v_i and the steering angle ω_i. The relations between these velocities are:

v_{i} = \frac{1}{2} (v_{i}^{L} + v_{i}^{R})

and

ω_{i} = \frac{1}{L} (v_{i}^{L} - v_{i}^{R}),

where L is the distance between two wheels. The kinematic model (4.1) of the WMR becomes:

{\begin{array}{l} {\dot{\overline{x}}}_{i} = \frac{1}{2} (v_{i}^{L} + v_{i}^{R}) \cos (θ_{i}); \\ {\dot{\overline{y}}}_{i} = \frac{1}{2} (v_{i}^{L} + v_{i}^{R}) \sin (θ_{i}); \\ {\dot{θ}}_{i} = \frac{1}{L} (v_{i}^{L} - v_{i}^{R}) . \end{array}

(4.2)

The kinematic model presents the nonholonomic constraint which is given by:

{\dot{\overline{x}}}_{i} \sin θ_{i} - {\dot{\overline{y}}}_{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]):

{\begin{array}{l} x_{i} = {\overline{x}}_{i} + d \cos (θ_{i}) \\ y_{i} = {\overline{y}}_{i} + d \sin (θ_{i}), \end{array}

(4.3)

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 (\begin{matrix} {\bar{x}}_{i} \\ {\bar{y}}_{i} \end{matrix})$ of the mobile robot is shifted alone the sagittal axel by d units. The new (effective) center of WMR is $P (\begin{matrix} x_{i} \\ y_{i} \end{matrix})$ with the effective radius of (r_i + d) and δ_i is the safety distance of the robot (see the Figure 4).

Figure 4.

The effective center and the effective radius of the safety disk

The derivatives of the new model of the WMR are:

{\begin{cases} {\dot{x}}_{i} = v_{i} \cos (θ_{i}) - d w_{i} \sin (θ_{i}) \\ {\dot{y}}_{i} = v_{i} \sin (θ_{i}) + d w_{i} \cos (θ_{i}) . \end{cases}

The nonlinear model of the mobile robot can be written under matrix form as:

{\dot{Z}}_{i} = G (θ_{i}) . U_{i},

(4.4)

where

Z_{i} = (\begin{matrix} x_{i} \\ y_{i} \\ θ_{i} \end{matrix}) and U_{i} = (\begin{matrix} v_{i} \\ w_{i} \end{matrix}),

and

G (θ_{i}) = (\begin{matrix} \cos (θ_{i}) & - d \sin (θ_{i}) \\ \sin (θ_{i}) & d \cos (θ_{i}) \\ 0 & 1 \end{matrix}) .

In this work, we consider the problem of tracking a reference trajectory given by the equation:

Z_{i, r e f} (t) = (\begin{matrix} q_{i, r e f} \\ θ_{i, r e f} \end{matrix})

(4.5)

for the i^th mobile robot. The reference trajectory $q_{i, ref} = (\begin{matrix} x_{q_{i, ref}} \\ y_{q_{i, ref}} \end{matrix})$ is deduced from the collision-free path planning discussed previously and θ_i,ref computed by: $θ_{i, ref} = \tan^{- 1} (\frac{y_{q_{i, ref}}}{x_{q_{i, ref}}})$ .

4.2 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:

J_{i} = \frac{1}{2} {‖ e_{i} (t + h) ‖}_{Q}^{2} + \frac{1}{2} {‖ U_{i} (t) ‖}_{R}^{2};

(4.6)

where e_i(t) = Z_i(t) − Z_i,ref (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,ref(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,ref (t + h). That is, the tracking error is defined by:

e_{i} (t + h) = Z_{i} (t + h) - Z_{i, r e f} (t + h)

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:

e_{i} (t + h) = e_{i} (t) + h (G (θ_{i}) \cdot U_{i} - {\dot{Z}}_{i, r e f})

(4.7)

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 $\frac{\partial J i}{\partial U_{i}} = 0$ , is given by the following equation

U_{i}^{*} = - {[h^{2} G {(θ_{i})}^{T} Q G (θ_{i}) + R]}^{- 1} [h G {(θ_{i})}^{T} Q (e_{i} (t) - h {\dot{Z}}_{i, r e f})] .

(4.8)

Tracking performance: For simplicity, let R = 0, equation (4.8) becomes:

U_{i}^{*} = - \frac{1}{h} {[G {(θ_{i})}^{T} Q G (θ_{i})]}^{- 1} [G {(θ_{i})}^{T} Q (e_{i} (t) - h {\dot{Z}}_{i, r e f})]

(4.9)

and we can show easily that the mobile robot model with the feedback control (4.9) leads to:

{\dot{Z}}_{i} = - \frac{1}{h} e_{i} (t) + {\dot{Z}}_{i, r e f} or {\dot{e}}_{i} (t) + \frac{1}{h} e_{i} (t) = 0.

(4.10)

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.

5. Simulation and Experimental Results

5.1 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.

Case 1: Without walls

The initial configuration q⁰ is

q^{0} = (q_{1}^{0}, q_{2}^{0}, q_{o b s 1}^{0}, q_{o b s 2}^{0}, q_{o b s 3}^{0}),

where

q₁⁰ = $(\begin{matrix} 0 \\ 0 \end{matrix})$ is the initial position of Rob. 1;

q₂⁰ = $(\begin{matrix} 0 \\ 4 \end{matrix})$ is the initial position of Rob. 2;

q_obs1⁰ = $(\begin{matrix} 2 \\ 1 \end{matrix})$ is the position of the obstacle Obs. 1;

q_obs2⁰ = $(\begin{matrix} 2 \\ 3 \end{matrix})$ is the position of the obstacle Obs. 2;

q_obs1⁰ = $(\begin{matrix} 7 \\ 2 \end{matrix})$ is the position of the obstacle Obs. 3;

The position of the targets are:

q_targ1 = $(\begin{matrix} 8.5 \\ 2.5 \end{matrix})$ is the position of the target Targ. 1;

q_targ2 = $(\begin{matrix} 8.5 \\ 1.5 \end{matrix})$ is the position of the target Targ. 2.

The radii used in the simulations are: r₁ = r₂ = 0.2; r_obs1 = r_obs2 = 0.7; r_obs3 = 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.

Figure 5.

Mid-path planning and obstacle avoidance of two mobile robots

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.

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.

Case 2: Walls as obstacles

The environment in this simulation contains two parallel walls as Figure 7 shows. The initial configuration q⁰ is

Figure 7.

Tracking performance of two mobile robots with wall obstacles

q⁰ = $(\begin{matrix} 0 \\ 1 \end{matrix})$ , where

q₁⁰ = $(\begin{matrix} 10 \\ 10 \end{matrix})$ is the initial position of Rob. 1;

q₂⁰ = $(\begin{matrix} 90 \\ 90 \end{matrix})$ is the initial position of Rob. 2.

The position of the targets are:

q_{t a r g_{1}} = (\begin{matrix} 90 \\ 90 \end{matrix}) and q_{t a r g_{2}} = (\begin{matrix} 10 \\ 10 \end{matrix})

The radii used in the simulations are: r₁ = r₂ = 2.5.

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.

5.2 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_i^s s = 0, …, 7, and we take the maximum over all the sensors,

I R_{i} : = \max {I R_{i}^{s}, s = 0, \dots,7} .

Figure 8.

Khepera-III

Figure 9.

Bottom view of the IR sensors in Khepera-III

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 (I R_{i, j}) = \frac{α}{I R_{i, j} + β}$ , for IR_i,j ∊ [100,4000], where α = 10⁵, β = 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 $(\begin{matrix} x_{j}^{obs} \\ y_{j}^{obs} \end{matrix})$ the (x, y)-position for the obstacle j. Using the well-known homogeneous transformation matrix (see for instance [26]), we derive the values of $(\begin{matrix} x_{j}^{obs} \\ y_{j}^{obs} \end{matrix})$ from the following formulas:

x_{j}^{o b s} = {[H (θ_{i}, x_{i}, y_{i}) (\begin{matrix} (r + f (I R_{i, j})) c o s (α_{i, j}) \\ (r + f (I R_{i, j})) s i n (α_{i, j}) \\ 0 \\ 1 \end{matrix})]}^{T} (\begin{matrix} 1 \\ 0 \\ 0 \\ 0 \end{matrix})

and

y_{j}^{o b s} = {[H (θ_{i}, x_{i}, y_{i}) (\begin{matrix} (r + f (I R_{i, j})) c o s (α_{i, j}) \\ (r + f (I R_{i, j})) s i n (α_{i, j}) \\ 0 \\ 1 \end{matrix})]}^{T} (\begin{matrix} 0 \\ 1 \\ 0 \\ 0 \end{matrix}) .

Here H (θ_i, x_i,y_i) is the homogeneous transformation matrix given by

H (θ_{i}, x_{i}, y_{i}) = (\begin{matrix} c o s (θ_{i}) & - s i n (θ_{i}) & 0 & x_{i} + d c o s (θ_{i}) \\ s i n (θ_{i}) & c o s (θ_{i}) & 0 & y_{i} + d s i n (θ_{i}) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix})

where θ_i is the orientation of Robot i, $(\begin{matrix} x_{i} \\ y_{i} \end{matrix})$ 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.

Figure 10.

Detection of Obstacle j by Robot i

The implemented algorithm used in our lab experiments is described as follows:

Algorithm:

Real-time obstacle avoidance for Robot i

We summarize the constants used in all our lab experiments in the following table:

Data	Description	Used Values
β₀	threshold for obstacles detection	400
ε,	tolerance for achieving the target	10 mm
V_max	maximum speed	300 mm/s
ω_max	maximum angular speed	15 rad/s
h	prediction horizon	0.01
Q	weighted matrix see ((4.6))	10³I₃
R	weighted matrix see ((4.6))	10⁻²I₂
L	distance between two wheels	89 mm
r	radius of the robot	60 mm
d	distance as shown in Figure 10	10 mm
δ	safety distance of the robot	20 mm

In our lab experiments we produced three scenarios as follows:

Scenario 1: One mobile robot and a static obstacle. In this scenario we examined the case when the mobile robot started from a given point going to a final target and avoiding a fixed obstacle which is situated in the mid-path as shown in Figure 11. The video showing this scenario is uploaded on YouTube at http://youtu.be/vrlxi7XHlrI.

Scenario 2: Two mobile robots. In this scenario we examined the case when two mobile robots started from two different starting points, going to their final targets, and avoiding each other as shown in Figure 12. The video showing this scenario is uploaded on YouTube at http://youtu.be/38wTGuWkkKI.

Scenario 3: This scenario is more complicated than the previous ones. We put three mobile robots at three different starting points, going to their final targets, and avoiding each other as shown in Figure 13. We point out that the starting points were very close to each other in order to get the more complicated scenario. The video showing this scenario is uploaded on YouTube at http://youtu.be/ZAwln4FlQf0.

Figure 11.

Snapshots for Scenario 1

Figure 12.

Snapshots for Scenario 2

Figure 13.

Snapshots for Scenario 3

The computation time of the algorithm in the real world experiments depends on the presence and on the number of obstacles. Indeed, in the case of no obstacles, the time processing has been evaluated to be 0.3233 seconds. However, in the presence of one mobile obstacle (Scenario 1) the algorithm reads the values of sensors (IR_s, s = 0 to 7), determines the nearest obstacles, and calls an external 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⁺⁺.

6. 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.

Footnotes

7.

The authors extend their appreciations to the Deanship of Scientific Research at King Saud University for funding the work through the research group project No. RGP-VPP-024.

References

Alami

Fleury

Herrb

Ingrand

Robert

(1998) Multi-robot cooperation in the MARTHA project. IEEE Robotics & Automation Magazine March: 36–47.

Bernabeu

E J

(2009) Fast generation of multiple collision-free and linear trajectories in dynamic environments. IEEE Trans. On Robotics 25 No.4: 967–973.

Belkhouche

(2009) Reactive path planning in a dynamic environment. IEEE Trans. on Robotics 25 No.4: 902–911.

Boreistein

Koren

(1991) The vector field histogram fast obstacle avoidance for mobile robots. IEEE Trans. On Robotics and Automation 7 No.1: 278–288.

Bounkhel

(2012) Regularity Concepts in Nonsmooth Analysis, Theory and Applications. Springer Optimization and Its Applications. 59.

Bounkhel

Thibault

(2005) Nonconvex sweeping process and prox-regularity in Hilbert space. J. Nonlinear Convex Anal. 6 No.2: 359–374.

Desai

J P

Ostrowski

J P

Kumar

(2001) Modeling and control of formations of nonholonomic mobile robots. IEEE Trans. On Robotics and Automation 17 No.6: 905–908.

Derenick

J C

Spletzer

J R

(2007) Convex optimization for coordinating large-scale robot formations. IEEE Trans. On Robotics 23 No.6,: 1–8.

Edmond

J F

Thibault

(2006) BV solutions of nonconvex sw eeping process differential inclusion with perturbation. J. Diff. Eqs 226 Issue 1: 135–179.

10.

Fahimi

Notaraj

Ashraiuon

(2009) Real-time obstacle avoidance for multiple mobile robots. Robotica 27: 189–198.

11.

Fox

Burgard

Thrun

(1997) The dynamic window approach to collision avoidance. IEEE Trans. On Robotics and Automation Magazine 4 No.1: 23–33.

12.

S S

Lewis

F L

(2006) Autonomous mobile robots: sensing, control, decision making and applications. Edited by Ge

S S

Lewis

F L

, CRC Press, Traylor & Francis Group.

13.

Hedjar

Alsulaiman

Muteb

(2009) One-step-ahead predictive control of steering mobile robot. IEEE ISIE 2009 - IEEE International Symposium on Industrial Electronics, Seoul, Korea. pp.5–8.

14.

Jones

J L

Mack

N E

Nugent

D M

Sandin

P E

(2005) Autonomous floor cleaning robot, U.S. Patent 6883201.

15.

Khatib

(1986) Real-time obstacle avoidance for manipulators and mobile robots. International Journal of Robotics Research 5 No.1: 90–98.

16.

Khatib

Chatila

(1995) An extended potential field approach for mobile robot sensor-based motions. In Proc. of Intelligent Autonomous Systems, pp. 490–496.

17.

Latombe

(1991) Robotic motion planning. Kluwer Acad. Pub.

18.

Longo

Muscato

(2006) The ALICIA climbing robot: A three-module robot for automatic wall inspection. IEEE Robotics & Automation Magazine March: 42–50.

19.

Loizou

S G

Kyriakopoulos

K J

(2008) Navigation of multiple kinematically constrained robots. IEEE Trans. On Robotics 24 No.1: 221–231.

20.

Maury

(2006) A time-stepping scheme for inelastic collisions. Numerische Mathematik 102 No.4: 649–679.

21.

Maury

Venel

(2011) A discrete contact model for crowd motion. ESAIM: Mathematical Modelling and Numerical Analysis 45: 145–168.

22.

Moreau

J J

(1971) Rafle par un convexe variable (Premiere partie). Sem. d'Anal. Convexe Montpellier expose No.15.

23.

Oriolo

De Luca

Vendittelli

(2002) WMR control via dynamic feedback linearization: Design, implementation, and experimental validation. IEEE Trans. On Control Systems Technology 10 No.6: 835–852.

24.

Prassler

Scholz

Fiorini

(2001) A robotic wheelchair for crowded public environment. IEEE Robotics & Automation Magazine March: 38–45.

25.

Saito

Tsumura

(1991) Collision free motion control of multiple mobile robots on path network. IEEE/RSJ international workshop on intelligent robots and systems IROS'91 Nov. 3–5.

26.

Siciliano

Sciavicco

Villani

Oriolo

(2009) Robotics modeling, planning and control. Springer Verlag.

27.

Simmons

(1996) The curvature-velocity method for local obstacle avoidance. In Proc. of IEEE Int. Conference on Robotics and Automation, pp.3375–3388.

28.

Snape

Van Den Berg

Guy

S J

Manocha

(2010) Smooth and collision-free navigation for multiple robots under differential drive constraints. Univ. N. Carolina Chapel Hill, Tech. Rep. Available at http://gamma.cs.unc.edu/S-ORCA/.

29.

Van der Berg

J P

Overmars

M H

(2005) Road map-based motion planning in dynamic environment. IEEE Trans. on Robotics 21 No.5: 885–897.

30.

Venel

(2008) Modelisation Mathematiques et numerique de mouvements de foule. [PhD Thesis]. Universite Paris XI.

31.

Yamashita

Arai

Ota

Asama

(2003) Motion planning of multiple mobile robots for cooperative Manipulation and Transpportation. IEEE Trans. on Robotics and Automation 19 No.2: 223–237.

32.