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

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.

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 step

In 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 of 𝑛 homological vehicles. The dynamic of the 𝑖th vehicle is 𝑞̇_𝑖=𝑝_𝑖𝑚_𝑖,𝑝̇_𝑖=𝑢_𝑖^tran,𝜃̇_𝑖=𝐼_𝑖𝐽_𝑖,𝐼̇_𝑖=𝑢^rota_𝑖. (1) Here 𝑚_𝑖 is mass and 𝐽_𝑖 is moment of inertia of the 𝑖th vehicle; 𝑞_𝑖 is the position and 𝜃_𝑖 is the orientation; 𝑝_𝑖 and 𝐼_𝑖 are linear momentum and angular momentum; 𝑢_𝑖^tran and 𝑢^rota_𝑖 are the controls, respectively.

The configuration space of 𝑛 vehicles is 𝑄=𝑅^2𝑛×𝑆¹×⋅⋅⋅×𝑆¹_𝑛, with Lagrangian of the 𝑖th vehicle 𝐿_𝑖∶𝑇𝑄→𝑅, 𝐿_𝑖̇𝜃̇=1𝑞,𝜃,𝑞,2𝑞̇^𝑇_𝑖𝑀_𝑖𝑞̇_𝑖+12𝜃̇^𝑇_𝑖𝐽_𝑖𝜃̇_𝑖−𝑉_𝑖𝑞−𝑉_𝑖(𝜃). (2) Here 𝑀_𝑖 is mass matrix of the 𝑖th vehicle and 𝑉_𝑖(𝑞) and 𝑉_𝑖(𝜃) are potential functions.

In the rest of this paper, we write 𝑞=(𝑞₁,𝑞₂,…,𝑞_𝑛) and 𝜃=(𝜃₁,𝜃₂,…,𝜃_𝑛). Denote 𝑒_𝑖𝑗=(𝑞_𝑖−𝑞_𝑗) and 𝜔_𝑖𝑗=(𝜃_𝑖−𝜃_𝑗). Formation preserving requires ‖𝑒_𝑖𝑗‖→𝑙_𝑖𝑗, ‖𝜔_𝑖𝑗‖→0, ‖𝑒̇_𝑖𝑗‖→0 and ‖𝜔̇_𝑖𝑗‖→0 where 𝑙_𝑖𝑗 is a desired distance between the 𝑖th and 𝑗th 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 the ith vehicle in formation is defined as 𝑉_𝑖𝑞=^𝑛_𝑗=1𝑉𝑞_𝑖,𝑞_𝑗1=−2^𝑛_𝑗=1𝑘_𝑖𝑗𝑞_𝑖−𝑞_𝑗−𝑙_𝑖𝑗², (3) with which 𝑉(𝑞_𝑖,𝑞_𝑗)=−(1/2)𝑘_𝑖𝑗(‖𝑞_𝑖−𝑞_𝑗‖−𝑙_𝑖𝑗)² and 𝑘_𝑖𝑗=𝐾𝑖,𝑗,𝑗≠𝑖,0,𝑗=𝑖. (4) Here 𝐾 is a symmetric matrix whose element 𝐾(𝑖,𝑗) is a constant or function and 𝐾(𝑖,𝑗)>0. The potential function 𝑉 is symmetric, which means that 𝑉(𝑞_𝑖,𝑞_𝑗)=𝑉(𝑞_𝑗,𝑞_𝑖).

Proposition 2. Potential force of the 𝑖th vehicle is 𝑓^𝑃_𝑖𝑞=−^𝑛_𝑗=1𝑘_𝑖𝑗𝑞_𝑖(𝑡)−𝑞_𝑗(𝑡)−𝑙_𝑖𝑗𝑞_𝑖(𝑡)−𝑞_𝑗(𝑡)𝑞_𝑖(𝑡)−𝑞_𝑗(𝑡.) (5)

Proof of Proposition 2. Assume that the Lagrangian of the 𝑖th vehicle in formation is 𝐿_𝑖(𝑞,𝑞)̇=(1/2)𝑞̇^𝑇_𝑖𝑀_𝑖𝑞̇_𝑖−𝑉_𝑖(𝑞). According to the Euler-Lagrange equation, a free system without external force satisfied 𝑓_𝑖=𝑀_𝑖𝑞̈_𝑖=𝜕𝑉_𝑖(𝑞)/𝜕𝑞_𝑖, which means 𝑓^𝑃_𝑖𝑞=𝜕𝑉_𝑖𝑞𝜕𝑞_𝑖=−^𝑛_𝑗=1𝑘_𝑖𝑗𝑞_𝑖−𝑞_𝑗−𝑙_𝑖𝑗𝜕𝑞_𝑖−𝑞_𝑗𝜕𝑞_𝑖. (6)
Consider the dot product of vectors. It holds 𝑞²=𝑞⋅𝑞=𝑞^𝑇𝑞=∑𝑞²_𝑖=(‖𝑞_𝑖‖)², and hence (‖𝑞_𝑖−𝑞_𝑗‖)²=(𝑞_𝑖−𝑞_𝑗)². Naturally it holds that 𝜕(‖𝑞_𝑖−𝑞_𝑗‖)²/𝜕𝑞_𝑖=𝜕(𝑞_𝑖−𝑞_𝑗)²/𝜕𝑞_𝑖, with which satisfying 𝜕𝑞_𝑖−𝑞_𝑗²𝜕𝑞_𝑖=2𝑞_𝑖−𝑞_𝑗𝜕𝑞_𝑖−𝑞_𝑗𝜕𝑞_𝑖,𝜕𝑞_𝑖−𝑞_𝑗²𝜕𝑞_𝑖=2𝑞_𝑖−𝑞_𝑗. (7) Therefore, it holds 𝜕(‖𝑞_𝑖−𝑞_𝑗‖)/𝜕𝑞_𝑖=(𝑞_𝑖−𝑞_𝑗)/‖𝑞_𝑖−𝑞_𝑗‖. Thus, potential force on the 𝑖th vehicles is 𝑓^𝑃_𝑖𝑞=−^𝑛_𝑗=1𝑘_𝑖𝑗𝑞_𝑖−𝑞_𝑗−𝑙_𝑖𝑗𝑞_𝑖−𝑞_𝑗𝑞_𝑖−𝑞_𝑗. (8)
Potential force between the 𝑖th and 𝑗th vehicles is 𝑓^𝑃_𝑖𝑗𝑞=−𝑘_𝑖𝑗𝑞_𝑖−𝑞_𝑗−𝑙_𝑖𝑗𝑞_𝑖−𝑞_𝑗𝑞_𝑖−𝑞_𝑗. (9)

