Human-machine shared control based on trajectory prediction

Abstract

The present invention relates to the field of intelligent vehicles, and more specifically, to a human-machine shared control method based on trajectory prediction. It comprises: establishing a road-vehicle model based on vehicle-related parameters and then designing a path-tracking controller; further predicting the driver's driving trajectory, evaluating the driving risk, and switching the control power between the automatic system and the human driver based on the driving risk. The present invention ensures that when the driver's inputs will cause high risk to the vehicle, the driver's input is deprived of; and that when the driver's driving inputs will not cause high risk to the vehicle, the driving is carried out by the driver, so that in the case where the automatic system is having a problem, the driver can effectively realize a safe intervention, and the safety of the intelligent driving is improved.

Description

PRIOR ART

The prior art discloses an intervention-based shared control method and device for forward collision avoidance scenarios of an autonomous driving vehicle. This invention proposes a vehicle model decoupling method for solving the controller and predicting the risk in a high-speed forward collision avoidance scenario. The acquired vehicle state information is input into a forward collision avoidance control model to obtain the optimal nominal collision avoidance trajectory of the self-driving vehicle, and the acquired steering input data of the driver is used to establish the shared control method under a forward collision avoidance scenario of an autonomous driving vehicle. However, the method will determine the driver's intervention or not only by whether the vehicle's risk of forward collision is large, and when the vehicle's risk of forward collision is already large enough, the system deprives the control of the driver and completely relies on autonomous driving to follow the optimal nominal collision avoidance trajectory, without judging the safety of the driver's own driving behavior. So the proposed shared control method fails to consider the driver's own driving behavior fully, and the system will not be able to evaluate the driving risk from the driver's own driving behavior. When the vehicle's risk is relatively high and the autopilot encounters problems, the driver is unable to intervene, resulting in a significant safety risk.

BACKGROUND OF THE INVENTION

Human-machine co-driving (or shared control) refers to the sharing of vehicle control between drivers and intelligent systems to complete the driving tasks through deep cooperation in perception, decision-making and control. Intelligent systems have the advantages of agile perception, rule-based decision, and precise control, while drivers have better multi-source information fusion, reasoning learning, and manipulating adaptation. Human-machine co-driving can give full play to the respective strengths of the driver and the intelligent system to realize the human-machine integrated driving process. However, when the driver and the intelligent system control the vehicle together, the intelligent system is not limited to a simple interaction mode to remind or assist drivers, and its performance is directly related to the degree of intelligence of the system. When the intelligent system is in line with the driver's maneuvering style and habits, the system will play a good auxiliary effect, but when contrary to the driver's behavioral pattern, it will increase the driver's driving load, and even lead to a serious vehicle accident. In order to solve the human-machine conflict problem caused by the introduction of intelligent driver assistance systems and to improve driving safety, researchers have successively used switching control, model predictive control, fuzzy control, game theory and elliptic safety field in the design of indirect shared steering control strategies. However, as the core of shared steering control of intelligent vehicles, the weight distribution strategy of the current research is mostly based on the influence of the current state of the vehicle on the control weight, and lacks the prediction and comprehensive analysis of the factors influencing the control to drive. In addition, the driver, as an important participant of human-machine shared control, is still insufficiently considered about the influence of the driver's driving behavior on the allocation of control weights. Thus when the automatic system fails, the driver's behavior is unable to change the situation of the vehicle, which causes a serious safety hazard.

SUMMARY OF THE INVENTION

The present invention designed a path tracking controller to predict the trajectory of the driver, and then evaluate the driving risk of the human driver based on the predictive path. Ultimately, the shared control system will determine the distribution strategy of the control weight between the human driver and the automatic system according to the driving risk of the driver. The present invention ensures that when the driver's control input will cause a higher risk to the vehicle, the human-machine shared control system will translate the control weight from the human driver to the automatic system, and that in the case where the driver's Driving inputs do not pose a high risk to the vehicle, driving is carried out by the driver, making it possible for the driver to effectively realize safe interventions in the event of a problem with the automatic driving, and improving the safety of intelligent driving.

BRIEF DESCRIPTION OF DRAWINGS

FIG. 1 shows the method for human-machine shared control based on trajectory prediction described in Embodiment 1;

FIG. 2 shows the assessment result of the driver's driving risk described in Embodiment 3;

FIG. 3 shows a graph of the steering angle for the driver and automatic system described in Embodiment 3;

FIG. 4 shows a graph of the steering angle based on the human-machine shared control strategy described in Embodiment 3;

DETAILED DESCRIPTION OF THE INVENTION

An object of the present invention is to disclose a trajectory-prediction-based shared control method for human-machine co-driving, which is more secure.

In order to realize the above purpose, the present invention provides a human-machine shared control method based on trajectory prediction, comprising:

• • S1: Establish the road-vehicle model according to the kinematic and dynamic characteristics of the vehicle;
  • S2: Design the path tracking controller based on the road-vehicle model;
  • S3: Trajectory prediction based on the path tracking controller;
  • S4: Risk assessment of the predicted trajectory;
  • S5: The switching of control between the automatic system and the driver is realized according to the driving risk.

Further, in step S1, the vehicle-related parameters include intrinsic parameters as well as implementation parameters;

Where the vehicle's intrinsic parameters include: vehicle mass m, longitudinal velocity v_y, sampling time T, rotational inertia I_z, distance from center of mass to front axle l_f, distance from center of mass to rear axle l_r, cornering stiffness of front wheels c_f, cornering stiffness of rear wheels c_r, position coordinates of static obstacles (X_ob,Y_ob), minimum steering angle of the vehicle δ_min, maximum steering angle of the vehicle δ_max, minimum steering increment of the vehicle Δδ_min, maximum steering increment of the vehicle Δδ_max, the shortest distance coefficient between the vehicle and an obstacle Z, safety distance threshold coefficient between the vehicle and an obstacle G₀;

Implementation parameters include: position coordinates of the vehicle (X,Y), lateral velocity v_y, heading angle ϕ and yaw rate r.

In step S2, establishing the road-vehicle model based on the vehicle-related parameters: first establishing a vehicle dynamics model and a vehicle kinematics model based on the vehicle-related parameters, and then establishing the road-vehicle model based on the two vehicle models obtained.

