CN115840454B

CN115840454B - Multi-vehicle track collaborative planning method and device for unstructured road conflict area

Info

Publication number: CN115840454B
Application number: CN202310134640.0A
Authority: CN
Inventors: 徐彪; 杨泽宇; 王冠; 秦晓辉; 秦兆博; 谢国涛; 王晓伟
Original assignee: Jiangsu Jicui Qinglian Intelligent Control Technology Co ltd
Current assignee: Jiangsu Jicui Qinglian Intelligent Control Technology Co ltd
Priority date: 2023-02-20
Filing date: 2023-02-20
Publication date: 2023-05-12
Anticipated expiration: 2043-02-20
Also published as: CN115840454A

Abstract

The invention discloses a multi-vehicle track collaborative planning method and device for unstructured road conflict areas, wherein the method comprises the following steps: step 1, determining the priority order of vehicles entering an unstructured road conflict area; step 2, planning the track of the bicycle according to the determined priority order to obtain a primary track; step 3, optimizing and smoothing the primary track output in the step 2 to obtain a final track; the step 2 comprises the following steps: step 21, combining a vehicle kinematic model to expand nodes; step 22, selecting an optimal node as a next father node according to each developed node; and step 23, fitting the track by taking a father node near the target point as a starting point and the target point as an end point and using an end point pose fitting mode, outputting a primary track if the fitting is successful, otherwise, continuing to expand the next father node in the step 22. The invention relates to the technical field of automatic driving, and the invention is efficient and safe and can avoid parking waiting to a great extent.

Description

Multi-vehicle track collaborative planning method and device for unstructured road conflict area

Technical Field

The invention relates to the technical field of automatic driving, in particular to a multi-vehicle track collaborative planning method and device for unstructured road conflict areas.

Background

The multi-vehicle track collaborative planning is a key technology for improving road traffic efficiency and ensuring driving safety in a conflict area. The technology takes the vehicle passing time, the planning success rate and the like as optimization targets, and plans the running track of each vehicle without collision and meeting the kinematic constraint of the vehicle.

At present, a great deal of researches on multi-vehicle track collaborative planning of structured road conflict areas such as ramp merging positions, crossroads, three-way intersections and the like are carried out, for example: planning methods based on priority, response, numerical optimization and speed coordination form a mature traffic strategy. For the irregular characteristics of the vehicle running track in the unstructured road conflict area, the priority-based planning method is difficult to design a priority passing rule meeting the complex running environment. In view of the fact that the number of vehicles increases due to the fact that the number of the vehicles in the unstructured road conflict area is too large, collision prevention constraint of each vehicle increases exponentially, and a planning method based on numerical optimization cannot be successfully solved. And a planning method based on reaction only can ensure local collision prevention, local conflict points are adjusted, and new conflict points can appear at other track intersections. Therefore, the current mature multi-vehicle track collaborative planning method for the structured road conflict area cannot be directly applied to unstructured roads.

Therefore, the multi-vehicle track collaborative planning method for the unstructured road conflict area begins to appear gradually, and at present, three main methods are as follows:

one is to use RRT algorithm to plan the track of multiple vehicles simultaneously in the range of time domain, and finally use Du Binsi curve to expand the nodes. In the process of expanding by using RRT, a plurality of invalid nodes can appear, the invalid nodes can influence each other, when the map area is small or the number of vehicles is large, a lot of calculation force can be wasted when the invalid nodes are processed, the generated track does not meet the kinematic constraint of the vehicles, larger errors can be generated during actual tracking, and collision between the tracks of the vehicles can not be guaranteed.

The other is to design a heuristic function, dynamically determine the priority order of the vehicles according to the number of homotopies of the vehicles so as to reduce the occurrence probability of deadlock, and then call a three-dimensional A-algorithm to search out the tracks of each vehicle in time and space. However, when the dynamic priority ranking is performed, the priorities of the vehicles are frequently changed, and the unstable decision finally leads to frequent switching of the speeds of the vehicles, so that a large amount of energy is wasted, and the energy-saving requirement is not met. And the track speed planned by the three-dimensional A-algorithm is not smooth, and the whole planning algorithm does not consider the kinematic constraint of the vehicle.

Yet another method is a fail-safe hybrid a, first a search is made for an initial path, then a circular risk area is designed to scale collision avoidance constraints, and an intermediate optimization problem is constructed to reduce computational dimensions, and an optimal solution that satisfies the full constraint is obtained through continuous iterations. According to the method, the intermediate optimal control problem is set, the iteration mode is adopted for solving, so that the solving success rate of the optimal control problem can be increased to a certain extent, but the faster solving speed is difficult to ensure. Moreover, as the number of vehicles increases, the collision avoidance constraint dimension of the optimal control problem increases exponentially, and often cannot be solved successfully.

Disclosure of Invention

The invention aims to provide an efficient and safe multi-vehicle track collaborative planning method and device for an unstructured road conflict area, which can avoid parking waiting to a great extent.

In order to achieve the above object, the present invention provides a multi-vehicle track collaborative planning method for unstructured road conflict areas, which includes:

step 1, determining the priority order of vehicles entering an unstructured road conflict area;

step 2, planning the track of the bicycle according to the priority order determined in the step 1, and obtaining a primary track;

Step 3, optimizing and smoothing the primary track output in the step 2 to obtain a smooth continuous trackable final track;

the step 2 specifically includes:

step 21, combining a vehicle kinematic model to expand nodes;

step 22, selecting an optimal node as a next father node according to each developed node;

step 23, using a father node near the target point as a starting point, using a mode of fitting the pose of the target point as an end point to fit the track, if the fitting is successful, outputting a primary track, otherwise, continuing to expand the next father node in the step 22;

step 22 specifically includes:

step 221, initializing open set and close set nodes, wherein the initialized open set is used for storing all nodes developed in the step 21, and the initialized close set is an empty set;

step 222, using the node with the minimum cost value f (n) of the open set calculated by the following formula (3) as a parent node, and adding the parent node into the closed set;

f(n)＝g(n)+h(n) (3)

wherein n represents the index of the open centralized node, h (n) is the estimated distance cost from the target point, and g (n) is the accumulated cost from the starting point;

step 223, judging whether the parent node obtained in step 222 is near the target point, if so, proceeding to step 23, otherwise proceeding to step 224;

Step 224, starting from the parent node, expanding a plurality of child nodes again, firstly judging whether each child node can pass through collision detection, if yes, continuing to judge whether the child node passing through collision detection is in an open set, if yes, further judging whether the accumulated cost value g (n) of the expansion is smaller than the value of g (n) recorded in the open set, if yes, updating the accumulated cost g (n) of the child node passing through collision detection and the parent node, and finally, putting the child node which is judged not to be in the closed set and the open set into the open set, and jumping to step 222.

Further, the step 1 specifically includes:

step 11, presetting a control area according to the area size of an unstructured road conflict area;

