when nonlinearity and coupling dominate the vehicle dynamics. ... and rescue activities. In order to deploy RUAVs ... the heavy computation load. In this paper ...
Proceedings of the American Control Conference Anchorage, AK May 8-10,2002
Nonlinear Model Predictive Tracking Control for Rotorcraft-based Unmanned Aerial Vehicles H. Jin Kim
David
H. Shim
Shankar Sastry
Department of Electrical Engineering & Computer Sciences University of California at Berkeley, Berkeley, CA 94720
{ j in,hcshim,sastry}@eecs .berkeley .edu Abstract In this paper, we investigate the feasibility of a nonlinear model predictive tracking control (NMPTC) for autonomous helicopters. We formulate a NMPTC algorithm for planning paths under input and state constraints and tracking the generated position and heading trajectories, and implement an on-line optimization controller using gradient-descent method. The proposed NMPTC algorithm demonstrates superior tracking performance over conventional multi-loop proportional-derivative (MLPD) controllers especially when nonlinearity and coupling dominate the vehicle dynamics. Furthermore, NMPTC shows outstanding robustness to parameter uncertainty, and input saturation and state constraints are easily incorporated. When the cost includes an potential function with a possibly moving obstacle or other agents' state information, the NMPTC can solve the trajectory planning and control problem in a single step. This constitutes a promising one-step solution for trajectory generation and regulation for RUAVs, which operate under various uncertainties and constraints arising from the vehicle dynamics and environmental contingencies. The computation load of this approach is significantly less than many existing model predictive algorithms, thus enabling real-time applications for autonomous helicopters.
Figure 1: A Berkeley RUAV in autonomous flight
reconnaissance, aerial surveillance, public safety search and rescue activities. In order to deploy RUAVs as intelligent robotic vehicles in this wide range of applications, it is essential that each RUAV be endowed with capabilities to independently sense, plan and act in coordination with other agents or environments. During the last decade, there has been considerable progress in RUAV research including modeling [l]and control [2, 31. However, wideenvelope and high-accuracy motion controllers for RUAVs have not been realized. Difficulties in measuring aerodynamic properties, uncertainties associated with system parameters, non-negligible disturbances as well as their highly nonlinear, time-varying, multi-variable nature have impeded implementing fully autonomous operations of RUAVs.
1 Introduction
Rotorcraft-based unmanned aerial vehicles (RUAVs) have emerged as useful platforms for intelligent mobile robots, due t o their flight capabilities. The unique lift generation mechanism of rotorcrafts enables hover, vertical take-off/landing, pirouette, and sideslip, which cannot be achieved by fixed-wing aircraft. These versatile flight modes are often needed for detection, location, and tracking of targets, in applications including 'This research was supported by the O N R grants N00014-971-0946and N00014-00-1-0621, and DARPA MICA F33615-01-C3150.
O-7803-7298-0/02/$17.000 2002 AACC
3576
Most approaches used for civilian and military helicopters are still based on classical frequency-domain design methods [4,51. Although they show a reasonable performance in hover and low-speed flight modes, they fall short of exploiting the full flight envelop of an autonomous helicopter. Feedback linearization is an immediate answer to account for the nonlinearity, yet the implementation is hindered by the non-minimum phase characteristics of helicopter dynamics as well as
the serious lack of robustness (21.
tracking controller under input saturation and state constraints. Section 3 presents the simulation results, comparing the performance of NMPTC with that of classical linear controller, and confirming the validating of NMPTC as a combined solution t o trajectory planning and control of RUAVs. Section 4 concludes the paper with the future directions.
Model predictive control (MPC), also referred t o as receding horizon control (RHC), has emerged as an attractive alternative. It employs an explicit model of the plant to predict the future output behavior. With this prediction, optimal control problems are solved, preferably on-line, t o minimize tracking error over a future horizon, possibly in the presence of constraints of input and states. As one of very few methods available for multi-variable systems subject to hard constraints on controls and states, MPC has been used in a wide variety of applications, especially when plants t o be controlled are sufficiently ' ~ 1 0 ~ ' .
2 Identification and Control This section addresses the issues of dynamic model identification, tracking control and designing a cost function t o combine trajectory generation and motion control under input saturation and state constraints in a single problem.
Nonlinear model predictive control (NMPC) is gromising as a control technique that explicitly addresses nonlznear systems with and operating constraints, and other demanding performance requirements. This comprehensiveness is particularly welcomed for RUAVs due t o their nonlinear dynamics and input/state constraints. However, a lot of theoretical and practical issues remain unresolved and there have been few a p plications with the exception of [6]. Especially, the need for 'fast' control algorithms for many dynamic systems has constricted the implementation of NMPC. Recently, Wan et a1 [7]presented model predictive neural control, a combination of neural network feedback controller and a state-dependent Riccati equation controller. However, the method is unlikely to be directly employable as an on-line controller for RUAVs due t o the heavy computation load.
2.1 Dynamic Model Identification Multi-input multi-output (MIMO), nonlinear characteristics, severe disturbance, and a wide flight envelope must be accounted for t o acquire precise models. A lumped parameter method models the dynamics as a combination of main rotor, tail rotor, and other miscellaneous parts such as fuselage and stabilizer fins. It should be noted that, contrary t o common misrepresentations, the helicopter dynamics are not a cascade of servorotor-attitude-translational sub-dynamics blocks. The inherent feedback of pitch and roll rates p and q, strengthened by the Bell-Hiller stabilizer system, modifies the rotor dynamics significantly. When uncontrolled, the coupling of horizontal velocities U and w with the lift generation and attitude dynamics leads t o instability. In our work, we employ a six degrees-offreedom rigid-body model augmented with first-order approximation of servorotors in [9]. The helicopter is an under-actuated system, whose configuration space is S E ( 3 ) 5 R3 x SO(3) yet only four degree-of-freedom can be achieved by four inputs t o the lateral cyclic pitch, longitudinal cyclic pitch, main rotor collective pitch, and tail rotor collective pitch. The system equation is given in the following:
In this paper, following the approach of Sutton et a1 [8], a numerically efficient nonlinear model predictive tracking control (NMPTC) algorithm is proposed for tracking control of autonomous helicopters. The tracking control problem is formulated as a cost minimization problem in the presence of input and state constraints. The minimization problem is solved with a gradient-descent method, which is computationally light and fast. The proposed NMPTC is applied for the trajectory generation and tracking control layer in a hierarchical flight management system for RUAVs. We compare the performance of this approach with the existing multi-loop P D (MLPD) controller of a Berkeley RUAV through a benchmark test of helical ascent with coordinated heading tracking, which signifies the nonlinearity and coupling of the helicopter dynamics. Furthermore, by formulating the cost to include the state information of moving obstacles or other agents, input saturation and state constraints, we show the performance of the NMPTC as a one-step solution for trajectory planning and control of RUAVs.
where S and B denote spatial and body coordinate, 4, 8 , and II, denote roll, pitch, and yaw, and p , q, and r are their rates, respectively. q, and bl, are longitudinal and lateral flapping angles, and r f b is the feedback
The remaining parts of this paper are divided as follows: Section 2 presents the dynamic model identification, and the design of a nonlinear model predictive
3577
gyro system state. The overall system model is divided into the kinematics (Eqn. (1) and (2)) and the systemspecific dynamics (Eqn. (3)).
By defining the Hamiltonian function as
A transformation matrix between the spatial and body velocities is given by RB'S E S0(3), i.e., the rotational matrix of the body axis relative to the spatial axis,represented by Z Y X Euler angles [4,0,$]. Newton-Euler equation yields the differential equation (3) for x D , which is characterized by nonlinear functions of the force and moment terms [9].
( 5 ) can be written as
H k
2.2 Tracking Controller Design The unstable RUAV dynamics need t o be stabilized by a proper feedback control, which is computed by the onboard real-time controller. The classical SISO approaches are still favored in industry or military applications, due t o their straightforward and intuitive design. A multi-loop SISO controller [5] and a linear robust control system designed with p-synthesis theory [9] have been applied t o RUAVs in our project. The MLPD controller demonstrated stable responses with 0.5 m accuracy in x and y direction, 0.1 m in the altitude, and 3" in the heading, when employed for hover and slow motion. However, it is impossible t o expect good tracking performance for complex trajectories due to nonlinear characteristics, coupling among modes, and inputlstate saturation of the RUAV dynamics. In order t o resolve with these issues, we propose engaging a nonlinear model predictive controller as a tracking layer.
=L
( x k ~ ~ukk ,)
+ Az+;,f ( x k , u k ) ,
(9)
N-1
J =~
-
( X N ) X$XN
-k
[Hk
- X r X k ] 4-
Ho.
(10)
k=l
Since we want to choose we take a look at
{Uk}:-'
that minimizes J ,
By choosing
we have
Only for controller design purposes, we discretize Eqn. (2) to =f
xk+l fd(xk) Bd
(xk,u k )
x k
fd(xk)
+BdUk
(4)
With a candidate input sequence {Uk}:-' and a given X O , on-line optimizations can be achieved by the following process [8]:
+T s f c ( x k )
4 T,B,,
where T, is the sampling time. For tracking, we define a cost function
while IAJ( > e do for k = l , . . ., N compute { X k } ? using (4) end fork=N,...,l compute Xk using (11) and (12) end for k = l;.. , N compute using (14) end ifAJ5O u k + l := u k + A k a for k = 0 , " . , N - 1 k else reduce A k end while
N-1
J =$(YN)
+
L(XktYkr u k )
(5)
k=O
l-T ~ ( Y N=) ,YNPOYN A
lL(Xk,Yk,Uk) = -Yz&Yk 2 A
(6) 1
+ -2x z s x k
f
1 ZuzRuk,
e
(7)
where Y 2 Y d - y , y = Cx E ]Rny, Y d k the desired trajectory, and S is introduced t o bound the state variables that do not directly appear in y . By introducing a sequence of Lagrange multiplier vectors { x k E ]Rnz}r=l, we rewrite Eqn. ( 5 ) : N-1
J =4(YN)
+
L(Xk,Ykr u k )
+xz+;,[f(Xk,u k ) - xk+l].
k=O
(8)
3578
By initializing u k at the beginning of the optimization a t each time step with the u k of the previous time sample, the iteration count reduces significantly.
2.3 Trajectory Generation and Tracking under Input/State Constraints The cost (5) can be formulated t o reflect the aspect of a potential function for path planning in the environment with moving obstacles or other agents. This allows the trajectory generation and approach t o be combined into a single problem. In order t o generate physically realizable trajectories, input constraints are enforced by projecting each uk into the constraint set. In our helicopter model, this corresponds to [uals,Ubls,UeM,ue,] E [-I, I]*. State constraints are also incorporated as an additional penalty in the cost function J :
all system parameters except for the gravity terms and evaluate the tracking performance for the spiral ascent profile identical t o that used in the previous case. As shown in Figure 4, while MLPD shows larger tracking error than in Figure 3, NMPTC shows negligible tracking error increase due t o the adaptive nature of its on-line optimization.
3.2 Collision-avoidance Planning and Control under Input/State Constraints In this simulation shown in Figure 2, three helicopters HO, H1, and H2 are originally given straight-line trajectories that will lead t o a collision at (lOO,O, 33) ft. The potential function (16) is added on-the-fly into the cost function of H1 and H2, to replan the trajectory, i.e., for j = 1 , 2 ,
where zk,yk, and zk denote the position of the helicopter 1 = 0 , 1 , 2 at time k , and constants a j , b j and K j determine the shape of repulsive potential field and thus, the adjusted trajectory.
3 Simulations
In order to generate the proper control input while trying t o avoid the collision, the input saturation conditions were enforced. Also included in the cost function is the state constraints in the form of Eqn (15), with [&at, %at Usat vsat 3 wsat Psat qsat ~ s a t l = [.rr/6, ~/6,16.7,16.7,16.7ft/s, 7r/2, ~ / 2r,/ 3 rad/s], which represent the safe region of state space.
In this section, we evaluate the effectiveness of the nonlinear model predictive trajectory planning and tracking controller proposed in Section 2 in simulations.
3.1 Tracking Performance Comparison This section compares the performance of the nonlinear model predictive tracking controller with the existing MLPD using a spiral ascent profile shown in Figure 3 and Figure 4. An additional constraint is imposed on the heading of an RUAV so that its nose pointing toward the center of the spiral trajectory, i.e., [zf,y:, zf, &2] = [R(t)cos R ( t )sin -$t, 7r for 0 5 t 5 30 s , R ( t ) = 5 $ ft. This particular trajectory is chosen t o differentiate the cability of controllers t o handle the nonlinear kinematics as well as the multivariable coupling in the system dynamics.
Et],
st, +
gt,
7
1
7
Figure 2(a) shows the collision-free trajectories the three helicopters dynamically replanned and followed, Figure 2(b) shows the control inputs generated by NMPTC. Figure 2(c) shows the state variables &,8, U , v of H1 and H2 are mostly kept within the proper limits.
+
3.3 Implementational Issues Horizon length N , as well as the weighting matrices Po, Q, and R, are important design parameters related t o the closed-loop stability. Step-size Ak should be carefully chosen t o consistently reduce the cost during the iteration. The selection of Ak is also related with the horizon length N as well as other weightings. We selected N = 25 30, and Ak = 0.0001 0.001 for the simulations presented in this paper. The effect of tuning these parameters and adjustment of potential function to also reflect the type, speed or heading of objects require further investigation.
The dynamic model used in the simulation is for a UAV based on a Yamaha R-50 industrial helicopter. In Figure 3(a), an RUAV controlled by the NMPC follows the given spiral trajectory and desired heading @ d . As shown in Figure 3(b), when the RUAV is controlled by MLPD controller, the deviation from the spiral trajectory becomes larger as time elapses. Figure 3(c) presents the tracking error of &'(t), y S ( t ) , z S ( t ) and +(t)under two controllers. The controller output u(t) of both cases are shown in Figure 3(d). The failure of the linear controller to follow complex trajectory is attributed t o its deficiency to handle the coupling as well as the nonlinear kinematics of rigid-body motion.
N
-
The computational load of the proposed NMPTC a l g e rithm per time step is around 2 MFLOPs (mega floating operations), which is significantly lower than other methods [7]. The required speed for our application is around 100 MFLOPs per second (i.e. sampling rate of 50Hz x 2 MFLOPs). This is within reach of today's high-end CPUs, which offer sustained floating-point
In the next simulation shown in Figure 4, we consider the impact of model uncertainty on the tracking performance. We introduce up to 20 % perturbation to
3579
.
on-line optimization controller using a gradient-descent method. The proposed NMPTC is evaluated in a spiral ascent with nose-in constraint under various cases. The result shows that NMPTC outperforms the conventional multi-loop PD controller in the tracking performance when the nonlinear kinematics and coupling dominate the vehicle dynamics even in the presence of model uncertainty. Furthermore, the potential function and state saturation penalty were added to the cost and the input saturation conditions were enforced t o show the viability of replanning trajectories dynamically t o avoid the collision while trying to minimize the quadratic error. The computational load of our NMPTC formulation with gradient-descent search is low enough for real-time control application of RUAVs. The effect of tuning the cost weight matrices and constants in the potential function is a n issue that requires further study to achieve good trajectory generation and tracking performance in an environment with various types and speed of objects. The proposed algorithm will be integrated into the hierarchical control system for a series of test flights.
References T. Kanade B. Mettler, M. B. Tischler. System identification of small-size unmanned helicopter dynamics. In American Helicopter Society 55th Forum, Montreal, Quebec, Canada, May 1999. [2] H. Shim, T. J. Koo, F. Hoffmann, and S. Sastry. A comprehensive study of control design for an autonomous helicopter. In Proc. of 37th IEEE Conference o n Decision and Control, pages 3653-3658, Dec. 1998. [3] J. E. Corban, A. J. Calise, and J. V. R. Prasad. Im[l]
plementation of adaptive nonlinear control for flight test on
an unmanned helicopter. In Proc. of 37th IEEE Conference o n Decision and Control, pages 3641-3646, 1998. [4] M. B. Tischler, J. D. Colbourne, M. R. Morel, W. S. Levine D. J. Biezad, and V. Moldoveanu. Conduit: A new multidisciplinary integration environment for flight control development. Technical report, NASA, -1997. [5] D. H. Shim, H. J. Kim, and S. Sastry. Hierarchical control system synthesis for rotorcraft-based unmanned aerial vehicles. In A I A A Guidance, Navigation and Control Conference, Denver, CO, August 2000. [6] F. Allgower and A. Zheng, editors. Nonlinear Model Predictive Control, volume 26 of Progress in Systems and Control Theory. Birkhauser Verlag, Basel-Boston-Berlin,
Figure 2: Nonlinear model predictive trajectory planning and tracking control for three helicopters: (a) trajectory of three helicopters (b) control inputs, and (c) constrained state variables
computation speed over 1 giga-FLOPS. For implementation, we employ a primary-secondary computer architecture, in which the secondary flight computer is dedicated to the online optimization while the primary computer handles all other flight management tasks including hard real-time control. An avionics system with such architecture has been constructed 191 and will be used for real-time flight control using NMPTCs in the near future.
2000.
[7] E. A. Wan and A. Bogdanov. Model predictive neural control with applications to a 6 dof helicopter model: In A I A A Guidance, Navigation and Control Conference, Montreal, Quebec, Canada, August 2001. [SI G. J. Sutton and R. R. Bitmead. I n [6], chapter Computational Implementation of Nonlinear Predictive Control on a Submarine, pages 461-471. [9] D. H. Shim. Hierarchical Control System Synthesis for Rotorcraft-based Unmanned Aerial Vehicles. PhD thesis, University of California at Berkeley, 2000.
4 Conclusion In this paper, we introduced a nonlinear model predictive tracking control (NMPTC) control for RUAV system. We formulated a NMPTC algorithm for the position and heading tracking problem and implement a n
3580
3581
