A safe navigation and localization method for bipedal robots based on discrete control barrier functions and hierarchical architecture

Abstract

This invention discloses a safe navigation and localization method for bipedal robots based on discrete control barrier functions and a hierarchical architecture, belonging to the field of robot control technology. The method employs the Relative Dynamics Test (RRT) algorithm to generate a global path and combines it with a Linear Inverted Pendulum Model (LIPM) to generate a stable gait. A discrete control barrier function (D-CBF) is used to ensure obstacle avoidance in dynamic environments, and the safe control input is solved through an optimization problem. Sensor fusion technology is used for real-time localization, and moving average filtering is applied to eliminate oscillation errors caused by gait. This invention, by combining discrete control barrier functions (D-CBF) with hybrid zero dynamics (HZD), achieves high-precision navigation and safe obstacle avoidance for bipedal robots in complex environments, effectively solving the problems of insufficient stability and lack of safety in traditional methods.

Description

Technical Field

[0001] A safe navigation and localization method for bipedal robots based on discrete control barrier functions and hierarchical architecture is proposed, belonging to the field of robot navigation and control technology. Background Technology

[0002] With the widespread application of bipedal robot technology in service, rescue and industrial automation, safe navigation and precise positioning in dynamic environments have become key requirements.

[0003] However, traditional navigation methods have significant shortcomings when dealing with complex environments and their own motion constraints: while A*-based path planning methods can generate collision-free paths, they do not consider the gait stability and kinematic constraints of bipedal robots, making the planned paths difficult to execute. Although the Control Barrier Function (CBF) method has been used for safety control in continuous systems, its theory and application in discrete-time systems are still incomplete, and traditional CBF construction cannot guarantee the convexity of the optimization problem and the feasibility of real-time solutions. In addition, vision- or inertial measurement-based localization methods are affected by the periodic oscillations of bipedal gait, resulting in measurement noise and phase delay, leading to the accumulation of localization errors and a decrease in path tracking accuracy. Existing studies, such as those by Agrawal et al., have extended CBF to discrete systems, but their nonlinear optimization problems cannot guarantee real-time performance; the hierarchical control architecture proposed by Karydis et al. improves the navigation robustness of micro-legged robots, but does not solve the problem of dynamic balance and safety constraint integration in high-degree-of-freedom bipedal systems; the localization filtering method designed by Kang et al. suppresses gait oscillation errors, but it does not coordinate with the safety control layer, resulting in delayed obstacle avoidance response.

[0004] Therefore, this invention proposes a safe navigation and localization method for bipedal robots based on discrete control barrier functions and a hierarchical architecture. By constructing a discrete control barrier function (D-CBF) and a gait event-driven quadratic constraint quadratic programming (QCQP) model, the method achieves real-time solution of safety control inputs. Combining hybrid zero dynamics (HZD) gait generation and moving average filtering localization, a closed-loop architecture of path planning, gait control, safety constraints, and state estimation is formed, effectively solving the combined problems of dynamic obstacle avoidance, gait oscillation suppression, and insufficient trajectory tracking accuracy. This provides a systematic solution for the reliable operation of bipedal robots in unstructured environments. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention proposes a safe navigation and positioning method for bipedal robots based on discrete control barrier functions and a hierarchical architecture.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] S1. Define the free space by eliminating the obstacle area using Equation 1. Use the RRT algorithm shown in Equation 2 to search for a collision-free path connecting the starting point and the target point in the free space. Calculate the desired heading angle based on the planned path function using Equation 3.

[0008] Formula 1:

[0009]

[0010] Formula 2:

[0011]

[0012] Formula 3:

[0013]

[0014] in, For free space, For the robot's pose, For pose space, robot pose and obstacles The distance between them For the first An obstacle, To maintain a safe distance; To plan the route, Starting from the pose, For the target point pose, To quickly explore random tree algorithms; For the desired heading angle, For path functions, describe the path in Axis position place coordinate, For robots Axis position, For path functions in The derivative at point;