step 12, initializing a counting variable p=1, calculating the moment when each vehicle arrives at the control area, and recording the track point moment t when the first vehicle arrives at the control area ₀ ；

Step 13, calculating the stop time t of the p-th batch ₀ +p.DELTA.T, find arrival time T _p Satisfy t ₀ +(p-1)·ΔT＜t _p ＜t ₀ Vehicles with +p.DELTA.T conditions are placed into the p-th batch of vehicle collection;

step 14, in the p batch of vehicle sets, the vehicles are arranged in descending order according to the weight level of the vehicles, and for vehicles with the same weight level, the vehicles are arranged in ascending order according to the moment of entering a control area;

Step 15, judging whether vehicles are not distributed in the batch set, if yes, making p=p+1, jumping to step 14, otherwise entering step 16;

and step 16, sequentially sequencing the vehicles from the 1 st batch to the last batch, and outputting the priority order of the vehicles.

Further, step 21 is: according to m different front wheel angles, n different speeds and 1 parking waiting node, expanding to obtain m.n+1 nodes in total according to one of the following modes;

the first way is: the same speed level is expanded according to m front wheel steering angle layers;

the second way is: the same front wheel steering angle is expanded in layers according to n speed grades.

Further, the method for expanding according to the m front wheel steering angle layers according to the same speed level to obtain m.n+1 nodes comprises the following steps:

substep 211 of taking one of n different speed levels as the current speed v of the vehicle _current Calculating a search expansion step s and a vehicle body yaw angle variation in the expansion step according to the following formula (1)

In the formula (1), deltaT is time resolution, l represents the wheelbase of front and rear wheel axles, and alpha represents the front wheel rotation angle of the vehicle;

sub-step 212, according to the change of the yaw angle of the vehicle body in the step s

Expanding through a vehicle kinematics constraint formula (2) to obtain nodes;

In the formula (2), θ _k+1 、(x _k+1 ,y _k+1 )、t _k+1 Respectively are provided withRepresenting heading angle of vehicle at the (k+1) th node, position coordinates of center point, time and theta _k 、(x _k ,y _k )、t _k Respectively representing the course angle of the vehicle at the developed kth node, the position coordinates of the center point and the moment.

Further, the accumulated cost g (n) in step 222 is calculated by the following equation (4):

in the formula (4), the amino acid sequence of the compound,

respectively representing the vehicle body yaw angle of the vehicle at the (n+1) th node and the (n) th node, v _n+1 、v _n Respectively representing the speeds of the vehicle at the (n+1) th node and the (n) th node, w _step 、w _θchange 、w _vchange 、w _stop Respectively a step length punishment coefficient, an angle switching punishment coefficient, a speed switching punishment coefficient and a parking punishment coefficient, P _stop Is a parking penalty value.

Further, step 23 specifically includes:

step 231 of setting parent node near the target point obtained in step 22 as the start point P _i The target point is the termination point P _f ；

Step 232, according to Du Binsi curve fitting mode, starting point P _i Toward termination point P _f Fitting was performed.

Step 233, detecting whether the path of the fitted Du Binsi curve collides with the static obstacle, if it is determined that the path does not collide with the static obstacle, the vehicle speed v is determined to be the current speed v of the vehicle _current A plurality of paths of Du Binsi curves with various speeds endowed with fitting are sampled up and down to obtain a plurality of fitting tracks, each path point moment of the plurality of fitting tracks is calculated according to the expansion moment of the father node near the target point obtained in the step 22, and if the path points are between the expansion moment of the father node and the moment of the fitting track end point, the path points are judged not to be in high priority with other paths If the level vehicle collides, a searching track is obtained by backtracking from a father node near the target point obtained in the step 22, the searching track and the fitting track are spliced to obtain a primary track, and if the sampled tracks do not meet the collision avoidance requirement of the dynamic and static obstacle, the step 22 is returned to continue expanding; wherein, the high-priority vehicles refer to all vehicles with higher priority than the own vehicle.

Further, the step 3 specifically includes:

step 31, building an ST diagram of the high-priority vehicle occupying the vehicle;

step 32, setting a cost function described by the following formula and establishing the following constraint:

wherein F is the cost function, s _i Is the distance, s, in the frenet coordinate system of the ith optimization point _i ' is s _i S is the first derivative of _i "is s _i S is the second derivative of _i+1 "distance s in the frenet coordinate system which is the (i+1) th optimization point _i+1 First derivative of (1), first term

To optimize the integral of the post-speed and raw speed difference, the second term

The third term is +.>

Integral of jerk, W _{cos_ref} ，W _{cos_dds} ，W _{cos_ddds} Penalty coefficients for the three indices, respectively;

first constraint, setting a speed constraint: s is more than or equal to 0 _i '≤V _max ，V _max Is the maximum speed limit of the current road;

a second constraint, establishing a curvature constraint:

Wherein kappa is the curvature of the locus point, a _ymax Is the maximum lateral acceleration of the current road;

third constraint, establish reverse constraint: s is(s) _i+1 ≥s _i ；

Fourth constraint, establishing acceleration constraint: a, a _min ≤s _i ”≤a _max ，a _min 、a _max Respectively refers to the minimum acceleration and the maximum acceleration of the smooth running of the vehicle;

fifth constraint, establishing collision avoidance constraint:

t ₁ ≤t≤t ₂ when 0 < s _i ＜s _obs1 ；

t ₃ ≤t≤t ₄ When in use; s is more than 0 and less than _i ＜s _obs2 ；

Wherein t is ₁ 、t ₂ The intrusion time and the departure time of the first vehicle are respectively t ₃ 、t ₄ The intrusion time and the departure time s of the second vehicle are respectively _obs1 ，s _obs2 Broadly referred to as dynamic obstacle 1 and dynamic obstacle 2, respectively;

sixth constraint, establishing two-point edge value constraint:

s ₀ ＝0，s ₀ '＝v _start ，s ₀ ”＝a _start ；

s _T ＝s _end ，s _T '＝v _end ，s _T ”＝0；

wherein s is ₀ Is the distance value in the starting point frenet coordinate system, v _start Point to the starting point speed, a _start Acceleration of finger start point s _T Refers to the distance value, s, in the target point frenet coordinate system _end Refers to the distance value, v, under the target point frenet coordinate system _end The speed of the target point;

seventh constraint on s _i+1 ,s _i+1 ' Taylor expansion of finite term, ignoring infinitesimal terms:

/>

solving to obtain s according to the cost function and constraint condition _i ，s _i '，s _i And (3) combining the coordinate values of the nodes obtained in the step 21 to form a final track with speed and acceleration information.

The invention also provides a multi-vehicle track collaborative planning device of the unstructured road conflict area, which comprises the following steps:

A vehicle priority determining unit for determining a priority order of vehicles entering the unstructured road conflict area;

the primary track acquisition unit is used for planning the bicycle track according to the priority order to acquire a primary track;