Definition 3. Potential function of orientation of the 𝑖th vehicle is defined as 𝑉_𝑖(𝜃)=^𝑛_𝑗=1𝑉𝜃_𝑖,𝜃_𝑗1=−2^𝑛_𝑗=1𝑘_𝑖𝑗𝜃_𝑖−𝜃_𝑗², (10) with which 𝑉(𝜃_𝑖,𝜃_𝑗)=−(1/2)𝑘_𝑖𝑗(𝜃_𝑖−𝜃_𝑗)² and 𝑘_𝑖𝑗=𝐾𝑖,𝑗,𝑗≠𝑖,0,𝑗=𝑖. (11) Here 𝐾 is a symmetric matrix whose element 𝐾(𝑖,𝑗) is a constant or function and 𝐾(𝑖,𝑗)>0. Respectively, the potential force of orientation of the 𝑖th vehicle is 𝑓^𝑃_𝑖(𝜃)=−^𝑛_𝑗=1𝑘_𝑖𝑗𝜃_𝑖(𝑡)−𝜃_𝑗(𝑡). (12) System that has dynamic 𝑥̈=−𝑘(𝑥−𝑥^∗) will act as a spring. With potential functions {(𝑉_𝑖(𝑞),𝑉_𝑖(𝜃))∣𝑖=1,2,…,𝑛} defined above, vehicles can have coordinated motion as if there were springs connecting them. Since the spring system is periodically stable, damping force 𝑥̈=−𝑣𝑥̇ is considered for stabilization.

Definition 4. Damping forces of the 𝑖th vehicle are 𝑓^𝐷_𝑖𝑞̇=^𝑛_𝑗=1𝑓^𝐷_𝑖𝑗=−^𝑛_𝑗=1𝑣_𝑖𝑗𝑞̇_𝑖(𝑡)−𝑞̇_𝑗(𝑡),𝑓^𝐷_𝑖𝜃̇=^𝑛_𝑗=1𝑓^𝐷_𝑖𝑗=−^𝑛_𝑗=1𝑣̂_𝑖𝑗𝜃̇_𝑖(𝑡)−𝜃̇_𝑗(𝑡), (13) with 𝑓^𝐷_𝑖𝑗=−𝑣_𝑖𝑗(𝑞̇_𝑖−𝑞̇_𝑗),𝑣_𝑖𝑗≥0 and 𝑓^𝐷_𝑖𝑗=−𝑣̂_𝑖𝑗(𝜃̇_𝑖−𝜃̇_𝑗),𝑣̂_𝑖𝑗≥0.

Same as the potential function of stress function, the damping force has corresponding potential functions {(𝑉^𝐷_𝑖(𝑞)̇,𝑉^𝐷_𝑖(𝜃̇))∣𝑖=1,2,…,𝑛}, which are 𝑉^𝐷_𝑖𝑞̇=^𝑛_𝑗=1𝑉𝑞̇_𝑖,𝑞̇_𝑗1=−2^𝑛_𝑗=1𝑣_𝑖𝑗𝑞̇_𝑖−𝑞̇_𝑗²,𝑉^𝐷_𝑖𝜃̇=^𝑛_𝑗=1𝑉𝜃̇_𝑖,𝜃̇_𝑗1=−2^𝑛_𝑗=1𝑣̂_𝑖𝑗𝜃̇_𝑖−𝜃̇_𝑗². (14)

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 variable 𝑒_𝑖𝑗(𝑡)=𝑞_𝑖(𝑡)−𝑞_𝑗(𝑡). 𝑒_𝑖𝑗 is the relative position and ‖𝑒_𝑖𝑗‖ is the relative distance between the 𝑖th and 𝑗th vehicles. It holds 𝑒_𝑖𝑗(𝑡)=−𝑒_𝑗𝑖(𝑡), 𝑒_𝑖𝑖=0 and 𝑒̇_𝑖𝑗(𝑡)=𝑞̇_𝑖(𝑡)−𝑞̇_𝑗(𝑡). 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 by 𝑓_𝑖(𝑡)=𝑓^𝑃_𝑖(𝑞(𝑡))+𝑓^𝐷_𝑖(𝑞(̇𝑡)). According to Newton's law of motion, 𝑞_𝑖(𝑡) can be given by integration as follows: 𝑞_𝑖(𝑡)=𝑞_𝑖(0)+^𝑡₀𝑞̇_𝑖(𝜏)𝑑𝜏=𝑞_𝑖(0)+^𝑡₀𝑞̇_𝑖(0)+1𝑚^𝜏₀𝑓_𝑖(𝑠)𝑑𝑠𝑑𝜏=𝑞_𝑖(0)+𝑞̇_𝑖(0)1𝑡+𝑚^𝑡₀^𝜏₀𝑓_𝑖(𝑠)𝑑𝑠𝑑𝜏, (15) where 𝑞_𝑖(0) and 𝑞̇_𝑖(0) are constants.

