Analysis and design of recovery behaviour of autonomous-vehicle avoidance manoeuvres

Autonomous vehicles allow utilisation of new optimal driving approaches that increase vehicle safety by combining optimal all-wheel braking and steering even at the limit of tyre–road friction. One important case is an avoidance manoeuvre that, in previous research, for example, has been approached by different optimisation formulations. An avoidance manoeuvre is typically composed of an evasive phase avoiding an obstacle followed by a recovery phase where the vehicle returns to normal driving. Here, an analysis of the different aspects of the recovery phase is presented, and a subsequent formulation is developed in several steps based on theory and simulation of a double lane-change scenario. Each step leads to an extension of the optimisation criterion. Two key results are a theoretical redundancy analysis of wheel-torque distribution and the subsequent handling of it. The overall contribution is a general treatment of the recovery phase in an optimisation framework, and the method is successfully demonstrated for three different formulations: lane-deviation penalty, minimum time, and squared lateral-error norm.


Introduction
The interwoven developments of sensor equipment and available computational power in modern passenger cars open up new possibilities for autonomous control functions in safety-critical situations to assist the driver, or even ultimately let the vehicle operate completely autonomously also in at-the-limit situations. One important manoeuvre, with significant passenger risk, is the double lane-change manoeuvre, e.g. as defined in the ISO double lane-change test [1]. This emergency manoeuvre is in Scandinavian countries also known as the 'moose test', i.e. an evasive manoeuvre for a moose suddenly appearing on the road, see Figure 1 for one version of the setup. A successful and promising approach to handle such manoeuvres has in recent research been dynamic optimisation for both computation and study of such vehicle manoeuvres [2,3]. Racing applications, and associated control of autonomous vehicles at the limit of tyre friction, based on optimal trajectories pre-computed offline with dynamic optimisation, provide another fruitful approach to the development of strategies for optimal vehicle manoeuvring [4]. Collision avoidance and safety manoeuvres with methods originating in, or based on inspiration from, optimisation have been proposed previously, see, e.g. [5][6][7][8][9][10]. An important observation for this paper is that an avoidance manoeuvre typically is composed of an evasive phase avoiding the obstacle followed by a recovery phase where the vehicle returns to its original lane. To give a safe and well-behaved overall manoeuvre, the recovery into the own lane needs to be swift. It turns out that several considerations are needed to extend a nominal optimisation criterion such that it results in a recovery phase with desired characteristics, and this is the topic of the present paper. The development of the recovery terms in the optimisation criterion is done in several steps based on both theory and simulation. One important extension of the criterion is based on that there are interesting redundancies in actuation distribution, and a theoretical analysis of the Jacobian rank guides the inclusion of some of the additional terms. To build up the formulation for the recovery part, one example criterion for the evasive part is used, namely the lane-deviation penalty function (LDP) from [11]. Then, the generality is demonstrated by applying the recovery extensions also to formulations based on minimum time and on a squared lateral-error norm. Overall it is demonstrated that, for the considered cases, the developed recovery extensions of the optimisation formulation significantly improve the recovery behaviour compared to the baseline formulation.

