A Local Path Planning Method for Quadruped Robots Based on Improved DWA and TEB Algorithms

Abstract

The present invention relates to the technical field of path planning, and more particularly to a local path planning method for a quadruped robot based on improved DWA and TEB algorithms. The method includes: Step S1: obtaining a first path based on the starting position, target position of the robot and static obstacle information; Step S2: smoothing the first path through a smoothing algorithm to obtain a second path; Step S3: calculating the maximum obstacle avoidance distance based on the current position of the moving obstacle, the maximum speed, the current position of the robot and the estimated average speed; Step S4: obtaining the obstacle avoidance speed based on the difference between the first distance between the robot and the moving obstacle and the safe obstacle avoidance distance and the collision speed window; Step S5: obtaining a third path based on the obstacle avoidance speed and the second path, and reaching the target position based on the third path. The present invention solves the problems of inaccurate obstacle avoidance and low driving efficiency, and improves the obstacle avoidance accuracy and driving efficiency.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and more particularly to a local path planning method for a quadruped robot based on improved DWA and TEB algorithms. Background Art

[0002] Path planning, as an important part of motion planning, presents a trend of diversification, intelligence, and cross-field integration in its development status. In the field of robotics, path planning is a key technology for realizing autonomous navigation and obstacle avoidance of robots. Whether it is an industrial robot or a service robot, it is necessary to plan a safe and efficient driving route through path planning. For example, in environments such as warehouses, factories, and disaster evacuation, automated guided vehicles need to plan paths according to the actual situation to achieve efficient and safe operation. Similar prior arts include a Chinese patent with the publication number CN118274847B, which proposes a navigation decision-making and planning method for an agricultural inspection robot based on road surface unevenness, including controlling the agricultural inspection robot to construct a two-dimensional grid map, performing global path planning based on the two-dimensional grid map to obtain the global optimal path. Measuring the height values of height data points, calculating the laser evaluation value according to the height values. Calculating the road surface unevenness according to the laser evaluation value and the visual evaluation value, and performing local path planning according to the road surface unevenness to obtain the local optimal path. Controlling the movement of the agricultural inspection robot, updating the global optimal path and the local optimal path until the agricultural inspection robot reaches the target end point. Combining the laser evaluation value and the visual evaluation value to calculate the road surface unevenness can identify and predict the road surface unevenness of the surrounding environment in advance, improving the accuracy and robustness of the road surface unevenness evaluation. Updating the global optimal path and the local optimal path in real time according to the actual situation of the scene can ensure the smooth progress of the inspection operation. In addition, a similar prior art is a US patent with the publication number US20210402599A1, which proposes a mobile robot control device including an online 3D modeler and a path planner. The online 3D modeler is configured to receive an image sequence from the mobile robot and generate a first map and a second map different from the first map based on the image sequence. The path planner is configured to generate a global path based on the first mapping, extract a target surface based on the second mapping, and generate a local inspection path with a moving unit smaller than the global path based on the global. The above two patents have both solved the problem of path planning, but neither has considered the problem of the speed change of moving obstacles and the path optimization during the avoidance of static obstacles. Summary of the Invention

[0003] In order to better solve the above problems, the present invention provides a local path planning method for a quadruped robot based on improved DWA and TEB algorithms, and the method includes the following steps:

[0004] Step S1: Obtain the starting position and the target position of the robot, also obtain the static obstacle information between the starting position and the target position, and obtain the first path based on the starting position, the target position and the static obstacle information;

[0005] Step S2: Smooth the first path through a smoothing algorithm to obtain the second path;

[0006] Step S3: During the process of the robot moving along the second path, the monitoring unit monitors the current position of the moving obstacles in the second path in real time, and obtains the maximum speed of the moving obstacles. Calculate the maximum obstacle avoidance distance based on the current position of the moving obstacle, the maximum speed, the current position of the robot and the estimated average speed;

[0007] Step S4: When the first distance between the robot and the moving obstacle is less than the maximum obstacle avoidance distance, and the real-time speed of the moving obstacle is less than the maximum speed, periodically obtain the average obstacle avoidance distance and the collision speed window, and obtain the obstacle avoidance speed based on the difference between the first distance between the robot and the moving obstacle and the safe obstacle avoidance distance and the collision speed window;

[0008] Step S5: Obtain the third path based on the obstacle avoidance speed and the second path, and reach the target position based on the third path.

[0009] As a preferred technical solution of the present invention, the step S1 includes:

[0010] Take the starting position as the first node of the first path, connect the starting position and the target position of the robot with a straight line, and obtain the static obstacle information on the straight line. The static obstacle closest to the i-th node on the first path is used as the obstacle avoidance target. Obtain the first approximate circle of the obstacle avoidance target, and draw the two tangent lines on both sides of the first approximate circle corresponding to the obstacle avoidance target with the i-th node as the starting point, and use the target points on the two tangent lines as the (i + 1)-th node. The target point is the point on the two tangent lines that can avoid the obstacle avoidance target when connected to the target position and is the closest to the straight line. Repeat this step to obtain all the nodes on the first path. The value of i is a positive integer greater than or equal to 2. The static obstacle includes the position of the static obstacle and the first approximate circle.

[0011] As a preferred technical solution of the present invention, the step S2 includes:

[0012] Connect any two non - consecutive nodes on the first path to obtain a first connection line. When the first connection line can avoid the static obstacle between the two non - consecutive nodes, delete the other nodes between the two non - consecutive nodes to obtain a new first path. Obtain the circumscribed circles of the two edges of each node in the new first path and two tangent points. Use the arc between the two tangent points as a smooth path. Obtain the second path based on the new first path and the smooth path, where by setting the radius of the arc, the arc can satisfy the stable driving of the robot.

[0013] As a preferred technical solution of the present invention, step S3 includes:

[0014] Step S31: During the process of the robot driving along the second path, the monitoring unit monitors the current position of the moving obstacle in real time. When the first distance between the current position of the robot and the current position of the moving obstacle is greater than a preset distance, obtain the real - time speed of the moving obstacle;

[0015] Step S32: When the first distance is less than or equal to the preset distance, obtain the maximum speed of the moving obstacle according to the real - time speed of the moving obstacle obtained previously;

[0016] Step S33: Based on the machine algorithm of the robot, obtain the estimated average speed of the robot before meeting with the moving obstacle. Calculate the maximum obstacle - avoidance distance through the obstacle - avoidance distance formula based on the maximum speed of the moving obstacle, the estimated average speed of the robot, and the radius of the second approximate circle of the moving obstacle. The obstacle - avoidance distance formula is:

[0017]

[0018] where, d max is the maximum obstacle - avoidance distance, V max is the maximum speed of the moving obstacle, V0 is the estimated average speed of the robot, and R0 is the radius of the second approximate circle.

[0019] As a preferred technical solution of the present invention, step S4 includes the following steps:

[0020] Step S41: When the first distance between the robot and the moving obstacle is less than the maximum obstacle avoidance distance and the real-time speed of the moving obstacle is greater than the minimum speed, periodically obtain the second average speed of the moving obstacle and the first average speed of the robot, and periodically calculate the average obstacle avoidance distance and the collision speed window corresponding to the average obstacle avoidance distance based on the first average speed, the second average speed, and the second approximate circle radius of the moving obstacle;

[0021] Step S42: Set the safe obstacle avoidance distance between the robot and the moving obstacle to be the sum of the average obstacle avoidance distance and the margin distance. When the difference between the first distance between the robot and the moving obstacle and the safe obstacle avoidance distance is greater than the set difference, the robot travels at a predetermined speed. When the difference between the first distance and the safe obstacle avoidance distance is less than the set difference, generate the obstacle avoidance speed of the robot based on the collision speed window, where the calculation formula for the margin distance d is:

[0022]

[0023] As a preferred technical solution of the present invention, the formula for the average obstacle avoidance distance is:

[0024]

[0025] where d1 is the obstacle avoidance distance, V1 is the first average speed of the robot, V2 is the second average speed of the moving obstacle, and R is the radius of the second approximate circle of the moving obstacle.

[0026] As a preferred technical solution of the present invention, step S5 includes:

[0027] Obtain the obstacle avoidance path with the shortest distance to the second path based on the obstacle avoidance speed of the robot, connect the obstacle avoidance nodes to the target position, and repeat step S1 and step S2 to obtain the third path, and the robot travels based on the third path.

[0028] As a preferred technical solution of the present invention, the second approximate circle is the circumcircle of the moving obstacle.

[0029] The present invention also provides a local path planning system for a quadruped robot based on an improved DWA and TEB algorithm. The system is used to implement the above method, and the system includes:

[0030] A path planning unit, configured to obtain the starting position and the target position of the robot, and also obtain the static obstacle information between the starting position and the target position, and obtain the first path based on the starting position, the target position, and the static obstacle information;

[0031] A smoothing unit, configured to smooth the first path through a smoothing algorithm to obtain a second path;

[0032] A monitoring unit, configured to, during the process that the robot travels along the second path, monitor the current position of a moving obstacle in the second path in real time, obtain the maximum speed of the moving obstacle, and calculate a maximum obstacle avoidance distance based on the current position of the moving obstacle, the maximum speed, the current position of the robot, and the estimated average speed;

[0033] A calculation unit, configured to, when a first distance between the robot and the moving obstacle is less than the maximum obstacle avoidance distance and the real-time speed of the moving obstacle is less than the maximum speed, periodically obtain an average obstacle avoidance distance and a collision speed window, and obtain an obstacle avoidance speed based on a difference between the first distance between the robot and the moving obstacle and a safe obstacle avoidance distance;

[0034] A path planning unit, further configured to obtain a third path based on the obstacle avoidance speed and the second path, and reach the target position based on the third path.

[0035] The present invention further provides a computer storage medium, where the storage medium stores program instructions, and when the program instructions run, the device where the storage medium is located is controlled to execute the above method.

[0036] Compared with the prior art, the beneficial effects of the present invention are at least as follows:

[0037] In the present invention, the starting position of the above-mentioned robot is connected to the above-mentioned target position by a straight line. The static obstacles passed through by the straight line are the static obstacles that the robot needs to avoid. Taking the above-mentioned $i$-th node as a point, two tangents to the circumcircle of the nearest static obstacle are made. The target points on the two tangents, whose connections with the target position can avoid the obstacle avoidance target and are the shortest distance from the straight line, are used as the $(i + 1)$-th node. All the nodes on the first path are obtained by the above method, so as to obtain the first path with the shortest obstacle avoidance. The first path is also smoothed to obtain the second path. During the process of the robot moving along the second path, the maximum obstacle avoidance distance is calculated based on the average speed of the robot, the second approximate circle radius of the moving obstacle, and the maximum speed, providing a basis for accurately avoiding the moving obstacle. When the first distance between the robot and the moving obstacle is less than the maximum obstacle avoidance distance and the real-time speed of the moving obstacle is less than the maximum speed, the average obstacle avoidance distance and the speed collision window corresponding to the average obstacle avoidance distance are calculated periodically, and the obstacle avoidance speed of the robot is adjusted according to the speed collision window and the distance from the second path. When it is detected that the first distance between the robot and the moving obstacle is greater than the safe obstacle avoidance distance, or the difference between the two is less than the set difference, the robot continues to drive at the predetermined speed. When the first distance is less than the safe obstacle avoidance distance, the difference between the two is continuously greater than or equal to the set difference for $N$ times, and the first distance is greater than the average obstacle avoidance distance, that is, the first distance between the robot and the moving obstacle can meet the requirements, but the distance from the average obstacle avoidance distance is relatively close, and this situation occurs continuously for $N$ times, the obstacle avoidance speed of the robot is generated in time through the above collision speed window. Through the mutual cooperation of the above technical solutions, it can ensure that the robot can accurately avoid obstacles in a stable driving state to the greatest extent, and can also accurately avoid obstacles and improve the overall driving efficiency of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 It is a flowchart of a local path planning method for a quadruped robot based on an improved DWA and TEB algorithms according to the present invention;

[0039] Figure 2 It is a structural diagram of a local path planning system for a quadruped robot based on an improved DWA and TEB algorithms according to the present invention;

[0040] Figure 3 It is a schematic diagram for calculating the balance obstacle avoidance distance according to the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0041] To make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0042] The present invention provides a local path planning method for a quadruped robot based on improved DWA and TEB algorithms, as Figure 1 shown. The method includes the following steps:

[0043] Step S1: Obtain the starting position and the target position of the robot, and also obtain the static obstacle information between the starting position and the target position, and obtain a first path based on the starting position, the target position and the static obstacle information;

[0044] Specifically, since the straight-line distance between two points is the shortest, therefore, the starting position of the above-mentioned robot is used as the first node of the above-mentioned first path, and the first node and the target position are connected by a straight line. The static obstacles passed through by the above-mentioned straight line are the static obstacles that the above-mentioned robot needs to avoid. In order to ensure that the first path is the shortest, the shorter the distance between the obstacle avoidance route and the above-mentioned straight path, the better. The static obstacle closest to the above-mentioned i-th node is used as the above-mentioned obstacle avoidance target, and a first approximate circle of the above-mentioned obstacle avoidance target is obtained. Wherein, the above-mentioned first approximate circle is the circumscribed circle of the above-mentioned static obstacle, and the two tangent lines of the above-mentioned first approximate circle are made with the above-mentioned i-th node as a point, and the above-mentioned target points on the above-mentioned two tangent lines whose connections with the above-mentioned target position can avoid the above-mentioned obstacle avoidance target and are the shortest distance from the above-mentioned straight line are used as the above-mentioned (i + 1)-th node. The above-mentioned (i + 1)-th node is the node that ensures the shortest above-mentioned first path. Repeating this step, that is, the TEB algorithm, to obtain all the nodes on the above-mentioned first path. Through the above technical solution, a first path with the shortest obstacle avoidance can be obtained, thereby shortening the driving time.

[0045] Step S2: Smooth the first path through a smoothing algorithm to obtain a second path;

[0046] Specifically, since the above first path is the shortest path that avoids each of the above static obstacles based on the straight-line path from the above starting position to the above target position, but does not consider the positional relationship between the above static obstacles. Therefore, by connecting any two discontinuous nodes on the above first path, when the above first connection line can avoid the above static obstacles between the two discontinuous nodes, since the straight-line distance is the shortest, other nodes between the two discontinuous nodes are deleted to obtain a new above first path, and two edges of each node in the new above first path are also obtained. Based on the common circumscribed circle of the above two edges and the two tangent points of the common circumscribed circle and the above two edges, and taking the arc between the two tangent points as the above smooth path, where the radius of the above common circumscribed circle is set according to the arc range within which the robot can travel smoothly. Through the above technical solution, the above second path after smooth processing of the first path is obtained, improving the driving efficiency.

[0047] Step S3: During the process of the robot traveling along the second path, the monitoring unit monitors the current position of the moving obstacle in the second path in real time, and obtains the maximum speed of the moving obstacle. Based on the current position of the moving obstacle, the maximum speed, and the current position and estimated average speed of the robot, calculate the maximum obstacle avoidance distance;

[0048] Specifically, during the process of the above robot traveling along the above second path, the monitoring unit detects the current position of the above moving obstacle within the monitoring range in real time. When the first distance between the two is relatively far, i.e., greater than the above preset distance, the monitoring unit detects the real-time speed of the above moving obstacle in real time. When the above moving obstacle approaches the robot, the maximum value in the real-time speed of the above moving obstacle is taken as the above maximum speed. The robot obtains the estimated average speed of the robot from its current position to the current position of the moving obstacle according to its machine algorithm, which is also the estimated average speed before the intersection with the moving obstacle, and calculates the above maximum obstacle avoidance distance based on the above estimated average speed of the robot, the second approximate circle radius of the above moving obstacle, and the above maximum speed. Through the above technical solution, the maximum obstacle avoidance distance that the robot needs to keep from the above moving obstacle to avoid the above moving obstacle can be obtained, providing a basis for accurately avoiding the above moving obstacle.