Hence, it holds 𝑞̇_𝑖(𝑡)=𝑞̇_𝑖(0)+1𝑚^𝑡₀𝑓_𝑖(𝑠)𝑞̈𝑑𝑠,_𝑖(𝑡)=1𝑚𝑓_𝑖(𝑡)−1𝑚𝑓_𝑖(0). (16) Boundary condition vanishes as 𝑓_𝑖(0)=0. Second derivation of 𝑒_𝑖𝑗(𝑡) satisfies 𝑒̈_𝑖𝑗(𝑡)=𝑞̈_𝑖(𝑡)−𝑞̈_𝑗(𝑡)=1𝑚𝑓^𝑃_𝑖𝑞(𝑡)+𝑓^𝐷_𝑖𝑞̇(𝑡−1)𝑚𝑓^𝑃_𝑗𝑞(𝑡)+𝑓^𝐷_𝑗𝑞̇(𝑡)1=−𝑚^𝑛_𝑠=1𝑘_𝑖𝑠𝑞_𝑖(𝑡)−𝑞_𝑠(𝑡)−𝑙_𝑖𝑠𝑞_𝑖(𝑡)−𝑞_𝑠(𝑡)𝑞_𝑖(𝑡)−𝑞_𝑠(𝑡+_𝑛𝑠=1)𝑣_𝑖𝑠𝑞̇_𝑖−𝑞̇_𝑠+1𝑚^𝑛_𝑠=1𝑘_𝑗𝑠𝑞_𝑗(𝑡)−𝑞_𝑠(𝑡)−𝑙_𝑗𝑠𝑞_𝑗(𝑡)−𝑞_𝑠(𝑡)𝑞_𝑗(𝑡)−𝑞_𝑠(𝑡+_𝑛𝑠=1)𝑣_𝑗𝑠𝑞̇_𝑗−𝑞̇_𝑠=1𝑚^𝑛_𝑠=1𝑘_𝑖𝑠𝑙_𝑖𝑠𝑒_𝑖𝑠(𝑡𝑒)−1_𝑖𝑠(𝑡)−^𝑛_𝑠=1𝑘_𝑗𝑠𝑙_𝑗𝑠𝑒_𝑗𝑠(𝑡𝑒)−1_𝑗𝑠(𝑡)−1𝑚^𝑛_𝑠=1𝑣_𝑖𝑠𝑒̇_𝑖𝑠(𝑡)−^𝑛_𝑠=1𝑣_𝑗𝑠𝑒̇_𝑗𝑠(𝑡). (17)

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 𝑚 and coefficients 𝑘,𝑣. 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:𝑒̈₁₂𝑒̈₁₃𝑒̈₂₃𝑒̇₁₂𝑒̇₁₃𝑒̇₂₃=−𝑣𝑚0𝑣𝑚𝑘𝑚𝑙₁₂𝑒₁₂𝑘−10−𝑚𝑙₂₃𝑒₂₃𝑣−10−𝑚−𝑣𝑚0𝑘𝑚𝑙₁₃𝑒₁₃𝑘−1𝑚𝑙₂₃𝑒₂₃𝑣−1𝑚−𝑣𝑚−2𝑣𝑚−𝑘𝑚𝑙₁₂𝑒₁₂𝑘−1𝑚𝑙₁₃𝑒₁₃−12𝑘𝑚𝑙₂₃𝑒₂₃𝑒̇−1100000010000001000¹²𝑒̇₁₃𝑒̇₂₃𝑒₁₂𝑒₁₃𝑒₂₃. (18)By denoting 𝑋=(𝑒̇₁₂,𝑒̇₁₃,𝑒̇₂₃,𝑒₁₂,𝑒₁₃,𝑒₂₃)^𝑇, (18) can be written in form of 𝑋̇=𝑓(𝑋).

Secondly, we consider the equilibrium point and the stability of the above system. Since 𝑒̇_𝑖𝑗 and 𝑒_𝑖𝑗 are vectors, we need to define a projection function 𝑔(𝑋)∶𝑅^𝑚×⋅⋅⋅×𝑅^𝑚_𝑛/{𝟎}→𝑅^𝑛, 𝑔(𝑋)=𝑋₁,𝑋₂,…,𝑋_𝑛^𝑇,𝑋=𝑋₁,𝑋₂,…,𝑋_𝑛^𝑇,𝑋_𝑖∈𝑅^𝑚. (19)

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 𝑌^∗=𝑔(𝑋^∗)=(0,0,0,𝑙₁₂,𝑙₁₃,𝑙₂₃)^𝑇 for dynamic system 𝑋̇=𝑓(𝑋). Solutions 𝑋^∗ stand for stable structures.

Corollary 5. 𝑌^∗=(0,0,0,𝑙₁₂,𝑙₁₃,𝑙₂₃)^𝑇 is an adjoint equilibrium point of system 𝑋̇=𝑓(𝑋).

Proof of Corollary 5. If 𝑋^∗=(𝑒̇^∗₁₂,𝑒̇^∗₁₃,𝑒̇^∗₂₃,𝑒^∗₁₂,𝑒^∗₁₃,𝑒^∗₂₃)^𝑇 satisfies 𝑌^∗=𝑔(𝑋^∗)=(0,0,0,𝑙₁₂,𝑙₁₃,𝑙₂₃)^𝑇, it holds (‖𝑒̇^∗₁₂‖,‖𝑒̇^∗₁₃‖,‖𝑒̇^∗₂₃‖,‖𝑒^∗₁₂‖,‖𝑒^∗₁₃‖,‖𝑒^∗₂₃‖)^𝑇=(0,0,0,𝑙₁₂,𝑙₁₃,𝑙₂₃)^𝑇 and 𝑋̇^∗=𝑓(𝑋^∗)=(𝑒̈^∗₁₂,𝑒̈^∗₁₃,𝑒̈^∗₂₃,𝑒̇^∗₁₂,𝑒̇^∗₁₃,𝑒̇^∗₂₃,)^𝑇=𝟎. Hence 𝑌^∗=(0,0,0,𝑙₁₂,𝑙₁₃,𝑙₂₃)^𝑇 is an adjoint equilibrium point for dynamic system 𝑋̇=𝑓(𝑋).

Consider the Jacobian for stable structures. Denote 𝑒_𝑖𝑗=(𝑥_𝑖𝑗,𝑦_𝑖𝑗)^𝑇, while 𝑒̇_𝑖𝑗=(𝑥̇_𝑖𝑗,𝑦̇_𝑖𝑗)^𝑇, 𝑒̈_𝑖𝑗=(𝑥̈_𝑖𝑗,𝑦̈_𝑖𝑗)^𝑇. Compute the Jacobian matrix of 𝑓(𝑥) with the individual components:𝜕𝑓₁𝜕𝑋₁=𝜕𝑥̈₁₂,𝑦̈₁₂𝜕𝑥̇₁₂,𝑦̇₁₂=−𝑣𝑚0𝑣0−𝑚,𝜕𝑓₁𝜕𝑋₂=𝜕𝑥̈₁₂,𝑦̈₁₂𝜕𝑥̇₁₃,𝑦̇₁₃=𝟎_2×2,𝜕𝑓₁𝜕𝑋₃=𝜕𝑥̈₁₂,𝑦̈₁₂𝜕𝑥̇₂₃,𝑦̇₂₃=𝑣𝑚00𝑣𝑚,𝜕𝑓₁𝜕𝑋₄=𝜕𝑥̈₁₂,𝑦̈₁₂𝜕𝑥₁₂,𝑦₁₂=𝑘𝑚𝑙₁₂𝑦²₁₂𝑥²₁₂+𝑦²₁₂^(3/2)−𝑘−1𝑚𝑙₁₂𝑥₁₂𝑦₁₂𝑥²₁₂+𝑦²₁₂^(3/2)−𝑘𝑚𝑙₁₂𝑥₁₂𝑦₁₂𝑥²₁₂+𝑦²₁₂^(3/2)𝑘𝑚𝑙₁₂𝑥²₁₂𝑥²₁₂+𝑦²₁₂^(3/2),−1𝜕𝑓₁𝜕𝑋₅=𝜕𝑥̈₁₂,𝑦̈₁₂𝜕𝑥₁₃,𝑦₁₃=𝟎_2×2,𝜕𝑓₁𝜕𝑋₆=𝜕𝑥̈₁₂,𝑦̈₁₂𝜕𝑥₂₃,𝑦₂₃=−𝑘𝑚𝑙₂₃𝑦²₂₃𝑥²₂₃+𝑦²₂₃^(3/2)𝑘−1𝑚𝑙₂₃𝑥₂₃𝑦₂₃𝑥²₂₃+𝑦²₂₃^(3/2)𝑘𝑚𝑙₂₃𝑥₂₃𝑦₂₃𝑥²₂₃+𝑦²₂₃^(3/2)−𝑘𝑚𝑙₂₃𝑥²₂₃𝑥²₂₃+𝑦²₂₃^(3/2).−1 (20)

