Quadruped robot obstacle avoidance path planning method and system based on ESDF-Free

Abstract

The invention provides a quadruped robot obstacle avoidance path planning method and system based on ESDF-Free, and the method comprises the steps: 1, generating a collision-free path without considering an obstacle based on a control point and a B spline curve, judging whether the control point on each path is affected by a collision object, if not, directly entering a step 3, and otherwise, executing a step 2; step 2, adding obstacle information and a distance field, generating a collision-free path by adopting an A * algorithm, and optimizing the collision-free path based on a gradient; and 3, judging whether the collision-free path is feasible or not, if the collision-free path is feasible, taking the path as a final planned path, and if the collision-free path is not feasible, optimizing the collision path obtained in the step 2 based on the time span path. According to the method, collision force estimation is avoided, the complexity of a quadruped robot system is reduced by simplifying data requirements, and calculation and storage burdens are reduced.

Description

Technical Field

[0001] The present application relates to the field of path planning, and specifically to an ESDF-Free-based quadruped robot obstacle avoidance path planning method and system. Background Art

[0002] The local path planning and obstacle avoidance technology of quadruped robots can play an important role in a variety of application scenarios. Some common application scenarios include:

[0003] Industrial Automation: Quadruped robots can be used in factories, warehouses and other environments to perform material handling, equipment maintenance and inspection tasks. Local path planning can ensure that quadruped robots can avoid collisions and complete tasks safely in busy industrial sites.

[0004] Search and rescue: In natural disasters or emergencies, quadruped robots can be used to search for survivors or perform emergency rescue missions. Local path planning can help quadruped robots traverse complex terrain, find the shortest path, and avoid dangerous areas.

[0005] Agriculture and rural areas: Quadruped robots can be used for agricultural tasks such as field inspections, crop spraying and harvesting. Path planning can help quadruped robots navigate between different terrains and crops, reducing losses and waste.

[0006] Logistics and delivery: In the field of logistics and delivery, quadruped robots can be used for package sorting, delivery, and warehouse management. Path planning can ensure that quadruped robots can avoid traffic and pedestrians in urban environments and deliver goods efficiently.

[0007] Building and Construction: Quadruped robots can be used for building site and construction tasks such as patrolling the site, moving materials, and performing construction operations. Path planning can help quadruped robots navigate complex construction sites, improving work efficiency and safety.

[0008] Indoor service robots: Quadruped robots can be used as indoor service robots to perform tasks such as cleaning, delivery, and reception in homes or commercial places. Local path planning can help them avoid obstacles and ensure safe movement in indoor environments.

[0009] In general, the local path planning and obstacle avoidance technology of quadruped robots can be applied to various fields that require autonomous navigation, collision avoidance, and efficient task execution to improve work efficiency and safety.

[0010] In autonomous exploration missions, most current research uses drones and wheeled robots, but drones have a small payload and are limited in their role when humans are needed to perform tasks. Wheeled robots have limited mobility and are difficult to navigate in complex, rugged terrain. Quadruped robots have the ability to move in nearly all directions and have strong terrain adaptability, so research on autonomous exploration of quadruped robots is very necessary and promising.

[0011] Many scholars at home and abroad have conducted relevant research on obstacle avoidance of quadruped robots. D Kim and D Carballo used a potential field algorithm, and the proposed algorithm helps the robot find an obstacle-free path according to a given walking direction; Zhiming Chen and Ze Wang proposed an improved dynamic window method (DWA), in which the evaluation function and constraints were modified, and the proposed algorithm significantly improved the success rate of avoiding dynamic obstacles; Facundo García-Cárdenas and Nelson Soberón proposed a LIDAR-based SLAM system, which uses the A* algorithm for path planning to enable the robot to map the surrounding environment while avoiding obstacles; Wang Peng wrote an obstacle avoidance program based on the four infrared ranging sensors carried by the quadruped robot, which can successfully avoid obstacles encountered when moving forward. Although the above-mentioned quadruped robot path planning algorithms are suitable for very complex road conditions, they mostly use the complex data structure of the Euclidean signed distance field (ESDF), which leads to large calculations, long time consumption, heavy storage burden and low operation efficiency. Summary of the invention

