Track planning method and system for sensing blind area when vehicle turns at non-signal-control intersection

Abstract

The invention relates to the technical field of unmanned driving, and discloses a track planning method and system for sensing a blind area when a vehicle turns at a non-signal-control intersection. The track planning method for sensing the blind area when the vehicle turns at the non-signal-control intersection comprises the following steps: S1, establishing a kinematic model of a vehicle track; s2, constructing an objective function J; s3, establishing constraint conditions for the data in the step S2, and optimizing the objective function J by using the constraint conditions to obtain an optimized overall optimization model; and S4, based on the overall optimization model, solving an optimized track through a nonlinear optimization solver. According to the invention, the passing efficiency and safety of the unmanned vehicle can be improved.

Description

Technical Field

[0001] The present invention relates to the field of unmanned driving technology, and in particular to a trajectory planning method and system for sensing blind spots when a vehicle turns at a non-signal-controlled intersection. Background Art

[0002] When an unmanned vehicle passes through a non-signalized intersection, it needs to plan a safe and feasible driving trajectory based on the surrounding environment, road maps, vehicle status, etc. However, parked vehicles or buildings at the intersection may cause blind spots in environmental perception, making it impossible for the unmanned vehicle to obtain complete environmental information.

[0003] Existing autonomous vehicle trajectory planning methods usually assume that the vehicle can obtain all environmental perception information, which is not a reasonable assumption. Due to the potential vehicles or pedestrians in the perception blind spot, the lack of assessment of these risks leads to the autonomous vehicle often taking conservative actions such as slowing down or stopping when approaching the intersection, which affects the traffic efficiency and may even cause the vehicle to "freeze".

[0004] Therefore, there is an urgent need for a new type of trajectory planning method and system for perceiving blind spots when turning at non-signal-controlled intersections to solve the above problems. Summary of the invention

[0005] 1. Technical issues to be resolved

[0006] In view of the above-mentioned shortcomings and deficiencies of the prior art, the present invention provides a trajectory planning method and system for perceiving blind spots when a vehicle turns at a non-signal-controlled intersection, so as to solve the problems of the prior art.

[0007] (II) Technical solution

[0008] To achieve the above object, a trajectory planning method for sensing a blind spot when a vehicle turns at a non-signal-controlled intersection comprises the following steps:

[0009] S1, by formula:

[0010] A kinematic model of vehicle trajectory is established; among them, is the derivative of the vehicle position abscissa y, is the derivative of the vehicle position abscissa y, is the vehicle heading angle The derivative of is the derivative of the vehicle steering angle δ, is the derivative of the velocity; x is the horizontal coordinate of the vehicle position, y is the vertical coordinate of the vehicle position, is the heading angle, δ is the steering angle, v is the speed, a is the acceleration, and L is the distance between the front and rear wheels of the vehicle.

[0011] S2, by the formula: Construct the objective function J;

[0012] Among them, N p For the prediction time domain, Q is its weight matrix, R is its weight matrix, and J obs,i is the obstacle avoidance function, J visibility,i is the visibility function, U i is the control input sequence, η(t+i|t) is the predicted state variable, η ref (t+i|t) is the reference state variable;

[0013] S3, establishing kinematic model constraint conditions, and using the constraint conditions to optimize the objective function J to obtain an optimized overall optimization model;

[0014] S4, based on the overall optimization model, solving the optimized trajectory through a nonlinear optimization solver.

[0015] Preferably, the constraint conditions in step S3 include one or more of the following steps:

[0016] S31, by formula: 0≤v k+i|k ≤v max (1), establish velocity constraints; where v max is the maximum speed that the vehicle can reach, k represents the kth moment, v k+i|k is the predicted value of the speed of the future i-th step at time k;

[0017] S32, by formula: a min ≤a k+i|k ≤a max (2), establish acceleration constraints; where a min is the maximum deceleration that the vehicle can achieve, a max is the maximum acceleration that the vehicle can achieve, a k+i|k is the predicted value of acceleration for the future i-th step at time k;

[0018] S33, by formula: δ min ≤δ k+i|k ≤δ max (3), establish the steering angle constraint; where δ min is the minimum steering angle that the vehicle can achieve, δ max is the maximum steering angle that the vehicle can achieve, δ k+i|k is the predicted value of the steering angle for the future i-th step at time k;

