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 [
Different frameworks of formation have been discussed in [
For consideration of a real-time control, numerical methods for high precision algorithms have been discussed in [
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 [
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.
Consider a formation of
The configuration space of
In the rest of this paper, we write
The potential field with stress function has been mentioned in [
Potential function of position of the
Potential force of the
Assume that the Lagrangian of the
Consider the dot product of vectors. It holds
Potential force between the
Potential function of orientation of the
Damping forces of the
Same as the potential function of stress function, the damping force has corresponding potential functions
Stability of formation in potential field is proved by using notion of error dynamic system. Define the error variable
Firstly, we consider the dynamic system for vehicles’ positions. Denote the control force by
Hence, it holds
We consider a leader-follower formation of three homogeneous vehicles, because it is a basic element of formation [
Secondly, we consider the equilibrium point and the stability of the above system. Since
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 [
If
Consider the Jacobian for stable structures. Denote
Use the same way to calculate the remaining parts. We can have the Jacobian matrix in form of
Hence, we need to introduce a body frame and discuss the stability in the body frame. As shown in Figure
The left illuminates the construction of body frame where the
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
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
The error variables have seven freedoms in body frame.
Error variable
The error dynamic system of formation is stable at the equilibrium point.
According to the center manifold theorem of nonlinear dynamic, the stability of original system is guaranteed if the reduced system is stable [
According to the definition of reduced system,
Naturally, it holds
The Jacobian matrix of reduced system at equilibrium point is
The Jacobian
Obstacle avoidance is investigated by adding modified obstacle force to vehicles. Obstacles are stored as
Obstacle force between the
The obstacle force on the
The left is an example of obstacle.
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 can be derived by applying discrete variational principle on discrete Lagrangian [
Given Lagrangian
Applying discrete Lagrange-d'Alembert principle on discrete action function
Discrete Legendre transformations are maps connecting different discrete configuration spaces as shown in Figure
Commutable diagram based on discrete Legendre transformations [
Variational integrator is defined as a compound map of discrete Legendre transformations as
With map
In a formation with
States of the
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
The discrete Lagrangian is
In the leader-follower formation, leader's trajectory
The reduced discrete Lagrange force is
Hence, we can have reduced discrete Legendre transformations
Substituting pairs of
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 [
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
Formation of three vehicles in safe environment is considered. The coefficients are
Trajectory of the leader vehicle is given in a discrete form as
The initial conditions are chosen as shown in Table
Initial conditions of vehicles.
Label of vehicle |
|
|
---|---|---|
1 | 9.33 | 50.0 |
2 | 5.00 | 47.5 |
3 | 5.00 | 52.5 |
As shown in Figure
Formation of three vehicles in safe environment. The leader vehicle moves in a curve of trigonometric function.
Formation of three vehicles in obstacle environment is considered. Obstacles are described in the form of
Formation of three vehicles in obstacle environment with
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.
The
As shown in Figure
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.
The authors declare that there is no conflict of interests regarding the publication of this paper.
This work was supported by a Grant from the National Natural Science Foundation of China (no. 61350010).