Automatic driving vehicle trajectory planning method and vehicle

Abstract

The application relates to the technical field of vehicles, in particular to an automatic driving vehicle trajectory planning method and a vehicle, wherein the method comprises the following steps: acquiring motion data of a current vehicle and motion data of a target vehicle; predicting the motion state of the current vehicle in a target time domain according to the motion data of the current vehicle, and predicting the motion state of the target vehicle in the target time domain according to the motion data of the target vehicle; identifying the risk feature boundary of the position uncertainty of the target vehicle under network delay, and planning the driving trajectory of the current vehicle based on the risk feature boundary and the respective motion states of the current vehicle and the target vehicle in the target time domain. Therefore, the problems that the uncertainty of an obstacle vehicle in planning a target is not considered in the related art, the optimal path planning is difficult to realize, and the scene adaptability of an automatic driving system is reduced are solved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of vehicles, in particular to an automatic driving vehicle trajectory planning method and vehicle. BACKGROUND

[0002] Intelligentization and networking have become important development directions of the current automobile industry. In the existing automatic driving system framework, the planning part is often designed according to the process of perception, decision, planning and control. The planning part further generates a collision-free optimal reference motion path or trajectory on the basis of decision, and it is the key to guarantee the safety of automatic driving vehicles as an intermediate link between perception and decision. The current perception system cannot achieve completely accurate estimation of the position coordinates of obstacles, and the networking of automatic driving will further worsen the time delay caused by the transmission process of perception signals. Such uncertainty of obstacles brings interference and challenges to the safe and reliable trajectory planning of automatic driving, and becomes a key problem and bottleneck that needs to be broken through in the current automatic driving technology.

[0003] In related technology (1), a trajectory planning method and framework based on random model predictive control and failure-proof backup planning are proposed. The normal distribution is used to represent the uncertainty of the position of the obstacle vehicle, and the obstacle avoidance boundary constraint of the current vehicle is determined by configuring the probability boundary. However, this obstacle avoidance representation method through hard constraint cannot consider the uncertainty of the obstacle in the planning target, and the hard constraint based on the probability distribution limits the trajectory optimization space of the vehicle, reducing the scene adaptability of the automatic driving system.

[0004] In related technology (2), an unmanned vehicle dynamic path planning method based on environmental uncertainty is proposed, including the following steps S1: establishing a vehicle kinematics model; S2: establishing a dynamic environment model and a satisfaction condition for re-planning a path; S3: obtaining a vehicle motion state starting value, a vehicle motion state initial target value and a vehicle motion state candidate target value; S4: generating a candidate path; S5: selecting an optimal path based on safety indicators and rapidity indicators; S6: when the unmanned vehicle motion environment satisfies the satisfaction condition for re-planning a path, re-planning the optimal path of the unmanned vehicle. However, this scheme generates a plurality of candidate paths based on the initial position and target state in a regular manner, and then selects a path according to the defined safety and rapidity indicators, which is difficult to achieve optimal path planning, and has poor working condition adaptability. When the planning space is large, the planning effect is seriously dependent on the generation interval of the candidate paths.

[0005] In the related technology (3), a vehicle path planning method and device based on risk field and uncertainty analysis are proposed, wherein the method comprises: performing label classification and position and speed prediction on a perception target to obtain label classification and position and speed prediction results of the perception target; based on the label classification and position and speed prediction results of the perception target, label probability risk field and prediction dynamic risk field are established to measure the uncertainty in label classification and position and speed prediction, respectively; based on the label probability risk field and the prediction dynamic risk field, a planning trajectory cluster is obtained by gradient descent method within each preset time step, and the planning trajectory cluster is post-processed to select an optimal path. However, this scheme uses the gradient descent method based on the probability risk field as the basic principle of trajectory planning, does not introduce the vehicle kinematic model, and is difficult to introduce the vehicle constraints to ensure the executability of the planning output; secondly, the gradient descent method depends on the monotonicity of the risk field, and it is difficult to plan an effective path when the vehicle is near the risk field saddle point.

[0006] In summary, the above technical solutions do not consider the uncertainty of obstacles in the planning target, which makes it difficult to achieve optimal path planning and reduces the scene adaptability of the automatic driving system. SUMMARY

[0007] The present application provides an automatic driving vehicle trajectory planning method and vehicle to solve the problems in the related art that the uncertainty of obstacles in the planning target is not considered, which makes it difficult to achieve optimal path planning and reduces the scene adaptability of the automatic driving system.

[0008] The first aspect embodiment of the present application provides an automatic driving vehicle trajectory planning method, comprising the following steps: obtaining motion data of a current vehicle and motion data of a target vehicle; predicting the motion state of the current vehicle in a target time domain according to the motion data of the current vehicle, and predicting the motion state of the target vehicle in the target time domain according to the motion data of the target vehicle; identifying the risk feature boundary of the position uncertainty of the target vehicle under network delay, and planning the driving trajectory of the current vehicle based on the risk feature boundary and the respective motion states of the current vehicle and the target vehicle in the target time domain.

[0009] Optionally, in an embodiment of the present application, the identification of the risk feature boundary of the position uncertainty of the target vehicle under network delay comprises: obtaining the Gaussian normal distribution of the lateral position, the Gaussian normal distribution of the longitudinal position, the Gaussian normal distribution of the speed and the heading angle of the target vehicle before network delay; determining a two-dimensional probability density function of the actual position of the target vehicle according to the Gaussian normal distribution, the Gaussian normal distribution of the longitudinal position, the Gaussian normal distribution of the speed and the heading angle, wherein the actual position of the target vehicle is subject to Gaussian normal distribution; determining the risk feature boundary of the position uncertainty of the target vehicle according to the two-dimensional probability density function and the confidence ellipse of the two-dimensional normal distribution.

