AGV dynamic path planning and obstacle avoidance implementation method based on improved D star algorithm, AGV trolley and medium

Abstract

The invention belongs to the technical field of mobile robot navigation, and particularly discloses an AGV dynamic path planning and obstacle avoidance implementation method based on an improved D star algorithm, an AGV trolley and a medium. The method disclosed by the invention comprises the following steps: firstly, on one hand, carrying out environment modeling based on map information, adding safety expansion to obstacles, and generating a two-dimensional grid map; on the other hand, the weight of the turning penalty term is added into the heuristic function, and a new heuristic function is generated; and then, an AGV moving path is obtained by using an improved D star algorithm, so that the AGV moves according to the path. If a new obstacle is monitored in the moving process, an obstacle influence domain is delimited again, a new heuristic function is used for path re-planning, and smooth processing is carried out to generate a new moving path. According to the method, the re-planning speed and the path optimization rate of the AGV in the path planning and real-time obstacle avoidance process are improved, the operation efficiency is improved, and the overall performance of the algorithm is improved. The method can be widely applied to navigation and path planning of the AGV.

Description

Technical Field

[0001] The invention belongs to the technical field of mobile robot navigation, and specifically relates to an AGV dynamic path planning and obstacle avoidance method based on an improved D-star algorithm, an AGV trolley and a medium. Background Art

[0002] The D-star algorithm is a path planning algorithm based on dynamic priority updates. The algorithm adapts to environmental changes by dynamically updating node priorities. It is particularly suitable for scenarios where dynamic obstacles frequently appear. Its core advantage lies in the ability to adjust the path in real time under dynamic environments. Its core is to maintain two key values ​​for each node: the ‌h value‌ represents the current estimate of the shortest path cost from the node to the target, and the ‌k value‌ records the historical state of the minimum h value when the node was added to the priority queue. The algorithm determines the priority of the node by comparing its k value and h value: if the k value is less than the h value, it means that the node may have a better path and needs to be re-expanded to update the path information. This mechanism enables D-star to quickly correct the path locally when obstacles appear or the cost changes, avoiding global re-planning. It is particularly suitable for scenarios where obstacles frequently change, such as warehousing and logistics. However, although the D-star algorithm is highly adaptable in theory, in the actual deployment of AGVs, the contradiction between its dynamic update mechanism and complex physical constraints has gradually become prominent, leading to many problems such as efficiency, stability and path feasibility. ‌

[0003] The bottlenecks of the D-star algorithm in AGV applications are mainly reflected in real-time and physical adaptability. First, its re-planning process needs to traverse nodes affected by dynamic obstacles, and the computational complexity grows exponentially with the map size. For example, the re-planning of a 10m×10m (0.1m resolution) map takes more than 200ms, which is difficult to meet the real-time requirements of highly dynamic scenes. Frequent path updates can easily cause AGV direction oscillations and reduce operational continuity. Second, the algorithm does not incorporate the AGV kinematic model, resulting in the planned path being unable to execute due to sudden changes in curvature. At the same time, its heuristic function is not accurate enough in complex obstacles or unstructured environments. For example, it relies too much on Manhattan distance estimation and underestimates the actual travel cost, causing the path to deviate from the global optimum or even planning failure. These defects jointly restrict the practical value of the D-star algorithm in AGV applications. ‌‌ Summary of the invention

[0004] The purpose of the present invention is to provide an AGV dynamic path planning and obstacle avoidance method based on an improved D-star algorithm, which improves the replanning speed and path optimization rate of the AGV in the process of path planning and real-time obstacle avoidance by optimizing the obstacle influence domain and introducing the weight of the steering penalty in the heuristic function, greatly improves the operating efficiency, and enhances the overall performance of the algorithm;

[0005] The second object of the present invention is to provide an AGV, which applies the above-mentioned AGV dynamic path planning and obstacle avoidance method based on the improved D-star algorithm when running;

