Formation Tracking with Orientation Convergence for Groups of Unicycles

Abstract

This paper presents three trajectory tracking control strategies for unicycle-type robots based on a leader-followers scheme. The leader robot converges asymptotically to a smooth trajectory, while the follower robots form an undirected open-chain configuration at the same time. It is also shown that the orientation angles of all the robots converge to the same value. The control laws are based on a dynamic extension of the kinematic model of each robot. The output function to be controlled is the midpoint of the wheel axis of every robot. This choice leads to an ill-defined control law when the robot is at rest. To avoid such singularities, a complementary control law is enabled momentarily when the linear velocity of the unicycles is close to zero. Finally, numerical simulations and real-time experiments show the performance of the control strategies.

1. Introduction

The coordination of multiple mobile robots involves a wide field of applications, such as toxic residue cleaning, transportation and manipulation of large objects, search and rescue, security, simulation of biological entities and behaviours, etc. [1]. Although legs and caterpillar-based locomotion has been studied in robotics literature, applications of multi-robot coordination have been mostly developed for wheeled mobile robots, specifically omnidirectional, unicycle and car-like robots. Current research focuses on the design and analysis of decentralized strategies that perform a desired group behaviour based on the local interactions of every robot with certain team members only. Therefore, the classical problems of a single mobile robot are extended to decentralized control laws applied to any number of robots and different scenarios of inter-robot communication. Current issues of multi-robot coordination can be decomposed into motion coordination, sensor networks, negotiation and task assignment [2, 3].

Motion coordination includes all the control laws that place or move the robots strategically within a workspace, according to the available local information about positions and goals of other team members. It encompasses formation control [4], collision avoidance [5], formation tracking [6] (also known as marching control [7]), dispersion, perimeter detection, traffic control, etc. The formation tracking control consists of the coordination of a group of robots to achieve a particular geometric pattern while the whole group follows a prescribed path (the marching path) at the same time. This problem is known as marching control or flocking behaviour [8]. The challenge of formation tracking is to ensure formation rigidity, obtained by the convergence of formation errors, when the robots are perturbed by the dynamics of the marching path. The main idea is to share as little information as possible about the goals and positions of other agents. This could reduce the computational load in the local processors of each agent. The problem is more complex in the case of non-holonomic robots than in the case of omnidirectional robots, specifically for unicycle-type robots, because the midpoint of the wheel axis cannot be stabilized by any continuous and time-invariant control law [9]. This restriction leads to an ill-defined control law when the robot is at rest. Thus, several works consider the front-points of robots as control outputs instead of the midpoint of the wheel axis in order to avoid singularities in the control law (see for instance [7, 10]). However, the resulting control laws do not influence the orientation angles directly, which remain uncontrolled and some conditions must be established to guarantee their convergence. Alternative works, such as [11], propose fuzzy logic control schemes but apply them to one robot only.

In the leader-followers scheme, the leader is a unique robot that possesses prior complete information about the marching trajectory, whilst the followers maintain some formation pattern during the path following [12]. To ensure formation preservation during path following, some information about the dynamics and velocity of the leader is transmitted directly or indirectly to all followers. For example, a convoy-type tracking formation is guaranteed if each robot knows the linear and angular velocities of the next robot in [13] or the velocity of the marching path in [7]. The trajectories of follower robots are designed in [14] to achieve relative orientation with respect to the leader. On the other hand, the convergence of the orientation angles is analysed in [15 and 16], if the leader always has a positive linear velocity. Finally, a control scheme based on dynamic feedback linearization is given in [16] and a path following control with tasks assigned to the follower robots is analysed in [17 and 18]. All this previous work does not address the singularities of the midpoint of the wheel axis. Also, the necessary information transmitted between robots can be translated in complex control algorithms, where the decentralization may be lost, due to the new requirements demanded by the control law for path following.

Inspired by our previous work [7], which related to the front point of unicycle robots, in this paper some control laws relating to the midpoint of the wheel axis are designed using a dynamic extension of the original kinematic model. The analysis is devoted to the case of the leader-follower scheme according to an open chain or convoy configuration [13]. The main aim of the paper is the design of three different control laws that ensure convergence of formation errors and convergence of the leader to the prescribed trajectory. The difference between these control laws resides in the state variables or signals that are fed back or fed forward, depending on the capacity of the agents and/or the communication devices. These approaches were presented briefly in [19]. It is proved that these three control laws also guarantee the convergence of the orientation angles. Our second contribution is the commutation of a secondary control law based on [20 and 21], when the longitudinal velocities are close to zero, which does not ensure path following but preserves the convergence of the orientation angles.

The paper is organized as follows. Section 2 presents the kinematic model of unicycles and the problem statement. The three control strategies are presented in Section 3, together with formal convergence proofs. Section 4 shows the performance of the marching control trough numerical simulations and real-time experiments. Finally some concluding remarks are given in Section 5.

2. Kinematic Model and Problem Statement