Vehicle model
The double-track model from [2,12] is used. It includes a sprung mass to model longitudinal and lateral load transfer (also called weight transfer). The equations of motion arev ψ(I xx sin(θ ) 2 + cos(θ ) 2 (I yy sin(φ) 2 + I zz cos(φ) 2 )) where v x , v y are the longitudinal and lateral velocities at the centre of gravity,ψ is the yaw rate, h is the distance from the roll centre to the centre of gravity, m is the vehicle mass, θ is the pitch angle and φ is the roll angle, I xx , I yy , and I zz are the vehicle chassis inertias along the x, y, and z directions, respectively. F X , F Y , and M Z are the forces defined as F X = F x,1 cos(δ) − F y,1 sin(δ) + F x,2 cos(δ) − F y,2 sin(δ) + F x, 3 F Y = F x,1 sin(δ) + F y,1 cos(δ) + F x,2 sin(δ) + F y,2 cos(δ) + F y,3 + F y,4 , M Z = l f (F y,1 cos(δ) + F y,2 cos(δ) + F x,1 sin(δ) + F x,2 sin(δ)) − l r (F y,3 + F y,4 ) + w(F y,1 sin(δ) − F y,2 sin(δ) − F x,1 cos(δ) + F x,2 cos(δ) − F x,3 + F x, 4 ), (6) where δ is the steering angle, F x,i , F y,i , i ∈ {1, 2, 3, 4}, are the longitudinal and lateral forces for the front and the rear wheels, and l f , l r are defined in Figure 2.
The pitch and roll dynamics are modelled as where K θ , D θ , K φ,i , and D φ,i , i ∈ {f , r}, are stiffness and damping parameters and g is the constant of gravitational acceleration. The equations of the longitudinal load transfer are where F z,i , i ∈ {1, 2, 3, 4}, denote normal forces. The lateral load transfer is determined by where w is defined in Figure 2. The numerical values of the chassis parameters are given in Table 1 (Panel A). The vehicle position in global X and Y coordinates is computed bẏ where ψ is the orientation of the vehicle in the global XY-frame. The dynamic equations for the wheels are (see [2]) where T i is the driving/braking torque for wheel i, I ω is the wheel inertia, ω i is the angular velocity for wheel i, and R ω is the wheel radius. The slip angles α i and the slip ratios κ i are described as in [13] according tȯ Note: The chassis and wheel parameters [2] and the tyre-model parameters [13] are adopted.
where σ is the relaxation length and v x,i , v y,i are the longitudinal and lateral velocities for wheel i, respectively, with respect to its own coordinate system. The nominal tyre forces F x0,i and F y0,i are computed using Pacejka's Magic Formula model [13] according to with parameters shown in the right column of Table 1 (Panel B). To summarise the model equations (1)- (21), the inputs, states, and algebraic variables are collected in vectors for future reference. The time derivatives of the wheel torques and the steering angle are considered as the input vector u, which is beneficial in the optimisation since it allows constraints also on the derivatives of the actual control inputs. The state vector is denoted as x. The algebraic variables in the model equations (1)- (21) are referred to by the vector z. The input, state, and algebraic vectors are given by By using the vectors of inputs, states, and algebraic variables, the function G is introduced as a compact representation of the equations of motion (1)-(3), the pitch and roll dynamics (7)-(8), the dynamics of the longitudinal load transfer (9) and the lateral load transfer (11)-(12), the kinematic equations for the global vehicle position (13), the dynamic equations for the wheels (14), the relaxation dynamics for the slip angles (15), and the equations connecting the states with the inputs. Further, the function h is introduced to represent the equations for the global forces (4)-(6), the normal forces (10), the slip ratios (16), the nominal tyre forces (17)- (18), and weighting functions (19)- (21). Thus, a semi-explicit index-1 differential-algebraic equation (DAE) system for the model can be compactly written asẋ

Road and obstacle
The scenario considered in this paper is an avoidance manoeuvre for a vehicle driving on a two-lane road with one lane for traffic in each direction. The situation considered is that there is an obstacle appearing on the road, which is blocking the driving lane of the vehicle. The dimensions of the scenario are shown in Figure 3. In the optimisation formulation, constraints are enforced only on the centre of gravity of the vehicle, and therefore, the road constraints are adjusted accordingly to account for the width and the length of the vehicle. The road boundaries are inspired by the ISO standard [1] and are formulated in an analytical form using a function H a r a o (a). This function represents a smooth approximation of the Heaviside step function with an offset a o and a rising distance a r ; the function is also known as the sigmoid function (see Figure 4) The right-hand side boundary of the drivable area Y bb (X(t)) is defined by the following parameters (see Figure 3): • X ou and X od , the points on the X-axis where the right-hand side boundary increases and decreases along the Y-direction, respectively, • X r , the sharpness of the transition (symmetric for both obstacle edges), • Y bbp , the peak value for the boundary.
The analytical form of the right-hand side road constraint is then introduced as Numerical values of the obstacle parameters are given in the left half in Table 2. Figure 3 shows the road and the vehicle representation used in the following formulation of the optimisation problem. The obstacle boundaries are represented using (28), and the allowed lateral manoeuvring interval (i.e. along the Y dimension) for the vehicle centre of gravity is set to be between 0 and 1.4 m for the own driving lane, and correspondingly to be between 3.2 and 4.6 m for the opposing driving lane.

Baseline formulation of optimal control problem
For a vehicle in a double lane-change manoeuvre, it is crucial to avoid the obstacle, but it is dangerous for the vehicle to be in the opposing lane because of the risk of collision there. It is then safe to drive in the original lane again after the obstacle. For illustration of the design of the recovery behaviour in an optimisation framework, the lane-deviation penalty (LDP) from [11] is used as the nominal objective to minimise.

