Trajectory Optimization for Multiple Vehicles Using a ... - Eric N. Johnson

AIAA 2005-7098

Infotech@Aerospace 26 - 29 September 2005, Arlington, Virginia

Trajectory Optimization for Multiple Vehicles Using a Reduced Order Formulation Shannon Twigg* , Anthony Calise† and Eric Johnson‡ School of Aerospace Engineering, Georgia Institute of Technology, Atlanta, GA 30332-0150

Different methods of optimizing terrain following and terrain masking trajectories have been investigated as an extension of earlier reduced order formulations. This paper examines simultaneous control of two aircraft maneuvering in the same terrain section, including cases where the coordinated arrivals at a given target are desired. In all cases, both vehicles are flying at a constant velocity. Two forms of the equations of motion are investigated – a simplified form and a local tangent plane set. In addition, the trajectories are optimized with respect to two different criteria – a minimum time case and a terrain masking case.

I. Introduction High-flying unmanned reconnaissance and surveillance systems are now being used extensively in the United States military. Current development programs will soon produce demonstrations of next -generation unmanned flight systems that are designed to perform combat missions. In practice, these vehicles must achieve a high level of autonomy in operations to be successfully deployed in large numbers. Their use in first-strike combat operations will dictate operations in densely cluttered environments that include unknown obstacles and threats, and will require the use of terrain for masking. In addition, the ability to coordinate the movements of more than one aircraft in the same area is an emerging challenge. The demand for autonomy of operations in such conditions dictates the need for the on-board capability to simultaneously compute optimal trajectories for multiple aircrafts. In the early 1990s, P. K. Menon and Eulgon Kim researched methods of optimal trajectory path planning for terrain following and terrain masking flight. This research produced a reduced order formulation based on a constant velocity approach using local tangent plane equations of motion.1,2 Recently, Twigg, Calise and Johnson have expended on the work performed by Menon and Kim. This has resulted in constant velocity reduced order formulations using both local tangent plane and simplified equations of mo tion to determine optimal trajectories in situations involving moving targets and threats.3 This paper expands on the previous work performed. Here, optimal trajectories are simultaneously found for two aircraft flying in the same terrain area using a reduced order formulation consisting of seven differential equations. These aircraft are both flying at constant velocities and use both the simplified equations of motion and the local tangent plane equations of motion. Three different trajectory scenarios are investigated. The first consists of simple collision avoidance where two vehicles are flying in the same area and must just stay a minimum distance apart. The second involves a case where the two vehicles are flying along the same path and a faster vehicle must pass a slower vehicle. The third case examined deals with coordinating the arrivals of the two vehicles at the same point; in this case, one vehicle must arrive exactly two seconds after the first vehicle, while both vehicle are coming from the same distance away and are flying at the same velocity.

II. Optimal Trajectory Formulations The problem of coordinating flights of two vehicles in a certain area will be demonstrated by using two different formulations – one using the local tangent plane equations of motion and one utilizing simplified equations of motion. Figure 1 depicts a sample terrain profile with the X-Y-H coordinate system and a local x1 -y 1 -z1 coordinate system. The moving local coordinate system has its origin on the terrain surface at a current x, y position with the x1 -y 1 plane being the tangent plane. The local tangent plane formulation incorporates the constraint that the vehicle flies tangentially to the local terrain directly into the equations of motion and can be written as *AIAA member, Graduate Research Assistant, Georgia Institute of Technology, Atlanta, Georgia † AIAA Fellow, Professor, Georgia Institute of Technology, Atlanta, Georgia ‡ AIAA member, Assistant Professor, Georgia Institute of Technology, Atlanta, Georgia

1 American Institute of Aeronautics and Astronautics Copyright © 2005 by authors. Published by the American Institute of Aeronautics and Astronautics, Inc., with permission.

•

x=

V cosψ Vf x f y sin ψ + A1 A1 A2 • − VA1 sin ψ y= A2

(1)

(2)

