**5. Stability Analysis**

Once the leader and follower control laws and attitude observers have been presented we can state the following theorem which summarizes the main result of the paper.

**Theorem 2.** *Consider a group of nonholonomic mobile robots described by* (1) *in closed loop with the control laws* (22) *and* (24) *in combination with the attitude observer* (8)*. Furthermore, assume that* |*θ <sup>i</sup>*(0)<sup>|</sup> <sup>&</sup>lt; *<sup>π</sup> and <sup>x</sup>*˙ *<sup>i</sup>*(*t*) -0 *for all t* ≥ 0*. Then, the leader-follower formation is achieved, i.e.,*

$$\lim\_{t \to \infty} \tilde{x}\_i(t) = \mathbf{0}, \quad \lim\_{t \to \infty} \dot{\theta}\_i(t) = 0$$

*with i* = , f*j.*

**Proof.** First, we develop the closed loop dynamics of the position and attitude tracking errors. By taking into account (16a), (18), (20)–(22), (24) and (25), the position error dynamics is given by

$$
\mathfrak{F}\_i = -\mathbb{K}\_{\beta i} \mathfrak{F}\_i - \mathbb{K}\_{\eta i} \mathfrak{v}\_i \tag{26}
$$

$$
\dot{\eta}\_i = -\mathcal{K}\_{\eta i} \eta\_i - \upsilon\_i (\Theta\_i - \Theta\_{\mathrm{d}i}).\tag{27}
$$

By using trigonometric identities, the term *νi*(**Θ***<sup>i</sup>* − **Θ**d*i*) can be written as

$$\nu\_i(\Theta\_i - \Theta\_{\rm di}) = \psi\_i(t, \tilde{\theta}\_i)\tilde{\theta}\_i = \nu\_i \mathbb{R}\_i \begin{bmatrix} (1 - \cos(\tilde{\theta}\_i)) / \tilde{\theta}\_i \\ -\sin(\tilde{\theta}\_i) / \tilde{\theta}\_i \end{bmatrix} \tilde{\theta}\_i. \tag{28}$$

On the other hand, by taking into account the attitude observer (8) and attitude control laws (22b) and (24b), the attitude error dynamics is given by

$$\dot{\tilde{\theta}}\_{i} = -k\_{\text{oi}}\sin(\tilde{\theta}\_{i}) + k\_{\text{oi}}\Theta\_{\text{di}}^{\top}\mathcal{S}\mathcal{R}\_{i}(I - \tilde{\mathcal{R}}\_{i}^{\top})\mathbf{e}\_{1} \tag{29}$$

$$
\dot{\tilde{R}}\_i = (\omega\_{\bar{i}} - \widehat{\omega}\_{\bar{i}}) \tilde{\mathcal{R}}\_i \mathbf{S} \tag{30}
$$

where the equalities **Θ** d*i S***Θ***<sup>i</sup>* = sin(*θ <sup>i</sup>*) and **<sup>Θ</sup>***<sup>i</sup>* <sup>−</sup> **<sup>Θ</sup>**- *<sup>i</sup>* <sup>=</sup> *<sup>R</sup>i*(*<sup>I</sup>* <sup>−</sup> *<sup>R</sup> <sup>i</sup>* )*e*<sup>1</sup> have been used.

Finally, by defining *z<sup>i</sup>* = 0 *β <sup>i</sup> η i* 1 ∈ <sup>4</sup> the whole closed-loop dynamics can be written as follows