[0006] The third object of the present invention is to provide a computer-readable storage medium for storing a corresponding computer program for AGV dynamic path planning and obstacle avoidance method based on the improved D-star algorithm.

[0007] In order to achieve the above object, the technical scheme adopted by the present invention is as follows:

[0008] An AGV dynamic path planning and obstacle avoidance method based on an improved D-star algorithm includes the following steps performed in sequence:

[0009] S1. On the one hand, environmental modeling is performed based on the existing map information, and safety expansion is added to obstacles to generate a two-dimensional grid map; on the other hand, the weight of the steering penalty term is added to the heuristic function of the D-star algorithm to generate a new heuristic function;

[0010] S2, using the improved D-star algorithm to obtain the path for the AGV to move to the target position, so that the AGV moves according to the path; step S2 includes the following steps performed in sequence:

[0011] S21, based on the new heuristic function, generate an initialization AGV moving path, and let the AGV move according to the initialization path;

[0012] S22, real-time detection of new obstacles during AGV movement;

[0013] If a new obstacle is detected, step S23 is executed;

[0014] If no new obstacle is detected, go to step S27;

[0015] S23, re-define the influence domain of the obstacle, and only update the points whose h values ​​are affected in the influence domain of the obstacle;

[0016] S24, use the new heuristic function to replan the path;

[0017] S25, smoothing the replanned path to obtain a new moving path;

[0018] S26, let the AGV move according to the new moving path, and return to step S22;

[0019] S27, AGV moves to the target position.

[0020] As a limitation, the method for re-defining the influence area of ​​the obstacle in step S23 is:

[0021] Obtain the obstacle position, and define the range of the Euclidean distance M from the obstacle position as the obstacle influence domain; M>0.

[0022] As a second limitation, the new heuristic function is:

[0023]

[0024]

[0025] In the formula, is the new heuristic function, and is the weight value, is the Manhattan distance, For the steering penalty term, is the coefficient, is the accumulated steering angle.

[0026] As a further limitation, the calculation formula of the cumulative steering angle includes:

[0027]

[0028] In the formula, It is the orientation angle of the AGV at the current point.

[0029] As a third limitation, the safety expansion in step S1 is to increase N on the basis of the obstacle radius as a new obstacle radius; N>0.

[0030] As a fourth limitation, step S25 includes the following steps performed in sequence:

[0031] S251, sorting the starting point, target point and each turning point on the replanned path from large to small according to the k value, and putting them into the feature point set A;

[0032] S252, calculate every two adjacent points in set A and Straight line distance ;

[0033] like ≤0.4m, take and midpoint Insert into set A, the insertion points are in the order of the path;

[0034] like >0.4m, then take the line segment Upper distance 0.2m point , and midpoint And distance 0.2m point , and , and Insert into set A, the insertion points are in the order of the path;

[0035] S253, according to The size of is used to fit the Saibel curve to the points of set A;

[0036] when ≤0.4m, Starting from the previous point of As the control point, As the target point, a second-order Saybell curve fitting is performed;

[0037] when >0.4m, Starting from the previous point of and As the control point, Target point Perform third-order Bezier curve fitting;

[0038] S254, taking the path obtained by Bezier curve fitting in step S253 as a new moving path.

[0039] An AGV vehicle, comprising a laser radar, an encoder, a drive motor group, and a controller deployed with the above method;

[0040] The laser radar is used to scan the surrounding environment of the AGV in real time, obtain point cloud data, and transmit the point cloud data to the controller;

[0041] The encoder is used to record the rotation angle of the AGV small wheel group and transmit the angle data to the controller;

[0042] The controller is used to receive point cloud data and angle data, calculate the moving path of the trolley, generate movement data, and transmit the movement data to the drive motor group;

[0043] The driving motor group receives and responds to the movement data transmitted by the controller to drive the AGV vehicle to move forward and turn.

[0044] A computer-readable storage medium stores a computer program, which, when executed by a processor, is used to implement the above-mentioned AGV dynamic path planning and obstacle avoidance method based on an improved D-star algorithm.

