Outline

1. TRAJECTORY PLANNING FOR UAVs BY SMOOTHING WITH MOTION PRIMITIVESC.L. Bottasso, D. Leonello, B. SaviniPolitecnico di MilanoAHS International Specialists' Meeting onUnmanned Rotorcraft Chandler, AZ, January 23-25, 2007

2. Outline • Path planning for UAV; • Limitation of existing procedures; • Proposed procedure: • - Motion primitives; • - Smoothing using motion primitives; • - Compatibilization primitives; • - Compilation of motion library; • Results; • Conclusions and outlook.

3. UAV Control Architecture Hierarchical three-layer control architecture (Gat 1998): • Strategic layer: assign mission objectives (typically relegated to a human operator); • Tactical layer: generate vehicle guidance information, based on input from strategic layer and sensor information; • Tactical layer: in this paper, smoothing by motion primitives; • Reflexive layer: track trajectory generated by tactical layer, control, stabilize and regulate vehicle. Vision/sensor range Obstacles Target

4. Tactical Layer: Path Planning • Goal: • Plan paths compatible with the flight envelope boundaries for high performance vehicles in complex/unstructured environments. • Divide-and-conquer approach: at each time step • Discretize space and identify candidate way-points; • Compute path by connecting way-points (e.g. A* search); • Smooth path so as to make it compatible with flight envelope boundaries. Obstacles Target

5. Limitations of Simple Planning Strategies Trajectory planning typically very simple (interpolation of way-points): ⇨ No guarantee of feasible within-the-envelope plan; ⇨ Need for Care-Free Maneuvering (CFM) systems (Massey, Horn, Calise & Prasad): Previous work: Anderson et al. 2005 smooth using a simple 2-D kinematic model, improved tracking but still no guarantee in general of feasibility. n 1) Predict limit onset 2) Cue pilot and/or modify control actions so as to avoid boundary violation V

6. Motion Primitives • Vehicle model: Maneuver Automaton (MA) (Frazzoli et al. 2001). • Only two possible states: trim or maneuver (finite-time transition between two trims). • Motion library: • Highlights: • Transcription of vehicle dynamics in compact solution space; • Transcribed dynamics compatible by design with envelope boundaries. • Drawbacks: • Planning with MA difficult (non-linear hybrid optimal control problem). T6: high speed right turn T5: high speed left turn T2: high speed level flight All maneuvers designed using optimal control with envelope protection constraints M21: deceleration from T2 to T1 T1: low speed level flight T4: low speed right turn T3: low speed left turn

7. Smoothing using Motion Primitives • Proposed approach: • First compute optimal sequence of way-points in 3-D space connected by straight flight trim conditions; • Next, smooth using motion primitives (trajectory compatibilization) in optimal way (minimum time, minimum distance from way-point, etc.) • Highlights: • Non-linear model-based planning; • Non-linearities confined within motion library and hidden to planner; • Closed form solution: real-time capable; • Plan compatible with vehicle dynamics: • Easier tracking; • Within flight envelope (up to tracking errors).

8. ¢ ¢ ¢ ¡ ¡    » C W W T A B D O M M ¢ T T k t t i j j e e e  ´ °  k k i j i j j 1 3 2 0 1 1 ¡ ¡ t r r r r r = T E E T ½ ¾ · ¸ ( ) 1 » j t t ¡ c o s ° ! E T 1 ¡ r ¢ r r j r = E E T ( ) 2 t t ¡ + i ´ c o s ° s n ° 1 ¡ r r r r Compatibilization Primitives 1. Turn at a way-point Minimum time transition Remark: other extremal solutions are possible (e.g. minimum distance from way-point) Initiation and termination points A & D:

9. ¢ ¢ ¢ ^ ^ · ¡ ¡ · 0 0 j ( 0 0 ) j j j z z z » » » » » » O W W A B C ¢ D T M T M ¢ W W T W W ¢ t t · · i j j i ¡ ¡ ¡ e e e  z  i i j j i j i 3 1 2 0 1 1 1 2 ¡ ¡ t 1 1 ¡ ¡ r r r r r r r r = T j V T z j ; Compatibilization Primitives 2. Climb between way-points Minimum time transition Initiation point A: If not, way-points too closely spaced for available climb gradient.

