Traction Adaptive Motion Planning at the Limits of Handling

In this paper, we address the problem of motion planning and control at the limits of handling, under locally varying traction conditions. We propose a novel solution method where traction variations over the prediction horizon are represented by time-varying tire force constraints, derived from a predictive friction estimate. A constrained finite time optimal control problem is solved in a receding horizon fashion, imposing these time-varying constraints. Furthermore, our method features an integrated sampling augmentation procedure that addresses the problems of infeasibility and sensitivity to local minima that arise at abrupt constraint alterations, e.g., due to sudden friction changes. We validate the proposed algorithm on a Volvo FH16 heavy-duty vehicle, in a range of critical scenarios. Experimental results indicate that traction adaptive motion planning and control improves the vehicle's capacity to avoid accidents, both when adapting to low local traction, by ensuring dynamic feasibility of the planned motion, and when adapting to high local traction, by realizing high traction utilization.


Introduction
Automated driving and advanced driver assistance technology show increasing potential to improve safety and mobility of transportation systems in the future. A major challenge in driving automation is handling of critical situations, i.e., situations that appear suddenly, in which an accident is imminent. Critical situations may originate from multiple sources, e.g., unpredictable behavior of other traffic agents or rapid changes in the operational conditions. The steady progress in sensors, perception and prediction algorithms indicate that a subset of critical situations can be anticipated and avoided ahead of time. However, considering the inherent unpredictability of humans in the traffic environment and the wide range of operational conditions, it is unlikely that all critical situations can be anticipated and avoided ahead of time, without sacrificing efficiency and availability by enforcing overly cautious behavior. When a critical situation does occur, passenger comfort is no longer a priority, and if necessary, we wish to utilize the full physical capability of the vehicle to  Entering the curve at 15m/s, the vehicle comfortably negotiates the corner when the road is dry, but slides out into the opposing lane when the road is wet, causing a hazardous situation.
avoid the imminent accident. This motivates research in the field of motion planning and control at the limits of handling. Previous research efforts present methods for which performance approaches the physical limits [1,2]. However, the motion capability of a road vehicle is greatly affected by locally varying traction conditions of the road. This phenomenon, that can be intuitively understood by the simple experiment depicted in Fig. 1, has been well studied and is included in standard vehicle dynamics literature [3,4], but has only recently been considered in the context of motion planning at the limits of handling [5].
The work presented in this paper represents a further step towards traction adaptive motion planning and control at the limits of handling. We propose a novel optimization based framework, in which the locally varying traction limit is represented as time-varying constraints on tire forces. The constraints are updated online as a function of the estimated friction coefficient and dynamic tire loads. Furthermore, we employ sampling augmentation to address the non-convexity and infeasibility issues of the arising optimization problem. We evaluate the proposed method in comparison with an equivalent non-adaptive scheme, in a sequence of experiments on various road surfaces.
The paper presents an extended and refined version of the method that was originally introduced in [6]. The extension includes refinement of the mathematical framework, a more accurate model of the vehicle and its traction limitations, a novel, GPU accelerated method to generate feasible sample trajectories, and an extensive experimental evaluation.
We summarize the contributions of the paper as follows: 1. A real-time capable algorithm for traction adaptive motion planning and control that produces optimal solutions with respect to time-varying tire force constraints.
2. An experimental evaluation of traction adaptive motion planning and control, showing that the concept improves capacity to avoid accidents in a range of critical scenarios, when adapting to low as well as high local traction.

Motivation
Critical scenarios at the limits of handling put the following high level requirements on the local motion planning functionality: 1. A relatively complex vehicle model is required to accurately describe the vehicle in aggressive motions.
2. A sufficiently long planning horizon is required to the handle the inertia and limited actuation capacity of the vehicle. 3. A short planning time is required in order to react promptly to a dynamically changing traffic scene 4. The planner has to be able to make discrete decisions, e.g., to go left or right of an obstacle 5. Local traction variations should be considered.
Requirements 1) -4) have been been identified in previous academic works with state of the art approaches presented in Section 3. Next, we motivate why 5) should also be considered in this context. From basic mechanics we have that for a single tire, the combined longitudinal, F x , and lateral, F y , force that can be exerted between tire and road is upper bounded by the friction coefficient µ times the normal force acting on the tire, F z , i.e., This limit forms a circle in the lateral-longitudinal plane and is therefore commonly referred to as the friction circle 1 . Both µ and F z typically vary substantially with time. The friction coefficient varies with, e.g., road surface, tire temperature and wear, and the normal force varies as a result of road inclination and dynamic forces caused by vehicle motion. Fig. 2 highlights the impact of this phenomenon in terms of motion planning near the physical limits. The figure visualizes to what extent the force limits of the front and rear tires vary under different operational conditions and motion cases.
The phenomenon presents an additional challenge to motion planning in critical situations. For example, not considering it and assuming static bounds on tire forces will have one of two consequences when planning aggressive evasive maneuvers 1. Planning of infeasible motions. The real vehicle will be unable to track the plan.
2. Reduced capability to avoid accident. The planner will not be able to fully utilize the available traction to avoid an accident.
Both 1) and 2) reduce the vehicle's capacity to avoid accidents in critical situations. This conclusion motivates us to pursue the development of algorithms for motion planning and control at the limits of handling, with a time-varying traction boundary. We refer to this concept as traction adaptive motion planning and control. Next, we give a brief summary of relevant academic works in related fields.

Related Work
Motion planning algorithms for automated vehicles can be roughly classified into random sampling based, graph search based, trajectory roll-out and optimization based [7,8]. Considering the special requirements in critical scenarios stated in Section 2, there is an evident computational trade-off between properties 1), 2) and 3), and therefore, efficient solving of the motion planning problem is a key challenge. Previous studies, e.g., [1,2] show that optimization based methods are well suited to handle such a computational trade-off, but have less favorable properties for requirement 4), compared to the other three classes. Random sampling based and graph search based algorithms are primarily designed to produce complex maneuver sequences [7], which is typically not required for short-term evasive maneuvers in critical situations. Also, such algorithms are inherently sequential, restricting efficient implementation through parallel computing. Therefore, we delimit our related work section to optimization based and trajectory rollout algorithms, as well as a few relevant concepts from the fields of tire-road friction estimation and adaptive control.

