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. 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.
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 . 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 . 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 . 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. . 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 . Cortés introduced the notion of a stress function for formation control from which we derive a Lagrangian . 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 . 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 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; and are the controls, respectively.
The configuration space of vehicles is , with Lagrangian of the th vehicle , 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 , , and 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 . 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 with which and Here is a symmetric matrix whose element is a constant or function and . The potential function is symmetric, which means that .
Proposition 2. Potential force of the th vehicle is
Proof of Proposition 2. Assume that the Lagrangian of the th vehicle in formation is . According to the Euler-Lagrange equation, a free system without external force satisfied , which means
Consider the dot product of vectors. It holds , and hence . Naturally it holds that , with which satisfying 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 and Here is a symmetric matrix whose element is a constant or function and . Respectively, the potential force of orientation of the th vehicle is System that has dynamic will act as a spring. With potential functions 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 with and .
Same as the potential function of stress function, the damping force has corresponding potential functions , which are
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 , 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: where and are constants.
Hence, it holds Boundary condition vanishes as . Second derivation of satisfies
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: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 ,
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 . In our system, we have an adjoint equilibrium point for dynamic system . Solutions stand for stable structures.
Corollary 5. is an adjoint equilibrium point of system .
Proof of Corollary 5. If satisfies , it holds and . Hence 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:
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 , 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.
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 , and vanish to zero, it has stable structure . The coordinates of and satisfy Its solutions areWithout losing generality, if we let , 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 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 Moreover, the error variables about velocities satisfy 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.
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
Naturally, it holds and . Hence, the equilibrium point is for original system. And the equilibrium point of the reduced system is .
The Jacobian matrix of reduced system at equilibrium point is
The Jacobian can be proved to have multiple conjugate eigenvalues with all real parts negative . 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 . 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  as follows.
Definition 9. Obstacle force between the th vehicle and the th obstacle is defined as Here .
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.
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
Given Lagrangian and Lagrange force , 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 which is defined as , we can get the forced Euler-Lagrange equation in discrete form as Here and stand for taking partial derivatives of 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 to . The calculation can be divided into two steps. The first step is to compute from by a substitution according to , and the second step is to compute 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 . 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 from . 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 Here means at node .
The discrete Lagrangian is Here
In the leader-follower formation, leader's trajectory is given. And are variables to be solved. Hence, we define the reduced variables and .
The reduced discrete Lagrange force is Here , .
Hence, we can have reduced discrete Legendre transformations and , Here 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 . is a vector and is a vector.
Substituting pairs of and into , we can obtain the algebraic equations In (48), are unknown variables to be solved. Above processes make the followers’ motion be calculated from to .
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 . 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 . 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 . 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 .
3.3. Numerical Examples
Example 1. Formation of three vehicles in safe environment is considered. The coefficients are kg and kgm2. The matrix of desired distances and , and are chosen as
Trajectory of the leader vehicle is given in a discrete form as Here m/s is the speed of velocity and 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.