Safe motion planning method and device for quadruped robot

Abstract

The invention provides a method and device suitable for safe motion planning of a quadruped robot in a complex environment, and the method comprises the steps: sensing the environment through a visual sensor and a laser radar, and constructing an environment sensing and spatial modeling system; environment point cloud information collected by the laser radar is sampled and clustered; parameterizing the enveloping boundary of the point cloud obstacle into an elliptic model, wherein the long axis direction of the elliptic model is determined according to the maximum extension dimension of the obstacle; performing motion prediction and trajectory planning, establishing a target optimization function of the model prediction function, and adding a control barrier function constraint condition; introducing an expected value of a CBF attenuation rate into a cost function of model prediction control; and solving an optimization problem of model prediction control, and sending an optimal solution of the optimization problem as control input to a motion control module of the quadruped robot to realize navigation of the quadruped robot. According to the method, the robustness and scene generalization ability of the quadruped robot in a complex dynamic environment are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of motion planning, and in particular to a method and device for safe motion planning of a quadruped robot in complex scenes. Background Art

[0002] With the rapid development of robotics, mobile robots have found widespread application in fields such as logistics, autonomous driving, and field exploration and rescue. Quadruped robots are developing particularly rapidly, offering enormous potential for applications in a variety of tasks, including inspection, rescue, and post-disaster exploration. Ensuring the safe operation of quadruped robots in complex operational scenarios remains a key challenge.

[0003] From a technological perspective, early patents for quadruped robots often employed a layered architecture, for example, decoupling path planning, trajectory generation, and underlying control modules. However, this approach struggles to address real-time obstacle avoidance requirements. The integrated MPC-CBF design reduces obstacle avoidance generation time by over 30% by unifying the trajectory tracking term and CBF constraint term in the optimization objective function, while ensuring the asymptotic stability of the closed-loop system through the Lyapunov function. This technical approach has been validated in a dual-model predictive control study published in IEEE Trans. Ind. Electron 2024.

[0004] In recent years, researchers have taken numerous approaches to enabling robots to track their paths and avoid obstacles in real time. For example, Chinese patent application number 202311337737.8 discloses a trajectory planning and tracking method based on MPC, which plans and tracks paths. However, this method fails to consider situations where the system requires both trajectory tracking and obstacle avoidance. Obstacle avoidance is only performed when an obstacle is about to be encountered, potentially violating the system's safety constraints. Chinese patent application number 202411889426.7 addresses a robust control method for a variable-speed cruise control system, but only implements robust control without considering the controller's adaptive capabilities and safety constraints. Chinese patent application number 202411865587.2 proposes a quadruped robot obstacle avoidance method and system that utilizes a LiDAR and depth camera. However, this method relies solely on the combined use of LiDAR and depth cameras for obstacle avoidance, returning to the previous stop upon encountering an obstacle, failing to achieve adaptive obstacle avoidance. Application number 202410277509.4 proposes a robust safety control method for moving bodies based on laser point cloud and obstacle function. This method uses obstacle function to establish safety constraints, but does not consider the situation where the control input conflicts with the safety constraints, making the quadratic programming problem unsolvable. Summary of the Invention

[0005] In response to the deficiencies in the prior art, the present invention provides a method and device for safe motion planning of a quadruped robot in a complex environment, which can ensure the safety and stability of the quadruped robot's movement in a complex environment. Due to the CBF function, the quadruped robot can make avoidance actions to obstacles in advance. In addition, the present invention also adds the expected value of the decision variable to the CBF constraint term, which alleviates the conflict between the control input restriction of the quadruped robot and the quadratic programming problem, and improves the adaptability of the controller.

[0006] To achieve the above objectives, a first aspect of the present invention provides a method for safe motion planning of a quadruped robot in a complex environment, comprising the following steps:

[0007] S1: Build an environmental perception and spatial modeling system to perceive the environment through visual sensors and lidar to achieve self-positioning.

[0008] S2: Downsample and cluster the environmental point cloud information collected by the lidar, select one of the point clouds in the local point cloud, and use a fixed step size d point Eliminate other adjacent points. At the same time, use the Euclidean clustering algorithm to filter the point cloud, and use the nearest neighbor query algorithm based on the KD tree to speed up the filtering process.

[0009] S3: The point cloud obstacle envelope boundary is parameterized into an elliptical model, whose major axis direction is dynamically determined according to the maximum extension dimension of the obstacle. It is represented as the obstacle parameter set (s, p), where s represents the coordinates of the obstacle's centroid (s1, s2, s3) and p describes its geometric characteristics.

[0010] S4: Perform motion prediction and trajectory planning, using Kalman filtering to estimate the obstacle's motion state and generate an elliptical envelope trajectory sequence of the obstacle in the forward time domain. Combined with the model predictive control framework, a multi-objective optimization function is established in the receding time domain:

[0011]

[0012] Among them, x N is the terminal state, is the terminal expected value; at the kth moment, the state information of the quadruped robot is represented by x k =[x k ,y k ,θ k ,r,v,ω] T , where (x k ,y k ) represents the two-dimensional coordinates of the quadruped robot in the current map, θ k represents the orientation angle of the quadruped robot, r represents the farthest radius of the quadruped robot, (v, w) represents the current linear velocity and angular velocity of the mobile robot; u kFor the control information of a quadruped robot; Q, R, and F are symmetric positive definite weight matrices; and are the state set and input set that the robot can physically reach.

[0013] The constraint of the control barrier function (CBF) is expressed as follows:

[0014] L f h(x)+L g h(x)u + λh(x) ≥ 0 (2)

[0015] where, L f and L g are Lie derivative operators to simplify the formula, λ is a decision variable, and h(x) is the control barrier function.

[0016] This constraint ensures that the time-varying distance between the robot's center and the elliptical boundary of the obstacle always satisfies the safety threshold.

[0017] S41: In step S4 further described,

[0018] The control barrier function constructs the barrier function as follows:

[0019] h(x) = ||x t (k) - s i ||2 - l i - d safe (3)

[0020] where, x t (k) is the current position of the robot, s i is the center position of the obstacle, l i is the distance from the intersection of the line connecting the robot's center and the elliptical center of the obstacle to the elliptical center, and d safe is the safety distance that the robot needs to maintain from the obstacle.

[0021] This inequality transforms the safety constraint into a linear or convex constraint condition of a QP problem.

[0022] S5: According to the derivative constraint of the described barrier function, introduce the expected value of the CBF decay rate and add it to the cost function, which is expressed as follows:

[0023]

[0024] where, Q, R, and F are used to adjust the penalty intensities of the state tracking error, control input energy consumption, and terminal state error; the constant r is the penalty factor, and the positive constant λ d is the expected value of λ.

[0025] S6: Solve the optimization problem of model predictive control, and use the optimal solution of this optimization problem as the control input and send it to the motion control module of the quadruped robot to achieve the navigation of the quadruped robot. The overall optimization problem is expressed as follows:

[0026]

[0027] where x k+1 = f(x k , u) is the kinematic constraint of the quadruped robot.

[0028] The second aspect of the present invention relates to a safety motion planning device for a quadruped robot, including a memory and one or more processors. Executable code is stored in the memory. When the one or more processors execute the executable code, it is used to implement the safety motion planning method for the quadruped robot of the present invention.

[0029] The third aspect of the present invention relates to a computer-readable storage medium, on which a program is stored. When the program is executed by a processor, it implements the safety motion planning method for the quadruped robot of the present invention.

[0030] Based on the robot state and obstacle information, the present invention combines the control barrier function and model predictive control to construct an obstacle avoidance constraint, and further converts this constraint into an obstacle avoidance cost term in the cost function and introduces it into the optimization objective of model predictive control. To further improve safety and algorithm feasibility, the present invention also improves the control barrier function: introducing the expected value of the decision variable into the cost function. When the robot approaches an obstacle, this improvement can enhance the obstacle avoidance behavior; while when it is far from the obstacle, it improves the solvability of the optimization problem. Experimental results show that the present invention improves the robustness and scenario generalization ability of the quadruped robot in complex dynamic environments.

[0031] The beneficial effects of the present invention are as follows:

[0032] The present invention is a safety motion planning method and device for a quadruped robot applied to complex scenarios, which is a safety motion planning method combining a control barrier function and model predictive control. On the premise of ensuring the safety of the mobile robot, it enables the robot to reach the destination more efficiently. Considering the volume problem of the quadruped robot itself, the present invention introduces the expected value of the control barrier function decision variable, which not only dynamically maintains the feasibility of solving the QP problem and the system safety, but also ensures the stability of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 is a flowchart of the method of the present invention.

[0034] Figure 2 is a flowchart of the local obstacle avoidance method of the present invention.

[0035] Figure 3 This is a simulation diagram of the quadruped robot in the gazebo environment of the present invention.

[0036] FIG4 is a schematic diagram of an obstacle ellipse according to the present invention.

[0037] FIG5 is a schematic diagram of local path planning of a quadruped robot according to the present invention.

[0038] Figure 6 It is a path tracking accuracy diagram of the present invention.

[0039] Figure 7 It is a device diagram of the present invention. DETAILED DESCRIPTION

[0040] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention is further described in detail below with reference to the accompanying drawings. It should be understood that the terms used in this specification are only for describing specific embodiments and are not intended to limit the present invention.

[0041] Example 1

[0042] This embodiment relates to a method for safe motion planning of a quadruped robot in a complex environment. The flow chart is as follows: Figure 1 As shown, the following steps are included:

[0043] S1: Build an environmental perception and spatial modeling system, using visual sensors and LiDAR to perceive the environment and synchronize positioning with mapping to update the 3D probability map in real time. While generating a navigation reference path based on the PF-RRT* algorithm, establish an obstacle recognition mechanism.

[0044] S2: Downsample and cluster the environmental point cloud information collected by the lidar, select one of the point clouds in the local point cloud, and use a fixed step size d point Eliminate other adjacent points. At the same time, use the Euclidean clustering algorithm to filter the point cloud, and use the nearest neighbor query algorithm based on the KD tree to speed up the filtering process.

[0045]

[0046] Among them, x and y are n-dimensional vectors, x1, x2,…, x n and x1,x2,…,x n is the corresponding vector, d point Take 0.05m.

[0047] S3: The point cloud obstacle envelope boundary is parameterized into an elliptical model, whose major axis direction is dynamically determined according to the maximum extension dimension of the obstacle. It is represented as the obstacle parameter set (s, p), where s represents the coordinates of the obstacle's centroid (s1, s2, s3) and p describes its geometric characteristics.

[0048] S4: Figure 2 The following is the local obstacle avoidance flowchart of the system. The system first performs motion prediction and trajectory planning, uses Kalman filtering to estimate the motion state of obstacles, and generates a sequence of obstacle elliptical envelope trajectories in the forward time domain. Combining with the model predictive control framework, a multi-objective optimization function is established in the rolling time domain:

[0049]

[0050] where x N is the terminal state, is the terminal expected value; at the k-th moment, the state information of the quadruped robot is expressed as x k = [x k , y k , θ k , r, v, ω] T , where (x k , y k ) represents the two-dimensional coordinates of the quadruped robot in the current map, θ k represents the orientation angle of the quadruped robot, r represents the farthest radius of the quadruped robot, (v, w) represents the current linear velocity and angular velocity of the mobile robot; u k is the control information of the quadruped robot; Q, R, and F are symmetric positive definite weight matrices; and are the state set and input set that the robot can physically reach.

[0051] The constraint conditions of the control barrier function (CBF) are expressed as follows:

[0052] L f h(x) + L g h(x)u + λh(x) ≥ 0 (2)

[0053] where L f and L g are Lie derivative operators to simplify the formula, λ is a decision variable, and h(x) is the control barrier function.

[0054] This constraint ensures that the time-varying distance between the robot center and the elliptical boundary of the obstacle always satisfies the safety threshold.

[0055] S41: Further in step S4,

[0056] The control barrier function constructs the barrier function as follows:

[0057] h(x) = ||x t (k) - s i ||2 - l i - d safe (3)

[0058] Among them, x t (k) is the position of the current robot, and s i is the center position of the obstacle, and l i is the distance from the intersection of the line connecting the center of the robot and the center of the obstacle ellipse to the center of the ellipse, and d safe is the safety distance that the robot needs to maintain from the obstacle.

[0059] This inequality transforms the safety constraint into a linear or convex constraint condition of the QP problem.

[0060] S5: Introduce the expected value of the CBF decay rate according to the derivative constraint of the obstacle function, and add it to the cost function. The cost function is expressed as follows:

[0061]

[0062] Among them, Q, R, and F are used to adjust the penalty intensity of the state tracking error, control input energy consumption, and terminal state error; the constant r is the penalty factor with a value of 1, and the positive constant λ d is the expected value of λ, with a value of 1.8.

[0063] S6: Solve the optimization problem of model predictive control, and use the optimal solution of this optimization problem as the control input and send it to the motion control module of the quadruped robot to achieve the navigation of the quadruped robot. The overall optimization problem is expressed as follows:

[0064]

[0065] Among them, x k+1 = f(x k , u) is the kinematic constraint of the quadruped robot.

[0066] By solving the above optimization problem, the optimal solution sequence at time t can be obtained, that is, u t+k∣t = {u t∣t ,..., u t+N-1|}, and N takes 12. Only the first element of the optimal solution sequence will be used as the control input and published to the robot, so that the robot can efficiently reach the destination while avoiding obstacles.

[0067] To illustrate the effectiveness of the method proposed in this embodiment, the ROS platform is used for simulation. Figure 3 It is the simulation of the quadruped robot in the gazebo environment. Figure 4a This is the virtual real environment set in gazebo in this example. Currently, three obstacles are set, Figure 4bThe safety boundary ellipse generated by three different obstacles is shown in rviz. The boundary ellipse parameters of the obstacle are generated by point cloud detection. Figure 5a and Figure 5b This is the local path generated by the quadruped robot when there is an obstacle in the forward time domain. The thick blue line is the global planned path, the thin green line is the local planned path, and the ellipse is the safety ellipse generated by the obstacle. Figure 5a It shows that when the robot is close to the obstacle safety boundary ellipse, the local path will always maintain a safe distance from the obstacle. Figure 5b It shows that when an obstacle appears on the global planned path, the robot will plan a local path to avoid the obstacle at a safe distance. Figure 6 This is the tracking accuracy diagram of the quadruped robot. It can be seen that the tracking accuracy of the quadruped robot is always within the range of 0.05m, indicating that the planned local path is very reasonable.