Optimization Based Methods
Optimization based methods dominate the field of motion planning at the limits of handling. Compared to other methods, they are not restricted to a discretized search space, scale comparatively well with model complexity [9], and allow explicit representation of constraints on states and inputs.
The key concept of optimization based motion planning is to solve a Constrained Finite Time Optimal Control Problem (CFTOC) at each planning iteration, where the solution is the control and state sequence that obeys the constraints and minimizes a cost function over a fixed time horizon. In general, the CFTOC representing a collision avoidance motion planning problem is nonconvex, due to the nonlinear vehicle dynamics and nonconvex state constraints representing obstacles [10]. The nonconvexity of the problem renders the problem nontrivial to solve under the time requirements associated with critical evasive maneuvers. However, there exist several methods such as quasi-Newton algorithms with line search or Sequential Quadratic Programming (SQP) that are capable of solving the problem given a near-optimal initial guess of the optimal trajectory [11][12][13]. SQP is an iterative approach in which a quadratic, and hence convex, approximation of the problem is formed and solved at each iteration. The approximation is obtained by performing a second order approximation of the cost function and a linearization of dynamics and constraints at the solution from the previous iteration. Thus, a Quadratic Program (QP) is solved at each iteration. Diehl et al. proposed selecting the solution trajectory from the previous planning iteration as the initial guess, and perform a single SQP iteration per planning iteration [14]. This approach, called Real Time Iteration (RTI), is exceptionally computationally efficient and has been proven successful in numerous applications, including motion planning for automated vehicles [1,9,15].
Ziegler et al. highlighted a drawback of the RTI approach in [9], noticing a sensitivity to local minima in situations where the motion planning problem contains discrete decision making. The intuition behind the phenomenon is that at each iteration the optimizer searches for the best solution locally around the initial guess, and therefore it is unlikely to find a solution in a different part of the state space, that may contain the globally optimal solution. Liniger et al. [1] identify the same issue and mitigate it by means of a high level path planner based on dynamic programming, a solution that works well in the particular application, but not for the general case.

Trajectory Roll-Out Methods
The trajectory roll-out concept represents a radically different approach to solving the motion planning problem. A large set of candidate trajectories are computed followed by a feasibility check and cost function evaluation that determines the optimal feasible candidate within the set [16][17][18]. Since the candidate trajectories are distributed across the drivable area, the method is less sensitive to local minima and thus performs well in situations where the motion planning problem contains discrete decisions.
Computing the individual trajectories connecting the initial state to a sampled goal state means solving a nonlinear optimal control problem. This is computationally intractable for large numbers of samples. However, for particular classes of trajectory candidates, e.g., quintic and quartic polynomials, these problems have closed form solutions [18]. While such a solution makes trajectory roll-out computationally tractable, the resulting reference trajectories are not suitable for maneuvers at the limits of handling, due to the inaccurate representation of vehicle dynamics and limitations.

Adaptive Control
To account for local variations in physical capabilities of the vehicle, we draw from developments in the field of adaptive control. Predictive control under model uncertainty is a well-studied topic [19][20][21]. Such frameworks allow the system to dynamically re-plan safer and more cost efficient trajectories with time, as model parameters get updated.
These and related concepts are increasingly being adopted to account for traction variations in motion planning and control of automated vehicles. For example, Alsterda et al. [5] present a novel approach in which a contingency plan, based on an alternative model parameter set, is computed in parallel to the nominal plan. As such, the method maintains a dynamically feasible contingency plan at all times, even in the event of, e.g., a rapid friction reduction. In this work, we instead leverage the notion of adaptive control by updating model parameters based on a predictive friction estimate.

Predictive Tire-Road Friction Estimation
There exists a large body of research on the topic of tire road friction estimation from on-board vehicle measurements [22,23]. Such estimates have previously been shown to improve performance in several vehicle control applications, e.g., automatic emergency braking [24,25] and path tracking control at deteriorated traction [26,27]. With the recent developments in computer vision and machine learning, methods for predictive friction estimation using camera images have emerged [28][29][30][31][32]. A predictive friction estimate enables motion planning according to the traction conditions ahead of the vehicle. This could also be obtained using a classical method such as, [22,23], under the assumption that the traction conditions are unchanged for a short distance ahead of the vehicle.
To perform motion planning and control with respect to such predictive friction estimates, the planner must reliably handle situations where the friction estimate varies over the prediction horizon, or changes between subsequent planning iterations.

Summary
To summarize, optimization based methods (RTI in particular) provide good performance in handling the trade-off between requirements 1), 2) and 3) in Section 2, but struggle with 4). Trajectory roll-out methods on the other hand, inherently handle 4), but have much lower performance in the trade-off between 1), 2) and 3). The method proposed in this work combines RTI, trajectory rollout and adaptive control to collectively address requirements 1) through 5). We leverage the notion of adaptive control by utilizing updated model information derived from a predictive friction estimate 2 , to account for time-varying traction limitations in local motion planning, including discrete decisions.

Problem Formulation
Our goal in algorithm design is to develop a method that efficiently computes dynamically feasible reference trajectories at a high degree of traction utilization on varying road surfaces, based on a predictive friction estimate. Furthermore, the algorithm must handle cases where the predictive friction estimate changes over the course of the planning horizon and/or changes abruptly between planning iterations. In this section we introduce the mathematical details of the considered motion planning problem and state the challenges it presents in further detail.

Planning Model
We derive a continuous time vehicle model with time varying parameters, shown in Appendix B, and denote it f c (·, ·). We let t c denote continuous time. For controller design, we obtain a discrete time version of this model as: with time index t, time step T s and the state and input vectors Remark 1 We avoid separate notations for "nominal" states from (2) and the true states from the vehicle for the sake of notational simplicity in the remainder of the paper. This can be done as we do not opt for a robust control design approach, such as [33][34][35].
State variables s, d and ∆ψ represent the progress along, the lateral deviation from and relative orientation to the lane center. Variablesψ, v x and v y denote yaw angular velocity, longitudinal and lateral velocity of the vehicle. We employ the single track modelling technique [4], in which the combined influence of two wheels on an axle is represented by a single tire in the middle of the axle, and select as control inputs: the lateral force on the front tire, F yf , the longitudinal force on the front tire, F xf , and the longitudinal force on the rear tire F xr .

