CN-121979188-A - Unstructured road static obstacle detouring method, device, medium, equipment and vehicle

Abstract

The application discloses an unstructured road static obstacle detouring method, device, medium, equipment and vehicle, wherein the method comprises the steps of obtaining an unstructured road hard boundary and a static obstacle boundary; the method comprises the steps of determining a central axis of a drivable area surrounded by an unstructured road hard boundary and a static obstacle boundary, obtaining a left-side obstacle-detouring heuristic search path and a right-side obstacle-detouring heuristic search path according to a heuristic search algorithm, the drivable area and the central axis, determining an obstacle detouring direction corresponding to the heuristic search path with the minimum total cost of all nodes as a target obstacle detouring direction, and establishing a vehicle path planning model according to vehicle kinematic constraint, system control quantity constraint and unstructured road hard boundary constraint and penalty terms about unstructured road soft boundary, and determining the optimal solution of the vehicle path planning model as the target planning path. Through the scheme, the safety and the comfort of planning the path can be improved while the humanized decision is improved.

Inventors

• ZHANG JIAXU
• SUN GANG
• Teng ting
• PEI PENG
• WANG TENGFEI

Assignees

• 魔门塔(苏州)科技有限公司

Dates

Publication Date

20260505

Application Date

20241031

Claims (15)

