Surrounding Vehicles Motion Prediction for Risk Assessment and Motion Planning of Autonomous Vehicle in Highway Scenarios

Safety is the cornerstone of autonomous driving vehicles. For autonomously controlled vehicles driving safely in complex and dynamic traffic scenarios, it is essential to precisely predict the evolution of the current traffic situation in the near future and make an accurate situational risk assessment. The precise motion prediction of surrounding vehicles is an essential prerequisite for risk assessment and motion planning of autonomous vehicles. In this paper, we propose a risk assessment and motion planning method for autonomously controlled vehicles based on motion prediction of surrounding vehicles. Firstly, surrounding vehicles’ trajectories are predicted based on fusing constant turn rate and acceleration-based motion prediction model and maneuver-based motion prediction model with interactive multiple models. Then, considering both the probability of collision event and collision severity, the collision risk assessment between autonomously controlled vehicle and surrounding vehicles is conducted with a collision risk index. After that, the motion planning of the autonomously controlled vehicle is formulated as a multi-objectives and multi-constraints optimization problem with a model predictive control framework. Finally, the proposed method is applied to several traffic scenarios to validate its feasibility and effectiveness.


I. INTRODUCTION
Safety is the eternal theme of automotive technology. The frequent road traffic accidents have led to a higher demand for automotive safety. Since autonomously controlled vehicles (ACVs) have great potential in improving road safety, researchers have drawn much attention to the research and development of autonomous driving systems [1]. Motion planning is one of the core technologies for ACV. With environmental perception information provided by multisensors, the motion planning module of ACV generates a safe, stable, comfortable, and feasible reference trajectory for the trajectory tracking module. The traffic scenario around ACV may involve dynamic and uncertain traffic participants. Meanwhile, the ACV must satisfy kinematic constraints and The associate editor coordinating the review of this manuscript and approving it for publication was Kai Li . kinetic constraints. All these make motion planning of ACV complicated.
Trajectory planning methods have been used in mobile robots and ACV for several decades. According to environmental modeling and searching strategies, trajectory planning methods can be classified into graph-based methods, sampling-based methods and optimization-based methods [2]. Graph-based methods separate the free space with graphs, such as the visibility graph [3] and the cell decomposition [4]. An optimal path is found with searching algorithms such as Dijkstra [5] and Anytime D * [1]. During the 2007 DRRPA Urban Challenge, the team named BOSS segmented the state space with state lattice consisting of position, heading angle and velocity, and Anytime D * was employed for searching the sub-optimal path in unstructured environments [1]. However, the completeness and optimality of graph-based methods are related to the size of state lattice. The complexity for finding the optimal path depends on the dimension of state space, the number of obstacles and the size of the state space. The graph-based methods are deterministic, while the sampling-based methods discrete the configuration space by sampling randomly. The rapidlyexploring-random-tree (RRT) is a typical sampling-based method [6]. MIT made a few extensions of standard RRT and applied it to uncertain and dynamic scenarios in the DARPA Urban Challenge [7]. Since collision detection is required during tree extension each time, the complexity of RRT is extremely high in crowded obstacle areas. Optimizationbased trajectory planning methods formulate the holonomic and non-holonomic constraints as equalities and inequalities, respectively. The optimal trajectory is found by solving a constrained optimization problem. However, there may exist several local extremums. A local and continuous trajectory planning with only a single global optimum was proposed for driving Bertha-Benz-Memorial-Route autonomously [8]. Static and dynamic obstacle constraints are represented as polygons. A left-right decision and geometric processing were incorporated for modifying the original polygons, which resulted in the solution converging to a single, global optimum.
The motion planning of ACV is essentially a multiobjectives and multi-constraints optimization problem. Safety is one of the basic demands of ACV. Risk assessment is used for evaluating the safety of ACV. Researchers typically construct risk indicators for risk assessment based on motion prediction of ego vehicle and surround vehicles (SVs) [9]- [11]. Consequently, risk assessment usually involves two essential parts, which refers to motion prediction of SVs and the construction of a reasonable risk indicator.
According to levels of information abstraction, motion prediction of vehicles are divided into physics-based, maneuver-based and interaction-aware motion models [12]. In physics-based motion models, vehicles are viewed as dynamic entities controlled by physical laws. Kinematic models or kinetic models are used for motion prediction of vehicles in the future. The typical physics-based motion models includes Constant Velocity (CV) Model, Constant Acceleration (CA) Model, Constant Turn Rate and Velocity (CTRV) Model, Constant Turn Rate and Acceleration (CTRA) Model, Constant Steering Angle and Velocity Model (CSAV), Constant Steering Angle and Acceleration (CSAA) Model [12]. Schubert et al. made a comprehensive comparison of numerous physics-based motion models [13]. Although physics-based motion models have good realtime efficiency, they are insufficient to describe changes in vehicle motion due to abrupt maneuvering behaviors or environmental factors. In maneuver-based motion models, vehicles' motion on the road network can be viewed as several independent maneuvers. The vehicles' trajectories are predicted based on recognized maneuvers. Typical maneuver recognition methods include Support Vector Machine [14], Hidden Markov Model [15] and Dynamic Bayesian Network [16], et al. However, maneuver-based motion models do not take into account the interaction among traffic participants. In interaction-aware motion models, the motion of one vehicle is influenced by the other traffic participants, which makes them the complete models for the motion prediction of vehicles. Li et al. proposed an interaction-aware motion model for surrounding vehicles, which modeled the interaction among vehicles with a performance function penalizing the possible collisions [17].
The existing risk indicators mainly include timebased, distance-based, deceleration-based, probability-based and field theory-based measures [18], such as Time-To-Collision (TTC) [19], Time-Headway (TH) [20], Time-To-React (TTR) [21], Predicted Minimum Distance [22], Required Deceleration [23], et al. However, in timebased, distance-based and deceleration-based risk indicators, the motion of ego vehicle and surrounding vehicles are commonly described with CV or CA models. Although they have good real-time performance, they are only suitable for single-lane risk assessment in rear-end collision scenarios. Xu et al. proposed an integrated risk assessment indicator for multi-lane traffic scenarios by synthesizing the TTC, TH and original constructed Time-To-Front (TTF) [24]. Interactive multiple models (IMMs) is used for motion prediction of surrounding vehicles. Hruschka et al. used maneuver-based models for predicting the motion of SVs, and evaluated the collision risk with a combination of collision probability and collision severity [25]. In literature [26], the risk is defined as the expected cost related to a future critical event. Future event probability and damage probability are used to determine risk indicator for motion planning of ego vehicle.
Due to low accuracy in the long prediction horizon, physics-based motion models are not suitable for risk assessment in complicated traffic scenarios. Maneuver-based and interaction-aware motion models mainly depend on machine learning methods. A great deal of data is needed for model training. Besides, the trained model may be constrained to road topology similar to those in the training process. Meanwhile, real-time efficiency cannot be satisfied due to high computation complexity. In this paper, a risk assessment and motion planning method based on motion prediction of SVs is proposed, as illustrated in Fig. 1. The motion of surrounding vehicles is predicted by integrating the CTRA model and a simplified maneuver model with IMM. Specifically, the adopted maneuver model recognizes maneuvers by the spatial-temporal relationship between the vehicle's historical trajectory and road geometry shape, which is different from traditional maneuver recognition methods relying on a large amount of training data. Then, the collision risk indicator is used for risk assessment between ACV and SVs, which synthesizes collision probability and collision severity. The proposed risk indicator is applied to the motion planning of ACV in highway scenarios.
The remainder of this work is structured as follows. Section II briefly presents an introduction to the definition of coordinates. In Section III, the motion prediction of surrounding vehicles is stated. The construction of the risk indicator is explained in Section IV. In Section V, the motion planning of ACV is formulated as a multi-objective and multi-constraints problem with model predictive control (MPC) framework. The proposed method is applied to several traffic scenarios to validate its feasibility and effectiveness in Section VI. Finally, Section VII gives the conclusions and future work.

