Linear vs Nonlinear MPC for Trajectory Tracking Applied to Rotary Wing Micro Aerial Vehicles

Transcription

1 Linear vs for Trajectory Tracking Applied to Rotary Wing Micro Aerial Vehicles Mina Kamel, Michael Burri and Roland Siegwart Authors are with the Autonomous Systems Lab, ETH Zürich, Switzerland, Abstract: Precise trajectory tracking is a crucial property for Micro Air Vehicles (MAVs) to operate in cluttered environment or under disturbances. In this paper we present a detailed comparison between two state-of-the-art model-based control techniques for MAV trajectory tracking. A classical Linear Model Predictive Controller (LMPC) is presented and compared against a more advanced Nonlinear Model Predictive Controller (NMPC) that considers the full system model. In a careful analysis we show the advantages and disadvantages of the two implementations in terms of speed and tracking performance. This is achieved by evaluating hovering performance, step response, and aggressive trajectory tracking under nominal conditions and under external wind disturbances. Keywords: UAVs, Predictive Control, Trajectory Tracking and Path Following, Real-time control.. INTRODUCTION MAVs are gaining a growing attention recently thanks to their agility and ability to perform tasks that humans are unable to do. Many researches have employed MAVs successfully to perform infrastructure inspection Bircher et al. (5), exploration tasks in unknown environment Bircher et al. (6), search and rescue operations Oettershagen et al. (6) Girard et al. (4) or forest resources monitoring as shown in Steich et al. (6). Precise trajectory tracking is an important feature for aerial robots when operating in real environment under external disturbances that may heavily affect the flight performance, especially in vicinity to structures. Furthermore, there is a big boost in personal drones which need to be able to track a moving object and take nice aerial footage requiring fast and agile trajectory tracking. Currently, it is possible to perform mapping, 3D reconstruction, localization, planning and control completely on-board thanks to the great advances in electronics and semiconductor technology. To fully exploit the robot capabilities and to take advantage of the available computation power, optimization-based control techniques are becoming suitable for real-time MAV control. In this paper, we present a comparison between two of the stateof-the-art trajectory tracking controllers. A LMPC based on a linearized model of the MAV and a more advanced NMPC that considers the full system dynamics. The goal of this comparison is to emphasis the benefits and drawbacks of considering the full system dynamics in terms of performance improvement, disturbance rejection and computation effort. The general control structure followed in this paper is a cascade control approach, where a reliable and system-specific lowlevel attitude controller is present as inner-loop, while a modelbased trajectory tracking controller is running as an outer-loop. This cascade approach is justified by the fact that critical flight control algorithm is running on a separate navigation hardware, This work has received funding from the European Union s Horizon Research and Innovation Programme under the Grant Agreement No.6448, AEROWORKS. Fig.. Sequence of the MAV poses during aggressive trajectory tracking experiment under wind disturbances. typically based on miro-controller such as PixHawk Meier (6) and Navio Emild Ltd (6), while high level tasks are typically executed on a more powerful on-board computer. This introduces a separation layer to keep critical code running despite any failure in the high-level computer. To achieve high performance with the cascade approach, it is necessary to account for the inner-loop dynamics in the trajectory tracking controller. Therefore, a system identification has been performed on the MAV to identify the closed loop attitude dynamics. paper contributions The main contribution of this paper is to perform a thorough comparison between a LMPC and NMPC controller enabling dynamic trajectory tracking for MAVs together with an open source C++ implementation of both controllers on Kamel (6). The comparison aims to highlight the benefits of full system dynamics consideration and its effect on the flight envelope.

