Improved differential wheeled robot path planning method of dynamic window algorithm

Abstract

The application discloses a differential robot path planning method for improving a dynamic window algorithm, and comprises the following steps: firstly, a kinematic model of a robot and a grid map of a surrounding environment are established, coordinates of a starting point and an ending point of the robot are acquired, and coordinates of obstacles are marked; secondly, a current position is detected, an A* global path is calculated, a smoothness function is established, and a gradient descent method is used to smooth the A* path; then, performance parameters and a sampling frequency are initialized to obtain an interval range of angular velocity and linear velocity, and various combinations of the linear velocity and the angular velocity; then, a track extrapolation is performed to obtain multiple estimated positions of the robot model, and obstacle distance evaluation scores, heading angle deviation scores, speed evaluation scores and A* path relationship scores are calculated; finally, total scores of the estimated positions of the model are obtained, and a speed combination with the highest evaluation score is selected to drive the robot to move. The application enhances global search capability of the algorithm, improves running stability of the robot, and effectively shortens a length of a running path of the robot.

Description

Technical Field

[0001] This invention belongs to the field of robotics technology, specifically relating to an improved dynamic window algorithm for path planning of differential wheel robots. Background Technology

[0002] Initially, the application of mobile robots was limited to manufacturing, but now they are widely used in mining, rescue, logistics, military, space, and transportation. With the development of automation technology, the application of robots is becoming increasingly widespread, and robots play an important role in modern industrial production and daily life. Today, people can use robotic technology to achieve autonomous driving, use robotic arms in factories for large-scale, rapid machining, the emergence of agricultural robots has greatly reduced the burden on farmers, and specialized robots have been launched into space to explore areas never before explored by mankind.

[0003] When autonomous navigation robots perform tasks, they are equipped with various sensors such as LiDAR and depth cameras to detect the environment and perform localization. After obtaining a model of the surrounding environment, a safe and efficient route needs to be obtained at the robot's starting position and the task's endpoint. Therefore, the ability to autonomously avoid obstacles and efficiently obtain a navigation path is an important part of robot navigation research. Path planning is divided into global path planning and local path planning. In global navigation, the robot needs to plan a path based on a map containing information such as the starting point, the endpoint, and static obstacles. In local path planning, the surrounding environment of the mobile robot is constantly changing, and information about surrounding obstacles needs to be obtained during operation to plan a path that is easy for the robot to follow.

[0004] In his 1959 paper, Dijkstra proposed a shortest path search algorithm based on a greedy strategy. Nils Nilsson, building on Dijkstra's work, proposed the A* algorithm in 1968, significantly reducing the complexity of path search. Fox D. first proposed the dynamic window method in 1997. Unlike previous methods, the dynamic window method samples the robot's linear and angular velocities and evaluates the sampled paths to obtain the optimal path. Lu C. et al. implemented an adaptive weight evaluation function using Q-learning to adapt to different obstacle environments, based on the original DWA algorithm. Lee D.H. et al. proposed a mobile robot obstacle avoidance algorithm based on finite memory filtering, which avoids dynamic obstacles by estimating the overall distribution, achieving superior performance compared to the traditional DWA algorithm. In current path planning algorithms, the traditional A* algorithm cannot complete the entire path planning process and can only serve as a guide for the robot's global path planning; correspondingly, the traditional DWA algorithm can only serve as a local path planner and has poor adaptability in complex environments, with the planned path exhibiting oscillation problems. Summary of the Invention

[0005] To address the aforementioned issues, this invention provides an improved dynamic window algorithm for differential robot path planning. It uses a smooth A* path for global guidance, incorporates a nonlinear artificial repulsion field obstacle evaluation function to reduce oscillations during robot operation, and adaptively changes the weights of the sub-evaluation function based on the obstacle environment to enhance the algorithm's adaptability.

[0006] To achieve the above objectives, the present invention provides the following technical solution.

[0007] A planning method for a differential wheel robot based on an improved dynamic window algorithm includes the following steps:

[0008] S1. Establish the robot's kinematic model and a grid map of the surrounding environment, obtain the robot's starting and ending coordinates, and mark the coordinates of obstacles. The robot's kinematic model is as follows:

[0009]

[0010] In the formula, X k =[pos x pos y [aug υel ω] represents the robot's state, pos x Let pos be the horizontal coordinate of the robot. y Let be the robot's ordinate, aug be the robot's heading angle, υel be the robot's linear velocity, and ω be the robot's angular velocity; F and B are the state transition matrices, u k The input angular velocity and linear velocity are given, and the subscript k represents the k-th sampling time.

[0011] S2, based on the grid map in S1, calculates the global planning path using the A* algorithm.

[0012] S3. Establish a smoothness function and use gradient descent to maximize the smoothness. The smoothness function is as follows:

[0013] f = c1||x i -y i ||+c2||y i -y i+1 ||

[0014] In the formula, the result of the traditional A* algorithm path planning is a point sequence of [x1, x2, ..., x...]. n The smoothed path planning result is a point sequence [y1, y2, ..., y]. n ], c1c2 are parameters, ||x i -y i || represents the degree of deviation between the smoothed point and the original point, ||y i-y i+1 || represents the distance between points after smoothing, which is obtained by gradient descent. i Make ||x i -y i || 2 (the degree of deviation between the smoothed point and the original point) and ||y i -y i+1 The value of || (distance between points after smoothing) is minimized; the smaller the value of f, the smoother the path.

[0015] S4, Initialize the robot's kinematic model and performance parameters, including the robot's maximum linear velocity υ. max angular velocity ω max Linear acceleration a max and angular acceleration α max And parameter sampling rate.

[0016] S5, detect the current robot position.

[0017] S6, based on the robot's performance parameters, obtains the range of angular velocity and linear velocity, i.e., the velocity window.

[0018] S7, based on the sampling rate set in S4 and the velocity window obtained in S6, samples the velocity window to obtain various combinations of linear velocity and angular velocity.

[0019] S8, based on the robot kinematics model in S1 and the combination of linear velocity and angular velocity in S7, the trajectory is deduced to estimate the position of multiple robot kinematics models and the corresponding combination of linear velocity and angular velocity.

[0020] S9. Based on the artificial repulsive potential field formula, the distance between the robot's kinematic model and the obstacle is estimated, and the obstacle distance evaluation score is calculated. The calculation formula is as follows:

[0021]

[0022] U req Let be the potential field function of the repulsive potential field, where k1 is the proportional gain coefficient, q is the robot's current coordinate, and q obs Let ρ(q, q) be the coordinates of the obstacle. obs Let be a vector, pointing from the obstacle to the robot, with a magnitude equal to the distance between the robot and the obstacle. ρ0 is a constant representing the maximum range over which the obstacle's repulsive field exerts its effect on the robot. The repulsive force refers to the force exerted on the robot by the repulsive potential field, with a magnitude equal to the negative gradient of the repulsive potential field. The calculation formula is as follows:

[0023]

[0024] S10: Calculate the deviation between the robot's current heading angle and the estimated position of the robot's kinematic model from the target point, and obtain the heading angle evaluation score. The calculation formula is as follows:

[0025] heading(υ,ω)=k2·(tan(ρ( qobs ,q))-aug)

[0026] In the formula, k2 is the proportionality coefficient, and q obs ρ(q) represents the obstacle coordinates, q represents the robot coordinates, and ρ(q) represents the obstacle coordinates. obs ,q) is a vector whose direction is from the robot to the obstacle, and auq is the robot's heading angle.

[0027] S11, calculate the speed score based on the absolute value of the linear velocity in the sampling velocity of S7, and obtain the speed evaluation score using the following formula:

[0028] υel(υ,ω)=k3·|υ|

[0029] Where υ is the linear velocity of the robot, ω is the angular velocity of the robot, and k3 is the proportional parameter.

[0030] S12, calculate the score for evaluating the relationship between the estimated position of the robot's kinematic model and the A* path position. The evaluation function is as follows:

[0031]

