Quadruped robot local path planning method based on improved DWA and TEB algorithms

Abstract

The invention relates to the technical field of path planning, in particular to a quadruped robot local path planning method based on improved DWA and TEB algorithms. The method comprises the following steps: S1, acquiring a first path based on an initial position, a target position and static obstacle information of a robot; s2, smoothing the first path through a smoothing algorithm to obtain a second path; s3, the maximum obstacle avoidance distance is calculated based on the current position and the maximum speed of the moving obstacle and the current position and the estimated average speed of the robot; s4, acquiring the obstacle avoidance speed based on the difference value between the first distance between the robot and the moving obstacle and the safe obstacle avoidance distance and the collision speed window; and S5, acquiring a third path based on the obstacle avoidance speed and the second path, and arriving at the target position based on the third path. According to the invention, the problems of inaccurate obstacle avoidance and low driving efficiency are solved, and the obstacle avoidance precision and the driving efficiency are improved.

Description

Technical Field

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

[0002] As an important part of motion planning, the current development of path planning shows a trend of diversification, intelligence and cross-domain integration. In the field of robotics, path planning is a key technology to achieve 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 relief, unmanned transport vehicles need to plan paths according to actual conditions to achieve efficient and safe operation. Similar prior art includes a Chinese patent with a publication number of CN118274847B, which proposes a navigation decision planning method for an agricultural inspection robot based on road surface roughness, including controlling the agricultural inspection robot to construct a two-dimensional grid map, performing global path planning based on the two-dimensional grid map, and obtaining a global optimal path. Measure the height value of the height data point, and calculate the laser evaluation value based on the height value. Calculate the road surface roughness based on the laser evaluation value and the visual evaluation value, and perform local path planning based on the road surface roughness to obtain a local optimal path. Control the movement of the agricultural inspection robot, update the global optimal path and the local optimal path, until the agricultural inspection robot reaches the target end. By combining the laser evaluation value and the visual evaluation value to calculate the road surface roughness, the road surface roughness of the surrounding environment can be identified and predicted in advance, which improves the accuracy and robustness of the road surface roughness assessment. The global optimal path and the local optimal path are updated in real time according to the actual situation of the scene, which can ensure the smooth progress of the inspection operation. In addition, similar prior art also has a US patent with a publication number of 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 the target surface based on the second mapping, and generate a local inspection path of a mobile unit with a mobile unit smaller than the global path based on the global. Both of the above patents solve the problem of path planning, but neither of them takes into account the problem of speed change of moving obstacles and the problem of path optimization during static obstacle avoidance. 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 an improved DWA and TEB algorithm, the method comprising the following steps:

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

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

[0006] Step S3: When the robot is traveling along the second path, the current position of the moving obstacle in the second path is monitored in real time by a monitoring unit, and the maximum speed of the moving obstacle is obtained, and the maximum obstacle avoidance distance is calculated based on the current position of the moving obstacle, the maximum speed, and the current position and estimated average speed of the robot;

[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 obtaining an average obstacle avoidance distance and a collision speed window, and obtaining an obstacle avoidance speed based on a 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: acquiring a third path based on the obstacle avoidance speed and the second path, and reaching the target position based on the third path.

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

[0010] The starting position is taken as the first node of the first path, the starting position and the target position of the robot are connected by a straight line, and the static obstacle information on the straight line is obtained, the static obstacle closest to the i-th node on the first path is taken as the obstacle avoidance target, and the first approximate circle of the obstacle avoidance target is obtained, and the tangents on both sides of the obstacle avoidance target corresponding to the first approximate circle are made with the i-th node as the starting point, and the target points on the tangents on both sides are taken as the i+1th node, wherein the target point is the point on the tangents on both sides that can avoid the obstacle avoidance target and is closest to the straight line with the line connecting the target position, and this step is repeated to obtain all nodes on the first path, the value of i is a positive integer greater than or equal to 2, and 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, step S2 includes:

[0012] Connect any two discontinuous nodes on the first path to obtain a first connecting line. When the first connecting line can avoid the static obstacle between the two discontinuous nodes, delete other nodes between the two discontinuous nodes to obtain a new first path. Obtain the circumscribed circles of the two sides and two tangent points of each node in the new first path, use the arc between the two tangent points as a smooth path, and obtain the second path based on the new first path and the smooth path, wherein the radius of the arc is set so that the arc can satisfy the smooth driving of the robot.

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

[0014] Step S31: When the robot is moving along the second path, the current position of the moving obstacle is monitored in real time by the monitoring unit, and when a first distance between the current position of the robot and the current position of the moving obstacle is greater than a preset distance, a real-time speed of the moving obstacle is obtained;

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

[0016] Step S33: obtaining the estimated average speed of the robot before it intersects the moving obstacle based on the machine algorithm of the robot, and calculating the maximum obstacle avoidance distance through an obstacle avoidance distance formula 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 obstacle avoidance distance formula is:

[0017]

[0018] Among them, 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 obtaining the second average speed of the moving obstacle and the first average speed of the robot, and periodically calculating 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: setting 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, generating the obstacle avoidance speed of the robot based on the collision speed window, wherein the calculation formula of the margin distance d is:

[0022]

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

[0024]

[0025] Wherein, 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] Based on the obstacle avoidance speed of the robot, an obstacle avoidance path with the shortest distance to the second path is obtained, and an obstacle avoidance node is connected to the target position, and steps S1 and S2 are repeated to obtain a 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 circumscribed circle of the moving obstacle.

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

