A Path Planning Method Based on Hybrid Astar and Spatial Corridor

Abstract

The present invention discloses a path planning method based on Hybrid Astar and spatial corridor, which includes constructing a global occupancy grid map in a multi-obstacle environment and setting the coordinates of the task start point and end point; performing path planning based on Hybrid Astar according to the constructed global occupancy grid map to obtain an initial planned path; expanding the path points of the obtained initial path to the obstacles to obtain a plurality of rectangular spaces; simplifying the quantity of the obtained plurality of rectangular spaces and forming a spatial corridor; performing secondary planning on the initial path within the spatial corridor through an optimal control model to obtain an optimized path. The present invention performs path planning in a multi-obstacle scenario. When optimizing the initial path, a spatial corridor is introduced, and an optimal control model is established to obtain an optimal path within the obstacle-free spatial corridor after optimization, which maximally ensures the safety of the vehicle while satisfying the vehicle kinematic constraints, and effectively avoids the danger of collision with nearby obstacles caused by large fluctuations in the optimized path.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and particularly relates to a path planning method based on Hybrid Astar and a spatial corridor. Background Art

[0002] Unmanned vehicles are commonly used in agriculture, transportation, military, or daily traffic. In the actual use process, a multi-obstacle environment is inevitable. Classic path planning algorithms for unmanned vehicles include the RRT series based on sampling, the A* series based on search, the polynomial-based, and the optimal control-based. In actual applications, in addition to being safe and obstacle-free, the path requirements also need to meet the requirements of smoothness and high comfort. Therefore, generally, after obtaining the initial planning result, methods such as polynomial or spline interpolation are used to smooth the initial result path. However, in a multi-obstacle environment, even if the initial result can ensure not hitting obstacles, the smoothed curve may still hit obstacles at corners or narrow places due to fluctuations. Or when designing the initial path in an obstacle avoidance scenario, in order to obtain a result with a lower path cost, a path that is relatively close to the obstacle is planned. At this time, if Bezier is used to smooth the initial path and a result with large fluctuations is generated, the possibility of hitting an obstacle will increase. Moreover, for Bezier, it is also very difficult to select new path points and splice the front and rear segments to optimize the path. Summary of the Invention

[0003] The object of the present invention is to overcome the deficiencies of the prior art. To achieve the above object, a path planning method based on Hybrid Astar and a spatial corridor is adopted to solve the problems raised in the above background art.

[0004] A path planning method based on Hybrid Astar and a spatial corridor includes:

[0005] Construct a global occupancy grid map in a multi-obstacle environment, and set the coordinates of the task start point and end point;

[0006] Based on the constructed global occupancy grid map, perform path planning based on Hybrid Astar to obtain the initial planned path;

[0007] Inflate the path points of the obtained initial path to the obstacles to obtain a plurality of rectangular spaces;

[0008] Simplify the quantity of the obtained plurality of rectangular spaces and form a spatial corridor;

[0009] Perform secondary planning on the initial path within the spatial corridor through an optimal control model to obtain the optimized path.

[0010] As a further solution of the present invention: The specific steps of constructing a global occupancy grid map in a multi-obstacle environment and setting the coordinates of the task start point and end point include:

[0011] Use the positioning device GPS for self-positioning to obtain the start point coordinates, and obtain the GPS coordinates of the task end point;

[0012] At the same time, obtain the obstacles around the current position of the vehicle through the environmental perception system of the autonomous vehicle to obtain a local occupancy grid map, and continuously update it;

[0013] Map the local occupancy grid map into the constructed global occupancy grid map through coordinate transformation.

[0014] As a further solution of the present invention: The specific steps of performing path planning based on HybridAstar according to the constructed global occupancy grid map to obtain the initial planned path include:

[0015] Obtain the start point, end point of the task, and the constructed global occupancy grid map;

[0016] Perform path planning based on Hybrid Astar:

[0017] When the expanded node is closest to the end point, it is determined that the end point has been reached;

[0018] When using Reeds_Sheep to hit the end point, ensure that the Reeds_Sheep waypoints are within the global occupancy grid map and cannot cross the boundary or touch obstacles, otherwise continue to use Hybrid Astar for path search.

[0019] As a further solution of the present invention: The specific steps of expanding the waypoints of the obtained initial path to the obstacles to obtain multiple rectangular spaces include:

[0020] Traverse the waypoints of the entire initial path in sequence, and initialize the four corner points P1, P2, P3, and P4 of the rectangular space box_now according to the coordinates of each waypoint;

[0021] Judge whether there are obstacles at the current waypoint coordinates. If so, skip this point, and initialize the maximum inflation times. Within the inflation times, continuously expand the horizontal and vertical coordinates of P2 and P3 to the obstacle boundary to obtain the horizontal and vertical coordinates of P2 and P3;

[0022] Assign the obtained horizontal and vertical coordinates of P2 and P3 to P1 and P4 to form the rectangular space corresponding to the current waypoint.

[0023] As a further solution of the present invention: The specific steps of simplifying the quantity of the obtained multiple rectangular spaces and forming a space corridor include:

[0024] Preset a rectangular space box_last with four corner points at (0, 0);

[0025] When the rectangular space box_last and the initialized rectangular space box_now have the same waypoints in the intersection part and no unique waypoints in the non - intersection part, the covered part is deleted.

[0026] As a further solution of the present invention: The specific steps of performing quadratic programming on the initial path in the space corridor through the optimal control model to obtain the optimized path include:

[0027] Step 1: Establish a kinematic model:

[0028] First, model the vehicle kinematics based on the bicycle model:

[0029]

[0030] Among them, the end point of the vehicle's rear axle is P0(x0, y0), θ0 represents the heading angle, v0 represents the speed of P0, a0 represents the linear acceleration, is the rotation angle of the front wheel, w0 is the angular velocity of the front wheel, and L w0 represents the length of the vehicle's rear axle;

[0031] When t ∈ [0, t f , the state variable boundaries at the moment are:

[0032] |a0(t)| ≤ a max ;

[0033] |v0(t)| ≤ v max ;

[0034]

[0035] |w0(t)| ≤ Ω max ;

[0036] Among them, a max 、v max 、 Ω max respectively represent the upper bounds of each variable;

[0037] Step 2: Set boundary constraints:

[0038] According to the vehicle's perception system, obtain the information of each variable of x0(0), y0(0), v0(0), θ0(0), a0(0), and w0(0) at the initial state t = 0;

[0039] At the termination moment t = tf If the entire vehicle system stops stably, the following formula can be obtained:

[0040] [v0(t f ), a0(t f ), w0(t f )] = [0, 0, 0];

[0041] Step 3. Set spatial corridor constraints:

[0042] If the initial path contains N fe waypoints, then each waypoint P c (x c (t), y c (t)) should be restricted within the spatial corridor, and the formula is:

[0043]

[0044] wherein, represents the abscissa of point P3 in the rectangular space corresponding to the waypoint, represents the abscissa of point P2 in the rectangular space corresponding to the waypoint, represents the ordinate of point P3 in the rectangular space corresponding to the waypoint, represents the ordinate of point P2 in the rectangular space corresponding to the waypoint;

[0045] Step 4. Obtain the cost function:

[0046] The cost function is:

[0047] where w1, w2 ≥ 0 are cost functions, t f represents the time for the vehicle to travel from the starting point to the ending point, the second term represents the change in vehicle acceleration, and the third term represents the change in angular velocity;

[0048] Step 5. Combine Steps 1 to 4 to obtain an optimal control model;

[0049] Then, based on the first-order explicit Runge-Kutta and the interior point method, solve the model to obtain an optimal path that is safe and obstacle-free within the spatial corridor.

[0050] Compared with the prior art, the present invention has the following technical effects:

[0051] By adopting the above technical solution, through path planning in a multi-obstacle scenario, introducing a spatial corridor during the optimization of the initial path, and establishing an optimal control model to obtain an optimal path that is obstacle-free within the spatial corridor after optimization, it can ensure the safety of the vehicle to the greatest extent while satisfying the vehicle kinematic constraints, and effectively avoid the danger of collision with nearby obstacles caused by large fluctuations in the optimized path. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] The following will describe in detail the specific embodiments of the present invention in conjunction with the accompanying drawings:

[0053] Figure 1 It is a schematic structural diagram of an open embodiment of this application;

[0054] Figure 2 It is a schematic diagram of six directions of the Hybrid Astar extension of the open embodiment of this application;

[0055] Figure 3 It is a schematic diagram of the distribution of four corner nodes of a rectangular space in the open embodiment of this application;

