Trajectory Planning for Robots

### **Chapter 2** Trajectory Planning

*Junsong Lei, Zhaojiang Luo and Chaoqun Li*

#### **Abstract**

This chapter discusses the trajectory planning of robot. First, the basic principle of trajectory planning is described, which is mainly determined by the geometric path and the motion law, and it is a motion law that defines time according to a given geometric path. Second, the characteristics of the joint space and the operational space are expounded. Then the trapezoidal trajectory and the trajectory with S velocity profile commonly used in industrial practice are introduced. Finally, point-to-point trajectories and multipoint trajectories are described. Point-to-point trajectories include a point-to-point trajectory in the joint space and a straight-line and circle trajectory in the operational space. Multipoint trajectories include Bezier and NURBS curves functions.

**Keywords:** trajectory planning, the geometric path, the motion law, the joint space, the operational space, point-to-point trajectories, multipoint trajectories

#### **1. Introduction**

Trajectory planning is a motion law that defines time according to a given geometric path. Therefore, the purpose of trajectory planning is to meet the needs of the tasks and generate the reference inputs required by the control system, so that the robots can perform the movements in Ref. [1]. The inputs of any trajectory planning algorithm are an expected motion path, kinematics, and dynamics system parameters of the robots, and the outputs are the positions, velocities, and accelerations of the interpolation points of each joint or the end effector in Refs. [2, 3]. This chapter will introduce trajectory planning from the perspective of industrial practice, including the basic principle of trajectory planning, the joint space and the operational space, the motion laws, and point-to-point and multipoint trajectories.

#### **2. The basic principle of trajectory planning**

The desired trajectory can be fully determined by the geometric path and the motion law. The geometry path is related to space, while the motion law is related to time.

#### **2.1 The geometry of the trajectory**

The geometric path *p* ¼ *p s*ð Þ expresses the path of the end effector from the start point to the end point in Ref. [4]. The end point B can be reached in two different path forms from the start point A, see **Figure 1**. Depending on whether it is necessary to specify the geometry of the trajectory, the planning of trajectory can be carried out in the joint space or the operational space.

#### **2.2 The motion law**

The motion law *s* ¼ *s t*ð Þ limits velocity, acceleration, and other parameters of the end effector from the start point to the end point. The end point B can be reached from the start point A according to two different motion laws, see **Figure 2**. Various functions, such as linear trajectory function with constant velocity or trajectory function with S velocity profile, can be selected to specify the motion law along the geometric path.

Therefore, based on the geometric path *p* ¼ *p s*ð Þ and the motion law *s* ¼ *s t*ð Þ, the trajectory can be written as

*p* ¼ *pst* ð Þ ð Þ (1)

**Figure 1.** *Two different path forms from the start point A to the end point B.*

**Figure 2.** *Two different motion laws from the start point A to the end point B.*

#### **3. The joint space and the operational space**

Trajectory planning is divided into the joint space and the operational space according to the different description spaces. Trajectory planning in the joint space has the characteristics of few constraints and fast calculation speed, while trajectory planning in the operational space is mainly used for scenarios where users have specified path requirements.

#### **3.1 The joint space**

Trajectory planning in the joint space is to describe the trajectory of the robot as a function of joint position with respect to time, that is, the trajectory of the end effector can be directly determined by joint variables, so it is easy and simple to carry out trajectory planning in the joint space. Since there is no continuous corresponding relationship between the joint space and the operational space, singularity, and redundant degrees of freedom in motion can be avoided. In addition, the complexity of its path depends on the specific kinematic characteristics of the robot, and the path in the operational space cannot be predicted in Ref. [5]. Therefore, trajectory planning in the joint space is only applicable to the operating scenarios without path requirements.

#### **3.2 The operational space**

Trajectory planning in the operational space is based on a function of position and orientation with respect to time to describe the trajectory of the robot, and the corresponding joint position is obtained by the inverse kinematic model.

Since the path planned in the operational space is determined, the trajectory planning in the operational space is suitable for the operating scenarios with strict requirements for the instantaneous change law of position and orientation. In addition, the planning of trajectory in the operational space is prone to problems such as unreachable workspace or singularity, and they are generally avoided by specifying the robots to pass through a set of intermediate points in Ref. [6].

#### **4. The motion law**

In order to ensure that the robots can operate efficiently and stably, it is generally required that the motion profile meet at least the continuous first derivative, and it needs to have the characteristics of fast acceleration and deceleration and constant velocity. At present, the velocity control algorithms commonly used in robot systems include the trapezoidal trajectory and the trajectory with S velocity profile.

#### **4.1 The trapezoidal trajectory**

The trapezoidal trajectory means that the velocity profile is trapezoidal, and its complete curves are shown in **Figure 3**. The complete trajectory consists of three phases. In the first phase, the acceleration changes from zero to a fixed acceleration,

**Figure 3.** *Position, velocity, and acceleration curves of a trapezoidal trajectory.*

and the velocity increases linearly with a fixed acceleration from zero. In the second phase, when the velocity reaches the desired value, it enters the constant velocity phase. At this phase, the velocity is constant and the acceleration suddenly changes to zero. In the third phase, the velocity begins to decrease with a fixed acceleration until it reaches zero.

Piecewise acceleration equations are expressed as

$$a(t) = \begin{cases} a\_{\max} & (0 \le t \le t\_1) \\\\ 0 & (t\_1 \le t \le t\_2) \\\\ -a\_{\max} & (t\_2 \le t \le t\_3) \end{cases} \tag{2}$$

Piecewise velocity equations are expressed as

$$v(t) = \begin{cases} a\_{\max}t & (0 \le t \le t\_1) \\\\ v\_2 & (t\_1 \le t \le t\_2) \\\\ v\_2 - a\_{\max}(t - t\_2) & (t\_2 \le t \le t\_3) \end{cases} \tag{3}$$

Piecewise position equations are expressed as

$$s(t) = \begin{cases} \frac{1}{2} a\_{\max} t^2 & (0 \le t \le t\_1) \\ s\_1 + v\_2(t - t\_1) & (t\_1 \le t \le t\_2) \\ s\_2 - \frac{1}{2} a\_{\max} \left(t - t\_2\right)^2 & (t\_2 \le t \le t\_3) \end{cases} \tag{4}$$

where the time instants *t*1, *t*<sup>2</sup> and *t*<sup>3</sup> are specified. *s*1, *s*<sup>2</sup> and *s*<sup>3</sup> are corresponding position values, and *v*1, *v*<sup>2</sup> and *v*<sup>3</sup> are corresponding velocity values. *a*max is a maximum acceleration value.

#### **4.2 The trajectory with S velocity profile**

The trajectory with S velocity profile means that the velocity profile is S-shaped, which presents that the jerk is constant during acceleration and deceleration, and a smoother velocity curve is generated by controlling the duration of jerk to reduce the impact on machinery. The complete trajectory with S velocity profile includes seven phases: constant positive jerk phase *T*<sup>1</sup> in acceleration, constant zero jerk phase *T*<sup>2</sup> in acceleration, constant negative jerk phase *T*<sup>3</sup> in acceleration, constant velocity phase *T*4, constant negative jerk phase *T*<sup>5</sup> in deceleration, constant zero jerk phase *T*<sup>6</sup> in deceleration, and constant positive jerk phase *T*<sup>7</sup> in deceleration, see **Figure 4**. In the first three phases, the velocity increases smoothly, the acceleration presents trapezoidal profile, and the jerk changes abruptly between the desired value and zero. In the fourth phase, when the velocity is accelerated to the desired value, it enters the constant velocity phase. When the end point is approaching, the deceleration phase begins in the last phases. The transition of the entire velocity curve is smooth, and there is a continuous acceleration.

Piecewise jerk equations are expressed as

$$j(t) = \begin{cases} j\_{\text{max}} & (0 \le t \le t\_1) \\ 0 & (t\_1 \le t \le t\_2) \\ -j\_{\text{max}} & (t\_2 \le t \le t\_3) \\ 0 & (t\_3 \le t \le t\_4) \\ -j\_{\text{max}} & (t\_4 \le t \le t\_5) \\ 0 & (t\_5 \le t \le t\_6) \\ j\_{\text{max}} & (t\_6 \le t \le t\_7) \end{cases} \tag{5}$$

Piecewise acceleration equations are expressed as

$$a(t) = \begin{cases} j\_{\max}t & (0 \le t \le t\_1) \\ j\_{\max}T\_1 & (t\_1 \le t \le t\_2) \\ j\_{\max}T\_1 - j\_{\max}(t - t\_1) & (t\_2 \le t \le t\_3) \\ 0 & (t\_3 \le t \le t\_4) \\ -j\_{\max}(t - t\_4) & (t\_4 \le t \le t\_5) \\ -j\_{\max}T\_4 & (t\_5 \le t \le t\_6) \\ -j\_{\max}T\_4 + j\_{\max}(t - t\_6) & (t\_6 \le t \le t\_7) \end{cases} \tag{6}$$

**Figure 4.**

*Position, velocity, and acceleration curves of a trajectory with S velocity profile.*

Piecewise velocity equations are expressed as

$$\nu(t) = \begin{cases} \boldsymbol{v}\_{\boldsymbol{s}} + \frac{1}{2} \boldsymbol{j}\_{\max} \boldsymbol{t}^2 & (0 \le \boldsymbol{t} \le t\_1) \\ \boldsymbol{v}\_1 + \boldsymbol{j}\_{\max} T\_1 (\boldsymbol{t} - \boldsymbol{t}\_1) & (\boldsymbol{t}\_1 \le \boldsymbol{t} \le t\_2) \\ \boldsymbol{v}\_2 + \boldsymbol{j}\_{\max} T\_1 (\boldsymbol{t} - \boldsymbol{t}\_2) - \frac{1}{2} \boldsymbol{j}\_{\max} (\boldsymbol{t} - \boldsymbol{t}\_2)^2 & (\boldsymbol{t}\_2 \le \boldsymbol{t} \le \boldsymbol{t}\_3) \\ \boldsymbol{v}\_3 & (\boldsymbol{t}\_3 \le \boldsymbol{t} \le \boldsymbol{t}\_4) \\ \boldsymbol{v}\_4 - \frac{1}{2} \boldsymbol{j}\_{\max} (\boldsymbol{t} - \boldsymbol{t}\_4)^2 & (\boldsymbol{t}\_4 \le \boldsymbol{t} \le \boldsymbol{t}\_5) \\ \boldsymbol{v}\_5 - \boldsymbol{j}\_{\max} T\_5 (\boldsymbol{t} - \boldsymbol{t}\_5) & (\boldsymbol{t}\_5 \le \boldsymbol{t} \le \boldsymbol{t}\_6) \\ \boldsymbol{v}\_6 - \boldsymbol{j}\_{\max} T\_5 (\boldsymbol{t} - \boldsymbol{t}\_6) + \frac{1}{2} \boldsymbol{j}\_{\max} (\boldsymbol{t} - \boldsymbol{t}\_6)^2 & (\boldsymbol{t}\_6 \le \boldsymbol{t} \le \boldsymbol{t}\_7) \end{cases} \tag{7}$$

Piecewise position equations are expressed as

$$\begin{cases} v\_t t + \frac{1}{6} j\_{\text{max}} t^3 & (0 \le t \le t\_1) \\\\ \dots & \dots & (t\_{n-1}) + 1 \end{cases} \tag{7.1747}$$

*s*<sup>1</sup> þ *v*1ð Þþ *t* � *t*<sup>1</sup> 2 *j* max*T*1ð Þ *t* � *t*<sup>1</sup> <sup>2</sup> ð Þ *<sup>t</sup>*<sup>1</sup> <sup>≤</sup>*<sup>t</sup>* <sup>≤</sup>*t*<sup>2</sup> 1

$$\begin{cases} v\_t t + \frac{1}{6} j\_{\text{max}} t^r & (0 \le t \le t\_1) \\\\ s\_1 + v\_1(t - t\_1) + \frac{1}{2} j\_{\text{max}} T\_1 (t - t\_1)^2 & (t\_1 \le t \le t\_2) \\\\ s\_2 + v\_2(t - t\_2) + \frac{1}{2} j\_{\text{max}} T\_1 (t - t\_2)^2 - \frac{1}{6} j\_{\text{max}} (t - t\_2)^3 & (t\_2 \le t \le t\_3) \\\\ s\_3 + v\_3(t - t\_3) & (t\_3 \le t \le t\_4) & \end{cases}$$

$$\begin{cases} s\_4 + v\_4(t - t\_4) - \frac{1}{6}j\_{\max}(t - t\_4)^3 & (t\_4 \le t \le t\_5) \\ s\_5 + v\_5(t - t\_5) - \frac{1}{2}j\_{\max}T\_5(t - t\_5)^2 & (t\_5 \le t \le t\_6) \\ s\_6 + v\_6(t - t\_6) - \frac{1}{2}j\_{\max}T\_5(t - t\_6)^2 + \frac{1}{2}j\_{\max}(t - t\_6)^3 & (t\_6 \le t \le t\_7) \end{cases}$$

$$\begin{cases} \mathbf{s}\_{\mathsf{5}} + \boldsymbol{\nu}\_{\mathsf{5}}(t - t\_{\mathsf{5}}) - \frac{\mathbf{1}}{2} \boldsymbol{j}\_{\max} \boldsymbol{T}\_{\mathsf{5}}(t - t\_{\mathsf{5}})^2 & (t\_{\mathsf{5}} \le t \le t\_{\mathsf{6}}) \\\ s\_{\mathsf{6}} + \boldsymbol{\nu}\_{\mathsf{6}}(t - t\_{\mathsf{6}}) - \frac{\mathbf{1}}{2} \boldsymbol{j}\_{\max} \boldsymbol{T}\_{\mathsf{5}}(t - t\_{\mathsf{6}})^2 + \frac{\mathbf{1}}{6} \boldsymbol{j}\_{\max}(t - t\_{\mathsf{6}})^3 & (t\_{\mathsf{6}} \le t \le t\_{\mathsf{7}}) \end{cases}$$

Where the time instants *t*1, *t*2, … , and *t*<sup>7</sup> are specified. *s*1, *s*2, … , and *s*<sup>7</sup> are corresponding position values, and *v*1, *v*2, … , and *v*<sup>7</sup> are corresponding velocity values. *a*max and *j* max are maximum acceleration and jerk values.

#### **5. Point-to-point trajectories and multipoint trajectories**

#### **5.1 Point-to-point trajectories**

Point-to-point trajectories are mainly used for robotic tasks such as palletizing, cutting, press tending, and other operations. Point-to-point control first ensures the position and orientation of the teach-in points, and the geometric path between the teach-in points, such as straight-lines and circles, can be specified when programming the robot's working program.

#### *5.1.1 Point-to-point trajectory in the joint space*

According to the requirements of the task, the end effector is required to move linearly in the joint space between point A and point B (**Figure 5**).

Therefore, the position of each joint at the time instant *t* þ Δ*t* can be expressed as

$$
\theta\_{t+\Delta t} = (1-u)\theta\_A + u\theta\_B(0 \le u \le 1) \tag{9}
$$

*5.1.2 Point-to-point trajectory in the operational space*

#### *5.1.2.1 A straight-line position trajectory*

According to the requirements of the task, the position of the end effector needs to be moved in a straight-line between point A and point B. Generally, the positions of the start point A and the end point B in the operational space are known, and the positions of a set of intermediate points on the trajectory AB are calculated, see **Figure 6**.

The position length of the trajectory AB is *LAB*. The end effector is located at the point *Pt* and *Pt*þΔ*<sup>t</sup>* on the trajectory AB, corresponding to the displacement *s t*ð Þ and

**Figure 6** *A straight-line position trajectory in the operational space.*

*s t*ð Þ¼ þ Δ*t s t*ð Þþ Δ*s* at the time instants *t* and *t* þ Δ*t*. Δ*t* is a control period, corresponding to the displacement Δ*s* .

Length function in normalized form can be expressed as

$$u = \frac{s(t + \Delta t)}{L\_{AB}} (0 \le u \le 1) \tag{10}$$

Therefore, the position of the end effector at the time instant *t* þ Δ*t* can be expressed as

$$
\begin{bmatrix} P\_x \\ P\_y \\ P\_x \end{bmatrix} = (\mathbf{1} - u) \begin{bmatrix} A\_x \\ A\_y \\ A\_x \end{bmatrix} + u \begin{bmatrix} B\_x \\ B\_y \\ B\_x \end{bmatrix} \tag{11}
$$

#### *5.1.2.2 A straight-line orientation trajectory*

According to the requirements of the task, the orientation of the end effector needs to be moved continuously between point A and point B. Generally, the orientations of the start point A and the end point B in the operational space are known, and the orientations of a set of intermediate points on the trajectory AB are calculated, see **Figure 7**. Since Euler angles and rotation matrices may have gimbal locking in rotation, and quaternions do not have this problem. Therefore, quaternions can be used to calculate the orientation.

**Figure 7** *A straight-line orientation trajectory in the operational space.*

The orientation unit vector of the start point A is *r* ! *A*, corresponding to the quaternion *QA*. The orientation unit vector of the end point B is *r* ! *<sup>B</sup>*, corresponding to the quaternion *QB*. The angle between two orientation unit vectors is *θ*. The end effector is located at the point *Pt* and *Pt*þΔ*<sup>t</sup>* on the trajectory AB, corresponding to the angle *θ*ð Þ*t* and *θ*ð Þ *t* þ Δ*t* at the time instants *t* and *t* þ Δ*t*. Its orientation unit vector is *r* ! *Pt*þΔ*<sup>t</sup>* , and the corresponding quaternion is *QPt*þΔ*<sup>t</sup>* .

Angle function in normalized form can be expressed as

$$u = \frac{\theta(t + \Delta t)}{\theta} (0 \le u \le 1) \tag{12}$$

The general linear interpolation will cause uneven changes in angular velocity, while spherical linear interpolation can ensure a smooth curve between two quaternions. Therefore, the orientation of the end effector at the time instant *t* þ Δ*t* can be expressed as

$$Q\_{\mathcal{P}\_{t+\Delta t}} = \frac{\sin[(1-u)\theta]Q\_A + \sin(u\theta)Q\_B}{\sin\theta} \tag{13}$$

When the angle *θ* is very small, it will result in a large value in the Eq. (13). Therefore, the linear interpolation formula can be used

$$Q\_{P\_{t+\Delta t}} = (\mathbf{1} - \boldsymbol{\omega}) Q\_A + \boldsymbol{\omega} Q\_B \tag{14}$$

#### *5.1.2.3 A circle position trajectory*

According to the requirements of the task, the position of the end effector needs to be moved in a circle between points A, B, and C. Generally, the positions of the start point A, the middle point B and the end point C in the operational space are known,

**Figure 8** *A circle position trajectory in the operational space.*

and the positions of a set of intermediate points on the trajectory ABC are calculated, see **Figure 8**. Based on the position of the start point A, the middle point B, and the end point C, the radius *R* and center *M* of the circle can be solved. Then the unit normal vector *n* ! of the circle, the unit tangent vector *e* ! of the start point, and the unit vector *w* ! *<sup>A</sup>* from the center to the start point can be solved.

Length function in normalized form can be expressed as

$$
\mu = \frac{s(t + \Delta t)}{L\_{\hat{A}\hat{B}C}} (0 \le u \le 1) \tag{15}
$$

Therefore, the position of the end effector at the time instant *t* þ Δ*t* can be expressed as

$$
\begin{bmatrix} P\_x \\ P\_y \\ P\_x \end{bmatrix} = \begin{bmatrix} M\_x \\ M\_y \\ M\_x \end{bmatrix} + R\bullet\cos\left(\frac{uS}{R}\right)\overrightarrow{w}\_A + R\bullet\sin\left(\frac{uS}{R}\right)\overrightarrow{e} \tag{16}
$$

#### *5.1.2.4 A circle orientation trajectory*

According to the requirements of the task, the orientation of the end effector needs to be moved continuously between points A, B, and C. Generally, the orientations of the start point A, the middle point B and the end point C in the operational space are known, and the orientations of a set of intermediate points on the trajectory ABC are calculated, see **Figure 9**.

**Figure 9.** *A circle orientation trajectory in the operational space.*

The orientation unit vector of the start point A is *r* ! *A*, corresponding to the quaternion *QA*. The orientation unit vector of the middle point B is *r* ! *<sup>B</sup>*, corresponding to the quaternion *QB*. The orientation unit vector of the end point C is *r* ! *<sup>C</sup>*, corresponding to the quaternion *QC*. The angle of the orientation unit vectors between the start and the middle point is *αAB*. The angle of the orientation unit vectors between the middle and end point is *αBC*. The angle of the orientation unit vectors between the start and end point is *α*. The end effector is located at the point *Pt*þΔ*<sup>t</sup>* on the trajectory AB, corresponding to the angle *α*ð Þ *t* þ Δ*t* at the time instant *t* þ Δ*t*. Its orientation unit vector is *r* ! *Pt*þΔ*<sup>t</sup>* , and the corresponding quaternion is *QPt*þΔ*<sup>t</sup>* .

Middle angle function in normalized form can be expressed as

$$
\mu\_{\text{Middle}} = \frac{a\_{AB}}{a} \tag{17}
$$

Angle function in normalized form can be expressed as

