Motion planning method based on improved A* and improved DWA fusion real-time topographic features

Abstract

The invention belongs to the technical field of path planning, and particularly relates to a motion planning method based on improved A * and improved DWA fusion real-time topographic features. Comprising the following steps: S1, establishing a three-dimensional map model by adopting a grid method; s2, setting a starting point and an ending point of path planning in the three-dimensional map model; s3, extracting a passable area in the three-dimensional map model; s4, performing global path planning according to the starting point, the terminal point and the passable area, and generating global path nodes of the global path planning; and S5, extracting a global path node as a target node of local motion planning, and performing local path planning to obtain an optimal path. The system can adapt to complex and changeable cross-country environments. And moreover, a more stable path can be planned, and the influence caused by elevation information change is reduced, so that the maneuvering characteristic and the driving stability of the unmanned vehicle are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning, and particularly relates to a motion planning method based on the fusion of improved A* and improved DWA with real-time terrain features. Background Art

[0002] Motion planning is to plan an optimal trajectory from the starting point to the ending point according to the principle of minimum cost, ensuring that the user can reach the destination smoothly. A good path planning algorithm can save the user's time and reduce the residence time of vehicles in the road network, thereby improving the traffic congestion situation of the entire road network. Its core idea is to find the shortest path. With the continuous improvement of user requirements, the shortest path problem not only satisfies the shortest distance, but also extends to other metrics, such as time, cost, etc.

[0003] Currently, whether it is existing commercial software or laboratory scientific research, their research content mainly focuses on path planning problems under road conditions. However, traditional path planning methods have poor adaptability under non-road conditions and cannot meet the planning requirements of off-road environments. This is mainly caused by the following three reasons.

[0004] First, traditional algorithms mainly focus on planar planning, and the conventional two-dimensional grid model cannot effectively reflect the characteristics of off-road environments; second, traditional algorithms consider insufficient constraint conditions for motion planning. When performing motion planning in off-road environments, in addition to considering obstacles such as buildings that impede passage, factors such as the maneuverability of unmanned vehicles, surface materials, and terrain slopes also need to be considered; third, the planning objective function needs to flexibly adapt to off-road requirements and cannot directly use the path length or maneuvering time as the objective function, but needs to make real-time responses to the dynamic changes of the environment.

[0005] Therefore, considering off-road conditions, how to perform path planning is a current research trend and an urgent problem to be solved. Summary of the Invention

[0006] In order to solve the technical problem that the path planning method in the prior art has poor adaptability in off-road environments, the present invention provides a motion planning method based on the fusion of improved A* and improved DWA with real-time terrain features.

[0007] To achieve the above object, the technical solution of the present invention is as follows: A motion planning method based on the fusion of improved A* and improved DWA with real-time terrain features, comprising: S1. Establish a three-dimensional map model using the grid method; S2. Set the starting point and ending point of path planning in the three-dimensional map model; S3. Extract the passable area in the three-dimensional map model; S4. Based on the starting point, the ending point, and the passable area, perform global path planning to generate global path nodes for the global path planning. S5. Extract the global path nodes as the target nodes for local motion planning, and perform local path planning to obtain the optimal path.

[0008] Furthermore, in step S3, extracting the passable area in the three-dimensional map model specifically includes: S301. Establish a wheel-ground interaction model to calculate the predicted settlement of the wheel; set the maximum wheel settlement amount that the vehicle can pass. If the predicted settlement amount is lower than the maximum settlement amount, set the grid in the three-dimensional map model as passable, otherwise mark it as non-passable. S302. Obtain the positions and characteristics of the obstacles around the vehicle, project the obstacles in the three-dimensional map model, and mark all the grids that intersect or are covered by the obstacles as non-passable. S303. Calculate the slope of each grid in the three-dimensional map model relative to the grid adjacent to it in the vehicle direction, and set the maximum slope threshold and the minimum slope threshold that the vehicle can pass. If the slope of the grid exceeds the maximum slope threshold or is lower than the minimum slope threshold, the grid is marked as non-passable. S304. After accumulating the non-passable areas in steps S301, S302, and S303, obtain the passable area of the three-dimensional map model.

[0009] Even further, the predicted settlement amount is:

[0010] In the above formula, represents the predicted settlement amount; s is the slip ratio; is the static settlement amount.

[0011] Even further, the slip ratio s is:

[0012] In the above formula, r represents the wheel radius, represents the angular velocity of wheel rotation, represents the forward speed of the wheel.

