4. Application to wheeled mobile robots

#### 4.1 System description

Figure 1 shows the individual robot system considered here. Robot state includes xi <sup>¼</sup> ½ � x y ϕ θ<sup>R</sup> <sup>θ</sup><sup>L</sup> vR vL <sup>T</sup> with both position ð Þ <sup>x</sup>; <sup>y</sup> and orientation ϕ and ð Þ θR; θL; vR; vL as the right and left wheel angular positions and velocities, respectively. Robot input includes the respective wheel torques <sup>u</sup><sup>i</sup> <sup>¼</sup> ½ � <sup>τ</sup><sup>R</sup> <sup>τ</sup><sup>L</sup> <sup>T</sup>. You can have the details of the applied nonlinear dynamic model f <sup>D</sup> xi ð Þ<sup>k</sup> ; <sup>u</sup><sup>i</sup> ð Þk ; t i s ð Þ<sup>k</sup> � � based on the system model developed in [34, 35]. Details of discretization and

Distributed Optimization of Multi-Robot Motion with Time-Energy Criterion DOI: http://dx.doi.org/10.5772/intechopen.85668

The minimizer update φminð Þ: is responsible to solve the single robot optimization (primal) problem according to the objective defined in (22) and subject to constraints in (23). In this paper, the minimizer update φminð Þ: is selected to be any state-of-the-art nonlinear programming (NLP) algorithm. Let us have the step size α for the dual update to be constant. This is sufficient for converging to a solution of the original problem [33]. You can read the algorithm updates in (24) at iteration m as each robot independently optimizes its whole motion throughout the whole time horizon k ¼ 0, 1, …, N while at the same time puts in mind the extra cost of cooper-

Path Planning for Autonomous Vehicles - Ensuring Reliable Driverless Navigation…

In this brief section, elaboration is put forth about how to practically use the algorithm. The ultimate goal is to optimize primal problem with no collision violation, i.e., reaching optimal dual (maximum) solution. At each global iteration, we only need to improve the primal problem values for the updated extra cost of the interaction constraint, λ<sup>i</sup> � �<sup>T</sup> Ω<sup>i</sup> � �. In this paper, a perfect solution is to optimize

of constraints for positive values, i.e., violations. So, a stopping criterion for the algorithm can be chosen to be some minimum change TolJ in the primal problem

�≤TolJ with <sup>J</sup>ð Þ <sup>m</sup> <sup>¼</sup> <sup>∑</sup><sup>L</sup>

We can also distribute the stopping decision to individual robots by observing the

<sup>∀</sup><sup>k</sup> should be less than a relatively small positive value TolΩ. So, for each robot,

∀k � �

Specific values of TolΩ and TolJ depend on the nonlinear programming algorithm and/or the global desired requirements. Note that the behavior of the two

With condition (25) on its own, we cannot always be satisfying the collision requirement. So, this condition can be accompanied by a condition on the collision constraint violation. For all robots, elements of the complete constraint vector

> ð Þ m h i

Figure 1 shows the individual robot system considered here. Robot state

based on the system model developed in [34, 35]. Details of discretization and

can have the details of the applied nonlinear dynamic model f <sup>D</sup> xi

tion ϕ and ð Þ θR; θL; vR; vL as the right and left wheel angular positions and velocities, respectively. Robot input includes the respective wheel torques <sup>u</sup><sup>i</sup> <sup>¼</sup> ½ � <sup>τ</sup><sup>R</sup> <sup>τ</sup><sup>L</sup>

ð Þ m h i<sup>T</sup>

<sup>∀</sup><sup>k</sup> ≤0. A logical property is to monitor the M-element vector

<sup>i</sup>¼<sup>1</sup><sup>J</sup> ui ð Þ m � �

≤TolΩ (26)

<sup>T</sup> with both position ð Þ <sup>x</sup>; <sup>y</sup> and orienta-

ð Þ<sup>k</sup> ; <sup>u</sup><sup>i</sup>

ð Þk ; t i s

ð Þ<sup>k</sup> � �

Ωi ð Þ m h i

and so on.

(25)

<sup>T</sup>. You

ation/interaction with others introduced by the term λ<sup>i</sup>

Jð Þ <sup>m</sup>þ<sup>1</sup> � Jð Þ <sup>m</sup>

� �

an extra stopping criterion along with criteria in (25) is to have

tolerance parameters could be competitive with each other.

4. Application to wheeled mobile robots

includes xi <sup>¼</sup> ½ � x y ϕ θ<sup>R</sup> <sup>θ</sup><sup>L</sup> vR vL

4.1 System description

max Ω<sup>i</sup>

� � �

change in individual objective values.