[0015] S2. Based on the linear inverted pendulum model, Equation 4 is used to describe the robot's center of mass dynamics, and Equation 5 is used to define the output error of the virtual constraints:

[0016] Formula 4:

[0017]

[0018] Formula 5:

[0019]

[0020] in, Let be the rate of change of the center of mass state. In the state of center of mass, For the system matrix, For the input matrix, Zero torque point control input; For output error, The actual output depends on the robot's pose. , The desired output depends on the gait phase. and Bessel coefficient , For gait phase, These are the Bessel coefficients;

[0021] S3. Construct a discrete control barrier function based on elliptical obstacles using Equation 6, and ensure the non-negativity of the barrier function value using the difference inequality constraint in Equation 7:

[0022] Formula 6:

[0023]

[0024] Formula 7:

[0025]

[0026] in, For the first The barrier function of the step, For the first Step robot Axis position, For the first Step robot Axis position, Center of the obstacle coordinate, Center of the obstacle coordinate, Let the semi-major axis of the elliptical obstacle be... Let be the semi-minor axis of the elliptical obstacle; The change in the barrier function. The attenuation coefficient satisfies 0 < ≤10< ≤1;

[0027] S4. The moving average filter of Equation 8 is used to smooth the original sensor measurement data, and the Kalman filter algorithm of Equation 9 is used to make the optimal estimate of the robot state:

[0028] Formula 8:

[0029]

[0030] Formula 9:

[0031]

[0032] in, For the first The moving average filter output of the step, For the size of the filter window, For the first The sensor measurements of the step; For the first Step-by-step state estimation, The state transition function depends on the previous state estimate. and control input , For the first Step-by-step state estimation For the first Step control input, For the first Kalman gain of the step, For the first Step sensor measurement value The observation function depends on the previous state estimate. ;

[0033] S5. Transform the path tracking and obstacle avoidance problem into a constrained optimization problem;

[0034] Furthermore, step S5 includes the following specific steps:

[0035] S51. Construct a safety set based on the obstacle positions and the robot's geometric model;

[0036] S52. Based on the exponential form of the control barrier function, construct discrete-time constraints;

[0037] S53. Design an optimization objective function that minimizes the variation of control input and path tracking error;

[0038] S54. Introduce a slack variable to handle constraint conflicts in the constraints controlling the Lyapunov function.

[0039] S55. Solve the quadratic constrained quadratic programming problem using numerical optimization tools;

[0040] S56. Check whether the control input obtained from the solution meets the physical constraints and safety requirements of the actuator;

[0041] S6. Calculate the adjustment amount of gait parameters using the Poincaré mapping step model in Equation 10, and predict the robot state for the next step using Equation 11:

[0042] Formula 10:

[0043]

[0044] Formula 11:

[0045]

[0046] in, For the first Gait parameter adjustment amount for each step For feedback gain, For the first The robot's state in steps, The desired state depends on the change in the desired heading angle. , For the desired change in heading angle; For the first The robot's state in steps, For Poincaré's mapping, For the first Gait parameters of the step;

[0047] S7. Construct a time-varying control barrier function using Equation 12:

[0048] Formula 12:

[0049]

[0050] in, For the first The dynamic barrier function of the step, For the first Step robot Axis position, For the first Step robot Axis position, Center of time-varying obstacles coordinate, Center of time-varying obstacles coordinate, Let the semi-major axis of the time-varying elliptical obstacle be... Let be the semi-minor axis of the time-varying elliptical obstacle;

[0051] S8. By combining the input-output linearization control and the output of the event controller through Equation 13, a total control command is formed, which is then verified using the complete system dynamics model in Equation 14.

[0052] Formula 13:

[0053]

[0054] Formula 14:

[0055]

[0056] in, This is the main control input. The output of the input-output linearization controller depends on the output error. , The output of the event controller depends on the gait parameter adjustment. ; Let be the rate of change of the system state. For the system dynamics function, For the input gain function, This refers to the system status.

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

