A static obstacle avoidance method for unmanned vehicles

Abstract

This invention discloses a static obstacle avoidance method for unmanned vehicles, relating to the field of static obstacle avoidance technology. The method includes the following steps: acquiring the vehicle's current pose, static obstacle information, and local endpoint pose; performing curve fitting and collision detection on the vehicle's current pose and local endpoint pose; if the collision detection passes, the optimal obstacle avoidance path is obtained; if the collision detection fails, several next-step vehicle poses are generated based on the vehicle's current pose, considering kinematic constraints; considering two-point boundary value constraints and curvature constraints, curve fitting and collision detection are performed on the vehicle's current pose and each next-step pose; the vehicle's current pose is replaced with the next-step pose corresponding to the optimal curve, and this process is repeated. This application generates the optimal obstacle avoidance path by comprehensively considering kinematic constraints, two-point boundary value constraints, curvature constraints, and collision constraints, taking into account driving efficiency, safety, and adaptability in complex scenarios.

Description

Technical Field

[0001] This application relates to the field of static obstacle avoidance technology, and in particular to a static obstacle avoidance method for unmanned vehicles. Background Technology

[0002] Obstacle avoidance is one of the core problems in autonomous vehicle local path planning. Based on the type of static and dynamic obstacles, autonomous vehicle obstacle avoidance can be divided into static obstacle avoidance and dynamic obstacle avoidance. For static obstacle avoidance, mainstream obstacle avoidance path generation algorithms include: graph search algorithms, sampling search algorithms, curve fitting algorithms, and collision detection algorithms.

[0003] Graph search algorithms first establish a gridded environment map of the driving space, then expand nodes according to a certain cost strategy to search for drivable paths in the gridded environment map. This algorithm focuses on path optimization. Sampling search algorithms attempt to establish a path from the initial state to the target state by connecting a series of sampled nodes in the obstacle-free space. This method avoids explicitly constructing obstacles in the state space, and the feasibility of the trajectory is verified by collision detection, saving significant computational costs. However, the path points planned by these two search algorithms are connected by polylines, ignoring curvature constraints, and the paths cannot be executed by the vehicle. Curve fitting algorithms are used when the drivable area is known. Based on a series of pose feature points in the drivable area, they fit a trajectory that the car can execute. However, this algorithm ignores the actual kinematic characteristics of the vehicle and collision avoidance constraints, resulting in paths that cannot be accurately tracked by the vehicle. Collision detection algorithms discard invalid trajectory segments by checking for collisions on candidate trajectory segments, but this may be too conservative, selecting overly long or complex paths to avoid collisions, reducing driving efficiency. Therefore, research on static obstacle avoidance methods for autonomous vehicles is needed.

[0004] In the prior art, Chinese patent CN117193300A discloses an optimal reference path generation method and system based on road boundary and curvature constraints. Based on lane-level navigation results, it samples the original path points from the lane center, removes road segments with curvature less than a set value, and optimizes the remaining path in segments. It optimizes path safety based on human driving habits, considering the shape of the vehicle body. By initially moving the path points of the initial path, the distance difference between the vehicle body and the road's two side boundaries does not exceed a set value, resulting in a pre-optimized path. Curve fitting is performed on the path points on the pre-optimized path, optimizing the path smoothness within a given minimum error range ε. The number of parameters is reduced and curvature smoothness is ensured through L0 norm optimization, generating a reference path that is easy for vehicles to follow and meets drivability requirements. It can plan a safe and reasonable reference path that satisfies both safety and curvature constraints.

[0005] However, the aforementioned existing technologies only consider safety constraints and curvature constraints, and fail to comprehensively consider kinematic constraints, two-point boundary value constraints, curvature constraints and collision constraints, thus failing to take into account driving efficiency, safety and adaptability in complex scenarios. Summary of the Invention

[0006] This application provides a static obstacle avoidance method for unmanned vehicles to address the problem that existing obstacle avoidance technologies for unmanned vehicles fail to comprehensively consider kinematic constraints, two-point boundary value constraints, curvature constraints, and collision constraints, and thus cannot balance driving efficiency, safety, and adaptability in complex scenarios.