[0030] A path planning unit, configured to obtain a starting position and a target position of the robot, and 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;

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

[0032] The monitoring unit is used to monitor the current position of the moving obstacle in the second path in real time and obtain the maximum speed of the moving obstacle when the robot is moving along the second path, and calculate the 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;

[0033] a calculation unit, configured to periodically obtain an average obstacle avoidance distance and a collision speed window when a first distance between the robot and the moving obstacle is less than the maximum obstacle avoidance distance and a real-time speed of the moving obstacle is less than a maximum speed, 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] The path planning unit is further used to acquire 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 also provides a computer storage medium, wherein the storage medium stores program instructions, wherein when the program instructions are executed, 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] The present invention connects the starting position of the robot to the target position through a straight line, the static obstacle passed by the straight line is the static obstacle that the robot needs to avoid, and uses the i-th node as a point to make the tangents on both sides of the circumscribed circle of the static obstacle that is closest to the robot, and uses the target point on the two tangents that can avoid the obstacle avoidance target and is the shortest to the straight line as the i+1-th node, and obtains all the nodes on the first path through the method, thereby obtaining the shortest obstacle avoidance first path, and also obtains the second path by smoothing the first path. In the process of the robot traveling 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, so as to provide a basis for accurately avoiding the moving obstacle, and also 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 When the speed is less than the maximum speed, the average obstacle avoidance distance and the speed collision window corresponding to the average obstacle avoidance distance are periodically calculated, 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 a predetermined speed. When the first distance is less than the safe obstacle avoidance distance and the difference between the two is greater than or equal to the set difference for N consecutive 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 is close to the average obstacle avoidance distance and occurs N consecutive 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, the robot can be guaranteed to avoid obstacles accurately under the state of stable driving to the maximum extent, and the robot can also be accurately avoided. The overall driving efficiency of the robot can also be improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 This is a flow chart of a local path planning method for a quadruped robot based on improved DWA and TEB algorithms of the present invention;

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

[0040] Figure 3 This is a schematic diagram of the balanced obstacle avoidance distance calculation of the present invention. DETAILED DESCRIPTION

[0041] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with 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 intended to limit the present invention.

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

[0043] Step S1: obtaining a starting position and a target position of the robot, and also obtaining static obstacle information between the starting position and the target position, and obtaining 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, the starting position of the robot is taken as the first node of the first path, and the first node and the target position are connected by a straight line. The static obstacle passed by the straight line is the static obstacle that the robot needs to avoid. In order to ensure that the first path is the shortest, the obstacle avoidance route is as short as possible from the straight line path. The static obstacle closest to the i-th node is taken as the obstacle avoidance target, and the first approximate circle of the obstacle avoidance target is obtained, wherein the first approximate circle is the circumscribed circle of the static obstacle, and the tangents on both sides of the first approximate circle are made with the i-th node as the point, and the target point on the tangents on both sides that can avoid the obstacle avoidance target and has the shortest distance from the straight line is taken as the i+1 node. The i+1 node is the node that ensures the shortest first path. Repeat this step, i.e., the TEB algorithm obtains all the nodes on the first path. Through the above technical solution, the shortest obstacle avoidance first path can be obtained, thereby shortening the driving time.

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

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