[0049] Step S4: When the first distance between the robot and the moving obstacle is less than the maximum obstacle avoidance distance and the real-time speed of the moving obstacle is less than the maximum speed, periodically obtain the average obstacle avoidance distance and the collision speed window, and obtain the obstacle avoidance speed based on the difference between the first distance between the robot and the moving obstacle and the safe obstacle avoidance distance and the collision speed window;

[0050] Specifically, when the first distance between the above-mentioned robot and the above-mentioned moving obstacle is less than the maximum obstacle avoidance distance and the real-time speed of the above-mentioned moving obstacle is less than the maximum speed, that is, the above-mentioned robot enters the area where it may collide with the above-mentioned moving obstacle. Since the speed of the above-mentioned moving obstacle may change in real time, if the obstacle avoidance speed of the above-mentioned robot is adjusted in real time according to the real-time obstacle avoidance distance of the above-mentioned moving obstacle, it will increase the instability of the above-mentioned robot. Also, the sum of the above-mentioned average obstacle avoidance distance and the above-mentioned margin distance is used as the above-mentioned safe obstacle avoidance distance. Since the above-mentioned average obstacle avoidance distance is obtained periodically, when it is detected that the above-mentioned first distance between the above-mentioned robot and the above-mentioned moving obstacle is greater than the above-mentioned safe obstacle avoidance distance, or the difference between the two is less than the above-mentioned set difference, that is, the first distance between the above-mentioned robot and the above-mentioned moving obstacle can meet the obstacle avoidance requirements, and there is a margin between the above-mentioned safe obstacle avoidance distance and the collision distance. Also, since the speed of the above-mentioned moving obstacle may still change, the robot can continue to travel at the predetermined speed. When the first distance is less than the above-mentioned safe obstacle avoidance distance and the difference between the two is continuously greater than or equal to the above-mentioned set difference for N times, and the first distance is greater than the above-mentioned average obstacle avoidance distance, and this occurs continuously for N times, where N is a positive integer greater than or equal to 3. Once this situation continues, it is very likely that the first distance will be less than the above-mentioned average obstacle distance, resulting in obstacle avoidance failure. Therefore, the obstacle avoidance speed of the above-mentioned robot is generated in a timely manner through the above-mentioned collision speed window, that is, the DWA algorithm. Through the above technical solution, it is possible to ensure the stable driving state of the above-mentioned robot to the greatest extent and accurately avoid obstacles.

[0051] Step S5: Obtain a third path based on the obstacle avoidance speed and the second path, and reach the target position based on the third path.

[0052] Specifically, obtain the obstacle avoidance path with the shortest distance from the second path through the above-mentioned obstacle avoidance speed of the above-mentioned robot, connect the above-mentioned obstacle avoidance node to the target position, and repeat step S1 and step S2 to obtain the third path. The robot travels based on the third path. Through the above technical solution, not only can the obstacle avoidance path, that is, the local path, for accurate obstacle avoidance be obtained, but also the second path, that is, the global path, can be updated in a timely manner according to the above-mentioned obstacle avoidance node, improving the driving efficiency of the robot.

[0053] Further, the step S1 includes:

[0054] Take the starting position as the first node of the first path. Connect the starting position and the target position of the robot with a straight line, and obtain the information of the static obstacles on the straight line. The static obstacle closest to the i-th node on the first path is used as the obstacle avoidance target. Obtain the first approximate circle of the obstacle avoidance target, and draw the two tangent lines on both sides of the first approximate circle corresponding to the obstacle avoidance target with the i-th node as the starting point. The target points on the two tangent lines are used as the (i + 1)-th node, where the target point is the point on the two tangent lines that can avoid the obstacle avoidance target when connected to the target position and is closest to the straight line. Repeat this step to obtain all the nodes on the first path. The value of i is a positive integer greater than or equal to 2. The static obstacle includes the position of the static obstacle and the first approximate circle.

[0055] Specifically, since the straight-line distance between two points is the shortest, the starting position of the above-mentioned robot is used as the first node of the above-mentioned first path, and the first node and the target position are connected by a straight line. The static obstacles passed through by the straight line are the static obstacles that the robot needs to avoid. In order to ensure that the first path is the shortest, the shorter the distance between the obstacle avoidance route and the straight-line path, the better. The static obstacle closest to the i-th node is used as the obstacle avoidance target, and the first approximate circle of the obstacle avoidance target is obtained. The first approximate circle is the circumcircle of the static obstacle. Draw the two tangent lines on both sides of the first approximate circle with the i-th node as the point, and the target points on the two tangent lines that can avoid the obstacle avoidance target when connected to the target position and are the shortest distance from the straight line are used as the (i + 1)-th node. The (i + 1)-th node is the node that ensures the shortest first path. Repeat this step to obtain all the nodes on the first path. Through the above technical solution, the first path with the shortest obstacle avoidance can be obtained, thereby shortening the driving time.