[0007] On the one hand, this application provides a static obstacle avoidance method for unmanned vehicles, including the following steps:

[0008] Step 1: Obtain vehicle physical dynamics information.

[0009] Step 2: Obtain the vehicle's current pose and a static local map of the area to be avoided. Obtain static obstacle information and local endpoint pose from the static local map. The static local map is a gridded static map.

[0010] Step 3: Based on the path generation algorithm, perform curve fitting on the current pose of the vehicle and the local endpoint pose to obtain a first fitted curve. Perform collision detection on the first fitted curve according to the static obstacle information. If the collision detection is successful, the first fitted curve is taken as the optimal obstacle avoidance path. If the collision detection is unsuccessful, proceed to step 4.

[0011] Step four: Based on the current pose of the vehicle, generate several next poses of the vehicle using a path search algorithm while considering the kinematic constraints of the vehicle's physical dynamics information.

[0012] Step 5: Based on the path generation algorithm, considering the two-point boundary value constraints and curvature constraints, curve fitting is performed on the current pose of the vehicle and the next pose of each vehicle to obtain several second fitting curves.

[0013] Step six: Perform collision detection on each of the second fitted curves based on the static obstacle information, select the optimal curve from the second fitted curves that have passed the collision detection based on the path optimization index, replace the current vehicle pose with the next vehicle pose corresponding to the optimal curve, and return to step three for looping.

[0014] In step six, if there is no second fitted curve that passes the collision detection, it means that there is no optimal obstacle avoidance path, and the method terminates.

[0015] In one possible implementation, in step one, the vehicle physical dynamics information includes: vehicle length, vehicle width, vehicle wheelbase, front overhang distance, rear overhang distance, upper limit of vehicle speed, lower limit of vehicle speed, upper limit of front wheel steering angle, lower limit of front wheel steering angle, and minimum turning radius.

[0016] In one possible implementation, in step two, the static obstacle information includes: obstacle shape, obstacle location, and obstacle size.

[0017] The step of obtaining static obstacle information from the static local map includes:

[0018] The shapes of obstacles in the static local map are abstracted into circles or rectangles by using circular or rectangular frames to enclose them.

[0019] If the shape of the obstacle is abstracted as a circle, then the location of the obstacle is the latitude and longitude of the center of the circle, and the size of the obstacle is the radius of the circle.

[0020] If the shape of an obstacle is abstracted as a rectangle, then the location of the obstacle is the latitude and longitude of the bottom left vertex of the rectangle, and the size of the obstacle is the length and width of the rectangle.

[0021] In one possible implementation, step three includes:

[0022] Step S31: The pose information corresponding to each small grid in the gridded static map is called node pose information and is numbered.

[0023] Step S32: Use a priority queue to store the node pose information and initialize the priority queue so that the priority queue only contains the current pose of the vehicle.

[0024] Step S33: Dequeue the element nodes of the priority queue.

[0025] Step S34: Perform curve fitting on the poses of the outgoing node and the local endpoint to obtain the first fitted curve.

[0026] Step S35: Perform collision detection on the first fitted curve based on the static obstacle information.

[0027] Step S36: If the first fitted curve passes the collision detection, then the first fitted curve is taken as the optimal obstacle avoidance path; if it fails the collision detection, then proceed to step four.

[0028] In one possible implementation, step S35 includes:

[0029] Step S351: Divide the first fitted curve into a straight line segment and a curved segment.

[0030] Step S352: Enclose the vehicle using a rectangle.

[0031] Step S353: For the straight line segment of the first fitted curve, if the obstacle is a rectangle, collision detection is performed using the directional bounding box method; if the obstacle is a circle, collision detection is performed using the improved circumcircle method. For the curve segment of the first fitted curve, the curve segment is regarded as an annular sector, and it is determined whether the obstacle is within the annular sector or whether the obstacle intersects with the annular sector. If either result is yes, it means that the vehicle and the obstacle will collide.

