Real-Time Adaptive Velocity Optimization for Autonomous Electric Cars at the Limits of Handling
Real-Time Adaptive Velocity Optimization for Autonomous Electric Cars at the Limits of Handling
Herrmann, Thomas;Wischnewski, Alexander;Hermansdorfer, Leonhard;Betz, Johannes;Lienkamp, Markus
2020-12-25 00:00:00
IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. XX, NO. X, MONTH 2021 1 Real-Time Adaptive Velocity Optimization for Autonomous Electric Cars at the Limits of Handling Thomas Herrmann, Alexander Wischnewski, Leonhard Hermansdorfer, Johannes Betz, Markus Lienkamp Variable power limit Abstract—With the evolution of self-driving cars, autonomous Energy Strategy Trajec- racing series like Roborace and the Indy Autonomous Challenge tory are rapidly attracting growing attention. Researchers partic- Path Velocity ipating in these competitions hope to subsequently transfer Path Planner Controller Optimization their developed functionality to passenger vehicles, in order to improve self-driving technology for reasons of safety, and Friction Estimation due to environmental and social benefits. The race track has Friction potential the advantage of being a safe environment where challenging situations for the algorithms are permanently created. To achieve minimum lap times on the race track, it is important to gather Fig. 1. Software environment of presented velocity optimization module. and process information about external influences including, e.g., the position of other cars and the friction potential between the road and the tires. Furthermore, the predicted behavior of the ego-car’s propulsion system is crucial for leveraging the available energy as efficiently as possible. In this paper, we therefore present an optimization-based velocity planner, mathematically formulated as a multi-parametric Sequential Quadratic Problem (mpSQP). This planner can handle a spatially and temporally varying friction coefficient, and transfer a race Energy Strategy (ES) to the road. It further handles the velocity-profile-generation task for performance and emergency trajectories in real time on the vehicle’s Electronic Control Unit (ECU). Index Terms—Real-Time Numerical Optimization, Optimal Fig. 2. TUM Roborace DevBot 2.0 on the race track. Control, Velocity Planning, Trajectory Planning, Autonomous Electric Vehicles, Energy Strategy, Variable Friction Potential actuator commands for the race car, called “DevBot 2.0”, see I. I NTRODUCTION Fig. 2. A huge motivation behind setting up an optimization- HE Technical University of Munich has been participat- based velocity planner was to be able to handle information ing in the Roborace competition since 2018. Many parts about the locally and temporally varying friction potential of our software stack are already available on an open source on the race track [7], and utilize the information provided basis [1] including the code of the algorithm in this work [2]. by the race Energy Strategy (ES) [8] as a vehicle’s velocity This paper explains an optimization-based Nonlinear Model profile has a significant influence on it’s energy consumption Predictive Planner (NLMPP), mathematically formulated as a and power losses [9]. The friction potential estimation and multi-parametric Sequential Quadratic Problem (mpSQP) [3], the calculation of the ES are handled by separate modules in to calculate the velocity profiles during a race. The velocity our software stack. Their outputs, the friction potential and planner inputs are the offered race paths (“performance” and variable power limits, are then considered in the presented “emergency”), stemming from our graph-based path-planning velocity optimization algorithm. framework [4], see Fig. 1. The presented velocity optimiza- To achieve real-time-capable calculation times, we build tion in combination with the path framework span our local local approximations of the nonlinear velocity-planning prob- trajectory planner that will be used within the competition. The lem, resulting in convex multi-parametric Quadratic Prob- trajectory planner’s output is forwarded to the underlying ve- lems (mpQP) [10] that can be solved iteratively using a hicle controller [5], [6], transforming the target trajectory into Sequential Quadratic Problem (SQP) method. We evaluated T. Herrmann (corresponding author), L. Hermansdorfer, J. Betz and M. different open-source Quadratic Problem (QP) solvers and Lienkamp are with the Institute of Automotive Technology, Department of compared their solution qualities and calculation times to a Mechanical Engineering, Technical University of Munich, Garching (Munich), direct solution of the Nonlinear Problem (NLP). We chose the 85748 Germany e-mail: thomas.herrmann@tum.de. A. Wischnewski is with the Institute of Automatic Control, Department of Operator Splitting Quadratic Problem (OSQP) [11] solver as it Mechanical Engineering, Technical University of Munich, Garching (Munich), outperformed its competitors on a standard x86-64 platform as 85748 Germany well as on the DevBot’s automotive-grade Electronic Control Manuscript received May 30, 2020; revised September 25, 2020; accepted December 24, 2020. Unit (ECU), the ARM-based NVIDIA Drive PX2. ©2020 IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses, in any current or future media, including reprinting/republishing this material for advertising or promotional purposes, creating new collective works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other works. arXiv:2012.13586v1 [cs.RO] 25 Dec 2020 IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. XX, NO. X, MONTH 2021 2 A. State of the art In the spline-based approach of Mercy et al. [23] trajectories for robots operating at low velocities are optimized. The The field of trajectory planning of vehicles at the limits calculation times of the general NLP-solver Interior Point of handling is attracting growing attention in research. The OPTimizer (IPOPT) [28] range up to several hundred mil- scenarios where the car is required to be operated at the limits liseconds, which is too long for race car applications. Another of its driving dynamics will become more and important as we general NLP-formulation is done by Svensson et al. [29]. see the spread of cars equipped with self-driving functionality, The latter describe a planning approach for safety trajectories and even fully autonomous vehicles. Through this, complex of automated vehicles, which they validate experimentally in scenarios with self-driving vehicles on the road will occur simulations for maximum velocities of 30 km h , leveraging more frequently. Research is also being carried out on the the general nonlinear optimal control toolkit ACADO [30]. race track where these challenging scenarios can deliberately Huang et al. [18] describe a two-step approach: first de- be created in a safe environment [12]. termining the path across discretized available space and In the field of global trajectory optimization for race tracks, then calculating a sufficient velocity. Also, Meng et al. [19] different mathematical concepts are applied. In the work of leverage a decoupled approach using a quadratic formulation Ebbesen et al. [13] a Second Order Conic Problem (SOCP) for the speed profile optimization, reaching real-time capable formulation is used to calculate the optimal power distribution calculation times below 0:1 s in this step. Nevertheless, both within the hybrid powertrain of a Formula One race car leading publications deal with low vehicle speeds in simulations of to globally time-optimal velocity profiles for a given path. max. 60 km h . Furthermore, Zhang et al. [20] implement a For the same racing format, Limebeer and Perantoni [14] two-step algorithm where they use MTSOS [31] for the speed took into account the 3D geometry of the race track within profile generation within several milliseconds for path lengths their formulation of an Optimal Control Problem (OCP) to of up to 100 m. The speed-profile optimization framework solve a Minimum Lap Time Problem (MTLP). In a similar MTSOS developed by Lipp and Boyd [31] works for fixed approach, Tremlett and Limebeer [15] consider the thermody- paths leveraging a change of variables. As in the aforemen- namic effects of the tires. Christ et al. [16] consider spatially tioned publications, they consider a static friction coefficient variable but temporally fixed friction coefficients along the and neglect the maximum available power of the car. The race track to calculate time-optimal global race trajectories same is true for the MPC algorithm by Carvalho et al. [24]. for a sophisticated Nonlinear Double Track Model (NDTM). They plan trajectories considering the driving dynamics of a They show a significant influence of the variable friction bicycle model, neglecting physical constraints stemming from coefficients on the achievable lap time when considered during the powertrain, like maximum available torque or power. This the trajectory optimization process. A minimum-curvature QP is a major drawback for our application, as the DevBot 2.0 is formulation, calculating the quasi-time-optimal trajectory for often operating at the power limit of its electric machines. an autonomous race car on the basis of an occupancy grid map, Subosits and Gerdes [21] formulate a Quadratically Con- is given by Heilmeier et al. [5]. Their advantage in comparison strained Quadratic Problem (QCQP) replanning path and ve- to [16] is the computation time, but the resulting trajectories locity of a race car at spatially fixed points on the track are suboptimal in terms of lap time. Also, Dal Bianco et al. to avoid static obstacles. They consider a constant friction [17] formulate an OCP to find the minimum lap time for a potential and the maximum available vehicle power. However, GP2 car and include a detailed multibody vehicle dynamics the obstacles need to be known in advance before the journey model with 14 degrees of freedom. However, none of these commences, and must be placed at a decent distance from approaches are intended to work in real-time on a vehicle the replanning points to allow the algorithm to find a feasible ECU, but to deliver detailed and close-to-reality results for lap passing trajectory, given the physical constraints. In order to time or for the sensitivity analysis of vehicle setup parameters. reach fast calculation times, Alrifaee et al. [25] use a sequential A further necessary and important part in a software stack linearization technique for real-time-capable trajectory opti- for autonomous driving is the online re-planning of trajectories mization. They consider the friction maxima, with included to avoid static and dynamic obstacles at high speeds. The velocity dependency that they determine beforehand. This literature can be structured into the three fields: dependency is assumed to be globally constant, thus neglecting “separated/two-step trajectory planning”, where the ve- the true track conditions during driving. Their experimental locity calculation is a subsequent process of the path results stem from simulations with peak computation times of planner [18]–[20]. several hundred milliseconds on a desktop PC. “combined trajectory planning”, optimizing both path and Considering variable friction on the road is attracting more velocity at the same time [21]–[23]. attention, as it is an emergency-relevant feature for passenger “Model Predictive Control (MPC) approaches” taking cars and a performance-critical topic for race cars. Therefore, into account the current vehicle state [10], [24]–[27]. Svensson et al. [22] describe an adaptive trajectory-planning In the following, we evaluate the literature according to the and optimization approach. They pre-sample trajectory prim- implemented features regarding itives to avoid local optima in subsequent SQPs stemming spatially and temporally varying friction coefficients, mainly from avoidance maneuvers to the suboptimal side of powertrain behavior, an obstacle. The vehicle adaptively reacts to a varying friction applicability at the limits of vehicle dynamics through potential on the road at speeds of up to 100 km h . The fast computation times. resulting problem is solved using simulations in MATLAB, IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. XX, NO. X, MONTH 2021 3 so no information about the calculation speed on embedded explain details about the recursive feasibility of our optimiza- hardware is given. tion problem. The Results section shows the realization of Stahl et al. [4] describe a two-step, multi-layered graph- the ES and the handling of variable friction by our velocity- based path planner. This approach allows for functionalities optimization algorithm. Furthermore, we contrast different such as following other vehicles and overtaking maneuvers, solvers in terms of their runtime and solution quality. also in non-convex scenarios at a high update rate. We use this path planner to generate the inputs for the velocity- II. P RELIMINARIES optimization algorithm. In this section, the mathematical background to an SQP optimization method to solve local approximations of an B. Contributions NLP with objective function J (o), h (o) and g (o), denoting b c In this paper we contribute to the state of the art in the field equality and inequality constraints of scalar quantity b and c, of real-time-capable trajectory planning with the following and optimization variables o is introduced. content. The standard form of a Nonlinear Optimal Control Problem (1) We formulate a tailored mpSQP algorithm capable (NOCP) is given by [36], [37]: of adaptive velocity planning for race cars operating at the limits of handling, and at velocities above 200 km h . The min J (x(s); u(s)) (1) | {z } planner computes velocity profiles for various paths using the path planner [4] in real time on the target hardware, dx(s) s:t: = f (x(s); u(s)) (2) an NVIDIA Drive PX2 [32] being an ECU already proven ds for autonomous driving. The adaptivity refers to the multi- h (o) = 0 (3) parametric input to the planner, depending on the vehicle’s g (o) 0: (4) environment. The quadratic subproblems within the mpSQP are handled using the OSQP [11] solver. Its primal and dual The independent space variable s describes the distance along infeasibility detection for convex problems [33] was integrated the vehicle’s path in our problem. The function f (x(s); u(s)) to flag up (as fast as possible) offered paths which could not specifies the derivatives of the state variable x(s) as a function feasibly be driven [4]. of the state x(s) and the control input u(s). (2) With the formulation of an mpSQP optimization algo- The standard form of a QP is expressed as [38] rithm, it is possible to integrate our race ES, described in our previous works [8], [34]. The necessary variable power param- T T min z Pz + q z qp qp qp eters are forwarded to the velocity planner and considered as 2 a hard constraint, see Fig. 1. In the case of electric race cars, s:t: l Az u; (5) qp such an ES is vital in order not to overstress the powertrain where z is the optimization vector, matrix P is the Hessian qp thermodynamically. k T matrix of the discretized objective J (o ) and the vector q (3) We further allow the friction coefficient on the race track equals the Jacobian of the discretized objective rJ (o ) with to vary spatially as well as temporally [7]. Therefore, global iterate k. Matrix A contains the linearized versions of the limits of the allowed longitudinal and lateral acceleration of constraints h and g in the optimization problem. Their upper b c the vehicle are omitted. This improves the achievable lap and lower bounds are summarized in both vectors, l and u. time significantly as the tires are locally exploited to their In an SQP method, the linearization point o is updated maximum. Via the temporal variation of the friction limits, after every QP iteration k using [39] we take into account varying grip due to, e.g., warming tires or changing weather conditions. k+1 k o = o + z (6) qp (4) To boost the solver selection for similar projects deal- k+1 k = (7) ing with trajectory optimization within the community, we qp compare the efficiency of different solver types regarding z = o o : (8) qp calculation speed and solution quality. Therefore, we solve the In the quadratic subproblem, a solution for z is computed. quadratic subproblems in our mpSQP using a first-order Alter- qp k+1 We chose to initialize the Lagrange multiplier vector nating Direction Method of Multipliers (ADMM) implemented using the previous QP solution as stated in the local SQP in the OSQP-framework. Its results are compared to the active- set solver QP Online Active SEt Strategy (qpOASES) [35]. algorithm in [40]. We contrast both SQPs with a direct solution of the nonlinear On the one hand, the steplength parameter must be velocity optimization problem with the open-source, second- calculated in order to perform a large step in the direction order interior point solver IPOPT [28], interfaced by the of the optimum o for fast convergence. On the other, symbolic framework CasADi [36]. must be small enough to not skip or oscillate around o . It is Section II introduces the mathematical background of an therefore necessary to define a suitable merit function, taking SQP method to solve an NLP. In the following Section III, into account the minimization of the objective function as well the nonlinear equations of our velocity planner are introduced. as the adherence of the constraints [37], [39]. As it is hard to We explain their efficient incorporation within an mpSQP and find such a merit function, we use the SQP Root Mean Square IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. XX, NO. X, MONTH 2021 4 Error (RMSE) " as well as the SQP infinity norm error 1) System dynamics: Let us first introduce the system SQP " ^ to determine whether a stepsize is suitable or not, dynamics of the point mass model for our physical vehicle SQP state v(s). Newton’s second law for a point mass m states k+1 k " = o o " (9) SQP SQP;tol 2 dv(s) m v(s) = m a (s): (14) v v x k+1 k " ^ = o o " ^ ; (10) ds SQP SQP;tol With the derivative of the kinetic energy, where K denotes the number of elements in o . In case one of dE (s) 1 dv (s) dv(s) kin the two errors " increases when applying (6), the counting SQP = m = m v(s) ; (15) v v ds 2 ds ds variable
is increased and, therefore, is reduced until the the system dynamics are given by the longitudinal acceleration tolerance criteria values " and " ^ are met: SQP;tol SQP;tol 1 dE (s) kin = : (11) a (s) = : (16) m ds The parameter 2 ]0; 1[ is to be tuned problem-dependent as The force F (s) applied by the powertrain to move the point x;p the Armijo rule states [37] with
2 [0; 1; 2; :::]. mass model can be calculated by To bring the objective J (o) and the necessary nonlinear F (s) = m a (s) + c v (s) (17) x;p v x r constraints h (o) and g (o) into the mathematical form of a b c QP (5), they are discretized and approximated quadratically where c is the product of the air density , the air resistance r a or linearly, respectively, using Taylor series expansions in the coefficient c and the vehicle’s frontal area A , w v form c = c A : (18) r a w v k T k k J (o) (o o ) P (o )(o o )+ 2) Equality and inequality constraints: To improve numer- k k k rJ (o )(o o ) + J (o ) (12) ical stability and avoid backward movement, the velocity v(s) is constrained, and 0 v(s) v (s): (19) max k k k g (o) rg(o )(o o ) + g(o ): (13) To ensure that the optimization remains feasible in combina- tion with a moving horizon, the terminal constraint III. OPTIMIZATION-BASED VELOCITY P LANNER v(s ) v (20) f end This section describes the implemented point mass model, on the last coordinate point s within the optimization horizon the used objective function, and the constraints necessary to is leveraged. Here, v denotes the minimal velocity the end optimize the velocity on the available paths. The point mass vehicle can take in the case of maximum specified track model was chosen, as it is commonly used to describe the curvature at the vehicle’s technically maximum possible max driving dynamics in the automotive context. Due to its sim- lateral acceleration a . Therefore, y;max plicity, it delivers a small number of optimization variables and constraints. Therefore, quick solver runtimes can be achieved. y;max v = : (21) end It still delivers quite accurate results for the task of pure max velocity optimization [13]. At the beginning of the optimization horizon, the velocity The concept of the optimization-based algorithm is to plan and acceleration must equal the vehicle’s target states of the velocities with inputs from other software modules, cf. Section currently executed plan v and a , ini x;ini I. We do not deal with sensor noise in the planner but in the vehicle dynamics controller [5], which receives the trajectory v(s ) = v ; s ini input. The trajectory planning module, consisting of a path a a (s ) a + ; (22) x;ini a x s x;ini a planner [4] and the presented velocity optimization algorithm, where s denotes the first coordinate within the moving always keeps the first discretization points of a new trajectory s optimization horizon and a small tolerance to account for constant with the solution from a previous planning step. After a numerical imprecision. matching the current vehicle position to the closest coordinate As the vehicle’s maximum braking as well as driving forces in the previously planned trajectory, a new plan starting from are technically limited, the resulting constraints are this position is made within the remaining part of a new trajectory. Through this, two control loops can be omitted, F F (s) F : (23) min x;p max and prevent from unnecessary inferences in the planning and The negative force constraint F does not affect the the control module. min optimization-problem feasibility, as the DevBot’s braking ac- tuators can produce more negative force than the tires can A. Nonlinear problem transform. This subsection presents the nonlinear velocity optimization The electric machine’s output power P (s) is computed problem, structured into its system dynamics, equality and using inequality constraints as well as its objective function. P (s) = F (s)v(s); (24) x;p IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. XX, NO. X, MONTH 2021 5 limited by the available maximum on the integral of the squared slack values. The linear ;q penalty term on the slack variable (s) is necessary to achieve P (s) P (s): (25) max an exact penalty maintaining the original problem’s optimum [v (s) (s)] if feasible [42]. Similar to a regularization term, We highlight that P (s) is a space-dependent parameter in max the integral of the squared slacks (s) is additionally added to contrast to the constant maximum force F . By this, the max improve numerical stability and the smoothness of the results. given race ES based on our previous works [8], [34] is realized. To further integrate the tire physics, we interpret the friction potential as a combined, diamond-shaped acceleration limit for B. Multi-parametric Sequential Quadratic Problem the vehicle [7] given by the inequality This chapter gives details about the implementation of the NLP given in Subsection III-A as an mpSQP in order to (a ^ (s); a ^ (s)) 1 + (s) (26) x y efficiently solve local approximations of the velocity planning where jjjj denotes the l -norm. Furthermore, the normalized 1 problem. We describe how to approximate the nonlinear longitudinal a ^ (s) as well as the lateral tire utilizations a ^ (s) x y objective function J (x(s)) (31) to achieve a constant and are given by tuneable Hessian matrix within our tailored mpSQP algorithm. Furthermore, we present a method to reduce the number of F (s) 1 a (s) x;p y a ^ (s) = ; a ^ (s) = : (27) x y slack variables (s) and the slack constraints (29) therefore m a (s) a (s) v x y necessary. Here, we use a (s) to indicate a variable, space-dependent x=y Our optimization vector z = o in a discrete formulation acceleration potential in both longitudinal and lateral direction, transforms into which is to be leveraged [7]. The lateral acceleration a (s) y T v (s ) : : : v (s ) (s ) : : : (s ) 1 1 M 1 M 1 0 0 N 1 N 1 reads [41] z = | {z } | {z } a (s) = (s)v (s) (28) (32) accounting for the target path geometry by the variable road K1 2 R curvature parameter (s). where K = M 1 + N where M denotes the number of In (26) the slack variable (s) ensures the recursive feasibil- discrete velocity points v and N the number of discrete slack ity of the optimization problem: details are given in Subsection variables used in the tire inequality constraints within one III-D. We constrain the slack variable (s) by optimization horizon. We drop the dependency of z on s in 0 (s) (29) max the following for the sake of readability. The velocity variable v is removed from the vector z as it is a fixed parameter to prohibit negative values and additionally keep the physical equaling the velocity planned in a previous SQP l 1 for the tire exploitation within a specified maximum. current position. Similarly to (21), the longitudinal and lateral acceleration To reduce the problem size, we apply one slack variable limits at the end of the optimization horizon a (s ) and a (s ) x f y f to multiple consecutive discrete velocity points v . This is n m must be set to the lowest physically possible acceleration limits done uniformly and leads to: a for the current track conditions, x=y;min " # v : : : v v : : : v v : : : v 1 ~ ~ ~ ~ M 1 a (s ) a : (30) N N +1 2N 2N +1 x=y x=y;min | {z } (33) | {z } | {z } 0 ::: 3) Continuous objective function: With the help of the introduced symbols and equations we can now formulate the Here, N is a problem-specific parameter setting a trade off objective function J (x(s)) to minimize the traveling time between the number of optimization variables and therefore along the given path: the calculation speed and accuracy in the solution. Z Z 2 From domain knowledge we know that the objective func- s s f f 2 1 d v(s) J (x(s)) = ds + ds+ tion can be approximated in the form v(s) s d t 0 0 Z Z 2 2 s s f f J (z) v v + v + ;l ;q max j 2 2 2 (s)ds + (s)ds: (31) | {z } | {z } s s f f 0 0 J J v j 2 2 We chose the optimization variables o to be the state velocity + : (34) ;l ;q 1 2 | {z } | {z } v(s) as well as the slacks (s). The control input to the vehicle J J ;l ;q u(s) = F (s) doesn’t occur explicitly in the objective x;p function but can be recalculated from the state trajectory v(s), The slack variables are transformed via the constant factor ; cf. (17). how this is selected is discussed at the end of this section. Minimizing the term is equivalent to the minimization By using the l -norm of the vector difference of v and v , v(s) max dt of the lethargy , which can be interpreted as the time the solution tends to minimize the travel time along the path. ds necessary to drive a unit distance [13]. To weight the different Still, this formulation in combination with (19) makes the car terms, the penalty parameters are used. These include a jerk keep a specified maximum velocity v (s) dependent on the max penalty , a slack weight on their integral and a penalty current position s to react, e.g., to other cars. To control the j ;l IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. XX, NO. X, MONTH 2021 6 vehicle’s jerk behavior, we add the Tikhonov regularization path might change. Therefore, the requested coordinates in v the acceleration map do also vary slightly, but are matched term [38] that approximates the second derivative M 3M 1 to the same local coordinate indices in s within the local of v. The tridiagonal Toeplitz matrix 2 R m 1 2 1 path in subsequent timesteps. This leads to differences in the contains the diagonal elements ( ) [38]. By the l - acceleration limits a (s ) in subsequent planning iterations norm within J , the summation of the absolute values of the m ;l x=y for identical local path indices m and therefore probably to slack variable vector entries in is achieved. To improve the infeasible problems in terms of optimization, see Section III-D. numerical conditioning of the problem, their l -norm is added additionally by J . ;q The nature of the discretization problem is illustrated in For the specific choice of cost function in (12), the Hessian Fig. 3. The given example provides stored acceleration limits KK matrix P 2 R does not depend on z. The condition (s ) in 10 m steps. The planning horizon ranges from a glo number of the Hessian P is tuned to be as close to 1 as s = 0 m to 300 m. Therefore, we show a snippet of the glo possible via the penalties and as well as denoting the j ;l end of the planning horizon (s = 200 m to 270 m) as the glo unit conversion factor of the tire slack variable values in to discretization issues are clearly visible here. At timestep t , SI units: the planning algorithm requests the stored acceleration limits 2 3 . . . . (s ) every 5:5 m, starting at s = 0 m. Within the a glo glo . . 6 7 depicted path snippet, the subsequent iteration at t starts at a 6 7 . . 6 . . 7 . . c 0 shift of 2:0 m with the same stepsize. 6 j j 7 6 7 . . P = 6 7 (35) . . . . 6 7 The simple approach of directly obtaining the local accel- 6 7 6 7 eration limits from the stored values (s ) by applying 2 0 ;q a glo 4 5 zero-order hold comes with drawbacks. A slight shift in the global coordinate selection can lead to situations where the acceleration limits a (s ) differ between subsequent The function c represents different constant entries j that x=y m j j 0 1 timesteps (t , t ) in the local path. If the subsequent accel- are linearly dependent on . This upper left part of P is a eration limits a (s ) at t are smaller, they can lead to bisymmetric matrix with constant entries on its main diagonal, x=y m infeasibility. In Fig. 3, the gray areas highlight the situations as well as on its first and second ones. where the obtained acceleration limits a (s ) at t are By using the approach of multi-parametric programming, x=y m smaller compared to t for identical local path indices m. we can vary several problem parameters online in the SQP (Section II) without changing the problem size. These param- To mitigate the discretization effects, we propose an interpo- eters include the lation scheme leading to the values (s ). It applies linear a glo spatial discretization length s . interpolation between the stored acceleration limits (s ) a glo curvature of the local path (s ) [4]. but acts cautiously in the sense that it always underestimates maximum allowed velocity v (s ). max m the actual values (s ). This can be seen from s = a glo glo power limitations P (s ) stemming from a global race max m 200 m to 210 m, where the value is kept constant instead of strategy taking energy limitations into account [8], [34]. 2 interpolating between 11 and 12 m s , and from s = 230 m glo longitudinal and lateral acceleration limits a (s ), x m to 240 m where the algorithm adapts to the decreasing values a (s ) [7]. 2 y m although the stored value is 13 m s . Then, zero-order hold is applied to this conservatively interpolated line. The limits 1 0 C. Variable acceleration limits obtained at t often lie above a (s ) at t , which allow x=y higher accelerations a (s ) than expected at t and thus To fully utilize the maximum possible tire forces, a time- x=y compensating for overestimated areas (gray areas). and location-dependent map of the race track, containing the maximum possible accelerations, is generated. The ac- The acceleration limits (s ) are constantly updated by a glo celeration limits can be interpreted as vehicle-related friction an estimation algorithm [7] and are therefore also considered coefficients, cf. Subsection III-A. The 1D map along the global time-variant. During the update process, it must be guaranteed coordinate s with variable discretization step length stores glo that the update does not lead to an infeasible vehicle state the individual acceleration limitations (s ) in longitudinal a glo for the velocity-planning algorithm, e.g., when the vehicle and lateral directions. The acceleration limits are used in is approaching a turn already utilizing full tire forces under the selected local path as the parameters a (s) (27). It is x=y braking, and the acceleration limits are suddenly decreased in important to know that while the vehicle proceeds, the target front of the vehicle. Therefore, the updates only take place path is updated constantly, i.e., the global coordinates s glo outside the planning horizon of the algorithm. selected as the local path s vary permanently in subsequent velocity optimizations. The path planning guarantees to re- The difference between subsequently obtained acceleration use the first few global coordinates s from a previous limits a (s ) at identical local path coordinates s can glo x=y m m timestep t as the starting coordinates s of the subsequently be controlled via the maximum change between the stored chosen path at t . Still, the path coordinates s at the end values (s ). The slope of the interpolated values (s ) m a glo a glo of the planning horizon are not guaranteed to precisely match can be used to calculate the maximum error when applying a all of the previously used global coordinates s since the particular step size s in path planning. glo m IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. XX, NO. X, MONTH 2021 7 0 1 TABLE I path coord. at t path coord. at t Σ (s ) a ¯ glo EMERGENCY- AND P ERFORMANCE-SQP PARAM ETRIZATION. 0 1 a ¯ (s ) at t a ¯ (s ) at t Σ (s ) x/y m x/y m a ¯ glo Parameter Unit Value Performance Emergency M - 115 50 N - 12 5 m s 0.1 inactive % 3.0 3.0 max - 3e 0.0 2 2 5 4 m s 1e 5e ;l 13 2 2 4 3 m s 1e 1e ;q n - 20 20 SQP;max t ms 300 100 max - 0:5 0:5 0 0 " - 1e 1:5e SQP;tol 0 0 " ^ - 1e 1:5e SQP;tol 2 2 " - 1e 1e QP;tol 200 210 220 230 240 250 260 270 Global coordinate s in m glo Fig. 3. Diagram of acceleration limits for two subsequently planned paths; using a numerical QP solver. We therefore employ a variable with pure readout values (s ) (top) and with the proposed interpolation glo transformation with = and optimize over instead. Re- n n scheme (s ) (bottom). Gray areas show where the subsequently planned glo alistic values for the maximum slack variable were found max path receives decreased maximum acceleration limits a (s ) due to x=y to be around 3 % in extensive simulations on different race tolerances in the spatial discretization. tracks (Berlin (Germany), Hong Kong (China), Indianapolis Motor Speedway (USA), Las Vegas Motor Speedway (USA), D. Recursive feasibility Millbrook (UK), Modena (Italy), Monteblanco (Spain), Paris (France), Upper Heyford (UK), Zalazone (Hungary)) and ob- The minimum-time optimization problem tends to produce stacle scenarios. We consider this to be an acceptable tolerance solutions with many active constraints, as it maximizes the level and believe it will be difficult to achieve significantly tire utilization. It follows from this property, that ensuring tighter guarantees in the face of the scenario complexity we recursive feasibility is a highly relevant aspect for the ap- tackle in [4]. plication of such an algorithm, and should be achieved via the terminal constraints (21) and (30) by making a worst-case assumption about the curvature (s ) and acceleration limits IV. R ESULTS a (s ) at the end of the optimization horizon. This property x=y f In this section, the results achieved with the presented holds as long as the optimization problem is shifted by an velocity mpSQP will be presented. We conducted the experi- integer multiple of the discretization s while the relations ments on our Hardware-in-the-Loop simulator, which consists of the local path coordinates s with the curvature (s ) and m m of a Speedgoat Performance real-time target machine, where the acceleration limits a (s ) and a (s ) remain constant. x m y m validated physics models of the real race car in combination This cannot be ensured since the path planner [4] might with realistic sensor noise are implemented. An additional slightly vary the target path due to an obstacle entering the NVIDIA Drive PX2 receives this sensor feedback and cal- planning horizon, or due to discretization effects. This leads culates the local trajectories. A Speedgoat Mobile real-time to a deviation in the curvature profile (s ) and deviations target machine transforms this trajectory input into low level in the admissible accelerations a (s ) and a (s ) since the x m y m vehicle commands to close the loop to the physics simulation. local coordinate s might refer to a different point in global Therefore, we used the DevBot 2.0 data: m = 1160 kg, coordinates s now, see Fig. 3. glo P = 270 kW, F = 7:1 kN, F = 20 kN, c = max max min r To mitigate this deficiency, we introduce slack variables 1 0:85 kg m . The results in this section have been produced based on the exact penalty function approach [42]. This with the velocity planner parametrizations given in Table I. strategy ensures that the hard-constrained solution is recovered We show results for two types of offered paths: performance if it is feasible, and therefore the solution is not altered by and emergency. The emergency path is identical to the perfor- addition of the slacks unless it is mandatory. The nature of the mance one, except for a coarser spatial discretization, and the combined acceleration constraint (26) allows for a straightfor- fact that the velocity planner tries to stop as soon as possible ward interpretation of the slack variables as a violation of on the emergency line. The optimization of the emergency line in %. Together with the upper bound on the slack variables requires therefore fewer variables M + N . This formulation in (29), we can therefore state that the optimization problem reduces the necessary calculation time of the emergency line is always feasible as long as the maximum required violation that must be updated more frequently for safety reasons. is limited to . In case no solution is found within the max specified tolerance band, a dedicated failure-handling strategy A. Objective function design is employed within the trajectory planning framework. We wish to point out that a suitable scaling of the slack variables To explain the chosen values of the penalty weights , we is crucial to achieve sufficiently tight tolerances " when show the values of the single objective function terms in J (z) QP;tol Acc. limits Acc. limits −2 −2 a ¯ in m s a ¯ in m s x/y x/y IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. XX, NO. X, MONTH 2021 8 the electric behavior of battery, power inverters and elec- J J J v ,l j tric machines, i.e., the power losses of these components during operation; the thermodynamics within the powertrain transforming −1 power losses into temperature contribution. 0 200 400 600 800 1,000 1,200 Fig. 5 depicts the output of the ES computed offline (top). We varied the amount of energy available for one race lap by the three values 100 % (E (s )), 80 % (E (s )) glo;100 glo glo;80 glo Fig. 4. Cost terms of the objective function J being minimized within the and 60 % (E (s )). The optimal power usage P (s ) glo;60 glo glo glo performance velocity profile. J shows the most significant influence on the belonging to these energy values E (s ) is depicted in the solution due to its value range compared to the other objective terms J , J glo glo j ;l and J (not displayed as it equals almost 0). Symbol l denotes the number ;q first diagram. The positive values in P (s ) become the glo glo of the optimized velocity profiles. parametric input of the power constraint within the velocity optimization (25). By this formulation, we only restrict the vehicle’s acceleration power but leave the braking force unaf- being minimized during the calculation of the performance fected. This experiment consists of one race lap with constant velocity profile in Fig. 4. The symbol l denotes the number of maximum acceleration values a on the Modena (Italy) race x=y the optimized velocity profiles during the driven lap (including circuit. the race start) as well as the vehicle’s stopping scenario. It The power usage P (s ) locally planned by the mp- loc;80 glo can clearly be seen that the velocity term J has the highest SQP is shown in the middle plot of Fig. 5. The positive power relative impact on the optimum solution. Its value range is at values in P (s ) remained below the maximum power loc;80 glo least two orders of magnitude higher than the slack penalty request allowed by the global strategy P (s ). Differ- glo;80 glo terms J , J (not displayed) and the jerk penalty J . The ;l ;q j ences between the globally optimal power usage P (s ) and glo glo penalty weight on the linear slack term was chosen to ;l the locally transformed power P (s ) stem from different loc glo increase the value of J to be one order of magnitude higher ;l model equations in both - offline ES and online mpSQP - than J if was fully exploited on all the slack variables v max optimization algorithms. The velocity planner’s point mass . Therefore, J prevents the solver from permanent usage n ;l is more limited in its combined acceleration potential due to of tire slack for further lap time gains. As is applied to ;q the diamond-shaped acceleration constraint (26) compared to the squared values , it is sufficient to keep the magnitude of the vehicle dynamics model (NDTM) in the ES. The NDTM one order smaller than . To provide a smooth velocity ;q ;l overshoots the dynamical capability of the car slightly in profile, we set the jerk penalty to increase the value of edge cases due to parameter-tuning difficulties. Furthermore, J to be higher than the slack penalty terms during normal the effect of longitudinal wheel-load transfer is considered operation. By this, effects on a possible lap time loss stay as within the NDTM. For these reasons, the point mass model small as possible, whilst a smoothing effect in the range of accelerates less but meets the maximum admissible power numerical oscillations on the velocity and acceleration profile P (s ) on the straights. The accumulated error in this glo glo is still visible. experiment can be expressed by the energy demand E loc We further integrated a calculation time limit t for the max resulting from velocity optimization and a maximum SQP iteration number n . In case of reached limits, the algorithm would SQP;max E = F (s )ds : (36) loc x;p glo glo return the last suboptimal but driveable solution. The SQP never reached these limits during our experiments, and they In total, an energy amount of E = 1:27 kW h was allowed can be considered as safety limitations. Instead, the SQP- glo whereas E = 1:07 kW h was used, implying 15:8 % drift. algorithm always terminated due to the reached tolerance loc With the help of a re-calculation strategy adjusting the ES criteria " and " ^ , cf. (9) and (10). SQP;tol SQP;tol during the race, this error can be significantly reduced. This feature was switched off during this experiment to isolate the working principle of the race ES, i.e., the interaction between B. Energy Strategy global and local planners. As stated in Subsection III-B, the presented mpSQP is able The effect of the ES on the vehicle speed v is de- P80 to implement our global race ES [8], [34]. The ES is pre- picted in Fig. 6. The left part (A) of both plots consists computed offline and re-calculated online due to disturbances, of a scenario where the race car is accelerating with the unforeseen events during the race and model uncertainties. maximum available machine power of P = 270 kW and a max Through this, we account for the limited amount of stored subsequent straight where the ES forces the vehicle to coast battery energy and the thermodynamic limitations of the (P (s ) = P (s ) = 0 kW). Acceleration without glo;80 m loc;80 m electric powertrain. Therefore, the ES delivers the maximum any power restriction on this straight would have resulted in permissible power P (s ) in order to reach the minimum max glo the velocity curve v meaning a slightly higher top speed P100 race time, see Fig. 1. It takes the following effects into account: by approx. 4 % or 8 km h . The second part (B) of the shown the vehicle dynamics in the form of an NDTM including planning horizon ensures recursive feasibility. Here, we force a nonlinear tire model; the velocity variable v(s ) to reach v (20). N end 2 −2 J in m s IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. XX, NO. X, MONTH 2021 9 E E E 12.5 glo,100 glo,80 glo,60 loc,80 10.5 8.5 Global coordinate s in m glo 6.5 100 200 300 x in m 0.5 Fig. 7. Race track map displaying the spatially variable acceleration potential (s ) including the driven path and markers starting from the global s- a glo 0 500 1,000 1,500 2,000 coordinate of 1400 m at 100 m gaps. Global coordinate s in m glo + − Fig. 5. Top: Optimal power usage P (s ) from the ES that should be a (a ¯ ) a (a ¯ ) a (a ¯ ) glo glo y y,var y y y,cst y,cst requested whilst driving on the Modena (Italy) race track. Mid: Power planned + − a ¯ a ¯ a ¯ y,var y,cst y,cst locally P (s ) compared to P (s ) resulting in an absolute drift loc;80 glo glo;80 glo in the energy demand between E (s ) and E (s ) of approx. glo;80 glo loc;80 glo 12.5 15:8 % within one race lap (bottom). 6.5 -6.5 -12.5 A v v B P P 80 100 v v max end Distance s in m loc,80 1,400 1,500 1,600 1,700 1,800 1,900 −500 glo,80 Global coordinate s in m glo 0 20 40 60 80 100 Fig. 8. Comparison of velocity v(s ) and lateral acceleration a (s ) pro- glo y glo Local coordinate index m files resulting from spatially and temporally varying acceleration coefficients a (s ). glo Fig. 6. The effect of the ES on one solution of the velocity-planning problem on the vehicle speed v . In part (A), the race car is accelerating with the P80 maximum available machine power of P = 270 kW and is forced to coast depicted the acceleration potentials a of the three scenarios (P (s ) = P (s ) = 0 kW) thereafter. Acceleration without any power m m glo loc restriction is denoted by v . Part (B) of the planning horizon ensures in combination with the planned lateral acceleration a (s ) P100 y glo recursive feasibility. (top) and the vehicle velocity v(s ) (bottom). In both scenar- glo ios with constant a values, the planner leverages the entire y;cst admissible lateral potentials which can be seen between s = glo C. Variable acceleration limits 1500 - 1600 m and s = 1650 - 1700 m. The results achieved glo Fig. 7 shows a locally variable acceleration map to deter- using the variable a (s) are interesting: The planned lateral y;var mine the values of a (s) along the driven path. We conducted acceleration a (s) stayed within the boundaries of the low- three experiments with a constant acceleration potential of a and high-friction experiments. The planner handles the drop = 12:5 m s and varied a in three different ways: of a (s) of 50 % at s = 1520 m by reducing a (s) in y;var glo y + 2 a has a constant value of 12:5 m s along the entire advance, while still fully leveraging a (s). This results in y;cst y;var track, describing a high friction potential. a vehicle velocity of v(s ) in the low-friction scenario. The glo a has a constant value of 6:5 m s along the entire same is true for s = 1670 m. Also during the rest of the y;cst glo track, describing a low friction potential. shown experiment, the oscillating values of a (s) can be y;var a (s) has a variable value in the range of 6:5 m s y;var handled by the mpSQP algorithm. This behavior allows us to to 12:5 m s with the steepest gradients between s = glo fully exploit the dynamical limits of the race car on a track 1520 - 1550 m and s = 1670 - 1700 m, as shown in glo with variable acceleration potential. Fig. 7. A further indicator that the velocity planner utilizes the full The results of these experiments can be seen in Fig. 8. We acceleration potential is shown in Fig. 9. Here, we depicted the Power Velocity Energy Power Power ES −1 P in kW v in m s E in kW h P in kW P in kW Σ glo 80 80 −2 −1 y in m a in m s v in m s y −2 a ¯ in m s y IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. XX, NO. X, MONTH 2021 10 ∗ ∗ ∗ z z z OSQP IPOPT qpOASES v z v max ini end tol 95 115 μ ¯ μ ¯ + max −5 μ (μ ¯ ) o l μ (μ ¯ ) o h −10 a ¯ x,m 4.7 0 4.6 −15 −10 −5 0 5 10 15 −2 a in m s −10 0 1 2 Fig. 9. Acceleration limit model including operating points at low- and high- friction experiments ( and ) for a planning horizon ranging from the 0 20 40 60 80 100 l h global coordinate of s = 1500 m to 1800 m. −2 glo ·10 Local coordinate index m vehicle’s optimized operating points (s) regarding combined −2 acceleration a (s) and a (s) within the planning horizon of x y s = 1500 - 1800 m. Both solid diamond shapes express the glo 0 2 4 6 8 10 given constant acceleration limits of the high-friction scenario, m Local coordinate index + 2 N = a & a = 12:5 & 12:5 m s , and the low-friction h x y;cst scenario = a & a = 12:5 & 6:5 m s . The horizontal l x y;cst Fig. 10. Comparison of the solution of the mpSQP (internal QPs solved by red dashed line indicates the maximum available electric the ADMM solver OSQP [11]) with the general NLP interior point solver max machine acceleration a = . From this diagram we see IPOPT [28] interfaced by CasADi [36] and the active set solver qpOASES x;m [35]. The chosen scenario includes the vehicle heading towards a narrow that the vehicle operates at the limits of the given acceleration right-hand bend at a high velocity of almost 200 km h . The optimization constraints in both scenarios. This means the mpSQP leverages horizon spans the curve, and includes the subsequent straight. the maximum acceleration potential in combination with fully available cornering potential. velocity vector v and the slack values . This behavior is l 1 expected, as the previous SQP solution z (s ) is shifted D. Solver comparison by the traveled distance and used as initialization z . The ini The number of variables in the performance profile within optimization outputs z and z overlap except at OSQP IPOPT one QP for the performance trajectory is 126, including the end of the planning horizon where IPOPT initially al- 810 constraints. From the problem formulation (34), a small lows more positive longitudinal force F (s ), resulting in x m number of non-zero entries in the matrices A and P of 2295 more aggressive braking to fulfill the hard constraint v . end in total arises with constant entries in the problem’s Hessian qpOASES’s solution oscillates in the force F (s ) at the x m P . steep gradients. All the algorithms keep the initially given Fig. 10 contains a comparison of the ADMM solver OSQP longitudinal force F (s ) within the specified tolerance of x s [11] and the active set solver qpOASES [35] as QP solvers for 0:1 kN, with OSQP matching the exact value (see magnified an mpSQP method as proposed in this paper. Furthermore, section in the second plot). The slack values are close we solve the original NLP using the interior point solver to zero. However, the OSQP-solution shows small numerical IPOPT [28] interfaced via CasADi [36] to obtain a measure oscillations within a negligible range of approx. 0:04 %. of solution quality for the proposed mpSQP. Note that IPOPT Nevertheless this behavior is typical for an ADMM algorithm, is widely used to benchmark the solution quality even if it and is thus noteworthy. is not specifically designed for embedded optimization. We Apart from the solution qualities, we further analyzed the chose a scenario where the vehicle is heading towards a narrow velocity optimization runtimes. The scenario consisted of two right-hand turn at a high velocity of almost 200 km h . The race laps, including a race start and coming to a standstill after optimization horizon spans this turn including the consecutive the second lap on the Monteblanco (Spain) race circuit with a straight where positive acceleration occurs. Therefore, the variable acceleration potential along the circuit, cf. Subsection solvers have to deal with high gradients for the longitudinal IV-C. The histograms in Fig. 11 display the calculation times force F (s ) and lateral acceleration a (s ) stemming from t for the number of calls C to optimize a speed profile. x m y m sol curve entry and exit. The velocity plot shows that the optimal C and C denote the calls for the performance and the P E solution z almost equals the initial guess z for both the emergency lines, respectively. Their mean values are given ini −2 a in m s −1 Slack in % Force F in kN Velocity v in m s x IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. XX, NO. X, MONTH 2021 11 TABLE II ˜ ˜ ˜ Δt Δt Δt OSQP IPOPT qpOASES S OLVER M EAN RUNTIMES. Δt Δt Δt OSQP IPOPT qpOASES CPU Intel i7-7820HQ A57 ARM Prob. formulation mpSQP NLP mpSQP mpSQP 400 Solver OSQP IPOPT qpOASES OSQP Performance in ms 6.20 40.4 164 32.4 50 Emergency in ms 6.95 29.8 26.1 34.2 0 0 0 20 40 60 0 200 400 by t . We wish to point out that the algorithm runtimes sol 2,000 shown refer to an entire SQP optimization process for a speed profile consisting of the solution of several QPs in the case 1,000 of the used solvers OSQP or qpOASES, which have been 0 5 10 15 warm-started. The optimization runtimes on the specific CPUs 0 0 are also summarized in Table II where the ARM A57 is the 0 20 40 60 80 100 0 50 100 Δt in ms Δt in ms NVIDIA Drive PX2 CPU. As we selected OSQP for our sol sol application on the target hardware, we do not show additional solver times of IPOPT or qpOASES for the ARM A57 CPU. Fig. 11. Solver runtimes for the mpSQP with the local QPs solved by OSQP Our mpSQP in combination with the QP solver OSQP [11] or by the active set solver qpOASES [35], compared to the problem reaches nearly equal mean runtimes of 6 ms to 7 ms for both solved by the general NLP formulation passed to the interior point solver velocity profiles on an Intel i7-7820HQ CPU and 32 ms to IPOPT interfaced by CasADi [36]. 34 ms on an A57 ARM CPU. An amount of 2 to 5 SQP iterations for the performance line was sufficient to reach the of maximum variation within these parameters to keep the defined tolerances " . To optimize the emergency speed SQP problem feasible. Additionally, we compared different solvers profile, a higher amount of SQP iterations in the range of 5 - applied to our problem formulation to compare calculation 10 was necessary. Therefore, the computational effort for the times as well as the solution qualities. Here, our mpSQP iterative linearizations increased on this profile. In contrast, it in combination with the ADMM solver OSQP outperformed was possible to solve the single QPs in less calculation time, the active set strategy qpOASES and the general NLP solver as less than half the number of optimization variables M +N IPOPT in terms of calculation time, but reached nearly the in comparison to the performance profile are present. With same solution quality as IPOPT. This indicates that the first maximum computation times of 16:9 ms (73:3 ms A57 ARM) order ADMM in OSQP shows its strength for the minimum- on the performance profile and 15:0 ms (77:1 ms A57 ARM) time optimization problem as it handles the large set of active for the emergency line, we managed to stay far below our constraints well. predefined process-timeouts, see Table I. In future work we will apply the presented algorithm in The same optimization problem was formulated with the autonomous races and implement a tailored trajectory opti- CasADi-language [36] as a general NLP and passed to the mization module based on the presented results and techniques interior point solver IPOPT. The IPOPT mean solver runtimes for comparison. are at least approximately five times higher, with maxima of around 0:1 s on the emergency line being too long for vehicle CONTRIBUTIONS & ACKNOWLEDGMENTS operations at velocities beyond 200 km h . On the emergency T. H. initiated the idea of the paper and contributed signif- profile, the active set solver qpOASES beats IPOPT slightly in icantly to the concept, modeling, implementation and results. terms of its mean runtime t but consumes four times the sol A. W. contributed to the design and feasibility analysis of IPOPT computation time to generate the performance speed the optimization problem. L. H. contributed essentially to the profile, see Table II. This behavior is rational, as a higher num- integration of variable acceleration limits. J. B. contributed to ber of optimization variables and active constraints increases the whole concept of the paper. M. L. provided a significant the calculation speed of an active set solver significantly. contribution to the concept of the research project. He revised Nevertheless, maximum computation times for the mpSQP of the paper critically for important intellectual content. M. L. around 0:5 s on the performance line exclude the QP solver gave final approval for the publication of this version and is qpOASES for this type of application. in agreement with all aspects of the work. As a guarantor, he accepts responsibility for the overall integrity of this paper. V. CONCLUSION We would like to thank the Roborace team for giving us the In this paper we presented a tailored mpSQP algorithm opportunity to work with them and for the use of their vehicles capable of adaptive velocity planning in real time for race for our research project. We would also like to thank the Bavar- cars operating at the limits of handling, and velocities above ian Research Foundation (Bayerische Forschungsstiftung) for 200 km h . The planner can deal with performance and funding us in connection with the “rAIcing” research project. emergency velocity profiles. Furthermore, the optimization This work was also conducted with basic research fund of handles multi-parametric input, e.g., from the friction estima- the Institute of Automotive Technology from the Technical tion module or the race ES. We also specified the boundaries University of Munich. E IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. XX, NO. X, MONTH 2021 12 REFERENCES [21] J. K. Subosits and J. C. Gerdes, “From the Racetrack to the Road: Real- Time Trajectory Replanning for Autonomous Driving,” IEEE Transac- [1] Chair of Automotive Technology, “Autonomous Driving Control tions on Intelligent Vehicles, vol. 4, no. 2, pp. 309–320, 2019. Software of TUM Roborace,” 2020. [Online]. Available: [22] L. Svensson, M. Bujarbaruah, N. R. Kapania, and M. Torngren, “Adap- https://github.com/TUMFTM tive Trajectory Planning and optimization at Limits of Handling,” in [2] ——, “Python Package: Velocity Optimization,” 2020. [Online]. 2019 IEEE/RSJ International Conference on Intelligent Robots and Available: https://pypi.org/project/velocity-optimization/ Systems (IROS). IEEE, pp. 3942–3948. [3] M. Morari, F. Allgower ¨ , L. Magni, D. M. Raimondo, and M. Thoma, [23] T. Mercy, R. van Parys, and G. Pipeleers, “Spline-Based Motion Nonlinear Model Predictive Control: Towards New Challenging Appli- Planning for Autonomous Guided Vehicles in a Dynamic Environment,” cations, ser. Lecture Notes in Control and Information Sciences. Berlin, IEEE Transactions on Control Systems Technology, vol. 26, no. 6, pp. Heidelberg: Springer Berlin Heidelberg, 2009, vol. 384. 2182–2189, 2018. [4] T. Stahl, A. Wischnewski, J. Betz, and M. Lienkamp, “Multilayer Graph- [24] A. Carvalho, Y. Gao, A. Gray, E. Tseng, and F. Borrelli, “Predictive Based Trajectory Planning for Race Vehicles in Dynamic Scenarios,” Control of an Autonomous Ground Vehicle using an Iterative Lineariza- in 2019 IEEE Intelligent Transportation Systems Conference (ITSC). tion Approach,” in 16th International IEEE Conference on Intelligent IEEE, pp. 3149–3154. Transportation Systems (ITSC), 2013. Piscataway, NJ: IEEE, 2013. [5] A. Heilmeier, A. Wischnewski, L. Hermansdorfer, J. Betz, M. Lienkamp, [25] B. Alrifaee and J. Maczijewski, “Real-time Trajectory Optimization for and B. Lohmann, “Minimum curvature trajectory planning and control Autonomous Vehicle Racing using Sequential Linearization,” in 2018 for an autonomous race car,” Vehicle System Dynamics, vol. 25, no. 8, IEEE Intelligent Transportation Systems Conference. Piscataway, NJ: pp. 1–31, 2019. IEEE, 2018, pp. 476–483. [6] J. Betz, A. Wischnewski, A. Heilmeier, F. Nobis, L. Hermansdorfer, [26] G. Williams, P. Drews, B. Goldfain, J. M. Rehg, and E. A. Theodorou, T. Stahl, T. Herrmann, and M. Lienkamp, “A Software Architecture for “Aggressive driving with model predictive path integral control,” in 2016 the Dynamic Path Planning of an Autonomous Racecar at the Limits IEEE International Conference on Robotics and Automation (ICRA). of Handling,” in 2019 IEEE International Conference on Connected IEEE, 2016, pp. 1433–1440. Vehicles and Expo (ICCVE). IEEE, 2019, pp. 1–8. [27] E. Alcala, ´ V. Puig, J. Quevedo, and U. Rosolia, “Autonomous racing [7] L. Hermansdorfer, J. Betz, and M. Lienkamp, “A Concept for Estimation using Linear Parameter Varying-Model Predictive Control (LPV-MPC),” and Prediction of the Tire-Road Friction Potential for an Autonomous Control Engineering Practice, vol. 95, p. 104270, 2020. Racecar,” in 2019 IEEE Intelligent Transportation Systems Conference [28] A. Wachter ¨ and L. T. Biegler, “On the implementation of an interior- (ITSC). IEEE, pp. 1490–1495. point filter line-search algorithm for large-scale nonlinear programming,” [8] T. Herrmann, F. Passigato, J. Betz, and M. Lienkamp, “Minimum Race- Mathematical Programming, vol. 106, no. 1, pp. 25–57, 2006. Time Control-Strategy for an Autonomous Electric Racecar,” in 2020 [29] L. Svensson, L. Masson, N. Mohan, E. Ward, A. P. Brenden, L. Feng, IEEE Intelligent Transportation Systems Conference (ITSC). IEEE, and M. Torngren, “Safe Stop Trajectory Planning for Highly Automated 2020 (In Press). Vehicles: An Optimal Control Problem Formulation,” in 2018 IEEE [9] E. Ozatay, U. Ozguner, and D. Filev, “Velocity profile optimization of Intelligent Vehicles Symposium (IV). IEEE, 2018, pp. 517–522. on road vehicles: Pontryagin’s Maximum Principle based approach,” [30] B. Houska, H. J. Ferreau, and M. Diehl, “ACADO toolkit-An open- Control Engineering Practice, vol. 61, pp. 244–254, 2017. source framework for automatic control and dynamic optimization,” [10] A. Liniger, A. Domahidi, and M. Morari, “Optimization-based au- Optimal Control Applications and Methods, vol. 32, no. 3, pp. 298– tonomous racing of 1:43 scale RC cars,” Optimal Control Applications 312, 2011. and Methods, vol. 36, no. 5, pp. 628–647, 2015. [31] T. Lipp and S. Boyd, “Minimum-time speed optimisation over a fixed [11] B. Stellato, G. Banjac, P. Goulart, A. Bemporad, and S. Boyd, “OSQP: path,” International Journal of Control, vol. 87, no. 6, pp. 1297–1311, an operator splitting solver for quadratic programs,” Mathematical Programming Computation, vol. 5, no. 1, p. 42, 2020. [32] T. Denton, Automated Driving and Driver Assistance Systems, first [12] J. Betz, A. Wischnewski, A. Heilmeier, F. Nobis, T. Stahl, L. Her- edition ed. Abingdon, Oxon and New York, NY: Routledge, 2020. mansdorfer, B. Lohmann, and M. Lienkamp, “What can we learn from [33] G. Banjac, P. Goulart, B. Stellato, and S. Boyd, “Infeasibility Detection autonomous level-5 motorsport?” in 9th International Munich Chassis in the Alternating Direction Method of Multipliers for Convex Opti- Symposium 2018, ser. Proceedings, P. Pfeffer, Ed. Wiesbaden: Springer mization,” Journal of Optimization Theory and Applications, vol. 183, Fachmedien Wiesbaden, 2019, pp. 123–146. no. 2, pp. 490–519, 2019. [13] S. Ebbesen, M. Salazar, P. Elbert, C. Bussi, and C. H. Onder, “Time- [34] T. Herrmann, F. Christ, J. Betz, and M. Lienkamp, “Energy Management optimal Control Strategies for a Hybrid Electric Race Car,” IEEE Strategy for an Autonomous Electric Racecar using Optimal Control,” Transactions on Control Systems Technology, vol. 26, no. 1, pp. 233– in 2019 IEEE Intelligent Transportation Systems Conference (ITSC). 247, 2018. IEEE, pp. 720–725. [14] D. J. N. Limebeer and G. Perantoni, “Optimal Control of a Formula One [35] H. J. Ferreau, C. Kirches, A. Potschka, H. G. Bock, and M. Diehl, Car on a Three-Dimensional Track—Part 2: Optimal Control,” Journal “qpOASES: a parametric active-set algorithm for quadratic program- of Dynamic Systems, Measurement, and Control, vol. 137, no. 5, p. ming,” Mathematical Programming Computation, vol. 6, no. 4, pp. 327– 051019, 2015. 363, 2014. [15] A. J. Tremlett and D. J. N. Limebeer, “Optimal tyre usage for a Formula [36] J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, One car,” Vehicle System Dynamics, vol. 54, no. 10, pp. 1448–1473, “CasADi: a software framework for nonlinear optimization and optimal 2016. control,” Mathematical Programming Computation, vol. 11, no. 1, pp. [16] F. Christ, A. Wischnewski, A. Heilmeier, and B. Lohmann, “Time- 1–36, 2019. optimal trajectory planning for a race car considering variable tyre-road [37] D. G. Luenberger and Y. Ye, Linear and Nonlinear Programming, friction coefficients,” Vehicle System Dynamics, vol. 3, no. 1, pp. 1–25, third edition ed., ser. International Series in Operations Research & Management Science. New York, NY and Berlin and Heidelberg: [17] N. Dal Bianco, R. Lot, and M. Gadola, “Minimum time optimal Springer, 2008, vol. 116. control simulation of a GP2 race car,” Proceedings of the Institution [38] S. P. Boyd and L. Vandenberghe, Convex optimization, ser. Safari Tech of Mechanical Engineers, Part D: Journal of Automobile Engineering, Books Online. Cambridge, UK and New York: Cambridge University vol. 232, no. 9, pp. 1180–1195, 2018. Press, 2004. [18] Y. Huang, H. Ding, Y. Zhang, H. Wang, D. Cao, N. Xu, and C. Hu, “A [39] P. T. Boggs and J. W. Tolle, “Sequential Quadratic Programming,” Acta Motion Planning and Tracking Framework for Autonomous Vehicles Numerica, vol. 4, pp. 1–51, 1995. Based on Artificial Potential Field Elaborated Resistance Network [40] J. Nocedal and S. J. Wright, Numerical Optimization, second edition ed., Approach,” IEEE Transactions on Industrial Electronics, vol. 67, no. 2, ser. Springer Series in Operations Research and Financial Engineering. pp. 1376–1386, 2020. New York, NY: Springer Science+Business Media LLC, 2006. [19] Y. Meng, Y. Wu, Q. Gu, and L. Liu, “A Decoupled Trajectory Planning [41] F. Braghin, F. Cheli, S. Melzi, and E. Sabbioni, “Race driver model,” Framework Based on the Integration of Lattice Searching and Convex Computers & Structures, vol. 86, no. 13-14, pp. 1503–1516, 2008. Optimization,” IEEE Access, vol. 7, pp. 130 530–130 551, 2019. [42] E. C. Kerrigan, “Soft Constraints and Exact Penalty Functions in Model [20] Y. Zhang, H. Chen, S. L. Waslander, J. Gong, G. Xiong, T. Yang, and Predictive Ccontrol,” in Proceedings of the United Kingdom Automatic K. Liu, “Hybrid Trajectory Planning for Autonomous Driving in Highly Control Council 2000, IFAC, Ed., 2000. Constrained Environments,” IEEE Access, vol. 6, pp. 32 800–32 819, 2018. IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. XX, NO. X, MONTH 2021 13 Thomas Herrmann was awarded a B.Sc. and an M.Sc. in Mechanical Engineering by the Technical University of Munich (TUM), Germany, in 2016 and 2018, respectively. He is currently pursuing his doctoral studies at the Institute of Automotive Technology at TUM where he is working as a Research Associate. His current research interests include optimal control in the field of trajectory planning for autonomous vehicles and the efficient incorporation of the electric powertrain behavior within these optimization problems. Alexander Wischnewski was awarded a B.Eng. in Mechatronics by the DHBW Stuttgart in 2015, and an M.Sc. in Electrical Engineering and Information Technology by the University of Duisburg-Essen, Germany, in 2017. He is currently pursuing his doctoral studies at the Chair of Automatic Control at the Department of Mechanical Engineering at Technical University of Munich (TUM), Germany. His research interests lie at the intersection of control engineering and machine learning, with a strong focus on autonomous driving. Leonhard Hermansdorfer was awarded a B.Sc. and an M.Sc. in Mechanical Engineering by the Technical University of Munich (TUM), Germany, in 2015 and 2018, respectively. He is currently pursuing his doctoral studies at the Institute of Auto- motive Technology at TUM where he is working as a Research Associate. His current research interest is the identification of a vehicle’s maximum transmit- table tire forces, with a strong focus on model-less approaches based on machine learning. Johannes Betz is currently a postdoctoral researcher at the Technical University of Munich (TUM). He graduated in 2013 with an M.Sc. in Automotive Technology from the University of Bayreuth, Ger- many. He graduated with a doctoral degree from TUM about the topic of fleet disposition for electric vehicles. Since then, he continues his research in the field of autonomous driving. His research topics include the dynamic trajectory and behavioral plan- ning for autonomous vehicles at the handling limits, as well as prediction of traffic participant behavior in non-deterministic environments. Markus Lienkamp studied Mechanical Engineering at TU Darmstadt, Germany, and Cornell Univer- sity, USA, and received a Ph.D. degree from TU Darmstadt in 1995. He was with Volkswagen as part of an International Trainee Program and took part in a joint venture between Ford and Volkswa- gen in Portugal. Returning to Germany, he led the Brake Testing Department, VW Commercial Vehicle Development Section, Wolfsburg. He later became the Head of the “Electronics and Vehicle” Research Department, Volkswagen AG’s Group Research Di- vision. His main priorities were advanced driver assistance systems and vehicle concepts for electromobility. He has been Head and Professor of the Institute of Automotive Technology, Technical University of Munich (TUM), Germany, since November 2009. He is conducting research in the area of electromobility, autonomous driving and mobility.
http://www.deepdyve.com/assets/images/DeepDyve-Logo-lg.png
Electrical Engineering and Systems Science
arXiv (Cornell University)
http://www.deepdyve.com/lp/arxiv-cornell-university/real-time-adaptive-velocity-optimization-for-autonomous-electric-cars-WTfSvkkHJA