Unmanned platform global path planning and optimizing method in unstructured environment

Abstract

The invention relates to an unmanned platform global path planning and optimizing method in an unstructured environment, and belongs to the technical field of unmanned driving and path planning. The method comprises the following steps: constructing an environment model based on a digital elevation map, extracting terrain gradient information and dividing passable areas; performing path search by adopting an improved A * algorithm, and realizing multi-objective optimization by introducing terrain risk density, elevation cost, gradient cost and rollover stability cost into an evaluation function; by setting a safety threshold and a five-direction search strategy, the safety and efficiency of node selection are improved; and extracting path key points by using a Douglas-Peucker algorithm, and performing path smoothing optimization based on an adaptive Bezier curve to generate a smooth path conforming to the motion constraint of the unmanned platform. According to the method, the safety, stability and search efficiency of path planning are effectively improved, and the method is suitable for autonomous navigation of unmanned platforms in unstructured environments such as cross-country and mountainous regions.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving and automatic navigation technology, specifically to a method for global path planning and optimization of unmanned platforms in unstructured environments. Background Technology

[0002] Ground-based unmanned platforms are intelligent equipment systems that integrate environmental perception, intelligent decision-making, and collaborative control, and have broad application prospects in fields such as military reconnaissance, disaster relief, and resource exploration. Path planning is one of the core technologies for unmanned platforms to achieve autonomous navigation; its task is to find an optimal or suboptimal collision-free path from the starting point to the target point in a specific environment.

[0003] Unlike structured road environments, unstructured environments (such as wilderness, mountains, and tidal flats) typically lack clear road markings, have dramatic terrain undulations and complex geology, and contain various obstacles such as steep slopes, ravines, and boulders. This poses a significant challenge to path planning for unmanned platforms: on the one hand, the planned path must not only consider geometric collision-free operation but also comprehensively assess terrain passability, vehicle stability, and driving safety; on the other hand, the complex terrain environment also places higher demands on the real-time performance and search efficiency of the planning algorithm.

[0004] Currently, numerous methods have been studied for path planning in unstructured environments. The A* algorithm and its variants are widely used due to their simple structure and ease of implementation. However, the traditional A* algorithm and most of its improvements primarily focus on minimizing path length. Their cost functions often fail to adequately consider key safety factors such as terrain slope, vehicle pitch and roll stability, leading to planned paths that may traverse steep slopes or rugged areas, posing risks of rollover or slippage. Furthermore, traditional algorithms are prone to getting stuck in locally complex terrain during the search process, resulting in a large number of search nodes and requiring further efficiency improvements. Although some studies have introduced methods such as artificial potential fields and random sampling to improve path quality or efficiency, the modeling of global path safety costs remains incomplete.

[0005] In path post-processing, the planned original path typically consists of a series of polyline segments with numerous redundant turning points, failing to meet the continuous curvature constraints of unmanned platforms. Directly tracking such paths leads to frequent vehicle starts, stops, and turns, affecting driving stability and energy consumption. Although Bézier curves and B-spline curves have been used for path smoothing, existing methods are usually treated as independent post-processing modules. The selection of control points often relies on experience, resulting in low coupling with the front-end path planner. In complex terrain, this may not guarantee that the smoothed path still meets safety constraints.

[0006] Therefore, the main problems in existing technologies can be summarized as follows: the path planning process does not adequately consider the driving safety risks induced by terrain; the algorithm has low search efficiency in complex terrain; and path smoothing optimization is disconnected from the global planning objective, making it difficult to achieve a truly smooth and traceable path while ensuring safety. To address these issues, there is an urgent need for a global path planning method that can deeply integrate terrain safety assessment, efficient search, and adaptive smoothing. Summary of the Invention

[0007] This invention aims to solve the following technical problems existing in the path planning of current unmanned platforms in unstructured environments (such as wilderness, mountains, hills, etc.):

[0008] Insufficient driving safety: Traditional path planning algorithms (such as the A* algorithm) usually take the shortest path length as the main optimization objective, without fully considering the threat to vehicle driving stability (such as rollover and skidding) caused by terrain slope and undulation. This may result in the planned path passing through steep slopes or rugged areas, which poses safety hazards.

[0009] Inefficient planning: In complex, large-scale unstructured environments, traditional global search algorithms (such as the eight-direction A* algorithm) will explore a large number of invalid or reverse nodes, resulting in high computational overhead and difficulty in meeting the planning timeliness requirements of real-time or near-real-time applications.

