# Stability Analysis and Variational Integrator for Real-Time Formation Based on Potential Field

Authors: Shengqing Yang; Jianqiao Yu
Journal: Mathematical Problems in Engineering
 (2014)

---

## Abstract

This paper investigates a framework of real-time formation of autonomous vehicles by using potential field and variational integrator. Real-time formation requires vehicles to have coordinated motion and efficient computation. Interactions described by potential field can meet the former requirement which results in a nonlinear system. Stability analysis of such nonlinear system is difficult. Our methodology of stability analysis is discussed in error dynamic system. Transformation of coordinates from inertial frame to body frame can help the stability analysis focus on the structure instead of particular coordinates. Then, the Jacobian of reduced system can be calculated. It can be proved that the formation is stable at the equilibrium point of error dynamic system with the effect of damping force. For consideration of calculation, variational integrator is introduced. It is equivalent to solving algebraic equations. Forced Euler-Lagrange equation in discrete expression is used to construct a forced variational integrator for vehicles in potential field and obstacle environment. By applying forced variational integrator on computation of vehicles' motion, real-time formation of vehicles in obstacle environment can be implemented. Algorithm based on forced variational integrator is designed for a leader-follower formation.


---

## Body

## 1. Introduction

Formation of autonomous vehicles has been a hot topic with applications such as formation of UAVs (unmanned aerial vehicles), UUVs (unmanned under-water vehicles), and space satellites [1]. Research of formation is focused on structure, stability, intelligent controls, and so forth. Formation is a mixture of the following tasks: (a) formation task of moving from start to goal; (b) maintaining relative positions and orientations; (c) obstacle avoidance; (d) splitting of formation [2]. Real-time formation based on above concepts requires a reasonable framework and efficient numerical methods. Framework of formation describes the basic mechanisms ensuring coordinated motion of vehicles. It usually results in a nonlinear system. Stability analysis of such nonlinear system is difficult because the stability is mainly about the group structure rather than coordinates of positions. Moreover, integration of such system plays an important role in real-time formation. Normal integration methods such as Runge-Kutta method have drawbacks in conversation of integration, while variational integrator has good performance on this. In this paper, specific potential functions are chosen to form a Lagrangian from which variational integrator is derived for integration.Different frameworks of formation have been discussed in [3–15]. The essence is to find a reasonable control law to make vehicles have coordinated motion. An apparent way is based on the geometric relationship of vehicles as the work of Anderson et al. [3–5]. Formation of three vehicles and formation based on triangular structures are considered by giving control laws according to vehicles' geometric relationships in plane [3, 4, 6]. Such control laws are usually hard to be unified. Adaptive control is studied for obstacle avoidance by taking the effect of obstacle into consideration [7]. All these methods in [3–7] can be seen as formation's framework on vehicles' dynamic. Motion can be integrated based on dynamic systems mentioned above. However, it is short in computation because normal integration methods have poor performances in conservation of computation. Computation based on conserved quantity (such as Lagrangian of a free system) is needed. Methods based on potential field are introduced which can be seen as formation's framework on vehicles' kinematic. Cooperative control using adaptive gradient climbing in artificial potential field achieves a mobile network as the work of Ögren et al. [10]. Potential field is also used for formation preserving in [8, 11, 14] and flocking in [12, 13], indicating that different potential fields result in different formations [15]. Cortés introduced the notion of a stress function for formation control from which we derive a Lagrangian [8]. However, the stability of such formation is not given. In this paper, stability is discussed in error dynamic system. Transformation of coordinates from inertial frame to specific body frame is introduced, because the stability of formation is related to structure rather than particular coordinates. Based on the error dynamic in body frame, analysis of stability is achieved by applying center manifold theorem on reduced dynamic system.For consideration of a real-time control, numerical methods for high precision algorithms have been discussed in [16–23]. Variational integrator is based on applying discrete variational principle on discrete Lagrangian. The result is equivalent to discrete Euler-Lagrange equation. Forced, constrained, nonsmooth, and nonholonomic forms of variational integrators are introduced for complex systems [16–18, 21, 24]. Junge et al. introduced discrete mechanic and optimal control (DMOC) based on variational integrator in [19, 22], where an optimization problem is stated by writing integrators at all discrete segments together. The works of Junge et al. for optimal reconfiguration of flying spacecrafts and Kobilarov for discrete geometric motion control of autonomous vehicles have shown the computational advantages of the variational integrator method [20, 23]. For its good performance in calculation and convenience in applying potential filed, variational integrator is chosen for calculation of real-time formation. In our work, the variational integrator is used for integration of vehicles' motion for every time stepIn this paper, potential field with stress function is considered to describe the interactions of vehicles, which partly forms the Lagrangian of vehicles in formation. Stability of formation in such potential field is discussed in error dynamic system which is usually used in tracking problems [25]. Transformation of coordinates from inertial frame to specific body frame is used to help simplify the model of dynamic. Reduced system is introduced for analysis of stability. Later, discrete Lagrange-d'Alembert principle is applied on discrete Lagrangian to form the forced variational integrator. Different from optimization in DMOC, we use the forced variational integrator to make integration for every time step. Formation in leader-follower structure is considered and modified obstacle force is used to make sure of avoidance. The rest of this paper is organized as follows. In Section 2, formation problem and potential field are introduced and stability analysis is addressed. In Section 3, variational integrator based on potential field is introduced and numerical examples are given. The comments on nonholonomic vehicles are also addressed in Section 3. The conclusion is drawn in the last section.


## 2. Formation and Potential Field

Potential functions are used to describe interactions in formation, resulting in a Lagrangian of formation. The stability of formation in such potential field is discussed in error dynamic system. With the methodology introduced for stability analysis, the equilibrium point is proved stable for the error dynamic system. Modified obstacle force is introduced for obstacle avoidance which is a part of forced variational integrator.


### 2.1. Formation of Vehicles

Consider a formation ofn homological vehicles. The dynamic of the ith vehicle is
(1)
q
˙
i
=
p
i
m
i
,
p
˙
i
=
u
i
tran
⁡
,
θ
˙
i
=
I
i
J
i
,
I
˙
i
=
u
i
rota
.
Here m
i is mass and J
i is moment of inertia of the ith vehicle; q
i is the position and θ
i is the orientation; p
i and I
i are linear momentum and angular momentum; u
i
tran
⁡ and u
i
rota are the controls, respectively.The configuration space ofn vehicles is Q
=
R
2
n
×
S
1
×
·
·
·
×
S
1
︸
n, with Lagrangian of the ith vehicle L
i
:
T
Q
→
R,
(2)
L
i
(
q
,
θ
,
q
˙
,
θ
˙
)
=
1
2
q
˙
i
T
M
i
q
˙
i
+
1
2
θ
˙
i
T
J
i
θ
˙
i
-
V
i
(
q
)
-
V
-
i
(
θ
)
.
Here M
i is mass matrix of the ith vehicle and V
i
(
q
) and V
-
i
(
θ
) are potential functions.In the rest of this paper, we writeq
=
(
q
1
,
q
2
,
…
,
q
n
) and θ
=
(
θ
1
,
θ
2
,
…
,
θ
n
). Denote e
i
j
=
(
q
i
-
q
j
) and ω
i
j
=
(
θ
i
-
θ
j
). Formation preserving requires ∥
e
i
j
∥
→
l
i
j, ∥
ω
i
j
∥
→
0, ∥
e
˙
i
j
∥
→
0 and ∥
ω
˙
i
j
∥
→
0 where l
i
j is a desired distance between the ith and jth vehicles.


### 2.2. Potential Field