[0019] S34, through the formula: x min ≤x k+i|k ≤x max(4), establish boundary constraints; where x min is the minimum lateral movement distance from the vehicle reference line to the left and right road boundaries, x max is the maximum lateral movement distance from the vehicle reference line to the left and right road boundaries, x k+i|k is the predicted value of the horizontal coordinate of the position of the future i-th step at time k;

[0020] S35, by formula: r k+1 =P(Z t+1 ∣c t+1 )≤ε(5), establish risk constraints; where ε is the risk threshold, r k+1 is the risk calculation value at time t+1, Z t+1 ∈{0,1} indicates whether there is a risky vehicle in the opposite lane of the blind spot intersection at time t+1, Z t+1 =1 means that a vehicle appears at time t+1, Z t+1 =0 means that there is no vehicle at time t+1; C represents the observation result, c t +1 ∈{0,1} represents the corresponding observation probability value, c t+1 =0 means no vehicle is observed at time t+1, c t+1 =1 means that the vehicle is observed at time t+1; P(Z t+1 ∣c t+1 ) is the posterior probability at time t+1.

[0021] Preferably, by the formula: Establish an overall optimization model.

[0022] Preferably, J in step S2 obs,i The calculation formula is:

[0023]

[0024] Among them, i represents the i-th moment, W obs is the weight coefficient, (x obs-i ,y obs-i ) is the position coordinate of the visible obstacle at time i relative to the unmanned vehicle body coordinate system, (x i ,y i ) is the coordinate of the center of mass of the unmanned vehicle at time i, ζ is a positive number to prevent the denominator from being 0, and v i is the speed of the vehicle at time i.

[0025] Preferably, J in step S2 visibility,i The calculation formula is:

[0026]

[0027] Among them, Wvis is the weight coefficient, θ i is the occlusion angle at time i, Δx i and Δy i is the distance between the position coordinates of the vehicle at time i and the blind spot coordinates of the slow point.

[0028] Preferably, the blind spot is the vertex of the perception blind area caused by the obstruction of the roadside buildings, and the blind spot coordinates are the geographic coordinates of the vertex provided by the map positioning system.

[0029] Preferably, P(Z) in step S35 t+1 ∣c t+1 ) is calculated as:

[0030]

[0031] Among them, P(Z t+1 ∣c t+1 ) is the posterior probability at time t+1, P(Z t+1 =1) is the prior probability of the risk vehicle appearing at time t+1, P(c t+1 ∣Z t+1 =0) is the probability of observing a vehicle when there is no risk vehicle at time t+1, P(c t+1 ∣Z t+1 =1) is the probability of observing a vehicle when a risky vehicle appears at time t+1, and P(C=c) is the prior probability distribution of the observation result C.

[0032] Preferably, the kinematic model described in step S1 is a combination of a bicycle kinematic model and an MPC model.

[0033] Preferably, the method further comprises step S36, wherein the calculation formula for establishing the kinematic constraint condition is: k+1 =f(s k ,u k ), where s k+1 is the state quantity at time k+1, u k is the control quantity at time k, s k is the state quantity at time k, and f is the state transfer function obtained from the state transfer matrix.

[0034] 10. A trajectory planning system for a vehicle to perceive a blind spot when turning at a non-signal-controlled intersection, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of a trajectory planning method for a vehicle to perceive a blind spot when turning at a non-signal-controlled intersection as described in any one of the above items when executing the computer program.

[0035] (III) Beneficial effects

[0036] The present invention can prevent unmanned vehicles from taking overly cautious actions due to perception blind spots through reasonable trajectory planning and risk assessment, thereby improving the efficiency of passing non-signal-controlled intersections and preventing vehicles from falling into a "frozen state", thereby improving driving efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 A schematic flow chart of a trajectory planning method for sensing a blind spot when a vehicle turns at a non-signal-controlled intersection according to the present invention;

[0038] Figure 2 A schematic diagram for generating a cubic Bezier curve of the present invention;

[0039] Figure 3 It is a schematic diagram of the kinematic model of the present invention;

[0040] Figure 4 It is a schematic diagram of the optimization curve of the present invention;

[0041] Figure 5 It is a risk assessment schematic diagram of the dynamic Bayesian network model;

[0042] Figure 6 It is a schematic diagram of the dynamic Bayesian network structure;