II. THE DEFINITION OF COORDINATES
This section introduces several coordinates used for motion prediction of SVs and motion planning of ACV. As shown in Fig. 2, there are global coordinates system, road coordinate system and ego vehicle coordinate system. The global coordinate system is denoted by superscript G. The origin point of the global coordinate system is fixed. Its x axis points to the east, and the y axis points to the north. The local road frame is a curvilinear coordinate system. The Frenet frame is adopted for the road coordinate system, which is labeled with superscript F. The origin points of the local road frame is fixed to the right road boundary. Its x axis coincides with the right road boundary. The ego vehicle coordinate system is distinguished with superscript E, whose origin point coincides with the center of mass of ACV. The direction of x axis in the ego vehicle coordinate system is the same as that of velocity for ACV. SVs and ACV are regarded as particles making a curvilinear motion in a two-dimensional plane. In

III. MOTION PREDICTION FOR SURROUNDING VEHICLES
In this section, the motion prediction of SV is discussed based on the CTRA motion model. Then, taking into account the temporal-spatial relationship between SV's historical trajectory and geometry shape of the road boundary, a simplified maneuver recognition model is proposed. Motion prediction of SV is conducted on the basis of the simplified maneuver recognition model. Finally, IMMs is used for predicting SV's trajectory by integrating the CTRA motion model and the simplified maneuver recognition model.