The vehicle-related parameters for modeling the vehicle dynamics characteristics comprise: letting the longitudinal velocity of the vehicle v_x be independently controlled and constant, and the lateral force of the vehicle can be expressed as:F_xf cos δ−F_xf sin δ+F_yr=m({dot over (v)}_y+v_xr)

Where δ is the front wheel steering angle of the vehicle, v_x and v_y are the longitudinal and lateral velocities at the center of mass of the vehicle, r is the yaw rate of the vehicle, F_yr and F_xr are the lateral and longitudinal forces on the rear wheels of the vehicle, and F_xf as well as F_xf are the lateral and longitudinal forces on the front wheels of the vehicle;

Assuming that the vehicle is moving only in a two-dimensional plane without lateral slip and the vehicle's tire force satisfy a linear model, so the moment equations of the vehicle, the front and rear wheel slip angle (α_f and α_r), and the tire forces on the front and rear wheels (F_xf and F_yr) can be expressed as follows, respectively:

$l_f\left(F_{\mathrm{yf}}\cos \delta \right)-l_r\left(F_{\mathrm{yr}}-F_{xf}\sin \delta \right)=I_z\dot{r}$ ${\alpha }_f={\tan }^{-1}\left(\frac{v_y+l_{f}r}{v_x}\right)-\delta$ ${\alpha }_r={\tan }^{-1}\left(\frac{v_y+l_{f}r}{v_x}\right)$ $F_{\mathrm{yf}}=-c_f{\alpha }_f$ $F_{\mathrm{yr}}=-c_r{\alpha }_r$

Where l_f and l_r are the distances from the center of mass to the centers of the front and rear wheels, I_z is the moment of the vehicle's inertia around the z axis, and c_f as well as c_r are the cornering stiffnesses of the front and rear wheels, respectively;

Therefore, the vehicle dynamics model can be obtained as follows:

${\stackrel{.}{v}}_y=\frac{-c_f\left[{\tan }^{-1}\left(\frac{v_y+l_{f}r}{v_x}\right)-\delta \right]\cos \delta -c_r{\tan }^{-1}\left(\frac{v_y-l_{r}r}{v_x}\right)}{m}-v_{x}r$ $\dot{r}=\frac{-l_f{c}_f\left[{\tan }^{-1}\left(\frac{v_y+l_{f}r}{v_x}\right)-\delta \right]\cos \delta +l_r{c}_r{\tan }^{-1}\left(\frac{v_y-l_{r}r}{v_x}\right)}{I_z}$

Using a small angle approximation of the trigonometric functions, the linearized model is obtained:

${\stackrel{.}{v}}_y=\frac{-\left(c_f+c_r\right)}{m{v}_x}{v}_y+\left[\frac{l_r{c}_r-l_f{c}_f}{m{v}_x}-v_x\right]r+\frac{c_f}{m}\delta$ $\stackrel{.}{r}=\frac{l_r{c}_r-l_f{c}_f}{I_z{v}_x}{v}_y+\frac{-\left(l_f^2{c}_f+l_r^2{c}_r\right)}{I_z{v}_x}r+\frac{l_f{c}_f}{m}\delta$

This two-wheeled model is transformed into a four-wheeled vehicle model, so the vehicle model can be represented as:

$\left[\begin{array}{c}{\stackrel{.}{v}}_y\\ \stackrel{.}{r}\end{array}\right]=\left[\begin{array}{cc}\frac{-2\left(c_f+c_r\right)}{m{v}_x}& \frac{2\left(l_r{c}_r-l_f{c}_f\right)}{m{v}_x}-v_x\\ \frac{2\left(l_r{c}_r-l_f{c}_f\right)}{I_z{v}_x}& \frac{-2\left(l_1^2{c}_f+l_r^2{c}_r\right)}{I_z{v}_x}\end{array}\right]\left[\begin{array}{c}{v}_y\\ r\end{array}\right]+\left[\begin{array}{c}\frac{2c_f}{m}\\ \frac{2l_f{c}_f}{m}\end{array}\right]\delta$

In addition, when the vehicle performs path tracking, the lateral error e_d and heading error e_ϕ can be expressed by the following equation:ė_d=v_x sin(e_ϕ)+v_y cos(e_ϕ)ė_ϕ=r−v_xρ

Where e_d is the lateral error between the center of the vehicle's mass and the reference trajectory, and e_ϕ is the heading error between the vehicle and the reference trajectory. In addition, a small angle assumption (i.e., sin(e_ϕ)≈e_ϕ,cos(e_ϕ)≈1) is used for the heading error e_ϕ, so the tracking error can be simplified as:ė_d=v_xe_ϕ+v_y ė_ϕ=r−v_xρ

In summary, the dynamics model of the vehicle can be expressed as:

$\left[\begin{array}{c}{\stackrel{.}{v}}_y\\ \stackrel{.}{r}\\ {\stackrel{.}{e}}_d\\ {\stackrel{.}{e}}_{\varphi }\end{array}\right]=\left[\begin{array}{cccc}\frac{-2\left(c_f+c_r\right)}{{\mathrm{mv}}_x}& \frac{2\left(l_r{c}_r-l_f{c}_f\right)}{{\mathrm{mv}}_x}-v_x& 0& 0\\ \frac{2\left(l_r{c}_r-l_f{c}_f\right)}{I_z{v}_x}& \frac{-2\left(l_f^2{c}_f+l_r^2{c}_r\right)}{I_z{v}_x}& 0& 0\\ 1& 0& 0& v_x\\ 0& 1& 0& 0\end{array}\right]\left[\begin{array}{c}{v}_y\\ r\\ e_d\\ e_{\varphi }\end{array}\right]+\left[\begin{array}{c}\frac{2c_f}{m}\\ \frac{2l_f{c}_f}{m}\\ 0\\ 0\end{array}\right]\delta +\left[\begin{array}{c}0\\ 0\\ 0\\ -v_x\end{array}\right]\rho$

Further, according to the kinematic characteristics of the vehicle, the kinematic model of the vehicle can be described as follows:{dot over (X)}=v_x cos ϕ−v_y,sin ϕ{dot over (Y)}=v_x sin ϕ+v_y cos ϕ