[0043] Figure 7 This is a schematic diagram of the planning system flow. DETAILED DESCRIPTION

[0044] In order to better explain the present invention and facilitate understanding, the present invention is described in detail below through specific implementation modes in conjunction with the accompanying drawings.

[0045] It should be noted that all directional indications in the embodiments of the present invention (such as up, down, left, right, front, back, etc.) are only used to explain the relative position relationship, movement status, etc. between the components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly.

[0046] In addition, in the present invention, descriptions such as "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" or "second" may explicitly or implicitly include at least one of the features. In the description of the present invention, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0047] To achieve this, please refer to Figures 1 to 7 , a trajectory planning method for sensing a blind spot when a vehicle turns at a non-signal-controlled intersection, comprising the following steps:

[0048] S1, by formula:

[0049] A kinematic model of vehicle trajectory is established; among them, is the derivative of the vehicle position abscissa y, is the derivative of the vehicle position abscissa y, is the vehicle heading angle The derivative of describes the speed at which the vehicle's direction changes. is the derivative of the vehicle steering angle δ, which describes the rate of change of the steering angle over time, is the derivative of the velocity, x is the horizontal coordinate of the vehicle position, y is the vertical coordinate of the vehicle position, is the heading angle, δ is the steering angle, v is the speed, a is the acceleration, and L is the distance between the front and rear wheels of the vehicle;

[0050] S2, construct the objective function J, through the formula:

[0051]

[0052] Among them, N p For the prediction time domain, Q is its weight matrix, R is its weight matrix, and J obs,i is the obstacle avoidance function, J visibility,i is the visibility function, U i is the control input sequence, η(t+i|t) is the predicted state variable, η ref (t+i|t) is the reference state variable;

[0053] S3, establishing constraint conditions for the data in step S2, and using the constraint conditions to optimize the objective function J to obtain an optimized overall optimization model;

[0054] S4, based on the overall optimization model, solving the optimized trajectory through a nonlinear optimization solver.

[0055] In step S1, since the driving task of the unmanned vehicle passing through the non-wire-controlled intersection belongs to a low-speed scenario, the vehicle's speed, acceleration and other parameters will not be too large, and the friction force is not saturated, so there is no need to consider dynamic parameters such as vehicle friction and tire slip. The bicycle kinematic model abstracts the vehicle's kinematic characteristics into a bicycle model by simplifying the vehicle dynamics and ignoring the longitudinal and lateral dynamic coupling of the vehicle, making the control algorithm easier to implement and calculate. At the same time, the bicycle kinematic model can accurately describe the movement of the vehicle in a two-dimensional plane, and is suitable for trajectory planning and tracking problems, especially the trajectory optimization problem of vehicles with low or medium speeds. Therefore, the bicycle kinematic model is selected in combination with the Model Predictive Control (MPC) model for trajectory optimization solution.

[0056] Preferably, the kinematic model described in step S1 is a combination of a bicycle kinematic model and an MPC model; see Figure 3 , x k That is, the current coordinate of the vehicle, y k That is, the current coordinates of the vehicle, φ k That is, the current heading angle of the vehicle, v is the speed of the vehicle, and L is the distance between the front and rear wheels of the vehicle.

[0057] For details, see Figure 2 , in the process of constructing the objective function J, the specific ideas are as follows:

[0058] The path curve segment is generated by cubic Bezier curve. The principle is that the shape of the Bezier curve can be changed by selecting control points to meet the requirements of the driving path of the unmanned vehicle.

[0059] The cubic Bezier curve requires 4 coordinate points for control, which are denoted as P 0 , P 1 , P 2 , P 3 ;

[0060] P 0 (Starting point): The position where the driverless vehicle is about to enter the intersection.

[0061] P 3 (End point): The point on the target lane that the autonomous vehicle enters after completing the left turn.

[0062] P 1 (First control point): The initial curved part used to guide the left turn, located at P 0 On the extension line of , offset to the left an appropriate distance.

[0063] P 3 (Second control point): used to determine the middle part of the left turn curve, located at P 3 The curve is extended to the left to ensure that the curve ends smoothly in the target lane.

[0064] The coordinates of the four control points can be pre-calculated from parameters such as intersection shape and lane coordinates calculated from the high-definition map provided by the map positioning system.

[0065] The calculation formula of the parameter equation B(t) of the cubic Bezier curve is:

[0066] B(t)=P 0 (1-t) 3 +3P 1 (1-t) 2 t+3P 2 (1-t)t2 +P 3 t 3 ,

[0067] Among them, B(t) represents the generated curve point, t is a parameter ranging from 0 to 1, and P i (i=0,1,2,3) are the coordinates of the control points.

[0068] By discretizing the parameter t, that is, selecting a series of t values ​​at equal intervals between 0 and 1, for each discrete t value, the corresponding curve point is calculated using the parameter equation of the cubic Bezier curve. Compared with the search-based methods such as A* and Dijkstra, the method based on the cubic Bezier curve has the advantages of continuous and differentiable curvature, easy tracking, and simple implementation (only a small number of control points are needed to generate a path). Those skilled in the art will not elaborate on this.

[0069] Since global planning does not consider dynamic obstacles and there are generally no static obstacles at intersections, in order to make the driverless vehicle pass through the intersection as quickly as possible, the expected speed can be set to the maximum safe speed determined by the maximum speed limit of the road and the turning radius constraint. The expected speed V ref The calculation formula is:

[0070]

[0071] Among them, v limit is the maximum speed limit of the road, r is the turning radius, g is the acceleration of gravity, and μ is the friction coefficient of the road surface.

[0072] Preferably, the obstacle avoidance function J in step S2 obs,i The calculation formula is:

[0073]

[0074] Among them, i represents the i-th moment, W obs is the weight coefficient, (x obs-i ,y obs-i ) is the position coordinate of the visible obstacle at time i relative to the unmanned vehicle body coordinate system, (x i ,y i ) is the coordinate of the center of mass of the unmanned vehicle at time i, ζ is a positive number to prevent the denominator from being 0, and v i is the speed of the vehicle at time i.

[0075] Before an unmanned vehicle enters an intersection, part of the lane will become a perception blind spot due to the occlusion of roadside buildings, but the size of the occlusion area will change as the position of the unmanned vehicle moves. The degree of occlusion is quantified by obtaining the occlusion key points and calculating the occlusion angle, and then the visibility reward function is designed based on the changing characteristics of the occlusion angle.

[0076] Define the angle between the light emitted by the driverless vehicle sensor and the blind spot O as the blocking angle θ, then the blocking angle θ at a certain time i before the driverless vehicle enters the intersection i The calculation formula is:

[0077]

[0078] Among them, Δy i , Δx i are the position coordinates (x i ,y i ) and the coordinates of the blind spot B (x occlued ,y occlued ) distance, Δy i , Δx i The calculation formula is:

[0079] Δx i =|x i -x occlued |,Δy i =|y i -y occlued |,

[0080] Before the driverless vehicle enters the intersection, the occlusion angle exists until it disappears after entering the intersection; when the driverless vehicle gradually approaches the intersection, the occlusion angle decreases as the vehicle moves forward; when the driverless vehicle moves to the side away from the building, the occlusion angle will also decrease.

[0081] Based on these characteristics, a visibility function can be designed to guide the driverless vehicle to increase its visibility in the blind spot by changing its position before entering the intersection, so that it can obtain more environmental perception information and achieve the effect of maximum information gain.

[0082] Preferably, J in step S2 visibility,i The calculation formula is:

[0083]

[0084] Among them, W vis is the weight coefficient.

[0085] The visibility function is defined as the ratio of the occlusion angle to the longitudinal coordinate distance. There are two main reasons for this: First, when the longitudinal distance between the driverless vehicle and the blind spot O is too large, J visibility The result tends to 0. At this time, the movement of the driverless vehicle will not be affected by the visibility function and will not change the lateral position of the vehicle prematurely. That is, the formula:

[0086]

[0087] Second, as the autonomous vehicle approaches the intersection and the longitudinal distance to blind spot B is closer, J visibility The result is similar to By increasing Δx, we can achieve J visibility Reduce to achieve the goal of increasing the visible area, that is, the formula:

[0088] Preferably, the blind spot is the vertex of the perception blind area caused by the obstruction of roadside buildings, and the blind spot coordinates are the geographic coordinates of the vertex provided by the map positioning system; when entering a non-signal-controlled intersection, the vehicle can adjust its own driving path according to the position of the blind spot to minimize the potential collision risk and improve driving safety and efficiency.