Remark 2
The primary reason for selecting tire force control inputs is that it allows us to explicitly model the locally varying traction limit as a time-varying constraint on the inputs. Furthermore, it contributes to the generality of the solution, since it can be instantiated on a wide range of vehicle types by setting a small number of easily measured parameters. A drawback compared to selecting steering angle as the control input for the lateral dimension is that it is non-trivial to represent actuator limitations such as maximum steering angle. However, for the speed range associated with the type of maneuvers we are targeting (v x > 5m/s) this problem does not occur. In order to translate the resulting tire force commands into the vehicle's actual control inputs, a vehicle specific control interface is required. The control interface developed for our test vehicle is described in Appendix C.
In addition to the dynamics in the xy-plane, the model includes relations to determine the front and rear normal forces F zf and F zr from the state x t . This, together with a predictive friction estimate, allows us to obtain a time-varying traction force boundary through (1). Further details on the model are provided in Appendix B.

Optimal Control Problem
In optimization based motion planning, a Constrained Finite Time Optimal Control Problem (CFTOC) is solved at regular time intervals, producing a new motion plan at each planning iteration as its solution. For the motion planning problem with traction limit variation we consider the CFTOC where the variables {u 0|t , u 1|t , . . . , u N −1|t } denote a predicted input sequence at time step t, which, when applied through the vehicle model f (·, ·), yields the corresponding predicted state sequence {x 0|t , x 1|t , . . . , x N |t }. The closed-loop state is reset again when x t+1 is measured. We denote a predicted trajectory consisting of the associated predicted state and control sequences as where Q N 0, Q 0 and R 0 are weight matrices that can be tuned by the designer. The choice here of a quadratic cost in (4) is done without loss of generality. Standard methods exists to make local quadratic approximations of non-quadratic cost functions, see Section 3.1.
In (3), the state trajectory constraints X k|t , for all k ∈ {0, 1, . . . , N } represent collision free configurations of the vehicle with respect to the road geometry, static obstacles as well as positions and predicted positions of dynamic obstacles.
Notice that the input constraints U k|t are time-varying along the prediction horizon for k ∈ {0, 1, . . . , (N − 1)}. This is the mechanism we use to adapt the planned motion to local traction limitations. The set U k|t is computed at run-time as a function of the predicted friction estimate µ k|t and normal forces F zf and F zr . A further description on how the tire force limits are obtained is } denote the optimal predicted trajectory.

Applying a State of the Art Approach: RTI
The Real Time Iteration approach [14,36], RTI for short, is a state of the art approach for solving CFTOCs, that enables extraordinary computational efficiency [37]. In the context of motion planning in critical situations, computational efficiency is essential to achieving an acceptable trade-off between properties 1) 2) and 3) of Section 2. Hence, we select RTI as a starting point for algorithm development.
The central idea of the algorithm is to use the solution from the previous planning iteration to formulate a QP approximation of the CFTOC, which can be solved efficiently. The quadratic formulation is obtained by linearizing the dynamics and expressing the constraints as linear inequalities [10]. At a single planning iteration starting at time t, a standard RTI algorithm goes through the following steps: First, the solution from the previous iteration T t−1 is shifted one step forward in time, to compensate for the time passing between planning iterations. Thus, at time step t, an initial guess is obtained as,T Next, the dynamics are linearized aboutT t . The system model matrices are obtained as for all k ∈ {0, 1, . . . , (N −1)}. State and input constraints are expressed as sets of linear inequalities: The procedure we use to linearize the input constraints of our vehicle model is described in Appendix B.
The final RTI style reformulation of (3) around an initial guessT t is shown in (5).
When a solution of (5) for time step t is obtained, the control is applied to the vehicle. At the next time step, (t + 1), problem (5) is solved again.

Challenges in Solving the RTI Problem (5)
The RTI algorithm builds on the assumption that T t−1 is a feasible and near-optimal initial guess for time step t. If this is not the case, performance of the algorithm deteriorates, leading to three essential drawbacks with respect to the problem formulation of this paper.
(i) Infeasibility w.r.t. P U k|t : In the presence of time-varying input constraints, there is no guarantee that at time step t the input sequence from, T t−1 is feasible with respect to P U k|t , i.e., the condition u k|t−1 ∈ P U k|t may or may not hold for all k ∈ {1, 2, . . . , N }.
(ii) Infeasibility w.r.t. P X k|t : Irrespective of (i), in a dynamically changing environment with sensor limitations and unpredictably moving human actors, there is no guarantee that at time step t, the state sequence of T t−1 will be collision free, i.e., the condition x k|t−1 ∈ P X k|t may or may not hold for all k ∈ {1, 2, . . . , N }.
(iii) Sensitivity to Local Minima: Due to the fact that the search for T t is only performed locally around T t−1 , RTI is prone to suboptimal decision making where the planning problem includes discrete decision making. In realistic traffic, new discrete decisions may appear dynamically [9].
Problem (i) is unique to our problem formulation, and stems from the addition of traction adaptation through time-varying input constraints P U k|t introduced in (3). A naive solution is to project u k|t−1 onto P U k|t for all k ∈ {1, 2, . . . , (N − 1)} and recomputeT t from the dynamics model (this is the first step of our proposed algorithm, see Section 5.1). However, this is not a complete solution as it, for non-trivial cases, merely transforms problem (i) into problem (ii). Problem (ii) has been identified in previous works and is unavoidable in a dynamic traffic environment. Computational feasibility can be obtained by introducing slack variables, such that state constraint violations are allowed, but with a very high cost [1,38]. Commonly however, this approach merely transforms Problem (ii) into Problem (iii). Problem (iii) in turn may be handled for specific applications by a higher level decision making layer [1]. In critical situations however, the optimal decision, for example to swerve left or right of a suddenly appearing obstacle, is highly dependent on, e.g., the geometry of the traffic scene, the vehicle's state, dynamics and dynamic limitations. This information is typically only available at the local motion planning level and therefore, an informed decision can only be made there. In such critical decision making, insufficient scene understanding may lead to suboptimal decisions regarding the selected maneuvers, which in turn reduces capacity to avoid accidents. Trajectory roll-out methods, in contrast to optimization based methods, inherently handle such decisions by construction at the local planning level, as described in Section 3.2.
In the next section, we present a novel variant of RTI, which we refer to as Sampling Augmented Adaptive RTI (SAA-RTI), to jointly address (i), (ii) and (iii).