[0013] Even further, the static settlement amount is:

[0014] In the above formula, W represents the load on the wheel, D represents the wheel diameter, Indicates the wheel width, Indicates the cohesion deformation modulus of the soil, Indicates the friction deformation modulus, Indicates the settlement index.

[0015] Furthermore, the slope between the grid and the adjacent grid in the 3D map model S is:

[0016] In the above formula, is the elevation change rate in the horizontal direction, is the elevation change rate in the vertical direction.

[0017] Furthermore, in step S4, the improved A* algorithm is used for global path planning.

[0018] Furthermore, when using the improved A* algorithm for global path planning, the cost function used is:

[0019]

[0020] In the above formula, 、 are weight coefficients, is the original heuristic function, S is the slope between the grid where the current node is located and the adjacent grid; represents the total cost from the starting point to the current node.

[0021] Furthermore, in step S5, the improved DWA algorithm is used for local path planning.

[0022] Furthermore, the evaluation function of the improved DWA algorithm is:

[0023] In the above formula, 、 、 、 are weight coefficients, is the heading evaluation function, is the speed evaluation function, is the distance evaluation function, is the traversal cost function.

[0024] Furthermore, the traversal cost function is calculated in the following way:

[0025] Among them, Indicates the slope passage cost function;

[0026] In the above formula, Indicates the maximum slope threshold; Indicates the slope of the vehicle at the grid point (i, j); Indicates the ground element passage cost function, and the value range is [0, 1].

[0027] Compared with the prior art, the present invention has the following beneficial effects: The motion planning method based on the fusion of improved A-star (A*) and improved DWA for real-time terrain features provided by the present invention first extracts the passable area in the grid map model, then performs global path planning, and then performs local path planning, considering the real-time three-dimensional terrain features, and thus can adapt to complex and changeable off-road environments. And it can plan a smoother path, reduce the influence brought by the change of elevation information, and thus improve the maneuverability and driving stability of the unmanned vehicle.

[0028] In addition, the present invention establishes a terrain subsidence estimation model to predict the subsidence degree under different terrain conditions. Combining the load constraint of the vehicle and the terrain subsidence index, a multi-modal feasible region discrimination algorithm is proposed to realize more accurate extraction of the passable area and improve the accuracy and adaptability of path planning.

[0029] Combining the ground element passage cost and elevation information, a comprehensive passage cost map is constructed, providing richer environmental information for path planning. A passage cost function is newly added to the DWA algorithm to meet the path planning requirements in off-road environments. Brief Description of the Drawings

[0030] Figure 1 is the flow chart of the present invention.

[0031] Figure 2 is the schematic diagram of slope solution based on the grid map.

[0032] Figure 3 is the schematic diagram of passable area extraction.

[0033] Figure 4 is the schematic diagram of the wheel-ground interaction model. Detailed Embodiment

[0034] The technical solutions of the present invention will be clearly described below with reference to the drawings. Obviously, the described embodiments are not all embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0035] It should be noted that, unless otherwise specifically stated, the relative arrangements of components and steps set forth in these embodiments and numerical expressions should not be construed as limiting the scope of the present invention.

[0036] The following description of exemplary embodiments is merely illustrative and in no sense limits the present invention or its application or use. Technologies, methods, and devices known to those of ordinary skill in the relevant art may not be discussed in detail herein, but when applicable, these technologies, methods, and devices should be regarded as part of this specification.

[0037] The present invention provides a motion planning method based on the fusion of improved A* and improved DWA for real-time terrain features, as Figure 1 shown, including: S1. Establish a three-dimensional map model using the grid method; S2. Set the starting point and ending point of path planning in the three-dimensional map model; S3. Extract the passable area in the three-dimensional map model; specifically including: S301. Establish a wheel-ground interaction model and calculate the predicted settlement of the wheel; set the maximum wheel settlement amount that the vehicle can pass. If the predicted settlement amount is lower than the maximum settlement amount, set the grid in the three-dimensional map model as passable, otherwise mark it as non-passable; Based on the Bekker bearing model, in the case of no slip, the settlement of the wheel is determined by the load on the wheel and the bearing characteristic parameters of the soil.

[0038] When a rigid wheel interacts with the soil, the following relationship exists:

[0039]

[0040]

[0041] Among them, is the load on the wheel, T is the driving torque, DP is the hitch traction force, r is the wheel radius, b is the wheel width; is the angle of entry into the ground, is the angle of exit from the ground, is the wheel-ground connection angle, is the normal stress of the rigid wheel on the soil, is the shear stress of the rigid wheel on the soil, as Figure 4 shown. Additionally, Figure 4 in, represents the wheel forward speed,W F represents the load on the wheel, and z represents the settlement amount.