[0089] The working principle of driverless vehicle perception can be considered that the sensor collects information about the surrounding environment by emitting light in all directions. If there is an object within the working range of the sensor, the emitted light cannot penetrate the object, resulting in a perception blind spot behind the object. Figure 2 It can be seen that the top left corner of the building is the key point that causes the perception blind spot. The coordinates of this point can be obtained through the high-definition map provided by the map positioning system, which is recorded as the blind spot O(x occlued ,y occlued ).

[0090] Furthermore, the constraint conditions in step S3 include one or more of the following steps:

[0091] S31, by formula: 0≤v k+i|k ≤v max (1), establish velocity constraints; where v max is the maximum speed that the vehicle can reach, k represents the kth moment, v k+i|k is the predicted value of the speed of the future i-th step at time k;

[0092] S32, by formula: a min ≤a k+i|k ≤a max (2), establish acceleration constraints; where a min is the maximum deceleration that the vehicle can achieve, a max is the maximum acceleration that the vehicle can achieve, a k+i|k is the predicted value of acceleration for the future i-th step at time k;

[0093] S33, by formula: δ min ≤δ k+i|k ≤δ max (3), establish the steering angle constraint; where δ minis the minimum steering angle that the vehicle can achieve, δ max is the maximum steering angle that the vehicle can achieve, δ k+i|k is the predicted value of the steering angle for the future i-th step at time k;

[0094] S34, through the formula: x min ≤x k+i|k ≤x max (4), establish boundary constraints; where x min is the minimum lateral movement distance from the vehicle reference line to the left and right road boundaries, x max is the maximum lateral movement distance from the vehicle reference line to the left and right road boundaries, x k+i|k is the predicted value of the lateral coordinate of the position of the i-th step in the future at time k;

[0095] S35, by formula: r k+1 =P(Z t+1 ∣c t+1 )≤ε(5), establish risk constraints; where ε is the risk threshold, r k+1 is the risk calculation value at time t+1, Z t+1 ∈{0,1} indicates whether there is a risky vehicle in the opposite lane of the blind spot intersection at time t+1, Z t+1 =1 means that a vehicle appears at time t+1, Z t+1 =0 means that there is no vehicle at time t+1; C represents the observation result, c t +1 ∈{0,1} represents the corresponding observation probability value, c t+1 =0 means no vehicle is observed at time t+1, c t+1 =1 means that the vehicle is observed at time t+1; P(Z t+1 ∣c t+1 ) is the posterior probability.

[0096] Dynamic Bayesian Network (DBN) can process time series data of multiple factors and infer the state of the system by combining prior probability and posterior probability. It is very suitable for decision-making planning of autonomous vehicles in complex environments. Therefore, the dynamic Bayesian network model is used to solve the risk assessment problem in non-wire-controlled intersection scenarios. The process and application relationship of dynamic Bayesian network in risk assessment of unmanned vehicles in this article are detailed in Figure 5 .

[0097] The scene context information is used to determine the prior probability, while the observation data of the driverless vehicle is used to update the posterior probability. The driverless vehicle samples data at a fixed time interval Δt and updates the potential risk prediction results in real time. Based on the prediction results, the driverless vehicle can adjust the speed to prevent risky driving or excessive caution. When the driverless vehicle is a certain distance away from the intersection and the visual range of the driverless vehicle gradually increases, the dynamic Bayesian network model can be used to predict whether there are potential vehicles in the perception blind spot entering the interaction area, thereby calculating the risk of collision with the autonomous vehicle. This prediction is achieved by considering the dependencies between nodes and updating the state in combination with new observation data. The specific steps are as follows:

[0098] Step 1: Select Bayesian network nodes

[0099] 1. Vehicle speed node, traffic density node

[0100] Consider the vehicle speed and traffic density as node traffic density in the dynamic Bayesian network, denoted as node B1 and node B2. The scene of passing through the non-wire-controlled intersection is a short-distance traffic section. The speed changes of vehicles on the same road section are similar. Therefore, the potential vehicles in the lanes in the perception blind spot can be described by a uniform speed V. From the Greenshields speed-traffic density model, it can be seen that there is a certain negative correlation between the potential vehicle speed V and the traffic density ρ. The calculation formula of the vehicle speed V is:

[0101]

[0102] Where V represents the potential vehicle speed, V f represents the maximum speed of potential vehicles when the traffic density is 0, ρ represents the traffic density, ρ j Represents congestion density (the density at which traffic is so dense that it is impossible to move).