[0032] In one possible implementation, step four includes:

[0033] Step S41: Use the current vehicle pose as input to the vehicle kinematics equation.

[0034] Step S42: Considering the kinematic constraints of the vehicle physical dynamics information, different values ​​of the vehicle physical dynamics information are taken and substituted into the vehicle kinematic equations to obtain several vehicle poses for the next step.

[0035] In one possible implementation, step five includes:

[0036] Step S51: Traverse the vehicle's next pose and generate several edge value pairs, each edge value pair including the vehicle's current pose and the vehicle's next pose.

[0037] Step S52: Use a curve fitting algorithm with continuous curvature to fit curves to each pair of boundary values, and obtain several second fitting curves.

[0038] In one possible implementation, step six includes:

[0039] Step S61: Perform collision detection on each of the second fitted curves based on the static obstacle information, and record the edge value pairs corresponding to the second fitted curves that pass the collision detection.

[0040] Step S62: For the next vehicle pose corresponding to the boundary value pair of the second fitted curve that passed the collision detection, those that have not been enqueued are added to the priority queue, and those that have been enqueued but have not yet been dequeued are updated in terms of cost. Only the vehicle pose with the lowest total cost is retained in the priority queue.

[0041] Step S63: If the priority queue is not empty, replace the current vehicle pose with the next vehicle pose retained in the priority queue, and proceed to step three for looping.

[0042] In one possible implementation, step S62, where the cost update for items that have been enqueued but not yet dequeued includes:

[0043] Step S621: For each vehicle that has been enqueued but not yet dequeued, calculate the cost from the initial vehicle pose to the next vehicle pose.

[0044] Step S622: Update the heuristic function to obtain the cost of the vehicle's next pose to the local endpoint pose.

[0045] Step S623, update the total cost.

[0046] The static obstacle avoidance method for unmanned vehicles in this application has the following advantages:

[0047] By comprehensively considering kinematic constraints, two-point boundary value constraints, curvature constraints, and collision constraints, the optimal obstacle avoidance path is generated, which balances driving efficiency and safety in complex scenarios. At the same time, it can cope with changes in different road scenarios and vehicle kinematic characteristics, thus improving adaptability. Attached Figure Description

[0048] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0049] Figure 1 A flowchart illustrating a static obstacle avoidance method for an unmanned vehicle provided in an embodiment of this application;

[0050] Figure 2 A schematic diagram of the driving path of the straight line segment of the first fitted curve provided in the embodiments of this application;

[0051] Figure 3 A schematic diagram of the driving path of the curve segment of the first fitted curve provided in the embodiments of this application. Detailed Implementation

[0052] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0053] like Figure 1 As shown in the figure, this application provides a static obstacle avoidance method for unmanned vehicles, including the following steps:

[0054] Step 1: Obtain vehicle physical dynamics information.

[0055] Step 2: Obtain the vehicle's current pose and a static local map of the area to be avoided. Obtain static obstacle information and local endpoint pose from the static local map. The static local map is a gridded static map.

[0056] Step 3: Based on the path generation algorithm, perform curve fitting on the current pose of the vehicle and the local endpoint pose to obtain a first fitted curve. Perform collision detection on the first fitted curve according to the static obstacle information. If the collision detection is successful, the first fitted curve is taken as the optimal obstacle avoidance path. If the collision detection is unsuccessful, proceed to step 4.

[0057] Step four: Based on the current pose of the vehicle, generate several next poses of the vehicle using a path search algorithm while considering the kinematic constraints of the vehicle's physical dynamics information.

[0058] Step 5: Based on the path generation algorithm, considering the two-point boundary value constraints and curvature constraints, curve fitting is performed on the current pose of the vehicle and the next pose of each vehicle to obtain several second fitting curves.

[0059] Step six: Perform collision detection on each of the second fitted curves based on the static obstacle information, select the optimal curve from the second fitted curves that have passed the collision detection based on the path optimization index, replace the current vehicle pose with the next vehicle pose corresponding to the optimal curve, and return to step three for looping.

