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

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. Ourmethodology 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.


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][4][5][6][7][8][9][10][11][12][13][14][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][4][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][4][5][6][7] can be seen as formation's framework on vehicles' dynamic.Motion 2 Mathematical Problems in Engineering 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][17][18][19][20][21][22][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 realtime 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 leaderfollower 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.

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.

Formation of Vehicles.
Consider a formation of  homological vehicles.The dynamic of the th vehicle is 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 Here   is mass matrix of the th vehicle and   () and   () are potential functions.

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 Therefore, it holds (‖  −   ‖)/  = (  −   )/‖  −   ‖.Thus, potential force on the th vehicles is Potential force between the th and th vehicles is Definition 3. Potential function of orientation of the th vehicle is defined as with which V(  ,   ) = −(1/2) k (  −   ) 2 and k = { K (, ) ,  ̸ = , 0,  = .
Here K is a symmetric matrix whose element K(, ) is a constant or function and K(, ) > 0. Respectively, the potential force of orientation of the th vehicle is System that has dynamic ẍ = −( −  * ) will act as a spring.

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 ė  () = q  () − q  ().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   () =    (()) +    ( q ()).According to Newton's law of motion,   () can be given by integration as follows: where   (0) and q  (0) are constants.Hence, it holds Boundary condition vanishes as 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 , 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: By denoting  = ( ė 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,  12 ,  13 ,  23 )  for dynamic system Ẋ = ().Solutions  * stand for stable structures.
Use the same way to calculate the remaining parts.We can have the Jacobian matrix in form of where Since the adjoint equilibrium point is  * = (0, 0, 0,  12 ,  13 ,  23 )  , we only know ( 2  +  2  ) =  2  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.

Inertial frame
Body frame 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.Definition 6.General transformation of coordinates from inertial frame to body frame needs translation and rotation as follows: 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 Since   1 ,   1 , and   2 vanish to zero, it has stable structure (0, 0, − 12 , 0, (  3 ) * , (  3 ) * )  .The coordinates of   3 and   3 satisfy Its solutions are Without losing generality, if we let  12 =  13 =  23 = , we can obtain 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.
The Jacobian matrix of reduced system at equilibrium point is The Jacobian ( Z * ) can be proved to have multiple conjugate eigenvalues with all real parts negative ( ≥ 0, V ≥ 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 Mathematical Problems in Engineering ith vehicle lth obstacle five has no analytical solutions, numerical computations are usually considered instead.By giving particular values of , V, 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.
The obstacle force on the th vehicle is The obstacle force introduced in Definition 9 can be decomposed into two orthogonal components as 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.

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.

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 (, q ) and Lagrange force (, q ), according to the Lagrange-d' Alembert principle: we can get the forced Euler-Lagrange equation: For the discrete vision, we define a discrete Lagrangian   :  ×  → , Applying discrete Lagrange-d' Alembert principle on discrete action function B  which is defined as B  () = ∑ −1 =0   (  ,  +1 ), we can get the forced Euler-Lagrange equation in discrete form as Here  1 and  2 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: The relationships of subscriptions can be referred to Figure 3.
Definition 10.Variational integrator is defined as a compound map of discrete Legendre transformations as  With map , 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

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.
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][28][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    ( 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: 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].Trajectory of the leader vehicle is given in a discrete form as

Numerical Examples
Here V 0 = 0.1 m/s is the speed of velocity and ℎ = 1 s is the time step.The function   is given as The initial conditions are chosen as shown in Table 1.
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.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.
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 , K, V, and V were well chosen.

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.

Figure 1 :
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.

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.

Figure 3 :
Figure 3: Commutable diagram based on discrete Legendre transformations [22].It shows the operations of variational integrator in discrete configuration space.

Example 1 .kg ⋅ m 2 /s 2 ,kg ⋅ m 2
Formation of three vehicles in safe environment is considered.The coefficients are   = 100 kg and   = 20 kg⋅m 2 .The matrix of desired distances  and , K, V, and V are chosen as /s.

Figure 4 :
Figure 4: Formation of vehicles in safe environment.The leader vehicle moves in a curve of trigonometric function.

Figure 5 :Figure 6 :
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 7 :
Figure 7: The error  means error to desired distance   between the th and th vehicles.

Table 1 :
Initial conditions of vehicles.