[0010] Optionally, in an embodiment of the present application, the two-dimensional probability density function is:

[0011] ,

[0012] wherein, , , , psi v is the heading angle of the obstacle vehicle in the earth coordinate system during the delay transmission process, mu y and mu x are the mean values of the original longitudinal and lateral positions of the vehicle given by the sensor or estimation system, respectively, mu v is the mean value of the speed distribution given by the sensor or estimation system, sigma y 2 and sigma x 2 are the variances of the longitudinal and lateral positions, respectively, is the signal transmission delay caused by the communication system, and are the variances of the longitudinal and lateral positions after the delay , respectively, is the variance of the Gaussian distribution to which the vehicle speed is subjected, and are the mean values of the longitudinal and lateral positions after the delay , respectively.

[0013] Optionally, in an embodiment of the present application, the method further comprises: constructing an initial kinematic model according to the running data of the vehicle; converting the initial kinematic model into a nonlinear system state equation, and discretizing the nonlinear system state equation to generate a kinematic model; inputting the motion data of the current vehicle into the kinematic model, and outputting the motion state of the current vehicle in the target time domain by the kinematic model.

[0014] Optionally, in an embodiment of the present application, the kinematic model is:

[0015] , , ,

[0016] wherein, X , Y , psi ,v , delta f and a These represent the vehicle's longitudinal coordinates, lateral coordinates, yaw angle, composite velocity, front wheel steering angle, and vehicle acceleration, respectively. x and u represent the vehicle system state and control inputs, respectively. k and u k These represent the discrete vehicle system state and control input, respectively, with T being the discrete sampling time step.

[0017] Optionally, in one embodiment of this application, predicting the motion state of the target vehicle in the target time domain based on the motion data of the target vehicle includes: inputting the motion data of the target vehicle into a prediction model, wherein a preset model outputs the motion state of the target vehicle in the target time domain.

[0018] Optionally, in one embodiment of this application, the prediction model is:

[0019] ，

[0020] Among them, among them, X obj,k , Y obj,k , , v obj,k The first j A vehicle with an obstacle k The longitudinal coordinate, lateral coordinate, yaw angle, and vehicle speed under the step length. a obj The acceleration of the target vehicle. omega obj The yaw rate is angular velocity. This represents the discrete time step of the prediction process.

[0021] Optionally, in one embodiment of this application, the step of planning the driving trajectory of the current vehicle based on the risk feature boundary and the respective motion states of the current vehicle and the target vehicle in the target time domain includes: obtaining the number of target vehicles, the distribution probability within the confidence ellipse, the envelope circles of the current vehicle and the target vehicle, and the target vehicle's trajectory in the target time domain. k The steps are: the confidence ellipse radius of the step size, the approach function of the target lane, and the approach function of the target speed; the obstacle avoidance performance function of the target vehicle uncertainty is determined based on the number of target vehicles, the probability distribution within the confidence ellipse, the envelope circles of the current vehicle and the target vehicle, and the confidence ellipse radius of the target vehicle at a preset step size; the driving trajectory of the current vehicle is planned based on the obstacle avoidance performance function, the approach function of the target lane, and the approach function of the target speed.

[0022] Optionally, in one embodiment of the present application, the planning the driving trajectory of the current vehicle according to the obstacle avoidance performance function, the approaching function of the target lane and the approaching function of the target speed comprises:

[0023] ,

[0024] wherein, co 1, co 2, co 3 are weight coefficients of the corresponding obstacle avoidance performance function R, the approaching function Q of the target lane and the approaching function V of the target speed;

[0025] wherein, the constraint condition is:

[0026] ,

[0027]

[0028]

[0029]

[0030]

[0031] wherein, wherein, (X k , Y k ) is the geodetic coordinate of the controlled vehicle at k moment, (X obj,k , Y obj,k ) is the geodetic coordinate of the jth obstacle vehicle at k moment, is the system control input at k moment, T is the system discrete step ，Y min and Y max is the coordinate of the lane boundary, L w is half of the body width of the current vehicle, delta fmin , delta fmax , a min and a max are minimum and maximum values of the steering angle and acceleration respectively, , , , are corresponding gradient limits; delta fk and a k are the front wheel steering angle and the acceleration of the controlled vehicle at k moment respectively.

[0032] The second aspect embodiment of the present application provides a vehicle, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, and the processor executes the program to perform the automatic driving vehicle trajectory planning method as described in the above embodiments.

[0033] Therefore, the present application has at least the following beneficial effects:

[0034] (1) The embodiments of the present application use Gaussian distribution to describe the uncertainty of obstacle vehicle perception, and derive the coupling relationship of position and speed uncertainty over time. The actual position uncertainty of the vehicle after considering the communication delay is modeled as an anisotropic two-dimensional normal distribution, which makes the model more accurate and more accurate for the actual position of the subsequent obstacle vehicle.

[0035] (2) Based on the uncertainty modeling, the embodiments of the present application further use the confidence ellipse of the two-dimensional normal distribution as the risk feature boundary based on the preset principle of normal distribution, so as to construct an adaptive risk field considering the uncertainty distribution of the obstacle, so as to limit the trajectory of the vehicle into the adaptive risk field in the subsequent, and prevent the current vehicle from colliding with the target vehicle.