3.3 Algorithm convergence

while maintaining Ω<sup>i</sup> � �

value:

Ω<sup>i</sup> � �

126

choice of parameters of the robot model can be found in [21]. As mentioned before, for choices of Q,R in Section 1, the Lagrangian for the problem is chosen to include the cost for energy spent by the torques of the wheels, the cost for kinetic energy spent by robot body, and the weight on time. Individual robot constraints include final desired configuration tolerance, torque limits, and the ensurance of zero final velocities (see more details in [36]).

#### 4.2 Collision avoidance

Here, we will discuss the formulation and the structure of the coupling constraints. The robots can be designed to perform any cooperative strategy in their motion. Here, we only consider the global goal of optimizing the motion of each robot in time and energy while avoiding colliding with each other during the motion. Let us define the coupling constraint vector across the discrete time indexes as

$$
\mathfrak{Q}^i(\mathbb{k}) = \mathfrak{Q}^i\left(\left\{\mathfrak{x}^i(\mathbb{k})\right\}\_{\mathbb{N}}, \left\{\mathfrak{u}^i(\mathbb{k})\right\}\_{\mathbb{N}}, \left\{\mathfrak{t}^i\_s(\mathbb{k})\right\}\_{\mathbb{N}}\right).
$$

For the ith robot, it tries to avoid colliding with the rest of L � 1 robots at each of its time indexes <sup>k</sup>. Let us label elements of the constraint vector as <sup>Ω</sup>ijð Þ<sup>k</sup> . Each element is corresponding to a definition of constraint at time index k for all other robots, <sup>j</sup> 6¼ <sup>i</sup>. So, for each robot, the constraint vector <sup>Ω</sup><sup>i</sup> is of size <sup>M</sup> <sup>¼</sup> ð Þ� <sup>L</sup> � <sup>1</sup> <sup>N</sup>; of course, the multiplier vector <sup>λ</sup><sup>i</sup> in (24) is of the same size.

We define the collision avoidance by constraining motion of other robots to be outside a safety circle region around each i robot at the position xi ; y<sup>i</sup> in the 2D plane:

$$\left(\mathfrak{x}^{i}(k) - \hat{\mathfrak{x}}^{j}(k)\right)^{2} + \left(\mathfrak{y}^{i}(k) - \hat{\mathfrak{y}}^{j}(k)\right)^{2} \ge p^{2} \cdot \mathfrak{x}^{i}$$

So, we can define each element of the constraint vector as

$$\mathcal{Q}^{\vec{\eta}}(k) = p^2 - \left(\boldsymbol{\varkappa}^i(k) - \hat{\boldsymbol{\varkappa}}^j(k)\right)^2 + \left(\boldsymbol{\jmath}^i(k) - \hat{\boldsymbol{\jmath}}^j(k)\right)^2 \tag{27}$$

The radius of the safety region is chosen as p. Because of the definition of the sampling period variable, at each of discrete time step k, the actual time variable does not necessarily imply t i ð Þ¼ k t j ð Þk for all the other L � 1 robots. That is why you see that the x- and y- coordinates in (23) of the jth robot are noted as x^<sup>j</sup> ; ^y<sup>j</sup> which are chosen to be calculated as linear interpolation of positions according to available actual times of t i ð Þ<sup>k</sup> and <sup>t</sup> <sup>j</sup> ð Þk .

x2 <sup>0</sup>; y<sup>2</sup> <sup>0</sup>; ϕ<sup>2</sup> 0

DOI: http://dx.doi.org/10.5772/intechopen.85668

x3 <sup>0</sup>; y<sup>3</sup> <sup>0</sup>; ϕ<sup>3</sup> 0 <sup>¼</sup> ð Þ <sup>5</sup>; <sup>0</sup>; <sup>π</sup> , x<sup>3</sup>

Figure 3.

Figure 4.

129

Collision avoidance (Example 1).

¼ �ð Þ <sup>10</sup>; �1; <sup>0</sup> , x<sup>2</sup>

Distributed Optimization of Multi-Robot Motion with Time-Energy Criterion

The time-energy objective values throughout global iterations (Example 1).

<sup>f</sup> ; y<sup>2</sup> <sup>f</sup> ; ϕ<sup>2</sup> f <sup>¼</sup> <sup>8</sup>; <sup>5</sup>;

<sup>f</sup> ; y<sup>3</sup> <sup>f</sup> ; ϕ<sup>3</sup> f ¼ �8; �1;

Here, robot 1 has equal objective weights of 5 on both time and energy, robot 2 has weights of 10 on energy and 1 on time, and robot 3 has 10 on time and 1 on energy. The maximum number of internal NLP iterations (primal update) is set to

