Local path planning method and device based on optimized D-star algorithm

Abstract

The invention discloses a local path planning method and device based on an optimized D-star algorithm. The method mainly comprises the following steps: acquiring point cloud environment data information around an unmanned vehicle; judging an obstacle; based on a grid map with obstacle information, a D * algorithm is used for planning an initial path, and in the searching process, according to the orientation relation between a current searching node and a starting point, selective expansion is conducted on adjacent nodes; and performing moving average processing on the obtained path to obtain a smooth path. According to the method and the device, the time and space complexity of path planning can be effectively reduced, a smooth path is obtained, the frequency change output by the controller is reduced, the jitter of the unmanned vehicle is reduced, and the attitude stability of the unmanned vehicle in the inspection process is ensured.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent driving path planning, and in particular to a local path planning method based on an optimized D-star or D* algorithm, which is used for local path planning when an unmanned vehicle encounters dynamic obstacles during its travel. Background Art

[0002] If the autonomous vehicle discovers that the actual environment differs from the initially planned map during its movement, or if a particular route proves to be more expensive than expected or encounters an obstacle, the algorithm can gradually replan to find a better path. Incremental replanning is computationally cheaper than full replanning, and the D* algorithm offers significant advantages in real-world environments with significant external disturbances.

[0003] While D* offers unparalleled advantages for path planning in dynamic environments, the algorithm also presents some challenges. Furthermore, it performs an extended search to the eight surrounding child nodes. During the search process, the algorithm diverges from the target location toward the starting location until the starting location is within the search range. While this algorithm can find an optimal path, it suffers from high algorithm overhead. Furthermore, the D* algorithm generates inflection points on the path, increasing the power consumption and time of the inspection vehicle. When the vehicle passes these inflection points, the thrusters receive highly variable signals, causing significant jitter and compromising the vehicle's stability. Summary of the Invention

[0004] The present disclosure provides a local path planning method based on an optimized D* algorithm, which is optimized on the basis of the classic D* algorithm: first, the time and space complexity of the D* algorithm is reduced to improve the real-time performance of the algorithm; then, the D* algorithm is combined with a sliding average algorithm to obtain a moving average through the path data, eliminating accidental changes and obtaining a smooth and natural path.

[0005] The local path planning method based on the optimized D* algorithm provided in the present disclosure mainly includes the following steps:

[0006] S1, obtains point cloud environment data information around the unmanned vehicle;

[0007] S2, pre-processing the acquired data, and counting the number of point clouds in the same map grid, and judging whether there are obstacles based on the number of point clouds in each grid;

[0008] S3, based on the grid map with obstacle information, uses the D* algorithm to plan a preliminary path. In the process of searching from the target location to the starting point, the adjacent nodes are selectively expanded according to the orientation relationship between the current search node and the starting point;

[0009] S4, performing sliding average processing on the obtained path to obtain a smooth path.

[0010] Furthermore, in step S1, a 32-line laser radar is used to obtain environmental information around the unmanned vehicle in real time, and the information is sent to a computer at a frequency of 10 frames per second for subsequent processing.

[0011] Furthermore, the step S2 specifically includes:

[0012] After acquiring the LiDAR data, the detection range is first intercepted, and then the effective information is retained according to the installation height of the LiDAR.

[0013] Calculate point cloud data based on position coordinates, count the number of point clouds in the same grid, and determine whether it is an obstacle based on the number of point clouds in each grid.

[0014] Furthermore, in step S3, the specific method of selectively expanding adjacent nodes according to the positional relationship between the current search node and the starting point includes:

[0015] Assume that the coordinates of the current node are (x1, y1), the coordinates of the starting node are (x2, y2), the current node is the coordinate origin, the horizontal direction is the x-axis, and the vertical direction is the y-axis. Then the direction relationship between the current node and the starting node is as follows:

[0016] (1) When x2-x1>0, y2-y1>0, the starting node is in the first quadrant of the current coordinate system, at node B, and adjacent nodes 5, 6, and 7 are expanded. Other adjacent nodes do not need to be expanded. The positions of the first to fourth quadrants are B, A, D, and C, respectively. The current node is 0, the left side is node 1, and the nodes 1-8 are arranged counterclockwise around the current node.

[0017] (2) When x2-x1<0, y2-y1>0, the starting node is in the second quadrant of the current coordinate system, at node A, and adjacent nodes 7, 8, and 1 are expanded. Other adjacent nodes do not need to be expanded;