Use the same way to calculate the remaining parts. We can have the Jacobian matrix in form of (𝑥)=𝐈𝐷𝑓𝐴𝐵_6×60, (21) where −𝑣Α=𝑚𝑣000𝑚0𝑣0−𝑚𝑣000𝑚𝑣00−𝑚𝑣0−𝑚0𝑣000−𝑚𝑣0−𝑚𝑣𝑚𝑣0−𝑚0−2𝑣𝑚00𝑣𝑚𝑣0−𝑚0−2𝑣𝑚, (22)   𝑘Β=𝑚𝑙₁₂𝑦²₁₂𝑥²₁₂+𝑦²₁₂^(3/2)−𝑘−1𝑚𝑙₁₂𝑥₁₂𝑦₁₂𝑥²₁₂+𝑦²₁₂^(3/2)𝑘00−𝑚𝑙₂₃𝑦²₂₃𝑥²₂₃+𝑦²₂₃^(3/2)𝑘−1𝑚𝑙₂₃𝑥₂₃𝑦₂₃𝑥²₂₃+𝑦²₂₃^(3/2)−𝑘𝑚𝑙₁₂𝑥₁₂𝑦₁₂𝑥²₁₂+𝑦²₁₂^(3/2)𝑘𝑚𝑙₁₂𝑥²₁₂𝑥²₁₂+𝑦²₁₂^(3/2)𝑘−100𝑚𝑙₂₃𝑥₂₃𝑦₂₃𝑥²₂₃+𝑦²₂₃^(3/2)−𝑘𝑚𝑙₂₃𝑥²₂₃𝑥²₂₃+𝑦²₂₃^(3/2)𝑘−100𝑚𝑙₁₃𝑦²₁₃𝑥²₁₃+𝑦²₁₃^(3/2)−𝑘−1𝑚𝑙₁₃𝑥₁₃𝑦₁₃𝑥²₁₃+𝑦²₁₃^(3/2)𝑘𝑚𝑙₂₃𝑦²₂₃𝑥²₂₃+𝑦²₂₃^(3/2)−𝑘−1𝑚𝑙₂₃𝑥₂₃𝑦₂₃𝑥²₂₃+𝑦²₂₃^(3/2)𝑘00−𝑚𝑙₁₃𝑥₁₃𝑦₁₃𝑥²₁₃+𝑦²₁₃^(3/2)𝑘𝑚𝑙₁₃𝑥²₁₃𝑥²₁₃+𝑦²₁₃^(3/2)−𝑘−1𝑚𝑙₂₃𝑥₂₃𝑦₂₃𝑥²₂₃+𝑦²₂₃^(3/2)𝑘𝑚𝑙₂₃𝑥²₂₃𝑥²₂₃+𝑦²₂₃^(3/2)−𝑘−1𝑚𝑙₁₂𝑦²₁₂𝑥²₁₂+𝑦²₁₂^(3/2)𝑘−1𝑚𝑙₁₂𝑥₁₂𝑦₁₂𝑥²₁₂+𝑦²₁₂^(3/2)𝑘𝑚𝑙₁₃𝑦²₁₃𝑥²₁₃+𝑦²₁₃^(3/2)−𝑘−1𝑚𝑙₁₃𝑥₁₃𝑦₁₃𝑥²₁₃+𝑦²₁₃^(3/2)2𝑘𝑚𝑙₂₃𝑦²₂₃𝑥²₂₃+𝑦²₂₃^(3/2)𝑘−1−2𝑚𝑙₂₃𝑥₂₃𝑦₂₃𝑥²₂₃+𝑦²₂₃^(3/2)𝑘𝑚𝑙₁₂𝑥₁₂𝑦₁₂𝑥²₁₂+𝑦²₁₂^(3/2)−𝑘𝑚𝑙₁₂𝑥²₁₂𝑥²₁₂+𝑦²₁₂^(3/2)−𝑘−1𝑚𝑙₁₃𝑥₁₃𝑦₁₃𝑥²₁₃+𝑦²₁₃^(3/2)𝑘𝑚𝑙₁₃𝑥²₁₃𝑥²₁₃+𝑦²₁₃^(3/2)𝑘−1−2𝑚𝑙₂₃𝑥₂₃𝑦₂₃𝑥²₂₃+𝑦²₂₃^(3/2)2𝑘𝑚𝑙₂₃𝑥²₂₃𝑥²₂₃+𝑦²₂₃^(3/2).−1 (23) Since the adjoint equilibrium point is 𝑌^∗=(0,0,0,𝑙₁₂,𝑙₁₃,𝑙₂₃)^𝑇, we only know (𝑥²_𝑖𝑗+𝑦²_𝑖𝑗)=𝑙²_𝑖𝑗 instead of particular 𝑥_𝑖𝑗 and 𝑦_𝑖𝑗. 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 Figure 1, the body frame is constructed by setting the vehicle 1 on the origin and vehicle 2 on the negative 𝑥-axis. 𝜃 is the angle from negative 𝑥-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 the 𝑉𝑖 stands for 𝑖th vehicle. The right is an example of stable structures of formation in body frame. (𝑉∗1;𝑉∗2;𝑉∗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: 𝑥Translation∶^′𝑖=𝑥_𝑖−𝑥₁𝑦^′_𝑖=𝑦_𝑖−𝑦₁,𝑥Rotation∶^𝑖′′𝑦_𝑖^′′=cos𝜃sin𝜃𝑥−sin𝜃cos𝜃𝑦^′𝑖′_𝑖. (24)

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 𝑥₁^′′𝑦=0,_1′′𝑥=0,_2′′=−𝑥₂−𝑥₁²+𝑦₂−𝑦₁²,𝑦₂^′′𝑥=0,_3′′=𝑥₃−𝑥₁cos𝜃+𝑦₃−𝑦₁𝑦sin𝜃,_3′′=−𝑥₃−𝑥₁sin𝜃+𝑦₃−𝑦₁cos𝜃. (25) Since 𝑥₁^′′,𝑦₁^′′, and 𝑦₂^′′ vanish to zero, it has stable structure (0,0,−𝑙₁₂,0,(𝑥₃^′′)^∗,(𝑦₃^′′)^∗)^𝑇. The coordinates of 𝑥₃^′′ and 𝑦₃^′′ satisfy 𝑥₃^′′∗−0²+𝑦₃^′′∗−0²=𝑙²₁₃,𝑥₃^′′∗−−𝑙₁₂²+𝑦₃^′′∗−0²=𝑙²₂₃. (26) Its solutions are𝑥₃^′′∗𝑙=−²¹²+𝑙²₁₃−𝑙²₂₃2𝑙₁₂,𝑦₃^′′∗=±𝑙₁₂+𝑙₁₃−𝑙₂₃𝑙₁₂−𝑙₁₃+𝑙₂₃𝑙₁₃−𝑙₁₂+𝑙₂₃𝑙₁₂+𝑙₁₃+𝑙₂₃2𝑙₁₂. (27)Without losing generality, if we let 𝑙₁₂=𝑙₁₃=𝑙₂₃=𝑙, we can obtain 𝑥₃^′′∗𝑙=−2,𝑦₃^′′∗√=±32𝑙. (28)

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,−𝑙,0,−𝑙/2,(√3/2)𝑙)^𝑇is used for later computation.