Sampling Augmented Adaptive RTI
The SAA-RTI algorithm decomposes the solving of (3) into four steps using the same planning horizon N and time discretization step T s . Furthermore, steps C and D use the same cost function (4). The steps are: A. Ensuring input feasibility of T t−1 , B. Generating additional initial guess candidates through sampling, C. Selection of initial guess trajectory, D. Constraint adaptive trajectory optimization.
Together, steps A-D remedy the limitations (i), (ii) and (iii) of RTI introduced in Section 4.4. In the following sub-sections we explain each step in further detail.

Ensuring Input Feasibility of T t−1
Due to the time-varying input constraints, situations when u k|t−1 / ∈ P U k|t for k ∈ {1, 2, . . . , (N − 1)} may occur. To resolve this, we replace the shifting procedure in conventional RTI as: where Proj Ω (·) denotes Euclidean projection operation to the set Ω. Thus, we modify individual inputs if needed, such thatũ k|t ∈ P U k|t for k ∈ {0, 1, . . . , (N − 1)}. Then, under the new input sequence, we propagate the system forward fromx 0|t = x 1|t−1 to determine the state sequencẽ Thereby, we obtain an initial guess that satisfies dynamics and time-varying input constraints, resolving limitation (i) of Section 4. We acknowledge that the procedure may alter the state sequence such that state constraints are violated, i.e.,x k|t / ∈ P X k|t , for some k ∈ {0, 1, . . . , N }. However, as stated in Problem (ii) in Section 4.4, this problem has to be addressed regardless of this addition, due to the dynamic environment. We address the problem in the next step of the algorithm by employing the trajectory roll-out concept [16].

Generating Additional Initial Guess Candidates Through Sampling
To handle the case when the initial guess (7) violates the state constraints,x k|t / ∈ P X k|t for any k ∈ {1, 2, . . . , N }, i.e., Problem (ii), and to avoid highly suboptimal local minima, i.e., Problem (iii), we employ a further developed version of the sampling augmentation concept that was first introduced in [6].
The procedure starts by selecting N s reference states x Reference values for d and v x are uniformly distributed within admissible values, the s-reference selected as the current s of the vehicle, and remaining state references are set to zero. For each reference state, we integrate the dynamics forward, from the initial statex under a Linear Quadratic Tracking (LQT) controller, for all i ∈ {1, 2, . . . , N s }, that minimizes deviation from the reference state. At each time step k, the optimal control input is computed asû where K t is the optimal gain matrix, recomputed at each planning iteration by linearizing the dynamics around the the initial state x t and solving the Discrete-time Algebraic Riccati Equation.
The cost function weights are selected such that behavior is primarily dominated by errors in d and v x . Errors for example in s, have small influence.
To ensureû . . , N } we apply the procedure described in Section 5.1 to project all control inputs onto the feasible set before propagating the state. The N s candidate trajectoriesT for all i ∈ {1, 2, . . . , N s } are stored in the trajectory set Notice that following the computation of K t , each trajectory is computed independently, enabling efficient GPU acceleration.

Selection of Initial Guess Trajectory
To determine the best available initial guess, each candidate trajectory in the setŜ t ∪T t is evaluated based on the shared cost function J(·) in (4). The lowest cost candidate, i.e., is selected for subsequent optimization. Trajectory candidates that violate state constraints, i.e., x k|t / ∈ P X k|t for any k ∈ {1, 2, . . . , N } are disregarded in the selection. In caseT t is colliding or is in a highly suboptimal local minima, an alternative feasible and near-optimal initial guess can be obtained fromŜ t . Thus, we exploit the inherent redundancy in the trajectory roll-out concept [18], to resolve Problems (ii) and (iii) of Section 4. This property of the algorithm is experimentally verified in Section 6.4.

Remark 3
In cases when changes to the time-varying state and input constraints are small between planning iterations, it is highly probable thatT t =T t . In this case, algorithm behavior is identical to that of RTI. This is further elaborated in Section 6.4.

Constraint Adaptive Trajectory Optimization
With the initial guessT t obtained from (11), the final step of the SAA-RTI algorithm is to solve the QP min ∆u 0|t ,...,∆u N −1|t In contrast to (5), the QP is initialized atT t , i.e., with the system model matrices given as for all k ∈ {0, 1, . . . , (N − 1)}. After solving (12) at any time step t, we apply the closed-loop command to the vehicle system through the low level control interface described in Appendix C. At time step (t + 1), (12) is re-solved.

Summary and Implications for Traction Adaptation
We summarize SAA-RTI in Algorithm 1. Here, M t represents local map features, e.g., lane boundaries and static obstacles, and O t denotes dynamic obstacles at time step t. Note that we assume availability of a predictive tire-road friction estimates µ k|t for k ∈ {0, 1, . . . , (N − 1)} at any time step t. The proposed approach of modelling locally varying tire force limits as time-varying input constraints allows the vehicle to plan motions that optimally utilizes traction throughout the prediction horizon. Fig. 3, shows an example where µ drops from 0.8 to 0.3 in the middle of the horizon. Recall that t c denotes our continuous time variable. The planned motion exploits the initial high traction to reduce velocity, Fig. 3b, and position the vehicle such that the curvature of the trajectory is reduced over the low traction part of the corner, Fig. 3b. This is done in a coordinated manner such that planned tire forces, shown for the front tire in Fig. 3b, are at the (varying) limit throughout the planned maneuver, i.e., for the front tire, (F xf ) 2 k|t + (F yf ) 2 k|t = µ k|t (F zf ) k|t . The SAA-RTI algorithm ensures efficient and reliable solving of the traction adaptive motion planning problem. The sampling augmentation procedure ensures that the optimization procedure is always provided a feasible and near-optimal initial guess, even when the predictive friction Inputs: (10) 10:T t = arg min Tt∈Ŝt∪Tt (J(T t )) from (11)

11:
A k|t , B k|t ← LinearizeModel(T t , ∀k) 12: (12) 15: Apply u t|t to the vehicle 16: end while estimate changes dramatically between planning iterations. Fig. 4 shows an example where the estimate of µ has dropped from 0.8 to 0.3 between the previous and the current planning iteration. The figure shows position and orientation, Fig. 4a, and velocity states, Fig. 4b of the forward shifted and input feasibility adjusted solution from the previous iterationT t (blue), the lowest cost SAA-RTI initial guess candidateT t (orange) and the final optimized trajectory T t (green). It is evident from Fig. 4a, that even when adjusted for input feasibility, Problem (i),T t is still not feasible with respect to state constraints, Problem (ii). The addition of sampling augmentation reliably alleviates this problem by providing an abundance of additional feasible candidates and selecting the best available optionT t . In addition, since the candidates are spread across the drivable area, sampling augmentation greatly reduces sensitivity to local minima in the presence of discrete decisions, Problem (iii), which we will revisit in Section. 6.4.

