Driver intention prediction and intention trajectory updating method

Abstract

The application provides a driver intention prediction and intention trajectory updating method, which firstly performs trajectory prediction based on a CTRV model, takes a vehicle state at a current time as an initial condition, and has high accuracy in predicting a future trajectory of the vehicle and an operation intention of the driver in a short time range, and then obtains a long-term intention trajectory conforming to ride comfort according to the driver intention based on a five polynomial trajectory of a structured road design. Through combination of the two, a long-term and short-term comprehensive prediction trajectory of the vehicle can be accurately expressed. The method judges the operation intention of the driver in each prediction period, and dynamically updates the intention trajectory, thereby avoiding human-machine conflict problems caused by active lane changing and obstacle avoidance of the driver, and avoiding excessive intervention problems of the auxiliary system on the driver.

Description

Technical Field

[0001] This invention belongs to the technical field of vehicle assisted steering control systems, and specifically relates to a method for predicting driver intent and updating intent trajectory. Background Technology

[0002] Currently, vehicle steering technology has made some progress in reducing the driver's handling burden on structured roads, improving lane keeping ability and safety performance, and avoiding collision risks and vehicle instability. However, considering the non-uniformity of driver operations on ordinary roads, some existing technologies that simply track the road centerline based on a fixed target trajectory are clearly not in line with drivers' driving habits. Partially avoiding conflicts between the steering system's operation and driver input actually increases driver fatigue and leads to distrust in the safety of the steering system, thus violating the original purpose of steering assistance. Therefore, there is an urgent need in this field to more accurately predict and update the driver's true intentions when driving on ordinary roads. Summary of the Invention

[0003] In view of this, and to address the technical problems existing in this field, the present invention provides a method for predicting driver intent and updating intent trajectory, specifically including the following steps:

[0004] S1. Input the current time k, including the vehicle's coordinates, heading angle, lateral acceleration, and yaw rate in the global coordinate system. Based on the CTRV kinematic model, predict the driving state within several sampling intervals after the current time. Transform the prediction results into the Frenet coordinate system to obtain the driver's short-term intention trajectory.

[0005] S2. Using the short-term intention trajectory of the driver obtained in step S1, determine whether the driver's operation intention is to stay in the original lane, change to the left lane, or change to the right lane.

[0006] S3. For the lateral displacement corresponding to the start and end points in the short-term intention trajectory, establish a fifth-order polynomial in terms of time, and solve the polynomial coefficients based on the minimum cumulative rate of change of lateral acceleration during the lane change period; combine the solved lateral displacement with the longitudinal displacement in the short-term intention trajectory to determine the driver's long-term intention trajectory in the Frenet coordinate system.

[0007] S4. The final driver intention prediction trajectory is obtained by weighted fusion of the driver's short-term intention trajectory and long-term intention trajectory, and the weights are calculated using experimental data or machine learning algorithms.

[0008] S5. Repeat steps S1 to S4 above to periodically and continuously update the driver's intention prediction trajectory.

[0009] Furthermore, in step S1, the vehicle's driving state at time k is defined in the following form:

[0010]

[0011] In the formula, x and y are the horizontal and vertical coordinates of the vehicle in the global coordinate system, respectively. v y w r Let be the vehicle's current heading angle, lateral velocity, and yaw rate, respectively. Based on the CTRV kinematic model, the predicted driving state at time k+1 is:

[0012]

[0013] In the formula, τ is the sampling time; by performing the above iterations, the driving state within the time interval n·τ is predicted:

[0014] Π p ={Π(k+1),Π(k+2),...,Π(k+n)}

[0015] Transforming the prediction results to the Frenet coordinate system yields the driver's short-term intention trajectory over time n·τ:

[0016] Ψ CTRV =[Ψ CTRV,1 ,Ψ CTRV,2 ,...Ψ CTRV,i ,...,Ψ CTRV,n ]

[0017] Ψ CTRV,i =[e li ,e ri ] T i = 1, 2, ..., n

[0018] In the formula, Ψ CTRV,i This represents the driving state at each sampling time in the Frenet coordinate system, e li e ri These represent the vehicle's coordinates and the distances to the left and right boundaries of the current lane, respectively.

[0019] Furthermore, in step S2, the vehicle's positional characteristics are first defined as follows:

[0020]