the final track acquisition unit is used for optimizing the smooth primary track to obtain a smooth continuous trackable final track;

the primary track acquisition unit specifically comprises:

the node expansion subunit is used for combining the vehicle kinematic model to expand the nodes;

the parent node selecting subunit is used for selecting an optimal node as a next parent node according to each developed node;

the final point pose fitting subunit is used for fitting the track by taking a father node near the target point as a starting point, the target point as an end point and using a mode of fitting the pose of the end point, if the fitting is successful, outputting a primary track, otherwise, selecting the subunit from the father node to continue expanding the next father node;

the parent node selecting subunit specifically includes:

the initialization module is used for initializing nodes of an open set and a close set, the initialized open set is used for storing all nodes developed in the step 21, and the initialized close set is an empty set;

A cost value calculation module for taking a node with the minimum cost value f (n) of the open set calculated by the following formula (3) as a parent node and adding the node into the closed set;

f(n)＝g(n)+h(n) (3)

the parent node screening module is used for judging whether the parent node obtained by the cost value calculation module is near the target point, and if so, the terminal pose fitting subunit fits the track; otherwise, starting from the father node, expanding a plurality of child nodes again, judging whether each child node can pass through collision detection, if yes, continuing to judge whether the child node passing through collision detection is in an open set, if yes, further judging whether the accumulated cost value g (n) of the expansion is smaller than the value of g (n) recorded in the open set, if yes, updating the accumulated cost g (n) of the child node passing through collision detection and the father node, and finally, putting the child node judged not to be in the closed set and the open set into the open set, taking the node with the minimum cost value f (n) of the open set calculated by using the formula (3) as the father node by a cost value calculation module, and adding the node into the closed set.

Further, the node expansion subunit is: according to m different front wheel angles, n different speeds and 1 parking waiting node, expanding to obtain m.n+1 nodes in total according to one of the following modes;

the second way is: the same front wheel steering angle is expanded in layers according to n speed grades;

the method for obtaining the total m.n+1 nodes comprises the following steps of:

substep 211 of taking one of n different speed levels as the current speed v of the vehicle _current Calculating a search expansion step s and a vehicle body in the expansion step according to the following formula (1)Yaw angle variation

/>

Expanding through a vehicle kinematics constraint formula (2) to obtain nodes;

in the formula (2), θ _k+1 、(x _k+1 ,y _k+1 )、t _k+1 Respectively representing the heading angle of the vehicle at the developed (k+1) th node, the position coordinates of the center point and the moment theta _k 、(x _k ,y _k )、t _k Respectively representing the course angle of the vehicle at the developed kth node, the position coordinates of the center point and the moment, R _r Indicating the rear wheel steering angle radius.

Further, the terminal pose fitting subunit specifically includes:

du Binsi curve fitting module for setting parent node near the target point obtained in the parent node selection subunit as the start point P _i The target point is the termination point P _f From the starting point P in a Du Binsi curve fitting manner _i Toward termination point P _f Fitting is carried out;

an initial trajectory output module for detecting whether the path of the fitted Du Binsi curve collides with the static obstacle, and if it is determined that the path does not collide with the static obstacle, determining the current speed v of the vehicle _current Up and downSampling paths of Du Binsi curves with various speeds endowed with fitting to obtain a plurality of fitting tracks, calculating each path point moment of the plurality of fitting tracks according to the expansion moment of a father node near a target point obtained in a father node selecting subunit, if the moment of expansion of the father node and the moment of a fitting track end point are judged not to collide with other vehicles with high priority, starting backtracking from the father node near the target point obtained in the father node selecting subunit to obtain a searching track, splicing the searching track and the fitting track to obtain a primary track, and if all the sampled tracks do not meet the collision avoidance requirement of a dynamic obstacle, continuing expansion by the father node selecting subunit; wherein, the high-priority vehicles refer to all vehicles with higher priority than the own vehicle.

Due to the adoption of the technical scheme, the invention has the following advantages:

1. the priority allocation principle of the unstructured road conflict area is set, so that the traffic efficiency of the intersection of the unstructured road conflict area is improved;

2. the invention provides a mixed A-scale algorithm for expanding the time dimension, which increases the space for adjusting the track conflict points, reduces the possibility of deadlock, adopts speed coordination and multi-vehicle collision prevention strategies, and ensures the optimality and safety of the planned track.

3. The invention also carries out smoothing treatment on the speed curve, thereby improving the running smoothness of the vehicle and the trackability of the track.

Drawings

Fig. 1 is a schematic view of road conditions under which 3 vehicles respectively enter from different intersections in an embodiment of the present invention.

Fig. 2 is a schematic diagram of a framework for collaborative planning of multiple vehicle trajectories in an unstructured road conflict area according to an embodiment of the present invention.

Fig. 3 is a schematic diagram of an unstructured road conflict area configured as a circular control area in an embodiment of the present invention.

Fig. 4 is a bicycle model.

Fig. 5 is a schematic diagram of a node in an embodiment of the present invention.

Fig. 6 is a flowchart of step 22 and step 23 in an embodiment of the present invention.

Fig. 7 is a graph showing a Du Binsi curve combination in the embodiment of the present invention.

Fig. 8 is a schematic view of a virtual circle of vehicle collision avoidance in an embodiment of the present invention.

Fig. 9 is a schematic diagram of an intrusion application scenario of a high-priority vehicle according to an embodiment of the present invention.

Fig. 10 is a schematic diagram of an ST view of an invaded vehicle in an embodiment of the present invention.

Detailed Description

The present invention will be described in detail with reference to the accompanying drawings and examples.

Unstructured roads refer to roads (such as rural roads, mines, ports, parking lots, parks, etc.) that have no obvious road boundaries and lane lines. The unstructured road collision area is an area where two or more vehicles travel paths on an unstructured road, which have a space collision such as intersection and overlap, and the vehicles may collide due to unreasonable speed arrangement.

As shown in fig. 1, there are shown unstructured road conflict areas with 5 intersections of different directions, and vehicles a, b and c respectively from different intersections, to be driven into. The broken line extending from the three vehicle traveling directions in the figure means the traveling locus of the vehicle, which extends to the target exit road junction. Vehicle b and vehicle c are each at t ₁ The respective moments arrive at p ₁ Position and p ₃ Position at t ₂ Respectively reach p at the moment ₂ Position and p ₄ Position, at the same time, vehicle a is exactly at t ₁ From time to t ₂ Arrival at p in the time period of the time instant _a Near the location, then at t ₁ From time to t ₂ During the time period of the moment, collision can occur among the three vehicles. At this time, at least the trajectories of two vehicles are adjusted to avoid collision. Then, the process of adjusting the original collision track of the vehicle until each vehicle is not collided is called multi-vehicle track collaborative planning.