A. MOTION PREDICTION OF SV BASED ON CTRA MOTION MODEL
Physics-based motion models view vehicles as dynamic entities governed by physical laws. Vehicle motion is described by correlations among motion states of vehicle. The most commonly used physics-based motion models are CV model, CA model, CTRV model, CTRA model. CV and CA models are one-dimensional, which assumes that the vehicle moves straightly with constant velocity or acceleration. CTRV and CTRA models are two-dimensional, which regards vehicle as a particle moving in a curvilinear line with constant yaw rate and velocity or constant yaw rate and acceleration. Obviously, two-dimensional models are much complete and more accurate in motion prediction. Consequently, the CTRA model is adopted for motion prediction of SVs in this work. Let x G o i be a state vector for surrounding vehicle i in the global coordinate system, as illustrated in Eq. (1). The present motion states information of SV can be obtained from active sensors equipped with ego vehicle or V2V between ego vehicle and SV.
is the tangential acceleration, and ω G o i is the yaw rate. In the CTRA motion model, the yaw rate and the acceleration are constant in the future. The state transition process is illustrated in Eq. (2), as shown at the bottom of the page. The statistical characteristic for process noise and measurement noise are illustrated in Eq. (2c). In particular, we adopt Kalman Filtering for system states prediction in the future instead of estimation of current system states. Since there are no measurement information in the future, the measurement equation is omitted in this work, where f(x G o i ,k ) is state transition function, w G o i ,k is process noise vector, w G o i ,a k is process noise for acceleration, w G o i ,ω k is process noise for yaw rate.
Both w G o i ,a k and w G o i ,ω k are zero-mean Gaussian white noise. Q G o i ,k is the non-negative covariance matrix for the process noise, t is the time interval of each step.
Since the transition equation involves sine items and cosine items, this process is nonlinear. Unscented Kalman Filter (UKF) is used for recursively estimating the state vector and the covariance matrix. UKF is a filter algorithm for approaching the posterior distribution of the nonlinear system based on Unscented Transformation (UT). In UT, several Sigma sampling points are chosen. These Sigma sampling points have the same mean and covariance of the system state distribution. The most important thing in UT is to determine the number, position and weight coefficients of Sigma sampling points. In this work, a symmetrical sampling strategy is adopted, as shown in Eq. (3). After the Sigma sampling points are obtained, the nonlinear transformation function f(·) is acted on them, as illustrated in Eq. 4. Finally, the state vector and covariance are calculated with Eq. (5). The prediction uncertainty of the state vector can be reflected in the covariance matrix.
where ξ i,k is Sigma sampling point,x G o i ,k is estimated state vector in the last moment, P G o i ,k is covariance matrix in the last moment, coefficient of sigma sampling point, W c i,k is the second-order weighting coefficient of sigma sampling point, λ is used to adjust the relative distance between sigma point and the state vector in the last moment.
is its covariance matrix, Q G o i ,k is process noise matrix.

B. MOTION PREDICTION OF SV BASED ON A SIMPLIFIED MANEUVER RECOGNITION MODEL
In maneuver-based vehicle trajectory prediction models, vehicle motion on the road network is associated with particular maneuvers, such as lane-keeping, lane change and turns. Each maneuver can be represented by a cluster of vehicle trajectories. In literature [27], the quaternion-based rotationally invariant longest common subsequence was adopted to measure similarities between trajectories. The radial basis function classified these trajectories into several types. Vehicle motion in the near future was predicted by matching its historical trajectory with these samples in the database collected in advance. In literature [28], the previously observed motion patterns were combined with Gaussian Mixture Models to infer a joint probability distribution as a motion model in the future. In summary, these maneuver recognition model based vehicle trajectory extremely rely on a large amount of training data. Meanwhile, the model is restricted to a specific road topology. In this work, we recognize maneuvers by the temporal-spatial correlation between the vehicle's historical trajectory and road geometry shape, which is independent of training data. After that, vehicle motion is predicted based on the recognized maneuver and its typical motion pattern.

1) MANEUVER RECOGNITION OF SV
Let x F o i be a state vector for SV i in the local road frame, as illustrated in Eq. (6).
where x F o i is the arc length along the right road boundary, y F o i is the lateral position relative to the right road boundary, v F o i ,x is the tangential velocity in road frame, v F o i ,y is the normal velocity in road frame, a F o i ,x and a F o i ,y are the tangential acceleration and the normal acceleration in road frame respectively.
Since y F o i represents SV's lateral position in the road frame, maneuvers such as lane-keeping, lane change left and lane change right can be distinguished by the time serial of lateral position y F o i . Consequently, we recognize maneuver by utilizing the temporal-spatial relationship between SV's past trajectory and road geometry shape. Taking into account the temporal sequence of SV's past trajectory, an integrated lateral position indicator is constructed, as shown in Eq. (7).
is the weighted lateral position in road frame after integrating several past lateral positions; α j is weighting coefficient, which allocates more importance to the nearer points in past trajectory; N is the number of steps.
Let ω L be the width of the lane. The sequence  are omitted in this work. In the lane-keeping maneuver-based motion prediction model, the longitudinal motion of SV is described by the CA model. The lateral motion is modeled as a continuous-time Ornstein-Uhlenbeck process, which means that the lateral position of SV converges to the middle of the current lane. The longitudinal and lateral motion of SV is illustrated in Eq. (9).
where l s is the projection length along the X F axis for the whole lane-change process, (x F o i ) * is the coordinate value of when the lane-change process starts, unknown parameters l s and (x F o i ) * can be calculated by nonlinear least-squares estimate with SV's historical trajectory in the near past. VOLUME 8, 2020