[0036] (3) The embodiments of the present application superimpose the adaptive risk field considering the uncertainty distribution and the obstacle boundary, and use the standard cumulative distribution probability in the confidence ellipse as the weight coefficient of the corresponding obstacle avoidance objective function. The obstacle avoidance objective function of the surrounding obstacle is accumulated along the prediction step, and a model predictive control trajectory planning method is constructed, which realizes the trajectory planning considering the uncertainty of the obstacle and improves the obstacle avoidance safety of the networked environment.

[0037] (4) The embodiments of the present application can combine the risk feature boundary with the model predictive control, propose a different risk feature boundary weight coefficient determination method based on the cumulative distribution probability of the confidence ellipse, construct an obstacle avoidance performance function considering the actual position uncertainty of the obstacle vehicle in the model predictive control, so as to realize the optimal trajectory planning of the automatic driving vehicle considering the uncertainty of the obstacle, and improve the scene adaptability of the automatic driving system.

[0038] Additional aspects and advantages of the present application will be in part apparent and in part pointed out hereinafter in the description of embodiments of the present application. BRIEF DESCRIPTION OF DRAWINGS

[0039] The above and / or additional aspects and advantages of the present application will become apparent and be readily appreciated from the following description of embodiments of the present application, taken in conjunction with the accompanying drawings:

[0040] Figure 1 A flowchart of an automatic driving vehicle trajectory planning method according to an embodiment of the present application is provided.

[0041] Figure 2 A schematic diagram of a vehicle monorail kinematic model according to an embodiment of the application is provided.

[0042] Figure 3 A structural schematic diagram of a vehicle according to an embodiment of the application is provided. DETAILED DESCRIPTION

[0043] Embodiments of the application are described in detail below with reference to examples illustrated in the accompanying drawings, in which the same or similar reference numerals are used throughout to denote the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to explain the present application, and cannot be understood as limiting the present application.

[0044] The automatic driving vehicle trajectory planning method and vehicle of the embodiments of the application are described below with reference to the accompanying drawings. In view of the problem that the related art does not consider the uncertainty of obstacles in the planning target, which makes it difficult to achieve optimal path planning and reduces the scene adaptability of the automatic driving system, the present application provides an automatic driving vehicle trajectory planning method. In the method, the risk feature boundary is combined with model predictive control, a different risk feature boundary weight coefficient determination method based on the cumulative distribution probability of the confidence ellipse is proposed, and an obstacle avoidance performance function considering the uncertainty of the actual position of the obstacle vehicle is constructed in the model predictive control, so as to realize optimal trajectory planning of the automatic driving vehicle considering the uncertainty of the obstacle, and improve the scene adaptability of the automatic driving system. Thus, the problem that the related art does not consider the uncertainty of obstacles in the planning target, which makes it difficult to achieve optimal path planning and reduces the scene adaptability of the automatic driving system, is solved.

[0045] Specifically, Figure 1 A flowchart of an automatic driving vehicle trajectory planning method according to an embodiment of the application is provided.

[0046] As Figure 1 shown, the automatic driving vehicle trajectory planning method includes the following steps:

[0047] In step S101, the motion data of the current vehicle and the motion data of the target vehicle are obtained.

[0048] It can be understood that the motion data of the current vehicle and the motion data of the target vehicle can be obtained, so as to facilitate subsequent prediction of the motion state of the current vehicle in the target time domain and the motion state of the target vehicle in the target time domain.

[0049] It should be noted that the motion data includes longitudinal coordinates, lateral coordinates, yaw angles, composite speeds, mass center side slip angles, front wheel steering angles, vehicle accelerations, distances from the mass center to the front and rear axles, etc., and is not specifically limited.

[0050] In step S102, the motion state of the current vehicle in the target time domain is predicted according to the motion data of the current vehicle, and the motion state of the target vehicle in the target time domain is predicted according to the motion data of the target vehicle.

[0051] It can be understood that the embodiments of the present application can predict the motion state of the current vehicle in the target time domain according to the motion data of the current vehicle, and predict the motion state of the target vehicle in the target time domain according to the motion data of the target vehicle, so as to subsequently plan the driving trajectory of the current vehicle based on the risk feature boundary and the respective motion states of the current vehicle and the target vehicle in the target time domain.

[0052] In the embodiments of the present application, predicting the motion state of the current vehicle in the target time domain according to the motion data of the current vehicle comprises: constructing an initial kinematic model according to the running data of the vehicle; converting the initial kinematic model into a nonlinear system state equation, and discretizing the nonlinear system state equation to generate a kinematic model; inputting the motion data of the current vehicle into the kinematic model, and the kinematic model outputting the motion state of the current vehicle in the target time domain.

[0053] Specifically, the current vehicle is kinematically modeled, and a kinematic model of a single-track vehicle is adopted, as shown in the following formula (1): Figure 1 wherein X, Y, ψ, v, β, δf and a are the longitudinal coordinates, lateral coordinates, yaw angles, composite speeds, mass center side slip angles, front wheel steering angles and vehicle accelerations of the current vehicle respectively, Lf and Lr are the distances from the mass center to the front and rear axles, and the model can be described by the following system differential equation set:

[0054] ;

[0055] The above system model is further arranged in the form of the following nonlinear system state equation, and is discretized according to the sampling time step T to obtain:

[0056] , , ,

[0057] wherein, X , Y , psi , v , deltaf and a These represent the vehicle's longitudinal coordinates, lateral coordinates, yaw angle, composite velocity, front wheel steering angle, and vehicle acceleration, respectively. x and u represent the vehicle system state and control inputs, respectively. k and u k These represent the discrete vehicle system state and control input, respectively, with T being the discrete sampling time step.