[0058] 1. This invention, through the construction of static and dynamic barrier functions, can accurately transform the geometric model of obstacles into safety constraints, effectively handling dynamic obstacles whose position and shape change at any time. The real-time optimization solver ensures that the final generated control commands meet both dynamic and safety constraints. This fundamentally avoids collisions between the robot and obstacles, offering higher safety and more predictable behavior compared to traditional reactive obstacle avoidance methods, thus significantly improving navigation reliability in complex scenarios filled with both static and dynamic obstacles.

[0059] 2. This invention employs a hierarchical collaborative control architecture, effectively solving the coupling problem between global path tracking and dynamic balance in bipedal robots. High-level motion guidance is generated through global path planning, and a stable low-level gait pattern is generated based on a linear inverted pendulum model and virtual constraints. Innovatively, a gait mapping model is used to transform high-level path commands into real-time adjustments to low-level gait parameters, achieving smooth synchronization between gait phase and motion direction. This enables the robot not only to accurately track the path but also to actively adjust its gait to maintain dynamic balance during turning and obstacle avoidance, overcoming the instability problems that may result from separating path tracking and balance control in traditional methods.

[0060] 3. This invention enhances the system's robustness against sensor noise and execution uncertainty by integrating state estimation, filtering, and optimization decision-making techniques. The employed moving average filtering and Kalman filtering effectively suppress measurement noise caused by periodic oscillations during bipedal walking, providing more accurate state estimates and laying a reliable data foundation for safe control. Furthermore, the entire navigation problem is formalized as a constrained optimization problem, whose objective function simultaneously minimizes tracking error and control energy consumption, and is subject to stability and safety constraints. It can calculate the optimal and absolutely safe control commands in real time, demonstrating strong anti-interference and online decision-making capabilities, ensuring stable and efficient robot operation even in the presence of noise, model uncertainty, and environmental changes. Attached Figure Description

[0061] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used together with the embodiments of the invention to explain the invention and do not constitute a limitation thereof.

[0062] Figure 1 This is a flowchart illustrating the overall technical process of a bipedal robot safety navigation and positioning method based on discrete control barrier functions and a hierarchical architecture proposed in this invention.

[0063] Figure 2 A flowchart illustrating the transformation of path tracking and obstacle avoidance problems into optimization methods. Detailed Implementation

[0064] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are for illustrative purposes only and are not intended to limit the invention.

[0065] Example 1, referring to Figure 1 , 2 The overall flowchart of this invention is shown below. Figure 1 The present invention transforms the path tracking and obstacle avoidance problem into a constrained optimization problem. The steps are as follows: Figure 2 :

[0066] First, the obstacle area is eliminated using Equation 1 to define the free space. Then, the RRT algorithm shown in Equation 2 is used to search for a collision-free path connecting the starting point and the target point in the free space. Finally, the desired heading angle is calculated based on the planned path function using Equation 3.

[0067] Formula 1:

[0068]

[0069] Formula 2:

[0070]

[0071] Formula 3:

[0072]

[0073] in, For free space, For the robot's pose, For pose space, robot pose and obstacles The distance between them For the first An obstacle, To maintain a safe distance; To plan the route, Starting from the pose, For the target point pose, To quickly explore random tree algorithms; For the desired heading angle, For path functions, describe the path in Axis position place coordinate, For robots Axis position, For path functions in The derivative at point;

[0074] Based on the linear inverted pendulum model, Equation 4 is used to describe the dynamics of the robot's center of mass, and Equation 5 is used to define the output error of the virtual constraints.

[0075] Formula 4:

[0076]

[0077] Formula 5:

[0078]

[0079] in, Let be the rate of change of the center of mass state. In the state of center of mass, For the system matrix, For the input matrix, Zero torque point control input; For output error, The actual output depends on the robot's pose. , The desired output depends on the gait phase. and Bessel coefficient , For gait phase, These are the Bessel coefficients;

