Fixed-Wing Aircraft Trajectory Generation Using Embedded Model ...
Fixed-Wing Aircraft Trajectory Generation Using Embedded Model Predictive Control Brandon Jones June 4, 2014
1
Background
Small, autonomous, fixed-wing aircraft are often deployed into environments where wind magnitudes are a significant portion of their operating airspeeds. In this scenario, it is difficult to perform confident and predictable path-following within confined areas. One common approach is to plan a trajectory offline, consisting of constant turn radii and straight line segments [Dub57]. This is problematic in high winds, as the aircraft may not be able to achieve the desired turning radius at high speeds, leading to divergent path errors, flight outside of area boundaries, and possible collision with obstacles. This project will implement a embedded Model Predictive Control (MPC) algorithm suitable for generating real-time trajectories that facilitate predictable and safe path following in the midst of significant wind.
2
Problem
Consider an aircraft traveling at velocity Va ∈ R within an airmass of velocity w ∈ R2 . The aircraft’s state consists of position p(t) ∈ S 2 , heading ψ(t) ∈ [0, 2π], roll φ(t) ∈ [−φmax , φmax ], and roll rate input u(t) ∈ [−umax , umax ] with t ∈ [0, T ]. A trajectory x(t) = T p(t) ψ(t) φ(t) u(t) is required to facilitate transition from xinitial = x(0) to a desired xfinal = x(T ) within a minimum time T , as depicted in Figure 1. Consider the non-convex, infinite dimensional optimization problem that includes aircraft kinematic constraints, minimize T subject to p(ψ, ˙ w, t) = Va [cos ψ(t) sin ψ(t)]T + w ˙ t) = g tan φ(t) ψ(φ, Va ˙ = u(t) φ(t) |u(t)| ≤ umax , |φ(t)| ≤ φmax p(t) ∈ S t ∈ [0, T ] x(0) = xinitial , x(T ) = xfinal 1
(1)
Figure 1: Aircraft Trajectory Definition
T with variables T and x(t) = p(t) ψ(t) φ(t) u(t) . Problem data are Va , w, φmax , umax , xinitial , xfinal , and gravity g.
3
Method
The aircraft kinematic constraints contain trigonometric functions that cause the optimization problem (1) to be non-convex. In addition, the objective T causes the problem to be infinite dimensional. To address this, the problem is discretized and the non-convex constraints are linearized. A locally optimal trajectory is found using Sequential Convex Programming (SCP) combined with a feasibility search over T .
3.1
Linearization
The non-convex equality constraints of the original optimization problem are linearized about ˆ A first order Taylor expansion is used, an estimate of ψ. ˆ (i) − sin ψ(t) ˆ (i) ψ(t) − ψ(t) ˆ (i) cos ψ(t) +w p(ψ, ˙ w, t) ≈ Va (2) (i) (i) (i) ˆ ˆ ˆ ψ(t) − ψ(t) sin ψ(t) + cos ψ(t) ˆ (i) is an estimated heading along the trajectory. In addition, where ψ(t) ˙ ≈ g φ(t), |φ(t)| ≤ 45◦ ψ(t) Va
3.2
(3)
Discretization
The problem is discretized to facilitate solving by a numerical solver in an embedded target platform. The time interval t ∈ [0, T ] is discretized with a total of N time steps of length h, where h = T /N and t = kh. Each derivative is approximated with a forward difference, 2
e.g. p(t) ˙ ≈ p(kh) ˙ = pk+1h−pk . The minimization objective T is replaced with kφk1 to penalize non-zero bank angles, to encourage ideal trajectories consisting of straight line segments and turns at maximum bank angles. Applying the linearizations of (2) and (3) and new objective results in the quasi-convex optimization problem, minimize kφk1 ˆ h, Vt , w)x(k) + B(k, ψ, ˆ h, Vt ), k = 1 . . . N subject to x(k + 1) = A(k, ψ, |x(k)| xmax , k = 1 . . . N x(0) = xinitial , x(N ) = xfinal
(4)
T with the variables x(k) = p(k) ψ(k) φ(k) u(k) , k = 1 . . . N . Problem data are xmax , xinitial , xfinal , A(k) ∈ R5×5 , and B(k) ∈ R5×1 , k = 1 . . . N . A trust region on the linearization about ψˆ is applied, such that ψ ≤ ρ ∈ T .
3.3
Sequential Convex Program
ˆ The quasi-convex optimization problem (4) requires requires linearization about ψ(k), k= ˆ 1 . . . N . A SCP is used to solve (4) repeatedly while updating ψ(k) until convergence is attained on a feasible and locally optimal trajectory consistent with original non-convex constraints of (1). ˆ First, the problem is seeded with an initial estimate of ψ(k), k = 1 . . . N consisting of a simple linear interpolation between ψ(0) and ψ(N ). An initial linearization trust region is ˆ − ψ(k)| ≤ ρ(k), k = 1 . . . N . The resulting convex problem applied ρ(k) ∈ T such that |ψ(k) (i) is then solved for xˆ(t) with an initial guess at a time step h. If this convex problem is infeasible, the time step h is increased until a feasible solution is attained. For each iteration i, the approximate trajectory xˆ∗ (k) is compared to the trajectory x(k) resulting from the approximate input uˆ(k)(i) for k = 1 . . . N , subject to the original nonlinear aircraft kinematic constraints. This linearization error represents consistency between the approximate trajectory xˆ(t)(i) and a trajectory subject to the original non-linear aircraft kinematic constraints. The trust region ρ(k) is reduced or enlarged by a factor of γ depending on the magnitude of the error at each k. The problem is re-linearized with ψˆ(i+1) and the new convex optimization problem is then solved again. The stopping criteria for the SCP is based on the magnitude of the linearization error, xerr , and a non-decreasing optimal value. given ψˆseed (k) k = 1, . . . , N and h(0) . Let ψˆ(0) = ψˆseed . repeat 1. Solve (4) (i+1) 2. if infeasible = h(i) + ∆h, goto 1 P h 3. break if |xerr | ≤ α and |x∗(i) − x∗(i−1) | ≤ β 3
ˆ (i+1) = ψ(k) ˆ (i) k = 1, . . . , N . 4. Update ψ(k) 5. Update trust region, ρ(k)(i+1) = γ(k)(i) ρ(k)(i) return x(k).
4
Implementation
The SCP was implemented using CVX [GB14] [GB08], ECOS [DCB13] as a solver under CVX, and CVXGEN [MB12] running in a MATLAB MEX interface. All three implementations used a constant step size of N = 40 to allow static memory allocation for running on embedded platforms. CVXGEN is a convex optimization code generation tool suitable for embedded convex optimization. The problem was described as a Linear Program (LP) resulting in a 1406×1406 KKT matrix with 3608 non-zero entries and 360 variables.
5
Numeric Results
A specific instance of the trajectory problem (4) was solved for an aircraft traveling at T Va = 15 m/s in no-wind, w = 0 m/s, and high-wind, w = 14 0 m/s conditions. The high-wind condition represents 93% the aircraft’s airspeed, which would typically be considered unsafe due to the inability of the aircraft to follow a path using classical reactionary Proportional Derivative (PD) path following techniques. Initial and final conditions are outlined in Table 1. Stopping criteria were set to α = β = 1. Figure 2 and figure 3 show resulting trajectories of the algorithm for no-wind and highwind conditions depicted in blue and red, respectively. The no-wind trajectory approximates a standard ”Dubins Path” [Dub57], starting with a maximum bank angle turn to a level-flight segment, and a final turn to the specified position and heading. The high-wind trajectory does not begin with a turn towards the final condition, rather the aircraft delays its turn while significantly ”crabbing” into the wind, demonstrating nongreedy control. The aircraft slowly travels north until it reaches a position such that a full bank angle turn away from the wind will result in hitting its final state. During this turn, the aircraft’s groundspeed rapidly increases from 1 m/s to a peak of 29 m/s and attains the desired final position and heading. A history of iterations of the algorithm is presented in Figure 2b. The algorithm begins with the feasibility search on h, represented by CVXGEN executing the specified maximum number of iterations of 25. Some initial feasible solutions are found around 40-60 iterations, however after performing several SCP iterations, the problem becomes infeasible for that time step h. These initial feasible solutions are based on initial inaccurate linearizations of ˆ ψ(t). The overall algorithm performance for the given problem instance with each solver is presented in Table 2. All runs were performed on a Macbook Pro 2 GHz Intel Core i7. 4
26
160
24
140
22
20
CVXGEN Steps
120
meters
100
80
60
No Wind Wind 14 m/s −−>
18
16
14
12
40 10
20
8
0 −50
0
50 meters
100
6
150
0
20
40
60 iteration # over h
80
100
120
(b) Convergence
(a) Position p(t) and Heading ψ(t) Figure 2: Trajectory
State k=0 T 0 0 m Position, p(k) Heading, ψ(k) 270◦ Bank Angle, φ(k) 0◦ Control Input, u(k) 0 deg/sec
k=T T 150 150 m 180◦ 0◦ 0 deg/sec
Table 1: Initial and Final Conditions, x(0), x(T )
Overall, the generated C code from CVXGEN was 230-400 times faster than CVX with SDPT3 and ECOS. The algorithm as implemented with CVXGEN has performance suitable to be run real-time as Model Predictive Controller on an aircraft. The number of SCP iterations required for CVXGEN was more than both CVX runs. This could be due to slightly different solver tolerances, leading to increased number of SCP iterations required to find a feasible solution.
6
Conclusion
A Model Predictive Controller (MPC) was created that provides real-time trajectory generation between two positions and headings for a fixed-wing aircraft. Generated trajectories take into account aircraft turn capabilities and the prevailing wind field. The non-convex optimization problem was linearized and a local solution was found using Sequential Convex Programming. Using compiled solver code, solutions were generated at speeds suitable for real-time embedded operation. This algorithm could be extended to provide real-time trajectory feasibility warnings, preventing controlled flight into terrain, thus allowing safe operations in constrained areas. 5
φ, bank angle, deg
20 0 −20 −40 −60
0
5
10
15
20
25
30
35
40
0
5
10
15
20
25
30
35
40
0
5
10
15
20 Seconds
25
30
35
40
u, input, deg/sec
100 50 0 −50
Ground Speed, m/s
−100
30 20 10 0
Figure 3: Aircraft φ, u, and Ground Speed versus time
Software CVX and SDPT3 4.0 No-Wind CVX and ECOS CVXGEN, MATLAB MEX CVX and SDPT3 4.0 High-Wind CVX and ECOS CVXGEN, MATLAB MEX
Solve Time SCP Iterations(1) 70 sec 58 49 sec 58 212 ms 77 120 sec 87 73 sec 82 294 ms 117
Note: (1) Includes iterations from feasibility search on time-step h Table 2: Algorithm Performance
6
Solver Iterations(1) 1695 527 1615 2581 1002 2576
References [DCB13] A. Domahidi, E. Chu, and S. Boyd. ECOS: An SOCP solver for embedded systems. In European Control Conference (ECC), pages 3071–3076, July 2013. [Dub57] L.E. Dubins. On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. American Journal of Mathematics, 79(3):497–516, 1957. [GB08]
Michael Grant and Stephen Boyd. Graph implementations for nonsmooth convex programs. In V. Blondel, S. Boyd, and H. Kimura, editors, Recent Advances in Learning and Control, Lecture Notes in Control and Information Sciences, pages 95–110. Springer-Verlag Limited, 2008. http://stanford.edu/~boyd/graph_ dcp.html.
[GB14]
Michael Grant and Stephen Boyd. CVX: Matlab software for disciplined convex programming, version 2.1. http://cvxr.com/cvx, March 2014.
[MB12]
J. Mattingley and S. Boyd. CVXGEN: A code generator for embedded convex optimization. Optimization and Engineering, 13(1):1–27, 2012.
7
1
Background
Small, autonomous, fixed-wing aircraft are often deployed into environments where wind magnitudes are a significant portion of their operating airspeeds. In this scenario, it is difficult to perform confident and predictable path-following within confined areas. One common approach is to plan a trajectory offline, consisting of constant turn radii and straight line segments [Dub57]. This is problematic in high winds, as the aircraft may not be able to achieve the desired turning radius at high speeds, leading to divergent path errors, flight outside of area boundaries, and possible collision with obstacles. This project will implement a embedded Model Predictive Control (MPC) algorithm suitable for generating real-time trajectories that facilitate predictable and safe path following in the midst of significant wind.
2
Problem
Consider an aircraft traveling at velocity Va ∈ R within an airmass of velocity w ∈ R2 . The aircraft’s state consists of position p(t) ∈ S 2 , heading ψ(t) ∈ [0, 2π], roll φ(t) ∈ [−φmax , φmax ], and roll rate input u(t) ∈ [−umax , umax ] with t ∈ [0, T ]. A trajectory x(t) = T p(t) ψ(t) φ(t) u(t) is required to facilitate transition from xinitial = x(0) to a desired xfinal = x(T ) within a minimum time T , as depicted in Figure 1. Consider the non-convex, infinite dimensional optimization problem that includes aircraft kinematic constraints, minimize T subject to p(ψ, ˙ w, t) = Va [cos ψ(t) sin ψ(t)]T + w ˙ t) = g tan φ(t) ψ(φ, Va ˙ = u(t) φ(t) |u(t)| ≤ umax , |φ(t)| ≤ φmax p(t) ∈ S t ∈ [0, T ] x(0) = xinitial , x(T ) = xfinal 1
(1)
Figure 1: Aircraft Trajectory Definition
T with variables T and x(t) = p(t) ψ(t) φ(t) u(t) . Problem data are Va , w, φmax , umax , xinitial , xfinal , and gravity g.
3
Method
The aircraft kinematic constraints contain trigonometric functions that cause the optimization problem (1) to be non-convex. In addition, the objective T causes the problem to be infinite dimensional. To address this, the problem is discretized and the non-convex constraints are linearized. A locally optimal trajectory is found using Sequential Convex Programming (SCP) combined with a feasibility search over T .
3.1
Linearization
The non-convex equality constraints of the original optimization problem are linearized about ˆ A first order Taylor expansion is used, an estimate of ψ. ˆ (i) − sin ψ(t) ˆ (i) ψ(t) − ψ(t) ˆ (i) cos ψ(t) +w p(ψ, ˙ w, t) ≈ Va (2) (i) (i) (i) ˆ ˆ ˆ ψ(t) − ψ(t) sin ψ(t) + cos ψ(t) ˆ (i) is an estimated heading along the trajectory. In addition, where ψ(t) ˙ ≈ g φ(t), |φ(t)| ≤ 45◦ ψ(t) Va
3.2
(3)
Discretization
The problem is discretized to facilitate solving by a numerical solver in an embedded target platform. The time interval t ∈ [0, T ] is discretized with a total of N time steps of length h, where h = T /N and t = kh. Each derivative is approximated with a forward difference, 2
e.g. p(t) ˙ ≈ p(kh) ˙ = pk+1h−pk . The minimization objective T is replaced with kφk1 to penalize non-zero bank angles, to encourage ideal trajectories consisting of straight line segments and turns at maximum bank angles. Applying the linearizations of (2) and (3) and new objective results in the quasi-convex optimization problem, minimize kφk1 ˆ h, Vt , w)x(k) + B(k, ψ, ˆ h, Vt ), k = 1 . . . N subject to x(k + 1) = A(k, ψ, |x(k)| xmax , k = 1 . . . N x(0) = xinitial , x(N ) = xfinal
(4)
T with the variables x(k) = p(k) ψ(k) φ(k) u(k) , k = 1 . . . N . Problem data are xmax , xinitial , xfinal , A(k) ∈ R5×5 , and B(k) ∈ R5×1 , k = 1 . . . N . A trust region on the linearization about ψˆ is applied, such that ψ ≤ ρ ∈ T .
3.3
Sequential Convex Program
ˆ The quasi-convex optimization problem (4) requires requires linearization about ψ(k), k= ˆ 1 . . . N . A SCP is used to solve (4) repeatedly while updating ψ(k) until convergence is attained on a feasible and locally optimal trajectory consistent with original non-convex constraints of (1). ˆ First, the problem is seeded with an initial estimate of ψ(k), k = 1 . . . N consisting of a simple linear interpolation between ψ(0) and ψ(N ). An initial linearization trust region is ˆ − ψ(k)| ≤ ρ(k), k = 1 . . . N . The resulting convex problem applied ρ(k) ∈ T such that |ψ(k) (i) is then solved for xˆ(t) with an initial guess at a time step h. If this convex problem is infeasible, the time step h is increased until a feasible solution is attained. For each iteration i, the approximate trajectory xˆ∗ (k) is compared to the trajectory x(k) resulting from the approximate input uˆ(k)(i) for k = 1 . . . N , subject to the original nonlinear aircraft kinematic constraints. This linearization error represents consistency between the approximate trajectory xˆ(t)(i) and a trajectory subject to the original non-linear aircraft kinematic constraints. The trust region ρ(k) is reduced or enlarged by a factor of γ depending on the magnitude of the error at each k. The problem is re-linearized with ψˆ(i+1) and the new convex optimization problem is then solved again. The stopping criteria for the SCP is based on the magnitude of the linearization error, xerr , and a non-decreasing optimal value. given ψˆseed (k) k = 1, . . . , N and h(0) . Let ψˆ(0) = ψˆseed . repeat 1. Solve (4) (i+1) 2. if infeasible = h(i) + ∆h, goto 1 P h 3. break if |xerr | ≤ α and |x∗(i) − x∗(i−1) | ≤ β 3
ˆ (i+1) = ψ(k) ˆ (i) k = 1, . . . , N . 4. Update ψ(k) 5. Update trust region, ρ(k)(i+1) = γ(k)(i) ρ(k)(i) return x(k).
4
Implementation
The SCP was implemented using CVX [GB14] [GB08], ECOS [DCB13] as a solver under CVX, and CVXGEN [MB12] running in a MATLAB MEX interface. All three implementations used a constant step size of N = 40 to allow static memory allocation for running on embedded platforms. CVXGEN is a convex optimization code generation tool suitable for embedded convex optimization. The problem was described as a Linear Program (LP) resulting in a 1406×1406 KKT matrix with 3608 non-zero entries and 360 variables.
5
Numeric Results
A specific instance of the trajectory problem (4) was solved for an aircraft traveling at T Va = 15 m/s in no-wind, w = 0 m/s, and high-wind, w = 14 0 m/s conditions. The high-wind condition represents 93% the aircraft’s airspeed, which would typically be considered unsafe due to the inability of the aircraft to follow a path using classical reactionary Proportional Derivative (PD) path following techniques. Initial and final conditions are outlined in Table 1. Stopping criteria were set to α = β = 1. Figure 2 and figure 3 show resulting trajectories of the algorithm for no-wind and highwind conditions depicted in blue and red, respectively. The no-wind trajectory approximates a standard ”Dubins Path” [Dub57], starting with a maximum bank angle turn to a level-flight segment, and a final turn to the specified position and heading. The high-wind trajectory does not begin with a turn towards the final condition, rather the aircraft delays its turn while significantly ”crabbing” into the wind, demonstrating nongreedy control. The aircraft slowly travels north until it reaches a position such that a full bank angle turn away from the wind will result in hitting its final state. During this turn, the aircraft’s groundspeed rapidly increases from 1 m/s to a peak of 29 m/s and attains the desired final position and heading. A history of iterations of the algorithm is presented in Figure 2b. The algorithm begins with the feasibility search on h, represented by CVXGEN executing the specified maximum number of iterations of 25. Some initial feasible solutions are found around 40-60 iterations, however after performing several SCP iterations, the problem becomes infeasible for that time step h. These initial feasible solutions are based on initial inaccurate linearizations of ˆ ψ(t). The overall algorithm performance for the given problem instance with each solver is presented in Table 2. All runs were performed on a Macbook Pro 2 GHz Intel Core i7. 4
26
160
24
140
22
20
CVXGEN Steps
120
meters
100
80
60
No Wind Wind 14 m/s −−>
18
16
14
12
40 10
20
8
0 −50
0
50 meters
100
6
150
0
20
40
60 iteration # over h
80
100
120
(b) Convergence
(a) Position p(t) and Heading ψ(t) Figure 2: Trajectory
State k=0 T 0 0 m Position, p(k) Heading, ψ(k) 270◦ Bank Angle, φ(k) 0◦ Control Input, u(k) 0 deg/sec
k=T T 150 150 m 180◦ 0◦ 0 deg/sec
Table 1: Initial and Final Conditions, x(0), x(T )
Overall, the generated C code from CVXGEN was 230-400 times faster than CVX with SDPT3 and ECOS. The algorithm as implemented with CVXGEN has performance suitable to be run real-time as Model Predictive Controller on an aircraft. The number of SCP iterations required for CVXGEN was more than both CVX runs. This could be due to slightly different solver tolerances, leading to increased number of SCP iterations required to find a feasible solution.
6
Conclusion
A Model Predictive Controller (MPC) was created that provides real-time trajectory generation between two positions and headings for a fixed-wing aircraft. Generated trajectories take into account aircraft turn capabilities and the prevailing wind field. The non-convex optimization problem was linearized and a local solution was found using Sequential Convex Programming. Using compiled solver code, solutions were generated at speeds suitable for real-time embedded operation. This algorithm could be extended to provide real-time trajectory feasibility warnings, preventing controlled flight into terrain, thus allowing safe operations in constrained areas. 5
φ, bank angle, deg
20 0 −20 −40 −60
0
5
10
15
20
25
30
35
40
0
5
10
15
20
25
30
35
40
0
5
10
15
20 Seconds
25
30
35
40
u, input, deg/sec
100 50 0 −50
Ground Speed, m/s
−100
30 20 10 0
Figure 3: Aircraft φ, u, and Ground Speed versus time
Software CVX and SDPT3 4.0 No-Wind CVX and ECOS CVXGEN, MATLAB MEX CVX and SDPT3 4.0 High-Wind CVX and ECOS CVXGEN, MATLAB MEX
Solve Time SCP Iterations(1) 70 sec 58 49 sec 58 212 ms 77 120 sec 87 73 sec 82 294 ms 117
Note: (1) Includes iterations from feasibility search on time-step h Table 2: Algorithm Performance
6
Solver Iterations(1) 1695 527 1615 2581 1002 2576
References [DCB13] A. Domahidi, E. Chu, and S. Boyd. ECOS: An SOCP solver for embedded systems. In European Control Conference (ECC), pages 3071–3076, July 2013. [Dub57] L.E. Dubins. On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. American Journal of Mathematics, 79(3):497–516, 1957. [GB08]
Michael Grant and Stephen Boyd. Graph implementations for nonsmooth convex programs. In V. Blondel, S. Boyd, and H. Kimura, editors, Recent Advances in Learning and Control, Lecture Notes in Control and Information Sciences, pages 95–110. Springer-Verlag Limited, 2008. http://stanford.edu/~boyd/graph_ dcp.html.
[GB14]
Michael Grant and Stephen Boyd. CVX: Matlab software for disciplined convex programming, version 2.1. http://cvxr.com/cvx, March 2014.
[MB12]
J. Mattingley and S. Boyd. CVXGEN: A code generator for embedded convex optimization. Optimization and Engineering, 13(1):1–27, 2012.
7