Corollary 7. The error variables have seven freedoms in body frame.

Proof of Corollary 7. Error variable 𝑒^′′_𝑖𝑗=(𝑥^′′_𝑖𝑗,𝑦^′′_𝑖𝑗)^𝑇=(𝑥_𝑖^′′−𝑥_𝑗^′′,𝑦_𝑖^′′−𝑦_𝑗^′′)^𝑇. According to (25), the variables 𝑥₁^′′,𝑦₁^′′, and 𝑦₂^′′ equal zeros. Therefore, it holds 𝑦^′′₁₂𝑥=0,_′′23=−𝑥^′′₁₂+𝑥^′′₁₃=−𝑥₂−𝑥₁²+𝑦₂−𝑦₁²+𝑥₁−𝑥₃+𝑦cos𝜃₁−𝑦₃𝑦sin𝜃,_′′23=𝑦^′′₁₃=𝑥₃−𝑥₁sin𝜃−𝑦₃−𝑦₁cos𝜃. (29) Moreover, the error variables about velocities satisfy 𝑥̇^′′₂₃=𝑥̇₂^′′−𝑥̇₃^′′=𝑥̇^′′₁₂−𝑥̇^′′₁₃,𝑦̇^′′₂₃=𝑦̇₂^′′−𝑦̇₃^′′=𝑦̇^′′₁₂−𝑦̇^′′₁₃. (30) Therefore, error variables 𝑋=(𝑥̇^′′₁₂,𝑦̇^′′₁₂,𝑥̇^′′₁₃,𝑦̇^′′₁₃,𝑥̇^′′₂₃,𝑦̇^′′₂₃,𝑥^′′₁₂,𝑦^′′₁₂,𝑥^′′₁₃,𝑦^′′₁₃,𝑥^′′₂₃,𝑦^′′₂₃)^𝑇 have seven free elements as 𝑍=(𝑥̇^′′₁₂,𝑦̇^′′₁₂,𝑥̇^′′₁₃,𝑦̇^′′₁₃,𝑥^′′₁₂,𝑥^′′₁₃,𝑦^′′₁₃)^𝑇. Denoting 𝑌=(𝑥̇^′′₂₃,𝑦̇^′′₂₃,𝑦^′′₁₂,𝑥^′′₂₃,𝑦^′′₂₃)^𝑇, the error dynamic system can be rewritten as ̇𝑍=𝑔(𝑍,ℎ(𝑍)). Here 𝑌=ℎ(𝑍) is invariable manifold of the original system.

Corollary 8. The error dynamic system of formation is stable at the equilibrium point.