[0103] 2. Traffic accident rate

[0104] For non-wire-controlled intersections with blind spots, traffic accidents are very likely to occur because traffic participants do not observe the traffic conditions sufficiently. The actual monitoring and recording data of the relevant departments at the intersection are used to calculate the traffic accident rate D, which is recorded as node B3.

[0105] 3. Sensor information

[0106] When an unmanned vehicle passes through a non-wire-controlled intersection with a blind spot, the probability of a collision risk in the blind spot is also related to the real-time observation data of the unmanned vehicle. The real-time observation data is divided into visible area information and occluded area information, which are recorded as nodes B4 and B5 respectively.

[0107] Step 2: Node network structure modeling

[0108] According to the principle of dynamic Bayesian network, the coupling relationship between various information factors is described by directed edges between nodes. The variables of various factors of scene context information and sensor information are used as child nodes of dynamic Bayesian network, and the risk assessment result of unit time period t is used as the parent node of dynamic Bayesian network (denoted as node A1). The network structure of dynamic Bayesian network established is detailed in Figure 6 .

[0109] Step 3: Create a conditional probability table

[0110] Prior probability refers to the probability that a node is in a certain state before considering any observed data. The sum of the prior probabilities of different possible states of each node is 1. In Bayesian networks, prior probabilities are usually determined based on historical data or expert knowledge. The conditional probability ρ of each prior node obtained based on historical data and parameter learning is shown in Table 1.

[0111] Table 1

[0112]

[0113] Assume that Z t ∈{0,1} indicates whether there is a risky vehicle in the opposite lane of the blind spot intersection at time t, Z t =1 means a vehicle appears, Z t =0 means no vehicle appears. P(Z t =0) and P(Z t =1) as the prior probability, then the result after Δt seconds is updated to P(Z t+1 ), P(Z t+1 ∣Z t ) represents the vehicle state at a given current time t (i.e., Z t The probability of a risky vehicle appearing or not appearing at the location at time t+1 under the condition of the value of P(Z) reflects the probability of transferring from the state at the current time t to the state at the next time t+1. t+1 ∣Z t ) is calculated as:

[0114]

[0115] Among them, k z is the state transfer coefficient of the dynamic Bayesian network, which can be allocated according to empirical data; ρ i is the prior probability of each node state; n is the number of node states.

[0116] Then use C to represent the observation result, c t+1∈{0,1} represents the corresponding observation probability value, c t+1 = 0 means no vehicle is observed, c t+1 =1 means that the vehicle is observed, and the observation data c is converted into t+1 and the prior probability Z after state transition t +1 Combined, the calculated posterior probability P(c t+1 ∣Z t+1 ) is the risk probability of the perception blind spot.

[0117] Preferably, P(Z) in step S35 t+1 ∣c t+1 ) is calculated as:

[0118]

[0119] Among them, P(c t+1 ∣Z t+1 ) is the probability of observing a specific result under different circumstances (i.e., whether there is a risky vehicle) at time t+1; P(Z t+1 =1) is the prior probability of the risk vehicle appearing at time t+1, P(c t+1 ∣Z t+1 =0) is the probability of observing a vehicle when there is no risk vehicle at time t+1, P(c t+1 ∣Z t+1 =1) is the probability of observing a vehicle when a risky vehicle appears at time t+1, and P(C=c) is the prior probability distribution of the observation result C.

[0120] In step S35, P(Z t+1 ∣c t+1 ) is the posterior probability, while the prior probability refers to the probability that a node is in a certain state before considering any observed data, and the sum of the prior probabilities of different possible states of each node is 1. In Bayesian networks, prior probabilities are usually determined based on historical data or expert knowledge.

[0121] The dynamic Bayesian network can process time series data of multiple factors and infer the state of the system by combining prior probability and posterior probability. It is very suitable for decision-making planning of autonomous driving vehicles in complex environments. Therefore, the dynamic Bayesian network model is used to solve risk assessment problems in non-wire-controlled intersection scenarios.

[0122] Consider the vehicle speed and traffic density as the node traffic density in the dynamic Bayesian network, denoted as node B1 and node B2. The scene of passing through the non-wire-controlled intersection is a short-distance traffic section. The speed changes of vehicles on the same road section are similar. Therefore, the potential vehicles in the lanes in the perception blind spot can be described by a uniform speed V. The calculation formula for the potential vehicle speed V is:

[0123]

[0124] Among them, V f represents the maximum speed of potential vehicles when the traffic density is 0, ρ represents the traffic density, ρ j Represents congestion density (the density when the traffic density is so high that vehicles cannot move).

[0125] Preferably, the method further comprises step S36, wherein the calculation formula for establishing the kinematic constraint condition is: k+1 =f(s k ,u k ), where s k+1 is the state quantity at time k+1, u k is the control quantity at time k, s k is the state quantity at time k, and f is the state transfer function obtained from the state transfer matrix.

[0126] Preferably, by the formula: Establish an overall optimization model.

[0127] In the above step S4, the optimization model established is a nonlinear constrained optimization problem, which can be solved using an existing nonlinear optimization solver. By continuously rolling optimization and iteratively updating the motion trajectory, the optimized trajectory can be solved. Figure 4 As shown, the trajectory contains a series of position, speed and angle information at different time points, which can be transmitted to the control system for execution to allow the unmanned vehicle to safely and efficiently pass through non-signal-controlled intersections with perceptual blind spots.

[0128] The present invention also provides a trajectory planning system for a vehicle to perceive a blind spot when turning at a non-signal-controlled intersection, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the steps of the above-mentioned trajectory planning method for a vehicle to perceive a blind spot when turning at a non-signal-controlled intersection are implemented.

[0129] Exemplarily, the computer program may be divided into one or more modules / units, which are stored in the memory and executed by the processor to implement the present invention. The one or more modules / units may be a series of computer program instruction segments capable of implementing specific functions, which are used to describe the execution process of the computer program in the computer.

[0130] The processor may be a central processing unit (CPU), other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor, etc.

[0131] The memory may be an internal storage unit, such as a hard disk or a memory; the memory may also be an external storage device, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. Further, the memory may include both an internal storage unit and an external storage device. The memory is used to store the computer program and other programs and data required by the terminal device. The memory may also be used to temporarily store data that has been output or is to be output.

[0132] It should be understood that the above description of the specific embodiments of the present invention is only for illustrating the technical route and features of the present invention, and its purpose is to enable those skilled in the art to understand the content of the present invention and implement it accordingly, but the present invention is not limited to the above specific implementation methods. Any changes or modifications made within the scope of the claims of the present invention should be included in the protection scope of the present invention.

Claims

1. A trajectory planning method for sensing blind spots when a vehicle turns at a non-signal-controlled intersection, characterized in that: The following steps are involved: S1, by formula: A kinematic model of vehicle trajectory is established; among them, is the derivative of the vehicle position abscissa y, is the derivative of the vehicle position abscissa y, is the vehicle heading angle The derivative of is the derivative of the vehicle steering angle δ, is the derivative of the velocity, x is the horizontal coordinate of the vehicle position, y is the vertical coordinate of the vehicle position, is the heading angle, δ is the steering angle, v is the speed, a is the acceleration, and L is the distance between the front and rear wheels of the vehicle. S2, by the formula: Construct the objective function J; Among them, N p For the prediction time domain, Q is its weight matrix, R is its weight matrix, and J obs,i is the obstacle avoidance function, J visibility,i is the visibility function, U i is the control input sequence, η(t+i|t) is the predicted state variable, η ref (t+i|t) is the reference state variable; S3, establishing kinematic model constraint conditions, and using the constraint conditions to optimize the objective function J to obtain an optimized overall optimization model; S4, based on the overall optimization model, solving the optimized trajectory through a nonlinear optimization solver.

