A robot path planning method and system based on improved DWA algorithm

Abstract

This application belongs to the field of path planning, and specifically relates to a robot path planning method and system that improves the DWA algorithm. This application combines an omnidirectional kinematic model and a dynamic cost field to improve the robot's obstacle avoidance ability and path planning performance in complex environments. By introducing the omnidirectional kinematic model, the robot can move flexibly in any direction within a plane, thus overcoming the limitations of the traditional DWA algorithm. Through the dynamic cost field, the robot can evaluate and avoid dynamic objects in real time, making path planning more intelligent and efficient. This improvement provides more reliable and efficient technical support for the robot's autonomous navigation in dynamic and complex environments.

Description

Technical Field

[0001] The present application belongs to the field of path planning, and specifically relates to a robot path planning method and system for improving the DWA algorithm. Background Art

[0002] Path planning technology is one of the core technologies in the field of robotics. It involves how to enable robots to autonomously determine their movement paths in a dynamic environment and avoid obstacles in real time. Traditional path planning methods can be roughly divided into global path planning and local path planning. Global path planning builds an environmental map and calculates the best path from the starting point to the end point based on algorithms (such as A*); local path planning focuses on how to avoid obstacles in real time in a dynamic environment to ensure that the robot can travel smoothly along the route planned by the global path.

[0003] Among them, the "Dynamic Window Algorithm (DWA)" is a technology widely used in local path planning. The DWA algorithm predicts multiple possible future trajectories based on the current state of the robot (position, speed, acceleration limit, etc.), and evaluates the safety and superiority of each trajectory based on a preset cost function. By comparing the costs of different trajectories, the DWA algorithm selects the trajectory with the lowest cost as the current motion trajectory of the robot. However, the traditional DWA algorithm has some limitations when dealing with dynamic objects, complex environments, and omnidirectional motion. Summary of the invention

[0004] Based on the above problems, this application proposes an improved DWA algorithm that combines the omnidirectional kinematic model and the dynamic cost field to improve the robot's obstacle avoidance and path planning performance in complex environments. The technical solution is:

[0005] A robot path planning method for improving the DWA algorithm includes the following steps:

[0006] S1. Initialize environment map information and robot information;

[0007] S2. Use the A* algorithm to obtain the global planning path;

[0008] The dynamic window set by S3.DWA will give the linear velocity and angular velocity of the robot at the next moment. A cost field that is updated in real time is added to the evaluation function of the DWA algorithm to give a cost value to each dynamic object, and optimize the robot's path by evaluating the environment in real time.

[0009] S4. The robot avoids obstacles using the optimal path.

[0010] Preferably, step S3 includes:

[0011] S31. Perform image acquisition and preprocess images using the LKT optical flow method;

[0012] S32. Calculate the cost of each dynamic object in the dynamic window;

[0013] S33. Calculate the cost of each candidate path and retain the path with the smallest cost.

[0014] Preferably, the environmental map information in step S1 is provided by a grid map constructed by a robot SLAM system, and each grid unit in the grid map identifies the passability of each area; the robot information initialization includes the robot's current position, orientation, and linear speed information.

[0015] Preferably, in step S2, the optimal path is selected by calculating the total cost of each node, and the node cost calculation formula is as follows:

[0016] ;

[0017] Represents the actual cost from the starting point to the current node, It represents the estimated cost from the current node to the end point, so as to select the node with the smallest cost value and finally generate the shortest path from the starting point to the end point.

[0018] Preferably, in step S3, the robot movement speed , robot movement direction , use the omnidirectional motion model to calculate the robot's motion component along the X axis , the motion component along the Y axis and the robot's in-plane angular velocity ; The solution formula is:

[0019] ;

[0020] ;

[0021] ;

[0022] At each subsequent time step, the system generates a motion combination ( , , ) to predict all possible motion trajectories of the robot; at the same time, according to the current movement speed, acceleration and turning speed of the robot, DWA will define a dynamic window, which will limit the linear velocity and angular velocity that the robot will take in the next moment.

[0023] Preferably, in step S31, the average optical flow vector of the entire image is calculated , find the pixels that deviate from the background motion, i.e. dynamic objects;

[0024] The calculation formula for the average optical flow vector is as follows:

[0025] ;

[0026] ;

[0027] Where N represents the number of pixels in the entire image, The pixel's motion velocity vector;

[0028] Calculate the deviation value for each pixel:

[0029] ;

[0030] ;

[0031] if If the value is less than or equal to the threshold, it means that the pixel is static, and if it is greater than the set threshold T, it means that the pixel is dynamic;