$$
\mu = \frac{a\_{l+\Delta t}}{a} \tag{18}
$$

If *u* ≤*uMiddle*, the orientation of the end effector at the time instant *t* þ Δ*t* can be expressed as

$$Q\_{P\_{i+\Delta t}} = \frac{\sin\left[\left(1 - \frac{u}{u\_{\text{Midder}}}\right) \alpha\_{AB}\right] Q\_A + \sin\left(\frac{u}{u\_{\text{Midder}}} \alpha\_{AB}\right) Q\_B}{\sin \alpha\_{AB}} \tag{19}$$

If *uMiddle* ≤ *u*, the orientation of the end effector at the time instant *t* þ Δ*t* can be expressed as

$$Q\_{\mathcal{R}\_{i+\Delta t}} = \frac{\sin\left[\left(1 - \frac{u - u\_{\text{Midle}}}{1 - u\_{\text{Midle}}}\right)a\_{BC}\right]Q\_B + \sin\left(\frac{u - u\_{\text{Midle}}}{1 - u\_{\text{Midle}}}a\_{BC}\right)Q\_C}{\sin a\_{BC}}\tag{20}$$

#### **5.2 Multipoint trajectories**

Multipoint trajectories are often used for complex operations such as painting, arc welding, or surface machining. The user teaches a series of points (such as start, end, and intermediate points) on a continuous path, and the robots connect the path points or multi-segment paths sequentially using polynomial functions, Bezier curve functions, or spline functions.

#### *5.2.1 Bezier curve functions*

In order to further improve the efficiency of the tasks, the robot end effector is required to achieve continuous operation between multiple trajectories. That is to say, the displacement and speed motion profiles are continuous. Due to the characteristics of the geometric invariance and convex hull, high-order Bezier curves are often used in industrial practice to connect multi-segment paths in Ref. [7]. Taking the connection of two straight-line trajectories as an example, the position of point A, point B and point C of line 1 and line 2 is generally known, and the end point B of line 1 is used as the start point of line 2, see **Figure 10**.

**Figure 10.** *A quartic Bezier curve trajectory.*

A circle with B as center point and the given transition value R as radius can be obtained, and it intersects the AB and BC trajectories at D and E points, respectively. A quartic Bezier curve is used to connect the two trajectories, and five control points need to be specified on the DB and BE segments. The center point B is the control point *P*2, and the points D and E are the control points *P*<sup>0</sup> and *P*4, respectively. *P*<sup>1</sup> is taken halfway through the DB segment and *P*<sup>3</sup> is taken halfway through the BE segment.

The quartic Bezier curve function can be written as

$$P(t) = \sum\_{i=0}^{4} P\_i B\_{i,4}(t), t \in [0, 1] \tag{21}$$

The corresponding expansion in Eq. (21) is

$$\begin{aligned} P(t) &= \left(\mathbf{1} - t\right)^4 P\_0 + 4t(\mathbf{1} - t)^3 P\_1 + 6t^2(\mathbf{1} - t)^2 P\_2 \\ &+ 4t^3(\mathbf{1} - t)P\_3 + t^4 P\_4, t \in [0, 1] \end{aligned} \tag{22}$$

Recursive equation is

$$P\_i^k = \begin{cases} P\_i & k = 0\\ (1-t)P\_i^{k-1} + tP\_{i+1}^{k-1}k = 1,2,\ldots,4, i = 0,1,\ldots,4-k \end{cases} \tag{23}$$

#### *5.2.2 NURBS curve functions*

For the complex trajectories of the end effector in the operation space, the NURBS curve has been widely used because it can accurately and uniformly express standard analytical curves and free curves and has the characteristic of flexible shape control in Ref. [8]. Any k-th degree NURBS curve can be represented as a piecewise rational polynomial function.

$$P(t) = \frac{\sum\_{i=0}^{n} d\_i \alpha\_i N\_{i,k}(t)}{\sum\_{i=0}^{n} \alpha\_i N\_{i,k}(t)}\tag{24}$$

where *ω<sup>i</sup>* are the weight factors, *di* are the curve control points, and *Ni*,*k*ð Þ*t* are B-spline basis functions determined by the node vectors *ti* .

Based on De Boor-Cox recursive function, the basis function can be written as

$$\begin{cases} N\_{i,0} = \begin{cases} 1 \ \text{,} \ t \in [t\_i, t\_{i+1}) \\ 0 \ \text{,} otherwise \end{cases} \\ N\_{i,k} = \frac{t - t\_i}{t\_{i+k} - t\_i} N\_{i,k-1}(t) + \frac{t\_{i+k+1} - t}{t\_{i+k+1} - t\_{i+1}} N\_{i+1,k-1}(t) \end{cases} \tag{25}$$

where <sup>0</sup> <sup>0</sup> ¼ 0 is specified in Eq. (25).

Due to the computational complexity of the control points solved by the via-points of the NURBS curve, the De Boor-Cox algorithm is often used to avoid repeated iteration. More details on NURBS curve calculations can be found in Ref. [9].

#### **6. Conclusions**

This chapter discusses the trajectory planning of robots. First, the basic principle of trajectory planning is described, which is mainly determined by the geometric path and the motion law. Second, the characteristics of the joint space and the operational space are expounded. Then the trapezoidal trajectory and the trajectory with S velocity profile commonly used in industrial practice are introduced. Finally, point-topoint trajectories and multipoint trajectories are described. Point-to-point trajectories include a point-to-point trajectory in the joint space and a straight-line and circle trajectory in the operational space. Multipoint trajectories include Bezier and NURBS curves functions.

#### **Author details**

Junsong Lei1,2\*, Zhaojiang Luo2 and Chaoqun Li<sup>2</sup>

1 Key Laboratory of High Performance Servo Systems in Guangdong Province, China

2 Zhuhai Gree Electric Co., Ltd., Zhuhai, China

\*Address all correspondence to: rey\_se@163.com

© 2023 The Author(s). Licensee IntechOpen. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

*Trajectory Planning DOI: http://dx.doi.org/10.5772/intechopen.111780*

#### **References**

[1] Zixing C, Bin X. Robotics. 3rd ed. Beijing: Tsinghua Press; 2015. p. 273

[2] Caselli S, Reggiani M, Sbravati R. Parallel path planning with multiple evasion strategies. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '02); 11–15 May 2002; Washington, DC. New York: IEEE; 2002. pp. 260-266

[3] Yonglun C, Shiqiang Z, Lijia L. Kinematics analysis and simulation of Q J-6R welding robot based on Matlab. Mechanical and Electrical Engineering Magazine. 2007;**24**:107-110. DOI: 10.3969/j.issn.1001-4551. 2007.11.034

[4] Luigi B, Claudio M. Trajectory Planning for Automatic Machines and Robots. Berlin: Springer-Verlag; 2008. pp. 7-9

[5] John JC. Introduction to Robotics: Mechanics and Control. 3rd ed. Beijing: China Machine Press; 2012. pp. 161-179

[6] Saeed NB. Introduction to Robotics: Analysis, Control, Applications. 2nd ed. Beijing: Electronic Industry Press; 2013. pp. 136-153

[7] Alba-gomez OG, Pamanes JA, Wenger P. Trajectory planning of parallel manipulators for global performance optimization. In: International Symposium on Advances in Robot Kinematics, 1-5 January 2008. Netherlands: Springer; 2008. pp. 253-261

[8] Faires JD, Burden RL. Numerical Methods. 4th ed. Pacific Grove: Brooks/ Cole Publishing Company; 2012. pp. 157-170

[9] Les P, Wayne T. The NURBS Book. 2nd ed. Beijing: Tsinghua Press; 2010. pp. 32-72

Section 3 Robot Control

#### **Chapter 3**

## A Multi-Layer, Multi-Robot Control Architecture for Long-Range, Dynamic Communication Links

*John Shepard and Christopher Kitts*

#### **Abstract**

A unified motion control architecture is presented for dynamic, long-range multirobot communications networks, incorporating task abstraction that disassociates goals from implementation. In the task space, communication link states are specified, directly measured, and explicitly controlled yielding well-behaved task state trajectories. The control architecture uses task-level compensation to generate multi-robot formation mobility commands, and a cluster space controller transforms those formation commands to mobility commands for individual robots. The number of robots are selected to meet communications requirements and controlled through a multitask coordination capability incorporated within the architecture. Robustness to performance commands, system configuration parameters, and external disturbances is demonstrated through a variety of simulations and experiments. These show how robots are dynamically positioned and switched into or out of operation in order to meet communications requirements.

**Keywords:** multi-robot systems, mobile robots, communication networks, robot control, collaborative robots

#### **1. Introduction**

Robotic systems are an integral tool in modern society, extending the capability of human operators, increasing their productivity and improving their quality of life. Multi-robot systems are able to enhance quantity-sensitive performance metrics like speed, coverage, throughput, and redundancy. In addition, their spatial diversity provides added capabilities for tasks like formation-keeping [1–3], escorting or guarding [4, 5], surveillance and feature tracking [6–9], object manipulation [10], and more specialized tasks such as automatic lighting [11] reconfigurable sparse antenna arrays [12], and minimally invasive surgery [13, 14]. New research is developing techniques for diverse task-oriented groups of robots to work together to perform broader and more sophisticated missions [15, 16]; as the mission evolves, the tasks may be preserved even though the environment, the performance objectives, and the assignment of robots may change.

In this research, we examine the task of long-range and dynamic communication link management due to its necessity as a supporting role in many applications and missions. By "long-range" we refer to communication links that require a single series of repeater stations to relay communication between remote end stations. As such, mesh network approaches such as those described in [17, 18] which provide multiple communication paths are not appropriate for the applications of interest. By "dynamic" we refer to the need for robot relays to adjust their positions to compensate for changing link conditions due to motion of the end stations, the attenuation environment, communications equipment performance, etc.; as such, deploying static relay stations, as is done in [19, 20], is not sufficient.

Model-based approaches to link management use a model of the link along with information regarding relay robot positions in order to estimate quality of service. These are typically well behaved due to smoothing simplifications [21] but can be inaccurate. In [22], a simple binary model is used, where nodes are assumed to be connected if within a fixed distance, leading to a strategy that evenly spaces robots between end-stations. More sophisticated models may be used, incorporating path models based on power, distance, and line-of- sight in order to identify a set of goal positions for the robots [23–25].

Measurement-based approaches remove model inaccuracies, allowing for improved performance and an expanded workspace [26], and are robust to complexities of the link behavior like obstructions, directional antenna patterns, and multipath effects. For example, in [27], unmanned aircraft are used to establish a relay network, with the airborne relays circling control points and measuring link gradients to relocate to optimal locations. In [28], it was found that the use of measurementbased approaches improved performance in maintaining a line of sight connection between robots. In [29], optimization techniques exploited measurements to minimize the path traveled by relay robots as they manuever to a fixed location to support end-station communication. In [30], a centralized planner guides a multirobot team in known indoor environments to establish a multihop network, with measurements used to improve local positioning. Finally, in [31, 32], multi-robot mesh communications is achieved using a potential-function-based, decentralized control scheme and measurements of communication bit error rate.

The work presented in this paper addresses repositioning of communication relay nodes operating within a long-range, dynamic link in order to ensure that a measured Quality of Service (QoS) level, signal strength, is maintained. Service is achieved and maintained using a multi-layer control architecture. At the highest level, a task-oriented operational space control approach produces well-behaved task state compensations. These are converted to robot formation motion commands using a modelbased inverse Jacobian transform. Formation compensations are then converted to individual robot drive commands to achieve the necessary level of position control. A flexible number of robots is used to support the link, with robots being switched into and out of service as necessary; to be more precise, unused robots are actually switched to a benign position-control task that maintains their location in a ready position for future use in maintaining the link. Formally, all robots, regardless of their assigned task, are controlled through a single unified position control framework that dynamically changes the transforms used to convert between control state spaces.

Each element of this control architecture is well-motivated and provides a useful innovation compared to previous work in managing dynamic long-range communication links. First, direct sensing of and control implementation in the operational task space improves application-oriented performance. Second, use of this space abstracts

#### *A Multi-Layer, Multi-Robot Control Architecture for Long-Range, Dynamic Communication… DOI: http://dx.doi.org/10.5772/intechopen.111825*

the specification of desired performance from implementation details such as the number of robots used to achieve the task and the mobility characteristics of these robots, thereby preventing the redesign of the specification process when these implementation details are changed. Third, our approach flexibly engages an appropriate number of robots in the link management task, thereby conserving resources when the full suite of robots is not required. Fourth, use of the intermediate cluster space representation provides a critical layer of abstraction that reflects the geometric nature of the application, thereby making the construction of task level transforms simpler; furthermore, this control architecture provides an intuitive intermediate layer for supervisory operators when specifying and monitoring performance as well as for developers when incrementally designing, verifying and troubleshooting functionality of the multi-robot system. Fifth, the architecture unifies motion control for all robots within a single control architecture (whether the controller itself is centralized or decentralized) thereby facilitating development and performance analysis of large-scale, multi-task missions. Overall, our approach provides enhanced performance, minimizes the use of robot resources, promotes modular composition, and has been verified experimentally; results show the technique to be robust to the dynamic link environment based on moving end stations, local attenuation, and variation in the state of the communication relays.

The article starts with a synopsis of the basic control architecture in Section 2, discusses extension to the task space in Section 3, integrates multiple tasks in Section 4, describes the experimental test bed in Section 5, presents results of the experiments and simulations in Sections 6, and considers future work in Section 7.

#### **2. Cluster space control**

The first layer of our control architecture addresses mobility control of the multirobot cluster. While a number of techniques exist for multi-robot formation control, the Cluster Space control architecture abstracts the cluster as a virtual articulating mechanism, allowing explicit control of all system states. The interested reader should consult [33] for comparisons. The underlying goal of the Cluster Space technique is simple motion specification and control of multi-robot systems. This is accomplished by considering multiple robots as a single geometric entity. The pose of a cluster is described by its location and shape, which are related to individual robot positions through a set of kinematic transforms. For a system of *n* robots with *q* total degrees of freedom, the generalized cluster and robot pose vectors and their kinematic relationships are:

$$\begin{aligned} \vec{C} &\equiv \begin{bmatrix} c\_1\\ c\_2\\ \vdots\\ c\_q \end{bmatrix} = \begin{bmatrix} \mathcal{g}\_1(r\_1 \dots r\_q)\\ \mathcal{g}\_2(r\_1 \dots r\_q)\\ \vdots\\ \mathcal{g}\_q(r\_1 \dots r\_q) \end{bmatrix} \tag{1} \\\\ \vec{R} &\equiv \begin{bmatrix} r\_1\\ r\_2\\ \vdots\\ r\_q \end{bmatrix} = \begin{bmatrix} h\_1(c\_1 \dots c\_q)\\ h\_2(c\_1 \dots c\_q)\\ \vdots\\ h\_q(c\_1 \dots c\_q) \end{bmatrix} \tag{2} \end{aligned} \tag{2}$$

#### **Figure 1.**

*The multilayer control architecture. Layers exist for task-space, cluster space and robot space control, with inverse Jacobian resolved rate controllers used within each space.*

where *ri* are robot pose states, *ci* are cluster pose states, *gi* ð Þ … are kinematic equations, and *hi*ð Þ … are inverse kinematic equations [33, 34]. Cluster state velocities are linearly mapped from robot velocities using a Jacobian matrix.

These mathematical transformations are the basis for the layered cluster space control framework shown in **Figure 1**. On the right, in red, are the individual robots, which accept platform-level velocity commands. The blue region is the cluster space controller, shown in the form of a kinematic, resolved rate controller. This layer accepts cluster-level commands regarding cluster mobility and geometry; its outputs are robot velocity commands for each robot in order to achieve the cluster-level goals. Forward kinematic equations are used to compute cluster states from estimated robot states, and cluster control effort is transformed to robot velocity commands using the inverse cluster Jacobian matrix. This control technique has been implemented experimentally in a wide variety of multi-robot systems operating in land, sea, and air [4, 5, 7, 35, 36]. The yellow region of the diagram is described in the next section.

#### **3. Task space control**

Just as operational space [37] kinematic transforms are used to allow control of robots based on their actuator configuration, the cluster space methodology uses kinematic transforms to establish the control task in terms of multirobot formation mobility and geometry. Here, we add yet another operational space control later for task-oriented specification of behavior. As shown in **Figure 1**, each control layer uses a resolved rate controller that provides rate commands to an inverse Jacobian function, which converts those commands to rate set-points for the next layer (full dynamic controllers have been demonstrated in other works). Using this approach, operator commands are issued and controlled at the task level, and compensation commands are then successively transformed to cluster velocity, robot velocity and finally actuator velocity set-point commands as the control architecture executes. Each successive layer acts as an inner control loop for the preceding layer.

Given this, the long-range dynamic communication link management system presented in this paper involves two interacting tasks. The first is a "communication space" task that uses a measurement-based, link-balancing control strategy to space robots along the inline dimension between the end stations (a model-based

#### *A Multi-Layer, Multi-Robot Control Architecture for Long-Range, Dynamic Communication… DOI: http://dx.doi.org/10.5772/intechopen.111825*

assumption that in-line positioning is best has been made, leading to the use of a simple cross-track nulling controller for the second dimension); furthermore, a measurement-based controller varies the number of robots required by the task to minimally satisfy the desired aggregate communication quality set points. The second task is a simple position control task to maintain unused robots in a ready state. While the second task is trivial, we treat the management and interplay of these multiple distinct tasks in a formal manner.

In total, we are using a multi-layer, multi-task controller. In [24] we develop the conditions to ensure Lyapunov stability for each layer as well as the conditions for task switching. Space limitations prevents their further discussion in this work.

The following subsections demonstrate use of the layered control methodology for the communication and position control tasks. For each task, a formal definition of the layered state spaces is provided, the kinematic and Jacobian transforms used to convert between spaces are established, and controllers are provided for each layer. Both tasks assume the use of planar robots given that this was the type of robot used in the simulations and experiments described in Section VI.

#### **3.1 Example task: long range communication**

Consider the task of long-range communications between two exogenous nodes using mobile relays. To maintain the link quality, *nc* robotic relay nodes will move to intermediate locations based on desired link characteristics.

#### *3.1.1 Spaces and states*

The relevant spaces for this scenario are the individual robot space, the cluster space describing the geometry of the task-specific group of robots, and the communication task space. The robot space is defined by the pose of all robots, specified below for quantity *nc* robots:

$$\overrightarrow{r} \triangleq \begin{bmatrix} \varkappa\_1, \wp\_1, \theta\_1, \dots, \varkappa\_{n\_c}, \wp\_{n\_c}, \theta\_{n\_c} \end{bmatrix}^T \tag{3}$$

where *xi*, *yi* � � is the Cartesian position and *θ<sup>i</sup>* is the orientation of robot *i*, assuming planar operation.

The cluster space pose vector describes the location and shape of the cluster. In this case, the separation distances *ρ<sup>i</sup>* and chain angles *α<sup>i</sup>* define the geometry, as depicted in **Figure 2**; although many other cluster definitions are possible, this choice is convenient due to the serial nature of the communications task.

Accordingly, the cluster state vector is defined:

$$\overrightarrow{\mathcal{L}} \triangleq \begin{bmatrix} \pounds\_{\varepsilon}, \mathcal{y}\_{\varepsilon}, \theta\_{\varepsilon}, \rho\_{1}, a\_{1}, \phi\_{1}, \dots, \rho\_{n\_{\varepsilon}-1}, a\_{n\_{\varepsilon}-1}, \phi\_{n\_{\varepsilon}-1} \end{bmatrix}^{T} \tag{4}$$

In the task space, the user is interested in maintaining sufficient communication quality of service (QoS) between two end nodes, with signals being relayed as needed. Quality of service proved impractical to quantify in real time, so the system measures the link power between nodes using the received signal strength indicator (RSSI). For line-of-sight, the RSSI may be modeled as inversely proportional to the square of the distance between two points, hence:

$$\mathbf{s}\_{i} = \frac{k}{\left(\mathbf{x}\_{i+1} - \mathbf{x}\_{i}\right)^{2} + \left(\mathbf{y}\_{i+1} - \mathbf{y}\_{i}\right)^{2}} = \frac{k}{\rho\_{i}^{2}}\tag{5}$$

where *k* is a constant associated with the antenna gain. It is important to note that RSSI is measured directly but this model is used to compute the Jacobian, similar to a model-based approach for gain scheduling.

As depicted in **Figure 3,** the quality of service between the end nodes is influenced by both the crosstrack error, *ext*, and the angles of alignment, *γi*. Assuming a line of sight model, the maximum total signal strength is achieved by minimizing the crosstrack error and angles of alignment. The ratio or balance, *Bi*, of the link power in each segment is also important to avoid data rate bottlenecks or backup in homogeneous systems, or to allow for imbalanced transmission rates in nonhomogeneous

**Figure 2.** *Serial chain cluster diagram for an i-hop chain.*

**Figure 3.** *Long-range link diagram for a 4-hop link performed by 3 robots.*

*A Multi-Layer, Multi-Robot Control Architecture for Long-Range, Dynamic Communication… DOI: http://dx.doi.org/10.5772/intechopen.111825*

