Route planning method based on special unmanned vehicle unstructured road and computer system

Abstract

The invention discloses a path planning method based on a special unmanned vehicle under an unstructured road, and the method comprises the steps: calculating the comprehensive cost of each node uniformly distributed in a space through dynamic planning logic according to a grid map containing dynamic and static obstacle information in a sensing range, and generating a preliminary path; and performing nonlinear optimization through a high-performance nonlinear programming solver in combination with the explored space corridor and a self-vehicle kinematics equation to generate a path for safely arriving at an expected target pose. The method has the advantages that the feasibility, comfort and safety of the path are ensured through dynamic planning logic and nonlinear optimization.

Description

Technical Field

[0001] The present invention relates to the field of unmanned driving technology, and in particular to a path planning method and a computer system based on a special unmanned vehicle on an unstructured road. Background Art

[0002] In the development of unmanned driving technology, high-precision Figure 1 High-precision maps are considered to be the basis for reliable navigation. However, with the continuous advancement of technology and the diversification of application scenarios, the limitations of high-precision maps have gradually become apparent, especially in the field of special operations. The application of high-precision maps in unstructured road environments faces many challenges, such as the limitations of driving scenarios, strict regulatory restrictions, and high resource consumption. Therefore, in the field of special operations, real-time dynamic mapping technology has become an inevitable choice.

[0003] Path planning is the core link in autonomous driving technology, which determines how unmanned vehicles can reach the target point safely and efficiently from the starting point. In the field of passenger car autonomous driving, path planning usually focuses on finding the shortest or fastest path. However, in the field of special operations, path planning needs to meet specific operational requirements. For example, in urban road cleaning tasks, the ideal path should be as close to the edge of the road as possible to ensure the cleaning effect. This path planning requirement contradicts the principle of avoiding obstacles in traditional algorithms, so it is necessary to develop a path planning method specifically for the characteristics of special operations.

[0004] Most existing path planning algorithms are designed based on structured road environments and are difficult to directly apply to special operations on unstructured roads. In these environments, vehicles need to intelligently select target points and plan appropriate paths based on changes in work tasks, work objects, and work environments without obvious lane lines and navigation maps. In addition, the vehicle must also consider its own kinematic constraints to ensure that the generated path is both physically feasible and meets the work requirements. This is where this application needs to focus on improvement. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide a path planning method and computer system based on unstructured roads for special unmanned vehicles, aiming to solve the special needs of path planning in the field of special operations and improve the operating efficiency and safety of unmanned vehicles in complex environments.

[0006] In order to solve the above technical problems, the present invention provides a path planning method based on unstructured roads for special unmanned vehicles. According to the grid map containing the information of dynamic and static obstacles within the perception range, the comprehensive cost uniformly distributed at each node in the space is calculated through dynamic programming logic, and a preliminary path is generated. Combining the explored spatial corridor and the kinematic equation of the vehicle, a high-performance nonlinear programming solver is used to perform nonlinear optimization to generate a path that safely reaches the expected target posture; specifically, the method comprises the following steps: Step S1, configuration and initialization: subscribe to and receive ROS topic data, configure the start and end point constraints, detect whether the path has a collision, if yes, initialize, set dynamic planning accessibility, passability and comfort related parameters; if no, wait for topic data update; Step S2, rough path generation: dynamically plan the driving path according to the terminal position sent by the upper layer; Step S3, path optimization: Based on the rough path, the optimization problem is constructed by combining the grid map containing dynamic and static obstacle information with the vehicle kinematic equation, and the solution is iteratively solved to optimize the path; The planned path takes obstacles into consideration, ensuring path safety, avoiding collisions and dangers, and ensuring smooth passage of vehicles; The planned path is smoothed and continuous to avoid sharp turns and sudden stops, ensuring the smoothness and continuity of the path; Dynamic update of target points: The planning module continues to work as the target points are updated to ensure the real-time and continuity of path planning; Step S4, post-processing: convert the coordinate system of the optimized path, store the path information, and send it to the subsequent modules; Step S5, visualization: The planning module visualizes the generated path and records and outputs the running time of each stage to facilitate monitoring of algorithm performance and observation and verification of planning results.

