A path planning method suitable for unmanned vehicles in a wild scene

Abstract

The application relates to the technical field of unmanned vehicle path planning, and provides a path planning method suitable for unmanned vehicles in a wild field scene, which comprises the following steps: performing crossable terrain evaluation to obtain the passability of the current ground; combining the passability and an improved Informed-RRT* algorithm to generate a global reference path; adopting a Bezier curve trajectory fitting on the global reference path to generate a smooth path; and combining the smooth path and obstacle information, and performing local planning and tracking control based on a kinematics model. The path planning framework provided by the application considers the passability of the terrain and the dynamic characteristics of the robot, so that the application is more in line with actual application requirements, and the operation efficiency and reliability of the unmanned vehicle in a complex terrain are improved.

Description

Technical Field

[0001] This invention relates to the field of unmanned vehicle path planning technology, and more specifically, to a path planning method for unmanned vehicles in outdoor scenarios. Background Technology

[0002] With advancements in technology, an increasing number of tasks require autonomous vehicles to navigate complex and irregular terrains. Especially in applications such as forest rescue, wilderness exploration, and mining transportation, autonomous vehicles need to effectively plan routes in changing outdoor environments. Motion planning is a core component of autonomous navigation systems, involving how an autonomous vehicle moves from its starting point to its destination while avoiding obstacles, adapting to terrain changes, and maintaining stable operation.

[0003] Existing autonomous vehicle navigation technologies are primarily designed for indoor or urban environments, where the terrain is relatively flat and regular. Currently mature autonomous navigation technologies often rely on two-dimensional maps and algorithms such as A / B navigation. Path planning can be performed using methods such as search, Dijkstra's algorithm, or dynamic window methods. However, these methods typically do not consider the three-dimensional properties of the terrain, such as variations in height and curvature. Therefore, directly applying these algorithms to complex and varied wilderness terrain can lead to poor planning results. In wilderness environments, autonomous vehicles often travel on steep slopes or irregular terrain, requiring path planning algorithms for these scenarios to consider not only the shortest or optimal path but also the safety and feasibility of the planned path.

[0004] Therefore, path planning in uneven terrain mainly involves the following two challenges: how to assess the accessibility of the terrain, and how to handle robot dynamics models related to the terrain.

[0005] Regarding the first challenge, current path planning technologies often lack effective mechanisms for measuring and considering terrain traversability. This means that the slope and roughness of the terrain are often overlooked during the planning process, which may lead to unforeseen obstacles and dangers for the robot in actual operation. For example, the robot may be damaged or its stability may be affected when traveling on steep or gravelly terrain.

[0006] The second challenge is that existing path planning methods often fail to tightly integrate the robot's dynamics model with terrain characteristics. This results in planned trajectories that may be physically infeasible on complex terrains such as sloping ground, because they ignore the varying throttle inputs required to achieve specific velocities or accelerations under terrain influence. Furthermore, dynamics models that do not consider these factors lack the precision required for coupling with terrain, making it difficult for the controller to accurately track the predetermined trajectory, thus impacting the robot's operational efficiency and safety.

[0007] Therefore, existing technologies have limited path planning capabilities on uneven terrain in the wild, often failing to generate trajectories that are both feasible in terms of terrain and can be effectively tracked by the autonomous vehicle control system. These limitations lie in their simplified handling of terrain influences and unrealistic assumptions about robot dynamics models, resulting in overly conservative or practically unfeasible paths. Summary of the Invention

[0008] This invention aims to provide a path planning method suitable for unmanned vehicles in field scenarios, in order to solve the problems of how traditional path planning algorithms evaluate the accessibility of terrain and how to handle robot dynamics models related to terrain. It realizes the close integration of the motion model of unmanned vehicles with terrain characteristics, and generates trajectories that not only conform to the actual feasibility of the terrain, but can also be effectively tracked by the unmanned vehicle control system. The generated trajectory has good applicability to field scenarios.

[0009] This invention provides a path planning method for unmanned vehicles in wilderness scenarios, comprising: Conduct a terrain assessment to determine the current ground accessibility; Combining accessibility and improved Informed-RRT The algorithm generates a global reference path; A smooth path is generated by fitting the trajectory of a Bézier curve to the global reference path; By combining smooth path and obstacle information, local planning and tracking control are performed based on a kinematic model.