[0058] In this embodiment of the application, predicting the motion state of the target vehicle in the target time domain based on the motion data of the target vehicle includes: inputting the motion data of the target vehicle into a prediction model, wherein the preset model outputs the motion state of the target vehicle in the target time domain.

[0059] The prediction model is as follows:

[0060] ，

[0061] in, X obj,k , Y obj,k , v obj,k The first j A vehicle with an obstacle k The longitudinal coordinate, lateral coordinate, yaw angle, and vehicle speed under the step length. a obj The acceleration of the target vehicle. omega obj The yaw rate is angular velocity. This represents the discrete time step of the prediction process.

[0062] It should be noted that, in order to predict the motion state of the obstacle vehicle in the model prediction time domain, the above-mentioned prediction model with fixed steering speed and acceleration is used, and the acceleration of the obstacle is... a obj and yaw rate omega obj The vehicle position state is assumed to be constant during the prediction process. The predicted vehicle position state within each step will follow a two-dimensional normal distribution model.

[0063] In step S103, the risk feature boundary of the uncertainty of the target vehicle's position under network latency is identified, and the driving trajectory of the current vehicle is planned based on the risk feature boundary and the respective motion states of the current vehicle and the target vehicle in the target time domain.

[0064] It can be understood that the embodiment of the application identifies the risk feature boundary of the position uncertainty of the target vehicle under the network delay, plans the driving track of the current vehicle based on the risk feature boundary and the motion state of the current vehicle and the target vehicle in the target time domain, so that the optimal track planning of the autonomous vehicle considering the uncertainty of the obstacle is realized, and the scene adaptation capability of the autonomous driving system is improved.

[0065] In the embodiment of the application, the risk feature boundary of the position uncertainty of the target vehicle under the network delay is identified, including: obtaining the Gaussian normal distribution of the lateral position, the Gaussian normal distribution of the longitudinal position, the Gaussian normal distribution of the speed and the heading angle of the target vehicle before the network delay; determining a two-dimensional probability density function of the actual position of the target vehicle according to the Gaussian normal distribution, the Gaussian normal distribution of the longitudinal position, the Gaussian normal distribution of the speed and the heading angle, wherein the actual position of the target vehicle is subject to the Gaussian normal distribution; and determining the risk feature boundary of the position uncertainty of the target vehicle according to the two-dimensional probability density function and the confidence ellipse of the two-dimensional normal distribution.

[0066] The two-dimensional probability density function is:

[0067] ,

[0068] wherein, , , , psi v is the heading angle of the obstacle vehicle in the delay transmission process in the geodetic coordinate system, mu y and mu x are the original longitudinal and lateral positioning means of the vehicle given by the sensor or the estimation system, mu v is the speed distribution mean value given by the sensor or the estimation system, sigma y 2 and sigma x 2 are the corresponding variances of the longitudinal and lateral positioning, is the signal transmission delay caused by the communication system, and are the corresponding variances of the longitudinal and lateral positioning after the delay , is the Gaussian distribution variance to which the vehicle speed is subject, and are the longitudinal and lateral positioning means after the delay .

[0069] It should be noted that due to different types and principles of vehicle positioning systems such as GPS, inertial navigation and SLAM, the positioning error often presents a random distribution around the center, therefore, first, two independent Gaussian normal distributions are used to describe the longitudinal and lateral positioning error distribution of the obstacle vehicle, as shown in the following formula:

[0070] ,

[0071] wherein, mu x and mu x are the longitudinal and lateral positioning means, sigma x 2 and sigma y 2 respectively, similarly, the uncertainty of the speed estimate is described by a normal distribution, and the mean and variance are mu v and sigma v 2 ;

[0072] ,

[0073] Under the action of network communication delay tau , ignoring the acceleration in the signal transmission delay process, the actual position change of the obstacle vehicle can be derived by the following formula;

[0074]

[0075] wherein, psi v is the heading angle of the obstacle vehicle in the geodetic coordinate system during the delay transmission process, since the initial position and vehicle speed are subject to Gaussian distribution, the linear combination will still be subject to Gaussian distribution, then the mean and variance of the actual position Gaussian distribution can be calculated according to the following formula:

[0076] ;

[0077] ;

[0078] ;

[0079] When the longitudinal and lateral coordinates of the obstacle are integrated into a two-dimensional variable, a two-dimensional normal distribution of the obstacle position in the geodetic coordinate system is constructed, due to the independence of the longitudinal and lateral distribution, the covariance is zero, and the two-dimensional probability density function can be described as:

[0080] ;

[0081] ;

[0082] In this embodiment, the driving trajectory of the current vehicle is planned based on the risk feature boundary and the respective motion states of the current vehicle and the target vehicle in the target time domain. This includes: obtaining the number of target vehicles, the distribution probability within the confidence ellipse, the envelope circles of the current vehicle and the target vehicle, and the target vehicle's trajectory in the [missing information]. k The confidence ellipse radius of the step size, the approach function of the target lane, and the approach function of the target speed are used to determine the obstacle avoidance performance function of the target vehicle based on the number of target vehicles, the probability distribution within the confidence ellipse, the envelope circles of the current vehicle and the target vehicle, and the confidence ellipse radius of the target vehicle at the preset step size. The current vehicle's trajectory is planned based on the obstacle avoidance performance function, the approach function of the target lane, and the approach function of the target speed.