[0012] The purpose of this application is to provide a quadruped robot obstacle avoidance path planning method and system based on ESDF-Free, which reduces the complexity of the quadruped robot system and alleviates the computing and storage burden.

[0013] The technical solution to achieve the purpose of the present invention is:

[0014] A quadruped robot obstacle avoidance path planning method based on ESDF-Free, characterized by comprising:

[0015] Step 1: Generate a collision-free path without considering obstacles based on control points and B-spline curves, and determine whether the control points on each path are affected by collision objects. If not, go directly to step 3, otherwise go to step 2;

[0016] Step 2: Add obstacle information and distance field, use A* algorithm to generate collision-free path, and optimize the collision-free path based on gradient;

[0017] Step 3, determine whether the collision-free path is feasible. If feasible, use the path as the final planned path. If not feasible, optimize the collision path obtained in step 2 based on the time span path.

[0018] Furthermore, in step 2, obstacle information and distance field are added, and the A* algorithm is used to generate a collision-free path, which specifically includes:

[0019] Determine the obstacles that collide with the control point Qi based on the distance field and add them to the obstacle collection;

[0020] Based on the obstacles, the A* algorithm is used to generate the { , }right, is the positioning point on the obstacle surface corresponding to the control point Qi, For Unit direction vectors in the same direction;

[0021] Calculate the distance field to update the obstacle collection, the distance field ,like , then the corresponding obstacle will be deleted from the obstacle collection;

[0022] Repeat the above steps and use the A* algorithm to generate { , }right.

[0023] Furthermore, the optimization of the collision-free path based on the gradient specifically includes:

[0024] Construct a penalty objective function for control points based on smoothness penalty, collision penalty, and feasibility penalty;

[0025] Based on the A* algorithm generated , }, the L-BFGS algorithm is used to solve the optimal solution of the control point penalty objective function, and the optimal { , } pair, based on the optimal { , } to generate a safe collision-free path.

[0026] Furthermore, the penalty objective function is:

[0027]

[0028] in is the smoothness penalty, It's a collision penalty. represents the dynamic feasibility penalty, , , is the weight of each penalty term.

[0029] Furthermore, the collision penalty is:

[0030]

[0031]

[0032]

[0033] in, is the safety allowable value, is the distance field, is the control point, , } is the corresponding { , }right.

[0034] Furthermore, the dynamic feasibility penalty is:

[0035]

[0036]

[0037] in, , , are weight coefficients, corresponding to the penalty weights of velocity, acceleration and acceleration rate of change, respectively. The F(C) function is a quadratic continuous differentiable metric function of the high-order derivative of the control point. V i , A i and J i Substitute them into the F(C) function to get the corresponding values, V i , A i and J i They represent the velocity, acceleration and acceleration rate of the ith control point on the trajectory, respectively, and c j and c m is the penalty threshold, a1, b1, c1, a2, b2, c2 and is the coefficient set.

[0038] Furthermore, when the L-BFGS algorithm is used to solve the problem, the following update iteration is used:

[0039]

[0040]

[0041] in, is the product of the matrix consisting of the gradient difference and the step size difference in the iteration, represents the gradient difference, , represents the difference in step size between the kth and k-1th iterations, , is the approximation of the Hessian matrix at the kth and k+1th iterations, I is the identity matrix, Indicates the initial value.

[0042] Furthermore, the optimization of the collision path obtained in step 2 based on the time span path specifically includes:

[0043] Calculate the time span, and refit the optimization path of step 2 based on the time span to generate a new spline curve;

[0044] The collision penalty in the penalty objective function is replaced by a new spline curve, and the optimal solution of the penalty objective function is solved to obtain a feasible trajectory.

[0045] Furthermore, the time span is:

[0046]

[0047]

[0048] in, The ratio of speed, The ratio of acceleration, It represents the ratio of acceleration. 1 is the constant term in the formula, which means that the limit value exceeding the ratio is at least 1.

[0049] A quadruped robot obstacle avoidance path planning system based on ESDF-Free, comprising:

[0050] A collision-free path generation unit generates a collision-free path without considering obstacles based on control points and B-spline curves;