[0047] Step S3: When the robot is traveling along the second path, the current position of the moving obstacle in the second path is monitored in real time by a monitoring unit, and the maximum speed of the moving obstacle is obtained, and the maximum obstacle avoidance distance is calculated based on the current position of the moving obstacle, the maximum speed, and the current position and estimated average speed of the robot;

[0048] Specifically, in the process of the robot traveling along the second path, the current position of the moving obstacle within the monitoring range is detected in real time by the monitoring unit. When the first distance between the two is relatively far, that is, greater than the preset distance, the real-time speed of the moving obstacle is detected in real time by the monitoring unit. When the moving obstacle approaches the robot, the maximum value of the real-time speeds of the moving obstacle is used as the maximum speed. The robot obtains the estimated average speed of the robot 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. The maximum obstacle avoidance distance is calculated based on the estimated average speed of the robot, the second approximate circle radius of the moving obstacle and the maximum speed. Through the technical solution, the maximum obstacle avoidance distance that the robot needs to retain with the moving obstacle to avoid the moving obstacle can be obtained, providing a basis for accurately avoiding the 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 obtaining an average obstacle avoidance distance and a collision speed window, and obtaining an obstacle avoidance speed based on a 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 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, that is, the robot has entered an area where it may collide with the moving obstacle. Since the speed of the moving obstacle may change in real time, if the obstacle avoidance speed of the robot is adjusted in real time according to the real-time obstacle avoidance distance of the moving obstacle, the instability of the robot will be increased. The sum of the average obstacle avoidance distance and the margin distance will be taken as the safe obstacle avoidance distance. Since the average obstacle avoidance distance is acquired periodically, 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, that is, the robot and the moving obstacle are at a certain distance. The first distance between the moving obstacles can meet the obstacle avoidance requirements, and there is a margin between the safe obstacle avoidance distance and the collision distance. Since the speed of the moving obstacles may change, it is possible to continue to travel at a predetermined speed. When the first distance is less than the safe obstacle avoidance distance and the difference between the two is greater than or equal to the set difference for N consecutive times, and the first distance is greater than the average obstacle avoidance distance, and it occurs N consecutively, and the value of N is a positive integer greater than or equal to 3, once this situation continues, it is likely to cause the first distance to be less than the average obstacle distance, resulting in obstacle avoidance failure. Therefore, the obstacle avoidance speed of the robot is generated in time through the collision speed window, i.e., the DWA algorithm. Through the above technical solution, it can be guaranteed to the maximum extent that the robot can accurately avoid obstacles under the state of stable driving.

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

[0052] Specifically, the obstacle avoidance path with the shortest distance to the second path is obtained through the obstacle avoidance speed of the robot, and the obstacle avoidance node is connected to the target position, and steps S1 and S2 are repeated to obtain the third path. The robot travels based on the third path. Through the technical solution, not only can the obstacle avoidance path, i.e., the local path, be obtained for accurate obstacle avoidance, but also the second path, i.e., the global path, can be updated in time according to the obstacle avoidance node, thereby improving the driving efficiency of the robot.

[0053] Furthermore, the step S1 comprises:

[0054] The starting position is taken as the first node of the first path, the starting position and the target position of the robot are connected by a straight line, and the static obstacle information on the straight line is obtained, the static obstacle closest to the i-th node on the first path is taken as the obstacle avoidance target, and the first approximate circle of the obstacle avoidance target is obtained, and the i-th node is used as the starting point to make the tangents on both sides of the obstacle avoidance target corresponding to the first approximate circle, and the target points on the tangents on both sides are taken as the i+1-th node, wherein the target point is the point on the tangents on both sides that can avoid the obstacle avoidance target and is closest to the straight line by connecting the target position, repeat this step to obtain all nodes on the first path, the value of i is a positive integer greater than or equal to 2, and 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 robot is taken as the first node of the first path, and the first node and the target position are connected by a straight line. The static obstacle passed by the straight line is the static obstacle that the robot needs to avoid. In order to ensure that the first path is the shortest, the obstacle avoidance route is as short as possible from the straight line path. The static obstacle closest to the i-th node is taken as the obstacle avoidance target, and the first approximate circle of the obstacle avoidance target is obtained, wherein the first approximate circle is the circumscribed circle of the static obstacle, and the tangents on both sides of the first approximate circle are made with the i-th node as the point, and the target point on the tangents on both sides that can avoid the obstacle avoidance target and has the shortest distance from the straight line is taken as the i+1 node. The i+1 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 shortest obstacle avoidance first path can be obtained, thereby shortening the driving time.