Formulation
Using auxiliary function (27), the lane-deviation penalty (LDP) function is where Y o = 2.3 m and Y r = 1.8 m. A projection of the LDP function on the Y-axis is illustrated with the thick vertical curve to the left in Figure 5. The vehicle starts at the point (X 0 , Y 0 ) with the velocity v 0 and zero heading angle (ψ = 0). The final point is at X t f and below Y t f , where the last condition is imposed, such that the vehicle ends at an appropriate lane position. The numerical values defining the manoeuvre are given in the right half in Table 2. The final point X t f is set further away than defined by the manoeuvre illustrated in Figure 1. The additional longitudinal distance is added to allow a detailed investigation of the recovery part of the manoeuvre and the effects of different terms in the objective function in the optimisation.  Table 2. The thick left vertical curve shows a projection of the LDP function (29). The obstacle and the right-hand side road boundary are described by (28). The dashed thick line shows a projection of the switching function (32). The thick vertical curve in the middle shows a projection of (36), which is used in the second part of the manoeuvre in the extended formulation (39) of the objective function in the optimisation.
The motion-planning problem for the double lane change with penalty function (29) is formulated as a continuous-time optimisation problem like in [2,11] as where t f is the total time of the manoeuvre, which is unknown a priori and thus part of the optimisation problem to determine. The limits on the wheel torque and the steering angle are introduced as in [2] (see Table 1, Panel A). The maximum wheel torque is set to zero, i.e.T i,max = 0, i ∈ {1, 2, 3, 4}, in this formulation of the optimal control problem, since the common way to navigate a double lane-change manoeuvre is to have the throttle released as it is described in the standard [1]. The right-hand side road boundary (28) is imposed as a constraint. For the used objective function, the left-hand side road boundary is not included in the optimisation formulation (Y tb (X(t)) = +∞) since the lane-deviation penalty itself acts as a soft constraint in that respect and an explicit constraint is thus not needed. The vehicle model (25)-(26) is also part of the constraints to be fulfilled.

Implementation and initialisation
The vehicle model described by the semi-explicit index-1 DAE in (25)-(26) is implemented using the declarative modelling language Modelica [14]. Numerical optimisation of systems described by DAEs is a well-established technology [15,16] and has been applied for optimal vehicle-dynamics control in extensive previous research (see, e.g. [2,17]). Numerical solutions to optimisation problem (30)-(31) are in this paper computed using the platform JModelica.org [15] and the IPOPT software package for nonlinear optimisation [18], together with the MA57 linear solver [19]. Direct simultaneous collocation [20] was used for transforming infinite-dimensional optimisation problem (30)-(31) to a finitedimensional nonlinear program to be solved by IPOPT. Further details on the model implementation and the optimisation methodology are available in [2]. Considering that the optimisation problem to solve is non-convex and that numerical iterative methods are used to search for a minimum, an initialisation of the trajectories for the model variables is beneficial for convergence of the optimisation. To solve problem (30)-(31) with the default parameters (Table 2), the optimisation solver in JModelica.org is provided with a dynamically feasible initial trajectory avoiding the obstacle (however, suboptimal and not fulfilling all initial and final constraints), obtained by simulation of the same model as used in the optimisation. When solving optimisation problem (30)-(31) for other initial velocities, the solution for the default initial velocity is used for initialisation.

Manoeuvres with the baseline LDP
Results obtained from optimisation using baseline formulation (30)-(31) for different initial velocities of the vehicle are shown with the dashed lines in Figure 6. The manoeuvres in Figure 6 as well as in further figures in the paper are shown as functions of X rather than the time to make comparisons between manoeuvres easier. Also, the Y-axis is scaled up relative to the X-axis in Figure 6 and in the following figures to emphasise differences in the lateral behaviour of the vehicle. The vehicle is fast to return to the original lane after having avoided the obstacle and thus minimising the risk of a potential collision with an oncoming vehicle. However, considering the oscillation in lateral position in the recovery part of the manoeuvre (after the return to its own lane) there is room for improvements, which is addressed in Section 5.

Extension of the optimisation formulation for recovery behaviour
This section presents extensions of problem formulation (30)-(31) for the baseline LDP in Section 4.1. The focus of the extensions is on improving the vehicle behaviour in the recovery part of the manoeuvre after the obstacle, while keeping the good avoidance performance in the first part of the manoeuvre.