[0045] Due to the adoption of the above technical solution, the present invention has the following technical advances compared with the prior art:

[0046] (1) The method of the present invention improves the D-star algorithm, thereby increasing the replanning speed and path optimization rate of the AGV in the process of path planning and real-time obstacle avoidance, greatly improving the operating efficiency and the overall performance of the algorithm;

[0047] (2) The method of the present invention adopts a new obstacle influence domain division. When a new obstacle appears, only the node information in the local area needs to be updated instead of the global map. This local update mechanism avoids repeated calculations of the global map, significantly reduces the amount of calculation when dynamically adjusting the path, and improves the algorithm's response speed to dynamic obstacles.

[0048] (3) The method of the present invention introduces the weight of the turning penalty term in the heuristic function, increases the cost weight of turning, avoids the path zigzag problem caused by frequent direction adjustment, makes the generated path closer to the actual motion constraints, reduces the sudden turning caused by temporary obstacle avoidance under the interference of dynamic obstacles, and enhances the coherence of the path;

[0049] (4) The method of the present invention introduces safe expansion of obstacles. By expanding the radius of obstacles, it can effectively offset the influence of sensor positioning errors or dynamic changes in the environment, thereby reducing the risk of collision. The expanded obstacle boundary can eliminate feasible channels that are too close to obstacles in the original path, reduce the complexity of path planning, and improve the convergence speed of the algorithm.

[0050] (5) In the method of the present invention, the k values ​​are sorted before curve fitting to ensure that high-weight turning points are preferentially involved in curve fitting, so as to avoid minor path details interfering with the main motion logic and make the control point distribution of curve fitting more consistent with the actual motion constraints;

[0051] (6) In the method of the present invention, a dynamic interpolation strategy is implemented based on the distance between adjacent points before curve fitting, and control points are encrypted in high curvature areas, thereby avoiding the phenomenon of path straightening due to insufficient control points during fitting, and retaining the washing point set in the low curvature area, thereby reducing redundant calculations and simplifying the calculations;

[0052] (7) In the method of the present invention, Saibel curve fitting is selected for curve fitting. By controlling the distance between control points and direction constraints, the path of the vehicle can be quickly generated. The shape of the path can be partially corrected by adjusting the control points. While reducing the consumption of computing resources for path generation, the continuous and smooth change of the path is ensured, avoiding sudden turns or acceleration changes of the AGV vehicle.

[0053] The present invention belongs to the technical field of mobile robot navigation. By improving the D-star algorithm, the re-planning speed and path optimization rate of the AGV in the process of path planning and real-time obstacle avoidance are improved, the operation efficiency is greatly improved, and the overall performance of the algorithm is enhanced. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] The accompanying drawings are used to provide further understanding of the present invention and constitute a part of the specification. They are used to explain the present invention together with the embodiments of the present invention and do not constitute a limitation of the present invention.

[0055] In the attached picture:

[0056] Figure 1 This is a processing flow chart of Embodiment 1 of the present invention;

[0057] Figure 2 This is a structural block diagram of embodiment 2 of the present invention. DETAILED DESCRIPTION

[0058] The preferred embodiments of the present invention are described below in conjunction with the accompanying drawings. It should be understood that the preferred embodiments described herein are only used to illustrate and explain the present invention, and are not used to limit the present invention.

[0059] Example 1

[0060] This embodiment is a method for AGV dynamic path planning and obstacle avoidance based on the improved D-star algorithm. Figure 1 As shown, this embodiment includes the following steps performed in sequence:

[0061] S1. On the one hand, environmental modeling is performed based on the existing map information, and safety expansion is added to obstacles to generate a two-dimensional grid map. On the other hand, the weight of the steering penalty term is added to the heuristic function of the D-star algorithm to generate a new heuristic function.

[0062] Before planning the path for the AGV, it is necessary to obtain a two-dimensional grid map. Therefore, it is necessary to model the environment based on the existing map information.