[0032] The above result is the displacement speed of the pixel, not the displacement speed of the object, so unit conversion is required:

[0033] ;

[0034] ;

[0035] ;

[0036] in D Indicates the depth of an object; F Indicates the focal length of the camera; Represents the optical flow speed of an object, Indicates the actual speed of the object.

[0037] Preferably, in step S32, the cost value of each dynamic object in the dynamic window is calculated as follows:

[0038] The displacement of dynamic objects ,speed Substitute the cost value calculation formula to calculate the cost value of each object;

[0039] The cost value calculation formula is as follows:

[0040] ;

[0041] C represents the cost value of the object; r represents the radius of the object; ε is a small constant to prevent division by zero; Is a coefficient used to adjust the impact of speed on the cost; the calculated cost value will affect the priority of trajectory selection. The area with a higher cost value will be considered a "worse" path when evaluated.

[0042] Preferably, step S33 assumes that there are multiple tracks , the cost of each trajectory is , the cost function of each trajectory Includes costs for speed, obstacles, and dynamic objects;

[0043] Cost function:

[0044] ;

[0045] represents the static obstacle cost, : number of obstacles, : The distance between the robot and the static obstacle;

[0046] represents the cost of dynamic obstacles in the trajectory area, : Indicates the number of dynamic obstacles; : Dynamic obstacle cost; represents the speed cost, : target speed; : Current speed; After calculating the cost value of each trajectory, the path with the smallest cost value is selected as the optimal path.

[0047] Preferably, the robot moves toward the target under the guidance of the global path generated by the A* algorithm. During the movement, the DWA algorithm calculates multiple trajectories to ensure that the optimal path is selected that can both exceed the target point and avoid obstacles at the lowest cost.

[0048] A robot path planning system with an improved DWA algorithm, using the robot path planning method with an improved DWA algorithm of the present application, comprises a data acquisition module, a data processing module, a control module and an output module; wherein:

[0049] Data acquisition module: obtain environmental map information and robot information;

[0050] Data processing module: Use the A* algorithm to obtain the global planning path, and add a real-time updated cost field to the evaluation function of the DWA algorithm to give a cost value to each dynamic object;

[0051] Control module: controls the robot to avoid obstacles in the best path by evaluating the environment in real time;

[0052] Output module: Visual output of the control process.

[0053] Compared with the prior art, the present invention has the following beneficial effects:

[0054] By introducing the omnidirectional kinematic model, the robot can move flexibly in any direction within the plane, thus overcoming the limitations of the traditional DWA algorithm. And through the dynamic cost field, the robot can evaluate and avoid dynamic objects in real time, making path planning more intelligent and efficient. This improved solution provides more reliable and efficient technical support for the robot's autonomous navigation in dynamic and complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 is a flow chart of the method of the present invention;

[0056] Figure 2 It is the global path planning graph of the A* algorithm;

[0057] Figure 3 This is the global path planning diagram of the algorithm in this application. DETAILED DESCRIPTION

[0058] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0059] A robot path planning method for improving the DWA algorithm includes the following steps:

[0060] Step 1: Initialize environment map information and robot information:

[0061] Before the entire system is running, the environment map information and the robot initial information must be initialized. First, the environment map information is provided by the grid map constructed by the robot SLAM system. Each grid cell in the grid map indicates the passability of each area. The robot information initialization includes the robot's current position, orientation, and linear speed information.

[0062] Step 2: Use the A* algorithm to obtain the global planning path:

[0063] The A* algorithm uses a heuristic search algorithm to find the shortest path from the starting point to the end point. The optimal path is selected by calculating the total cost of each node. The node cost calculation formula is as follows:

[0064] ;

[0065] Represents the actual cost (path length) from the starting point to the current node, Indicates the estimated cost from the current node to the end point (the straight-line distance to the end point). This is used to select the node with the smallest cost value, and finally generate the shortest path from the start point to the end point.

[0066] Step 3. The dynamic window set by DWA will give the linear velocity and angular velocity of the robot at the next moment. A real-time updated cost field is added to the evaluation function of the DWA algorithm, a cost value is given to each dynamic object, and the robot's path is optimized by evaluating the environment in real time.

[0067] First, the traditional motion model in the DWA algorithm is replaced with an omnidirectional motion model, which enables more accurate control of the robot's motion direction and speed. Specifically, the robot's position information, including the robot's motion speed, is obtained. , robot movement direction Then, the basic formula of the omnidirectional motion model is used to calculate the motion component of the robot along the X axis. , the motion component along the Y axis and the robot's in-plane angular velocity , the solution formula is:

[0068] ;

[0069] ;

[0070] ;

[0071] is the angular velocity, which is equal to the derivative of the angle with respect to time t.

[0072] At each subsequent time step, the system generates a motion combination (including , , ) to predict all possible motion trajectories of the robot. At the same time, according to the current movement speed, acceleration and turning speed of the robot, DWA will define a dynamic window, which will limit the linear velocity and angular velocity that the robot will take in the next moment.

[0073] A real-time updated cost field is added to the evaluation function of the DWA algorithm, a cost value is given to each dynamic object, and the robot's path is optimized by evaluating the environment in real time.

[0074] Step 31: The camera collects images and preprocesses the images using the LKT optical flow method:

[0075] In this step, an RGB_D camera is used, which can obtain the RGB information and depth information of the environment. Then the LKT optical flow method is used to calculate the speed, acceleration and other information of dynamic objects in the RGB image. The core idea of ​​the optical flow method is to track the changes in the pixel position of an object in two consecutive frames to estimate its speed and other information. During the movement of the robot, the entire image will move. However, the optical flow vector of a static object is in one direction (for example, the robot moves forward and the static pixel moves backward). At this time, the average optical flow vector of the entire image is calculated. , and then find the pixels that deviate from the background motion, that is, dynamic objects. The formula for calculating the average optical flow vector is as follows:

[0076] ;

[0077] ;

[0078] Where N represents the number of pixels in the entire image, The pixel's motion velocity vector. Then calculate the deviation value of each pixel:

[0079] ;

[0080] ;

[0081] if If the value is very small, it means the pixel is static. If it is greater than the set threshold T, it means the pixel is dynamic (i.e. dynamic pixel). Because what we get now is the displacement speed of the pixel, not the displacement speed of the object, we need to convert the units:

[0082] ;

[0083] ;

[0084] ;

[0085] in D Indicates the depth of the object (obtained from the depth image); F Indicates the focal length of the camera; Indicates the optical flow speed of the object (pixels / second), Indicates the actual speed of an object (m / s). Acceleration is obtained from the change in speed.

[0086] Step 32: Calculate the cost of each dynamic object in the dynamic window:

[0087] The displacement of the dynamic object ( ),speed( ) into the cost value calculation formula to calculate the cost value of each object. The cost value calculation formula is as follows:

[0088] ;

[0089] In the formula: C represents the cost value of the object; r represents the radius of the object; ε is a small constant to prevent division by zero; Is a coefficient used to adjust the effect of speed on cost.

[0090] Step 33: Calculate the cost of each candidate path and retain the path with the smallest cost:

[0091] The calculated cost value affects the priority of trajectory selection. The path corresponding to the area with a higher cost value will be considered a "worse" path when evaluated. Suppose there are multiple trajectories now , the cost of each trajectory is , the cost function of each trajectory Contains multiple factors including speed, obstacles and dynamic objects. Cost function:

[0092] ;

[0093] represents the static obstacle cost, where : Number of obstacles, : The distance between the robot and the static obstacle. represents the cost of dynamic obstacles in the trajectory area, where : Indicates the number of dynamic obstacles, : Dynamic obstacle cost. represents the speed cost, where : Target speed, : Current speed. After calculating the cost of each trajectory, the path with the smallest cost is selected as the optimal path.

[0094] Step 4: The A* algorithm ensures that the robot moves toward the target point, and the DWA algorithm ensures that the robot avoids obstacles in the optimal path:

[0095] The robot moves towards the target under the guidance of the global path generated by the A* algorithm. During the movement, the DWA algorithm calculates multiple trajectories to ensure that the optimal path is selected that can both exceed the target point and avoid obstacles at the lowest cost.

[0096] A robot path planning system with an improved DWA algorithm, the system is constructed to run the method of the present application, including a data acquisition module, a data processing module, a control module and an output module;

[0097] Data acquisition module: obtain environmental map information and robot information;

[0098] Data processing module: Use the A* algorithm to obtain the global planning path, and add a real-time updated cost field to the evaluation function of the DWA algorithm to give a cost value to each dynamic object;

[0099] Control module: Controls the optimal path to avoid obstacles by evaluating the environment in real time;

[0100] Output module: Visual output of the control process.

