- Email: [email protected]
Vehicle Lateral Motion Control with Performance and Safety Guarantees
Preprints, 8th IFAC International Symposium on Preprints, IFAC Advances in Automotive Control Symposium Preprints, 8th 8th IFAC International International Symposium on on Preprints, 8th IFAC International Symposium on Advances Automotive Control June 19-23,in 2016. Norrköping, Sweden Available online at www.sciencedirect.com Advances in Automotive Control Advances in2016. Automotive Control June 19-23, Norrköping, Sweden June June 19-23, 19-23, 2016. 2016. Norrköping, Norrköping, Sweden Sweden
ScienceDirect
IFAC-PapersOnLine 49-11 (2016) 285–290
Vehicle Lateral Motion Control with Vehicle Lateral Motion Control with Vehicle Lateral Motion Control with Vehicle Lateral Motion Control with Performance and Safety Guarantees Performance and Safety Guarantees Performance and Safety Guarantees Performance and Safety Guarantees ∗ ∗ ∗ ∗∗
L. Ni ∗ A. Gupta ∗ P. Falcone ∗ L. Johannesson ∗∗ ∗∗ L. Ni ∗∗ A. Gupta ∗∗ P. Falcone ∗∗ L. Johannesson ∗∗ L. L. Ni Ni A. A. Gupta Gupta P. P. Falcone Falcone L. L. Johannesson Johannesson ∗ Chalmers University of Technology Department of Signals and ∗ ∗ Chalmers University of Technology - Department of Signals and ∗ Chalmers University of Technology Department of Signals Systems, Gothenburg, Sweden (e-mail: [email protected], Chalmers University Sweden of Technology - Department of Signals and and Systems, Gothenburg, (e-mail: [email protected], Systems, Gothenburg, Sweden (e-mail: [email protected], [email protected], [email protected]). Systems, Gothenburg, Sweden (e-mail: [email protected], [email protected], [email protected]). ∗∗ [email protected], [email protected]). Cars Corporation, Gothenburg, Sweden, (e-mail: [email protected], [email protected]). ∗∗ Volvo ∗∗ Volvo Cars Corporation, Gothenburg, Sweden, (e-mail: ∗∗ Volvo [email protected]) Cars Corporation, Gothenburg, Volvo [email protected]) Cars Corporation, Gothenburg, Sweden, Sweden, (e-mail: (e-mail: [email protected]) [email protected]) Abstract: This paper explores the use of Model Predictive Control (MPC) techniques to solve Abstract: This paper explores the use of Model Predictive Control (MPC) techniques to solve Abstract: This paper the Model Predictive Control (MPC) to vehicle lateral control problem highway scenarios. In particular, the problem of Abstract: Thismotion paper explores explores the use use of ofon Model Predictive Control (MPC) techniques techniques to solve solve vehicle lateral motion control problem on highway scenarios. In particular, the problem of vehicle lateral motion control problem on highway scenarios. In particular, the problem of autonomously driving a vehicle along a desired path is formulated, where safety constraints and vehicle lateral driving motiona control problem on highway scenarios. Inwhere particular, the problemand of autonomously vehicle along a desired path is formulated, safety constraints autonomously driving a vehicle along a desired path is formulated, where safety constraints and performance levels must be guaranteed for all possible road curvatures within a compact set. autonomously driving a vehicle along a desired path is formulated, where safety constraints and performance levels must be guaranteed for all possible curvatures within a compact set. performance levels must be for road curvatures within set. Safety constraints translated into a maximum lateralroad deviation and orientation error w.r.t. performance levelsare must be guaranteed guaranteed for all all possible possible road curvatures within aa compact compact set. constraints are translated into aa maximum lateral deviation and orientation error w.r.t. Safety constraints are translated into maximum lateral deviation and orientation error w.r.t. aSafety desired path, while performance requirements are formulated in terms of bounded lateral Safety constraints are translated into a maximum lateral deviation and orientation error w.r.t. a desired path, performance requirements formulated in terms of bounded lateral a desired while performance requirements are formulated in terms of lateral acceleration and while velocity. Preliminary simulation are results show that designed controller is a desired path, path, while performance requirements are formulated in the terms of bounded bounded lateral acceleration and velocity. Preliminary simulation results show that the designed controller is acceleration and velocity. Preliminary simulation results show that the designed controller is capable of delivering acceptable performance at the cost of limited online computational costs. acceleration and velocity. Preliminary simulation results show thatonline the designed controller is capable of delivering acceptable performance at the cost of limited computational costs. capable of delivering acceptable performance at the cost of limited online computational costs. capable of delivering acceptable performance at the cost of limited online computational costs. © 2016, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Vehicle motion control, safety, model predictive control, autonomous driving Keywords: Vehicle Vehicle motion control, control, safety, model model predictive control, control, autonomous driving driving Keywords: Keywords: Vehicle motion motion control, safety, safety, model predictive predictive control, autonomous autonomous driving 1. INTRODUCTION proaches to the overall problem of vehicle motion control, 1. INTRODUCTION INTRODUCTION proaches to the overall problem vehicle motion 1. proaches to overall problem of vehicle control, climbing the ASIL ladder, from of a Quality Measurecontrol, (QM) 1. INTRODUCTION proaches to the the overall problem of vehicle motion motion control, climbing the ASIL ladder, from a Quality climbing the will ASIL ladder, from aa Quality Quality Measure (QM) to ASIL D, lead to enormous, costly Measure and time(QM) conAmong the technologies advancing within the automotive climbing the ASIL ladder, from Measure (QM) to ASIL D, will lead to enormous, costly and time conAmong the technologies technologies advancing within the automotive automotive to ASIL D, will lead to enormous, costly and time consuming verification problems, which inevitably will stall Among the advancing within the field, autonomous driving is definitely emerging with most to ASIL D, will lead to enormous, costly and time conAmong the technologies advancing within the automotive suming verification problems, which inevitably will stall field, autonomous driving is definitely emerging with most suming verification problems, which inevitably will the product development. In this paper we focus on the field, autonomous driving is definitely emerging with most promises of improving many aspects emerging of our lifestyles re- suming verification problems, which inevitably will stall stall field, autonomous driving is definitely with most the product development. In this paper we focus on the promises of improving many aspects of our lifestyles rethe product development. In this paper we focus on problem of designing a vehicle lateral motion controller promises of improving many aspects of our lifestyles related to transport. Road safety, traffic congestions and product development. In this papermotion we focus on the the promises of improving many aspects of our lifestylesand re- the problem of designing a vehicle lateral controller lated to transport. Road safety, traffic congestions problem of designing designing vehicle lateral motion motion controller with performance and aasafety guarantees and explore the lated to transport. traffic congestions pollutant emissions, Road transitsafety, efficiency, healthiness of and ur- problem of vehicle lateral controller lated to transport. Road safety, traffic congestions and with performance and safety guarantees and explore the pollutant emissions,totransit transit efficiency, healthiness of urwith performance and guarantees and the Model Predictive Control (MPC) techniques, pollutant emissions, ban environments, name efficiency, a few, are healthiness recognized of to urbe use withof performance and safety safety guarantees and explore explorewith the pollutant emissions,totransit efficiency, healthiness of of Model Predictive Control (MPC) techniques, with ban environments, environments, name a few, few, are recognized to urbe use use of Model Predictive Control (MPC) techniques, with the objective of providing systematic design methodologies ban to name a are recognized to be potentially and highly impacted by autonomous driving. use of Model Predictive Control (MPC) techniques, with ban environments, to name a few, are recognized to be objective of providing design methodologies potentially and highly highly impacted by autonomous autonomous driving. the the objective of systematic design satisfy ASIL D-type of systematic requirements. potentially and impacted by driving. It is then natural to question the maturity of the available the objective of providing providing systematic design methodologies methodologies potentially and highly impacted by autonomous driving. to to satisfy ASIL D-type of requirements. It is then natural to question the maturity of the available to satisfy satisfy ASIL ASIL D-type D-type of of requirements. requirements. It is then natural to question the maturity of the autonomous driving technologies, especially withavailable respect to It is then natural to question the maturity of the available In Guldner et al. (1996) steering control for passenger autonomous driving technologies, especiallyonwith with respect autonomous driving technologies, especially to the new and demanding requirements the respect vehicle In Guldner et al. (1996) steering control for passenger autonomous driving technologies, especiallyonwith respect In Guldner et al. (1996) steering control for passenger cars on automated highways is analyzed and to the new and demanding requirements the vehicle Guldner et al. (1996) steering controland for conditions passenger to the new and demanding on the vehicle motion control imposed by requirements Level 4 NHTSA (2013) of In cars on automated highways is analyzed conditions to the new and demanding requirements on the vehicle cars on automated highways is analyzed and conditions for the safety and performance criteria are proposed. In motion control imposed by Level 4 NHTSA (2013) of cars on automated highways is analyzed and conditions motion control imposed by Level 4 NHTSA (2013) of autonomous driving. for the safety and performance criteria are proposed. motion control imposed by Level 4 NHTSA (2013) of Lei for the safety and criteria are proposed. In et al. (2006) a performance vision-based lane detection method In is autonomous driving. for the safety and performance criteria are proposed. In autonomous driving. Lei et al. (2006) aa avision-based lane detection method is autonomous driving. Lei et al. (2006) vision-based lane detection method is along with PID controller for the lateral control. While the existing vehicle motion controllers use the driver utilized Lei et al. (2006) a avision-based lanefordetection method is utilized along with PID controller the lateral control. While the existing vehicle motion controllers use the driver utilized along with a PID controller for the lateral control. A comparative study of linear controllers for lane keeping While the existing vehicle motion controllers use the driver as a failsafe fall-back, in autonomous driving, the deviautilized along with a PID controller for the lateral control. While the existing vehicle motion controllers use the driver A comparative study of et linear controllers for lane lane keeping keeping as aa form failsafe fall-back, infor autonomous driving, the deviadevia- A of linear controllers for cancomparative be found instudy Taylor al. (1999). A dynamic as failsafe fall-back, autonomous driving, the tion a given path,in example, must be guaranteed comparative study of et linear controllers for lanefeedback keeping as a form failsafe fall-back, infor autonomous driving, the devia- A can be found in Taylor al. (1999). A dynamic feedback tion a given path, example, must be guaranteed can be found in Taylor et al. (1999). A dynamic feedback controller is proposed in Benine-Neto et al. (2010), which tion form aaangiven path, for example, must be guaranteed to satisfy Automotive Safety Integrity Level (ASIL) can be found in Taylor et al. (1999). A dynamic feedback tion form given path, for example, must be guaranteed controller is proposed in Benine-Neto et al. (2010), which to satisfy satisfy an an see Automotive Safety Integrity Level (ASIL) (ASIL) controller is proposed in Benine-Neto et al. (2010), which considers road curvature as bounded disturbance input. to Automotive Safety Integrity Level requirement, ISO-26262 (2011) for a description of the controller is proposed in Benine-Neto etdisturbance al. (2010), input. which to satisfy an see Automotive Safety Integrity Level (ASIL) considers road curvature as bounded requirement, ISO-26262 (2011) for a description of the considers road curvature as bounded disturbance input. Since the vehicle motion dynamics are nonlinear, conrequirement, see ISO-26262 (2011) for a description of the ASIL standard. The determination of the required ASIL considers road curvature as bounded disturbance input. requirement, see ISO-26262 (2011) for a description of the Since the the vehicle motion dynamics are can nonlinear, conASIL standard. The determination determination of the the required Smith ASIL straints Since vehicle motion dynamics are nonlinear, conrelated to safety and performance be naturally ASIL standard. The of required ASIL is the result of hazard analysis and risk assessment Since the vehicle motion dynamics are nonlinear, conASIL standard. The determination of the required Smith ASIL straints related to safety and performance can be naturally is the result of hazard analysis and risk assessment straints related to safety and performance can be naturally accommodated with MPC techniques, like in Falcone et al. is the result of hazard analysis and risk assessment Smith and Simpson (2010), which means that functionalities straints related to safety and performance can be naturally is the result of hazard analysis and risk assessment Smith accommodated with MPC techniques, like in Falcone et al. and Simpson Simpson (2010),forwhich which means that functionalities functionalities accommodated with MPC techniques, like in Falcone et al. (2007), where a MPC strategy for steering control of vehiand (2010), means that with likely potential severely life-threatening or fatal accommodated with MPC techniques, like in Falcone et al. and Simpson (2010),forwhich means that functionalities (2007), where a MPC strategy for steering control of vehiwith likely potential severely life-threatening or fatal (2007), where a MPC strategy for steering control of vehicle on slippery road is proposed. In Lee et al. (2012) a fast with likely potential for severely life-threatening or fatal injury in the event of a malfunction will be classified as (2007), where aroad MPC strategy forInsteering control ofavehiwith likely potential for severely life-threatening or fatal cle on slippery is proposed. Lee et al. (2012) fast injury D, in requiring the event event of of aavehicle malfunction will be be classified classified as MPC cle on slippery is In et a is proposed for lateral The paper injury in the malfunction will as ASIL manufacturer guarantee on strategy slippery road road is proposed. proposed. In Lee Leecontrol. et al. al. (2012) (2012) a fast fast injury in requiring the eventthe of avehicle malfunction will be to classified as cle MPC strategy is proposed for lateral control. The paper ASIL D, the manufacturer to guarantee −8vehicle MPC strategy is proposed for lateral control. The paper proposes an algorithm to approximate solution of the optiASIL D, requiring the manufacturer to guarantee aASIL failure rate of 10 eventsmanufacturer per hour. Intoconclusion, MPC strategy is proposed for lateral control. The paper D, requiring the guarantee −8vehicle proposes an algorithm to approximate of the optifailure rate of 10 10−8 events per hour. In conclusion, proposes an to solution of optiproblem underlying the MPCsolution controller, by using aa of events hour. In in Level 4rate autonomous staying lane will mization proposes an algorithm algorithm to approximate approximate solution of the the optia failure failure rate of 10−8 driving, events per per hour.within In conclusion, conclusion, mization problem underlying the MPC controller, by using in Level 4 autonomous driving, staying within lane will mization problem problem underlying the MPC MPC controller, by using using precomputed solutions. A MPC controller is designed to in Level 4 autonomous driving, staying within lane will need to be guaranteed with ASIL D. Hence, the ASIL mization underlying the controller, by in Level 4 autonomous driving, staying within lane will precomputed solutions. A MPC controller is designed to need to be guaranteed with ASIL D. Hence, the ASIL precomputed solutions. A MPC controller is designed to resemble the driver behavior in Gray et al. (2012). The need to be guaranteed with ASIL D. Hence, the ASIL D requirement to stay within the lane will reflect into precomputed solutions. A MPC controller is designed to need to be guaranteed with ASIL D. Hence, the ASIL resemble the driver behavior in Gray et al. (2012). The D requirement to stay within the lane will reflect into resemble the driver behavior in Gray et al. (2012). The controller is designed to only apply the correcting control D requirement to stay within the lane will reflect into a stringent requirement on the maximum totalreflect deviation resemble the driver behavior in Gray et al. (2012). The D requirement to stay within the lane will into controller designed only apply the correcting control a stringent requirement on the maximum total deviation controller is designed to only apply the control thatis necessaryto avoid violation of the safety cona stringent requirement on total deviation from the desired trajectory/path (performance safety action controller isis designed toto only apply the correcting correcting control a stringent requirement on the the maximum maximum totaland deviation action that is necessary to avoid violation of the safety confrom the desired trajectory/path (performance and safety action that that is necessary necessary to avoid avoid violation of the the safety safety constraints. A MPC problem for obstacle avoidance and lane from the desired trajectory/path (performance and safety guarantees), compatibly with the sensing technology. It is action is to violation of confrom the desired trajectory/path (performance and safety straints. A MPC problem for obstacle avoidance and lane guarantees), compatibly with the sensing technology. It is straints. A MPC problem for obstacle avoidance and lane keeping is proposed in Turri et al. (2013), based on linear guarantees), compatibly with the sensing technology. It is then clear that, without systematic control engineering apstraints. A MPC problem for obstacle avoidance and lane guarantees), compatibly with the sensing technology. It is keeping is proposed in Turri et al. (2013), based on linear then clear that, without systematic control engineering apkeeping is proposed in Turri et al. (2013), based on linear decoupled lateral and longitudinal dynamics, thus helping then clear that, without systematic control engineering apkeeping is proposed in Turri et al. (2013), based on linear then clear that, without systematic control engineering apdecoupled lateral and longitudinal dynamics, thus helping This work is partially supported by the Vinnova FFI Complex decoupled decoupled lateral lateral and and longitudinal longitudinal dynamics, dynamics, thus thus helping helping This work is partially supported by the Vinnova FFI Complex
Control Program, under the grant No. 2015-02309. is supported by the This This work work is partially partially supported by2015-02309. the Vinnova Vinnova FFI FFI Complex Complex Control Program, under the grant No. Control Program, under the grant No. 2015-02309. Control Program, under the grant No. 2015-02309.
Copyright © 2016, 2016 IFAC 292 Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright © 2016 IFAC 292 Copyright © 2016 IFAC 292 Peer review under responsibility of International Federation of Automatic Copyright © 2016 IFAC 292Control. 10.1016/j.ifacol.2016.08.043
IFAC AAC 2016 286 June 19-23, 2016. Norrköping, Sweden
L. Ni et al. / IFAC-PapersOnLine 49-11 (2016) 285–290
in framing a convex QP problem for fast calculation of the solution. In this paper, we explore MPC approaches to the vehicle lateral motion control problem. We focus on the problem of controlling the lateral vehicle motion subject to safety and performance requirements, along low curvature paths like in, e.g., highways. The road curvature is considered as disturbance input to the system. Safety and performance requirements are formulated in terms of the maximum deviation from the desired path and constraints on the vehicle states stemming from a desired comfort envelope. These constraints are guaranteed to be persistently satisfied within a known set of vehicle states for a curvature of the desired path within given boundaries. Preliminary simulation results show the performance and the viability of the proposed approach, encouraging further developments. The paper is structured as follows. In Section 2 we introduce a vehicle model, notations, and formally state the vehicle lateral motion control problem. Section 3 presents few preliminary results on invariant set and an algorithm to calculate the invariant set. In Section 4, the design procedure is shown. while Section 5 show the results of numerical simulations. The paper is concluded is in Section 6 with final remarks about the presented results and future research directions. 2. PROBLEM FORMULATION 2.1 Vehicle Modeling Consider the vehicle model sketched in Figure 1. For small road bank angle, the vehicle motion w.r.t. the path Γdes , subject to the lateral and yaw dynamics, is described by the following set of differential equations (Rajamani, 2006). mv˙ y = −mvx ψ˙ + 2 Fy + Fy , (1a) f
Jz ψ¨ = 2[lf Fyf − lr Fyr ], ˙ e˙ ψ = ψ˙ des − ψ,
r
(1b)
(1c) (1d)
e˙ y = −vy + vx eψ , ˙ (1e) ψdes = vx γ, x where m and Jz denote the vehicle mass and yaw inertia, respectively, lf and lr are the distances of the vehicle center of gravity from the front and rear axles, respectively, vx and vy are the longitudinal and lateral velocities, respectively, in the vehicle body frame, ψ˙ is the turning rate, where ψ denotes the vehicle orientation w.r.t. the fixed global frame (X, Y ) in Figure 1. Fyf , Fyr are the lateral tire forces at the front and rear axles, respectively. In (1c) and (1d), eψ and ey denote the vehicle orientation and position, respectively, w.r.t. the path Γdes and ψdes is the desired vehicle orientation, i.e., the slope of the tangent to the path Γdes in the point O. The lateral tire forces in (1a) and (1b) are generated at the tire contact patch and are, in general, nonlinear functions of the vehicle states. In this paper, we compute the lateral tire forces as, Fyi = −Ci αi , i ∈ {f, r}, (2) where Ci are the tire cornering stiffness coefficients at the two axles and αi are the tyre slip angles which, for small 293
values, can be approximated as, vy − lr ψ˙ vy + lf ψ˙ − δ, αr = , (3) αf = vx vx where δ denotes the front steering angle as depicted in Figure 1. In order to use the steering rate as control input,
Fig. 1. Vehicle in a desired path based coordinate system the model (1) is augmented with an integrator. Hence, for a given vehicle longitudinal speed vx , the model (1)-(3) can be compactly written as, x(t) ˙ = Ax(t) + Bu(t) + Ew(t), (4) T ˙ e ψ , ey , δ where x = vy , ψ, and w = γ are the state and the disturbance vectors and u = δ˙ is the steering input command. 2.2 System Constraints The input, state and disturbance vector in (4) is subject to a set of physical and design constraints. These constrains are the result of safety, performance and physical limitation of a vehicle. The safety requirements, for the considered problem, translate into the following constraints on the position ey eymin ≤ ey ≤ eymax , (5) To preserve the driving comfort, we impose bounds on the lateral vehicle speed and acceleration, which, for a given speed vx , can be written as, vymin ≤ vy ≤ vymax , (6a) a aymin y max ≤ ψ˙ ≤ , (6b) vx vx Further physical constraints stem from the limited steering and steering rate of the steering actuator. δmin ≤ δ ≤ δmax , (7) δ˙min ≤ δ˙ ≤ δ˙max .
The constraints (5)-(7) can be compactly rewritten for the system (4) as, X = {x ∈ R4 : Hx x ≤ hx }, (8) U = {u ∈ R : Hu u ≤ hu }. Finally, we assume that the curvature γ of the reference path Γdes is bounded, i.e., it belongs to the set, W = [γmin , γmax ] .
(9)
IFAC AAC 2016 June 19-23, 2016. Norrköping, Sweden
L. Ni et al. / IFAC-PapersOnLine 49-11 (2016) 285–290
2.3 Problem Statement Consider the problem of regulating the states of the system for a given constant speed vx . x(t + 1) = Ad x(t) + Bd u(t) + Ed w(t), (10) subject to the constraints (8), (9). Where Ad , Bd and Ed are the discrete versions of system matrices in (4). The problem can be formulated as a finite time, constrained optimal control problem, solved in receding horizon. In particular, the closed-loop, state feedback control law. ucl (x(t)) = u∗t|t (x(t)) , (11) u∗t|t
is adopted, where (x(t)) is found as the solution of the following optimization problem. Problem 1 t+N −1 xk|t 2Q + uk|t 2R Jt∗ (x(t)) = min xN |t 2P + Ut
k=t
(12a)
subj. to xk+1|t = Ad xk|t + Bd uk|t + Ed wk|t , (12b) (12c) xk|t ∈ X , uk|t ∈ U, wk|t ∈ W, (12d) xt+N |t ∈ Xf , (12e) xt|t = x(t), where Ut = ut|t , . . . , ut+N −1|t , N is a finite time horizon, P, Q 0 and R 0 are weighting matrices of appropriate sizes. Further, Xf is a terminal constraint, selected as a Robust Control Invariant Set, such that persistent feasibility for Problem 1 can be guaranteed (Kerrigan, 2000). The construction of this set is explained in Section 4. In order to assist the construction of terminal constraints we need to recall few definition on invariant sets in Section 3. Also, an approach is proposed for the construction of robust control invariant (RCI) set. 3. INVARIANT SET 3.1 Preliminaries In this section we introduce a few definitions and recall basic results on set invariance theory and reachability analysis for constrained systems and provide the results on feasibility of MPC schemes used next in this paper. A comprehensive survey of papers on set invariance theory can be found in Blanchini (1999). We will denote the set of all real numbers and positive integers by R and N+ , respectively. For the system, x(t + 1) = f (x(t), u(t), w(t)), (13) subject to the constraints x(t) ∈ X , u(t) ∈ U ⊆ Rm , w(t) ∈ W. (14) Definition 1. (Pref set). we define the set of states which can be driven into the target set S in one time step as Pref (S, W) {x ∈ Rn | ∃ u ∈ U s.t. f (x, u, w) ∈ S, ∀w ∈ W}. (15) The Pref set introduced in Definiton 1 can be used to calculate the robust control invariant sets. The following 294
287
definitions are derived from Blanchini (1999); Bertsekas and Rhodes (1971); Bertsekas (1971); Kolmanovsky and Gilbert (1998). Definition 2. (Robust Control Invariant Set). A set C ⊆ X is a robust control invariant set for the system in (13) subject to the constraints in (14), if x(t) ∈ C ⇒ ∃ u(t) ∈ U such that f (x(t), u(t), w(t)) ∈ C, ∀w(t) ∈ W, ∀t ∈ N+ (16) A robust control invariant set can be calculated by using the condition provided by the following theorem Kerrigan (2000); D´orea and Hennet (1999) Theorem 1. (Condition for invariance). A set C ⊆ X is a robust control invariant set for the system (13) subject to constraints (14), if and only if (17) Pref (C, W) ∩ C = C Definition 3. (Maximal Robust Control Invariant Set C∞ ). The set C∞ is the maximal robust control invariant set for the system in (13) subject to the constraints in (14), if it is robust control invariant and contains all robust control invariant sets contained in X . 3.2 Calculation of RCI In order to calculate the robuust control invariant set for system (12b) we have proposed an algorithm. The set C in (17) can be used, together with the definition of the set Pref in equation (18), written for the model in (12b) and the sets (8) and (9). In particular, the set C can be calculated by the following algorithm Algorithm 1 Computation of the robust control invariant set C INPUT: X , W, Ad , Bd , Ed OUTPUT: C 1: C0 ← X 2: k ← 0 3: Ck+1 ← Pref (Ck , W) ∩ Ck 4: if Ck+1 = Ck , then 5: goto 10, 6: else 7: C k ← Ck+1 8: k ← k + 1, 9: goto 3. 10: return Ck where, the set Pref at step 3 is calculated as Pref (X , W) {x ∈ X | ∃ u ∈ U s.t. Hx Ad x + Hx Bd u ≤ min hx − Hx Ed w}.
(18)
w∈W
Since the set Ck+1 is the result of a sets intersection, the complexity of its representation, i.e., the number of inequalities, increases at every iteration. Since Algorithm 1 stops as the condition at step 4 is verified, the representation of the set C might consist of an unnecessarily high number of inequalities, thus complicating the Problem 1. In this paper, Algorithm 1 is stopped when the Euclidean distance between the Chebyshev’s centers of Ck+1 and Ck ,
IFAC AAC 2016 288 June 19-23, 2016. Norrköping, Sweden
L. Ni et al. / IFAC-PapersOnLine 49-11 (2016) 285–290
respectively, is below a given threshold. That is, the condition at step 4 is replaced with the following condition rk − rk+1 ≤ rthr , where rk denotes the Chebyshev’s center of Ck and rthr is a threshold. Further stopping conditions can be added like, e.g., monitoring the radius of the Chebishev’s ball enclosed in Ck+1 and Ck .We show next how robust control invariant sets C can be used to guarantee feasibility of MPC schemes. 4. CONTROL DESIGN Since a MPC controller iteratively solves optimization Problem 1, persistent feasibility has to be guaranteed. Persistent feasibility implies that, if Problem 1 is feasible for some initial x(0) ∈ X0 , where X0 is the set of initial states for which the Problem 1 is feasible, the state trajectory is guaranteed to evolve within X0 . Persistent feasibility cannot be guaranteed for any choice of the tuning parameters. As mentioned in Kerrigan (2000), by selecting Xf as a robust control invariant set persistent feasibility can be guaranteed i.e. xt+N |t ∈ C, (19) where C is a robust control invariant set mentioned in Definition 2. Results enforcing robust closed-loop stability are available for min-max schemes Kerrigan (2000). In this paper, we enforce closed-loop stability condition for the nominal system only, i.e., with w = 0 in (12b). Although our approach does not guarantee stability for all possible road curvatures, extensive simulations have shown stable operation of the closed loop system for severe road curvature changes.
Jt (x(t), Ut ) = xt+N |t 2P +
k=t
xk|t 2Q + uk|t 2R , (20)
where the matrix P is the solution of the following Algebraic Riccati Equation (ARE) −1 T P = ATd P − P Bd BdT P Bd + R Bd P Ad + Q. (21)
Furthermore, instead of (19) we add the following constraint, xt+N |t ∈ O, (22) where O is the maximal robust invariant set for the closedloop system x(k + 1) = (Ad + Bd K) x(k) + Ed w(k), (23) and K is the Linear Quadratic (LQ) regulator. The robust invariant set O can be calculated with an algorithm similar to Algorithm 1, after re-defining the set Pref at step 3 as Pref (X , W) {x ∈ X |Hx (Ad + Bd K) x ≤ min hx − Hx Ed w}.
(b) 2-norm of the difference between consecutive radii of the Chebyshev ball.
Fig. 2. Calculation of the maximal robust control invariant set.
The terminal cost term in the cost function (12a), t+N −1
(a) Volume.
(24)
w∈W
Remark 1. Here we only focus on nominal stability i.e for w = 0. Robust stability can be acheived by introducing changes in road curvature as additional variable ∆w = w(k + 1) − w(k) and using the same framework. 5. SIMULATIONS The controller (12) has been tested in simulations and compared against a LQR regulator. The physical pa295
rameters used in the vehicle model (1) are reported in Table 1. The following bounds have been used in the Table 1. Model parameters Parameter m Jz Cr Cf lr lf
Description Mass Yaw moment of inertia Rear cornering stiffness coeff. Front cornering stiffness coeff. Rear axle to CoG distance Front axle to CoG distance
Value 2164 [kg] 4373 [kg × m2 ] 122380 [N/rad] 150540 [N/rad] 1.6456 [m] 1.3384 [m]
constraints (5)-(7) to construct the sets X , U in (12) ey = −eymin = 0.2 [m], max v ymax = −vymin = 0.4 [m/s], ◦ eψmax = −eψmin = 5[ ] = 0.0873 [rad], (25) aymax = −aymin = 3[m/s2 ], ◦ ˙ ˙ = − ψ = 8.251[ ]/s = 0.144 [rad/s], ψ min max ◦ δmax = −δmin = 2.5[ ] = 0.0436 [rad], ˙ δmax = −δ˙min = 2.86 [◦ ]/s = 0.05 [rad/s].
Furthermore the set (9) is defined by the following bounds on the road curvature. γmax = −γmin = 0.0263. The constraints in (25) may look more restrictive than necessary for normal driving conditions. Nevertheless, we recall that we consider high-speed, highway scenarios where an autonomous driving system should deliver a comfortable driving experience. The matrices Ad , Bd , Ed in (12b)
IFAC AAC 2016 June 19-23, 2016. Norrköping, Sweden
L. Ni et al. / IFAC-PapersOnLine 49-11 (2016) 285–290
289
have been obtained from the model (4) for vx = 75 [km/h], by applying the Zero-Order Hold (ZOH) discretization method, with a sampling time Ts = 25 [ms]. The weighting matrices of the cost (12a) have been chosen as follows 40 0 0 0 0
0 1 0 0 0 Q = 0 0 1 0 0 , R = 2. 0 0 0 1 0 0 0 0 0 20
With this choice of the tuning parameters, the LQ gain in (23) and the corresponding solution of the ARE (21) are, respectively, K = [3.6538 − 0.5135 27.9792 − 1.5585 − 14.6065],
0.0588 −0.0057 0.2098 −0.0040 −0.0360
(a) Lateral deviation.
−0.0057 0.0011 −0.0273 0.0001 0.0050 P = 104 × 0.2098 −0.0273 1.1977 −0.0358 −0.2660 . −0.0040 0.0001 −0.0358 0.0036 −0.0360 0.0050 −0.2660 0.0145
0.0145 0.1311
In particular, the matrix P has been used as terminal cost in (20). Finally, the terminal constraint (22) is enforced, where the invariant set O is calculated as in Algorithm 1 with the set Pref defined as in (24). Figure 2 shows the evolution of the Euclidean distance between the Chebyshev’s centers of Ok+1 and Ok and the radius of the Chebyshev’s ball enclosed in Ok . In this work, Algorithm 1 is stopped by using the stopping criterion mentioned in Section 3.2 and by monitoring the volume of Ok . In Figure 3 the curvature of the desired path Γdes
(b) Orientation error
(c) Yaw rate
Fig. 4. Safety constraints. Deviation of the vehicle from the desired path with the LQ and the MPC controller.
Fig. 3. Curvature of the desired path. used in our simulation is reported. A comparison of the LQR and the MPC controllers is shown in Figures 4-6, where the control input generated by LQR controller has been clipped according to the control input constraints in (25), before applying it to system. The simulations show that the clipped LQR controller is unable to stabilize the system, thus leading to unstable oscillations. The MPC controller, on the other hand, allow to easily accommodate closed- loop stability and design and physical constraints.
a LQ controller shows improvements in the constraints satisfaction at the cost of reasonable on-line computational complexity. Although preliminary, this work is the basis for further investigations aiming at understanding the impact on the conservativeness and the complexity of the controller of a design guaranteeing the satisfaction of safety and performance requirements, despite of model uncertainty and measurement noise.
6. CONCLUSIONS
REFERENCES
This paper presents preliminary simulation results of a MPC-based the lateral vehicle motion controller. In particular, the controller aims at minimizing the lateral deviation and the orientation error w.r.t. a desired path, while guaranteeing safety and performance requirements. Namely, bounding the deviation form the path and the lateral acceleration, respectively. A comparison against
Benine-Neto, A., Scalzi, S., Mammar, S., and Netto, M. (2010). Dynamic controller for lane keeping and obstacle avoidance assistance system. IEEE International Conference on Intelligent Transportation Systems, 1363– 1368. Bertsekas, D.P. (1971). Control of Uncertain Systems with a set–membership description of the uncertainty. Ph.D. thesis, Electronic Systems Laboratory, MIT.
296
IFAC AAC 2016 290 June 19-23, 2016. Norrköping, Sweden
L. Ni et al. / IFAC-PapersOnLine 49-11 (2016) 285–290
(a) Lateral velocity.
(b) Lateral acceleration.
Fig. 5. Performance (comfort) constraints satisfaction of the LQ and the MPC controller.
(a) Steering rate.
(b) Steering angle.
Fig. 6. Control signal of the LQ and the MPC controller. Bertsekas, D.P. and Rhodes, I.B. (1971). On the minimax reachability of target sets and target tubes. Automatica, 7, 233–247. 297
Blanchini, F. (1999). Set invariance in control — a survey. Automatica, 35(11), 1747–1768. D´orea, C. and Hennet, J.C. (1999). (a, b)-invariant polyhedral sets of linear discrete-time systems. Journal of Optimization Theory and Applications, 103(3), 521– 542. Falcone, P., Borrelli, F., Asgari, J., Tseng, H.E., and Hrovat, D. (2007). Predictive active steering control for autonomous vehicle systems. IEEE Transactions on Control Systems Technology, 15, 566–580. Gray, A., Ali, M., Gao, Y., Hedrick, J.K., and Borrelli, F. (2012). Integrated threat assessment and control design for roadway departure avoidance. IEEE International Conference on Intelligent Transportation Systems, 1714–1719. Guldner, J., Tan, H., and Patwardhan, S. (1996). Analysis of automatic steering control for highway vehicles with look-down lateral reference systems. Vehicle System Dynamics, 26(4), 243–269. ISO-26262 (2011). Road Vehicles Functional Safety, ISO 26262–Part–9, Geneva, Switzerland. Kerrigan, E.C. (2000). Robust Constraints Satisfaction: Invariant Sets and Predictive Control. Ph.D. thesis, Department of Engineering, University of Cambridge, Cambridge, England. Kolmanovsky, I. and Gilbert, E.G. (1998). Theory and computation of disturbance invariant sets for discretetime linear systems. Mathematical Problems in Egineering, 4, 317–367. Lee, S.H., Lee, Y.O., Kim, B.A., and Chung, C.C. (2012). Proximate model predictive control strategy for autonomous vehicle lateral control. American Control Conference, 3605–3610. Lei, G., Jianqiang, W., and Keqiang, L. (2006). Lane keeping system based on thasv-ii platform. IEEE International Conference on Vehicular Electronics and Safety, 305–308. NHTSA (2013). U.S. Department of Transportation Policy Release on Automated Vehicle Development. Rajamani, R. (2006). Vehicle Dynamics and Control. Springer. Smith, D. and Simpson, K. (2010). Safety Critical Systems Handbook - A Straightforward Guide to Functional Safety, IEC 61508 and Related Standards. Taylor, C., Kosecka, J., Blasi, R., and Malik, J. (1999). A comparative study of vision-based lateral control strategies for autonomous highway driving. The International Journal of Robotics Research, 18(5), 442–453. Turri, V., Carvalho, A., Tseng, H.E., Johansson, K.H., and Borrelli, F. (2013). Dynamic controller for lane keeping and obstacle avoidance assistance system. IEEE International Conference on Intelligent Transportation Systems, 378–383.
ScienceDirect
IFAC-PapersOnLine 49-11 (2016) 285–290
Vehicle Lateral Motion Control with Vehicle Lateral Motion Control with Vehicle Lateral Motion Control with Vehicle Lateral Motion Control with Performance and Safety Guarantees Performance and Safety Guarantees Performance and Safety Guarantees Performance and Safety Guarantees ∗ ∗ ∗ ∗∗
L. Ni ∗ A. Gupta ∗ P. Falcone ∗ L. Johannesson ∗∗ ∗∗ L. Ni ∗∗ A. Gupta ∗∗ P. Falcone ∗∗ L. Johannesson ∗∗ L. L. Ni Ni A. A. Gupta Gupta P. P. Falcone Falcone L. L. Johannesson Johannesson ∗ Chalmers University of Technology Department of Signals and ∗ ∗ Chalmers University of Technology - Department of Signals and ∗ Chalmers University of Technology Department of Signals Systems, Gothenburg, Sweden (e-mail: [email protected], Chalmers University Sweden of Technology - Department of Signals and and Systems, Gothenburg, (e-mail: [email protected], Systems, Gothenburg, Sweden (e-mail: [email protected], [email protected], [email protected]). Systems, Gothenburg, Sweden (e-mail: [email protected], [email protected], [email protected]). ∗∗ [email protected], [email protected]). Cars Corporation, Gothenburg, Sweden, (e-mail: [email protected], [email protected]). ∗∗ Volvo ∗∗ Volvo Cars Corporation, Gothenburg, Sweden, (e-mail: ∗∗ Volvo [email protected]) Cars Corporation, Gothenburg, Volvo [email protected]) Cars Corporation, Gothenburg, Sweden, Sweden, (e-mail: (e-mail: [email protected]) [email protected]) Abstract: This paper explores the use of Model Predictive Control (MPC) techniques to solve Abstract: This paper explores the use of Model Predictive Control (MPC) techniques to solve Abstract: This paper the Model Predictive Control (MPC) to vehicle lateral control problem highway scenarios. In particular, the problem of Abstract: Thismotion paper explores explores the use use of ofon Model Predictive Control (MPC) techniques techniques to solve solve vehicle lateral motion control problem on highway scenarios. In particular, the problem of vehicle lateral motion control problem on highway scenarios. In particular, the problem of autonomously driving a vehicle along a desired path is formulated, where safety constraints and vehicle lateral driving motiona control problem on highway scenarios. Inwhere particular, the problemand of autonomously vehicle along a desired path is formulated, safety constraints autonomously driving a vehicle along a desired path is formulated, where safety constraints and performance levels must be guaranteed for all possible road curvatures within a compact set. autonomously driving a vehicle along a desired path is formulated, where safety constraints and performance levels must be guaranteed for all possible curvatures within a compact set. performance levels must be for road curvatures within set. Safety constraints translated into a maximum lateralroad deviation and orientation error w.r.t. performance levelsare must be guaranteed guaranteed for all all possible possible road curvatures within aa compact compact set. constraints are translated into aa maximum lateral deviation and orientation error w.r.t. Safety constraints are translated into maximum lateral deviation and orientation error w.r.t. aSafety desired path, while performance requirements are formulated in terms of bounded lateral Safety constraints are translated into a maximum lateral deviation and orientation error w.r.t. a desired path, performance requirements formulated in terms of bounded lateral a desired while performance requirements are formulated in terms of lateral acceleration and while velocity. Preliminary simulation are results show that designed controller is a desired path, path, while performance requirements are formulated in the terms of bounded bounded lateral acceleration and velocity. Preliminary simulation results show that the designed controller is acceleration and velocity. Preliminary simulation results show that the designed controller is capable of delivering acceptable performance at the cost of limited online computational costs. acceleration and velocity. Preliminary simulation results show thatonline the designed controller is capable of delivering acceptable performance at the cost of limited computational costs. capable of delivering acceptable performance at the cost of limited online computational costs. capable of delivering acceptable performance at the cost of limited online computational costs. © 2016, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Vehicle motion control, safety, model predictive control, autonomous driving Keywords: Vehicle Vehicle motion control, control, safety, model model predictive control, control, autonomous driving driving Keywords: Keywords: Vehicle motion motion control, safety, safety, model predictive predictive control, autonomous autonomous driving 1. INTRODUCTION proaches to the overall problem of vehicle motion control, 1. INTRODUCTION INTRODUCTION proaches to the overall problem vehicle motion 1. proaches to overall problem of vehicle control, climbing the ASIL ladder, from of a Quality Measurecontrol, (QM) 1. INTRODUCTION proaches to the the overall problem of vehicle motion motion control, climbing the ASIL ladder, from a Quality climbing the will ASIL ladder, from aa Quality Quality Measure (QM) to ASIL D, lead to enormous, costly Measure and time(QM) conAmong the technologies advancing within the automotive climbing the ASIL ladder, from Measure (QM) to ASIL D, will lead to enormous, costly and time conAmong the technologies technologies advancing within the automotive automotive to ASIL D, will lead to enormous, costly and time consuming verification problems, which inevitably will stall Among the advancing within the field, autonomous driving is definitely emerging with most to ASIL D, will lead to enormous, costly and time conAmong the technologies advancing within the automotive suming verification problems, which inevitably will stall field, autonomous driving is definitely emerging with most suming verification problems, which inevitably will the product development. In this paper we focus on the field, autonomous driving is definitely emerging with most promises of improving many aspects emerging of our lifestyles re- suming verification problems, which inevitably will stall stall field, autonomous driving is definitely with most the product development. In this paper we focus on the promises of improving many aspects of our lifestyles rethe product development. In this paper we focus on problem of designing a vehicle lateral motion controller promises of improving many aspects of our lifestyles related to transport. Road safety, traffic congestions and product development. In this papermotion we focus on the the promises of improving many aspects of our lifestylesand re- the problem of designing a vehicle lateral controller lated to transport. Road safety, traffic congestions problem of designing designing vehicle lateral motion motion controller with performance and aasafety guarantees and explore the lated to transport. traffic congestions pollutant emissions, Road transitsafety, efficiency, healthiness of and ur- problem of vehicle lateral controller lated to transport. Road safety, traffic congestions and with performance and safety guarantees and explore the pollutant emissions,totransit transit efficiency, healthiness of urwith performance and guarantees and the Model Predictive Control (MPC) techniques, pollutant emissions, ban environments, name efficiency, a few, are healthiness recognized of to urbe use withof performance and safety safety guarantees and explore explorewith the pollutant emissions,totransit efficiency, healthiness of of Model Predictive Control (MPC) techniques, with ban environments, environments, name a few, few, are recognized to urbe use use of Model Predictive Control (MPC) techniques, with the objective of providing systematic design methodologies ban to name a are recognized to be potentially and highly impacted by autonomous driving. use of Model Predictive Control (MPC) techniques, with ban environments, to name a few, are recognized to be objective of providing design methodologies potentially and highly highly impacted by autonomous autonomous driving. the the objective of systematic design satisfy ASIL D-type of systematic requirements. potentially and impacted by driving. It is then natural to question the maturity of the available the objective of providing providing systematic design methodologies methodologies potentially and highly impacted by autonomous driving. to to satisfy ASIL D-type of requirements. It is then natural to question the maturity of the available to satisfy satisfy ASIL ASIL D-type D-type of of requirements. requirements. It is then natural to question the maturity of the autonomous driving technologies, especially withavailable respect to It is then natural to question the maturity of the available In Guldner et al. (1996) steering control for passenger autonomous driving technologies, especiallyonwith with respect autonomous driving technologies, especially to the new and demanding requirements the respect vehicle In Guldner et al. (1996) steering control for passenger autonomous driving technologies, especiallyonwith respect In Guldner et al. (1996) steering control for passenger cars on automated highways is analyzed and to the new and demanding requirements the vehicle Guldner et al. (1996) steering controland for conditions passenger to the new and demanding on the vehicle motion control imposed by requirements Level 4 NHTSA (2013) of In cars on automated highways is analyzed conditions to the new and demanding requirements on the vehicle cars on automated highways is analyzed and conditions for the safety and performance criteria are proposed. In motion control imposed by Level 4 NHTSA (2013) of cars on automated highways is analyzed and conditions motion control imposed by Level 4 NHTSA (2013) of autonomous driving. for the safety and performance criteria are proposed. motion control imposed by Level 4 NHTSA (2013) of Lei for the safety and criteria are proposed. In et al. (2006) a performance vision-based lane detection method In is autonomous driving. for the safety and performance criteria are proposed. In autonomous driving. Lei et al. (2006) aa avision-based lane detection method is autonomous driving. Lei et al. (2006) vision-based lane detection method is along with PID controller for the lateral control. While the existing vehicle motion controllers use the driver utilized Lei et al. (2006) a avision-based lanefordetection method is utilized along with PID controller the lateral control. While the existing vehicle motion controllers use the driver utilized along with a PID controller for the lateral control. A comparative study of linear controllers for lane keeping While the existing vehicle motion controllers use the driver as a failsafe fall-back, in autonomous driving, the deviautilized along with a PID controller for the lateral control. While the existing vehicle motion controllers use the driver A comparative study of et linear controllers for lane lane keeping keeping as aa form failsafe fall-back, infor autonomous driving, the deviadevia- A of linear controllers for cancomparative be found instudy Taylor al. (1999). A dynamic as failsafe fall-back, autonomous driving, the tion a given path,in example, must be guaranteed comparative study of et linear controllers for lanefeedback keeping as a form failsafe fall-back, infor autonomous driving, the devia- A can be found in Taylor al. (1999). A dynamic feedback tion a given path, example, must be guaranteed can be found in Taylor et al. (1999). A dynamic feedback controller is proposed in Benine-Neto et al. (2010), which tion form aaangiven path, for example, must be guaranteed to satisfy Automotive Safety Integrity Level (ASIL) can be found in Taylor et al. (1999). A dynamic feedback tion form given path, for example, must be guaranteed controller is proposed in Benine-Neto et al. (2010), which to satisfy satisfy an an see Automotive Safety Integrity Level (ASIL) (ASIL) controller is proposed in Benine-Neto et al. (2010), which considers road curvature as bounded disturbance input. to Automotive Safety Integrity Level requirement, ISO-26262 (2011) for a description of the controller is proposed in Benine-Neto etdisturbance al. (2010), input. which to satisfy an see Automotive Safety Integrity Level (ASIL) considers road curvature as bounded requirement, ISO-26262 (2011) for a description of the considers road curvature as bounded disturbance input. Since the vehicle motion dynamics are nonlinear, conrequirement, see ISO-26262 (2011) for a description of the ASIL standard. The determination of the required ASIL considers road curvature as bounded disturbance input. requirement, see ISO-26262 (2011) for a description of the Since the the vehicle motion dynamics are can nonlinear, conASIL standard. The determination determination of the the required Smith ASIL straints Since vehicle motion dynamics are nonlinear, conrelated to safety and performance be naturally ASIL standard. The of required ASIL is the result of hazard analysis and risk assessment Since the vehicle motion dynamics are nonlinear, conASIL standard. The determination of the required Smith ASIL straints related to safety and performance can be naturally is the result of hazard analysis and risk assessment straints related to safety and performance can be naturally accommodated with MPC techniques, like in Falcone et al. is the result of hazard analysis and risk assessment Smith and Simpson (2010), which means that functionalities straints related to safety and performance can be naturally is the result of hazard analysis and risk assessment Smith accommodated with MPC techniques, like in Falcone et al. and Simpson Simpson (2010),forwhich which means that functionalities functionalities accommodated with MPC techniques, like in Falcone et al. (2007), where a MPC strategy for steering control of vehiand (2010), means that with likely potential severely life-threatening or fatal accommodated with MPC techniques, like in Falcone et al. and Simpson (2010),forwhich means that functionalities (2007), where a MPC strategy for steering control of vehiwith likely potential severely life-threatening or fatal (2007), where a MPC strategy for steering control of vehicle on slippery road is proposed. In Lee et al. (2012) a fast with likely potential for severely life-threatening or fatal injury in the event of a malfunction will be classified as (2007), where aroad MPC strategy forInsteering control ofavehiwith likely potential for severely life-threatening or fatal cle on slippery is proposed. Lee et al. (2012) fast injury D, in requiring the event event of of aavehicle malfunction will be be classified classified as MPC cle on slippery is In et a is proposed for lateral The paper injury in the malfunction will as ASIL manufacturer guarantee on strategy slippery road road is proposed. proposed. In Lee Leecontrol. et al. al. (2012) (2012) a fast fast injury in requiring the eventthe of avehicle malfunction will be to classified as cle MPC strategy is proposed for lateral control. The paper ASIL D, the manufacturer to guarantee −8vehicle MPC strategy is proposed for lateral control. The paper proposes an algorithm to approximate solution of the optiASIL D, requiring the manufacturer to guarantee aASIL failure rate of 10 eventsmanufacturer per hour. Intoconclusion, MPC strategy is proposed for lateral control. The paper D, requiring the guarantee −8vehicle proposes an algorithm to approximate of the optifailure rate of 10 10−8 events per hour. In conclusion, proposes an to solution of optiproblem underlying the MPCsolution controller, by using aa of events hour. In in Level 4rate autonomous staying lane will mization proposes an algorithm algorithm to approximate approximate solution of the the optia failure failure rate of 10−8 driving, events per per hour.within In conclusion, conclusion, mization problem underlying the MPC controller, by using in Level 4 autonomous driving, staying within lane will mization problem problem underlying the MPC MPC controller, by using using precomputed solutions. A MPC controller is designed to in Level 4 autonomous driving, staying within lane will need to be guaranteed with ASIL D. Hence, the ASIL mization underlying the controller, by in Level 4 autonomous driving, staying within lane will precomputed solutions. A MPC controller is designed to need to be guaranteed with ASIL D. Hence, the ASIL precomputed solutions. A MPC controller is designed to resemble the driver behavior in Gray et al. (2012). The need to be guaranteed with ASIL D. Hence, the ASIL D requirement to stay within the lane will reflect into precomputed solutions. A MPC controller is designed to need to be guaranteed with ASIL D. Hence, the ASIL resemble the driver behavior in Gray et al. (2012). The D requirement to stay within the lane will reflect into resemble the driver behavior in Gray et al. (2012). The controller is designed to only apply the correcting control D requirement to stay within the lane will reflect into a stringent requirement on the maximum totalreflect deviation resemble the driver behavior in Gray et al. (2012). The D requirement to stay within the lane will into controller designed only apply the correcting control a stringent requirement on the maximum total deviation controller is designed to only apply the control thatis necessaryto avoid violation of the safety cona stringent requirement on total deviation from the desired trajectory/path (performance safety action controller isis designed toto only apply the correcting correcting control a stringent requirement on the the maximum maximum totaland deviation action that is necessary to avoid violation of the safety confrom the desired trajectory/path (performance and safety action that that is necessary necessary to avoid avoid violation of the the safety safety constraints. A MPC problem for obstacle avoidance and lane from the desired trajectory/path (performance and safety guarantees), compatibly with the sensing technology. It is action is to violation of confrom the desired trajectory/path (performance and safety straints. A MPC problem for obstacle avoidance and lane guarantees), compatibly with the sensing technology. It is straints. A MPC problem for obstacle avoidance and lane keeping is proposed in Turri et al. (2013), based on linear guarantees), compatibly with the sensing technology. It is then clear that, without systematic control engineering apstraints. A MPC problem for obstacle avoidance and lane guarantees), compatibly with the sensing technology. It is keeping is proposed in Turri et al. (2013), based on linear then clear that, without systematic control engineering apkeeping is proposed in Turri et al. (2013), based on linear decoupled lateral and longitudinal dynamics, thus helping then clear that, without systematic control engineering apkeeping is proposed in Turri et al. (2013), based on linear then clear that, without systematic control engineering apdecoupled lateral and longitudinal dynamics, thus helping This work is partially supported by the Vinnova FFI Complex decoupled decoupled lateral lateral and and longitudinal longitudinal dynamics, dynamics, thus thus helping helping This work is partially supported by the Vinnova FFI Complex
Control Program, under the grant No. 2015-02309. is supported by the This This work work is partially partially supported by2015-02309. the Vinnova Vinnova FFI FFI Complex Complex Control Program, under the grant No. Control Program, under the grant No. 2015-02309. Control Program, under the grant No. 2015-02309.
Copyright © 2016, 2016 IFAC 292 Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright © 2016 IFAC 292 Copyright © 2016 IFAC 292 Peer review under responsibility of International Federation of Automatic Copyright © 2016 IFAC 292Control. 10.1016/j.ifacol.2016.08.043
IFAC AAC 2016 286 June 19-23, 2016. Norrköping, Sweden
L. Ni et al. / IFAC-PapersOnLine 49-11 (2016) 285–290
in framing a convex QP problem for fast calculation of the solution. In this paper, we explore MPC approaches to the vehicle lateral motion control problem. We focus on the problem of controlling the lateral vehicle motion subject to safety and performance requirements, along low curvature paths like in, e.g., highways. The road curvature is considered as disturbance input to the system. Safety and performance requirements are formulated in terms of the maximum deviation from the desired path and constraints on the vehicle states stemming from a desired comfort envelope. These constraints are guaranteed to be persistently satisfied within a known set of vehicle states for a curvature of the desired path within given boundaries. Preliminary simulation results show the performance and the viability of the proposed approach, encouraging further developments. The paper is structured as follows. In Section 2 we introduce a vehicle model, notations, and formally state the vehicle lateral motion control problem. Section 3 presents few preliminary results on invariant set and an algorithm to calculate the invariant set. In Section 4, the design procedure is shown. while Section 5 show the results of numerical simulations. The paper is concluded is in Section 6 with final remarks about the presented results and future research directions. 2. PROBLEM FORMULATION 2.1 Vehicle Modeling Consider the vehicle model sketched in Figure 1. For small road bank angle, the vehicle motion w.r.t. the path Γdes , subject to the lateral and yaw dynamics, is described by the following set of differential equations (Rajamani, 2006). mv˙ y = −mvx ψ˙ + 2 Fy + Fy , (1a) f
Jz ψ¨ = 2[lf Fyf − lr Fyr ], ˙ e˙ ψ = ψ˙ des − ψ,
r
(1b)
(1c) (1d)
e˙ y = −vy + vx eψ , ˙ (1e) ψdes = vx γ, x where m and Jz denote the vehicle mass and yaw inertia, respectively, lf and lr are the distances of the vehicle center of gravity from the front and rear axles, respectively, vx and vy are the longitudinal and lateral velocities, respectively, in the vehicle body frame, ψ˙ is the turning rate, where ψ denotes the vehicle orientation w.r.t. the fixed global frame (X, Y ) in Figure 1. Fyf , Fyr are the lateral tire forces at the front and rear axles, respectively. In (1c) and (1d), eψ and ey denote the vehicle orientation and position, respectively, w.r.t. the path Γdes and ψdes is the desired vehicle orientation, i.e., the slope of the tangent to the path Γdes in the point O. The lateral tire forces in (1a) and (1b) are generated at the tire contact patch and are, in general, nonlinear functions of the vehicle states. In this paper, we compute the lateral tire forces as, Fyi = −Ci αi , i ∈ {f, r}, (2) where Ci are the tire cornering stiffness coefficients at the two axles and αi are the tyre slip angles which, for small 293
values, can be approximated as, vy − lr ψ˙ vy + lf ψ˙ − δ, αr = , (3) αf = vx vx where δ denotes the front steering angle as depicted in Figure 1. In order to use the steering rate as control input,
Fig. 1. Vehicle in a desired path based coordinate system the model (1) is augmented with an integrator. Hence, for a given vehicle longitudinal speed vx , the model (1)-(3) can be compactly written as, x(t) ˙ = Ax(t) + Bu(t) + Ew(t), (4) T ˙ e ψ , ey , δ where x = vy , ψ, and w = γ are the state and the disturbance vectors and u = δ˙ is the steering input command. 2.2 System Constraints The input, state and disturbance vector in (4) is subject to a set of physical and design constraints. These constrains are the result of safety, performance and physical limitation of a vehicle. The safety requirements, for the considered problem, translate into the following constraints on the position ey eymin ≤ ey ≤ eymax , (5) To preserve the driving comfort, we impose bounds on the lateral vehicle speed and acceleration, which, for a given speed vx , can be written as, vymin ≤ vy ≤ vymax , (6a) a aymin y max ≤ ψ˙ ≤ , (6b) vx vx Further physical constraints stem from the limited steering and steering rate of the steering actuator. δmin ≤ δ ≤ δmax , (7) δ˙min ≤ δ˙ ≤ δ˙max .
The constraints (5)-(7) can be compactly rewritten for the system (4) as, X = {x ∈ R4 : Hx x ≤ hx }, (8) U = {u ∈ R : Hu u ≤ hu }. Finally, we assume that the curvature γ of the reference path Γdes is bounded, i.e., it belongs to the set, W = [γmin , γmax ] .
(9)
IFAC AAC 2016 June 19-23, 2016. Norrköping, Sweden
L. Ni et al. / IFAC-PapersOnLine 49-11 (2016) 285–290
2.3 Problem Statement Consider the problem of regulating the states of the system for a given constant speed vx . x(t + 1) = Ad x(t) + Bd u(t) + Ed w(t), (10) subject to the constraints (8), (9). Where Ad , Bd and Ed are the discrete versions of system matrices in (4). The problem can be formulated as a finite time, constrained optimal control problem, solved in receding horizon. In particular, the closed-loop, state feedback control law. ucl (x(t)) = u∗t|t (x(t)) , (11) u∗t|t
is adopted, where (x(t)) is found as the solution of the following optimization problem. Problem 1 t+N −1 xk|t 2Q + uk|t 2R Jt∗ (x(t)) = min xN |t 2P + Ut
k=t
(12a)
subj. to xk+1|t = Ad xk|t + Bd uk|t + Ed wk|t , (12b) (12c) xk|t ∈ X , uk|t ∈ U, wk|t ∈ W, (12d) xt+N |t ∈ Xf , (12e) xt|t = x(t), where Ut = ut|t , . . . , ut+N −1|t , N is a finite time horizon, P, Q 0 and R 0 are weighting matrices of appropriate sizes. Further, Xf is a terminal constraint, selected as a Robust Control Invariant Set, such that persistent feasibility for Problem 1 can be guaranteed (Kerrigan, 2000). The construction of this set is explained in Section 4. In order to assist the construction of terminal constraints we need to recall few definition on invariant sets in Section 3. Also, an approach is proposed for the construction of robust control invariant (RCI) set. 3. INVARIANT SET 3.1 Preliminaries In this section we introduce a few definitions and recall basic results on set invariance theory and reachability analysis for constrained systems and provide the results on feasibility of MPC schemes used next in this paper. A comprehensive survey of papers on set invariance theory can be found in Blanchini (1999). We will denote the set of all real numbers and positive integers by R and N+ , respectively. For the system, x(t + 1) = f (x(t), u(t), w(t)), (13) subject to the constraints x(t) ∈ X , u(t) ∈ U ⊆ Rm , w(t) ∈ W. (14) Definition 1. (Pref set). we define the set of states which can be driven into the target set S in one time step as Pref (S, W) {x ∈ Rn | ∃ u ∈ U s.t. f (x, u, w) ∈ S, ∀w ∈ W}. (15) The Pref set introduced in Definiton 1 can be used to calculate the robust control invariant sets. The following 294
287
definitions are derived from Blanchini (1999); Bertsekas and Rhodes (1971); Bertsekas (1971); Kolmanovsky and Gilbert (1998). Definition 2. (Robust Control Invariant Set). A set C ⊆ X is a robust control invariant set for the system in (13) subject to the constraints in (14), if x(t) ∈ C ⇒ ∃ u(t) ∈ U such that f (x(t), u(t), w(t)) ∈ C, ∀w(t) ∈ W, ∀t ∈ N+ (16) A robust control invariant set can be calculated by using the condition provided by the following theorem Kerrigan (2000); D´orea and Hennet (1999) Theorem 1. (Condition for invariance). A set C ⊆ X is a robust control invariant set for the system (13) subject to constraints (14), if and only if (17) Pref (C, W) ∩ C = C Definition 3. (Maximal Robust Control Invariant Set C∞ ). The set C∞ is the maximal robust control invariant set for the system in (13) subject to the constraints in (14), if it is robust control invariant and contains all robust control invariant sets contained in X . 3.2 Calculation of RCI In order to calculate the robuust control invariant set for system (12b) we have proposed an algorithm. The set C in (17) can be used, together with the definition of the set Pref in equation (18), written for the model in (12b) and the sets (8) and (9). In particular, the set C can be calculated by the following algorithm Algorithm 1 Computation of the robust control invariant set C INPUT: X , W, Ad , Bd , Ed OUTPUT: C 1: C0 ← X 2: k ← 0 3: Ck+1 ← Pref (Ck , W) ∩ Ck 4: if Ck+1 = Ck , then 5: goto 10, 6: else 7: C k ← Ck+1 8: k ← k + 1, 9: goto 3. 10: return Ck where, the set Pref at step 3 is calculated as Pref (X , W) {x ∈ X | ∃ u ∈ U s.t. Hx Ad x + Hx Bd u ≤ min hx − Hx Ed w}.
(18)
w∈W
Since the set Ck+1 is the result of a sets intersection, the complexity of its representation, i.e., the number of inequalities, increases at every iteration. Since Algorithm 1 stops as the condition at step 4 is verified, the representation of the set C might consist of an unnecessarily high number of inequalities, thus complicating the Problem 1. In this paper, Algorithm 1 is stopped when the Euclidean distance between the Chebyshev’s centers of Ck+1 and Ck ,
IFAC AAC 2016 288 June 19-23, 2016. Norrköping, Sweden
L. Ni et al. / IFAC-PapersOnLine 49-11 (2016) 285–290
respectively, is below a given threshold. That is, the condition at step 4 is replaced with the following condition rk − rk+1 ≤ rthr , where rk denotes the Chebyshev’s center of Ck and rthr is a threshold. Further stopping conditions can be added like, e.g., monitoring the radius of the Chebishev’s ball enclosed in Ck+1 and Ck .We show next how robust control invariant sets C can be used to guarantee feasibility of MPC schemes. 4. CONTROL DESIGN Since a MPC controller iteratively solves optimization Problem 1, persistent feasibility has to be guaranteed. Persistent feasibility implies that, if Problem 1 is feasible for some initial x(0) ∈ X0 , where X0 is the set of initial states for which the Problem 1 is feasible, the state trajectory is guaranteed to evolve within X0 . Persistent feasibility cannot be guaranteed for any choice of the tuning parameters. As mentioned in Kerrigan (2000), by selecting Xf as a robust control invariant set persistent feasibility can be guaranteed i.e. xt+N |t ∈ C, (19) where C is a robust control invariant set mentioned in Definition 2. Results enforcing robust closed-loop stability are available for min-max schemes Kerrigan (2000). In this paper, we enforce closed-loop stability condition for the nominal system only, i.e., with w = 0 in (12b). Although our approach does not guarantee stability for all possible road curvatures, extensive simulations have shown stable operation of the closed loop system for severe road curvature changes.
Jt (x(t), Ut ) = xt+N |t 2P +
k=t
xk|t 2Q + uk|t 2R , (20)
where the matrix P is the solution of the following Algebraic Riccati Equation (ARE) −1 T P = ATd P − P Bd BdT P Bd + R Bd P Ad + Q. (21)
Furthermore, instead of (19) we add the following constraint, xt+N |t ∈ O, (22) where O is the maximal robust invariant set for the closedloop system x(k + 1) = (Ad + Bd K) x(k) + Ed w(k), (23) and K is the Linear Quadratic (LQ) regulator. The robust invariant set O can be calculated with an algorithm similar to Algorithm 1, after re-defining the set Pref at step 3 as Pref (X , W) {x ∈ X |Hx (Ad + Bd K) x ≤ min hx − Hx Ed w}.
(b) 2-norm of the difference between consecutive radii of the Chebyshev ball.
Fig. 2. Calculation of the maximal robust control invariant set.
The terminal cost term in the cost function (12a), t+N −1
(a) Volume.
(24)
w∈W
Remark 1. Here we only focus on nominal stability i.e for w = 0. Robust stability can be acheived by introducing changes in road curvature as additional variable ∆w = w(k + 1) − w(k) and using the same framework. 5. SIMULATIONS The controller (12) has been tested in simulations and compared against a LQR regulator. The physical pa295
rameters used in the vehicle model (1) are reported in Table 1. The following bounds have been used in the Table 1. Model parameters Parameter m Jz Cr Cf lr lf
Description Mass Yaw moment of inertia Rear cornering stiffness coeff. Front cornering stiffness coeff. Rear axle to CoG distance Front axle to CoG distance
Value 2164 [kg] 4373 [kg × m2 ] 122380 [N/rad] 150540 [N/rad] 1.6456 [m] 1.3384 [m]
constraints (5)-(7) to construct the sets X , U in (12) ey = −eymin = 0.2 [m], max v ymax = −vymin = 0.4 [m/s], ◦ eψmax = −eψmin = 5[ ] = 0.0873 [rad], (25) aymax = −aymin = 3[m/s2 ], ◦ ˙ ˙ = − ψ = 8.251[ ]/s = 0.144 [rad/s], ψ min max ◦ δmax = −δmin = 2.5[ ] = 0.0436 [rad], ˙ δmax = −δ˙min = 2.86 [◦ ]/s = 0.05 [rad/s].
Furthermore the set (9) is defined by the following bounds on the road curvature. γmax = −γmin = 0.0263. The constraints in (25) may look more restrictive than necessary for normal driving conditions. Nevertheless, we recall that we consider high-speed, highway scenarios where an autonomous driving system should deliver a comfortable driving experience. The matrices Ad , Bd , Ed in (12b)
IFAC AAC 2016 June 19-23, 2016. Norrköping, Sweden
L. Ni et al. / IFAC-PapersOnLine 49-11 (2016) 285–290
289
have been obtained from the model (4) for vx = 75 [km/h], by applying the Zero-Order Hold (ZOH) discretization method, with a sampling time Ts = 25 [ms]. The weighting matrices of the cost (12a) have been chosen as follows 40 0 0 0 0
0 1 0 0 0 Q = 0 0 1 0 0 , R = 2. 0 0 0 1 0 0 0 0 0 20
With this choice of the tuning parameters, the LQ gain in (23) and the corresponding solution of the ARE (21) are, respectively, K = [3.6538 − 0.5135 27.9792 − 1.5585 − 14.6065],
0.0588 −0.0057 0.2098 −0.0040 −0.0360
(a) Lateral deviation.
−0.0057 0.0011 −0.0273 0.0001 0.0050 P = 104 × 0.2098 −0.0273 1.1977 −0.0358 −0.2660 . −0.0040 0.0001 −0.0358 0.0036 −0.0360 0.0050 −0.2660 0.0145
0.0145 0.1311
In particular, the matrix P has been used as terminal cost in (20). Finally, the terminal constraint (22) is enforced, where the invariant set O is calculated as in Algorithm 1 with the set Pref defined as in (24). Figure 2 shows the evolution of the Euclidean distance between the Chebyshev’s centers of Ok+1 and Ok and the radius of the Chebyshev’s ball enclosed in Ok . In this work, Algorithm 1 is stopped by using the stopping criterion mentioned in Section 3.2 and by monitoring the volume of Ok . In Figure 3 the curvature of the desired path Γdes
(b) Orientation error
(c) Yaw rate
Fig. 4. Safety constraints. Deviation of the vehicle from the desired path with the LQ and the MPC controller.
Fig. 3. Curvature of the desired path. used in our simulation is reported. A comparison of the LQR and the MPC controllers is shown in Figures 4-6, where the control input generated by LQR controller has been clipped according to the control input constraints in (25), before applying it to system. The simulations show that the clipped LQR controller is unable to stabilize the system, thus leading to unstable oscillations. The MPC controller, on the other hand, allow to easily accommodate closed- loop stability and design and physical constraints.
a LQ controller shows improvements in the constraints satisfaction at the cost of reasonable on-line computational complexity. Although preliminary, this work is the basis for further investigations aiming at understanding the impact on the conservativeness and the complexity of the controller of a design guaranteeing the satisfaction of safety and performance requirements, despite of model uncertainty and measurement noise.
6. CONCLUSIONS
REFERENCES
This paper presents preliminary simulation results of a MPC-based the lateral vehicle motion controller. In particular, the controller aims at minimizing the lateral deviation and the orientation error w.r.t. a desired path, while guaranteeing safety and performance requirements. Namely, bounding the deviation form the path and the lateral acceleration, respectively. A comparison against
Benine-Neto, A., Scalzi, S., Mammar, S., and Netto, M. (2010). Dynamic controller for lane keeping and obstacle avoidance assistance system. IEEE International Conference on Intelligent Transportation Systems, 1363– 1368. Bertsekas, D.P. (1971). Control of Uncertain Systems with a set–membership description of the uncertainty. Ph.D. thesis, Electronic Systems Laboratory, MIT.
296
IFAC AAC 2016 290 June 19-23, 2016. Norrköping, Sweden
L. Ni et al. / IFAC-PapersOnLine 49-11 (2016) 285–290
(a) Lateral velocity.
(b) Lateral acceleration.
Fig. 5. Performance (comfort) constraints satisfaction of the LQ and the MPC controller.
(a) Steering rate.
(b) Steering angle.
Fig. 6. Control signal of the LQ and the MPC controller. Bertsekas, D.P. and Rhodes, I.B. (1971). On the minimax reachability of target sets and target tubes. Automatica, 7, 233–247. 297
Blanchini, F. (1999). Set invariance in control — a survey. Automatica, 35(11), 1747–1768. D´orea, C. and Hennet, J.C. (1999). (a, b)-invariant polyhedral sets of linear discrete-time systems. Journal of Optimization Theory and Applications, 103(3), 521– 542. Falcone, P., Borrelli, F., Asgari, J., Tseng, H.E., and Hrovat, D. (2007). Predictive active steering control for autonomous vehicle systems. IEEE Transactions on Control Systems Technology, 15, 566–580. Gray, A., Ali, M., Gao, Y., Hedrick, J.K., and Borrelli, F. (2012). Integrated threat assessment and control design for roadway departure avoidance. IEEE International Conference on Intelligent Transportation Systems, 1714–1719. Guldner, J., Tan, H., and Patwardhan, S. (1996). Analysis of automatic steering control for highway vehicles with look-down lateral reference systems. Vehicle System Dynamics, 26(4), 243–269. ISO-26262 (2011). Road Vehicles Functional Safety, ISO 26262–Part–9, Geneva, Switzerland. Kerrigan, E.C. (2000). Robust Constraints Satisfaction: Invariant Sets and Predictive Control. Ph.D. thesis, Department of Engineering, University of Cambridge, Cambridge, England. Kolmanovsky, I. and Gilbert, E.G. (1998). Theory and computation of disturbance invariant sets for discretetime linear systems. Mathematical Problems in Egineering, 4, 317–367. Lee, S.H., Lee, Y.O., Kim, B.A., and Chung, C.C. (2012). Proximate model predictive control strategy for autonomous vehicle lateral control. American Control Conference, 3605–3610. Lei, G., Jianqiang, W., and Keqiang, L. (2006). Lane keeping system based on thasv-ii platform. IEEE International Conference on Vehicular Electronics and Safety, 305–308. NHTSA (2013). U.S. Department of Transportation Policy Release on Automated Vehicle Development. Rajamani, R. (2006). Vehicle Dynamics and Control. Springer. Smith, D. and Simpson, K. (2010). Safety Critical Systems Handbook - A Straightforward Guide to Functional Safety, IEC 61508 and Related Standards. Taylor, C., Kosecka, J., Blasi, R., and Malik, J. (1999). A comparative study of vision-based lateral control strategies for autonomous highway driving. The International Journal of Robotics Research, 18(5), 442–453. Turri, V., Carvalho, A., Tseng, H.E., Johansson, K.H., and Borrelli, F. (2013). Dynamic controller for lane keeping and obstacle avoidance assistance system. IEEE International Conference on Intelligent Transportation Systems, 378–383.