A Stable Formation Control Using Approximation of Translational and Angular Accelerations

2.1. Formation

We consider a group of non-holonomic mobile robots move along a desired trajectory while maintaining a desired formation. In any case and at any time, the group of robots must do two basic tasks: forming and maintaining. Fig. 1 illustrates an example where a robot team moves along a road with a requirement to maintain a pyramid formation when the road is wide enough and a sequential formation when the road is narrow. The formation is, therefore, required to switch back and forth between the two configurations. With the leader-follower formation strategy, there is defined a group leader R0 which leads the group bulk motion, and the other robots, labelled as R _i (i = 1, 2, … n) are the followers that maintain the respective relationships with the group leader R₀, in general. However, when the number of robots in the group is large, the relationships of some followers with R₀ are hard to define due to the limitation of sensors' working range. Therefore, the definition of whole group relationships is a combination of unit relationships. Each unit contains one follower and one leader (SLSF) or two leaders (TLSF). The leaders here are local leaders, which are the robots physically close to the follower for easy sensorial connection. Hence, all robots in the group are linked, either directly or indirectly. Without loss of generality, we assume that the desired formation configurations are designed in a feasible way such that the robots can form the required formations without conflicts with each other.

OPEN IN VIEWER

Figure 1.

Motion and formation process of a group of robots with two basic tasks: forming and maintaining

This paper focuses on the formation task control only and is neither going into the details of the formation protocols for coordinating and organizing the grouped robots to accomplish the formation task, nor collision avoidance. The environment is also assumed to be obstacle free. The problem to be investigated is formulated as follows: a group of n non-holonomic mobile robots are controlled to follow a group leader R₀, which moves along a desired trajectory, and to maintain a desired form implicitly defined by the relative distance and angle between each follower and its leader (in the SLSF scheme) or the relative distances between a follower and its two leaders, as well as the relative angle with one of those two leaders (in the TLSF scheme). The SLSF and TLSF schemes are shown in Fig. 2.

OPEN IN VIEWER

Figure 2.

(a) SLSF scheme in diamond formation, (b) SLSF scheme in zigzag formation, and (c) TLSF scheme in diamond formation of a four-robot team.

In Fig. 2, the robot R _i (i = 1, 2, 3) must keep a relative distance d _i and a relative bearing angle φ _i with its leader in SLSF scheme. Robot R₂ and R₃ can have many choices of its leader, e.g. the leader of R₂ is R₀ in Fig. 2(a) while its leader in Fig. 2(b) is R₁. In TLSF scheme, the follower R₂ is required to follow two leaders at a distance of d₂₀ and d₂₁, respectively, as well as a bearing angle φ₂ with one of them. By changing required relative distances and a relative bearing angles, it is easily to switch from diamond form to zigzag form and vice versa.

2.2. Robot Model

This section considers a system of n non-holonomic mobile robots labelled as R₀,…, R_n-1, where R₀ is the global leader. The problem of this study deals with wheeled mobile robots with two degrees of freedom and the dynamics of the ith robot are described by the unicycle model, as follows:

[ x . i y . i θ . i ] = [ cos θ i 0 sin θ i 0 0 1 ] [ v i ω i ]

(1)

where v _i is the linear velocity, ω _i is the angular velocity of the mobile robot, θ _i is the angle between the heading direction and the x axis, and (x _i ,y _i ) are the Cartesian coordinates of the centre of mass of the vehicle (see Fig. 3).

OPEN IN VIEWER

Figure 3.

SLSF scheme

2.3. Formation Control Framework for SLSF scheme

In SLSF configuration (shown in Fig. 3), R _i is leader robot and R _k is follower robot. Let d _ki denote the actual distance between R _i and R _k , φ _ki is the actual bearing angle from the orientation of R _k to the d _ki -axis (the axis connecting R _i and R _k ). The definition of the SLSF formation scheme requires that distance between R _i and R _k to be equal to d _k0 and the bearing angle from the orientation of R _k to the d _ki -axis is desired to be φ _k0 .

The difference between headings of two robots is defined as:

Δ θ k = θ k − θ i

(2)

Based on the configuration and these definitions, the dynamics of the system are expressed as:

d . k i = − v k cos φ k i + v i cos ( φ k i + Δ θ k )