[0042] Thus, the relationship between the normal stress and the wheel-ground connection angle is obtained:

[0043] Among them, is the cohesion deformation modulus of the soil, is the friction deformation modulus, N is the settlement index, is the angle corresponding to the maximum stress, describes the normal stress distribution in the wheel-ground contact area; is an extension of considering a more complex stress distribution; these parameters are the basic parameters characterizing the soil bearing characteristics.

[0044] The static settlement amount of the wheel The calculation formula is expressed as:

[0045] In the above formula, W represents the load on the wheel, D represents the wheel diameter, represents the wheel width, represents the cohesion deformation modulus of the soil, represents the friction deformation modulus, represents the settlement index.

[0046] In the case of wheel slip, the settlement amount will increase. Therefore, the corrected settlement amount considering slip is the predicted settlement amount, which is expressed as:

[0047] In the above formula, represents the predicted settlement amount; s is the slip ratio; is the static settlement amount.

[0048] Among them, the slip ratio s is:

[0049] In the above formula, r represents the wheel radius, represents the wheel rotational angular velocity, represents the wheel forward speed.

[0050] Among them, the maximum wheel settlement amount for the vehicle to pass is set according to the surface material.

[0051] S302. Obtain the positions and characteristics of obstacles around the vehicle, project the obstacles onto the 3D map model, and mark all the grid cells that intersect or are covered by the obstacles as impassable; Based on the environmental information transmitted in real time by the perception module, the vehicle can obtain the specific positions and characteristics of the surrounding obstacles. Process this information about the obstacles that cannot be crossed or collided with, and project it onto the grid map. Specifically, any cell grid that intersects or is covered by the obstacles will be marked as an impassable area.

[0052] S303. Calculate the slope of each grid cell in the 3D map model relative to the grid cell adjacent to it in the vehicle's direction, and set the maximum slope threshold and the minimum slope threshold that the vehicle can pass; if the slope of the grid cell exceeds the maximum slope threshold or is lower than the minimum slope threshold, then the grid cell is marked as impassable; The elevation extraction formula for the cell grid is:

[0053] where, i represents the index of the cell grid, k represents the index of each voxel belonging to the rectangular area, represents the total number of voxels belonging to this rectangular area.

[0054] For each grid cell in the grid map, as Figure 2 shown, calculate the slope relative to the grid cell adjacent to it in the direction of the unmanned vehicle. The slope S is:

[0055] In the above formula, is the elevation change rate in the horizontal direction, is the elevation change rate in the vertical direction.

[0056] S304. After accumulating the impassable areas in steps S301, S302, and S303, obtain the passable area of the 3D map model, as Figure 3 shown.

[0057] S4. According to the starting point, the ending point, and the passable area, perform global path planning to generate the global path nodes for global path planning; The traditional A* algorithm uses the cost function to represent the estimated cost:

[0058] where, represents the total cost from the starting point to the current node, represents the estimated cost from the current node to the ending point.

[0059] However, it should be noted that the traditional A* algorithm uses the Euclidean distance between two points as the cost, and the slope change caused by the elevation change can be ignored on the paved road surface. However, in the off-road environment of non-urban paved roads, the elevation change of the terrain may be very drastic. If only the shortest straight-line distance from the starting point to the ending point is pursued, the planned path may contain too many drastic elevation changes, which will seriously affect the driving performance and stability of the driverless vehicle. If the slope fluctuation in the planned path is too large, it may cause the path to be actually impassable. Therefore, it is necessary to improve the heuristic function to adapt to the off-road environment.

[0060] The terrain slope refers to the ratio of the change in ground height to the change in distance, that is, the degree of inclination of the ground. On a road or path, the change in slope can have a significant impact on vehicle driving. The change in slope will change the movement conditions of the vehicle on the road and have a significant impact on the traction demand. Due to the increase in gravity and ramp resistance, the vehicle requires greater traction to overcome the resistance of going uphill and maintain an appropriate speed and power. Therefore, the impact of slope undoubtedly poses higher requirements for traction.

[0061] In the grid map, the terrain slope describes the general inclination degree and unevenness of the terrain. For the slope, the greater the slope, the greater the ramp resistance and the worse the passing cost.