1. 1. A method of static obstacle detouring of an unstructured road, the method comprising: obtaining an unstructured road hard boundary and a static obstacle boundary, wherein the region surrounded by the unstructured road hard boundary comprises the static obstacle boundary; determining a central axis of a drivable area surrounded by the unstructured road hard boundary and the static obstacle boundary; According to a heuristic search algorithm, the drivable area and the central axis, a left-side obstacle-detouring heuristic search path and a right-side obstacle-detouring heuristic search path are obtained, and an obstacle-detouring direction corresponding to the heuristic search path with the minimum total cost of all nodes is determined as a target obstacle-detouring direction; And establishing a vehicle path planning model according to vehicle kinematics constraint, system control quantity constraint, unstructured road hard boundary constraint and penalty items related to unstructured road soft boundaries, and determining an optimal solution of the vehicle path planning model as a target planning path, wherein the unstructured road soft boundaries are determined according to the unstructured road hard boundaries, and the penalty items related to the unstructured road soft boundaries are penalty items set according to the minimum distance from a path planning point to the unstructured road soft boundaries.
2. 2. The method of claim 1, wherein the acquiring a static obstacle boundary comprises: Acquiring a perceived static obstacle point set, wherein the static obstacle point set comprises boundary points of a static obstacle; Sequencing the obstacle points in the static obstacle point set according to the descending order of the abscissa, and obtaining a sequenced static obstacle point set; Acquiring the first two obstacle points from the ordered static obstacle point set, adding the first two obstacle points into a target set, traversing each remaining obstacle point in sequence according to the sequence in the ordered static obstacle point set, and executing target operation on each traversed obstacle point respectively; After the ordered static obstacle point set is traversed in the forward direction, adding the last obstacle point in the ordered static obstacle point set into the current target set, traversing each target obstacle point in sequence from the last-to-last obstacle point in the ordered static obstacle point set to the first obstacle point according to the order from back to front in the ordered static obstacle point set, and executing the target operation for each traversed target obstacle point respectively, wherein when the currently traversed obstacle point is not in the current target set, determining the currently traversed obstacle point as the target obstacle point; After traversing the ordered static obstacle point sets in the reverse direction, obtaining a final target set, and obtaining the static obstacle boundary by sequentially connecting the obstacle points in the final target set; Wherein the target operation comprises: For a currently traversed obstacle point, constructing a first vector and a second vector according to the currently traversed obstacle point and the last two obstacle points in a current target set, wherein the first vector is that the last-last obstacle point in the current target set points to the last obstacle point, and the second vector is that the last-last obstacle point in the current target set points to the currently traversed obstacle point; adding the currently traversed obstacle point to the current target set when a cross product between the first vector and the second vector is greater than 0; And when the cross multiplication result between the first vector and the second vector is smaller than or equal to 0, replacing the last obstacle point in the current target set by using the currently traversed obstacle point.
3. 3. The method of claim 1, wherein the obtaining a left-hand obstacle detouring heuristic search path and a right-hand obstacle detouring heuristic search path in accordance with a heuristic search algorithm, the drivable region and the central axis comprises: taking a vehicle planning starting point as an initial node, starting from the initial node, and expanding the current node in the drivable region by utilizing the heuristic search algorithm to obtain at least one alternative child node of the current node; inserting at least one alternative child node of the current node into a priority queue, wherein nodes in the priority queue are arranged according to the order of node cost from low to high, the cost of each node comprises the sum of a first child cost and a second child cost, the first child cost is the basic driving cost of the node, and the second child cost is the cost determined based on the position deviation between the central axis and the nodes; and popping up the node with the lowest cost from the priority queue as a new current node, and continuing to expand the new current node until the vehicle planning endpoint is reached, so as to obtain a left-side obstacle-detouring heuristic search path and a right-side obstacle-detouring heuristic search path.
4. 4. A method according to claim 3, wherein said inserting at least one candidate child node of the current node into a priority queue comprises: Rasterizing the drivable region to obtain a plurality of grids; determining a target grid of each alternative child node in the at least one alternative child node; When the target grid is empty or a father-son relationship or a brother relationship exists between the alternative child node and the nodes already contained in the target grid, reserving the alternative child node in the target grid, and inserting the alternative child node into the priority queue; When a father-son relationship does not exist between the alternative child node and the node contained in the target grid, and no brother relationship exists, if the first child cost of the alternative child node is smaller than the first child cost of the target grid, reserving the alternative child node in the target grid, and inserting the alternative child node into the priority queue, wherein the first child cost of the target grid is the minimum cost of the first child costs of each node contained in the target grid; And when the alternate child node and the nodes contained in the target grid have no parent-child relationship and no brother relationship, discarding the alternate child node if the first child cost of the alternate child node is greater than or equal to the first child cost of the target grid.
5. 5. The method of claim 3, further comprising establishing a small top heap of the priority queue, splitting the small top heap into a plurality of sub-heaps, and recording a node cost range for each sub-heap; inserting at least one alternative child node of the current node into a priority queue, wherein the method comprises the steps of determining a node cost range to which the cost of the alternative child node belongs, and inserting the alternative child node into a sub-stack corresponding to the determined node cost range, wherein each sub-stack accords with the structure of a small top stack; And popping the node with the lowest cost from the priority queue to serve as a new current node, wherein the node with the lowest cost in the partial heap of the incomplete empty node is determined to be the node with the lowest cost in the priority queue when each partial heap is sequentially traversed from the node cost range to the large until the partial heap of the incomplete empty node is traversed for the first time, and the node with the lowest cost is popped from the priority queue to serve as the new current node.
6. 6. The method of any of claims 3-5, wherein the first sub-cost is calculated from at least one of a shift count cost, a shift point curvature discontinuity cost, a path length cost, an obstacle distance cost, and an S-path cost.
7. 7. An unstructured road static obstacle detouring device, the device comprising: The device comprises an acquisition unit, a control unit and a control unit, wherein the acquisition unit is used for acquiring an unstructured road hard boundary and a static obstacle boundary, and the static obstacle boundary is included in an area surrounded by the unstructured road hard boundary; The central axis determining unit is used for determining the central axis of a drivable area surrounded by the unstructured road hard boundary and the static obstacle boundary; The searching unit is used for obtaining a left-side obstacle-detouring heuristic searching path and a right-side obstacle-detouring heuristic searching path according to a heuristic searching algorithm, the drivable area and the central axis; the direction determining unit is used for determining the obstacle detouring direction corresponding to the heuristic search path with the minimum total cost of all the nodes as a target obstacle detouring direction; The system comprises a building unit, a vehicle path planning module and a control unit, wherein the building unit is used for building a vehicle path planning model according to vehicle kinematics constraint, system control quantity constraint, unstructured road hard boundary constraint and penalty items related to unstructured road soft boundaries, the unstructured road soft boundaries are determined according to the unstructured road hard boundaries, and the penalty items related to the unstructured road soft boundaries are penalty items set according to the minimum distance from a path planning point to the unstructured road soft boundaries; And the path determining unit is used for determining the optimal solution of the vehicle path planning model as a target planning path.
8. 8. The apparatus of claim 7, wherein the acquisition unit comprises: the system comprises an acquisition module, a detection module and a control module, wherein the acquisition module is used for acquiring a perceived static obstacle point set, and the static obstacle point set comprises boundary points of a static obstacle; The sequencing module is used for sequencing the obstacle points in the static obstacle point set according to the descending order of the abscissa, so as to obtain a sequenced static obstacle point set; The adding module is used for acquiring the first two obstacle points from the ordered static obstacle point set and adding the first two obstacle points into a target set; The execution module is used for traversing each rest obstacle point in sequence according to the sequence in the ordered static obstacle point set and executing target operation for each traversed obstacle point respectively; The adding module is further configured to add a last obstacle point in the ordered static obstacle point set to the current target set after the ordered static obstacle point set is traversed in the forward direction; The execution module is further configured to sequentially traverse each target obstacle point from a last-to-first obstacle point in the ordered static obstacle point set according to a sequence from back to front in the ordered static obstacle point set, and execute the target operation for each traversed target obstacle point, where when the currently traversed obstacle point is not in the current target set, the currently traversed obstacle point is determined to be the target obstacle point; The connecting module is used for obtaining a final target set after the ordered static obstacle point sets are traversed reversely, and obtaining the static obstacle boundaries by sequentially connecting the obstacle points in the final target set; Wherein the target operation comprises: For a currently traversed obstacle point, constructing a first vector and a second vector according to the currently traversed obstacle point and the last two obstacle points in a current target set, wherein the first vector is that the last-last obstacle point in the current target set points to the last obstacle point, and the second vector is that the last-last obstacle point in the current target set points to the currently traversed obstacle point; adding the currently traversed obstacle point to the current target set when a cross product between the first vector and the second vector is greater than 0; And when the cross multiplication result between the first vector and the second vector is smaller than or equal to 0, replacing the last obstacle point in the current target set by using the currently traversed obstacle point.
9. 9. The apparatus of claim 7, wherein the search unit comprises: The expansion module is used for taking a vehicle planning starting point as an initial node, starting from the initial node, expanding the current node in the drivable region by utilizing the heuristic search algorithm, and obtaining at least one alternative child node of the current node; The inserting module is used for inserting at least one alternative child node of the current node into a priority queue, wherein the nodes in the priority queue are arranged according to the order of low node cost and the cost of each node comprises the sum of first child cost and second child cost, the first child cost is the basic driving cost of the node, and the second child cost is the cost determined based on the position deviation between the central axis and the nodes; The pop-up module is used for popping up the node with the lowest cost from the priority queue to serve as a new current node; And the inserting module and the ejecting module are further used for continuing to expand the new current node until reaching the vehicle planning end point, and obtaining a left obstacle-detouring heuristic search path and a right obstacle-detouring heuristic search path.
10. 10. The apparatus of claim 9, wherein the insertion module is configured to: Rasterizing the drivable region to obtain a plurality of grids; determining a target grid of each alternative child node in the at least one alternative child node; When the target grid is empty or a father-son relationship or a brother relationship exists between the alternative child node and the nodes already contained in the target grid, reserving the alternative child node in the target grid, and inserting the alternative child node into the priority queue; When a father-son relationship does not exist between the alternative child node and the node contained in the target grid, and no brother relationship exists, if the first child cost of the alternative child node is smaller than the first child cost of the target grid, reserving the alternative child node in the target grid, and inserting the alternative child node into the priority queue, wherein the first child cost of the target grid is the minimum cost of the first child costs of each node contained in the target grid; And when the alternate child node and the nodes contained in the target grid have no parent-child relationship and no brother relationship, discarding the alternate child node if the first child cost of the alternate child node is greater than or equal to the first child cost of the target grid.
11. 11. The apparatus of claim 9, wherein the search unit further comprises: The method comprises the steps of establishing a splitting module, which is used for establishing a small top stack of a priority queue, splitting the small top stack into a plurality of sub-stacks, and recording the node cost range of each sub-stack; The inserting module is configured to determine a node cost range to which the cost of the candidate child node belongs, and insert the candidate child node into a split stack corresponding to the determined node cost range, where each split stack conforms to a structure of a small top stack; And the pop-up module is used for sequentially traversing each sub-heap according to the order of the node cost range from small to large until the node is traversed to the sub-heap of the incompletely empty node for the first time, determining the node with the minimum cost in the sub-heap of the incompletely empty node as the node with the lowest cost in the priority queue, and popping up the node with the lowest cost from the priority queue as a new current node.
12. 12. The apparatus of any one of claims 9-11, wherein the first sub-cost is calculated from at least one of a shift number cost, a shift point curvature discontinuity cost, a path length cost, an obstacle distance cost, and an S-path cost.
13. 13. A computer readable storage medium, on which a computer program is stored, characterized in that the program, when being executed by a processor, implements the method according to any of claims 1-6.
14. 14. An electronic device, the electronic device comprising: One or more processors; The processor is coupled with a storage device for storing one or more programs; The one or more programs, when executed by the one or more processors, cause the electronic device to implement the method of any of claims 1-6.
15. 15. A vehicle comprising the apparatus of any one of claims 7-12 or the electronic device of claim 14.