[0007] The step S3 introduces a high-performance solver to solve the optimization problem, as follows: Step S31, during initialization, a preliminary guess path is generated based on the rough path, and the preliminary states are calculated: speed, acceleration, yaw angle, etc.; Step S32, in each subsequent iteration, the position of the special vehicle is gradually expanded and adjusted by checking the collision conditions in four directions until a feasible area is found, and with the help of the drivable area constraints, collisions are avoided by continuous adjustments to ensure that the special vehicle does not hit obstacles or exceed the road boundary when driving on the road, and these constraints are used to correct the path; Step S33, using a nonlinear programming solver to solve according to the current state information, constraints and rough path to obtain an updated path; In each round of iteration in step S32, the cost function in the problem is optimized by gradually increasing the penalty factor, and the feasibility of the path is gradually improved; at the end of the iteration, if the result meets the specified feasibility tolerance, the optimization is completed, and the final result is returned to obtain the final optimized traceable path.

[0008] The path planning method of the present invention generates a path that satisfies the vehicle kinematic constraints based on the target endpoint posture information and the grid map provided by the front module according to the cleaning task requirements.

[0009] The present invention also provides a computer system, including a memory, a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of a path planning method based on a special unmanned vehicle on an unstructured road.

[0010] The beneficial effects of the present invention are: 1) It overcomes the limitations of traditional high-precision maps in complex environments and improves the environmental adaptability and operating efficiency of unmanned vehicles; 2) Through dynamic programming logic and nonlinear optimization, the feasibility, comfort and safety of the path are ensured, while taking into account the dynamic characteristics of the vehicle and environmental constraints; 3) Obstacle information is taken into account to ensure the safety and passability of the path. BRIEF DESCRIPTION OF THE DRAWINGS

[0011] The drawings constituting a part of the present application are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings: Figure 1 It is a flow chart of a path planning method according to a specific embodiment of the present invention. DETAILED DESCRIPTION

[0012] The following describes the embodiments of the present invention in detail with reference to the accompanying drawings, taking the path planning of a special unmanned vehicle on an unstructured road as an example, and taking the special operation requirements and the urban road cleaning task as a specific embodiment for detailed description.

[0013] like Figure 1 As shown, the present invention provides a path planning method based on unstructured roads for special unmanned vehicles. According to the grid map containing the information of dynamic and static obstacles within the perception range, the comprehensive cost uniformly distributed at each node in the space is calculated through dynamic programming logic, and a preliminary path is generated. Combining the explored spatial corridor and the kinematic equation of the vehicle, a high-performance nonlinear programming solver is used to perform nonlinear optimization to generate a path that safely reaches the expected target posture, including the following steps: Step S1, subscribe to and receive data from the ROS topic, including the speed and steering angle of the special vehicle, the environment map and the terminal posture, configure the start and end constraints, set the expected speed, target point weight and obstacles, lateral offset, lateral speed change, longitudinal speed, and penalty coefficients for longitudinal speed change, detect whether the path collides with obstacles in the grid map, and use this as the basis for determining whether to start the subsequent planning method; if not, wait for the topic data to be updated.

[0014] Step S2, provide the target terminal position information according to the cleaning task requirements, and send the terminal position information, and evenly scatter points along the starting and ending point connection direction in the potential driving domain in the spatial dimension. The initial cost of all points is set to positive infinity to find the optimal solution in the subsequent iteration process.

[0015] Step S3, optimizing the path by calculating the comprehensive cost of the nodes through dynamic programming logic; Step S31, establish a parent-child relationship between each node, and use dynamic programming logic to calculate the comprehensive cost of each point based on the cost of the parent node and taking into account multiple factors of the current node, the lateral offset cost of the deviation from the road center, the lateral change cost of the lateral offset change from the previous moment, the longitudinal speed cost of the longitudinal acceleration or deceleration of the special vehicle, and the distance cost to the target point; Step S32, based on the above method for determining the cost of each node, by calculating the cost of each state layer by layer, selecting the path with the minimum cost each time, and finally obtaining the point with the minimum cost, and using it as the end point of the dynamic programming algorithm path, tracing back each point to its parent node, and generating a path sequence containing spatial information as the final path output of the dynamic programming algorithm.

