IROS2019国际学术会议论文集Model Predictive Control based Dynamic Path Tracking of a Four-Wheel Steering Mobile Robot

Model Predictive Control based Dynamic Path Tracking of a Four-Wheel Steering Mobile Robot Mohamed Fnadi1, Fr ed eric Plumet1and Fa z Benamar1 AbstractThis paper develops a new constrained model predictive control for a dynamic path tracking of an off-road mobile robot with a double steering axle. The controller is based on a dynamic model that includes wheel-ground lateral slippage and terrain geometry parameters. It is formulated as an optimization problem that computes at each time-step the optimal front and rear steering angles required to perform a desired path, with respect to multiple constraints, essentially the steering joint limits and the tire adhesion area bounds (i.e., pseudo-sliding zone limits). The capabilities of such a path tracking controller are shown and discussed through numerical simulations and experiments on a real off-road mobile robot at different speeds. I. INTRODUCTION Unmanned Ground Vehicles (UGV) are being widely used in various applications (civilian and military) since they prevent people from performing dangerous duties (safety in- surance) and reduce human labor. Such autonomous vehicles require highly accurate and stable control laws which should respect vehicle constraints, even if the terrain geometry and wheel-ground contact conditions are expected to change, mainly at high speed. Furthermore, double steering vehicles are demonstrated to be interesting and promising for robotics applications, mainly for lateral dynamics control at high speed and for maneuvering in cluttered environments where small turning radius makes navigation extremely easy 12. Globally, in numerous research works, path tracking con- trollers are often designed using kinematic models or ex- tended ones (i.e., classical kinematic model including sliding effects) in Fr enet frame 34. For instance, authors in 5 and 6 developed an adaptive and predictive path tracking control law with sliding effects, where the predictive al- gorithm is mostly related to the reference path curvature change. Moreover, 9 and 10 use the kinematic based Model Predictive Control (MPC) to obtain the appropriate control law which deals with the system physical constraints (input saturation and state bounds). In addition, a nonlinear constrained MPC (NLMPC) is developed in 11 for the stabilization of the kinematic model of a two-wheel mobile robot with actuator saturations and state constraints. Never- theless, such kinds of controllers are more relevant mainly for on-road activities (such as logistics, industry, etc.) and/or when the vehicle moves at a limited speed. As a result, the effi ciency of kinematic controllers or extended ones can be signifi cantly damaged since the slippage and the vehicle 1Authors are with Sorbonne Universit e, CNRS UMR 7222, Institut des Syst emes Intelligents et de Robotique, ISIR, F-75005 Paris - France firstname.lastenamesorbonne-universite.fr dynamics become exceedingly important for high speed robots and/or off-road applications (agriculture, mining, etc.). To overcome the limitations of kinematic controllers and achieve good outcomes, several path tracking approaches incorporate dynamic models and predictive strategies. For example, dynamic path tracking controllers are developed by applying nonlinear continuous-time generalized predictive control (NCGPC) in 1 and a Linear Quadratic Regulator (LQR) in 2. Both of them aim to compute the required front and rear steering angles to track a desired trajectory. However, these controllers do not take into account any physical or intrinsic constraints. The main advantage of the MPC is indeed the ability to anticipate future changes in setpoints and handle constraints that are critical and neces- sary for the safety and stability of the vehicle. For instance, a NLMPC and a linear MPC (on-line linearization of the vehicle model) are designed and compared for active front steering control design on slippery surfaces 1213 and 14. They are developed essentially to stabilize the vehicle along a desired path while fulfi lling its physical constraints (such as front steering angle and its velocity saturation). In addition, authors in 15, 16 and 17 introduced two MPC approaches for autonomous ground vehicles : the high-level MPC is used for a path planner with obstacle avoidance considering various constraints while the low-level one tracks the planned trajectory by controlling the front steering angle and differential braking. In the present work, we synthesize a new controller for a dynamic path tracking by using constrained model predictive control (MPC) for double steering off-road ve- hicles. We apply this controller to fast cornering that can exhibit skidding phenomena and to other situations such as moving across slopes. The control problem is expressed as a Linearly Constrained Quadratic Programming (QP) 20 to compute the optimal and dynamically-consistent front and rear steering angles required to achieve a desired path. This paper is structured as follows. In section II, we highlight the dynamic modeling for double steering off-road robots. Section III defi nes the criteria and different inequality constraints used in the QP optimization. In section IV, we validate the constrained MPC through numerical simulations using ROS-GAZEBO and experiments using the real off-road double steering mobile robot. Finally, section V closes the paper with conclusion and future works. II. DYNAMIC MODELING This section reminds the dynamic vehicle model used for the controller design. It has been acquired from 7 and 8 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Macau, China, November 4-8, 2019 978-1-7281-4003-2/19/$31.00 2019 IEEE4518 and it is recapped in this section. To summarize, the model is based on a double steering bicycle model, focusing on lateral and yaw vehicle dynamics and assuming a pseudo- lateral slippage (adhesion condition). Notations used in this paper are : G is the vehicle center of mass, a and b are the front and rear half-wheelbases, Iz and m are the yaw-inertia moment and vehicle mass, Fxiand Fyiare the longitudinal and lateral forces generated on the front and rear tires respectively (i f,r), Cfand Crare respectively the front and rear lateral cornering stiffnesses, g is the acceleration of gravity, V, Vxand Vyare respectively the yaw rate, longitudinal and lateral velocities, aydenotes the lateral acceleration, f, r, are the front, rear and global sideslip angles, respectively, is the path curvature, fand rare the front and rear steering angles, and refare the vehicle and reference yaw angle, eyand eare the lateral and angular errors of the robot with respect to the reference path, and r, rand sare respectively road grade, road bank angle and suspension defl ection angle (see Fig. 1). The vehicle lateral dynamics is developed taking into account the sliding parameters and the ground geometry characteristics (gravity components). Relying on the linear tire model 19 (Fy(f,r)=C(f,r)(f,r) (see Fig. 2), the bicy- cle model representation (Fig. 1(a) and assuming that the longitudinal speed Vxis constant, the dynamic model can be rewritten by the linear representation (1) 118. ? = A( ss)+B(uuss), y =C( ss), (1) where = Vy,V,ey,eTdenotes the state vector, u = f,rTis the input vector, y=V,ey,eTthe output vector and ssand ussdepict the steady-state of the linear system with drift (see 2 for more details). A = a11a1200 a21a2200 100Vx 012Vx0 a11= 2Cf +Cr mVx a21= 2aCf bCr IzVx a12= 2aCf bCr mVx Vx,a22= 2a 2Cf +b2Cr IzVx C = 0100 0010 0001 ,B = b11b12 b21b22 00 00 b11= 2Cf m , b12= 2Cr m , b21= 2aCf Iz , b22= 2bCr Iz The steady-state vectors depend mainly on the ground geometry and robot parameters, and they are expressed as, ss= Vxa22(b11b12)+(Vxa12+gcosrsinr)(b22b21) a11(b21b22)a21(b11b12) Vx 0 Vxa22(b11b12)+(Vxa12+gcosrsinr)(b22b21) Vx(a11(b21b22)a21(b11b12) uss= Vx(a11a22a21a12)a21gcosrsinr a21(b11b12)a11(b21b22) Vx(a11a22a21a12)+a21gcosrsinr a21(b11b12)a11(b21b22) xi yi Vy Vx V G OX Y f a b f Fxr Fyr Fxf Fyf r r Reference Path ey ref 1 (a) (b) Fig. 1.(a) Dynamic bicycle model with position and velocity parameters and path tracking notations in yaw frame. (b) Road grade angle Top and road bank angle Bottom. The continuous-time state space model (1) can be dis- cretized following the zero-order-hold method. By using forward differences, it is easy to calculate an approximate discrete-time model for a small sampling time denoted Td: ( Xk+1= Xk+Uk, yk=CXk, (2) where = I44+ TdA and = TdB are the state space matrices, I denotes the identity matrix, Xk= kssand Uk= ukussare the state and input vectors. III. CONSTRAINED CONVEX PROBLEM FORMULATION Model Predictive Control (MPC) is well known to generate optimal and smooth control inputs, that can satisfy system constraints. The goal of this part is to design the path tracking controller based on a MPC approach. The controller can be expressed as an optimization problem in which the cost function defi nes the path tracking task while fulfi lling several constraints at each sampling time as listed below ; Respect the steering angles and their rates of change limitations. Conform with the tire adhesion area by limiting the slippage angles up to some given values (see Fig. 2). A. Objective function formulation Predictive control is based on the minimization of a quadratic cost function subjected to the different physical constraints. From (2), the predicted outputs can be described as follows : Y = PxXk+PuU,(3) wherePu= C0.0 CC.0 . . . . . . . CNp1CC , and 4519 Y = yk+1|k yk+2|k . . . yk+Np|k , Px= C C2 . . . CNp , U = Uk Uk+1 . . . Uk+Np1 where Npis the prediction horizon. To simplify the notation, we denote q = 3Npand r = 2Np. The matrix Pu Rrr is a lower triangular matrix with constant diagonals (i.e., Toeplitz matrix). U is the optimization variable defi ned as a sequence of control inputs. Taking into account (3), the cost function can be stated in a standard quadratic form that is compounded by the error between the predicted outputs Y and the reference signals Yrefas well as the control inputs U. J(U) = ? ?Y Yref ? ?2 Q+kUk 2 R, (4a) = ? ?PxXk+PuU Yref ? ?2 Q+kUk 2 R, (4b) = 1 2U THU + fTU +ETQE, (4c) with H = 2(PT uQPu+R), E = PxXkYref,f = 2PT uQE. k k is the Euclidean norm, H is a Hessian matrix that describes the quadratic part of the objective function, and the vector f describes its linear part. ETQE is independent of U which does not affect the computation of the optimal solution U?. QRqqand RRrrare weighting matrices. B. Constraints on steering angles Quadratic programming integrates quite easily constraints as inequalities such as those preventing input saturation. In practice, the front and rear steering angles (f,kand r,k) must lie within an angle range. These constraints can be written as follows : Irr Irr U Umax Umin+ (5) where Umax=Umin=max f ,max r ,.,max f ,max r TRr1 are steering angle limits and = uss,uss,.,ussT Rr1 depends on the steady input vector. C. Constraints on steering angle rate The steering actuator have some maximal permitted speed. Also, a great change of steering angles between two time- steps can signifi cantly impact the lateral stability of the robot. Thus, the rate of change on steering angles U = Uk,Uk+1,.,Uk+Np1Tshould be limited accordingly. Taking into account (14), constraints on change rates of steering angles can be deduced as below (see appendix A): IrrF Irr+F U Umax+M UminM (6) where F Rrrand M Rr1are detailed in appendix A. Umaxand Uminare steering angle rate limits. D. Slippage angle constraints and tire adhesion The tire-ground model is often described by nonlinear relationships between tangential contact forces and slippage parameters. The predictive model assumes a linear function between the lateral force Fyiand the slippage angle (f,r) (see Fig. 2). This linear area characterizes a good adhesion between the ground and the tire. When the lateral force exceeds a certain value due to the centrifugal force (e.g., during cornering), the skidding zone is achieved, in which the lateral force at the tire contact reaches the friction limit, causing the vehicle drifts. To avoid this uncontrollable state, a constraint characterizing slippage angle limitation needs to be applied. Fig. 2.Nonlinear tire behavior, reduced to a pseudo-sliding area where the wheels offer the best grip conditions. To maintain the tire in the pseudo-sliding area, sideslip angles must be limited between certain lower and upper bounds. Hence, the following constraints can be derived thus; min f f,k max f andmin r r,k max r (7) We recall that the sideslip angle (f,r) is defi ned as the angle between the longitudinal axis of the wheel frame and its velocity vector (Fig. 1(a). When the slippage angles are small, they may be expressed linearly as a function of the state and input variables as below : f,k= Vy,k+aV,k Vx f,kand r,k= Vy,kaV,k Vx r,k(8) From (2), we can rewrite (8) as follows ; k= f,k r,k =Tk+Juk=T(Xk+ss)+J(Uk+uss), (9) whereT = 1 Vx a Vx 00 1 Vx b Vx 00 and J = 10 01 From (18) developed in appendix B, the sideslip angle constraints can be formulated as following : Z+ Z U maxWXk min+WXk+ (10) where max=min=max f ,max r ,.,max f ,max r TRr1 are the lower and upper augmented slip angle limits and W Rr4, Z Rrr, Rrrand Rr1are detailed in appendix B. 4520 E. Constrained Quadratic Programming Formulation The control of the mobile robot requires to fi nd the optimal command input U?= U? k,U ? k+1,.,U ? k+Np1 T at each time step k that satisfi es the constraints and minimizes the objective function. This problem can be reformulated as a quadratic programming optimization which is defi ned as, ( U?= argmin U (4c), s.t.GU h, (11) with G is the global constraint matrix and h its associated limit vector which gather all the constraints (5), (6) and (10). IV. SIMULATIONANDEXPERIMENTSVALIDATION The control framework proposed in section III is tested on a double steering mobile robot ”SPIDO” (Fig. 3) with a weight of 880 kg, a front and rear half-wheelbases of 0.85 m and a yaw-inertia of 300 kg.m2. The QP problem is implemented using the CVXOPT library, a real-time open- source QP solver 21. First, simulation results are reported using an advanced physical engine (Fig. 3). The second part of this section presents the experimental results, recorded with the ”SPIDO” robot platform shown in Fig. 3. RTK GPS Xsens IMU Fig. 3.Left: Virtual SPIDO robot; Right: Experimental platform and embedded sensors. Our controller uses two on-board sensors : a Real Time Kinematic GPS (EMLID Reach RTK-GPS) gives the accu- rate absolute position with respect to the base station (cm- level accuracy at 5Hz sampling frequency) and an IMU (Xsens MTi-G) provides the yaw angle and yaw rate vari- ables (0.1.s1accuracy at 100Hz sampling rate). Besides, the lateral velocity is observed by a Kalman-Bucy fi lter that makes use of both steering angles and embedded sensors measurements 2. A. Simulation Results with Gazebo Simulator In order to evaluate the performance of this constrained path tracking controller, we perform fi rst numerical simu- lations using ROS-GAZEBO at the velocities Vx= 5m.s1 and Vx= 10m.s1. Two reference paths were tested : The fi rst one is a ”Z path” with two maneuvers right and left (Fig. 6(a). The second one is a ”O path” with a right turn (Fig. 6(b). Both start with straight lines. All tests have been carried out on sloping ground (15). The road bank and road grade angles (rand r) are on-line estimated by the linear Luenberger observer developed in 8. Fig. 6.Recorded paths at the velocities Vx= 5m.s1and Vx= 10m.s1: (a) ”Z path”; (b) ”O path”. For the simulation part, we set the following parameters: sample time : Td= 0.2s, constraints on extremum steering angles : 10and constraints on extremum steering angle rate : 3. For a given longitudinal vehicle speed Vx, the tuning parameters are the prediction horizon (Np=20 atVx= 5m.s1and Np= 40 at Vx= 10m.s1) and the weighting matrices Q and R. The ground friction coeffi cient was set to be a constant value ( = 0.35) which approximately represents the rubber and grass contact friction coeffi cient. The front and rear tire cornering stiffnesses were assumed to be known and are observed on-line by the nonlinear observer proposed in 7. They converge to the same constant value (C(f,r) 16 kN.rad1). Therefore, the front and rear sideslip angle constraints can be approximated using a vertical force due to the total weight of the vehicle equally balanced between the front and rear axles (max (f,r) Fz 2.C(f,r) mg 2.C(f,r) 6). Fig. 4.Simulation results for ”Z path” at Vx= 5 m.s1and Vx= 10 m.s1: (a) Front (b) Front (c) Lateral deviations; (d) Lateral accelerations. Fig. 5.Simulation results for ”O path” at Vx= 5 m.s1and Vx= 10 m.s1: (a) Front (b) Front (c) Lateral deviations; (d) Lateral accelerations. 4521 The front and rear steering angles calculated by the MPC controller at the velocities Vx= 5 m.s1and Vx= 10 m.s1 are depicted on Fig 4(a) and Fig. 5(a) for Z (b) Front (c) Lateral deviations; (d) Lateral accelerations. As can be noticed from Fig. 8(b), the front and rear sideslip angles are higher when the longitudinal speed Vx increases and during cornering process. Otherwise, they are perfectly within the bounds of the pseudo-sliding area (6). To sum