Experimental Evaluation
We evaluate the performance of traction adaptive motion planning and control, realized by Algorithm 1, in terms of its capacity to avoid accidents in a set of critical scenarios. We compare the traction adaptive algorithm (abbreviated TA) with a baseline non-adaptive RTI scheme with static tire force constraints (abbreviated NA). The TA scheme uses time-varying tire force constraints computed as a function of predicted normal forces and a predictive tire-road friction estimate µ k|t = µ est (s) for all k ∈ {0, 1, . . . , (N − 1)} as per equations (16) and (17) in Appendix B with traction utilization factor λ = 0.90 for all scenarios. The NA scheme on the other hand uses static tire force constraints associated with a static friction estimate µ k|t = µ sta for all k ∈ {0, 1, . . . , (N − 1)} and a x = 0. All evaluated configurations use N = 40 and T s = 0.1s, i.e., a planning horizon length of 4s.
The critical scenarios selected for evaluation are: (i) Turn at Low Local Traction   For the evaluation in this paper, we assume that an accurate predictive friction estimate is available, i.e., µ est (s) = µ gt (s), where µ gt (s) denotes the ground truth tire-road friction, which was obtained for the experiments by performing friction measurements ahead of time and storing the values in a map of the track. The tests were performed on flat and level road surfaces and therefore, evaluation of the effects of road inclination is not included in this work, i.e., θ = φ = 0 rad. in (14), Appendix B. In the collision avoidance scenarios, all suddenly appearing obstacles remain at a static position after appearing.
Remark 4 This delimitation is not done because of limitations in the evaluated algorithms with respect to dynamic obstacles. For both the TA and NA schemes, state constraints P X k|t are set individually for each k ∈ {0, 1, . . . , N }. Hence, including avoidance of for example a confidence interval of predicted positions of the obstacle, is trivial from the planning perspective. Rather, the delimitation was made to ensure consistency between runs, and to avoid introducing additional uncertainty from the prediction functionality. Since the traction adaptive and the non-adaptive schemes are equivalent in this regard, we are confident that the conclusions we make for static obstacles will also hold for dynamic obstacles.
In the following sub-sections, 6.1, 6.2, 6.3 and 6.4, we present and discuss results from each of the scenarios. The experimental setup is detailed in Appendix A.

Scenario (i): Turn at Low Local Traction
At the start of the first critical scenario at t c = 0s and s = 0m, the vehicle approaches a turn at 8 m/s. Traction has deteriorated such that µ gt (s) = 0.2 for s ≥ 0m instead of the previous µ gt (s) = 0.8 for s < 0m. The objective of the vehicle is to maintain its initial velocity and stay close to the center of the lane. Fig. 5, shows a driving behavior analysis comparing the TA scheme with time-varying tire force constraints corresponding to µ est (s) = µ gt (s) and the NA scheme with static tire force constraints corresponding to µ sta = 0.8.

Not Adapting to Low Local Traction
We make the following observations of the behavior under the NA scheme. At t c = 0s, before reaching the corner, Fig. 5a, the planned motion appears to follow the intended objective of tracking the center of the lane and maintaining velocity. Immediately after entering the corner, Fig. 5c, the vehicle starts to exhibit under-steering behavior that was not previously anticipated, i.e., the planned motion at t c = 0s did not include this motion, and the plan gets gradually shifted towards the outside of the corner. The vehicle continues to under-steer heavily, (see front slip angle in Fig. 5l), such that it eventually enters the opposing lane, Fig. 5e, at a velocity of 7m/s Fig. 5k. At this point in the experiment, the planner disengages and full braking is applied. The vehicle eventually comes to a stop 15m outside of the lane, Fig. 5g. The planned trajectory at the beginning of the scenario, Fig. 5a, differs dramatically from the resulting closed loop trajectory, Figs. 5c, 5e, 5g.
The reason behind this undesirable behavior is evident from the planned tire forces at t c = 0s, plotted in Fig. 5i. The dashed blue and dotted orange lines represent the maximum horizontal forces that can be generated by the front and rear tires respectively, according to (1), given the recorded state trajectory. It is clear that the non-adaptive scheme plans tire forces (solid blue) that violate the physical limitations. Therefore, the vehicle is unable to realize the planned motion and promptly veers out of its lane.

Adapting to Low Local Traction
The adaptive scheme however, sets the tire force constraints dynamically according to the predictive friction estimate µ est (s). Therefore the planned tire forces (dash-dotted orange in Fig. 5i) do not violate the physical limitations of the tires. Instead, it plans a motion, Fig. 5b, that minimizes the deviation from the objective (track the center of the lane and maintain 8 m/s), while satisfying the updated tire force constraints. With the TA scheme, the vehicle slows down from 8m/s to 5.5m/s before entering the corner, Fig. 5k and keeps a position on the outside of the turn. About a third of the way through the turn, Fig. 5d, the vehicle has cut across the lane and is positioned along the inside of the corner, Fig. 5j. Towards the end of the turn, Fig. 5h, it starts cutting back across to the outside of the corner. Finally, the vehicle completes the turn safely, positioned at the outer edge of the lane, Fig. 5h, Fig. 5j. In contrast to the NA scheme, here the planned trajectory at the beginning of the scenario, Fig. 5b, corresponds well with the closed loop trajectory of the vehicle, Figs. 5d, 5f, 5h, We break this behavior down into three coordinated characteristics of the maneuver that enable the vehicle to successfully negotiate the turn. First, contrary to the NA scheme, the TA scheme generates feasible tire force commands, utilizing approximately 90% (λ = 0.90) of the available traction for this surface, see Fig. 5i. This in turn generates slip angles within acceptable limits, Fig 5l, yielding high lateral acceleration, Fig 5m. Second, the maneuver utilizes the full width of the lane to minimize the curvature of the vehicle trace, thus reducing the lateral acceleration required to negotiate the turn. Third, the longitudinal velocity is reduced early in the maneuver such that the maximum tire forces are sufficient to negotiate the turn.
These coordinated adjustments to the motion plan are a direct result of the reduced tire force constraint set P U k|t , for all k ∈ {0, 1, . . . , (N −1)} in the optimization problem (12). The planned motion utilizes 90% of the available local traction, such that control authority is maintained throughout the maneuver. Thus, the planning/control scheme safely mitigates the critical situation. Next, we evaluate the traction adaptation concept in collision avoidance scenarios.