2. A trajectory planning method for sensing blind spots when a vehicle turns at a non-signal-controlled intersection as claimed in claim 1, characterized in that: The constraint conditions in step S3 include one or more of the following steps: S31, by formula: 0≤v k+i|k ≤v max (1) Establish speed constraints; Among them, v max is the maximum speed that the vehicle can reach, k represents the kth moment, v k+i|k is the predicted value of the speed of the future i-th step at time k; S32, by formula: a min ≤a k+i|k ≤a max (2), establish acceleration constraints; where a min is the maximum deceleration that the vehicle can achieve, a max is the maximum acceleration that the vehicle can achieve, a k+i|k is the predicted value of acceleration for the future i-th step at time k; S33, by formula: δ min ≤δ k+i|k ≤δ max (3), establish the steering angle constraint; where δ min is the minimum steering angle that the vehicle can achieve, δ max is the maximum steering angle that the vehicle can achieve, δ k+i|k is the predicted value of the steering angle for the future i-th step at time k; S34, through the formula: x min ≤x k+i|k ≤x max (4), establish boundary constraints; where x min is the minimum lateral movement distance from the vehicle reference line to the left and right road boundaries, x max is the maximum lateral movement distance from the vehicle reference line to the left and right road boundaries, x k+i|k is the predicted value of the lateral coordinate of the position of the i-th step in the future at time k; S35, by formula: r k+1 =P(Z t+1 ∣c t+1 )≤ε(5), establish risk constraints; where ε is the risk threshold, r k+1 is the risk calculation value at time t+1, Z t+1 ∈{0,1} indicates whether there is a risky vehicle in the opposite lane of the blind spot intersection at time t+1, Z t+1 =1 means that a vehicle appears at time t+1, Z t+1 =0 means that there is no vehicle at time t+1; C represents the observation result, c t+1 ∈{0,1} represents the corresponding observation probability value, c t+1 =0 means no vehicle is observed at time t+1, c t+1 =1 means that the vehicle is observed at time t+1; P(Z t+1 ∣c t+1 ) is the posterior probability at time t+1.

3. A trajectory planning method for sensing blind spots when a vehicle turns at a non-signal-controlled intersection as claimed in claim 2, characterized in that: By formula: Establish an overall optimization model.

4. A trajectory planning method for sensing blind spots when a vehicle turns at a non-signal-controlled intersection as claimed in claim 1, characterized in that: J in step S2 obs，i The calculation formula is: Among them, i represents the i-th moment, W obs is the weight coefficient, (x obs-i ,y obs-i ) is the position coordinate of the visible obstacle at time i relative to the unmanned vehicle body coordinate system, (x i ,y i ) is the coordinate of the center of mass of the unmanned vehicle at time i, ζ is a positive number to prevent the denominator from being 0, and v i is the speed of the vehicle at time i.

5. A trajectory planning method for sensing blind spots when a vehicle turns at a non-signal-controlled intersection as claimed in claim 4, characterized in that: J in step S2 visibility,i The calculation formula is: Among them, W vis is the weight coefficient, θ i is the occlusion angle at time i, Δx i and Δy i is the distance between the position coordinates of the vehicle at time i and the blind spot coordinates of the slow point.

6. A trajectory planning method for sensing blind spots when a vehicle turns at a non-signal-controlled intersection as claimed in claim 5, characterized in that: The blind spot is the vertex of the perception blind area caused by the obstruction of the roadside buildings, and the blind spot coordinates are the geographic coordinates of the vertex provided by the map positioning system.

7. A trajectory planning method for sensing blind spots when a vehicle turns at a non-signal-controlled intersection as claimed in claim 2, characterized in that: P(Z) in step S35 t+1 ∣c t+1 ) is calculated as: Among them, P(Z t+1 =1) is the prior probability of the risk vehicle appearing at time t+1, P(c t+1 ∣Z t+1 =0) is the probability of observing a vehicle when there is no risk vehicle at time t+1, P(c t+1 ∣Z t+1 =1) is the probability of observing a vehicle when a risky vehicle appears at time t+1, and P(C=c) is the prior probability distribution of the observation result C.

8. A trajectory planning method for sensing blind spots when a vehicle turns at a non-signal-controlled intersection as claimed in claim 1, characterized in that: The kinematic model described in step S1 is a combination of a bicycle kinematic model and an MPC model.

9. A trajectory planning method for sensing blind spots when a vehicle turns at a non-signal-controlled intersection as claimed in claim 2, characterized in that: The method further includes step S36, wherein the calculation formula for establishing the kinematic constraint condition is: s k+1 =f(s k ,u k ), where s k+1 is the state quantity at time k+1, u k is the control quantity at time k, s k is the state quantity at time k, and f is the state transfer function obtained from the state transfer matrix.

10. A trajectory planning system for sensing blind spots when a vehicle turns at a non-signal-controlled intersection, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the trajectory planning method for perceiving a blind spot when a vehicle turns at a non-signal-controlled intersection as described in any one of claims 1 to 9 are implemented.

Images