{R₁,…,R_n} denotes a set of n unicycle-type robots moving in the plane. The kinematic model of each robot R_i, as shown in Figure 1, is given by

Figure 1.

Kinematic model of unicycles

$\left[\begin{array}{c}{\stackrel{.}{\mathrm{x}}}_{\mathrm{i}}\\ {\stackrel{.}{\mathrm{y}}}_{\mathrm{i}}\\ {\stackrel{.}{\mathrm{\theta }}}_{\mathrm{i}}\end{array}\right]=\left[\begin{array}{cc}\cos {\mathrm{\theta }}_{\mathrm{i}}& 0\\ \sin {\mathrm{\theta }}_{\mathrm{i}}& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{v}}_{\mathrm{i}}\\ {\mathrm{w}}_{\mathrm{i}}\end{array}\right],\mathrm{ }\mathrm{i}=1,\dots ,\mathrm{n}$

(1)

where x_i,y_i,θ_i, are the coordinates and the orientation of the midpoint of the wheel axis and v_i, w_i are its longitudinal and angular velocity, respectively. The outputs of the system (1) are given by z_i = [x_i y_i]^T, i = 1,…,n and their dynamics are given by

${\stackrel{.}{\mathrm{z}}}_{\mathrm{i}}=\widetilde{{\mathrm{A}}_{\mathrm{i}}}\left({\mathrm{\theta }}_{\mathrm{i}}\right)\left[\begin{array}{c}{\mathrm{v}}_{\mathrm{i}}\\ {\mathrm{w}}_{\mathrm{i}}\end{array}\right],\mathrm{ }\widetilde{{\mathrm{ }\mathrm{A}}_{\mathrm{i}}}\left({\mathrm{\theta }}_{\mathrm{i}}\right)=\left[\begin{array}{cc}\cos {\mathrm{\theta }}_{\mathrm{i}}& 0\\ \sin {\mathrm{\theta }}_{\mathrm{i}}& 0\end{array}\right],$

(2)

where Ã(θ_i) is the so-called decoupling matrix of every R_i. The decoupling matrix is singular for every value of θ_i. Moreover, as mentioned before, the dynamical system (1) cannot be stabilized by any continuous and time-invariant control law [9]. Using a dynamical extension of the kinematic model, we consider

$\left[\begin{array}{c}{\stackrel{.}{\mathrm{x}}}_{\mathrm{i}}\\ {\stackrel{.}{\mathrm{y}}}_{\mathrm{i}}\\ \begin{array}{c}{\stackrel{.}{\mathrm{\xi }}}_{\mathrm{i}}\\ {\stackrel{.}{\mathrm{\theta }}}_{\mathrm{i}}\end{array}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\xi }}_{\mathrm{i}}\cos {\mathrm{\theta }}_{\mathrm{i}}\\ {\mathrm{\xi }}_{\mathrm{i}}\sin {\mathrm{\theta }}_{\mathrm{i}}\\ \begin{array}{c}{\mathrm{u}}_{\mathrm{i}}\\ {\mathrm{w}}_{\mathrm{i}}\end{array}\end{array}\right]$

(3)

where ξ_i = v_i is a new state variable and u_i is the new control signal. The previous system is obtained by the addition of a single integrator before input v_i in the original model. The dynamics of the variables z_i, i = 1,…,n for the extended system (3) is now given by

${\stackrel{‥}{\mathrm{z}}}_{\mathrm{i}}={\mathrm{A}}_{\mathrm{i}}\left({\mathrm{\theta }}_{\mathrm{i}},{\mathrm{\xi }}_{\mathrm{i}}\right)\left[\begin{array}{c}{\mathrm{u}}_{\mathrm{i}}\\ {\mathrm{w}}_{\mathrm{i}}\end{array}\right],$

(4)

Note that det[A_i(θ_i,ξ_i)]=ξ_i. Therefore, the new decoupling matrix A_i(θ_i,ξ_i) is non-singular whenever ξ_i ≠ 0. Considering this restriction, it is possible to design a strategy to control the output functions z_i. Based on the leader-followers scheme, consider R_n as the group leader and the rest as followers. Let z*_i = [x*_i y*_i]^T be the desired position of R_i in a particular formation pattern. In this work, we define z_i* as

$\begin{array}{c}{\mathrm{z}}_{\mathrm{i}}^{\mathrm{*}}={\mathrm{z}}_{\mathrm{i}+1}+{\mathrm{c}}_{\left(\mathrm{i}+1\right)\mathrm{i}},\mathrm{ }\mathrm{i}=1,\dots ,\mathrm{n}-1,\\ {\mathrm{z}}_{\mathrm{n}}^{\mathrm{*}}=\mathrm{m}\left(\mathrm{t}\right)\mathrm{ }\end{array}$

(5)

