Shared Steering Control for Lane Keeping and Obstacle Avoidance Based on Multi-Objective MPC

This paper presents a shared steering control framework for lane keeping and obstacle avoidance based on multi-objective model predictive control. One of the control objectives is to track the reference trajectory, which is updated continuously by the trajectory planning module; whereas the other is to track the driver’s current steering command, so as to consider the driver’s intention. By adding the two control objectives to the cost function of an MPC shared controller, a smooth combination of the commands of the driver and the automation can be achieved through the optimization. The authority of the driver and the automation is allocated by adjusting the weights of the objective terms in the cost function, which is determined by the proposed situation assessment method considering the longitudinal and lateral risks simultaneously. The results of the CarSim-Matlab/Simulink joint simulations show that the proposed shared controller can assist the driver to complete the tasks of lane keeping and obstacle avoidance smoothly while maintaining a good level of vehicle stability.


Introduction
In the past decade, the development of autonomous vehicles has received a great amount of attention and brought considerable benefits. Although full automation under all driving circumstances remains an extremely challenging task due to numerous technical limitations and legal issues [1,2], vehicles equipped with advanced driver assistance systems (ADAS), which fit into the level of partial automation (PA) or below as defined by SAE [3], have already been proven to reduce traffic accidents [4] while lowering the burden on drivers [5]. Industrial entities and researchers are, therefore, motivated to exploit benefits of higher levels of automation which involve more frequent and tight human-automation cooperation.
Early studies proposed to switch the control authority back and forth between the driver and the automation system based on the driver's attentiveness [6] and/or the capacity of the automation system [7]. However, some studies have shown that it is in fact challenging for the driver to resume control over the vehicle due to issues such as over-trust in automation [8], or lack of situation awareness [9].
Therefore, it has been proposed to keep the driver in the loop while allowing automation systems to cooperate with drivers so as to improve driving safety [1]. Shared control, which allows the driver and the automation system to drive the vehicle simultaneously by combining control actions from both, is considered as a feasible and promising solution to the transition from low levels of autonomy to conditional or even high automation [10]. is designed to determine the weights of the two optimization objectives, so that timely responses at the operational level can be achieved under challenging driving conditions. Finally, with matching the driver's commands and tracking the planned obstacle avoidance trajectories both taken as the optimization objectives, the proposed controller manages to keep the driver in the loop and avoids having to interpret the driver's intentions. By dynamically adjusting the weights of the two optimization objectives in the MPC cost function, rather than blending commands of the driver and the automation system, the authority allocation can be implemented smoother in an optimal controller. The main contribution of this paper is as follows:

1.
A new shared control framework based on MPC is proposed, which includes an NMPC-based trajectory planning module, a situation assessment module considering both lateral and longitudinal risks, and a shared control module based on multiobjective MPC.

2.
A shared control scheme based on multi-objective MPC is proposed, in which matching the driver's commands and tracking the reference trajectories are both taken as the optimization objectives, and the authority of the driver and the automation are allocated by adjusting the weights of the two objectives, which are determined by the situation assessment module.

Vehicle Model
In this section, a vehicle dynamics model, which is used as the prediction model of the MPC-based shared controller, is introduced. The parameters used in the vehicle model are presented in Table 1.
Computation time is a major challenge in the design of an MPC, in which the complexity of the prediction model affects a lot. However, the complexity and the accuracy of the prediction model are two conflictive aspects. Many studies have proposed to simplify the prediction model to achieve a trade-off between accuracy and complexity. The bicycle model, which has been proved to be effective in achieving a trade-off between modelling accuracy and computation efficiency [32], was adopted as the prediction model of the MPC-based controller in this study. Since only the lateral control of the vehicle was considered, the longitudinal speed of the vehicle was set to be unchanged throughout the control process, and only the front wheel angle was considered as the input of the vehicle dynamics model. Meanwhile, it was assumed that the vehicle was equipped with a steer-by-wire system so that the mechanical connection between the steering wheel and the front wheel can be decoupled. As shown in Figure 2, a dynamics model of the bicycle vehicle was established with three degrees of freedom, including longitudinal, lateral, and yaw motion. According to Newton's second law, the following force balance equations can be obtained along the X-axis, Y-axis, and Z-axis: where v y and v x , respectively, represent the lateral and longitudinal speed of the vehicle in the body-fixed coordinate system. The tire force in this study is simplified as follows [33]: Assuming that the vehicle was equipped with an Antilock Brake System (ABS) system, the slip ratio κ i was identified. When the slip angle is relatively small, the tire cornering angle can be obtained with the following formula [34]: The state variables in the vehicle coordinate system are converted to those in the inertial coordinate system as:

Dynamic Trajectory Planning Based on NMPC
A dynamic trajectory planner is designed to plan the obstacle avoidance trajectory in real-time, as the reference trajectory for which the shared controller tracks. To reduce the computational complexity of the NMPC, the motion of the vehicle is described as a particle [35]: By discretizing Equations (6) and (7) with the Euler method, the prediction equation of the trajectory planner can be obtained: η t = J t ξ t i = 1, 2, . . . P t , J t = 0 0 0 0 1 (8) where, Γ represents the discrete form of Equations (6) and (7), ξ t = v x v y ϑ X Y represents the vehicle states in the trajectory planning controller, P t is the prediction horizon, and ξ t (s + i|s ) represents the predicted vehicle state at step s + i. The lateral acceleration of the vehicle γ y is chosen as the control input, and u t and u t (s + i|s ) represent the control input at time step s + i. To reduce the amount of calculation, it is assumed that the control input outside the control horizon remains unchanged: where, C t is the control horizon, and C t ≤ P t . The goal of dynamic trajectory planning is to avoid obstacles while minimizing the error between the planned trajectory and the global reference trajectory, so the mathematical expression of the objective function is expressed as follows: where, η gr (s + i|s ), i = 1, 2, . . . , P t represents the global reference trajectory in the prediction horizon, Q t and R t represent the weight matrix of controlled outputs and inputs, M obs represents the number of obstacle points, and J obs,j represents the obstacle avoidance function of the jth obstacle point, which is inversely proportional to the Euclidean distance between the COG (Center Of Gravity) and the obstacle, and is proportional to the vehicle speed. J obs,j is a function designed to produce a penalty for obstacle avoidance. The mathematical expression is as follows: where, v denotes the vehicle speed, K obs denotes the weight of the obstacle avoidance function, (x c , y c ) is the coordinate of the COG, x obs,j , y obs,j represents the coordinates of obstacle points, and ζ is a very small number to prevent the denominator from being 0. The diagram of the obstacle avoidance function is shown in Figure 3. To lower the computational overhead, the reference trajectory is produced with the fifth-order polynomial curve fitting, whose mathematical expression is as follows: where, a 0 , a 1 , a 2 , a 3 , a 4 and a 5 are the parameters to be fitted, Y re f can be the reference lateral position y re f or the reference heading angle ϑ re f , and X indicates the longitudinal position of the vehicle.

MPC-Based Shared Controller
The MPC-based shared controller is designed to track the reference trajectory while matching the driver's steering commands whenever safe. The trade-off between the two objectives is achieved through adjusting the weights, which are determined on the basis of a real-time assessment of the driving situation.