Velocity recovery
The manoeuvres for the baseline formulation in Figure 6 are obtained under a non-positive wheel-torque condition. That condition results in the decrease of the vehicle velocity during the avoidance part of the manoeuvre and also results in a noticeably lower vehicle velocity in the second part of the manoeuvre. To promote that the velocity increases to the original velocity again after the obstacle has been avoided, the maximum allowed rear wheel torquesT 3,max andT 4,max in (31) are modified as follows. By defining a smooth switching function at point X 1 using (27) as the maximum rear wheel torques in (31) are re-defined in the following way: where T r,max is defined as in [2] (see Table 1, Panel A). The switching point X 1 is set to 3.5 m after the end of the obstacle X od . A projection of H X 1 is shown with the dashed thick line in Figure 5. This modification allows the vehicle to apply a driving torque for the rear-driven wheels when the obstacle is avoided and the vehicle is almost back to the original lane.
To regulate the vehicle velocity during the recovery part of the manoeuvre, the objective function in the optimisation is modified to include a small penalty on the vehicle velocity deviation from the reference velocity v ref (equal to v 0 in the considered case) multiplied by the switching function H X 1 according to where p v is a weighting factor. Results for velocity-recovery formulation (34) with p v = 0.2 are shown with the dotted lines in Figure 6. As desired, the vehicle is quick to return to the original velocity after the obstacle, while having a similar path and velocity for the obstacle-avoidance part of the manoeuvre compared to the results for the baseline formulation. However, a number of undesirable characteristics are observed in the results for the velocity-recovery formulation. For instance, the manoeuvre for v 0 = 70 km/h has an unsatisfactory path during the stabilising part of the manoeuvre after the obstacle, showing an oscillatory behaviour in the second part of the manoeuvre, and the manoeuvre for v 0 = 50 km/h has a positive driving torque for the rear wheels and a significant braking torque on the front wheels. Also, for the lower initial velocity, the vehicle tends to the right-most side of its own driving lane after the obstacle is passed. The following subsections present modifications to the optimisation formulation to address these observations.

Driving in the middle of the lane
When the vehicle returns to its own lane, LDP function (29) gives very small penalty values. However, the derivative of the function is not equal to zero, with a function-value decrease in the direction towards Y = 0 m. By tightening the optimisation tolerance, manoeuvres are obtained where the vehicle position after the obstacle tends to be very close to the right-hand side road boundary, which is visible in Figure 6(a) for both the baseline and the velocity-recovery formulations where the lateral position is close to zero for X > 80 m.
A way to promote the vehicle to drive in the middle of its own lane after the obstacle is to add an extra term to the objective function, where H Y 1 is defined as for the function in (29), but for a different switching point Y 1 instead of Y o . The term is added in such a way that the minimum of when the vehicle has passed the obstacle (i.e. in the region where H X 1 ≈ 1) is located in the middle of the original driving lane of the vehicle. The desired property is approximately achieved when the offset Y 1 is equal to half of the negative value of the vehicle width, which is Y 1 = −0.9 m for the considered case. A projection of the modified penalty function is shown with the thick vertical curve in the middle of Figure 5.

Force distribution
It was observed for the velocity-recovery formulation in Figure 6 that the vehicle applies braking torques for the front wheels and driving torques for the rear wheels even when driving straight with constant velocity after the obstacle. The reason for this is that the optimisation criterion so far, in the situation of constant speed, specifies the sum of the driving forces, however, not the individual actuator quantities. This inherent property of the model typically has no effect in optimisation formulations with braking only or for minimum-time formulations with no speed reference, since it is beneficial in these cases to apply the full driving force while driving straight. When a speed reference (as in (34)) is included, an analysis of all redundancies between actuators and vehicle motion is needed. Such an analysis is given in Section 6, and the result in the present case is that regularisation penalties on the inputs (wheel torques and the steering angle) solve the problem of force distribution when the vehicle is after the obstacle. The regularisation is, therefore, applied after the obstacle (X > X 1 ) using the weighting factors p T and p δ as

Preference for straight driving
The recovery performance for manoeuvres with higher initial velocities is observed not to be satisfactory (observe the results for the velocity-recovery formulation in Figure 6(b) for the XY position, where the vehicle is oscillating along the lateral position and not driving straight). The recovery performance is improved by introducing a small constant penalty γ > 0 in the objective function after the obstacle as When integrated into the objective function in the optimisation, the constant γ acts as a penalty on the total time of the manoeuvre after the obstacle, i.e. it is a minimum-time penalty. The penalty in (34) has its optimum when the vehicle keeps the reference velocity v ref . However, many paths are possible with constant speed, to go straight or to alternatively turn left and right. Of these possible paths, of course, the straight path is the one with the shortest duration, i.e. it satisfies a minimum-time criterion, so the penalty γ straightens and stabilises the vehicle motion.