As shown in fig. 2, the multi-vehicle track collaborative planning is completed by a track planning module in the figure. Knot(s)Referring to fig. 2 and 6, before implementing the multi-vehicle trajectory collaborative planning, the trajectory planning module obtains the overall target of the entire driving task of the vehicle from the task planning module, such as obtaining the starting point, the target point, the vehicle parameters, the map boundary information, and the initial time t of the entire driving task as shown in fig. 6 ₀ . The trajectory planning module also obtains the vehicle position, i.e. the coordinates of the vehicle in the geodetic coordinate system, from the positioning module. The track planning module also acquires a high-precision map (comprising map boundary information) required by the whole driving task from the map module, and performs rasterization processing on the acquired map to acquire an H value lookup table, so that an abscissa x, an ordinate y, a course angle theta and a moment t of the position of the center of the vehicle (generally referred to as the center of the rear axle of the vehicle) can be acquired.

The process of implementing the multi-vehicle track collaborative planning by the track planning module can be basically summarized as follows: and planning an optimal track according to the obtained overall target, the high-precision map and the vehicle position of the whole driving task, wherein the track information comprises information such as an abscissa x, an ordinate y, a course angle theta, a curvature k, a speed v, a moment t and the like of the position of the vehicle under a geodetic coordinate system. And finally, the track planning module sends the planned track information to the motion control module, and the motion control module controls the executing mechanisms such as an accelerator, a brake and a steering wheel of the vehicle according to the motion track, so that the vehicle runs along the planned track according to the track planning module. The "optimum" here is determined according to the overall objective of the mission planning system, typically the objective of shortest path, or the objective of fastest arrival time, etc.

In the embodiment of the invention, the method for the track planning module to carry out the multi-vehicle track collaborative planning comprises the following steps:

step 1, determining the priority order of vehicles entering an unstructured road conflict area.

As a preferred embodiment of step 1, the priority of each batch of vehicles entering the control area may be first determined by using the planned time period, and then further determined by the weight of the vehicles in the same batch, wherein the vehicles of the same weight class are determined by the time sequence of entering the preset control area. The method for determining the priority order of the vehicles entering the unstructured road conflict area provided by the embodiment specifically comprises the following steps:

Step 11, presetting a control area according to the area size of the unstructured road conflict area, for example: as shown in FIG. 3, the preset control area is set as a circular area with radius R, and the road speed limit v _max 。

Step 12, initializing a counting variable p=1, calculating the moment when each vehicle arrives at the control area, and recording the track point moment t when the first vehicle arrives at the control area ₀ 。

Step 13, calculating the stop time t of the p-th batch ₀ +p.DELTA.T, find arrival time T _p Satisfy t ₀ +(p-1)·ΔT＜t _p ＜t ₀ Vehicles with + p deltat conditions are placed into the p-th batch of vehicle collection. Each batch of vehicles is classified by using a planned time period, the first batch must be prioritized over the second batch, and so on. Wherein the time period is planned

Average traffic speed of preset control zone +.>

Step 14, in the p-th batch of vehicle collection, vehicles are arranged in descending order according to the weight level of the vehicles, and vehicles with the same weight level are arranged in ascending order according to the moment of entering the control area. That is, in the same batch, the vehicles with a large weight are classified according to the weight of the vehicles, the vehicles with the same weight are classified according to the time of entering the intersection control area, and the earlier the entering time, the higher the priority. The present embodiment considers: heavy vehicle braking acceleration will consume more energy, and from the standpoint of fuel economy for multiple vehicles, it is desirable to reduce the speed variation of heavy vehicles.

Step 15, judging whether there is a vehicle which is not allocated to the batch set, if so, making p=p+1, jumping to step 14, otherwise, entering step 16.

When the task starts, each vehicle obtains the original track and only avoids static obstacles in the map, and collision between the vehicle and other vehicles is not considered. The time t of reaching the circular control area can be calculated according to the original track of each vehicle _p The track is re-planned for each vehicle according to the priority order, and the vehicle runs according to the newly planned track after actually reaching the control area, so that the collision area can be quickly and safely passed.

In addition to the above, the priority order of the vehicles entering the unstructured road conflict area may be determined in other manners, for example, the preset control area may be set to other shapes and the like regardless of the weight of the vehicles.

And 2, planning the track of the bicycle according to the priority order determined in the step 1, and obtaining a primary track.

In one embodiment, step 2 may use a conventional hybrid a-algorithm, a random tree algorithm, a trip point algorithm, and a dijkstra algorithm to perform track planning, or may use a hybrid a-algorithm with an extended time dimension, and perform track planning sequentially according to the priority order of each vehicle, so that in each planning step, a vehicle with a low priority needs to avoid a static obstacle and a vehicle with a high priority. The hybrid a-algorithm for extending the time dimension provided in this embodiment specifically includes:

And step 21, combining a vehicle kinematic model to expand the nodes. For example, according to m different front wheel angles, n different speeds and 1 parking waiting node, a total of m.n+1 nodes are obtained through expansion.

In one embodiment, the vehicle kinematic model may select a bicycle model that satisfies the ackerman steering constraint, describing the state of the vehicle in the state space as

In this embodiment, by adding the time dimension t to the state space, the state space of the vehicle is expressed as +.>

Of course, other vehicle kinematic models may be adapted according to the actual scene. As shown in fig. 4, the center represents the center point ++of the vehicle for the bicycle model satisfying the ackerman steering constraint>

For the body yaw angle of the vehicle, α represents the front wheel rotation angle of the vehicle, α _max Represents the maximum value of the front wheel rotation angle of the vehicle, l represents the wheelbase of the front and rear wheel axles, R _f Indicating the radius of the front wheel steering angle, R _r Indicating the rear wheel steering angle radius. Thus, the vehicle can be modeled in a kinematic model by the front wheel steering angle maximum value α _max Restraining the angle range of the node, and calculating the yaw angle variation of the vehicle body according to the vehicle wheelbase l, the searching step s and the front wheel steering angle alpha>

/>

In one embodiment, a method of expanding the obtaining of a total of m·n+1 nodes according to m different front wheel angles, n different speed levels, and 1 parking waiting node may be described with reference to an example shown in fig. 5.

The rectangular box in fig. 5 represents the vehicle, and the solid dots are nodes in the present embodiment, and the positions of the nodes correspond to the positions of the vehicle center points. m=3, meaning that there are 3 different front wheel corners in this example, e.g., left, center, right corner directions. n=3, meaning that there are 3 different speed levels in this example, for example: high speed, medium speed, low speed. Then, according to 3 different front wheel angles and 3 different speed levels, the same front wheel angle shown in the figure can be layered according to n=3 speed levels, n=3 layers, the same speed level is layered according to m=3 front wheel angles, and m=3 layers, so that the number of expandable nodes is m·n, and meanwhile, 1 parking waiting node is provided, and then (m·n+1) =10 nodes in total.

More specifically, the same speed level is expanded according to m front wheel steering angle layers, and the method for obtaining the node comprises the following sub-steps:

In the formula (1), Δt is time resolution, and the acquisition method is as follows: according to space connectivity and vehicle kinematics constraint, the space is discretized into four-dimensional discrete space (x, y, theta, T), and then time resolution deltat, grid resolution deltax deltay and heading angle resolution deltatheta are respectively set on four dimensions of the discrete space so as to control the number of developed nodes and ensure the searching speed.

And (3) expanding through a vehicle kinematics constraint formula (2) to obtain the node. For example, in the following formula (2), the node (x _k+1 ,y _k+1 ,θ _k+1 ,t _k+1 ) Representing a node (x _k ,y _k ,θ _k ,t _k ) Node obtained through delta T expansion under front wheel rotation angle alpha:

in the formula (2), θ _k+1 、(x _k+1 ,y _k+1 )、t _k+1 Respectively representing the heading angle of the vehicle at the developed (k+1) th node, the position coordinates of the center point and the moment theta _k 、(x _k ,y _k )、t _k Respectively representing the course angle of the vehicle at the developed kth node, the position coordinates of the center point and the moment.

Of course, the method of "the same front wheel steering angle is expanded in layers according to n speed classes" may also be implemented by using sub-steps 211 and 212, so that no further description is given.

In the structured road conflict area, no reversing situation exists, and the speed of the vehicle needs to be adjusted to avoid dynamic obstacles. According to the embodiment, the expansion of the reversing node is canceled, and the layering expansion is carried out forwards according to n different speeds under the condition of m different front wheel corners. When the number of vehicles is increased, the collision-free multi-vehicle track can not be found out with high probability only through deceleration, and a parking waiting node for expansion is added, so that the success rate of planning can be increased.

And 22, selecting the optimal node as the next father node according to each node developed in the step 21.

In one embodiment, as shown in fig. 6, step 22 specifically includes:

step 221, initializing open set and close set nodes, wherein the initialized open set is used for storing all nodes developed in step 21, and the initialized close set is an empty set.

Step 222, taking out all nodes from the open set, calculating the cost value f (n) of all nodes by using the following formula (3), taking the node with the minimum cost value f (n) as a father node, and adding the father node into the closed set.

f(n)＝g(n)+h(n) (3)

Wherein n represents the index of the open centralized node, f (n) is a cost function, and the cost function f (n) consists of estimated distance cost h (n) from a target point and accumulated cost g (n) from a starting point, and is used for guiding the node to search towards a target direction, avoiding unnecessary node expansion and improving the path searching efficiency; h (n) is the estimated distance cost of each grid from the target point, the estimated distance cost h (n) of each node from the target point can be obtained by pre-calculating the target point by using the Dijkstra algorithm as the initial position and by looking up a table in the path searching process in the later stage; g (n) is the accumulated cost from the starting point.

In one embodiment, the accumulated cost g (n) includes an angle cost, an accumulated distance cost, a parking cost, and a speed cost, wherein: the parking cost is negative excitation additionally arranged on the parking waiting node, the speed cost is calculated according to the speed difference between the father node and the current expansion node, and is used for punishing excessive speed change as shown in the following formula (4), and the speed cost is calculated in real time in the node expansion process through the following formula:

in the formula (4), the amino acid sequence of the compound,

respectively representing the vehicle body yaw angle of the vehicle at the (n+1) th node and the (n) th node, v _n+1 、v _n Respectively representing the speeds of the vehicle at the (n+1) th node and the (n) th node, w _step 、w _θchange 、w _vchange 、w _stop Respectively a step length punishment coefficient, an angle switching punishment coefficient, a speed switching punishment coefficient and a parking punishment coefficient, P _stop For the parking punishment value, the numerical values of the coefficients can be adaptively adjusted to adapt to different planning targets, the values are obtained according to actual planning tasks in a debugging way, and the numerical values follow the following debugging principle: step size penalty w _step The larger the value setting, the more the node tends to expand to a distance; angle switching penalty w _θchange The larger the numerical value is set, the easier the straight line is planned; speed switching penalty w _vchange The greater the numerical setting, the more stable the speed of the vehicle; parking penalty w _stop The larger the numerical value is, the less parking state is easy to occur, but the calculation speed is influenced by the fact that the parameters are too large.

Step 223, determining whether the parent node obtained in step 222 is in the vicinity of the target point, if so, proceeding to step 23, otherwise proceeding to step 224. Here, "nearby" may be understood as that the distance between two points is smaller than a preset value (e.g., 30 m), and the parent node is considered to be near the target point.

Step 224, starting from the parent node determined in step 222, expanding a plurality of child nodes again, firstly judging whether each child node can pass the collision detection, if yes, namely, passing the collision detection, continuously judging whether the child node passing the collision detection is in an open set, if yes, namely, in the open set, further judging whether the expanded accumulated cost value g (n) is smaller than the value of g (n) recorded in the open set, if yes, updating the accumulated cost g (n) of the child node passing the collision detection and the parent node, otherwise, not processing. Finally, the child node determined not to be in the closed set and the open set is placed in the open set, and step 222 is skipped.

When the parent node is updated, the parent node only records which node he expands, and the parent node is directly changed in the storage structure.

Step 23, referring to fig. 6, the track is fitted by using the parent node near the target point as a starting point and the target point as an end point and using the fitting mode of the end point pose, if the fitting is successful, the primary track is output, otherwise, the step 22 is skipped to continue expanding the next parent node.

In one embodiment, a Du Binsi (Dubins) curve is used in conjunction with a speed coordination strategy to impart speed to each waypoint of the path of the fitted trajectory so that collisions with dynamic obstacles can be avoided. As shown in fig. 7, S represents a straight line, L represents a circle in the left-hand direction, R represents a circle in the right-hand direction, and step 23 specifically includes:

step 231 of setting parent node near the target point obtained in step 22 as the start point P _i The target point is the termination point P _f 。

Step 232, according to Du Binsi curve fitting mode, starting point P _i Toward termination point P _f The principle of fitting, du Binsi curve fitting, is as follows:

can be according to the starting point P _i End point P _f And the middle point of the introduced tangent point 1 and the tangent point 2 divides the path of the fitting track into three sections, which are respectively: from the starting point P _i A first line segment between the tangent point 1, a second line segment between the tangent point 1 and the tangent point 2, and a second line segment between the tangent point 2 and the end point P _f Between which are locatedIs a third line segment of (c). The three line segments are of the type of one of the dubin curve sets d= { LSL, RSR, RSL, LSR, RLR, LRL }, where L represents a circular arc segment rotating counterclockwise, S represents a straight line segment, and R represents a circular arc segment rotating clockwise.