$$\Sigma\_1: \left\{ \begin{array}{c} \dot{\mathbf{z}}\_i = \mathbf{A}\_i \mathbf{z}\_i + \mathbf{Y}\_i(\mathbf{t}\_\prime \tilde{\theta}\_i)\tilde{\theta}\_i \end{array} \right. \\ \tag{31a}$$

$$\begin{aligned} \Sigma\_2: \begin{cases} \dot{\overline{\theta}}\_i = & -k\_{\rm oi} \sin(\overline{\theta}\_i) + k\_{\rm oi} \Theta\_{\rm di}^\top \mathcal{S} \mathcal{R}\_i (I - \overline{\mathcal{R}}\_i^\top) \mathbf{e}\_1 \\\ \dot{\overline{\mathcal{R}}}\_i = & (\omega\_i - \widehat{\omega}\_i) \overline{\mathcal{R}}\_i \mathcal{S} \end{cases} \end{aligned} \tag{31b}$$

where

$$\mathbf{A}\_{i} = \begin{bmatrix} -\mathbf{K}\_{\tilde{\rho}i} & -\mathbf{K}\_{\eta i} \\ \mathbf{O} & -\mathbf{K}\_{\eta i} \end{bmatrix}, \quad \mathbf{Y}\_{i}(t, \tilde{\theta}\_{i}) = \begin{bmatrix} \mathbf{0} \\ \mathbf{\varphi}\_{i}(t, \tilde{\theta}\_{i}) \end{bmatrix}.$$

Clearly, the complete closed-loop system presents a cascade structure between the position and attitude error dynamics. If *R<sup>i</sup>* = *I*, then, *R*-*<sup>i</sup>* = *R<sup>i</sup>* which implies that **Θ**- *<sup>i</sup>* = **Θ***i*. In this case, the term *<sup>ω</sup><sup>i</sup>* <sup>−</sup> *<sup>ω</sup>*-*<sup>i</sup>* becomes

$$\boldsymbol{\omega}\_{i} - \boldsymbol{\hat{\omega}}\_{i} = k\_{\text{a}i} \boldsymbol{\sigma}\_{i}^{\top} \mathbf{S} \boldsymbol{\Theta}\_{i} = \frac{k\_{\text{a}i}}{||\boldsymbol{\dot{\pi}}\_{i}||} \boldsymbol{\dot{\pi}}\_{i}^{\top} \mathbf{S} \boldsymbol{\Theta}\_{i} = \boldsymbol{0} \implies \dot{\boldsymbol{\bar{R}}} = \boldsymbol{\mathcal{O}}$$

The result follows from the nonholonomic constrain (2). The previous result shows that the equilibrium point of (31) is (*z <sup>i</sup>* , *θ <sup>i</sup>* , *<sup>R</sup> <sup>i</sup>* )=(**0**, *kπ*, *I*) with *k* = 0, 1, 2, . . ..

Now let us analyze the subsystem Σ<sup>2</sup> which is independent of the state *z<sup>i</sup>* and has a cascade structure with *k*o*i***Θ** d*i SRi*(*<sup>I</sup>* <sup>−</sup> *<sup>R</sup> <sup>i</sup>* )*e*<sup>1</sup> as an interconnection term. This term is bounded and according to Theorem 1 vanishes since the attitude observer error *R<sup>i</sup>* → *I* as *t* → ∞. Moreover, the equilibrium point *θ <sup>i</sup>* = 0 of the unperturbed system

$$\dot{\overline{\theta}}\_{i} = -k\_{\text{oi}} \sin(\overline{\theta}\_{i}) \tag{32}$$

is locally asymptotically stable with Lyapunov function *Vθ<sup>i</sup>* = 1 − cos(*θ <sup>i</sup>*) ∀*θ <sup>i</sup>* ∈ (−*π*, *π*). Furthermore, *θ <sup>i</sup>* = 0 is locally exponentially stable since the linear approximation of (32) is given by ˙ *θ<sup>i</sup>* = −*k*o*iθ <sup>i</sup>*. Therefore, according with Lemma A1 (see Appendix A) the subsystem ˙ *θ<sup>i</sup>* = −*k*o*<sup>i</sup>* sin(*θ <sup>i</sup>*) + *k*o*i***Θ** d*i SRi*(*<sup>I</sup>* <sup>−</sup> *<sup>R</sup> <sup>i</sup>* )*e*<sup>1</sup> is Input-to-State Stable (ISS) with input (*<sup>I</sup>* <sup>−</sup> *<sup>R</sup> <sup>i</sup>* )*e*1. Clearly, the attitude subsystem Σ<sup>2</sup> satisfies the condition of Theorem A1 given in Appendix A, thus, it is concluded that the equilibrium point (*θ <sup>i</sup>*, *Ri*)=(0, *I*) is uniformly asymptotically stable.

The position subsystem Σ<sup>1</sup> can be analyzed using similar arguments. It is straightforward to show that the matrix *A<sup>i</sup>* is Hurwitz. Therefore, the equilibrium point *z<sup>i</sup>* = **0** of the unforced subsystem *z*˙ *<sup>i</sup>* = *Aiz<sup>i</sup>* (with *θ <sup>i</sup>* = 0) is exponentially stable. This implies that the subsystem Σ<sup>1</sup> is ISS with input *θ <sup>i</sup>*. The closed loop system Σ<sup>1</sup> and Σ<sup>2</sup> satisfy the conditions of Theorem A1. As a result, it is concluded that (*zi*, *θ <sup>i</sup>*, *Ri*) → (**0**, 0, *I*) as *t* → ∞. The convergence of *z<sup>i</sup>* to zero implies that *η<sup>i</sup>* and *β<sup>i</sup>* also converge asymptotically to zero. Then, it follows that the position tracking error *x<sup>i</sup>* <sup>=</sup> *<sup>η</sup><sup>i</sup>* <sup>+</sup> *<sup>β</sup><sup>i</sup>* <sup>→</sup> **<sup>0</sup>** as *<sup>t</sup>* <sup>→</sup> <sup>∞</sup>. This completes the proof.

#### **6. Experimental Results**

In this section, experimental results are presented to validate the performance of the attitude observer and control laws developed in Sections 3 and 4. The testbed is composed of three Khepera III mobile robots from *K-Team* and six infrared *Optitrack* cameras which measure the Cartesian position of the robots (see Figure 2). Although the infrared cameras can also measure the orientation of the robot, This measurement is used only for comparison purposes and do not influence the behavior of the controller. The control laws and the observer were programmed in Matlab with a sample time of 20 [ms]. The control signals were sent to the robots via WIFI communication channel. Table 1 summarizes the parameter (a) (b)

values of the control law and the orientation observer employed in the experiments. It is worth to notice, that the initial postures of the robots are selected arbitrarily in every case.

**Figure 2.** Experimental testbed: (**a**) Differential drive Khepera III mobile robot, and (**b**) Optitrack infrared camera.



The control algorithms together with the attitude observer were tested using two desired trajectories, a circular path and a lemniscate curve. The parametric equations of both desired trajectories are shown below

$$\mathbf{x}\_{\text{dm}\_1} = \begin{bmatrix} 0.3 \cos \left( \frac{\pi}{15} t \right) \\\\ 0.3 \sin \left( \frac{\pi}{15} t \right) \end{bmatrix} \begin{bmatrix} \text{m} \end{bmatrix}, \quad \mathbf{x}\_{\text{dm}\_2} = \begin{bmatrix} 0.35 \sin \left( \frac{2\pi}{45} t \right) \\\\ 0.35 \sin \left( \frac{4\pi}{45} t \right) \end{bmatrix} \begin{bmatrix} \text{m} \end{bmatrix} \tag{33}$$

The Cartesian velocity *x*˙ *<sup>i</sup>* can be computed by means of numerical differentiation. However, we obtained better results with the following velocity observer [27]

$$\begin{aligned} \dot{\hat{\mathbf{x}}}\_i &= \mathbf{x}\_i (\mathbf{x}\_i - \hat{\mathbf{x}}\_i) + \boldsymbol{\mu}\_i \\ \dot{\boldsymbol{\mu}}\_i &= \mathbf{J}\_i (\mathbf{x}\_i - \hat{\mathbf{x}}\_i) \end{aligned}$$

where *<sup>κ</sup>i*, *<sup>ζ</sup><sup>i</sup>* ∈ 2×<sup>2</sup> are positive definite matrices and -*x<sup>i</sup>* ∈ <sup>2</sup> is an estimate of the Cartesian position *xi*.

The observer, controller gains and the parameters of the delay dynamic equation are shown in Table 1. Regarding the parameter *a*j, for the first trajectory was set to *a*<sup>1</sup> = 3 and for the second one we have *a*<sup>2</sup> = 5. All other parameters were the same for both trajectories.

The trajectories of the robots obtained during the two experiments with the desired Cartesian trajectories are shown in Figures 3 and 4, respectively. The figures also shown the robots' positions at the time instants *t* = 0 [s], *t* = 13 [s] for the first experiment and *t* = 0 [s] and *t* = 20 [s] for the second experiment. In both cases, after the transient response the robots successfully achieve the convoy formation.

**Figure 3.** Trajectory of the robot group following the circular path: Robots at the time instants *t* = 0 and *t* = 15 s.

**Figure 4.** Trajectory of the robot group following the lemniscate curve: (**a**) Robots at *t* = 0 s, and (**b**) Robots at *t* = 30 s.

In order to assess the performance of the attitude observer and control laws we compute the orientation errors *θ <sup>i</sup>* <sup>=</sup> *<sup>θ</sup><sup>i</sup>* <sup>−</sup> *<sup>θ</sup>*d*<sup>i</sup>* and ¯ *θ<sup>i</sup>* = *θ<sup>i</sup>* − *θ <sup>i</sup>* where *θ<sup>i</sup>* is the angle measured by the cameras and the estimated angle *θ <sup>i</sup>* is extracted from **Θ**- *<sup>i</sup>* as follows

$$\bar{\theta}\_i = \operatorname{atan2}(\bar{\Theta}\_{2i}, \bar{\Theta}\_{1i})$$

where atan2(·, ·) is the two argument arctangent function and **Θ**- *<sup>i</sup>* = col(Θ-1*i*, Θ-2*i*). The time evolution of the position and attitude tracking errors obtained in each experiment are shown in Figures 5 and 6. It is observed in the Figures that despite the unmodeled dynamics and discretization of the control laws, a good tracking was achieved. The time evolution of the time delays are shown in Figures 7 and 8. Notice that for the circular path the delays converge to a constant value while for the second trajectory the time delays change slowly while their magnitude increases at the points of the curve with greater curvature (see Figure 4b). This behavior was expected since at this points the robots come closer to each other.

In order to assess the performance of the proposed algorithm, the RMS error is computed for the distance and orientation of each robot to its desired trajectory, additionally to the RMS error of the estimation of the orientation observer is presented. Table 2 collects the results for the circular desired trajectory while Table 3 shows the results corresponding to the lemniscate curve desired trajectory experiment. On both cases it is observed that the RMS distance error is bellow 0.04 m, while RMS orientation error is under 0.155 radians.

**Figure 5.** Time evolution of the position and attitude errors in the first experiment (circular path): (**a**) Norm of the position error *x*i, (**b**) orientation error *<sup>θ</sup>* <sup>i</sup> <sup>=</sup> *<sup>θ</sup>*d*<sup>i</sup>* <sup>−</sup> *<sup>θ</sup>i*, (**c**) observation error ¯ *θ*<sup>i</sup> = *θ<sup>i</sup>* − *θ i*.

**Figure 6.** Time evolution of the position and attitude errors in the second experiment (lemniscate curve): (**a**) Norm of the position error *x*i, (**b**) orientation error *<sup>θ</sup>* <sup>i</sup> = *θ*d*<sup>i</sup>* − *θi*, (**c**) observation error ¯ *θ*<sup>i</sup> = *θ<sup>i</sup>* − *θ i*.

**Figure 7.** Time evolution of the delay for the first trajectory.

**Figure 8.** Time evolution of the delay for the second trajectory.

**Table 2.** RMS errors of distance, orientation and observed orientation of the robots in the circular trajectory experiment.


**Table 3.** RMS errors of distance, orientation and observed orientation of the robots in the lemniscate trajectory experiment.


Finally, Figures <sup>9</sup> and <sup>10</sup> show the control inputs. Notice that *<sup>ν</sup>i*(*t*) - 0 for all *t* ≥ 0 this implies that the assumption *<sup>x</sup>*˙ *<sup>i</sup>*(*t*) -0 is satisfied in both experiments.

**Figure 9.** Control inputs obtained in the first experiment: (**a**) *ν*i, (**b**) *ω*i.

**Figure 10.** Control inputs obtained in the second experiment: (**a**) *ν*i, (**b**) *ω*i.

#### **7. Conclusions**

In this paper, we proposed kinematic control laws in combination with an attitude observer that solve the problem of convoy formation for a group of nonholonomic mobile robots without using attitude measurements. The proposed control approach is based on the leader-follower scheme but contrary to other works, we used delayed reference signals for the follower robots. The time delays depend on the distance between the robots and are obtained as the outputs of a dynamical system that couples the leader and follower dynamics. As result, collisions between the members of the group are avoided. The kinematic control laws were designed by exploiting the cascade structure of the robots' kinematic model. The proposed controllers are decentralized since only require its own

position and the position of the nearest leader. On the other hand, the attitude observer was designed directly on *SO*(2) and it can be used in open and closed loop schemes. Finally, real-time experiments are presented to show the effectiveness of the proposed control-observer approach.

**Author Contributions:** Conceptualization, J.H.-A. and J.P.-J.; Formal analysis, J.H.-A. and J.P.-J.; Funding acquisition, C.C.-H.; Investigation, J.H.-A.; Methodology, R.M.-C.; Project administration, C.C.-H.; Software, R.M.-C.; Supervision, C.C.-H.; Validation, J.H.-A.; Writing—original draft, J.H.-A. and J.P.-J.; Writing—review & editing, C.C.-H. and R.M.-C. All authors have read and agreed to the published version of the manuscript.

**Funding:** This research was funded by Consejo Nacional de Ciencia y Tecnología, grant number 166654 and CICESE, grant number F0F156.

**Institutional Review Board Statement:** Not applicable.

**Informed Consent Statement:** Not applicable.

**Data Availability Statement:** Not applicable.

**Acknowledgments:** This work was supported by the CONACYT research project Synchronization of complex systems with applications under grant 166654 (A1-S-31628) and CICESE internal project F0F156 Generation of collective behaviors and their applications.

**Conflicts of Interest:** The authors declare no conflict of interest.

#### **Appendix A**

In this Appendix A, we recall some results regarding the stability of interconnected systems and Input-to-State stability (ISS) [28].

**Lemma A1.** *Consider the system x*˙ = *f*(*t*, *x*, *u*) *where f*(*t*, *x*, *u*) *is locally Lipschitz in (x,u) and uniformly in t. If the unforced system x*˙ = *f*(*t*, *x*, **0**) *has a uniformly asymptotically stable stable equilibrium point at x* = **0***, then the system is locally ISS.*

**Theorem A1.** *Consider the interconnected system*

$$
\dot{\mathbf{x}} = f(t, \mathbf{x}, \mathbf{y}) \tag{A1a}
$$

$$
\dot{\mathbf{y}} = \mathbf{g}(t, \mathbf{y}) \tag{A1b}
$$

*if the subsystem* (A1a) *with y as input is ISS and y* = **0** *is a uniformly asymptotically stable equilibrium point of the subsystem* (A1b)*, then, the origin (x*, *y)=(***0**, **0***) of the interconnected system* (A1a) *and* (A1b) *is uniformly asymptotically stable.*

#### **References**