Complete penalty function for improved recovery behaviour
Combining the penalty factors presented in the preceding subsections, the following complete objective function for the extended LDP formulation for the double lane-change problem is obtained where the same penalty function H Y o as in (30) is acting during the first part of the manoeuvre (X ≤ X 1 ), while in the second part of the manoeuvre the additional penalty factors are acting to improve the recovery-behaviour performance. For interpretation of the additional terms in objective function (39), they can be grouped into three parts acting during the recovery phase. The term f 1 is forming the left-hand and the right-hand sides, respectively, of the penalty function implicitly defining the driving lane of the vehicle. This term promotes driving in the middle of the lane. The term f 2 favours driving with a velocity close to the reference velocity in a straight motion and the term f 3 includes the penalties on the vehicle inputs that make the distribution between the different actuators unique (see the analysis in Section 6). Figure 6 shows with the dash-dotted lines manoeuvres obtained by optimisation using formulation (39) with the following parameters: Y o = 2.3 m, Y 1 = −0.9 m, p v = 0.2, p T = 2 · 10 −11 , p δ = 0.25, γ = 0.25, and X 1 = X od + 3.5 = 40 m. The vehicle is still quick to return to the original lane after passing the obstacle, and in addition recovers and keeps the original velocity in the second part of the manoeuvre, and it does not apply counteracting driving or braking forces while driving straight forward with constant velocity.

Analysis of actuation distribution
As pointed out in Section 5.3, it is necessary to resolve the distribution of actuation. Otherwise, as clearly illustrated in Figure 6, the optimiser will determine the optimal motion of the vehicle and thus the total force and moment, but at a constant speed, the front wheels may brake and the rear wheels drive equally much such that the sum of forces is zero. This section provides an insight into the character of the redundancies to motivate and guide the design of the extension of the recovery criterion with the additional terms (37) in the extended objective function (39). To this purpose, it is assumed that (Assumption 1) a trajectory is given. Then the trajectory gives the high-level forces F X , F Y , and M Z , and it includes δ and the velocities of the chassis and thus also determines the lateral slip angles α i , i ∈ {1, 2, 3, 4}. Assuming further (Assumption 2) that the vehicle is operated in a slip region where a bijective mapping between the slip and the tyre forces exists, the tyre forces can be considered as the independent variables.
To get a complete picture, the Jacobian of the motion equations is analysed, first in Sections 6.1-6.4 using the friction ellipse to gain insight, and then in Section 6.5 using a complete tyre model.

Friction ellipse for the theoretical analysis
The model of the tyre forces is an approximation of the actual tyre-road interaction characteristics. In the first analysis in this section, a simplified version of the model is adopted, namely the friction ellipse [21]. Consider the friction ellipse in Figure 7, where F y is plotted as a function of F x for a given value of the lateral slip angle α. From the friction ellipse, it can be seen that at every point along the ellipse, the following relation between the derivative of the forces holds dF where k α is the derivative at that point for a given lateral slip angle α.

Jacobian for the theoretical analysis
To analyse the vehicle-dynamics model, its Jacobian J is considered. In the model equations (1)- (6), there are eight wheel-force components (F x,i and F y,i , i ∈ {1, 2, 3, 4}). However, from the friction ellipse and Assumptions 1 and 2, it follows that all F y,i can be expressed as functions of F x,i . Thus, there are five input variables (four wheel torques and the steering angle), and therefore, the Jacobian is studied as a function of the five variables F x,i , i ∈ {1, 2, 3, 4}, and δ. It is of interest to study possible solutions in the null space of J, since such solutions satisfying the same global forces can imply that the optimisation solver moves arbitrarily between these during the iterations, sometimes without convergence or with convergence to counteracting forces. The partial derivatives of the model equations (4)-(6) with respect to F x,i and δ are, therefore, computed to form the Jacobian where The Jacobian is a function of five variables and there are three motion equations, so it is a 3 × 5 matrix. The matrix cannot have a rank higher than the number of rows, i.e. not more than three, and therefore, it corresponds to an underdetermined set of equations, and it is thus natural that there are redundancies between the five actuators. However, the rank of the Jacobian may be lower than three, and it is highly interesting to observe that this can be the case. Before continuing the analysis of the full Jacobian J in Section 6.4, it is advantageous to first investigate the single-track model in the next section.