[0016] Step S4, based on the planner configuration, further integrates the precise position and heading requirements of the start and end points specified by the upper layer, sets them as the start and end point constraints, and constructs the preliminary path obtained based on dynamic programming, combined with the preprocessed raster map information and the vehicle kinematic equation as an optimization problem, fine-tunes the path, and ensures that the path meets the kinematic limitations and environmental constraints of special vehicles.

[0017] Step S5, introducing a high-performance HSL solver to solve the optimization problem; Step S51, during initialization, a preliminary guess path is generated based on the preliminary path, and preliminary states are calculated: velocity, acceleration, and yaw angle; Step S52, in each subsequent iteration, the position of the special vehicle is gradually expanded and adjusted by checking the collision conditions in four directions until a feasible area is found, and the collision is avoided by continuous adjustment with the help of the drivable area constraints, ensuring that the special vehicle will not hit obstacles or exceed the road boundary when driving on the road, and these constraints are used to correct the path; Step S53, using a high-performance nonlinear programming solver to solve according to the current state information, constraints and the preliminary path to obtain an updated path; In each round of iteration of step S531, the cost function in the problem is optimized by gradually increasing the penalty factor, and the feasibility of the path is gradually improved; at the end of the iteration, if the result meets the specified feasibility tolerance, the optimization is completed, and the final result is returned to obtain the final optimized traceable path.

[0018] Step S6, extract the code and store the optimized path parameters: position, speed, acceleration, etc., use the real-time special vehicle posture information obtained by the positioning module based on ROS, convert the trackable path from the vehicle coordinate system to the global coordinate system, and send it to the underlying path tracking module; during the processing, the planning module visualizes the generated path and records and outputs the running time of each stage, so that developers can monitor the algorithm performance and observe and verify the planning results.

[0019] The present invention also provides a computer system, including a memory, a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of a path planning method based on a special unmanned vehicle on an unstructured road.

[0020] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A path planning method based on a special unmanned vehicle on an unstructured road, characterized by: Based on the grid map containing information about dynamic and static obstacles within the perception range, the comprehensive cost distributed at each node in the space is calculated through dynamic programming logic, and a preliminary path is generated. Combining the explored spatial corridors and the kinematic equations of the vehicle, a high-performance nonlinear programming solver is used for nonlinear optimization to generate a path that safely reaches the expected target position.

2. The path planning method based on unstructured roads for special unmanned vehicles according to claim 1 is characterized in that: The specific steps include: Step S1, configuration and initialization: subscribe to and receive ROS topic data, configure the start and end point constraints, detect whether the path has a collision, if yes, initialize, set dynamic planning accessibility, passability and comfort related parameters; if no, wait for topic data update; Step S2, rough path generation: dynamically plan the driving path according to the terminal position sent by the upper layer; Step S3, path optimization: Based on the rough path, the optimization problem is constructed by combining the grid map containing dynamic and static obstacle information with the kinematic equation of the vehicle, and the solution is iteratively solved to optimize the path; Step S4, post-processing: convert the coordinate system of the optimized path, store the path information, and send it to the subsequent modules; Step S5, visualization: The planning module visualizes the generated path and records and outputs the running time of each stage to facilitate monitoring of algorithm performance and observation and verification of planning results.

3. The path planning method based on unstructured roads for special unmanned vehicles according to claim 2 is characterized in that: The step S3 introduces a high-performance solver to solve the optimization problem, as follows: Step S31, during initialization, a preliminary guess path is generated based on the rough path, and the preliminary states are calculated: speed, acceleration, yaw angle, etc.; Step S32, in each subsequent iteration, the position of the special vehicle is gradually expanded and adjusted by checking the collision conditions in four directions until a feasible area is found, and with the help of the drivable area constraints, collisions are avoided by continuous adjustments to ensure that the special vehicle does not hit obstacles or exceed the road boundary when driving on the road, and these constraints are used to correct the path; Step S33, using a nonlinear programming solver to solve according to the current state information, constraints and rough path to obtain an updated path; In each round of iteration in step S32, the cost function in the problem is optimized by gradually increasing the penalty factor, and the feasibility of the path is gradually improved; at the end of the iteration, if the result meets the specified feasibility tolerance, the optimization is completed, and the final result is returned to obtain the final optimized traceable path.

4. A computer system comprising a memory, a processor and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the path planning method based on unstructured roads for special unmanned vehicles as described in any one of claims 1-3.

Images