Again, assuming that the vehicle is in a driving condition with a small steering angle, the model is simplified as:{dot over (X)}=v_x−v_yϕ{dot over (Y)}=v_xϕ+v_y

Further, building a road-vehicle model based on the vehicle dynamics model and the vehicle kinematics model:{dot over (x)}(t)=Ax(t)+Bu(t)+B₁₀₇ω(t)

Where

$x\left(t\right)={\left[v_y,r,\varphi ,e_d,e_{\varphi },X,Y\right]}^T,$ $u\left(t\right)=\delta \left(t\right),$ $\omega \left(t\right)={\left[\rho ,v_x\right]}^T$ $A=\left[\begin{array}{ccccccc}\frac{-2\left(C_f+C_r\right)}{{\mathrm{mv}}_x}& \frac{2\left(l_r{C}_r-l_f{C}_f\right)}{{\mathrm{mv}}_x}& 0& 0& 0& 0& 0\\ \frac{2\left(l_r{C}_r-l_f{C}_f\right)}{I_z{v}_x}& \frac{-2\left(l_f^2{C}_f+l_r^2{C}_r\right)}{I_z{v}_x}& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 1& 0& 0& 0& v_x& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ -\varphi & 0& 0& 0& 0& 0& 0\\ 1& 0& v_x& 0& 0& 0& 0\end{array}\right],$ $B=\left[\begin{array}{c}\frac{2c_f}{m}\\ \frac{2l_f{c}_f}{m}\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\right],$ $B_{\omega }=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ v_x& 0\\ 0& 1\\ 0& 0\end{array}\right]$

For computer simulation, T is used as the sampling time and Euler's method is utilized:

$\dot{x}\left(t\right)=\frac{x\left(k+1\right)-x\left(k\right)}{T}$

Converting a continuous vehicle model into a discrete vehicle model:ξ(k+1)=Ãξ(k)+{tilde over (B)}u(k)+{tilde over (B)}_ωω(k)

Where Ā=AT+I, B=BT, B_ω=B_ωT, I denotes the unit matrix with corresponding dimensions.

Further, in step S2, designing the path-tracking controller according to the road-vehicle model: the prediction step of the path-tracking controller is N_p, the control step is N_c, and the system states at each moment of time x(t) will be used as the starting point of the prediction sequence x(0|t) for the optimization computation, and the optimal control sequence u*(k|t), k∈[0,N_p−1] and sequences of states x*(k|t), k∈[1,N_p] will be obtained within the prediction time domain [t,t+N_p] and the first value of the optimal control sequence (i.e., u*(0|t)) will be used as a control input at the current moment of time to act on the vehicle;

Due to the dynamic change of the vehicle state, it is necessary to calculate the optimal steering angle according to the current actual vehicle condition while avoiding frequent steering angle changes. Therefore, the path tracking controller based on the model predictive control method is designed with the objective function:min J(x(k),u(k),ω(k))=min(Y^TQY+ΔU^TRΔU)s.t. ξ(k+1)=Ãξ(k)+{tilde over (B)}u(k)+{tilde over (B)}_ωω(k)

Where ξ(k+1)=Ãξ(k)+{tilde over (B)}u(k)+{tilde over (B)}_ωω(k) is the road-vehicle model, and Y=[y(k+1), y(k+2), . . . , y(k+N_p)]^T the predicted output of the system; Q denotes the output weight matrix, and ΔU=[Δu(k),Δu(k+1), . . . ,Δu(k+N_c−1)]^T the control increment sequence; R denotes the control weight matrix, and in addition for achieving the goal of path tracking, the output equation is taken as

$y=\left[\begin{array}{c}{e}_d\\ e_{\varphi }\end{array}\right];$

Considering the control limit constraints and the control increment constraints in the control process:u_min(t+k)≤u(t+k)≤u_max(t+k)Δu_min(t+k)≤Δu(t+k)≤Δu_max(t+k)k=0,1, . . . ,N_c−1

Further, in step S3, the driver's driving trajectory is predicted according to the path tracking controller:min J(x(k),u(k),ω(k))=min(Y^TQY+ΔU^TRΔU)s.t. ξ(k+1)=Ãξ(k)+{tilde over (B)}u(k)+{tilde over (B)}_ωω(k)  Objective function:u_min(t+k)≤u(t+k)≤u_max(t+k)Δu_min(t+k)≤Δu(t+k)≤Δu_max(t+k)k=0,1, . . . ,N_c−1  Constraints:

By solving this quadratic programming problem, the optimal control sequence to achieve target tracking can be obtained:U=[u(k),u(k+1), . . . ,u(k+N_c−1)]

The predicted sequence of system states Ξ is also worked out by utilizing both the state equations of the system and the obtained optimal control sequence:Ξ=Γx(k)+γU+ΠΩ

Where