C. MOTION PREDICTION OF SV BASED ON IMM
The physics-based motion prediction model is accurate only in a short prediction horizon, less than one second. The maneuver-based motion prediction model achieves more precise performance in the long term. Combing physics and maneuver-based models for vehicle trajectory prediction gets more accurate prediction results in the whole prediction horizon. Aris et al. proposed a hierarchical-structured method fusing an adaptive kinematic model with the lane-keeping maneuver model in a linear weighting way for vehicle trajectory prediction [32]. In literature [31], the cubic spline curve was selected as a weighting function for combing the CTRA motion model with lane-keeping and lane-change maneuversbased motion models. The maneuver was recognized by a weighted distance between the vehicle's historical trajectory and the road centerline. Kim and Yi integrated a vehicle state filter, a road geometry filter and a path-following model for vehicle motion prediction, where the path-following model was adopted to generate the desired yaw rate in the future for the vehicle state filter [33]. Xie et al. proposed a vehicle trajectory prediction method by utlizing the CTRA motion model and the DBN-based maneuver recognition model [16].
For systems whose structure are unknown or parameters are variable, the hybrid-system-based multiple-models is a powerful adaptive estimation method. Multiple-modelsbased estimation usually involves the model set design, choosing filters, estimation fusion and filters re-initialization. How to re-initialize each filer in the model set is associated with the accuracy and time efficiency of the multiple-models method. IMMs is a typical fixed structure multiple-models method. Assuming the model transition obeys the Markov process, the performance of IMMs is the same as the secondorder generalized pseudo-Bayesian algorithm and its time efficiency is comparable with the first-order generalized pseudo-Bayesian algorithm. In this work, we propose a vehicle trajectory prediction method by fusing the CTRA model and the simplified maneuver recognition-based motion model with IMMs, as illustrated in Fig. 4. IMMs is a classic suboptimal algorithm for state estimate of the hybrid system [34]. When IMMs is used for state estimation, considering each sub-model in the model set may be an effective model at present, the initial condition of each sub-model is a composite of the prediction result of each filter at the previous moment. The overall state estimate vector and covariance are obtained by integrating all outputs from sub-models. There are two sub-models in this research. The one is the CTRA motion model. The other is a simplified maneuver recognition-based motion model. IMMs is recursive. It typically includes four steps: model-conditional re-initialization, model-conditional filtering, model probability update and estimate fusion. The details are the following.
Model-conditional re-initialization refers to re-calculating the input of each sub-model with both outputs from the last moment. Supposing the matching sub-model at the present time k is M i k , and the matching sub-model in the next moment k + 1 is M j k+1 , the mixed probability is illustrated in Eq. (11).
wherec j is the normalization factor, µ i k is the prior probability for sub-model i, π ij is model transfer probability.
After model-conditional re-initialization, the state vector and covariance matrix for each sub-model are shown in Eq. (12).
Given inputs from model-conditional re-initialization, model-conditional filtering means predicting vehicle trajectory with each sub-model, as shown in Eq. (13).
wherex j k+1|k is the estimate state vector from the sub-model j in the next moment k +1,P j k+1|k is the covariance matrix from the sub-model j in the next moment k + 1.
Model probability update refers to calculating the posterior probability of each sub-model, as illustrated in Eq. (14) µ j k+1 VOLUME 8, 2020 where µ j k+1 is the posterior probability, j k+1 is the possibility for sub-model matching with the hybrid system, P j k+1|k (x j k+1 ) is prediction variance in x axis for sub-model, P j k+1|k (y j k+1 ) is prediction variance in y axis for sub-model, is a normalization factor.
Estimate fusion means calculating the overall state vector and variance matrix with that of sub-models, as shown in Eq. (15) wherex k+1 is the overall estimated state vector for the hybrid system,P k+1 is the overall estimated covariance for the hybrid system.