[0056] Further, step S2 includes:

[0057] Connect any two non-consecutive nodes on the first path to obtain a first connecting line. When the first connecting line can avoid the static obstacles between the two non-consecutive nodes, delete the other nodes between the two non-consecutive nodes to obtain a new first path. Obtain the common circumcircle and two tangent points of the two sides of each node on the new first path. The arc between the two tangent points is used as a smooth path. Based on the new first path and the smooth path, obtain the second path, where by setting the radius of the arc, the arc can satisfy the stable driving of the robot.

[0058] Specifically, since the above-mentioned first path is the shortest path that avoids each of the above-mentioned static obstacles based on the straight-line path from the above-mentioned starting position to the above-mentioned target position, but does not consider the positional relationship between the above-mentioned static obstacles. Therefore, by connecting any two discontinuous nodes on the above-mentioned first path, and when the above-mentioned first connection line can avoid the above-mentioned static obstacles between the two discontinuous nodes, since the straight-line distance is the shortest, other nodes between the two discontinuous nodes are deleted to obtain a new above-mentioned first path. Also, two edges of each node in the new above-mentioned first path are obtained. Based on the common circumscribed circle of the above-mentioned two edges, and the two tangent points of the common circumscribed circle and the above-mentioned two edges, and using the arc between the two tangent points as the above-mentioned smooth path, where the radius of the common circumscribed circle is set according to the arc range within which the robot can travel smoothly. Through the above technical solution, the above-mentioned second path after smooth processing of the first path is obtained, improving the driving efficiency.

[0059] Further, the step S3 includes the following steps:

[0060] Step S31: During the process of the robot traveling along the second path, the monitoring unit is used to monitor the current position of the moving obstacle in real time. When the first distance between the current position of the robot and the current position of the moving obstacle is greater than the preset distance, the real-time speed of the moving obstacle is obtained;

[0061] Step S32: When the first distance is less than or equal to the preset distance, the maximum speed of the moving obstacle is obtained according to the previously obtained real-time speed of the moving obstacle;

[0062] Step S33: Based on the machine algorithm of the robot, the estimated average speed of the robot before meeting with the moving obstacle is obtained. Based on the maximum speed of the moving obstacle, the estimated average speed of the robot, and the second approximate circle radius of the moving obstacle, the maximum avoidance distance is calculated through the avoidance distance formula. The avoidance distance formula is:

[0063]

[0064] where, d max is the maximum avoidance distance, V max is the maximum speed of the moving obstacle, V0 is the estimated average speed of the robot, and R0 is the radius of the second approximate circle.

[0065] Specifically, during the process of the above-mentioned robot traveling along the above-mentioned second path, the current position of the above-mentioned moving obstacle within the monitoring range is detected in real time by the monitoring unit. When the above-mentioned moving obstacle enters the monitoring range of the monitoring unit, the first distance between the two is relatively far, that is, greater than the above-mentioned preset distance. The real-time speed of the above-mentioned moving obstacle is detected in real time by the monitoring unit. When the above-mentioned moving obstacle approaches the above-mentioned robot, that is, when the above-mentioned first distance is less than or equal to the above-mentioned preset distance, the maximum value in the real-time speed of the above-mentioned moving obstacle is taken as the above-mentioned maximum speed. The above-mentioned robot obtains, according to its machine algorithm, the average speed of the above-mentioned robot to the current position of the above-mentioned moving obstacle, which is also the estimated average speed before the intersection with the above-mentioned moving obstacle, and calculates the above-mentioned maximum obstacle avoidance distance based on the estimated average speed of the above-mentioned robot, the second approximate circle radius of the above-mentioned moving obstacle, and the above-mentioned maximum speed. Through the above technical solution, the maximum obstacle avoidance distance that the above-mentioned robot needs to keep from the above-mentioned moving obstacle to avoid the above-mentioned moving obstacle can be obtained, providing a basis for accurately avoiding the above-mentioned moving obstacle.

[0066] Further, step S4 includes the following steps:

[0067] Step S41: When the first distance between the above-mentioned robot and the above-mentioned moving obstacle is less than the above-mentioned maximum obstacle avoidance distance, and the real-time speed of the above-mentioned moving obstacle is less than the maximum speed, periodically obtain the first average speed of the above-mentioned moving obstacle and the second average speed of the above-mentioned robot, and periodically calculate the average obstacle avoidance distance and the collision speed window corresponding to the average obstacle avoidance distance based on the first average speed, the second average speed, and the second approximate circle radius of the above-mentioned moving obstacle;

[0068] Specifically, since the above-mentioned maximum obstacle avoidance distance is the obstacle avoidance distance of the above-mentioned moving obstacle at the maximum speed, however, in actual situations, the speed of the above-mentioned moving obstacle is not constant. Therefore, when the first distance between the above-mentioned robot and the above-mentioned moving obstacle is less than the above-mentioned maximum obstacle avoidance distance and the real-time speed of the above-mentioned moving obstacle is less than the maximum speed, that is, the above-mentioned robot enters the area where it may collide with the above-mentioned moving obstacle. Since the speed of the above-mentioned moving obstacle may change in real time, the corresponding real-time obstacle avoidance distance also changes in real time. If the obstacle avoidance speed of the above-mentioned robot is adjusted in real time according to the real-time obstacle avoidance distance of the above-mentioned moving obstacle, it will increase the instability of the above-mentioned robot. Through the above technical solution, by periodically calculating the average obstacle avoidance distance and the speed collision window corresponding to the average obstacle avoidance distance, and adjusting the obstacle avoidance speed of the robot according to the speed collision window and the distance from the above-mentioned second path, not only can accurate obstacle avoidance be achieved, but also the stability of the above-mentioned robot is increased, and at the same time, the driving path is optimized.