Singularity analysis of a single-track model
Analysis of a single-track model will provide valuable insight into the structure of the Jacobian at least in two different ways. It shows some algebraic manipulations involved. More importantly, even if the single-track Jacobian is a 3 × 3 matrix, it will be shown to have singularities. This shows that the question of redundancy, or singularities, is deeply rooted and is already intrinsic in the single-track model. The model equations are the standard equations for single track corresponding to (1)-(6) (see, e.g. [21]) To determine singularities, the determinant of the Jacobian J ST in (50) is computed. By multiplying the second row with l f and subtracting the result from the third row, it zeros the elements (3, 1) and (3,3). Thereafter, the determinant is computed using the minor expansion formula starting from the element (3,2). After simplifications, several terms cancel each other, and for others, the Pythagorean trigonometric identity can be used. The result is It can now be concluded that the determinant is zero when k α,r = 0 or when F x,f + k α,f F y,f = 0. For the insights valuable in the analysis of the full Jacobian (41), only the case k α,r = 0 is needed. A more complete analysis of both factors can be found in [22]. The factor k α,r in (51) is zero in the cases when F x,r = 0 (implying being on the top or bottom of the friction ellipse and k α,r is, therefore, zero) or when driving straightforward with no lateral slip (F x,r arbitrary, but α r = 0 and δ = 0). In the latter case, the solutions in the null space of J ST need to satisfy where the variables denoted by represent the local deviations from the state in the actual trajectory under study. The second equation is trivially satisfied when δ = 0. The first equation implies that the driving forces can be distributed arbitrarily between the front and the rear wheels. This observation could as well be related to the results discussed in Section 5.3, where an unfavourable torque distribution between the front (T 1 and T 2 ) and the rear (T 3 and T 4 ) wheels is pointed out in Figure 6(a) (see in particular the torques shown with the dotted lines after the obstacle). Referring back to the model equations (47)-(49), it also follows directly that and it is clear that there is a non-uniqueness in the distribution of longitudinal force-i.e. different combinations of longitudinal force give the same resulting global vehicle force F X .

Analysis of the full model Jacobian
Now return to the 3 × 5 Jacobian in (41). The null space has dimension at least two. One way of finding any other redundancies is to form ten different 3 × 3 submatrices of J. The requirement for loss of rank is that the determinants of these ten submatrices (also called minors) all are zero. Thus, let |J [i,j,k] | denote the minor where columns i, j, k have been selected for the submatrix. These minors will also be used in the evaluation in Section 6.5. Computing all minors involves the same type of algebraic manipulations as for |J ST | in (50), but the expressions are lengthy so it is preferably done with the assistance of computer algebra, e.g. Mathematica [23]. Consider first the general case with no further assumptions on the motion. For the single-track model, there was a singularity when k α,r = 0. Here, there are four k α,i , i ∈ {1, 2, 3, 4}, but ten equations that need to be zero, which means that the system of equations might be overdetermined without a solution. Nevertheless, there are, in fact, values k α,i that result in a loss of rank, and for these values the rank of the Jacobian is two, and not less. This means that the actuation distribution is determined by two equations in this case. The full expressions for k α,i are lengthy and this also holds for the two remaining equations, so to get insight into the two governing equations their interpretation in a special case of motion is studied.
Consider the special case of δ = 0 with no side slip on the wheels, k α,i = 0, i ∈ {1, 2, 3, 4} (this is related to k α,r = 0 in the single-track case). This motion corresponds to the desired condition after recovery. Then the Jacobian (41) becomes ⎛ This corresponds to the following equations The similarity with (52)-(53) is obvious. Here, the second equation gives δ = 0. Inserting that into the first and third equation results in Hence, it is seen that the remaining governing equations are the equations for total force and total moment. This is rather helpful since it shows that the solution moves in two hyperplanes, and thus, the penalties introduced in (37) resolve the non-uniqueness, and it does so in an advantageous way. Note that the analysis so far provides key insights, but it is based on the simplifying assumption of the friction ellipse. In the next section, the results are verified for the complete Pacejka tyre model with weighting functions.