D. SIMULATION ANALYSIS OF MOTION PREDICTION METHODS
In order to verify the effectiveness and superiority of the presented IMM-based motion prediction methods, we compare it with two commonly used motion prediction models: CTRA and Lane Keeping Model (LKM). The simulation scenario is defined in Fig.5.  We mainly focus on the prediction results for the lane change duration. The prediction horizon is 5s. The Root Mean Square Error (RMSE) is adopted as a metric for evaluating prediction accuracy. The prediction errors of three different approaches in the lane change process is shown in Table 2. Fig.7 depicts the prediction results for three different methods at 1s, 2s, 3s and 4s after lane change starts. As shown in Table 2 and Fig.7, the CTRA-based trajectory prediction method has a large prediction error in the whole lane change process. The trajectory prediction accuracy of the LKM model is low before the vehicle enters the target lane, as illustrated in Fig. 7.a, Fig. 7.b and Fig. 7.c. After the vehicle crosses the lane line, the prediction results of the LKM model is extremely close to that of IMM-based  approaches, as depicted in Fig. 7.d. Specifically, the proposed IMM-based motion prediction method has an excellent prediction accuracy in the whole process.

IV. RISK ASSESSMENT A. RISK ASSESSMENT INDICATOR
In the area of functional safety, the risk is defined as the combination of the probability of harm occurrence and the severity of that harm [35]. Researchers in intelligent transportation systems view risk as the expectation value of the cost related to a future critical event. The evaluation of risk usually includes two essential components, the probability of a critical event occurrence and its severity. Damerow and Eggert adopted several metrics such as Distance of Closest Encounter, Time of Closest Encounter and Time To Closest Encounter for evaluating the probability of collision event based on Gaussian function expression [36]. The partial elastic collision model was used to measure the severity of the collision event. Considering the lane-based probabilistic VOLUME 8, 2020 motion prediction for surrounding vehicles, Kim and Kum proposed a metric for assessing the collision probability between ego vehicle and surrounding vehicle based on vehicle shape and TTC [11]. In literature [37], the relative kinetic energy density between the ego vehicle and the preceding vehicle was employed for collision risk assessment in multi-vehicle collision avoidance scenarios. In literature [38], the projected area of ego vehicle along the direction of relative velocity was used as a metric for measuring the severity of the collision event.
Taking into account the temporal-spatial relationship between ACV and SV, an innovative metric is proposed for collision risk assessment. The probability of a collision event is measured by the longitudinal distance, lateral distance and the time headway between ACV and SV. The severity of a collision event is evaluated by the kinetic energy of ACV. The detailed expressions are illustrated in Eq. (16).
is the collision risk between ACV and SV i, P(x F e , x F o i , t) is the probability of a collision event, S(x F e , t) is the severity of a collision event, x F e is the state vector of ACV in road frame, x F o i is the state vector of SV i in road frame, m e is mass of ACV, α 1 , α 2 , α 3 and α 4 are parameters for adjusting the corresponding weight in collision risk. Fig. 8 shows the initial condition for a rear-end collision scenario. The initial speed of the ego vehicle is 25m/s. There is a static vehicle in front of the ego vehicle. The initial distance between the ego vehicle and the static vehicle is 80m. Fig.9 illustrates the collision risk map for rear-end avoidance with different braking accelerations. When the ego vehicle brakes with a low deceleration, the collision risk is high, as illustrated in Fig. 9. When the ego vehicle brakes with a high deceleration, the collision risk is lower at the end of the prediction horizon.

V. MOTION PLANNING OF ACV A. BEHAVIOR PLANNING OF ACV
Given the global reference path consisting of a serial of road sections, behavior planning means generating 209364 VOLUME 8, 2020  reasonable behaviors for guiding ACV along the global reference path based on the perception of other traffic participants, road conditions and traffic signs. Since the driving environment and optional behaviors can be represented as several sets, each behavior is indicated as a particular state in a finite state machine. In this paper, we model the behavior selection process in highway scenarios with the finite state machine, as illustrated in Fig. 10. The motion of SVs is predicted with methods introduced in section III. The motion prediction of ACV is constrained by an optimized control vector, which is presented in part B of section V in detail.
The normal behaviors in highway scenarios include lane keeping and lane change. Lane-keeping can be further divided into cruising, following and leading based on the relative position between ACV and SVs. When there is a SV in the front of ACV, ACV is assigned the following behavior, as shown in Fig. 11.a. The reference lane and reference speed for ACV in the following behavior are illustrated in Eq. (17a). When there is a SV approaching ACV from behind, the given behavior for ACV is leading, as indicated in Fig. 11.b. The reference lane and reference speed for ACV are shown in Eq. (17b). If there is no SV around ACV in its current lane, the behavior for ACV is cruising, as illustrated in Fig 11.c. The reference lane and reference speed for ACV are shown in Eq. (17c). If the reference speed in the adjacent lane approaches the desired speed more close than that of the current lane, a lane change behavior is assigned for ACV, as illustrated in Fig. 12. In this case, the setting of the reference lane and reference speed is shown in Eq. (18).