[0083] Specifically, based on the above prediction process, an obstacle avoidance performance function considering obstacle uncertainty is constructed, as shown in the following equation:

[0084]

[0085] In the formula, N ob This refers to the number of surrounding obstacle vehicles that may interfere with the current vehicle. P tσ ( t =1, 2) is the corresponding 1 sigma or 2 sigma The probability distribution within the confidence ellipse. r ego and r obj,k For the current vehicle and the first j The envelope circle of the obstacle vehicle. sigma obj,k,t For the first j The obstacle is in the first... k step length t- sigma Confidence ellipse radius, here sigma obj,k,t Since the relative direction between the obstacle vehicle and the current vehicle is relevant, the slope of the relative direction is calculated first:

[0086]

[0087] Then the radius of the confidence ellipse sigma obj,k,t The calculation is as follows:

[0088]

[0089] In addition to the obstacle avoidance performance function, define the approach function to the target lane and target speed respectively as:

[0090]

[0091]

[0092] Define the vehicle obstacle collision boundary constraint, lane boundary constraint respectively as:

[0093]

[0094]

[0095] Wherein, Y min And Y max is the lateral coordinate of the lane boundary, L w is half the width of the current vehicle body.

[0096] In addition, define the amplitude and gradient constraints of the front wheel steering angle and vehicle acceleration as:

[0097]

[0098]

[0099] In the formula, delta fmin , delta fmax , a min And a max is the minimum and maximum value of the steering angle and acceleration, is the corresponding gradient limit.

[0100] Through the obstacle avoidance performance function of the vehicle model in the prediction time domain, the target lane approach function and the target speed approach function, the above constraints need to be met, and the following model predictive control problem is constructed and solved, realizing the trajectory planning considering uncertainty.

[0101] Therefore, according to the planning of the driving trajectory of the current vehicle according to the obstacle avoidance performance function, the approach function of the target lane and the approach function of the target speed, including:

[0102] ,

[0103] Wherein, co 1, co 2,​​​co 3 is a weight coefficient of the corresponding obstacle avoidance performance function R, the approaching function Q of the target lane, and the approaching function V of the target speed;

[0104] wherein the constraint condition is:

[0105] ,

[0106]

[0107]

[0108]

[0109]

[0110] wherein (X k , Y k ) is the geodetic coordinate of the controlled vehicle at k moment, (X obj,k , Y obj,k ) is the geodetic coordinate of the jth obstacle vehicle at k moment, is the system control input at k moment, and T is the system discrete step length ，Y min and Y max is the coordinate of the lane boundary, L w is half of the current vehicle body width, delta fmin , delta fmax , a min and a max are minimum and maximum values of the steering angle and acceleration, respectively, , , , is the corresponding gradient limit; delta fk and a k are the front wheel steering angle and the acceleration of the controlled vehicle at k moment, respectively.

[0111] It can be understood that the embodiments of the present application can plan the driving trajectory of the current vehicle according to the obstacle avoidance performance function, the approaching function of the target lane, and the approaching function of the target speed, thereby realizing optimal trajectory planning of the autonomous vehicle considering obstacle uncertainty, and improving the scene adaptability of the autonomous driving system.

[0112] According to the automatic driving vehicle trajectory planning method provided in the embodiment of the application, the motion state of the current vehicle in a target time domain is predicted according to the motion data of the current vehicle, and the motion state of the target vehicle in the target time domain is predicted according to the motion data of the target vehicle; the risk feature boundary of the position uncertainty of the target vehicle under the network delay condition is identified, and the driving trajectory of the current vehicle is planned based on the risk feature boundary and the respective motion states of the current vehicle and the target vehicle in the target time domain, so as to realize the optimal trajectory planning of the automatic driving vehicle considering the uncertainty of the obstacle, and improve the scene adaptability of the automatic driving system.

[0113] The automatic driving vehicle trajectory planning method of the application will be described in detail below, wherein the target vehicle of the application can be described as an obstacle vehicle, and the specific steps are as follows:

[0114] S1: Based on the network communication delay and the distribution characteristics of the positioning and speed estimation of the obstacle vehicle, the uncertainty two-dimensional Gaussian normal distribution probability density function of each obstacle vehicle is constructed

[0115] Since different types and principles of vehicle positioning systems such as GPS, inertial navigation and SLAM have positioning errors that often present a random distribution form around the center, first, two independent Gaussian normal distributions are used to describe the longitudinal and lateral positioning error distributions of the obstacle vehicle, as shown in the following formula:

[0116] (1)

[0117] wherein, mu x and mu x are the longitudinal and lateral positioning means, sigma x 2 and sigma y 2 are the corresponding variances, similarly, the uncertainty of the speed estimation is described by a normal distribution, and the mean and variance are mu v and sigma v 2

[0118] (2)

[0119] Under the action of the network communication delay tau , the acceleration in the signal transmission delay process is ignored, and the actual position change of the obstacle vehicle can be derived by the following formula

[0120] (3)

[0121] where, psi v is the heading angle of the obstacle vehicle in the ECEF coordinate system during the delay propagation process, since both the initial position and the vehicle speed obey Gaussian distribution, the linear combination of them will still obey Gaussian distribution, then the mean and variance of the actual position Gaussian distribution can be calculated as

[0122] (4)

[0123] When the longitudinal and lateral coordinates of the obstacle are integrated into a two-dimensional variable, a two-dimensional normal distribution of the obstacle position in the ECEF coordinate system is constructed, due to the independence of the longitudinal and lateral distributions, the covariance is zero, and the two-dimensional probability density function can be described as

[0124] (5)