The potential field with stress function has been mentioned in [8]. The equivalent force in such potential field can be proved to have the same dynamic with spring. Also, we can introduce a damping force and corresponding potential function for stabilization.Definition 1.
Potential function of position of theith vehicle in formation is defined as
(3)
V
i
(
q
)
=
∑
j
=
1
n
V
~
(
q
i
,
q
j
)
=
-
1
2
∑
j
=
1
n
k
i
j
(
∥
q
i
-
q
j
∥
-
l
i
j
)
2
,
with which V
~
(
q
i
,
q
j
)
=
-
(
1
/
2
)
k
i
j
(
∥
q
i
-
q
j
∥
-
l
i
j
)
2 and
(4)
k
i
j
=
{
K
(
i
,
j
)
,
j
≠
i
,
0
,
j
=
i
.
Here K is a symmetric matrix whose element K
(
i
,
j
) is a constant or function and K
(
i
,
j
)
>
0. The potential function V
~ is symmetric, which means that V
~
(
q
i
,
q
j
)
=
V
~
(
q
j
,
q
i
).Proposition 2.
Potential force of theith vehicle is
(5)
f
i
P
(
q
)
=
-
∑
j
=
1
n
k
i
j
(
∥
q
i
(
t
)
-
q
j
(
t
)
∥
-
l
i
j
)
(
q
i
(
t
)
-
q
j
(
t
)
)
∥
q
i
(
t
)
-
q
j
(
t
)
∥
.Proof of Proposition2.
Assume that the Lagrangian of theith vehicle in formation is L
i
(
q
,
q
˙
)
=
(
1
/
2
)
q
˙
i
T
M
i
q
˙
i
-
V
i
(
q
). According to the Euler-Lagrange equation, a free system without external force satisfied f
i
=
M
i
q
¨
i
=
∂
V
i
(
q
)
/
∂
q
i, which means
(6)
f
i
P
(
q
)
=
∂
V
i
(
q
)
∂
q
i
=
-
∑
j
=
1
n
k
i
j
(
∥
q
i
-
q
j
∥
-
l
i
j
)
∂
(
∥
q
i
-
q
j
∥
)
∂
q
i
.
Consider the dot product of vectors. It holdsq
2
=
q
·
q
=
q
T
q
=
∑
q
i
2
=
(
∥
q
i
∥
)
2, and hence (
∥
q
i
-
q
j
∥
)
2
=
(
q
i
-
q
j
)
2. Naturally it holds that ∂
(
∥
q
i
-
q
j
∥
)
2
/
∂
q
i
=
∂
(
q
i
-
q
j
)
2
/
∂
q
i, with which satisfying
(7)
∂
(
∥
q
i
-
q
j
∥
)
2
∂
q
i
=
2
(
∥
q
i
-
q
j
∥
)
∂
(
∥
q
i
-
q
j
∥
)
∂
q
i
,
∂
(
q
i
-
q
j
)
2
∂
q
i
=
2
(
q
i
-
q
j
)
.
Therefore, it holds ∂
(
∥
q
i
-
q
j
∥
)
/
∂
q
i
=
(
q
i
-
q
j
)
/
∥
q
i
-
q
j
∥. Thus, potential force on the ith vehicles is
(8)
f
i
P
(
q
)
=
-
∑
j
=
1
n
k
i
j
(
∥
q
i
-
q
j
∥
-
l
i
j
)
(
q
i
-
q
j
)
∥
q
i
-
q
j
∥
.
Potential force between theith and jth vehicles is
(9)
f
i
j
P
(
q
)
=
-
k
i
j
(
∥
q
i
-
q
j
∥
-
l
i
j
)
(
q
i
-
q
j
)
∥
q
i
-
q
j
∥
.Definition 3.
Potential function of orientation of theith vehicle is defined as
(10)
V
-
i
(
θ
)
=
∑
j
=
1
n
V
^
(
θ
i
,
θ
j
)
=
-
1
2
∑
j
=
1
n
k
^
i
j
(
θ
i
-
θ
j
)
2
,
with which V
^
(
θ
i
,
θ
j
)
=
-
(
1
/
2
)
k
^
i
j
(
θ
i
-
θ
j
)
2 and
(11)
k
^
i
j
=
{
K
^
(
i
,
j
)
,
j
≠
i
,
0
,
j
=
i
.
Here K
^ is a symmetric matrix whose element K
^
(
i
,
j
) is a constant or function and K
^
(
i
,
j
)
>
0. Respectively, the potential force of orientation of the ith vehicle is
(12)
f
-
i
P
(
θ
)
=
-
∑
j
=
1
n
k
^
i
j
(
θ
i
(
t
)
-
θ
j
(
t
)
)
.
System that has dynamic x
¨
=
-
k
(
x
-
x
*
) will act as a spring. With potential functions {
(
V
i
(
q
)
,
V
-
i
(
θ
)
)
∣
i
=
1,2
,
…
,
n
} defined above, vehicles can have coordinated motion as if there were springs connecting them. Since the spring system is periodically stable, damping force x
¨
=
-
v
x
˙ is considered for stabilization.Definition 4.
Damping forces of theith vehicle are
(13)
f
i
D
(
q
˙
)
=
∑
j
=
1
n
f
i
j
D
=
-
∑
j
=
1
n
v
i
j
(
q
˙
i
(
t
)
-
q
˙
j
(
t
)
)
,
f
-
i
D
(
θ
˙
)
=
∑
j
=
1
n
f
-
i
j
D
=
-
∑
j
=
1
n
v
^
i
j
(
θ
˙
i
(
t
)
-
θ
˙
j
(
t
)
)
,
with f
i
j
D
=
-
v
i
j
(
q
˙
i
-
q
˙
j
)
,
v
i
j
≥
0 and f
-
i
j
D
=
-
v
^
i
j
(
θ
˙
i
-
θ
˙
j
)
,
v
^
i
j
≥
0.Same as the potential function of stress function, the damping force has corresponding potential functions{
(
V
i
D
(
q
˙
)
,
V
-
i
D
(
θ
˙
)
)
∣
i
=
1,2
,
…
,
n
}, which are
(14)
V
i
D
(
q
˙
)
=
∑
j
=
1
n
V
~
(
q
˙
i
,
q
˙
j
)
=
-
1
2
∑
j
=
1
n
v
i
j
(
q
˙
i
-
q
˙
j
)
2
,
V
-
i
D
(
θ
˙
)
=
∑
j
=
1
n
V
^
(
θ
˙
i
,
θ
˙
j
)
=
-
1
2
∑
j
=
1
n
v
^
i
j
(
θ
˙
i
-
θ
˙
j
)
2
.


### 2.3. Stability of Formation in Potential Field

Stability of formation in potential field is proved by using notion of error dynamic system. Define the error variablee
i
j
(
t
)
=
q
i
(
t
)
-
q
j
(
t
). e
i
j is the relative position and ∥
e
i
j
∥ is the relative distance between the ith and jth vehicles. It holds e
i
j
(
t
)
=
-
e
j
i
(
t
), e
i
i
=
0 and e
˙
i
j
(
t
)
=
q
˙
i
(
t
)
-
q
˙
j
(
t
). However, the calculation of the Jacobian is unsolvable, because the stability of formation is related to the structure of formation rather than the particular coordinates. Transformation of coordinates from inertial frame to body frame is considered. Reduced system is derived by specific substitution. According to the center manifold theorem of nonlinear dynamic, stability of original system is guaranteed if the reduced system is stable.Firstly, we consider the dynamic system for vehicles’ positions. Denote the control force byf
i
(
t
)
=
f
i
P
(
q
(
t
)
)
+
f
i
D
(
q
˙
(
t
)
). According to Newton's law of motion, q
i
(
t
) can be given by integration as follows:
(15)
q
i
(
t
)
=
q
i
(
0
)
+
∫
0
t
q
˙
i
(
τ
)
d
τ
=
q
i
(
0
)
+
∫
0
t
{
q
˙
i
(
0
)
+
1
m
∫
0
τ
f
i
(
s
)
d
s
}
d
τ
=
q
i
(
0
)
+
q
˙
i
(
0
)
t
+
1
m
∫
0
t
∫
0
τ
f
i
(
s
)
d
s
d
τ
,
where q
i
(
0
) and q
˙
i
(
0
) are constants.Hence, it holds(16)
q
˙
i
(
t
)
=
q
˙
i
(
0
)
+
1
m
∫
0
t
f
i
(
s
)
d
s
,
q
¨
i
(
t
)
=
1
m
f
i
(
t
)
-
1
m
f
i
(
0
)
.
Boundary condition vanishes as f
i
(
0
)
=
0. Second derivation of e
i
j
(
t
) satisfies
(17)
e
¨
i
j
(
t
)
=
q
¨
i
(
t
)
-
q
¨
j
(
t
)
=
1
m
(
f
i
P
(
q
(
t
)
)
+
f
i
D
(
q
˙
(
t
)
)
)
-
1
m
(
f
j
P
(
q
(
t
)
)
+
f
j
D
(
q
˙
(
t
)
)
)
=
-
1
m
(
∑
s
=
1
n
k
i
s
(
∥
q
i
(
t
)
-
q
s
(
t
)
∥
-
l
i
s
)
(
q
i
(
t
)
-
q
s
(
t
)
)
∥
q
i
(
t
)
-
q
s
(
t
)
∥
+
∑
s
=
1
n
v
i
s
(
q
˙
i
-
q
˙
s
)
)
+
1
m
(
∑
s
=
1
n
k
j
s
(
∥
q
j
(
t
)
-
q
s
(
t
)
∥
-
l
j
s
)
(
q
j
(
t
)
-
q
s
(
t
)
)
∥
q
j
(
t
)
-
q
s
(
t
)
∥
+
∑
s
=
1
n
v
j
s
(
q
˙
j
-
q
˙
s
)
)
=
1
m
(
∑
s
=
1
n
k
i
s
(
l
i
s
∥
e
i
s
(
t
)
∥
-
1
)
e
i
s
(
t
)
-
∑
s
=
1
n
k
j
s
(
l
j
s
∥
e
j
s
(
t
)
∥
-
1
)
e
j
s
(
t
)
)
-
1
m
(
∑
s
=
1
n
v
i
s
e
˙
i
s
(
t
)
-
∑
s
=
1
n
v
j
s
e
˙
j
s
(
t
)
)
.We consider a leader-follower formation of three homogeneous vehicles, because it is a basic element of formation [3, 9]. Suppose vehicles have the same mass m and coefficients k
,
v. In the leader-follower formation, vehicle 1 is usually selected to be the leader. It does not have interactions from vehicle 2 and vehicle 3. Hence, the error dynamic has the following form:(18)
[
e
¨
12
e
¨
13
e
¨
23
e
˙
12
e
˙
13
e
˙
23
]
=
[
-
v
m
0
v
m
k
m
(
l
12
∥
e
12
∥
-
1
)
0
-
k
m
(
l
23
∥
e
23
∥
-
1
)
0
-
v
m
-
v
m
0
k
m
(
l
13
∥
e
13
∥
-
1
)
k
m
(
l
23
∥
e
23
∥
-
1
)
v
m
-
v
m
-
2
v
m
-
k
m
(
l
12
∥
e
12
∥
-
1
)
k
m
(
l
13
∥
e
13
∥
-
1
)
2
k
m
(
l
23
∥
e
23
∥
-
1
)
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
]
[
e
˙
12
e
˙
13
e
˙
23
e
12
e
13
e
23
]
.By denoting X
=
(
e
˙
12
,
e
˙
13
,
e
˙
23
,
e
12
,
e
13
,
e
23
)
T, (18) can be written in form of X
˙
=
f
(
X
).Secondly, we consider the equilibrium point and the stability of the above system. Sincee
˙
i
j and e
i
j are vectors, we need to define a projection function g
(
X
)
:
R
m
×
·
·
·
×
R
m
︸
n
/
{
0
}
→
R
n,
(19)
g
(
X
)
=
(
∥
X
1
∥
,
∥
X
2
∥
,
…
,
∥
X
n
∥
)
T
,
X
=
(
X
1
,
X
2
,
…
,
X
n
)
T
,
X
i
∈
R
m
.According to the work of Laplace and Lagrange, if a system is conservative, then a state corresponding to zero kinetic energy and minimum potential energy is a stable equilibrium point [26]. In our system, we have an adjoint equilibrium point Y
*
=
g
(
X
*
)
=
(
0,0
,
0
,
l
12
,
l
13
,
l
23
)
T for dynamic system X
˙
=
f
(
X
). Solutions X
* stand for stable structures.Corollary 5.
Y
*
=
(
0,0
,
0
,
l
12
,
l
13
,
l
23
)
T is an adjoint equilibrium point of system X
˙
=
f
(
X
).Proof of Corollary5.
IfX
*
=
(
e
˙
12
*
,
e
˙
13
*
,
e
˙
23
*
,
e
12
*
,
e
13
*
,
e
23
*
)
T satisfies Y
*
=
g
(
X
*
)
=
(
0,0
,
0
,
l
12
,
l
13
,
l
23
)
T, it holds (
∥
e
˙
12
*
∥
,
∥
e
˙
13
*
∥
,
∥
e
˙
23
*
∥
,
∥
e
12
*
∥
,
∥
e
13
*
∥
,
∥
e
23
*
∥
)
T
=
(
0,0
,
0
,
l
12
,
l
13
,
l
23
)
T and X
˙
*
=
f
(
X
*
)
=
(
e
¨
12
*
,
e
¨
13
*
,
e
¨
23
*
,
e
˙
12
*
,
e
˙
13
*
,
e
˙
23
*
,
)
T
=
0. Hence Y
*
=
(
0,0
,
0
,
l
12
,
l
13
,
l
23
)
T is an adjoint equilibrium point for dynamic system X
˙
=
f
(
X
).Consider the Jacobian for stable structures. Denotee
i
j
=
(
x
i
j
,
y
i
j
)
T, while e
˙
i
j
=
(
x
˙
i
j
,
y
˙
i
j
)
T, e
¨
i
j
=
(
x
¨
i
j
,
y
¨
i
j
)
T. Compute the Jacobian matrix of f
(
x
) with the individual components:
(20)
∂
f
1
∂
X
1
=
∂
(
[
x
¨
12
,
y
¨
12
]
)
∂
(
[
x
˙
12
,
y
˙
12
]
)
=
[
-
v
m
0
0
-
v
m
]
,
∂
f
1
∂
X
2
=
∂
(
[
x
¨
12
,
y
¨
12
]
)
∂
(
[
x
˙
13
,
y
˙
13
]
)
=
0
2
×
2
,
∂
f
1
∂
X
3
=
∂
(
[
x
¨
12
,
y
¨
12
]
)
∂
(
[
x
˙
23
,
y
˙
23
]
)
=
[
v
m
0
0
v
m
]
,
∂
f
1
∂
X
4
=
∂
(
[
x
¨
12
,
y
¨
12
]
)
∂
(
[
x
12
,
y
12
]
)
=
[
k
m
(
l
12
y
12
2
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
1
)
-
k
m
l
12
x
12
y
12
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
k
m
l
12
x
12
y
12
(
x
12
2
+
y
12
2
)
(
3
/
2
)
k
m
(
l
12
x
12
2
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
1
)
]
,
∂
f
1
∂
X
5
=
∂
(
[
x
¨
12
,
y
¨
12
]
)
∂
(
[
x
13
,
y
13
]
)
=
0
2
×
2
,
∂
f
1
∂
X
6

=
∂
(
[
x
¨
12
,
y
¨
12
]
)
∂
(
[
x
23
,
y
23
]
)

=

[
-
k
m
(
l
23
y
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
k
m
(
l
23
x
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
]
.Use the same way to calculate the remaining parts. We can have the Jacobian matrix in form of(21)
D
f
(
x
)
=
[
A
B
I
6
×
6
0
]
,
where
(22)
Α
=
[
-
v
m
0
0
0
v
m
0
0
-
v
m
0
0
0
v
m
0
0
-
v
m
0
-
v
m
0
0
0
0
-
v
m
0
-
v
m
v
m
0
-
v
m
0
-
2
v
m
0
0
v
m
0
-
v
m
0
-
2
v
m
]
,
(23)
Β
=
[
k
m
(
l
12
y
12
2
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
1
)
-
k
m
l
12
x
12
y
12
(
x
12
2
+
y
12
2
)
(
3
/
2
)
0
0
-
k
m
(
l
23
y
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
k
m
l
12
x
12
y
12
(
x
12
2
+
y
12
2
)
(
3
/
2
)
k
m
(
l
12
x
12
2
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
1
)
0
0
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
k
m
(
l
23
x
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
0
0
k
m
(
l
13
y
13
2
(
x
13
2
+
y
13
2
)
(
3
/
2
)
-
1
)
-
k
m
l
13
x
13
y
13
(
x
13
2
+
y
13
2
)
(
3
/
2
)
k
m
(
l
23
y
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
-
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
0
0
-
k
m
l
13
x
13
y
13
(
x
13
2
+
y
13
2
)
(
3
/
2
)
k
m
(
l
13
x
13
2
(
x
13
2
+
y
13
2
)
(
3
/
2
)
-
1
)
-
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
k
m
(
l
23
x
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
-
k
m
(
l
12
y
12
2
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
1
)
k
m
l
12
x
12
y
12
(
x
12
2
+
y
12
2
)
(
3
/
2
)
k
m
(
l
13
y
13
2
(
x
13
2
+
y
13
2
)
(
3
/
2
)
-
1
)
-
k
m
l
13
x
13
y
13
(
x
13
2
+
y
13
2
)
(
3
/
2
)
2
k
m
(
l
23
y
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
-
2
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
k
m
l
12
x
12
y
12
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
k
m
(
l
12
x
12
2
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
1
)
-
k
m
l
13
x
13
y
13
(
x
13
2
+
y
13
2
)
(
3
/
2
)
k
m
(
l
13
x
13
2
(
x
13
2
+
y
13
2
)
(
3
/
2
)
-
1
)
-
2
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
2
k
m
(
l
23
x
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
]
.
Since the adjoint equilibrium point is Y
*
=
(
0,0
,
0
,
l
12
,
l
13
,
l
23
)
T, we only know (
x
i
j
2
+
y
i
j
2
)
=
l
i
j
2 instead of particular x
i
j and y
i
j. As a result, we cannot analyze the stability in inertial frame. In reality, the structure of formation is the basis of stability instead of particular coordinates of vehicles. In other words, there are infinite possible pairs of vehicles’ coordinates with which formation is stable while the number of corresponding stable structure is limited.Hence, we need to introduce a body frame and discuss the stability in the body frame. As shown in Figure1, the body frame is constructed by setting the vehicle 1 on the origin and vehicle 2 on the negative x-axis. θ is the angle from negative x-axis to the connecting line between vehicle 1 and vehicle 3. The right of Figure 1 shows the stable structures of formation in body frame.Figure 1
The left illuminates the construction of body frame where theV
i stands for ith vehicle. The right is an example of stable structures of formation in body frame. (
V
1
*
;
V
2
*
;
V
3
*
) is the combination of coordinates of stable structure.Definition 6.
General transformation of coordinates from inertial frame to body frame needs translation and rotation as follows:(24)
Translation
:
{
x
i
′
=
x
i
-
x
1
y
i
′
=
y
i
-
y
1
,
Rotation
:
[
x
i
′′
y
i
′′
]
=
[
cos
⁡
θ
sin
θ
-
sin
θ
cos
⁡
θ
]
[
x
i
′
y
i
′
]
.In order to achieve the transformation from the inertial frame to required body frame, the new body coordinates can be represented by inertial coordinates as(25)
x
1
′′
=
0
,
y
1
′′
=
0
,
x
2
′′
=
-
(
x
2
-
x
1
)
2
+
(
y
2
-
y
1
)
2
,
y
2
′′
=
0
,
x
3
′′
=
(
x
3
-
x
1
)
cos
⁡
θ
+
(
y
3
-
y
1
)
sin
θ
,
y
3
′′
=
-
(
x
3
-
x
1
)
sin
θ
+
(
y
3
-
y
1
)
cos
⁡
θ
.
Since x
1
′′
,
y
1
′′, and y
2
′′ vanish to zero, it has stable structure (
0,0
,
-
l
12
,
0
,
(
x
3
′′
)
*
,
(
y
3
′′
)
*
)
T. The coordinates of x
3
′′ and y
3
′′ satisfy
(26)
(
(
x
3
′′
)
*
-
0
)
2
+
(
(
y
3
′′
)
*
-
0
)
2
=
l
13
2
,
(
(
x
3
′′
)
*
-
(
-
l
12
)
)
2
+
(
(
y
3
′′
)
*
-
0
)
2
=
l
23
2
.
Its solutions are
(27)
(
x
3
′′
)
*
=
-
(
l
12
2
+
l
13
2
-
l
23
2
)
2
l
12
,
(
y
3
′′
)
*
=
±
(
l
12
+
l
13
-
l
23
)
(
l
12
-
l
13
+
l
23
)
(
l
13
-
l
12
+
l
23
)
(
l
12
+
l
13
+
l
23
)
2
l
12
.
Without losing generality, if we let l
12
=
l
13
=
l
23
=
l, we can obtain
(28)
(
x
3
′′
)
*
=
-
l
2
,
(
y
3
′′
)
*
=
±
3
2
l
.In this paper, stable structure means vehicles form a desired formation structure with the stress function that is equal to zero. For consideration of illumination, stable structure(
0,0
,
-
l
,
0
,
-
l
/
2
,
(
3
/
2
)
l
)
Tis used for later computation.Corollary 7.
The error variables have seven freedoms in body frame.Proof of Corollary7.
Error variablee
i
j
′′
=
(
x
i
j
′′
,
y
i
j
′′
)
T
=
(
x
i
′′
-
x
j
′′
,
y
i
′′
-
y
j
′′
)
T. According to (25), the variables x
1
′′
,
y
1
′′, and y
2
′′ equal zeros. Therefore, it holds
(29)
y
12
′′
=
0
,
x
23
′′
=
-
x
12
′′
+
x
13
′′
=
-
(
x
2
-
x
1
)
2
+
(
y
2
-
y
1
)
2
+
(
x
1
-
x
3
)
cos
⁡
θ
+
(
y
1
-
y
3
)
sin
θ
,
y
23
′′
=
y
13
′′
=
(
x
3
-
x
1
)
sin
θ
-
(
y
3
-
y
1
)
cos
⁡
θ
.
Moreover, the error variables about velocities satisfy
(30)
x
˙
23
′′
=
x
˙
2
′′
-
x
˙
3
′′
=
x
˙
12
′′
-
x
˙
13
′′
,
y
˙
23
′′
=
y
˙
2
′′
-
y
˙
3
′′
=
y
˙
12
′′
-
y
˙
13
′′
.
Therefore, error variables X
~
=
(
x
˙
12
′′
,
y
˙
12
′′
,
x
˙
13
′′
,
y
˙
13
′′
,
x
˙
23
′′
,
y
˙
23
′′
,
x
12
′′
,
y
12
′′
,
x
13
′′
,
y
13
′′
,
x
23
′′
,
y
23
′′
)
T have seven free elements as Z
~
=
(
x
˙
12
′′
,
y
˙
12
′′
,
x
˙
13
′′
,
y
˙
13
′′
,
x
12
′′
,
x
13
′′
,
y
13
′′
)
T. Denoting Y
~
=
(
x
˙
23
′′
,
y
˙
23
′′
,
y
12
′′
,
x
23
′′
,
y
23
′′
)
T, the error dynamic system can be rewritten as Z
~
˙
=
g
(
Z
~
,
h
(
Z
~
)
). Here Y
~
=
h
(
Z
~
) is invariable manifold of the original system.Corollary 8.
The error dynamic system of formation is stable at the equilibrium point.Proof of Corollary8.
According to the center manifold theorem of nonlinear dynamic, the stability of original system is guaranteed if the reduced system is stable [27].According to the definition of reduced system,Z
~
˙
=
(
x
¨
12
′′
,
y
¨
12
′′
,
x
¨
13
′′
,
y
¨
13
′′
,
x
˙
12
′′
,
x
˙
13
′′
,
y
˙
13
′′
)
T. The elements of Z
~
˙ are functions of X
~. Substitute x
˙
23
′′
,
y
˙
23
′′
,
y
12
′′
,
x
23
′′, and y
23
′′ by the free error variables according to (29) and (30). Then, take the partial derivatives of the free error variables. One can obtain the Jacobian as
(31)
D
g
(
Z
~
)
=
[
-
2
v
m
0
v
m
0
k
m
(
l
23
(
y
13
′′
)
2
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
2
)
-
k
m
(
l
23
(
y
13
′′
)
2
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
1
)
-
k
m
(
l
23
y
13
′′
(
x
12
′′
-
x
13
′′
)
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
)
0
-
2
v
m
0
v
m
k
m
(
l
23
y
13
′′
(
x
12
′′
-
x
13
′′
)
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
)
-
k
m
(
l
23
y
13
′′
(
x
12
′′
-
x
13
′′
)
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
)
k
m
(
l
23
(
y
13
′′
)
2
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
l
23
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
1
/
2
+
1
)
v
m
0
-
2
v
m
0
-
k
m
(
l
23
(
y
13
′′
)
2
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
1
)
k
m
(
l
13
(
y
13
′′
)
2
(
(
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
+
l
23
(
y
13
′′
)
2
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
2
)
k
m
(
l
23
y
13
′′
(
x
12
′′
-
x
13
′′
)
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
l
13
x
13
′′
y
13
′′
(
(
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
)
0
v
m
0
-
2
v
m
-
k
m
(
l
23
y
13
′′
(
x
12
′′
-
x
13
′′
)
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
)
k
m
(
l
23
y
13
′′
(
x
12
′′
-
x
13
′′
)
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
l
13
x
13
′′
y
13
′′
(
(
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
)
k
m
(
l
13
(
x
13
′′
)
2
(
(
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
+
l
23
(
x
12
′′
)
2
+
l
23
(
x
13
′′
)
2
-
2
l
23
x
12
′′
x
13
′′
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
2
)
1
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
1
0
0
0
]
.Naturally, it holds(32)
(
(
x
12
′′
)
*
,
(
y
12
′′
)
*
,
(
x
13
′′
)
*
,
(
y
13
′′
)
*
,
(
x
23
′′
)
*
,
(
y
23
′′
)
*
)
T
=
(
0
-
(
3
2
l
)
,
(
-
l
)
-
(
-
l
2
)
,
0
-
(
3
2
l
)
0
-
(
-
l
)
,
0
-
0,0
-
(
-
l
2
)
,
0
-
(
3
2
l
)
,
(
-
l
)
-
(
-
l
2
)
,
0
-
(
3
2
l
)
)
T
=
(
l
,
0
,
l
2
,
3
2
l
,
-
l
2
,
-
3
2
l
)
T
and (
(
x
˙
12
′′
)
*
,
(
y
˙
12
′′
)
*
,
(
x
˙
13
′′
)
*
,
(
y
˙
13
′′
)
*
,
(
x
˙
23
′′
)
*
,
(
y
˙
23
′′
)
*
)
T
=
(
0,0
,
0,0
,
0,0
)
T. Hence, the equilibrium point is X
~
*
=
(
0,0
,
0,0
,
0,0
,
l
,
0
,
l
/
2
,
(
3
/
2
)
l
,
-
l
/
2
,
-
(
3
/
2
)
l
)
Tfor original system. And the equilibrium point of the reduced system is Z
~
*
=
(
0,0
,
0,0
,
l
,
l
/
2
,
(
3
/
2
)
l
)
T.The Jacobian matrix of reduced system at equilibrium point is(33)
D
g
(
Z
~
*
)
=
[
-
2
v
m
0
v
m
0
-
5
4
k
m
1
4
k
m
3
4
k
m
0
-
2
v
m
0
v
m
-
3
4
k
m
3
4
k
m
3
4
k
m
v
m
0
-
2
v
m
0
1
4
k
m
-
1
2
k
m
0
0
v
m
0
-
2
v
m
3
4
k
m
0
-
3
2
k
m
1
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
1
0
0
0
]
.The JacobianD
g
(
Z
~
*
) can be proved to have multiple conjugate eigenvalues with all real parts negative (
k
≥
0
,
v
≥
0
,
m
≥
0
). When processing the eigenvalues of Jacobian, characteristic polynomial comes to be a seven older algebraic equation. Since the algebraic equation of older higher than five has no analytical solutions, numerical computations are usually considered instead. By giving particular values of k
,
v, and m, numerical computation can be easily made to check the eigenvalues to be multiple conjugate roots with all real parts negative. This means the error dynamic system of formation is stable at the equilibrium point. One can also try to prove this by the Routh criterion with computation of symbols.


### 2.4. Obstacle Avoidance

Obstacle avoidance is investigated by adding modified obstacle force to vehicles. Obstacles are stored as{
(
q
~
l
,
R
l
,
k
l
)
∣
l
=
1,2
,
…
,
m
}. Here q
~
l is position of center of the lth obstacle, R
l is safe bound, and k
l is coefficient for obstacle force. A classical obstacle force is introduced in [15] as follows.Definition 9.
Obstacle force between theith vehicle and the lth obstacle is defined as
(34)
f
i
l
O
(
q
)
=
{
(
k
l
∥
d
i
l
∥
-
k
l
R
l
)
d
i
l
∥
d
i
l
∥
∥
d
i
l
∥
<
R
l
,
0
otherwise
.
Here d
i
l
=
(
q
i
-
q
~
l
).The obstacle force on theith vehicle is
(35)
f
i
O
(
q
)
=
∑
l
=
1
m
f
i
l
O
(
q
)
.
The obstacle force introduced in Definition 9 can be decomposed into two orthogonal components as
(36)
f
i
O
=
f
i
O
,
h
+
f
i
O
,
⊥
.
Here f
i
O
,
h is a force along the direction of velocity which can change the speed of velocity, while f
i
O
,
⊥ can change the direction of velocity. Here, f
i
O
,
⊥ is chosen to be modified obstacle force. The left of Figure 2 is an example of obstacle with safe bound. Orthogonal decomposition of force is shown in the right of Figure 2.Figure 2
The left is an example of obstacle.R
l is safe bound and d
i
l is distance between the ith vehicle and the lth obstacle. The right is orthogonal decomposition of obstacle force.


## 2.1. Formation of Vehicles

Consider a formation ofn homological vehicles. The dynamic of the ith vehicle is
(1)
q
˙
i
=
p
i
m
i
,
p
˙
i
=
u
i
tran
⁡
,
θ
˙
i
=
I
i
J
i
,
I
˙
i
=
u
i
rota
.
Here m
i is mass and J
i is moment of inertia of the ith vehicle; q
i is the position and θ
i is the orientation; p
i and I
i are linear momentum and angular momentum; u
i
tran
⁡ and u
i
rota are the controls, respectively.The configuration space ofn vehicles is Q
=
R
2
n
×
S
1
×
·
·
·
×
S
1
︸
n, with Lagrangian of the ith vehicle L
i
:
T
Q
→
R,
(2)
L
i
(
q
,
θ
,
q
˙
,
θ
˙
)
=
1
2
q
˙
i
T
M
i
q
˙
i
+
1
2
θ
˙
i
T
J
i
θ
˙
i
-
V
i
(
q
)
-
V
-
i
(
θ
)
.
Here M
i is mass matrix of the ith vehicle and V
i
(
q
) and V
-
i
(
θ
) are potential functions.In the rest of this paper, we writeq
=
(
q
1
,
q
2
,
…
,
q
n
) and θ
=
(
θ
1
,
θ
2
,
…
,
θ
n
). Denote e
i
j
=
(
q
i
-
q
j
) and ω
i
j
=
(
θ
i
-
θ
j
). Formation preserving requires ∥
e
i
j
∥
→
l
i
j, ∥
ω
i
j
∥
→
0, ∥
e
˙
i
j
∥
→
0 and ∥
ω
˙
i
j
∥
→
0 where l
i
j is a desired distance between the ith and jth vehicles.


## 2.2. Potential Field

The potential field with stress function has been mentioned in [8]. The equivalent force in such potential field can be proved to have the same dynamic with spring. Also, we can introduce a damping force and corresponding potential function for stabilization.Definition 1.
Potential function of position of theith vehicle in formation is defined as
(3)
V
i
(
q
)
=
∑
j
=
1
n
V
~
(
q
i
,
q
j
)
=
-
1
2
∑
j
=
1
n
k
i
j
(
∥
q
i
-
q
j
∥
-
l
i
j
)
2
,
with which V
~
(
q
i
,
q
j
)
=
-
(
1
/
2
)
k
i
j
(
∥
q
i
-
q
j
∥
-
l
i
j
)
2 and
(4)
k
i
j
=
{
K
(
i
,
j
)
,
j
≠
i
,
0
,
j
=
i
.
Here K is a symmetric matrix whose element K
(
i
,
j
) is a constant or function and K
(
i
,
j
)
>
0. The potential function V
~ is symmetric, which means that V
~
(
q
i
,
q
j
)
=
V
~
(
q
j
,
q
i
).Proposition 2.
Potential force of theith vehicle is
(5)
f
i
P
(
q
)
=
-
∑
j
=
1
n
k
i
j
(
∥
q
i
(
t
)
-
q
j
(
t
)
∥
-
l
i
j
)
(
q
i
(
t
)
-
q
j
(
t
)
)
∥
q
i
(
t
)
-
q
j
(
t
)
∥
.Proof of Proposition2.
Assume that the Lagrangian of theith vehicle in formation is L
i
(
q
,
q
˙
)
=
(
1
/
2
)
q
˙
i
T
M
i
q
˙
i
-
V
i
(
q
). According to the Euler-Lagrange equation, a free system without external force satisfied f
i
=
M
i
q
¨
i
=
∂
V
i
(
q
)
/
∂
q
i, which means
(6)
f
i
P
(
q
)
=
∂
V
i
(
q
)
∂
q
i
=
-
∑
j
=
1
n
k
i
j
(
∥
q
i
-
q
j
∥
-
l
i
j
)
∂
(
∥
q
i
-
q
j
∥
)
∂
q
i
.
Consider the dot product of vectors. It holdsq
2
=
q
·
q
=
q
T
q
=
∑
q
i
2
=
(
∥
q
i
∥
)
2, and hence (
∥
q
i
-
q
j
∥
)
2
=
(
q
i
-
q
j
)
2. Naturally it holds that ∂
(
∥
q
i
-
q
j
∥
)
2
/
∂
q
i
=
∂
(
q
i
-
q
j
)
2
/
∂
q
i, with which satisfying
(7)
∂
(
∥
q
i
-
q
j
∥
)
2
∂
q
i
=
2
(
∥
q
i
-
q
j
∥
)
∂
(
∥
q
i
-
q
j
∥
)
∂
q
i
,
∂
(
q
i
-
q
j
)
2
∂
q
i
=
2
(
q
i
-
q
j
)
.
Therefore, it holds ∂
(
∥
q
i
-
q
j
∥
)
/
∂
q
i
=
(
q
i
-
q
j
)
/
∥
q
i
-
q
j
∥. Thus, potential force on the ith vehicles is
(8)
f
i
P
(
q
)
=
-
∑
j
=
1
n
k
i
j
(
∥
q
i
-
q
j
∥
-
l
i
j
)
(
q
i
-
q
j
)
∥
q
i
-
q
j
∥
.
Potential force between theith and jth vehicles is
(9)
f
i
j
P
(
q
)
=
-
k
i
j
(
∥
q
i
-
q
j
∥
-
l
i
j
)
(
q
i
-
q
j
)
∥
q
i
-
q
j
∥
.Definition 3.
Potential function of orientation of theith vehicle is defined as
(10)
V
-
i
(
θ
)
=
∑
j
=
1
n
V
^
(
θ
i
,
θ
j
)
=
-
1
2
∑
j
=
1
n
k
^
i
j
(
θ
i
-
θ
j
)
2
,
with which V
^
(
θ
i
,
θ
j
)
=
-
(
1
/
2
)
k
^
i
j
(
θ
i
-
θ
j
)
2 and
(11)
k
^
i
j
=
{
K
^
(
i
,
j
)
,
j
≠
i
,
0
,
j
=
i
.
Here K
^ is a symmetric matrix whose element K
^
(
i
,
j
) is a constant or function and K
^
(
i
,
j
)
>
0. Respectively, the potential force of orientation of the ith vehicle is
(12)
f
-
i
P
(
θ
)
=
-
∑
j
=
1
n
k
^
i
j
(
θ
i
(
t
)
-
θ
j
(
t
)
)
.
System that has dynamic x
¨
=
-
k
(
x
-
x
*
) will act as a spring. With potential functions {
(
V
i
(
q
)
,
V
-
i
(
θ
)
)
∣
i
=
1,2
,
…
,
n
} defined above, vehicles can have coordinated motion as if there were springs connecting them. Since the spring system is periodically stable, damping force x
¨
=
-
v
x
˙ is considered for stabilization.Definition 4.
Damping forces of theith vehicle are
(13)
f
i
D
(
q
˙
)
=
∑
j
=
1
n
f
i
j
D
=
-
∑
j
=
1
n
v
i
j
(
q
˙
i
(
t
)
-
q
˙
j
(
t
)
)
,
f
-
i
D
(
θ
˙
)
=
∑
j
=
1
n
f
-
i
j
D
=
-
∑
j
=
1
n
v
^
i
j
(
θ
˙
i
(
t
)
-
θ
˙
j
(
t
)
)
,
with f
i
j
D
=
-
v
i
j
(
q
˙
i
-
q
˙
j
)
,
v
i
j
≥
0 and f
-
i
j
D
=
-
v
^
i
j
(
θ
˙
i
-
θ
˙
j
)
,
v
^
i
j
≥
0.Same as the potential function of stress function, the damping force has corresponding potential functions{
(
V
i
D
(
q
˙
)
,
V
-
i
D
(
θ
˙
)
)
∣
i
=
1,2
,
…
,
n
}, which are
(14)
V
i
D
(
q
˙
)
=
∑
j
=
1
n
V
~
(
q
˙
i
,
q
˙
j
)
=
-
1
2
∑
j
=
1
n
v
i
j
(
q
˙
i
-
q
˙
j
)
2
,
V
-
i
D
(
θ
˙
)
=
∑
j
=
1
n
V
^
(
θ
˙
i
,
θ
˙
j
)
=
-
1
2
∑
j
=
1
n
v
^
i
j
(
θ
˙
i
-
θ
˙
j
)
2
.


## 2.3. Stability of Formation in Potential Field

Stability of formation in potential field is proved by using notion of error dynamic system. Define the error variablee
i
j
(
t
)
=
q
i
(
t
)
-
q
j
(
t
). e
i
j is the relative position and ∥
e
i
j
∥ is the relative distance between the ith and jth vehicles. It holds e
i
j
(
t
)
=
-
e
j
i
(
t
), e
i
i
=
0 and e
˙
i
j
(
t
)
=
q
˙
i
(
t
)
-
q
˙
j
(
t
). However, the calculation of the Jacobian is unsolvable, because the stability of formation is related to the structure of formation rather than the particular coordinates. Transformation of coordinates from inertial frame to body frame is considered. Reduced system is derived by specific substitution. According to the center manifold theorem of nonlinear dynamic, stability of original system is guaranteed if the reduced system is stable.Firstly, we consider the dynamic system for vehicles’ positions. Denote the control force byf
i
(
t
)
=
f
i
P
(
q
(
t
)
)
+
f
i
D
(
q
˙
(
t
)
). According to Newton's law of motion, q
i
(
t
) can be given by integration as follows:
(15)
q
i
(
t
)
=
q
i
(
0
)
+
∫
0
t
q
˙
i
(
τ
)
d
τ
=
q
i
(
0
)
+
∫
0
t
{
q
˙
i
(
0
)
+
1
m
∫
0
τ
f
i
(
s
)
d
s
}
d
τ
=
q
i
(
0
)
+
q
˙
i
(
0
)
t
+
1
m
∫
0
t
∫
0
τ
f
i
(
s
)
d
s
d
τ
,
where q
i
(
0
) and q
˙
i
(
0
) are constants.Hence, it holds(16)
q
˙
i
(
t
)
=
q
˙
i
(
0
)
+
1
m
∫
0
t
f
i
(
s
)
d
s
,
q
¨
i
(
t
)
=
1
m
f
i
(
t
)
-
1
m
f
i
(
0
)
.
Boundary condition vanishes as f
i
(
0
)
=
0. Second derivation of e
i
j
(
t
) satisfies
(17)
e
¨
i
j
(
t
)
=
q
¨
i
(
t
)
-
q
¨
j
(
t
)
=
1
m
(
f
i
P
(
q
(
t
)
)
+
f
i
D
(
q
˙
(
t
)
)
)
-
1
m
(
f
j
P
(
q
(
t
)
)
+
f
j
D
(
q
˙
(
t
)
)
)
=
-
1
m
(
∑
s
=
1
n
k
i
s
(
∥
q
i
(
t
)
-
q
s
(
t
)
∥
-
l
i
s
)
(
q
i
(
t
)
-
q
s
(
t
)
)
∥
q
i
(
t
)
-
q
s
(
t
)
∥
+
∑
s
=
1
n
v
i
s
(
q
˙
i
-
q
˙
s
)
)
+
1
m
(
∑
s
=
1
n
k
j
s
(
∥
q
j
(
t
)
-
q
s
(
t
)
∥
-
l
j
s
)
(
q
j
(
t
)
-
q
s
(
t
)
)
∥
q
j
(
t
)
-
q
s
(
t
)
∥
+
∑
s
=
1
n
v
j
s
(
q
˙
j
-
q
˙
s
)
)
=
1
m
(
∑
s
=
1
n
k
i
s
(
l
i
s
∥
e
i
s
(
t
)
∥
-
1
)
e
i
s
(
t
)
-
∑
s
=
1
n
k
j
s
(
l
j
s
∥
e
j
s
(
t
)
∥
-
1
)
e
j
s
(
t
)
)
-
1
m
(
∑
s
=
1
n
v
i
s
e
˙
i
s
(
t
)
-
∑
s
=
1
n
v
j
s
e
˙
j
s
(
t
)
)
.We consider a leader-follower formation of three homogeneous vehicles, because it is a basic element of formation [3, 9]. Suppose vehicles have the same mass m and coefficients k
,
v. In the leader-follower formation, vehicle 1 is usually selected to be the leader. It does not have interactions from vehicle 2 and vehicle 3. Hence, the error dynamic has the following form:(18)
[
e
¨
12
e
¨
13
e
¨
23
e
˙
12
e
˙
13
e
˙
23
]
=
[
-
v
m
0
v
m
k
m
(
l
12
∥
e
12
∥
-
1
)
0
-
k
m
(
l
23
∥
e
23
∥
-
1
)
0
-
v
m
-
v
m
0
k
m
(
l
13
∥
e
13
∥
-
1
)
k
m
(
l
23
∥
e
23
∥
-
1
)
v
m
-
v
m
-
2
v
m
-
k
m
(
l
12
∥
e
12
∥
-
1
)
k
m
(
l
13
∥
e
13
∥
-
1
)
2
k
m
(
l
23
∥
e
23
∥
-
1
)
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
]
[
e
˙
12
e
˙
13
e
˙
23
e
12
e
13
e
23
]
.By denoting X
=
(
e
˙
12
,
e
˙
13
,
e
˙
23
,
e
12
,
e
13
,
e
23
)
T, (18) can be written in form of X
˙
=
f
(
X
).Secondly, we consider the equilibrium point and the stability of the above system. Sincee
˙
i
j and e
i
j are vectors, we need to define a projection function g
(
X
)
:
R
m
×
·
·
·
×
R
m
︸
n
/
{
0
}
→
R
n,
(19)
g
(
X
)
=
(
∥
X
1
∥
,
∥
X
2
∥
,
…
,
∥
X
n
∥
)
T
,
X
=
(
X
1
,
X
2
,
…
,
X
n
)
T
,
X
i
∈
R
m
.According to the work of Laplace and Lagrange, if a system is conservative, then a state corresponding to zero kinetic energy and minimum potential energy is a stable equilibrium point [26]. In our system, we have an adjoint equilibrium point Y
*
=
g
(
X
*
)
=
(
0,0
,
0
,
l
12
,
l
13
,
l
23
)
T for dynamic system X
˙
=
f
(
X
). Solutions X
* stand for stable structures.Corollary 5.
Y
*
=
(
0,0
,
0
,
l
12
,
l
13
,
l
23
)
T is an adjoint equilibrium point of system X
˙
=
f
(
X
).Proof of Corollary5.
IfX
*
=
(
e
˙
12
*
,
e
˙
13
*
,
e
˙
23
*
,
e
12
*
,
e
13
*
,
e
23
*
)
T satisfies Y
*
=
g
(
X
*
)
=
(
0,0
,
0
,
l
12
,
l
13
,
l
23
)
T, it holds (
∥
e
˙
12
*
∥
,
∥
e
˙
13
*
∥
,
∥
e
˙
23
*
∥
,
∥
e
12
*
∥
,
∥
e
13
*
∥
,
∥
e
23
*
∥
)
T
=
(
0,0
,
0
,
l
12
,
l
13
,
l
23
)
T and X
˙
*
=
f
(
X
*
)
=
(
e
¨
12
*
,
e
¨
13
*
,
e
¨
23
*
,
e
˙
12
*
,
e
˙
13
*
,
e
˙
23
*
,
)
T
=
0. Hence Y
*
=
(
0,0
,
0
,
l
12
,
l
13
,
l
23
)
T is an adjoint equilibrium point for dynamic system X
˙
=
f
(
X
).Consider the Jacobian for stable structures. Denotee
i
j
=
(
x
i
j
,
y
i
j
)
T, while e
˙
i
j
=
(
x
˙
i
j
,
y
˙
i
j
)
T, e
¨
i
j
=
(
x
¨
i
j
,
y
¨
i
j
)
T. Compute the Jacobian matrix of f
(
x
) with the individual components:
(20)
∂
f
1
∂
X
1
=
∂
(
[
x
¨
12
,
y
¨
12
]
)
∂
(
[
x
˙
12
,
y
˙
12
]
)
=
[
-
v
m
0
0
-
v
m
]
,
∂
f
1
∂
X
2
=
∂
(
[
x
¨
12
,
y
¨
12
]
)
∂
(
[
x
˙
13
,
y
˙
13
]
)
=
0
2
×
2
,
∂
f
1
∂
X
3
=
∂
(
[
x
¨
12
,
y
¨
12
]
)
∂
(
[
x
˙
23
,
y
˙
23
]
)
=
[
v
m
0
0
v
m
]
,
∂
f
1
∂
X
4
=
∂
(
[
x
¨
12
,
y
¨
12
]
)
∂
(
[
x
12
,
y
12
]
)
=
[
k
m
(
l
12
y
12
2
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
1
)
-
k
m
l
12
x
12
y
12
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
k
m
l
12
x
12
y
12
(
x
12
2
+
y
12
2
)
(
3
/
2
)
k
m
(
l
12
x
12
2
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
1
)
]
,
∂
f
1
∂
X
5
=
∂
(
[
x
¨
12
,
y
¨
12
]
)
∂
(
[
x
13
,
y
13
]
)
=
0
2
×
2
,
∂
f
1
∂
X
6

=
∂
(
[
x
¨
12
,
y
¨
12
]
)
∂
(
[
x
23
,
y
23
]
)

=

[
-
k
m
(
l
23
y
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
k
m
(
l
23
x
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
]
.Use the same way to calculate the remaining parts. We can have the Jacobian matrix in form of(21)
D
f
(
x
)
=
[
A
B
I
6
×
6
0
]
,
where
(22)
Α
=
[
-
v
m
0
0
0
v
m
0
0
-
v
m
0
0
0
v
m
0
0
-
v
m
0
-
v
m
0
0
0
0
-
v
m
0
-
v
m
v
m
0
-
v
m
0
-
2
v
m
0
0
v
m
0
-
v
m
0
-
2
v
m
]
,
(23)
Β
=
[
k
m
(
l
12
y
12
2
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
1
)
-
k
m
l
12
x
12
y
12
(
x
12
2
+
y
12
2
)
(
3
/
2
)
0
0
-
k
m
(
l
23
y
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
k
m
l
12
x
12
y
12
(
x
12
2
+
y
12
2
)
(
3
/
2
)
k
m
(
l
12
x
12
2
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
1
)
0
0
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
k
m
(
l
23
x
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
0
0
k
m
(
l
13
y
13
2
(
x
13
2
+
y
13
2
)
(
3
/
2
)
-
1
)
-
k
m
l
13
x
13
y
13
(
x
13
2
+
y
13
2
)
(
3
/
2
)
k
m
(
l
23
y
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
-
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
0
0
-
k
m
l
13
x
13
y
13
(
x
13
2
+
y
13
2
)
(
3
/
2
)
k
m
(
l
13
x
13
2
(
x
13
2
+
y
13
2
)
(
3
/
2
)
-
1
)
-
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
k
m
(
l
23
x
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
-
k
m
(
l
12
y
12
2
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
1
)
k
m
l
12
x
12
y
12
(
x
12
2
+
y
12
2
)
(
3
/
2
)
k
m
(
l
13
y
13
2
(
x
13
2
+
y
13
2
)
(
3
/
2
)
-
1
)
-
k
m
l
13
x
13
y
13
(
x
13
2
+
y
13
2
)
(
3
/
2
)
2
k
m
(
l
23
y
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
-
2
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
k
m
l
12
x
12
y
12
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
k
m
(
l
12
x
12
2
(
x
12
2
+
y
12
2
)
(
3
/
2
)
-
1
)
-
k
m
l
13
x
13
y
13
(
x
13
2
+
y
13
2
)
(
3
/
2
)
k
m
(
l
13
x
13
2
(
x
13
2
+
y
13
2
)
(
3
/
2
)
-
1
)
-
2
k
m
l
23
x
23
y
23
(
x
23
2
+
y
23
2
)
(
3
/
2
)
2
k
m
(
l
23
x
23
2
(
x
23
2
+
y
23
2
)
(
3
/
2
)
-
1
)
]
.
Since the adjoint equilibrium point is Y
*
=
(
0,0
,
0
,
l
12
,
l
13
,
l
23
)
T, we only know (
x
i
j
2
+
y
i
j
2
)
=
l
i
j
2 instead of particular x
i
j and y
i
j. As a result, we cannot analyze the stability in inertial frame. In reality, the structure of formation is the basis of stability instead of particular coordinates of vehicles. In other words, there are infinite possible pairs of vehicles’ coordinates with which formation is stable while the number of corresponding stable structure is limited.Hence, we need to introduce a body frame and discuss the stability in the body frame. As shown in Figure1, the body frame is constructed by setting the vehicle 1 on the origin and vehicle 2 on the negative x-axis. θ is the angle from negative x-axis to the connecting line between vehicle 1 and vehicle 3. The right of Figure 1 shows the stable structures of formation in body frame.Figure 1
The left illuminates the construction of body frame where theV
i stands for ith vehicle. The right is an example of stable structures of formation in body frame. (
V
1
*
;
V
2
*
;
V
3
*
) is the combination of coordinates of stable structure.Definition 6.
General transformation of coordinates from inertial frame to body frame needs translation and rotation as follows:(24)
Translation
:
{
x
i
′
=
x
i
-
x
1
y
i
′
=
y
i
-
y
1
,
Rotation
:
[
x
i
′′
y
i
′′
]
=
[
cos
⁡
θ
sin
θ
-
sin
θ
cos
⁡
θ
]
[
x
i
′
y
i
′
]
.In order to achieve the transformation from the inertial frame to required body frame, the new body coordinates can be represented by inertial coordinates as(25)
x
1
′′
=
0
,
y
1
′′
=
0
,
x
2
′′
=
-
(
x
2
-
x
1
)
2
+
(
y
2
-
y
1
)
2
,
y
2
′′
=
0
,
x
3
′′
=
(
x
3
-
x
1
)
cos
⁡
θ
+
(
y
3
-
y
1
)
sin
θ
,
y
3
′′
=
-
(
x
3
-
x
1
)
sin
θ
+
(
y
3
-
y
1
)
cos
⁡
θ
.
Since x
1
′′
,
y
1
′′, and y
2
′′ vanish to zero, it has stable structure (
0,0
,
-
l
12
,
0
,
(
x
3
′′
)
*
,
(
y
3
′′
)
*
)
T. The coordinates of x
3
′′ and y
3
′′ satisfy
(26)
(
(
x
3
′′
)
*
-
0
)
2
+
(
(
y
3
′′
)
*
-
0
)
2
=
l
13
2
,
(
(
x
3
′′
)
*
-
(
-
l
12
)
)
2
+
(
(
y
3
′′
)
*
-
0
)
2
=
l
23
2
.
Its solutions are
(27)
(
x
3
′′
)
*
=
-
(
l
12
2
+
l
13
2
-
l
23
2
)
2
l
12
,
(
y
3
′′
)
*
=
±
(
l
12
+
l
13
-
l
23
)
(
l
12
-
l
13
+
l
23
)
(
l
13
-
l
12
+
l
23
)
(
l
12
+
l
13
+
l
23
)
2
l
12
.
Without losing generality, if we let l
12
=
l
13
=
l
23
=
l, we can obtain
(28)
(
x
3
′′
)
*
=
-
l
2
,
(
y
3
′′
)
*
=
±
3
2
l
.In this paper, stable structure means vehicles form a desired formation structure with the stress function that is equal to zero. For consideration of illumination, stable structure(
0,0
,
-
l
,
0
,
-
l
/
2
,
(
3
/
2
)
l
)
Tis used for later computation.Corollary 7.
The error variables have seven freedoms in body frame.Proof of Corollary7.
Error variablee
i
j
′′
=
(
x
i
j
′′
,
y
i
j
′′
)
T
=
(
x
i
′′
-
x
j
′′
,
y
i
′′
-
y
j
′′
)
T. According to (25), the variables x
1
′′
,
y
1
′′, and y
2
′′ equal zeros. Therefore, it holds
(29)
y
12
′′
=
0
,
x
23
′′
=
-
x
12
′′
+
x
13
′′
=
-
(
x
2
-
x
1
)
2
+
(
y
2
-
y
1
)
2
+
(
x
1
-
x
3
)
cos
⁡
θ
+
(
y
1
-
y
3
)
sin
θ
,
y
23
′′
=
y
13
′′
=
(
x
3
-
x
1
)
sin
θ
-
(
y
3
-
y
1
)
cos
⁡
θ
.
Moreover, the error variables about velocities satisfy
(30)
x
˙
23
′′
=
x
˙
2
′′
-
x
˙
3
′′
=
x
˙
12
′′
-
x
˙
13
′′
,
y
˙
23
′′
=
y
˙
2
′′
-
y
˙
3
′′
=
y
˙
12
′′
-
y
˙
13
′′
.
Therefore, error variables X
~
=
(
x
˙
12
′′
,
y
˙
12
′′
,
x
˙
13
′′
,
y
˙
13
′′
,
x
˙
23
′′
,
y
˙
23
′′
,
x
12
′′
,
y
12
′′
,
x
13
′′
,
y
13
′′
,
x
23
′′
,
y
23
′′
)
T have seven free elements as Z
~
=
(
x
˙
12
′′
,
y
˙
12
′′
,
x
˙
13
′′
,
y
˙
13
′′
,
x
12
′′
,
x
13
′′
,
y
13
′′
)
T. Denoting Y
~
=
(
x
˙
23
′′
,
y
˙
23
′′
,
y
12
′′
,
x
23
′′
,
y
23
′′
)
T, the error dynamic system can be rewritten as Z
~
˙
=
g
(
Z
~
,
h
(
Z
~
)
). Here Y
~
=
h
(
Z
~
) is invariable manifold of the original system.Corollary 8.
The error dynamic system of formation is stable at the equilibrium point.Proof of Corollary8.
According to the center manifold theorem of nonlinear dynamic, the stability of original system is guaranteed if the reduced system is stable [27].According to the definition of reduced system,Z
~
˙
=
(
x
¨
12
′′
,
y
¨
12
′′
,
x
¨
13
′′
,
y
¨
13
′′
,
x
˙
12
′′
,
x
˙
13
′′
,
y
˙
13
′′
)
T. The elements of Z
~
˙ are functions of X
~. Substitute x
˙
23
′′
,
y
˙
23
′′
,
y
12
′′
,
x
23
′′, and y
23
′′ by the free error variables according to (29) and (30). Then, take the partial derivatives of the free error variables. One can obtain the Jacobian as
(31)
D
g
(
Z
~
)
=
[
-
2
v
m
0
v
m
0
k
m
(
l
23
(
y
13
′′
)
2
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
2
)
-
k
m
(
l
23
(
y
13
′′
)
2
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
1
)
-
k
m
(
l
23
y
13
′′
(
x
12
′′
-
x
13
′′
)
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
)
0
-
2
v
m
0
v
m
k
m
(
l
23
y
13
′′
(
x
12
′′
-
x
13
′′
)
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
)
-
k
m
(
l
23
y
13
′′
(
x
12
′′
-
x
13
′′
)
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
)
k
m
(
l
23
(
y
13
′′
)
2
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
l
23
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
1
/
2
+
1
)
v
m
0
-
2
v
m
0
-
k
m
(
l
23
(
y
13
′′
)
2
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
1
)
k
m
(
l
13
(
y
13
′′
)
2
(
(
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
+
l
23
(
y
13
′′
)
2
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
2
)
k
m
(
l
23
y
13
′′
(
x
12
′′
-
x
13
′′
)
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
l
13
x
13
′′
y
13
′′
(
(
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
)
0
v
m
0
-
2
v
m
-
k
m
(
l
23
y
13
′′
(
x
12
′′
-
x
13
′′
)
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
)
k
m
(
l
23
y
13
′′
(
x
12
′′
-
x
13
′′
)
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
l
13
x
13
′′
y
13
′′
(
(
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
)
k
m
(
l
13
(
x
13
′′
)
2
(
(
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
+
l
23
(
x
12
′′
)
2
+
l
23
(
x
13
′′
)
2
-
2
l
23
x
12
′′
x
13
′′
(
(
x
12
′′
-
x
13
′′
)
2
+
(
y
13
′′
)
2
)
3
/
2
-
2
)
1
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
1
0
0
0
]
.Naturally, it holds(32)
(
(
x
12
′′
)
*
,
(
y
12
′′
)
*
,
(
x
13
′′
)
*
,
(
y
13
′′
)
*
,
(
x
23
′′
)
*
,
(
y
23
′′
)
*
)
T
=
(
0
-
(
3
2
l
)
,
(
-
l
)
-
(
-
l
2
)
,
0
-
(
3
2
l
)
0
-
(
-
l
)
,
0
-
0,0
-
(
-
l
2
)
,
0
-
(
3
2
l
)
,
(
-
l
)
-
(
-
l
2
)
,
0
-
(
3
2
l
)
)
T
=
(
l
,
0
,
l
2
,
3
2
l
,
-
l
2
,
-
3
2
l
)
T
and (
(
x
˙
12
′′
)
*
,
(
y
˙
12
′′
)
*
,
(
x
˙
13
′′
)
*
,
(
y
˙
13
′′
)
*
,
(
x
˙
23
′′
)
*
,
(
y
˙
23
′′
)
*
)
T
=
(
0,0
,
0,0
,
0,0
)
T. Hence, the equilibrium point is X
~
*
=
(
0,0
,
0,0
,
0,0
,
l
,
0
,
l
/
2
,
(
3
/
2
)
l
,
-
l
/
2
,
-
(
3
/
2
)
l
)
Tfor original system. And the equilibrium point of the reduced system is Z
~
*
=
(
0,0
,
0,0
,
l
,
l
/
2
,
(
3
/
2
)
l
)
T.The Jacobian matrix of reduced system at equilibrium point is(33)
D
g
(
Z
~
*
)
=
[
-
2
v
m
0
v
m
0
-
5
4
k
m
1
4
k
m
3
4
k
m
0
-
2
v
m
0
v
m
-
3
4
k
m
3
4
k
m
3
4
k
m
v
m
0
-
2
v
m
0
1
4
k
m
-
1
2
k
m
0
0
v
m
0
-
2
v
m
3
4
k
m
0
-
3
2
k
m
1
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
1
0
0
0
]
.The JacobianD
g
(
Z
~
*
) can be proved to have multiple conjugate eigenvalues with all real parts negative (
k
≥
0
,
v
≥
0
,
m
≥
0
). When processing the eigenvalues of Jacobian, characteristic polynomial comes to be a seven older algebraic equation. Since the algebraic equation of older higher than five has no analytical solutions, numerical computations are usually considered instead. By giving particular values of k
,
v, and m, numerical computation can be easily made to check the eigenvalues to be multiple conjugate roots with all real parts negative. This means the error dynamic system of formation is stable at the equilibrium point. One can also try to prove this by the Routh criterion with computation of symbols.


## 2.4. Obstacle Avoidance

Obstacle avoidance is investigated by adding modified obstacle force to vehicles. Obstacles are stored as{
(
q
~
l
,
R
l
,
k
l
)
∣
l
=
1,2
,
…
,
m
}. Here q
~
l is position of center of the lth obstacle, R
l is safe bound, and k
l is coefficient for obstacle force. A classical obstacle force is introduced in [15] as follows.Definition 9.
Obstacle force between theith vehicle and the lth obstacle is defined as
(34)
f
i
l
O
(
q
)
=
{
(
k
l
∥
d
i
l
∥
-
k
l
R
l
)
d
i
l
∥
d
i
l
∥
∥
d
i
l
∥
<
R
l
,
0
otherwise
.
Here d
i
l
=
(
q
i
-
q
~
l
).The obstacle force on theith vehicle is
(35)
f
i
O
(
q
)
=
∑
l
=
1
m
f
i
l
O
(
q
)
.
The obstacle force introduced in Definition 9 can be decomposed into two orthogonal components as
(36)
f
i
O
=
f
i
O
,
h
+
f
i
O
,
⊥
.
Here f
i
O
,
h is a force along the direction of velocity which can change the speed of velocity, while f
i
O
,
⊥ can change the direction of velocity. Here, f
i
O
,
⊥ is chosen to be modified obstacle force. The left of Figure 2 is an example of obstacle with safe bound. Orthogonal decomposition of force is shown in the right of Figure 2.Figure 2
The left is an example of obstacle.R
l is safe bound and d
i
l is distance between the ith vehicle and the lth obstacle. The right is orthogonal decomposition of obstacle force.


## 3. Variational Integrator

In this section, basic definition of variational integrator is introduced based on discrete Lagrangian and discrete Lagrange-d'Alembert principle. Basically speaking, the variational integrator is a compound of discrete Legendre transformations, which implements integration in discrete configuration space. It is equivalent to solving algebraic equations for every time step.


### 3.1. Variational Integrator

Variational integrator can be derived by applying discrete variational principle on discrete Lagrangian [16, 21]. Forced variational integrator is used for calculation of vehicles' motion.Given LagrangianL
(
q
,
q
˙
) and Lagrange force f
(
q
,
q
˙
), according to the Lagrange-d'Alembert principle:
(37)
δ
∫
0
T
L
(
q
(
t
)
,
q
˙
(
t
)
)
d
t
+
∫
0
T
f
(
q
(
t
)
,
q
˙
(
t
)
)
·
δ
q
(
t
)
d
t
=
0
,
we can get the forced Euler-Lagrange equation:
(38)
∂
L
∂
q
(
q
,
q
˙
)
-
d
d
t
(
∂
L
∂
q
˙
(
q
,
q
˙
)
)
+
f
(
q
,
q
˙
)
=
0
.
For the discrete vision, we define a discrete Lagrangian L
d
:
Q
×
Q
→
R,
(39)
L
d
(
q
k
,
q
k
+
1
)
=
∫
k
h
(
k
+
1
)
h
L
(
q
,
q
˙
)
d
t
.Applying discrete Lagrange-d'Alembert principle on discrete action functionℬ
d which is defined as ℬ
d
(
q
)
=
∑
k
=
0
N
-
1
L
d
(
q
k
,
q
k
+
1
), we can get the forced Euler-Lagrange equation in discrete form as
(40)
D
2
L
d
(
q
k
-
1
,
q
k
)
+
D
1
L
d
(
q
k
,
q
k
+
1
)
+
f
d
+
(
q
k
-
1
,
q
k
)
+
f
d
-
(
q
k
,
q
k
+
1
)
=
0
.
Here D
1 and D
2 stand for taking partial derivatives of L
d
(
q
k
-
1
,
q
k
) according to the variables’ position. f
d
+ and f
d
- are discrete Lagrange forces.Discrete Legendre transformations are maps connecting different discrete configuration spaces as shown in Figure3. They are defined based on (40) as follows:
(41)
F
-
L
d
u
k
(
q
k
,
q
k
+
1
)
=
-
D
1
L
d
(
q
k
,
q
k
+
1
)
-
f
d
-
(
q
k
,
q
k
+
1
)
,
F
+
L
d
u
k
(
q
k
,
q
k
+
1
)
=
D
2
L
d
(
q
k
,
q
k
+
1
)
+
f
d
+
(
q
k
,
q
k
+
1
)
.
The relationships of subscriptions can be referred to Figure 3.Figure 3
Commutable diagram based on discrete Legendre transformations [22]. It shows the operations of variational integrator in discrete configuration space.Definition 10.
Variational integrator is defined as a compound map of discrete Legendre transformations asF
L
d
u
k
-
1
,
u
k
:
(
q
k
-
1
,
q
k
)
↦
(
q
k
,
q
k
+
1
),
(42)
F
L
d
u
k
-
1
,
u
k
=
(
F
-
L
d
u
k
)
-
1
∘
F
+
L
d
u
k
-
1
.With mapF
L
d
u
k
-
1
,
u
k, vehicles' motion can be calculated. Each step, it maps (
q
k
-
1
,
q
k
) to (
q
k
,
q
k
+
1
). The calculation can be divided into two steps. The first step is to compute (
q
k
,
p
k
) from (
q
k
-
1
,
q
k
) by a substitution according to F
+
L
d
u
k
-
1, and the second step is to compute (
q
k
,
q
k
+
1
) from (
q
k
,
p
k
) by solving algebraic equations which is equivalent to the inverse map (
F
-
L
d
u
k
)
-
1.


### 3.2. Variational Integrator for Formation

In a formation withn vehicles, a leader vehicle is chosen to follow a given trajectory. Follower vehicles move in potential fields.States of theith vehicle include positions {
q
k
i
} and orientations {
θ
k
i
} with i
=
1,2
,
…
,
n. Both kinds of variables can be calculated by the variational integrator introduced as above. Different from the situation in continuous space, the labels for different vehicles are in the superscripts of discrete variables. According to the definition of variational integrator, we can calculate (
q
k
,
q
k
+
1
,
θ
k
,
θ
k
+
1
) from (
q
k
-
1
,
q
k
,
θ
k
-
1
,
θ
k
). In the rest of this paper, q
k
=
(
q
k
1
,
q
k
2
,
…
,
q
k
n
) and θ
k
=
(
θ
k
1
,
θ
k
2
,
…
,
θ
k
n
).The precision of algorithm depends on choice of numerical integrations. Without losing of universality, midpoint rule is chosen for instruction. The discrete Lagrange forces in midpoint rule are(43)
f
d
-
(
q
k
,
q
k
+
1
)
=
f
d
+
(
q
k
,
q
k
+
1
)
=
h
4
(
f
k
+
f
k
+
1
)
.
Here f
k means f
(
q
(
t
k
)
,
q
˙
(
t
k
)
) at node k [18].The discrete Lagrangian is(44)
L
d
(
q
k
,
q
k
+
1
,
θ
k
,
θ
k
+
1
)
=
∑
i
=
1
n
{
1
2
(
q
k
+
1
i
-
q
k
i
h
)
T
M
i
(
q
k
+
1
i
-
q
k
i
h
)
+
1
2
(
θ
k
+
1
i
-
θ
k
i
h
)
T
J
i
(
θ
k
+
1
i
-
θ
k
i
h
)
-
V
d
i
(
q
k
,
q
k
+
1
)
-
V
-
d
i
(
θ
k
,
θ
k
+
1
)
-
V
d
i
,
D
(
q
k
,
q
k
+
1
)
-
V
-
d
i
,
D
(
θ
k
,
θ
k
+
1
)
{
1
2
(
q
k
+
1
i
-
q
k
i
h
)
T
M
i
(
q
k
+
1
i
-
q
k
i
h
)
}
.
Here
(45)
V
d
i
(
q
k
,
q
k
+
1
)
=
V
i
(
q
k
+
q
k
+
1
2
)
,
V
-
d
i
(
θ
k
,
θ
k
+
1
)
=
V
-
i
(
θ
k
+
θ
k
+
1
2
)
,
V
d
i
,
D
(
q
k
,
q
k
+
1
)
=
V
i
D
(
q
k
+
1
-
q
k
h
)
,
V
-
d
i
,
D
(
θ
k
,
θ
k
+
1
)
=
V
-
i
D
(
θ
k
+
1
-
θ
k
h
)
.In the leader-follower formation, leader's trajectory{
(
q
k
1
,
θ
k
1
)
∣
k
=
1,2
,
…
,
N
} is given. And {
(
q
k
+
1
i
,
θ
k
+
1
i
)
∣
i
=
2,3
,
…
,
n
} are variables to be solved. Hence, we define the reduced variables q
~
k
=
(
q
k
2
,
q
k
3
,
…
,
q
k
n
) and θ
~
k
=
(
θ
k
2
,
θ
k
3
,
…
,
θ
k
n
).The reduced discrete Lagrange force is(46)
f
~
d
-
(
q
k
,
q
k
+
1
)
=
f
~
d
+
(
q
k
,
q
k
+
1
)
=
(
f
~
2
O
,
⊥
,
f
~
3
O
,
⊥
,
…
,
f
~
n
O
,
⊥
)
.
Here f
~
i
O
,
⊥
=
(
h
/
4
)
(
f
i
O
,
⊥
(
q
k
)
+
f
i
O
,
⊥
(
q
k
+
1
)
), i
=
2,3
,
…
,
n.Hence, we can have reduced discrete Legendre transformationsF
~
+
L
d
u
k
-
1 and F
~
-
L
d
u
k,
(47)
(
p
~
k
;
I
~
k
)
=
F
~
+
L
d
u
k
-
1
(
q
k
-
1
1
,
q
~
k
-
1
,
q
k
1
,
q
~
k
,
θ
k
-
1
1
,
θ
~
k
-
1
,
θ
k
1
,
θ
~
k
)
,
(
p
~
k
;
I
~
k
)
=
F
~
-
L
d
u
k
(
q
k
1
,
q
~
k
,
q
k
+
1
1
,
q
~
k
+
1
,
θ
k
1
,
θ
~
k
,
θ
k
+
1
1
,
θ
~
k
+
1
)
.
Here (
q
k
-
1
1
,
q
k
1
,
θ
k
-
1
1
,
θ
k
1
) are known variables used as inputs. p
~
k and I
~
k stand for the reduced linear momentum and reduced angular momentum in discrete space. They are calculated by substitution F
+
L
d
u
k
-
1. p
~
k is a 4
×
1 vector and I
~
k is a 2
×
1 vector.Substituting pairs of(
q
~
k
,
θ
~
k
) and (
p
~
k
,
I
~
k
) into F
~
-
L
d
u
k, we can obtain the algebraic equations
(48)
F
~
-
L
d
u
k
(
q
k
1
,
q
~
k
,
q
k
+
1
1
,
q
~
k
+
1
,
θ
k
1
,
θ
~
k
,
θ
k
+
1
1
,
θ
~
k
+
1
)
=
(
p
~
k
;
I
~
k
)
.
In (48), (
q
~
k
+
1
,
θ
~
k
+
1
) are unknown variables to be solved. Above processes make the followers’ motion be calculated from (
q
~
k
-
1
,
q
~
k
,
θ
~
k
-
1
,
θ
~
k
) to (
q
~
k
,
q
~
k
+
1
,
θ
~
k
,
θ
~
k
+
1
).The real-time property is guaranteed by the computation sequences designed in the algorithm. The computation sequences are implemented by circulations. The algebraic equations are derived by computation of symbols at the beginning of circulations. For every time step, the integration is implemented by a substitution and solving the derived algebraic equations.Formation of nonholonomic vehicles is a hot and difficult research field [27–29], because the nonholonomic constrains make the variables of position and orientation highly coupled. According to the existing literature, the researches of formation of nonholonomic vehicles mainly concern the designing of controllers, such as the work in [30]. Moreover, because the nonholonomic system cannot be stabilized by smooth feedback control, sliding model control is introduced to design controller for steering motion of nonholonomic vehicles. Different surfaces in manifold of configuration space are defined and controllers in these surfaces are designed respectively [31]. Variables can achieve zeros at the same time in the defined surfaces. It is difficult to find the surfaces and corresponding controllers.The formation in potential field will result in potential forces on vehicles, which is equivalent to giving a controller. In order to be extended for situation of nonholonomic vehicles, the method can be changed as following ways. One way is to define complex potential fields. Different from the potential fields, respectively, defined by positions and orientations, the adaptive potential fields should be functionsV
i
(
q
,
θ
) and V
i
D
(
q
˙
,
θ
˙
). The potential functions can be derived from known controllers, such as first-order controllers in [30]. Another way is to define feedback coefficients of potential fields as follows:
(49)
k
i
j
(
t
)
=
k
i
j
(
q
(
t
)
,
θ
(
t
)
)
,
k
^
i
j
(
t
)
=
k
^
i
j
(
q
(
t
)
,
θ
(
t
)
)
,
v
i
j
(
t
)
=
v
i
j
(
q
˙
(
t
)
,
θ
˙
(
t
)
)
,
v
^
i
j
(
t
)
=
v
^
i
j
(
q
˙
(
t
)
,
θ
˙
(
t
)
)
.
The definition of such coefficients can be referred to the sliding model control which is a kind of feedback controller. This can help the potential field to form the control forces of positions and orientations, resulting in these variables to be stabilized in required trend. Both two kinds of ways will result in time-varying potential fields; then, the nonholonomic integrator can be used for the calculation [24].


### 3.3. Numerical Examples

Example 1.
Formation of three vehicles in safe environment is considered. The coefficients arem
i
=
100 kg and J
i
=
20 kg·m2. The matrix of desired distances l and K
,
K
^
,
v, and v
^ are chosen as
(50)
l
=
[
0
5
5
5
0
5
5
5
0
]
m
,
K
=
[
0
1000
1000
1000
0
1000
1000
1000
0
]
N
/
m
,
K
^
=
[
0
100
100
100
0
100
100
100
0
]
N
·
s
/
m
,
v
=
[
0
500
500
500
0
500
500
500
0
]
kg
·
m
2
/
s
2
,
v
^
=
[
0
50
50
50
0
50
50
50
0
]
kg
·
m
2
/
s
.Trajectory of the leader vehicle is given in a discrete form as(51)
x
k
+
1
1
=
x
k
1
+
v
0
h
,
y
k
+
1
1
=
y
k
1
+
v
0
cos
⁡
(
x
k
+
1
1
)
h
,
θ
k
+
1
1
=
f
θ
(
x
k
+
1
1
,
y
k
+
1
1
)
.
Here v
0
=
0.1 m/s is the speed of velocity and h
=
1 s is the time step. The function f
θ is given as
(52)
f
θ
(
x
k
+
1
1
,
y
k
+
1
1
)
=
{
arctan
(
y
k
+
1
1
x
k
+
1
1
)
x
k
+
1
1
≥
0
,
arctan
(
y
k
+
1
1
x
k
+
1
1
)
+
π
x
k
+
1
1
<
0
,
y
k
+
1
1
≥
0
,
arctan
(
y
k
+
1
1
x
k
+
1
1
)
-
π
x
k
+
1
1
<
0
,
y
k
+
1
1
<
0
.The initial conditions are chosen as shown in Table1.Table 1
Initial conditions of vehicles.
Label of vehiclei
x
0
i (m)
y
0
i (m)
1
9.33
50.0
2
5.00
47.5
3
5.00
52.5As shown in Figure4, it is a formation in safe environment. Vehicles can well preserve their formation by tracking leader in potential field. The positions and orientations of vehicles are coordinated, which makes the vehicles act as a fishes school. The results of simulation show that the formation method based on potential field is effective. The error dynamic system has been proved stable at the equilibrium point. For point away from the equilibrium point, the potential field will cause a big force to make the point move to the equilibrium point. For this situation, an upper limit of control force can be set. Therefore, the point can finally achieve the equilibrium point under conditions of reality limit. In a word, the vehicles in potential fields can achieve a desired formation and keep the formation. In reality applications, limit of control force can be added. It would not break the validity of this method.Figure 4
Formation of three vehicles in safe environment. The leader vehicle moves in a curve of trigonometric function.Example 2.
Formation of three vehicles in obstacle environment is considered. Obstacles are described in the form of{
(
30,40,4
,
8
)
,
(
45,58,4
,
8
)
,
(
70,40,4
,
8
)
} m as mentioned in Section 2.4. Figure 5 shows the formation motion in obstacle environment. The leader’s trajectory is the same to the trajectory used in Example 1. Figure 6 shows the vehicles’ motion with modified obstacle force. As indicated in Figure 7, erro
r
i
j which measures error to desired distance l
i
j shows that vehicles can well keep the relative positions.Figure 5
Formation of three vehicles in obstacle environment withk
l
=
2000 N/m. The out circles are safe bounds, stepping in which will cause obstacle forces.Figure 6
The obstacle force is used as the Lagrange force in computation of variational integrator. The variables of vehicles are solved together. Once, one of the vehicles steps in a safe bound of obstacle. The obstacle force will result in control forces on all the vehicles.Figure 7
Theerro
r
i
j means error to desired distance l
i
j between the ith and jth vehicles.As shown in Figure7, there were slight oscillations for variables erro
r
i
j. The oscillations were brought in for the following reason. The interactions between vehicles were described by potential field in form of stress function. The stress function can be written into a simple expression x
¨
=
-
k
(
x
-
x
*
), which is the motion equation of a free spring. Hence, the variables of vehicles in such potential field will have oscillations. The impact of oscillations will be decreased if the coefficients K
,
K
^
,
v, and v
^ were well chosen.


## 3.1. Variational Integrator

Variational integrator can be derived by applying discrete variational principle on discrete Lagrangian [16, 21]. Forced variational integrator is used for calculation of vehicles' motion.Given LagrangianL
(
q
,
q
˙
) and Lagrange force f
(
q
,
q
˙
), according to the Lagrange-d'Alembert principle:
(37)
δ
∫
0
T
L
(
q
(
t
)
,
q
˙
(
t
)
)
d
t
+
∫
0
T
f
(
q
(
t
)
,
q
˙
(
t
)
)
·
δ
q
(
t
)
d
t
=
0
,
we can get the forced Euler-Lagrange equation:
(38)
∂
L
∂
q
(
q
,
q
˙
)
-
d
d
t
(
∂
L
∂
q
˙
(
q
,
q
˙
)
)
+
f
(
q
,
q
˙
)
=
0
.
For the discrete vision, we define a discrete Lagrangian L
d
:
Q
×
Q
→
R,
(39)
L
d
(
q
k
,
q
k
+
1
)
=
∫
k
h
(
k
+
1
)
h
L
(
q
,
q
˙
)
d
t
.Applying discrete Lagrange-d'Alembert principle on discrete action functionℬ
d which is defined as ℬ
d
(
q
)
=
∑
k
=
0
N
-
1
L
d
(
q
k
,
q
k
+
1
), we can get the forced Euler-Lagrange equation in discrete form as
(40)
D
2
L
d
(
q
k
-
1
,
q
k
)
+
D
1
L
d
(
q
k
,
q
k
+
1
)
+
f
d
+
(
q
k
-
1
,
q
k
)
+
f
d
-
(
q
k
,
q
k
+
1
)
=
0
.
Here D
1 and D
2 stand for taking partial derivatives of L
d
(
q
k
-
1
,
q
k
) according to the variables’ position. f
d
+ and f
d
- are discrete Lagrange forces.Discrete Legendre transformations are maps connecting different discrete configuration spaces as shown in Figure3. They are defined based on (40) as follows:
(41)
F
-
L
d
u
k
(
q
k
,
q
k
+
1
)
=
-
D
1
L
d
(
q
k
,
q
k
+
1
)
-
f
d
-
(
q
k
,
q
k
+
1
)
,
F
+
L
d
u
k
(
q
k
,
q
k
+
1
)
=
D
2
L
d
(
q
k
,
q
k
+
1
)
+
f
d
+
(
q
k
,
q
k
+
1
)
.
The relationships of subscriptions can be referred to Figure 3.Figure 3
Commutable diagram based on discrete Legendre transformations [22]. It shows the operations of variational integrator in discrete configuration space.Definition 10.
Variational integrator is defined as a compound map of discrete Legendre transformations asF
L
d
u
k
-
1
,
u
k
:
(
q
k
-
1
,
q
k
)
↦
(
q
k
,
q
k
+
1
),
(42)
F
L
d
u
k
-
1
,
u
k
=
(
F
-
L
d
u
k
)
-
1
∘
F
+
L
d
u
k
-
1
.With mapF
L
d
u
k
-
1
,
u
k, vehicles' motion can be calculated. Each step, it maps (
q
k
-
1
,
q
k
) to (
q
k
,
q
k
+
1
). The calculation can be divided into two steps. The first step is to compute (
q
k
,
p
k
) from (
q
k
-
1
,
q
k
) by a substitution according to F
+
L
d
u
k
-
1, and the second step is to compute (
q
k
,
q
k
+
1
) from (
q
k
,
p
k
) by solving algebraic equations which is equivalent to the inverse map (
F
-
L
d
u
k
)
-
1.


## 3.2. Variational Integrator for Formation

In a formation withn vehicles, a leader vehicle is chosen to follow a given trajectory. Follower vehicles move in potential fields.States of theith vehicle include positions {
q
k
i
} and orientations {
θ
k
i
} with i
=
1,2
,
…
,
n. Both kinds of variables can be calculated by the variational integrator introduced as above. Different from the situation in continuous space, the labels for different vehicles are in the superscripts of discrete variables. According to the definition of variational integrator, we can calculate (
q
k
,
q
k
+
1
,
θ
k
,
θ
k
+
1
) from (
q
k
-
1
,
q
k
,
θ
k
-
1
,
θ
k
). In the rest of this paper, q
k
=
(
q
k
1
,
q
k
2
,
…
,
q
k
n
) and θ
k
=
(
θ
k
1
,
θ
k
2
,
…
,
θ
k
n
).The precision of algorithm depends on choice of numerical integrations. Without losing of universality, midpoint rule is chosen for instruction. The discrete Lagrange forces in midpoint rule are(43)
f
d
-
(
q
k
,
q
k
+
1
)
=
f
d
+
(
q
k
,
q
k
+
1
)
=
h
4
(
f
k
+
f
k
+
1
)
.
Here f
k means f
(
q
(
t
k
)
,
q
˙
(
t
k
)
) at node k [18].The discrete Lagrangian is(44)
L
d
(
q
k
,
q
k
+
1
,
θ
k
,
θ
k
+
1
)
=
∑
i
=
1
n
{
1
2
(
q
k
+
1
i
-
q
k
i
h
)
T
M
i
(
q
k
+
1
i
-
q
k
i
h
)
+
1
2
(
θ
k
+
1
i
-
θ
k
i
h
)
T
J
i
(
θ
k
+
1
i
-
θ
k
i
h
)
-
V
d
i
(
q
k
,
q
k
+
1
)
-
V
-
d
i
(
θ
k
,
θ
k
+
1
)
-
V
d
i
,
D
(
q
k
,
q
k
+
1
)
-
V
-
d
i
,
D
(
θ
k
,
θ
k
+
1
)
{
1
2
(
q
k
+
1
i
-
q
k
i
h
)
T
M
i
(
q
k
+
1
i
-
q
k
i
h
)
}
.
Here
(45)
V
d
i
(
q
k
,
q
k
+
1
)
=
V
i
(
q
k
+
q
k
+
1
2
)
,
V
-
d
i
(
θ
k
,
θ
k
+
1
)
=
V
-
i
(
θ
k
+
θ
k
+
1
2
)
,
V
d
i
,
D
(
q
k
,
q
k
+
1
)
=
V
i
D
(
q
k
+
1
-
q
k
h
)
,
V
-
d
i
,
D
(
θ
k
,
θ
k
+
1
)
=
V
-
i
D
(
θ
k
+
1
-
θ
k
h
)
.In the leader-follower formation, leader's trajectory{
(
q
k
1
,
θ
k
1
)
∣
k
=
1,2
,
…
,
N
} is given. And {
(
q
k
+
1
i
,
θ
k
+
1
i
)
∣
i
=
2,3
,
…
,
n
} are variables to be solved. Hence, we define the reduced variables q
~
k
=
(
q
k
2
,
q
k
3
,
…
,
q
k
n
) and θ
~
k
=
(
θ
k
2
,
θ
k
3
,
…
,
θ
k
n
).The reduced discrete Lagrange force is(46)
f
~
d
-
(
q
k
,
q
k
+
1
)
=
f
~
d
+
(
q
k
,
q
k
+
1
)
=
(
f
~
2
O
,
⊥
,
f
~
3
O
,
⊥
,
…
,
f
~
n
O
,
⊥
)
.
Here f
~
i
O
,
⊥
=
(
h
/
4
)
(
f
i
O
,
⊥
(
q
k
)
+
f
i
O
,
⊥
(
q
k
+
1
)
), i
=
2,3
,
…
,
n.Hence, we can have reduced discrete Legendre transformationsF
~
+
L
d
u
k
-
1 and F
~
-
L
d
u
k,
(47)
(
p
~
k
;
I
~
k
)
=
F
~
+
L
d
u
k
-
1
(
q
k
-
1
1
,
q
~
k
-
1
,
q
k
1
,
q
~
k
,
θ
k
-
1
1
,
θ
~
k
-
1
,
θ
k
1
,
θ
~
k
)
,
(
p
~
k
;
I
~
k
)
=
F
~
-
L
d
u
k
(
q
k
1
,
q
~
k
,
q
k
+
1
1
,
q
~
k
+
1
,
θ
k
1
,
θ
~
k
,
θ
k
+
1
1
,
θ
~
k
+
1
)
.
Here (
q
k
-
1
1
,
q
k
1
,
θ
k
-
1
1
,
θ
k
1
) are known variables used as inputs. p
~
k and I
~
k stand for the reduced linear momentum and reduced angular momentum in discrete space. They are calculated by substitution F
+
L
d
u
k
-
1. p
~
k is a 4
×
1 vector and I
~
k is a 2
×
1 vector.Substituting pairs of(
q
~
k
,
θ
~
k
) and (
p
~
k
,
I
~
k
) into F
~
-
L
d
u
k, we can obtain the algebraic equations
(48)
F
~
-
L
d
u
k
(
q
k
1
,
q
~
k
,
q
k
+
1
1
,
q
~
k
+
1
,
θ
k
1
,
θ
~
k
,
θ
k
+
1
1
,
θ
~
k
+
1
)
=
(
p
~
k
;
I
~
k
)
.
In (48), (
q
~
k
+
1
,
θ
~
k
+
1
) are unknown variables to be solved. Above processes make the followers’ motion be calculated from (
q
~
k
-
1
,
q
~
k
,
θ
~
k
-
1
,
θ
~
k
) to (
q
~
k
,
q
~
k
+
1
,
θ
~
k
,
θ
~
k
+
1
).The real-time property is guaranteed by the computation sequences designed in the algorithm. The computation sequences are implemented by circulations. The algebraic equations are derived by computation of symbols at the beginning of circulations. For every time step, the integration is implemented by a substitution and solving the derived algebraic equations.Formation of nonholonomic vehicles is a hot and difficult research field [27–29], because the nonholonomic constrains make the variables of position and orientation highly coupled. According to the existing literature, the researches of formation of nonholonomic vehicles mainly concern the designing of controllers, such as the work in [30]. Moreover, because the nonholonomic system cannot be stabilized by smooth feedback control, sliding model control is introduced to design controller for steering motion of nonholonomic vehicles. Different surfaces in manifold of configuration space are defined and controllers in these surfaces are designed respectively [31]. Variables can achieve zeros at the same time in the defined surfaces. It is difficult to find the surfaces and corresponding controllers.The formation in potential field will result in potential forces on vehicles, which is equivalent to giving a controller. In order to be extended for situation of nonholonomic vehicles, the method can be changed as following ways. One way is to define complex potential fields. Different from the potential fields, respectively, defined by positions and orientations, the adaptive potential fields should be functionsV
i
(
q
,
θ
) and V
i
D
(
q
˙
,
θ
˙
). The potential functions can be derived from known controllers, such as first-order controllers in [30]. Another way is to define feedback coefficients of potential fields as follows:
(49)
k
i
j
(
t
)
=
k
i
j
(
q
(
t
)
,
θ
(
t
)
)
,
k
^
i
j
(
t
)
=
k
^
i
j
(
q
(
t
)
,
θ
(
t
)
)
,
v
i
j
(
t
)
=
v
i
j
(
q
˙
(
t
)
,
θ
˙
(
t
)
)
,
v
^
i
j
(
t
)
=
v
^
i
j
(
q
˙
(
t
)
,
θ
˙
(
t
)
)
.
The definition of such coefficients can be referred to the sliding model control which is a kind of feedback controller. This can help the potential field to form the control forces of positions and orientations, resulting in these variables to be stabilized in required trend. Both two kinds of ways will result in time-varying potential fields; then, the nonholonomic integrator can be used for the calculation [24].


## 3.3. Numerical Examples

Example 1.
Formation of three vehicles in safe environment is considered. The coefficients arem
i
=
100 kg and J
i
=
20 kg·m2. The matrix of desired distances l and K
,
K
^
,
v, and v
^ are chosen as
(50)
l
=
[
0
5
5
5
0
5
5
5
0
]
m
,
K
=
[
0
1000
1000
1000
0
1000
1000
1000
0
]
N
/
m
,
K
^
=
[
0
100
100
100
0
100
100
100
0
]
N
·
s
/
m
,
v
=
[
0
500
500
500
0
500
500
500
0
]
kg
·
m
2
/
s
2
,
v
^
=
[
0
50
50
50
0
50
50
50
0
]
kg
·
m
2
/
s
.Trajectory of the leader vehicle is given in a discrete form as(51)
x
k
+
1
1
=
x
k
1
+
v
0
h
,
y
k
+
1
1
=
y
k
1
+
v
0
cos
⁡
(
x
k
+
1
1
)
h
,
θ
k
+
1
1
=
f
θ
(
x
k
+
1
1
,
y
k
+
1
1
)
.
Here v
0
=
0.1 m/s is the speed of velocity and h
=
1 s is the time step. The function f
θ is given as
(52)
f
θ
(
x
k
+
1
1
,
y
k
+
1
1
)
=
{
arctan
(
y
k
+
1
1
x
k
+
1
1
)
x
k
+
1
1
≥
0
,
arctan
(
y
k
+
1
1
x
k
+
1
1
)
+
π
x
k
+
1
1
<
0
,
y
k
+
1
1
≥
0
,
arctan
(
y
k
+
1
1
x
k
+
1
1
)
-
π
x
k
+
1
1
<
0
,
y
k
+
1
1
<
0
.The initial conditions are chosen as shown in Table1.Table 1
Initial conditions of vehicles.
Label of vehiclei
x
0
i (m)
y
0
i (m)
1
9.33
50.0
2
5.00
47.5
3
5.00
52.5As shown in Figure4, it is a formation in safe environment. Vehicles can well preserve their formation by tracking leader in potential field. The positions and orientations of vehicles are coordinated, which makes the vehicles act as a fishes school. The results of simulation show that the formation method based on potential field is effective. The error dynamic system has been proved stable at the equilibrium point. For point away from the equilibrium point, the potential field will cause a big force to make the point move to the equilibrium point. For this situation, an upper limit of control force can be set. Therefore, the point can finally achieve the equilibrium point under conditions of reality limit. In a word, the vehicles in potential fields can achieve a desired formation and keep the formation. In reality applications, limit of control force can be added. It would not break the validity of this method.Figure 4
Formation of three vehicles in safe environment. The leader vehicle moves in a curve of trigonometric function.Example 2.
Formation of three vehicles in obstacle environment is considered. Obstacles are described in the form of{
(
30,40,4
,
8
)
,
(
45,58,4
,
8
)
,
(
70,40,4
,
8
)
} m as mentioned in Section 2.4. Figure 5 shows the formation motion in obstacle environment. The leader’s trajectory is the same to the trajectory used in Example 1. Figure 6 shows the vehicles’ motion with modified obstacle force. As indicated in Figure 7, erro
r
i
j which measures error to desired distance l
i
j shows that vehicles can well keep the relative positions.Figure 5
Formation of three vehicles in obstacle environment withk
l
=
2000 N/m. The out circles are safe bounds, stepping in which will cause obstacle forces.Figure 6
The obstacle force is used as the Lagrange force in computation of variational integrator. The variables of vehicles are solved together. Once, one of the vehicles steps in a safe bound of obstacle. The obstacle force will result in control forces on all the vehicles.Figure 7
Theerro
r
i
j means error to desired distance l
i
j between the ith and jth vehicles.As shown in Figure7, there were slight oscillations for variables erro
r
i
j. The oscillations were brought in for the following reason. The interactions between vehicles were described by potential field in form of stress function. The stress function can be written into a simple expression x
¨
=
-
k
(
x
-
x
*
), which is the motion equation of a free spring. Hence, the variables of vehicles in such potential field will have oscillations. The impact of oscillations will be decreased if the coefficients K
,
K
^
,
v, and v
^ were well chosen.


## 4. Conclusions

In this paper, real-time formation is investigated by using potential field and variational integrator. A methodology for stability analysis is introduced. Potential fields with stress function and damping force are used to describe interactions of formation. The stability of a leader-follower formation of three vehicles in potential field is discussed by using error dynamic system. Transformation of coordinates from inertial frame to specific body frame is used to help the analysis of stable structure of formation. Since the freedom of system is not of full rank, reduced system is introduced by a specific substitution. The equilibrium point of reduced system is stable and the stability of original system is guaranteed according to the center manifold theorem. The calculation of motion is made by forced variational integrator which ensures an efficient computation. Integration is implemented by solving algebraic equations in every time step. Obstacle avoidance is guaranteed by modified obstacle force used in forming forced variational integrator. The simulation results show that our work has an agreeable performance in application.In future work, research will be focused on intelligent control based on work in this paper. Especially, the work on biological motion of fishes school where the leader of group might be changed according to a strategy for optimality. Food search of a group of fishes is such a situation.



---

*Source: 602424-2014-03-02.xml*