10. ¡ ¢ ¢ ¢ ¢ ¢ ¡ ¡ ¡ z z z z i j k k l j ( = ) ° ¢ ¢ ¢ ¢ 2 2 ¡ ¡ ¡ ¡ ¡ t t t   ! o o r ! ¼ ¼   k k l i j T T T T j = T j j j j t j V T = T z j k ; ! T 0 0 k » C W E F ¢ T ¢ D B T W W T T A M M M O W t t e e e ´  z  k l k k l j i i j j 1 2 3 0 1 1 ¡ ¡ 1 ¡ r r r r r r E E T ½ ¾ · ¸ ( ) » 1 t t ¡ c o s ° E ¡ 1 r r r ¢ r = E E T ( ) 2 t t ¡ + i ´ c o s ° s n ° ¡ 1 r r r r Compatibilization Primitives 3. Climb between closely-spaced way-points Minimum time transition Initiation and termination points A & D:

11. Motion Library • Trim Trajectories: computed off-line solving non-linear trim problems for the vehicle model equations. • Maneuvers: computed off-line solving Maneuver Optimal Control (OC) problems (Bottasso et al. 2004) whose ingredients are: • A cost function (index of performance); • Constraints: • Vehicle model equations; • Physical limitations (limited control authority, flight envelope boundaries, etc.); • Procedural limitations. Remark: cost function and constraints collectively define in a compact and mathematically clear way a maneuver.

12. T l l l p a n Z ( ( ( ( ( ( ) ) ) ) ) [ [ ) [ ] ] ] à à f à à à à T T T T _ T T T p a n p a n 0 2 2 2 g x x x x x u u g g = T T j i j i l 0 0 0 ¯ i ( ) ( ) p a n ; ; ; ; , ; ; ; , , . Á d i i J L m a x t m n + m m n n m m a a x x x u x u = ¯ ; ; ; T T 0 Motion Library: Maneuvers Goal: plan a maneuver which is compatible with flight envelope boundaries. Starting trim Arrival trim • Optimal control: min • Subjected to: • Reduced model equations: • Boundary conditions: (initial) • (final) • Constraints:

13. Numerical Solution of Maneuver Optimal Control Problems Optimal Control Problem Optimal Control Governing Eqs. Indirect Discretize Discretize Direct Numerical solution NLP Problem • Indirect approach: • Need to derive optimal control governing equations; • Need to provide initial guesses for co-states; • For state inequality constraints, need to define a priori constrained and unconstrained sub-arcs. • Direct approach: all above drawbacks are avoided.

14. d » ¤ ¤ ¤ ¤ ¤ P P P P O v e e e v   3 1 2 i i 1 ¡ Vehicle Model and Reflexive Controller • Vehicle model: • Blade element and inflow theory (Prouty, Peters); • Quasi-steady flapping dynamics, aerodynamic damping correction; • Look-up tables for aerodynamic coefficients of lifting surfaces; • Effects of compressibility and downwash at the tail due to main rotor; • Process and measurement noise, delays. • Reflexive controller: • State reconstruction by Extended Kalman Filtering; • Output-feedback LQR at 50 Hz; • PI drift and heading compensator at 1 Hz.

15. Goal trajectory With compatibilization Aggressive flight sequence: Without compatibilization Start Remark: axes not to scale

17. Results Yaw rate. Solid: plan; dashed: tracked. Non compatibilized plan Compatibilized plan

18. Results Angular speed tracking error. Non compatibilized plan Compatibilized plan

19. Results Load factor. Solid: plan; dashed: tracked. Non compatibilized plan Compatibilized plan

20. Conclusions • Proposed novel path planning based on smoothing using motion primitives; • Non-linear model-based smoothing; • Non-linearities confined within motion library (either experimental of obtained by off-line numerical solutions); • Motion library (transcribed dynamics) potentially very faithful to plant; • Plan compatible with vehicle dynamics up to tracking errors; • Closed form solution: hard-real-time capable; • Basic concept demonstrated in a high-fidelity virtual environment.

21. Outlook • Real-time implementation and integration in a rotorcraft UAV (in progress) at the Autonomous Flight Lab at PoliMI; • Testing and extensive experimentation; • Integration with vision for fully autonomous navigation in complex environments.