[0018] (3) When x2-x1<0, y2-y1<0, the starting node is in the third quadrant of the current coordinate system, at node D, and adjacent nodes 1, 2, and 3 are expanded. Other adjacent nodes do not need to be expanded.

[0019] (4) When x2-x1>0, y2-y1<0, the starting node is in the fourth quadrant of the current coordinate system, at node C, and adjacent nodes 3, 4, and 5 are expanded. Other adjacent nodes do not need to be expanded;

[0020] If the above expansion conditions are not met, the classic D* algorithm is executed to expand all 8 adjacent nodes.

[0021] Furthermore, the specific method of step S4 includes:

[0022] Assume that the observation sequence x t , the true value is g t , the expected noise has a mean of 0 and a variance of δ 2 , in order to reduce the influence of noise, the observation values at adjacent moments are added and averaged:

[0023]

[0024] p t represents the filtering result at time t, x t-1 represents the observation value at time t-1, and n is the sliding window radius;

[0025] The sliding average algorithm is used to smooth the path and finally output a smooth path.

[0026] A local path planning device based on the optimized D* algorithm using the above method includes: a sensor data acquisition and processing module, a D* algorithm time and space complexity optimization module, and a D* algorithm path smoothing module;

[0027] The sensor data acquisition and processing module is used to obtain point cloud environmental data information around the unmanned vehicle in real time, pre-process the data, retain valid information, and determine whether there are obstacles;

[0028] The D* algorithm time and space complexity optimization module is used to plan a preliminary path based on a grid map with obstacle information using the D* algorithm. During the search process, adjacent nodes are expanded and selected based on the orientation relationship between the current search node and the starting point.

[0029] The D* algorithm path smoothing module is used to perform sliding average processing on the obtained path to obtain a smooth path.

[0030] Compared with the existing technology, the beneficial effects of the present invention are: ① Compared with the classic D* algorithm that diverges and expands aimlessly in a breadth-first manner, the present invention establishes directional guidance between the current node and the target node, which can greatly reduce the time and space complexity; ② The present invention optimizes the situation where the path planned by the D* algorithm has inflection points, which causes the unmanned vehicle to jitter during movement. Combined with the sliding average algorithm, it can meet the needs of the unmanned vehicle to quickly and in real time complete the path smoothing during movement and restore a smooth path; ③ The frequency change of the controller output is reduced, the jitter of the unmanned vehicle is reduced, and the stability of the posture of the unmanned vehicle during the inspection process is ensured. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] The above and other objects, features and advantages of the present disclosure will become more apparent through a more detailed description of exemplary embodiments of the present disclosure in conjunction with the accompanying drawings, wherein like reference numerals generally represent like components throughout the exemplary embodiments of the present disclosure.

[0032] Figure 1 is a flow chart of an exemplary optimized D* algorithm according to the present disclosure;

[0033] Figure 2 To improve the D* node expansion diagram;

[0034] Figure 3 is the path smoothing based on the sliding average algorithm. DETAILED DESCRIPTION

[0035] The preferred embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although preferred embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. Rather, these embodiments are provided to make the present disclosure more thorough and complete, and to fully convey the scope of the present disclosure to those skilled in the art.

[0036] The present disclosure provides a local path planning method based on the optimized D* algorithm. The flowchart of the exemplary embodiment is shown in the attached figure. Figure 1 As shown:

[0037] Environmental perception information collection is preferably completed by lidar. The D* algorithm performs real-time local path planning based on the surrounding environmental information. At the same time, a cost evaluation and path smoothing algorithm between the previous node and the starting node are designed to ultimately output a smooth path.

[0038] The details are as follows:

[0039] Step 1: Use a 32-line laser radar to obtain real-time environmental information around the unmanned vehicle and send it to the computer at a frequency of 10 frames per second;

[0040] Step 2: After the computer acquires the LiDAR data, it first intercepts the data within the detection range. Then, it retains the valid information above the ground based on the installation height of the LiDAR. It calculates the point cloud data based on the position coordinates and counts the number of point clouds in the same grid (in this embodiment, the grid size is 0.1m*0.1m). The number of point clouds in each grid determines whether it is an obstacle.

[0041] Step 3: Input the obtained grid map into the D* algorithm with optimized time and space complexity. The D* algorithm plans a path with inflection points based on the grid map with obstacle information.

[0042] Step 4: Input the obtained path into the sliding average algorithm for smoothing to obtain a smooth path.