where c_(i+1)i = [c_(i+1)i,x c_(i+i)i,y]^T ϵ R²,i = 1, …, n - 1, denotes the desired relative position of R_i with respect to R_i+1 and m(t) = [x_d(t) y_d(t)T is a twice continuously differentiable function that corresponds to the desired trajectory of the leader. Therefore, the main objective of marching control with convergence of the orientation angles can be formally established as follows.

Problem statement (Marching with orientation): The control goal is to design a control law [u_i(t) w_i(t)] ^T = f_t(z_t(t),z_t+1(f)), i = 1,…, n- 1, for every follower robot and [u_n(t) w_n(t) ^T = f_n(z_n(t),m(t)) for the leader robot, such that

$\begin{array}{c}\underset{\mathrm{t}\to \mathrm{\infty }}{\lim }{(\mathrm{z}}_{\mathrm{i}}\left(\mathrm{t}\right)-{\mathrm{z}}_{\mathrm{i}}^{\mathrm{*}}(\mathrm{t}))=0,\mathrm{ }\mathrm{i}=1,\dots ,\mathrm{n}-1,\\ \mathrm{ }\underset{\mathrm{t}\to \mathrm{\infty }}{\lim }{(\mathrm{z}}_{\mathrm{n}}(\mathrm{t})-\mathrm{m}(\mathrm{t}))=0,\\ \underset{\mathrm{t}\to \mathrm{\infty }}{\lim }{(\mathrm{\theta }}_{\mathrm{i}}\left(\mathrm{t}\right)-{\mathrm{\theta }}_{\mathrm{j}}(\mathrm{t})=0,\forall \mathrm{i}\ne \mathrm{j}\end{array}$

Figure 2 illustrates the position of the robots when they satisfy the desired trajectory tracking and formation pattern. The goal of the leader is to follow the marching path, whereas the goal of the followers is to maintain the desired pattern formation. Note that the marching control also requires that the orientation angles converge to the same value.

Figure 2.

Marching with orientation control of unicycles

3. Control strategies

In this section, three marching strategies are presented for the group of unicycles. After that, a complementary control law is designed for when any of the main control laws approaches a singularity. The error coordinates are defined as

$\begin{array}{c}{\mathrm{e}}_{\mathrm{i}}={\mathrm{z}}_{\mathrm{i}}-{\mathrm{z}}_{\mathrm{i}}^{\mathrm{*}},\mathrm{ }\mathrm{i}=1,\dots ,\mathrm{n}-1,\\ {\mathrm{e}}_{\mathrm{n}}={\mathrm{z}}_{\mathrm{n}}-\mathrm{m}\left(\mathrm{t}\right).\end{array}$

(6)

The first n-1 coordinates correspond to the formation errors of the follower robots, while e_n corresponds to the path following error of the leader. Using these error coordinates, we define the following marching control laws

${\mathrm{\eta }}_1:\left\{\begin{array}{c}\left[\begin{array}{c}{\mathrm{u}}_{\mathrm{i}}\\ {\mathrm{w}}_{\mathrm{i}}\end{array}\right]={\mathrm{A}}_{\mathrm{i}}^{-1}\left({\mathrm{\theta }}_{\mathrm{i}},{\mathrm{\xi }}_{\mathrm{i}}\right)\left[\stackrel{‥}{\mathrm{m}}-{\mathrm{k}}_1{\stackrel{.}{\mathrm{e}}}_{\mathrm{i}}-{\mathrm{k}}_0{\mathrm{e}}_{\mathrm{i}}\right]\\ \left[\begin{array}{c}{\mathrm{u}}_{\mathrm{n}}\\ {\mathrm{w}}_{\mathrm{n}}\end{array}\right]={\mathrm{A}}_{\mathrm{n}}^{-1}\left({\mathrm{\theta }}_{\mathrm{n}},{\mathrm{\xi }}_{\mathrm{n}}\right)\left[\stackrel{‥}{\mathrm{m}}-{\mathrm{c}}_1{\stackrel{.}{\mathrm{e}}}_{\mathrm{n}}-{\mathrm{c}}_0{\mathrm{e}}_{\mathrm{n}}\right]\end{array}\right.$

(7)

${\mathrm{\eta }}_2:\left\{\begin{array}{c}\left[\begin{array}{c}{\mathrm{u}}_{\mathrm{i}}\\ {\mathrm{w}}_{\mathrm{i}}\end{array}\right]={\mathrm{A}}_{\mathrm{i}}^{-1}\left({\mathrm{\theta }}_{\mathrm{i}},{\mathrm{\xi }}_{\mathrm{i}}\right)\left[{\stackrel{‥}{\mathrm{z}}}_{\mathrm{i}+1}-{\mathrm{k}}_1{\stackrel{.}{\mathrm{e}}}_{\mathrm{i}}-{\mathrm{k}}_0{\mathrm{e}}_{\mathrm{i}}\right]\\ \left[\begin{array}{c}{\mathrm{u}}_{\mathrm{n}}\\ {\mathrm{w}}_{\mathrm{n}}\end{array}\right]={\mathrm{A}}_{\mathrm{n}}^{-1}\left({\mathrm{\theta }}_{\mathrm{n}},{\mathrm{\xi }}_{\mathrm{n}}\right)\left[\stackrel{‥}{\mathrm{m}}-{\mathrm{c}}_1{\stackrel{.}{\mathrm{e}}}_{\mathrm{n}}-{\mathrm{c}}_0{\mathrm{e}}_{\mathrm{n}}\right]\end{array}\right.$

(8)

${\mathrm{ }\mathrm{\eta }}_3:\left\{\begin{array}{c}\left[\begin{array}{c}{\mathrm{u}}_{\mathrm{i}}\\ {\mathrm{w}}_{\mathrm{i}}\end{array}\right]={\mathrm{A}}_{\mathrm{i}}^{-1}\left({\mathrm{\theta }}_{\mathrm{i}},{\mathrm{\xi }}_{\mathrm{i}}\right)\left[\stackrel{‥}{\mathrm{m}}-{\mathrm{k}}_1({\stackrel{.}{\mathrm{z}}}_{\mathrm{i}}-\stackrel{.}{\mathrm{m}})-{\mathrm{k}}_0{\mathrm{e}}_{\mathrm{i}}\right]\\ \left[\begin{array}{c}{\mathrm{u}}_{\mathrm{n}}\\ {\mathrm{w}}_{\mathrm{n}}\end{array}\right]={\mathrm{A}}_{\mathrm{n}}^{-1}\left({\mathrm{\theta }}_{\mathrm{n}},{\mathrm{\xi }}_{\mathrm{n}}\right)\left[\stackrel{‥}{\mathrm{m}}-{\mathrm{c}}_1{\stackrel{.}{\mathrm{e}}}_{\mathrm{n}}-{\mathrm{c}}_0{\mathrm{e}}_{\mathrm{n}}\right] \end{array}\right.$

(9)

for i = 1,…, n - 1, where k₀, k₁, c₀, c₁ are scalar design parameters. In the first strategy (7), the control law of every follower robot R_i includes the feed forward of the marching path acceleration and the position and velocity of robot R_i+1. This is modified in the second strategy (8) so that the follower robot R_i now has feedback with the position, velocity and acceleration o robot R_i+1 instead of the marching path. Finally, in the third strategy (9), the follower robots include the feed forward of the velocity and acceleration of the marching path and the position of the robot R_i+1. Note that the term ż_i - · in (9) can be represented in the error coordinates as $\sum _{\mathrm{i}}^{\mathrm{n}}{\stackrel{.}{\mathrm{e}}}_{\mathrm{i}}$ . The control law for the leader robot is the same for all marching strategies.

Remark 1. Note that the leader is the only robot with complete information about the position, velocity and acceleration of the marching path. Thus, the advantage of the control strategies is that the follower robots are not required to process complete information about the path of marching and the positions of all agents. In related works, for instance [22], all agents must know the target position of the leader, the marching trajectory and more than one desired relative positions with respect to other robots. Therefore, the proposed control laws constitute a decentralized approach.

Theorem 1. Consider the system (3) and the control laws (7), (8) or (9). Suppose that ${\xi }_i\ne 0\mathrm{ }\forall t\ge 0,\mathrm{ }{\mathrm{ }k}_0,\mathrm{ }{k_1,c}_0,\mathrm{ }{c}_1>0$ . Then, in the closed-loop systems (3)-(7), (3)-(8) or (3)-(9) the follower robots converge to the desired formation, i.e., $\mathrm{ }\underset{t\to \mathrm{\infty }}{\mathit{lim}}{(z}_i-z_i^{\mathrm{*}}),\mathrm{ }i=1,\dots ,n-1\mathrm{ }$ , whereas R_n converge to the marching trajectory i.e., $\mathrm{ }\underset{t\to \mathrm{\infty }}{\mathit{lim}}{(z}_n-m)=0$ . Moreover, it holds that $\mathrm{ }\underset{t\to \mathrm{\infty }}{\mathit{lim}}{(\theta }_i\left(t\right)-{\theta }_j\left(t\right))=0,\forall i\ne j$ .

Proof. The dynamics of the error coordinates for the closed loop systems (3)-(7), (3)-(8) or (3)-(9) are given, respectively, by

$\stackrel{‥}{\mathrm{e}}=\left({\mathrm{F}}_{\mathrm{i}}\otimes {\mathrm{I}}_2\right)\stackrel{.}{\mathrm{e}}+\left({\mathrm{D}}_{\mathrm{i}}\otimes {\mathrm{I}}_2\right)\mathrm{e}$

(10)

where ⊗ denotes the Kronecker product and the matrices F_i, D_i are given, respectively, by

$\begin{array}{c}{\mathrm{F}}_1=\left[\begin{array}{ccccccc}{-\mathrm{k}}_1& {\mathrm{k}}_1& 0& 0& \cdots & 0& 0\\ 0& {-\mathrm{k}}_1& {\mathrm{k}}_1& 0& \cdots & 0& 0\\ ⋮& & & & \ddots & & ⋮\\ 0& 0& 0& 0& \cdots & {-\mathrm{k}}_1& {\mathrm{c}}_1\\ 0& 0& 0& 0& \cdots & 0& {-\mathrm{c}}_1\end{array}\right],\\ {\mathrm{D}}_1=\left[\begin{array}{ccccccc}{-\mathrm{k}}_0& {\mathrm{k}}_0& 0& 0& \cdots & 0& 0\\ 0& {-\mathrm{k}}_0& {\mathrm{k}}_0& 0& \cdots & 0& 0\\ ⋮& & & & \ddots & & ⋮\\ 0& 0& 0& 0& \cdots & {-\mathrm{k}}_0& {\mathrm{c}}_0\\ 0& 0& 0& 0& \cdots & 0& {-\mathrm{c}}_0\end{array}\right],\\ \begin{array}{c}{\mathrm{F}}_2=\left[\begin{array}{ccccccc}{-\mathrm{k}}_1& 0& 0& 0& \cdots & 0& 0\\ 0& {-\mathrm{k}}_1& 0& 0& \cdots & 0& 0\\ ⋮& & & & \ddots & & ⋮\\ 0& 0& 0& 0& \cdots & {-\mathrm{k}}_1& 0\\ 0& 0& 0& 0& \cdots & 0& {-\mathrm{c}}_1\end{array}\right],\\ \begin{array}{c}{\mathrm{D}}_2=\left[\begin{array}{ccccccc}{-\mathrm{k}}_0& 0& 0& 0& \cdots & 0& 0\\ 0& {-\mathrm{k}}_0& 0& 0& \cdots & 0& 0\\ ⋮& & & & \ddots & & ⋮\\ 0& 0& 0& 0& \cdots & {-\mathrm{k}}_0& 0\\ 0& 0& 0& 0& \cdots & 0& {-\mathrm{c}}_0\end{array}\right],\\ F_3=\left[\begin{array}{cccccccc}{-k}_1& 0& 0& 0& \cdots & 0& 0& 0\\ 0& {-k}_1& 0& 0& \cdots & 0& 0& 0\\ ⋮& & & & \ddots & & & ⋮\\ 0& 0& 0& 0& \cdots & {-k}_1& 0& 0\\ 0& 0& 0& 0& \cdots & 0& {-k}_1& {-k}_1{+c}_1\\ 0& 0& 0& 0& \cdots & 0& 0& {-c}_1\end{array}\right]\end{array}\end{array}\end{array}$

and

$D_3=\left[\begin{array}{cccccccc}{-k}_0& k_0& 0& 0& \cdots & 0& 0& 0\\ 0& {-k}_0& k_0& 0& \cdots & 0& 0& 0\\ ⋮& & & & \ddots & & & ⋮\\ 0& 0& 0& 0& \cdots & {-k}_0& k_0& 0\\ 0& 0& 0& 0& \cdots & 0& {-k}_0& c_0\\ 0& 0& 0& 0& \cdots & 0& 0& {-c}_0\end{array}\right].$

The key point now is to choose an appropriate permutation of the error coordinates. Define

$\mathrm{\gamma }={\left[{\mathrm{e}}_{11}\mathrm{ }{\stackrel{.}{\mathrm{e}}}_{11}\mathrm{ }{\mathrm{e}}_{12}\mathrm{ }{\stackrel{.}{\mathrm{e}}}_{12\mathrm{ }\cdots \mathrm{ }}{\mathrm{e}}_{\mathrm{n}1}\mathrm{ }{\stackrel{.}{\mathrm{e}}}_{\mathrm{n}1}\mathrm{ }{\mathrm{e}}_{\mathrm{n}2}\mathrm{ }{\stackrel{.}{\mathrm{e}}}_{\mathrm{n}2}\mathrm{ }\right].}^{\mathrm{T}}$

(11)

With this choice, the dynamics of the error coordinates for the three closed-loop systems are given, respectively, by

$\stackrel{.}{\mathrm{\gamma }}=\left({\mathrm{M}}_{\mathrm{i}}\otimes {\mathrm{I}}_2\right)\mathrm{\gamma }$

(12)

where

${\mathrm{M}}_1=\left[\begin{array}{cccccccc}\mathrm{A}& {\mathrm{A}}_1& 0& 0& \cdots & 0& 0& 0\\ 0& \mathrm{A}& {\mathrm{A}}_1& 0& \cdots & 0& 0& 0\\ ⋮& & & & \ddots & & & ⋮\\ 0& 0& 0& 0& \cdots & 0& \mathrm{A}& {\mathrm{A}}_2\\ 0& 0& 0& 0& \cdots & 0& 0& {\mathrm{A}}_{\mathrm{c}}\end{array}\right],$

with

$\begin{array}{c}\mathrm{A}=\left[\begin{array}{cc}0& 1\\ {-\mathrm{k}}_0& {-\mathrm{k}}_1\end{array}\right],\mathrm{ } {\mathrm{A}}_1=\left[\begin{array}{cc}0& 0\\ {\mathrm{k}}_0& {\mathrm{k}}_1\end{array}\right],\\ {\mathrm{A}}_2=\left[\begin{array}{cc}0& 0\\ {\mathrm{c}}_0& {\mathrm{c}}_1\end{array}\right],\mathrm{ } {\mathrm{ } \mathrm{A}}_{\mathrm{c}}=\left[\begin{array}{cc}0& 1\\ {-\mathrm{c}}_0& {-\mathrm{c}}_1\end{array}\right],\\ {\mathrm{M}}_2=\left[\begin{array}{cccccc}\mathrm{B}& 0& 0& \cdots & 0& 0\\ 0& \mathrm{B}& 0& \cdots & 0& 0\\ ⋮& & & \ddots & & ⋮\\ 0& 0& 0& \cdots & {\mathrm{B}}_{\mathrm{c}}& 0\\ 0& 0& 0& \cdots & \cdots & {\mathrm{B}}_{\mathrm{c}}\end{array}\right],\end{array}$

and

$\mathrm{B}=\left[\begin{array}{cc}0& 1\\ {-\mathrm{k}}_0& {-\mathrm{k}}_1\end{array}\right],\mathrm{ } {\mathrm{B}}_{\mathrm{c}}=\left[\begin{array}{cc}0& 1\\ {-\mathrm{c}}_0& {-\mathrm{c}}_1\end{array}\right]$

with

${\mathrm{M}}_3=\left[\begin{array}{ccccccccc}\mathrm{R}& {\mathrm{R}}_1& 0& 0& \cdots & 0& 0& 0& 0\\ 0& \mathrm{R}& {\mathrm{R}}_1& 0& \cdots & 0& 0& 0& 0\\ ⋮& & & & \ddots & & & & ⋮\\ 0& 0& 0& 0& \cdots & \mathrm{R}& {\mathrm{R}}_2& 0& 0\\ 0& 0& 0& 0& \cdots & 0& \mathrm{R}& {\mathrm{R}}_2& 0\\ 0& 0& 0& 0& \cdots & 0& 0& 0& {\mathrm{R}}_{\mathrm{c}}\end{array}\right],$

It turns out that, for each one of the cases under analysis, the matrices M_i are block upper triangular. Therefore, the analysis of the spectra of M_i is reduced to the study of simple two-dimensional matrices. The fact that, for every control law k₀,k₁,c₀, c₁ > 0 implies straightforwardly that the whole dynamics are asymptotically stable.

On the other hand, the fact that $\underset{t\to \infty }{\mathit{lim}}{(\mathrm{z}}_{\mathrm{i}}-{\mathrm{z}}_{\mathrm{i}}^{\mathrm{*}})=0$ implies directly that $\underset{t\to \infty }{\mathit{lim}}{\stackrel{.}{\mathrm{x}}}_{\mathrm{i}}=\underset{t\to \infty }{lim} {\stackrel{.}{\mathrm{x}}}_{\mathrm{i}+1}$ and that $\underset{t\to \infty }{\mathit{lim}}{\stackrel{.}{\mathrm{y}}}_{\mathrm{i}}= \underset{t\to \infty }{\mathit{lim}}{\stackrel{.}{\mathrm{y}}}_{\mathrm{i}+1}$ . Since ${\mathrm{\theta }}_{\mathrm{i}}={\tan }^{-1}\left(\frac{{\stackrel{.}{\mathrm{y}}}_{\mathrm{i}}}{{\stackrel{.}{\mathrm{x}}}_{\mathrm{i}}}\right),\mathrm{ }\mathrm{ }\forall \mathrm{i}$ it follows that $\underset{t\to \infty }{\mathit{lim}}{\mathrm{\theta }}_{\mathrm{i}}=\underset{\mathrm{t}\to \mathrm{\infty }}{\mathrm{l}\mathrm{i}\mathrm{m}}{\mathrm{\theta }}_{\mathrm{j}},\mathrm{ }\forall \mathrm{i}\ne \mathrm{j}$ .

3.1. Commutation scheme

Recall that the control laws η₁,η₂ and η₃ are ill defined when ξ_i = 0. When the robots are close to rest, consider the alternative output functions

${\mathrm{h}}_{\mathrm{i}}={\left[\begin{array}{cc}{\mathrm{\xi }}_{\mathrm{i}}& {\mathrm{\theta }}_{\mathrm{i}}\end{array}\right]}^{\mathrm{T}},\mathrm{ }\mathrm{i}=1,\dots ,\mathrm{n}$

(13)

The dynamics of these variables is simply given by

$\left[\begin{array}{c}{\stackrel{.}{\mathrm{\xi }}}_{\mathrm{i}}\\ {\stackrel{.}{\mathrm{\theta }}}_{\mathrm{i}}\end{array}\right]=\mathrm{M}\left[\begin{array}{c}{\mathrm{u}}_{\mathrm{i}}\\ {\mathrm{w}}_{\mathrm{i}}\end{array}\right],\mathrm{ }\mathrm{M}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$

(14)

where M is the non-singular decoupling matrix. Therefore, it is possible to define the next feedback

${\mathrm{\eta }}_4:\mathrm{ }\left[\begin{array}{c}{\mathrm{u}}_{\mathrm{i}}\\ {\mathrm{w}}_{\mathrm{i}}\end{array}\right]={\mathrm{h}}_{\mathrm{d}}-{\mathrm{q}}_0\left({\mathrm{h}}_{\mathrm{i}}-{\mathrm{h}}_{\mathrm{d}}\right),\mathrm{ }\mathrm{i}=1,\dots ,\mathrm{n},$

(15)

where h_d = [h_1d h_2d]^T = [ξ_d θ_d]^T and q₀ is a design parameter. The control law η₄ is the alternative strategy used when control laws (7), (8) or (9) are closed to a singularity. The rule of commutation can be established by

$\mathrm{\eta }=\left\{\begin{array}{l}{\mathrm{\eta }}_1,\mathrm{ }{\mathrm{\eta }}_2,\mathrm{ }{\mathrm{\eta }}_3\mathrm{ }\mathrm{w}\mathrm{h}\mathrm{e}\mathrm{n}\mathrm{ }\left|{\mathrm{\xi }}_{\mathrm{i}}\right|\ge \mathrm{\delta }\hfill \\ {\mathrm{\eta }}_4\mathrm{ } \mathrm{w}\mathrm{h}\mathrm{e}\mathrm{n}\mathrm{ }\left|{\mathrm{\xi }}_{\mathrm{i}}\right|<\delta \hfill \end{array}\right.$

(16)

with δ > 0 as the commutation threshold. Finally, it is necessary to define the desired trajectory of h_d to apply control law (15) according to

${\mathrm{\xi }}_{\mathrm{d}}={\stackrel{.}{\mathrm{x}}}_{\mathrm{d}}cos{\mathrm{ }\mathrm{\theta }}_{\mathrm{d}}+{\stackrel{.}{\mathrm{y}}}_{\mathrm{d}}sin{\mathrm{ }\mathrm{\theta }}_{\mathrm{d}},\mathrm{ }{\mathrm{ }\mathrm{\theta }}_{\mathrm{d}}={\tan }^{-1}\left(\frac{{\stackrel{.}{\mathrm{y}}}_{\mathrm{d}}}{{\stackrel{.}{\mathrm{x}}}_{\mathrm{d}}}\right)\mathrm{ }$

(17)

Control law (15) does not guarantee path following by the marching control. It only attempts to preserve the convergence of the orientation angles during the commutation interval.

Theorem 2. Consider the switched closed-loop system (3)-(16) using any of the control laws (7), (8) or (9) and the alternative control law (15) and suppose q₀ > 0. Then, it holds that $\underset{t\to \mathrm{\infty }}{lim}{(\theta }_i-{\theta }_j)=0,\mathrm{ }\forall i\ne j$

Proof. Define the orientation error coordinates as ϵ_i = θ_i - θ_d for i = 1,…, n. Control law (15) is globally defined and imposes the following first order dynamics

${\stackrel{.}{\mathrm{ϵ}}}_{\mathrm{i}}=-{\mathrm{q}}_0{\mathrm{ϵ}}_{\mathrm{i}},\mathrm{ }\mathrm{i}=1,\dots ,\mathrm{n}.$

The results follow.

4. Numerical Simulations and Real-Time Experiments

4.1. Simulation results

Figure 3 shows a numerical simulation of the closed-loop system (3)-(7) for n = 3,k₀ = k₁ = c₁ = 5, c₀ = 3, q₀ = 2, c₃₂ = [-0.3 0], c₂₁ = [-0.3 0] and δ = 0.017. The desired formation pattern is a horizontal line where the robots are separated by a distance equal to 0.3m and the desired marching path is a parabola given by m(t) = [0.1 sin(wt) 0.1sin²(wt)] where $\mathrm{w}=\frac{2\mathrm{\pi }}{\mathrm{T}}$ with period T = 10. The initial conditions are given by

Figure 3.

Trajectory of robots in plane.

$\begin{array}{c}\left[\begin{array}{cc}{\mathrm{x}}_1& \begin{array}{ccc}{\mathrm{y}}_1& {\mathrm{\xi }}_1& {\mathrm{\theta }}_1\end{array}\end{array}\right]=\left[\begin{array}{cc}-0.63& \begin{array}{ccc}0.02& 0& 0\end{array}\end{array}\right],\\ \left[\begin{array}{cc}{\mathrm{x}}_2& \begin{array}{ccc}{\mathrm{y}}_2& {\mathrm{\xi }}_2& {\mathrm{\theta }}_2\end{array}\end{array}\right]=\left[\begin{array}{cc}-0.28& \begin{array}{ccc}-0.04& 0& 0\end{array}\end{array}\right],\\ \left[\begin{array}{cc}{\mathrm{x}}_3& \begin{array}{ccc}{\mathrm{y}}_3& {\mathrm{\xi }}_3& {\mathrm{\theta }}_3\end{array}\end{array}\right]=\left[\begin{array}{cc}0.03& \begin{array}{ccc}0.03& 0& 0\end{array}\end{array}\right].\end{array}$

Note that the orientation angles errors (θ_i - θ_j) of Figure 4 converge to zero. Finally, the control inputs are presented in Figure 5 where the effects of the commutation become apparent.

Figure 4.

Orientation angles errors.

Figure 5.

Control for leader and follower robots.

4.2. Comparison of the control laws

A comparison among the three control strategies is done by using a performance index defined by

$J\left(e\left(t_f\right)\right)=\frac{1}{t_f}\underset{0}{\overset{t_f}{\int }}\left(\sqrt{e_1^2\left(t\right)+e_2^2\left(t\right)+e_3^2\left(t\right)}\right)dt$

where e_i(t) = ||z_i(t) - z*_t(t) ||. Choosing t_f = 20 with the same initial conditions and parameters given in the simulation result, we can determine that J(e(t_f)) = 0.1627 for η₁, J(e(t_f)) = 0.1616 for η₂ and J(e(t_f)) = 0.1456 for η₃. With these results we can conclude that the performance of the three control strategies is similar and it will depend on the information available in order to choose the correct control law.

4.3. Real-time experiments

The Real-Time Experiments in this Section were carried out over an experimental setup composed of the following.

1. Three Yujin Robots, model YSR-A normally used for soccer competitions, each one equipped with a RF receiver, two brushless motors, two 512 pulses per turn encoders and built-in velocity controllers (Figure 6).

2. One DALSA video camera, model Genie M640-1/3, able to provide 64 frames per second at a maximum resolution of 640×480 pixels.

3. Two Pentium 4-based computers. The first computer processes the data provided by the video camera to estimate the posture of each robot. The second computer calculates and sends the required control input for each robot through a RF transmitter (Figure 7). Both computers are linked through unidirectional RS-232 communication.

Figure 6.

Yujin Robots.

Figure 7.

RF transmitter.

The experiments were completed using a sampling period of 20ms. The vision computer works using C# with the Foundation Package of Common Vision Blox. The control computer works using Matlab. Figure 8 shows a comparison of a numerical simulation and the real-time experiment involving the closed-loop system (3)-(8) for n = 3, k₀ = c₀ = 16, k₁ = c₁ = 8,q₀ =2 and δ = 0.03. The desired formation is a triangle-shape pattern given by c₃₂ = [0.2,0.2] and c₂₁ = [-0.2,0.2]. The initial conditions are

Figure 8.

Trajectory of robots in plane.

$\begin{array}{l}\mathrm{ }\left[{\mathrm{x}}_1\mathrm{ }{\mathrm{ }\mathrm{y}}_1{\mathrm{ }\mathrm{\xi }}_1\mathrm{ }{\mathrm{\theta }}_1\right]=\left[\begin{array}{cc}0.67\hfill & -\begin{array}{ccc}0.53\hfill & 0\hfill & 0.34\hfill \end{array}\hfill \end{array}\right],\hfill \\ \mathrm{ }\left[{\mathrm{x}}_2\mathrm{ }{\mathrm{y}}_2\mathrm{ }{\mathrm{\xi }}_2\mathrm{ }{\mathrm{\theta }}_2\right]=\left[\begin{array}{cc}0.47\hfill & \begin{array}{ccc}-0.4\hfill & 0\hfill & 0.42\hfill \end{array}\hfill \end{array}\right],\hfill \\ \left[{\mathrm{x}}_3\mathrm{ }{\mathrm{y}}_3\mathrm{ }{\mathrm{\xi }}_3\mathrm{ }{\mathrm{\theta }}_3\right]=\left[\begin{array}{cc}0.3\hfill & \begin{array}{ccc}-0.57\hfill & 0\hfill & 0.76\hfill \end{array}\hfill \end{array}\right].\hfill \end{array}$

The desired marching path is the same as that of the previous simulation. Note that the orientation angle errors (θ_i - θ_j) obtained both in the simulation and the real-time experiment in Figure 9 converge to zero. Finally, the control inputs are presented in Figure 10 and 11, where the effects of the commutation can be appreciated.

Figure 9.

Orientation angles errors.

Figure 10.

Control for right wheels of the leader and follower robots.

Figure 11.

Control for left wheels of the leader and follower robots.

5. Conclusions

Three control strategies have been proposed to solve the problem of formation tracking for groups of unicycles, using the extended kinematic model of the unicycle type-robot and partial information about other robots and the marching trajectory. The control laws steer the midpoint of the wheels' axis, therefore, exhibiting singularities when the longitudinal velocities are close to zero. To solve this drawback, a complementary control law is proposed, which does not ensure convergence to the desired trajectory, but preserves convergence of the orientation angles during the commutation intervals. The approach was successfully tested both through numerical simulations and real-time experiments. A natural outlook of this work would be to generalize the results to any type of connected graphs, not necessarily the open chain topology.