[0056] Furthermore, the step S2 comprises:

[0057] Connect any two discontinuous nodes on the first path to obtain a first connecting line. When the first connecting line can avoid the static obstacle between the two discontinuous nodes, delete other nodes between the two discontinuous nodes to obtain a new first path. Obtain the common circumscribed circle and two tangent points of the two sides of each node in the new first path, use the arc between the two tangent points as a smooth path, and obtain the second path based on the new first path and the smooth path, wherein the radius of the arc is set so that the arc can satisfy the smooth driving of the robot.

[0058] Specifically, since the first path is the shortest path based on a straight line path from the starting position to the target position that avoids each of the static obstacles, but does not take into account the positional relationship between the static obstacles, by connecting any two discontinuous nodes on the first path, the first line can avoid the static obstacles between the two discontinuous nodes. Since the straight line distance is the shortest, the other nodes between the two discontinuous nodes are deleted to obtain the new first path, and the two edges of each node in the new first path are also obtained. Based on the common circumscribed circle of the two edges and the two tangent points of the common circumscribed circle and the two edges, the arc at the two tangent points is used as the smooth path, wherein the radius of the common circumscribed circle is set according to the arc range in which the robot can travel smoothly. Through the technical solution, the second path after the first path is smoothed is obtained, thereby improving the driving efficiency.

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

[0060] Step S31: When the robot is moving along the second path, the current position of the moving obstacle is monitored in real time by the monitoring unit, and when a first distance between the current position of the robot and the current position of the moving obstacle is greater than a preset distance, a 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, obtaining the maximum speed of the moving obstacle according to the previously obtained real-time speed of the moving obstacle;

[0062] Step S33: obtaining the estimated average speed of the robot before it intersects the moving obstacle based on the machine algorithm of the robot, and calculating the maximum obstacle avoidance distance through an obstacle avoidance distance formula 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 obstacle avoidance distance formula is:

[0063]

[0064] Among them, 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.

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

[0066] Furthermore, the step S4 comprises the following steps:

[0067] 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 less than the maximum speed, periodically obtaining the first average speed of the moving obstacle and the second average speed of the robot, and periodically calculating 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;