Step 233, detecting whether the path of the fitted Du Binsi curve collides with the static obstacle, if it is determined that the path does not collide with the static obstacle, the current speed v of the vehicle selected when the node is expanded in the substep 211 is determined _current And (3) up-down sampling paths of the Du Binsi curves with various speeds endowed with fitting to obtain a plurality of fitting tracks, calculating each path point moment of the plurality of fitting tracks according to the expansion moment of the father node near the target point obtained in the step (22), if the collision with other high-priority vehicles is not judged between the expansion moment of the father node and the moment of the finishing point of the fitting track, starting backtracking from the father node near the target point obtained in the step (22) to obtain a searching track, splicing the searching track and the fitting track to obtain a primary track, and returning to the step (22) to continue expansion if the sampled plurality of tracks do not meet the collision avoidance requirements of the dynamic and static barriers. Wherein. A high priority vehicle refers to all vehicles having a higher priority than the host vehicle.

And 3, optimizing and smoothing the primary track output in the step 2 to obtain a smooth continuous trackable final track.

Step 31, an ST (English is called distance time; chinese is called distance time) diagram is established.

As shown in fig. 8, the body of the vehicle is wrapped with a virtual circle, which is required to satisfy: 1. non-overlapping; 2. the collision-free track between vehicles can be ensured; 3. having the same collision avoidance radius R for vehicles of the same size _{col_i} 。

Fig. 9 provides a schematic view of an intrusion application scenario of the intrusion type of a high-priority vehicle, showing three main intrusion forms of the high-priority vehicle, the primary track of the vehicle a is denoted La, and the vehicles b, c and d are all high-priority vehicles, wherein: the primary track of the vehicle b is denoted as Lb, the primary track of the vehicle c is denoted as Lc, the primary track of the vehicle d is denoted as Ld, and these 4 vehicles are of the same size.

If the four vehicles all travel along their respective primary trajectories, then there is the result that: the vehicle b invades the primary track La of the vehicle a transversely, the vehicle c invades the primary track La of the vehicle a in the same direction, and the vehicle d invades the primary track La of the vehicle a reversely.

If the primary track La of the vehicle a is shifted to the left and right by the respective distances R as indicated by the double-headed arrow in FIG. 9 _{col_1} +R _{col_i} And obtaining collision risk area boundaries Lal and Laf. Meanwhile, the collision risk areas of the vehicle a and other vehicles are overlapped, and are all areas in the middle of the collision area risk boundary in fig. 9, the point that the center of the virtual circle of the other vehicles reaches the area in the middle of the collision area risk boundary is an intrusion point, and the point that the center of the virtual circle of the other vehicles leaves the area is a driving-out point. P (P) _{2_in} Is the intrusion point P of the vehicle b _{2_out} For the driving-off point, P, of the vehicle b _{3_in} Is the intrusion point P of the vehicle c _{4_in} As the intrusion point of the vehicle d, since the vehicle c and the vehicle d are longitudinally intruded, the driving-off point is the target point and the starting point of the vehicle a, respectively.

According to the intrusion situation of each vehicle, an ST diagram of the vehicle a occupied by the high-priority vehicle is produced. As shown in fig. 10, the ordinate Send of the ordinate axis S represents the distance of the trajectory target point, the abscissa t represents the time, the origin O represents the initial time 0S, the vehicle is at the start position, P represents the intrusion or departure point, and accordingly, the intrusion point time is represented as a subscript in and the departure point time is represented as a subscript out. Projecting the speed of the high priority vehicle onto the trajectory of vehicle a results in an initial intrusion profile that is translated up and down by one R each _{col_1} +R _{col_i} And (3) obtaining an ST graph of each high-priority vehicle occupying the vehicle a, namely, a quadrilateral-like shape presented in the graph. For example, in FIG. 10, L _{p2_u} And L is equal to _{p2_l} Respectively represent upward translation R of vehicle b _{col_1} +R _{col_i} Intrusion profile of distance of (2) downward translation R _{col_1} +R _{col_i} Intrusion profile of distance of (d) intrusion point P _{2_in} And travel-out point P _{2_out} The quadrilateral of the vehicle is that the vehicle b occupiesAccording to the ST diagram of the vehicle a. L (L) _{p3_u} And L is equal to _{p3_l} Respectively represent upward translation R of vehicle c _{col_1} +R _{col_i} Intrusion profile of distance of (2) downward translation R _{col_1} +R _{col_i} Intrusion profile of distance of (d) intrusion point P _{3_in} And travel-out point P _{3_out} The quadrilateral in which the vehicle c occupies the ST view of the vehicle a. L (L) _{p4_u} And L is equal to _{p4_l} Respectively represent upward translation R of vehicle d _{col_1} +R _{col_i} Intrusion profile of distance of (2) downward translation R _{col_1} +R _{col_i} Intrusion profile of distance of (d) intrusion point P _{4_in} And travel-out point P _{4_out} The quadrilateral is the ST view of vehicle d occupying vehicle a.

In view of this, the trajectory of the vehicle a can be prevented from collision as long as it does not interfere with the above 3 quadrangles.

The scenario in the example of fig. 9 is not large, the track length is short, all ST graphs can be made, if the collision area is large (the circular area with the radius of more than 1km is large in the collision area), the track formed by the vehicle can be longer, the speed planning can be carried out by using the whole track to build the ST graph, so that the solving speed is reduced due to the large calculated quantity, at the moment, the method of combining the uniform acceleration planning and the numerical optimization should be adopted, the speed curve among a plurality of collision points is solved first, and other collision-free places can be used for uniform acceleration driving. Assuming that the ST plot trace in fig. 10 is long, first, solve for t _{2_in} To t _{2_out} Time period and t _{3_in} A speed profile of the time period to the target point moment. The vehicle starts from the starting point moment to t _{2_in} T is solved by taking the initial speed at the moment 0 as the initial speed _{2_in} The speed at the moment is the target point speed to perform uniform acceleration and deceleration movement, at t _{2_out} To t _{3_in} At t _{2_out} The speed at the moment is the initial speed, t _{3_in} The speed at the moment is the target point speed and carries out uniform acceleration and deceleration movement. And splicing the two sections of uniform acceleration and deceleration speed curves with the speed curve obtained by numerical solution to obtain the final speed curve of the vehicle.

Step 32, setting a cost function described by the following formula (5), and optimizing the primary track, wherein the optimization needs to meet three indexes: firstly, the speed difference between the optimized track and the primary track is small; secondly, the optimized speed is as high as possible to ensure the rapid passing of the vehicle; third, acceleration is small to ensure smoothness of running.

Wherein F is the cost function, s in quadratic programming _i Is the distance, s, in the frenet coordinate system of the ith optimization point _i ' is s _i S is the first derivative of _i "is s _i S is the second derivative of _i+1 "distance s in the frenet coordinate system which is the (i+1) th optimization point _i+1 First derivative of (1), first term

For the optimization of the integral of the post-speed with the original speed difference, the second term +. >

The third term is the integral of the acceleration