[0043] In step 3, the D* algorithm searches for neighboring nodes by radiating from the target location until the starting point is expanded to the search range. During the search toward the starting point, there's no cost evaluation between the current node and the starting node, meaning there's no directional guidance or constraints, which results in significant time and space complexity. In this embodiment, the D* algorithm incorporates the directional relationship between the current search node and the starting point as a consideration for selecting neighboring nodes, minimizing computational overhead.

[0044] like Figure 2 As shown in the figure, 0 represents the current node, 1 to 8 represent the adjacent nodes of the current node, and the black grid represents the starting node. Assuming that the coordinates of the current node are (x1, y1), the coordinates of the starting node are (x2, y2), the current node is the coordinate origin, the horizontal direction is the x-axis, and the vertical direction is the y-axis, then the direction relationship between the current node and the starting node is as follows.

[0045] (1) When x2-x1>0, y2-y1>0, the starting node is in the first quadrant of the current coordinate system, at node B, and adjacent nodes 5, 6, and 7 are expanded. Other adjacent nodes do not need to be expanded.

[0046] (2) When x2-x1<0, y2-y1>0, the starting node is in the second quadrant of the current coordinate system, at node A, and adjacent nodes 7, 8, and 1 are expanded. Other adjacent nodes do not need to be expanded.

[0047] (3) When x2-x1<0, y2-y1<0, the starting node is in the third quadrant of the current coordinate system, at node D. Adjacent nodes 1, 2, and 3 are expanded, and other adjacent nodes do not need to be expanded.

[0048] (4) When x2-x1>0 and y2-y1<0, the starting node is in the fourth quadrant of the current coordinate system, at node C. Adjacent nodes 3, 4, and 5 are expanded, and other adjacent nodes do not need to be expanded.

[0049] If the above expansion conditions are not met, then the classic D* algorithm is executed to expand all 8 adjacent nodes. After the above operation, the number of expanded adjacent nodes is greatly reduced, reducing the time and space complexity of the algorithm search.

[0050] In step 4, the D* algorithm can quickly find a path, but the generated path is often unnatural and contains many unnecessary inflection points. In this embodiment, the path generated by the D* algorithm is smoothed using a sliding average algorithm to reduce the vibration of the unmanned vehicle and achieve more efficient and accurate path tracking. Figure 3The main advantage of the sliding average algorithm is its simplicity. For dynamic data processing, it uses recursive calculations, which reduces the amount of calculation and saves storage space. It can meet the needs of unmanned vehicles to quickly and in real time complete path smoothing during movement.

[0051] For an observation sequence, each observation value is noisy, and the expected noise has a mean of 0 and a variance of δ 2 , the relationship between the observed value and the true value is as follows:

[0052] g t =x t +ε t (1)

[0053] Among them, x t is the observed value, g t is the true value, ε t is noise. In order to reduce the impact of noise, the observation values at adjacent moments are added and averaged. The formula is as follows:

[0054]

[0055] p t represents the filtering result at time t, x t-1 Represents the observation value at time t-1, and n represents the sliding window radius.

[0056] Substituting formula (1) into formula (2), we can obtain:

[0057]

[0058] Assume that the mean of the noise is 0, so is 0, then the result is:

[0059]

[0060] When the true value of the observed data changes slightly, or changes linearly, it can be approximately considered that:

[0061]

[0062] Use the sliding average algorithm to smooth the path, and finally output a smooth path.

[0063] This embodiment uses lidar and an optimized path planning algorithm to achieve real-time and accurate local path planning, ensuring the unmanned vehicle's real-time obstacle avoidance and re-planning capabilities.

[0064] In another exemplary embodiment, a local path planning device based on the optimized D* algorithm using the above method mainly includes: a sensor data acquisition and processing module, a D* algorithm time and space complexity optimization module, and a D* algorithm path smoothing module;

[0065] The sensor data acquisition and processing module preferably uses a 32-line laser radar. The point cloud data of the 32-line laser radar is sent to the computer at a frequency of 10 frames per second. After receiving the point cloud data, the computer first intercepts the data within the detection range, and then retains the effective information above the ground according to the height of the laser radar installation;

[0066] The D* algorithm time and space complexity optimization module mainly restricts the search part of the classic D* algorithm. The classic D* algorithm diverges aimlessly in a breadth-first manner, while this embodiment establishes directional guidance between the current node and the target node, which can significantly reduce time and space complexity.