[0080] A discrete control barrier function based on elliptical obstacles is constructed using Equation 6, and the non-negativity of the barrier function value is ensured by the difference inequality constraint in Equation 7.

[0081] Formula 6:

[0082]

[0083] Formula 7:

[0084]

[0085] in, For the first The barrier function of the step, For the first Step robot Axis position, For the first Step robot Axis position, Center of the obstacle coordinate, Center of the obstacle coordinate, Let the semi-major axis of the elliptical obstacle be... Let be the semi-minor axis of the elliptical obstacle; The change in the barrier function. The attenuation coefficient satisfies 0 < ≤10< ≤1;

[0086] The moving average filter of Equation 8 is used to smooth the raw sensor measurement data, and the Kalman filter algorithm of Equation 9 is used to make the optimal estimate of the robot state.

[0087] Formula 8:

[0088]

[0089] Formula 9:

[0090]

[0091] in, For the first The moving average filter output of the step, For the size of the filter window, For the first The sensor measurements of the step; For the first Step-by-step state estimation The state transition function depends on the previous state estimate. and control input , For the first Step-by-step state estimation For the first Step control input, For the first Kalman gain of the step, For the first Step sensor measurement value The observation function depends on the previous state estimate. ;

[0092] The path tracking and obstacle avoidance problem is transformed into a constrained optimization problem;

[0093] Furthermore, the transformation of the path tracking and obstacle avoidance problem into a constrained optimization problem includes the following specific steps:

[0094] 1) Construct a safety set based on the obstacle positions and the robot's geometric model;

[0095] 2) Based on the exponential form of the control barrier function, construct discrete-time constraints;

[0096] 3) Design an optimization objective function that minimizes the variation in control input and the path tracking error;

[0097] 4) Introduce a slack variable to handle constraint conflicts in the constraints controlling the Lyapunov function;

[0098] 5) Solve the quadratic constrained quadratic programming problem using numerical optimization tools;

[0099] 6) Check whether the control input obtained from the solution meets the physical constraints and safety requirements of the actuator;

[0100] The adjustment amount of gait parameters is calculated using the stepping model of the Poincaré map in Equation 10, and the robot state for the next step is predicted using Equation 11:

[0101] Formula 10:

[0102]

[0103] Formula 11:

[0104]

[0105] in, For the first Gait parameter adjustment amount for each step For feedback gain, For the first The robot's state in steps, The desired state depends on the change in the desired heading angle. , For the desired change in heading angle; For the first The robot's state in steps, For Poincaré's mapping, For the first Gait parameters of the step;

[0106] Construct a time-varying control barrier function using Equation 12:

[0107] Formula 12:

[0108]

[0109] in, For the first The dynamic barrier function of the step, For the first Step robot Axis position, For the first Step robot Axis position, Center of time-varying obstacles coordinate, Center of time-varying obstacles coordinate, Let the semi-major axis of the time-varying elliptical obstacle be... Let be the semi-minor axis of the time-varying elliptical obstacle;

[0110] Equation 13 combines the input-output linearization control and the output of the event controller to form a total control command, which is then verified using the complete system dynamics model in Equation 14.

[0111] Formula 13:

[0112]

[0113] Formula 14:

[0114]

[0115] in, This is the main control input. The output of the input-output linearization controller depends on the output error. , The output of the event controller depends on the gait parameter adjustment. ; Let be the rate of change of the system state. For the system dynamics function, For the input gain function, This refers to the system status.

Claims