[0125] where, ψ v is the heading angle of the obstacle vehicle in the ECEF coordinate system during the delay propagation process, μ y and μ x are the mean values of the longitudinal and lateral positioning respectively, σ y 2 and σ x 2 are the corresponding variances of the longitudinal and lateral positioning respectively, is the signal transmission delay caused by the communication system, and are the corresponding variances of the longitudinal and lateral positioning after the delay is removed, is the Gaussian distribution variance obeyed by the vehicle speed, and are the mean values of the longitudinal and lateral positioning after the delay is removed.

[0126] The above model analyzes the coupling of speed uncertainty and position uncertainty in the delay process on the basis of considering the communication delay in the networked environment, and forms a two-dimensional normal distribution model of the anisotropy of the actual position of the obstacle vehicle.

[0127] S2: Construct 1σ and 2σ confidence ellipses of two-dimensional normal distribution as uncertainty risk feature boundaries

[0128] Different obstacle vehicle positioning often has different degrees of variance, in order to facilitate further unified description of driving risks caused by uncertainty in the planning process, the uncertainty risk feature boundary is constructed based on the 3-σ principle of normal distribution. For any mean value mu and variancesigma a normal distribution, whose cumulative probability distribution obeys the following characteristics

[0129] (6)

[0130] Correspondingly, for the two-dimensional normal distribution as shown in formula (5), the 1σ and 2σ confidence intervals in the longitudinal and lateral directions respectively constitute the confidence ellipse of the two-dimensional normal distribution, as shown in the following formula

[0131] (7)

[0132] In the formula, t=(1, 2) is a factor for adjusting the different confidence ellipses of 1σ and 2σ. Here, it is considered that the 2σ confidence ellipse has covered 95.44% of the probability, so the 1σ and 2σ confidence ellipses are taken as the risk feature boundary for describing the uncertainty of the position of the obstacle vehicle, and are integrated with the trajectory planning based on model predictive control, so as to realize the unified description of the relative distance and uncertainty of the obstacle vehicle.

[0133] S3: Based on the discretized vehicle kinematic model, the position of the vehicle in the prediction time domain is deduced

[0134] Further, the current vehicle is kinematically modeled, and the kinematic model of a single-track vehicle is as shown in formula (2), wherein Figure 1 , X , Y , psi , v , β , delta f and a are the longitudinal coordinate, lateral coordinate, yaw angle, resultant speed, center of mass side slip angle, front wheel steering angle and vehicle acceleration of the current vehicle, L f and L r are the distances from the center of mass to the front axle and the rear axle, and the model can be described by the following system of differential equations:

[0135] (8)

[0136] Further, the above system model is arranged in the form of the following nonlinear system state equation, as shown in formula (9), and is discretized according to the sampling time step Figure 2 : T

[0137] (9)​

[0138] in, X , Y , psi , v , delta f and a These represent the vehicle's longitudinal coordinates, lateral coordinates, yaw angle, composite velocity, front wheel steering angle, and vehicle acceleration, respectively. x and u represent the vehicle system state and control inputs, respectively. k and u k These represent the discrete vehicle system state and control input, respectively, with T being the discrete sampling time step.

[0139] The vehicle's position in the prediction time domain is deduced using a discretized vehicle kinematics model.

[0140] S4: A prediction model based on fixed steering speed and acceleration predicts the position of each obstacle vehicle in the prediction time domain.

[0141] To predict the motion state of the vehicle with obstacles in the model prediction time domain, the following prediction model with fixed steering speed and acceleration is adopted.

[0142] (10)

[0143] In the formula, X obj,k , Y obj,k , , v obj,k For the first j A vehicle with an obstacle k - The longitudinal coordinates, lateral coordinates, yaw angle, and vehicle speed at each step, and the acceleration of the vehicle with the obstacle. a obj and yaw rate omega obj It is assumed to be constant during the prediction process. Let be the discrete time step of the prediction process. The vehicle position state within each predicted step will follow a two-dimensional normal distribution model represented by equation (5).

[0144] S5: Calculate the 1σ and 2σ confidence ellipse radii based on the relative positions and directions of the current vehicle and obstacle vehicles within each prediction step, and calculate the obstacle avoidance performance function based on the confidence ellipse radii, as well as the target lane and speed approach functions.

[0145] Based on the above prediction process, an obstacle avoidance performance function considering the uncertainty of vehicles with obstacles is constructed, as shown in the following equation:

[0146] (11)

[0147] where, N ob is the number of obstacle vehicles that can potentially interfere with the current vehicle, P tσ (1 t =1, 2) is the distribution probability of the corresponding 1 sigma or 2 sigma dimensional confidence ellipse, r ego and r obj,k is the envelope circle of the current vehicle and the i j th obstacle vehicle, sigma obj,k,t is the t j h obstacle vehicle's t k h step confidence ellipse radius, where sigma sigma obj,k,t is related to the relative direction of the obstacle vehicle and the current vehicle, so the slope of the relative direction is calculated first

[0148] (12)

[0149] Then the confidence ellipse radius sigma obj,k,t is calculated as

[0150] (13)

[0151] In addition to the obstacle avoidance performance function, the approaching functions to the target lane and target speed are defined as

[0152] (14)

[0153] (15)

[0154] The vehicle obstacle vehicle collision boundary constraint and lane boundary constraint are defined as

[0155] (16)

[0156] (17)

[0157] where, Y min and Y max is the geodetic lateral coordinate of the lane boundary, L w is half of the current vehicle's body width