systems. The first and last links are functions of the position of the end nodes being connected *xE*<sup>1</sup> , *yE*<sup>1</sup> � �, *xE*<sup>2</sup> , *yE*<sup>2</sup> � � which are uncontrolled states of the environment. Lastly, the orientation of the robot, *ψi*, is included to fully define all degrees freedom of system. The communication "pose" vector is defined:

$$\overrightarrow{t}\left(\mathbf{x}\_{E\_1}, \mathbf{y}\_{E\_1}, \mathbf{x}\_{E\_2}, \mathbf{y}\_{E\_2}\right) \triangleq \begin{bmatrix} B\_1, \dots, B\_{n\_c}, \mathbf{e}\_{\text{xt}}, \mathbf{y}\_1, \dots, \mathbf{y}\_{n\_c - 1}, \mathbf{y}\_1, \dots, \mathbf{y}\_i \end{bmatrix}^T \tag{6}$$

Given these definitions, we define the desired states as follows. For a uniformly balanced network, *Bi* ¼ 1. For minimum crosstrack error for maximum link quality, *ext* ¼ 0. For aligning the robots between end points, *γ<sup>i</sup>* ¼ 0. In this case, the robots have a holonomic constraint and so the robot headings, *ψi*, are controlled at the platform level. Trajectory generation is a major topic of research for robotic communication networks [38] but it is not our focus in this work. These commands remain constant throughout each presented experiment, unless otherwise noted. It may be argued that more sophisticated control algorithms require less sophisticated command trajectories.

#### *3.1.2 Kinematic transformation equations*

Robot states are transformed into the cluster states using kinematic equations derived from formation geometry:

Cluster frame:

$$\propto\_c \triangleq \propto\_1 \ y\_c \triangleq \overline{\nu}\_1 \ \theta\_c \triangleq \theta\_1 \tag{7}$$

Chain length:

$$\rho\_i \triangleq \sqrt{\left(\mathbf{x}\_{i+1} - \mathbf{x}\_i\right)^2 + \left(\mathbf{y}\_{i+1} - \mathbf{y}\_i\right)^2} \tag{8}$$

Chain angle:

$$a\_i \triangleq \operatorname{atan2} \left( y\_{i+1} - y\_i, \mathbf{x}\_{i+1} - \mathbf{x}\_i \right) - \sum\_{j=1}^{i-1} a\_j \tag{9}$$

Node orientation:

$$
\phi\_i \triangleq \theta\_i \tag{10}
$$

where *atan*2ð Þ … , … *:* is the two-argument function that calculates a four-quadrant arc tangent with a range of ½ � *π*, �*π*

These cluster states are transformed into the task states using the measured link states and system geometry:

Balance:

$$B\_{i} \triangleq \frac{\rho\_{i+1}}{s\_{i}} = \begin{cases} \frac{(\mathbf{x}\_{E1} - \mathbf{x}\_{t1})^{2} + \left(y\_{E1} - y\_{t1}\right)^{2}}{\rho\_{1}^{2}} \text{ for } i = 1 \\\\ \frac{\rho\_{2}^{2}}{\left(\mathbf{x}\_{E2} - \mathbf{x}\_{t} + \rho\_{2}\cos(\alpha\_{1} + \alpha\_{2}) + \rho\_{1}\cos\alpha\_{1}\right)^{2} + \left(y\_{E2} - y\_{c} + \rho\_{2}\sin(\alpha\_{1} + \alpha\_{2}) + \rho\_{1}\sin\alpha\_{1}\right)^{2}} \text{ for } i = n\_{p} - 1 \\\\ \frac{\rho\_{i}^{2}}{\rho\_{i+1}^{2}} \text{ otherwise} \end{cases}$$

Crosstrack error:

$$e\_{\rm xf} = \sqrt{\frac{\left(\left(\mathbf{x}\_{E\_2} - \mathbf{x}\_{E\_1}\right)\left(\mathbf{y}\_{E\_1} - \mathbf{y}\_c\right) - \left(\mathbf{x}\_{E\_1} - \mathbf{x}\_c\right)\left(\mathbf{y}\_{E\_2} - \mathbf{y}\_{E\_1}\right)\right)^2}{\left(\mathbf{x}\_{E\_2} - \mathbf{x}\_{E\_1}\right)^2 + \left(\mathbf{y}\_{E\_2} - \mathbf{y}\_{E\_1}\right)^2}}\tag{12}$$

Angle of alignment

$$
\gamma\_i = a\_i \tag{13}
$$

Orientation:

$$
\psi\_i = \phi\_i \tag{14}
$$

where *xE*<sup>1</sup> , *yE*<sup>1</sup> � � and *xE*<sup>2</sup> , *yE*<sup>2</sup> � � are the positions of the end stations that are being connected by the multi-robot communication system.

#### *3.1.3 Jacobian matrices*

The Jacobian matrices are computed from the kinematic equations to map velocities between spaces. The solution is typically lengthy and so not shown here but easily computed using (5) with (7–10) for the cluster Jacobian or with (11)–(14) for the task Jacobian.

#### *3.1.4 Control design and performance*

In addition to determining the kinematic transforms, control laws must also be formulated. For these experiments, simple linear controllers suffice as the testbed platforms have well-behaved dynamics, but the system architecture can accommodate any type of control algorithm within each space.

As many commercially available robotic platforms control their own local velocity, a detailed discussion of platform control is not necessary. It is important to note that if the robots are nonholonomic, orientation and global translation are coupled which precludes independent control of all degrees of freedom. For details on our testbed nonholonomic control, please see [36].

The following equations can be used to design controllers in higher spaces using traditional techniques. The transfer functions at each layer can be approximated as linear, time-invariant (LTI) with proper tuning, maintaining diagonal dominance, and avoiding singularities. General system stability and performance is discussed in Section III.C.

Cluster space response:

$$\dot{\vec{\mathcal{L}}}^{\dot{\cdot}} = \left(\mathbf{J}\_{\varepsilon}^{-1} + \mathbf{G}\_{\eta}\mathbf{J}\_{\varepsilon}^{-1}\mathbf{H}\_{\varepsilon}\right)^{-1}\mathbf{G}\_{\eta}\mathbf{J}\_{\varepsilon}^{-1}\mathbf{H}\_{\varepsilon}\dot{\vec{\mathcal{L}}}\_{\,\,d} = \mathbf{G}\_{\varepsilon}\dot{\vec{\mathcal{L}}}\_{\,\,d} \tag{15}$$

Task space response:

$$\overrightarrow{t} = \left(\mathrm{J}\_t^{-1}\mathrm{s}I + \mathrm{G}\_c\mathrm{J}\_t^{-1}\mathrm{H}\_t\right)^{-1}\mathrm{G}\_c\mathrm{J}\_t^{-1}\mathrm{H}\_t\,\mathrm{\check{t}} = \mathrm{G}\_t\overrightarrow{t}\_d\tag{16}$$

*A Multi-Layer, Multi-Robot Control Architecture for Long-Range, Dynamic Communication… DOI: http://dx.doi.org/10.5772/intechopen.111825*

where *Gx* represents a diagonal matrix of transfer functions in space *x*, *Hx* represents a controller in space *x*. The system pose is represented by *r* in robot space, *c* in cluster space, and *t* in task space. As subscripts, these letters associate the variable with a space. Subscript *d* denotes desired states.

For these particular experiments, the cluster space control law utilizes proportional feedforward and feedback, shown below, for response time and error rejection respectively:

$$
\overrightarrow{\boldsymbol{u}}\_{\mathcal{E}} = \boldsymbol{H}\_{\mathcal{E}} \left( \overset{\cdot}{\boldsymbol{\mathcal{C}}}\_{\mathcal{d}}, \overset{\cdot}{\boldsymbol{\mathcal{C}}}\_{\mathcal{E}} \right) = \boldsymbol{K}\_{\mathcal{E}\_{f}} \overset{\cdot}{\boldsymbol{\mathcal{C}}}\_{\mathcal{d}} + \boldsymbol{K}\_{\mathcal{E}\_{\mathcal{P}}} \left( \overset{\cdot}{\boldsymbol{\mathcal{C}}}\_{\mathcal{d}} - \overset{\cdot}{\boldsymbol{\mathcal{C}}}\_{\mathcal{E}} \right) \tag{17}
$$

where *uc* denotes cluster space control effort, \_ *c* ! *<sup>d</sup>* denotes desired cluster velocity, *Kcf* denotes proportional feedforward gain matrix, and *Kcp* denotes proportional feedback gain matrix.

For these particular experiments, the communication task space uses proportional feedback control, shown below:

$$
\overrightarrow{\boldsymbol{u}}\_{t} = \boldsymbol{H}\_{t} \left( \overrightarrow{\boldsymbol{t}}\_{d}, \overrightarrow{\boldsymbol{t}} \right) = \boldsymbol{K}\_{t\_{p}} \left( \overrightarrow{\boldsymbol{t}}\_{d} - \overrightarrow{\boldsymbol{t}} \right) \tag{18}
$$

where *Ktp* is the feedback gain matrix and *t* ! *<sup>d</sup>* is the desired state. These desired states are discussed in Section III.A.1). While simplistic, these control laws yield sufficient performance in this application and our prior work with different tasks performed in land, sea, and aerial environments [4, 5, 7, 12, 39].

#### **3.2 Example task: position control**

In this task, *np* robots are tasked to go to and maintain a specified position. Since the task is a direct specification of individual robot positions, the task is identical to many one-robot clusters which is identical to control of individual robots. While this degenerates into trivial task, we adhere to the layered control architecture in order to provide unified control of all mission-related robots.

Space limitations prevent a complete description of this task. Given it's simplicity, key aspects of its implementation are reviewed here. To begin, the robot cluster and task pose vectors are:

$$\overrightarrow{r} \triangleq \begin{bmatrix} \varkappa\_1, \varkappa\_1, \theta\_1, \dots, \varkappa\_{n\_p}, \varkappa\_{n\_p}, \theta\_{n\_p} \end{bmatrix}^T \tag{19}$$

$$\overrightarrow{\mathcal{L}} \triangleq \begin{bmatrix} \mathcal{X}\_{c\_1}, \mathcal{Y}\_{c\_1}, \theta\_{c\_1}, \dots, \mathcal{X}\_{c\_{np}}, \mathcal{Y}\_{c\_{np}}, \theta\_{c\_{np}} \end{bmatrix}^T \tag{20}$$

$$\overrightarrow{t} \triangleq \begin{bmatrix} \boldsymbol{\omega}\_{t\_1}, \boldsymbol{\mathcal{y}}\_{t\_1}, \boldsymbol{\theta}\_{t\_1}, \dots, \boldsymbol{\mathcal{x}}\_{t\_{n\_p}}, \boldsymbol{\mathcal{y}}\_{t\_{n\_p}}, \boldsymbol{\theta}\_{t\_{n\_p}} \end{bmatrix}^T \tag{21}$$

where *xi*, *yi* , *θ<sup>i</sup>* � � is the robot *i* pose, *xci* , *yci* , *θci* � � is the cluster pose for cluster *<sup>i</sup>*, and *xti* , *yti* , *θti* � � is the task-level pose for robot *<sup>i</sup>*. For the position control task, the cluster positions and the task positions are both equated to the robot positions. Accordingly, the forward and inverse kinematic relationships are unity, and the Jacobians are the unit matrix.

For our experiments, the cluster space velocity control law utilizes proportional feedforward and feedback, shown below, for response time and error rejection respectively:

$$
\overrightarrow{\boldsymbol{\mu}}\_{c} = \boldsymbol{K}\_{c\_f} \overrightarrow{\boldsymbol{\mathcal{C}}}\_{d} + \boldsymbol{K}\_{c\_p} \left( \overrightarrow{\boldsymbol{\mathcal{C}}}\_{d} - \overrightarrow{\boldsymbol{\mathcal{C}}}^{\cdot} \right) \tag{22}
$$

where *u* ! *<sup>c</sup>* denotes cluster space control effort, \_ *c* ! *<sup>d</sup>* denotes the desired cluster velocity, *Kcf* denotes a proportional feedforward gain matrix, and *Kcp* denotes a proportional feedback gain matrix. Similarly, the task-space state controller utilizes proportional feedback for error rejection:

$$
\overrightarrow{u}\_t = K\_{t\_p} \left( \overrightarrow{t}\_d - \overrightarrow{t} \right) \tag{23}
$$

where *u* ! *<sup>t</sup>* denotes task space control effort, *t* ! *<sup>d</sup>* denotes the desired task state and *Ktp* denotes a proportional feedback control gain.

#### **4. Multi-tasking missions**

In the previous section, an approach for developing a layered task control architecture for the motion requirements of a multi-robot task is presented. Our interest, however, is not only in task-oriented control of a robot group but also in conducting activities that require multiple interacting tasks, each potentially implemented by a multi-robot group. We term the conduct of such an activity a "mission." Furthermore, we are interested in collaborative multi-task missions, with "collaboration" implying the ability for one task to support another and for tasks to share resources, such as robots, in an appropriate manner.

To achieve this vision, **Figure 4** shows a control architecture that integrates the two tasks required for the communication relay mission. The architecture integrates the operation of the tasks in two ways. First, on the "front end" of the architecture, a mission-level specification interface provides a mechanism for defining missionoriented objectives and assigning them to the tasks. In addition, the tasks are allowed to interact, providing a mechanism for tasks to issue commands to and to set constraints for each other. For the communication relay mission, the communication task acts as a master task in order to determine the number of robots it requires; once this determination is made, it specifies the number of remaining robots (to assign to a position hold task) to the position task for position control. In ongoing work with more complex missions, the robot allocation process is independent of any one task. Second, on the "back end" of the architecture, unified motion control is enabled by consolidating the control elements of multiple tasks as described in the following subsection.

Multi-agent systems have the capacity for functional diversity which motivates the ability to change roles during operation. As such, an additional focus of this work investigates the process of reallocating autonomous agents within the given framework. Doing so requires (A) a framework for supporting and transitioning multiple clusters within the same architecture, (B) defining allocation policies to determine when and how to reallocate resources between tasks, and (C) analytic methods to safely transition between robot configurations.

*A Multi-Layer, Multi-Robot Control Architecture for Long-Range, Dynamic Communication… DOI: http://dx.doi.org/10.5772/intechopen.111825*

**Figure 4.**

*A multi-task collaborative Mission control architecture. The diagram shows the consolidated control architecture with additional functions to provide mission specification and resource allocation.*

#### **4.1 Representation framework**

Simultaneously accomplishing multiple mobility tasks requires assigning different tasks to different robots, which, in our framework, implies the use of multiple taskspecific robot clusters. While multiple instances of the control framework of Section III could be run in parallel, this approach is static and unable to conveniently support the reconfiguration of clusters as robots are reallocated. Instead, the single-task multilayered framework is extended to become a multi-task controller that operates on a "federated" system state vector. At a given moment in time, given an assignment of a specific number of robots to specific tasks as defined by the robot allocation vector in (39), the federated kinematic equations and federated Jacobian matrices are composed of the cluster-specific kinematic equations and Jacobian matrices, as shown in (41) and (42).

Robot allocation vector:

$$\overrightarrow{n} = [n\_1, n\_2, \dots, n\_o]^T\\\text{where } n = \sum\_{i=1}^o n\_i \tag{24}$$

Federated pose vectors:

$$
\overrightarrow{r}\_f \triangleq \begin{bmatrix} \overrightarrow{r}\_1, \overrightarrow{r}\_2 \dots, \overrightarrow{r}\_o \end{bmatrix}^T \tag{25}
$$

$$
\overrightarrow{\boldsymbol{c}}\_{f} \triangleq \begin{bmatrix} \overrightarrow{\boldsymbol{c}}\_{1}, \overrightarrow{\boldsymbol{c}}\_{2} \dots, \overrightarrow{\boldsymbol{c}}\_{o} \end{bmatrix}^{T} \tag{26}
$$

$$\overrightarrow{t}\_f \triangleq \begin{bmatrix} \overrightarrow{t}\_1, \overrightarrow{t}\_2 \dots, \overrightarrow{t}\_o \end{bmatrix}^T \tag{27}$$

Federated kinematic equations:

$$\text{KIN}\_{c\_f} \left( \stackrel{\rightarrow}{c\_f} \right) \triangleq \left[ \text{KIN}\_{c\_1} \left( \stackrel{\rightarrow}{r}\_1 \right), \text{KIN}\_{c\_2} \left( \stackrel{\rightarrow}{r}\_2 \right), \dots, \text{KIN}\_{c\_o} \left( \stackrel{\rightarrow}{r}\_o \right) \right]^T \tag{28}$$

$$\text{KIN}\_{t\_f} \left( \stackrel{\rightarrow}{t\_f} \right) \triangleq \left[ \text{KIN}\_{t\_1} \left( \stackrel{\rightarrow}{c}\_1 \right), \text{KIN}\_{t\_2} \left( \stackrel{\rightarrow}{c}\_2 \right), \dots, \text{KIN}\_{t\_\bullet} \left( \stackrel{\rightarrow}{c}\_\bullet \right) \right]^T \tag{29}$$

Federated Jacobian matrices:

$$J\_{c\_f} \triangleq \begin{bmatrix} J\_{c\_1} & \cdots & \mathbf{0} \\ \vdots & \ddots & \vdots \\ \mathbf{0} & \cdots & J\_{c\_o} \end{bmatrix} \tag{30}$$
 
$$J\_{c\_f} \triangleq \begin{bmatrix} J\_{c\_1} & \cdots & \mathbf{0} \\ \vdots & \ddots & \vdots \\ \mathbf{0} & \cdots & J\_{c\_o} \end{bmatrix} \tag{31}$$

where subscript *f* denotes federated elements; *ni* is the number of robots assigned to task *i*; *r* ! *<sup>i</sup>*,*c* ! *<sup>i</sup>* and *t* ! *<sup>i</sup>* are the robot space, cluster space, and task space pose vectors for task *i*; *KINci r* ! *i* � � and *KINti <sup>c</sup>* ! *i* � � are the cluster space and task space kinematic equations for task *i*; *Jci* and *Jti* are the cluster space and task space Jacobian matrices for task *i*; and *o* is the number of tasks spanning the multi-robot system. The federated pose vector is formulated by concatenating pose vectors, and similarly for the federated kinematic equations. The federated Jacobian matrix is block-diagonal, comprised of uncoupled Jacobian matrices. As the agents shift between tasks, individual elements change size, but the size of the federated elements and the overall structure of the mission-level control system remain constant.

As a simple example of multitasking, consider combining the two previously described tasks into the following two-task mission: maintain communication between two end points or otherwise move to an idle parking position. A subset of the federated elements are shown below for two configurations of a *n* ¼ 3 robot system: (1) one robot is allocated to the communications task and two robots are idle (*n* ! <sup>¼</sup> *ncom*, *nidle* ½ �*<sup>T</sup>* <sup>¼</sup> ½ � 1, 2 *<sup>T</sup>*) and (2) two robots are allocated to the communications task and one robot is idle (*n* ! <sup>¼</sup> *ncom*, *nidle* ½ �*<sup>T</sup>* <sup>¼</sup> ½ � 2, 1 *<sup>T</sup>*):

Federated cluster pose vector:

$$\overrightarrow{\boldsymbol{c}}\_{M} = \begin{cases} \begin{bmatrix} \left[\boldsymbol{\kappa}\_{\varepsilon}, \boldsymbol{\mathcal{y}}\_{\varepsilon}, \theta\_{\varepsilon}\right] \left[\boldsymbol{\kappa}\_{I\_{1}}, \boldsymbol{\mathcal{y}}\_{I\_{1}}, \theta\_{I\_{1}}, \boldsymbol{\mathcal{x}}\_{I\_{1}}, \boldsymbol{\mathcal{y}}\_{I\_{2}}, \theta\_{I\_{2}}\right] \end{bmatrix} \prescript{T}{}{\textbf{for}}\ \overrightarrow{n} = \begin{bmatrix} \boldsymbol{1}, \boldsymbol{2} \end{bmatrix}^{T} \\\\ \begin{bmatrix} \left[\boldsymbol{\kappa}\_{\varepsilon}, \boldsymbol{\mathcal{y}}\_{\varepsilon}, \theta\_{\varepsilon}, \boldsymbol{\rho}\_{1}, \boldsymbol{\alpha}\_{1}, \boldsymbol{\phi}\_{1}\right] \left[\boldsymbol{\kappa}\_{I\_{1}}, \boldsymbol{\mathcal{y}}\_{I\_{1}}, \theta\_{I\_{1}}\right] \end{bmatrix}^{T} \text{for } \overrightarrow{n} = \begin{bmatrix} \boldsymbol{2}, \boldsymbol{1} \end{bmatrix}^{T} \end{cases} \tag{32}$$

#### **4.2 Allocation policies**

Given the general desire to accommodate multiple tasks and the ability to reassign robots between tasks, we need to incorporate an allocation function for assigning specific robotics to specific tasks. The allocation problem is well studied [38, 40, 41], and we are not proposing any particular innovations in this area; rather, our interest is in determining how any such allocation policy fits into the proposed architecture. To date, we believe that the necessary interface consists of providing the allocation policy with the commanded and actual task state vector such that it can compute *n* !, which is consistent with the approaches discussed in [38]. This value is then provided to each layer of the control architecture in order for controllers and kinematic transforms with appropriate internal dimensions to be selected.