Scenario (ii): Collision Avoidance at Low Local Traction
The second scenario is aimed at replicating the conditions when a pedestrian has suddenly entered the lane from the side of the road. At the start of the second scenario, t c = 0s, the pedestrian has been detected by the vehicle's perception stack and the planner/controller starts reacting. The initial velocity of the vehicle is 8m/s and the pedestrian is detected 20m ahead of the vehicle. Traction conditions are the same as in the previous case, i.e., µ gt (s) = 0.8 for s < 0 and µ gt (s) = 0.2 for s ≥ 0. The NA scheme operates under static tire force constraints corresponding to a static friction estimate of µ sta = 0.8, while the adaptive scheme sets its tire force constraints dynamically according to µ est (s) = µ gt (s). The objective of the vehicle is to avoid collision with the obstacle, while minimizing deviations from the lane center. The performance of the NA and TA schemes in this scenario is presented in Fig. 6.

Not Adapting to Low Local Traction
Under the non-adaptive scheme, we make the following observations. Upon detecting the pedestrian, Fig. 6a, the NA scheme initiates an aggressive maneuver, i.e., high curvature given the speed, that is meant to quickly move the vehicle away from its collision course with the obstacle. However, the vehicle is not able to realize the aggressive motion, so the plan is gradually shifted forward, ahead of the vehicle. At t c = 2s, Fig. 6c, the vehicle has eventually moved 2m to the right of the lane center, Fig. 6j, enough to clear the obstacle. However, this occurs closer to the obstacle than  was planned at t c = 0s. Also, at this point the vehicle exhibits heavy under-steering behavior while turning right (see plotted front slip angle in Fig. 6l). After clearing the obstacle, the vehicle starts steering left to return to the lane, but the grip is insufficient to counteract the motion out of the lane that was initiated during the first part of the maneuver. At t c = 3.1s, Fig. 6e, the high slip angles have lead to a complete loss of control authority, and the vehicle is now veering out of the lane. At this point in the experiment, the planner disengages and full braking is applied. The vehicle eventually comes to a stop 7m outside the lane Fig. 6g. The underlying reason is again revealed by the planned tire forces at t c = 0s, plotted in Fig. 6i. The NA scheme (solid blue), plans tire forces that exceed the physical limit (dotted blue), and therefore, the planned motion is not physically attainable. The fact that the real vehicle cannot realize the planned motion has two separate unfavorable consequences in this case: First, the vehicle cannot change direction as fast as the planner dictates, therefore, the margin to the obstacle ends up being smaller than what was originally planned, see Figs. 6a and 6c. Second, the overly aggressive maneuver leads to high slip angles, Fig. 6l. which eventually leads to complete loss of control authority, Fig. 6e, due to tire saturation [4].

Adapting to Low Local Traction
As we saw in the previous scenario, the TA scheme dynamically updates tire force constraints such that the planned motions are dynamically feasible with respect to the local traction conditions. Therefore, in this case we observe that the initial plan from the TA scheme at t c = 0s, Fig. 6b is a less aggressive maneuver compared to the NA scheme. At t c = 2s, Fig. 6b, the vehicle has moved 2m to the right of the lane center, enough to clear the obstacle, without generating excessive front wheel slip, Fig. 6l. At t c = 3.1s, Fig. 6f, the vehicle has cleared the obstacle and at t c = 5s, Fig. 6h, the vehicle is returning from a comparatively small deviation from the lane, maintaining full control authority. Fig. 6i reveals that the planned motion from the TA scheme at t c = 0s exploits approximately 90% (λ = 0.90) of the available traction force for large parts of the maneuver (dash-dotted orange), while the NA scheme (solid blue) exceeds the traction bound by a large margin. Hence, apart from a small margin to the absolute traction limit, it delivers the most aggressive plan that is achievable at the prevailing conditions, yielding manageable slip angles, Fig. 6l, and therefore maintained control authority. As such, the critical scenario is mitigated.
Furthermore, we observe from Fig. 6j, showing deviation from the lane center, that for the first two seconds of the evasive maneuver, the sideways motion of the vehicle is almost identical for the two schemes. From this we get an indication that sacrificing control authority of the vehicle in an overly aggressive maneuver, does not improve the capacity to avoid colliding with the obstacle. It does however increase the risk of a secondary accident as the vehicle enters the opposing lane at high speed in an uncontrolled manner.

Scenario (iii): Collision Avoidance at High Local Traction
A naive solution for handling scenarios 1 and 2 with static tire force constraints is to select a conservative static constraint, corresponding to the worst case traction. However, this strategy will restrict the utilization of the vehicle's capability at favorable traction conditions. Next, we investigate this aspect for collision avoidance at high µ.
In this scenario, µ gt (s) = 0.8 for s ≥ 0, and the pedestrian is detected 15m ahead of the vehicle. The initial velocity of the vehicle is 15m/s. The NA scheme operates under static tire force constraints corresponding to a conservative static friction estimate of µ sta = 0.4, while the  adaptive scheme sets its tire force constraints dynamically according to µ est (s) = µ gt (s) as in previous scenarios. Performance for the NA and TA schemes in this scenario is presented in Fig. 7.

Not Adapting to High Local Traction
For the NA scheme, we make the following observations. Upon detecting the pedestrian, Fig. 7a, the planner initiates an evasive maneuver under conservative static tire force constraints. Given the vehicle state at the time when the planner/controller starts reacting to the obstacle, in combination with the restrictive tire force constraints, there is no collision free solution to the motion planning problem. As a result, the planner selects the least violating trajectory. At t c = 0.7s, Fig. 7c, the vehicle is colliding with the obstacle at a velocity of 13.9 m/s. Notice that in this scenario the final motion of the vehicle corresponds well with the initial plan from the NA scheme, which was not the case in the low µ scenario. This is because in this case, the NA scheme is not violating the physical limitations in terms of tire forces, see Fig. 7f (blue dots). Rather, traction force utilization for the front wheel is consistently below 50% throughout the maneuver.