[0060] In step six, if there is no second fitted curve that passes the collision detection, it means that there is no optimal obstacle avoidance path, and the method terminates.

[0061] For example, in step one, the vehicle physical dynamics information includes: vehicle length, vehicle width, vehicle wheelbase, front overhang distance, rear overhang distance, upper limit of vehicle speed, lower limit of vehicle speed, upper limit of front wheel steering angle, lower limit of front wheel steering angle, and minimum turning radius.

[0062] For example, in step two, the static obstacle information includes: obstacle shape, obstacle location, and obstacle size.

[0063] The step of obtaining static obstacle information from the static local map includes:

[0064] The shapes of obstacles in the static local map are abstracted into circles or rectangles by using circular or rectangular frames to enclose them.

[0065] If the shape of the obstacle is abstracted as a circle, then the location of the obstacle is the latitude and longitude of the center of the circle, and the size of the obstacle is the radius of the circle.

[0066] If the shape of an obstacle is abstracted as a rectangle, then the location of the obstacle is the latitude and longitude of the bottom left vertex of the rectangle, and the size of the obstacle is the length and width of the rectangle.

[0067] Specifically, the vehicle's perception range is mainly determined by the sensors equipped on the vehicle, generally between 100 and 200 meters. However, in order to achieve real-time and optimal obstacle avoidance, the obstacle avoidance area selected during planning is generally within 20 meters in front of the vehicle, rather than the entire perception range. In this embodiment, the static local map adopts a gridded static map within 20 meters in front of the vehicle.

[0068] In this embodiment, the current vehicle pose is denoted as [x now ,y now ,θ now ], where x now ,y now θ represents the longitude and latitude of the current rear axle endpoint of the vehicle, respectively. now The heading angle represents the vehicle's current heading angle, i.e., the angle from the positive X-axis to the positive longitudinal axis, with counterclockwise rotation being positive; the local endpoint pose is denoted as [x local_end ,y local_end ,θ local_end ], where x local_end ,y local_end θ represents the longitude and latitude of the rear axle endpoint, which is the local endpoint of the vehicle. local_end The heading angle represents the local endpoint of the vehicle, which is the angle from the positive X-axis direction to the positive longitudinal axis direction of the local endpoint of the vehicle, with counterclockwise rotation being positive.

[0069] Specifically, in this embodiment, the static obstacle information of the circular obstacle is denoted as [x 圆 ,y 圆 ,r 圆 ], where x 圆 ,y 圆 These represent the latitude and longitude of the center of the circular frame, r and r respectively. 圆 The radius of the circular frame is represented by [x]. Static obstacle information for rectangular obstacles is denoted as [x]. 方 ,y 方 ,l 方 ,w 方 ], where x 方 ,y 方 These represent the latitude and longitude of the bottom left vertex of the rectangle, l 方 ,w 方 These represent the length and width of the rectangle, respectively.

[0070] For example, step three includes:

[0071] Step S31: The pose information corresponding to each small grid in the gridded static map is called node pose information and is numbered.

[0072] Step S32: Use a priority queue to store the node pose information and initialize the priority queue so that the priority queue only contains the current pose of the vehicle.

[0073] Step S33: Dequeue the element nodes of the priority queue.

[0074] Step S34: Perform curve fitting on the poses of the outgoing node and the local endpoint to obtain the first fitted curve.

[0075] Step S35: Perform collision detection on the first fitted curve based on the static obstacle information.

[0076] Step S36: If the first fitted curve passes the collision detection, then the first fitted curve is taken as the optimal obstacle avoidance path; if it fails the collision detection, then proceed to step four.

[0077] Specifically, in this embodiment, the curve fitting in step S34 adopts the Dubins curve fitting method. In other possible embodiments, the Reeds_Shepp or Bezier curve fitting methods may also be used.

[0078] For example, step S35 includes:

[0079] Step S351: Divide the first fitted curve into a straight line segment and a curved segment.

[0080] Step S352: Enclose the vehicle using a rectangle.