Situation Assessment
In this paper, we chose Time to Lane Crossing (TLC) [36] and Time to Collision (TTC) [37] as two factors of the driving risk assessment. TLC is defined as the time remaining for the front wheels to reach the border of the current lane if the vehicle maintains the current lateral acceleration. The expression of TLC is as follows: where y ll can be the distance from the left (or right) front wheel to the left (or right) lane boundary. The measurement of the lateral vehicle speed v y is quite challenging for it is not available from a conventional sensor. However, previous studies have proposed to estimate the lateral vehicle speed based on measurable parameters [36,38]. Therefore, it is assumed that the lateral vehicle speed v y can be obtained by estimation. As shown in Figure 4, TTC is defined as the time remaining for the vehicle to collide with the obstacle if the vehicle maintains the current velocity. The expression of TTC is as follows: where, (x o and y o ) denote the coordinates of the obstacle, (x c and y c ) denote the coordinates of the vehicle, and p c,o is the vector of the mass center of the vehicle pointing to the mass center of the obstacle. In this paper, TLC and TTC were both used for the situation assessment. When there is no collision risk, the authority of the controller is adjusted according to TLC for the purpose of lane keeping. If TLC reaches the low-risk threshold, the control authority transits from matching the driver's commands to tracking the global reference trajectory; When obstacle avoidance becomes the prominent and pressing task, TTC is used as the risk indicator, and the control authority transits from matching the driver's commands to tracking the dynamically planned obstacle avoidance trajectory. As suggested in previous studies [39,40], 2 s and 4 s are used as the high-risk threshold and the low-risk threshold of TTC, respectively, while the high-risk threshold of TLC is defined as [41]: where, t d is the total response time of the actuator and the driver, and we chose t d = 1 s in this paper. In addition, the low-risk threshold of TLC is defined as TLC max = 2TLC min . Considering the conflict between the driver and the automation in the scenario of lane keeping, a threshold of the driver's steering command σ lim is introduced when evaluating the lane departure risk. When the risk of lane departure increases and the driver input continues to exceed this threshold, we consider that the lane keeping system needs to be deactivated and the driver has the control of the vehicle. In addition, we set σ lim = 2 • in this study. The weight factor is obtained by the following formula: where, TLC min and TTC min represent the high-risk threshold of TLC and TTC, and TLC max and TTC max represent the low-risk threshold of TLC and TTC, respectively.

Design of MPC Shared Controller
After discretizing the vehicle dynamics model established in Section 2 with a fixed sampling time T s , the following expression is obtained: where, s represents the current step, and ∆δ f is the control input. To reduce the computational complexity so as to improve the real-time performance of the controller, Equation (17) is linearized around an operating point: where, The control objectives of MPC shared controller are to track the reference trajectories and match the driver's steering commands. A dynamic and smooth shift of humanmachine control authorities is accomplished by adjusting the weight matrix of the two control objectives.
where, η s represents the output of the prediction model, η re f represents the reference output, P s and C s represent the prediction horizon and the control horizon in the shared controller, respectively, δ d represents the driver's current steering command, and Q, S and R, respectively, represent the weight matrix of the trajectory tracking, driver's command, and control inputs. σ is the shared control weight factor, which falls within the interval (0,1). When σ is large, the objective of the tracking reference trajectories prevails; on the contrary, the driver dominates the control. Equation (21) represents the constraints on the control input and control increment.
Assuming that the front wheel angle output by the driver remains unchanged in the control horizon and define: . . . The objective function in Equation (20) is transformed into the following quadratic programming (QP) form: where, By solving Equation (22), the sequence of control inputs at each step is obtained, and the first element ∆δ * f (s) in the sequence is chosen as the actual control variable: The above process is repeated at the next step.

Simulation Results and Analysis
To verify the proposed shared control framework, a series of cooperative emergency obstacle avoidance simulation studies were conducted jointly with CarSim-Matlab/Simulink. Parameters of the vehicle model used in CarSim are shown in Table 2. In this paper, the prediction horizon and control horizon of the proposed shared controller are P s = 25 and C s = 5, respectively, and the sampling time is T s = 0.02 s. In addition, considering the physical limitations of the steering system, the constraints of the control input and the control increment are set as: To examine the robustness of the shared controller, three scenarios were designed with the varying of the driver's steering commands, representing different driver's states as described below: 1.
Distracted driving with no steering command; 2.
Obstacle avoidance in a panic with oscillating steering commands; 3.
In these scenarios, the global reference trajectory is the centerline of the lane, and the local reference trajectory is the obstacle avoidance trajectory generated by the trajectory planning module. The proposed shared controller was tested at the speeds of 36 km/h, 72 km/h, and 108 km/h, respectively, in each scenario.
The sideslip angle, lateral acceleration and the heading angle deviation were used as indices to evaluate the performance of controllers. To verify the advantages of the smooth transition of the control authority, two controllers were designed based on the shared control framework proposed in the previous sections and simulated under the same conditions with only the strategy of authority allocation differing. Specifically, controller A conducted a smooth transition of control authority defined by Equation (16), while controller B conducted a step shift of control authority, which can be formulated as:

Results and Analysis of Scenario 1
In this scenario, the vehicle is traveling on a road with a friction coefficient of 0.85, an obstacle appears at X = 150 m ahead, and the driver takes no action to avoid the obstacle.
As shown in Figures 5-7, both controller A and controller B managed to steer the vehicle to avoid the obstacle successfully at different speeds, under the circumstance that the driver takes no steering action. Figure 5b, Figure 6b or Figure 7b clearly show that the shared controllers track the driver's commands until the situation assessment module determines that the vehicle is in danger of collision and, hence, adjust the weights of the two objective terms in the MPC cost function. In the process of authority allocation in controller A, as marked as instance one in the figures, the front wheel angle changed more slowly and smoothly than that of controller B, which resulted in a smaller sideslip angle and lateral acceleration. Although controller A has full control later than controller B, the performance of controller A had no decrease compared with that of controller B, as there was no significant difference between the two controllers in the maximum sideslip angle and lateral acceleration during obstacle avoidance. Therefore, when the driver takes no action, a smooth transition of control authority can obtain better stability at different speeds.
In these scenarios, the global reference trajectory is the centerline of the lane, and the local reference trajectory is the obstacle avoidance trajectory generated by the trajectory planning module. The proposed shared controller was tested at the speeds of 36 km/h, 72 km/h, and 108 km/h, respectively, in each scenario.
The sideslip angle, lateral acceleration and the heading angle deviation were used as indices to evaluate the performance of controllers. To verify the advantages of the smooth transition of the control authority, two controllers were designed based on the shared control framework proposed in the previous sections and simulated under the same conditions with only the strategy of authority allocation differing. Specifically, controller A conducted a smooth transition of control authority defined by Equation (16), while controller B conducted a step shift of control authority, which can be formulated as:

Results and Analysis of Scenario 1
In this scenario, the vehicle is traveling on a road with a friction coefficient of 0.85, an obstacle appears at X = 150 m ahead, and the driver takes no action to avoid the obstacle.
As shown in Figures 5-7, both controller A and controller B managed to steer the vehicle to avoid the obstacle successfully at different speeds, under the circumstance that the driver takes no steering action. Figure 5b, Figure 6b or Figure 7b clearly show that the shared controllers track the driver's commands until the situation assessment module determines that the vehicle is in danger of collision and, hence, adjust the weights of the two objective terms in the MPC cost function. In the process of authority allocation in controller A, as marked as instance one in the figures, the front wheel angle changed more slowly and smoothly than that of controller B, which resulted in a smaller sideslip angle and lateral acceleration. Although controller A has full control later than controller B, the performance of controller A had no decrease compared with that of controller B, as there was no significant difference between the two controllers in the maximum sideslip angle and lateral acceleration during obstacle avoidance. Therefore, when the driver takes no action, a smooth transition of control authority can obtain better stability at different speeds.

Results and Analysis of Scenario 2
To verify the effectiveness of the proposed shared controller when the driver takes actions incorrectly, due to being flustered or immature driving skills, and turns the steering wheel left and right repeatedly. The driver's steering command is simulated with a sine curve in this scenario.
As shown in Figure 8a, both controller A and controller B can help drivers keep the lane and avoid obstacles, under the circumstance that the driver takes oscillating steering actions. As shown in Figure 8c, the situation assessment module detected that the vehicle was about to deviate from the lane at instance 1, and adjusted the weights in the MPC cost function to keep the lane. When it was in danger of collision at instance 2, the driver steered the vehicle in the opposite direction of obstacle-avoidance. The controllers, therefore, set the objective to track the collision-free trajectory. Figure 8d,f show that the performance of controller B had a significant drop compared with that of controller A, as the peak values of the sideslip angle and the lateral acceleration of controller B were both higher than those of controller A. During the entire obstacle avoidance, the maximum sideslip angle and lateral acceleration of both controllers were no more than 1° and 0.2 g, respectively. Figures 9 and 10 present the results of the same scenario, only with the vehicle speed increased to 72 km/h and 108 km/h, respectively. Again, it was shown that both controllers can assist the driver to complete the task of lane keeping and obstacle avoidance. However, when the risk of collision was detected at instance 2, the driver took the