2 The remainder of this paper is organized as follows: In Section the related work of the problem of MAV trajectory tracking control is discussed. In Section 3 the MAV model is presented. The Linear and Nonlinear Model Predictive Controller are presented in Section 4 and Section 5 respectively. The external disturbance observer is briefly discussed in Section 6. Finally, experimental results are presented and discussed in Section 7.. RELATED WORK F i F aero,i F T,i Bv Bv F i n i B y B z B x Many researchers have been focusing on the control problem for MAVs. Among the first controller evaluation on MAV is the work done by Samir Bouabdallah in Bouabdallah et al. (4) where a comparison between classic PID controller and LQ controller has been conducted for attitude stability. Surprisingly the authors found that a simple PID controller was performing better than an LQ controller for attitude stability. The authors justify this result as a result of imperfect model. A commonly used nonlinear controller was proposed by Lee et al. (), where the attitude error is directly calculated on the manifold to have a globally stable controller. Using a very similar formulation, impressive trajectory tracking results were obtained using high gain feedback control Mellinger et al. (). These controllers, however, are not able to guarantee any state or input constraints and the trajectory needs to be selected carefully. Model Predictive Controller (MPC) on the other hand is able to directly include constraints in the optimization, which is important for real systems with physical constraints. In Bemporad et al. (9) the authors employed a hierarchical MPC to achieve stability and autonomous navigation for MAVs. To achieve stable flight, a is employed while a hybrid MPC approach is used to generate collision-free trajectory which is tracked by the. However, the proposed stabilizing controller has not been tested experimentally and controller performance is not evaluated under disturbances. To add robustness with respect to time delays in on-board estimation Blösch et al. () proposed to separate the controller into an attitude controller running on the micro controller and an Linear-Quadratic Regulator (LQR) for position tracking. For better tracking performance the closed loop dynamics of the attitude controller was identified and included in the LQR This approach was extended in Burri et al. () with a LMPC for position tracking. In this work we extended this controller with a simple aerodynamic drag model and use it as our reference implementation. A similar formulation was also used in Alexis et al. (6). The authors present a Robust MPC approach to stabilize the vehicle and demonstrate the effectiveness of the proposed controller under wind disturbances and slung load. The authors in Berkenkamp and Schoellig (5) propose a novel approach to deal with conservativeness of robust control techniques. The system uncertainty is learned online using Gaussian Process and the linear robust controller is updated according to the learned uncertainties achieving less conservative linear robust controller. 3. MAV MODEL In this section we present the MAV model employed in the controllers formulation. We first introduce the full vehicle model I z I y I x p, RIB mg I Fig.. A schematic of MAV showing Forces and torques acting on the MAV and aerodynamic forces acting on a single rotor. Inertial and CoG frames are also shown. and explain the forces and moment acting on the system. Next, we will briefly discuss the closed-loop attitude model employed in the trajectory tracking controller. System model We define the world fixed inertial frame I and the body fixed frame B attached to the MAV in the Center of Gravity (CoG) as shown in Figure. The vehicle configuration is described by the position of the CoG in the inertial frame p R 3, the vehicle velocity in inertial frame v, the vehicle orientation R IB SO(3) and the body angular rate ω. The main forces acting on the vehicle are generated from the propellers. Each propeller generates thrust proportional to the square of the propeller rotation speed and angular moment due to the drag force. The generated thrust F T,i and moment M i from the i th propeller is given by: F T,i = k n n i e z, M i = ( ) i k m F T,i, (a) (b) where k n and k m are positive constants and e z is a unit vector in z direction. Moreover, we consider two important effects that appear in case of dynamic maneuvers. These effects are the blade flapping and induced drag. The importance of these effects stems from the fact that they introduce additional force in the x y rotor plane adding some damping to the MAV velocity as shown in Mahony et al. (). It is possible to combine these effects as shown in Omari et al. (3); Burri et al. (6) into one lumped drag coefficient k D. This leads to the aerodynamic force F aero,i : F aero,i = f T,i K drag R T IBv () where K drag = diag(k D, k D, ) and f T,i is the z component of the i th thrust force. The motion of the vehicle can be described by the following equations:

3 ṗ = v, ( v = m + [ g R IB N r ] Ṙ IB = R IB ω, (3a) ) N r F T,i R IB F aero,i + F ext i= J ω = ω Jω + A i=, (3b) n. n N r, (3c) (3d) where m is the mass of the vehicle and F ext is the external forces acting on the vehicle (i.e wind). J is the inertia matrix, A is the allocation matrix and N r is the number of propellers. Attitude model We follow a cascaded approach as described in Blösch et al. () and assume that the vehicle attitude is controlled by an attitude controller. For completeness we quickly summarize the findings in the following. To achieve accurate trajectory tracking, it is crucial for the high level controller to consider the inner loop system dynamics. Therefore, it is necessary to consider a simple model of the attitude closed-loop response. These dynamics can either be calculated by simplifying the closed loop dynamic equations (if the controller is known) or by a simple system identification procedure in case of an unknown attitude controller (on commercial platforms for instance). In this work we used the system identification approach to identify a first order closedloop attitude response. The inner-loop attitude dynamics are then expressed as follows: φ = (k φ φ cmd φ), (4a) τ φ θ = (k θ θ cmd θ), τ θ (4b) ψ = ψ cmd (4c) where k φ, k θ and τ φ, τ θ are the gains and time constant of roll and pitch angles respectively. φ cmd and θ cmd are the commanded roll and pitch angles and ψ cmd is commanded angular velocity of the vehicle heading. The aforementioned model will be employed in the subsequent trajectory tracking controllers to account for the inner-loop dynamics. Note that the vehicle heading angular rate ψ is assumed to track the command instantaneously. This assumption is reasonable because the MAV heading angle has no effect on the MAV position. 4. LINEAR MPC In this section, we describe a Model Predictive Controller to achieve trajectory tracking using a simple model of the MAV Borrelli et al. (5). In Subsection 4., the vehicle model is linearized around hovering condition. In Subsection 4., we formulate the LMPC controller and discuss nonlinearity compensation. Finally, in Subsection?? we briefly discuss the external disturbances estimator. 4. Model Linearization The vehicle model can be linearize around hovering condition assuming small attitude angles and vehicle heading aligned with the first axis of inertial frame (ψ = ). We define the following state vector: x = ( p T v T Iφ I θ ) T, (5) and control input vector: u = ( I φ cmd I θ cmd T cmd ) T (6) where T cmd is the commanded thrust, which we assume can be achieve instantaneously as the motors dynamics are typically very fast. I φ, I θ are the heading free roll and pitch angles which we denote in inertial frame to get rid of the vehicle heading ψ from the model. The transformation between attitude angles and heading free attitude angles is given by ( φ = θ) ( cos ψ sin ψ sin ψ cos ψ ) ( Iφ Iθ ). (7) After linearization and discretization of the model described in Section 3, the following linear state-space model is obtained: x k+ = Ax k + Bu k + B d F ext,k. (8) 4. Controller Formulation The LMPC scheme repeatedly solves the following Optimal Control Problem (OCP): N ( (xk x ref,k ) + uk u ref,k Q x R u min U,X k= + xn x ref,n P subject to x k+ = Ax k + Bu k + B d F ext,k ; F ext,k+ = F ext,k, k =,..., N u k U x = x (t ), F ext, = F ext (t ). where Q x is the penalty on the state error, R u is the penalty on control input error and P is the terminal state error penalty. x ref,k and u ref,k are respectively the target state vector and target control input at time k. U is the control input constraint given by: { ] ]} U = u R 3 [ φmin θ min T cmd,min u [ φmax θ max T cmd,max (9) () Note that we assume constant disturbances along the prediction horizon. Only the first control input u is applied to the system, and the process is repeated the next time step in a receding horizon fashion. The attitude commands I φ cmd, I θ cmd is transformed into the MAV body frame by applying (7). Moreover, the non-zero MAV attitude effects on the generated lift can be compensated. The actual thrust control input to the MAV low-level attitude controller is given by: T cmd = T cmd + g cos φ cos θ () To achieve a better tracking performance of dynamic trajectories, we can include a feed-forward term by setting the reference control input u ref as follows:

4 u ref,k = ( B ÿ ref,k /g B ẍ ref,k /g B z ref ) T () where B ẍ ref,k, Bÿ ref,k, B z ref,k are the desired trajectory acceleration expressed in MAV body frame B. 5. NONLINEAR MPC In this section, a continuous time NMPC controller that considers the full system dynamics explained in Section 3 is formulated. In Subsection 5. we formulate the OCP and in Subsection 5. we discuss the technique employed to solve the OCP to achieve real-time implementation. 5. Controller Formulation To formulate the NMPC, we first define the following state vector: x = ( p T v T φ θ ψ ) T, (3) and control input vector: u = ( φ cmd θ cmd T cmd ) T (4) Now, we can define the OCP as follows: min U,X T t= x(t) xref (t) Q x + u(t) uref (t) R u dt + x(t ) xref (T ) ) P subject to ẋ = f(x, u); u(t) U x() = x (t ). (5) where f is composed by Equations (3a), (3b) and (4). The controller is implemented in a receding horizon fashion, where the aforementioned optimization problem needs to be solved every time step in and only the first control input is actually applied to the system. Solving (5) repeatedly in real-time is not a trivial task. Direct methods techniques has gained a particular attention recently to address optimal control problems. In the next subsection we describe the method employed by the solver to solve (5). 5. OCP Solution Multiple shooting technique is employed to solve (5). The system dynamics and constraints are discretized over a coarse discrete time grid t,..., t N within the time interval [t k, t k+ ]. For each interval, a Boundary Value Problem (BVP) is solved, where additional continuity constrains are imposed. An implicit RK integrator of order 4 is employed to forward simulate the system dynamics along the interval. At this point, the OCP can be expressed as a Nonlinear Program (NLP) that can be solved using Sequential Quadratic Programming (SQP) technique where an active set or interior point method can be used to solve the Quadratic Program (QP). 6. EXTERNAL DISTURBANCES ESTIMATION In this section, we discuss the external disturbances estimator employed to achieve offset-free trajectory tracking. The external disturbances F ext is estimated by an augmented state Extended Kalman Filter (EKF) that includes external forces. The EKF is employing the same model used in the controller design, but it includes also vehicle heading angle ψ. In this way, the external forces will capture also modeling error achieving zero steady state tracking error Borrelli et al. (5). The filter is employing the inner-loop attitude dynamics presented in Equation (4). 7. IMPLEMENTATION AND RESULTS In this section, we present the results of multiple experiments to evaluate the performance of the previously presented LMPC and NMPC. 7. System Description The aforementioned LMPC and NMPC have been implemented and evaluated on Asctec NEO hexacopter equipped with an Intel 3. GHz i7 Core processor, 8 GB RAM. The on-board computer is running Robot Operating System (ROS), and the controllers are integrated into ROS. The vehicle state is estimated using an EKF as described in Lynen et al. (3) by fusing vehicle Inertial Measurement Unit (IMU) with external Vicon motion capture system. 7. Controllers Implementation To implement the LMPC, an efficient QP solver using CVX- GEN code generator framework by Mattingley and Boyd () is employed to solve the optimization problem (9) every time step. While the NMPC is implemented by solving (5) every time step using an efficient solver generated using ACADO toolkit by Houska et al. (). A real time iteration scheme based on Gauss-Newton is employed to approximate the optimization problem and iteratively improve the solution during the runtime of the process. In this way, an improvement of the computation time is achieved. Both controllers are running at Hz while internally the prediction is performed at Hz, in this way we achieve longer prediction horizon with less computational efforts. The prediction horizon is chosen to be steps resulting into seconds prediction horizon. The penalties for both controllers are chosen such that the cost functions are identical, in this way we emphasis the benefits of full dynamics model over a linearized model. The terminal cost in both controllers is chosen to account for the infinite horizon cost. 7.3 Hovering Performance LMPC NMPC without wind ( ) (..7.5 ) with wind (.6.9. ) (...9 ) Table : RMSE of individual axes of LMPC and NMPC during hovering experiment with and without external disturbances. Error is in cm. The purpose of these experiments is to evaluate the hovering performance of the MAV in nominal conditions and under external wind disturbances. Figure 3 shows the x, y error under no external disturbances for LMPC and NMPC. Clearly the performance of both controllers is very comparable, and this result is expected since the LMPC is employing a model linearized around hovering condition. The RMSE is found to be.84 cm and.5 cm in the case of LMPC and NMPC respectively.

5 3 Hovering performance of Linear and No external disturbances is acting on the system Set point 4 3 Step repsonse of linear and non No external disturbances reference y err x [m] x err Fig. 5. Step response in x direction. Fig. 3. x y error during hovering using LMPC and NMPC. No external wind is applied. 4 Thrust command during step response Hovering performance of Linear and under external wind set point wind direction thrust [N] y err Fig. 6. Thrust command during step response. clearly highlights that the NMPC outperforms the LMPC in this case. 7.5 Aggressive Trajectory Tracking x err Fig. 4. x y error during hovering using LMPC and NMPC. An external wind of around m /s is applied in the direction indicated by yellow arrows. Under external wind of around m /s, the x y error is shown in Figure 4. Both controllers achieve an RMSE of.7 cm and.5 cm in the case of LMPC and NMPC respectively. The RMSE is shown in Table. 7.4 Step Response In this experiment, we compare both controllers capabilities to track an aggressive polynomial trajectory under external disturbances. The trajectory tracking error of both controllers is shown in Figure 7, while the actual trajectory tracking is shown in Figure 8. The RMSE is found to be.8 cm and 7. cm in the case of LMPC and NMPC respectively. To compare the computation effort required by each controller, in Figure 9 we show the time employed by the solver to find the control action. Clearly, the real time iteration scheme implemented in the NMPC greatly improves the solver speed, achieving an average solve time of.45 ms for the NMPC compared to.35 ms for the LMPC case, improving the computation effort by a factor of 5. Even though the tracking error is weighted in the same way in both controllers, the NMPC outperforms the LMPC in the step response as can be seen in Figure 5. The main reason for the is the exploitation of the full system dynamics, in particular, the thrust command coupling with lateral motion. This leads to faster response with no noticeable overshoot. Figure 6 shows the thrust command during step response. The NMPC exploits the full thrust command between T cmd,min and T cmd,max while the LMPC is employing the thrust command in a very limited way, which is introduced by the nonlinear compensation shown in Equation (). To perform qualitative comparison, we compare the rise time and overshoot percentage. The rise time is around.6 s and. s for the LMPC and NMPC respectively. While the overshoot percentage is found to be.98 % for both controllers. This 8. CONCLUSIONS In this paper we presented a Linear and Nonlinear Model Predictive Controllers for trajectory tracking of MAV. Detailed comparison has been performed during hovering, step response and aggressive trajectory tracking under external disturbances. Both controllers showed comparable behavior while the NMPC showed a slightly better disturbance rejection capability, step response, tracking performance and computational effort. An open source implementation of these controllers can be found on Kamel (6). REFERENCES Alexis, K., Papachristos, C., Siegwart, R., and Tzes, A. (6). Robust model predictive flight control of unmanned rotor-

6 x err y err z err Aggressive trajectory tracking error Linear MPC Fig. 7. Aggressive trajectory error plots under external wind disturbances. Wind speed is measured to be around m /s. x [m] y [m] Aggressive trjectory tracking of Linear and Linear MPC Reference z [m] Fig. 8. Aggressive trajectory tracking plots under external wind disturbances. solver time [ms] Computation effort of Linear MPC and during aggressive trajectory tracking t [s] Fig. 9. Computation time comparison between LMPC and NMPC during aggressive trajectory tracking. crafts. Journal of Intelligent & Robotic Systems, 8(3), Bemporad, A., Pascucci, C., and Rocchi, C. (9). Hierarchical and hybrid model predictive control of quadcopter air vehicles. {IFAC} Proceedings Volumes, 4(7), 4 9. doi: 3rd {IFAC} Conference on Analysis and Design of Hybrid Systems. Berkenkamp, F. and Schoellig, A.P. (5). Safe and robust learning control with Gaussian processes. In Proc. of the European Control Conference (ECC), Bircher, A., Kamel, M., Alexis, K., Oleynikova, H., and Siegwart, R. (6). Receding horizon next-best-view planner for 3d exploration. In 6 IEEE International Conference on Robotics and Automation (ICRA), doi:. 9/ICRA Bircher, A., Kamel, M., Alexis, K., Burri, M., Oettershagen, P., Omari, S., Mantel, T., and Siegwart, R. (5). Threedimensional coverage path planning via viewpoint resampling and tour optimization for aerial robots. Autonomous Robots,. Blösch, M., Weiss, S., Scaramuzza, D., and Siegwart, R. (). Vision based mav navigation in unknown and unstructured environments. In Robotics and automation (ICRA), IEEE international conference on, 8. IEEE. Borrelli, F., Bemporard, A., and Morari, M. (5). Predictive Control for Linear and Hybrid Systems. Bouabdallah, S., Noth, A., and Siegwart, R. (4). Pid vs lq control techniques applied to an indoor micro quadrotor. In Intelligent Robots and Systems, 4.(IROS 4). Proceedings. 4 IEEE/RSJ International Conference on, volume 3, IEEE. Burri, M., Nikolic, J., Oleynikova, H., Achtelik, M.W., and Siegwart, R. (6). Maximum likelihood parameter identification for mavs. In 6 IEEE International Conference on Robotics and Automation (ICRA), doi:.9/ ICRA Burri, M., Nikolic, J., Hürzeler, C., Caprari, G., and Siegwart, R. (). Aerial service robots for visual inspection of thermal power plant boiler systems. In Applied Robotics for the Power Industry (CARPI), nd International Conference on, IEEE. Emild Ltd (6). Navio autopilot. URL com. Accessed: 6-8. Girard, A., Howell, A., and Hedrick, J. (4). Border patrol and surveillance missions using multiple unmanned air vehicles. In Decision and Control, 4. CDC. 43rd IEEE Conference on. Houska, B., Ferreau, H., and Diehl, M. (). ACADO Toolkit An Open Source Framework for Automatic Control and Dynamic Optimization. Optimal Control Applications and Methods, 3(3), Kamel, M. (6). Open source mav mpc controllers. URL rw. Lee, T., Leoky, M., and McClamroch, N.H. (). Geometric tracking control of a quadrotor uav on se (3). In 49th IEEE conference on decision and control (CDC), IEEE. Lynen, S., Achtelik, M.W., Weiss, S., Chli, M., and Siegwart, R. (3). A robust and modular multi-sensor fusion approach applied to mav navigation. In Intelligent Robots and Systems (IROS), 3 IEEE/RSJ International Conference on, 393

7 399. IEEE. Mahony, R., Kumar, V., and Corke, P. (). Multirotor aerial vehicles: Modeling, estimation, and control of quadrotor. IEEE Robotics Automation Magazine, 9(3), 3. doi:.9/mra Mattingley, J. and Boyd, S. (). Cvxgen: A code generator for embedded convex optimization. Optimization and Engineering, 3(), 7. Meier, L. (6). Pixhawk autopilot. URL pixhawk.org. Accessed: 6-8. Mellinger, D., Michael, N., and Kumar, V. (). Trajectory generation and control for precise aggressive maneuvers with quadrotors. The International Journal of Robotics Research, Oettershagen, P., Stastny, T., Mantel, T., Melzer, A., Rudin, K., Gohl, P., Agamennoni, G., Alexis, K., and Siegwart, R. (6). Long-endurance sensing and mapping using a handlaunchable solar-powered uav. In Field and Service Robotics, Springer. Omari, S., Hua, M.D., Ducard, G., and Hamel, T. (3). Nonlinear control of vtol uavs incorporating flapping dynamics. In 3 IEEE/RSJ International Conference on Intelligent Robots and Systems, doi:.9/iros Steich, K., Kamel, M., Beardsleys, P., Obrist, M.K., Siegwart, R., and Lachat, T. (6). Tree cavity inspection using aerial robots. In 6 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).