[0063] When modeling the environment, obstacles need to be classified and marked: static obstacles are directly marked as inaccessible areas, and dynamic obstacles are set to survive for 30 seconds and automatically cleared after the timeout. Before generating a two-dimensional grid map, it is also necessary to add safety expansion to the obstacles: add 0.2m to the obstacle radius as the new obstacle radius to avoid collision risks. In this embodiment, in order to ensure the path accuracy of the AGV, the grid resolution of the two-dimensional grid map is 0.1m×0.1m, and the grid resolution here can be modified according to actual conditions.

[0064] Before planning the path for the AGV, the weight of the steering penalty term needs to be added to the heuristic function.

[0065] The heuristic function is a function used in the path planning algorithm to evaluate the path quality. In the path planning of the AGV, in order to consider the turning penalty, the turning penalty term is also added to the heuristic function. Specifically, the heuristic function can be expressed as the weighted sum of the Manhattan distance and the turning penalty term. In this way, the heuristic function can comprehensively consider the length of the path and the frequency of turning, thereby selecting a smoother and more efficient path.

[0066] Turning penalty is a mechanism introduced to prevent the car from turning too frequently during path planning. It calculates a penalty term based on the cumulative turning angle, which is added to the total cost of the path. When the car turns too much or turns too many times, the turning penalty term will increase, making the total cost of this path higher and reducing the probability of it being selected.

[0067] The cumulative steering angle refers to the sum of all steering angles of the AGV during operation. This angle is obtained by calculating the absolute value of the steering angle difference of the vehicle at each turning point and adding up these differences. This value reflects the frequency and total magnitude of the vehicle's steering during path planning.

[0068] Specifically, the heuristic function for adding the weight of the steering penalty term is:

[0069]

[0070]

[0071] In the formula, is the heuristic function, and is the weight value, =0.7, =0.3, is the Manhattan distance, For the steering penalty term, is the coefficient, =10°, is the accumulated steering angle.

[0072] The calculation formula for the cumulative steering angle includes:

[0073]

[0074] In the formula, It is the orientation angle of the AGV at the current point.

[0075] S2. Use the improved D-star algorithm to obtain the path for the AGV to move to the target location, so that the AGV moves according to the path.

[0076] Specifically, the method comprises the following steps which are performed in sequence:

[0077] S21. Based on the new heuristic function, generate an initialization AGV moving path, and let the AGV move according to the initialization path.

[0078] Before the AGV starts moving, it first generates an initialization path based on the new heuristic function according to the D-star algorithm: backpropagates the cost value from the target point, builds an OPEN priority queue according to the size of the k value, and outputs the node sequence of the initialization path.

[0079] S22, real-time detection of new obstacles during AGV movement;

[0080] If a new obstacle is detected, step S23 is executed;

[0081] If no new obstacle is detected, go to step S27.

[0082] After the AGV moves along the initialized path, the lidar will monitor in real time whether there are new obstacles during the movement.

[0083] S23. Redefine the influence domain of the obstacle, and only update the points whose h values ​​are affected in the influence domain of the obstacle.

[0084] After detecting a new obstacle, the obstacle position is obtained, and the range of 3.0m of Euclidean distance from the obstacle position is defined as the obstacle influence domain. Only the points whose h values ​​are affected in the obstacle influence domain are updated, and the k values ​​of the unchanged nodes are reused.

[0085] S24. Use the new heuristic function to replan the path.

[0086] S25. Smoothing the replanned path to obtain a new moving path.

[0087] After the path is replanned, since the path is generated based on a grid structure and is connected by discrete nodes, there may be right-angle turns or jagged trajectories, so the replanned path needs to be smoothed.

[0088] Specifically, the smoothing process includes the following steps performed in sequence:

[0089] S251. Sort the starting point, target point and each turning point on the replanned path from large to small according to the k value, and put them into the feature point set A.

[0090] S252, calculate every two adjacent points in set A and Straight line distance ;

[0091] like ≤0.4m, take and midpoint Insert into set A, the insertion points are in the order of the path;