[0010] Poor path executability: The planned original path is usually composed of a series of broken line segments with sharp corners and redundant turning points, which does not meet the kinematic constraints of the unmanned platform (such as the maximum steering angle limit), causing the vehicle to be unable to track smoothly, frequently start and stop and turn, affecting driving stability and energy consumption.

[0011] Planning and smoothing are disconnected: Existing path smoothing methods are usually treated as independent post-processing modules with low coupling to the front-end path planner. The smoothing process may deviate from the safe zone of the original path or fail to make full use of the terrain information in the planning stage, resulting in a decrease in the safety of the final path. Attached Figure Description

[0012] To more clearly illustrate the technical solutions in the embodiments of the present invention 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 the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0013] Figure 1 This invention relates to a research method for path planning and optimization of unmanned platforms in unstructured environments.

[0014] Figure 2This invention is an elevation 3D map model established based on a digital geographic model;

[0015] Figure 3 This invention is a binarized model obtained based on a three-dimensional elevation map model and accessibility analysis;

[0016] Figure 4 This is a schematic diagram of the method for calculating the density of terrain risk areas according to the present invention;

[0017] Figure 5 This is a schematic diagram illustrating the principle of setting the security threshold for the improved A* algorithm of this invention.

[0018] Figure 6 This is a schematic diagram of the unstructured environment path planning results of the present invention.

[0019] Figure 7 This is a schematic diagram of the improved A* algorithm search process of the present invention.

[0020] Figure 8 This is a schematic diagram of the Douglas-Puk key point extraction results of the present invention.

[0021] Figure 9 This is a schematic diagram of the adaptive Bézier curve smoothing optimization result of the present invention.

[0022] Figure 10 This is a schematic diagram of the binary map path planning of the present invention.

[0023] Figure 11 This is a schematic diagram of the path planning pitch angle of the present invention.

[0024] Figure 12 This is a schematic diagram of the path planning tilt angle of the present invention.

[0025] The above description discloses only one preferred embodiment of the present invention, and should not be construed as limiting the scope of the present invention. Those skilled in the art will understand that all or part of the processes of the above embodiments can be implemented, and equivalent changes made in accordance with the claims of the present invention are still within the scope of the invention. Detailed Implementation

[0026] To address the aforementioned technical problems, this invention provides a method for global path planning and optimization of unmanned platforms in unstructured environments, characterized by the following steps:

[0027] S1: Environmental Model Construction and Passability Analysis

[0028] Acquire digital elevation map data for the task area and construct a raster map environment model. Calculate the terrain slope of each raster based on the DEM data, and divide the raster into passable and impassable areas according to a preset slope threshold, generating a binarized passability raster map.

[0029] A digital terrain model based on geographic data is used to describe the global environment model for path planning. The required data is provided by elevation data maps, so the mathematical expression can be described as follows:

[0030]

[0031] In the formula Point Topographic feature values, which can be extracted from digital elevation maps obtained from remote sensing data, show significant differences in grid pixel values ​​in satellite images of different surface features. This paper uses the geometric mean to calculate the grayscale value of each grid cell.

[0032]

[0033] In the formula, Gray represents the grayscale value of the raster; Indicates the number of color channels in an image; i Indicates the sequence number of the corresponding grid; This represents the RGB value corresponding to a pixel.

[0034] A path planner for an unmanned platform is designed to address safety concerns, with slope data being one of its planning objectives. The terrain slope is calculated using elevation data, as shown below:

[0035] ,

[0036]

[0037]

[0038] In the formula, It is the rate of change of elevation in the north-south direction; It is the rate of change of elevation in the east-west direction; This indicates that the terrain is higher in the east and lower in the west. This indicates that the terrain is higher in the north and lower in the south, and vice versa. Elevation values ​​corresponding to the third-order unweighted difference model For terrain resolution, The calculated slope is usually expressed as a percentage. From this, we can deduce that the terrain angle θ can be expressed as:

[0039]

[0040] Considering the power performance of general off-road vehicles and the unstable driving conditions such as sideslip and rollback that are prone to occur on extreme slopes, a passability threshold is set for the off-road capability of the unmanned platform. Grids that exceed this threshold are set as impassable.

[0041] S2: Improved A* Algorithm Path Search