Proof of Corollary 8. 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, ̇𝑍=(𝑥̈^′′₁₂,𝑦̈^′′₁₂,𝑥̈^′′₁₃,𝑦̈^′′₁₃,𝑥̇^′′₁₂,𝑥̇^′′₁₃,𝑦̇^′′₁₃)^𝑇. The elements of ̇𝑍 are functions of 𝑋. Substitute 𝑥̇^′′₂₃,𝑦̇^′′₂₃,𝑦^′′₁₂,𝑥^′′₂₃, and 𝑦^′′₂₃ 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 𝑍=𝑣𝐷𝑔−2𝑚0𝑣𝑚0𝑘𝑚𝑙₂₃𝑦^′′₁₃²𝑥^′′₁₂−𝑥^′′₁₃²+𝑦^′′₁₃^23/2−𝑘−2𝑚𝑙₂₃𝑦^′′₁₃²𝑥^′′₁₂−𝑥^′′₁₃²+𝑦^′′₁₃^23/2−𝑘−1𝑚𝑙₂₃𝑦^′′₁₃𝑥^′′₁₂−𝑥^′′₁₃𝑥^′′₁₂−𝑥^′′₁₃²+𝑦^′′₁₃^23/2𝑣0−2𝑚0𝑣𝑚𝑘𝑚𝑙₂₃𝑦^′′₁₃𝑥^′′₁₂−𝑥^′′₁₃𝑥^′′₁₂−𝑥^′′₁₃²+𝑦^′′₁₃^23/2−𝑘𝑚𝑙₂₃𝑦^′′₁₃𝑥^′′₁₂−𝑥^′′₁₃𝑥^′′₁₂−𝑥^′′₁₃²+𝑦^′′₁₃^23/2𝑘𝑚𝑙₂₃𝑦^′′₁₃²𝑥^′′₁₂−𝑥^′′₁₃²+𝑦^′′₁₃^23/2−𝑙₂₃𝑥^′′₁₂−𝑥^′′₁₃²+𝑦^′′₁₃^21/2𝑣+1𝑚𝑣0−2𝑚𝑘0−𝑚𝑙₂₃𝑦^′′₁₃²𝑥^′′₁₂−𝑥^′′₁₃²+𝑦^′′₁₃^23/2𝑘−1𝑚𝑙₁₃𝑦^′′₁₃²𝑥^′′₁₃²+𝑦^′′₁₃^23/2+𝑙₂₃𝑦^′′₁₃²𝑥^′′₁₂−𝑥^′′₁₃²+𝑦^′′₁₃^23/2𝑘−2𝑚𝑙₂₃𝑦^′′₁₃𝑥^′′₁₂−𝑥^′′₁₃𝑥^′′₁₂−𝑥^′′₁₃²+𝑦^′′₁₃^23/2−𝑙₁₃𝑥^′′₁₃𝑦^′′₁₃𝑥^′′₁₃²+𝑦^′′₁₃^23/20𝑣𝑚𝑣0−2𝑚−𝑘𝑚𝑙₂₃𝑦^′′₁₃𝑥^′′₁₂−𝑥^′′₁₃𝑥^′′₁₂−𝑥^′′₁₃²+𝑦^′′₁₃^23/2𝑘𝑚𝑙₂₃𝑦^′′₁₃𝑥^′′₁₂−𝑥^′′₁₃𝑥^′′₁₂−𝑥^′′₁₃²+𝑦^′′₁₃^23/2−𝑙₁₃𝑥^′′₁₃𝑦^′′₁₃𝑥^′′₁₃²+𝑦^′′₁₃^23/2𝑘𝑚𝑙₁₃𝑥^′′₁₃²𝑥^′′₁₃²+𝑦^′′₁₃^23/2+𝑙₂₃𝑥^′′₁₂²+𝑙₂₃𝑥^′′₁₃²−2𝑙₂₃𝑥^′′₁₂𝑥^′′₁₃𝑥^′′₁₂−𝑥^′′₁₃²+𝑦^′′₁₃^23/2.−2100000000100000001000 (31)

Naturally, it holds 𝑥^′′₁₂^∗,𝑦^′′₁₂^∗,𝑥^′′₁₃^∗,𝑦^′′₁₃^∗,𝑥^′′₂₃^∗,𝑦^′′₂₃^∗𝑇=()−𝑙0−−𝑙,0−0,0−2,√0−32𝑙,()−−𝑙−𝑙2√,0−32𝑙^𝑇=𝑙𝑙,0,2,√32𝑙𝑙,−2√,−32𝑙^𝑇 (32) and ((𝑥̇^′′₁₂)^∗,(𝑦̇^′′₁₂)^∗,(𝑥̇^′′₁₃)^∗,(𝑦̇^′′₁₃)^∗,(𝑥̇^′′₂₃)^∗,(𝑦̇^′′₂₃)^∗)^𝑇=(0,0,0,0,0,0)^𝑇. Hence, the equilibrium point is 𝑋^∗=(0,0,0,0,0,0,𝑙,0,𝑙/2,(√3/2)𝑙,−𝑙/2,−(√3/2)𝑙)^𝑇for original system. And the equilibrium point of the reduced system is 𝑍^∗=(0,0,0,0,𝑙,𝑙/2,(√3/2)𝑙)^𝑇.

The Jacobian matrix of reduced system at equilibrium point is 𝐷𝑔𝑍^∗=𝑣−2𝑚0𝑣𝑚50−4𝑘𝑚14𝑘𝑚√34𝑘𝑚𝑣0−2𝑚0𝑣𝑚−√34𝑘𝑚√34𝑘𝑚34𝑘𝑚𝑣𝑚𝑣0−2𝑚014𝑘𝑚−12𝑘𝑚00𝑣𝑚𝑣0−2𝑚√34𝑘𝑚30−2𝑘𝑚.100000000100000001000 (33)

