POLI di MI tecnicolano TRAJECTORY PLANNING FOR UAVs BY SMOOTHING WITH MOTION PRIMITIVES C.L. Bottasso, D. Leonello, B. Savini Politecnico di Milano AHS International Specialists' Meeting on Unmanned Rotorcraft Chandler, AZ, January 23-25, 2007

Trajectory Smoothing using Motion Primitives POLITECNICO di MILANO DIA 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.

Trajectory Smoothing using Motion Primitives POLITECNICO di MILANO DIA UAV Control Architecture Target Obstacles Hierarchical three-layer control architecture Hierarchical three-layer control architecture (Gat 1998): Vision/sensor range 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; Reflexive layer: track trajectory generated by tactical layer, control, stabilize and regulate vehicle. Tactical layersmoothing by motion primitivesTactical layer: in this paper, smoothing by motion primitives;

Trajectory Smoothing using Motion Primitives POLITECNICO di MILANO DIA Goal Goal: Plan paths compatible with the flight envelope boundaries for high performance vehicles in complex/unstructured environments. Tactical Layer: Path Planning Target Divide-and-conquer approach 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

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

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

Trajectory Smoothing using Motion Primitives POLITECNICO di MILANO DIA Proposed approach Proposed approach: First compute optimal sequence of way-points in 3-D space connected by straight flight trim conditions; trajectory compatibilization Next, smooth using motion primitives (trajectory compatibilization) in optimal way (minimum time, minimum distance from way-point, etc.) Highlights 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). Smoothing using Motion Primitives

Trajectory Smoothing using Motion Primitives POLITECNICO di MILANO DIA t r ¡ 1 t r W r ¡ 1 W r ABCD » ´ ° r ¢ Â Â 0 e 1 e 3 T i T j T k M ij M j k O e 2 Compatibilization Primitives 1. Turn at a way-point Minimum time transition ½ » ´ ¾ = 1 s i n 2 ° r · ( t E r ¡ 1 ¡ cos° r t E r ) T ( ¡ cos° r t E r ¡ 1 + t E r ) T ¸ ¢ r E Initiation and termination points A & D: t T j = ¢ Â ¡ ¢ Â ij ¡ ¢ Â j k ! T j Remark Remark: other extremal solutions are possible (e.g. minimum distance from way-point)

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

Trajectory Smoothing using Motion Primitives POLITECNICO di MILANO DIA t r ¡ 1 t r W r ¡ 1 W r ABCD¢  T i T j W 0 r W 0 r ¡ 1 EFT k T l M j k M kl M ij  0 e 1 e 3 O e 2 » ´ ¢ z Compatibilization Primitives 3. Climb between closely-spaced way-points Minimum time transition Initiation and termination points A & D: ½ » ´ ¾ = 1 s i n 2 ° r · ( t E r ¡ 1 ¡ cos° r t E r ) T ( ¡ cos° r t E r ¡ 1 + t E r ) T ¸ ¢ r E t T k = ¢  ¡ ¢  ij ¡ ¡ ! T j t T j ¡ ° oor ( ! T j t T j = 2 ¼ ) 2 ¼ ¢ ¡ ¢  j k ¡ ¢  kl ! T k t T j = ¢ z ¡ ¢ z ij ¡ ¢ z j k ¡ ¢ z kl V z ; T j

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

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

Trajectory Smoothing using Motion Primitives POLITECNICO di MILANO DIA Numerical Solution of Maneuver Optimal Control Problems Optimal Control Problem Optimal Control Governing Eqs. Discretize NLP Problem Numerical solution Direct Indirect Indirect approach 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 Direct approach: all above drawbacks are avoided.

Trajectory Smoothing using Motion Primitives POLITECNICO di MILANO DIA P ¤ i » e 1 e 3 O e 2 P ¤ i ¡ 1 P ¤ v ¤ v P Â Â ¤ d Vehicle Model and Reflexive Controller Vehicle model 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 Reflexive controller: State reconstruction by Extended Kalman Filtering; Output-feedback LQR at 50 Hz; PI drift and heading compensator at 1 Hz.

Trajectory Smoothing using Motion Primitives POLITECNICO di MILANO DIA Aggressive Aggressive flight sequence: Goal trajectory With compatibilization Without compatibilization Start Remark Remark: axes not to scale

Trajectory Smoothing using Motion Primitives POLITECNICO di MILANO DIA

Trajectory Smoothing using Motion Primitives POLITECNICO di MILANO DIA Non compatibilized plan Compatibilized plan Results Yaw rate Yaw rate. Solid: plan; dashed: tracked.

Trajectory Smoothing using Motion Primitives POLITECNICO di MILANO DIA Non compatibilized plan Compatibilized plan Results Angular speed tracking error Angular speed tracking error.

Trajectory Smoothing using Motion Primitives POLITECNICO di MILANO DIA Results Non compatibilized plan Compatibilized plan Load factor Load factor. Solid: plan; dashed: tracked.

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

Trajectory Smoothing using Motion Primitives POLITECNICO di MILANO DIA 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.