Results and Analysis of Scenario 2
To verify the effectiveness of the proposed shared controller when the driver takes actions incorrectly, due to being flustered or immature driving skills, and turns the steering wheel left and right repeatedly. The driver's steering command is simulated with a sine curve in this scenario.
As shown in Figure 8a, both controller A and controller B can help drivers keep the lane and avoid obstacles, under the circumstance that the driver takes oscillating steering actions. As shown in Figure 8c, the situation assessment module detected that the vehicle was about to deviate from the lane at instance 1, and adjusted the weights in the MPC cost function to keep the lane. When it was in danger of collision at instance 2, the driver steered the vehicle in the opposite direction of obstacle-avoidance. The controllers, therefore, set the objective to track the collision-free trajectory. Figure 8d,f show that the performance of controller B had a significant drop compared with that of controller A, as the peak values of the sideslip angle and the lateral acceleration of controller B were both higher than those of controller A. During the entire obstacle avoidance, the maximum sideslip angle and lateral acceleration of both controllers were no more than 1 • and 0.2 g, respectively. Figures 9 and 10 present the results of the same scenario, only with the vehicle speed increased to 72 km/h and 108 km/h, respectively. Again, it was shown that both controllers can assist the driver to complete the task of lane keeping and obstacle avoidance. However, when the risk of collision was detected at instance 2, the driver took the correct steering command, and controller A chose to track the driver's command to avoid the obstacles while controller B ignored the driver's steering commands and directly turned to track the reference trajectory. Therefore, it was proved that controller A attempted to track the driver's command whenever it was safe and, consequently, improved the driver's acceptance of automation without impacting the tracking performance.

Results and Analysis of Scenario 3
In scenario 3, the driver's stochastic steering command was considered, and white noise was applied to simulate the driver's steering command, which obeys standard normal distribution (0,1).
As shown in Figure 11, at different vehicle speeds, the driver's steering command changed randomly and frequently. Both controller A and controller B could complete the task of lane keeping and obstacle avoidance. The robustness of the shared controllers to

Results and Analysis of Scenario 3
In scenario 3, the driver's stochastic steering command was considered, and white noise was applied to simulate the driver's steering command, which obeys standard normal distribution N(0, 1).
As shown in Figure 11, at different vehicle speeds, the driver's steering command changed randomly and frequently. Both controller A and controller B could complete the task of lane keeping and obstacle avoidance. The robustness of the shared controllers to changes in driver's inputs and vehicle speeds was proved. As shown in Figure 11b, the proposed shared controllers have the advantages of smoothing the driver's steering commands. Moreover, also as shown in Figure 11b, the front wheel angle of controller B deviated from the driver's command many times due to the risk of lane departure, while that of controller A deviated fewer times and was more consistent with the driver's commands. changes in driver's inputs and vehicle speeds was proved. As shown in Figure 11b, the proposed shared controllers have the advantages of smoothing the driver's steering commands. Moreover, also as shown in Figure 11b, the front wheel angle of controller B deviated from the driver's command many times due to the risk of lane departure, while that of controller A deviated fewer times and was more consistent with the driver's commands.

Conclusions
This paper presents a shared control strategy for lane keeping and obstacle avoidance in which the driver and the automation interact tightly at two levels, namely the tactical level and the operational level. At the tactical level, a trajectory planning controller based on NMPC generated the collision-free reference trajectories for the shared controller. A risk assessment algorithm was designed to determine the allocation of control authority, which considers the lateral and longitudinal risks simultaneously. At the operational level, a shared control framework based on MPC was proposed, whose cost function consists of two objective terms representing the objective to track reference trajectories and match the driver's steering commands, respectively. A smooth combination of control actions between the driver and the automation can be realized in an optimal controller by adjusting the weights of the objective terms, rather than changing the weights of the control commands of the driver and the automation. The simulation results show that the proposed control framework can robustly assist the driver to complete the task of lane keeping and obstacle avoidance when the vehicle velocity and the driver's steering command varies, while maintaining vehicle stability. It was also discovered that, as compared to a step shift of control authority, a smooth transition of control authority can further improve the performance while offering more freedom to the driver. Future research should attempt to explore more human factors in the design of the shared control scheme to improve the driver's acceptance of automation.