Figure 1: Relationship Between Inertial Frame and Local Tangent Plane The simplified equations of motion are an approximation written in the local level frame and neglect the effects of the terrain slope in the position kinematics. •

x = V cos ψ

(3)

•

y = V sin ψ

(4)

A. Simplified Equations of Motion For this formulation, the simplified equations of motion are used, as described above. •

x i = Vi cosψ i

(5)

•

y i = Vi sin ψ i

(6)

Here, with i equal to one, (5) and (6) depict the equations of motion for vehicle 1 with a velocity, V1 , and a heading angle ψ1 . With i equal to two, (5) and (6) depict the equations of motion for vehicle 2 with a velocity of V2 and a heading angle of ψ2 . The variables x1 and x2 represent the positions with respect to the northward x-axis of vehicle 1 and 2, respectively while y1 and y2 are the positions with respect to the eastward y-axis of vehicle 1 and 2, respectively. For this problem, the initial and final positions for each vehicle are specified. The cost equation used for this problem is

J= with

∫

tf

0

W  (1 − K ) + Kf 1 ( x1 , y1 ) + Kf 2 ( x2 , y 2 ) + d 

  dt 

d = ( x1 − x 2 ) + ( y1 − y 2 ) 2

2

2 American Institute of Aeronautics and Astronautics

(7)

(8)

In these equations, W is a weighing parameter and d is a measure of the square of the distance between the two vehicles. It can be seen that d will create a singularity in J at collision. The altitudes of each of the vehicles are represented by the sum of f1 (x 1 , y1 ) and f2 (x 2 , y2 ) -- the terrain height at each respective vehicle position -- and a constant ground clearance. The weighting parameter, K, can vary between 0 and 1 and determines the relative importance of time and terrain masking/threat avoidance used in the optimization. When K = 0, the equations are optimized with respect to time. When K is set to 1, the path is optimized with respect to the threats and the terrain. The final time for the problem is the maximum time needed for either vehicle to complete its trajectory. The Hamiltonian equation for this system can be written as

H = A4 + λ x1V1 cosψ 1 + λ y1V1 sin ψ 1 + λ x 2V2 cosψ 2 + λ y 2V2 sin ψ 2

(9)

where

A4 = (1 − K ) + Kf1 ( x1 , y1 ) + Kf 2 ( x 2 , y 2 ) +

W d

(10)

The costate variables for the four states are given by λxi and λyi. The optimality conditions for this problem are

Hψi = 0

(11)

where Hψ1 represents the partial derivative of the Hamiltonian with respect to ψ1 while Hψ2 represents the partial derivative of the Hamiltonian equation with respect to ψ2 . Evaluating equation (11) results in the following two relationships.

λ yi = λ xi

sin ψ i cosψ i

(12)

Since the Hamiltonian equation is not explicitly dependent on time, it will equal zero at all times. Using this fact and equations (9) and (12), the following two expressions are found

 A4 cosψ 1 + λx1V1    cosψ 1   − cosψ 2  A4 cosψ 1 + λx1V1    = V2 cos ψ 1  

λy 2 = λ x2

− sin ψ 2 V2

(13)

(14)

Differential equations for the costates can be found by taking the partial derivatives of the Hamiltonian equation as shown below. •

λ xi = − H ix •

(15)

λ yi = − H iy This results in the following four differential equations. •

λ x1 = − Kf1x + B1

(16)

•

λ y1 = −Kf1y + B2

(17)

•

λ x 2 = −Kf 2 x − B1


(18)

•

λ y 2 = − Kf 2 y − B2 with

(19)

2W ( x1 − x 2 ) d2 2W ( y1 − y 2 ) B2 = d2 B1 =

(20) (21)

The time derivative of equation (12) is taken and set equal to its counterpart in equation (17), with i equal to one in both cases. Rearranging this expression will result in a differential equation for ψ1 . •

ψ1 =