π 2 

> π 2

#### 4.3 Simulation examples

You can follow the whole distributed algorithm for the time-energy optimization of multi-robot system with collision avoidance in the flowchart in Figure 2. View the flowchart as the process for each individual robot (subproblem). We implement the algorithm in Figure 2. For the primal minimizer update in (24), the nonlinear programming (NLP) function of fmincon in MATLAB is used. We solved the problem for motion of L ¼ 3 mobile robots. Utilizing the parallel capability in MATLAB, the distributed steps are solved independently utilizing three parallel processors. We choose number of instants N ¼ 40; so, we are going to optimize 120 control variables for each robot. More details can be found in [36].

Example 1. In this example, exploration of the behavior of the algorithm is shown. The problem has the following desired values of initial and final positions and orientations for the three robots:

$$\left(\mathfrak{x}\_0^1, \mathfrak{y}\_0^1, \mathfrak{\phi}\_0^1\right) = \left(\mathbf{0}, -8, \frac{\pi}{2}\right), \left(\mathfrak{x}\_f^1, \mathfrak{y}\_f^1, \mathfrak{\phi}\_f^1\right) = (\mathbf{0}, \mathfrak{5}, \pi)$$

Figure 2. Distributed algorithm flowchart to optimize multi-robot motion.

Distributed Optimization of Multi-Robot Motion with Time-Energy Criterion DOI: http://dx.doi.org/10.5772/intechopen.85668

$$(\mathbf{x}\_0^2, \mathbf{y}\_0^2, \phi\_0^2) = (-\mathbf{1}\mathbf{0}, -\mathbf{1}, \mathbf{0}), \left(\mathbf{x}\_f^2, \mathbf{y}\_f^2, \phi\_f^2\right) = \left(\mathbf{8}, \mathbf{5}, \frac{\pi}{2}\right)$$

$$\left(\mathbf{x}\_0^3, \mathbf{y}\_0^3, \phi\_0^3\right) = (\mathbf{5}, \mathbf{0}, \pi), \left(\mathbf{x}\_f^3, \mathbf{y}\_f^3, \phi\_f^3\right) = \left(-\mathbf{8}, -\mathbf{1}, \frac{\pi}{2}\right)$$

Here, robot 1 has equal objective weights of 5 on both time and energy, robot 2 has weights of 10 on energy and 1 on time, and robot 3 has 10 on time and 1 on energy. The maximum number of internal NLP iterations (primal update) is set to

Figure 3. The time-energy objective values throughout global iterations (Example 1).

Figure 4. Collision avoidance (Example 1).

see that the x- and y- coordinates in (23) of the jth robot are noted as x^<sup>j</sup>

Path Planning for Autonomous Vehicles - Ensuring Reliable Driverless Navigation…

control variables for each robot. More details can be found in [36].

actual times of t

Figure 2.

128

Distributed algorithm flowchart to optimize multi-robot motion.

i

and orientations for the three robots:

x1 <sup>0</sup>; y<sup>1</sup> <sup>0</sup>; ϕ<sup>1</sup> 0 <sup>¼</sup> <sup>0</sup>; �8;

4.3 Simulation examples

ð Þ<sup>k</sup> and <sup>t</sup> <sup>j</sup>

ð Þk .

are chosen to be calculated as linear interpolation of positions according to available

You can follow the whole distributed algorithm for the time-energy optimization of multi-robot system with collision avoidance in the flowchart in Figure 2. View the flowchart as the process for each individual robot (subproblem). We implement the algorithm in Figure 2. For the primal minimizer update in (24), the nonlinear programming (NLP) function of fmincon in MATLAB is used. We solved the problem for motion of L ¼ 3 mobile robots. Utilizing the parallel capability in MATLAB, the distributed steps are solved independently utilizing three parallel processors. We choose number of instants N ¼ 40; so, we are going to optimize 120

Example 1. In this example, exploration of the behavior of the algorithm is shown. The problem has the following desired values of initial and final positions

> π 2

, x<sup>1</sup> <sup>f</sup> ; y<sup>1</sup> <sup>f</sup> ; ϕ<sup>1</sup> f 

¼ ð Þ 0; 5; π

; ^y<sup>j</sup> which

only 10. The step size is set to α ¼ 0:1. Maximum global iterations are allowed for 100 iterations. The safety circle radius is chosen to be p ¼ 2.