[0081] Step S353: For the straight line segment of the first fitted curve, if the obstacle is a rectangle, collision detection is performed using the directional bounding box method; if the obstacle is a circle, collision detection is performed using the improved circumcircle method. For the curve segment of the first fitted curve, the curve segment is regarded as an annular sector, and it is determined whether the obstacle is within the annular sector or whether the obstacle intersects with the annular sector. If either result is yes, it means that the vehicle and the obstacle will collide.

[0082] Specifically, such as Figure 2As shown, the straight line segment of the first fitted curve is EFCD. If the obstacle is rectangular, collision detection is performed using the oriented bounding box method. The oriented bounding box method includes: using the separating axis theorem to determine whether the vehicle and the obstacle enclosed by the rectangular frame intersect. If there exists a straight line (separating axis) that separates the two, they do not intersect, thus avoiding a collision. If the obstacle is circular, collision detection is performed using the improved circumcircle method. The improved circumcircle method includes: first, using the triangle area method to determine whether the center of the circular frame falls within the rectangular frame enclosing the vehicle. If so, the circular frame and the rectangular frame intersect, and the vehicle and the obstacle collide. If not, then using the criterion of whether the distance from the center to the straight line is less than the radius of the circle, it checks whether each side of the rectangular frame enclosing the vehicle intersects with the circular frame. If any side intersects with the circular frame, the vehicle and the obstacle collide; otherwise, no collision occurs.

[0083] Specifically, such as Figure 3 As shown, the curve segment of the first fitted curve is HGBA, which can be considered as a circular sector. If the obstacle is rectangular, it is determined whether the rectangle is within the circular sector or whether the rectangle intersects with the circular sector. If so, the vehicle and the obstacle will collide. If the obstacle is circular, it is determined whether the distance from the center of the circle to the center of the circular sector is greater than the outer radius of the sector plus the radius of the circle or less than the inner radius of the sector minus the radius of the circle. If so, the circle and the circular sector do not intersect, and the circle is not within the circular sector, so the vehicle and the obstacle will not collide.

[0084] For example, step four includes:

[0085] Step S41: Use the current vehicle pose as input to the vehicle kinematics equation.

[0086] Step S42: Considering the kinematic constraints of the vehicle physical dynamics information, different values ​​of the vehicle physical dynamics information are taken and substituted into the vehicle kinematic equations to obtain several vehicle poses for the next step.

[0087] Specifically, in this embodiment, in step S41, the vehicle kinematic equations are as follows:

[0088]

[0089] Where [x(t),y(t),θ(t)] represents the vehicle's pose at time t, [x0,y0,θ0] represents the initial vehicle pose, and v represents the vehicle's velocity. θ represents the front wheel steering angle, with leftward deviation being positive; θ represents the vehicle heading angle, which is the angle from the positive X-axis to the positive longitudinal axis, with counterclockwise rotation being positive; L represents the front wheel steering angle, with leftward deviation being positive. m t represents the vehicle's wheelbase, and t represents time.

[0090] In this embodiment, in step S42, the control quantities in the vehicle physical dynamics information—vehicle speed and front wheel steering angle—are assigned different values, as follows:

[0091]

[0092] Among them, v min ,v max These represent the lower limit and upper limit of vehicle speed, respectively. These represent the lower limit and upper limit of the front wheel steering angle, respectively.

[0093] For example, step five includes:

[0094] Step S51: Traverse the vehicle's next pose and generate several edge value pairs, each edge value pair including the vehicle's current pose and the vehicle's next pose.

[0095] Step S52: Use a curve fitting algorithm with continuous curvature to fit curves to each pair of boundary values, and obtain several second fitting curves.

[0096] Specifically, the curve fitting in step S52 uses the Dubins curve fitting method. In other possible embodiments, the Reeds_Shepp or Bezier curve fitting methods may also be used.

[0097] For example, step six includes:

[0098] Step S61: Perform collision detection on each of the second fitted curves based on the static obstacle information, and record the edge value pairs corresponding to the second fitted curves that pass the collision detection.