[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, but in actual circumstances, 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 maximum obstacle avoidance distance and the real-time speed of the above-mentioned moving obstacle is less than the maximum speed, the above-mentioned robot has entered an 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, the instability of the above-mentioned robot will increase. Through the above-mentioned technical solution, by periodically calculating the average obstacle avoidance distance and the speed collision window corresponding to the above-mentioned 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 the driving path is also optimized.

[0069] 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 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 a 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 greater than or equal to the set difference for N consecutive 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 of the margin distance d is:

[0070]

[0071] Specifically, in order to ensure that the above-mentioned moving obstacles can be avoided in time, 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 acquired 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. Since the speed of the above-mentioned moving obstacle may still change, it is possible to 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 greater than or equal to the above-mentioned set difference for N consecutive times, and the first distance is greater than the above-mentioned average obstacle avoidance distance, that is, the first distance between the above-mentioned robot and the above-mentioned moving obstacle can meet the requirements, but is close to the above-mentioned average obstacle avoidance distance, and occurs N times in succession, and the value of N is a positive integer greater than or equal to 3. Once this situation continues, it is likely to cause the first distance to be less than the above-mentioned average obstacle distance, resulting in obstacle avoidance failure. Therefore, the obstacle avoidance speed of the robot is generated in time through the above-mentioned collision speed window. Through the above-mentioned technical solution, it can maximize the guarantee that the above-mentioned robot can accurately avoid obstacles under the state of stable driving.

[0072] Furthermore, the average obstacle avoidance distance formula 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, Figure 3As shown, the sampling speed area between the robot and a and b is obtained by 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, the collision speed range of the robot and the moving obstacle. In the figure, d1 is the collision distance between the robot and the moving obstacle, and V1 is the vector opposite to the second average speed vector V3 of the moving obstacle, which is equivalent to the moving obstacle being stationary and the robot traveling toward the moving obstacle at a speed of V3. Since the moving obstacle is more likely to collide with the robot when traveling toward it, 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 robot, and V is the vector sum of V2 and V3, that is, the moving obstacle is The moving obstacle is regarded as stationary, and the relative speed of the robot relative to the moving obstacle. Since both the robot and the moving obstacle are moving, the moving obstacle is regarded as stationary through the above relative speed, and the above relative speed is used to represent the moving speed of the robot relative to the above moving obstacle. When the speed of the above moving obstacle is toward the above robot, the two are more likely to collide. V, as the relative speed between the robot and the moving obstacle, always falls into the collision speed window between a and b. Therefore, when the above 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, so we can get d1=R*V1 / V2, which is the above average obstacle avoidance distance.

[0076] Furthermore, the step S5 comprises:

[0077] Based on the obstacle avoidance speed of the robot, an obstacle avoidance path with the shortest distance to the second path is obtained, and the obstacle avoidance node is connected to the target position, and steps S1 and S2 are repeated to obtain a third path, and the robot travels based on the third path, wherein the obstacle avoidance node is a point on the obstacle avoidance path that can avoid the moving obstacle and is closest to the second path.

[0078] Specifically, through the above technical solution, not only can the obstacle avoidance path, i.e., the local path, for accurate obstacle avoidance be obtained, but also the second path, i.e., the global path, can be updated in time according to the above obstacle avoidance node, thereby improving the driving efficiency of the robot.

[0079] Furthermore, the second approximate circle is a circumscribed circle of the moving obstacle.

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

[0081] A path planning unit, configured to obtain a starting position and a target position of the robot, and 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 by using a smoothing algorithm to obtain a second path;

[0083] The monitoring unit is used to monitor the current position of the moving obstacle in the second path in real time and obtain the maximum speed of the moving obstacle when the robot is moving along the second path, and calculate the 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;

[0084] a calculation unit, configured to periodically obtain an average obstacle avoidance distance and a collision speed window when a first distance between the robot and the moving obstacle is less than the maximum obstacle avoidance distance and a real-time speed of the moving obstacle is less than a maximum speed, 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 used to acquire 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 also provides a computer storage medium, wherein the storage medium stores program instructions, wherein when the program instructions are executed, the device where the storage medium is located is controlled to execute the above method.

[0087] In summary, the present invention connects the starting position of the robot to the target position through a straight line, the static obstacle passed by the straight line is the static obstacle that the robot needs to avoid, and uses the i-th node as a point to make the tangents on both sides of the circumscribed circle of the static obstacle that is closest to the robot, and uses the target point on the tangents on both sides that can avoid the obstacle avoidance target and is the shortest to the straight line as the i+1-th node, and obtains all the nodes on the first path through the above method, thereby obtaining the shortest obstacle avoidance first path, and also obtains the second path by smoothing the first path. In the process of the robot traveling 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, so as to provide a basis for accurately avoiding the moving obstacle, and also when the first distance between the robot and the moving obstacle is less than the maximum obstacle avoidance distance and the actual speed of the moving obstacle is less than the maximum obstacle avoidance distance, 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. When the speed is less than the maximum speed, the average obstacle avoidance distance and the speed collision window corresponding to the average obstacle avoidance distance are periodically calculated, 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 a predetermined speed. When the first distance is less than the safe obstacle avoidance distance and the difference between the two is greater than or equal to the set difference for N consecutive 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 is close to the average obstacle avoidance distance and occurs N consecutive 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, the robot can be guaranteed to avoid obstacles accurately under the state of stable driving to the maximum extent, and the robot can also be accurately avoided. The overall driving efficiency of the robot can also be improved.

[0088] The technical features of the above-mentioned embodiments can be combined arbitrarily. In order to make the description concise, not all possible combinations of the technical features in the above-mentioned embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0089] The above-mentioned embodiments only express several implementation methods of the present invention, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the patent of the present invention. It should be pointed out that, for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present invention, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention shall be subject to the attached claims.