[0032] In the formula, υ is the linear velocity of the robot, ω is the angular velocity of the robot, and x i+m Let y be the robot's x-coordinate. i+m Let d be the robot's vertical coordinate, and d represent the distance between the robot and the aforementioned point.

[0033] S13: After normalizing the evaluation scores in S9, S10, S11, and S12, multiply them by their respective weights and sum them to obtain the total evaluation score of the model's predicted location. The calculation formula is as follows:

[0034] J(υ,ω)=ω1·heading(υ,ω)+w2·uel(u,ω)+w3·U req (υ,ω)+w4·AStarDist(υ,ω)

[0035] In the formula, w1, w2, w3, and w4 are the weights of each sub-item in the overall evaluation score formula, w2 and w4 are fixed parameters, w1 and w3 are adaptive parameters based on the environment, and υ and ω are the speed and linear velocity adopted by the robot, respectively.

[0036] S14, In step 13, the sum of the weights of the heading angle and obstacle distance evaluation score formulas is set to a constant value, i.e., w1 + w3 = w′. The weight of the heading angle evaluation sub-function is calculated using the following formula:

[0037]

[0038] In the formula, k4 is a coefficient, and d obs θ can be viewed as the angle formed with the obstacle and the distance to the obstacle, and w1 is positively correlated with the product of the two, where θ is the angle formed with the obstacle and the distance to the obstacle. It is the lower limit of w1 and the upper limit is less than w′.

[0039] S15 selects the combination with the highest evaluation score from various combinations of linear velocity and angular velocity.

[0040] S16 drives the robot to move based on the combination of the highest-scoring linear velocity and angular velocity.

[0041] S17. If the robot reaches the target point, it stops moving; otherwise, repeat S5-S16.

[0042] The beneficial effects of this invention are as follows: First, this invention uses A* path for assisted guidance, enhancing the global path search capability of the traditional dynamic window algorithm. Second, this invention introduces an artificial repulsive potential field nonlinear function to evaluate the relationship between the robot and obstacles, replacing the traditional algorithm's evaluation of only the impact of the nearest surrounding obstacles on the robot. Instead, it simultaneously evaluates the impact of multiple surrounding obstacles on the robot, reducing oscillations during robot operation and improving the stability of robot operation. Finally, this invention judges the degree of obstruction of the robot based on the distance and angle between the surrounding obstacles and the robot, introducing adaptive evaluation weights to replace the fixed weights in the original algorithm. This enhances the algorithm's adaptability in different environments, avoids meaningless obstacle avoidance behavior by the robot, and effectively shortens the length of the robot's running path. Attached Figure Description

[0043] Figure 1 This is an algorithm flowchart provided in the example of the present invention;

[0044] Figure 2 This is a schematic diagram of the algorithm trajectory extrapolation principle provided in this invention example;

[0045] Figure 3 This is a schematic diagram of the repulsive potential field of an artificial obstacle provided in an example of the present invention;

[0046] Figure 4 This is a diagram of the original algorithm path planning result before algorithm optimization in a simple environment provided by the examples of this invention;

[0047] Figure 5This is a path planning result diagram of the improved algorithm of the present invention in a simple environment provided by the examples of the present invention;

[0048] Figure 6 This is a diagram of the original algorithm path planning result before algorithm optimization in a complex environment, provided by an example of the present invention.

[0049] Figure 7 This is a diagram showing the path planning results of the improved algorithm of this invention under complex environments, provided by an example of this invention. Detailed Implementation

[0050] To make the objectives, technical solutions, and improvements of this invention clearer, the invention will be described in more detail below with reference to schematic diagrams and practical examples.

[0051] S1. Establish a kinematic model and a grid map of the surrounding environment, marking the coordinates, start point, and end point of obstacles. The robot's kinematic model is as follows:

[0052]