[0099] Step S62: For the next vehicle pose corresponding to the boundary value pair of the second fitted curve that passed the collision detection, those that have not been enqueued are added to the priority queue, and those that have been enqueued but have not yet been dequeued are updated in terms of cost. Only the vehicle pose with the lowest total cost is retained in the priority queue.

[0100] Step S63: If the priority queue is not empty, replace the current vehicle pose with the next vehicle pose retained in the priority queue, and proceed to step three for looping.

[0101] For example, in step S62, updating the cost of items that have been enqueued but not yet dequeued includes:

[0102] Step S621: For each vehicle that has been enqueued but not yet dequeued, calculate the cost from the initial vehicle pose to the next vehicle pose.

[0103] Step S622: Update the heuristic function to obtain the cost of the vehicle's next pose to the local endpoint pose.

[0104] Step S623, update the total cost.

[0105] Specifically, in this embodiment, in step S621, the node of the next pose of a vehicle that has been enqueued but not yet dequeued is set as n′, and the cost g from the initial vehicle pose to the next vehicle pose is... new (n′) is shown in the following formula:

[0106] g new (n′)=g(n)+c(n,n′).

[0107] Where g(n) represents the movement cost from the initial vehicle pose to node n of the current vehicle pose, and c(n,n′) represents the movement cost from node n of the current vehicle pose to node n′ of the next vehicle pose.

[0108] In step S622, the heuristic function is set as h(n′), which represents the cost from node n′ of the vehicle's next pose to the local endpoint pose.

[0109] In step S623, the total cost f(n′) is as follows:

[0110] f(n′=g new (n′)+h(n′).

[0111] The embodiments of this application generate the optimal obstacle avoidance path by comprehensively considering kinematic constraints, two-point boundary value constraints, curvature constraints and collision constraints, which takes into account driving efficiency and safety in complex scenarios, and can cope with changes in different road scenarios and vehicle kinematic characteristics, thus improving adaptability.

[0112] Although the preferred embodiments of the present application have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present application.

[0113] Obviously, those skilled in the art can make various modifications and variations to this application without departing from the spirit and scope of this application. Therefore, if such modifications and variations fall within the scope of the claims of this application and their equivalents, this application also intends to include such modifications and variations.

Claims

1. A static obstacle avoidance method for unmanned vehicles, characterized in that, Includes the following steps: Step 1: Obtain vehicle physical dynamics information; Step 2: Obtain the vehicle's current pose and a static local map of the area to be avoided; obtain static obstacle information and local endpoint pose from the static local map; the static local map is a gridded static map. Step 3: Based on the path generation algorithm, perform curve fitting on the current pose of the vehicle and the local endpoint pose to obtain a first fitted curve. Perform collision detection on the first fitted curve according to the static obstacle information. If the collision detection is successful, the first fitted curve is taken as the optimal obstacle avoidance path. If the collision detection is unsuccessful, proceed to step 4. Step 4: Based on the current pose of the vehicle, generate several next poses of the vehicle using a path search algorithm while considering the kinematic constraints of the vehicle's physical dynamics information. Step 5: Based on the path generation algorithm, considering the two-point boundary value constraints and curvature constraints, perform curve fitting on the current pose of the vehicle and the next pose of each vehicle to obtain several second fitting curves. Step 6: Perform collision detection on each of the second fitted curves based on the static obstacle information. Select the optimal curve from the second fitted curves that have passed the collision detection based on the path selection index. Replace the current vehicle pose with the next vehicle pose corresponding to the optimal curve. Then proceed to step 3 for looping. In step six, if there is no second fitted curve that passes the collision detection, it means that there is no optimal obstacle avoidance path, and the method terminates.

2. The static obstacle avoidance method for unmanned vehicles according to claim 1, characterized in that, In step one, the vehicle physical dynamics information includes: vehicle length, vehicle width, vehicle wheelbase, front overhang distance, rear overhang distance, upper limit of vehicle speed, lower limit of vehicle speed, upper limit of front wheel steering angle, lower limit of front wheel steering angle, and minimum turning radius.

3. The static obstacle avoidance method for unmanned vehicles according to claim 1, characterized in that, In step two, the static obstacle information includes: obstacle shape, obstacle location, and obstacle size; The step of obtaining static obstacle information from the static local map includes: The shapes of obstacles in the static local map are abstracted into circles or rectangles by using circular or rectangular frames to enclose them. If the shape of the obstacle is abstracted as a circle, then the position of the obstacle is the latitude and longitude of the center of the circle, and the size of the obstacle is the radius of the circle. If the shape of an obstacle is abstracted as a rectangle, then the location of the obstacle is the latitude and longitude of the bottom left vertex of the rectangle, and the size of the obstacle is the length and width of the rectangle.

4. The static obstacle avoidance method for unmanned vehicles according to claim 1, characterized in that, Step three includes: Step S31: The pose information corresponding to each small grid in the gridded static map is called node pose information and is numbered. Step S32: Use a priority queue to store the node pose information and initialize the priority queue so that the priority queue only contains the current pose of the vehicle; Step S33: Dequeue the element nodes of the priority queue; Step S34: Perform curve fitting on the pose of the outgoing node and the local endpoint to obtain the first fitted curve; Step S35: Perform collision detection on the first fitted curve based on the static obstacle information; Step S36: If the first fitted curve passes the collision detection, then the first fitted curve is taken as the optimal obstacle avoidance path; if it fails the collision detection, then proceed to step four.

5. The static obstacle avoidance method for unmanned vehicles according to claim 4, characterized in that, Step S35 includes: Step S351: Divide the first fitted curve into a straight line segment and a curve segment; Step S352: Enclose the vehicle using a rectangle; Step S353: For the straight line segment of the first fitted curve, if the obstacle is a rectangle, collision detection is performed using the directional bounding box method; if the obstacle is a circle, collision detection is performed using the improved circumcircle method. For the curve segment of the first fitted curve, the curve segment is regarded as an annular sector, and it is determined whether the obstacle is within the annular sector or whether the obstacle intersects with the annular sector. If either result is yes, it means that the vehicle and the obstacle will collide.

6. The static obstacle avoidance method for unmanned vehicles according to claim 1, characterized in that, Step four includes: Step S41: Use the current vehicle pose as the input to the vehicle kinematics equations; Step S42: Considering the kinematic constraints of the vehicle physical dynamics information, different values ​​of the vehicle physical dynamics information are taken and substituted into the vehicle kinematic equations to obtain several vehicle poses for the next step.

7. The static obstacle avoidance method for unmanned vehicles according to claim 1, characterized in that, Step five includes: Step S51: Traverse the next pose of the vehicle and generate several edge value pairs, each edge value pair including the current pose of the vehicle and the next pose of the vehicle. Step S52: Use a curve fitting algorithm with continuous curvature to fit curves to each pair of boundary values, and obtain several second fitting curves.

8. The static obstacle avoidance method for unmanned vehicles according to claim 1, characterized in that, Step six includes: Step S61: Perform collision detection on each of the second fitted curves based on the static obstacle information, and record the edge value pairs corresponding to the second fitted curves that pass the collision detection. Step S62: For the next vehicle pose corresponding to the boundary value pair of the second fitted curve that passed the collision detection, those that have not been enqueued are put into the priority queue, and those that have been enqueued but have not yet been dequeued are updated in terms of cost. Only the next vehicle pose with the lowest total cost is retained in the priority queue. Step S63: If the priority queue is not empty, replace the current vehicle pose with the next vehicle pose retained in the priority queue, and proceed to step three for looping.

9. A static obstacle avoidance method for unmanned vehicles according to claim 8, characterized in that, In step S62, updating the cost for items that have been enqueued but not yet dequeued includes: Step S621: For each vehicle that has been enqueued but not yet dequeued, calculate the cost from the initial vehicle pose to the next vehicle pose. Step S622: Update the heuristic function to obtain the cost of the vehicle's next pose to the local endpoint pose; Step S623, update the total cost.

Images