[0158] ​Furthermore, the amplitude and gradient constraints of front wheel steering angle and vehicle acceleration are defined as

[0159] (18)

[0160] (19)

[0161] wherein, delta fmin , delta fmax , a min and a max are the minimum and maximum values of steering angle and acceleration, , , , are the corresponding gradient limits.

[0162] S6: Construct a model predictive control problem, solve within the collision boundary constraint, lane boundary constraint, controller amplitude constraint and gradient constraint range, and obtain the optimal trajectory.

[0163] By optimizing the obstacle avoidance performance function, target lane approaching function and target speed approaching function of the vehicle model of formula (9) in the prediction time domain, while satisfying the constraints of formula (16) to (19), a model predictive control problem is constructed and solved, which realizes the trajectory planning considering uncertainty, wherein co 1, co 2, co 3 are the corresponding weight coefficients.

[0164] According to the obstacle avoidance performance function, the target lane approaching function and the target speed approaching function, the driving trajectory of the current vehicle is planned, including:

[0165]

[0166] wherein, co 1, co 2, co 3 are the weight coefficients of the obstacle avoidance performance function R, the target lane approaching function Q and the target speed approaching function V;

[0167] wherein, the constraint conditions are:

[0168] ,

[0169] ,

[0170] ,

[0171] , (20)

[0172] ,

[0173] wherein (X k , Y k ) is the controlled vehicle geodetic coordinate at time k, (X obj,k , Y obj,k ) is the jth obstacle vehicle geodetic coordinate at time k, is the system control input at time k, and T is the system discrete step ，Y min and Y max is the coordinate of the lane boundary, L w is half of the current vehicle body width, delta fmin , delta fmax , a min and a max are the minimum and maximum values of the steering angle and acceleration, respectively, , , , are the corresponding gradient limits; delta fk and a k are the front wheel steering angle and the controlled vehicle acceleration at time k, respectively.

[0174] In summary, the application proposes an obstacle vehicle uncertainty description method considering the space-time coupling of communication delay and perception error for a networked automatic driving vehicle. By analyzing the evolution relationship of vehicle state error along the communication delay, a two-dimensional normal distribution model of the obstacle vehicle position considering the communication delay is constructed. Based on the 3-σ principle of normal distribution, the 1σ and 2σ confidence ellipses of the two-dimensional normal distribution are proposed as the risk feature boundary considering the positioning uncertainty. The risk feature boundary is combined with the model predictive control, and a different risk feature boundary weight coefficient determination method based on the cumulative distribution probability of the confidence ellipse is proposed. An obstacle avoidance performance function considering the actual position uncertainty of the obstacle vehicle is constructed in the model predictive control, so as to realize the optimal trajectory planning of the automatic driving vehicle considering the obstacle vehicle uncertainty.

[0175] Figure 3 is a structural schematic diagram of a vehicle provided by an embodiment of the application. The vehicle can include:

[0176] a memory 301, a processor 302, and a computer program stored on the memory 301 and executable on the processor 302.

[0177] The processor 302 implements the automatic driving vehicle trajectory planning method provided in the above embodiments when executing a program.

[0178] Further, the vehicle further comprises:

[0179] The communication interface 303 is configured to communicate between the memory 301 and the processor 302.

[0180] The memory 301 is configured to store a computer program capable of being executed by the processor 302.

[0181] The memory 301 can include a high-speed RAM memory, and can further include a non-volatile memory, for example, at least one disk memory.

[0182] If the memory 301, the processor 302 and the communication interface 303 are implemented independently, the communication interface 303, the memory 301 and the processor 302 can be connected to each other through a bus and complete communication between each other. The bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. The bus can be divided into an address bus, a data bus, a control bus, etc. For the convenience of representation, Figure 3 Only one thick line is used in the figure, but it does not mean that there is only one bus or one type of bus.

[0183] Optionally, in a specific implementation, if the memory 301, the processor 302 and the communication interface 303 are integrated on a chip, the memory 301, the processor 302 and the communication interface 303 can complete communication between each other through an internal interface.

[0184] The processor 302 can be a Central Processing Unit (CPU), or an Application Specific Integrated Circuit (ASIC), or one or more integrated circuits configured to implement one or more embodiments of the present application.

[0185] In the description of the specification, the description of the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" and the like means that the specific features, structures, materials or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present application. In the description of the specification, the illustrative description of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any appropriate manner in any one or N embodiments or examples. In addition, those skilled in the art can combine and combine the different embodiments or examples described in the specification and the features of the different embodiments or examples without contradiction.

[0186] In addition, the terms "first", "second" are only for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined with "first", "second" can explicitly or implicitly include at least one of the features. In the description of the present application, the meaning of "N" is at least two, for example, two, three, etc., unless otherwise specifically limited.

[0187] Any process or method descriptions in flow charts or otherwise described herein can be understood as representing code modules, segments, or portions of code that include one or more executable instructions for implementing specific logic functions (or steps) in the process, and that the various embodiments of the application can include additional or fewer steps or processes in alternative implementations, as will be appreciated by those skilled in the art. The various embodiments of the application can be implemented in hardware, software, firmware, or a combination thereof, as desired.

[0188] It should be understood that parts of the present application can be implemented in hardware, software, firmware, or a combination thereof. In the above-described embodiments, N steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. As in another embodiment, if implemented in hardware, any one or more of the following technologies known in the art can be used: discrete logic circuit with logic gates for implementing logic functions on data signals, application specific integrated circuit with suitable combination logic gates, programmable gate array (PGA), field programmable gate array (FPGA), etc.