[0056] Figure 4 It is an experimental diagram in a multi-obstacle environment of the open embodiment of this application. Specific Embodiments

[0057] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0058] Please refer to Figure 1 , in the embodiment of the present invention, a path planning method based on Hybrid Astar and a space corridor specifically includes the following steps:

[0059] S1. Construct a global occupancy grid map in a multi-obstacle environment and set the coordinates of the task start point and end point;

[0060] In this embodiment, before path planning, it is necessary to model the environment and establish a global occupancy grid map. An accurate and real-time occupancy grid map is the premise of effective planning and the guarantee for planning a safe path.

[0061] Use the environmental perception system of the unmanned vehicle to obtain the current position of the vehicle and the surrounding obstacle information, model the surrounding environment, and give a global occupancy grid map.

[0062] As Figure 4 shown, the figure is an experimental diagram in a multi-obstacle environment, with the lower left corner as the coordinate origin, the right as the x-axis direction, and the up as the y-axis direction to construct an X*Y global occupancy grid map, where X is the length of the map and Y is the width of the map. The resolution is d*d, and d is the side length of the grid. Among them, the position, size, and resolution of the global occupancy grid map are all preset, and the heading angle starts from the horizontal axis and is positive counterclockwise. It remains unchanged throughout the planning process.

[0063] Since the range of the global occupancy grid map exceeds the range of the vehicle's own perception system, a real-time local occupancy grid map within a certain range around the vehicle is constructed, projected onto the global occupancy grid map, and continuously updated;

[0064] The entire environmental perception obtains obstacle information around the vehicle through sensors such as lidar, millimeter-wave radar, cameras, GPS, and IMU installed on the unmanned vehicle, and uses laser SLAM technology to establish a local occupancy grid map of size Xp*Yp in real time, where Xp is the length of the local occupancy grid map in the vehicle's forward direction, and Yp is the length of the local occupancy grid map in the direction perpendicular to Xp. After matching and correction, the local occupancy grid map is mapped into the global occupancy grid map through coordinate transformation.

[0065] S2. Based on the constructed global occupancy grid map, perform path planning using Hybrid Astar to obtain the initial planned path;

[0066] In this embodiment, given the known task start point s, end point e, and the global occupancy grid map, HybridA* can be used for path planning. When the distance between the expanded node and the end point is relatively close, it is considered that the end point has been reached. When using Reeds_Sheep to reach the end point, this embodiment requires that the Reeds_Sheep waypoints must also be included in the global occupancy grid map, without crossing the boundary or touching obstacles, otherwise continue to use Hybrid A* for path search.

[0067] The general process of the Hybrid A* algorithm is as follows:

[0068] Mark the start point as in the open state and add it to the open set;

[0069] When the open set is not empty, continue to the next step, otherwise return to the start point;

[0070] Take the point nPred with the smallest F value from the open set and start processing;

[0071] If nPred is in the open state, it means it is an expandable point, so continue, otherwise return to this point;

[0072] Mark the state of nPred as closed and remove it from the open set;

[0073] Check whether this point is the target point. If so, directly return this node nPred, indicating that the search is over; otherwise, continue the search.

[0074] Give priority to using Reeds_Sheep to hit the target point. If the Reeds_Sheep method can directly hit and the Reeds_Sheep waypoints are not out of bounds and there are no obstacles, there is no need to enter the Hybrid A* search, and the result nSucc is directly returned.

[0075] If Reeds_Sheep does not hit, create the next expansion node. When reversing is possible, there are 6 possibilities, such as Figure 2 shown in the figure, which shows the schematic diagrams of the 6 expanded directions and is iteratively queried in sequence;

[0076] According to the number of the iteratively queried direction, calculate the pose value of the expandable node nSucc this time, that is, according to the turning radius, the preset changes in X, Y, and heading angle on each maneuver, so as to obtain the specific pose of nSucc;

[0077] If the expansion node is within the global occupancy grid map range and does not collide, calculate its F, G, and H values and add them to the open set. If the parent node is updated, the G and F values of the child node need to be recalculated. Otherwise, discard nSucc;

[0078] Backtrack the path, starting from the end point, connect the parent nodes of each node to obtain the initial path planned by Hybrid A*.

[0079] Preset the turning radius to 6 and the front wheel rotation angle to 6.67 degrees (DEG). Then, in each maneuver situation, the changes in the horizontal, vertical, and heading directions are as follows:

[0080] dx = {0.7068582, 0.705224, 0.705224};

[0081] dy = {0, -0.0415893, 0.0415893};

[0082] dt = {0, 0.1178097, -0.11780097}.

[0083] Then the specific pose calculation model is as follows:

[0084] Forward:

[0085] xSucc = x + dx[i] * cos(t) - dy[i] * sin(t); ySucc = y + dx[i] * sin(t) - dy[i] * cos(t); tSucc = t + dt[i]; i ∈ {1, 2, 3}, specifically for Figure 2 the 3 forward maneuver directions in.

[0086] Backward:

[0087] xSucc = x - dx[i - 3] * cos(t) - dy[i - 3] * sin(t); ySucc = y - dx[i - 3] * sin(t) + dy[i - 3] * cos(t); tSucc = t - dt[i - 3]; i ∈ {4, 5, 6}, specifically Figure 2 for the 3 backward maneuver directions in Figure 2 . Among them, x, y, and t are the pose information of nPred, and after obtaining tSucc, it needs to be converted to radians for further calculation.

[0088] The H value consists of three terms:

[0089] 1. The kinematic - constraint - free heuristic value, represented by max(Reeds_Sheep distance, Euclidean distance);

[0090] 2. The obstacle - constraint heuristic value. When U - turns and dead - ends are detected, A* is used for planning to obtain the G value. Take the larger value of 1 and 2 as the H value of this point;

[0091] The G value has a penalty penaltyTurning when the vehicle direction changes, and a penalty penaltyReversing when the direction is opposite. Both can be satisfied simultaneously.

[0092] The specific model is g = g + k × dx[0];

[0093] Among them, dx[0] is a preset value. When there is a turn, k × penaltyTurning(1.05), when the direction is opposite, k × penaltyReversing(2.0), and when the direction remains unchanged, k = 1. The F value is the sum of the H value and the G value, and the cost value is the F value.

[0094] S3. Expand the waypoints of the obtained initial path to the obstacles to obtain multiple rectangular spaces;

[0095] In this embodiment, the specific steps for obtaining multiple rectangular spaces include:

[0096] S31. Traverse all the waypoints of the initial path in sequence:

[0097] Initialize the four corners P1, P2, P3, and P4 of the rectangular space box_now according to the coordinates of each waypoint, as Figure 3 shown. The figure shows the distribution schematic diagram of the four corner nodes of the rectangular space. Determine whether the waypoint is within box_last. If it is, skip this point; otherwise, execute the next step;

[0098] S31. Expand the initialized rectangular space box_now:

[0099] First, determine whether there is an obstacle at the current waypoint coordinates. If there is, skip this waypoint. Initialize the maximum inflation times max_inflate_iter to 1000. P2 expands upward and to the right, and P3 expands downward and to the left. Within the inflation times, for each unit increase in the ordinate of P2, check from the abscissa of P3 to the abscissa of P2 to see if there is an obstacle collision or if it exceeds the global occupancy grid map range. If so, decrease the ordinate of P2 by one unit and pause the inflation of the ordinate of P2. For each unit right shift of the abscissa of P2, check from the ordinate of P3 to the ordinate of P2 to see if there is an obstacle collision or out-of-bounds. If there is, shift the abscissa of P2 one unit to the left and pause the inflation of the abscissa of P2. For each unit decrease in the ordinate of P3, check from the abscissa of P3 to the abscissa of P2. If there is an out-of-bounds or obstacle collision, increase the ordinate of P3 by one unit and pause the inflation of the ordinate of P3. For each unit left shift of the abscissa of P3, check from the ordinate of P3 to the ordinate of P2. If the grid point is out-of-bounds or has an obstacle collision, shift the abscissa of P3 one unit to the right and pause the inflation of the abscissa of P3. Assign the corresponding abscissas and ordinates of P2 and P3 to P1 and P4 respectively to form a rectangular space corresponding to the waypoint.

[0100] S4. Simplify the quantity of the obtained multiple rectangular spaces and form a space corridor. The specific steps include:

[0101] S41. Preset box_last before iteratively inflating box_now in step S3. Initialize the four corner points P1, P2, P3, and P4 to (0, 0). If box_last and box_now have the same waypoints only in the intersection part and there are no unique waypoints in the non-intersection part, it means that one rectangular space covers another, and the covered part can be deleted.