[0010] In a preferred embodiment, the step of performing a traversable terrain assessment to obtain the current ground accessibility includes: A raster map is obtained by downsampling the point cloud map of the current environment; For each point on the raster map, perform plane fitting to calculate the slope, flatness, and sparsity of each local plane; Based on the slope, flatness, and sparsity of each local plane, as well as the maximum slope, maximum flatness, and maximum sparsity allowed by the autonomous vehicle, the current ground accessibility is calculated.

[0011] In a preferred embodiment, the combination of accessibility and improved Informed-RRT The algorithm generates global reference paths, including: Improved Informed-RRT The algorithm performs global path planning based on the cost function, first initializing and determining whether an initial path has been found: If so, read the optimal path cost and the straight-line distance between the starting node and the target node in the current grid map, construct an ellipsoid, generate sampling points within the ellipsoid, find the nearest neighbor node of the sampling point, and generate a new node according to the step size; If not, then randomly generate sampling points across the entire map, traverse all nodes on the map to find the nearest neighbor node of the sampling point, and determine the new node based on the step size. In the neighborhood node set and several generations of parent nodes of the neighborhood node set, the node with the minimum overall path cost and no collision is selected as the parent node of the new node; then, other nodes in the neighborhood node set are connected to the new node and its parent node to reduce the local path cost; finally, it is determined whether the termination condition is met. If the termination condition is met, the optimal path existing in the current map is read as the global reference path. Otherwise, the above steps are repeated until the optimal global reference path is found. The termination condition is set based on generality.

[0012] In a preferred embodiment, the cost function is expressed as:

[0013] in, Represents a node and The Euclidean distance between them Represents a node versatility Represents a node versatility This represents the weighting coefficient, used to adjust the relative importance of each sub-cost.

[0014] In a preferred embodiment, the step of generating a smooth path by fitting the trajectory of a Bézier curve to the global reference path includes: In the initialization phase of curve fitting for the global reference path, the start and end points of the global reference path are used as the first and last control points of the cubic Bézier curve, respectively. In each iteration of curve fitting, the position of the midpoint of each Bézier curve segment is first determined, then the interpolation point corresponding to the original path point is obtained, and the residual between the interpolation point and the original path point is calculated. If the residual at the midpoint of the Bézier curve interpolation point is greater than a set threshold, the global reference path is segmented at the path point corresponding to the midpoint with the largest residual. This process is repeated until the residual at the midpoint of each curve segment obtained by fitting meets the requirements. Finally, a smooth path through the cubic Bézier curve is obtained, and each path point in the smooth path contains position and heading angle information.

[0015] In a preferred embodiment, the step of combining smooth path and obstacle information to perform local planning and tracking control based on a kinematic model includes: The smooth path is replanned based on obstacle information to create a local desired trajectory that bypasses the obstacles; Autonomous vehicles are tracked and controlled based on local desired trajectories.

[0016] In a preferred embodiment, the process of replanning the smooth path to recurs the desired local trajectory around the obstacle based on the obstacle information is expressed as:

[0017] in, express The state parameters of the autonomous vehicle at all times, including The x-axis of the autonomous vehicle's location at any given time and ordinate and heading angle ; Include The control variables of autonomous vehicles at all times, including Front wheel steering angle of the driverless car and vehicle speed , Indicates control quantity The minimum value, Indicates control quantity The maximum value; Q This represents the state error weight matrix, used to measure the current state of the autonomous vehicle. Compared with reference state The importance of the deviation ; R This represents the control input weight matrix, used to penalize the control quantity. Size, ; This is the obstacle avoidance function. N The number of obstacle points. i This refers to the obstacle point number.

[0018] In a preferred embodiment, the obstacle avoidance function is expressed as:

[0019] in, These are the weighting coefficients. , and For driverless cars and Speed ​​in the direction, These are the position coordinates of the obstacle point in the vehicle coordinate system. These are the centroid coordinates of the autonomous vehicle. This parameter is set to prevent the denominator from being zero.

[0020] In a preferred embodiment, the positions of each path point and the front wheel steering angle in the local desired trajectory need to be processed as a fifth-order polynomial as a fitting curve.

[0021] In a preferred embodiment, the process of tracking and controlling the unmanned vehicle based on the local desired trajectory is expressed as follows:

[0022] in, Represents slack variables; Represents the effect of slack variables The weighting coefficient for imposing penalties; This represents the change in control input between two adjacent control moments; These represent the minimum and maximum values ​​of the control limit variation for the autonomous vehicle, respectively. Given the desired local trajectory, we obtain the control input for the autonomous vehicle, and control the autonomous vehicle to travel according to the replanned trajectory.

[0023] In summary, due to the adoption of the above technical solution, the beneficial effects of the present invention are: 1. By accurately measuring and considering terrain traversability, this invention can significantly improve the safety of unmanned vehicles on different terrains, enabling them to prioritize flat and passable terrain and avoid steep and impassable areas, thereby reducing the potential risk of damage.

[0024] 2. The dynamic model that effectively handles terrain-related factors in this invention can improve the executability of the trajectory and the tracking performance of the controller, ensuring that the unmanned vehicle can travel along the expected trajectory in actual operation and adapt to complex terrain environments.

[0025] 3. Combining the above two points, the path planning framework proposed in this invention takes into account the accessibility of the terrain and the dynamic characteristics of the robot, making the invention more in line with the needs of practical applications and improving the operating efficiency and reliability of unmanned vehicles in complex terrain. Attached Figure Description

[0026] Figure 1 This is a flowchart of a path planning method for unmanned vehicles in field scenarios, provided as an embodiment of the present invention.

[0027] Figure 2 An improved Informed-RRT in this embodiment of the invention A schematic diagram illustrating the selection of the sampling range for the algorithm.

[0028] Figure 3This is a flowchart illustrating the adaptive reference path curve fitting process in an embodiment of the present invention.

[0029] Figure 4 This is a schematic diagram of two consecutive cubic Bézier curves in an embodiment of the present invention.

[0030] Figure 5 This is a schematic diagram of a local path planning and control framework based on NMPC in an embodiment of the present invention. Detailed Implementation

[0031] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0032] Therefore, the following detailed description of the embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.

[0033] like Figure 1 As shown, this embodiment of the invention provides a path planning method suitable for unmanned vehicles in wilderness scenarios. For wilderness scenarios, the drivability of the current terrain is first calculated, and then the drivability is combined with Informed-RRT. The algorithm obtains a global reference path, then uses cubic Bézier curve fitting to obtain a smooth path containing curvature information. Finally, based on the local environment of the autonomous vehicle, it continuously replans and controls the vehicle so that the autonomous vehicle can reach the target position autonomously according to the plan.

[0034] S100, conduct a traversable terrain assessment to determine the current ground accessibility. This includes the following sub-steps: S101, downsample the point cloud map of the current environment to obtain a raster map; In this embodiment, Light Laser Detection and Ranging (LiDAR) and Simultaneous Localization and Mapping (SLAM) technologies are used to acquire a point cloud map of the current environment. The point cloud map is then downsampled to generate a raster map. The generated raster map is a 3D raster map.

[0035] S102, perform plane fitting on each point of the raster map to calculate the slope, flatness, and sparsity of each local plane; the specific implementation is as follows: For each point on the ground Points on the raster map are selected by a rectangular bounding box (the size of which can be set to the size of the autonomous vehicle), represented as:

[0036] By fitting the plane using the singular value decomposition method, the unit normal vector can be obtained. The three-dimensional coordinate system of the fitting plane consists of three unit vectors. , , definition:

[0037] in, , express exist Projection on Represents the rotation of a plane in a local coordinate system. Indicates the position of the plane in the local coordinate system; Each node Corresponding to the local plane Each node The accessibility of the surrounding area is determined by the slope. Flatness and sparsity The three criteria are determined, and the calculations are as follows:

[0038] in, and It is a constant coefficient. This represents the projection of the plane normal vector onto the z-axis of the world coordinate system. This indicates the proportion of the empty portion on the plane. and These represent the minimum and maximum acceptable percentages of voids, respectively. When voids are concentrated, it indicates that there may be potholes and depressions in the ground.

[0039] S103, based on the slope, flatness, and sparsity of each local plane, as well as the maximum slope, maximum flatness, and maximum sparsity allowed by the autonomous vehicle, calculates the current ground accessibility using the following formula:

[0040] in, , and Slope Flatness and sparsity The weight, and . , and These represent the maximum allowable slope, maximum flatness, and maximum sparsity for autonomous vehicles, respectively. The value range is 0~1. The higher the value, the worse the ground conditions. When =0 indicates that the ground is very flat, suitable for autonomous vehicles; when A value of 1 indicates that the ground is completely unsuitable for driverless vehicles.