[0067] The D* algorithm path smoothing module is mainly used to optimize the situation where the path planned by the D* algorithm has inflection points, which causes the unmanned vehicle to jitter during movement. Combined with the sliding average algorithm, it can meet the requirements of the unmanned vehicle to quickly and in real time complete the path smoothing during movement, and restore a smooth path.

[0068] The above technical solutions are only exemplary embodiments of the present invention. For those skilled in the art, it is easy to make various types of improvements or modifications based on the application methods and principles disclosed in the present invention, and are not limited to the methods described in the above specific embodiments of the present invention. Therefore, the methods described above are only preferred and do not have a restrictive meaning.

Claims

1. A local path planning method based on an optimized D-star algorithm, comprising the following steps: S1, obtains point cloud environment data information around the unmanned vehicle; S2, pre-processing the acquired data, and counting the number of point clouds in the same map grid, and judging whether there are obstacles based on the number of point clouds in each grid; S3, based on the grid map with obstacle information, uses the D* algorithm to plan a preliminary path. In the process of searching from the target location to the starting point, the adjacent nodes are selectively expanded according to the orientation relationship between the current search node and the starting point; S4, performing sliding average processing on the obtained path to obtain a smooth path.

2. The method according to claim 1, characterized in that In step S1, a 32-line laser radar is used to obtain environmental information around the unmanned vehicle in real time, and the information is sent to a computer at a frequency of 10 frames per second for subsequent processing.

3. The method according to claim 2, characterized in that The step S2 specifically includes: After acquiring the LiDAR data, the detection range is first intercepted, and then the effective information is retained according to the installation height of the LiDAR. Calculate point cloud data based on position coordinates, count the number of point clouds in the same grid, and determine whether it is an obstacle based on the number of point clouds in each grid.

4. The method according to any one of claims 1 to 3, characterized in that In step S3, the specific method of selectively expanding adjacent nodes based on the positional relationship between the current search node and the starting point includes: Assume that the coordinates of the current node are (x1, y1), the coordinates of the starting node are (x2, y2), the current node is the coordinate origin, the horizontal direction is the x-axis, and the vertical direction is the y-axis. Then the direction relationship between the current node and the starting node is as follows: (1) When x2-x1>0, y2-y1>0, the starting node is in the first quadrant of the current coordinate system, at node B, and adjacent nodes 5, 6, and 7 are expanded. Other adjacent nodes do not need to be expanded. The positions of the first to fourth quadrants are B, A, D, and C, respectively. The current node is 0, the left side is node 1, and the nodes 1-8 are arranged counterclockwise around the current node. (2) When x2-x1<0, y2-y1>0, the starting node is in the second quadrant of the current coordinate system, at node A, and adjacent nodes 7, 8, and 1 are expanded. Other adjacent nodes do not need to be expanded; (3) When x2-x1<0, y2-y1<0, the starting node is in the third quadrant of the current coordinate system, at node D, and adjacent nodes 1, 2, and 3 are expanded. Other adjacent nodes do not need to be expanded. (4) When x2-x1>0, y2-y1<0, the starting node is in the fourth quadrant of the current coordinate system, at node C, and adjacent nodes 3, 4, and 5 are expanded. Other adjacent nodes do not need to be expanded; If the above expansion conditions are not met, the classic D* algorithm is executed to expand all 8 adjacent nodes.

5. The method according to claim 1, wherein The specific method of step S4 includes: Assume that the observation sequence x t , the true value is g t , the expected noise has a mean of 0 and a variance of δ 2 , in order to reduce the influence of noise, the observation values at adjacent moments are added and averaged: p t represents the filtering result at time t, x t-1 represents the observation value at time t-1, and n is the sliding window radius; The sliding average algorithm is used to smooth the path and finally output a smooth path.

6. A local path planning device based on an optimized D-star algorithm using the method according to any one of claims 1 to 5, characterized in that: include: Sensor data acquisition and processing module, D* algorithm time and space complexity optimization module, D* algorithm path smoothing module; The sensor data acquisition and processing module is used to obtain point cloud environmental data information around the unmanned vehicle in real time, pre-process the data, retain valid information, and determine whether there are obstacles; The D* algorithm time and space complexity optimization module is used to plan a preliminary path based on a grid map with obstacle information using the D* algorithm. During the search process, adjacent nodes are expanded and selected based on the orientation relationship between the current search node and the starting point. The D* algorithm path smoothing module is used to perform sliding average processing on the obtained path to obtain a smooth path.

Images