Analysis for complete tyre model
In the analysis performed in the previous subsections, it was assumed that the tyres are operating on the friction ellipse for a particular lateral slip angle. However, during the transition from one lateral slip angle to another (i.e. from one friction ellipse to another), zero crossings where the Jacobian loses rank might occur. Therefore, a more complete analysis based on Pacejka's Magic Formula (17) For analysis of the Jacobian given the states of the vehicle along a certain manoeuvre, the derivatives k α,i are needed. From the definition of these derivatives in (40) and using (61)-(62), k p α,i at a certain discretisation point p is where α p i is given (from the trajectory), and the function representing the forces thus only varies with κ i , i.e. only varies with the longitudinal force applied to the wheel. The functions g x,i and g y,i are nonlinear functions of the longitudinal slip ratio κ i and the lateral slip angle α i (see (61)-(62)). Their derivatives with respect to the slip quantities are possible to compute analytically, but an elegant and computationally efficient alternative is to apply automatic differentiation (AD) [24], available, e.g. in CasADi [25]. This software tool is used to obtain k α,i in the further presented results.
To study vicinity to singularities, i.e. vicinity to losing rank of the Jacobian, during a manoeuvre, the minors |J [i,j,k] | introduced in Section 6.4 can be used. They are all zero in a singularity, and if they all approach zero it indicates approaching a singularity. Driving straightforward, Equation (57) gives δ = 0. With δ = 0 the influence from the fifth column of the Jacobian J is zeroed, and it is natural to study the first four columns of Jacobian (41), i.e. to study the four minors |J [1,2,3] |, |J [1,2,4] |, |J [1,3,4] |, and |J [2,3,4] |. These minors are dependent on the steering angle δ and the four derivatives k α,i , i ∈ {1, 2, 3, 4}. Figure 8 shows the path and the steering angle for an avoidance manoeuvre with the initial velocity v 0 = 50 km/h, using extended formulation (39) for improved recovery behaviour. Figure 8 also includes numerical results for the minors |J [1,2,3] |, |J [1,2,4] |, |J [1,3,4] |, |J [2,3,4] | and the derivatives k α,i calculated at each discretisation point of the trajectory. The steering angle δ, the derivatives k α,i , and consequently, the minors tend to zero for the last 40 m of the manoeuvre. That part of the manoeuvre corresponds to where the vehicle applies combined counteracting driving and braking torque when using only the velocityrecovery formulation in (34) (see the dotted lines in Figure 6), but it behaves well with the recovery terms (37) in place (see the dashed lines in Figure 6). In the first part of the manoeuvre, the minors cross zero when delta passes zero at about 4, 20, and 37 m, but these singularity passages do not cause ambiguity in the force distribution.
The singularity results presented in Figure 8 motivate the need to resolve the ambiguities related to the force distribution between the available actuators, as was done by the inclusion of additional regularisation terms (37) with penalties on the inputs in extended formulation (39). If not handled, the optimisation will typically oscillate between different equivalent solutions, or converge to a solution with unnecessary counteraction of the actuators.

Demonstration of performance for other evasive manoeuvres
The analysis in the previous section motivated the need for the additional recovery terms in extended LDP formulation (39). However, it should be noted that the analysis is general, i.e. it does not rely on LDP or any other specific formulation. Since redundancy is an inherent property of the force distribution to the actuators, it can be concluded that the additional recovery terms should be beneficial for other evasive formulations as well, and this is in fact demonstrated in this section for other formulations of evasive manoeuvres. More specifically, baseline formulations for a minimum-time objective function (see, e.g. [2]) and for a squared lateral-error norm are considered, and then they are extended with recovery behaviour.

Minimum-time formulation
The baseline minimum-time formulation is obtained as in [11] by modifying general optimisation problem (30)-(31). First, objective function (30) is replaced by where p t is a constant time-penalty parameter. Further, to promote the return of the vehicle to the original lane after the obstacle, the constraint Y tb (X(t)) for the left-hand side of the vehicle in (31) is defined, as in [11], by where Y lw is the adjusted lane width and X otu , X otd are the points where the top boundary goes up and down, respectively. The values of these parameters used in the optimisation are 1.4, 12, and 47 m, respectively. Path and selected vehicle variables for an example trajectory with the initial velocity v 0 = 70 km/h and p t = 1 are shown with the dashed lines in Figure 9. As in the baseline LDP formulation (Section 4), non-positive torque inputs T i , i ∈ {1, 2, 3, 4}, result in an undesired velocity decrease over the manoeuvre.
To allow velocity recovery after the obstacle,T 3,max andT 4,max in (31) are updated according to (33). Since a minimum-time solution implies acceleration to maximum velocity, a reference velocity is introduced as a constraint by v ≤ v ref .
(66) Figure 9 shows an example trajectory for this velocity-recovery formulation in the dotted line. As desired, the velocity of the vehicle increases to the reference velocity after the obstacle. However, the torque inputs (dotted lines) have undesirable characteristics with counteracting forces on the front and rear wheels similar to those discussed in Section 5.3. Following the steps in Section 5, the velocity-recovery minimum-time formulation is extended with the recovery terms in the following way This final formulation is called extended minimum-time formulation. The values of the scaling parameters p T and p δ are the same as for the extended LDP formulation in Section 5, and p t is adjusted to 1/9 to roughly balance its influence in (67). An example trajectory for this formulation is shown in Figure 9 with the dash-dotted line. For the extended formulation, the velocity of the vehicle is returning to the reference velocity as in the velocity-recovery formulation. In addition, note that the torque distribution for the extended formulation is now favourable.
When comparing trajectories in Figure 9, it is interesting to note that the vehicle path is almost identical between different formulations for X between 0 and 50 m. The shown vehicle variables are very similar between the different formulations for X in the range 0 to 40 m, and similar results are obtained when comparing trajectories for other initial velocities in the range v 0 = 50 to 80 km/h (not shown in plots here). This makes it possible to conclude that the extended formulation, as desired, has a small influence on the avoidance part and improves the recovery part of the manoeuvre.

