A method and system for unmanned vehicle path planning
The method and system for unmanned vehicle path planning using double-position circles and heuristic search address safety and feasibility issues by generating a circular path that considers vehicle geometry and obstacle positions, enhancing safety and reliability.
Patent Information
- Application Number
- CN202210396695.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-04-15
- Publication Date
- 2024-12-03
- Estimated Expiration
- 2042-04-15
AI Technical Summary
Existing path planning methods for unmanned vehicles fail to consider vehicle heading and kinematics, leading to safety issues and reduced feasible space when obstacles are close, especially for vehicles with larger length and width, such as car-like robots in complex environments.
A method and system for unmanned vehicle path planning that uses double-position circles to cover the vehicle, involving 4 open and 4 close lists, with a backtracking and heuristic search to determine a circular path, ensuring safety and feasibility by considering vehicle parameters and obstacle positions.
Ensures safe and efficient path planning by generating a circular path that covers the vehicle, addressing the limitations of existing methods by considering vehicle geometry and obstacle proximity, enhancing safety and reliability.
Smart Images

Figure CN114764243B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of path planning, and in particular to a method and system for unmanned vehicle path planning. Background Art
[0002] At present, mobile robot technology is developing rapidly. With the continuous expansion of the application of various robots, the path planning algorithms used in robot navigation are also constantly improving and developing, among which the path planning methods of unmanned vehicles have also been greatly developed and innovated.
[0003] The existing solution based on spatial exploration and heuristic search is to use the center point of the rear axle of the unmanned vehicle as the center of a single exploration circle, and the radius of the single exploration circle is the closest distance to the surrounding obstacles minus the inner circle radius of the vehicle. Create an open list and a close list, such as Figure 1 As shown in the figure, it is used for spatial exploration from the starting circle to the end circle. When a certain exploration circle overlaps with the end circle, the spatial exploration phase is considered to be over, thereby obtaining a circular path. In the heuristic search phase, the unmanned vehicle is driven forward according to the pre-defined motion primitives, and multiple alternative paths are obtained by numerical integration. The information obtained from the circular path is used to calculate the heuristic time cost, thereby selecting the optimal alternative trajectory. The process of driving the unmanned vehicle forward using the pre-defined motion primitives is repeated until an alternative trajectory reaches a certain neighborhood of the target point.
[0004] Existing space exploration-based methods do not consider vehicle heading or kinematics when conducting space exploration. Therefore, the method of setting the distance from the exploration circle to the nearest obstacle as the radius of the vehicle's inner circle cannot guarantee its safety. If the radius of the vehicle's exploration circle is set to the distance from the center of the vehicle's rear axle to the nearest obstacle minus the distance from the center of the vehicle's rear axle to two points in front of the vehicle, although safety can be guaranteed, this approach sacrifices part of the feasible space. This method may work better on circular or square robots, but it may fail to generate the initial circle or exploration circle on unmanned vehicles with larger length and width, such as car-like robots, especially in complex environments with dense obstacles. Summary of the invention
[0005] The purpose of the present invention is to provide a path planning method and system for an unmanned vehicle, which solves the problem that an initial circle or an exploration circle cannot be generated when an obstacle is close to the unmanned vehicle.
[0006] To achieve the above object, the present invention provides the following solutions:
[0007] A method for unmanned vehicle path planning, comprising:
[0008] Step 101: Obtain the starting position, the ending position and the position of a rectangular obstacle of the unmanned vehicle in a map coordinate system, wherein the rectangular obstacle is a rectangle that completely covers the obstacle;
[0009] Step 102: Calculate the center coordinates of the position circle at the starting position and the center coordinates of the position circle at the ending position respectively according to the vehicle parameters, the starting position and the ending position of the unmanned vehicle; the position circle includes a front position circle and a rear position circle; the vehicle parameters include the wheelbase of the unmanned vehicle, the distance from the center of the front axle of the unmanned vehicle to the front end of the vehicle, and the distance from the center of the rear axle of the unmanned vehicle to the rear end of the vehicle;
[0010] Step 103: Create 4 open lists and 4 close lists, the 4 open lists are f1o list, r1o list, f2o list and r2o list respectively; the 4 close lists are f1c list, r1c list, f2c list and r2c list respectively, and add the front position circle at the starting point pose, the rear position circle at the starting point pose, the front position circle at the end point pose and the rear position circle at the end point pose to the f1o list, the r1o list, the f2o list and the r2o list respectively;
[0011] Step 104: Pop out the circle with the smallest total cost in the f1o list as the first target circle; pop out the circle corresponding to the first target circle in the r1o list as the second target circle; pop out the circle with the smallest total cost in the f2o list as the third target circle, and pop out the circle corresponding to the third target circle in the r2o list as the fourth target circle;
[0012] Step 105: determining whether the first target circle and the third target circle have an overlapping portion, and obtaining a first determination result;
[0013] Step 106: If the first judgment result is yes, a circular path is determined using a backtracking method according to the first target circle and the third target circle, and step 108 is executed;
[0014] Step 107: If the first judgment result is no, a circular path is determined according to the f1c list, the f2c list, the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle, and step 108 is executed; the first sub-circle is a circle selected from the first extended circle set; the second sub-circle is a circle selected from the second extended circle set and corresponding to the same vehicle posture as the first sub-circle; the third sub-circle is a circle selected from the third extended circle set; the fourth sub-circle is a circle selected from the fourth extended circle set and corresponding to the same vehicle posture as the third sub-circle; the first extended circle set, the second extended circle set, the third extended circle set and the fourth extended circle set respectively correspond to the extended circles of the first target circle, the second target circle, the third target circle and the fourth target circle obtained according to the position of the rectangular obstacle;
[0015] Step 108: Determine the final path of the unmanned vehicle using a heuristic search method based on the circular path.
[0016] Optionally, step 107 specifically includes:
[0017] Step 701: Determine whether the first target circle exists in the f1c list and whether the third target circle exists in the f2c list to obtain a second determination result;
[0018] Step 702: If the second judgment result is yes, return to step 104;
[0019] Step 703: If the second judgment result is no, a circular path is determined according to the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle, and then step 108 is executed.
[0020] Optionally, before step 102, the following steps are further included:
[0021] Step 1011: Calculate the expansion distance according to the vehicle parameters and the width of the unmanned vehicle;
[0022] Step 1012: Expand the position of the rectangular obstacle according to the expansion distance to obtain the expanded position of the rectangular obstacle.
[0023] Optionally, the step 703 specifically includes:
[0024] Step 7031: based on the position of the rectangular obstacle, the first target circle, the second target circle, the third target circle and the fourth target circle are respectively expanded to obtain a first extended circle set, a second extended circle set, a third extended circle set and a fourth extended circle set;
[0025] Step 7032: Select a first sub-circle from the first extended circle set, select a circle corresponding to the same vehicle posture as the first sub-circle from the second extended circle set as a second sub-circle; select a third sub-circle from the third extended circle set, and select a circle corresponding to the same vehicle posture as the third sub-circle from the fourth extended circle set as a fourth sub-circle;
[0026] Step 7033: Add the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle to the f1o list, the r1o list, the f2o list and the r2o list respectively, and add the first target circle, the second target circle, the third target circle and the fourth target circle to the f1c list, the r1c list, the f2c list and the r2c list respectively;
[0027] Step 7034: determining whether the f1o list or the f2o list is empty to obtain a third determination result;
[0028] Step 7035: If the third judgment result is yes, then end the loop;
[0029] Step 7036: If the third judgment result is no, return to step 104.
[0030] Optionally, step 108 specifically includes:
[0031] Step 1081: Create a start node, an end node, a node open list, and a node close list;
[0032] Step 1082: Add the start node to the node open list;
[0033] Step 1083: Determine whether the node open list is empty, and obtain a fourth determination result;
[0034] Step 1084: If the fourth determination result is yes, then the loop ends;
[0035] Step 1085: If the fourth judgment result is no, the final path of the unmanned vehicle is determined based on the node open list, the node close list, and the heuristic time cost from the current node to the target node; the heuristic time cost is calculated based on the circular path.
[0036] Optionally, the step 1085 specifically includes:
[0037] Step 851: Pop out the node with the smallest total cost in the node open list as the current node and determine whether the distance between the current node and the end node is within a set threshold range to obtain a fifth determination result;
[0038] Step 852: If the fifth judgment result is yes, the current node is set as the parent node of the end node, and the final path of the unmanned vehicle is obtained by backtracking according to the end node;
[0039] Step 853: If the fifth judgment result is no, determine whether the current node exists in the node close list to obtain a sixth judgment result;
[0040] Step 854: If the sixth determination result is yes, return to step 1083;
[0041] Step 855: If the sixth judgment result is no, the final path of the unmanned vehicle is determined based on the heuristic time cost from the current node to the target node.
[0042] Optionally, the step 855 specifically includes:
[0043] Step 551: determining a heuristic time cost from the current node to the target node according to the distance between the center of each circle in the circular path and the current node;
[0044] Step 552: Determine the heuristic circle of the current node according to the heuristic time cost;
[0045] Step 553: Determine the step size and motion primitive according to the heuristic circle of the current node;
[0046] Step 554: Integrate the vehicle motion dynamics model according to the motion primitive and the step length to determine multiple candidate trajectories and sub-nodes corresponding to each candidate trajectory; the sub-node is the last trajectory point in the candidate trajectory;
[0047] Step 555: setting the current node as the parent node of each of the child nodes;
[0048] Step 556: Add all the child nodes to the node open list, add the current node to the node close list, and then return to step 1083.
[0049] A path planning system for an unmanned vehicle, comprising:
[0050] An acquisition module, used to acquire the starting position, the ending position and the position of the rectangular obstacle of the unmanned vehicle in the map coordinate system, wherein the rectangular obstacle is a rectangle that completely covers the obstacle;
[0051] A position circle calculation module is used to calculate the center coordinates of the position circle at the starting position and the center coordinates of the position circle at the ending position according to the vehicle parameters, the starting position and the ending position of the unmanned vehicle; the position circle includes a front position circle and a rear position circle; the vehicle parameters include the wheelbase of the unmanned vehicle, the distance from the center of the front axle of the unmanned vehicle to the front end of the vehicle, and the distance from the center of the rear axle of the unmanned vehicle to the rear end of the vehicle;
[0052] A list creation module is used to create 4 open lists and 4 close lists, the 4 open lists are f1o list, r1o list, f2o list and r2o list respectively; the 4 close lists are f1c list, r1c list, f2c list and r2c list respectively, and the front position circle at the starting position, the rear position circle at the starting position, the front position circle at the end position and the rear position circle at the end position are added to the f1o list, the r1o list, the f2o list and the r2o list respectively;
[0053] A pop-up module is used to pop out the circle with the smallest total cost in the f1o list as the first target circle; pop out the circle corresponding to the first target circle in the r1o list as the second target circle; pop out the circle with the smallest total cost in the f2o list as the third target circle, and pop out the circle corresponding to the third target circle in the r2o list as the fourth target circle;
[0054] A judgment module, used for judging whether the first target circle and the third target circle have overlapping parts, and obtaining a first judgment result;
[0055] A first result module, configured to determine a circular path using a backtracking method according to the first target circle and the third target circle respectively if the first judgment result is yes, and execute a path determination module;
[0056] a second result module, for determining a circular path according to the f1c list, the f2c list, the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle, and executing a path determination module if the first judgment result is no; the first sub-circle is a circle selected from the first extended circle set; the second sub-circle is a circle selected from the second extended circle set and corresponding to the same vehicle posture as the first sub-circle; the third sub-circle is a circle selected from the third extended circle set; the fourth sub-circle is a circle selected from the fourth extended circle set and corresponding to the same vehicle posture as the third sub-circle; the first extended circle set, the second extended circle set, the third extended circle set and the fourth extended circle set respectively correspond to the extended circles of the first target circle, the second target circle, the third target circle and the fourth target circle obtained according to the position of the rectangular obstacle;
[0057] The path determination module is used to determine the final path of the unmanned vehicle using a heuristic search method based on the circular path.
[0058] Optionally, the second result module specifically includes:
[0059] A first judgment unit is used to judge whether the first target circle exists in the f1c list and whether the third target circle exists in the f2c list to obtain a second judgment result;
[0060] A first result determination unit, configured to return to the pop-up module if the second determination result is yes;
[0061] The second result determination unit is configured to determine a circular path according to the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle if the second judgment result is no, and then execute a path determination module.
[0062] Optionally, the path determination module specifically includes:
[0063] Create a unit to create the start node, end node, node open list and node close list;
[0064] A node open list adding unit, used for adding the start node to the node open list;
[0065] A second judgment unit is used to judge whether the node open list is empty to obtain a fourth judgment result;
[0066] A third result determination unit, configured to end the loop if the fourth determination result is yes;
[0067] The fourth result determination unit is used to determine the final path of the unmanned vehicle according to the node open list, the node close list, and the heuristic time cost from the current node to the target node if the fourth judgment result is no; the heuristic time cost is calculated based on the circular path.
[0068] According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects: the present invention obtains the starting position, end position and position of a rectangular obstacle of an unmanned vehicle in a map coordinate system; calculates the center coordinates of a position circle at the starting position and the center coordinates of a position circle at the end position respectively according to vehicle parameters and the starting position and end position of the unmanned vehicle; creates 4 open lists and 4 close lists to add the front position circle at the starting position, the rear position circle at the starting position, the front position circle at the end position and the rear position circle at the end position to the f1o list, the r1o list, the f2o list and the r2o list respectively; pops out the circle with the smallest total cost in the f1o list as the first target circle; pops out the circle corresponding to the first target circle in the r1o list as the second target circle. two target circles; the circle with the smallest total cost in the f2o list is popped out as the third target circle, and the circle corresponding to the third target circle in the r2o list is popped out as the fourth target circle; it is determined whether the first target circle and the third target circle have overlapping parts, if so, the circular path is determined according to the first target circle and the third target circle by backtracking method respectively, and the final path of the unmanned vehicle is determined by a heuristic search method according to the circular path; if not, the circular path is determined according to the f1c list, the f2c list, the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle, and the final path of the unmanned vehicle is determined by a heuristic search method according to the circular path. The present invention uses a position circle and a double circle to cover the vehicle to solve the problem that an initial circle or an exploration circle cannot be generated when an obstacle is close to the unmanned vehicle. BRIEF DESCRIPTION OF THE DRAWINGS
[0069] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.
[0070] Figure 1 This is a schematic diagram of one-way exploration;
[0071] Figure 2 A schematic diagram of a bidirectional exploration provided by an embodiment of the present invention;
[0072] Figure 3 It is the vehicle geometry information diagram;
[0073] Figure 4 A double-circle vehicle coverage diagram provided by an embodiment of the present invention;
[0074] Figure 5 A two-way space exploration result diagram provided by an embodiment of the present invention;
[0075] Figure 6 A schematic diagram of the process of improving the space exploration method and the heuristic search method provided by the embodiment of the present invention;
[0076] Figure 7 An improved space exploration flow chart provided for an embodiment of the present invention;
[0077] Figure 8 An improved heuristic search flow chart provided for an embodiment of the present invention. DETAILED DESCRIPTION
[0078] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0079] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.
[0080] Existing unmanned vehicle path planning methods are mainly divided into graph search methods based on grid maps, represented by A-star and D-star; random sampling methods, represented by RRT and PRM, and deterministic sampling methods based on state lattices, etc. The bidirectional dynamic path planning method for an autonomous driving unmanned vehicle based on the A-star algorithm is to initialize, load a grid map, set the starting point and end point and the moving step of the unmanned vehicle, create two open lists and two close lists, which are used to search from the starting point to the target point using the A-star algorithm and search from the target point to the starting point respectively; when the same node appears in the two open lists, it is considered that the path under the current map is found, and then the position of the unmanned vehicle is moved, and the new position is set as the new starting point. By judging whether the new position is the target position, the movement of the unmanned vehicle is ended if it is the target position. When the unmanned vehicle does not reach the target position, the map information is updated, and the path finding and moving process is repeated until the unmanned vehicle reaches the target position. Because the existing A-star-based methods are basically planned based on grid maps, the quality of the obtained path is proportional to the time required by the algorithm, that is, if you want to obtain a better path, the resolution of the map is higher, which leads to an increase in the path finding time; and if the map resolution is low, the quality of the obtained path is relatively low. The unmanned vehicle path planning method based on the probabilistic target biased rapidly expanding random tree (RRT) of the dynamic variable sampling area is to first initialize the map information, and determine the area according to the dynamic variable sampling area formula; on this basis, collision detection with a reserved safety distance is performed, and a new node is generated according to the probabilistic target bias formula and the step size selection formula, and the above steps are repeated until the distance between the new node and the target node is less than the distance threshold, reverse search, and output the path; finally, the output path is optimized by reverse optimization and cubic B-spline curve fitting considering the maximum turning angle constraint. The existing RRT-based methods have a relatively fast path finding speed, but due to the strong randomness of the RRT sampling nodes, the optimality of the paths obtained by most of the RRT algorithm-based solutions cannot be met. If you want to obtain a high-quality path, the time spent may be huge, and whether it is the A-star method in the grid map or this type of RRT-based method, the path obtained is composed of straight lines. Most of them regard the unmanned vehicle as a complete robot, which does not meet the non-complete constraints of the unmanned vehicle. These broken line paths need to be processed and smoothed in the future before they can be sent to the vehicle control module for execution.
[0081] In view of the above-mentioned defects of the prior art, an embodiment of the present invention provides a method for unmanned vehicle path planning, comprising:
[0082] Step 101: Obtain the starting position, end position and position of a rectangular obstacle of the unmanned vehicle in a map coordinate system. The rectangular obstacle is a rectangle that completely covers the obstacle.
[0083] Step 102: Calculate the center coordinates of the position circle at the starting position and the center coordinates of the position circle at the ending position respectively according to the vehicle parameters, the starting position and the ending position of the unmanned vehicle; the position circle includes a front position circle and a rear position circle; the vehicle parameters include the wheelbase of the unmanned vehicle, the distance from the center of the front axle of the unmanned vehicle to the front end of the vehicle, and the distance from the center of the rear axle of the unmanned vehicle to the rear end of the vehicle.
[0084] Step 103: Create 4 open lists and 4 close lists, the 4 open lists are f1o list, r1o list, f2o list and r2o list respectively; the 4 close lists are f1c list, r1c list, f2c list and r2c list respectively, and add the front position circle at the starting pose, the rear position circle at the starting pose, the front position circle at the end pose and the rear position circle at the end pose to the f1o list, the r1o list, the f2o list and the r2o list respectively.
[0085] Step 104: Pop out the circle with the smallest total cost in the f1o list as the first target circle; pop out the circle corresponding to the first target circle in the r1o list as the second target circle; pop out the circle with the smallest total cost in the f2o list as the third target circle, and pop out the circle corresponding to the third target circle in the r2o list as the fourth target circle.
[0086] Step 105: Determine whether the first target circle and the third target circle have an overlapping portion, and obtain a first determination result.
[0087] Step 106: If the first judgment result is yes, a circular path is determined using the backtracking method according to the first target circle and the third target circle, and step 108 is executed; a circular path is a path from the starting point to the end point formed by connecting circles with overlapping parts.
[0088] Step 107: If the first judgment result is no, a circular path is determined according to the f1c list, the f2c list, the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle, and step 108 is executed; the first sub-circle is a circle selected from the first extended circle set; the second sub-circle is a circle selected from the second extended circle set and corresponding to the same vehicle posture as the first sub-circle; the third sub-circle is a circle selected from the third extended circle set; the fourth sub-circle is a circle selected from the fourth extended circle set and corresponding to the same vehicle posture as the third sub-circle; the first extended circle set, the second extended circle set, the third extended circle set and the fourth extended circle set respectively correspond to the extended circles of the first target circle, the second target circle, the third target circle and the fourth target circle obtained according to the position of the rectangular obstacle.
[0089] Step 108: Determine the final path of the unmanned vehicle using a heuristic search method based on the circular path.
[0090] In practical applications, step 107 specifically includes:
[0091] Step 701: Determine whether the first target circle exists in the f1c list and whether the third target circle exists in the f2c list to obtain a second determination result.
[0092] Step 702: If the second judgment result is yes, return to step 104.
[0093] Step 703: If the second judgment result is no, a circular path is determined according to the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle, and then step 108 is executed.
[0094] In practical applications, before step 102, the following steps are also included:
[0095] Step 1011: Calculate the expansion distance according to the vehicle parameters and the width of the unmanned vehicle.
[0096] Step 1012: Expand the position of the rectangular obstacle according to the expansion distance to obtain the expanded position of the rectangular obstacle.
[0097] In practical applications, step 703 specifically includes:
[0098] Step 7031: Based on the position of the rectangular obstacle, the first target circle, the second target circle, the third target circle and the fourth target circle are respectively expanded to obtain a first extended circle set, a second extended circle set, a third extended circle set and a fourth extended circle set.
[0099] Step 7032: Select a first sub-circle from the first extended circle set, and select a circle corresponding to the same vehicle posture as the first sub-circle from the second extended circle set as the second sub-circle; select a third sub-circle from the third extended circle set, and select a circle corresponding to the same vehicle posture as the third sub-circle from the fourth extended circle set as the fourth sub-circle.
[0100] Step 7033: Add the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle to the f1o list, the r1o list, the f2o list and the r2o list respectively, and add the first target circle, the second target circle, the third target circle and the fourth target circle to the f1c list, the r1c list, the f2c list and the r2c list respectively.
[0101] Step 7034: Determine whether the f1o list or the f2o list is empty to obtain a third determination result.
[0102] Step 7035: If the third judgment result is yes, then the loop ends, the path finding is considered to have failed, and the program exits directly.
[0103] Step 7036: If the third judgment result is no, return to step 104.
[0104] In practical applications, after step 7032 and before step 7033, the following steps are also included:
[0105] The smallest radius of the first sub-circle and the second sub-circle is determined as the radius of the first sub-circle and the second sub-circle to obtain a new first sub-circle and a new second sub-circle.
[0106] The smallest radius of the third sub-circle and the fourth sub-circle is determined as the radius of the third sub-circle and the fourth sub-circle to obtain a new third sub-circle and a new fourth sub-circle.
[0107] In practical applications, step 108 specifically includes:
[0108] Step 1081: Create a start node, an end node, a node open list, and a node close list.
[0109] Step 1082: Add the start node to the node open list.
[0110] Step 1083: Determine whether the node open list is empty, and obtain a fourth determination result.
[0111] Step 1084: If the fourth judgment result is yes, then the loop ends, and the path finding is considered to have failed, and the process exits directly.
[0112] Step 1085: If the fourth judgment result is no, the final path of the unmanned vehicle is determined based on the node open list, the node close list, and the heuristic time cost from the current node to the target node; the heuristic time cost is calculated based on the circular path.
[0113] In practical applications, the step 1085 specifically includes:
[0114] Step 851: Pop out the node with the smallest total cost in the node open list as the current node and determine whether the distance between the current node and the end node is within a set threshold range to obtain a fifth determination result.
[0115] Step 852: If the fifth judgment result is yes, the current node is set as the parent node of the end node, and the final path of the unmanned vehicle is obtained by backtracking based on the end node.
[0116] Step 853: If the fifth judgment result is no, determine whether the current node exists in the node close list to obtain a sixth judgment result.
[0117] Step 854: If the sixth judgment result is yes, return to step 1083.
[0118] Step 855: If the sixth judgment result is no, the final path of the unmanned vehicle is determined based on the heuristic time cost from the current node to the target node.
[0119] In practical applications, the step 855 specifically includes:
[0120] Step 551: Determine the heuristic time cost from the current node to the target node according to the distance between the center of each circle in the circular path and the current node.
[0121] Step 552: Determine the heuristic circle of the current node according to the heuristic time cost.
[0122] Step 553: Determine the step size and motion primitive according to the heuristic circle of the current node.
[0123] Step 554: Integrate the vehicle motion dynamics model according to the motion primitive and the step size to determine multiple candidate trajectories and sub-nodes corresponding to each candidate trajectory; the sub-node is the last trajectory point in the candidate trajectory.
[0124] Step 555: Set the current node as the parent node of each of the child nodes.
[0125] Step 556: Add all the child nodes to the node open list, add the current node to the node close list, and then return to step 1083.
[0126] In view of the above method, an embodiment of the present invention further provides an unmanned vehicle path planning system, including:
[0127] The acquisition module is used to obtain the starting position, end position and position of the unmanned vehicle in the map coordinate system, and the position of the rectangular obstacle, wherein the rectangular obstacle is a rectangle that completely covers the obstacle.
[0128] The position circle calculation module is used to calculate the center coordinates of the position circle at the starting position and the center coordinates of the position circle at the ending position according to the vehicle parameters, the starting position and the ending position of the unmanned vehicle; the position circle includes a front position circle and a rear position circle; the vehicle parameters include the wheelbase of the unmanned vehicle, the distance from the center of the front axle of the unmanned vehicle to the front end of the vehicle, and the distance from the center of the rear axle of the unmanned vehicle to the rear end of the vehicle.
[0129] A list creation module is used to create 4 open lists and 4 close lists, the 4 open lists are f1o list, r1o list, f2o list and r2o list; the 4 close lists are f1c list, r1c list, f2c list and r2c list, and the front position circle at the starting position, the rear position circle at the starting position, the front position circle at the end position and the rear position circle at the end position are added to the f1o list, the r1o list, the f2o list and the r2o list respectively.
[0130] A pop-up module is used to pop out the circle with the smallest total cost in the f1o list as the first target circle; pop out the circle corresponding to the first target circle in the r1o list as the second target circle; pop out the circle with the smallest total cost in the f2o list as the third target circle, and pop out the circle corresponding to the third target circle in the r2o list as the fourth target circle.
[0131] The judgment module is used to judge whether the first target circle and the third target circle have overlapping parts, and obtain a first judgment result.
[0132] The first result module is used to determine a circular path using a backtracking method according to the first target circle and the third target circle respectively if the first judgment result is yes, and execute a path determination module.
[0133] a second result module, for determining a circular path according to the f1c list, the f2c list, the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle, and executing a path determination module if the first judgment result is no; the first sub-circle is a circle selected from the first extended circle set; the second sub-circle is a circle selected from the second extended circle set and corresponding to the same vehicle posture as the first sub-circle; the third sub-circle is a circle selected from the third extended circle set; the fourth sub-circle is a circle selected from the fourth extended circle set and corresponding to the same vehicle posture as the third sub-circle; the first extended circle set, the second extended circle set, the third extended circle set and the fourth extended circle set respectively correspond to the extended circles of the first target circle, the second target circle, the third target circle and the fourth target circle obtained according to the position of the rectangular obstacle.
[0134] The path determination module is used to determine the final path of the unmanned vehicle using a heuristic search method based on the circular path.
[0135] As an optional implementation manner, the second result module specifically includes:
[0136] The first judgment unit is used to judge whether the first target circle exists in the f1c list and whether the third target circle exists in the f2c list to obtain a second judgment result.
[0137] The first result determination unit is configured to return to the pop-up module if the second determination result is yes.
[0138] The second result determination unit is configured to determine a circular path according to the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle if the second judgment result is no, and then execute a path determination module.
[0139] As an optional implementation, the path determination module specifically includes:
[0140] The creation unit is used to create the start node, end node, node open list and node close list.
[0141] The node open list adding unit is used to add the start node to the node open list.
[0142] The second judgment unit is used to judge whether the node open list is empty to obtain a fourth judgment result.
[0143] The third result determination unit is used to end the loop if the fourth judgment result is yes, consider the path finding as failed, and exit directly.
[0144] The fourth result determination unit is used to determine the final path of the unmanned vehicle according to the node open list, the node close list, and the heuristic time cost from the current node to the target node if the fourth judgment result is no; the heuristic time cost is calculated based on the circular path.
[0145] The embodiment of the present invention also provides a more specific unmanned vehicle path planning method based on improved space exploration method and heuristic search, such as Figure 2 , Figure 5 and Figure 6As shown, the technical solution proposed in this embodiment is mainly divided into stages: 1. Obtaining a circular path based on the improved space exploration method: First, using a double-circle covering vehicle instead of a single-circle covering vehicle makes the path obtained by the space exploration method safer and more reliable; secondly, accelerating the speed of the double-circle exploration method by a two-way exploration method of exploring from the starting circle to the end circle and from the end circle to the starting circle; 2. Based on the circular path information obtained by the improved space exploration method, different motion primitives are selected according to different heuristic circles in the circular path information to drive the vehicle forward, and multiple alternative paths are obtained. The optimal alternative path is selected by the time heuristic cost, and the heuristic search step is repeated until the neighborhood of the end point is reached, which is regarded as the end of path finding.
[0146] Among them, improving space exploration methods such as Figure 7 As shown:
[0147] (1) Initialize the map information, obtain the location and size of obstacles and map boundary information, use the current initial position of the unmanned vehicle as the starting point, and set a target point.
[0148] Taking the lower left corner of the map as the origin of the map coordinate system, obtain the starting position (x_start, y_start, yaw_start) and the end position (x_goal, y_goal, yaw_goal) of the unmanned vehicle in the map coordinate system.
[0149] At the same time, the obstacles in the map are covered with convex rectangles to obtain rectangular obstacles. Each rectangular obstacle is represented by the coordinates of the four points of the rectangle. The map boundary information is represented by the minimum x coordinate (min_map_x), maximum x coordinate (max_map_x), minimum y coordinate (min_map_y), and maximum y coordinate (max_map_y) of the map. This will be used when calculating the radius of the exploration circle. The generated exploration circle must be within the map.
[0150] (2) First, the value of R_D can be calculated according to the following radius formula (2-2), and the rectangular obstacles obtained in (1) are expanded: that is, each rectangular obstacle is expanded outward by the length of R_D. Then, according to the following position formula (2-1) and the vehicle parameters (Lw, Lf, Lr), the center coordinates of the front position circle and the center coordinates of the rear position circle are calculated at the starting position (x_start, y_start, yaw_start) and the end position (x_goal, y_goal, yaw_goal), for a total of 4 circles. Finally, the costs of the 4 circles are calculated respectively. Each circle has three costs, f, g, and h, with a parent circle and multiple child circles (the 4 position circles at the starting point and the end point have only child circles, no parent circle). The relationship between the three costs is f=g+h. f is the total cost of the circle; g is the cumulative distance cost from the starting point to the circle, calculated by adding the g of its parent circle to the radius of the parent circle; h is the heuristic cost from the center of the circle to the end point, calculated using the Euclidean distance (Euclidean distance is the straight-line distance between two points) as the heuristic cost. If there is no parent circle, g in the formula f=g+h is 0.
[0151] Position formula:
[0152]
[0153] Among them, x(t), y(t), θ(t) are the center of the rear axle of the vehicle Figure 3 and Figure 4 The pose of point p (the center of the rear axle of the unmanned vehicle) in the figure is that when calculating the front position circle and the rear position circle of the starting position, x(t), y(t), and θ(t) are substituted into the starting position. When calculating the front position circle and the rear position circle of the end position, x(t), y(t), and θ(t) are substituted into the end position. Lw, Lf, and Lr are the wheelbase of the vehicle, the distance from the center of the front axle of the vehicle to the front end of the vehicle, and the distance from the center of the rear axle of the vehicle to the rear end of the vehicle. x_f(t) and y_f(t) are the center coordinates of the front position circle, and x_r(t) and y_r(t) are the center coordinates of the rear position circle. For a specific schematic diagram, please refer to Figure 3 .
[0154] Radius formula:
[0155]
[0156] Where Lb is the width of the vehicle.
[0157] (3) First, create 4 empty open lists and 4 empty close lists, each of which is a priority queue. The 4 open lists are represented as f1o list, r1o list, f2o list and r2o list. The f1o list is used to store the starting position and the front position circle in the forward space exploration process, the r1o list is used to store the starting position and the back position circle in the forward space exploration process, the f2o list is used to store the end position and the front position circle in the reverse space exploration process, and the r2o list is used to store the end position and the back position circle in the reverse space exploration process. The 4 close lists are represented as f1c list, r1c list, f2c list and r2c list. The f1c list and r1c list are used to store the front position parent circle and the back position parent circle used for expansion in each loop during the forward space exploration process, respectively, and the f2c list and r2c list are used to store the front position circle and the back position circle used for expansion in each loop during the reverse space exploration process, respectively. Each time, a circle with the lowest cost is taken out from the open list. When a subcircle is expanded from this circle with the lowest cost, this circle with the lowest cost is added to the close list.
[0158] Then add the four circles created at the starting and ending positions in (2) to the four empty open lists above. That is, the front position circle created by the front position point of the starting position is added to the f1o list, and the back position circle created by the back position point of the starting position is added to the r1o list; the front position circle created by the front position point of the ending position is added to the f2o list, and the back position circle created by the back position point of the ending position is added to the r2o list.
[0159] (4) From the list of front position circles f1o corresponding to the forward space exploration, pop out the circle circle1_f with the smallest total cost f, and then according to the position formula (2-1), pop out the back position circle circle1_r corresponding to circle1_f from the list of back position circles r1o, and from the list of front position circles f2o corresponding to the reverse space exploration, pop out the circle circle2_f with the smallest total cost f, and then according to the position formula (2-1), pop out the back position circle circle2_r corresponding to circle2_f from the list of back position circles r2o.
[0160] Determine whether circle1_f and circle2_f have overlapping parts. If so, end the exploration and proceed to (7); if not, continue to determine whether circle1_f and circle2_f exist in the close lists of their respective forward exploration and reverse exploration. If so, repeat (4); if not, proceed to the next step.
[0161] (5) First, according to the set rules, circle1_f, circle1_r and circle2_f, circle2_r are expanded respectively, and circle1_f, circle1_r, circle2_f and circle2_r are set as the parent circles of their respective extended sub-circles. Then, from the extended sub-circles of circle1_f and circle1_r, two extended sub-circles corresponding to the same posture, that is, satisfying formula (2-1), are found and determined as the front position circle and the rear position circle. One posture corresponds to one front position circle and one rear position circle. Then, the smaller radius of the two front position circles and the rear position circle is used as the new radius of the front position circle and the rear position circle to obtain a new front position circle and a new rear position circle. Similarly, find two extended sub-circles corresponding to the same posture from the extended sub-circles of circle2_f and circle2_r, that is, two extended sub-circles that satisfy formula (2-1) are determined as the front position circle and the rear position circle. One posture corresponds to one front position circle and one rear position circle. Then, the smaller radius of the two front position circles and the rear position circle is used as the new radius of the front position circle and the rear position circle to obtain the new front position circle and the new rear position circle. Then, the three costs f, g, and h are calculated for these new front position circles and new rear position circles, respectively, where f is the total cost of the sub-circle, the cost g of the sub-circle is equal to the cost g of its parent circle plus the radius of the parent circle; the h of the sub-circle is equal to the heuristic cost from the center of the sub-circle to the end point, which is the Euclidean distance length. (The process of determining the extended sub-circle: uniformly sample n points on the boundary of the parent circle, with the sampling point as the center of the sub-circle, and the distance from the sampling point to the nearest rectangular obstacle is the radius of the sub-circle).
[0162] After the cost calculation of the child circle is completed, the new front position circle and the back position circle are added to their respective open lists. Finally, circle1_f, circle1_r, circle2_f, and circle2_r are added as parent circles to their respective close lists.
[0163] (6) Determine whether one of the two open lists (f1o list and f2o list) corresponding to the previous circle is empty. If it is empty, the exploration ends directly, the path finding is considered to have failed, and the process exits directly. If it is not empty, the process returns to (4) and repeats (4) to (6) until the conditions for ending the exploration are met, and then executes (7) at last.
[0164] (7) When the loop end condition in (4) is met, that is, circle1_f and circle2_f have overlapping parts, the spatial exploration path finding is considered successful. At this time, by backtracking from circle1_f and circle2_f, a circular path from the starting point to the end point can be found.
[0165] Among them, the improved heuristic search method is as follows Figure 8 As shown:
[0166] This method mainly uses the circular path obtained by the improved spatial exploration method to estimate the heuristic time cost from the vehicle's current state node to the target point. Each circle in the circular path can be called an exploration circle in the improved heuristic search method, and the circular path is mainly used to calculate the heuristic time cost in (4) of the improved heuristic search method, so as to propagate the state of the unmanned vehicle forward and finally obtain a feasible path from the starting point to the end point that satisfies the vehicle's non-complete constraints.
[0167] Here are the steps:
[0168] (1) First, create the starting node q_start (the starting node is a node composed of the starting position (x, y), direction (yaw) and speed (v) of the starting point), the end node q_goal (the end node is actually a node composed of the end position (x, y), direction (yaw) and speed (v) of the end point) and an empty open list (node open list) and close list (node close list) according to the starting position and the end position. Then calculate the three time costs (t_f, t_g, t_h) of the starting node q_start and add the starting node q_start to the node open list. The relationship among t_f, t_g and t_h is: t_f=t_g+t_h, where t_f is the total cost of the node; t_g is the cumulative time cost from the starting node to the node, and the cumulative time cost is equal to the t_g cost of the parent node of the node plus the time cost from the parent node to the node, and the time cost t from the parent node to the node can be calculated by the following formula (6-3). In particular, the starting node has no parent node, so the t_g of the starting node is 0; t_h is the heuristic time cost from the node to the end point, and the heuristic time cost t_h can be calculated by formula (4-1).
[0169] (2) Determine whether the node open list is empty. If the node open list is not empty, pop out the target node q with the minimum cost from the node open list; otherwise, the loop path search fails.
[0170] (3) Check whether the distance between the target node q obtained in (2) and the end node q_goal is within a certain threshold. If so, the path finding is considered successful, and the target node q is set as the parent node of the end node q_goal, and then the loop execution (8) ends; if the distance exceeds a certain threshold, continue to check whether the target node q in (2) is in the node close list. If so, jump to (2) to execute; if not, continue to (4).
[0171] (4) Estimate the heuristic time cost from the target node q to the end node q_goal based on the distance information of the circular path obtained by the improved spatial exploration method. The calculation formula of the heuristic time cost is as follows:
[0172]
[0173] Where d_q,c represents the distance between the target node q and the center of circle c, d_c is the distance from the center of circle c to the end point, and circle c refers to any circle in the circular path. v is the current speed of the vehicle, and t_h is the heuristic time cost. For the target node q, the heuristic time cost t_h is calculated multiple times using the nearest exploration circle and the neighbor circles of the nearest exploration circle (the circles in front and behind the nearest circle), and then the minimum time is taken as the heuristic time cost of the state node.
[0174] For a node, the nearest circle and its neighbor circles (the neighbor circles refer to the circles in front and behind the nearest circle) are used to calculate multiple heuristic time cost estimates t_h, and the minimum t_h is used as the effective heuristic time cost. At the same time, the exploration circle corresponding to the minimum t_h is called the heuristic circle of the node.
[0175] (5) (4) The heuristic circle of node q is obtained. In this step, the appropriate step size is selected according to the radius of the heuristic circle of node q. The formula for the step size is as follows:
[0176] s=min(α·r_c,β·dc,s_min), where s represents the step size of the integration, α and β are two constant coefficients, r_c is the radius of the heuristic circle, dc is the distance from the heuristic circle to the end point, and s_min is the minimum step size, which is a constant.
[0177] (6) Predefined commonly used motion primitives: Motion primitives are composed of many pairs of different accelerations and angular velocities of the front wheel turning angles. The movement of the vehicle can only be selected from these predefined pairs of accelerations and angular velocities of the front wheel turning angles.
[0178] Different actions on the vehicle's steering wheel, accelerator, and brake can make the vehicle move in different ways. Table 6-1 and Table 6-2 show the different movements that the vehicle can make with different combinations of the steering wheel, accelerator, and brake, which are called motion primitives.
[0179] Max_a: The maximum acceleration of the vehicle; Max_w: The maximum angular velocity of the vehicle.
[0180] decelerate: Indicates that the vehicle will decelerate.
[0181] decelerate_right: Indicates that the vehicle will turn the steering wheel to the right and slow down.
[0182] decelerate_left: indicates that the vehicle will turn the steering wheel to the left and slow down.
[0183] accelerate: means the vehicle will accelerate.
[0184] accelerate_right: Indicates that the vehicle will turn the steering wheel to the right and accelerate.
[0185] accelerate_left: Indicates that the vehicle will turn the steering wheel to the left and accelerate.
[0186] right: Indicates that the vehicle will turn the steering wheel to the right at a constant speed.
[0187] Stright: Indicates that the vehicle will maintain the direction of the steering wheel and keep a constant speed.
[0188] left: Indicates that the vehicle will turn the steering wheel to the left at a constant speed.
[0189] Table 6-1 The first group of motion primitives
[0190] -Max_a 0 Max_a -Max_w decelerate_right right accelerate_right 0 decelerate stright accelerate Max_w decelerate_left left accelerate_left
[0191] Table 6-2 The second group of motion primitives
[0192] -Max_a / 2 0 Max_a / 2 -Max_w / 2 decelerate_right right accelerate_right 0 decelerate stright accelerate Max_w / 2 decelerate_left left accelerate_left
[0193] A set of different motion primitives is selected based on the size of the integral step s obtained from (5), that is, when the integral step s is greater than a certain domain value, the first set of motion primitives is selected; when it is less than a certain domain value, the second set of motion primitives is selected. There are 9 different (a, w) combinations in each set of motion primitives in Table 6-1 and Table 6-2 (u (a, w): u represents the input vector of the system, which is composed of the vehicle's acceleration a and the angular velocity w of the front wheel steering). Therefore, according to the 9 different (a, w) and the following vehicle motion dynamics model formula (6-1), forward integration is performed to obtain 9 different alternative trajectories and 9 sub-nodes (the sub-node is the last trajectory point of each trajectory).
[0194]
[0195]
[0196] t=n*Δt (6-3)
[0197] The number of forward integrations n can be calculated by formula (6-2). Where s is the integration step s obtained in (5), |v| is the velocity of the current node q, and Δt is the integration time interval.
[0198] (7) Set the target node q as the parent node of each child node obtained by integration in (6), and calculate the t_f, t_g, and t_h costs of the child nodes; then add each child node to the node open list and add the target node q to the node close list. Then loop (2)-(7) until the loop end condition is met.
[0199] (8) When the loop ends because the path finding in (3) is successful, we can backtrack from the target node q to obtain a feasible path from the start point to the end point that satisfies the vehicle's non-holonomic constraints.
[0200] In the existing path planning method based on traditional A-star, the path of the unmanned vehicle must be planned based on the grid map. The quality of the obtained path depends on the resolution of the grid map. When the map resolution is reasonable, a global path with a shorter path can be obtained, but when the map range becomes larger, the execution time of the algorithm and the path quality may not be guaranteed. In addition, the obtained path needs further smoothing before it can be handed over to the unmanned vehicle for execution. Even if the path is found through the technical solution based on the traditional RRT, due to the inherent randomness of RRT, it is difficult for such solutions to find a feasible and better path for the unmanned vehicle within a limited time. Therefore, the present invention uses an improved spatial exploration method and heuristic search to first quickly extract a collision-free feasible circular path from the working space of the unmanned vehicle, and then searches for an executable path to the target configuration through a heuristic search method based on the distance and other information provided by the circular path. Among them, the main advantages of the present invention compared with the traditional unmanned vehicle technical solution and the original spatial exploration and heuristic search method are as follows:
[0201] (1) Compared with the traditional grid map-based method, such as A-star, the method adopted by the present invention does not need to use a grid map, thus saving storage and call space.
[0202] (2) Compared with the traditional random sampling-based methods, such as RRT and other methods, the path obtained according to the present invention can obtain a path executable by the unmanned vehicle without performing further smoothing operations.
[0203] (3) Since the present invention constructs a double-circle coverage unmanned vehicle based on the two quarter position points of the vehicle, a double-circle coverage and two-way exploration strategy is adopted, which solves the problem that when the surrounding environment of the unmanned vehicle is relatively dense and complex, especially when the unmanned vehicle is relatively close to obstacles, it is impossible to find a circular path that meets the collision-free requirement through the spatial exploration method. The method of exploring from the starting circle to the end circle and from the end circle to the starting circle is adopted to accelerate the speed of spatial exploration to obtain the circular path, which solves the problem that the method may have a long solution time when the environment complexity is high. It can not only increase the path finding success rate of the improved method, but also accelerate the path finding speed, and enhance the feasibility and safety of obtaining a circular path in a complex environment.
[0204] (4) In the heuristic search stage, the present invention does not use fixed motion primitives to generate candidate nodes and paths, but selects different motion primitives according to the different radii of the heuristic circles of the exploration nodes. This makes the path obtained by the method of the present invention closer to the global path point while enhancing the safety of the path. In addition, the search process of the circular path is accelerated by constructing a heuristic function. At the same time, predefined motion primitives are used in the heuristic search stage to generate a path that satisfies the non-intact constraints.
[0205] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.
[0206] The principles and implementation methods of the present invention are described in this article using specific examples. The description of the above embodiments is only used to help understand the method and core idea of the present invention. At the same time, for those skilled in the art, according to the idea of the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting the present invention.
Claims
1. A method for unmanned vehicle path planning, characterized in that: include: Step 101: Obtain the starting position, the ending position and the position of a rectangular obstacle of the unmanned vehicle in a map coordinate system, wherein the rectangular obstacle is a rectangle that completely covers the obstacle; Step 102: Calculate the center coordinates of the position circle at the starting position and the center coordinates of the position circle at the ending position respectively according to the vehicle parameters, the starting position and the ending position of the unmanned vehicle; the position circle includes a front position circle and a rear position circle; the vehicle parameters include the wheelbase of the unmanned vehicle, the distance from the center of the front axle of the unmanned vehicle to the front end of the vehicle, and the distance from the center of the rear axle of the unmanned vehicle to the rear end of the vehicle; Step 103: Create 4 open lists and 4 close lists, the 4 open lists are f1o list, r1o list, f2o list and r2o list respectively; the 4 close lists are f1c list, r1c list, f2c list and r2c list respectively, and add the front position circle at the starting point pose, the rear position circle at the starting point pose, the front position circle at the end point pose and the rear position circle at the end point pose to the f1o list, the r1o list, the f2o list and the r2o list respectively; Step 104: Pop out the circle with the smallest total cost in the f1o list as the first target circle; pop out the circle corresponding to the first target circle in the r1o list as the second target circle; pop out the circle with the smallest total cost in the f2o list as the third target circle, and pop out the circle corresponding to the third target circle in the r2o list as the fourth target circle; Step 105: determining whether the first target circle and the third target circle have an overlapping portion, and obtaining a first determination result; Step 106: If the first judgment result is yes, a circular path is determined using a backtracking method according to the first target circle and the third target circle, and step 108 is executed; Step 107: If the first judgment result is no, a circular path is determined according to the f1c list, the f2c list, the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle, and step 108 is executed; the first sub-circle is a circle selected from the first extended circle set; the second sub-circle is a circle selected from the second extended circle set and corresponding to the same vehicle posture as the first sub-circle; the third sub-circle is a circle selected from the third extended circle set; the fourth sub-circle is a circle selected from the fourth extended circle set and corresponding to the same vehicle posture as the third sub-circle; the first extended circle set, the second extended circle set, the third extended circle set and the fourth extended circle set respectively correspond to the extended circles of the first target circle, the second target circle, the third target circle and the fourth target circle obtained according to the position of the rectangular obstacle; Step 108: Determine the final path of the unmanned vehicle using a heuristic search method based on the circular path.
2. The unmanned vehicle path planning method according to claim 1, characterized in that: The step 107 specifically includes: Step 701: Determine whether the first target circle exists in the f1c list and whether the third target circle exists in the f2c list to obtain a second determination result; Step 702: If the second judgment result is yes, return to step 104; Step 703: If the second judgment result is no, a circular path is determined according to the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle, and then step 108 is executed.
3. The unmanned vehicle path planning method according to claim 1, characterized in that: Before step 102, the method further includes: Step 1011: Calculate the expansion distance according to the vehicle parameters and the width of the unmanned vehicle; Step 1012: Expand the position of the rectangular obstacle according to the expansion distance to obtain the expanded position of the rectangular obstacle.
4. The unmanned vehicle path planning method according to claim 2, characterized in that: Step 703 specifically includes: Step 7031: based on the position of the rectangular obstacle, the first target circle, the second target circle, the third target circle and the fourth target circle are respectively expanded to obtain a first extended circle set, a second extended circle set, a third extended circle set and a fourth extended circle set; Step 7032: Select a first sub-circle from the first extended circle set, select a circle corresponding to the same vehicle posture as the first sub-circle from the second extended circle set as a second sub-circle; select a third sub-circle from the third extended circle set, and select a circle corresponding to the same vehicle posture as the third sub-circle from the fourth extended circle set as a fourth sub-circle; Step 7033: Add the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle to the f1o list, the r1o list, the f2o list and the r2o list respectively, and add the first target circle, the second target circle, the third target circle and the fourth target circle to the f1c list, the r1c list, the f2c list and the r2c list respectively; Step 7034: determining whether the f1o list or the f2o list is empty to obtain a third determination result; Step 7035: If the third judgment result is yes, then end the loop; Step 7036: If the third judgment result is no, return to step 104.
5. The unmanned vehicle path planning method according to claim 1, characterized in that: The step 108 specifically includes: Step 1081: Create a start node, an end node, a node open list, and a node close list; Step 1082: Add the start node to the node open list; Step 1083: Determine whether the node open list is empty, and obtain a fourth determination result; Step 1084: If the fourth determination result is yes, then the loop ends; Step 1085: If the fourth judgment result is no, the final path of the unmanned vehicle is determined based on the node open list, the node close list, and the heuristic time cost from the current node to the end node; the current node is the node with the smallest total cost in the node open list; the heuristic time cost is calculated based on the circular path.
6. The unmanned vehicle path planning method according to claim 5, characterized in that: The step 1085 specifically includes: Step 851: Pop out the node with the smallest total cost in the node open list as the current node and determine whether the distance between the current node and the end node is within a set threshold range to obtain a fifth determination result; Step 852: If the fifth judgment result is yes, the current node is set as the parent node of the end node, and the final path of the unmanned vehicle is obtained by backtracking according to the end node; Step 853: If the fifth judgment result is no, determine whether the current node exists in the node close list to obtain a sixth judgment result; Step 854: If the sixth determination result is yes, return to step 1083; Step 855: If the sixth judgment result is no, the final path of the unmanned vehicle is determined based on the heuristic time cost from the current node to the end node.
7. The unmanned vehicle path planning method according to claim 6, characterized in that: The step 855 specifically includes: Step 551: determining a heuristic time cost from the current node to the end node according to the distance between the center of each circle in the circular path and the current node; Step 552: Determine the heuristic circle of the current node according to the heuristic time cost; Step 553: Determine the step size and motion primitive according to the heuristic circle of the current node; Step 554: Integrate the vehicle motion dynamics model according to the motion primitive and the step length to determine multiple candidate trajectories and sub-nodes corresponding to each candidate trajectory; the sub-node is the last trajectory point in the candidate trajectory; Step 555: setting the current node as the parent node of each of the child nodes; Step 556: Add all the child nodes to the node open list, add the current node to the node close list, and then return to step 1083.
8. An unmanned vehicle path planning system, characterized in that: include: An acquisition module, used to acquire the starting position, the ending position and the position of the rectangular obstacle of the unmanned vehicle in the map coordinate system, wherein the rectangular obstacle is a rectangle that completely covers the obstacle; A position circle calculation module is used to calculate the center coordinates of the position circle at the starting position and the center coordinates of the position circle at the ending position according to the vehicle parameters, the starting position and the ending position of the unmanned vehicle; the position circle includes a front position circle and a rear position circle; the vehicle parameters include the wheelbase of the unmanned vehicle, the distance from the center of the front axle of the unmanned vehicle to the front end of the vehicle, and the distance from the center of the rear axle of the unmanned vehicle to the rear end of the vehicle; A list creation module is used to create 4 open lists and 4 close lists, the 4 open lists are f1o list, r1o list, f2o list and r2o list respectively; the 4 close lists are f1c list, r1c list, f2c list and r2c list respectively, and the front position circle at the starting position, the rear position circle at the starting position, the front position circle at the end position and the rear position circle at the end position are added to the f1o list, the r1o list, the f2o list and the r2o list respectively; A pop-up module is used to pop out the circle with the smallest total cost in the f1o list as the first target circle; pop out the circle corresponding to the first target circle in the r1o list as the second target circle; pop out the circle with the smallest total cost in the f2o list as the third target circle, and pop out the circle corresponding to the third target circle in the r2o list as the fourth target circle; A judgment module, used for judging whether the first target circle and the third target circle have overlapping parts, and obtaining a first judgment result; A first result module, configured to determine a circular path using a backtracking method according to the first target circle and the third target circle respectively if the first judgment result is yes, and execute a path determination module; a second result module, for determining a circular path according to the f1c list, the f2c list, the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle, and executing a path determination module if the first judgment result is no; the first sub-circle is a circle selected from the first extended circle set; the second sub-circle is a circle selected from the second extended circle set and corresponding to the same vehicle posture as the first sub-circle; the third sub-circle is a circle selected from the third extended circle set; the fourth sub-circle is a circle selected from the fourth extended circle set and corresponding to the same vehicle posture as the third sub-circle; the first extended circle set, the second extended circle set, the third extended circle set and the fourth extended circle set respectively correspond to the extended circles of the first target circle, the second target circle, the third target circle and the fourth target circle obtained according to the position of the rectangular obstacle; The path determination module is used to determine the final path of the unmanned vehicle using a heuristic search method based on the circular path.
9. The unmanned vehicle path planning system according to claim 8, characterized in that: The second result module specifically includes: A first judgment unit is used to judge whether the first target circle exists in the f1c list and whether the third target circle exists in the f2c list to obtain a second judgment result; A first result determination unit, configured to return to the pop-up module if the second determination result is yes; The second result determination unit is configured to determine a circular path according to the first sub-circle, the second sub-circle, the third sub-circle and the fourth sub-circle if the second judgment result is no, and then execute a path determination module.
10. The unmanned vehicle path planning system according to claim 8, characterized in that: The path determination module specifically includes: Create a unit to create the start node, end node, node open list and node close list; A node open list adding unit, used for adding the start node to the node open list; A second judgment unit is used to judge whether the node open list is empty to obtain a fourth judgment result; A third result determination unit, configured to end the loop if the fourth determination result is yes; The fourth result determination unit is used to determine the final path of the unmanned vehicle according to the node open list, the node close list, and the heuristic time cost from the current node to the end node if the fourth judgment result is no; the current node is the node with the smallest total cost in the node open list; the heuristic time cost is calculated based on the circular path.
Citation Information
Patent Citations
Low-speed unmanned vehicle obstacle avoidance method and device, equipment and medium
CN111338361A
Unstructured road vehicle path planning method
CN111857148A