Adapting to High Local Traction
Because µ est (s) > µ sta in this case, adapting the tire force constraints means expanding the set of allowed tire forces compared to the NA case. Hence, the TA scheme is able to produce a more aggressive plan at t c = 0s, Fig. 7b, that manages to avoid colliding with the obstacle. At t c = 0.7s, Fig. 7d, the vehicle passes beside the obstacle as per the initial plan, and at t c = 3s, Fig. 7e, the vehicle is returning to the lane, fully recovered from the incident. Fig. 7f reveals that at t c = 0s, the TA scheme plans to exploit a larger part of the available traction. This is also reflected by the measured front slip angle, Fig. 7i. Also, in this scenario, because of the higher accelerations, the effect of dynamic normal loads become more apparent, see Fig. 7f. In the beginning of the maneuver, when the vehicle is braking hard, the normal force, and hence the maximum horizontal force, is larger on the front tire and smaller on the rear. Notice that the TA scheme is able to exploit also this effect, and allocates 12% higher front tire force to the front tire a the start of the maneuver (while the vehicle is braking hard) compared to the end of the maneuver (when the vehicle is no longer decelerating). Although this effect is small compared to the 50% difference between the TA and NA schemes due to the friction estimate, it is not insignificant. The effect will be more pronounced at higher accelerations, and thus, is even more important to consider for vehicles that are smaller and more agile than our 8.5 ton test vehicle.
The higher utilization of the available traction for the TA scheme leads to a quicker sideways motion compared to the NA scheme Fig. 7g, and for this case, such a performance difference represents the difference between success and failure of the collision avoidance maneuver. Here, in contrast to Scenarios (i) and (ii), the TA scheme mitigates the critical situation by expanding the tire force constraints instead of reducing them. In all three cases however, it utilizes around 90% of the locally available traction.
From this example we conclude that conservative friction estimation may reduce collision avoidance performance. To further investigate the extent of this phenomenon, we ran three configurations of the planning/ctrl schemes, TA with µ est = 0.8, NA with µ sta = 0.6 and NA with µ sta = 0.4, through a sequence of 40 challenging collision avoidance scenarios each. A sequence of relative positions of the suddenly appearing obstacles was pre-generated by sampling from a uniform distribution, −1.0m ≤ d obs ≤ 1.0m, s + 11.0m ≤ s obs ≤ s + 13.0m. The initial velocity at obstacle detection was 10m/s. With the three configurations, the vehicle managed to avoid collision in 11/40, 6/40 and 4/40 cases, respectively. Fig. 8 shows individual differences in terms of minimum distance margin to the obstacle, ∆ m , for the 11 cases in which at least one configuration performed a successful avoidance maneuver.
Note that the most conservative configuration (NA with µ sta = 0.4), has the least amount of successful avoidance maneuvers, and the four successful avoidance maneuvers are very near misses, with less than 0.10m of clearance to the obstacle. The least conservative configuration (TA with µ est = 0.8) has the largest number of successful avoidance maneuvers and tends to also have the largest amount of clearance to the avoided obstacles. This is true for all cases except for obstacles 5 and 8, where (NA with µ sta = 0.6) has slightly higher margin. We believe this stems from variations in the state of the vehicle when the obstacle is detected. In particular, the lateral position within the lane exhibited small variations during the experiment, which had noticeable effect on the margin to the obstacle.

Scenario (iv): Collision Avoidance with Discrete Decisions
As mentioned in Section 4.4, the RTI method handles situations when the initial guessT t violates state constraints,x k|t / ∈ P X k|t , for any k ∈ {0, 1, . . . , N }, by softening the constraints and giving constraint violations a high cost. This in combination with the fact that the algorithm only searches for a solution locally aroundT t leads to sensitivity to highly suboptimal local minima. The problem occurs when the constraint configuration changes abruptly between planning iterations, e.g., at suddenly appearing obstacles, or in our approach to traction adaptation, at large changes in traction conditions. Therefore, we employ the sampling augmentation procedure, see Sections 5.2 and 5.3, to mitigate this problem. In this section we evaluate the safety implications of sampling augmentation in critical scenarios with discrete decisions. Scenario (iv) is a collision avoidance scenario where two obstacles appear simultaneously 20m and 35m in front of the vehicle. The first obstacle appears slightly to the left of the lane center and the second obstacle appears on the right side of the lane. State constraints are set such that obstacles should be avoided by a margin of 0.5m. We compare performance of the non-augmented RTI scheme (abbreviated RTI), with our sampling augmented variant (abbreviated SA). For this test both schemes use the same static tire force constraints. Results are presented in Fig. 9. The left and right columns of overhead views correspond to the non-augmented RTI scheme (RTI) and sampling augmented (SA) schemes respectively. Subfigure 9g show planned tire forces for the front tires at t c = 1.0s. Subfigure 9h shows a comparison in terms of smallest distance to either obstacle. Subfigure 9i shows which candidate was selected as initial guess with the SA scheme.

Without Sampling Augmentation (RTI)
When the obstacles are detected at t c = 1.0s, Fig 9a, the non-augmented RTI scheme produces an evasive plan that avoids the obstacles by going in between them (right of the first and left of the second obstacle). The vehicle manages to avoid collision, Figs. 9c, 9e, but needs to use the maximum front tire force almost throughout the maneuver, Fig. 9g, and has a very small distance margin ∆ m to the obstacles (state constraints are violated), Fig. 9h.
The decision to plan the maneuver between the obstacles stems from the last planning iteration before the obstacles appeared. At the first planning iteration after the obstacle is detected, the solution from the previous iteration T t−1 and hence the initial guess,T t , is at the center of the lane. When the obstacles appear,T t , violates the state constraints,x k|t / ∈ P X k|t for some k ∈ {0, 1, . . . , N }. Since state constraints are soft, the optimization problem is still feasible, but the decision of adjusting the trajectory left or right of an obstacle is determined by minimizing cost locally around T t−1 . Since in this case, the first obstacle appears slightly to the left of the lane center, the planner makes the decision to go between the obstacles.