[0041] S200, combining accessibility and improved Informed-RRT The algorithm generates a global reference path; Specifically, improve Informed-RRT The algorithm uses a random search tree to search for passable regions; for impassable regions, the random search tree stops expanding. This improves upon Informed-RRT. The algorithm constructs an ellipsoid using the starting and target nodes as the foci of an ellipse, incorporating factors such as drivability, optimal path cost, and the straight-line distance between the starting and target nodes. Sampling nodes are constrained by this ellipsoid, and its size continuously decreases as the cost parameter decreases, achieving real-time reduction of the sampling range. New surface nodes are generated by finding the nearest neighbor of each sampling node and projecting it onto a plane, then fitted to the new node. If the new node is within the drivable area, the nearest neighbor is searched, the best parent node is selected, and the path is updated, while reconnection operations are performed to optimize the entire path. Each iteration continuously updates the path until a global reference path is found. This approach combines ground drivability and improves Informed-RRT. The algorithm effectively utilizes terrain analysis and 3D map data, ensuring rapid response and efficient path planning in complex environments.

[0042] In practice, the starting node is first set. and target node Since drivability is added to the random search tree, the termination condition is set based on drivability, i.e., within the drivability range. The random search tree stops expanding at this time, avoiding useless analysis. For a given two nodes... , , Represents a node and nodes Given the Euclidean distance between the two nodes, the cost function of the straight line connecting them is:

[0043] in, This represents the weighting coefficient, used to adjust the relative importance of each sub-cost.

[0044] Improved Informed-RRT The algorithm performs global path planning based on the cost function, first initializing and determining whether an initial path has been found: If so, read the optimal path cost from the current raster map. and the starting node With the target node straight-line distance An ellipsoid is constructed in this way, and sampling points are generated within the ellipsoid. ,like Figure 2 As shown, find the sampling point. nearest neighbor node Generate new nodes based on step size ; If not, then randomly generate sampling points across the entire map. Traverse all nodes on the map to find sampling points. nearest neighbor node Determine the new node based on the step size. , In the neighborhood node set and the set of neighboring nodes of Parent node set In the middle, is the new node. Select the node with the lowest overall path cost and no collisions as the new node. parent node Then the neighborhood node set Other nodes and new nodes and its parent node Attempt to connect to reduce local path costs; finally, determine if the termination condition is met. If the termination condition is met, read the optimal path that exists in the current map, which is the global reference path. Otherwise, repeat the above steps until the optimal global reference path is found.

[0045] S300 uses Bézier curve trajectory fitting to generate a smooth path for the global reference path; Due to improvements in Informationed-RRT The algorithm generates a global reference path with many inflection points, and the global reference path does not contain discrete waypoint sequences with curvature information. To extract the curvature information of the global reference path, a Bézier curve is used to divide the global reference path into multiple segments for curve fitting to obtain a smooth path.

[0046] This embodiment employs cubic Bézier curves. Specifically, during the initialization phase of curve fitting for the global reference path, the start and end points of the global reference path are used as the first and last control points of the cubic Bézier curve, respectively. In each iteration of curve fitting, the positions of the midpoints of each Bézier curve segment are first determined. Then, the interpolation points corresponding to the original path points are obtained, and the residuals between the interpolation points and the original path points are calculated. If the residual at the midpoint of the Bézier curve interpolation point is greater than a set threshold, the global reference path is segmented at the path point corresponding to the midpoint with the largest residual. This process is repeated until the residuals at the midpoints of each fitted curve segment meet the fitting accuracy requirements. Finally, a dense, smooth path through the cubic Bézier curve is obtained, where each path point contains position and heading angle information.

[0047] In specific implementation, such as Figure 3 As shown, a cubic Bézier curve is represented as:

[0048] To ensure a smooth transition between the two curve segments (where the tangent vectors at the connection point have the same direction but different magnitudes), the three control points at the connection point of the fitted adjacent curve segments need to be collinear, such as... Figure 4 As shown. Taking a cubic Bézier curve as an example. , These are the control points for the first and second segments of the curve, respectively. , As the endpoint, , The point is the midpoint. The curve must be continuous at the connection point.