[0042] Based on the environment model constructed in step S1, an improved A* algorithm is used for initial path planning. The improvements include:

[0043] Improved Algorithm Evaluation Function: A terrain risk zone density factor is introduced into the evaluation function of the A* algorithm, and a multi-objective optimization function integrating elevation cost, slope cost, and rollover stability cost is constructed. Specifically, the evaluation function is:

[0044]

[0045] in, For the total estimated cost, From the starting node to the current node The actual cost, for Heuristic cost estimation from the current node to the target node. The density of terrain risk areas within the current node's target range is calculated as follows.

[0046]

[0047] In the formula, Density of terrain risk areas, The area of ​​the risk zone within the pre-aiming range, For the area of ​​a single risk zone, For the area of ​​the risk-prone zone, To expand the field of view for pre-aiming, This is the two-dimensional Euclidean distance from the node to the target point.

[0048] Elevation Cost The three-dimensional Euclidean distance between a child node and its parent node is calculated using the following formula:

[0049]

[0050] Slope cost and roll stability The cost is calculated as follows:

[0051]

[0052] Multi-objective optimization enables unmanned platforms to travel in a safer direction. By calculating the pitch and roll angles of waypoints, driving stability can be determined, allowing unmanned platforms to operate on safer roads.

[0053] Improved search functionality:

[0054] An adaptive five-direction search strategy based on the direction of the line connecting the starting point and the target point is adopted to replace the traditional eight-neighbor search, thereby reducing the number of search nodes and improving efficiency.

[0055] A safety threshold constraint is introduced during the node selection process to ensure that the planned path maintains a safe distance from the vertices of impassable areas and avoids diagonally crossing obstacle corners.

[0056] S3: Adaptive Path Smoothing Optimization

[0057] Perform smooth optimization on the initial path obtained in step S2:

[0058] The Douglas-Puk algorithm is used to extract key points of the path, filter out redundant noise, and retain the macroscopic shape of the path.

[0059] Based on the tangent direction of the key point, the principle formula is as follows:

[0060]

[0061] In the formula, for Tangent direction at the point , They are respectively The two adjacent points. The direction of the tangent obtained should be consistent with the path direction. Therefore, the direction consistency judgment is as follows: The final verified tangent direction is then used to obtain the radian angle by taking the arctangent.

[0062]

[0063]

[0064] The principle behind adaptively generating control points for a third-order Bézier curve is as follows: For path smoothing optimization of the third-order adaptive Bézier curve, four control points need to be determined. Two control points are the start and end points of each path segment, and the other two are adaptively generated based on the path points' directions. Given four control points... The parametric equations of a third-order Bézier curve are:

[0065]

[0066] The adaptively generated control points are related to the orientation of the intermediate points; a curvature factor is defined. And then according to , The offset distance of the control point can be calculated. As shown below:

[0067]

[0068] , The determination of control points is related to the direction of the tangent of the intermediate path, and the direction angle is known to be... ,but , The coordinates can be determined by the following formula.

[0069]

[0070]

[0071] Based on the convex hull property of Bézier curves, Selecting and improving the safety threshold of the A* algorithm The size is related to the control points calculated. , The constrained convex hull formed within the convex polygon constructed by the safety threshold can ensure that the optimized curve does not overlap with obstacles.

[0072] A smooth path is generated using the control points, and this two-dimensional smooth path is projected onto the elevation dimension, ultimately outputting a three-dimensional smooth path that conforms to the kinematic constraints of the unmanned platform.

[0073] Beneficial effects:

[0074] To address the safety and stability requirements of vehicle path planning in off-road environments, an improved A* algorithm framework is proposed. By establishing a dynamic estimation model of pitch and roll angles coupled with terrain longitudinal slope, cross slope, and vehicle heading angle, the heuristic function of the A* algorithm is reconstructed. Furthermore, a multi-objective optimization function integrating elevation cost, slope cost, and rollover cost is constructed. Introducing an obstacle threshold mechanism and a risk area estimation factor significantly improves the robustness of off-road path planning. Experiments show that:

[0075] (1) Path safety optimization: The pitch angle and roll angle of the path generated by the improved algorithm are lower on average than those of the traditional A* algorithm, which effectively suppresses the risk of vehicle rollover and pitch.