[0068] Example 2

[0069] Reference Figure 7 This embodiment relates to a safe motion planning device for a quadruped robot, including a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, they are used to implement the safe motion planning method for the quadruped robot of the present invention.

[0070] Example 3

[0071] This embodiment relates to a computer-readable storage medium having a program stored thereon. When the program is executed by a processor, the safe motion planning method for a quadruped robot of the present invention is implemented.

[0072] The contents described in the embodiments of this specification are merely an enumeration of the implementation forms of the inventive concept. The scope of protection of the present invention should not be regarded as limited to the specific forms described in the embodiments. The scope of protection of the present invention also extends to equivalent technical means that can be conceived by those skilled in the art based on the inventive concept.

Claims

1. A safety motion planning method for a quadruped robot, comprising the following steps: Step 1: Sense the environment through a vision sensor and a lidar, and construct an environmental perception and spatial modeling system; Step 2: Sample and cluster the environmental point cloud information collected by the lidar; Step 3: Parameterize the envelope boundary of the point cloud obstacle as an ellipse model, and determine the major axis direction according to the maximum expansion dimension of the obstacle; Step 4: Perform motion prediction and trajectory planning, establish an objective optimization function of the model prediction function, and add a control barrier function constraint condition; Step 5: Introduce the expected value of the CBF decay rate into the cost function of the model predictive control; Step 6: Solve the optimization problem of the model predictive control, and use the optimal solution of this optimization problem as the control input and send it to the motion control module of the quadruped robot to achieve the navigation of the quadruped robot.

2. The safety motion planning method for a quadruped robot according to claim 1, characterized in that: In Step 2, sampling and clustering the collected environmental point cloud information, the specific steps include: Step 2.1: Downsample and cluster the environmental point cloud information collected by the lidar, select one point cloud from the local point cloud, and use a fixed step size d point to remove other adjacent points; Step 2.2: Use the Euclidean clustering algorithm to filter the point cloud, and accelerate the filtering speed based on the nearest neighbor query algorithm of the KD tree.

3. The safety motion planning method of a quadruped robot according to claim 1, characterized in that: In Step 4, the established control barrier function constraint, the specific steps include: Step 4.1: The constraint of the control barrier function (CBF) is expressed as follows: L f h(x)+L g h(x)u + λh(x) ≥ 0 (2) Among them, L f and L g are Lie derivative operators for simplifying the formula, λ is a decision variable, and h(x) is a control barrier function; Step 4.2: The control barrier function constructs the barrier function as follows: h(x) = ||x t (k)-s i ||2-l i -d safe (3) Among them, x t (k) is the position of the current robot, s i is the center position of the obstacle, l i is the distance from the intersection point of the line connecting the center of the robot and the center of the obstacle ellipse to the center of the ellipse to the center of the ellipse, d safe is the safe distance that the robot needs to maintain from the obstacle; This constraint ensures that the time-varying distance between the robot center and the elliptical boundary of the obstacle always satisfies the safety threshold.

4. The safety motion planning method of a quadruped robot according to claim 1, characterized in that: In Step 5, introducing the expected value of the CBF decay rate into the cost function, the specific steps include: Step 5.1: The cost function of the model predictive control is expressed as: where x N is the terminal state, and k is the terminal expected value; at the k-th moment, the state information of the quadruped robot is expressed as x k = [x k , y k , θ T , r, v, ω] k , where (x k , y k ) represents the two-dimensional coordinates of the quadruped robot in the current map, θ k represents the orientation angle of the quadruped robot, r represents the farthest radius of the quadruped robot, and (v, w) represents the current linear velocity and angular velocity of the mobile robot; u k is the control information of the quadruped robot; Step 5.2: After introducing the expected value of the CBF decay rate, it is expressed as: Among them, Q, R, and F are used to adjust the state tracking error, control the input energy consumption, and the penalty intensity of the terminal state error; the constant r is the penalty factor, and the positive constant λ d is the expected value of λ.

5. A safety motion planning device for a quadruped robot, characterized in that, Including a memory and one or more processors, executable code is stored in the memory, and when the one or more processors execute the executable code, it is used to implement the safety motion planning method for the quadruped robot according to any one of claims 1-4.

6. A computer-readable storage medium, characterized in that, A program is stored thereon, and when the program is executed by a processor, it implements the safety motion planning method for the quadruped robot according to any one of claims 1-4.

Images