[0049] in, It is a direction vector. The parameter is a scalar parameter to be determined.

[0050] Since curve fitting for the global reference path does not require the fitted curve to pass precisely through each discrete waypoint, the least squares method is chosen to fit the midpoints of each segment of the global reference path. Let... Given discrete waypoints for the global reference path, the sum of squares of the fitted residuals is expressed as:

[0051] in, This represents the number of discrete points contained in this path segment. For cubic Bézier curves in parameters The coordinates at that location. Through [the...] For the intermediate control points respectively , Find the partial derivative and set it to zero, that is:

[0052] Intermediate control points can be obtained. , The analytical solution is expressed as:

[0053] in:

[0054] The obtained intermediate control points , Substituting the equation of the Bézier curve yields the precise expression for the curve segment. The residuals at intermediate interpolation points are then calculated and compared to a preset threshold. If the residuals at all intermediate interpolation points do not exceed the threshold, the curve fitting accuracy is considered satisfactory. If there are intermediate points with excessive residuals, the path segment is split at the original path position corresponding to the point of maximum residual, forming two new path intervals. The fitting process is then repeated for each new interval.

[0055] Through the above iterations, until the residuals of the intermediate interpolation points of all curve segments meet the accuracy requirements, a smooth and continuous multi-segment cubic Bézier curve path can be obtained. This path not only ensures the continuity of position and first derivative (direction vector), but also takes into account the smoothness of curvature changes, thus providing a high-quality reference trajectory for subsequent local replanning and control modules.

[0056] Step 4: Combine smooth path and obstacle information to perform local planning and tracking control based on the kinematic model.

[0057] A trajectory replanning layer, also known as a local planning layer, is built above the trajectory tracking layer. This planning layer can replan the desired local trajectory to avoid obstacles based on the obstacle information obtained by the sensors, referencing the path (i.e., the smooth path obtained in step 3). The tracking control layer performs tracking control on the autonomous vehicle based on the desired local trajectory, achieving obstacle avoidance while tracking the global reference path. This part includes a trajectory replanning module and a tracking control module, such as... Figure 5 As shown. The tracking control module receives the local desired trajectory from the trajectory replanning module and outputs the front wheel steering angle and vehicle speed; the trajectory replanning module receives obstacle information from the sensors and the smooth path from step 3, plans the local desired trajectory through the model predictive control algorithm, and then sends it to the tracking control module.

[0058] Trajectory replanning is the core component of the trajectory planning layer. Its main task is to design a reasonable evaluation function that, under various constraints, achieves obstacle avoidance while minimizing the vehicle's deviation from the global reference path. Finally, it outputs the result to the tracking control layer in a suitable manner. For ease of subsequent modeling, a point-mass vehicle dynamics model is adopted, and its kinematic equations in the inertial coordinate system are as follows:

[0059] in, x , y This represents the position of the autonomous vehicle in the global coordinate system. and For the longitudinal / lateral velocities in the vehicle coordinate system, For heading angle; and The autonomous vehicles exert combined forces in the longitudinal and lateral directions respectively. This represents the resultant force of the longitudinal resistance (including rolling resistance, air resistance, and gradient resistance) experienced by the autonomous vehicle. This is the equivalent deceleration caused by resistance; This is lateral acceleration.

[0060] Considering dynamic safety constraints, road-tire adhesion constraints are introduced:

[0061] in, Road-tire adhesion coefficient, Let be the gravitational acceleration constant (taken as 9.81 m / s²). 2 ).

[0062] In summary, the autonomous vehicle's position and heading angle are taken as its state vector, denoted as... The system can be abbreviated as:

[0063] in, To respectively, for driverless cars in x , y The velocity in the direction and the angular velocity of the heading. To control the quantities, let's represent the front wheel steering angle and vehicle speed, respectively; and These represent the upper and lower bounds of the control quantity, respectively.

[0064] Choosing the initial reference trajectory point is a crucial issue during trajectory replanning. To avoid the autonomous vehicle reusing previously visited reference points as new reference points when obstacles are large, target point information is incorporated into the reference trajectory point selection process. Regardless of its position during tracking, the autonomous vehicle will make a trajectory parallel to the target point in the global coordinate system.x shaft and y The two straight lines on the axes intersect the globally desired trajectory. Since there are always two intersection points, the distances from these two intersection points to the target point are calculated separately, and the reference point closer to the target point is selected as the true reference starting point.