(3)

φ . k i = − ω k + v k sin φ k i d k i − v i sin ( φ k i + Δ θ k ) d k i

(4)

Δ θ . k = ω k − ω i

(5)

where (5) can be derived easily from (2) by taking the derivative of both sides. Equation (3) is found from the observation that on d _ki -axis, the change of relative distance between two robots (d _ki ) is the difference in the velocities of R _i and R _k . To derive equation (4), note that the change of bearing angle (φ̇ _ki ) is caused by three components: the angular velocity of R _k , the relative motion of R _i to R _k , and the relative motion of R _k to R _i . The objective of the leader–follower control in SLSF scheme, therefore, is stated as following:

Control objective 1: Given v _i (t) and ω _i (t), find control v _k (t) and ω _k (t) for SLSF scheme such that

d k i → d k 0 , φ k i → φ k 0 , Δ θ k → 0 as t → ∞ , i = 0 , n − 1 ̄ , k = 1 , n ̄ , | φ k 0 | ≤ π 2 .

(6)

In order to solve this problem, a reference point M(x _km , y _km ) is chosen (see Fig. 3) at a distance d _k0 from the leader in the direction whose angular deviation with the orientation of the follower robot is φ _k0 . Hence, the coordinates of reference point can be calculated by:

[ x k m y k m ] = [ x i y i ] + [ − cos ( θ k + φ k 0 ) − sin ( θ k + φ k 0 ) ] d k 0

(7)

With the definition of this reference point, the desired control of the follower robot can be obtained by controlling the position of follower (x _k , y _k ) towards (x _km , y _km ). This means that the distance d _ki and the angle of orientation, φ _ki , can be simultaneously controlled so that the relative distance d _ki and relative bearing angle φ _ki approaches d _k0 and φ _k0 respectively.

3.1. Formation Control Framework for TLSF Scheme

The system of n mobile robots is the same as SLSF scheme and the robot R _j is required to keep a distance d _j0 from its leader R _i and its bearing angle at φ _j0 . However, the robot R _k in this scheme must simultaneously follow two leaders with two desired separations to R _i and R _j of d _k0 and l _k0 , respectively, besides the desired bearing angle φ _k0 from the orientation of R _k to the d _ki -axis.

As shown in Fig. 4, the three robots are currently located at A, B, and C, and the desired formation is the triangle ΔADE (d_j0 = AE, d_k0 = AD, l _k0 = DE). The triangle ΔAMN is found by rotating the triangle ΔADE an angle Ψ so that the side AE always lies on the d _ji -axis. By this definition, the robot R _j is controlled to reach E (N comes towards E) and the robot R _k is controlled to reach M (M will come towards D because of N). When R _j is at E, R _k is obviously at D, as is required. Therefore, TLSF helps both followers reach the desired points nearly at the same time, and the oscillations in the motion of R _k on the way to reach a stable state are restricted by the constraint with the second leader R _j . This is the major difference between the two schemes. In the SLSF scheme, the second follower can be only at a stable state after the first follower is stable. However, the TLSF scheme implicitly requires that the two leaders must be well configured and controlled, or the follower may encounter difficulties.

OPEN IN VIEWER

Figure 4.

TLSF scheme

The pair R _i - R _j is the SLSF scheme, then the remaining is finding the reference point for R _k . From two required separations d _k0 and l _k0 , and desired bearing angle φ _k0 , the instantaneous bearing angle φ _km can be calculated as follows:

φ k m = arccos ( x i − x m d k 0 ) − θ k

(8)

where x m = α 2 ± ( α 2 2 - 4 α 1 α 3 ) α 1 , with

α 0 = 1 2 [ ( x i 2 − x n 2 ) + ( y i 2 − y n 2 ) − d k 0 2 + l k 0 2 ]

α 1 = 2 ( Δ x 2 + Δ y 2 )

α 2 = 2 [ α 0 Δ x − Δ y ( y i Δ x − x i Δ y ) ]

α 3 = ( α 0 − y i Δ y ) 2 − ( x i Δ y ) 2 − ( d k 0 Δ y ) 2

Δ x = x i − x n

Δ y = y i − y n

x n = x i + d j 0 d j i ( x j − x i )