The Jacobian 𝐷𝑔(𝑍^∗) can be proved to have multiple conjugate eigenvalues with all real parts negative (𝑘≥0,𝑣≥0,𝑚≥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 𝑘,𝑣, and 𝑚, 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 {(𝑞_𝑙,𝑅_𝑙,𝑘_𝑙)∣𝑙=1,2,…,𝑚}. Here 𝑞_𝑙 is position of center of the 𝑙th obstacle, 𝑅_𝑙 is safe bound, and 𝑘_𝑙 is coefficient for obstacle force. A classical obstacle force is introduced in [15] as follows.

Definition 9. Obstacle force between the 𝑖th vehicle and the 𝑙th obstacle is defined as 𝑓^𝑂_𝑖𝑙𝑞=𝑘_𝑙𝑑_𝑖𝑙−𝑘_𝑙𝑅_𝑙𝑑_𝑖𝑙𝑑_𝑖𝑙𝑑_𝑖𝑙<𝑅_𝑙,0otherwise. (34) Here 𝑑_𝑖𝑙=(𝑞_𝑖−𝑞_𝑙).

The obstacle force on the 𝑖th vehicle is 𝑓^𝑂_𝑖𝑞=^𝑚_𝑙=1𝑓^𝑂_𝑖𝑙𝑞. (35) The obstacle force introduced in Definition 9 can be decomposed into two orthogonal components as 𝑓^𝑂_𝑖=𝑓_𝑖^𝑂,ℎ+𝑓_𝑖^𝑂,⟂. (36) Here 𝑓_𝑖^𝑂,ℎ is a force along the direction of velocity which can change the speed of velocity, while 𝑓_𝑖^𝑂,⟂ can change the direction of velocity. Here, 𝑓_𝑖^𝑂,⟂ 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. 𝑅𝑙 is safe bound and 𝑑𝑖𝑙 is distance between the 𝑖th vehicle and the 𝑙th 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 Lagrangian 𝐿(𝑞,𝑞)̇ and Lagrange force 𝑓(𝑞,𝑞)̇, according to the Lagrange-d'Alembert principle: 𝛿^𝑇₀𝐿𝑞(𝑡),𝑞̇(𝑡)𝑑𝑡+^𝑇₀𝑓𝑞(𝑡),𝑞̇(𝑡)⋅𝛿𝑞(𝑡)𝑑𝑡=0, (37) we can get the forced Euler-Lagrange equation: 𝜕𝐿𝑞̇−𝑑𝜕𝑞𝑞,𝑑𝑡𝜕𝐿𝜕𝑞̇𝑞,𝑞̇+𝑓𝑞,𝑞̇=0. (38) For the discrete vision, we define a discrete Lagrangian 𝐿_𝑑∶𝑄×𝑄→𝑅, 𝐿_𝑑𝑞_𝑘,𝑞_𝑘+1=^(𝑘+1)ℎ_𝑘ℎ𝐿𝑞,𝑞̇𝑑𝑡. (39)

Applying discrete Lagrange-d'Alembert principle on discrete action function ℬ_𝑑 which is defined as ℬ_𝑑(𝑞)=∑^𝑁−1_𝑘=0𝐿_𝑑(𝑞_𝑘,𝑞_𝑘+1), we can get the forced Euler-Lagrange equation in discrete form as 𝐷₂𝐿_𝑑𝑞_𝑘−1,𝑞_𝑘+𝐷₁𝐿_𝑑𝑞_𝑘,𝑞_𝑘+1+𝑓⁺_𝑑𝑞_𝑘−1,𝑞_𝑘+𝑓⁻_𝑑𝑞_𝑘,𝑞_𝑘+1=0. (40) Here 𝐷₁ and 𝐷₂ stand for taking partial derivatives of 𝐿_𝑑(𝑞_𝑘−1,𝑞_𝑘) according to the variables’ position. 𝑓⁺_𝑑 and 𝑓⁻_𝑑 are discrete Lagrange forces.

Discrete Legendre transformations are maps connecting different discrete configuration spaces as shown in Figure 3. They are defined based on (40) as follows: 𝐹⁻𝐿^𝑢𝑘_𝑑𝑞_𝑘,𝑞_𝑘+1=−𝐷₁𝐿_𝑑𝑞_𝑘,𝑞_𝑘+1−𝑓⁻_𝑑𝑞_𝑘,𝑞_𝑘+1,𝐹⁺𝐿^𝑢𝑘_𝑑𝑞_𝑘,𝑞_𝑘+1=𝐷₂𝐿_𝑑𝑞_𝑘,𝑞_𝑘+1+𝑓⁺_𝑑𝑞_𝑘,𝑞_𝑘+1. (41) 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 as 𝐹^{𝑢𝑘−1,𝑢𝑘}_𝐿𝑑∶(𝑞_𝑘−1,𝑞_𝑘)↦(𝑞_𝑘,𝑞_𝑘+1), 𝐹^{𝑢𝑘−1,𝑢𝑘}_𝐿𝑑=𝐹⁻𝐿^𝑢𝑘_𝑑⁻¹∘𝐹⁺𝐿^𝑢𝑘−1_𝑑. (42)

With map 𝐹^{𝑢𝑘−1,𝑢𝑘}_𝐿𝑑, vehicles' motion can be calculated. Each step, it maps (𝑞_𝑘−1,𝑞_𝑘) to (𝑞_𝑘,𝑞_𝑘+1). The calculation can be divided into two steps. The first step is to compute (𝑞_𝑘,𝑝_𝑘) from (𝑞_𝑘−1,𝑞_𝑘) by a substitution according to 𝐹⁺𝐿^𝑢𝑘−1_𝑑, and the second step is to compute (𝑞_𝑘,𝑞_𝑘+1) from (𝑞_𝑘,𝑝_𝑘) by solving algebraic equations which is equivalent to the inverse map (𝐹⁻𝐿^𝑢𝑘_𝑑)⁻¹.

3.2. Variational Integrator for Formation

In a formation with 𝑛 vehicles, a leader vehicle is chosen to follow a given trajectory. Follower vehicles move in potential fields.

States of the 𝑖th vehicle include positions {𝑞^𝑖_𝑘} and orientations {𝜃^𝑖_𝑘} with 𝑖=1,2,…,𝑛. 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 (𝑞_𝑘,𝑞_𝑘+1,𝜃_𝑘,𝜃_𝑘+1) from (𝑞_𝑘−1,𝑞_𝑘,𝜃_𝑘−1,𝜃_𝑘). In the rest of this paper, 𝑞_𝑘=(𝑞¹_𝑘,𝑞²_𝑘,…,𝑞^𝑛_𝑘) and 𝜃_𝑘=(𝜃¹_𝑘,𝜃²_𝑘,…,𝜃^𝑛_𝑘).

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 𝑓⁻_𝑑𝑞_𝑘,𝑞_𝑘+1=𝑓⁺_𝑑𝑞_𝑘,𝑞_𝑘+1=ℎ4𝑓_𝑘+𝑓_𝑘+1. (43) Here 𝑓_𝑘 means 𝑓(𝑞(𝑡_𝑘),𝑞(̇𝑡_𝑘)) at node 𝑘 [18].

The discrete Lagrangian is 𝐿_𝑑𝑞_𝑘,𝑞_𝑘+1,𝜃_𝑘,𝜃_𝑘+1=^𝑛_𝑖=112𝑞^𝑖_𝑘+1−𝑞^𝑖_𝑘ℎ^𝑇𝑀_𝑖𝑞^𝑖_𝑘+1−𝑞^𝑖_𝑘ℎ+12𝜃^𝑖_𝑘+1−𝜃^𝑖_𝑘ℎ^𝑇𝐽_𝑖𝜃^𝑖_𝑘+1−𝜃^𝑖_𝑘ℎ−𝑉^𝑖_𝑑𝑞_𝑘,𝑞_𝑘+1−𝑉^𝑖_𝑑𝜃_𝑘,𝜃_𝑘+1−𝑉_𝑑^𝑖,𝐷𝑞_𝑘,𝑞_𝑘+1−𝑉_𝑑^𝑖,𝐷𝜃_𝑘,𝜃_𝑘+1. (44) Here 𝑉^𝑖_𝑑𝑞_𝑘,𝑞_𝑘+1=𝑉_𝑖𝑞_𝑘+𝑞_𝑘+12,𝑉^𝑖_𝑑𝜃_𝑘,𝜃_𝑘+1=𝑉_𝑖𝜃_𝑘+𝜃_𝑘+12,𝑉_𝑑^𝑖,𝐷𝑞_𝑘,𝑞_𝑘+1=𝑉^𝐷_𝑖𝑞_𝑘+1−𝑞_𝑘ℎ,𝑉_𝑑^𝑖,𝐷𝜃_𝑘,𝜃_𝑘+1=𝑉^𝐷_𝑖𝜃_𝑘+1−𝜃_𝑘ℎ. (45)

In the leader-follower formation, leader's trajectory {(𝑞¹_𝑘,𝜃¹_𝑘)∣𝑘=1,2,…,𝑁} is given. And {(𝑞^𝑖_𝑘+1,𝜃^𝑖_𝑘+1)∣𝑖=2,3,…,𝑛} are variables to be solved. Hence, we define the reduced variables 𝑞_𝑘=(𝑞²_𝑘,𝑞³_𝑘,…,𝑞^𝑛_𝑘) and 𝜃_𝑘=(𝜃²_𝑘,𝜃³_𝑘,…,𝜃^𝑛_𝑘).

The reduced discrete Lagrange force is 𝑓⁻_𝑑𝑞_𝑘,𝑞_𝑘+1=𝑓⁺_𝑑𝑞_𝑘,𝑞_𝑘+1=𝑓₂^𝑂,⟂,𝑓₃^𝑂,⟂,…,𝑓_𝑛^𝑂,⟂. (46) Here 𝑓_𝑖^𝑂,⟂=(ℎ/4)(𝑓_𝑖^𝑂,⟂(𝑞_𝑘)+𝑓_𝑖^𝑂,⟂(𝑞_𝑘+1)), 𝑖=2,3,…,𝑛.

Hence, we can have reduced discrete Legendre transformations 𝐹⁺𝐿^𝑢𝑘−1_𝑑 and 𝐹⁻𝐿^𝑢𝑘_𝑑, 𝑝_𝑘;𝐼_𝑘̃=𝐹⁺𝐿^𝑢𝑘−1_𝑑𝑞¹_𝑘−1,𝑞_𝑘−1,𝑞¹_𝑘,𝑞_𝑘,𝜃¹_𝑘−1,𝜃_𝑘−1,𝜃¹_𝑘,𝜃_𝑘,𝑝_𝑘;𝐼_𝑘̃=𝐹⁻𝐿^𝑢𝑘_𝑑𝑞¹_𝑘,𝑞_𝑘,𝑞¹_𝑘+1,𝑞_𝑘+1,𝜃¹_𝑘,𝜃_𝑘,𝜃¹_𝑘+1,𝜃_𝑘+1. (47) Here (𝑞¹_𝑘−1,𝑞¹_𝑘,𝜃¹_𝑘−1,𝜃¹_𝑘) are known variables used as inputs. 𝑝_𝑘 and 𝐼_𝑘̃ stand for the reduced linear momentum and reduced angular momentum in discrete space. They are calculated by substitution 𝐹⁺𝐿^𝑢𝑘−1_𝑑. 𝑝_𝑘 is a 4×1 vector and 𝐼_𝑘̃ is a 2×1 vector.

Substituting pairs of (𝑞_𝑘,𝜃_𝑘) and (𝑝_𝑘,𝐼_𝑘̃) into 𝐹⁻𝐿^𝑢𝑘_𝑑, we can obtain the algebraic equations 𝐹⁻𝐿^𝑢𝑘_𝑑𝑞¹_𝑘,𝑞_𝑘,𝑞¹_𝑘+1,𝑞_𝑘+1,𝜃¹_𝑘,𝜃_𝑘,𝜃¹_𝑘+1,𝜃_𝑘+1=𝑝_𝑘;𝐼_𝑘̃. (48) In (48), (𝑞_𝑘+1,𝜃_𝑘+1) are unknown variables to be solved. Above processes make the followers’ motion be calculated from (𝑞_𝑘−1,𝑞_𝑘,𝜃_𝑘−1,𝜃_𝑘) to (𝑞_𝑘,𝑞_𝑘+1,𝜃_𝑘,𝜃_𝑘+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 functions 𝑉_𝑖(𝑞,𝜃) and 𝑉^𝐷_𝑖(𝑞,̇𝜃̇). 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) 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 are 𝑚_𝑖=100 kg and 𝐽_𝑖=20 kg⋅m². The matrix of desired distances 𝑙 and 𝐾,𝐾,𝑣, and 𝑣̂ are chosen as 𝑙=055505550m,𝐾=010001000100001000100010000N/m,𝐾=010010010001001001000N⋅s/m,𝑣=050050050005005005000kg⋅m²/s²,𝑣̂=050505005050500kg⋅m²/s. (50)

Trajectory of the leader vehicle is given in a discrete form as 𝑥¹_𝑘+1=𝑥¹_𝑘+𝑣₀𝑦ℎ,_1𝑘+1=𝑦¹_𝑘+𝑣₀cos𝑥¹_𝑘+1𝜃ℎ,_1𝑘+1=𝑓_𝜃𝑥¹_𝑘+1,𝑦¹_𝑘+1. (51) Here 𝑣₀=0.1 m/s is the speed of velocity and ℎ=1 s is the time step. The function 𝑓_𝜃 is given as 𝑓_𝜃𝑥¹_𝑘+1,𝑦¹_𝑘+1=𝑦arctan^1𝑘+1𝑥¹_𝑘+1𝑥¹_𝑘+1𝑦≥0,arctan^1𝑘+1𝑥¹_𝑘+1+𝜋𝑥¹_𝑘+1<0,𝑦¹_𝑘+1𝑦≥0,arctan^1𝑘+1𝑥¹_𝑘+1−𝜋𝑥¹_𝑘+1<0,𝑦¹_𝑘+1<0. (52)

The initial conditions are chosen as shown in Table 1.

Table 1 

Initial conditions of vehicles.

As shown in Figure 4, 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, error_𝑖𝑗 which measures error to desired distance 𝑙_𝑖𝑗 shows that vehicles can well keep the relative positions.

Figure 5 

Formation of three vehicles in obstacle environment with 𝑘𝑙=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 

The error𝑖𝑗 means error to desired distance 𝑙𝑖𝑗 between the 𝑖th and 𝑗th vehicles.

As shown in Figure 7, there were slight oscillations for variables error_𝑖𝑗. 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 𝑥̈=−𝑘(𝑥−𝑥^∗), 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 𝐾,𝐾,𝑣, and 𝑣̂ 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.

Conflict of Interests

The authors declare that there is no conflict of interests regarding the publication of this paper.

Acknowledgment

This work was supported by a Grant from the National Natural Science Foundation of China (no. 61350010).