[0102] That is, set a flag. Traverse the waypoints of the entire initial path. If a certain point or a certain segment of points is inside box_last and outside box_now, and there is a certain point or a certain segment of points inside box_now and outside box_last, then set the flag to 0. This indicates that both box_last and box_now have unique waypoints in their respective rectangular spaces, so both need to be retained;

[0103] If a certain point or a certain segment of points is within box_last and box_now, and there are no other unique points within box_now, then flag is set to 1, and box_now is not added to box_list this time; if a certain point or a certain segment of points is within box_last and box_now, and there are no unique path points in box_last, then flag is set to 2, and at this time, box_last is deleted and box_now is retained. After S4 ends, box_last will be judged and updated as needed. If box_now is retained this time, then box_last is updated to box_now.

[0104] S42. Simplify the box_list set. Initialize the space set temp as box_list, record the first rectangular space in box_list as old, clear box_list, and traverse the entire temp set. If this rectangular space now does not completely coincide with the previous rectangular space old, then add the previous rectangular space old to the box_list set, and assign this space now to the previous rectangular space old. When the rectangular space old intersects with the last rectangular space in temp, exit the traversal and insert the last rectangular space into box_list. At this time, box_list is the final available space corridor.

[0105] S5. Perform quadratic programming on the initial path within the space corridor through the optimal control model to obtain the optimized path. The specific steps include:

[0106] S51. Establish a kinematic model:

[0107] Since tire side slip is ignored in the low-speed environment, the vehicle kinematics is modeled based on the bicycle model:

[0108]

[0109] Among them, the end point of the vehicle's rear axle is P0(x0, y0), θ0 represents the heading angle, v0 represents the speed of P0, a0 represents the linear acceleration, is the front wheel rotation angle, w0 is the front wheel angular velocity, L w0 represents the length of the vehicle's rear axle.