B. TRAJECTORY PLANNING OF ACV BASED ON MPC
Due to its superiority in dealing with the constrained optimization problems, MPC has been widely used for trajectory planning and trajectory tracking of ACV [10], [39]- [44]. In this work, taking into account the motion prediction of SVs and ACV, a predictive trajectory planning framework is proposed for ACV. It incorporates internal and external constraints resulted from kinematic and kinetic constraints of ACV, traffic rules and safe distance between ACV and  SVs. Based on the MPC framework, the trajectory of ACV is planned in a predictive way in order to adapt to the dynamic traffic environments.

1) MOTION PREDICTION OF ACV
The ACV is regarded as a particle moving in a curvilinear way. The position of the particle coincides with the mass point of ACV. Let x e be s state vector representing motion of ACV, as illustrated in Eq. (19).
where x F e is the longitudinal position of ACV in the road frame, y F e is the lateral position of ACV in the road frame, v G e is the velocity of ACV, a G e is the acceleration of ACV, ψ F e is the heading angle of ACV in the road frame,ψ G e is the yaw rate of ACV in the global coordinate system.
With the control input of the desired acceleration and desired yaw rate, the state transition process of ACV is shown in Eq. (20).
where a G des andψ G des are control inputs from the solution of MPC problem, a G des is desired acceleration,ψ G des is desired yaw rate, ψ R is the angle between x axis of road frame and x axis of the global coordinate system,ψ R is the turn rate of road frame in the global coordinate system, T a and Tψ are time constants when the lower dynamic control system of ACV is simplified as a first-order system [33].
The rotation of the road frame is illustrated in Eq. (21).
where κ(x F e ) is the curvature of the right road boundary in x F e , the road geometry shape is known in advance.

2) CONSTRAINTS
For generating a safe, comfortable and feasible trajectory for ACV, it's essential to take into account internal and external constraints, which involves kinematic and kinetic constraints of ACV, traffic rules and safe distance constraint between SVs and ACV. Internal constraints include kinematic and kinetic constraints of ACV. Kinematic constraint means that the curvature of the ACV should be bounded, as illustrated in Eq. (23). Kinetic constraint means that ACV must satisfy the tire-road friction ellipse condition, as shown in Eq. (24).
where κ max is the maximum curvature. where µ H is the peak adhesion coefficient, a E e,x is the tangential acceleration of ACV, a E e,y is the normal acceleration of ACV, f e,y is a scale factor for normal acceleration, ξ g is a relaxation factor used to make ACV stay in the stable area.
External constraints refer to the influence of traffic rules and SVs. The speed of ACV in highway scenarios should be bounded, as illustrated in Eq. (25). Meanwhile, the ACV should stay within the drivable area, as shown in Eq. (26). ACV should keep a safe distance with SVs. The vehicle shape and position prediction uncertainty should also be considered, as illustrated in Eq. (27).
where v max is the maximum speed, v min is the minimum speed.
where y F min represents the right road boundary, y F max represents the left road boundary.
where x F e,o i is the minimum safe distance in x axis of road frame between ACV and SV i, y F e,o i is the minimum safe distance in y axis of road frame between ACV and SV i, L e is the length of ACV, W e is the width of ACV, L o i is the length of SV i, W o i is the width of SV i, σ F o i ,x and σ F o i ,y are standard deviations in x and y axes of road frame for SV i respectively, which represents the position prediction uncertainty.

3) MPC PROBLEM FORMULATION
On the basis of internal and external constraints, the trajectory planning of ACV is formulated as solving a constrained optimization problem, as illustrated in Eq. (28). The first item in the objective function means minimizing the reference path tracking error. The second item in objective function refers to minimizing energy consumption of control inputs. The third item in the objective function represents minimizing collision risk between ACV and SVs. The motion of ACV is subject to equality constraints and inequality constraints. The equality constraints are the state transfer process of ACV, as illustrated in Eq. (20). The inequality constraints indicate the influence of SVs and traffic rules on ACV, as shown in Eq. (23)- (27).
st.ẋ e = f e x e,k , u k , t k c x e,k , x o i ,k ≤ 0 where N p is the number of control steps, N o is the number of surrounding vehicles, y F e,k represents the reference lane, v E e,k is the reference speed, y F e,k and v E e,k determine the reference path for ACV, which are given from the behavior generation module, P, Q, R are weighted matrixes for tracking error, control energy consumption and collision risk respectively.

A. PARAMETERS DEFINITION
The proposed predictive trajectory planning method is applied to two scenarios to validate its effectiveness and feasibility, as illustrated in Fig. 13 and Fig. 22. The parameters for motion prediction of SVs with uncertainty are defined in Table 3. The parameters for MPC-based trajectory planning are shown in Table 4.