[0065] After selecting the initial reference trajectory point, LiDAR is used to detect the point cloud data of obstacles near the autonomous vehicle. Based on the influence of the autonomous vehicle's speed and the weight of the penalty function in the objective function on obstacle avoidance, the following obstacle avoidance function is selected:

[0066] in, These are the weighting coefficients. , and For driverless cars and Speed ​​in the direction, These are the position coordinates of the obstacle point in the vehicle coordinate system. These are the centroid coordinates of the autonomous vehicle. To set the parameter, it is usually a small positive number to prevent the denominator from being 0.

[0067] In summary, the control objective in the trajectory replanning layer is to minimize the deviation from the global reference path and to avoid obstacles. Obstacle avoidance is achieved through a penalty function. The specific form of the model predictive controller in the trajectory replanning layer is as follows:

[0068] in, express The state parameters of the autonomous vehicle at all times, including The x-axis of the autonomous vehicle's location at any given time ordinate and heading angle ; This represents the state variables of the autonomous vehicle in a smooth path, including the vehicle's x-coordinate. and ordinate and heading angle ; Include The control quantity of autonomous vehicles at all times Indicates control quantity The minimum value, Indicates control quantity The maximum value, including Front wheel steering angle of the driverless car and vehicle speed , NThe number of obstacle points. i The obstacle point number; Q This represents the state error weight matrix, used to measure the current state of the autonomous vehicle. Compared with reference state The importance of the deviation, in the formula ; R This represents the control input weight matrix, used to penalize the control quantity. The size (or change) in the formula ; In trajectory replanning algorithms, the control cycles of the planning layer and the control layer are not consistent, making it difficult for the control layer to complete trajectory tracking based on discrete reference trajectory points. Considering these factors, it is necessary to process the locally desired path planned by the trajectory replanning algorithm to achieve smooth integration between the planning and control layers. Since autonomous vehicles have kinematic constraints, such as continuous vehicle position requiring continuous curves, continuous yaw angle requiring first-order continuous curves, and acceleration constraints requiring second-order continuous curves, this invention uses a fifth-order polynomial as the fitting curve to process the position of each path point and the front wheel steering angle in the locally desired path, as follows:

[0069] in, and For the parameters to be determined, Indicates location information, This indicates the steering angle of the front wheels.

[0070] Finally, path tracking control is executed. To reduce computational load and ensure real-time performance while also considering the replanning module, NMPC model predictive control is chosen. The tracking controller formula is as follows:

[0071] To avoid situations where there are too many obstacles or the path is too narrow, slack variables are used. Set to 0.01; Represents the effect of slack variables The weighting coefficient for imposing penalties; Q This represents the state error weight matrix, used to measure the current state of the autonomous vehicle. With local expected trajectory The importance of the deviation, in the formula ; R This represents the control increment weight matrix, used to penalize changes in control. The magnitude of the value should be adjusted to prevent control commands (such as direction, angle, and acceleration) from changing too rapidly in adjacent moments. In the formula... ; This represents the change in control input between two adjacent control moments; These represent the minimum and maximum values ​​of the control variation limits for the autonomous vehicle, respectively.

[0072] The control variables (including vehicle speed and front wheel steering angle) of the autonomous vehicle are obtained from the local desired trajectory output after replanning, and the autonomous vehicle is controlled to travel according to the local desired trajectory output after replanning.