[

cosψ 1 ( Kf1x − B1 )sin ψ 1 − (Kf1 y − B2 )cosψ 1

]

(22)

λ x1

Next, this is repeated for either equations (14) and (18) or equations (13) and (19), with i equal to two, to determine a differential equation for ψ2 . •

ψ2 =

[

V2 cosψ 1 (Kf2 y + B2 )cosψ 2 − (Kf 2x + B1 )sin ψ 2

]

(23)

A4 cosψ 1 + V1λ x1

This results in a system with seven differential equations – the four state equations, the two heading equations and the costate λx1. B. Local Tangent Plane Equations of Motion For this section the equations of motion used were seen above in equations (1) and (2) and are repeated here.

x& i =

Vi cosψ i Vi f ix f iy sin ψ i + A1i A1i A2i − Vi A1i sin ψ i y& i = A2i

(24)

(25)

where

A1i = 1 + f ix2

(26)

A2i = 1 + f ix2 + f iy2

This process used is the same as above. The cost equation used is the same as in the previous section and can be seen in equation (7). The Hamiltonian equation is found to be

 V cosψ i Vi f ix f iy sin ψ i   − V A sin ψ i  + λ yi  i 1i H = A4 + λ xi  i + A1i A2i A2 i   A1i 

  

(27)

where A 4 is defined in equation (10). As before, equations were found for the costates using the Hamiltonian equation and the optimality equation seen in (11). These were found to be, as a function of λx1 ,


λy 2 =

 − A21 sin ψ 1 + f1 x f 1 y cosψ 1   λ y1 = λ x1  A112 cosψ 1   − A12 cosψ 2  A4 A11 cosψ i + V1λ x1    λ x2 = V2 A11 cosψ 1   ( A4 A11 cosψ i + V1λx1 )(− f 2 x f 2 y cosψ 2 + A22 sin ψ 2 )

(28)

(29)

(30)

V1 A11 cosψ 1 A12

The differential equations for the costates were found through evaluating equation (15). This resulted in

V1  sin 2 ψ 1    L cosψ 1 + L2 sin ψ 1 + L3 3  1 A113 A21 cos ψ 1   2 V  sin ψ 1   λ& y1 = −Kf1 y + D2 − 3 1 3  L4 cosψ 1 + L5 sin ψ 1 + L6 A11 A21  cosψ 1  λ& x1 = − Kf1x + D1 −

(32)

 sin 2 ψ 2   L1 cosψ 2 + L2 sin ψ 2 + L3  cos ψ 2   2  V sin ψ 2   − D2 − 3 2 3  L4 cosψ 2 + L5 sin ψ 2 + L6 A12 A22  cosψ 2 

λ& x 2 = − Kf2 x − D1 − λ& y 2 = −Kf 2 y

(31)

V2 3 3 A12 A22

(33)

(34)

where

L1 = − A2i f ix f ixx λ xi 3

[ = (− A

]

(35)

]

(36)

L2 = A12i A22i ( f iy f ixx + f ix f ixy ) − A12i f ix f iy ( f ix f ixx + f iy f ixy ) − A22i f ix2 f iy f ixx λ xi L3

4 1i

)

f iy f ixy + A12i f ix f iy2 f ixx λ yi

L4 = − A23i f ix f ixy λ xi