[0053] In the formula, x = [pos x pos y [aug υel ω] represents the robot's state, pos x Let pos be the horizontal coordinate of the robot. y denoted as the robot's ordinate, aug as the robot's heading angle, υel as the robot's linear velocity, and ω as the robot's angular velocity; F and B are the state transition matrices, u represents the input angular velocity and linear velocity, and the subscript k represents the k-th sampling time.

[0054] S2: Based on the grid map in S2, the global planning path is calculated using the A* algorithm.

[0055] S3. Establish a smoothness function and use gradient descent to maximize the smoothness. The smoothness function is as follows:

[0056] f = c1||x i -y i ||+c2||y i -y i+1 ||

[0057] In the formula, the result of the traditional A* algorithm path planning is a point sequence of [x1, x2, ..., x...]. n The smoothed path planning result is a point sequence [y1, y2, ..., y]. n ], c1c2 are parameters, ||x i -y i || represents the degree of deviation between the smoothed point and the original point, ||y i -yi+1 || represents the distance between points after smoothing, which is obtained by gradient descent. i Make ||x i -y i || 2 (the degree of deviation between the smoothed point and the original point) and ||y i -y i+1 The value of || (distance between points after smoothing) is minimized; the smaller the value of f, the smoother the path.

[0058] S4, initialize the robot's kinematic model and performance parameters, including a maximum linear velocity of 1 m / s, an angular velocity of 20° / s, and a linear acceleration of 0.2 m / s². 2 Angular acceleration 50° / s 2 The design speed sampling rate is 0.1s.

[0059] S5, detect the current robot position.

[0060] S6, based on the robot's performance parameters, obtains the range of angular velocity and linear velocity, i.e., the velocity window.

[0061] S7, based on the sampling rate set in S4 and the velocity window obtained in S6, samples the velocity window to obtain various combinations of linear velocity and angular velocity.

[0062] S8, based on the robot kinematics model in S1 and the combination of linear velocity and angular velocity in S8, the trajectory is deduced to obtain the estimated position of multiple robot models and the corresponding velocity combination.

[0063] S9. Calculate the distance between the robot model's estimated position and the obstacle based on the artificial repulsive potential field formula, and obtain the obstacle distance evaluation score. The calculation formula is as follows:

[0064]

[0065] In the formula, U req Let be the potential field function of the repulsive potential field, where k1 is the proportional gain coefficient, q is the robot's current coordinate, and q obs Let ρ(q, q) be the coordinates of the obstacle. obs Let be a vector, pointing from the obstacle to the robot, with a magnitude equal to the distance between the robot and the obstacle. ρ0 is a constant representing the maximum range over which the obstacle's repulsive field exerts its effect on the robot. The magnitude of the repulsive force is the negative gradient of the repulsive potential field, as follows:

[0066]

[0067] S10, Calculate the deviation of the robot model's estimated position from the target point based on the robot's current heading angle, and obtain the heading angle evaluation score. The calculation formula is as follows:

[0068] heading(υ,ω)=k2·(tan(ρ(q obs ,q))-aug)

[0069] In the formula, k2 is the proportionality coefficient, and q obs ρ(q) represents the obstacle coordinates, q represents the robot coordinates, and ρ(q) represents the obstacle coordinates. obs q) is a vector pointing from the robot to the obstacle, and aug is the robot's heading angle.

[0070] S11, calculate the speed score based on the absolute value of the linear velocity in the sampling velocity of S7, and obtain the speed evaluation score using the following formula:

[0071] υel(υ,ω)=k3·|υ|

[0072] In the formula, u is the linear velocity of the robot, ω is the angular velocity of the robot, and k3 is the proportional parameter.

[0073] S12, calculate the score of the relationship between the predicted location of the evaluation model and the location of the A* path. The evaluation function is as follows:

[0074]