[0073] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A path planning method suitable for unmanned vehicles in wilderness scenarios, characterized in that, include: Conduct a terrain assessment to determine the current ground accessibility; Combining accessibility and improved Informed-RRT The algorithm generates a global reference path; A smooth path is generated by fitting the trajectory of a Bézier curve to the global reference path; By combining smooth path and obstacle information, local planning and tracking control are performed based on a kinematic model; The assessment of traversable terrain to determine the current ground accessibility includes: A raster map is obtained by downsampling the point cloud map of the current environment; For each point on the raster map, perform plane fitting to calculate the slope, flatness, and sparsity of each local plane; Based on the slope, flatness, and sparsity of each local plane, as well as the maximum slope, maximum flatness, and maximum sparsity allowed by the autonomous vehicle, the current ground accessibility is calculated. The combination of accessibility and improved Informationd-RRT The algorithm generates global reference paths, including: Improved Informed-RRT The algorithm performs global path planning based on the cost function, first initializing and determining whether an initial path has been found: If so, read the optimal path cost and the straight-line distance between the starting node and the target node in the current grid map, construct an ellipsoid, generate sampling points within the ellipsoid, find the nearest neighbor node of the sampling point, and generate a new node according to the step size; If not, then randomly generate sampling points across the entire map, traverse all nodes on the map to find the nearest neighbor node of the sampling point, and determine the new node based on the step size. In the neighborhood node set and several generations of parent nodes of the neighborhood node set, the node with the minimum overall path cost and no collision is selected as the parent node of the new node; then, other nodes in the neighborhood node set are connected to the new node and its parent node to reduce the local path cost; finally, it is determined whether the termination condition is met. If the termination condition is met, the optimal path existing in the current map is read as the global reference path; otherwise, the above steps are repeated until the optimal global reference path is found; the termination condition is set based on generality. The process of generating a smooth path by fitting the trajectory of a Bézier curve to the global reference path includes: In the initialization phase of curve fitting for the global reference path, the start and end points of the global reference path are used as the first and last control points of the cubic Bézier curve, respectively. In each iteration of curve fitting, the position of the midpoint of each Bézier curve segment is first determined, then the interpolation point corresponding to the original path point is obtained, and the residual between the interpolation point and the original path point is calculated. If the residual at the midpoint of the Bézier curve interpolation point is greater than a set threshold, the global reference path is segmented at the path point corresponding to the midpoint with the largest residual. This process is repeated until the residual at the midpoint of each curve segment obtained by fitting meets the requirements. Finally, a smooth path through the cubic Bézier curve is obtained, and each path point in the smooth path contains position and heading angle information.

2. The path planning method for unmanned vehicles in wilderness scenarios according to claim 1, characterized in that, The cost function is expressed as: in, Represents a node and The Euclidean distance between them Represents a node versatility Represents a node versatility This represents the weighting coefficient, used to adjust the relative importance of each sub-cost.

3. The path planning method for unmanned vehicles in wilderness scenarios according to claim 1, characterized in that, The method of combining smooth path and obstacle information, and performing local planning and tracking control based on a kinematic model, includes: The smooth path is replanned based on obstacle information to create a local desired trajectory that bypasses the obstacles; Autonomous vehicles are tracked and controlled based on local desired trajectories.

4. The path planning method for unmanned vehicles in wilderness scenarios according to claim 3, characterized in that, The process of replanning the smooth path to recurs the desired local trajectory that bypasses the obstacle based on the obstacle information is expressed as: in, express The state parameters of the autonomous vehicle at all times, including The x-axis of the autonomous vehicle's location at any given time and ordinate and heading angle ; Include The control variables of autonomous vehicles at all times, including Front wheel steering angle of the driverless car and vehicle speed , Indicates control quantity The minimum value, Indicates control quantity The maximum value; Q This represents the state error weight matrix, used to measure the current state of the autonomous vehicle. Compared with reference state The importance of the deviation ; R This represents the control input weight matrix, used to penalize the control quantity. Size, ; This is the obstacle avoidance function. N The number of obstacle points. i This refers to the obstacle point number.

5. The path planning method for unmanned vehicles in wilderness scenarios according to claim 4, characterized in that, The obstacle avoidance function is expressed as follows: in, These are the weighting coefficients. , and For driverless cars and Speed ​​in the direction, These are the position coordinates of the obstacle point in the vehicle coordinate system. These are the centroid coordinates of the autonomous vehicle. This parameter is set to prevent the denominator from being zero.

6. The path planning method for unmanned vehicles in wilderness scenarios according to claim 4, characterized in that, The positions of each path point and the front wheel steering angle in the desired local trajectory need to be processed as a fifth-order polynomial as a fitting curve.

7. The path planning method for unmanned vehicles in wilderness scenarios according to claim 4, characterized in that, The process of tracking and controlling the unmanned vehicle based on the local desired trajectory is expressed as follows: in, Represents slack variables; Represents the effect of slack variables The weighting coefficient for imposing penalties; This represents the change in control input between two adjacent control moments; These represent the minimum and maximum values ​​of the control limit variation for the autonomous vehicle, respectively. Given the desired local trajectory, we obtain the control input for the autonomous vehicle, and control the autonomous vehicle to travel according to the replanned trajectory.

Images