$\Xi =\left[\begin{array}{c}x\left(k+1\right)\\ x\left(k+2\right)\\ ⋮\\ x\left(k+N_c\right)\end{array}\right],$ $U=\left[\begin{array}{c}u\left(k\right)\\ u\left(k+1\right)\\ ⋮\\ u(k+N_c-1\end{array}\right],$ $\Omega =\left[\begin{array}{c}\omega \left(k\right)\\ \omega \left(k+1\right)\\ ⋮\\ \omega \left(k+N_c-1\right)\end{array}\right],$ $\Gamma =\left[\begin{array}{c}\tilde{A}\\ {\tilde{A}}^2\\ ⋮\\ {\tilde{A}}^{N_c}\end{array}\right],$ $\Upsilon =\left[\begin{array}{cccc}\tilde{B}& 0& \dots & 0\\ \tilde{A}\tilde{B}& \tilde{B}& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ {\tilde{A}}^{\left(N_c-1\right)}\tilde{B}& {\tilde{A}}^{\left(N_c-2\right)}\tilde{B}& \dots & \tilde{B}\end{array}\right],$ $\prod =\left[\begin{array}{cccc}\tilde{B}& 0& \dots & 0\\ \tilde{A}{\tilde{B}}_{\omega }& {\tilde{B}}_{\omega }& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ {\tilde{A}}^{\left(N_c-1\right)}{\tilde{B}}_{\omega }& {\tilde{A}}^{\left(N_c-2\right)}{\tilde{B}}_{\omega }& \dots & {\tilde{B}}_{\omega }\end{array}\right]$

The predictive trajectory sequence of driver is:[(X_k+1,Y_k+1),(X_k+2,Y_k+2), . . . . ,(X_k+N,Y_k+Nc)]

Further, in step S4, the driving risk based on the predicted trajectory could be evaluated by the following way:

The driving risk based on the positional relationship between the driver and the obstacle will be evaluated by employing an artificial potential field approach;

First, define the field strength of the repulsive field:

$U_R\left(X\right)=\{\begin{array}{cc}0.5{Z\left(\frac{1}{R_R}-\frac{1}{G_0}\right)}^2,& R_R\le G_0\\ 0,& R_R>G_0\end{array}$

Where (X,Y) denotes the current position of the vehicle and (X_OB,Y_OB) denotes the position of the obstacle; Z denotes the shortest distance coefficient between the vehicle and an obstacle, G₀ is used to characterize the safety distance threshold coefficient for an obstacle, and R_R Indicates the actual shortest distance between the vehicle and an obstacle R_R=√{square root over ((X−X_OB)²+(Y−Y_OB)²)}.

Thus, the field force of the repulsive field F_R formed around the obstacle can be described by a negative gradient in the field strength

$U_R\left(X\right):F_R=-\mathrm{grad}\left[U_R\left(X\right)\right]=\{\begin{array}{cc}Z\left(\frac{1}{R_R}-\frac{1}{G_0}\right)\times \frac{1}{R_R^2},& R_R\le G_0\\ 0,& R_R>G_0\end{array}$

While the field force of the repulsive field can be categorized into longitudinal field force F_Rx and lateral field force F_Ry:

$\{\begin{array}{c}{F}_{Rx}=F_R·\cos \left(\varphi \right)\\ F_{\mathrm{Ry}}=F_R·\sin \left(\varphi \right)\end{array}$

Where ϕ denotes the angle formed by the line connecting the vehicle and the obstacle with the x-axis of the global coordinates;

Thus, the predicted trajectory with the control step N_c has a longitudinal risk R_x and a lateral risk R_y described by the average of the risks, and the total driving risk R_risk is obtained:

$\{\begin{array}{c}{R}_x=\frac{\sum _{k=1}^{N_c}{F}_{Rx}\left(X_{pred}^k,Y_{pred}^k\right)}{N_c}\\ R_y=\frac{\sum _{k=1}^{N_c}{F}_{Ry}\left(X_{pred}^k,Y_{pred}^k\right)}{N_c}\end{array}$ $R_{\mathrm{risk}}=R_x+R_y$

Further, in step S5, realizing the switching of control between the automatic system and the driver based on the driving risk. The detail can be described as follow: performing a risk assessment of the predicted trajectory in step S4 to obtain a driving risk R_risk, giving a threshold of the driver's driving risk R_risk* and switching the control of the vehicle to the automatic system when the driving risk be equal or greater than the risk threshold (i.e. R_risk≥R_risk*); and switching the control of the vehicle to the human driver when the driving risk be less than the risk threshold (i.e. R_risk<R_risk*).

The beneficial effects of the technical solution from the present invention compared to the prior art are as below:

The present invention designs a path-tracking controller to predict the trajectory, and then predicts the driver's driving risk of the trajectory to determine whether the vehicle is controlled by the driver or by the automatic system. The present invention ensures that when the driver's driving input will cause a higher risk to the vehicle, the shared control module will deprive the driver of his or her driving input, and drive entirely by the automated system, and that when the driver's driving inputs do not pose a high risk to the vehicle, the driving manipulation is carried out by the driver, making it possible for the driver to effectively realize safe interventions in the event of a problem from the automatic system, and improving the safety of intelligent driving.

Application Instances

The drawings are attached to exemplary illustration only and are not to be construed as a limitation of this patent.

The technical solution of the present invention is further described below in connection with the accompanying drawings and embodiments.

Embodiment 1

This embodiment discloses, as shown in figure. 1, a human-machine shared control method based on trajectory prediction, comprising the following steps:

• • S1: Establish the road-vehicle model according to the kinematic and dynamic characteristics;
  • S2: Design the path tracking controller based on the road-vehicle model;
  • S3: Trajectory prediction based on the path tracking controller;
  • S4: Risk assessment of predicted trajectories;
  • S5: Switching of control between the automatic system and the driver is realized according to the driving risk.

The embodiment predicts the trajectory by designing a path tracking controller, and then determines whether the vehicle is controlled by the driver or by the automatic system by means of predicting the driving risk of the driver. This embodiment ensures that when the driving input from the driver would pose a higher risk to the vehicle, the shared control module will deprive the driver of his or her driving input, and drive entirely by the automatic system. The embodiment also ensures that when the driver's driving inputs do not pose a high risk to the vehicle, the driving manipulation is carried out by the driver, making it possible for the driver to effectively realize safe interventions in the event of a problem from the automatic system, and improving the safety of intelligent driving.

Embodiment 2

This embodiment is further disclosed based on embodiment 1:

• • In step S1, the vehicle-related parameters include intrinsic parameters as well as implementation parameters;
  • Where the vehicle's intrinsic parameters include: mass m, longitudinal velocity v_y, sampling time T, rotational inertia I_z, distance from center of mass to front axle l_f, distance from center of mass to rear axle l_r, cornering stiffness of front wheels c_f, cornering stiffness of rear wheels c_r, position coordinates of static obstacles (X_ob,Y_ob), minimum steering of the vehicle δ_min, maximum steering of the vehicle δ_max, minimum steering increment of the vehicle Δδ_min, maximum steering increment of the vehicle Δδ_max, parameter of the shortest distance between the vehicle and an obstacle Z, safety distance threshold parameter between the vehicle and an obstacle G₀;
  • Implementation parameters include: position coordinates of the vehicle (X,Y), lateral velocity v_y, heading angle ϕ and angular velocity r.

Establishing the road-vehicle model according to the vehicle-related parameters in step S2: first establishing a vehicle dynamics model and a vehicle kinematics model based on the vehicle-related parameters, and then establishing the vehicle road-vehicle model according to the vehicle dynamics model and the vehicle kinematics model.

The modeling process of vehicle dynamics based on vehicle-related parameters is as follows: letting the longitudinal speed of the vehicle v_x be independently controlled and constant, and the lateral force of the vehicle can be expressed as:F_xf cos δ−F_xf sin δ+F_yr=m({dot over (v)}_y+v_xr)

Where δ is the front wheel angle of the vehicle, v_x and v_y are the longitudinal and lateral velocities at the center of mass of the vehicle, r is the yaw rate of the vehicle, F_yr and F_r are the lateral and longitudinal forces on the rear wheels of the vehicle, and F_xf and F_xf are the lateral and longitudinal forces on the front wheels of the vehicle;

Assuming that the vehicle is moving only in a two-dimensional plane without lateral slip and the vehicle's tire force is a linear model, the moment equations of the vehicle, the front and rear wheel slip angle (α_f and α_r), and the tire forces on the front and rear wheels (F_xf and F_yr) can be expressed as follows, respectively:

$\begin{array}{c}{l}_f\left(F_{\mathrm{yf}}\cos \delta \right)-l_r\left(F_{\mathrm{yr}}-F_{xf}\sin \delta \right)=I_z\dot{r}\\ {\alpha }_f={\tan }^{-1}\left(\frac{v_y+l_{f}r}{v_x}\right)-\delta \\ {\alpha }_r={\tan }^{-1}\left(\frac{v_y-l_{r}r}{v_x}\right)\\ F_{\mathrm{yf}}=-c_f{\alpha }_f\\ F_{\mathrm{yr}}=-c_r{\alpha }_r\end{array}$

Where l_f and l_r are the distances from the center of mass to the centers of the front and rear wheels, I_z is the moment of the vehicle's inertia around the z axis, and c_f and c_r are the cornering stiffnesses of the front and rear wheels, respectively;

Therefore, the vehicle dynamics model can be obtained as follows:

$\begin{array}{c}{\stackrel{.}{v}}_y=\frac{-c_f\left[{\tan }^1\left(\frac{v_Y+l_{f}r}{v_X}\right)-\delta \right]\cos \delta -c_r{\tan }^1\left(\frac{v_Y-l_{r}r}{v_X}\right)}{m}-v_{x}r\\ \dot{r}=\frac{-l_f{c}_f\left[{\tan }^{-1}\left(\frac{v_y+l_{f}r}{v_x}\right)-\delta \right]\cos \delta +l_r{c}_r{\tan }^{-1}\left(\frac{v_y-l_{r}r}{v_x}\right)}{I_z}\end{array}$

Using the small angle approximation of the trigonometric functions, a linearization is obtained:

$\begin{array}{c}{\stackrel{.}{v}}_y=\frac{-\left(c_f+c_r\right)}{m{v}_x}{v}_y+\left[\frac{l_r{c}_r-l_f{c}_f}{m{v}_x}-v_x\right]r+\frac{c_f}{m}\delta \\ \stackrel{.}{r}=\frac{l_r{c}_r-l_f{c}_f}{I_z{v}_x}{v}_y+\frac{-\left(l_f^2{c}_f+l_r^2{c}_r\right)}{I_z{v}_x}r+\frac{l_f{c}_f}{m}\delta \end{array}$

This two-wheeled model is transformed into a four-wheeled vehicle model, so the vehicle model can be represented as:

$\left[\begin{array}{c}{\stackrel{.}{v}}_y\\ \stackrel{.}{r}\end{array}\right]=\left[\begin{array}{cc}\frac{-2\left(c_f+c_r\right)}{m{v}_x}& \frac{2\left(l_r{c}_r-l_f{c}_1\right)}{m{v}_x}-v_x\\ \frac{2\left(l_r{c}_r-l_f{c}_f\right)}{I_z{v}_x}& \frac{-2\left(l_f^2{c}_f+l_r^2{c}_r\right)}{I_z{v}_x}\end{array}\right]\left[\begin{array}{c}{v}_y\\ r\end{array}\right]+\left[\begin{array}{c}\frac{2c_f}{m}\\ \frac{2l_f{c}_f}{m}\end{array}\right]\delta$

In addition, when the vehicle performs path tracking the lateral error e_d and heading error e_ϕ can be expressed by the following equation:ė_d=v_x sin(e_ϕ)+v_y cos(e_ϕ)ė_ϕ=r−v_xρ

Where, e_d is the lateral error between the center of the vehicle's mass and the reference trajectory, and e_ϕ is the heading angle error between the vehicle and the reference trajectory, and a small angle assumption (i.e., sin(e_ϕ)≈e_ϕ, cos (e_ϕ)≈1) is used for the heading error e_ϕ, so the tracking error can be simplified as:ė_d=v_xe_ϕ+v_y ė_ϕ=r−v_xρ

In summary, the dynamics model of the vehicle can be expressed as:

$\left[\begin{array}{c}{\stackrel{.}{v}}_y\\ \stackrel{.}{r}\\ {\stackrel{.}{e}}_d\\ {\stackrel{.}{e}}_{\varphi }\end{array}\right]=\left[\begin{array}{cccc}\frac{-2\left(c_f+c_r\right)}{m{v}_x}& \frac{2\left(l_r{c}_r-l_f{c}_1\right)}{m{v}_x}-v_x& 0& 0\\ \frac{2\left(l_r{c}_r-l_f{c}_f\right)}{I_z{v}_x}& \frac{-2\left(l_f^2{c}_f+l_r^2{c}_r\right)}{I_z{v}_x}& 0& 0\\ 1& 0& 0& v_x\\ 0& 1& 0& 0\end{array}\right]\left[\begin{array}{c}{v}_y\\ r\\ e_d\\ e_{\varphi }\end{array}\right]+\left[\begin{array}{c}\frac{2c_f}{m}\\ \frac{2l_f{c}_f}{m}\\ 0\\ 0\end{array}\right]\delta +\left[\begin{array}{c}0\\ 0\\ 0\\ -v_x\end{array}\right]\rho$

Modeling the vehicle's kinematic characteristics based on vehicle-related parameters: according to the kinematic characteristics of the vehicle, the kinematic model of the vehicle can be described as follows:{dot over (X)}=v_x cos ϕ−v_y sin ϕ{dot over (Y)}=v_x sin ϕ+v_y cos ϕ

Assuming again that the vehicle is in a driving condition with a small steering angle, the model is simplified as:{dot over (X)}=v_x−v_yϕ{dot over (Y)}=v_xϕ+v_y

Further, establishing the road-vehicle model based on the vehicle dynamics model and the vehicle kinematics model:{dot over (x)}(t)=Ax(t)+Bu(t)+B₁₀₇ω(t)

Where

$x\left(t\right)={\left[v_y,r,\varphi ,e_d,e_{\varphi },X,Y\right]}^T,u\left(t\right)=\delta \left(t\right).\omega \left(t\right)={\left[\rho ,v_x\right]}^T$ $A=\left[\begin{array}{ccccccc}\frac{-2\left(C_f+C_r\right)}{m{v}_x}& \frac{2\left(l_r{C}_r-l_f{C}_1\right)}{m{v}_x}-v_x& 0& 0& 0& 0& 0\\ \frac{2\left(l_r{C}_r-l_f{C}_f\right)}{I_z{v}_x}& \frac{-2\left(l_f^2{C}_f+l_r^2{C}_r\right)}{I_z{v}_x}& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 1& 0& 0& 0& v_x& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ \varphi & 0& 0& 0& 0& 0& 0\\ 1& 0& v_x& 0& 0& 0& 0\end{array}\right],$ $B=\left[\begin{array}{c}\frac{2C_f}{m}\\ \frac{2l_f{C}_f}{m}\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\right],B_{\omega }=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ v_x& 0\\ 0& 1\\ 0& 0\end{array}\right]$

To discretize the road-vehicle model, T is used as the sampling time and Euler's method is utilized:

$\dot{x}\left(t\right)=\frac{x\left(k+1\right)-x\left(k\right)}{T}$

Converting a continuous vehicle model into a discrete vehicle model:ξ(k+1)=Ãξ(k)+{tilde over (B)}u(k)+{tilde over (B)}_ωω(k)

• • where Ā=AT+I, B=BT, B_ω=B_ωT, I denotes the unit matrix of the corresponding dimensions.

In step S2, designing the path tracking controller according to the road-vehicle model: the prediction step of the path tracking controller is N_p, and the control step is N_c, the system measurements at each momentx(t) will be used as the starting point of the prediction sequence x(0|t) for the optimization calculation, and the optimal sequence of the controlled variables u*(k|t), k∈[0,N_p−1] and the sequence of the states x*(k|t), k∈[0,N_p−1] will be obtained within the prediction time domain [t,t+N_p] and the first value of the optimal control sequence (i.e., u*(0|t)) will be used as the control input at the current moment to act on the vehicle;

Due to the dynamic change of vehicle states, it is necessary to calculate the optimal steering angle according to the current actual vehicle condition while avoiding frequent steering angle changes. Therefore, the path tracking controller based on the model predictive control method is designed with the objective function:min J(x(k),u(k),ω(k))=min(Y^TQY+ΔU^TRΔU)s.t. ξ(k+1)=Ãξ(k)+{tilde over (B)}u(k)+{tilde over (B)}_ωω(k)

Where, ξ(k+1)=Ãξ(k)+{tilde over (B)}u(k)+{tilde over (B)}_ωω(k) is the road-vehicle model, the predicted output of the system Y=[y(k+1), y(k+2), . . . , y(k+N_p)]^T, Q denotes the output weight matrix, the control increment sequence ΔU=[Δu(k),Δu(k+1), . . . ,Δu(k+N_c−1)]^T, R denotes the control weight matrix, and in addition for achieving the goal of path tracking, the output equation is taken as

$y=\left[\begin{array}{c}{e}_d\\ e_{\varphi }\end{array}\right];$

Considering the limit constraints and the increment constraints of the controlled variables in the control process:u_min(t+k)≤u(t+k)≤u_max(t+k)Δu_min(t+k)≤Δu(t+k)≤Δu_max(t+k)k=0,1, . . . ,N_c−1

In step S3, the driving trajectory is predicted according to the path tracking controller:min J(x(k),u(k),ω(k))=min(Y^TQY+ΔU^TRΔU)s.t. ξ(k+1)=Ãξ(k)+{tilde over (B)}u(k)+{tilde over (B)}_ωω(k)  Objective function:u_min(t+k)≤u(t+k)≤u_max(t+k)Δu_min(t+k)≤Δu(t+k)≤Δu_max(t+k)k=0,1, . . . ,N_c−1  Constraints:

By solving this quadratic programming problem, the optimal control sequence to achieve target tracking can be obtained:U=[u(k),u(k+1), . . . ,u(k+N_c−1)]

The predicted sequence of system states Ξ is also computed by utilizing both the state equations of the system and the obtained optimal control sequence:Ξ=Γx(k)+γU+ΠΩ

Where

$$\begin{gathered} \Xi =\left[\begin{array}{c}x\left(k+1\right)\\ x\left(k+2\right)\\ ⋮\\ x\left(k+N_c\right)\end{array}\right],U=\left[\begin{array}{c}u\left(k\right)\\ u\left(k+1\right)\\ ⋮\\ u\left(k+N_c-1\right)\end{array}\right],\Omega =\left[\begin{array}{c}\omega \left(k\right)\\ \omega \left(k+1\right)\\ ⋮\\ \omega \left(k+N_c-1\right)\end{array}\right],\Gamma =\left[\begin{array}{c}\tilde{A}\\ {\tilde{A}}^2\\ ⋮\\ {\tilde{A}}^{N_c}\end{array}\right]·\text{ \\ ϒ}=\left[\begin{array}{cccc}\tilde{B}& 0& \dots & 0\\ \tilde{A}\tilde{B}& \tilde{B}& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ {\tilde{A}}^{\left(N_c-1\right)}\tilde{B}& {\tilde{A}}^{\left(N_c-2\right)}\tilde{B}& \dots & \tilde{B}\end{array}\right],\Pi =\left[\begin{array}{cccc}{\tilde{B}}_{\omega }& 0& \dots & 0\\ \tilde{A}{\tilde{B}}_{\omega }& {\tilde{B}}_{\omega }& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ {\tilde{A}}^{\left(N_c-1\right)}{\tilde{B}}_{\omega }& {\tilde{A}}^{\left(N_c-2\right)}{\tilde{B}}_{\omega }& \dots & {\tilde{B}}_{\omega }\end{array}\right] \end{gathered}$$

The driver prediction sequence is:[(X_k+1,Y_k+1),(X_k+2,Y_k+2), . . . . ,(X_k+N,Y_k+Nc)]

In step S4, performing a risk assessment of the predicted trajectory: the driver's driving risk will be evaluated using an artificial potential field approach based on the positional relationship between the driver and the obstacle.

Firstly, define the field strength of the barrier field:

$U_R\left(X\right)=\{\begin{array}{cc}0.5{Z\left(\frac{1}{R_R}-\frac{1}{G_0}\right)}^2,& R_R\le G_0\\ 0,& R_R>G_0\end{array}$

Where (X,Y) denotes the current position coordinates of the vehicle and (X_OB,Y_OB) denotes the position coordinates of the obstacle. Z denotes the shortest distance coefficient between a vehicle and an obstacle, G₀ is used to characterize the safety distance threshold for an obstacle; the R_R indicates the shortest distance between a vehicle and an obstacle R_R=√{square root over ((X−X_OB)²+(Y−Y_OB)²)}.

Thus, the field force of the repulsive field F_R formed around the obstacle can be described by a negative gradient in the field strength U_R (X)

$F_R=-\mathrm{grad}\left[U_R\left(X\right)\right]\{\begin{array}{cc}Z\left(\frac{1}{R_R}-\frac{1}{G_0}\right)\times \frac{1}{R_R^2},& R_R\le G_0\\ 0,& R_R>G_0\end{array}$

At this point, the field force of the repulsive field can be categorized into the longitudinal field force F_Rx and the lateral field force F_Ry:

$\{\begin{array}{c}{F}_{Rx}=F_R·\cos \left(\varphi \right)\\ F_{\mathrm{Ry}}=F_R·\sin \left(\varphi \right)\end{array}$

Where ϕ denotes the angle formed by the line connecting the vehicle and the obstacle with the x-axis of the global coordinates;

Thus, the longitudinal risk R_x, the lateral risk R_y and the total driving risk R_risk of the predicted trajectory with a control step N_c are obtained:

$\{\begin{array}{c}{R}_x=\frac{\sum _{k=1}^{N_c}{F}_{Rx}\left(X_{pred}^k,Y_{pred}^k\right)}{N_c}\\ R_y=\frac{\sum _{k=1}^{N_c}{F}_{Ry}\left(X_{pred}^k,Y_{pred}^k\right)}{N_c}\end{array}$ $R_{\mathrm{risk}}=R_x+R_y$

In step S5, realizing the switching of control between the automatic system and the driver based on the driving risk: performing the driving risk assessment of the predicted trajectory in step S4 to obtain a driving risk R_risk, given a threshold of the driver's driving risk R_risk*, switching the control of the vehicle to the automatic system when the driving risk be equal or greater than the risk threshold (i.e. R_risk≥R_risk*); and switching the control of the vehicle to the human driver when the driving risk is less than the risk threshold (i.e. R_risk<R_risk*).

The present embodiment predicts the trajectory by designing a path tracking controller, and then determines whether the vehicle is actually controlled by the driver or by the automatic system by predicting the driver's driving risk when there is a driving input from the driver, this embodiment ensures that when the driving input from the driver will cause a higher risk to the vehicle, the driving input from the driver is deprived, and the automatic driving is carried out entirely by the automatic system, and in the event that the driver's driving inputs do not pose a high risk to the vehicle, and the driving is carried out by the driver so that the driver can effectively realize safe interventions when the automatic driving has a problem, and the safety of the intelligent driving is improved. Moreover, this embodiment adopts the method of artificial potential field, comprehensively considers the lateral and longitudinal risk of the vehicle, and improves the accuracy of the driving risk assessment.

Embodiment 3

This embodiment is disclosed based on Embodiment 2:

The vehicle parameters are set as follows: vehicle's mass m=1575, rotational inertia I_z=2875, distance from center of mass to front axle l_f=1.2, distance from center of mass to rear axle l_r=1.6, cornering stiffness of front wheels c_f=19000, cornering stiffness of rear wheels c_r=33000, output weight matrix Q=I_(Np×Np) ⊗diag(100,100), control weight matrix R=5·I_(Nc×Nc), (where I_(N×N) denotes the unit matrix of dimension N×N and the symbol ⊗ denotes the Kronecker product). The initial coordinates of the vehicle are X=0 and Y=0, the coordinates of the static obstacle are X_ob=50 and Y_ob=0.1, the parameter in the risk assessment is taken as Z=1000, G₀=100, the risk threshold is taken ask R_risk*=0.8, and the limit constraints and the increment constraints in the objective function are taken as:u_min=−0.78,u_max=0.78,Δu_min=−0.0082,Δu_max=0.0082

Applied to the scenario of lane change to the left with a duration of 60 s, sampling time 0.05 s, prediction step N_p=6, and control step N_c=3, we obtain the graphs of driver's driving risk assessment, the graphs of steering angle from the driver and the automated system, and the graphs of vehicle steering angle based on the human-machine shared control strategy as shown in figures 1. 2, 3, and 4.

The present embodiment predicts the trajectory by designing a path tracking controller, and then determines whether the vehicle is actually controlled by the driver or by the automated system by predicting the driving risk of the trajectory when there is a driving input from the driver. This embodiment ensures that when the driving input from the driver will cause a higher risk to the vehicle, the driving input from the driver is deprived, and the automated driving is carried out entirely by the automated system, and ensures that when the driver's driving input do not pose a high risk to the vehicle, and the driving is carried out by the driver, so that the driver can effectively realize safe interventions when the automatic driving has a problem, and the safety of the intelligent driving is improved. Moreover, this embodiment adopts the method of artificial potential field, comprehensively considers the lateral and longitudinal risk of the vehicle, and improves the accuracy of the driving risk assessment.

In summary, embodiments of the present invention provide a human-machine shared control method based on trajectory prediction. This includes: establishing a road-vehicle model based on vehicle-related parameters; designing a path-tracking controller based on the road-vehicle model; predicting a driving trajectory based on the path-tracking controller; performing a driving risk assessment of the predicted trajectory; and finally realizing switching of control between the automation system and the human driver based on the driving risk. It ensures that when the driver's own driving inputs will cause high risk to the vehicle, the driver's own driving inputs are deprived of, and the vehicle is completely carried out by the automatic driving, and in the case that the driver's own driving inputs will not cause high risk to the vehicle, the vehicle is carried out by the driver, so that the driver can effectively realize the safe intervention, and the safety of the intelligent driving is improved.

Obviously, the above embodiments of the present invention are merely embodiments for the purpose of clearly illustrating the present invention, and are not intended to be a limitation of the embodiments of the present invention. For those of ordinary skill in the professional filed, other variations or changes in different forms may be made on the basis of the above description. It is neither necessary nor possible to exhaust all the embodiments herein. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention shall be included in the scope of protection of the claims of the present invention.

Claims

1. A method for human-machine shared control based on trajectory prediction, characterized in that it comprises the following steps: S1: establishing a road-vehicle model according to vehicle-related parameters;S2: designing a path tracking controller according to the road-vehicle model: first, establishing a vehicle dynamics model and a vehicle kinematics model according to the vehicle-related parameters, and then establishing the road-vehicle model according to the vehicle dynamics model and the vehicle kinematics model;wherein according to the vehicle dynamics model and the vehicle kinematics model, the road-vehicle model is obtained as follows: {dot over (x)}(t)=Ax(t)+Bu(t)±Bωω(t)wherein

2. The method for human-machine shared control based on trajectory prediction according to claim 1, characterized in that the vehicle-related parameters comprise intrinsic parameters and implementation parameters of the vehicle in step S1; wherein the intrinsic parameters include: mass in, longitudinal velocity vy sampling time T, rotational inertia lz, distance from center of mass to front axle lf, distance from center of mass to rear axle lr, cornering stiffness of front wheels cf, cornering stiffness of rear wheels cr, position coordinates of static obstacles (Xob, Yob), minimum steering of the vehicle δmin, maximum steering of the vehicle δmax, minimum steering increment of the vehicle Δδmin, maximum steering increment of the vehicle Δδmax, parameter of a shortest distance between the vehicle and an obstacle Z, safety distance threshold parameter between the vehicle and an obstacle G0;the implementation parameters comprise: position coordinates of the vehicle (X, Y), lateral velocity vy, heading angle ϕ and angular velocity r.

3. The method for human-machine shared control based on trajectory prediction according to claim 1, wherein the step of establishing the vehicle dynamics model based on vehicle-related parameters comprises; setting a longitudinal velocity vx of the vehicle to be independently controlled and constant, and a lateral force of the vehicle is expressed as: Fxf cos δ−Fxf sin δ+Fyr=m({dot over (v)}y+vxr)wherein δ is a front wheel steering angle of the vehicle, vx and vy, are longitudinal and lateral velocities at a center of mass of the vehicle, r is a yaw rate of the vehicle, Fyr and Fxr are lateral and longitudinal forces on rear wheels of the vehicle, and Fxf and Fxf are lateral and longitudinal forces on front wheels of the vehicle;under the condition that the vehicle is moving only in a two-dimensional plane without lateral slip and a vehicle's tire force is a linear model, a moment equations of the vehicle, a front and rear wheel slip angle (αf and αr), and tire forces on the front and rear wheels (Fyf and Fyr) is expressed as follows, respectively:

4. The method for human-machine shared control based on trajectory prediction according to claim 1, wherein the step of establishing the vehicle kinematic model based on vehicle-related parameters comprises: according to kinematic characteristics of the vehicle, setting the kinematic model of the vehicle as follows: {dot over (X)}=vx cos ϕ−vy sin ϕ{dot over (Y)}=vx sin ϕ+vy cos ϕwherein the vehicle is in a driving condition with a small steering angle, the kinematic model is simplified as: {dot over (X)}=vx−vyϕ{dot over (Y)}=vxϕ+vy.

5. The method for human-machine shared control based on trajectory prediction according to claim 1, wherein in step S3, the step of predicting the driving trajectories according to the path tracking controller comprises: min J(x(k),u(k),ω(k))=min(YTQY+ΔUTRΔU)s.t. ξ(k+1)=Ãξ(k)+{tilde over (B)}u(k)+{tilde over (B)}ωω(k)  objective function:umin(t+k)≤u(t+k)≤umax(t+k)Δumin(t+k)≤Δu(t+k)≤Δumax(t+k)k=0,1, . . . ,Nc−1  constraints:by solving a quadratic programming problem, obtaining the optimal control sequence to achieve target tracking: U=[u(k),u(k+1), . . . ,u(k+Nc−1)]calculating a predicted sequence of system states Ξ by utilizing both a state equation of the system and the optimal control sequence: Ξ=Γx(k)+γU+ΠΩwherein

6. The method for human-machine shared control based on trajectory prediction according to claim 5, wherein the step of evaluating the driving risk of the predicted driving trajectories in step S4 comprises: evaluating the driving risk based on a positional relationship between the driver and an obstacle-by using an artificial potential field approach, wherein a field strength of a barrier field is defined:

7. The method for human-machine shared control based on trajectory prediction according to claim 6, wherein the step of performing the switching of control between the automatic system and the driver based on the driving risk in step S5 comprises: performing a risk assessment of the predicted driving trajectory in step S4 to obtain a driving risk Rrisk giving a risk threshold of a driver's driving risk Rrisk*, and switching the control of the vehicle to the automatic system when the driving risk is equal or greater than the risk threshold Rrisk≥Rrisk*; switching the control of the vehicle to the driver when the driving risk be less than the risk threshold (i.e. Rrisk<Rrisk*).