Figure 3 contains the objective (time-energy) value evolution throughout iterations. You can see the stable convergence as the algorithm progresses. Figure 4 shows each of the robots'safety clearances during the optimized motion. In Figure 5, snapshots of motion of the three robots at different time instants are depicted. This illustrates the collision avoidance attained throughout the optimized motion. Observe also how different are the speeds of each robot because of objective weights; note from Figure 4 that each robot has a different final time for their motion. The algorithm has shown good performance at eliminating collision constraint violations. Figures 4 and 5 show an instant (around t = 12) where robots 1 and 3 violate collision distance with very small value, but no collision occurs. This is because the maximum number of iterations of the algorithm is exhausted. This

indicates the possibility for the motion to be optimized even more if collision constraints were relaxed or if more algorithm iterations were allowed. Initialization

Distributed Optimization of Multi-Robot Motion with Time-Energy Criterion

DOI: http://dx.doi.org/10.5772/intechopen.85668

Optimized trajectories of the three robots: each row of plots shows x-coordinate error, y-coordinate error, and orientation error, respectively; each column of plots show robots 1, 2, and 3 errors, respectively (Example 2).

of the algorithm also plays a role in algorithm evolution.

Figure 6.

Figure 7.

131

Collision avoidance (Example 2).

Figure 5. Snapshots of optimized motions at different instants (Example 1).

Distributed Optimization of Multi-Robot Motion with Time-Energy Criterion DOI: http://dx.doi.org/10.5772/intechopen.85668

Figure 6.

only 10. The step size is set to α ¼ 0:1. Maximum global iterations are allowed for

Figure 3 contains the objective (time-energy) value evolution throughout iterations. You can see the stable convergence as the algorithm progresses. Figure 4

shows each of the robots'safety clearances during the optimized motion. In Figure 5, snapshots of motion of the three robots at different time instants are depicted. This illustrates the collision avoidance attained throughout the optimized motion. Observe also how different are the speeds of each robot because of objective weights; note from Figure 4 that each robot has a different final time for their motion. The algorithm has shown good performance at eliminating collision constraint violations. Figures 4 and 5 show an instant (around t = 12) where robots 1 and 3 violate collision distance with very small value, but no collision occurs. This is because the maximum number of iterations of the algorithm is exhausted. This

Path Planning for Autonomous Vehicles - Ensuring Reliable Driverless Navigation…

100 iterations. The safety circle radius is chosen to be p ¼ 2.

Figure 5.

130

Snapshots of optimized motions at different instants (Example 1).

Optimized trajectories of the three robots: each row of plots shows x-coordinate error, y-coordinate error, and orientation error, respectively; each column of plots show robots 1, 2, and 3 errors, respectively (Example 2).

indicates the possibility for the motion to be optimized even more if collision constraints were relaxed or if more algorithm iterations were allowed. Initialization of the algorithm also plays a role in algorithm evolution.

Figure 7. Collision avoidance (Example 2).

Example 2. This example illustrates more the satisfaction of the objectives. In this example the safety circle radius is put as p ¼ 3. Here we choose the following initial and final positions and orientations for the three robots:

$$\left(\mathfrak{x}\_{0}^{1},\mathfrak{y}\_{0}^{1},\mathfrak{\phi}\_{0}^{1}\right) = \left(-8,0,\frac{\pi}{2}\right), \left(\mathfrak{x}\_{f}^{1},\mathfrak{y}\_{f}^{1},\mathfrak{\phi}\_{f}^{1}\right) = \left(8,0,\frac{\pi}{2}\right)$$

$$\left(\mathfrak{x}\_{0}^{2},\mathfrak{y}\_{0}^{2},\mathfrak{\phi}\_{0}^{2}\right) = \left(0,8,0\right), \left(\mathfrak{x}\_{f}^{2},\mathfrak{y}\_{f}^{2},\mathfrak{\phi}\_{f}^{2}\right) = \left(\mathbf{0},-8,\mathbf{0}\right)$$

$$\left(\mathfrak{x}\_{0}^{3},\mathfrak{y}\_{0}^{3},\mathfrak{\phi}\_{0}^{3}\right) = \left(8,\mathbf{0},-\frac{\pi}{2}\right), \left(\mathfrak{x}\_{f}^{3},\mathfrak{y}\_{f}^{3},\mathfrak{\phi}\_{f}^{3}\right) = \left(-8,\mathbf{0},-\frac{\pi}{2}\right)$$

After applying our approach, you can see the resulting optimized motions in Figure 6. In Figure 6, errors in x- and y-coordinates and orientation of each robot are shown with respect to time. It is clear that errors of zero are achieved. In Figure 7 for each robot, constraint evaluations, i.e., safety clearance, are displayed for the other two robots throughout time. You can see that robots come close to each other sometimes but without violating the safety distance. This result is attained maybe because of special structure of initial and final positions and orientations. That could have given flexibility for the algorithm.