[0101] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, can also be implemented by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0102] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A robot path planning method based on an improved DWA algorithm, characterized in that: The following steps are involved: S1. Initialize environment map information and robot information; S2. Use the A* algorithm to obtain the global planning path; S3. The dynamic window set by DWA will give the linear velocity and angular velocity of the robot at the next moment. A cost field that is updated in real time is added to the evaluation function of the DWA algorithm to give a cost value to each dynamic object, and optimize the robot's path by evaluating the environment in real time. Calculate the cost value of each dynamic object in the dynamic window as follows: The displacement of dynamic objects ,speed Substitute the cost value calculation formula to calculate the cost value of each object; The cost value calculation formula is as follows: ； C represents the cost value of the object; r represents the radius of the object; ε is a small constant that prevents division by zero; is a coefficient used to adjust the impact of speed on cost. The calculated cost value will affect the priority of trajectory selection. The area with higher cost value will be considered as a "worse" path when evaluated. S4. The robot avoids obstacles using the optimal path.

2. The robot path planning method of the improved DWA algorithm according to claim 1, characterized in that: Step S3 includes: S31. Perform image acquisition and preprocess images using the LKT optical flow method; S32. Calculate the cost of each dynamic object in the dynamic window; S33. Calculate the cost of each candidate path and retain the path with the smallest cost.

3. The robot path planning method of the improved DWA algorithm according to claim 1, characterized in that: The environmental map information in step S1 is provided by a grid map constructed by the robot SLAM system, in which each grid cell identifies the passability of each area; the robot information initialization includes the robot's current position, orientation, and linear speed information.

4. The robot path planning method of the improved DWA algorithm according to claim 1, characterized in that: In step S2, the optimal path is selected by calculating the total cost of each node. The node cost calculation formula is as follows: ； Represents the actual cost from the starting point to the current node, It represents the estimated cost from the current node to the end point, so as to select the node with the smallest cost value and finally generate the shortest path from the starting point to the end point.

5. The robot path planning method of the improved DWA algorithm according to claim 1, characterized in that: In step S3, the robot movement speed , robot movement direction , use the omnidirectional motion model to calculate the robot's motion component along the X axis , the motion component along the Y axis and the robot's in-plane angular velocity ; The solution formula is: ； ； ； At each subsequent time step, a motion combination is generated ( , , ) to predict all possible motion trajectories of the robot; at the same time, according to the current movement speed, acceleration and turning speed of the robot, DWA sets a dynamic window to limit the linear velocity and angular velocity that the robot will take in the next moment.

6. The robot path planning method of the improved DWA algorithm according to claim 2, characterized in that: In step S31, the average optical flow vector of the entire image is calculated , find the pixels that deviate from the background motion, i.e. dynamic objects; The average optical flow vector calculation formula is as follows: ； ； Where N represents the number of pixels in the entire image, The pixel's motion velocity vector; Calculate the deviation value for each pixel: ； ； if If the value is less than or equal to the threshold, it means that the pixel is static, and if it is greater than the set threshold T, it means that the pixel is dynamic; The above result is the displacement speed of the pixel, not the displacement speed of the object, so unit conversion is required: ； ； ； Where D represents the depth of the object; F represents the focal length of the camera; Represents the optical flow speed of an object, Indicates the actual speed of the object.

7. The robot path planning method of the improved DWA algorithm according to claim 2, characterized in that: Step S33 assumes that there are now multiple tracks , the cost of each trajectory is , the cost function of each trajectory Includes costs for speed, obstacles, and dynamic objects; Cost function: ； represents the static obstacle cost, : number of obstacles, : The distance between the robot and the static obstacle; represents the cost of dynamic obstacles in the trajectory area, : Indicates the number of dynamic obstacles; : Dynamic obstacle cost; represents the speed cost, : target speed; : Current speed; After calculating the cost value of each trajectory, the path with the smallest cost value is selected as the optimal path.

8. The robot path planning method of the improved DWA algorithm according to claim 1, characterized in that: The robot moves towards the target under the guidance of the global path generated by the A* algorithm. During the movement, the DWA algorithm calculates multiple trajectories to ensure that the optimal path is selected that can both exceed the target point and avoid obstacles at the lowest cost.

9. A robot path planning system with an improved DWA algorithm, characterized in that: A robot path planning method using the improved DWA algorithm according to any one of claims 1 to 8 comprises a data acquisition module, a data processing module, a control module and an output module; wherein: Data acquisition module: obtain environmental map information and robot information; Data processing module: Use the A* algorithm to obtain the global planning path, and add a real-time updated cost field to the evaluation function of the DWA algorithm to give a cost value to each dynamic object; Control module: controls the robot to avoid obstacles in the best path by evaluating the environment in real time; Output module: Visual output of the control process.

Images