[0051] The collision-free path initial optimization unit adds obstacle information and distance field, and uses the A* algorithm to generate a collision-free path;

[0052] The feasible path generation unit improves the path output by the collision-free path primary optimization unit based on the time span.

[0053] Compared with the prior art, the present invention has the following significant advantages:

[0054] (1) Efficient obstacle avoidance path planning

[0055] Traditional path planning methods usually use complex Euclidean signed distance fields (ESDF) to represent obstacles in the environment, but their computational and maintenance costs are expensive, especially in dynamic environments; this application adopts a new path planning strategy and introduces collision avoidance force estimation technology, which is a simpler but equally efficient method for real-time obstacle detection and obstacle avoidance path planning. This innovation not only reduces the computational cost, but also improves the real-time performance of path planning, enabling the quadruped robot to adapt to environmental changes more quickly; abandoning the traditional ESDF method means that the quadruped robot responds to environmental changes more quickly, improving the real-time and adaptability of navigation.

[0056] (2) Simplify resource requirements

[0057] The ESDF-Free path planning technology innovation in this patent reduces the reliance on complex data structures. Traditional ESDF methods require a lot of computing and storage resources to maintain and update data structures, which is a challenge for resource-constrained quadruped robots. The ESDF-Free path planning strategy reduces the complexity of the quadruped robot system and reduces the computing and storage burden by simplifying data requirements. For resource-constrained quadruped robot applications such as mobile robots, automated warehousing, and emergency rescue missions, this technical feature enables quadruped robots to better adapt to limited computing and storage resources while maintaining efficient path planning capabilities.

[0058] (3) Dynamics-aware path planning

[0059] In the local path planning project of the quadruped robot, the focus of this application is not limited to obstacle avoidance and path generation, but also introduces the dynamic model of the quadruped robot; the technical innovation lies in that this application not only generates safe and collision-free paths, but also ensures that these paths are feasible and optimized under the dynamic constraints of the quadruped robot; this technical feature means that after the planner generates the path, a simple uniform time allocation method is no longer used to control the movement of the quadruped robot along the path; on the contrary, the dynamic model of the quadruped robot will be comprehensively considered to reallocate the time of each point on the path to ensure that the quadruped robot can travel in an efficient manner in a dynamic environment; this reallocation can be dynamically adjusted according to factors such as the speed, acceleration and mechanical structure of the quadruped robot, so as to maximize the performance of the quadruped robot. This innovation will significantly improve the navigation and movement efficiency of the quadruped robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 Algorithm flow for checking and adding obstacle information.

[0061] Figure 2 Flowchart of the planning framework algorithm.

[0062] Figure 3 Schematic diagram of the {p,v} pair.

[0063] Figure 4 Schematic diagram of the convex hull of a B-spline curve.

[0064] Figure 5 To optimize To fit Schematic diagram of . DETAILED DESCRIPTION

[0065] In order to better illustrate the purpose and advantages of the present invention, the implementation method of the present invention is further described in detail below with reference to examples.

[0066] This embodiment provides a quadruped robot obstacle avoidance path planning method based on ESDF-Free, focusing on solving the problem that the current quadruped robot path planning algorithm is suitable for very complex road conditions, such as passing through plum blossom piles, but its calculation amount is large, time-consuming, and limited to very slow movement. The method of this application abandons the complex data structure of the Euclidean signed distance field (ESDF) in the traditional path planning method, introduces collision avoidance force estimation, reduces the complexity of the quadruped robot system, and reduces the calculation and storage burden. Figure 2 , the specific process is:

[0067] Step 1: Avoid collision force estimation

[0068] Step 1.1: Collision-free path generation without considering obstacles

[0069] First, define the control point of the B-spline curve as Q, and generate a B-spline curve Φ that satisfies the terminal constraints but does not consider obstacles. Then, it is necessary to determine whether each control point is affected by the collision in the environment. If not, you can directly proceed to step 3; if affected, you need to use the optimization program (A* algorithm) to generate a collision-free path Γ for each detected collision line segment, and then proceed to steps 1.2 and 2.

[0070] Step 1.2 Add obstacle information and distance field