[0075] In the formula, υ is the robot's linear velocity, ω is the robot's angular velocity, x is the robot's x-coordinate, and y is the robot's y-coordinate. The smoothed A* path points are numbered from the starting point to the ending point, (x... i y i The subscript 'i' represents the coordinates of the point closest to the robot's current position among all A* path points, with 'i' being the index of that point; 'i+m' represents the m points following that point, where 'm' is a parameter intended to prevent the robot from oscillating or deviating its heading after adding this subfunction. The index of the robot's aiming point should be after the nearest point. In this formula, a larger 'm' results in poor tracking response performance but less oscillation; a smaller 'm' results in strong tracking response performance but greater oscillation. 'd' represents the distance between the robot and the aforementioned points. The inverse relationship between distance and target means that the larger the distance, the lower the score. Therefore, to obtain a higher score, the robot will travel along a path closer to a smooth A* path.

[0076] S13: After normalizing the evaluation scores in S10, S11, S12, and S13, multiply them by their respective weights and sum them to obtain the total evaluation score of the model's predicted location. The calculation formula is as follows:

[0077] J(υ,ω)=w1·heading(υ,ω)+w2·υelocity(υ,ω)+w3·U reg(υ,ω)+w4·AStarDist(υ,ω)

[0078] In the formula, w1, w2, w3, and w4 are the weights of each sub-item in the overall evaluation score formula, w2 and w4 are fixed parameters, w1 and ω3 are adaptive parameters based on the environment, and υ and ω are the speed and linear velocity adopted by the robot, respectively.

[0079] S14, In step 13, the sum of the weights of the heading angle and obstacle distance evaluation score formulas is set to a constant value, i.e., ω1 + w3 = w′. The weight of the heading angle evaluation sub-function is calculated using the following formula:

[0080]

[0081] In the formula, k4 is a coefficient, and d obs θ can be viewed as the angle formed with the robot and the obstacle, and the distance to the obstacle. The smaller the angle between the robot and the obstacle, the greater the obstacle's influence on preventing the robot from reaching the target point; the closer the obstacle is to the robot, the greater its obstruction to the robot's movement. Therefore, w1 is positively correlated with the product of the two, where... It is the lower limit of w1 and the upper limit is less than w′.

[0082] S15 selects the combination with the highest evaluation score from multiple speed combinations.

[0083] S16 drives the robot to move based on the speed combination with the highest evaluation score.

[0084] S17. If the robot reaches the target point, it stops moving; otherwise, repeat S5-S16.

[0085] In this embodiment:

[0086] Reference Figure 1 The improved algorithm in this application makes several improvements based on the traditional dynamic window method, mainly including the introduction of a global path, a nonlinear obstacle evaluation function, and adaptive weights for sub-evaluation functions within the evaluation function. The global path is generated based on the initial path generated by the A* algorithm, a global path planning algorithm that addresses the poor pathfinding performance of the traditional dynamic window method in a global environment. The nonlinear function is introduced because the traditional dynamic window method only evaluates the impact of the nearest obstacle on the robot. However, in practical applications, obstacles can be considered as composed of a large number of discrete points, requiring simultaneous evaluation of the combined impact of multiple surrounding obstacles on the robot.

[0087] like Figure 2In the basic dynamic window method, the robot extrapolates its trajectory based on multiple sets of sampled velocities and selects the combination of velocities with the highest score according to the evaluation function. Obstacle distance is only related to the nearest obstacle to the robot; this evaluation function cannot consider the impact of multiple obstacles on the robot's position, sometimes resulting in poor paths. When the robot enters an area with symmetrical obstacles on both sides, its initial position is biased to the right, farther from the obstacle on the left, and all paths to the left will receive higher scores. However, when the robot moves to the left, the situation changes because the robot only evaluates the distance to the nearest obstacle; therefore, paths to the right will receive higher scores than paths to the left, leading to significant path oscillations. This invention uses a nonlinear function in an artificial repulsive field instead of a single obstacle evaluation method, effectively solving this problem. (Refer to...) Figure 3 The visualization shows the repulsive potential field generated on two parallel straight obstacles. It can be seen that the obstacles form a high repulsive potential field, and the closer to the obstacle, the stronger the repulsive potential field. In contrast, the potential field is lower in the walkable area between the parallel obstacles, indicating that the introduction of the repulsive potential field makes the obstacles and the walkable area more distinguishable.

[0088] In the original algorithm, the weight coefficients in the path evaluation function remain constant, resulting in poor robot adaptability to varying obstacle conditions. In reality, robots face highly complex road conditions. When in a complex obstacle environment, the robot's primary task is to avoid obstacles, so the weight of the obstacle distance evaluation function should be appropriately increased to ensure robot safety by keeping it away from obstacles. However, when there are few or no obstacles between the robot and the target point, the obstacle distance evaluation function becomes meaningless, and its weight should be reduced. Increasing the heading angle and target distance evaluation functions reinforces the robot's ability to drive directly towards obstacles, thus shortening the path planning distance. Therefore, the weights of the heading angle evaluation function and the obstacle avoidance evaluation function are contradictory. The sum of their weights can be set to a constant value, w1 + w3 = w′, where w1 is the weight of the heading angle evaluation function, w3 is the weight of the obstacle avoidance evaluation function, and w′ is a constant parameter. The weight of the heading angle evaluation function can be determined using the formula... Decision. In the formula, d obs θ can be viewed as the angle formed with the obstacle and the distance from the obstacle. It is the lower limit value and is less than ω′.

[0089] Experimental verification in this embodiment:

[0090] Reference Figure 4 , 5 and Figure 6 , 7The experimental section of this application is divided into small-scale simple scenarios and large-scale complex scenarios to verify the effectiveness of the improved algorithm. (See reference...) Figure 4 and Figure 5 For comparing algorithm performance in a simple scenario, the map size was 9m × 9m, the starting coordinates were (1,1), the ending coordinates were (7,7), and the initial heading angle was 0. Both the traditional and improved algorithms could plan feasible paths in a simple obstacle environment. Compared to the traditional algorithm, the improved algorithm produced shorter paths, reducing the planned path length by 13.2% and the running time by 21.3%. It also exhibited less oscillation and a smoother path. (Reference) Figure 6 and Figure 7 The improved algorithm in this application also outperforms the traditional algorithm in complex environments. In a complex scene, the map size is 9m×9m, the starting coordinates are (0.5,25), the ending coordinates are (25,0.5), and the initial heading angle is π / 4. Compared with the chaotic path performance of the traditional algorithm, the improved algorithm reduces the path length by 23% and the running time by 11%. The improved algorithm is more stable and can better meet the requirements of mobile robots to obtain a smoother path in complex environments.

[0091] The path planning results of the traditional algorithm and the improved algorithm in simple obstacle environment and complex obstacle environment are shown in Table 1 and Table 2.

[0092] Table 1 Comparison of results of the two algorithms in a simple environment

[0093]

[0094] Table 2 Comparison of results of the two algorithms under complex environments

[0095]

[0096] The above are merely preferred embodiments of the present invention and are 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 within the protection scope of the present invention.

Claims

1. An improved dynamic window algorithm for differential robot path planning, characterized in that... Includes the following steps: S1, establish a kinematic model of the robot and a grid map of the surrounding environment, obtain the coordinates of the robot's starting and ending points, and mark the coordinates of obstacles; S2, based on the grid map in S1, calculates the global planning path using the A* algorithm; S3, establish a smoothness function and use gradient descent to maximize the smoothness; S4, Initialize the robot's kinematic model and performance parameters, including the robot's maximum linear velocity. angular velocity Linear acceleration and angular acceleration and parameter sampling rate; S5, detect the current robot position; S6, based on the robot's performance parameters, obtain the range of angular velocity and linear velocity, i.e., the velocity window; S7, based on the sampling rate set in S4 and the velocity window obtained in S6, samples the velocity window to obtain multiple combinations of linear velocity and angular velocity; S8. Based on the robot kinematics model in S1 and the combination of linear velocity and angular velocity in S7, the trajectory is deduced to estimate the position of multiple robot kinematics models and the corresponding combination of linear velocity and angular velocity. S9, calculate the distance between the robot's kinematic model and the obstacle based on the artificial repulsive potential field formula, and calculate the obstacle distance evaluation score; S10, calculate the deviation of the heading angle between the estimated position of the robot's kinematic model and the target point based on the robot's current heading angle, and calculate the heading angle evaluation score; S11, calculate the speed score based on the absolute value of the linear velocity in the sampling speed of S7, and obtain the speed evaluation score; S12, calculate the score of the relationship between the estimated position of the robot's kinematic model and the position of the A* path; S13: After normalizing the evaluation scores in S9, S10, S11, and S12, multiply them by their respective weights and sum them to obtain the total evaluation score of the model's predicted location. The calculation formula is as follows: In the formula, , , , These are the heading angle evaluation score, velocity evaluation score, obstacle distance evaluation score, and the score evaluating the relationship between the robot's kinematic model estimated position and the A* path position. , , , These are the weights of each sub-item in the overall evaluation score formula. , These are fixed parameters. , To adapt parameters according to the environment, , These are the robot's speed and angular velocity, respectively. S14, In step 13, the sum of the weights of the heading angle and obstacle distance evaluation score formula is set to a constant value, that is, set... The calculation formula is as follows: In the formula, For coefficients, and Consider it as the angle formed with the obstacle and the distance from the obstacle. yes lower limit and The upper limit is less than ; S15: Select the combination with the highest evaluation score from a variety of linear and angular velocities. S16, drive the robot to move based on the highest combination of linear velocity and angular velocity in the evaluation score; S17. If the robot reaches the target point, it stops moving; otherwise, repeat S5-S16.

2. The differential robot path planning method based on an improved dynamic window algorithm according to claim 1, characterized in that: In S1, the robot kinematic model is as follows: In the formula, For the robot's state, pos x Let pos be the horizontal coordinate of the robot. y Let be the robot's ordinate, and aug be the robot's heading angle. Let the linear velocity of the robot be . Let ω be the robot's angular velocity. F and B are the state transition matrices. For the input angular velocity and linear velocity, the subscripts are... Representing the Each sampling time.

3. The differential robot path planning method based on an improved dynamic window algorithm according to claim 1, characterized in that: In S3, the smoothness function is as follows: In the formula, the result of the traditional A* algorithm path planning is a point sequence. The result of the smoothed path planning is a point sequence. , For parameters, To determine the degree of deviation between the smoothed point and the original point, The distance between points after smoothing is obtained using the gradient descent method. Make and The smaller the value of f, the smoother the path.

4. The differential robot path planning method based on an improved dynamic window algorithm according to claim 1, characterized in that: In S9, the obstacle distance evaluation score is calculated using the following formula: Let be the potential field function of the repulsive potential field, where q is the proportional gain coefficient, and q is the robot's current coordinate. obs Here are the coordinates of the obstacle. It is a vector, its direction is from the obstacle to the robot, and its magnitude is the distance from the robot to the obstacle. It is a constant, representing the maximum range within which the repulsive force field of the obstacle exerts its effect on the robot; The formula for calculating repulsive force is as follows: The repulsive force refers to the force exerted on the robot by the repulsive potential field, and its magnitude is the negative gradient of the repulsive potential field.

5. The differential robot path planning method based on an improved dynamic window algorithm according to claim 1, characterized in that: In S10, the formula for calculating the heading angle evaluation score is as follows: In the formula, This is the proportionality coefficient. These are the coordinates of the obstacle. These are robot coordinates. It is a vector, with its direction from the robot towards the obstacle. This is the robot's heading angle.

6. The differential robot path planning method based on an improved dynamic window algorithm according to claim 1, characterized in that: In S11, the speed evaluation score is calculated using the following formula: In the formula, This is a proportional parameter.

7. The differential robot path planning method based on an improved dynamic window algorithm according to claim 1, characterized in that: In S12, the score for evaluating the relationship between the robot's kinematic model estimated position and the A* path position is given, and the evaluation function is as follows: In the formula, Let x be the robot's x-coordinate. Using the robot's ordinate, number the smoothed A* path points from the start to the end. Represents the coordinates of the point closest to the robot's current position among all A* path points, with the subscript... This is the index of the point; After this point One point, For parameters, This represents the distance between the robot and the coordinates mentioned above.

Images