[0092] like >0.4m, then take the line segment Upper distance 0.2m point , and midpoint And distance 0.2m point , and , and Insert into set A, with the insertion points in the order of the path.

[0093] S253, according to The size of is used to fit the Saibel curve to the points of set A;

[0094] when ≤0.4m, Starting from the previous point of As the control point, As the target point, a second-order Saybell curve fitting is performed;

[0095] when >0.4m, Starting from the previous point of and As the control point, Target point Perform third-order Bezier curve fitting.

[0096] The choice of starting point here is based on The size is determined by:

[0097] like ≤0.4m, then the starting point is ;

[0098] like >0.4m, the starting point is .

[0099] S254, taking the path obtained by Bezier curve fitting in step S253 as a new moving path.

[0100] S26: Let the AGV move along the new moving path and return to step S22.

[0101] Every time a new obstacle is encountered, the above method is used to obtain a new moving path. After obtaining the new moving path, the AGV moves along the new path until it moves to the target position.

[0102] S27, AGV moves to the target position.

[0103] In this embodiment, M is 3.0m, N is 0.2m, is 0.7, is 0.3, The above data are not unique values ​​and can be modified according to the actual movement of the AGV.

[0104] In summary, this embodiment improves the replanning speed and path optimization rate of the AGV during path planning and real-time obstacle avoidance by optimizing the obstacle influence domain and introducing the weight of the steering penalty in the heuristic function, thereby greatly improving the operating efficiency and the overall performance of the algorithm.

[0105] Example 2

[0106] This embodiment is an AGV vehicle, which moves according to the method described in Embodiment 1. Its components are as follows: Figure 2 As shown, it includes a laser radar, an encoder, a drive motor group, and a controller deployed with the method described in Example 1.

[0107] The laser radar has a scanning frequency of 10 Hz and a scanning accuracy of ±2 cm. It is used to scan the surrounding environment of the AGV in real time, obtain point cloud data, and transmit the point cloud data to the controller.

[0108] The encoder is used to record the rotation angle of the AGV wheel group and transmit the angle data to the controller with an accuracy of 0.1m and a sampling period of 10ms.

[0109] The controller uses ARM Cortex-A72 to receive point cloud data and angle data, calculate the movement path of the car, generate movement data, and transmit the movement data to the drive motor group.

[0110] The drive motor group is a differential drive motor group, which is used to receive and respond to the movement data transmitted by the controller to drive the AGV to move and turn, and its maximum steering angle is ±45°.

[0111] The maximum linear speed of the AGV during operation is 1.2m / s, and the angular speed is limited to ±1.5rad / s. When the distance between the AGV and the obstacle is less than 0.25m, the vehicle stops moving, replans a new path, and then runs again.

[0112] Example 3

[0113] This embodiment is a computer-readable storage medium. The computer-readable storage medium in this embodiment stores a computer program. When the computer program is executed by a processor, it is used to implement an AGV dynamic path planning and obstacle avoidance method based on an improved D-star algorithm in Example 1.

[0114] Among them, the computer-readable storage medium can be a computer storage medium or a communication medium. The communication medium includes any medium that facilitates the transmission of a computer program from one place to another. The computer storage medium can be any available medium that can be accessed by a general-purpose or special-purpose computer. For example, a computer-readable storage medium is coupled to a processor so that the processor can read information from the computer-readable storage medium and write information to the computer-readable storage medium. Of course, the computer-readable storage medium can also be a component of the processor. The processor and the computer-readable storage medium can be located in an application-specific integrated circuit (ASIC). In addition, the ASIC can be located in a user device. Of course, the processor and the computer-readable storage medium can also exist in a communication device as discrete components. Specifically, the computer-readable storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as a static random access memory (SRAM), an electrically erasable programmable read-only memory (EEPROM), an erasable programmable read-only memory (EPROM), a programmable read-only memory (PROM), a read-only memory (ROM), a magnetic memory, a flash memory, a magnetic disk or an optical disk, etc. The storage medium can be any available medium that can be accessed by a general-purpose or special-purpose computer.