[0090] The above are only 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 in 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 comprises the following steps: Step S1: obtaining a starting position and a target position of the robot, and also obtaining static obstacle information between the starting position and the target position, and obtaining a first path based on the starting position, the target position and the static obstacle information; Step S2: Smoothing the first path using a smoothing algorithm to obtain a second path; Step S3: When the robot is traveling along the second path, the current position of the moving obstacle in the second path is monitored in real time by a monitoring unit, and the maximum speed of the moving obstacle is obtained, and the maximum obstacle avoidance distance is calculated based on the current position of the moving obstacle, the maximum speed, and the current position and estimated average speed of the robot; 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 obtaining an average obstacle avoidance distance and a collision speed window, and obtaining an obstacle avoidance speed based on a 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: acquiring a third path based on the obstacle avoidance speed and the second path, and reaching the target position based on the third path.

2. The method according to claim 1, characterized in that The step S1 comprises: The starting position is taken as the first node of the first path, the starting position and the target position of the robot are connected by a straight line, and the static obstacle information on the straight line is obtained, the static obstacle closest to the i-th node on the first path is taken as the obstacle avoidance target, and the first approximate circle of the obstacle avoidance target is obtained, and the tangents on both sides of the obstacle avoidance target corresponding to the first approximate circle are made with the i-th node as the starting point, and the target points on the tangents on both sides are taken as the i+1th node, wherein the target point is the point on the tangents on both sides that can avoid the obstacle avoidance target and is closest to the straight line with the line connecting the target position, and this step is repeated to obtain all nodes on the first path, the value of i is a positive integer greater than or equal to 2, and the static obstacle includes the position of the static obstacle and the first approximate circle.

3. The method according to claim 1, characterized in that The step S2 comprises: Connect any two discontinuous nodes on the first path to obtain a first connecting line. When the first connecting line can avoid the static obstacle between the two discontinuous nodes, delete other nodes between the two discontinuous nodes to obtain a new first path. Obtain the circumscribed circles of the two sides and two tangent points of each node in the new first path, use the arc between the two tangent points as a smooth path, and obtain the second path based on the new first path and the smooth path, wherein the radius of the arc is set so that the arc can satisfy the smooth driving of the robot.

4. The method according to claim 1, characterized in that The step S3 comprises: Step S31: When the robot is moving along the second path, the current position of the moving obstacle is monitored in real time by the monitoring unit, and when a first distance between the current position of the robot and the current position of the moving obstacle is greater than a preset distance, a real-time speed of the moving obstacle is obtained; Step S32: when the first distance is less than or equal to the preset distance, obtaining the maximum speed of the moving obstacle according to the previously obtained real-time speed of the moving obstacle; Step S33: obtaining the estimated average speed of the robot before it intersects the moving obstacle based on the machine algorithm of the robot, and calculating the maximum obstacle avoidance distance through an obstacle avoidance distance formula 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 obstacle avoidance distance formula is: Among them, 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: The step S4 comprises 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, periodically obtaining the second average speed of the moving obstacle and the first average speed of the robot, and periodically calculating 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; Step S42: setting 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, generating the obstacle avoidance speed of the robot based on the collision speed window, wherein the calculation formula of the margin distance d is:

6. The method according to claim 5, characterized in that The average obstacle avoidance distance formula is: Wherein, 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 The step S5 comprises: Based on the obstacle avoidance speed of the robot, an obstacle avoidance path with the shortest distance to the second path is obtained, and an obstacle avoidance node is connected to the target position, and steps S1 and S2 are repeated to obtain a third path, and the robot travels based on the third path.

8. The method according to claim 4, characterized in that The second approximate circle is the circumscribed circle of the moving obstacle.

9. A quadruped robot local path planning system based on improved DWA and TEB algorithms, the system being used to implement the method according to any one of claims 1 to 8, characterized in that: The system comprises: A path planning unit, configured to obtain a starting position and a target position of the robot, and 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; a smoothing unit, configured to smooth the first path using a smoothing algorithm to obtain a second path; The monitoring unit is used to monitor the current position of the moving obstacle in the second path in real time and obtain the maximum speed of the moving obstacle when the robot is moving along the second path, and calculate the 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, configured to periodically obtain an average obstacle avoidance distance and a collision speed window when a first distance between the robot and the moving obstacle is less than the maximum obstacle avoidance distance and a real-time speed of the moving obstacle is less than a maximum speed, 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; The path planning unit is further used to acquire 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, wherein when the program instructions are executed, the device where the storage medium is located is controlled to execute the method according to any one of claims 1 to 8.

Images