1. A method for safe navigation and localization of a bipedal robot based on discrete control barrier functions and a hierarchical architecture, characterized in that, Includes the following steps: S1. Define the free space by eliminating the obstacle area using Equation 1. Use the RRT algorithm shown in Equation 2 to search for a collision-free path connecting the starting point and the target point in the free space. Calculate the desired heading angle based on the planned path function using Equation 3. Formula 1: Formula 2: Formula 3: in, For free space, For the robot's pose, For pose space, robot pose and obstacles The distance between them For the first An obstacle, To maintain a safe distance; To plan the route, Starting from the pose, For the target point pose, To quickly explore random tree algorithms; For the desired heading angle, For path functions, describe the path in Axis position place coordinate, For robots Axis position, For path functions in The derivative at point; S2. Based on the linear inverted pendulum model, Equation 4 is used to describe the robot's center of mass dynamics, and Equation 5 is used to define the output error of the virtual constraints: Formula 4: Formula 5: in, Let be the rate of change of the center of mass state. In the state of center of mass, For the system matrix, For the input matrix, Zero torque point control input; For output error, The actual output depends on the robot's pose. , The desired output depends on the gait phase. and Bessel coefficient , For gait phase, These are the Bessel coefficients; S3. Construct a discrete control barrier function based on elliptical obstacles using Equation 6, and ensure the non-negativity of the barrier function value using the difference inequality constraint in Equation 7: Formula 6: Formula 7: in, For the first The barrier function of the step, For the first Step robot Axis position, For the first Step robot Axis position, Center of the obstacle coordinate, Center of the obstacle coordinate, Let the semi-major axis of the elliptical obstacle be... Let be the semi-minor axis of the elliptical obstacle; The change in the barrier function. The attenuation coefficient satisfies 0 < ≤10< ≤1; S4. The moving average filter of Equation 8 is used to smooth the original sensor measurement data, and the Kalman filter algorithm of Equation 9 is used to make the optimal estimate of the robot state: Formula 8: Formula 9: in, For the first The moving average filter output of the step, For the size of the filter window, For the first The sensor measurements of the step; For the first Step-by-step state estimation, The state transition function depends on the previous state estimate. and control input , For the first Step-by-step state estimation, For the first Step control input, For the first Kalman gain of the step, For the first Step sensor measurement value The observation function depends on the previous state estimate. ; S5. Transform the path tracking and obstacle avoidance problem into a constrained optimization problem; S6. Calculate the adjustment amount of gait parameters using the Poincaré mapping step model in Equation 10, and predict the robot state for the next step using Equation 11: Formula 10: Formula 11: in, For the first Gait parameter adjustment amount for each step For feedback gain, For the first The robot's state in steps, The desired state depends on the change in the desired heading angle. , For the desired change in heading angle; For the first The robot's state in steps, For Poincaré's mapping, For the first Gait parameters of the step; S7. Construct a time-varying control barrier function using Equation 12: Formula 12: in, For the first The dynamic barrier function of the step, For the first Step robot Axis position, For the first Step robot Axis position, Center of time-varying obstacles coordinate, Center of time-varying obstacles coordinate, Let the semi-major axis of the time-varying elliptical obstacle be... Let the semi-minor axis of the time-varying elliptical obstacle be denoted by ; S8. By combining the input-output linearization control and the output of the event controller through Equation 13, a total control command is formed, which is then verified using the complete system dynamics model in Equation 14. Formula 13: Formula 14: in, This is the main control input. The output of the input-output linearization controller depends on the output error. , The output of the event controller depends on the gait parameter adjustment. ; Let be the rate of change of the system state. For the system dynamics function, For the input gain function, This refers to the system status.

2. The method for safe navigation and positioning of a bipedal robot based on discrete control barrier functions and a hierarchical architecture as described in claim 1, characterized in that, The S5 step includes the following specific steps: 1) Construct a safety set based on the obstacle positions and the robot's geometric model; 2) Based on the exponential form of the control barrier function, construct discrete-time constraints; 3) Design an optimization objective function that minimizes the variation in control input and the path tracking error; 4) Introduce a slack variable to handle constraint conflicts in the constraints controlling the Lyapunov function; 5) Solve the quadratic constrained quadratic programming problem using numerical optimization tools; 6) Check whether the control input obtained from the solution meets the physical constraints and safety requirements of the actuator.

Images