[0110] When t ∈ [0, t f , the state variable boundaries at the moment are:

[0111] |a0(t)| ≤ a max ,

[0112] |v0(t)| ≤ v max ,

[0113]

[0114] |w0(t)| ≤ Ω max ,

[0115] where a max , v max , Ω max represent the upper bounds of each variable, respectively.

[0116] S52, Boundary constraints:

[0117] At the initial state t = 0, each variable of x0(0), y0(0), v0(0), θ0(0), a0(0), w0(0) can be truly obtained by the vehicle's perception system. At the termination time t = t f , the entire vehicle system needs to stop stably, so it is required that:

[0118] [v0(t f ), a0(t f ), w0(t f )] = [0, 0, 0]; (2)

[0119] S53, Spatial corridor constraints:

[0120] If the initial path has N fe waypoints, then each waypoint P c (x c (t), y c (t)) should be within the corridor, that is, the constraint:

[0121]

[0122] where represents the abscissa of the P3 point of the rectangular space corresponding to the waypoint, represents the abscissa of the P2 point of the rectangular space corresponding to the waypoint, represents the ordinate of the P3 point of the rectangular space corresponding to the waypoint, represents the ordinate of the P2 point of the rectangular space corresponding to the waypoint.

[0123] S54, Cost function:

[0124] The cost function is:

[0125]

[0126] where w1, w2 ≥ 0 are cost functions, t fThe first term represents the travel time of the vehicle from the starting point to the ending point, the second term describes the change in vehicle acceleration, and the third term describes the change in angular velocity. That is, the planned path is required to be safe, time-consuming, and comfortable.

[0127] S55. Optimal control model:

[0128] According to formulas (1), (2), (3), and (4), an optimal control model for describing the path planning task of the entire vehicle system is obtained.

[0129] Finally, based on using the first-order explicit Runge-Kutta and the interior point method to solve this NLP problem, an optimized path that is safe and unobstructed within the spatial corridor is obtained.

[0130] Although the embodiments of the present invention have been shown and described, for those of ordinary skill in the art, it can be understood that various changes, modifications, substitutions, and variations can be made to these embodiments without departing from the principles and spirit of the present invention. The scope of the present invention is defined by the appended claims and their equivalents, and all should be included within the protection scope of the present invention.

Claims

1. A path planning method based on Hybrid Astar and spatial corridor, characterized in that The specific steps include: Construct a global occupancy grid map in a multi-obstacle environment, and set the coordinates of the starting point and the ending point of the task; Based on the constructed global occupancy grid map, perform path planning using Hybrid Astar to obtain the initial planned path; Inflate the path points of the obtained initial path to the obstacles to obtain multiple rectangular spaces; Simplify the quantity of the obtained multiple rectangular spaces and form a space corridor; Perform quadratic programming on the initial path within the space corridor through an optimal control model to obtain the optimized path. The specific steps include: Step 1: Establish a kinematic model: First, model the vehicle kinematics based on the bicycle model: ； Among them, the end point of the vehicle's rear axle is , represents the heading angle, represents the speed of represents the linear acceleration, is the front wheel rotation angle, is the front wheel angular velocity, represents the vehicle's rear axle length; When the state variable boundary at the moment is: ； ； ； ； Among them, , , respectively represent the upper bounds of each variable; Step 2: Set boundary constraints: According to the perception system of the vehicle, at the initial state t = 0, , , , and the information of each variable; At the termination moment , the entire vehicle system stops stably, and the formula is obtained as follows: ； Step 3: Set space corridor constraints: The initial path contains waypoints, and each waypoint ( , ) should be restricted within the spatial corridor, and the formula is: ； Among them, among them represents the abscissa of point P3 in the rectangular space corresponding to the waypoint, represents the abscissa of point P2 in the rectangular space corresponding to the waypoint, represents the ordinate of point P3 in the rectangular space corresponding to the waypoint, represents the ordinate of point P2 in the rectangular space corresponding to the waypoint; Step 4: Obtain the cost function: The cost function is as follows: ; Among them, is the cost function, the first term represents the travel time of the vehicle from the starting point to the end point, the second term represents the change in vehicle acceleration, and the third term represents the change in angular velocity; Step 5: Combine Steps 1 to 4 to obtain the optimal control model; Then, solve the model based on the first-order explicit Runge-Kutta and the interior point method to obtain a safe and obstacle-free optimal path restricted within the space corridor.

2. The path planning method based on Hybrid Astar and spatial corridor according to claim 1, characterized in that, The specific steps of constructing the global occupancy grid map in a multi-obstacle environment and setting the coordinates of the starting point and the ending point of the task include: Use the positioning device GPS for self-positioning to obtain the starting point coordinates, and obtain the GPS coordinates of the task ending point; At the same time, obtain the obstacles around the current position of the vehicle through the environmental perception system of the autonomous unmanned vehicle to obtain a local occupancy grid map, and continuously update it; Map the local occupancy grid map to the constructed global occupancy grid map through coordinate transformation.

3. The path planning method based on Hybrid Astar and spatial corridor according to claim 1, characterized in that, The specific steps of performing path planning using Hybrid Astar based on the constructed global occupancy grid map to obtain the initial planned path include: Obtain the starting point, the ending point of the task, and the constructed global occupancy grid map; Perform path planning based on Hybrid Astar: When the expanded node is closest to the ending point, it is determined that the ending point has been reached; When using Reeds_Sheep to hit the ending point, ensure that the Reeds_Sheep path points are included within the global occupancy grid map, and cannot cross the boundary or touch the obstacles. Otherwise, continue to use Hybrid Astar for path search.

4. The path planning method based on Hybrid Astar and space corridor according to claim 1, characterized in that, The specific steps of inflating the path points of the obtained initial path to the obstacles to obtain multiple rectangular spaces include: Traverse the path points of the entire initial path in sequence, and initialize the four corner points P1, P2, P3, and P4 of the rectangular space box_now according to the coordinates of each path point; Judge whether there are obstacles at the current path point coordinates. If there are, skip this point, and initialize the maximum inflation times. Within the inflation times, continuously expand the horizontal and vertical coordinates of P2 and P3 to the obstacle boundary to obtain the horizontal and vertical coordinates of P2 and P3; Assign the obtained horizontal and vertical coordinates of P2 and P3 to P1 and P4 to form the rectangular space corresponding to the current path point.

5. The path planning method based on Hybrid Astar and spatial corridor according to claim 4, characterized in that, The specific steps of simplifying the quantity of the obtained multiple rectangular spaces and forming a space corridor include: Preset a rectangular space box_last with four corner points being (0, 0); When the rectangular space box_last and the initialized rectangular space box_now have the same waypoints in the intersection part, and there are no unique waypoints in the non-intersection part, the covered part is deleted.

Images