Integral of jerk, W _{cos_ref} ，W _{cos_dds} ，W _{cos_ddds} Penalty coefficients of the three indexes are respectively, the specific numerical values of the three indexes are mutually influenced, the specific value is required to be determined according to a specific planning task, and the setting principle comprises the following steps: w (W) _{cos_ref} The degree of influence on the approach to the original speed is larger, and the calculated speed is closer to the original speed; w (W) _{cos_dds} The speed of the vehicle is influenced, and the larger the numerical value is, the faster the vehicle speed is; w (W) _{cos_ddds} The acceleration of the vehicle is affected, and the larger the numerical value is, the smaller the acceleration is.

Establishing a constraint:

the first constraint, the maximum speed limit is considered in the optimization process, and the speed constraint is set: s is more than or equal to 0 _i '≤V _max ，V _max Is the maximum speed limit of the current road;

the second constraint, when the curvature of the track is too large, the speed of the vehicle cannot be too high, otherwise sideslip occurs, and the curvature constraint is established:

a _ymax =0.2×9.8, where kappa is the curvature of the trace point, a _ymax Is the maximum lateral acceleration of the current road;

third constraint, the intersection traffic does not allow reversing, and reversing constraint is established: s is(s) _i+1 ≥s _i ；

Fourth constraint, ensuring vehicle running smoothness and acceleration constraint: a, a _min ≤s _i ”≤a _max ，a _min 、a _max Respectively, minimum and maximum acceleration of the vehicle during smooth driving, generally a _max ＝2m/s ² ，a _min ＝-4m/s ² ；

Fifth constraint, obtaining collision avoidance constraint of the vehicle according to the ST image, namely at each discrete time point, the track of the vehicle does not interfere with polygons of the ST image occupied by other vehicles, and the collision avoidance constraint is established:

t ₁ ≤t≤t ₂ When 0 < s _i ＜s _obs1 ；

sixth constraint, ensuring consistency of starting point and target point states, and establishing two-point edge value constraint:

s ₀ ＝0，s ₀ '＝v _start ，s ₀ ”＝a _start ；

s _T ＝s _end ，s _T '＝v _end ，s _T ”＝0；

wherein s is ₀ Is that the distance value in the starting point frenet coordinate system is generally equal to 0, v _start Refers to a starting point speed, a, obtained by presetting according to a planning scene _start Refers to the starting point acceleration preset according to the planning scene, s _T Refers to the distance value, s, in the target point frenet coordinate system _end Refers to the distance value, v, under the target point frenet coordinate system _end The speed of a target point preset according to a planning scene;

a seventh constraint, ensuring continuity of speed and acceleration, i.e. third derivative

Δt is the solving resolution of the quadratic programming, and the specific numerical value thereof is determined to be 0.1s according to the actually set equation; fourth derivative and above 0 for s _i+1 ,s _i+1 ' Taylor expansion of finite terms is performed, ignoring infinitesimal terms.

The method is arranged to meet the equality constraint of second derivative continuity:

solving to obtain s according to the cost function and constraint condition _i ，s _i '，s _i And (3) combining the coordinate values of the nodes obtained in the step 21 to form a final track with speed and acceleration information. Storing the smoothed track, and estimating the position of the vehicle at each moment according to the track information, wherein each low-priority vehicle is provided with a plurality of low-priority vehicles And avoiding the high-priority vehicle at each moment, and carrying out collision detection and speed planning according to the stored track information.

The embodiment of the invention also provides a multi-vehicle track collaborative planning device for the unstructured road conflict area, which comprises a vehicle priority determining unit, a primary track acquiring unit and a primary track acquiring unit, wherein:

the vehicle priority determination unit is configured to determine a priority order of vehicles entering the unstructured road conflict area.

The primary track acquisition unit is used for planning the track of the bicycle according to the priority order to acquire a primary track.

The final track acquisition unit is used for optimizing the smooth primary track to obtain a smooth continuous trackable final track.

The primary track acquisition unit specifically comprises a node expansion subunit, a father node selection subunit and an end point pose fitting subunit, wherein:

the node expansion subunit is used for combining the vehicle kinematic model to expand the nodes.

The parent node selecting subunit is configured to select, according to each developed node, an optimal node as a next parent node.

The final point pose fitting subunit is configured to fit the track by using a parent node near the target point as a starting point and the target point as an end point and using an end point pose fitting mode, if the fitting is successful, output a primary track, otherwise, select the child unit from the parent node to continue expanding the next parent node.

The parent node selecting subunit specifically comprises an initialization module, a cost value calculation module and a parent node screening module, wherein:

the initialization module is used for initializing nodes of an open set and a close set, the initialized open set is used for storing all nodes developed in the step 21, and the initialized close set is an empty set.

And a cost value calculation module for taking the node with the smallest cost value f (n) of the open set calculated by the formula (3) as a father node and adding the node into the closed set.

In one embodiment, the node expansion subunit is: according to m different front wheel angles, n different speeds and 1 parking waiting node, expanding to obtain m.n+1 nodes in total according to one of the following modes;

wherein the method for obtaining a total of m·n+1 nodes according to the same speed class and according to m front wheel angular layer expansions comprises the above-mentioned sub-step 211 and sub-step 212.

In one embodiment, the final point pose fitting subunit specifically includes a Du Binsi curve fitting module and an initial trajectory output module, where:

du Binsi the curve fitting module is used for taking the parent node near the target point obtained in the parent node selection subunit as the starting point P _i The target point is the termination point P _f From the starting point P in a Du Binsi curve fitting manner _i Toward termination point P _f Fitting was performed.

The path of the initial trajectory output module for detecting the fitted Du Binsi curve isIf the vehicle is not in collision with the static obstacle, the vehicle is started at the current speed v _current Sampling paths of Du Binsi curves with various speeds up and down to obtain a plurality of fitting tracks, calculating each path point moment of the plurality of fitting tracks according to the expansion moment of a father node near a target point obtained in a father node selecting subunit, if collision with other high-priority vehicles is not judged to occur between the expansion moment of the father node and the moment of a fitting track end point, starting backtracking from the father node near the target point obtained in the father node selecting subunit to obtain a searching track, splicing the searching track and the fitting track to obtain a primary track, and if all the sampled tracks do not meet the collision avoidance requirement of a dynamic obstacle, selecting the child unit by the father node to continue expansion; wherein, the high-priority vehicles refer to all vehicles with higher priority than the own vehicle.

In the above embodiments, the priority allocation rule in the present invention is simply modified, or a path planning algorithm of other graph searches is adopted to replace, the parameters and the optimization indexes of the numerical optimization are adjusted, and the constraint forms are changed to achieve approximately the same effect, but all the changes belong to the concept scope of the present invention.

Finally, it should be pointed out that: the above embodiments are only for illustrating the technical solution of the present invention, and are not limiting. Those of ordinary skill in the art will appreciate that: the technical schemes described in the foregoing embodiments may be modified or some of the technical features may be replaced equivalently; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. The multi-vehicle track collaborative planning method for the unstructured road conflict area is characterized by comprising the following steps of:

the step 2 specifically includes:

step 21, combining a vehicle kinematic model to expand nodes;

step 22 specifically includes:

f(n)＝g(n)+h(n) (3)