[0071] For each control point Qi of the line segment where the collision occurs, an anchor point is generated on the obstacle surface. , and corresponds to a repulsive unit direction vector and In the same direction, the {p,v} pair diagram is as follows Figure 3 As shown. Each pair of {p,v} corresponds to a specific control point Q. Algorithm 1 is the generation process of {p,v} pairs, and the flowchart is as follows Figure 1First, we need to find the obstacles that collide with the control point Qi and add them to the obstacle collection. The A* algorithm will generate a collision-free path based on the obstacle collection, and finally determine the matching {p, v} pair based on the control point and the collision-free path.

[0072] The distance from Qi to the jth obstacle is as follows. Note that the unit vector v will not change again after it is generated for the first time, so The value of is positive or negative.

[0073]

[0074] In order to avoid repeatedly generating {p, v} pairs before the trajectory escapes the current obstacle during the first few iterations, this application adopts a criterion that only For all valid j that satisfies When Obstacles that are present are considered newly discovered obstacles. In addition, this criterion allows only necessary obstacles that contribute to the final trajectory to be included in the optimization. As a result, operation time is significantly reduced.

[0075] Step 2: Gradient-based trajectory optimization

[0076] Step 2.1 Construction of control point penalty function

[0077] The optimization problem is formulated as follows:

[0078]

[0079] in is the smoothness penalty, It's a collision. Indicates feasibility, , , is the weight of each penalty term.

[0080] Smoothness penalty: Penalizes acceleration and jerk squared without time integration. Thanks to the convex hull property, control points that minimize the second and third order derivatives of the B-spline trajectory are sufficient to reduce these derivatives along the entire curve.

[0081] Collision Penalty: The collision penalty pushes the control point away from the obstacle. By adopting the safety permission and use To further promote optimization, a twice continuously differentiable penalty function is constructed , and with The decrease of suppresses its slope, thus obtaining a piecewise function:

[0082]

[0083]

[0084] in is the safety interval value, which is a preset safety threshold. The function of this threshold is when the control point Qi is close to a point on the obstacle surface. The distance between Less than safety interval When , a penalty is imposed on the control point. This penalty is achieved by a twice continuously differentiable penalty function This function is implemented as The purpose of this design is to push the control point away from obstacles to ensure the safety of the trajectory.

[0085] Feasibility constraints: Feasibility is ensured by constraining the high-order derivatives of each individual dimension of the trajectory. For example, for each time t, apply . Thanks to the convexity, Figure 4 As shown, the derivatives of the constrained control points are sufficient to constrain the entire B-spline. Therefore, the penalty function is formulated as:

[0086]

[0087]

[0088] Step 2.2 Optimization of penalty term

[0089] There are two problems with the above penalty function. First, the objective function J changes adaptively according to the newly discovered obstacles. It requires the solver to be able to restart quickly. Second, the quadratic term dominates the formula of the objective function, making J approximately a quadratic term. This means that the utilization of the Hessian matrix information can significantly accelerate convergence. However, in real-time applications, obtaining the exact inverse Hessian is prohibitive because it consumes a non-negligible amount of computation. To avoid this, a quasi-Newton method that approximates the inverse Hessian from gradient information is used.

[0090] Considering the performance of the solver, the L-BFGS algorithm was finally selected from the three algorithms belonging to the quasi-Newton method, which balances the loss of restart and the accuracy of the Hessian inverse estimate. A brief description of the algorithm is as follows. For unconstrained optimization problems , where f(x) is a vector from the real vector space The mapping function from the real number set R is usually called the objective function or loss function, which represents the quantity that the application wants to minimize or optimize. The variable x is an n-dimensional real vector that represents the variables or parameters of the optimization problem. The goal of this application is to find a value of this vector that minimizes the objective function f(x). The update of the variable x follows an approximate Newton step.

[0091]

