- Email: [email protected]
Experimental Validation of a Kinematic Bicycle Model Predictive Control with Lateral Acceleration Consideration⁎
10th IFAC Symposium on Intelligent Autonomous Vehicles 10th IFAC Symposium on Autonomous 10th IFACPoland, Symposium on Intelligent Intelligent Autonomous Vehicles Vehicles Gdansk, July 3-5, 2019 10th IFACPoland, Symposium on Intelligent Autonomous Vehicles Gdansk, July 3-5, 2019 Available online at www.sciencedirect.com Gdansk, Poland, July 3-5, 2019 10th IFACPoland, Symposium on Intelligent Autonomous Vehicles Gdansk, July 3-5, 2019 Gdansk, Poland, July 3-5, 2019
ScienceDirect
IFAC PapersOnLine 52-8 (2019) 289–294
Experimental Experimental Validation Validation of of a a Kinematic Kinematic Experimental Validation of a Kinematic Bicycle Model Predictive Control with Experimental a Kinematic Bicycle ModelValidation PredictiveofControl with Bicycle Model Predictive Control with Lateral Acceleration Consideration Bicycle Model Predictive Control with Lateral Acceleration Consideration Lateral Acceleration Consideration Lateral Acceleration Consideration ∗,∗∗ ∗,∗∗ ∗ ∗,∗∗ Jose A. Matute ∗,∗∗ Mauricio Marcano ∗,∗∗ ∗,∗∗ Sergio Diaz ∗ ∗
Jose A. Matute Mauricio Marcano Sergio Diaz Jose Marcano ∗ ∗,∗∗ Sergio Diaz ∗ ∗ Joshue Perez Jose A. A. Matute Matute ∗,∗∗ Mauricio Mauricio Marcano Sergio Diaz ∗ Perez ∗,∗∗ Joshue ∗,∗∗ Joshue Perez ∗ Jose A. Matute Mauricio Marcano Sergio Diaz ∗ Joshue Perez ∗ ∗ Perez ∗ Tecnalia Research Research and and Joshue Innovation, Derio, 48160 48160 Spain Spain (e-mail: (e-mail: ∗ Tecnalia Innovation, Derio, and Innovation, Derio, ∗ Tecnalia Research [email protected]). Tecnalia Research and Innovation, Derio, 48160 48160 Spain Spain (e-mail: (e-mail: [email protected]). ∗ [email protected]). ∗∗ Tecnalia Research and Innovation, Derio, 48160 Spain (e-mail: ∗∗ University of the Basque Country, Bilbao, 48013 Spain [email protected]). ∗∗ University of the Basque Country, Bilbao, 48013 Spain (e-mail: (e-mail: Basque Country, Bilbao, 48013 Spain (e-mail: ∗∗ University of the [email protected]). [email protected]) University of the Basque Country, Bilbao, 48013 Spain (e-mail: [email protected]) ∗∗ [email protected]) University of the Basque Country, Bilbao, 48013 Spain (e-mail: [email protected]) [email protected]) Abstract: Nowadays, Automated Driving has growing interest in scientific and industrial Abstract: Nowadays, Nowadays, Automated Automated Driving Driving has has aaa growing growing interest interest in in the the scientific scientific and and industrial industrial Abstract: automotive community. The vehicle motion is an essential safe Abstract: Nowadays, Automated Driving hasplanning a growing interest in the theprocedure scientific to andobtain industrial automotive community. The vehicle motion planning is an essential procedure to obtain safe automotive community. The vehicle motion planning is an essential procedure to obtain safe Abstract: Nowadays, Automated Driving has a growing interest in the scientific and industrial and comfortable trajectories, adapting the longitudinal speed to the road legal limits and automotive community. The vehicle motion planning is an essential procedure to obtain safe and comfortable trajectories, adapting the longitudinal speed to the road legal limits and and comfortable trajectories, adapting the longitudinal speed to the road legal limits and automotive community. The vehicle motion planning is an essential procedure to obtain safe mainly to avoid the excessive lateral accelerations along the journey. Typically, the proper and comfortable trajectories, adapting the longitudinal speed to the road legal limits and mainly to to avoid avoid the the excessive excessive lateral lateral accelerations accelerations along along the the journey. journey. Typically, Typically, the the proper proper mainly and trajectories, adapting theto to path, the road legal the limits and speed of the vehicle is intrinsically related the curvature of the requiring a previous mainly avoid the lateral accelerations alongspeed the Typically, proper speedcomfortable oftothe the vehicle isexcessive intrinsically related tolongitudinal the curvature curvature of journey. the path, requiring previous speed of vehicle is intrinsically related to the of the path, requiring aaa previous mainly to avoid the excessive lateral accelerations along the journey. Typically, the proper approximation of this parameter in the planning stage. In this work, a novel procedure to speed of the vehicle is intrinsically related to the curvature of the path, requiring previous approximation of of this this parameter parameter in in the the planning planning stage. stage. In In this this work, work, aa novel novel procedure procedure to to approximation speed of the vehicle is intrinsically related to the curvature of the path, requiring a previous follow a route trajectory and speed limits considering the lateral acceleration parameter is approximation of this parameter in the planning stage. In this work, a novel procedure to follow a route trajectory and speed limits considering the lateral acceleration parameter is follow a route trajectory and speed limits considering the lateral acceleration parameter is approximation of this parameter in the planning stage. In this work, a novel procedure to presented. A lateral jerk equation was developed and introduced into a kinematic bicycle model follow a route trajectory and speed limits considering the lateral acceleration parameter is presented. A A lateral lateral jerk jerk equation equation was was developed developed and and introduced introduced into into aa kinematic kinematic bicycle model presented. bicycle model follow a route trajectory and An speed limits considering the lateral acceleration parameter is presented. A lateral jerk equation was developed and introduced intothat a kinematic bicycle model predictive control formulation. An adaptive speed weight equation that depends on on the lateral lateral predictive control formulation. adaptive speed weight equation depends the predictive control formulation. An adaptive speed weight equation that depends on the lateral presented. A lateral jerk equation was developed and introduced into a kinematic bicycle model acceleration is presented to improve the lateral positioning. A vehicle motion control simulation, predictive control formulation. An adaptive speed weight equation that depends on the lateral acceleration is is presented presented to to improve improve the the lateral positioning. positioning. A A vehicle vehicle motion motion control control simulation, simulation, acceleration predictive control formulation. An with adaptive speed weight equation depends on simulation, the lateral developed in is validated some real tests. The show the capabilities of the acceleration isDynacar, presented to improve the lateral lateral positioning. Aresults vehiclethat motion control developed in Dynacar, is validated with some real tests. The results show the capabilities of the the developed in Dynacar, is validated with some real tests. The results show the capabilities of acceleration is presented to improve the lateral positioning. A vehicle motion control simulation, proposed approach. An accurate vehicle motion control considers the lateral acceleration to developed in Dynacar, is validated with some real tests. The results show the capabilities of the proposed approach. approach. An An accurate accurate vehicle vehicle motion motion control control considers considers the the lateral lateral acceleration acceleration to to proposed developed in Dynacar, is accurate validated vehicle with some real tests. results show the capabilities of the proposed approach. motion controlThe considers the lateral acceleration to avoid unfeasibility unfeasibility inAn optimization problem. avoid in optimization problem. avoid unfeasibility in optimization problem. proposed approach.inAn accurate vehicle motion control considers the lateral acceleration to avoid unfeasibility optimization problem. © 2019, IFAC (International Federationproblem. of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. avoid unfeasibility in optimization Keywords: Intelligent Path Intelligent Systems. Keywords: Intelligent Intelligent Control, Control, Path Path Planning, Planning, Intelligent Intelligent Transportation Transportation Systems. Systems. Keywords: Keywords: Intelligent Control, Control, Path Planning, Planning, Intelligent Transportation Transportation Systems. Keywords: Intelligent Control, Path Planning, Intelligent Transportation Systems. 1. INTRODUCTION that pursue performance, safety and comfort. In (Matute 1. INTRODUCTION INTRODUCTION that pursue performance, safety and comfort. In (Matute 1. that pursue performance, safety and comfort. In (Matute 1. INTRODUCTION et al., 2018), MPC is implemented as a speed controller that pursue performance, safety and comfort. In (Matute et al., 2018), MPC is implemented as a speed controller et al., 2018), MPC is implemented as a speed controller 1. INTRODUCTION that pursue performance, safety and comfort. In (Matute with constraints of longitudinal jerk and acceleration, In Automated Driving, the vehicle motion planning is et al., 2018), MPC is implemented as a speed controller with constraints constraints of of longitudinal longitudinal jerk jerk and and acceleration, acceleration, In Automated Automated Driving, Driving, the the vehicle vehicle motion motion planning planning is is with In et al., 2018), MPC is implemented as a speed controller using a triple integrator model. However, it failed on In Automated Driving, the vehicle motion planning is an essential procedure to obtain a safe and comfortable with constraints of longitudinal jerk and acceleration, aa triple integrator model. However, it failed on an essential essential procedure procedure to to obtain obtain aa safe safe and and comfortable comfortable using using triple integrator model. However, it failed on an with constraints of longitudinal jerk and acceleration, In Automated Driving, the vehicle motion planning is considering the lateral actions of the vehicle, which have driving experience in automated mode. Researchers have using a triple integrator model. However, it failed on an essential procedure to obtain a safe and comfortable considering the lateral actions of the vehicle, which have driving experience in automated mode. Researchers have considering the lateral actions of the vehicle, which have driving experience in automated mode. Researchers have using a triple integrator model. However, it failed on an essential procedure to obtain a safe and comfortable a high impact on passenger comfort. Other works (Mata been putting efforts on this area proposing the use of considering the lateral actions of the vehicle, which have driving experience in automated mode. Researchers have high impact impact on on passenger passenger comfort. comfort. Other Other works works (Mata (Mata been putting putting efforts efforts on on this this area area proposing proposing the the use use of of aa high been considering theon lateral actions of the Other vehicle, which have driving experience in automated mode. Researchers have et al., 2017; Kong et al., 2015), include a kinematic bicycle the use of smooth curves for obstacle free trajectories, and speed a high impact passenger comfort. works (Mata been putting efforts on this area proposing et al., 2017; Kong et al., 2015), include a kinematic bicycle smooth curves curves for for obstacle obstacle free free trajectories, trajectories, and and speed speed et al., 2017; Kong et al., 2015), include a kinematic bicycle smooth high impact on the passenger comfort. Other works (Mata been putting on thisfree area proposing the of aet model to couple lateral and longitudinal control of smooth curvesefforts for on obstacle trajectories, and use speed profiles dependent curvature to reduce discomfort in al., 2017; Kong et al., 2015), include a kinematic bicycle model to couple the lateral and longitudinal control of profiles dependent on curvature to reduce discomfort discomfort in model to couple the lateral and longitudinal control of profiles dependent on curvature to reduce in et al., 2017; Kong et al., 2015), include a kinematic bicycle smooth curves for obstacle free trajectories, and speed the vehicle in a single optimization problem. However, bends and lateral maneuvers (Lattarulo et al., 2018). model to couple the lateral and longitudinal control of profiles dependent on curvature to reduce discomfort in the vehicle in a single optimization problem. However, bends and lateral maneuvers (Lattarulo et al., 2018). the vehicle in a single optimization problem. However, bends and lateral maneuvers (Lattarulo et al., 2018). model to couple the lateral and longitudinal control of profilesand dependent on curvature to reduce discomfort when different objectives functions compete (e.g. increase vehicle in a single optimization problem. However, bends lateral maneuvers (Lattarulo et al., 2018). in the when different objectives functions compete (e.g. increase However, comfort should not be considered in the planning when different objectives functions compete (e.g. increase the vehicle in a single optimization problem. However, However, comfort should not be considered in the planning bends and lateral maneuvers (Lattarulo et al., 2018). when different objectives functions compete (e.g. increase tracking performance, performance, reach reach aa speed speed set set point point and and reduce reduce However, comfort should not be be considered considered in the planning planning tracking stage alone, because it assumes that the controller will performance, reach aa problem speed set point and reduce However, comfort should not the when different objectives functions compete (e.g. increase stage alone, alone, because it assumes assumes that the the in controller will tracking steering rate) the optimization may be unfeasible, tracking performance, reach speed set point and reduce stage because it that controller will steering rate) the optimization problem may be unfeasible, However, comfort should not be considered in the planning follow the references perfectly. Instead, it must be considsteering rate) the optimization problem may be unfeasible, stage alone, because it assumes that the controller will performance, reach a speed set point and reduce follow the the references references perfectly. perfectly. Instead, Instead, it it must must be be considconsid- tracking producing unexpected behaviors on the vehicle motion and steering rate) the optimization problem may be unfeasible, follow unexpected behaviors on the vehicle motion and stage alone, because itcontrol assumes thatof controller will producing ered together with the aspect driving. Although producing unexpected behaviors on the vehicle motion and follow the references Instead, it must be considsteering rate) the optimization problem may be unfeasible, ered together together with the theperfectly. control aspect ofthe driving. Although compromising safety. producing unexpected behaviors on the vehicle motion and ered with control aspect of driving. Although compromising safety. follow the references perfectly. Instead, it must be considcomfort in a dynamic driving task is not formally defined compromising safety. ered together with the control aspect of driving. Although producing unexpected behaviors on the vehicle motion and comfort in a dynamic driving task is not formally defined compromising safety. comfort in a dynamic driving task is not formally defined ered together with theit control aspect of Although this sense, (Polack et al., 2017) states that keeping the (Bellem et 2018), is related in practice with variables comfort in al., a dynamic driving task is notdriving. formally defined In In this sense, (Polack et al., 2017) states that keeping the compromising safety. (Bellem et al., 2018), it is related in practice with variables In this sense, (Polack et al., 2017) states that keeping the (Bellem et al., 2018), itdriving is related related in is practice withwhich variables comfort in a dynamic task not formally defined lateral acceleration under a threshold of 0.5g guarantees such as jerk and acceleration (Bautista, 2017), are In this sense, (Polack et al., 2017) states that keeping thea (Bellem et al., 2018), it is in practice with variables acceleration under aa threshold of 0.5g guarantees a such as as jerk jerk and and acceleration acceleration (Bautista, (Bautista, 2017), 2017), which which are are lateral lateral acceleration under threshold of 0.5g guarantees such In this sense, (Polack et al., 2017) states that keeping thea (Bellem et al., 2018), it is related in practice with variables lateral acceleration under a threshold of 0.5g guarantees a feasible solution solution when when aa kinematic kinematic bicycle bicycle model model is is used. used. characteristics of acceleration the control control stage. stage. such as jerk and (Bautista, 2017), which are feasible characteristics of the feasible solution when a kinematic bicycle model is used. characteristics of the control stage. lateral acceleration under a threshold of 0.5g guarantees a such as jerk and (Bautista, 2017), which are feasible Considering this, the novelty of the present work relies on solution when a kinematic bicycle model is used. characteristics of acceleration the control stage. Considering this, the novelty of the present work relies on Nonetheless, not all control techniques allow to consider this, the novelty of the present work relies on feasible solution when a kinematic bicycle model is used. Nonetheless, not not all control control techniques allow to to consider consider Considering characteristics of the control techniques stage. the inclusion of the lateral jerk formula within the MPC Considering this, the novelty of the present work relies on Nonetheless, all allow inclusion of the lateral jerk formula within the MPC Nonetheless, all control techniques allowe.g. to consider these vehicle vehicle not states in the the design design process, e.g. classical the the inclusion of lateral formula the MPC Considering the noveltyjerk of the present work relies on these states in process, classical formulation. This consideration allows aawithin new state conthe inclusionthis, of the the lateral jerk formula within the MPC these vehicle states in the design process, e.g. classical formulation. This consideration allows new state conNonetheless, not all control techniques allow to consider controllers. In contrast, Model Predictive Control (MPC) formulation. This consideration allows a new state conthese vehicle states in the design process, e.g. classical the inclusion of the lateral jerk formula within the MPC controllers. In contrast, Model Predictive Control (MPC) formulation. This consideration allows a new state constraint limiting the lateral acceleration to assure a feasible controllers. In states contrast, Model Predictive Control (MPC) straint limiting the lateral acceleration to assure a feasible these vehicle in the design process, e.g. functions classical technique allows to combine different objectives straint limiting the lateral to a controllers. In contrast, Model Predictive Control (MPC) formulation. This consideration allows new state contechnique allows to combine combine different objectives functions solution. This also the use of simple vehicle model straint limiting thepermits lateral acceleration acceleration toa assure assure a feasible feasible technique allows to different objectives functions solution. This also permits the use of simple vehicle model controllers. In contrast, Model Predictive Control (MPC) This also permits the use of simple vehicle model technique allows to combine different objectives functions solution. straint limiting the lateral acceleration to assure a feasible This project has received funding from the Electronic Component which require less computational effort. Additionally, an solution. This also permits the use of simple vehicle model This project project has received received funding from the theobjectives Electronic Component Component This which require less computational effort. Additionally, an technique allows to combine different functions has funding from Electronic which require less computational effort. Additionally, an Systems for European Leadership Undertaking grant solution. This also permits the use of simple vehicle model This project has received funding Joint from the Electronic under Component adaptive speed weight equation that depends on the latwhich require less computational effort. Additionally, an Systems for European Leadership Joint Undertaking under grant adaptive speed weight equation that depends on the latSystems for European Leadership Joint Undertaking under grant adaptive speed weight equation that depends on the latagreement No 737469 (AutoDrive Project). This Joint Undertaking This project has received funding from the Electronic Component which require less computational effort. Additionally, an Systems for European Leadership Joint Undertaking under grant adaptive speed weight equation that depends on the lateral acceleration is also included to improve the tracking agreement No 737469 (AutoDrive Project). This Joint Undertaking eral acceleration is also included to improve the tracking agreement No 737469 (AutoDrive Project). This Joint Undertaking eral acceleration is also included to improve the tracking receives support from the European Union 2020 research and agreement 737469 (AutoDrive Project). This Joint Undertaking Systems forNoEuropean Leadership Joint Horizon Undertaking under grant adaptive speed weight equation that depends on the latreceives support from the European Union Horizon 2020 research and performance of the vehicle. eral acceleration is also included to improve the tracking receives support from the European Union Horizon 2020Italy, research and performance of the vehicle. innovation programme and Germany, Austria, Spain, Latvia, performance of agreement No 737469 (AutoDrive Project). This Joint Undertaking receives support from the European Union Horizon 2020Italy, research and innovation programme and Austria, Spain, Latvia, eral acceleration is vehicle. also included to improve the tracking performance of the the vehicle. innovation programme Sweden, and Germany, Germany, Austria, Spain, Italy, Latvia, Belgium, Netherlands, Finland, Lithuania, Czech Republic, innovation programme and Germany, Austria, Spain, Italy, Latvia, receives support from the European Union Horizon 2020 research and This work is organized as follows. In Section II, the control Belgium, Netherlands, Sweden, Finland, Lithuania, Czech Republic, This work is organized as follows. In Section II, the control performance of the vehicle. Belgium, Netherlands, Sweden, Finland, Lithuania, Czech Republic, This work is organized as follows. In Section II, the control Romania,Netherlands, Norway. ThisSweden, workGermany, was developed at Tecnalia Tecnalia Research & innovation programme and Austria, Spain, Italy, Latvia, Belgium, Finland, Lithuania, Czech Republic, architecture developed is described in detail. Section III, This work is organized as follows. In Section II, the control Romania, Norway. This work was developed at Research & architecture developed is described in detail. Section III, Romania, Norway. This work was developed at Tecnalia Research & architecture developed is described in detail. Section III, Innovation facilities supporting this research. Belgium, Netherlands, Sweden, Finland, Lithuania, Czech Republic, Romania, Norway. This work was developed at Tecnalia Research & This work is organized as follows. In Section II, the control Innovation facilities supporting this research. architecture developed is described in detail. Section III, Innovation facilities supporting this research. Romania, work was developed InnovationNorway. facilitiesThis supporting this research.at Tecnalia Research & architecture developed is described in detail. Section III,
Innovation supporting this research. 2405-8963 © ©facilities 2019, IFAC IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Copyright 2019 Copyright © 2019 Copyright © under 2019 IFAC IFAC Peer review responsibility of International Federation of Automatic Control. Copyright © 2019 IFAC 10.1016/j.ifacol.2019.08.085 Copyright © 2019 IFAC
2019 IFAC IAV 290 Gdansk, Poland, July 3-5, 2019
Jose A. Matute et al. / IFAC PapersOnLine 52-8 (2019) 289–294
Fig. 1. Control architecture discuss the results obtained in simulation and experimental tests. Lastly, Section IV presents final conclusions and future research work.
Fig. 2. Route for real and simulated tests
2. CONTROL ARCHITECTURE This section describes the algorithm developed for path tracking applications based on a vehicle predictive model. Using the state of the vehicle, the trajectory planning determines the references for the longitudinal and lateral vehicle motion control, estimating and defining the command positions for the pedals and steering wheel. This approach was developed as a general framework for path following in automated driving applications. The aim is to follow a predefined route with the maximum level of safety and desired comfort. In this work, the same Dynamic Driving Task (DDT) is defined for both virtual and real situations as depicted in the Fig. 1. The problem constraints are stated according with the physical capabilities of the real actuators installed in the vehicle, considering the maximum displacements and change rates. 2.1 Trajectory Planning The references to be followed by the vehicle motion control are defined performing a strategy of offline and online stages described by (Matute et al., 2018). The offline stage contains relevant information about the predefined map for path tracking. The procedure to generate the route used in this work is broadly described by (Lattarulo et al., 2018). The route contains several kinds of traffic maneuvers very usual in urban environments as roundabouts, intersections and lane changes. Depending on the maneuver to be performed, the path is built using B´ezier curves of fourth order or higher using hard points as those depicted in the legend of the Figure 2. From these hard points, it is possible to obtain smooth routes with continuous curvatures described by an ordered list of waypoints which contain relevant information for path tracking as; locations in Cartesian coordinates (X and Y ), orientations (Ψ), speed limits (vx ), curvatures and traveled distance from the starting point of the map. Moreover, physical constraints for vehicle motion control are specified. The online stage uses the current and future locations of the vehicle to project them to the nearest sections of the route using the map developed in the offline stage. The location of the vehicle is projected onto the map using dot 290
Fig. 3. Route references of curvature and velocity products to obtain the references to perform the lateral vehicle motion control as shown in the Fig. 3. The speed reference is generated considering that the lateral acceleration never exceeds 1.0m/s2 —, fairly uncomfortable-based on the ISO 2631-1 Standard (ISO, 1997). The speed limit of this section is used as reference parameter to perform the longitudinal vehicle motion control. It is important to note that the planned route has curvatures higher than those generally expected in real traffic scenarios. A path radius lower than 6.67m (k > 0.15) is usually considered for slow speed maneuvers. This map is generated considering the space available for real tests. 2.2 Vehicle Motion Control This DDT sub-task includes the maintaining of the speeds below specified limits applying propulsion or braking inputs, as well as maintaining an appropriate lateral positioning through the application of steering inputs. The sustained regulation of the x-axis and y-axis components of vehicle motion depicted in Fig. 4 are performed using a coupled MPC strategy. The lateral acceleration is considered as an additional parameter to improve the lateral positioning, defining also a desired level of comfort during the path tracking on curve roads. Lateral Acceleration Consideration Considering small time steps for the computation of the vehicle’s motion, its lateral acceleration ay is approximated as a uniform circular motion expressed as ay = vx2 /R, where vx is the vehicle’s longitudinal speed and R is the path radius. From the bicycle model assumption considering a front wheel steered vehicle, it is possible to approximate the path radius as R = L/δ for small values of slip angle
2019 IFAC IAV Gdansk, Poland, July 3-5, 2019
Jose A. Matute et al. / IFAC PapersOnLine 52-8 (2019) 289–294
291
The control strategy consists to track a desired trajectory under specific speeds considering the current lateral accelerations of the vehicle. The output differential states to be optimized are defined in the Eq. 4. The term of ay is not optimized being only used as a constraint.
1 0 η(k) = h(χ(k)) = 0 0
0 1 0 0
0 0 1 0
0 0 0 1
0 0 χ(k) 0 0
(4)
The cost function for the optimization is in the Eq. 5.
Fig. 4. Simplified kinematic bicycle model
J(χ(t), u(t)) =
(β) (Rajamani, 2011). Therefore, the lateral acceleration equation can be expressed as shown in the Eq. 1. ay =
vx2 δ L
(1)
This formula is later on differentiated to be included as a differential state variable into the kinematic bicycle MPC formulation as depicted in the Eq. 2e. Kinematic Bicycle Model The MPC formulation considers a kinematic bicycle model of the vehicle as shown in Fig. 4. The equations are based on geometric relationships without considering any forces. The two left and right wheels are represented as two single wheels at points A and B, representing the front and rear ends of the vehicle wheelbase L. The center of gravity of the vehicle is at point C and defines the location of the vehicle. The vehicle is assumed to have a planar motion. The location coordinates and orientation (X, Y, Ψ) describe the motion of the vehicle. The longitudinal speed (vx ) and lateral acceleration (ay ) are parallel and perpendicular to the longitudinal axis of the vehicle, respectively. The longitudinal jerk (jx ) and the steering change rate (∆δ) are the two inputs on this model. X˙ = vx cos(Ψ) Y˙ = vx sin(Ψ) ˙ = vx tan(δ) Ψ L v˙ x = ax
vx a˙ y = (2ax δ + vx ∆δ) L a˙ x = jx δ˙ = ∆δ
(2a) (2b) (2c) (2d) (2e) (2f) (2g)
MPC Formulation The nonlinear vehicle dynamics formulation described in Eqs. 2a-2e can be described in the general compact form depicted in the Eq. 3. dχ = f (χ(t), u(t)) dt
(3)
where the differential state and control parameters are ˙ T , respectively. χ = [X, Y, Ψ, vx , ay ]T and u = [a˙ x , δ] 291
H i=1
2
2
ref ηt+i,t − ηt+i,t Q + ut+i,t R
(5)
where η = [X, Y, Ψ, vx ]T and η ref contains the references for each of the differential states to be optimized, all of them related with information provided from the trajectory planner. At the right side of the Eq. 5, the first term denoted the penalty on the differential states and the second one measures the control variables. The weight matrices are defined intuitively in order to provide a balance between performance and smoothness as Q = diag([1, 1, 1, S]) and R = diag([1, 0.01]), where Q and R are the weight matrices for the differential states and control parameters, respectively. The term S within the matrices is related to the weight of the longitudinal velocity, which changes according to the value of the current lateral acceleration as is explained later in the subsection 2.2.4. The finite horizon of the Optimal Control Problem (OCP) formulation is solved at each step t as: min J(χt , ut )
(6a)
χk+1,t = f (χk,t , uk,t ) ηk,t = h(χk,t ) k = t, ..., t + H ζf,min ≤ ζk,t ≤ ζf,max uf,min ≤ uk,t ≤ uf,max χt,t = χ(t)
(6b) (6c) (6d) (6e) (6f) (6g)
η(·),u(·)
s.t.
where ηt+i,t is the optimization vector at time t + i beginning from state χt,t = χ(t). The differential states prediction horizon is defined as H. The OCP is solved at each time step and a new H is generated in according to χ(t + 1). The sequence for the solution is settled as k = t, ..., t + H. The constraints of the problem are defined by ζk,t and uk,t . Adaptive Speed Weight The highest errors in path following occur before, during and after the vehicle makes a turn. An adaptive speed weight in the OCP formulation highly contributes to a more accurate trajectory tracking on this case. The aim is to reduce the weight of the speed as the lateral acceleration is increasing. This strategy permits to give more importance to the location of the vehicle, reducing the lateral and angular errors when the vehicle is taking a curve. A proposal to obtain an adaptive weight (n = 2) is presented in the Eq. 7.
2019 IFAC IAV 292 Gdansk, Poland, July 3-5, 2019
S =1−
Jose A. Matute et al. / IFAC PapersOnLine 52-8 (2019) 289–294
|ay |n max(ay )
(7)
where ay is the current laterals acceleration of the vehicle and max(ay ) is the maximum lateral acceleration permitted and settled for driving constraints. The value remains always among 0 and max(ay ). Driving Constraints The bounds for the differential states (ζk,t ) and control (uk,t ) are selected considering the physical capabilities of the vehicle and actuation devices, as well as comfort level to be perceived by the passenger. The constraint values are depicted in the Table 1. Table 1. Driving constraints States
Controls
Parameter ayk,t
Min. -2
Max. 2
Unit m/s2
x vk,t ax k,t δk,t x jk,t ∆δk,t
0 -10 -0.69 -0.50 -0.50
ref vk,t 1 0.69 0.50 0.50
m/s m/s2 rad m/s3 rad/s
Fig. 5. Test vehicle instrumentation
According to (Polack et al., 2018), values of lateral acceleration below 0.5g avoid the appearance of unfeasible results in the solution of the OCP when the kinematic bicycle model is employed in vehicle motion control. MPC Solver The open-source ACADO Toolkit is used to solve the OCP (Quirynen et al., 2015). The OCP is reformulated to an approximate nonlinear program (NLP) using a direct multiple shooting discretization method. The generalized Gauss-Newton approximation iterates by solving the Sequential Quadratic Program (SQP) algorithm to solve the NLP. The SQP is then solved by the dense linear algebra solver qpOASES3. A continuous output Implicit Runge-Kutta of Gauss-Legendre integrator of order 2 is exported by the code to simulate the system with 20 integration steps. The H estimation is parameterized to obtain 10 elements discretized at 300ms. 2.3 Test Vehicle The test vehicle selected for real and simulated experiments is a Renault Twizy E80, an electric quadricycle which technical specifications are depicted in the Table 2. A multi-body model is developed in the software Dynacar and is employed to simulate the behavior of the vehicle as explained in (Lattarulo et al., 2017). The power-train, brakes and steering are characterized to resemble the real devices as detailed in (Marcano et al., 2018). Instrumentation and Data-acquisition The vehicle has been instrumented as shown in the Fig. 5 to carry out the experiments. A Global Navigation Satellite System plus an Inertial Navigation System device (GNSS+INS) is used to obtain Real-Time Kinematic positioning with a position accuracy of 2cm. A high-performance personal computer (PC) runs the control architecture software under MATLAB/Simulink environment sending the control commands to an industrial Programmable Logic Controller (PLC). The PLC is connected through a CAN bus network to the low-level control of the vehicle. The throttle pedal 292
has a parallel connection that simulates the pedal position for propulsion signals when the vehicle is in automated mode. The brake pedal is physically connected with a rotary servo motor through a steel cable. The steering wheel is also physically connected with a rotary servo motor through a synchronous belt though. Both brake and steer have position controllers that receive and execute the control from position commands delivered by the PLC. An encoder is connected to the rotary servo motor of the steering wheel to deliver the position feed-back to the PLC being useful as an MPC differential state. The sampling process for all the measurements in the real platform are synchronized with the GNSS+INS with a frequency 100Hz, the same used in the simulated platform. 3. RESULTS AND DISCUSSION The same tests were performed in both the simulator and the physical vehicle, following the route depicted on Fig. 2. This test circuit represents several traffic urban conditions, with different curvature radius and reference speeds (see Fig. 3). In this section, the behavior of the kinematic bicycle MPC under such conditions is evaluated and validated both in simulation and in actual vehicle tests. Note that the same controller is used in both cases. The computational time of the control cycle was 1ms in average both in simulation and real-life experiments, proving the applicability of the algorithm aa well as to be efficiently enough for real-time applications. Table 2. Vehicle technical specifications Parameter Mass Dimensions CG location Wheelbase Track-width Inertia Front wheel radius Rear wheel radius Steering ratio Traction torque Transmission ratio Braking torque
Value 611.50 2.34 x 1.23 x 1.45 -0.93 x 0.00 x 0.49 1.69 1.09 243.18, 430.17, 430.17 0.27 0.28 14.27:1 57 1:9.23 500
Unit kg m m m m kg-m2 m m N-m N-m
2019 IFAC IAV Gdansk, Poland, July 3-5, 2019
Jose A. Matute et al. / IFAC PapersOnLine 52-8 (2019) 289–294
Fig. 6. Command signals to actuation devices
293
Fig. 7. Longitudinal and lateral accelerations
Pedal and steering wheel actuation is portrayed in Fig. 6. First, note the similarity between simulations and experiments. However, it is also noted that a significantly stronger brakes application is required prior to curve entrances for the experimental case. This might be related to the fact that brakes are simulated with “as new” parameters, while in the experimental bench brakes are actually nearing the end of their life-cycle. It is also apparent that, in the straights, the experimental setup requires more acceleration (i.e. torque) than the simulation. Steering wheel actuation in the actual vehicle closely resembles that obtained from the simulations, both in amplitude and phasing. Some oscillations not seen on the simulation do appear on the experiments, both for the steering and pedal positions. It is speculated that such oscillation might be related to the behavior of the low-level control and actuators, particularly on the pedals, which might present delays, free play, and hysteresis not fully modeled in the simulations. Recall that the lateral and longitudinal motions are coupled both by the vehicle dynamics and by the implemented MPC. Fig. 7 shows longitudinal and lateral accelerations. Again, the experiments follow closely the simulations, fully complying with the dynamic and comfort bounds. The controller is able to adjust pedal inputs on the actual vehicle to compensate for the extra braking and motor torque requirements (as discussed above) in order to follow the circuit defined path with the same dynamic behavior than the simulated vehicle (showing similar longitudinal and lateral accelerations). As expected, the oscillations observed on the actuators for the experimental bench are also evident on the accelerations. As shown by the present tests, it is a bound effect and the system can acceptably work with it. Further research will be performed to reliably determine the source of this oscillation and to establish the most appropriate means to deal with it. It will be investigated whether an improved actuator design and/or a purposely tuned control can reduce the reported oscillation. Lateral and heading errors along the circuit are shown in Fig. 8, and Fig. 9 depicts statistical parameters of the magnitude (i.e. absolute value) of such errors. Lateral error is generally smaller for the simulated case, as the 293
Fig. 8. Errors and speed references experimental case is affected by the aforementioned oscillations. Bias is inverted, as the simulated vehicle tends to turn on the inside of the reference trajectory (lateral error of the same sign of the lateral acceleration) while the actual vehicle tends to turn on the outside of the reference (lateral error of opposite sign to the lateral acceleration). On the other hand, heading error (estimated as the vehicle attitude angle with respect to the reference trajectory) is generally smaller for the experimental test than for the simulation. The oscillation phenomenon is again clearly observed in the experimental case, with the angular error fluctuating around zero. The behavior on the higher curvature turns exposes the most notorious difference. On the simulated tests, the vehicle heading error grows significantly with the curvature, with the vehicle pointing outwards of the trajectory, as expected for an under-steering vehicle. Whereas, for the experimental tests the heading error oscillates around zero, a behavior closer to a neutral steering (or less under-steering) vehicle. It is also worth noting that, when the tires are more loaded laterally, a high frequency component does appear on the heading error both for the simulation and the experimental tests.
2019 IFAC IAV 294 Gdansk, Poland, July 3-5, 2019
Jose A. Matute et al. / IFAC PapersOnLine 52-8 (2019) 289–294
REFERENCES
Fig. 9. Lateral and angular errors Fig. 8 also shows speed limits for comfort and the actual speed profiles for simulation and experiments. When seen alongside the acceleration profiles of Fig. 7, it can be concluded that the proposed control strategy can satisfactorily fulfill speed, safety and comfort requirements in both plants (the simulated and the experimental one).
4. CONCLUSIONS AND FUTURE WORK A novel procedure for the design of a safe and comfortable vehicle motion controller, considering parameters such as jerk and accelerations has been presented. The main contribution of this work is the MPC controller based on the extended kinematic bicycle model which achieves real time performance in urban conditions. Specially, the lateral jerk formula is a novelty in this work allowing to include the lateral acceleration as a new constraint. This addition has been shown to be beneficial to avoid unfeasible solutions in the optimization problem. Our approach has been validated in a high-fidelity dynamic simulator (Dynacar) and an instrumented platform (Renault Twizy) in real urban scenarios. This controller gave good results for different maneuvers, including: intersections, roundabouts and lane change. The lateral and heading errors show a good performance in both simulation and real tests. The method proposed in the paper, can also be employed to resolve optimal controllers in an efficient manner using available MPC solvers for Matlab/Simulink. The easy integration of the controllers first tested in simulation and then in the vehicle platform shows that this controller is suitable for real vehicles with available commercial sensors. Although small differences are present between simulations and experiments, e.g. oscillations and command signals amplitude (see Fig. 6). It is worth noticing that the proposed control architecture based on MPC is available to handle platforms even with small behavior differences, while achieving the objectives of performance, safety and comfort (see Figs. 7-8). 294
Bautista, D.G. (2017). Functional architecture for automated vehicles trajectory planning in complex environments. Ph.D. thesis, PSL Research University. Bellem, H., Thiel, B., Schrauf, M., and Krems, J.F. (2018). Comfort in automated driving: An analysis of preferences for different automated driving styles and their dependence on personality traits. Transportation research part F: traffic psychology and behaviour, 55, 90–100. ISO (1997). Mechanical Vibration and Shock: Evaluation of Human Exposure to Whole-body Vibration. Part 1, General Requirements: International Standard ISO 2631-1: 1997 (E). ISO. Kong, J., Pfeiffer, M., Schildbach, G., and Borrelli, F. (2015). Kinematic and dynamic vehicle models for autonomous driving control design. In Intelligent Vehicles Symposium, 1094–1099. Lattarulo, R., Gonzalez, L., Marti, E., Matute, J., Marcano, M., and Perez, J. (2018). Urban motion planning framework based on n-b´ezier curves considering comfort and safety. Journal of Advanced Transportation, 13 pages. doi:https://doi.org/10.1155/2018/6060924. Article ID 6060924. Lattarulo, R., P´erez, J., and Dendaluce, M. (2017). A complete framework for developing and testing automated driving controllers. IFAC-PapersOnLine, 50(1), 258 – 263. doi:https://doi.org/10.1016/j.ifacol.2017.08.043. 20th IFAC World Congress. Marcano, M., Matute, J.A., Lattarulo, R., Marti, E., and Perez, J. (2018). Low speed longitudinal control algorithms for automated vehicles in simulation and real platforms. Complexity, 2018, 12. Mata, S., Zubizarreta, A., Cabanes, I., Nieva, I., and Pinto, C. (2017). Linear time varying model based model predictive control for lateral path tracking. International Journal of Vehicle Design, 75(1-4), 1–22. Matute, J.A., Marcano, M., Zubizarreta, A., and Perez, J. (2018). Longitudinal model predictive control with comfortable speed planner. In 2018 IEEE International Conference on Autonomous Robot Systems and Competitions (ICARSC), 60–64. doi: 10.1109/ICARSC.2018.8374161. Polack, P., Altch´e, F., D’Andrea-Novel, B., and de La Fortelle, A. (2018). Guaranteeing consistency in a motion planning and control architecture using a kinematic bicycle model. In 2018 Annual American Control Conference (ACC), 3981–3987. doi:10.23919/ACC.2018.8430886. Polack, P., Altch´e, F., d’Andr´ea Novel, B., and de La Fortelle, A. (2017). The kinematic bicycle model: A consistent model for planning feasible trajectories for autonomous vehicles? In Intelligent Vehicles Symposium (IV), 2017 IEEE, 812–818. IEEE. Quirynen, R., Vukov, M., Zanon, M., and Diehl, M. (2015). Autogenerating microsecond solvers for nonlinear mpc: A tutorial using acado integrators. Optim. Control Appl. Meth., 36, 685–704. Rajamani, R. (2011). Vehicle Dynamics and Control. Mechanical Engineering Series. Springer US.
ScienceDirect
IFAC PapersOnLine 52-8 (2019) 289–294
Experimental Experimental Validation Validation of of a a Kinematic Kinematic Experimental Validation of a Kinematic Bicycle Model Predictive Control with Experimental a Kinematic Bicycle ModelValidation PredictiveofControl with Bicycle Model Predictive Control with Lateral Acceleration Consideration Bicycle Model Predictive Control with Lateral Acceleration Consideration Lateral Acceleration Consideration Lateral Acceleration Consideration ∗,∗∗ ∗,∗∗ ∗ ∗,∗∗ Jose A. Matute ∗,∗∗ Mauricio Marcano ∗,∗∗ ∗,∗∗ Sergio Diaz ∗ ∗
Jose A. Matute Mauricio Marcano Sergio Diaz Jose Marcano ∗ ∗,∗∗ Sergio Diaz ∗ ∗ Joshue Perez Jose A. A. Matute Matute ∗,∗∗ Mauricio Mauricio Marcano Sergio Diaz ∗ Perez ∗,∗∗ Joshue ∗,∗∗ Joshue Perez ∗ Jose A. Matute Mauricio Marcano Sergio Diaz ∗ Joshue Perez ∗ ∗ Perez ∗ Tecnalia Research Research and and Joshue Innovation, Derio, 48160 48160 Spain Spain (e-mail: (e-mail: ∗ Tecnalia Innovation, Derio, and Innovation, Derio, ∗ Tecnalia Research [email protected]). Tecnalia Research and Innovation, Derio, 48160 48160 Spain Spain (e-mail: (e-mail: [email protected]). ∗ [email protected]). ∗∗ Tecnalia Research and Innovation, Derio, 48160 Spain (e-mail: ∗∗ University of the Basque Country, Bilbao, 48013 Spain [email protected]). ∗∗ University of the Basque Country, Bilbao, 48013 Spain (e-mail: (e-mail: Basque Country, Bilbao, 48013 Spain (e-mail: ∗∗ University of the [email protected]). [email protected]) University of the Basque Country, Bilbao, 48013 Spain (e-mail: [email protected]) ∗∗ [email protected]) University of the Basque Country, Bilbao, 48013 Spain (e-mail: [email protected]) [email protected]) Abstract: Nowadays, Automated Driving has growing interest in scientific and industrial Abstract: Nowadays, Nowadays, Automated Automated Driving Driving has has aaa growing growing interest interest in in the the scientific scientific and and industrial industrial Abstract: automotive community. The vehicle motion is an essential safe Abstract: Nowadays, Automated Driving hasplanning a growing interest in the theprocedure scientific to andobtain industrial automotive community. The vehicle motion planning is an essential procedure to obtain safe automotive community. The vehicle motion planning is an essential procedure to obtain safe Abstract: Nowadays, Automated Driving has a growing interest in the scientific and industrial and comfortable trajectories, adapting the longitudinal speed to the road legal limits and automotive community. The vehicle motion planning is an essential procedure to obtain safe and comfortable trajectories, adapting the longitudinal speed to the road legal limits and and comfortable trajectories, adapting the longitudinal speed to the road legal limits and automotive community. The vehicle motion planning is an essential procedure to obtain safe mainly to avoid the excessive lateral accelerations along the journey. Typically, the proper and comfortable trajectories, adapting the longitudinal speed to the road legal limits and mainly to to avoid avoid the the excessive excessive lateral lateral accelerations accelerations along along the the journey. journey. Typically, Typically, the the proper proper mainly and trajectories, adapting theto to path, the road legal the limits and speed of the vehicle is intrinsically related the curvature of the requiring a previous mainly avoid the lateral accelerations alongspeed the Typically, proper speedcomfortable oftothe the vehicle isexcessive intrinsically related tolongitudinal the curvature curvature of journey. the path, requiring previous speed of vehicle is intrinsically related to the of the path, requiring aaa previous mainly to avoid the excessive lateral accelerations along the journey. Typically, the proper approximation of this parameter in the planning stage. In this work, a novel procedure to speed of the vehicle is intrinsically related to the curvature of the path, requiring previous approximation of of this this parameter parameter in in the the planning planning stage. stage. In In this this work, work, aa novel novel procedure procedure to to approximation speed of the vehicle is intrinsically related to the curvature of the path, requiring a previous follow a route trajectory and speed limits considering the lateral acceleration parameter is approximation of this parameter in the planning stage. In this work, a novel procedure to follow a route trajectory and speed limits considering the lateral acceleration parameter is follow a route trajectory and speed limits considering the lateral acceleration parameter is approximation of this parameter in the planning stage. In this work, a novel procedure to presented. A lateral jerk equation was developed and introduced into a kinematic bicycle model follow a route trajectory and speed limits considering the lateral acceleration parameter is presented. A A lateral lateral jerk jerk equation equation was was developed developed and and introduced introduced into into aa kinematic kinematic bicycle model presented. bicycle model follow a route trajectory and An speed limits considering the lateral acceleration parameter is presented. A lateral jerk equation was developed and introduced intothat a kinematic bicycle model predictive control formulation. An adaptive speed weight equation that depends on on the lateral lateral predictive control formulation. adaptive speed weight equation depends the predictive control formulation. An adaptive speed weight equation that depends on the lateral presented. A lateral jerk equation was developed and introduced into a kinematic bicycle model acceleration is presented to improve the lateral positioning. A vehicle motion control simulation, predictive control formulation. An adaptive speed weight equation that depends on the lateral acceleration is is presented presented to to improve improve the the lateral positioning. positioning. A A vehicle vehicle motion motion control control simulation, simulation, acceleration predictive control formulation. An with adaptive speed weight equation depends on simulation, the lateral developed in is validated some real tests. The show the capabilities of the acceleration isDynacar, presented to improve the lateral lateral positioning. Aresults vehiclethat motion control developed in Dynacar, is validated with some real tests. The results show the capabilities of the the developed in Dynacar, is validated with some real tests. The results show the capabilities of acceleration is presented to improve the lateral positioning. A vehicle motion control simulation, proposed approach. An accurate vehicle motion control considers the lateral acceleration to developed in Dynacar, is validated with some real tests. The results show the capabilities of the proposed approach. approach. An An accurate accurate vehicle vehicle motion motion control control considers considers the the lateral lateral acceleration acceleration to to proposed developed in Dynacar, is accurate validated vehicle with some real tests. results show the capabilities of the proposed approach. motion controlThe considers the lateral acceleration to avoid unfeasibility unfeasibility inAn optimization problem. avoid in optimization problem. avoid unfeasibility in optimization problem. proposed approach.inAn accurate vehicle motion control considers the lateral acceleration to avoid unfeasibility optimization problem. © 2019, IFAC (International Federationproblem. of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. avoid unfeasibility in optimization Keywords: Intelligent Path Intelligent Systems. Keywords: Intelligent Intelligent Control, Control, Path Path Planning, Planning, Intelligent Intelligent Transportation Transportation Systems. Systems. Keywords: Keywords: Intelligent Control, Control, Path Planning, Planning, Intelligent Transportation Transportation Systems. Keywords: Intelligent Control, Path Planning, Intelligent Transportation Systems. 1. INTRODUCTION that pursue performance, safety and comfort. In (Matute 1. INTRODUCTION INTRODUCTION that pursue performance, safety and comfort. In (Matute 1. that pursue performance, safety and comfort. In (Matute 1. INTRODUCTION et al., 2018), MPC is implemented as a speed controller that pursue performance, safety and comfort. In (Matute et al., 2018), MPC is implemented as a speed controller et al., 2018), MPC is implemented as a speed controller 1. INTRODUCTION that pursue performance, safety and comfort. In (Matute with constraints of longitudinal jerk and acceleration, In Automated Driving, the vehicle motion planning is et al., 2018), MPC is implemented as a speed controller with constraints constraints of of longitudinal longitudinal jerk jerk and and acceleration, acceleration, In Automated Automated Driving, Driving, the the vehicle vehicle motion motion planning planning is is with In et al., 2018), MPC is implemented as a speed controller using a triple integrator model. However, it failed on In Automated Driving, the vehicle motion planning is an essential procedure to obtain a safe and comfortable with constraints of longitudinal jerk and acceleration, aa triple integrator model. However, it failed on an essential essential procedure procedure to to obtain obtain aa safe safe and and comfortable comfortable using using triple integrator model. However, it failed on an with constraints of longitudinal jerk and acceleration, In Automated Driving, the vehicle motion planning is considering the lateral actions of the vehicle, which have driving experience in automated mode. Researchers have using a triple integrator model. However, it failed on an essential procedure to obtain a safe and comfortable considering the lateral actions of the vehicle, which have driving experience in automated mode. Researchers have considering the lateral actions of the vehicle, which have driving experience in automated mode. Researchers have using a triple integrator model. However, it failed on an essential procedure to obtain a safe and comfortable a high impact on passenger comfort. Other works (Mata been putting efforts on this area proposing the use of considering the lateral actions of the vehicle, which have driving experience in automated mode. Researchers have high impact impact on on passenger passenger comfort. comfort. Other Other works works (Mata (Mata been putting putting efforts efforts on on this this area area proposing proposing the the use use of of aa high been considering theon lateral actions of the Other vehicle, which have driving experience in automated mode. Researchers have et al., 2017; Kong et al., 2015), include a kinematic bicycle the use of smooth curves for obstacle free trajectories, and speed a high impact passenger comfort. works (Mata been putting efforts on this area proposing et al., 2017; Kong et al., 2015), include a kinematic bicycle smooth curves curves for for obstacle obstacle free free trajectories, trajectories, and and speed speed et al., 2017; Kong et al., 2015), include a kinematic bicycle smooth high impact on the passenger comfort. Other works (Mata been putting on thisfree area proposing the of aet model to couple lateral and longitudinal control of smooth curvesefforts for on obstacle trajectories, and use speed profiles dependent curvature to reduce discomfort in al., 2017; Kong et al., 2015), include a kinematic bicycle model to couple the lateral and longitudinal control of profiles dependent on curvature to reduce discomfort discomfort in model to couple the lateral and longitudinal control of profiles dependent on curvature to reduce in et al., 2017; Kong et al., 2015), include a kinematic bicycle smooth curves for obstacle free trajectories, and speed the vehicle in a single optimization problem. However, bends and lateral maneuvers (Lattarulo et al., 2018). model to couple the lateral and longitudinal control of profiles dependent on curvature to reduce discomfort in the vehicle in a single optimization problem. However, bends and lateral maneuvers (Lattarulo et al., 2018). the vehicle in a single optimization problem. However, bends and lateral maneuvers (Lattarulo et al., 2018). model to couple the lateral and longitudinal control of profilesand dependent on curvature to reduce discomfort when different objectives functions compete (e.g. increase vehicle in a single optimization problem. However, bends lateral maneuvers (Lattarulo et al., 2018). in the when different objectives functions compete (e.g. increase However, comfort should not be considered in the planning when different objectives functions compete (e.g. increase the vehicle in a single optimization problem. However, However, comfort should not be considered in the planning bends and lateral maneuvers (Lattarulo et al., 2018). when different objectives functions compete (e.g. increase tracking performance, performance, reach reach aa speed speed set set point point and and reduce reduce However, comfort should not be be considered considered in the planning planning tracking stage alone, because it assumes that the controller will performance, reach aa problem speed set point and reduce However, comfort should not the when different objectives functions compete (e.g. increase stage alone, alone, because it assumes assumes that the the in controller will tracking steering rate) the optimization may be unfeasible, tracking performance, reach speed set point and reduce stage because it that controller will steering rate) the optimization problem may be unfeasible, However, comfort should not be considered in the planning follow the references perfectly. Instead, it must be considsteering rate) the optimization problem may be unfeasible, stage alone, because it assumes that the controller will performance, reach a speed set point and reduce follow the the references references perfectly. perfectly. Instead, Instead, it it must must be be considconsid- tracking producing unexpected behaviors on the vehicle motion and steering rate) the optimization problem may be unfeasible, follow unexpected behaviors on the vehicle motion and stage alone, because itcontrol assumes thatof controller will producing ered together with the aspect driving. Although producing unexpected behaviors on the vehicle motion and follow the references Instead, it must be considsteering rate) the optimization problem may be unfeasible, ered together together with the theperfectly. control aspect ofthe driving. Although compromising safety. producing unexpected behaviors on the vehicle motion and ered with control aspect of driving. Although compromising safety. follow the references perfectly. Instead, it must be considcomfort in a dynamic driving task is not formally defined compromising safety. ered together with the control aspect of driving. Although producing unexpected behaviors on the vehicle motion and comfort in a dynamic driving task is not formally defined compromising safety. comfort in a dynamic driving task is not formally defined ered together with theit control aspect of Although this sense, (Polack et al., 2017) states that keeping the (Bellem et 2018), is related in practice with variables comfort in al., a dynamic driving task is notdriving. formally defined In In this sense, (Polack et al., 2017) states that keeping the compromising safety. (Bellem et al., 2018), it is related in practice with variables In this sense, (Polack et al., 2017) states that keeping the (Bellem et al., 2018), itdriving is related related in is practice withwhich variables comfort in a dynamic task not formally defined lateral acceleration under a threshold of 0.5g guarantees such as jerk and acceleration (Bautista, 2017), are In this sense, (Polack et al., 2017) states that keeping thea (Bellem et al., 2018), it is in practice with variables acceleration under aa threshold of 0.5g guarantees a such as as jerk jerk and and acceleration acceleration (Bautista, (Bautista, 2017), 2017), which which are are lateral lateral acceleration under threshold of 0.5g guarantees such In this sense, (Polack et al., 2017) states that keeping thea (Bellem et al., 2018), it is related in practice with variables lateral acceleration under a threshold of 0.5g guarantees a feasible solution solution when when aa kinematic kinematic bicycle bicycle model model is is used. used. characteristics of acceleration the control control stage. stage. such as jerk and (Bautista, 2017), which are feasible characteristics of the feasible solution when a kinematic bicycle model is used. characteristics of the control stage. lateral acceleration under a threshold of 0.5g guarantees a such as jerk and (Bautista, 2017), which are feasible Considering this, the novelty of the present work relies on solution when a kinematic bicycle model is used. characteristics of acceleration the control stage. Considering this, the novelty of the present work relies on Nonetheless, not all control techniques allow to consider this, the novelty of the present work relies on feasible solution when a kinematic bicycle model is used. Nonetheless, not not all control control techniques allow to to consider consider Considering characteristics of the control techniques stage. the inclusion of the lateral jerk formula within the MPC Considering this, the novelty of the present work relies on Nonetheless, all allow inclusion of the lateral jerk formula within the MPC Nonetheless, all control techniques allowe.g. to consider these vehicle vehicle not states in the the design design process, e.g. classical the the inclusion of lateral formula the MPC Considering the noveltyjerk of the present work relies on these states in process, classical formulation. This consideration allows aawithin new state conthe inclusionthis, of the the lateral jerk formula within the MPC these vehicle states in the design process, e.g. classical formulation. This consideration allows new state conNonetheless, not all control techniques allow to consider controllers. In contrast, Model Predictive Control (MPC) formulation. This consideration allows a new state conthese vehicle states in the design process, e.g. classical the inclusion of the lateral jerk formula within the MPC controllers. In contrast, Model Predictive Control (MPC) formulation. This consideration allows a new state constraint limiting the lateral acceleration to assure a feasible controllers. In states contrast, Model Predictive Control (MPC) straint limiting the lateral acceleration to assure a feasible these vehicle in the design process, e.g. functions classical technique allows to combine different objectives straint limiting the lateral to a controllers. In contrast, Model Predictive Control (MPC) formulation. This consideration allows new state contechnique allows to combine combine different objectives functions solution. This also the use of simple vehicle model straint limiting thepermits lateral acceleration acceleration toa assure assure a feasible feasible technique allows to different objectives functions solution. This also permits the use of simple vehicle model controllers. In contrast, Model Predictive Control (MPC) This also permits the use of simple vehicle model technique allows to combine different objectives functions solution. straint limiting the lateral acceleration to assure a feasible This project has received funding from the Electronic Component which require less computational effort. Additionally, an solution. This also permits the use of simple vehicle model This project project has received received funding from the theobjectives Electronic Component Component This which require less computational effort. Additionally, an technique allows to combine different functions has funding from Electronic which require less computational effort. Additionally, an Systems for European Leadership Undertaking grant solution. This also permits the use of simple vehicle model This project has received funding Joint from the Electronic under Component adaptive speed weight equation that depends on the latwhich require less computational effort. Additionally, an Systems for European Leadership Joint Undertaking under grant adaptive speed weight equation that depends on the latSystems for European Leadership Joint Undertaking under grant adaptive speed weight equation that depends on the latagreement No 737469 (AutoDrive Project). This Joint Undertaking This project has received funding from the Electronic Component which require less computational effort. Additionally, an Systems for European Leadership Joint Undertaking under grant adaptive speed weight equation that depends on the lateral acceleration is also included to improve the tracking agreement No 737469 (AutoDrive Project). This Joint Undertaking eral acceleration is also included to improve the tracking agreement No 737469 (AutoDrive Project). This Joint Undertaking eral acceleration is also included to improve the tracking receives support from the European Union 2020 research and agreement 737469 (AutoDrive Project). This Joint Undertaking Systems forNoEuropean Leadership Joint Horizon Undertaking under grant adaptive speed weight equation that depends on the latreceives support from the European Union Horizon 2020 research and performance of the vehicle. eral acceleration is also included to improve the tracking receives support from the European Union Horizon 2020Italy, research and performance of the vehicle. innovation programme and Germany, Austria, Spain, Latvia, performance of agreement No 737469 (AutoDrive Project). This Joint Undertaking receives support from the European Union Horizon 2020Italy, research and innovation programme and Austria, Spain, Latvia, eral acceleration is vehicle. also included to improve the tracking performance of the the vehicle. innovation programme Sweden, and Germany, Germany, Austria, Spain, Italy, Latvia, Belgium, Netherlands, Finland, Lithuania, Czech Republic, innovation programme and Germany, Austria, Spain, Italy, Latvia, receives support from the European Union Horizon 2020 research and This work is organized as follows. In Section II, the control Belgium, Netherlands, Sweden, Finland, Lithuania, Czech Republic, This work is organized as follows. In Section II, the control performance of the vehicle. Belgium, Netherlands, Sweden, Finland, Lithuania, Czech Republic, This work is organized as follows. In Section II, the control Romania,Netherlands, Norway. ThisSweden, workGermany, was developed at Tecnalia Tecnalia Research & innovation programme and Austria, Spain, Italy, Latvia, Belgium, Finland, Lithuania, Czech Republic, architecture developed is described in detail. Section III, This work is organized as follows. In Section II, the control Romania, Norway. This work was developed at Research & architecture developed is described in detail. Section III, Romania, Norway. This work was developed at Tecnalia Research & architecture developed is described in detail. Section III, Innovation facilities supporting this research. Belgium, Netherlands, Sweden, Finland, Lithuania, Czech Republic, Romania, Norway. This work was developed at Tecnalia Research & This work is organized as follows. In Section II, the control Innovation facilities supporting this research. architecture developed is described in detail. Section III, Innovation facilities supporting this research. Romania, work was developed InnovationNorway. facilitiesThis supporting this research.at Tecnalia Research & architecture developed is described in detail. Section III,
Innovation supporting this research. 2405-8963 © ©facilities 2019, IFAC IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Copyright 2019 Copyright © 2019 Copyright © under 2019 IFAC IFAC Peer review responsibility of International Federation of Automatic Control. Copyright © 2019 IFAC 10.1016/j.ifacol.2019.08.085 Copyright © 2019 IFAC
2019 IFAC IAV 290 Gdansk, Poland, July 3-5, 2019
Jose A. Matute et al. / IFAC PapersOnLine 52-8 (2019) 289–294
Fig. 1. Control architecture discuss the results obtained in simulation and experimental tests. Lastly, Section IV presents final conclusions and future research work.
Fig. 2. Route for real and simulated tests
2. CONTROL ARCHITECTURE This section describes the algorithm developed for path tracking applications based on a vehicle predictive model. Using the state of the vehicle, the trajectory planning determines the references for the longitudinal and lateral vehicle motion control, estimating and defining the command positions for the pedals and steering wheel. This approach was developed as a general framework for path following in automated driving applications. The aim is to follow a predefined route with the maximum level of safety and desired comfort. In this work, the same Dynamic Driving Task (DDT) is defined for both virtual and real situations as depicted in the Fig. 1. The problem constraints are stated according with the physical capabilities of the real actuators installed in the vehicle, considering the maximum displacements and change rates. 2.1 Trajectory Planning The references to be followed by the vehicle motion control are defined performing a strategy of offline and online stages described by (Matute et al., 2018). The offline stage contains relevant information about the predefined map for path tracking. The procedure to generate the route used in this work is broadly described by (Lattarulo et al., 2018). The route contains several kinds of traffic maneuvers very usual in urban environments as roundabouts, intersections and lane changes. Depending on the maneuver to be performed, the path is built using B´ezier curves of fourth order or higher using hard points as those depicted in the legend of the Figure 2. From these hard points, it is possible to obtain smooth routes with continuous curvatures described by an ordered list of waypoints which contain relevant information for path tracking as; locations in Cartesian coordinates (X and Y ), orientations (Ψ), speed limits (vx ), curvatures and traveled distance from the starting point of the map. Moreover, physical constraints for vehicle motion control are specified. The online stage uses the current and future locations of the vehicle to project them to the nearest sections of the route using the map developed in the offline stage. The location of the vehicle is projected onto the map using dot 290
Fig. 3. Route references of curvature and velocity products to obtain the references to perform the lateral vehicle motion control as shown in the Fig. 3. The speed reference is generated considering that the lateral acceleration never exceeds 1.0m/s2 —, fairly uncomfortable-based on the ISO 2631-1 Standard (ISO, 1997). The speed limit of this section is used as reference parameter to perform the longitudinal vehicle motion control. It is important to note that the planned route has curvatures higher than those generally expected in real traffic scenarios. A path radius lower than 6.67m (k > 0.15) is usually considered for slow speed maneuvers. This map is generated considering the space available for real tests. 2.2 Vehicle Motion Control This DDT sub-task includes the maintaining of the speeds below specified limits applying propulsion or braking inputs, as well as maintaining an appropriate lateral positioning through the application of steering inputs. The sustained regulation of the x-axis and y-axis components of vehicle motion depicted in Fig. 4 are performed using a coupled MPC strategy. The lateral acceleration is considered as an additional parameter to improve the lateral positioning, defining also a desired level of comfort during the path tracking on curve roads. Lateral Acceleration Consideration Considering small time steps for the computation of the vehicle’s motion, its lateral acceleration ay is approximated as a uniform circular motion expressed as ay = vx2 /R, where vx is the vehicle’s longitudinal speed and R is the path radius. From the bicycle model assumption considering a front wheel steered vehicle, it is possible to approximate the path radius as R = L/δ for small values of slip angle
2019 IFAC IAV Gdansk, Poland, July 3-5, 2019
Jose A. Matute et al. / IFAC PapersOnLine 52-8 (2019) 289–294
291
The control strategy consists to track a desired trajectory under specific speeds considering the current lateral accelerations of the vehicle. The output differential states to be optimized are defined in the Eq. 4. The term of ay is not optimized being only used as a constraint.
1 0 η(k) = h(χ(k)) = 0 0
0 1 0 0
0 0 1 0
0 0 0 1
0 0 χ(k) 0 0
(4)
The cost function for the optimization is in the Eq. 5.
Fig. 4. Simplified kinematic bicycle model
J(χ(t), u(t)) =
(β) (Rajamani, 2011). Therefore, the lateral acceleration equation can be expressed as shown in the Eq. 1. ay =
vx2 δ L
(1)
This formula is later on differentiated to be included as a differential state variable into the kinematic bicycle MPC formulation as depicted in the Eq. 2e. Kinematic Bicycle Model The MPC formulation considers a kinematic bicycle model of the vehicle as shown in Fig. 4. The equations are based on geometric relationships without considering any forces. The two left and right wheels are represented as two single wheels at points A and B, representing the front and rear ends of the vehicle wheelbase L. The center of gravity of the vehicle is at point C and defines the location of the vehicle. The vehicle is assumed to have a planar motion. The location coordinates and orientation (X, Y, Ψ) describe the motion of the vehicle. The longitudinal speed (vx ) and lateral acceleration (ay ) are parallel and perpendicular to the longitudinal axis of the vehicle, respectively. The longitudinal jerk (jx ) and the steering change rate (∆δ) are the two inputs on this model. X˙ = vx cos(Ψ) Y˙ = vx sin(Ψ) ˙ = vx tan(δ) Ψ L v˙ x = ax
vx a˙ y = (2ax δ + vx ∆δ) L a˙ x = jx δ˙ = ∆δ
(2a) (2b) (2c) (2d) (2e) (2f) (2g)
MPC Formulation The nonlinear vehicle dynamics formulation described in Eqs. 2a-2e can be described in the general compact form depicted in the Eq. 3. dχ = f (χ(t), u(t)) dt
(3)
where the differential state and control parameters are ˙ T , respectively. χ = [X, Y, Ψ, vx , ay ]T and u = [a˙ x , δ] 291
H i=1
2
2
ref ηt+i,t − ηt+i,t Q + ut+i,t R
(5)
where η = [X, Y, Ψ, vx ]T and η ref contains the references for each of the differential states to be optimized, all of them related with information provided from the trajectory planner. At the right side of the Eq. 5, the first term denoted the penalty on the differential states and the second one measures the control variables. The weight matrices are defined intuitively in order to provide a balance between performance and smoothness as Q = diag([1, 1, 1, S]) and R = diag([1, 0.01]), where Q and R are the weight matrices for the differential states and control parameters, respectively. The term S within the matrices is related to the weight of the longitudinal velocity, which changes according to the value of the current lateral acceleration as is explained later in the subsection 2.2.4. The finite horizon of the Optimal Control Problem (OCP) formulation is solved at each step t as: min J(χt , ut )
(6a)
χk+1,t = f (χk,t , uk,t ) ηk,t = h(χk,t ) k = t, ..., t + H ζf,min ≤ ζk,t ≤ ζf,max uf,min ≤ uk,t ≤ uf,max χt,t = χ(t)
(6b) (6c) (6d) (6e) (6f) (6g)
η(·),u(·)
s.t.
where ηt+i,t is the optimization vector at time t + i beginning from state χt,t = χ(t). The differential states prediction horizon is defined as H. The OCP is solved at each time step and a new H is generated in according to χ(t + 1). The sequence for the solution is settled as k = t, ..., t + H. The constraints of the problem are defined by ζk,t and uk,t . Adaptive Speed Weight The highest errors in path following occur before, during and after the vehicle makes a turn. An adaptive speed weight in the OCP formulation highly contributes to a more accurate trajectory tracking on this case. The aim is to reduce the weight of the speed as the lateral acceleration is increasing. This strategy permits to give more importance to the location of the vehicle, reducing the lateral and angular errors when the vehicle is taking a curve. A proposal to obtain an adaptive weight (n = 2) is presented in the Eq. 7.
2019 IFAC IAV 292 Gdansk, Poland, July 3-5, 2019
S =1−
Jose A. Matute et al. / IFAC PapersOnLine 52-8 (2019) 289–294
|ay |n max(ay )
(7)
where ay is the current laterals acceleration of the vehicle and max(ay ) is the maximum lateral acceleration permitted and settled for driving constraints. The value remains always among 0 and max(ay ). Driving Constraints The bounds for the differential states (ζk,t ) and control (uk,t ) are selected considering the physical capabilities of the vehicle and actuation devices, as well as comfort level to be perceived by the passenger. The constraint values are depicted in the Table 1. Table 1. Driving constraints States
Controls
Parameter ayk,t
Min. -2
Max. 2
Unit m/s2
x vk,t ax k,t δk,t x jk,t ∆δk,t
0 -10 -0.69 -0.50 -0.50
ref vk,t 1 0.69 0.50 0.50
m/s m/s2 rad m/s3 rad/s
Fig. 5. Test vehicle instrumentation
According to (Polack et al., 2018), values of lateral acceleration below 0.5g avoid the appearance of unfeasible results in the solution of the OCP when the kinematic bicycle model is employed in vehicle motion control. MPC Solver The open-source ACADO Toolkit is used to solve the OCP (Quirynen et al., 2015). The OCP is reformulated to an approximate nonlinear program (NLP) using a direct multiple shooting discretization method. The generalized Gauss-Newton approximation iterates by solving the Sequential Quadratic Program (SQP) algorithm to solve the NLP. The SQP is then solved by the dense linear algebra solver qpOASES3. A continuous output Implicit Runge-Kutta of Gauss-Legendre integrator of order 2 is exported by the code to simulate the system with 20 integration steps. The H estimation is parameterized to obtain 10 elements discretized at 300ms. 2.3 Test Vehicle The test vehicle selected for real and simulated experiments is a Renault Twizy E80, an electric quadricycle which technical specifications are depicted in the Table 2. A multi-body model is developed in the software Dynacar and is employed to simulate the behavior of the vehicle as explained in (Lattarulo et al., 2017). The power-train, brakes and steering are characterized to resemble the real devices as detailed in (Marcano et al., 2018). Instrumentation and Data-acquisition The vehicle has been instrumented as shown in the Fig. 5 to carry out the experiments. A Global Navigation Satellite System plus an Inertial Navigation System device (GNSS+INS) is used to obtain Real-Time Kinematic positioning with a position accuracy of 2cm. A high-performance personal computer (PC) runs the control architecture software under MATLAB/Simulink environment sending the control commands to an industrial Programmable Logic Controller (PLC). The PLC is connected through a CAN bus network to the low-level control of the vehicle. The throttle pedal 292
has a parallel connection that simulates the pedal position for propulsion signals when the vehicle is in automated mode. The brake pedal is physically connected with a rotary servo motor through a steel cable. The steering wheel is also physically connected with a rotary servo motor through a synchronous belt though. Both brake and steer have position controllers that receive and execute the control from position commands delivered by the PLC. An encoder is connected to the rotary servo motor of the steering wheel to deliver the position feed-back to the PLC being useful as an MPC differential state. The sampling process for all the measurements in the real platform are synchronized with the GNSS+INS with a frequency 100Hz, the same used in the simulated platform. 3. RESULTS AND DISCUSSION The same tests were performed in both the simulator and the physical vehicle, following the route depicted on Fig. 2. This test circuit represents several traffic urban conditions, with different curvature radius and reference speeds (see Fig. 3). In this section, the behavior of the kinematic bicycle MPC under such conditions is evaluated and validated both in simulation and in actual vehicle tests. Note that the same controller is used in both cases. The computational time of the control cycle was 1ms in average both in simulation and real-life experiments, proving the applicability of the algorithm aa well as to be efficiently enough for real-time applications. Table 2. Vehicle technical specifications Parameter Mass Dimensions CG location Wheelbase Track-width Inertia Front wheel radius Rear wheel radius Steering ratio Traction torque Transmission ratio Braking torque
Value 611.50 2.34 x 1.23 x 1.45 -0.93 x 0.00 x 0.49 1.69 1.09 243.18, 430.17, 430.17 0.27 0.28 14.27:1 57 1:9.23 500
Unit kg m m m m kg-m2 m m N-m N-m
2019 IFAC IAV Gdansk, Poland, July 3-5, 2019
Jose A. Matute et al. / IFAC PapersOnLine 52-8 (2019) 289–294
Fig. 6. Command signals to actuation devices
293
Fig. 7. Longitudinal and lateral accelerations
Pedal and steering wheel actuation is portrayed in Fig. 6. First, note the similarity between simulations and experiments. However, it is also noted that a significantly stronger brakes application is required prior to curve entrances for the experimental case. This might be related to the fact that brakes are simulated with “as new” parameters, while in the experimental bench brakes are actually nearing the end of their life-cycle. It is also apparent that, in the straights, the experimental setup requires more acceleration (i.e. torque) than the simulation. Steering wheel actuation in the actual vehicle closely resembles that obtained from the simulations, both in amplitude and phasing. Some oscillations not seen on the simulation do appear on the experiments, both for the steering and pedal positions. It is speculated that such oscillation might be related to the behavior of the low-level control and actuators, particularly on the pedals, which might present delays, free play, and hysteresis not fully modeled in the simulations. Recall that the lateral and longitudinal motions are coupled both by the vehicle dynamics and by the implemented MPC. Fig. 7 shows longitudinal and lateral accelerations. Again, the experiments follow closely the simulations, fully complying with the dynamic and comfort bounds. The controller is able to adjust pedal inputs on the actual vehicle to compensate for the extra braking and motor torque requirements (as discussed above) in order to follow the circuit defined path with the same dynamic behavior than the simulated vehicle (showing similar longitudinal and lateral accelerations). As expected, the oscillations observed on the actuators for the experimental bench are also evident on the accelerations. As shown by the present tests, it is a bound effect and the system can acceptably work with it. Further research will be performed to reliably determine the source of this oscillation and to establish the most appropriate means to deal with it. It will be investigated whether an improved actuator design and/or a purposely tuned control can reduce the reported oscillation. Lateral and heading errors along the circuit are shown in Fig. 8, and Fig. 9 depicts statistical parameters of the magnitude (i.e. absolute value) of such errors. Lateral error is generally smaller for the simulated case, as the 293
Fig. 8. Errors and speed references experimental case is affected by the aforementioned oscillations. Bias is inverted, as the simulated vehicle tends to turn on the inside of the reference trajectory (lateral error of the same sign of the lateral acceleration) while the actual vehicle tends to turn on the outside of the reference (lateral error of opposite sign to the lateral acceleration). On the other hand, heading error (estimated as the vehicle attitude angle with respect to the reference trajectory) is generally smaller for the experimental test than for the simulation. The oscillation phenomenon is again clearly observed in the experimental case, with the angular error fluctuating around zero. The behavior on the higher curvature turns exposes the most notorious difference. On the simulated tests, the vehicle heading error grows significantly with the curvature, with the vehicle pointing outwards of the trajectory, as expected for an under-steering vehicle. Whereas, for the experimental tests the heading error oscillates around zero, a behavior closer to a neutral steering (or less under-steering) vehicle. It is also worth noting that, when the tires are more loaded laterally, a high frequency component does appear on the heading error both for the simulation and the experimental tests.
2019 IFAC IAV 294 Gdansk, Poland, July 3-5, 2019
Jose A. Matute et al. / IFAC PapersOnLine 52-8 (2019) 289–294
REFERENCES
Fig. 9. Lateral and angular errors Fig. 8 also shows speed limits for comfort and the actual speed profiles for simulation and experiments. When seen alongside the acceleration profiles of Fig. 7, it can be concluded that the proposed control strategy can satisfactorily fulfill speed, safety and comfort requirements in both plants (the simulated and the experimental one).
4. CONCLUSIONS AND FUTURE WORK A novel procedure for the design of a safe and comfortable vehicle motion controller, considering parameters such as jerk and accelerations has been presented. The main contribution of this work is the MPC controller based on the extended kinematic bicycle model which achieves real time performance in urban conditions. Specially, the lateral jerk formula is a novelty in this work allowing to include the lateral acceleration as a new constraint. This addition has been shown to be beneficial to avoid unfeasible solutions in the optimization problem. Our approach has been validated in a high-fidelity dynamic simulator (Dynacar) and an instrumented platform (Renault Twizy) in real urban scenarios. This controller gave good results for different maneuvers, including: intersections, roundabouts and lane change. The lateral and heading errors show a good performance in both simulation and real tests. The method proposed in the paper, can also be employed to resolve optimal controllers in an efficient manner using available MPC solvers for Matlab/Simulink. The easy integration of the controllers first tested in simulation and then in the vehicle platform shows that this controller is suitable for real vehicles with available commercial sensors. Although small differences are present between simulations and experiments, e.g. oscillations and command signals amplitude (see Fig. 6). It is worth noticing that the proposed control architecture based on MPC is available to handle platforms even with small behavior differences, while achieving the objectives of performance, safety and comfort (see Figs. 7-8). 294
Bautista, D.G. (2017). Functional architecture for automated vehicles trajectory planning in complex environments. Ph.D. thesis, PSL Research University. Bellem, H., Thiel, B., Schrauf, M., and Krems, J.F. (2018). Comfort in automated driving: An analysis of preferences for different automated driving styles and their dependence on personality traits. Transportation research part F: traffic psychology and behaviour, 55, 90–100. ISO (1997). Mechanical Vibration and Shock: Evaluation of Human Exposure to Whole-body Vibration. Part 1, General Requirements: International Standard ISO 2631-1: 1997 (E). ISO. Kong, J., Pfeiffer, M., Schildbach, G., and Borrelli, F. (2015). Kinematic and dynamic vehicle models for autonomous driving control design. In Intelligent Vehicles Symposium, 1094–1099. Lattarulo, R., Gonzalez, L., Marti, E., Matute, J., Marcano, M., and Perez, J. (2018). Urban motion planning framework based on n-b´ezier curves considering comfort and safety. Journal of Advanced Transportation, 13 pages. doi:https://doi.org/10.1155/2018/6060924. Article ID 6060924. Lattarulo, R., P´erez, J., and Dendaluce, M. (2017). A complete framework for developing and testing automated driving controllers. IFAC-PapersOnLine, 50(1), 258 – 263. doi:https://doi.org/10.1016/j.ifacol.2017.08.043. 20th IFAC World Congress. Marcano, M., Matute, J.A., Lattarulo, R., Marti, E., and Perez, J. (2018). Low speed longitudinal control algorithms for automated vehicles in simulation and real platforms. Complexity, 2018, 12. Mata, S., Zubizarreta, A., Cabanes, I., Nieva, I., and Pinto, C. (2017). Linear time varying model based model predictive control for lateral path tracking. International Journal of Vehicle Design, 75(1-4), 1–22. Matute, J.A., Marcano, M., Zubizarreta, A., and Perez, J. (2018). Longitudinal model predictive control with comfortable speed planner. In 2018 IEEE International Conference on Autonomous Robot Systems and Competitions (ICARSC), 60–64. doi: 10.1109/ICARSC.2018.8374161. Polack, P., Altch´e, F., D’Andrea-Novel, B., and de La Fortelle, A. (2018). Guaranteeing consistency in a motion planning and control architecture using a kinematic bicycle model. In 2018 Annual American Control Conference (ACC), 3981–3987. doi:10.23919/ACC.2018.8430886. Polack, P., Altch´e, F., d’Andr´ea Novel, B., and de La Fortelle, A. (2017). The kinematic bicycle model: A consistent model for planning feasible trajectories for autonomous vehicles? In Intelligent Vehicles Symposium (IV), 2017 IEEE, 812–818. IEEE. Quirynen, R., Vukov, M., Zanon, M., and Diehl, M. (2015). Autogenerating microsecond solvers for nonlinear mpc: A tutorial using acado integrators. Optim. Control Appl. Meth., 36, 685–704. Rajamani, R. (2011). Vehicle Dynamics and Control. Mechanical Engineering Series. Springer US.