Claims

1. An AGV dynamic path planning and obstacle avoidance method based on an improved D-star algorithm, characterized in that: The process includes the following steps: S1. On the one hand, environmental modeling is performed based on the existing map information, and safety expansion is added to obstacles to generate a two-dimensional grid map; on the other hand, the weight of the steering penalty term is added to the heuristic function of the D-star algorithm to generate a new heuristic function; S2, using the improved D-star algorithm to obtain the path for the AGV to move to the target position, so that the AGV moves according to the path; step S2 includes the following steps performed in sequence: S21, based on the new heuristic function, generate an initialization AGV moving path, and let the AGV move according to the initialization path; S22, real-time detection of new obstacles during AGV movement; If a new obstacle is detected, step S23 is executed; If no new obstacle is detected, go to step S27; S23, re-define the influence domain of the obstacle, and only update the points whose h values ​​are affected in the influence domain of the obstacle; S24, use the new heuristic function to replan the path; S25, smoothing the replanned path to obtain a new moving path; S26, let the AGV move according to the new moving path, and return to step S22; S27, AGV moves to the target position.

2. The AGV dynamic path planning and obstacle avoidance method based on the improved D-star algorithm according to claim 1 is characterized in that: The method for re-defining the influence area of ​​the obstacle in step S23 is: Obtain the obstacle position, and define the range of the Euclidean distance M from the obstacle position as the obstacle influence domain; M>0.

3. The AGV dynamic path planning and obstacle avoidance method based on the improved D-star algorithm according to claim 1 or 2, characterized in that: The new heuristic function is: In the formula, is the new heuristic function, and is the weight value, is the Manhattan distance, is the turning penalty term, is the coefficient, is the accumulated steering angle.

4. The AGV dynamic path planning and obstacle avoidance method based on the improved D-star algorithm according to claim 3 is characterized in that: The calculation formula of the cumulative steering angle includes: In the formula, It is the orientation angle of the AGV at the current point.

5. The AGV dynamic path planning and obstacle avoidance method based on the improved D-star algorithm according to claim 1 is characterized in that: The safety expansion in step S1 is to increase N on the basis of the obstacle radius as a new obstacle radius; N>0.

6. The AGV dynamic path planning and obstacle avoidance method based on the improved D-star algorithm according to claim 1 is characterized in that: Step S25 includes the following steps performed in sequence: S251, sorting the starting point, target point and each turning point on the replanned path from large to small according to the k value, and putting them into the feature point set A; S252, calculate every two adjacent points in set A and Straight line distance ; like ≤0.4m, take and midpoint Insert into set A, the insertion points are in the order of the path; like >0.4m, then take the line segment Upper distance 0.2m point , and midpoint And distance 0.2m point , and , and Insert into set A, the insertion points are in the order of the path; S253, according to The size of is used to fit the Saibel curve to the points of set A; when ≤0.4m, Starting from the previous point of As the control point, As the target point, a second-order Saybell curve fitting is performed; when >0.4m, Starting from the previous point of and As the control point, As the target point, a third-order Bezier curve is fitted; S254, taking the path obtained by Bezier curve fitting in step S253 as a new moving path.

7. An AGV vehicle, comprising a laser radar, an encoder, and a drive motor group, characterized in that: Also includes a controller deployed with the method according to any one of claims 1 to 6; The laser radar is used to scan the surrounding environment of the AGV in real time, obtain point cloud data, and transmit the point cloud data to the controller; The encoder is used to record the rotation angle of the AGV small wheel group and transmit the angle data to the controller; The controller is used to receive point cloud data and angle data, calculate the moving path of the trolley, generate movement data, and transmit the movement data to the drive motor group; The driving motor group receives and responds to the movement data transmitted by the controller to drive the AGV vehicle to move forward and turn.

8. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, which, when executed by a processor, is used to implement an AGV dynamic path planning and obstacle avoidance method based on an improved D-star algorithm as described in any one of claims 1 to 6.

Images