y n = y i + d j 0 d j i ( y j − y i )

To choose the unique value of φ _km , we need to compare directions of the two vectors AN→ and AM→, as shown in Fig. 4, which are defined as

{ A N → = ( ( x n − x i ) , ( y n − y i ) ) A M → = ( ( x m − x i ) , ( y m − y i ) )

These give a criterion of

α 4 = ( x m − x i ) ( y n − y i ) − ( x n − x i ) ( y m − y i )

(9)

When M is at the left-hand side of AN→, select x _m such that α₄ > 0, and vice versa.

The objective of the leader–follower control in TLSF scheme is re-stated from SLSF scheme as following:

Control objective 2: Given v _i (t) and ω _i (t), find control v _k (t) and ω _k (t) for TLSF scheme such that

d k i → d k 0 , φ k i → φ k m , Δ θ k → 0 as t → ∞ , i = 0 , n − 1 ̄ , k = 2 , n ̄ .

(10)

Compared with the SLSF scheme, the only difference is that the bearing angle in the SLSF scheme is a constant, but the bearing angle in the TLSF is not. This is an advantage in building the control rule because the control rule just needs to deal with the time-variant bearing angle φ _km then it can apply to both schemes, so that the bearing angle in the proposed control algorithm, which is presented in the next section, is time-varying.

3.2. Proposed Control Law

Fig. 5 is redrawn from Fig. 4 with some supporting information in order to find the following control: as t→∞, the follower robot's position (x _k (t),y _k (t)) can be controlled to reach the reference point (x _km (t),y _km (t)), i.e. the point C needs to approach the point M.

OPEN IN VIEWER

Figure 5.

TLSF scheme with detailed information

Now consider the d _k0 -axis. On this direction, the change of position from C to M is mirrored as the change from F to M and the component on the d _k0 -axis of the relative velocity between v _k and v _i will do this task. Therefore, the following is obtained:

v k cos ( φ k m ) − v i cos ( φ k m + Δ θ k ) = k 1 [ d k i cos ( φ k i − φ k m ) − d k 0 ]

(11)

where k₁ is a postitive tuning coefficient.

Next, in order to find ω _k , the a _k -axis should be considered, as shown in Fig. 5 to be perpendicular to v _k . Similar to the way of calculating v _k , the change of velocity on this axis (C→G) is caused by the rotation with angular velocity ω _k and v _i which gives:

( ω k + ω i ) d k i cos ( φ k m ) = k 1 [ d k i cos ( φ k i ) − d k 0 sin ( φ k m ) ] − v i sin ( Δ θ k )

(12)

One problem is that the above control rule and most other controllers in the literature require the measurement of the leader's speed v _i which is practically impossible using only onboard sensors particularly if the speed is supposed to be fed back into the robot's own speed regulation, as in equations (11) and (12). In addition, ω _i is also impossible to measure.

In (Gustavi, T. & Hu, X., 2008), the authors tried to estimate only v _i by mathematically transform the system model into a cascade system for easier analysis. However, they must assume that ω _i = 0 and v̇ _i = 0 to decouple the system for proving stability by Lyapunov theory. The aim of our method is to build a control law that can both compensate the change of v _i and ω _i , and drive the follower to the desired position, without knowing the exact value of v _i and ω _i , based on a kinematic analysis (Fig. 5). In order to do this, R _k should be considered in the coordinate of R _i , i.e. the coordinate (x _i , y _i ), instead of (x,y). In this coordinate, R _i is not moving and there is a velocity -v _i applied to R _k . Assuming that v _i at time t becomes v _i + Δv _i at time t + Δt which will put into the control of R _k a component Δv to adaptively compensate the change of v _i . This is the change of velocity in the time step, so it is also the instantaneous translational acceleration of leader robot, and can be approximated by:

v . a = k 2 sgn [ cos ( 2 θ i ) ] × [ d k i cos ( φ k i + Δ θ k ) − d k 0 cos ( φ k m + Δ θ k ) ]

(13)

where k₂ > 0 is another tuning coefficient, and v _a = v _i + Δv. Using the same technique as applied with the leader's velocity, that is, the change Δω of ω _km is also considered from time t to time t+Δt and Δω is then the angular acceleration of the reference point and can be approximated by:

ω . a = k x e x ( sin ( Δ θ k ) sin φ k m − cos ( Δ θ k ) ) d k 0 cos φ k m − k y e y ( cos ( Δ θ k + φ k m ) sin φ k m + sin ( Δ θ k ) ) d k 0 cos φ k m

(14)

where k _x and k _y are tuning coefficients. ω _a = ω _km + Δω, e _x = -d_k0 sin(Δθ _k + φ _km ) + d _ki sin(Δθ _k + φ _ki ), and e _y = d_k0 cos(Δθ _k + φ _km ) - d _ki cos(Δθ _k + φ _ki ).

In summary, the proposed leader-follower control is equation (15). As seen in equation (15), the control law requires values of d _ki , φ _ki , Δθ _k which are assumed to be available by measurements from the on-board sensors.

{ v . a = k 2 sgn [ cos ( 2 θ i ) ] ​ × [ d k i cos ( ϕ k i + Δ θ k ) - d k 0 cos ( ϕ k m + Δ θ k ) ] ω . a = k x e x ( sin ( Δ θ k ) sin ϕ k m - cos ( Δ θ k ) ) d k 0 cos ϕ k m ​ - k y e y ( cos ( Δ θ k + ϕ m ) sin ϕ k m + sin ( Δ θ k ) ) d k 0 cos ϕ k m ν k = k 1 d k i cos ( ϕ k i - ϕ k m ) - d k 0 cos ( ϕ k m ) + ν a cos ( ϕ k m + Δ θ k ) cos ( ϕ k m ) ω k = k 1 d k i cos ( ϕ k i ) - d k 0 sin ( ϕ m ) d k i cos ( ϕ k m ) - ν a sin ( Δ θ k ) d k i cos ( ϕ k m ) - ω a

(15)

The necessary information includes only distance and angles, and many kinds of sensor can be used to gather those data. In the SLSF scheme, φ _km = φ _k0 = const. and ω _a = 0, so it is not necessary to use the second equation of (15). Our approach is based on the approximation of accelerations, which helps to build a common control law for both SLSF and TLSF schemes. Therefore, it is generally different from the control law in (Gustavi, T. & Hu, X., 2008) which can be applied to an SLSF scheme only.

Theorem : Suppose that the motion of the leader robot R _i satisfies the following condition:

v _i (t) ⩾ v₀ > 0, v̇ _a (t) ∈ L₂[0, ∞), ω _i (t) ∈ L₂[0,∞), ω̇ _a (t) ∈ L₂[0,∞). Then, with the control (15), where we let

{ 0 < k x < k 1 2 − k 2 sgn [ cos ( 2 θ i ) ] < k y < k 1 2 − k 2 sgn [ cos ( 2 θ i ) ]

(16)

as t → ∞ we will have globally (x _k , y _k ) → (x _km , y _km ), i.e. d _ki → d _k0 and φ _ki → φ _km .

Proof: Let

[ x e y e ] = [ x k y k ] − [ x k m y k m ]

(17)

and define

[ e x e y ] = [ sin θ i − cos θ i cos θ i sin θ i ] [ x e y e ]

(18)

Equations (3), (4) and (5) can be rewritten as

[ e . x e . y ] = − k 1 [ e x e y ] + Δ v [ 0 1 ] − Δ ω d k 0 [ cos ( Δ θ k + φ k m ) sin ( Δ θ k + φ k m ) ]

(19)

Consider a Lyapunov function candidate as

V = ( e x + ω c k 1 ) 2 + ( e y − Δ v k 1 + ω s k 1 ) 2 + 1 k 1 2 ( ω s − Δ v ) 2

(20)

where ω _c = Δωd_k0 cos(Δθ _k + φ _km ), ω _s = Δωd_k0 sin[Δθ _k + φ _km ). Before examining this Lyapunov function, there are several matrices, and a number which needs to be defined:

γ = Δ θ k + φ k m

(21)

P = [ k x 0 0 k y ] = P T

(22)

s = sgn [ cos ( 2 θ i ) ]

(23)

G = [ s i n γ − c o s γ c o s ( Δ θ k ) s i n ( Δ θ k ) ]

(24)

⇒ G − 1 = 1 c o s ( φ k m ) [ s i n ( Δ θ k ) c o s γ − c o s ( Δ θ k ) s i n γ ]

(25)

Q = [ 0 0 0 − s ]

(26)

W = [ ω c ω s ]

(27)

The derivation of this Lyapunov function is long but not difficult, so only some of the key steps are shown here, in which the derivative of a scalar variable is denoted by a dot, while the derivative of a matrix valued function is denoted by a prime. The variable in scalar is in lower case, and the matrix is presented in capital letters (except V).

Taking derivative of V, we have

V . 2 = [ e x + ω c k 1 e y − Δ v k 1 + ω s k 1 ] ( [ e x e y − Δ v k 1 ] + [ ω c ω s ] ) ' + 1 k 1 2 ( − Δ v + W T ) ( − Δ v . + W ' )

(28)

By noting the following properties,

[ 0 1 ] Δ v . = k 2 Q [ e x e y ]

(29)

Q [ 0 1 ] Δ v k 1 = [ 0 − s ] Δ v k 1

(30)

Q [ ω c ω s ] = 0

(31)

W ' = [ ω c ω s ] ' = P [ e x e y ] = [ k x 0 0 k y ] [ e x e y ] ⇒ { ω . c = k x e x ω . s = k y e y

(32)

1 k 1 2 ( − Δ v + W T ) ( − Δ v . + W ' ) = [ e x e y ] ( − k 2 Q k 1 + P k 1 ) × ( 1 k 1 [ ω c ω s ] − [ 0 1 ] Δ v k 1 )

(33)

it is easily to derive

V . 2 = − ( k 1 − k x k 1 ) ( e x + ω c k 1 ) 2 − ( k 1 − s k 2 k 1 − k y k 1 ) ( e y − Δ v k 1 + ω s k 1 ) 2 − k x k 1 3 ω c 2 − 1 k 1 2 ( s k 2 k 1 + k y k 1 ) ( ω s − Δ v ) 2

(34)

It is obvious that if the condition (16) is satisfied, v̇ ⩽ 0. This implies that e _x and e _y are exponentially convergent to zero. ▪

4.1. First Simulation

A simulation with control law (15) is performed to form a line formation of the three robots, where robot R₀ is the global leader. At the beginning the three robots may be placed anywhere in the field. However for this simulation, we will specify the initial states of the three robots as [x₀, y₀, θ₀] = [0, 0, 0], [x ₁ , y ₁ , θ ₁ ] = [−1.5, −2.6, 0], and [x₂, y₂, θ₂] = [−3, −5.2, 0]. The desired distance between the follower R₁ and R₀ is 3m and the desired bearing angle is π/6. The follower R₂ is required to keep a distance of 3m and 6m to R₁ and R₀ respectively, and a bearing angle of π/6 with R₀. Another simulation using control law [Ref] is also conducted with a similar configuration and the only difference is that R₂ is not required to keep a distance of 6m with R₀.

Fig. 6 shows the performance of the two control laws in the global x-y coordinates. There is only a small difference in the trajectory of robot R₂ can be seen from Figs. 6(a) and 6(b). It is not apparent that R₂ tracks the leaders better (quicker convergence) when using control law (15) than when using control law [Ref]. Figs. 7, 8 and 9 will show this more clearly. As seen from Figs. 7(a), 8(a) and 9(a), the robots' performance based on the new control law (15) is not significantly different from performance based on control law [Ref] because this is the SLSF scheme in both cases.

OPEN IN VIEWER

Figure 6.

Performance of (a) control law [Ref] and (b) control law (15).

OPEN IN VIEWER

Figure 7.

Trajectory seen from leader robot R₀ of (a) R₁ and (b) R₂.

OPEN IN VIEWER

Figure 8.

Relative distance over time between (a) R₁ and R₀ and (b) R₂ and R₀.

OPEN IN VIEWER

Figure 9.

Relative bearing angle over time between (a) R₁ and R₀, and (b) R₂ and R₀.

However, there is an advantage of the new control law (15) in the TLSF scheme, as seen in Figs. 7(b), 8(b) and 9(b). In Fig. 7(b), the robot R₂ quickly approaches the desired position with the trajectory being nearly a straight line due to the existence of the constraint about the distance with R₁, while robot R₂ using control [Ref] moves in a long curve before reaching the desired position. In Fig. 8(b), the error of R₂ in the SLSF seems to be cumulative with the error of R₁, so the farthest distance that it has with R₀ is about double. Meanwhile the TLSF scheme causes R₂ to have a similar behavior as R₁, and to converge to a stable state nearly at the same time as R₁. These are two advantages of the new controller (15) within the TLSF scheme. Fig. 9(b) shows the advantage of the approximation of the angular velocity. In the SLSF scheme, there is no constraint for the bearing angle, so oscillations easily occur.

By approximating the angular acceleration of the reference point in the new control law (15) and choosing appropriate parameters, the oscillations and damping can be reduced.

4.2. Second Simulation

The second simulation demonstrates that it is possible and beneficial to apply the algorithm to a large team of robots. The number of robots is now 5 and the trajectory of the leader is still sinusoidal and the robots form a line with the distance between each adjacent robot being 3m, and the bearing angle is π/6. The performance of the controller [Ref] and the new controller (15) are compared in Fig. 10.

OPEN IN VIEWER

Figure 10.

Performance of a team of 5 robots using (a) the control law [Ref] and (b) the control law (15) in the TLSF scheme.

From Fig. 10 and the displacement error values presented in Tables 1 and Table 2, it can be seen that the controller (15) has better performance, especially for robots which are at greater distance from the global leader. The convergence rate is faster and the transient errors are smaller. The mean and standard deviation of both distance and angular errors of the second, third and fourth follower robots using controller (15) are much less than when using controller [Ref]. Moreover, those values do not show much difference nor are they incremental as when using controller [Ref]. This means that the TLSF scheme simply keeps the errors away from cumulation.

OPEN IN VIEWER

Table 1.

Displacement errors of control law [Ref]

Follower no.	mean Λ/dk0	std Δ/dk0	mean Δφ km	std Δφ km
1	0.6539	2.2384	−0.0982	0.2514
2	0.6595	2.3966	−0.0624	0.2653
3	0.6790	2.5985	0.0429	0.5103
4	0.7543	2.8104	0.1909	0.9620

OPEN IN VIEWER

Table 2.

Displacement errors of control law (15)

Follower no.	mean Δd/dk0	std Δd/dk0	mean Δφ km	std Δφ km
1	0.6536	2.2384	−0.0982	0.2514
2	0.3229	1.1045	−0.0798	0.2139
3	0.3203	1.1910	−0.0804	0.2318
4	0.3212	1.1895	−0.0786	0.2216

4.3. Third Simulation

In above two simulations, the formation-maintaining task is shown. In third simulation, the forming task is performed. A four-robot team will change from a triangular form to a line form (Fig. 11). At first, the three robots are keeping the triangular form (form A), where R₀ is the global leader. The initial poses of R₀, R₁, and R₂ are (0, 0, 0), (−2, 2, 0), and (−2, −2, 0), respectively. The team is required to change to a line form (form B) where R₁ follows R₀ with a distance of 3m and bearing angle π/4; R₂ follows R₀ with bearing angle of π/4 and distance of 6m, and simultaneously keeps a distance 3m with R₁. The results are depicted in Fig. 11. It is shown that the formation is switched well and quickly. The trajectory of R2 when using controller (15) is not as smooth as when using controller [Ref], but the overall error is smaller. All the advantages of TLSF scheme and controller (15) stated in section 4.2 above are also shown in this simulation (see Table 3 and Table 4).

OPEN IN VIEWER

Figure 11.

Performance of a team of 3 robots in switching from a triangular formation to a line formation using (a) control law [Ref] and (b) control law (15)

OPEN IN VIEWER

Table 3.

Displacement errors of control law [Ref] in forming task

Follower no.	mean Δd/dk0	std Δd/dk0	mean Δφ km	std Δφ km
1	2.4222	4.9308	−0.2771	0.3772
2	2.4602	5.2911	−0.1560	0.4420

OPEN IN VIEWER

Table 4.

Displacement errors of control law (15) in forming task

Follower no.	mean Δd/dk0	std Δd/dk0	mean Δφ km	std Δφ km
1	2.1946	4.6062	−0.2610	0.3709
2	1.3069	2.4982	−0.2038	0.3831