[0021] In the formula, e l e r , ρ fThese represent the distances between the vehicle's coordinates and the left and right boundaries of the current lane, the vehicle's heading deviation relative to the tangent at the mapping point in the current lane, and the curvature of the vehicle's trajectory, respectively. The curvature of the vehicle's trajectory is specifically expressed as:

[0022]

[0023] At time k, the mapping point of the vehicle on the centerline of the current lane (here, the shortest Euclidean distance from the vehicle to the reference point of the lane centerline) also has the same positional characteristics as the vehicle's position, denoted as:

[0024]

[0025] For the location features of the mapped point within the prediction time, P is approximately calculated using the current vehicle speed as the speed along the lane tangent. ref (k+1), i = 1, 2, ..., n;

[0026] The correlation Ω between the short-term intention trajectory at time k and the lane centerline x(k) Recorded as:

[0027]

[0028] In the formula, Q and S are the weighting coefficient and normalization coefficient, respectively. The specific weights and normalization coefficients can be obtained from specific experiments or learned and identified through learning-based algorithms. For simplicity, this invention linearly normalizes each quantity within its domain and assigns the same weight.

[0029] Within the time range of n·τ, considering that the kinematic prediction based on the current driving state will accumulate errors over time, an exponentially decreasing weighting coefficient is used to calculate the correlation between some predicted segments and the current road centerline in the short-term intention trajectory:

[0030]

[0031]

[0032] In the formula, h is the number of predicted segments;

[0033] To avoid fluctuations in short-term intended trajectory prediction caused by erroneous measurements or frequent changes in heading angle, the prediction is delayed by m update steps, expressed as:

[0034]

[0035] The driver's intention is determined by comparing Ω with a specific threshold. If Ω does not exceed the threshold, it is assumed that the driver wants to stay in the original lane. If Ω is greater than the threshold, it is determined whether the driver will change to the left or right lane.

[0036] Furthermore, in step S3, for the lateral displacement d corresponding to the start point to the end point in the short-term intention trajectory, the following fifth-degree polynomial with respect to time t is established:

[0037] d(t)=c5t 5 +c4t 4 +c3t 3 +c2t 2 +c1t+c0

[0038] In the formula, c0 to c5 are polynomial coefficients;

[0039] By combining the relationship between longitudinal displacement and time t, the driver's long-term intention trajectory in the Frenet coordinate system can be obtained;

[0040] The following cost function is used to construct the long-term intent trajectory:

[0041] J traC =η Tc T c +η a max(|a y (t)|)

[0042] In the formula, η Tc η a All are positive weighting coefficients; the two terms on the right side of the equation are the time cost and the penalty term corresponding to the maximum lateral acceleration, respectively; take the lane-changing time T. c for:

[0043] T c =[T c,min ,T c,min +iT c,s ,...,T c,max ], i>0

[0044] In the formula, T c,s Let be the sampling time step; after solving the above fifth-order polynomial to obtain a set of candidate trajectories, the optimal cost path is then obtained based on the cost function, which serves as the long-term intent trajectory; after transformation to the Frenet coordinate system, the following is obtained:

[0045] Ψ Op =[Ψ Op,1 ,Ψ Op ,2,...,Ψ Op ,i...,Ψ Op,n ]

[0046] Ψ Op,i =[e li ,e ri ] T i = 1, 2, ..., n

[0047] In the formula, Ψ Op,i This represents the driving status at each sampling moment in the long-term intention trajectory.

[0048] Furthermore, step S4 weights and fuses the driver's short-term intention trajectory and long-term intention trajectory based on the following formula:

[0049] Ψ p =g(t)Ψ CTRV +(1-g(t))Ψ Op

[0050] In the formula, the weight g(t) is calculated using experimental data or machine learning algorithms to obtain the final driver intention prediction trajectory Ψ. p .

[0051] The driver intention prediction and intention trajectory update method provided by the present invention firstly uses trajectory prediction based on a CTRV model, taking the current vehicle state as the initial condition. This method achieves high accuracy in predicting the driver's operational intention and the vehicle's future trajectory within a short timeframe. Then, based on a fifth-order polynomial trajectory derived from structured road design, a long-term intention trajectory that conforms to passenger comfort is obtained according to the driver's intention. By combining these two methods, the comprehensive prediction trajectory of the vehicle in both the short and long term can be expressed relatively accurately. The method judges the driver's operational intention in each prediction cycle and dynamically updates the intention trajectory, avoiding human-machine conflict caused by the driver actively changing lanes or avoiding obstacles, and also avoiding excessive intervention by the driver assistance system. Attached Figure Description