[0062] The present invention is based on the grid map and uses the central difference method to calculate the slope. For each grid cell, calculate the elevation difference between its surrounding adjacent grids. Usually, the adjacent grids in the vertical and horizontal positive directions are used to calculate the slope.

[0063] The improved cost function is:

[0064]

[0065] In the above formula, 、 are weight coefficients, is the original heuristic function, that is ; S is the slope between the grid where the current node is located and the adjacent grid. It should be noted that in order for the two to be at the same measurement level, normalization processing needs to be carried out before calculation.

[0066] S5. Extract the global path nodes as the target nodes for local motion planning, perform local path planning, and obtain the optimal path.

[0067] As a local path planning method, the DWA algorithm may lead to path planning failure or only find a local optimal solution when facing complex obstacle layouts such as C-shaped obstacles due to the lack of global path guidance. In addition, the traditional DWA algorithm performs poorly in dealing with the obstacle avoidance problem of dynamic obstacles, often requiring the vehicle to stop and wait for the obstacle to move away or choose a long-distance detour, making it difficult to achieve an ideal optimal path planning.

[0068] Therefore, the present invention proposes a fusion path planning strategy. First, the A* global planning algorithm considering elevation information is used to generate a global path. In this way, the unmanned vehicle can use the improved global path planning information to guide the DWA algorithm for local path planning, ensuring both effective local obstacle avoidance and global path optimization.

[0069] DWA (Dynamic Window Approach) is a motion planning algorithm that samples possible velocities considering the kinematic constraints of the robot, generates multiple predicted trajectories based on the current velocity, and evaluates these trajectories through an evaluation mechanism. Finally, the velocity command corresponding to the trajectory with the highest evaluation score is selected to control the movement of the robot. The DWA algorithm mainly includes two core links: velocity sampling and trajectory evaluation. The specific content of these two links will be elaborated below.

[0070] In the two-dimensional search space, the DWA algorithm periodically sends velocity commands to the unmanned vehicle to complete the task. If the unmanned vehicle can stop moving in time before encountering the nearest obstacle, then the velocity pair is a set of safe velocities. Assume the velocity vector the gap width between the corresponding trajectory and the obstacle is , the maximum acceleration / deceleration of the unmanned target vehicle is , represents the acceleration / deceleration of the linear velocity, represents the acceleration / deceleration of the angular velocity; to ensure that the unmanned vehicle can stop in time and avoid hitting the obstacle, it is necessary to determine the velocity set that satisfies the following conditions:

[0071] The velocity constraint space is defined as the set of feasible velocity-angular velocity of the unmanned vehicle. Among them, represents the minimum linear velocity of the unmanned vehicle, represents the maximum linear velocity of the unmanned vehicle, represents the minimum angular velocity of the unmanned vehicle, represents the maximum angular velocity of the unmanned vehicle. In summary, the velocity constraint space can be expressed as:

[0072] Acceleration constraint space It is defined as the set composed of the limit acceleration and angular acceleration of the driverless vehicle.

[0073]

[0074] Considering the three constraints of the driverless vehicle, the feasible speed set 𝑉 is defined as the set of speed-angular velocity combinations that satisfy all constraints. Therefore:

[0075] The traditional evaluation function of the DWA algorithm is:

[0076] Among them, , , are weight coefficients, which are used to calculate the weight values of sub-functions in the trajectory evaluation function.

[0077] is the heading evaluation function, which is used to evaluate the angle difference between the current moving direction and the target point; is the speed evaluation function, and the higher the speed, the higher the score; is the distance evaluation function, which represents the distance to the nearest obstacle, and the closer the distance, the lower the score.

[0078] The existing evaluation function is only applicable to roads with single attributes, flat road surfaces and small undulations. However, when performing motion planning in off-road environments, in addition to considering obstacles such as buildings that impede traffic, it is also necessary to consider influencing factors such as the dynamic performance of the driverless vehicle, the surface material, and the terrain slope. Obviously, the existing evaluation function cannot be fully applicable to off-road environments, and the existing algorithm needs to be improved to adapt to off-road environments.

[0079] After extracting the passable area, the passing costs of the extracted passable areas are also different, and the average speed of the vehicle will still decrease as the passing cost increases. This means that the traditional binary grid can only represent whether a grid point can be passed, and cannot express the specific impact of the terrain environment elements of the area on vehicle passing.

[0080] To express the different degrees of difficulty of the vehicle driving on different slopes within the maximum climbing gradient range, the elevation information is converted into the vehicle driving cost. The slope passing cost function of the vehicle at the (i, j) grid point is:

[0081] In the above formula, represents the maximum slope threshold, and the maximum slope threshold is set according to the maximum slope that the vehicle can pass; Denote the slope of the vehicle at the grid point (i, j); To express the difficulty of the vehicle in driving on different ground environments, a vehicle passing cost value corresponding to each ground element is established. That is, the ground element passing cost , and the value range is [0, 1]. The larger the value, the more difficult it is for the vehicle to drive on the grid with this ground element.

[0082] Passing cost function It is calculated in the following way:

[0083] The evaluation function of the improved DWA algorithm is:

[0084] In the above formula, , , , are weight coefficients, is the heading evaluation function, is the speed evaluation function, is the distance evaluation function, is the trajectory evaluation function.

[0085] Use the improved DWA algorithm for local path planning.

[0086] The above specific implementation manners are only used to illustrate the technical solutions of the present invention rather than to limit them. Although the present invention has been described in detail with reference to the examples, those of ordinary skill in the art should understand that the technical solutions of the present invention can be modified or equivalently replaced without departing from the scope of the technical solutions of the present invention, and they should all be covered within the scope of the claims of the present invention.

Claims

1. A motion planning method based on improved A* and improved DWA fusion of real-time terrain features, characterized in that: include: S1. Use the grid method to build a three-dimensional map model; S2. Setting the starting point and end point of the path planning in the three-dimensional map model; S3, extracting the traversable area in the three-dimensional map model; S4. Perform global path planning according to the starting point, the end point and the passable area, and generate a global path node of the global path planning; S5, extracting the global path node as the target node of the local motion planning, performing local path planning, and obtaining the optimal path; In step S3, extracting the traversable area in the three-dimensional map model specifically includes: S301, establishing a wheel-ground interaction model, calculating the predicted wheel sinking amount; setting the maximum wheel sinking amount that the vehicle can pass, if the predicted sinking amount is lower than the maximum sinking amount, setting the grid in the three-dimensional map model as passable, otherwise marking it as impassable; S302, obtaining the positions and features of obstacles around the vehicle, projecting the obstacles into the three-dimensional map model, and marking all grids intersecting or covering the obstacles as impassable; S303, calculating the slope of each grid in the three-dimensional map model relative to the grid immediately adjacent to the vehicle direction, and setting a maximum slope threshold and a minimum slope threshold that the vehicle can pass; if the slope of the grid exceeds the maximum slope threshold, or is lower than the minimum slope threshold, the grid is marked as impassable; S304, accumulating the impassable areas in step S301, step S302, and step S303 to obtain a traversable area of ​​the three-dimensional map model.

2. The motion planning method according to claim 1, characterized in that: The predicted subsidence is: In the above formula, It indicates the predicted subsidence amount; s is the slip rate; is the static settlement amount.

3. The motion planning method according to claim 2, characterized in that: The slip rate s is: In the above formula, r represents the wheel radius, is the wheel rotation angular velocity, Indicates the forward speed of the wheel; Static settlement for: In the above formula, W Indicates the load on the wheel. D Indicates the wheel diameter, Indicates the wheel width, represents the soil cohesive deformation modulus, represents the friction deformation modulus, Represents the subsidence index.

4. The motion planning method according to claim 1, characterized in that: Slope of grid and adjacent grid in 3D map model S for: In the above formula, is the rate of change of elevation in the horizontal direction, is the rate of change of elevation in the vertical direction.

5. The motion planning method according to claim 1, characterized in that: In step S4, the improved A* algorithm is used for global path planning.

6. The motion planning method according to claim 5, characterized in that: When using the improved A* algorithm for global path planning, the cost function used is: In the above formula, , is the weight coefficient, is the original heuristic function, S is the slope of the grid where the current node is located and the adjacent grid; Represents the total cost from the starting point to the current node.

7. The motion planning method according to claim 1, characterized in that: In step S5, the improved DWA algorithm is used to perform local path planning.

8. The motion planning method according to claim 7, characterized in that: The evaluation function of the improved DWA algorithm is: In the above formula, , , , is the weight coefficient, is the heading evaluation function, is the speed evaluation function, is the distance evaluation function, is the pass cost function.

9. The motion planning method according to claim 8, characterized in that: Pass cost function Calculated as follows: in, represents the slope pass cost function; In the above formula, represents the maximum slope threshold, represents the slope of the vehicle at the (i, j) grid point; Represents the ground feature passage cost function, with a value range of [0, 1].

Images