*A Multi-Layer, Multi-Robot Control Architecture for Long-Range, Dynamic Communication… DOI: http://dx.doi.org/10.5772/intechopen.111825*

To explore this set of interfaces, we have adopted a simple-but-common state-machine based allocation policy for the communication-idle task allocation process. In particular, we define an aggregate QoS metric known as the chain capacity [27]:

$$\delta \triangleq \frac{n\_{com} + 1}{\sum\_{i=1}^{n\_{com} + 1} \frac{1}{s\_i}} \tag{33}$$

which we use for the transition function:

$$[n\_1 \ n\_2] = \begin{cases} [n\_1 + \mathbf{1} \quad n\_2 - \mathbf{1}], \text{if } \delta < k\_1 \\\\ [n\_1 - \mathbf{1} \quad n\_2 + \mathbf{1}], \text{if } \delta > k\_2 \\\\ [n\_1 \quad n\_2], \text{otherwise} \end{cases} \tag{34}$$

where k1 and k2 are performance thresholds that together provide switching hysteresis. For the communication systems and scenarios explored later in this paper, k1 and k2 were set to values equivalent to the line-of-sight link power at 40 m and 10 m, respectively. For example, if *n* ¼ ½ � 2 1 and the signal strength was measured *s* ¼ ½ � �<sup>75</sup> �<sup>76</sup> �<sup>78</sup> *<sup>T</sup>*dBm giving *<sup>δ</sup>* ¼ �76*:*4dBm, and the thresholds were *<sup>k</sup>* <sup>¼</sup> ½ � �<sup>52</sup> �<sup>76</sup> *<sup>T</sup>*dBm, then the next robot allocation would be *<sup>n</sup>* <sup>¼</sup> ½ � 3 0 .

To reiterate, this policy sets the number of robots to achieve a given link quality while the task-level control provides link balance as stated in (11). This policy is appropriate for the communication relay application and it is also consistent with the general interface requirements hypothesized in the previous paragraph. Future researchers can incorporate the allocation policy that is most appropriate for their tasks, whether state of the art techniques (see [38]) or commonplace yet effective techniques like state machines.

#### **5. Experimental testbed**

Experimental work used the proven [35] SCU multi-robot test bed, with a communication relay payload. This student-developed system consists of several Pioneer 3-AT skid steered robots with a custom suite of avionics and a centralized off-board control workstation. Wireless 28.8 kbps Ricochet modems are used to relay robot drive commands and position data between the control workstation and the robots. BasicX microcontrollers route drive commands to the robot's built-in speed controller and collect data from a Garmin GPS18 unit and a Devantech CMPS10 compass. Based on experimental evaluation, robot velocity dynamics are approximated as a secondorder system with *ζ* ¼0.7 and *ω<sup>n</sup>* ¼ 2*π*0*:*25 rad/s; simulations used these speed response characteristics for each robot.

Within the control workstation, an open source real-time data streaming server, known as the DataTurbine, relays information between MATLAB/Simulink and simple applications that handle serial port data flow to/from the wireless modems. Controllers execute in real-time within Simulink; this promotes rapid, iterative development in the field and supports rudimentary operator interfaces. Using a dual-core laptop computer, the system maintains a 5 Hz servo loop rate.

**Figure 5.**

*Components in the multi-robot communications relay test bed.*

Each robot carries a communications relay payload comprised of two Digi International XBee Series 2 wireless transceivers connected by a BASIC Stamp microcontroller, shown in **Figure 5**. The microcontroller appends the received signal strength indicator (RSSI) value to each message prior to retransmission. When the end station ultimately receives the message, it also obtains the RSSI state for the multihop link. This state data is provided to the task layer controller in order to determine how to adjust the position of the relay cluster. Messages and the associated RSSI measurements were generally executed at 1 Hz, but dropouts due to the inexpensive hardware often resulted in short periods of slower execution, adding a realistic challenge to the experiment.

#### **6. Experimental results**

A number of simulations and experiments were executed to demonstrate functionality of the system and to showcase particular advantages of the control architecture. First, two scenarios were examined with the single communications task: (A) an experiment showing the single robot system response to hardware configuration changes such as reductions in transmission power, and (B) a simulation showing multi-robot system response to a mobile end station and environmental attenuation. Next, three scenarios were examined performing multiple tasks of communications and idle position control, thereby allowing robots to be added or removed from the communication task: (C) an experiment showing responses to desired link quality commands that require robot reallocation, (D) a simulation showing system response to a moving mobile end station in which robot reallocation is required, and (E) an experiment showing the same capability as in D. Experimental work has demonstrated that the architecture is tolerant of real-world phenomena such as sensor noise, quantization, model mismatches, and communication delays. Simulations allowed rapid exploration with higher numbers of robots, allowing clear demonstration of the architecture behavior without hardware constraints.

*A Multi-Layer, Multi-Robot Control Architecture for Long-Range, Dynamic Communication… DOI: http://dx.doi.org/10.5772/intechopen.111825*

#### **6.1 Experiment: single task single robot behavior with hardware configuration change**

This scenario examines the control system response to configuration changes such as component degradation or a power reduction used to conserve energy. One robot is used to relay communications between two fixed end stations. The system is commanded to achieve unity link balance with null crosstrack error. In this experiment, the system initially moves to an equilibrium position given a nominal communications configuration. Then, at t = 800 sec, the transmission power of end station *E*<sup>2</sup> is reduced, and the relay robot moves to achieve link equilibrium.

An overhead view of robot position is shown in **Figure 6** where each subplot corresponds to a different time window; in the first, the robot moves to an equilibrium position, and in the second, the robot adjusts its position to balance the link given the power reduction at station E2. In **Figure 7** the RSSI values and the link balance parameter (commanded to 1) are shown. In each phase, the systems moves to achieve the commanded link balance, resulting in balanced RSSI values.

#### **6.2 Simulation: single task multi-robot behavior with a mobile end station and local attenuation**

This scenario evaluates system behavior given local attenuation effects such as obstructions, fog, or foliage. Three robots are used to relay communications between a fixed base station and a mobile end-node. A comparison is made of the system's performance with and without measurement compensation for these effects to demonstrate how our communications task improves performance compared to the use of a simple link model.

A overhead view is shown in **Figure 8** with robot trajectories plotted for both ideal and attenuated scenarios. Messages are relayed between the base node, located at the origin of the plot, and the mobile end-node, which has a quarter-circle trajectory plotted in black and running from (60,0) to (0,60). A region of power attenuation exists for *y*> 40, where any link involving a robot within this area is reduced by half. As the remote end-node traverses its arc at a constant speed, a three-robot cluster maintains link balance as described before. In the ideal case, the robots spread evenly and follow the traverse in concentric arcs, consistent with a model-based approach. In

#### **Figure 6.**

*Overhead view of positions of robot R***<sup>1</sup>** *and fixed end nodes E***<sup>1</sup>** *and E***<sup>2</sup>** *at specified times during hardware configuration change experiment.*

**Figure 7.**

*Time history of link power and balance ratio during hardware configuration change experiment.*

#### **Figure 8.**

*Overhead view of robot R positions comparing trajectories in ideal transmission environments (dashed) and trajectories responding to an encountered region of attenuation (solid).*

the non-ideal case with attenuation, the multi robot system begins as before, but alters its trajectory to rebalance the links when it senses a drop in signal strength as nodes enters the region of attenuation.

*A Multi-Layer, Multi-Robot Control Architecture for Long-Range, Dynamic Communication… DOI: http://dx.doi.org/10.5772/intechopen.111825*

This example demonstrates the value of direct measurement of communication states and high-level task-space control. Sensing the signal strength allows the system to maintain the desired state despite unanticipated characteristics of the environment. In contrast, an open-loop, model-based approach would evenly distribute the nodes as shown, yielding lower performance in non-ideal environments.

#### **6.3 Experiment: multi-task multi-robot behavior with configuration link quality command response**

This scenario demonstrates changing user requirements for better connectivity or higher throughput thereby forcing a change in the cluster configuration. With fixed communication endpoints and an increase in the commanded link quality, two robots are sequentially reallocated from the idle task to the communication relay task. Each newly incorporated robot moves from its idle position to a location determined through execution of the communication task in order to achieve the commanded link quality and link balance set-points while the other robots in the communication task reposition themselves accordingly.

**Figure 9** shows the fixed end nodes *E*<sup>1</sup> and *E*<sup>2</sup> and two mobile robots. In the top plot, for time *t* ¼ ½ � 0 : 155 , the commanded link quality is such that the two end

**Figure 9.** *Overhead view of positions of robots R in response to a link quality command between end nodes E:*

stations communicate directly with each either without the need for a relay node. As a result, the configuration is *n* ! <sup>¼</sup> ½ � 0, 2 *<sup>T</sup>*, with both robots assigned to an idle task and holding their position. As seen in **Figure 10**, link quality is maintained within an acceptable deadband; the link balance is not shown given the single hop.

For time *t* ¼ ½ � 155 : 501 , the link quality setpoint is increased. As seen in **Figure 10b**, the link quality deadband is violated, leading to a change in the assignment of tasks such that *n* ! <sup>¼</sup> ½ � 1, 1 *<sup>T</sup>*, shown in **Figure 10a**. This results in controlling B1, the link balance between the two existing sub-links, by achieve balance as shown in **Figure 10c** by the repositioning of robot 1, as shown in **Figure 9b**.

During time *t* ¼ ½ � 501 : 800 , the system responds to another command to increase link quality. Again, the link quality deadband is violated, leading the addition of robot *R*<sup>2</sup> to the communication task. With two intermediate robots, two link balancing operations take place in parallel, shown in **Figure 10c**. The initial position of the added robot creates a large switching transient in the link balance. Accordingly, the robots alter their positions, as shown in **Figure 9c**.

The plots in **Figure 10** show that the sensed RSSI parameters were clearly not ideal, exhibiting noise and quantization and the effects of a wide range on non-ideal characteristics of the wireless links. Because of these non-ideal characteristics, the robots do not move to the geometric center of the end points. These real-world phenomena are challenging but the control architecture is sufficiently robust to tolerate these unmodeled effects.

#### **6.4 Simulation: multi-task multi-robot behavior with a mobile end station**

This simulation demonstrates control of link quality and balance with a mobile endpoint, gracefully adding and removing robots as appropriate for the task. The

*A Multi-Layer, Multi-Robot Control Architecture for Long-Range, Dynamic Communication… DOI: http://dx.doi.org/10.5772/intechopen.111825*

scenario starts with a mobile end station communicating directly with a fixed end station and with five robots executing the idle position hold task. As the mobile end station executes an elliptical trajectory that moves it away from and then closer to the fixed end station, the five relay robots are sequentially added and then removed to the communication task, maintaining the specified link quality and balance (**Figure 11**).

The evolution of the system's state can be seen in **Figure 12**. For time t = [0,500], the end station moves away from the fixed end station, lowering link quality, as shown in **Figure 12b**. At times t = �110 sec, 193 sec, 265 sec, �340 sec and � 490 sec, the link quality hits the threshold for acceptable link quality. At each of these times, the controller re-assigns a new robot from the idle task to the communications task, thereby causing *n* ! to change at these times, as seen if **Figure 12a**, and new robots being added to the multi-hop link, as seen in **Figure 12c**; idle robots are controlled to remain in their default position. These additions to sub-links in the communication chain lead to new balance parameters to be controlled, B1 through B5.

As the mobile end station turns back towards the base node, the link quality increases. Each time this value hits the high deadband, at times t = 845 sec, �870 sec, �890 sec, �910 sec, and � 935 sec, an active communication task robot is returned to the idle task. This reduces the number of link balance parameters, leading to a transient in link balance that is quickly controlled through the repositioning of the remaining communication task robots. Interestingly, the deadband causes unequal times between transitions as the robots are faster to move out due to the task state definition and allocation policy and slower to move into the communication cluster.

**Figure 11.** *Overhead view of robots R and end nodes E during specified times for mobile endpoint simulation.*

**Figure 12.** *Time history of key system states for mobile endpoint simulation.*

This demonstrates the ability of the control architecture to respond to motion of the end node based on sensed link characteristics and to reallocate robots without any additional commanding.

#### **6.5 Experiment: Multi-task multi-robot behavior with a mobile end station**

Like the simulation presented in Section VI.D, this experiment demonstrates the control of link quality and balance with a mobile end node. The experiment starts with the end stations near each other and directly communicating, with two relay robots in an idle position. As the mobile end station moves away, relay robots are sequentially added to maintain the specified level of link quality and balance.

**Figure 13** shows the paths taken by the robots and endpoints, and **Figure 14** shows the corresponding state trajectories. In **Figure 13a**, for time *t* ¼ ½ � 0 : 148 , the mobile endpoint can be seen moving away from moving away from the stationary endpoint while the link quality remains within the deadband. The communication relay robots are allocated to idle, *n* ! <sup>¼</sup> ½ � 0, 2 *<sup>T</sup>*, and can be seen parking themselves.

At time *t* ¼ 148, the link quality exceeds the lower bounds of the deadband and the allocation policy adds a robot to the communication relay task, changing the configuration vector to *n* ! <sup>¼</sup> ½ � 1, 1 *<sup>T</sup>*. In **Figure 13b**, for time *<sup>t</sup>* <sup>¼</sup> ½ � <sup>148</sup> : <sup>591</sup> , the new robot relay moves to balance the communication links while the mobile end station continues moving away from the stationary endpoint. Though there is not significant movement of the relay robot, the measured link states, shown in **Figure 14**, indicate that the balance setpoint is achieved during this time. This demonstrates the complexity and non-intuitiveness of RF fields and the benefit of communication-space measurement

*A Multi-Layer, Multi-Robot Control Architecture for Long-Range, Dynamic Communication… DOI: http://dx.doi.org/10.5772/intechopen.111825*

**Figure 13.** *Overhead view of robots R and end nodes E during specified times for mobile endpoint experiment.*

**Figure 14.** *Time history of key system states for mobile endpoint experiment.*

and control, including the effects of the deadbands; alternatively locating the relay robots in the geometric center of the two points would yield worse performance.

At time *t* ¼ 591, the link quality again exceeds the lower bounds of the deadband and the allocation policy adds the second robot to the communication relay task, changing the configuration vector to *n* ! <sup>¼</sup> ½ � 2, 0 *<sup>T</sup>*. In **Figure 13c**, for time *t* ¼ ½ � 591 : 1062 , both robots move to balance the communication links. The switching transient can be seen in **Figure 14** starting at t = �600 sec and settling by t = �950 sec. The final overhead plot, **Figure 13d**, shows the mobile endpoint arcing back towards the stationary endpoint and the relay robots mimic its motion to maintain link balance.

#### **7. Future work and summary**

In this article, we presented a multi-robot control architecture providing explicit task control for improved performance yet with abstraction from implementation. Direct sensing and operational task space control eliminate errors due to modeling and implicit specification. In addition, controllers can compensate for non-ideal behavior in the appropriate space of the layered architecture. Abstraction provides the flexibility to engage different types and quantities of robots to accomplish tasks. This segregates individual task complexity in order to facilitate large-scale missions with many tasks. We proposed a design methodology for composing new spatially-sensitive tasks that includes conditions for stability and quantification of responsiveness. The architecture was demonstrated using the example task of communication. Experiments and simulations exhibited explicit control of task states, compensating for the complex behavior inherent in real-world communication networks. The system successfully reacts to a dynamic environment, varying operator commands, and hardware configuration changes.

Ongoing work leverages this architecture for larger missions comprised of more tasks with more complex interactions. This article is a step towards multi-task missions indicative of systems of systems. Continued development of this architecture with new applications and new environments increases the utility of integrated multi-agent systems.

#### **Author details**

John Shepard and Christopher Kitts\* Santa Clara University, Santa Clara, CA, USA

\*Address all correspondence to: ckitts@scu.edu

© 2023 The Author(s). Licensee IntechOpen. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

*A Multi-Layer, Multi-Robot Control Architecture for Long-Range, Dynamic Communication… DOI: http://dx.doi.org/10.5772/intechopen.111825*

#### **References**

[1] Joordens MA, Jamshidi M. Consensus control for a system of underwater swarm robots. IEEE Systems Journal. 2022;**4**(1):65-73

[2] Balch T, Arkin RC. Behavior-based formation control for multi-robot teams. IEEE Transactions on Robotics and Automation. 1998;**14**(6):926-939

[3] Ailon A, Zohar I. Control strategies for driving a group of nonholonomic kinematic mobile robots in formation along a time-parameterized path. IEEE/ ASME Transactions on Mechatronics. 2012;**17**(2):326-336

[4] Mahacek P, Kitts C, Mas I. Dynamic guarding of marine assets through cluster control of automated surface vessel fleets. IEEE/ASME Transactions on Mechatronics. 2012;**17**(1):65-75

[5] Mas I, Li S, Acain J, Kitts CA. Entrapment/escorting and patrolling missions in multi-robot cluster space control. In: IEEE/RSJ International Conference on Intelligent Robots and Systems; St. Louis, USA. New York City, NY, USA: IEEE; 2009

[6] Elston J, Frew EW. Hierarchical distributed control for search and tracking by heterogeneous aerial robot networks. In: IEEE International Conference on Robotics and Automation; Pasadena, CA, USA. New York City, NY, USA: IEEE; 2008

[7] Adamek T, Kitts C, Mas I. Gradientbased cluster space navigation for autonomous surface vessels. IEEE/ASME Transactions on Mechatronics. 2014;**20** (2):506-518

[8] Li S, Kong R, Guo Y. Cooperative distributed source seeking by multiple robots: Algorithms and experiments.

IEEE/ASME Transactions on Mechatronics. 2014;**19**(6):1810-1820

[9] Jevtic A, Gutierrez A, Andina D, Jamshidi M. Distributed bees algorithm for task allocation in swarm of robots. IEEE Systems Journal. 2012;**6**(2): 296-304

[10] Ross A, Layton T, Romanishin J, Rus D. IkeaBot: An autonomous multi-robot coordinated furniture assembly system. In: IEEE International Conference on Robotics and Automation; Karlsruhe, Germany. New York City, NY, USA: IEEE; 2013

[11] Yoon D-K, Seo J-T, Yi B-J. Automatic lighting system using multiple robotic lamps. IEEE/ASME Transactions on Mechatronics. 2014;**19**(3):963-974

[12] Okamoto G, Chen C, Kitts C. Beamforming performance for a reconfigurable sparse array smart antenna system via multiple mobile robotic systems. In: Proc of the SPIE - The International Society for Optical Engineers. 2010

[13] Tortora G, Dario P, Menciassi A. Array of robots augmenting the kinematics of endocavitary surgery. IEEE/ASME Transactions on Mechatronics. 2014;**19**(3):1821

[14] Zemiti N, Morel G, Ortmaier T, Bonnet N. Mechatronic design of a new robot for force control in minimally invasive surgery. IEEE/ASME Transactions on Mechatronics. 2007; **12**(2):143-153

[15] Ansola PG, Higuera AG, Otamendi FJ, Morenas J. Agent-based distributed control for improving complex resource scheduling: Application to airport

ground handling operations. IEEE Systems Journal. 2014;**8**(4):1145-1157

[16] Grocholsky B, Keller J, Kumar V, Pappas G. Cooperative air and ground surveillance. IEEE Robotics and Automation Magazine. 2006;**2006**:16-26

[17] Kudelski M, Gambardella LM, Di Caro GA. A mobility-controlled link quality learning protocol for multi-robot coordination tasks. In: 2014 IEEE International Conference on Robotics and Automation (ICRA). 2014. pp. 5024-5031

[18] Kim J, Ladosz P, Oh H. Optimal communication relay positioning in mobile multi-node networks. Robotics and Autonomous Systems. 2020;**129**: 103517