Squared lateral-error norm formulation
An alternative approach to formulate the objective function of optimal control problem (30)-(31) is to consider an integral squared-error norm of lateral displacement. Such a norm is used in [6] for the recovery part of the double lane-change manoeuvre. Here, it is applied for the full manoeuvre. The baseline formulation is obtained by replacing (30) with Then, the velocity-recovery formulation is obtained by allowing positive torque after the obstacle (i.e. adoption of (33)) and including the velocity penalty in the objective function in the following way: where p i = 1/7 is a scaling parameter chosen to roughly balance the influence of the first term. As seen in the dotted line in Figure 10, the torque distribution between the front and the rear wheels (compare T 1 and T 2 with T 3 and T 4 for the last 20 m) is not satisfactory. Continuing with the steps addressing the recovery behaviour, the extended formulation is defined by augmenting objective function (69) with the recovery formulation as (70) Compared to extended LDP objective function (39), the penalty (Y 0 − Y) 2 favours driving in the middle of the lane, so the term H X 1 (1 − H Y 1 ) after the obstacle is not needed here.
Comparing the results in Figure 10 for the different cases with the initial velocity v 0 = 70 km/h (as well as considering results for other initial velocities in the range 50-80 km/h not shown in plots here), it is noted that the path and the velocity profile are very similar for the interval X = 0 to 40 m. For the extended formulation, the vehicle velocity returns to the reference velocity with a favourable torque distribution and reduced overshoot in the vehicle lateral position after the obstacle. As for the extended minimum-time formulation, it is thus concluded that the extended squared lateral-error norm formulation results in improved recovery behaviour with a limited influence on the avoidance part when compared with the baseline formulation.

Generality of recovery behaviour
The recovery behaviour in (39) is defined via the terms f 2 and f 3 to complement the main penalty function (term f 1 ) that focuses on the avoidance part. The objective function is designed to result in a swift return of the vehicle to the original driving mode when the obstacle has been passed. This is achieved by including the terms to specify the desired velocity and stabilise the motion (f 2 ) and to resolve ambiguities in the force allocation to the actuators (f 3 ). The terms defining the recovery behaviour are clearly beneficial also in other formulations of the term f 1 as shown in the previous subsections for the minimumtime formulation with upper velocity limit (64)-(66) and for squared lateral-error norm formulation (68). These results illustrate the generality of the devised terms to improve recovery behaviour for different nominal objective functions.

Conclusions
Avoidance manoeuvres, like the moose test, are important components in an autonomous car to minimise passenger risk. An avoidance manoeuvre is typically composed of an evasive phase avoiding an obstacle followed by a recovery phase returning to its own lane. The fundamental takeaways from the present paper are the analysis of different aspects of the recovery phase and the subsequent demonstration that several considerations are needed to design a recovery phase fulfilling the desired objectives. A formulation was developed in several steps based on theory and simulation. One simple observation is that driving straight with constant speed defines the total force on the vehicle, but there are still several force distributions possible between front and rear wheels, which, of course, makes convergence impossible for the optimiser if it is not handled. The overall result from this and other considerations is the introduction of additional penalty factors in the objective function. It has been demonstrated that the new recovery formulation results in efficient and stable complete manoeuvres for three different criteria for evasive manoeuvres: lane-deviation penalty, minimum time, and squared lateral-error norm, which also demonstrates the generality of the approach.

Disclosure statement
No potential conflict of interest was reported by the author(s).