[0092] in is the step size, which determines how many steps are needed to update the variable along the negative gradient direction (through The size of the stride taken (after transformation). Represents the gradient vector of the objective function F(x) at the kth iteration. It is an approximation of the Hessian matrix, which is used to replace the real Hessian matrix in the kth iteration to calculate the update direction of the variable. Each iteration is updated according to the following formula

[0093]

[0094]

[0095] in is a matrix product of the gradient differences and step size differences from previous iterations, Represents the gradient difference, that is, the difference between the current iteration and the previous iteration gradient: . Represents the step size difference, that is, the difference between the current iteration point and the previous iteration point: . I is the identity matrix.

[0096] Step 3: Determine the optimized safety trajectory Is it feasible? If so, will it be a safe trajectory? As the final planned path, estimated time reallocation and trajectory improvement will be performed if it is not feasible.

[0097] It is not reasonable to assign an exact time profile before optimization, since the planner does not know the information about the final trajectory at that time. Therefore, an additional time reallocation procedure is essential to ensure dynamic feasibility. Safe trajectories generated in “Gradient-based Trajectory Optimization” Regenerate a reasonable time redistribution uniform B-spline trajectory , using anisotropic matching method to make the newly generated trajectory Freely optimize its control points to satisfy higher-order constraints while maintaining and Almost the same shape.

[0098] First calculate the limit beyond the ratio:

[0099]

[0100] in The part represents the ratio of speed, The part represents the ratio of acceleration, The part represents the ratio of acceleration, and 1 is the constant term of the formula, which means that the limit value exceeding the ratio is at least 1, which means that no matter what the value of other terms is, The value of will not be less than 1. Relative to , should be extended How much time is allocated? New time span:

[0101]

[0102] The time span is of Initialized and maintained under bounds With the same shape and control points, smoothness and feasibility are refined by solving a closed-form least-squares problem followed by optimization.

[0103] To summarize the planning framework: Set the B-spline order to , control point The number of changes around 25, which is determined by the planning distance (about 7m) and the initial distance interval between adjacent points (about 0.3m). This is an empirical parameter that can balance the complexity and degrees of freedom of the problem. The time complexity is , because according to the local support characteristics of B-spline, a control point only affects nearby line segments. The complexity of L-BFGS is also linear on the same relative tolerance. For collision-free path search, the A* algorithm is used, which has a good advantage that the path Γ always tends to naturally approach the obstacle surface. Therefore, the present application can directly select p at Γ without performing an obstacle surface search.

[0104] for Figure 5 The vector defined in , can be derived from the properties of uniform b-spline parameterization, satisfy

[0105]

[0106] To further enforce safety, a collision check is performed against a circular tube with a fixed radius around the final trajectory to provide adequate obstacle clearance. The optimizer stops when no collision is detected.

[0107] A quadruped robot obstacle avoidance path planning system based on ESDF-Free, comprising:

[0108] A collision-free path generation unit generates a collision-free path without considering obstacles based on control points and B-spline curves;

[0109] The collision-free path initial optimization unit adds obstacle information and distance field, and uses the A* algorithm to generate a collision-free path;

[0110] The feasible path generation unit improves the path output by the collision-free path primary optimization unit based on the time span.

[0111] The local path planning and obstacle avoidance of quadruped robots requires the integration of multidisciplinary knowledge, covering sensor technology, map construction, path planning algorithms, obstacle avoidance algorithms, control technology and other fields to ensure that the quadruped robot can move safely and efficiently in complex environments. This patent proposes a new ESDF-Free path planning technology and obstacle avoidance method, which abandons the complex data structure of the Euclidean signed distance field (ESDF) in the traditional path planning method and introduces collision avoidance force estimation. The ESDF-Free path planning strategy reduces the complexity of the quadruped robot system by simplifying data requirements and reduces the computing and storage burden.

[0112] Although the preferred embodiments of the present invention have been described, those skilled in the art may make other changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

[0113] Obviously, those skilled in the art can make various changes and modifications to the embodiments of the present invention without departing from the spirit and scope of the embodiments of the present invention. Thus, if these modifications and variations of the embodiments of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims

1. A quadruped robot obstacle avoidance path planning method based on ESDF-Free, characterized in that: include: Step 1: Generate a collision-free path without considering obstacles based on control points and B-spline curves, and determine whether the control points on each path are affected by collision objects. If not, go directly to step 3, otherwise go to step 2; Step 2: Add obstacle information and distance field, use A* algorithm to generate collision-free path, and optimize the collision-free path based on gradient; Step 3, determine whether the collision-free path is feasible. If feasible, use the path as the final planned path. If not feasible, optimize the collision path obtained in step 2 based on the time span path.

2. The ESDF-Free based quadruped robot obstacle avoidance path planning method according to claim 1, characterized in that: In step 2, obstacle information and distance field are added, and the A* algorithm is used to generate a collision-free path, which specifically includes: Determine the obstacles that collide with the control point Qi based on the distance field and add them to the obstacle collection; Based on the obstacles, the A* algorithm is used to generate the { , }right, is the positioning point on the obstacle surface corresponding to the control point Qi, For Unit direction vectors in the same direction; Calculate the distance field to update the obstacle collection, the distance field ,like , then the corresponding obstacle will be deleted from the obstacle collection; Repeat the above steps and use the A* algorithm to generate { , }right.

3. The ESDF-Free based quadruped robot obstacle avoidance path planning method according to claim 2, characterized in that: The optimization of collision-free paths based on gradients specifically includes: Construct a penalty objective function for control points based on smoothness penalty, collision penalty, and feasibility penalty; Based on the A* algorithm generated , }, the L-BFGS algorithm is used to solve the optimal solution of the control point penalty objective function, and the optimal { , } pair, based on the optimal { , } to generate a safe collision-free path.