Description

Unstructured road static obstacle detouring method, device, medium, equipment and vehicle Technical Field The application relates to the technical field of intelligent driving, in particular to an unstructured road static obstacle detouring method, an unstructured road static obstacle detouring device, medium, equipment and a vehicle. Background The unstructured road static obstacle detouring characteristic is the representation of the core capability of an urban road pilot auxiliary system, how to rapidly decide a reasonable static obstacle detouring direction, and then plan a safe and comfortable guiding path based on the static obstacle detouring direction to form a bottleneck of mass production landing of the unstructured road static obstacle detouring characteristic. In the traditional method, the static obstacle detouring direction is determined based on the information such as unstructured road traffic width and the like, and then a spline curve is utilized to plan a guiding path. The guiding path obtained by the method has the problem of poor humanization of the static obstacle detouring direction of decisions, and can not strictly meet constraints such as automobile kinematics, so that comfort and safety are lacking when the path is planned along the unstructured road static obstacle detouring. Disclosure of Invention The application provides an unstructured road static obstacle detouring method, an unstructured road static obstacle detouring device, medium, equipment and a vehicle, which can solve the technical problems that the traditional method for deciding the static obstacle detouring direction based on the information such as the traffic width of an unstructured road and the like and then planning a guiding path by using a spline curve is bad in decision obstacle detouring direction humanization and the planned path lacks comfort and safety. The specific technical scheme is as follows: In a first aspect, an embodiment of the present application provides an unstructured road static obstacle detouring method, where the method includes: obtaining an unstructured road hard boundary and a static obstacle boundary, wherein the region surrounded by the unstructured road hard boundary comprises the static obstacle boundary; determining a central axis of a drivable area surrounded by the unstructured road hard boundary and the static obstacle boundary; According to a heuristic search algorithm, the drivable area and the central axis, a left-side obstacle-detouring heuristic search path and a right-side obstacle-detouring heuristic search path are obtained, and an obstacle-detouring direction corresponding to the heuristic search path with the minimum total cost of all nodes is determined as a target obstacle-detouring direction; And establishing a vehicle path planning model according to vehicle kinematics constraint, system control quantity constraint, unstructured road hard boundary constraint and penalty items related to unstructured road soft boundaries, and determining an optimal solution of the vehicle path planning model as a target planning path, wherein the unstructured road soft boundaries are determined according to the unstructured road hard boundaries, and the penalty items related to the unstructured road soft boundaries are penalty items set according to the minimum distance from a path planning point to the unstructured road soft boundaries. According to the scheme, the embodiment of the application can utilize the drivable area surrounded by the hard boundary of the unstructured road and the static obstacle boundary and the central axis thereof to obtain the heuristic search path of the left obstacle detouring and the heuristic search path of the right obstacle detouring, and the obstacle detouring direction corresponding to the heuristic search path with the minimum total cost of all nodes is determined as the target obstacle detouring direction, so that the target obstacle detouring direction can better consider humanized factors, and then the path planning is carried out through a vehicle path planning model established by vehicle kinematic constraint, system control quantity constraint, unstructured road hard boundary constraint and punishment terms related to the unstructured road soft boundary, so that the planned guiding path gives consideration to factors related to comfort and safety such as kinematic constraint and obstacle safety constraint and the like, and the comfort and safety of the subsequent vehicle obstacle detouring are improved. In one possible embodiment, the acquiring the static obstacle boundary includes: Acquiring a perceived static obstacle point set, wherein the static obstacle point set comprises boundary points of a static obstacle; Sequencing the obstacle points in the static obstacle point set according to the descending order of the abscissa, and obtaining a sequenced static obstacle point set; Acquiring the first two obstacle points from the ordered s