step 224, starting from the parent node, expanding a plurality of child nodes again, firstly judging whether each child node can pass through collision detection, if yes, continuing to judge whether the child node passing through collision detection is in an open set, if yes, further judging whether the accumulated cost value g (n) of the expansion is smaller than the value of g (n) recorded in the open set, if yes, updating the accumulated cost g (n) of the child node passing through collision detection and the parent node, and finally, putting the child node which is judged not to be in the closed set and the open set into the open set, and jumping to step 222;

step 21 is: according to m different front wheel angles, n different speeds and 1 parking waiting node, expanding to obtain m.n+1 nodes in total according to one of the following modes;

the method for expanding according to the same speed level and m front wheel steering angle layers and obtaining total m.n+1 nodes comprises the following steps:

In the formula (1), l represents the wheelbase of the front and rear wheel axles, alpha represents the front wheel rotation angle of the vehicle, and delta T is the time resolution;

Expanding through a vehicle kinematics constraint formula (2) to obtain nodes; />

In the formula (2), θ _k+1 、(x _k+1 ,y _k+1 )、t _k+1 Respectively representing the heading angle of the vehicle at the developed (k+1) th node, the position coordinates of the center point and the moment theta _k 、(x _k ,y _k )、t _k Respectively representing the course angle of the vehicle at the developed kth node, the position coordinates of the center point and the moment, R _r Representing the rear wheel steering angle radius;

the accumulated cost g (n) in step 222 is calculated by the following equation (4):

in the formula (4), the amino acid sequence of the compound,

respectively representing the vehicle body yaw angle of the vehicle at the (n+1) th node and the (n) th node, v _n+1 、v _n Respectively representing the speeds of the vehicle at the (n+1) th node and the (n) th node, w _step 、w _θcnange 、w _vchange 、w _stop Respectively a step length punishment coefficient, an angle switching punishment coefficient, a speed switching punishment coefficient and a parking punishment coefficient, P _stop Is a parking penalty value.

2. The method for collaborative planning of multiple vehicle trajectories in an unstructured road conflict area according to claim 1, wherein step 1 specifically comprises:

Step 13, calculating the stop time t of the p-th batch ₀ +p.DELTA.T, find arrival time T _p Satisfy t ₀ +(p-1)·ΔT<t _p <t ₀ Vehicles with +p.DELTA.T conditions are placed into the p-th batch of vehicle collection;

3. The method for collaborative planning of multiple vehicle trajectories in an unstructured road conflict area according to claim 1, wherein step 23 specifically comprises:

Step 232, according to Du Binsi curve fitting mode, starting point P _i Toward termination point P _f Fitting is carried out;

step 233, detecting whether the path of the fitted Du Binsi curve collides with the static obstacle, if it is determined that the path does not collide with the static obstacle, the vehicle speed v is determined to be the current speed v of the vehicle _current The paths of the Du Binsi curves with various speeds endowed with fitting are sampled up and down to obtain a plurality of fitting tracks, each path point moment of the plurality of fitting tracks is calculated according to the expansion moment of the father node near the target point obtained in the step 22, if collision with other vehicles with high priority is not judged between the expansion moment of the father node and the moment of the end point of the fitting track, a search track is obtained by tracing back from the father node near the target point obtained in the step 22, the search track and the fitting track are spliced to obtain a primary track, and if the sampled plurality of tracks do not meet the collision avoidance requirement of the dynamic and static obstacle, the step 22 is returned to continue expansion; wherein, the high-priority vehicles refer to all vehicles with higher priority than the own vehicle.

4. The method for collaborative planning of multiple vehicle trajectories in an unstructured road conflict area of claim 3, wherein step 3 specifically comprises:

wherein F is the cost function, s _i Is the distance, s, in the frenet coordinate system of the ith optimization point _i ' is s _i S is the first derivative of _i "is s _i S is the second derivative of _i+1 "distance s in the frenet coordinate system which is the (i+1) th optimization point _i+1 Second derivative of (1) first term

The third term is +.>

a second constraint, establishing a curvature constraint:

third constraint, establish reverse constraint: s is(s) _i+1 ≥s _i ；

Fourth constraint, establishing acceleration constraint: a, a _min ≤s _i ”≤a _max ，a _min 、a _max Respectively, minimum and maximum of smooth running of vehicleAcceleration;

fifth constraint, establishing collision avoidance constraint:

t ₁ ≤t≤t ₂ When 0 < s _i ＜s _obs1 ；

sixth constraint, establishing two-point edge value constraint:

s ₀ ＝0，s ₀ '＝v _start ，s ₀ ”＝a _start ；

s _T ＝s _end ，s _T '＝v _end ，s _T ”＝0；

/>

5. A multi-vehicle track collaborative planning apparatus for unstructured road conflict areas, comprising:

The primary track acquisition unit specifically comprises:

the parent node selecting subunit specifically includes:

the initialization module is used for initializing the open set and the close set nodes, the initialized open set is used for storing all nodes developed by the node expansion subunit, and the initialized close set is an empty set;

f(n)＝g(n)+h(n) (3)

the parent node screening module is used for judging whether the parent node obtained by the cost value calculation module is near the target point, and if so, the terminal pose fitting subunit fits the track; if so, further judging whether the accumulated cost value g (n) of the expansion is smaller than the value of g (n) recorded in the open set, if so, updating the accumulated cost g (n) of the child nodes passing through the collision detection and the father node, and finally, putting the child nodes which are judged not to be in the closed set and the open set into the open set by a cost value calculation module, and taking the node with the minimum cost value f (n) of the open set calculated by using the formula (3) as the father node;

The node expansion subunit is as follows: according to m different front wheel angles, n different speeds and 1 parking waiting node, expanding to obtain m.n+1 nodes in total according to one of the following modes;

Expanding through a vehicle kinematics constraint formula (2) to obtain nodes;

6. The unstructured road conflict zone multi-vehicle trajectory co-planning device of claim 5, wherein the final point pose fitting subunit specifically comprises:

an initial trajectory output module for detecting whether the path of the fitted Du Binsi curve collides with the static obstacle, and if it is determined that the path does not collide with the static obstacle, determining the current speed v of the vehicle _current Sampling paths of Du Binsi curves with multiple speeds endowed with fitting up and down to obtain multiple fitting tracks, calculating each path point moment of the multiple fitting tracks according to the expansion moment of a father node near a target point obtained in a father node selection subunit, and judging that the path points are not in high priority with other paths if the path points are between the expansion moment of the father node and the moment of the fitting track end pointIf the level vehicle collides, a parent node near a target point obtained from a parent node selection subunit starts backtracking to obtain a search track, the search track and the fitting track are spliced to obtain a primary track, and if all the sampled tracks do not meet the collision prevention requirement of the dynamic and static obstacle, the parent node selection subunit continues expanding; wherein, the high-priority vehicles refer to all vehicles with higher priority than the own vehicle.