[0069] Step S42: Set the safe obstacle avoidance distance between the robot and the moving obstacle as the sum of the average obstacle avoidance distance and the margin distance. When the first distance between the robot and the moving obstacle is greater than or equal to the safe obstacle avoidance distance, or when the difference between the two is less than the set difference, the robot travels at a predetermined speed. When the first distance is less than the safe obstacle avoidance distance and the difference between the two is continuously greater than or equal to the set difference for N times, and when the first distance is greater than the average obstacle avoidance distance, generate the obstacle avoidance speed of the robot based on the collision speed window. Wherein, the set difference is less than the margin distance, and the calculation formula for the margin distance d is:

[0070]

[0071] Specifically, in order to ensure timely avoidance of the above-mentioned moving obstacle, it is necessary to ensure a sufficient safe obstacle avoidance distance. Therefore, the sum of the above-mentioned average obstacle avoidance distance and the above-mentioned margin distance is used as the above-mentioned safe obstacle avoidance distance. Since the above-mentioned average obstacle avoidance distance is obtained periodically, when it is detected that the above-mentioned first distance between the robot and the above-mentioned moving obstacle is greater than the above-mentioned safe obstacle avoidance distance, or the difference between the two is less than the above-mentioned set difference, that is, the first distance between the robot and the above-mentioned moving obstacle can meet the obstacle avoidance requirements, and there is a margin between the above-mentioned safe obstacle avoidance distance and the collision distance. Also, since the speed of the above-mentioned moving obstacle may change, the robot can continue to travel at a predetermined speed. When the first distance is less than the above-mentioned safe obstacle avoidance distance and the difference between the two is continuously greater than or equal to the above-mentioned set difference for N times, and the first distance is greater than the above-mentioned average obstacle avoidance distance, that is, the first distance between the robot and the above-mentioned moving obstacle can meet the requirements, but is relatively close to the above-mentioned average obstacle avoidance distance and appears continuously for N times. The value of N is a positive integer greater than or equal to 3. Once this situation continues, it is very likely that the first distance will be less than the above-mentioned average obstacle distance, resulting in obstacle avoidance failure. Therefore, generate the obstacle avoidance speed of the robot through the above-mentioned collision speed window in a timely manner. Through the above technical solution, it is possible to ensure the stable driving state of the above-mentioned robot to the greatest extent and accurately avoid obstacles.

[0072] Further, the formula for the average obstacle avoidance distance is:

[0073]

[0074] Wherein, d1 is the obstacle avoidance distance, V1 is the second average speed of the moving obstacle, V2 is the first average speed of the robot, and R is the radius of the second approximate circle of the moving obstacle.

[0075] Specifically, as Figure 3As shown in the figure, the sampling speed region between the above-mentioned robot and a and b is obtained through the TEB algorithm as the collision speed window, that is, when the moving obstacle is regarded as stationary and the robot is traveling at the first average speed, it is the collision speed range between the robot and the moving obstacle. In the figure, d1 is the collision distance between the above-mentioned robot and the above-mentioned moving obstacle, V1 is the vector opposite to the direction of the second average speed vector V3 of the above-mentioned moving obstacle, which is equivalent to the moving obstacle being stationary and the robot traveling towards the moving obstacle at speed V3. Since it is easier to collide when the moving obstacle is moving towards the robot, V1 is on the line connecting the robot and the center of the second approximate circle. V2 is the first average speed vector of the above-mentioned robot, and V is the vector sum of V2 and V3, that is, when the above-mentioned moving obstacle is regarded as stationary, it is the relative speed of the robot relative to the moving obstacle. Since both the robot and the moving obstacle are moving, the above-mentioned moving obstacle is regarded as stationary through the above-mentioned relative speed, and the relative speed is used to represent the moving speed of the robot relative to the above-mentioned moving obstacle. When the speed of the above-mentioned moving obstacle is towards the above-mentioned robot, it is easier for the two to collide. V, as the relative speed between the robot and the moving obstacle, always falls within the collision speed window between the above-mentioned a and b. Therefore, when the above-mentioned relative speed V coincides with a or b, it is the boundary position of whether a collision occurs. d1 is the minimum distance that can avoid obstacles, that is, V2 / V1 = R / d1, and thus d1 = R*V1 / V2, which is the above-mentioned average obstacle avoidance distance.

[0076] Further, the step S5 includes:

[0077] Based on the obstacle avoidance speed of the robot, obtain the obstacle avoidance path with the shortest distance from the second path, connect the obstacle avoidance node with the target position, and repeat step S1 and the step S2 to obtain the third path. The robot travels based on the third path, where the obstacle avoidance node is the point on the obstacle avoidance path that can avoid the moving obstacle and is the closest to the second path.