[ = (− A

L5 = A12i A22i ( f iy f ixy + f ix f iyy ) − A12i f ix f iy ( f ix f ixy + f iy f iyy ) − A22i f ix2 f iy f ixy λ xi L6

)

f f iyy + A12i f ix f iy2 f ixy λ yi

4 1 i iy

Using the technique described above, differential equations for the two heading angles are then found. These can be represented as

ψ& 1 = ψ& 2 =

A113 A21 cosψ 1 (P1 sin ψ 1 + P2 cosψ 1 ) + λ x1V1 f 1y P3

[

2 A113 A21 λx1

V2 A11 A122 A22 cosψ 1T1 + ( A4 A11 cosψ 1 + V1λ x1 )T2 A123 A222 ( A4 A11 cosψ 1 + V1λ x1 )

with


]

(37)

(38)

P1 = A21 (Kf1x − D1 )

P2 = A112 (Kf1y − D2 ) − f 1x f 1 y ( Kf1x − D1 )

(39)

P3 = f 1x f 1y f 1xx sin ψ 1 − A112 f 1xy sin ψ 1 + A21 f 1xx cosψ 1

T1 = ( Kf2 x + D1 )( f 2x f 2 y cosψ 2 − A22 sin ψ 2 ) − (Kf 2 y + D2 )A122 cosψ 2

(

)

T2 = f 2 y f 2 x f 2 y f 2 xx − A122 f 2 xy sin ψ 2 + A22 f 2 y f 2 xx cosψ 2

(40)

Again, this process results in a system with seven differential equations – the four state equations, the two heading equations and the costate λx1. C. Solving the Problem As mentioned above, the problem is reduced to a set of seven differential equations with three unknown initial values. These equations are the four differential equations of motions for both vehicles, the differential equations of the two heading angles and the differential equation for the costate λx1. The initial values are unknown for the costate and the two heading angles. To solve for the three initial values, a genetic algorithm and conjugate gradient search were performed. First, the genetic algorithm was used to get close to the solution, and then the conjugate gradient search was used to find the solution. This was done because the cost function used contains many local minima. The cost function used was

c = a1 dist 1 + a 2 dist 2 + a 3c1 + a 4 c2

(41)

c1 = ∫ [(1 − K ) + Kf1 ( x1 , y1 ) + Kf 2 ( x 2 , y 2 ) ]dt

(42)

t f W  c 2 = ∫   dt 0 d 

(43)

with tf

0

In equation (41), a 1 , a 2 , a 3 , and a 4 are weighing parameters and dist1 and dist2 are the distances from the final position of the current trajectory to the final target position for vehicles 1 and 2, respectively. In general, it was found that the parameter W should be scaled on the order of d 2 , when d is defined as in equation (8). Then, when the different possible solutions were checked, the final solution was chosen such that both vehicles reached their respective targets, the minimum distance between the two vehicles exceeded the desired minimum and it had the lowest cost, c1 , as defined in equation (42). In all the results presented, it was decided that the desired minimum distance between the two vehicles was 30 feet.

III. Numerical Results The first case presented tests simple collision avoidance while flying over a flat plain. The results for this can be seen in Figure 2. Vehicle 1 begins at point (800, 1700) and ends at (800, 100) and flies at a speed of 80 ft/s while vehicle 2 begins at point (500, 1700) and ends at point (1000, 740) and flies at a speed of 90 ft/s. In the plot, the trajectory start points are marked with a red circle while the end points are marked with a red x. In the plot, three sets of trajectories can be seen. The first set is solid, straight lines that represent the optimal trajectories when either plane is flying alone; the planes would collide if these paths were utilized during simultaneous flight. The dotted line indicates the solution found using the simplified equations of motion and represents an optimal flight with a minimum distance between the planes of 36.3 feet. The dashed line depicts the solution found using the local tangent plane equations of motion and represents an optimal flight with a minimum distance between the planes of 37.2 feet. In all cases, the paths for vehicle 1 are shown in blue while the trajectories for vehicle 2 are shown in green. In both cases W was 810000. Because there is no terrain, there is more than one possible solution with a minimum cost value.


Figure 2: The blue lines represent vehicle 1 flying from (800, 1700) to (800, 100) and the green lines represent vehicle 2 flying from (500, 1700) to (1000, 740). The solid line is the uncorrected optimal paths that would collide. The dashed lines are solutions from the local tangent plane equations of motion. The dotted lines are the solutions from the simplified equations of motion. The second case presented shows an instance where the two vehicles are flying along the same path but vehicle 2 – the faster plane – must pass around vehicle 1. This test again uses a flat terrain. The results for this are shown in Figure 3. Here the starting points are (800, 1700) and (800, 1800) for vehicles 1 and 2, respectively, while the ending positions are (800, 300) and (800, 200), respectively. As before, the dotted lines represent the solution using the simplified equations of motion and the dashed lines represent the trajectories found with the local tangent plane equations of motion. The trajectory with the local tangent plane equations of motion had a minimum distance between the planes of 32.9 feet and used a value of 810000 for W. However, in this scenario, the results from using the simplified equations of motions did not keep the planes as far from each other as desired. The path shown had a minimum distance of 20.3 feet between the vehicles even though W was increased to 6250000.

Figure 3: The blue lines represent vehicle 1 flying from (800, 1700) to (800, 300) and the green lines represent vehicle 2 flying from (800, 1800) to (800, 200). The dashed lines are solutions from the local tangent plane equations of motion. The dotted lines are the solutions from the simplified equations of motion.


The next case – seen in Figures 4 and 5 – deals with the situation when the two vehicles must arrive at the target a set amount of time apart. Figure 4 shows the results using the local tangent plane equations of motion while Figure 5 shows the results using the simplified equations of motion. In this case, vehicle 2 must arrive at the target two seconds after vehicle 1 arrives. This case was performed using a set of real terrain data for an area around Columbus, Ohio. In this case, the starting positions for vehicles 1 and 2, respectively, are (10000, 10000) and (15000, 15000) while the final target position is (15000, 10000) for both vehicles and the speeds of both vehicles are 80 ft/s. In the plots, the solid lines represent the optimal trajectories when each vehicle is alone. In this case, when using the local tangent plane equations of motion, vehicle 1 arrives in 67.54 seconds and vehicle 2 arrives in 63.42 seconds; when using the simplified equations of motion, vehicle 1 arrives is 63.93 seconds and vehicle 2 arrives in

Figure 4: The blue lines represent vehicle 1 flying from (10000, 10000) to (15000, 10000) and the black lines represent vehicle 2 flying from (15000, 15000) to (15000, 10000). The dashed lines are solutions from the local tangent plane equations of motion. 62.5 seconds. In both cases, when considering coordinated flight, vehicle 2 will be constrained to arrive two seconds after vehicle 1. The dashed lines in the figures represent the new optimal trajectories. In this flight, using the local tangent plane equations of motion results in vehicle 1 arriving in 64.14 seconds and vehicle 2 arriving in 66.14 seconds while using the simplified equations of motion results in vehicle 1 arriving in 62.5 seconds and vehicle 2 arriving in 64.5 seconds.


Figure 5: The blue lines represent vehicle 1 flying from (10000, 10000) to (15000, 10000) and the black lines represent vehicle 2 flying from (15000, 15000) to (15000, 10000). The dashed lines are solutions from the simplified equations of motion.

IV. Future Research In the future, more work will be completed concerning investigating the results for optimal path planning for the case with two vehicles in different configurations and with different objectives. These calculated optimal paths will then be implemented in a six degree-of-freedom flight simulator to determine the validity of the analytical solution. The trajectories can be used as inputs to the simulator and the actual flight path of the simulator can then be compared to the optimal trajectory to determine if the solutions found are actually viable. In addition, a similar formulation using more than two vehicles will be examined. Finally, investigating a reduced order formulation using three degree-of-freedom equations of motion and a variable velocity will be completed.


V. References (1) Kim, Eulgon. Optimal Helicopter Trajectory Planning for Terrain Following Flight. Thesis. Georgia Institute of Technology. 1990. (2) Menon, P.K., E. Kim, V.H.L. Cheng. "Optimal Trajectory Planning for Terrain Following Flight" Journal of Guidance, Control and Dynamics, Vol. 14, No. 4, July – August 1991, pp. 807 - 813. (3) Twigg, S, A.Calise, E. Johnson. “On-line Trajectory Optimization Including Moving Threats and Targets.” AIAA GNC Conference. August, 2004.