B. SCENARIO 1 SIMULATION RESULTS ANALYSIS
As shown in Fig. 13, scenario 1 is a straight road three-lane scene. The initial state parameters of ACV and SVs are defined in Table 5. At the initial moment, SV1 is moving in VOLUME 8, 2020   from the trajectory planning of ACV in the whole process, we analyze the collision risk between ACV and SVs at three moments t = 0s, 10s, 15s under different control inputs of acceleration and yaw rate for ACV. The trajectories of SVs are predicted by IMM. The motion of ACV is determined by state transfer equation and control inputs, as illustrated in Eq. (20). The collision risk between ACV and SVs is calculated with Eq. (16). The length of the prediction time window is 5s.
At t = 0s, the ACV's current position and the predicted trajectories of SVs are shown in Fig. 14.a. All SVs stay in their original lanes and make lane-keeping maneuvers. SV1 and SV3 are moving at a constant speed. SV2 slows down. Fig. 15 indicates the collision risk in the prediction horizon between ACV and SVs with a different control input of acceleration for ACV. When the acceleration control input is positive, the longitudinal distance between ACV and   SV2 decreases with time. As a consequence, the collision risk between ACV and SV2 increases with time under positive acceleration control input, as shown in Fig. 15. When the yaw rate control input is negative, ACV turns right towards lane 1. The lateral distance between ACV and SV1 decreases with time at first and then increase. That results in a peak value for collision risk between ACV and SV1 at a certain moment, as illustrated in Fig. 16.a. When the yaw rate control input is positive, ACV makes left turns. The distance lateral between ACV and SV3 firstly decreases and then increases. The collision risk between ACV and SV3 achieves high value with positive yaw rate input, as depicted in Fig. 16.c. No matter the yaw rate input is positive or negative, the lateral distance between ACV and SV2 increases with time under non-zeros yaw rate input. As a result, the collision risk between ACV and SV2 appears minimum values at the end of the prediction time horizon, as illustrated in Fig. 16.b.
At t = 10s, Fig. 14 Fig. 17. When there is a positive yaw rate input for ACV, the distance between   ACV and SV2 or SV3 firstly decreases and then increases. The collision between ACV and SV2 or SV3 reaches maximum values with positive yaw rate inputs, as illustrated in Fig. 18.b and Fig. 18.c. When the yaw rate input is nonzero, ACV makes left or right turns. The lateral distance between ACV and SV1 increases. Therefore, the collision between ACV and SV1 decreases with time under non-zero yaw rate inputs, as depicted in Fig. 18.a.
At t = 15s, ACV and SV1 are moving in lane 1, as shown in Fig. 14.c. The longitudinal distance between ACV and SV1 is smaller than that at t = 10s. SV2 has become static. SV3 makes a lane-keeping maneuver in lane 3. When the acceleration control input is negative, ACV brakes in lane 1. The longitudinal distance between ACV and SV1 or SV3 increases with time. The collision risk between ACV and SV1 or SV3 reaches a maximum value at the beginning of the prediction horizon, as illustrated in Fig. 19.a and Fig. 19.c. When the acceleration for ACV is large enough, ACV will take overtake SV1 or SV3 in a short time, the longitudinal distance between ACV and SV1 or SV3 firstly decreases and then increases, which leads to a lower value at the end of the prediction window. No matter the acceleration control input is negative or positive, the longitudinal distance between ACV and SV2 increases with time. As a result, the collision risk between ACV and SV2 decreases with time, as shown in Fig. 19. b. The ACV turns left with a positive yaw rate input. The lateral between SV2 or SV3 achieves a minimum value at a certain moment, which makes the collision risk between ACV and SV2 or SV3 reaches the maximum, as depicted in Fig. 20.b and Fig. 20.c. The collision risk between ACV and SV1 decreases with time under non-zero yaw rate inputs, as illustrated in Fig. 20.a.
The planned trajectory for ACV in scenario 1 is depicted in Fig. 21. ACV moves from lane 2 to lane 1 at the beginning. Then, ACV follows SV1 in lane 1. After ACV has overtaken SV2 in the longitudinal direction, ACV moves back to lane 2. Finally, ACV is moving at a speed of 30m · s −1 . Throughout the whole process, the lateral and longitudinal acceleration keeps inside the tire-road friction ellipse. The maximum lateral acceleration is 2.8m · s −2 .
C. SCENARIO 2 SIMULATION RESULTS ANALYSIS Scenario 2 is a three-lane curve road scene, as shown in Fig. 22. The initial condition for ACV and SVs is indicated in Table 6. SV1 and SV2 are moving in lane 1. SV1 is 100m in front of ACV. At t = 4s, SV1 makes a left lane change from lane 1 to lane 2. The lane change duration is about 4 s. SV2 is 100m behind ACV, moving at a speed of 30m · s −1 . SV3 is moving in lane 3 at a speed of 35m · s −1 . The collision risk between ACV and SVs at t = 0s, 5s, 10s under different acceleration or yaw rate control inputs is described as follows. The ACV's current position and SVs' predicted trajectories in the prediction horizon at t = 0s are depicted in Fig. 23.a. SV1 and SV2 are moving in lane 1. SV1 is in front of SV2. SV3 moves in lane 3 at a constant speed. When the acceleration control input for ACV is negative, the longitudinal distance between ACV and SV2 or SV3 decreases with time. The collision risk between ACV and SV2 or SV3 increases with time under negative acceleration inputs, as illustrated in Fig. 24. b and Fig. 24.c. Since positive acceleration leads to the decreasing longitudinal distance between ACV and SV1, the collision risk between ACV and SV1 increases with time under positive acceleration inputs, as shown in Fig. 24.a. The ACV turns right with negative yaw rate inputs. The distance between ACV and SV1 or SV2 will achieve a minimum at a certain moment, which leads to the collision risk between ACV and SV1 or SV2 reaches the maximum at that point, as depicted in Fig. 25.a and Fig. 25.b. Similarly, the collision risk between ACV and SV3 becomes high at a particular moment in the prediction horizon under positive yaw rate inputs.
At t = 5s, the ACV's current position and SVs' predicted trajectories in the prediction horizon are depicted in Fig. 23.b SV1 is moving from lane 1 to lane 2. SV2 and SV3 still keep in original lanes. Fig. 26 and Fig. 27 depict the collision risk with all SVs under different acceleration or yaw rate control inputs. In this case, the collision risk with SV3 is obviously smaller than that of SV1 and SV2. The collision risk with all SVs appears peak values at the beginning of the prediction horizon.
At t = 10s, ACV has made a right lane change from lane 2 to lane 1, moving in front of SV2, as depicted in Fig. 23.c. SV1 keeps in lane 2. SV2 is moving in lane 1. SV3 keeps in lane 3. The longitudinal distance between ACV and SV1 decreases with time under positive acceleration inputs, which results in the collision risk between ACV and SV1 increases with time, as depicted in Fig. 28.a. When the acceleration input is negative, the distance between ACV and      SV2 firstly decreases and then increases. The collision risk between ACV and SV2 achieves the minimum at the end of prediction under negative acceleration inputs, as illustrated in Fig. 28. b. Since the lateral distance between ACV and SV3 is larger than others, the collision risk between ACV and SV3 is relatively smaller, as depicted in Fig. 28.c. ACV makes VOLUME 8, 2020 left turns with positive yaw rate control input. The lateral distance between ACV and SV1 or SV3 firstly decreases and then increases, which leads to the collision risk between ACV and SV1 or SV3 reaches the maximum at a certain moment in the prediction horizon, as illustrated in Fig. 29.a and Fig. 29.c. When the yaw rate control input is non-zero, the lateral distance between ACV and SV2 increases with time. As a consequence, the collision risk between ACV and SV2 achieves the minimum at the end of the prediction window, as shown in Fig. 29.c.
The trajectory planning results for ACV in scenario 2 is depicted in Fig. 30. In the beginning, ACV keeps in lane 2. At t = 4.3s, ACV starts moving from lane 2 to lane 1 in order to avoid colliding with SV1 in the next few moments. After ACV ends its lane-change maneuver, ACV keeps in lane 1 at a speed of 30m · s −1 . In the whole process, the maximum lateral acceleration of ACV is 2.6m · s −2 , which satisfies the tire-road friction ellipse constraint.

VII. CONCLUSION
In this work, a predictive trajectory planning method for ACV is proposed based on the motion prediction of SVs. Firstly, the IMM is used for predicting the motion of SV by integrating the CTRA motion model and the simplified maneuver-based motion model. Specifically, the maneuver is recognized through the temporal-spatial revelence between the historical trajectory and road geometry shape, which is independent of training data. Then, a risk indicator is constructed for collision risk assessment, which involves the possibility of a collision event and the severity of a collision event. After that, taking account the internal and external constraints resulted from kinematic and kinetic characteristics of ACV, traffic rules and safe distance restrict between ACV and SVs, the trajectory planning of ACV is formulated as a constrained optimization problem based on the MPC framework. Finally, the proposed predictive trajectory planning method is applied to two scenarios to validate its effectiveness and feasibility. In future work, predictive trajectory planning methods should involve an interaction-aware motion prediction model for SVs. Since 2015, he has been an Assistant Professor with the School of Automotive Studies, Tongji University. His research interests include automotive noise and vibration control, vehicle dynamic control, and sensor fusion.