[0052] Figure 1 This is a flowchart illustrating the method provided by the present invention;

[0053] Figure 2 This is a schematic diagram illustrating the principle of weight g(t) calculation in the method provided by the present invention;

[0054] Figure 3 This is a schematic diagram illustrating the principle of dynamically updating the driver's intention trajectory based on the method of the present invention. Detailed Implementation

[0055] The technical solution of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0056] The present invention provides a method for predicting driver intent and updating intent trajectory, such as... Figure 1 As shown, the specific steps include:

[0057] S1. Input the current time k, including the vehicle's coordinates, heading angle, lateral acceleration, and yaw rate in the global coordinate system. Based on the CTRV kinematic model, predict the driving state within several sampling intervals after the current time. Transform the prediction results into the Frenet coordinate system to obtain the driver's short-term intention trajectory.

[0058] S2. Using the short-term intention trajectory of the driver obtained in step S1, determine whether the driver's operation intention is to stay in the original lane, change to the left lane, or change to the right lane.

[0059] S3. For the lateral displacement corresponding to the start and end points in the short-term intention trajectory, establish a fifth-order polynomial in terms of time, and solve the polynomial coefficients based on the minimum cumulative rate of change of lateral acceleration during the lane change period; combine the solved lateral displacement with the longitudinal displacement in the short-term intention trajectory to determine the driver's long-term intention trajectory in the Frenet coordinate system.

[0060] S4. The final driver intention prediction trajectory is obtained by weighted fusion of the driver's short-term intention trajectory and long-term intention trajectory, and the weights are calculated using experimental data or machine learning algorithms.

[0061] S5. Repeat steps S1 to S4 above to periodically and continuously update the driver's intention prediction trajectory.

[0062] In a preferred embodiment of the present invention, in step S1, the driving state of the vehicle at time k is defined as follows:

[0063]

[0064] In the formula, x and y are the horizontal and vertical coordinates of the vehicle in the global coordinate system, respectively. v y w r Let be the vehicle's current heading angle, lateral velocity, and yaw rate, respectively. Based on the CTRV kinematic model, the predicted driving state at time k+1 is:

[0065]

[0066] In the formula, τ is the sampling time; by performing the above iterations, the driving state within the time interval n·τ is predicted:

[0067] Π p ={Π(k+1),Π(k+2),...,Π(k+n)}

[0068] Transforming the prediction results to the Frenet coordinate system yields the driver's short-term intention trajectory over time n·τ:

[0069] Ψ CTRV =[Ψ CTRV,1 ,Ψ CTRV,2 ,...Ψ CTRV,i ,...,Ψ CTRV,n ]

[0070] Ψ CTRV,i =[e li ,e ri ] T i = 1, 2, ..., n

[0071] In the formula, Ψ CTRV,i This represents the driving state at each sampling time in the Frenet coordinate system, e li e ri These represent the vehicle's coordinates and the distances to the left and right boundaries of the current lane, respectively.

[0072] In a preferred embodiment of the present invention, the positional characteristics of the vehicle are first defined in step S2 as follows:

[0073]

[0074] In the formula, e l e r , ρ f These represent the distances between the vehicle's coordinates and the left and right boundaries of the current lane, the vehicle's heading deviation relative to the tangent at the mapping point in the current lane, and the curvature of the vehicle's trajectory, respectively. The curvature of the vehicle's trajectory is specifically expressed as:

[0075]

[0076] At time k, the mapping point of the vehicle on the centerline of the current lane (here, the shortest Euclidean distance from the vehicle to the reference point of the lane centerline) also has the same positional characteristics as the vehicle's position, denoted as:

[0077]

[0078] For the location features of the mapped point within the prediction time, P is approximately calculated using the current vehicle speed as the speed along the lane tangent. ref (k+1), i = 1, 2, ..., n;

[0079] The correlation Ω between the short-term intention trajectory at time k and the lane centerline x(k) Recorded as:

[0080]