With Sampling Augmentation (SA)
When the sampling augmented RTI algorithm is exposed to the same scenario, we observe that upon detecting the obstacle, the SA scheme instead produces an evasive plan to the left of both obstacles, Fig. 9b. When following this plan, the vehicle is able to clear the obstacles more comfortably, Figs. 9c, 9e. It uses the maximum front tire force for a smaller amount of the time, Fig. 9g, keeps above the minimum margin to both obstacles (not violating state constraints), Fig. 9h and recovers more quickly after the incident (compare Fig. 9e and Fig. 9f).
Just as before, at the first planning iteration after the obstacles are detected, the initial guessT t originating from T t−1 is in collision with the obstacles. However, due to the sampling augmentation procedure, at t c = 0.1s, a sampled trajectoryT t is selected as initial guess. In the subsequent planning iterations throughout the maneuver,T t is selected, Fig. 9i. Fig. 9i reveals that the sampled trajectories were only used in a single planning iteration just after the obstacles appeared. This indicates that sampling augmentation steps in exactly when needed, to avoid unfavorable local minima, i.e., at rapid changes in the constraint configuration of the motion planning problem. Otherwise, when changes to the constraint configuration are small, T t typically has the lowest cost and gets selected. This result indicates that sampling augmentation alleviates Problem (iii) of Section 4.4 RTI, without impairing the otherwise desirable properties of the RTI method.
The sampling augmentation procedure should be viewed as an exploratory effort, that continuously evaluates a range of dynamically feasible maneuver alternatives, spread throughout the physically reachable drivable area, such that discrete decision making is made based on the full reachable state space of the vehicle (given current traction conditions), represented byŜ t ∪T t rather than being solely determined by local information around T t−1 .

Conclusions and Future Work
In this paper we address the problem of motion planning at the limits of handling under locally varying traction conditions. We evaluate the proposed traction adaptive algorithm by comparing it to an equivalent non-adaptive scheme in a sequence of critical scenarios at various traction conditions. Results from experimental evaluation indicate that traction adaptation improves capacity to avoid accident in all four tested critical scenarios. The proposed algorithm's performance in this regards stems from two properties: 1. Ensured dynamic feasibility of planned motions, even at varying local traction conditions. 2. A high ratio (90% for the tested configuration) of locally available traction is utilized to avoid collision if necessary.
Moreover, we demonstrate experimentally that the sampling augmentation procedures (steps A-D of Section 5) of the proposed algorithm successfully mitigates the computational challenges associated with infeasibility and sensitivity to local minima (listed in Section 4.4) that emerges in the planning problem at rapid changes to the constraint configuration.
In Section 3.4 we briefly introduce a selection of approaches to obtaining predictive friction estimates online. It is intuitively clear that the characteristics and performance of such friction estimates will influence the planned behavior. We do not include that aspect in the scope of this paper, instead we present a follow-up study, dedicated to that topic [39], where we conclude that combining camera based and traditional friction estimation techniques enable near-optimal traction adaptive motion planning and control. These preliminary results are obtained by emulating state of the art friction estimation methods in a simulated environment. Thus, the next step is to return to the test track and evaluate traction adaptive motion planning and control in closed loop with state of the art friction estimation.  The rear lateral force F yr is not included among the control inputs, instead it is computed from the state through a time-varying linear tire model with the rear slip angle α r = − arctan ((v y − l rψ ))/v x . The time-varying rear cornering stiffnessC r is obtained by linearizing Pacejka's Magic Formula tire model, [41], F y =D(µ) sin ( C(µ) arctan ( B(µ)α f + · · · − E(µ) ( B(µ)α f − arctan ( B(µ)α f ) ) ) ) , with parameters B(µ), C(µ), D(µ) and E(µ) obtained as functions of µ from a look-up table that approximately associate the tire parameters to the friction coefficient for various road surface conditions.

Remark 5
In general, the linear tire model used in (15) does not accurately predict the relation between slip angle and tire force at high slip angles [4]. However, in this context, the combined lateral and longitudinal force command is always inside the friction circle, by a margin determined by the utilization factor λ. Inside such a boundary, a time-varying linear model corresponds well with the Magic Formula. Considering the computational trade-off mentioned in Section 2, this design represents a compromise between complexity and accuracy that have proven to be practically viable for our application.
The normal forces F zf and F zr acting on the tires of the model are obtained from moment balances about the contact points of the rear and front tires respectively as F zf = 1 l f + l r (−mv x h − mgh sin θ + mgl r cos θ) , F zr = 1 l f + l r (mv x h + mgh sin θ + mgl f cos θ) .
See geometric relations in Fig. 10b. Combining these relations with the friction estimate µ we define front and rear time-varying 6 force boundaries as where λ ∈ [0, 1] is a user adjustable traction utilization factor. If λ = 1.0, the algorithm is allowed to utilize the full friction circle. The experiments for this paper were run at λ = 0.9. To comply with these bounds, tire force commands need to fulfill where F yr is determined by (15). In addition to the bounded tire forces (17), the drivetrain of the vehicle imposes additional constraints due to rear wheel drive and limited tractive force from the motor F (ub,dr) r . Finally, the algorithmic framework used in this paper requires linear input constraints. To accommodate this, we under-approximate the input constraints (17) and (18) by a three-dimensional convex polytope P U , specific to each prediction step k, visualized in Fig. 11.

Appendix C Low-Level Control Interface
The purpose of the control interface is to translate tire force commands, F yf , F xf and F xr from the planned trajectory T t into the actual control inputs of the vehicle. For our particular test vehicle, those are steering angle request δ req and longitudinal acceleration request a req .
For the longitudinal control input, the transformation is trivially computed as a req = (F xf + F xr )/m. For the steering input δ req , we begin by computing the kinematic component of the steering angle, given by δ kin , i.e., the steering angle that would yield the planned motion in the absence of tire slip. We fit a circle segment of curvature ρ to the Cartesian representation of the position states s k|t and d k|t for k ∈ {0, 1, . . . , (N fit )} of T t , where N fit is selected such that only the initial part of the trajectory is used. Then, the kinematic steering angle component is obtained as δ kin = ρ(l f + l r ). The dynamic steering angle component, i.e., the desired front wheel slip angle, is 6 Variables µ, F zf and Fzr vary in time and hence, so do F . Just as with the state variables in (14) the time dependency is omitted from the equations to simplify notation. Figure 11: Input constraint polytope P U (for single prediction step k) bounding control signals produced by the algorithm (dark gray) and theoretical traction limits according to (1).
obtained as α f = F yf /C f , where the time-varying cornering stiffnessC f is obtained by linearizing the Pacejka Magic Formula tire model as per the procedure described in Appendix B. The steering angle request is then obtained as δ req = δ kin + α f . Finally, the control commands δ req and a req are passed to the vehicle via the ROS-to-OpenDLV bridge described in Appendix A.