[0078] Specifically, through the above technical solution, not only can the obstacle avoidance path for accurate obstacle avoidance, that is, the local path, be obtained, but also the second path, that is, the global path, can be updated in a timely manner according to the above-mentioned obstacle avoidance node, improving the driving efficiency of the robot.

[0079] Further, the second approximate circle is the circumcircle of the moving obstacle.

[0080] The present invention also provides a quadruped robot local path planning system based on an improved DWA and TEB algorithms, and the system is used to implement the above method, as Figure 2 shown, the system includes:

[0081] A path planning unit, configured to obtain the starting position and the target position of the robot, also obtain static obstacle information between the starting position and the target position, and obtain a first path based on the starting position, the target position, and the static obstacle information;

[0082] A smoothing unit, configured to smooth the optimal static path through a smoothing algorithm to obtain a second path;

[0083] A monitoring unit, configured to, during the process of the robot traveling along the second path, monitor the current position of a moving obstacle in the second path in real time, obtain the maximum speed of the moving obstacle, and calculate a maximum obstacle avoidance distance based on the current position of the moving obstacle, the maximum speed, the current position of the robot, and the estimated average speed;

[0084] A calculation unit, configured to, when a first distance between the robot and the moving obstacle is less than the maximum obstacle avoidance distance and the real-time speed of the moving obstacle is less than the maximum speed, periodically obtain an average obstacle avoidance distance and a collision speed window, and obtain an obstacle avoidance speed based on a difference between the first distance between the robot and the moving obstacle and a safe obstacle avoidance distance;

[0085] The path planning unit is further configured to obtain a third path based on the obstacle avoidance speed and the second path, and reach the target position based on the third path.

[0086] The present invention further provides a computer storage medium, where the storage medium stores program instructions, and when the program instructions run, the device where the storage medium is located is controlled to execute the above method.

[0087] In summary, in the present invention, the starting position of the robot is connected to the target position by a straight line, and the static obstacles passed through by the straight line are the static obstacles that the robot needs to avoid. Taking the i-th node as a point, two tangent lines to the circumcircle of the nearest static obstacle are made, and the target points on the two tangent lines whose connection lines to the target position can avoid the obstacle avoidance target and are the shortest distance from the straight line are used as the (i + 1)-th node. All the nodes on the first path are obtained by the above method, so as to obtain the first path with the shortest obstacle avoidance. The first path is also smoothed to obtain the second path. During the process of the robot moving along the second path, the maximum obstacle avoidance distance is obtained and calculated based on the average speed of the robot, the second approximate circle radius of the moving obstacle, and the maximum speed, providing a basis for accurately avoiding the moving obstacle. When the first distance between the robot and the moving obstacle is less than the maximum obstacle avoidance distance and the real-time speed of the moving obstacle is less than the maximum speed, the average obstacle avoidance distance and the speed collision window corresponding to the average obstacle avoidance distance are calculated periodically, and the obstacle avoidance speed of the robot is adjusted according to the speed collision window and the distance from the second path. When it is detected that the first distance between the robot and the moving obstacle is greater than the safe obstacle avoidance distance, or the difference between the two is less than the set difference, the robot continues to travel at the predetermined speed. When the first distance is less than the safe obstacle avoidance distance, the difference between the two is continuously greater than or equal to the set difference for N times, and the first distance is greater than the average obstacle avoidance distance, that is, the first distance between the robot and the moving obstacle can meet the requirements, but the distance from the average obstacle avoidance distance is relatively close and appears continuously for N times, the obstacle avoidance speed of the robot is generated in time through the collision speed window. Through the mutual cooperation of the above technical solutions, it is possible to ensure the stable driving state of the robot to the greatest extent, accurately avoid obstacles, and also accurately avoid obstacles and improve the overall driving efficiency of the robot.

[0088] The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity of description, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered as the scope described in this specification.

[0089] The above embodiments only express several implementation manners of the present invention, and their descriptions are relatively specific and detailed, but they should not be construed as limiting the scope of the invention patent. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present invention, several deformations and improvements can be made, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the invention patent should be subject to the appended claims.

[0090] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included within the protection scope of the present invention.

Claims

1. A local path planning method for a quadruped robot based on improved DWA and TEB algorithms, characterized in that, The method includes the following steps: Step S1: Obtain the starting position and target position of the robot, also obtain the static obstacle information between the starting position and the target position, and obtain a first path based on the starting position, the target position, and the static obstacle information; Step S2: Smooth the first path through a smoothing algorithm to obtain a second path; Step S3: During the process of the robot traveling along the second path, the monitoring unit monitors the current position of the moving obstacles in the second path in real time, and obtains the maximum speed of the moving obstacles. Calculate the maximum obstacle avoidance distance based on the current position of the moving obstacles, the maximum speed, the current position of the robot, and the estimated average speed; Step S4: When the first distance between the robot and the moving obstacle is less than the maximum obstacle avoidance distance, and the real-time speed of the moving obstacle is less than the maximum speed, periodically obtain the average obstacle avoidance distance and the collision speed window, and obtain the obstacle avoidance speed based on the difference between the first distance between the robot and the moving obstacle and the safe obstacle avoidance distance and the collision speed window; Step S5: Obtain a third path based on the obstacle avoidance speed and the second path, and reach the target position based on the third path.