[0081] In the formula, Q and S are the weighting coefficient and normalization coefficient, respectively. The specific weights and normalization coefficients can be obtained from specific experiments or learned and identified through learning-based algorithms. For simplicity, this invention linearly normalizes each quantity within its domain and assigns the same weight.

[0082] Within the time range of n·τ, considering that the kinematic prediction based on the current driving state will accumulate errors over time, an exponentially decreasing weighting coefficient is used to calculate the correlation between some predicted segments and the current road centerline in the short-term intention trajectory:

[0083]

[0084]

[0085] In the formula, h is the number of predicted segments;

[0086] To avoid fluctuations in short-term intended trajectory prediction caused by erroneous measurements or frequent changes in heading angle, the prediction is delayed by m update steps, expressed as:

[0087]

[0088] The driver's intention is determined by comparing Ω with a specific threshold. If Ω does not exceed the threshold, it is assumed that the driver wants to stay in the original lane. If Ω is greater than the threshold, it is determined whether the driver will change to the left or right lane.

[0089] In a preferred embodiment of the present invention, in step S3, for the lateral displacement d corresponding to the start point to the end point in the short-term intention trajectory, the following fifth-degree polynomial with respect to time t is established:

[0090] d(t)=c5t 5 +c4t 4 +c3t 3 +c2t 2 +c1t+c0

[0091] In the formula, c0 to c5 are polynomial coefficients;

[0092] By combining the relationship between longitudinal displacement and time t, the driver's long-term intention trajectory in the Frenet coordinate system can be obtained;

[0093] The following cost function is used to construct the long-term intent trajectory:

[0094] J traC =η Tc T c +η a max(|a y (t)|)

[0095] In the formula, η Tc η a All are positive weighting coefficients; the two terms on the right side of the equation are the time cost and the penalty term corresponding to the maximum lateral acceleration, respectively; take the lane-changing time T. c for:

[0096] T c =[T c,min ,T c,min +iT c,s ,...,T c,max ], i>0

[0097] In the formula, T c,s Let be the sampling time step; after solving the above fifth-order polynomial to obtain a set of candidate trajectories, the optimal cost path is then obtained based on the cost function, which serves as the long-term intent trajectory; after transformation to the Frenet coordinate system, the following is obtained:

[0098] Ψ Op =[Ψ Op,1 ,Ψ Op,2 ,...,Ψ Op,i ...,Ψ Op,n ]

[0099] Ψ Op,i =[e li ,e ri ] T i = 1, 2, ..., n

[0100] In the formula, Ψ Op,i This represents the driving status at each sampling moment in the long-term intention trajectory.

[0101] Furthermore, step S4 weights and fuses the driver's short-term intention trajectory and long-term intention trajectory based on the following formula:

[0102] Ψ p =g(t)Ψ CTRV +(1-g(t))Ψ Op

[0103] In the formula, the weight g(t) is calculated using experimental data or machine learning algorithms, and its specific form is as follows: Figure 2 The curve function form shown yields the final driver intention prediction trajectory Ψ. p .

[0104] Figure 3 The continuous dynamic update process of the method provided by the present invention is illustrated.

[0105] It should be understood that the sequence number of each step in the embodiments of the present invention does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0106] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A driver intent prediction and intent trajectory update method, characterized by: Specifically comprising the following steps: S1, input the driving state of the vehicle at the current k moment including the coordinates of the vehicle in the global coordinate system, the heading angle, the lateral acceleration and the yaw rate, predict the driving state in the next sampling interval based on the CTRV kinematics model, and convert the prediction result to the Frenet coordinate system to obtain the short-term intention trajectory of the driver; The specific process includes: defining the driving state of the vehicle at the k moment as follows: In the formula, x, y are respectively the lateral and longitudinal coordinates of the vehicle in the global coordinate system, v y , w r are respectively the current heading angle, lateral velocity and yaw rate of the vehicle, then according to the CTRV kinematics model, the driving state at k+1 time is predicted as: In the formula, τ is the sampling time; by executing the above iteration, the driving state in n·τ time is predicted: Π p = {Π(k+1),Π(k+2),...,Π(k+n)} Convert the prediction result to the Frenet coordinate system to obtain the short-term intention trajectory of the driver in n·τ time: Ψ CTRV = [Ψ CTRV,1 ,Ψ CTRV,2 ,...,Ψ CTRV,i ,...,Ψ CTRV,n ] Ψ CTRV,i = [e li , e ri ] T , i = 1, 2,..., n In the formula, Ψ CTRV,i represents the driving state of each sampling time under the Frenet coordinate system, e li , e ri respectively represent the distance between the vehicle coordinate and the left and right boundaries of the current lane. S2, using the short-term intention trajectory of the driver obtained in step S1, judging the operation intention of the driver, which is one of the original lane driving, changing to the left lane and changing to the right lane; S3, for the lateral displacement corresponding to the start point to the end point in the short-term intention trajectory, a quintic polynomial about time is established, and the polynomial coefficients are solved based on the minimum cumulative lateral acceleration change rate during lane changing; combining the solved lateral displacement and the longitudinal displacement in the short-term intention trajectory, the long-term intention trajectory of the driver in the Frenet coordinate system is determined; S4, based on the weighted fusion of the short-term intention trajectory and the long-term intention trajectory of the driver, the final driver intention prediction trajectory is obtained, and the weight is calculated by using test data or machine learning algorithm; S5, repeatedly executing the preceding steps S1 to S4, periodically and continuously updating the driver intention prediction trajectory.