[19] Pezeshkian N, Nguyen HG, Burmeister A. Unmanned ground vehicle radio relay deployment system for nonline-of-sight operations. In: Proceedings of the 13th IASTED International Conference on Robotics and Applications (RA '07). San Diego CA, USA: Space and Naval Warfare Systems Center; 2007. pp. 501-506

[20] Ollero A, Marron PJ, Bernard M, et al. AWARE: Platform for autonomous self-deploying and operation of wireless sensor-actuator networks cooperating with unmanned AeRial vehiclEs. In: Proc of IEEE International Workshop on Safety, Security and Rescue Robotics (SSRR '07). New York City, NY, USA: IEEE; 2007. pp. 1-6

[21] Pezeshkian N, Neff J, Hart A. Link quality estimator for a mobile robot. In: Int. Conf. on Informatics in Control, Automation and Robotics; Rome, Italy. 2012

[22] Mehta VK, Arrichiello F. Ithica, NY, USA: Cornell University; 2013. [Online]. http://arxiv.org/abs/1312.2526v1

[23] Sweeney J, Grupen R, Shenoy P. Active QoS flow maintenance in controlled mobile networks. In: The Fourth International Symposium on Robotics and Automation (ISRA); Queretaro, Mexico. 2005

[24] Shepard J, Kitts C. A multi-robot control architecture for collaborative missions comprised of tightly- coupled, interconnected tasks. IEEE Systems Journal. 2016;**12**(2):1435-1446

[25] Gao Y, Chen H, Li Y, Lyu C, Liu Y. Autonomous Wi-Fi relay placement with mobile robots. IEEE/ASME Transactions on Mechatronics. 2017;**22**(6):2532-2542

[26] Im H-J, Lee C-E, Cho Y-J, Sunghoon K. RSSI-based control of mobile cooperative robots for seamless networking. In: Control, Automation, and Systems, International Conference on; Jeju Island, Korea. New York City, NY, USA: IEEE; 2012

[27] Dixon C, Frew EW. Optimizing cascaded chains of unmanned aircraft acting as communication relays. IEEE Journal on Selected Areas in Communications. New York City, NY, USA: IEEE; 2012;**30**(5):883-898

[28] Bryan J. Thibodeau, Andrew H. Fagg, and Brian N. Levine, "Signal Strength Coordination for Cooperative Mapping; Amherst, USA, 2005.

[29] Min BC, Kim Y, Lee S, Jung J, Matson ET. Finding the optimal location and allocation of relay robots for building a rapid end-to-end wireless communication. Ad Hoc Networks. Amsterdam, Netherlands: Elsevier; 2016; **39**:23-44

[30] Stephan J, Fink J, Kumar V, Ribeiro A. Concurrent control of mobility and communication in multirobot systems.

*A Multi-Layer, Multi-Robot Control Architecture for Long-Range, Dynamic Communication… DOI: http://dx.doi.org/10.5772/intechopen.111825*

IEEE Transactions on Robotics. 2017;**33** (5):1248-1254

[31] Bezzo N, Yan Y, Fierro R, Mostofi Y. A decentralized connectivity strategy for mobile robot swarms. In: 18th World Congress of the International Federation of Automatic Control; Milan, Italy. New York City, NY, USA: IFAC; 2011. pp. 4501-4506

[32] Bezzo N et al. A cooperative heterogeneous mobile wireless mechatronic system. IEEE/ASME Transactions on Mechatronics. New York City, NY, USA: IEEE; 2014;**19**(1): 20-31

[33] Kitts C, Mas I. Cluster space specification and control of mobile multirobot systems. IEEE/ASME Transactions on Mechatronics. 2009;**14** (2):207-218

[34] John J. Craig, Introduction to Robotics: Mechanics and Control. 3rd ed. Pearson Prentice Hall; 2005

[35] Mas I, Petrovic O, Kitts C. Cluster space specification and control of a 3 robot mobile system. In: Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on; Pasadena, CA, USA. 2008. pp. 3763-3768

[36] Mas I, Kitts C. Obstacle avoidance policies for cluster space control of nonholonomic multirobot systems. IEEE/ASME Transactions on Mechatronics. New York City, NY, USA: IEEE; 2012;**17**(6):1068-1079

[37] Khatib O. A unified approach for motion and force control of robot manipulators: The operational space formulation. Robotics and Automation, IEEE Journal of. 1987;**3**(1):1068

[38] Kopeikin A, Ponda SS, How JP. Control of communication networks for teams of UAVs. In: Valavanis KP, Vachtsevanos GJ, editors. Handbook of Unmanned Aerial Vehicles. Springer Science+Business; 2015

[39] Agnew MS, Canto PD, Kitts CA, Li SQ. Cluster space control of aerial robots. In: IEEE/ASME International Conference on Advanced Intelligent Mechatronics; Montreal, Canada. New York City, NY, USA: IEEE; 2010

[40] Ayorkor Korsah G, Stentz A, Bernardine Dias M. A comprehensive taxonomy for multi-robot task allocation. The International Journal of Robotics Research. 2013;**32**(12): 1495-1512

[41] Ponda SS, Johnson LB, Kopeikin AN, How JP. Distributed planning strategies to ensure network connectivity for dynamic heterogeneous teams. IEEE Journal on Selected Areas in Communications. June 2012;**30**(5): 861-869

## Controlling a Fleet of Autonomous LHD Vehicles in Mining Operation

*Alexander Ferrein, Gjorgji Nikolovski, Nicolas Limpert, Michael Reke, Stefan Schiffer and Ingrid Scholl*

#### **Abstract**

In this chapter, we report on our activities to create and maintain a fleet of autonomous load haul dump (LHD) vehicles for mining operations. The ever increasing demand for sustainable solutions and economic pressure causes innovation in the mining industry just like in any other branch. In this chapter, we present our approach to create a fleet of autonomous special purpose vehicles and to control these vehicles in mining operations. After an initial exploration of the site we deploy the fleet. Every vehicle is running an instance of our ROS 2-based architecture. The fleet is then controlled with a dedicated planning module. We also use continuous environment monitoring to implement a life-long mapping approach. In our experiments, we show that a combination of synthetic, augmented and real training data improves our classifier based on the deep learning network Yolo v5 to detect our vehicles, persons and navigation beacons. The classifier was successfully installed on the NVidia AGX-Drive platform, so that the abovementioned objects can be recognised during the dumper drive. The 3D poses of the detected beacons are assigned to lanelets and transferred to an existing map.

**Keywords:** autonomous vehicles, fleet control, planning, mining, computer vision, machine learning

#### **1. Introduction**

Along the lines of digitalisation and transforming industries towards Industry 4.0, also the mining industry as being a rather conservative industry is moving into this direction. For instance, in [1] the authors envision Mining 4.0 as a future concept of mining operations. The labour of a future mine worker will be smarter, more collaborative, more connected including augmented/virtual reality (AR/VR) technologies. In that sense, technology will also transform this sector into the Industry 4.0 direction. This has also an impact on the degree of automation deployed in future mines. In [2], mining is connected with key technologies such as intelligent systems, machine learning, and AR/VR. Additionally, there is a trend of the European raw materials industry towards changing from open pit mining to underground mining to reduce the environmental footprint of the mine. Digitalisation and automation are key technologies for further transforming mining operations into a decarbonised and more sustainable

operations (see, e.g. [3–5]). This leads to hybrid mines, where parts of the mine are still open pit and parts are underground mines. This poses, in particular, additional challenges for the automation process. Whilst in the open pit part of the mine, methods from autonomous driving using GPS etc. can be deployed, in the underground part, methods from mobile robotics need to be used in an ever-changing environment with the need to continuously monitor and track the changes in the robots' maps.

Under certain limitations such as loaders following a prerecorded path, automated LHD vehicles are already commercially available [6]. However, the problem of fully automated guided load-haul dumpers remains a challenging problem. This results in a large body of related works that focus on the core problems like navigating and localising LHD vehicles. Some recent works are, for instance, [7], [8], or [9]. The former proposes topological navigation for underground haulage vehicles. The basic idea is that in underground mines with many tunnels, a highly precise localisation pose in the tunnel is not of importance, the LHD vehicle could localise at crossings or other important waypoints of a topological map. In our work, we also follow the idea of using topological maps, specifically in the Lanelet2 format [10], which is a common map format in autonomous driving.

In [8], a robust localisation system integrating cameras, LiDARs, and odometry information for underground LHD vehicles is proposed. The system is tested on mining datasets and shows good accuracy with mean errors below 1 meter. The methods deployed in this work are very similar to our approach. An interesting addition to common sensor cues is proposed in [9], where IMU data are used to record ground ripples as an additional localisation information. For measuring similarities and dissimilarities in the recorded data in order to recognise ground patterns, the dynamic time warping algorithm is deployed. The basic idea behind this approach is not to rely on visual landmarks such as visual tags to keep the extra infrastructure required in the mine as little as possible. On the other hand, extending the infrastructure in mines such as installing Wi-Fi at least in parts of the mine is no longer out of question. Some approaches make use of vehicle-to-vehicle (V2V) communication for establishing communication networks in underground mines [11]. The purpose of this work is to localise other vehicles underground deploying V2V communication. But localising other vehicles underground is not the only interest for mining operations as tracking mine workers underground is mandatory in many countries. Seguel et al. [12] overviews relevant positioning technologies to track mine workers underground. Finally, fleet control is one of the tasks that needs to be solved for automated haulage and artificial intelligence (AI) approaches are being deployed. Bnouachir et al. [13] overviews some approaches to intelligent fleet management in mining operations, whilst [14] proposes a real-time scheduling algorithm based on flow-achieving scheduling trees to overcome shortcoming of off-the-shelf software which often is based on myopic heuristics. In our case of fleet management, we make use of a planning approach which is based on hierarchical task networks [15].

Many related works concentrate on particular open problems in automating mining operations focusing mainly on localisation and navigation challenges or fleet-level planning focusing either on underground or open pit mines. Hybrid mines, which combine underground and open pit mine operations, pose a particular challenge for autonomous vehicles.

In this chapter, we report on the current state of affairs in our endeavour to automate hauling operations in hybrid mines. This work is based in part on our previous works [16–22]. In particular, we report on (1) the hardware setup of our fleet of robot vehicles including the LHD vehicles and a tracked exploration robot, (2) the

overall ROS 2-based system architecture, (3) a model-based predictive control approach for controlling the articulated LHD vehicles, (4) an HTN-based approach to the tour planning of the fleet of vehicles.

The rest of this chapter is organised as follows. We present the hardware and sensor setup of the articulated LHD vehicles as well as of our exploration vehicle in Section 2 before we show the overall ROS 2-based software architecture (Section 3). Section 4 addresses the low-level navigation approach for controlling the articulated LHD vehicles. In Section 5, we outline the fleet and mission control system. Then, Section 6 reports on our approach to classifying drive ways in the mine and to mapping the changing mining environment. In Section 7, we show some experimental results. Then we conclude.

#### **2. Hardware setup**

In our mining automation projects, we are using two different platforms. One platform has been deployed for exploring and mapping the mine environment (Section 2.1), the other one is smaller-scale articulated dumping vehicle from Wacker Neuson that we turned into autonomous vehicle (Section 2.2).

#### **2.1 Exploration vehicle**

As mining environments are constantly changing, also the maps required for autonomous LHD vehicles need to be updated constantly. In our previous work [19, 23], we developed an exploration vehicle for mapping underground mines with dense 3D point clouds. As we can only give a brief overview of this work here, we refer to our previous work for further details.

The exploration vehicle is shown in **Figure 1a** and **b**. It is a skid-steered tracked robot based on a platform similar to a mini excavator but using a suspended undercarriage. It is equipped with a number of different sensors used for navigation, localisation, and mapping. The robot reaches speeds of up to 3 m s<sup>1</sup> and is controlled via the ROS [24] Movebase. For navigation, collision avoidance, and terrain classification, two Velodyne VLP-16 Puck LiDARS are mounted at the front. For mapping, we equipped the exploration vehicle with a custom-built rotating sensor platform shown in **Figure 1c** and **d**. It allows to acquire a (nearly) complete sphere around the vehicle. A Velodyne VLP-16 PUCK LiDAR with 16 scan lines, a vertical opening angle of 30°, and a horizontal range of 360° is mounted with a 14° inclination to the vertical axis. For short-range measurements, we additionally equipped the device with a 2D Hokuyo UTM-30LX-EW range scanner mounted in a 90° angle to the rotation plane. For teleoperation during mapping missions, an Allied Vision GT6600C highresolution camera with a wide-angle lens is mounted at the front of the robot. As a safety feature, we mounted a FLIR A315 thermal camera at the front of the robot in order to be able to detect persons even when not sufficient light is available. Additionally, an IMU for providing the orientation of the platform with respect to the ground is mounted on the vehicle.

For registering the 3D point clouds from the scanning device, we deploy the Iterative Closest Point (ICP) method [25] based on the Point Cloud Library implementation [26]. To minimise the errors of pairwise registration of many point clouds, all point clouds are registered globally making use of the GraphSLAM [27] algorithm

#### **Figure 1.**

*Exploration robot developed for mapping underground mining sites (from [23]).*

after Lu and Milios [28]. It creates a graph of the connections between all point clouds with overlaps and minimises the alignment errors of all connections simultaneously. Finally, for an internal representation of the 3D map, Octomap [29] is used. This offers the advantage of being able to query the information in various resolutions and to map the distinctions that are important for navigation between free, occupied, and unknown cells.

We present an example map from an underground mine in Krölpa, Germany, in Section 7.

#### **2.2 LHD vehicles**

The base vehicle we use is an articulated haul-dumper from Wacker Neuson (Model 1501).<sup>1</sup> Our fleet consists of three of those vehicles. **Figure 2** shows an exemplary prototype. This chapter is based on a paper that has been previously published. We refer to [18] for further details.

The dumper is an off-the-shelf model that can handle loads of up to 1500 kg. The control of the brakes and the angle of the articulated joint of the vehicle is realised by a hydraulic system, which depends on the hydraulic pressure generated by a diesel engine. In order to automate the vehicle, we installed electric linear actuators with our

<sup>1</sup> https://www.wackerneuson.de/produkte/dumper/raddumper/raddumper-1501/

**Figure 2.** *Picture of one of the articulated haul-dumpers at the test site.*

project partner Fritz Rensmann GmbH & Co. KG to control the brake and the throttle. Additionally, we attached a rotational servo motor to the steering axis and replaced the manual valves by electromagnetic valves to control the skip. We also installed hall sensors on each wheel to obtain information on the vehicle's state.

The control system comprises the different components shown in **Figure 3**. These parts are presented in the following. The software architecture of the vehicle will be discussed in the next section. As can be seen in the figure, each dumper can be controlled via our high-level control stack, where the tasks for the individual vehicles are generated by the overall mission planner for the whole mine to fulfil the daily production goals. The high-level control system is described in Section 5. Additionally, each vehicle can be controlled via remote control. In particular, this comes in handy when the vehicle needs to be steered onto a low loader trailer for transport.

*Real-time Controller*. The low-level real-time controller runs on a programmable logic controller by Beckhoff. To communicate with the motors and the PLC, we make use of a CAN interface. The PLC serves as a message filter, that prevents unsafe commands to be propagated to the motors, and as a kill-switch manager for controlling a number of kill-switches, which are installed on the vehicle including a radio killswitch. The PLC runs a PID controller regulating the angle of the articulated joint and the engine speed by properly actuating the motor moving the steering axis and the motor opening and closing the throttle valve. For more detail, we refer to [30].

*Compute Nodes*. As described below (Section 4), we implemented a GPU-based model-predictive path-follower, a high-level control system, and various computer vision algorithms. All the software modules run on two dedicated compute devices.

**Figure 3.** *System architecture of haul-dumper control system (From: [18]).*

We make use of a Zotac ZBOX Magnus One including an NVIDIA GPU to run the path-follower, the high-level control, and semantic and life-long mapping algorithms. In addition, the computer vision tasks are deployed on an NVIDIA Drive AGX Xavier. All high-bandwidth sensors such as the cameras are interfaced to the AGX unit, which directly processes the data. Both compute devices run Linux as its operating system.

*Sensors*. In the open pit part of the hybrid mine, GPS localisation is possible. For this task, we deployed an OxTS RT3000v3 dGPS on one vehicle and equipped the two remaining vehicles with two OxTS xNav650 from Oxford Technical Solutions. The RT3000 shares its correction data via the xNAV650 devices. Each dGPS contains an IMU. Especially, in the underground part of the hybrid mine, where no GPS is available, we scanned the environment and made use of two VLP-16 Lidars and six cameras for localising and navigation. A mesh network spanning over multiple local stations in the testing area is used for vehicle interaction within the fleet and vehicle interaction with loading and unloading stations (V2X in **Figure 4**). Local access points are available on each of the vehicles that are connected to the mesh. For identification of the vehicles and direct addressing, we make use of ROS2 namespacing.

*Controlling a Fleet of Autonomous LHD Vehicles in Mining Operation DOI: http://dx.doi.org/10.5772/intechopen.113044*

**Figure 4.**

*Diagram of the software architecture (From: [18]).*

#### **3. Software architecture**

In [21], we proposed a ROS 2-based architecture for self-driving cars. For our LHD vehicle, we adopted the architecture to fit the needs of the vehicle. Figure shows an overview of our architecture. The architecture also allows for using some common ROS packages such as Navigation2 [31] and robot\_localisation [32]. In the following, we give an overview of the different parts of the architecture.

*Centralised Mission Management Block*. At the top-most level in the hierarchy of decision-making and control of the fleet is the high-level control block (leftmost block in **Figure 4**). In the LHD context, it means that a certain tonnage that should be hauled is defined and the high-level system is to find a plan for all the vehicles in the fleet to reach the tonnage. We deploy a SHOP3 [33] planning system for high-level planning. An additional fleet manager distributes the plans and dispatches the actions to the vehicles. Whilst the vehicles execute these actions, they continuously update their current positions and these of their locked resources. The world model gets information of the actual status of all agents and the hauled tonnage. If needed, in case of bigger differences to the original plan, a re-planning for the whole fleet is initiated.

*Vehicle Unspecific Block*. The vehicle unspecific section of the architecture consists of mid-tier functionalities, that are similar to three-tier robotics architectures with modules for localisation or path planning. A global route is planned via a free-space planner like smac or map-based planner using a Lanelet2 HD map. The vehicle follows this path utilising a path-following module, including a feedback loop [34, 35]. As path-following algorithm, we use a model-predictive controller (MPC) which uses a GPU-based grid-search on a set of predicted trajectories achievable by the vehicle's kinematic model. We introduce the details of the MPC in Section 4.

*Vehicle Specific Block*. The vehicle specific block (rightmost block in **Figure 4**) mostly consists of drivers and vehicle communication modules such as drivers for cameras, IMUs, GPS units, LiDARs, or the wheel encoders. For object detection, we have implemented an object detection with cameras using YOLOv5 to aid in the semantic and life-long mapping that is being presented in Section 6. We also implemented a modular way of integrating state-of-the-art deep neural networks for 3D object detection in point clouds. The detailed presentation of the latter is part of

previous work presented in [36]. Our presented architecture is implemented using ROS 2 and deployed to each vehicle of the fleet. The underlying DDS network of ROS 2 is also used for the communication with the centralised high-level control system.

Next, we address the model-predictive controller used on the vehicle, before we discuss the high-level control software in Section 5.

#### **4. Model-predictive control**

In this section, we show the low-level control system based on a model-predictive control approach. We first introduce the kinematic model of the dumper, before we discuss the software implementation. Further details of the approach can be found in [17].

The kinematic model that has been used to model the haul-dumper in the MPC is described in [37] and shown in **Figure 5a**. This model has been chosen, as it models a centre-articulated platform and steering is done by changing the angle of the active joint. This model fits our haul-dumper well. The model is continuous and therefore the equations need to be discretised for our use case. The equations in the discrete form are as follows:

$$\begin{aligned} \mathbf{x}\_{t+1} &= \mathbf{x}\_t + \Delta\_t \, ^\*(v \cos \psi) \\ \mathbf{y}\_{t+1} &= \mathbf{y}\_t + \Delta\_t \, ^\*(v \sin \psi) \\ \boldsymbol{\psi}\_{t+1} &= \boldsymbol{\psi}\_t + \Delta\_t \, ^\* \left( \frac{\sin \phi}{l\_2 + l\_1 \cos \phi} \boldsymbol{\upsilon} + \frac{l\_2}{l\_2 + l\_1 \cos \phi} \boldsymbol{\alpha} \right) \end{aligned}$$

The constant *l*<sup>1</sup> describes the length of the front part, *l*<sup>2</sup> the rear part of the vehicle, *x* and *y* are the current position values within the Cartesian coordinate system, and *v* the current velocity of the vehicle. The current steering angle is *ϕ*, *ω* denotes the angular velocity of the joint, and *ψ* is the heading of the front part within the Cartesian coordinate system as shown in **Figure 5a**. Δ*<sup>t</sup>* represents the time interval between two control steps within the prediction process. These equations are used to predict the travel of the vehicle given the different inputs for each iteration of the optimisation.

**Figure 5.** *Dumper kinematics and model-predictive control approach (From: [17]).*

#### *Controlling a Fleet of Autonomous LHD Vehicles in Mining Operation DOI: http://dx.doi.org/10.5772/intechopen.113044*

**Figure 5b** shows the MPC control cycle. Based on the actual pose and the articulated haul-dumper model (**Figure 5a**), a predicted path is calculated for a set of steering angle sequences. Subsequently, for each predicted path, the costs are estimated by three weights:


Finally, in a grid-search optimisation, the output is determined as the first steering angle from the sequence of the steering angles for which the lowest cost was estimated.

The control result of this approach is very good, however, the calculation costs are very high. We therefore chose a GPU-based implementation of the controller based on our experiences described in [38]. The advantage of this approach is that the predicted paths and their individual costs can be calculated for all steering angle sequences in parallel in one step of the GPU. This leads to an improved control result at efficient calculation costs as we showed in [38].

The algorithm of the controller is shown in Algorithm 1, where the separation of GPU and CPU calculations is shown. To get an optimal trajectory resolution, multiple iterations are executed. The whole algorithm is calculated at a period of 20 ms. To adapt the original algorithm from [38] to the here-used long-haul-dumpers, we only had to change the kinematic model of vehicle and had to re-calibrate the control parameters (cf also to [17] for details).

#### **5. High-level fleet planning**

For larger-scale mining scenarios, several vehicles need to work together in a fleet. Such a fleet then needs to be coordinated for the work to be organised and distributed amongst the different vehicles in an optimised fashion. To realise the high-level fleet coordination system, we implemented a planning server embedding of SHOP 3 [33]. SHOP 3 is a domain-independent planning system based on ordered task decomposition. Ordered task decomposition is a modified version of hierarchical task network (HTN) planning [39] in which the planning order respects the actual order of execution of each task [33]. The interaction between the embedding sever and our vehicles is done by first communicating or loading a world model into the planner. Afterwards, a query for a day plan can be sent to the planner as a string, which the planner then processes. The query is a problem statement describing the resource allocation, available agents, and goals. As a result, the planner produces a string containing the day plan for all vehicles. This plan can then be distributed to the fleet. Each agent can then parse the plan and start executing the assigned actions.

As an example, **Figure 6** visualises a resource assignment. The blue circles in the figure represent resource sources. Above each source, we show the type of resource which can be loaded from that source and its quantity (rounded corners rectangles). The resource quantities of ores are modelled as infinite. The waste resource is constrained to 500 tons. The white circle represents a stockpiling point, which would be used to store resources if direct hauling to the unloading points would lead to congestion. In black, we colour the waste dumping points which can be used to unload waste from sources S1 and S2. The circles in yellow are fuel stations, that agents need to drive to fill their fuel tanks. Each agent in the problem statement is defined with a fuel state which is being used up when the agents transport resources. The arrows in the visualisation show possible direct load/unload relations. For example, the arrow from S1&2 to G13 means that the agent loads from S1 or S2 and unloads at the goal G13.

**Figure 6.** *Schematic of one of the problem statements evaluated in our research.*

**Module KPIs** Model Predictive Control Mean lateral error in curves Mean lateral error on straight section 0.9 (m) 0.1 (m) System Delays Mean steering delay Mean engine RPM delay 200 (ms) 100 (ms) Mission Planning (SHOP3) Idle-time (min) of fleet after 8 h in simulation Scenario Name Number of vehicles simulated Strategy 1 Strategy 2 Strategy 3 Strategy 4 Simulation Scenario 1 75 25,782 26,096 26,046 25,938 Simulation Scenario 2 40 2093 5343 3512 3084 Overall hauled resources (t) Scenario Name Number of vehicles simulated Strategy 1 Strategy 2 Strategy 3 Strategy 4 Simulation Scenario 1 75 18,550 18,550 18,550 18,550 Simulation Scenario 2 40 13,865 12,685 13,080 10,305 Object Detection Model mAP@0.5 Person mAP@0.5 Wheel-dumper mAP@0.5 Car mAP@0.5 Beacon Detection Frequency [hz] PointPillar (3D)a 0.34 0.4 0.4 — 20 YOLOv5m (2D) 0.994 0.978 — 0.966 30

*Controlling a Fleet of Autonomous LHD Vehicles in Mining Operation DOI: http://dx.doi.org/10.5772/intechopen.113044*

*a The results exclude performance for beacons, because the LiDAR sensor did not measure many points on the target beacons. Detection of beacons was, therefore, unreliable with the given hardware.*

#### **Table 1.**

*Table on the quantified key performance indices for some of our modules we have researched by now (From: [18]).*

Another problem statement we defined for the evaluation of the high-level control is less complex with only 5 sources with infinite resources and 2 goals. The meaning of sources and goals is the same as for **Figure 6**. The evaluation presented as an aggregation of key performance indicators in **Table 1** in Section 7 shows the results when different criteria are imposed on a plan. A plan can be made to take into consideration waiting times of the vehicles in a fleet, the time a vehicle needs to fulfil a load-unload cycle accumulated for a whole fleet, or idle time of the fleet as a whole. For comparison, we show the results for a plan that is created by generating plans randomly and selecting the first valid plan for a problem. This is the fourth strategy as presented in the evaluation. The plan that takes into consideration the waiting time of the agents is strategy 1. Strategy 2 takes the accumulated cycle time for all agents into account and tries to minimise it. The last strategy, strategy 3, minimises the time the vehicles in the fleet spend idling.

In our scenario, SHOP 3 instructs the navigation system to execute an action called DRIVE\_TO. The action's main argument is an ID to a lanelet [10] in a map representing the routing graph. Lanelet is an HD map format which is an extension of OSM. With its integrated extensions for routing, it can support the execution of many tasks in navigation. Obstructions in the global map are represented within the routing graph and can be derived from an occupancy grid, which is implemented in the costmap ROS package. Given the goal, the action server runs a behaviour-tree [40] which computes the centre line of a route calculated by the lanelet\_planner. This centre line is referenced to a world-fixed frame (i.e. the entrance of the mine) which is in turn passed on to our MPC to follow the path. Regular actuation commands are calculated by the MPC by calling its respective action server from within the behaviour-tree at a certain frequency.

#### **6. Life-long mapping**

With life-long mapping, we refer to the concept of mapping the environment and ensuring that changes in the environment can be detected and processed into a map update. To achieve this, we identified two approaches. First, one can use representations of a traffic network as maps and use static delimiting objects in the environment as markers that one can infer a change in the environment. Secondly, one can use shape representations of the environment (such as a point cloud) to extract a traffic network representation of the environment from the shape. To accommodate for changes in the environment, one can simply repeat the process of generating the traffic network from the environment.

For the first approach, three major components are needed: the object detection and classification of static and dynamic objects, the localisation of static object in a unified coordinate frame, and the integration of the information from the 2 previous components into an HD map. In the following, we explain this in more detail:

*Object Detection*. The artificial intelligence framework known as YOLOv5 was used for object recognition. The focus was on the recognition of humans, navigation aids such as beacons marking the way, and different types of vehicles commonly used in mining. To facilitate the recognition of previously unknown objects using this YOLOv5 mechanism, a three-part training methodology was devised:

Firstly, a solution using synthetic data was developed to automate the annotation process for training deep learning networks. By creating a realistic 3D mining world with Unreal Engine and capturing annotated images from virtual cameras on dumpers, see **Figure 7**, these synthetic datasets serve as cost-effective training data for the YOLOv5 network, addressing the challenges of manual annotation for new objects and environments.

As second stage, synthetically generated data for neural network training often lacks impurities found in real-world images, such as noise or blur. To address this, we developed an image augmentation tool. This tool introduces variations into the training images by adding noise, modifying lighting, saturation, image resolution, and horizontal alignment. By incorporating these variations, the synthetic training data becomes more realistic and better aligns with real-world conditions. Thirdly, the training data is supplemented by data from real journeys which are manually annotated and augmented.

#### **Figure 7.**

*Virtual reality world with 3D models created with unreal engine 5 to get annotated training data from simulated driving.*

The classifier, trained with a blend of synthetic and real images over 299 epochs, attains 0.9835 mAP@0.5 and 0.9836 mAP@0.95 with YOLOv5m and effectively detects partially obscured objects. After converting from PyTorch to ONNX, then to TensorRT, it is deployed on Nvidia AGX Drive, with an inference time near 35 milliseconds.

Lane detection. Beacons serve as static boundary markers for navigable roads. We calculate the 3D position of these beacons by projecting the midpoint of the lower boundary box onto a ground plane, considering the camera's extrinsic parameters relative to the vehicle centre point. Finally, we derive the global UTM coordinates from the estimated vehicle-relative positions.

*Boundary Matching and Map Correction*. In order to correlate the positions of the beacons with a lane boundary, a filtration procedure is implemented. A binning filter reduces measurement noise by averaging close positions. A logical constraint filter ensures feasible interpolation of the lane boundary. Positions are then re-indexed based on their medial distance from the lane boundary segments. The updated lane boundary state is shown in **Figures 8** and **9**.

*LiDAR Map To HD Map*. Our approach for generating an HD map from a LiDAR map involves segmenting the navigable ground segment from the point cloud, calculating the concave hull, creating a Voronoi graph [41], finding the longest chain of vertices in the graph, smoothing the vertices, and converting the trajectory into a lane. The process includes steps such as elevation map creation, filtering of sample points based on a simple morphological filter from [42],

#### **Figure 8.**

*State of the lane boundary throughout the four stages of the lane boundary adjustment process.*

#### **Figure 9.**

*Visualisation of the update process and its result.*

calculating a spanning polygon, applying Delaunay triangulation, and constructing a lane with a consistent width. **Figures 10** and **11** show a visualisation of each step.

*Controlling a Fleet of Autonomous LHD Vehicles in Mining Operation DOI: http://dx.doi.org/10.5772/intechopen.113044*

**Figure 10.**

*Processed point cloud map after each step of the conversion from point cloud map to HD map.*

**Figure 11.** *Visualisation of the integration scenario, which was presented at a demonstration day.*

#### **7. Real-world experiments**

In this section, we show some experimental results of our work.

3D Mapping. Regarding underground mapping with the exploration vehicle described in Section 2.1, we show maps from the MAXIT underground anhydrite mine in Krölpa, Germany in **Figure 12**. The map was recorded by exploration vehicle in a stop-and-scan fashion taking a full spherical 3D scan every 10 m. The recording of the map took place in a teleoperated manner, the point cloud data and the odometry data were stored in a rosbag file and were processed offline following the procedure described in detail in [19, 20]. **Figure 12a** shows a 2D occupancy grid of the mapped part of the mine. **Figure 12b** visualises the point clouds themselves and **Figure 12c** a 3D Octomap from the point clouds. Whilst the results show that precise 3D maps of the underground mining environment can be produced with the exploration vehicle, one has to admit, on the downside, that the process is not fully automated and human expertise is required to avoid errors in registering the different point clouds into a consistent map. The overall map size of the map is about 800 m.

*Vehicle Automation.* The haul-dumper's hardware setup has proved reliable throughout our project, with hundreds of testing hours and no system-wide failures. The most frequent issue involves power supply to the vehicle's components. The installed PLC and sluggish system caused by the hydraulic mechanism lead to an average latency of 200 ms from command issuance to actuators. There is also latency

**Figure 12.** *Parts of the MAXIT anhydride mine in Krölpa, Germany.*

in emergency stops, taking 1.5 seconds to fully halt after triggering the switch. The path-following feature shows a mean lateral deviation of 0.1 m on straight sections and 0.9 m on curves.

*Fleet-level Planning.* Fleet-level planning was tested through coarse simulations that mimic mining processes and through integration in the lower modules of the architecture. Two simulation scenarios were created with different levels of complexity: one with simple targets and resources, the other emulating a hybrid mine with multiple resource types. In each scenario, four strategies were analysed, each examining a different heuristic: Minimising the idle time of all vehicles in the fleet, minimising the average duration of loading and unloading the dump truck, minimising the idle time of the dump truck without moving loads, and maximising the haulage mass from randomly generated plans. Idle time and total haulage mass were observed in an 8-h simulation. The results are shown in **Table 1**.

*Long-term Mapping.* We carried out a functional test on live map correction by manually altering the map and monitoring the global route planner and vehicle

response. This often involved tweaking the lane sections leading to the loading station, as demonstrated in the integration scenario **Figure 11**.

*Integration Scenario.* In an integration test, we utilised loading and unloading stations to handle pick-up and unload payloads. The unloading heap has two access points, whereas the loading station has only one. There are two bidirectional lanes to the unloading sites and a single bidirectional lane to the loading site.

The daily transport schedule of fleet management includes repeated loading and unloading operations for the vehicle fleet, with only one vehicle having access to the loading point. The high-level controller coordinates resource access to avoid conflicts. Resource blocking is implemented by the operations scheduling system. The first vehicle to reach the fork in the middle of the traffic network whilst the loading resource is free secures the lock and blocks the lane to the resource. Following vehicles must wait for fleet management to release the resource. This mechanism proved robust in an 8-h operation and successfully implemented the daily schedule.

#### **8. Conclusion**

In this chapter, we present our results from automating load-haul-dump (LHD) operations in hybrid mines. Hybrid mining operations are mines where mining in part is done in an open pit fashion, and parts are underground. As for the autonomous vehicles deployed in such a mine, it means that the vehicles cannot simply rely on GPS data for localising themselves over ground, but also need classical approaches to mapping the environment. We reported on our exploration vehicle that is equipped with a rotating LiDAR scanner to produce detailed 3D point clouds which then are integrated into Octree maps that the LHD vehicles could use for localising themselves in the mine.

The haul-dumpers that we deploy are modified off-the-shelf articulated dumpers which were turned into autonomous vehicle. The vehicles are equipped with GPS, LiDAR, and camera sensors. The software architecture is based on ROS2 and was also deployed in similar projects related to autonomous driving including a modified version of a model-predictive control (MPC) algorithm. It had to be adapted to the articulated kinematics of the dumper. Further, to control a fleet of LHD vehicles (three in our case), we made use of the HTN-based planner SHOP3, which generates a global mission plan for the vehicles based on the required haulage capacities. Each vehicle is following this global plan for generating their local missions. As another important contribution, we introduced Lanelet2-based maps for navigating the LHD vehicles, including dedicated drive ways and right of way rules. Lanelet maps are commonly used for self-driving cars and not so much in robotics applications. Using this approach facilitates the coordination of a fleet of vehicles to a great extend. Finally, we showed our approach to object and drive-way detection including the automatic generation of the aforementioned Lanelet drive ways.

The presented approach has been tested in real-world scenarios underground and in open pit mines under controlled conditions. We showed how mapping successfully took place with our exploration vehicle in the Krölpa mine in Germany. The fleet of dumpers was tested in an open gravel pit in Buir, Germany. We could show that the low-level control such as MPC works well also on the dumpers with their articulated kinematics and how the fleet of dumpers could be coordinated on a mission level. Whilst the tests and experimental results show that the overall approach is working,

next steps would be to deploy this work in real mining operations under the realistic hard mining environment conditions with limited communication means.

### **Acknowledgements**

We gratefully acknowledge the contributions of (in alphabetical order) N. Adam, A. Braining, D. Bulla, M. Claer, B. Decker, J. D. Eichenbaum, F. Engels, J. Grabe, K. Kramer, K. Krückel, M. Lubos, A. Lyrmann, T. Neumann, Y. Otten, J.C. Richter, D. Scholl, P. Shah, P. Stricker, and S. Weyer. The work presented in this chapter was funded with grants from the German Federal Ministry of Education and Research (BMBF) under grant numbers 033R126C, 033R126CN.

#### **Author details**

Alexander Ferrein\*, Gjorgji Nikolovski, Nicolas Limpert, Michael Reke, Stefan Schiffer and Ingrid Scholl Mobile Autonomous Systems and Cognitive Robotics Institute, FH Aachen - University of Applied Sciences, Aachen, Germany

\*Address all correspondence to: ferrein@fh-aachen.de

© 2023 The Author(s). Licensee IntechOpen. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

### **References**

[1] Lööw J, Abrahamsson L, Johansson J. Mining 4.0—The impact of new technology from a work place perspective. Mining, Metallurgy & Exploration. 2019;**36**:701-707

[2] Faz-Mendoza A, Gamboa-Rosales NK, Medina-Rodríguez C, Casas-Valadez MA, Castorena-Robles A, López-Robles JR. Intelligent processes in the context of mining 4.0: Trends, research challenges and opportunities. In: 2020 International Conference on Decision Aid Sciences and Application (DASA). IEEE; 2020. pp. 480-484

[3] Batterham R. The mine of the future – Even more sustainable. Minerals Engineering, Sustainable Minerals. 2017; **107**:2-7

[4] Sánchez F, Hartlieb P. Innovation in the mining industry: Technological trends and a case study of the challenges of disruptive innovation. Mining, Metallurgy & Exploration. 2020;**37**(5): 1385-1399

[5] Clausen E, Sörensen A. Required and desired: Breakthroughs for futureproofing mineral and metal extraction. Mineral Economics. 2022;**35**(3):521-537

[6] Paraszczak J, Gustafson A, Schunnesson H. Technical and operational aspects of autonomous lhd application in metal mines. International Journal of Mining, Reclamation and Environment. 2015;**29**(5):391-403

[7] Mascaró M, Parra-Tsunekawa I, Tampier C, Ruiz-del Solar J. Topological navigation and localization in tunnels— Application to autonomous load-hauldump vehicles operating in underground mines. Applied Sciences. 2021;**11**(14):6547

[8] Jacobson A, Zeng F, Smith D, Boswell N, Peynot T, Milford M. What localizes beneath: A metric multisensor localization and mapping system for autonomous underground mining vehicles. Journal of Field Robotics. 2021; **38**(1):5-27

[9] Stefaniak P, Jachnik B, Koperska W, Skoczylas A. Localization of LHD machines in underground conditions using IMU sensors and DTW algorithm. Applied Sciences. 2021;**11**(15):6751. DOI: 10.3390/app11156751

[10] Poggenhans F, Pauls J-H, Janosovits J, Orf S, Naumann M, Kuhnt F, et al. Lanelet2: A highdefinition map framework for the future of automated driving. In: 2018 21st International Conference on Intelligent Transportation Systems (ITSC). IEEE; 2018. pp. 1672-1679

[11] Dong L, Sun D, Han G, Li X, Hu Q, Shu L. Velocity-free localization of autonomous driverless vehicles in underground intelligent mines. IEEE Transactions on Vehicular Technology. 2020;**69**(9):9292-9303

[12] Seguel F, Palacios-Játiva P, Azurdia-Meza CA, Krommenacker N, Charpentier P, Soto I. Underground mine positioning: A review. IEEE Sensors Journal. 2022;**22**(6):4755-4771

[13] Bnouachir H, Chergui M, Machkour N, Zegrari M, Chakir A, Deshayes L, et al. Intelligent fleet management system for open pit mine. International Journal of Advanced Computer Science and Applications. 2020;**11**(5):327-332

[14] Seiler KM, Palmer AW, Hill AJ. Flow-achieving online planning and dispatching for continuous transportation with autonomous

vehicles. IEEE Transactions on Automation Science and Engineering. 2020;**19**(1):457-472

[15] Erol K, Hendler JA, Nau DS. UMCP: A sound and complete procedure for hierarchical task-network planning. In: Proceedings of the Second International Conference on Artificial Intelligence Planning Systems (AIPS'94). AAAI Press; 1994. pp. 249-254

[16] Eichenbaum J, Nikolovski G, Mülhens L, Reke M, Ferrein A, Scholl I. Towards a lifelong mapping approach using lanelet 2 for autonomous open-pit mine operations. In: Proc. IEEE CASE 23. IEEE; 2023

[17] Nikolovski G, Limpert N, Nessau H, Reke M, Ferrein A. Model-predictive control with parallelised optimisation for the navigation of autonomous mining vehicles. In: 2023 IEEE Intelligent Vehicles Symposium, IV 2023. IEEE; 2023

[18] Ferrein A, Reke M, Scholl I, Decker B, Limpert N, Nikolovski G, et al. Towards a fleet of autonomous hauldump vehicles in hybrid mines. In: ICAART (1). SCITEPRESS; 2023a. pp. 278-288

[19] Ferrein A, Scholl I, Neumann T, Krückel K, Schiffer S. A system for continuous underground site mapping and exploration. In: Reyhanoglu M, Cubber GD, editors. Unmanned Robotic Systems and Applications. London, UK, Rijeka: IntechOpen; 2019

[20] Donner R, Rabel M, Scholl I, Ferrein A, Donner M, Geier A, et al. Die Extraktion bergbaulich relevanter Merkmale aus 3D-Punktwolken eines untertagetauglichen mobilen multisensorsystems. Tagungsband Geomonitoring. 2019;**S**:91-110. DOI: 10.15488/4515

[21] Reke M, Peter D, Schulte-Tigges J, Schiffer S, Ferrein A, Walter T, et al. A self-driving car architecture in ROS2. In: 2020 International SAUPEC/RobMech/ PRASA Conference. IEEE; 2020. pp. 1-6

[22] Macenski S, Foote T, Gerkey B, Lalancette C, Woodall W. Robot operating system 2: Design, architecture, and uses in the wild. Science Robotics. 2022;**7**(66):eabm6074

[23] Neumann T, Dülberg E, Schiffer S, Ferrein A. A rotating platform for swift acquisition of dense 3D point clouds. In: ICIRA 9834 of Lecture Notes in Computer Science. Cham: Springer; 2016. pp. 257-268

[24] Quigley M, Conley K, Gerkey B, Faust J, Foote T, Leibs J, et al. Ros: An open-source robot operating system. In: ICRA Workshop on Open Source Software. Vol. 3. Kobe, Japan; 2009. p. 5

[25] Besl P, McKay ND. A method for registration of 3-D shapes. IEEE Transactions on Pattern Analysis & Machine Intelligence. 1992;**14**(2):239-256

[26] Rusu RB, Cousins S. 3D is here: Point cloud library (PCL). In: 2011 IEEE International Conference on Robotics and Automation. IEEE; 2011. pp. 1-4

[27] Thrun S, Montemerlo M. The graph SLAM algorithm with applications to large-scale mapping of urban structures. The International Journal of Robotics Research. 2006;**25**(5–6):403-429

[28] Lu F, Milios E. Robot pose estimation in unknown environments by matching 2d range scans. Journal of Intelligent and Robotic Systems. 1997; **18**(3):249-275

[29] Hornung A, Wurm KM, Bennewitz M, Stachniss C, Burgard W. OctoMap: An efficient probabilistic 3d

*Controlling a Fleet of Autonomous LHD Vehicles in Mining Operation DOI: http://dx.doi.org/10.5772/intechopen.113044*

mapping framework based on octrees. Autonomous Robots. 2013;**34**(3):189-206

[30] Sürken D. Programmierung einer Basisautomatisierung für eine Wacker Neuson 1501 Modellmulde [Thesis]. FH Aachen University of Applied Sciences in German; 2021

[31] Macenski S, Martin F, White R, Ginés Clavero J. The marathon 2: A navigation system. In: 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE; 2020

[32] Moore T, Stouch D. A generalized extended kalman filter implementation for the robot operating system. In: Proceedings of the 13th International Conference on Intelligent Autonomous Systems (IAS-13). Cham: Springer; 2014

[33] Goldman RP, Kuter U. Hierarchical task network planning in common lisp: The case of SHOP3. In: Proceedings of the 12th European Lisp Symposium (ELS), April 1-2, 2019, Genova, Italy. Zenodo; 2019. pp. 73-80. DOI: 10.5281/ zenodo.2633324

[34] Pivtoraiko M, Knepper RA, Kelly A. Optimal, Smooth, Nonholonomic Mobile Robot Motion Planning in State Lattices. Pittsburgh, PA: Robotics Institute, Carnegie Mellon University; 2007

[35] Limpert N, Schiffer S, Ferrein A. A local planner for Ackermann-driven vehicles in ROS SBPL. In: 2015 Pattern Recognition Association of South Africa and Robotics and Mechatronics International Conference (PRASA-RobMech). IEEE; 2015. pp. 172-177

[36] Nikolovski G, Reke M, Elsen I, Schiffer S. Machine learning based 3D object detection for navigation in unstructured environments. In: IEEE Intelligent Vehicles Symposium Workshops, IV 2021. IEEE; 2021. pp. 236-242

[37] Delrobaei M, McIsaac KA. Design and steering control of a centerarticulated mobile robot module. Journal of Robotics. 2011;**2011**:621879

[38] Chajan E, Schulte-Tigges J, Reke M, Ferrein A, Matheis D, Walter T. Gpu based model-predictive path control for self-driving vehicles. In: 2021 IEEE Intelligent Vehicles Symposium (IV). IEEE; 2021. pp. 1243-1248

[39] Dvorak F, Barták R, Bit-Monnot A, Ingrand F, Ghallab M. Planning and acting with temporal and hierarchical decomposition models. In: 2014 IEEE 26th International Conference on Tools with Artificial Intelligence. IEEE; 2014. pp. 115-121

[40] Ghzouli R, Berger T, Johnsen EB, Dragule S, Wasowski A. Behavior trees in action: A study of robotics applications. In: Proceedings of the 13th ACM SIGPLAN International Conference on Software Language Engineering. New York, NY, United States: Association for Computing Machinery; 2020. pp. 196-209

[41] Bhattacharya P, Gavrilova ML. Voronoi diagram in optimal path planning. In: 4th International Symposium on Voronoi Diagrams in Science and Engineering (ISVD 2007). IEEE; 2007. pp. 38-47

[42] Pingel TJ, Clarke KC, McBride WA. An improved simple morphological filter for the terrain classification of airborne lidar data. ISPRS Journal of Photogrammetry and Remote Sensing. 2013;**77**:21-30

#### **Chapter 5**

## Geometric Control of Robotic Systems

*Anis Bousclet*

#### **Abstract**

In this chapter, we'll present geometrical methods to control robotic systems, we'll start by presenting the classical approach and its benefits, as well as its limits, and we will explain how the geometric approach allow us to get rid of these limits.

**Keywords:** robotic systems, Riemannian geometry, geometric control, optimal control, regulation under constraints

#### **1. Introduction**

Robotic systems are powerful tools that allow us to gain time, money and enery, they had revolutionized several fields as industry, medicine, agriculture and many other fields.

The main difficulty caused by robotic systems is the nonlinearity of the dynamics. The classical approach modelise the configuration space by an Euclidean space, and applies the principle of least action to derive the equation of motion, and applies Lyapunov and Lasalle's techniques to control the robot [1, 2].

However, this elegent approach do not fit well with regulation under constraints problem or optimal regulation, the goal here is to illustrate these limits, and to show how the geometric method allow us to get ride of these limits.

#### **2. The classical approach and its limits**

The configuration space of the robot is *<sup>n</sup>* where *n* is the number of degrees of liberty (DOF) of the robot, the principle of least action gives the dynamics

$$\mathbf{G}(q)q^{\prime} + \frac{\mathbf{1}}{2}\mathbf{G}(q)^{\prime} + \mathbf{S}(q, q^{\prime})q^{\prime} + \mathbf{g}(q) = \boldsymbol{\tau}.\tag{1}$$

This allow us to ensure locally the regulation to a configuration *q* <sup>∗</sup> ∈ *<sup>n</sup>* with the control

$$\pi(q, q') = \mathfrak{g}(q) - K\_p(q - q') - K\_v q' \tag{2}$$

which is a PD controller.

The classical approach with all its benefits [1, 2], do not fit well with the problem of regulation under constraints or optimal regulation.

For the optimal control problem, the general formula which gives the angular velocity in function of the configuration of a Rigid body that minimizes the cost [3]

$$J(\Omega) = \int\_0^\infty \left[ tr\left(I - R\_d^T R\right) + \frac{1}{2} \left\|\Omega\right\|^2 \right] dt \tag{3}$$

for the kinematic *R*<sup>0</sup> ¼ *R*Ω and ensure regulation *R* ! *Rd*

$$\boldsymbol{\mathfrak{Q}}^{\*}\left(\boldsymbol{R}\right) = -\frac{\boldsymbol{R}\_d^T \boldsymbol{R} - \boldsymbol{R}^T \boldsymbol{R}\_d}{\sqrt{\mathbf{1} + tr\left(\boldsymbol{R}\_d^T \boldsymbol{R}\right)}}\tag{4}$$

which has not the form of a proportional controller, so this prevents to get an optimal PD regulator for robotic systems.

For the regulation under constaints problem (tool of the robot must stay in a security surface *<sup>S</sup>*), the set of admissibles configurations is *<sup>N</sup>* <sup>¼</sup> *<sup>x</sup>*�<sup>1</sup>ð Þ*<sup>S</sup>* where *<sup>x</sup>* is the function which gives the position of the tool in function of the configuration. The set *N* has no reason to be a linear subspace of R since *x* is a nonlinear function, the set *N* will be a curved space in *<sup>n</sup>* and we arrive to a formalism naturally adapted to the resolution of the problem of regulation under constraints.

Sensitivity with respect to initial conditions remains one of the main problems in conceptions of efficients observers.

For example, for a rigid body which dynamics are1,2

$$R' = R\Omega \tag{5}$$

$$\underline{\alpha}' = I^{-1}(I\alpha \times \alpha) - 2I^{-1}[\text{Skew}(R)]^\times \tag{6}$$

2 *Skew R*ð Þ¼ *<sup>R</sup>*�*RT* <sup>2</sup> .

<sup>1</sup> ½ � Ω � is the unique vector *ω* such that *ω* � *x* ¼ Ω*:x*, and *j* is its inverse.

where *I* ¼ ½ � 2, 2, 8 and *R*ð Þ¼ 0 exp 0½ � *:*82, 0*:*21, 0*:*73 � ð Þ ωð Þ¼ 0 ½ � 0*:*8, 0*:*5, 0*:*6 , the Luenberger observer [4] is

$$\hat{\boldsymbol{R}}' = \hat{\boldsymbol{R}} \left( \hat{\boldsymbol{\Omega}} - \mathbf{2} \boldsymbol{j} \left( \boldsymbol{I}^{-1} \left[ \boldsymbol{\text{Skew}} \left( \boldsymbol{\text{R}}^{T} \hat{\boldsymbol{R}} \right) \right]^{\times} \right) \right) \tag{7}$$

$$\hat{\mathbf{a}}' = I^{-1}(I\hat{\mathbf{a}} \times \hat{\mathbf{a}}) - \mathfrak{L}I^{-1}[\text{Skew}(\mathbf{R})]^\times - \mathfrak{L}I^{-1}\left[\text{Skew}\left(\mathbf{R}^T\hat{\mathbf{R}}\right)\right]^\times \tag{8}$$

where *<sup>R</sup>*^ð Þ¼ <sup>0</sup> exp 0½ � *:*8, 0*:*5, 0*:*<sup>6</sup> � ð Þ and <sup>ω</sup>^ð Þ¼ <sup>0</sup> ½ � 0, 0, 0 , the simulations shows that the Luennberger observer is not efficient when real and observed configuration and velocity are lightly different (**Figure 1**).

#### **3. The geometric formalism**

The idea of the geometric formalism is to pass the complexity of the equation to the configuration space, thus in the geometric formalism, the configuration space is not an Euclidean space, but a curved space that we call manifold.

Manifolds [5] are objects that looks like Euclidean space locally, we can always reduce the local study of manifolds to computations in *<sup>n</sup>*, it is exactly what we do when we parametrize the configuration of a rigid body with Euler angles.

The configuration space of a rigid body [6] which has a fixed point is the space of Euclidean rotations *SO*ð Þ3 , when all its points moves freely the configuration space becomes *SO*ð Þ� <sup>3</sup> <sup>3</sup> , the possible translations of a point cover <sup>3</sup> .

A robot is an assembly of *s* rigid bodies, which are related from there extremeties by articulations, those links are called holonomic constraints.

The configuration space of the robot is *M* a submanifold of dimension *n* of *SO*ð Þ� <sup>3</sup> <sup>3</sup> *<sup>s</sup>* , the holonomic constraints are related to the local coordinates by the following result.

**Proposition 1** *Let M be a subset of SO*ð Þ� <sup>3</sup> <sup>3</sup> *<sup>s</sup> , there is equivalence between*


*M is said to be a manifold of dimension n, the number n is the degree of freedom of the robot, and the number r is the number of constraints, we have the well known formula n* ¼ 6*s* � *r*.

Once we know that we are dealing with curved objects, it may seems more complicated to do computations. Thanks to the tangent space *TqM*, which is the set of virtual velocities *γ*\_ð Þ 0 of curves *γ*ð Þ*t* that pass throught *γ*ð Þ¼ 0 *q*∈ *M* and stay in *γ*ð Þ*t* ∈ *M*, this space is linear and of dimension *n* (**Figure 2**).

<sup>3</sup> A submersion is a smooth function for which the differential is everywhere onto.

<sup>4</sup> An immersion is a smooth function for which the differential is everywhere one to one.

<sup>5</sup> Invertible continuous map with continuous inverse.

When we do parametrization with holonomic constraints, the tangent space is *TqM* ¼ *Ker df <sup>q</sup>* , if we parametrize our manifold with local coordinates, then *TqM* <sup>¼</sup> *Range d*ð Þ *<sup>θ</sup>*<sup>0</sup> , the basis adapted to the coordinates is *<sup>∂</sup><sup>i</sup>* <sup>¼</sup> *<sup>d</sup>θ*ð Þ *ei* where *ei* is the canonical basis of *<sup>n</sup>*.

The tangent linear map between two manifolds *ϕ* : *M* ! *N* is the linear map

$$T\_q \phi : T\_q \mathcal{M} \to T\_{\phi(q)} \mathcal{N} \tag{9}$$

defined by

$$T\_q \phi(\upsilon) = (\phi \circ \gamma)'(0) \tag{10}$$

where *γ*ð Þ¼ 0 *q*, *γ*ð Þ*t* ∈ *M* and *γ*<sup>0</sup> ð Þ¼ 0 *v* (**Figure 3**).

A vector field is the data of a tangent vector in *TqM* for each *q*∈ *M* which a smooth way, the set of vector fields is Γð Þ *TM* .

To each vector field corresponds a motion of diffeomorphisms<sup>6</sup> by solving the following ODE (**Figure 4**)

$$
\partial\_t \phi\_t^X(q) = X\left(\phi\_t^X(q)\right) \tag{11}
$$

$$
\phi\_0^X(q) = q.\tag{12}
$$

When we have two vector fields *X*, *Y*, a natural question is to ask when we have

$$
\phi\_t^X \circ \phi\_s^Y = \phi\_s^Y \circ \phi\_t^X \tag{13}
$$

the obstruction of the commutation between the flows is measured by the Lie bracket ½ � *X*, *Y* which is defined by

<sup>6</sup> Diffeomorphism is an invertible smooth mapping with smooth inverse

*Geometric Control of Robotic Systems DOI: http://dx.doi.org/10.5772/intechopen.110895*

**Figure 3.** *Linear tangent map.*

**Figure 4.** *Vector field and its flow.*

$$\mathbb{E}[X,Y] = \frac{d}{dt}\bigg|\_{t=0} T\phi\_{-t}^X \left( Y \circ \phi\_t^X \right). \tag{14}$$

The Lie derivative is related to the derivation of functions by

$$[X,Y]f = X.(Yf) - Y.(Xf) \tag{15}$$

where *X:f* ¼ *Tf X*ð Þ.

Now that we have introduced all necessary tools, we will talk about Riemannian geometry. The idea behind Riemannian geometry is to consider a scalar product that is dependent of the position, this will simplify the formalism, provided that we know how to manipulate the covariant differential calculus, which we will present right now.

#### **Figure 5.** *The rigid body.*

The kinetic energy of a ball that is constrained to stay in a surface *S* is independant of the position. In the general case, when we compute the kinetic energy of a rigid body or two link robot, this energy will depend of the configuration, this gives a Riemannian metric that is inherited from the inertia of the robot.

The innovative viewpoint of Arnold [6] is to see the motion of a rigid body fixed on one point as a curve in *SO*ð Þ3 , the kinetic energy depends only on the left angular velocity<sup>7</sup> and thus we say that the Riemannian metric is left invariant (**Figure 5**).

Return to our ball in a surface, in the absence of forces and potential, the principle of least action postulates that the real trajectory is a critical point of the kinetic energy, this means that the acceleration of the ball must be orthogonal to the surface, such trajectories are called geodesic, remark that if the ball was not constrained in *S*, this condition would give that the acceleration is 0, and thus the motion is a straightline, let us do the calculation

the kinetic energy for a ball with mass *m* is

$$E(\boldsymbol{\chi}) = \int\_0^1 \frac{1}{2} m |\dot{\boldsymbol{\chi}}(t)|^2 dt \tag{16}$$

since *γ*ð Þ*t* ∈*S*, the authorized perturbations are *γs*ð Þ*t* ∈*S* such that *γ*0ðÞ¼ *t γ*ð Þ*t* , *γs*ð Þ¼ 0 *γ*ð Þ 0 and *γs*ð Þ¼ 1 *γ*ð Þ1 .

The postulate is that

$$\left. \frac{d}{ds} \right|\_{s=0} E(\chi\_s) = 0 \tag{17}$$

this gives

7 <sup>Ω</sup> <sup>¼</sup> *<sup>R</sup>*�<sup>1</sup> *R*\_ . *Geometric Control of Robotic Systems DOI: http://dx.doi.org/10.5772/intechopen.110895*

$$\int\_{0}^{1} m \dot{V}(t) \cdot \dot{\boldsymbol{\gamma}}(t) dt = \mathbf{0} \tag{18}$$

where *V t*ðÞ¼ *<sup>∂</sup> ∂s* � � *<sup>s</sup>*¼0*γs*ð Þ*<sup>t</sup>* <sup>∈</sup>*T<sup>γ</sup>*ð Þ*<sup>t</sup> <sup>S</sup>* after integrating by parts we get

$$\int\_{0}^{1} V(t). \gamma'(t)dt = 0\tag{19}$$

since *V t*ð Þ∈*T<sup>γ</sup>*ð Þ*<sup>t</sup> S*, we get that

$$
\gamma''(t) \perp T\_{\gamma(t)} \mathbb{S}. \tag{20}
$$

The most important step in these calculations is the following one

$$\frac{d}{dt}X(t).Y(t) = \frac{dX}{dt}.Y(t) + X(t).\frac{dY}{dt} \tag{21}$$

where *X*, *Y* are tangent vector field to *γ*ð Þ*t* ∈*S*, that is *X t*ð Þ, *Y t*ð Þ∈*Tγ*ð Þ*<sup>t</sup> S*. In this case the kinetic energy which is the Euclidean scalar product does not depend on the position, for our applications, the kinetic energy depends on the position so we need to construct a "good derivative" that allow us to derive scalar products by computing the product of derivative only without computing the derivative of the metric.

The parallel transport [7] of a vector *v*∈ *TqM* along a curve *γ*ð Þ*t* ∈ *M* is defined by the following way: the point at the origin of the vector follows the curve and the vector evolves continuously by preserving its length and the angle with *γ*\_ð Þ*t* .

This process defines a mapping *P*ð Þ *γ*ð Þ1 , *γ*ð Þ 0 : *Tγ*ð Þ <sup>0</sup> *M* ! *Tγ*ð Þ<sup>1</sup> *M* which is a linear isometry (preserving angle and length) (**Figure 6**).

The convariant derivative of two vector fields is

$$\nabla\_X Y(q) = \frac{d}{dt}\Big|\_{t=0} P(q, \phi\_t^X(q)) Y(\phi\_t^X(q)) \tag{22}$$

this covariant derivative satisfies

$$\nabla \mathbf{g} = \mathbf{0} \tag{23}$$

which means that

$$X\mathfrak{g}(Y,Z) = \mathfrak{g}(\nabla\_X Y, Z) + \mathfrak{g}(Y, \nabla\_X Z),\tag{24}$$

we say that this connexion kills the metric, in addition of that, this connexion is without torsion, that is we can close locally the parallelogram by doing parallel transport along geodesics by two different paths, the mathematical formula is

$$T(X,Y) = \nabla\_X Y - \nabla\_Y X - [X,Y] = \mathbf{0} \tag{25}$$

for the computations in coordinates, we defines

$$
\nabla\_{\partial\_i} \partial\_{\vec{j}} = \Gamma^k\_{\vec{ij}} \partial\_k \tag{26}
$$

#### **Figure 6.** *Parallel transport.*

the previous identities are<sup>8</sup>

$$
\Gamma^k\_{\vec{\eta}} = \Gamma^k\_{\vec{\mu}}, \quad T = \mathbf{0} \tag{27}
$$

$$\Gamma^{k}\_{\vec{\imath}\vec{\jmath}} = \frac{1}{2} \mathbf{g}^{km} \left( \mathbf{g}\_{m\vec{\jmath},i} + \mathbf{g}\_{mi\vec{\jmath}} - \mathbf{g}\_{\vec{\imath}\vec{\jmath},m} \right), \quad \nabla \mathbf{g} = \mathbf{0} \tag{28}$$

we construct a covariant derivative *<sup>D</sup> Dt* : Γð Þ!*γ* Γð Þ*γ* along curves *γ*ð Þ*t* ∈ *M* that satisfies


In coordinates we get by setting *X t*ðÞ¼ *<sup>X</sup><sup>i</sup>* ð Þ*<sup>t</sup> <sup>∂</sup><sup>i</sup>*∣*γ*ð Þ*<sup>t</sup>*

$$\frac{DX}{Dt} = \left(\frac{d}{dt}\left(X^i\right) + \Gamma^i\_{kl}\frac{d}{dt}\left(q^k\right)X^l\right)\partial\_{i\left[\gamma\left(t\right)\right]}.\tag{29}$$

This covariant derivative satisfies

$$\frac{d}{dt}\mathbf{g}(V,W) = \mathbf{g}\left(\frac{DV}{Dt}, W\right) + \mathbf{g}\left(V, \frac{DW}{Dt}\right). \tag{30}$$

<sup>8</sup> The component of the metric in local coordinates are *gij*ð Þ¼ *<sup>q</sup> gq <sup>∂</sup><sup>i</sup>*∣*<sup>q</sup>*, *<sup>∂</sup><sup>j</sup>*∣*<sup>q</sup>* .

*Geometric Control of Robotic Systems DOI: http://dx.doi.org/10.5772/intechopen.110895*

Now we suppose that our robot must evolves from *q*<sup>1</sup> to *q*<sup>2</sup> in *M* by minimizing the kinetic energy, we consider the real trajectory *q t*ð Þ and its admissible variations *qs* ð Þ*t* , by deriving we obtain

$$\frac{d}{ds}\Big|\_{s=0} \mathbf{E}(q\_s) = \frac{1}{2} \Big|\_{0}^{1} \mathbf{g}\_{q\_s(t)}(\dot{q}\_s(t), \dot{q}\_s(t))dt = \int\_{0}^{1} \mathbf{g}\_{q\_s(t)}\left(\frac{D}{Ds}\Big|\_{s=0} \dot{q}\_s(t), \dot{q}(t)\right)dt\tag{31}$$

using the Schwarz lemma

$$\frac{D}{Dt}\frac{\partial}{\partial t} = \frac{D}{Dt}\frac{\partial}{\partial t} \tag{32}$$

we get

$$\int\_{0}^{1} \mathbf{g}\_{q(t)} \left( \frac{D}{Dt} V(t), \dot{q}(t) \right) dt = \mathbf{0} \tag{33}$$

which gives by integration by parts that

$$\frac{D}{Dt}\dot{q} = 0\tag{34}$$

which is the geodesic equation which are curves that are parallel along themselves.

Now we will talk about curvature, curvature is the opposite of flatness, in an Euclidean space, when we look at the parallel transport along closed curves, the obtained linear map is always identity, in a sphere, the set of obtained linear mappings cover all *SO*ð Þ2 , because of the curvature of the sphere.

So the curvature is the change of a vector by parallel transport along the parallelogram obtained by geodesics and parallel transport, the mathematical formula is

$$R(X,Y)Z = \nabla\_X \nabla\_Y Z - \nabla\_Y \nabla\_X Z - \nabla\_{[X,Y]} Z \tag{35}$$

the curvature is a tensor, that is we can compute *R u*ð Þ , *v w* for *u*, *v*, *w* ∈*TqM* without needing any extention.

The sectional curvature is

$$K(u,v) = \frac{\mathbf{g}(R(u,v)v, u)}{|u|^2|v|^2 - \mathbf{g}(u,v)},\tag{36}$$

and it is the Gaussian curvature of the geodesic surface generated by ð Þ *u*, *v* . Curvature is also a factor in the sensitivity with respect to initial conditions, the geodesic deviation is related to the sign of curvature by the following formula

$$\frac{D^2 \dot{J}}{Dt^2} + R(\dot{J}, \dot{q})\dot{q} = 0\tag{37}$$

where *J* is a virtual motion that represents a local variation of geodesic (**Figure 7**).

When curvature is 0, the sensitivity is linear, which allow the Luenberger observer to be performant, when the curvature is negative, the geodesic instabillity is exponential [7], which explains that the Luenberger observer diverges (**Figure 8**).

**Figure 8.** *Negative curvature and sensitivity of the geodesic flow.*

The exponential map corresponds to a tangent vector the range by the geodesic after a unitary time, we denote it exp *<sup>q</sup>* : *Vq* ⊂*TqM* ! *M*, the exponential is a local diffeomorphism, so around a configuration, each configuration can be joined by a unique geodesic, the parallel transport along this geodesic is denoted by *P* (**Figure 9**).

when the exponential map is defined on *TqM*, we say the ð Þ *M*, *g* is complete, it is the fact when *M* is compact.

The geodesic distance is a natural distance induced by the Riemannian metric

$$d\_{\mathfrak{g}}(q\_1, q\_2) = \inf\_{\text{curves in }M \text{ joining } q\_1 \text{ to } q\_2.} \int\_0^1 \sqrt{\mathcal{g}\_{q(t)}(\dot{q}(t), \dot{q}(t))} dt. \tag{38}$$

Thanks to Cauchy-Schwarz inequality, we can show that

$$\frac{1}{2}d\_{\xi}(q\_1, q\_2)^2 = \frac{1}{2} \inf\_{\text{curves in }M\text{\"oning}\, q\_1 \text{ to }q\_2} \int\_0^1 \mathbf{g}\_{q(t)}(\dot{q}(t), \dot{q}(t)dt. \tag{39}$$

*Geometric Control of Robotic Systems DOI: http://dx.doi.org/10.5772/intechopen.110895*

The gradient of a function *f* is the unique vector field ∇*f* that satisfies

$$df(X) = \mathbf{g}(\nabla f, X) \tag{40}$$

The gradient of the function *U q*ð Þ¼ , *:* <sup>1</sup> 2 *d*2 *<sup>g</sup>* ð Þ *q*, *:* is

$$\nabla U(p) = -\exp\_p^{-1}(q). \tag{41}$$

The configuration space of the robot is a *n* dimensional compact manifold equipped with a Riemannian metric *g* that is the kinetic energy. In the presence of potential, the claculus of variations must applies to the Lagrangian *L q*ð Þ¼ , *<sup>v</sup>* <sup>1</sup> <sup>2</sup> *g v*ð Þ� , *v W q*ð Þ and this gives the equation

$$\frac{D\dot{q}}{Dt} = -\nabla W(q) \tag{42}$$

in robotics, we have a control law that allow us to enslave the robot, so the equation becomes [8, 9]

$$\frac{D\dot{q}}{Dt} = -\nabla W(q) + \mu \tag{43}$$

We recover a Newton-like equation, the left term quantifies an obstruction of parallelism, and the right hand term an exterior force, as well as the Newton fundamental law "The trajectory of an object remains parallel along itself as long as there are no forces applying to it" (**Figure 10**).

The kinetic energy theorem gives the variation of the mechanical energy in function of the applied forces, this will be useful to compute feedback law to ensure regulation (**Figure 11**)

$$\frac{d}{dt}\left(\frac{1}{2}\left|\dot{q}(t)\right|\_{q(t)}^2 + W(q(t))\right) = \mathbf{g}\_{q(t)}(\dot{q}(t), u(t)).\tag{44}$$

In some applications, especially those concerning the motion of a rigid body, the configuration space is a Lie group *G*, which is a group furnished with a manifold structure

**Figure 10.** *Inverse of the exponential.*

**Figure 11.** *Two link manipulator and its configuration space.*

such that multiplication and inverse are smooth, the consideration of a Riemannian metric that is left invariant allow us simplificate the geodesic equation [9], the curvature tensor and the parallel transport, and thus simplificate the practical implementation of the optimal regulator [10, 11] and the Riemannian observer [12].

#### **4. Regulation under constraints**

In this section, we will propose a method to ensure regulation under constraints, before going further, lets see the case of regulation without constraints.

Let fix some reference configuration *<sup>q</sup>* <sup>∗</sup> <sup>∈</sup> *<sup>M</sup>*, and consider *<sup>W</sup>* <sup>¼</sup> 0, by taking

*Geometric Control of Robotic Systems DOI: http://dx.doi.org/10.5772/intechopen.110895*

$$\mu(t) = \exp\_{q(t)}^{-1}(q^\*) - k\dot{q}(t) \tag{45}$$

we get

$$\frac{d}{dt}\left(\frac{1}{2}|\dot{q}(t)|\_{q(t)}^2 + \frac{1}{2}d\_{\mathcal{g}}^2(q(t),q^\*)\right) = -k|\dot{q}(t)|\_{q(t)}^2\tag{46}$$

which ensure regulation *q t*ðÞ! *<sup>q</sup>* <sup>∗</sup> by Lasalle's invariance principle, this result is very classical, but what we will prove in Section 6, is that this Riemannian PDregulator is optimal for a natural cost.

Now we are interested by the regulation of the tool of a robot [13], the tool is the terminal organ of the robot, the control of the tool of a robot is frequent tackled problem in industry (**Figure 12**).

The position of the tool is modelised by a function *<sup>x</sup>* : *<sup>M</sup>* ! <sup>3</sup> , we call the workspace *Ws* ¼ *x M*ð Þ the range of *x*, that is the set of all possible positions that can be tooken by the tool.

A singular configuration is a configuration where the tool can not moves locally in all directions, that is a configuration *q* ∈ *M* where *Tqx* is not onto.

We have the following result that relates absence of singularities to the possibility of regulation of the tool [10].

**Proposition 2** Suppose that the tool function is without singularities, take *xd* ∈*Ws* and

$$
\mu = -\nabla V - k\nu \tag{47}
$$

*where V q*ð Þ¼ <sup>1</sup> <sup>2</sup> k k *x q*ð Þ� *xd* 2 *, this controle stabilize the tool to xd whith* 0 *velocity*.

**Figure 12.** *Regulation of the tool.*

**Proof E:** quilibrium configurations of this dynamics are critical points of *V*, which are *q*∈ *M* such that

$$
\pi T\_q V(\nu) = (\mathfrak{x}(q) - \mathfrak{x}\_d) . d\mathfrak{x}\_q(\nu) = \mathbf{0} \tag{48}
$$

since *<sup>q</sup>* is not singular, we must have *<sup>q</sup>*∈*x*�1ð Þ *xd* .

Since the function <sup>1</sup> <sup>2</sup> j j *v* 2 *<sup>q</sup>* þ *V q*ð Þ satisfies Lasalle's invariance principle hypethesis ans

$$\frac{d}{dt}\left(V(q(t)) + \frac{1}{2} \left|\dot{q}(t)\right|\_{q(t)}^2\right) = -k|\dot{q}(t)|\_{q(t)}^2 \le 0\tag{49}$$

The large subset invariant subset *<sup>M</sup>* � 0 is <sup>Ω</sup> <sup>¼</sup> *<sup>x</sup>*�<sup>1</sup>ð Þ� *xd* 0. W

Now we turn to the regulation under constraints [13], if one wants to ensure regulation of the robot that leaves the tool in a safety area *<sup>S</sup>* <sup>¼</sup> <sup>Φ</sup>�<sup>1</sup> ð Þ 0 where <sup>Φ</sup> : <sup>3</sup> ! is a submersion on *x M*ð Þ (**Figure 13**).

We reduce the problem *xqt* ð Þ ð Þ <sup>∈</sup>*<sup>S</sup>* to *q t*ð Þ<sup>∈</sup> *<sup>N</sup>* where *<sup>N</sup>* <sup>¼</sup> *<sup>x</sup>*�<sup>1</sup>ð Þ*<sup>S</sup>* is a hypersurface of *M*, the same calculus of variations done in the previous section allow us to see that

$$\frac{D\dot{q}}{Dt} + \nabla U(q) = \mathbf{C} \in T\_q N^\perp \tag{50}$$

*C* can be seen as a contact force which force the configuration to stay in *N* (**Figure 14**)

Thanks to an inverse calculation [10] one can show that the expression of the unique orthogonal component that allow the tool to remain in *s* is given by

$$u(q, v) = \frac{- < D\_v \nabla \Psi(q), v >}{\left| \nabla \Psi(q) \right|^2} \nabla \Psi(q), \tag{51}$$

where Ψ ¼ *x*∘Φ.

In some papers, the problem of tracking is tackled, and the most useful result is this one [14].

**Figure 13.** *Regulation under constraints.*

*Geometric Control of Robotic Systems DOI: http://dx.doi.org/10.5772/intechopen.110895*

**Figure 14.** *Contact force.*

**Proposition 3** *For the dynamic Dq*<sup>0</sup> *Dt* ¼ *u, let a smooth trajectory reference on M, the regulator u* ¼ *uFF* þ *uPD decreases the function*

$$\mathbf{t} \in \mathbb{R} \to \frac{1}{2} d\_{\mathbf{g}}^2 \Big( q\_{r\mathbf{f}}(t), q(t) \Big) + \frac{1}{2} \Big| v(t) - P(q(t), q\_{r\mathbf{f}}(t)) v\_{r\mathbf{f}}(t) \Big|^2 \tag{52}$$

where

$$u\_{\rm FF}(q,v) = \frac{d}{dt}P\Big(q, q\_{\rm ref}(t)\Big)v\_{\rm ref}(t) + D\_v P\Big(q, q\_{\rm ref}\Big)v\_{\rm ref},\tag{53}$$

$$\mu\_{\rm PD} = \exp\_q^{-1}\left(q\_{\rm ref}(t)\right) - k.c\left(v - P\left(q, q\_{\rm ref}\right)v\_{\rm ref}\right). \tag{54}$$

#### **5. The Riemannian observer**

Another application of this formalism is the conception of observers of velocity. We want to get an estimation *t* ! ^*v t*ð Þ of the velocity with the only data of the configuration *q t*ð Þ.

We have explained in the second section that negative curvature is closely related to the sensitivity to the initial conditions of the geodesic flow. So for free systems, if we do not take into account the curvature, the observer will diverge if the observed initial velocity is far from the initial real velocity, even if the two configurations are very close.

The idea [15] is to copy the dynamic of the system and add a correction that cancel the curvature term that appears naturally in the derivation of the linearized equation around the real trajectory (Jacobi equation when absence of external forces). The observer of the dynamic

$$\frac{Dq'}{Dt} = \mathcal{S}(q),$$

is

$$\frac{d\hat{q}}{dt} = \hat{v} + k\_1 \exp\_{\hat{q}}^{-1}(q(t)),$$

$$\frac{D\hat{v}}{Dt} = P(\hat{q}, q(t))(S(q(t))) + R\left(\hat{v}, \exp\_{\hat{q}}^{-1}(q(t))\right)\hat{v} + k\_2 \exp\_{\hat{q}}^{-1}(q(t)).$$

**Figure 15.** *Convergence of the Riemannian observer.*

We can compute the form of this observer in the case of a Lie group furnished with a left invariant metric [12], this will give for a rigid body dynamic<sup>9</sup>

$$
\dot{\hat{R}} = \hat{R} . (\hat{\Omega} - \mathcal{Z}\dot{\jmath}(\zeta)),
\tag{55}
$$

$$\begin{aligned} \dot{\hat{\mathbf{o}}} &= I^{-1} . [(I.\hat{\mathbf{o}}) \times \hat{\mathbf{o}} - (I \hat{\mathbf{o}} \times \boldsymbol{\zeta} + I \boldsymbol{\zeta} \times \hat{\mathbf{o}})] + \boldsymbol{\zeta} \times \hat{\mathbf{o}} - 2\boldsymbol{\zeta} - 2\boldsymbol{\hat{R}}^T . R. \\ I^{-1} [skew(\boldsymbol{R})]^\times + \boldsymbol{R} (\hat{\boldsymbol{\alpha}}, \boldsymbol{\zeta}) \hat{\mathbf{o}}, \end{aligned} \tag{56}$$

with *ζ* ¼ *I* �1 *: log R<sup>T</sup>:R*^ � � � � � .

With the same conditions as the Luenberger observer of the first section, we get for the Riemannian observer the following simulations (**Figure 15**).

#### **6. Optimal regulation and tracking**

In this section we are concerned by the optimal control of robots, we consider the dynamics

$$\frac{D\dot{q}}{Dt} = u\tag{57}$$

and we consider the cost

$$J(u) = \int\_0^\infty \left[\frac{1}{2} |v\_u|^2\right]\_\mathcal{g} + \frac{1}{2} d\_\mathcal{g} \left(q\_u(t), q^\*\right)^2 + |u|\_\mathcal{g}^2 |e^{-\gamma t} dt \tag{58}$$

where *qu*, *vu* � � is the unique solution to the dynamics (after fixing an initial position and velocity).

9 logð Þ¼ *<sup>R</sup> <sup>ϕ</sup>*ð Þ *<sup>R</sup>* sinð Þ *<sup>ϕ</sup>*ð Þ *<sup>R</sup> skew R*ð Þ, with *<sup>ϕ</sup>*ð Þ¼ *<sup>R</sup>* arccos *tr R*ð Þ�<sup>1</sup> 2 � �. The HJB theory gives that the feedback control [10]

$$
\mu\_{\rm PD} = k \exp\_{q(t)}^{-1}(q^\*) - k' \dot{q}(t) \tag{59}
$$

where *k*, *k*<sup>0</sup> solves an appropriate Riccati equation, is the unique that solves the previous problem.

For the tracking problem, we have the following result [10].

**Proposition 4** *Let a smooth reference trajectory qref* : ½ �! 0, *T M, the tracking regulator that minimizes the cost*

$$J(u) = \int\_0^T \left[ \frac{1}{2} d\_{\mathcal{g}}^2 \left( q\_{\rm r\%}(t), q\_u(t) \right) + \frac{1}{2} \left| v\_u(t) - P(q\_u(t), q\_{\rm r\%}(t)) v\_{\rm r\%}(t) \right|^2 + \frac{1}{2} \left| u - u\_{\rm F\%}(t, q, v) \right|^2 \right] dt,\tag{60}$$

is a PD + Feed forward regulator *u* ¼ *uFF* þ *uPD* where

$$u\_{\rm FF}(t, q, \upsilon) = \frac{d}{dt} P\left(q, q\_{\rm ref}(t)\right) v\_{\rm ref}(t) + D\_{\upsilon} P\left(q, q\_{\rm ref}(t)\right) v\_{\rm ref}(t),\tag{61}$$

$$u\_{\rm PD}(t, q, v) = k\_1(t) \exp\_q^{-1} \left( q\_{\rm ref}(t) \right) - k\_2(t) \left[ v - P \left( q, q\_{\rm ref} \right) v\_{\rm ref}(t) \right],\tag{62}$$

*avec k*ð Þ 1, *k*<sup>2</sup> *solves an appropriate Riccati equation*. For the applications to a rigid body motion [11, 16]

$$R' = R\Omega,\tag{63}$$

$$
\underline{\alpha}' = I^{-1} . (I\alpha \times \alpha) + \text{r.} \tag{64}
$$

We have the following PD that ensures optimal regulation to *Rd*

$$\pi\_{\rm PD} = -k\_P \left[ \log \left( R\_d^T.\mathcal{R} \right) \right]^\times - k\_D \alpha. \tag{65}$$

For the optimal tracking problem, the PD + FF regulator is

$$\pi\_{\rm PD} = -k\_P \left[ \log \left( R\_{\rm ref}^T R \right) \right]^\times - k\_D \left( \alpha - R^T R\_{\rm ref}, \alpha\_{\rm ref} \right), \tag{66}$$

$$\tau\_{\rm FF} = \frac{1}{2} \left( \boldsymbol{\alpha} \times \left( \boldsymbol{R}^T \boldsymbol{R}\_{\rm ref}, \boldsymbol{\alpha}\_{\rm ref} \right) - I^{-1} .{\left( \left( I.R^T \boldsymbol{R}\_{\rm ref} \boldsymbol{\alpha}\_{\rm ref} \right) \times \boldsymbol{\alpha} + \left( I.\boldsymbol{\alpha} \right) \times \left( \boldsymbol{R}^T \boldsymbol{R}\_{\rm ref} \boldsymbol{\alpha}\_{\rm ref} \right) \right)} \right) \tag{67}$$

#### **7. Conclusion**

Classical robots allow us to ensure local regulation of the configuration and optimal control of kinematics of rigid body. However, some observations shows that it do not fit well with regulation under constraints and optimal regulation of the dynamics. We cannot explain the origin of the sensitivity with respect to initial conditions, which is present in most robotic systems and prevent us to design performant observers.

We have introduced the geometric formalism with a comprehensive way, and we explained the origin of the sensitivity with respect to initial conditions by the notion of curvature, that we avoid by the Riemannian observer.

We have exposed how to ensure regulation under constraints under a hypothesis of absence of singularities, and we showed how to ensure optimal regulation and tracking by algorithmic methods.

However, this method requires fastidious computations and sometimes difficult, approximations are quiete often used, and by doing that we see that we recover regulators that are proposed by the Euclidean approach.

We can see the classical robotics as an approximation of the geometric robotics, a bit like newtonian mechanics is the approximation of general relativity.

Geometric robotics is a field active in research, and most of recent results are presented in this chapter. Open problems remains in soft robotics, where we control a quasilinear hyperbolic PDE written in the Lie algebra of the group of Euclidean isometries, for this purpose one can see the work of [17] and the reference within.

#### **Author details**

Anis Bousclet Université de Technologies de Troyes, Rosières-Près-Troyes, France

\*Address all correspondence to: anis.bousclet@utt.fr

© 2023 The Author(s). Licensee IntechOpen. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### **References**

[1] Khalil W, Dombre E. Modélisation, identification et commande des robots. Paris: Hermes Science Publication; 1999

[2] Slotine J-J. Applied Nonlinear Control. Pearson; 1990

[3] Aguiar AP, Sacoon A, Hauser J. Exploration of kinematic optimal controlon the Lie Group SO(3). In: 8th IFAC Symposium on Nonlinear Control Systems. Vol. 43. Issue 14. 2010. p. 1302-1307

[4] Luenberger DG. Observing the state of a linear system. IEEE Transaction on Military Electronics. Vol. 8. New York. 1964. pp. 74-80

[5] Lee J. Introduction to Smooth Manifolds. Graduate Texts in Mathematics. 2012

[6] Arnold V. Mathematical Methods of Classical Mechanics. Graduate Texts in Mathematics. Vol. 60. New York: Springer; 1978

[7] Lee J. Riemannian Geometry: An Introduction to Curvature. New York: Springer-Verlag; 1997

[8] Altafini C. Geometric control methods for nonlinear systems and robotic applications [Ph.D thesis]. Sweden: Royal Institute of Technology, University of Stockholm; 2001

[9] Lewis AD, Bullo F. Geometric Control of Mechanical Systems. Texts in Applied Mathematics. 2005

[10] Bousclet A. Régulateurs Optimaux en Robotique. Mémoires de fin d'études de l'École Nationale Polytechnique; 2021

[11] Guo J, Liu C, Tang S. Intrinsic optimal control for mechanical systems on lie groups. Advances in Mathematical Physics. 2017:1-8

[12] Jordan M, Berg D, Maithripala HS. An Intrisic Observer for a Class of Simple Mechanical Systems on a Lie Group. Vol. 2. New Orleans USA: Proceedings of the American Control Conference; 2004. pp. 1546-1551

[13] Sekimoto M, Tahara K, Arimoto S, Yoshida M. A Riemannian geomtry approach for control of robotic systems under constraints. SICE Journal of Control, Measurement, and System Integration. 2021:107-116

[14] Murray RM, Bullo F. Tracking for fully actuated mechanical systems: A geometric framework. Automatica. 1999; **35**(1):17-34

[15] Aghannan N, Rouchon P. An intrinsic observer for a class of Lagrangian systems. IEEE Transactions on Automation and Control. 2003;**48**(6): 936-945

[16] Tayebi A, Berkane S. Some optimisation aspects on the lie group SO (3). IFAC-PapersOnLine. 2015;**48**(3): 1117-1121

[17] Bousclet A. Geometric Formulation of Dynamics of Snake Robot. Master thesis of Paris Saclay University; 2022

Section 4