[0189] Those skilled in the art of the present technology can understand that all or part of the steps carried out by the above-described embodiment method can be completed by a program instructing the relevant hardware, and the program can be stored in a computer readable storage medium. The program, when executed, includes one or a combination of steps of the method embodiments.

Claims

1. An automatic driving vehicle trajectory planning method, characterized in that, The method comprises the following steps: acquiring motion data of a current vehicle and motion data of a target vehicle; predicting motion states of the current vehicle in a target time domain according to the motion data of the current vehicle and predicting motion states of the target vehicle in the target time domain according to the motion data of the target vehicle; identifying a risk feature boundary of position uncertainty of the target vehicle under network delay, and planning a driving trajectory of the current vehicle based on the risk feature boundary and the motion states of the current vehicle and the target vehicle in the target time domain, wherein the identifying the risk feature boundary of position uncertainty of the target vehicle under network delay comprises: acquiring a Gaussian normal distribution of a lateral position, a Gaussian normal distribution of a longitudinal position, a Gaussian normal distribution of a speed and a heading angle of the target vehicle before network delay, determining a two-dimensional probability density function of an actual position of the target vehicle according to the Gaussian normal distribution of the lateral position, the Gaussian normal distribution of the longitudinal position, the Gaussian normal distribution of the speed and the heading angle, wherein the actual position of the target vehicle is subject to the Gaussian normal distribution, and determining a risk feature boundary of position uncertainty of the target vehicle according to the two-dimensional probability density function and a confidence ellipse of the two-dimensional normal distribution, wherein the two-dimensional probability density function is: ， where, , , , ψ v is the heading angle of the obstacle vehicle in the earth coordinate frame during the delay transmission process, μ y and μ x are the raw longitudinal and lateral position means given by the sensor or estimation system respectively, μ v is the speed distribution mean given by the sensor or estimation system, σ y 2 and σ x 2 are the corresponding variances of the longitudinal and lateral position respectively, is the signal transmission delay caused by the communication system, and are the corresponding variances of the longitudinal and lateral position after the delay , is the Gaussian distribution variance of the vehicle speed, and are the corresponding means of the longitudinal and lateral position after the delay . 2.The automatic driving vehicle trajectory planning method of claim 1, wherein, The predicting the motion states of the current vehicle in the target time domain according to the motion data of the current vehicle comprises: constructing an initial kinematic model according to the running data of the vehicle; converting the initial kinematic model into a nonlinear system state equation, and discretizing the nonlinear system state equation to generate a kinematic model; inputting the motion data of the current vehicle into the kinematic model, and outputting the motion states of the current vehicle in the target time domain by the kinematic model. 3.The automatic driving vehicle trajectory planning method of claim 2, wherein, The kinematic model is: ， ， ， wherein, X , Y , ψ , v , δ f and a are the longitudinal coordinate, lateral coordinate, yaw angle, composite speed, front wheel steering angle and vehicle acceleration of the current vehicle, x and u are the vehicle system state and control input, x k and u k are the discretized vehicle system state and control input, T is the discrete sampling time step. 4.The automatic driving vehicle trajectory planning method of claim 1, wherein, The predicting the motion states of the target vehicle in the target time domain according to the motion data of the target vehicle comprises: inputting the motion data of the target vehicle into a prediction model, wherein the prediction model outputs the motion states of the target vehicle in the target time domain.

5. The method of claim 4, wherein, The prediction model is: wherein, the longitudinal coordinate, lateral coordinate, yaw angle and vehicle speed of the i-th obstacle vehicle at the step size of j k a obj is the acceleration of the target vehicle, ω obj is the yaw rate, is the discrete time step size of the prediction process.​​ 6. The method of claim 5, wherein, The planning the driving trajectory of the current vehicle based on the risk feature boundary and the motion states of the current vehicle and the target vehicle in the target time domain comprises: the number of target vehicles, the distribution probability within the confidence ellipse, the envelope circle of the current vehicle and the target vehicle, the envelope circle of the target vehicle in the first k the confidence ellipse radius of the step length, the approaching function of the target lane, and the approaching function of the target speed; determining an obstacle avoidance performance function of the target vehicle uncertainty according to the number of the target vehicles, the distribution probability in the confidence ellipse, the envelope circle of the current vehicle and the target vehicle, and the confidence ellipse radius of the target vehicle at a preset step length; planning the driving trajectory of the current vehicle according to the obstacle avoidance performance function, an approaching function of the target lane and an approaching function of the target speed.

7. The method of claim 6, wherein, The planning the driving trajectory of the current vehicle according to the obstacle avoidance performance function, the approaching function of the target lane and the approaching function of the target speed comprises: wherein, co 1, co 2, co 3 are weight coefficients of the corresponding obstacle avoidance performance function R, the approaching function Q of the target lane, and the approaching function V of the target speed. wherein the constraint condition is: ， where (X k , Y k ) is the controlled vehicle geodetic coordinate at time k, (X obj,k , Y obj,k ) is the jth obstacle vehicle geodetic coordinate at time k, is the system control input at time k, and T is the system discrete step size ，Y min and Y max are the coordinates of the lane boundary, L w is half of the current vehicle body width, δ fmin , δ fmax , a min and a max are the minimum and maximum values of the steering angle and acceleration, respectively, , , , are the corresponding gradient limits; δ fk and a k are the front wheel steering angle and the controlled vehicle acceleration at time k, respectively.

8. A vehicle comprising: a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to perform the trajectory planning method of the autonomous vehicle according to any one of claims 1-7.

Images