[0076] (2) Enhanced terrain adaptability: The obstacle threshold mechanism makes the path path further away from high slope areas, leaving a certain driving error tolerance for dangerous situations such as landslides and rockfalls. Furthermore, the multi-objective cost function achieves the synergistic optimization of terrain constraints and dynamic performance.

[0077] (3) Improved computational efficiency: By improving the search method of A*, a five-direction search method can be adopted to a large extent to reduce the number of grid searches, improve search efficiency, and better meet engineering requirements.

[0078] (4) Improved smoothness: Based on the Douglas-Peucker algorithm to extract key points, the path generated by the adaptive Bézier curve retains the original shape while improving the curvature continuity, filtering out the sharp bends and noise of the original path, and meeting the vehicle steering control requirements.

[0079] The proposed method has been verified to be effective in the actual terrain measured by DEM, providing theoretical support and technical implementation for the stable driving of unmanned off-road platforms.

Claims

1. A method for global path planning and optimization of unmanned platforms in unstructured environments, characterized in that, Includes the following steps: Step S1: Geographic Information Extraction and Environmental Model Construction: Obtain digital elevation map data of the task area and construct a raster map environmental model; Step S2: Slope Calculation and Accessibility Analysis: Calculate the terrain slope of each grid cell and divide the grid cells into accessible and inaccessible areas according to the preset slope threshold. Step S3: Improve the A* algorithm fitness function: Based on the environment model constructed in step S1, the improved A* algorithm is used for initial path planning; the improvement includes: introducing terrain risk area density factor, elevation cost, slope cost and rollover stability cost into the evaluation function of the A* algorithm to perform multi-objective optimization; Step S4: Improve the A* algorithm search method: adopt an adaptive five-direction search strategy based on the direction of the line connecting the starting point and the target point to expand the nodes; introduce a safety threshold constraint during the node selection process to ensure that the planned path maintains a safe distance from the vertices of the inaccessible area; Step S5: Path smoothing optimization: For the initial path obtained in step 3, the Douglas-Puk algorithm is used to extract key points; control points of the third-order Bézier curve are adaptively generated based on the tangent direction of the key points; a smooth path is generated using the control points, and the two-dimensional smooth path is projected to the elevation dimension to finally output a three-dimensional smooth path.

2. The method according to claim 1, characterized in that, In step S3, the evaluation function of the A* algorithm is: in, For the total estimated cost, From the starting node to the current node The actual cost, for Heuristic cost estimation from the current node to the target node. The density of terrain risk areas within the target range of the current node. The density of terrain risk areas within the current node's target range is calculated as follows. In the formula, Density of terrain risk areas, The area of ​​the risk zone within the pre-aiming range, For the area of ​​a single risk zone, For the area of ​​the risk-prone region, To expand the field of view for pre-aiming, This is the two-dimensional Euclidean distance from the node to the target point. Elevation Cost The three-dimensional Euclidean distance between a child node and its parent node is calculated using the following formula: Slope cost and roll stability The cost is calculated as follows: Multi-objective optimization enables unmanned platforms to travel in a safer direction. By calculating the pitch and roll angles of waypoints, driving stability can be determined, allowing unmanned platforms to operate on safer roads.

3. The method according to claim 2, characterized in that, The actual cost The calculation formula is: in, The step size cost from parent node to child node. , , These are measures of path elevation cost. Slope cost Lateral stability The weighting coefficient of the cost For the coordinates of the parent node, The coordinates of the child nodes are given. The elevation cost is given in the formula. This represents the three-dimensional Euclidean distance between the child node and the parent node.

4. The method according to claim 1, characterized in that, In step S4, the specific implementation of the adaptive five-directional search strategy is as follows: The angle α between the line connecting the starting point and the target point and due north is used to delete three reverse or lateral directions from the traditional eight search directions, based on the angle range of α, and retain five dominant search directions.

5. The method according to claim 1, characterized in that, In step S4, the specific implementation of the safety threshold constraint is as follows: When expanding a node, the distance from the line segment between the current node and the pre-selected child node to the nearest obstacle vertex is determined. If the distance is less than a preset safety threshold, the pre-selected child node is discarded.

6. The method according to claim 1, characterized in that, In step S5, the adaptive generation of control points for the third-order Bézier curve based on the tangent direction of the key points specifically includes: For key points , For a given path segment, first calculate the intermediate points. tangent direction ; According to the tangent direction and starting point and the finish line Distance between Adaptively determine two intermediate control points and The calculation formula is:

Images