2. The method of claim 1, wherein: In step S2, the position characteristics of the vehicle are first defined as: where e l , e r , ρ f denote the distance of the vehicle coordinate from the left and right boundaries of the current lane, the heading deviation of the vehicle with respect to the tangent at the mapping point in the current lane, and the curvature of the vehicle trajectory, respectively, where the curvature of the vehicle trajectory is specifically expressed as: At k moment, the mapping point of the vehicle on the center line of the current lane also has the same position characteristics as the vehicle position, denoted as: Then for the mapping point position feature in the prediction time, the approximation is calculated with the current vehicle speed as the speed along the lane tangent ref (k + 1), i = 1, 2,..., n; The relevance Ω between the short-term intention trajectory at time k and the lane center line x(k) is denoted as: In the formula, Q and S are weight coefficient and normalization coefficient respectively, and the specific weight and normalization coefficient are obtained according to specific test or learned and identified by learning-based algorithm; In the time range of n·τ, the correlation degree of part of the prediction segment in the short-term intention trajectory with the current road center line is calculated by using exponentially decreasing weight coefficient: In the formula, h is the number of prediction segments; The prediction is delayed by m update steps, which is expressed as: By comparing Ω with a specific threshold, the operation intention of the driver is judged, when Ω does not exceed the threshold, it is considered that the driver wants to keep driving in the original lane, when Ω is greater than the threshold, it is judged which one of the left lane or the right lane will be changed to.

3. The method of claim 2, wherein: In step S3, for the lateral displacement d corresponding to the start point to the end point in the short-term intention trajectory, the following quintic polynomial about time t is established: d(t) = c5t 5 + c4t 4 + c3t 3 + c2t 2 + c1t + c0 In the formula, c0~c5 are polynomial coefficients; Combining the relationship between longitudinal displacement and time t, the long-term intention trajectory of the driver in the Frenet coordinate system is obtained; The following cost function of long-term intention trajectory is constructed: where ηT c , ηL a are positive weight coefficients, and the two terms on the right-hand side are the penalties for time spent and maximum lateral acceleration, respectively; and the lane change time T c is given by: T c = [T c,min , T c,min , iT c,s ,..., T c,max ], i > 0 In the formula, T c,s Let be the sampling time step; after solving the above fifth-order polynomial to obtain a set of candidate trajectories, the optimal cost path is then obtained based on the cost function, which serves as the long-term intent trajectory; after transformation to the Frenet coordinate system, the following is obtained: P Op =[Ψ Op,1 ,P Op,2 ,...,Ψ Op,i ...,P Op,n ] Ψ Op,i = [e li , e ri ] T , i = 1, 2,..., n In the formula, Ψ Op,i is the driving state of each sampling time in the long-term intention trajectory.

4. The method of claim 3, wherein: In step S4, the short-term intention trajectory and the long-term intention trajectory of the driver are weighted and fused based on the following formula: Ψ p = g(t) Ψ CTRV + (1 - g(t)) Ψ Op wherein the weights g(t) are calculated using test data or a machine learning algorithm to obtain a final driver intent prediction trajectory Ψ p .

Images