2. The method according to claim 1, wherein The said step S1 includes: Take the starting position as the first node of the first path, connect the starting position and the target position of the robot with a straight line, and obtain the static obstacle information on the straight line. The static obstacle closest to the i-th node on the first path is used as the obstacle avoidance target. Obtain the first approximate circle of the obstacle avoidance target, and draw two tangent lines on both sides of the first approximate circle corresponding to the obstacle avoidance target with the i-th node as the starting point. The target points on the two tangent lines are used as the (i + 1)-th node. Among them, the target point is the point on the two tangent lines that can avoid the obstacle avoidance target when connected to the target position and is the closest to the straight line. Repeat this step to obtain all the nodes on the first path. The value of i is a positive integer greater than or equal to 2. The static obstacle includes the position of the static obstacle and the first approximate circle.

3. The method according to claim 1, wherein The said step S2 includes: Connect any two discontinuous nodes on the first path to obtain a first connection line. When the first connection line can avoid the static obstacles between the two discontinuous nodes, delete the other nodes between the two discontinuous nodes to obtain a new first path. Obtain the circumscribed circles of the two sides of each node on the new first path and the two tangent points. The arc between the two tangent points is used as the smoothing path. Obtain the second path based on the new first path and the smoothing path. By setting the radius of the arc, the arc can satisfy the smooth driving of the robot.

4. The method according to claim 1, characterized in that, The said step S3 includes: Step S31: During the process of the robot traveling along the second path, the monitoring unit monitors the current position of the moving obstacle in real time. When the first distance between the current position of the robot and the current position of the moving obstacle is greater than a preset distance, the real-time speed of the moving obstacle is obtained; Step S32: When the first distance is less than or equal to the preset distance, the maximum speed of the moving obstacle is obtained according to the real-time speed of the moving obstacle obtained previously; Step S33: Based on the machine algorithm of the robot, the estimated average speed of the robot before meeting with the moving obstacle is obtained. Based on the maximum speed of the moving obstacle, the estimated average speed of the robot, and the second approximate circle radius of the moving obstacle, the maximum obstacle avoidance distance is calculated through the obstacle avoidance distance formula. The obstacle avoidance distance formula is: where d max is the maximum obstacle avoidance distance, V max is the maximum speed of the moving obstacle, V0 is the estimated average speed of the robot, and R0 is the radius of the second approximate circle.

5. The method according to claim 1, characterized in that, Step S4 includes the following steps: Step S41: When the first distance between the robot and the moving obstacle is less than the maximum obstacle avoidance distance and the real-time speed of the moving obstacle is greater than the minimum speed, the second average speed of the moving obstacle and the first average speed of the robot are periodically obtained, and the average obstacle avoidance distance and the collision speed window corresponding to the average obstacle avoidance distance are periodically calculated based on the first average speed, the second average speed, and the second approximate circle radius of the moving obstacle; Step S42: Set the safe obstacle avoidance distance between the robot and the moving obstacle as the sum of the average obstacle avoidance distance and the margin distance. When the difference between the first distance between the robot and the moving obstacle and the safe obstacle avoidance distance is greater than the set difference, the robot travels at a predetermined speed. When the difference between the first distance and the safe obstacle avoidance distance is less than the set difference, the obstacle avoidance speed of the robot is generated based on the collision speed window. The calculation formula for the margin distance d is:

6. The method according to claim 5, wherein The average obstacle avoidance distance formula is: where d1 is the obstacle avoidance distance, V1 is the first average speed of the robot, V2 is the second average speed of the moving obstacle, and R is the radius of the second approximate circle of the moving obstacle.

7. The method according to claim 1, characterized in that Step S5 includes: Obtain the obstacle avoidance path with the shortest distance from the second path based on the obstacle avoidance speed of the robot, connect the obstacle avoidance node to the target position, and repeat Step S1 and Step S2 to obtain the third path. The robot travels based on the third path.

8. The method according to claim 4, wherein The second approximate circle is the circumcircle of the moving obstacle.

9. A local path planning system for a quadruped robot based on improved DWA and TEB algorithms, the system being used to implement the method according to any one of claims 1-8, characterized in that, The system includes: A path planning unit, configured to obtain the starting position and the target position of the robot, also obtain the static obstacle information between the starting position and the target position, and obtain the first path based on the starting position, the target position, and the static obstacle information; A smoothing unit, configured to smooth the first path through a smoothing algorithm to obtain the second path; A monitoring unit is used to, during the process of the robot traveling along the second path, monitor in real time the current position of a moving obstacle in the second path, obtain the maximum speed of the moving obstacle, and calculate a maximum obstacle avoidance distance based on the current position of the moving obstacle, the maximum speed, and the current position and estimated average speed of the robot; A calculation unit is used to, when a first distance between the robot and the moving obstacle is less than the maximum obstacle avoidance distance and the real-time speed of the moving obstacle is less than the maximum speed, periodically obtain an average obstacle avoidance distance and a collision speed window, and obtain an obstacle avoidance speed based on the difference between the first distance between the robot and the moving obstacle and a safe obstacle avoidance distance; A path planning unit is further used to obtain a third path based on the obstacle avoidance speed and the second path, and reach the target position based on the third path.

10. A computer storage medium, characterized in that, The storage medium stores program instructions, and when the program instructions are running, the device where the storage medium is located is controlled to execute the method according to any one of claims 1-8.

Images