4. The ESDF-Free based quadruped robot obstacle avoidance path planning method according to claim 3, characterized in that: The penalty objective function is: ； in is the smoothness penalty, It's a collision penalty. represents the dynamic feasibility penalty, , , is the weight of each penalty term.

5. The ESDF-Free based quadruped robot obstacle avoidance path planning method according to claim 4, characterized in that: The collision penalty is: ； ； ； in, is the safety allowable value, is the distance field, is the control point, , } is the corresponding { , }right.

6. The ESDF-Free based quadruped robot obstacle avoidance path planning method according to claim 5, characterized in that: The dynamic feasibility penalty is: ； ； in, , , are weight coefficients, corresponding to the penalty weights of velocity, acceleration and acceleration rate of change, respectively. The F(C) function is a quadratic continuous differentiable metric function of the high-order derivative of the control point. V i , A i and J i Substitute them into the F(C) function to get the corresponding values, V i , A i and J i They represent the velocity, acceleration and acceleration rate of the ith control point on the trajectory, respectively, and c j and c m is the penalty threshold, a1, b1, c1, a2, b2, c2 and is the coefficient set.

7. The ESDF-Free based quadruped robot obstacle avoidance path planning method according to claim 5, characterized in that: When using the L-BFGS algorithm to solve, the following update iteration is used: ； ； in, is the product of the matrix consisting of the gradient difference and the step size difference in the iteration, represents the gradient difference, , represents the difference in step size between the kth and k-1th iterations, , is the approximation of the Hessian matrix at the kth and k+1th iterations, I is the identity matrix, Indicates the initial value.

8. The ESDF-Free based quadruped robot obstacle avoidance path planning method according to claim 1, characterized in that: The optimization of the collision path obtained in step 2 based on the time span path specifically includes: Calculate the time span, and refit the optimization path of step 2 based on the time span to generate a new spline curve; The collision penalty in the penalty objective function is replaced by a new spline curve, and the optimal solution of the penalty objective function is solved to obtain a feasible trajectory.

9. The ESDF-Free based quadruped robot obstacle avoidance path planning method according to claim 8, characterized in that: The time span is: ； ； in, The ratio of speed, The ratio of acceleration, It represents the ratio of acceleration. 1 is the constant term in the formula, which means that the limit value exceeding the ratio is at least 1.

10. An ESDF-Free based quadruped robot obstacle avoidance path planning system for implementing any of the methods described in claims 1-9, characterized in that: include: A collision-free path generation unit generates a collision-free path without considering obstacles based on control points and B-spline curves; The collision-free path initial optimization unit adds obstacle information and distance field, and uses the A* algorithm to generate a collision-free path; The feasible path generation unit improves the path output by the collision-free path primary optimization unit based on the time span.

Images