Quadruped robot landing planning method based on scene decoupling and risk avoidance

Abstract

The invention discloses a quadruped robot landing planning method based on scene decoupling and risk avoidance. The quadruped robot landing planning method aims at solving the problems that in the prior art, perception is not precise in a complex environment, and the obstacle avoidance capacity is insufficient. The method comprises the following steps: acquiring and fusing multi-source sensor data, and segmenting an original point cloud; generating a scene decoupling elevation map of a multi-layer structure by using the segmentation point cloud; constructing a multi-objective optimization problem taking risk avoidance as a core based on the elevation map, wherein the multi-objective optimization problem comprises a comprehensive cost function and a strict obstacle avoidance constraint; utilizing a reaction formula adjusting module of a capturable region theory to cope with a dynamic instability risk; and solving the multi-target and multi-constraint optimization problem in real time by adopting a hierarchical solving strategy, and finally generating an optimal foot end drop point considering safety and stability. According to the method, the terrain adaptability, the motion stability and the decision intelligence of the quadruped robot in an unstructured environment are remarkably improved through fine decoupling of a scene and quantitative avoidance of multi-source risks.

Description

Technical Field

[0001] This invention belongs to the field of robot motion planning and environmental perception technology, and in particular relates to a method for planning the landing of quadruped robots based on scene decoupling and risk avoidance. Background Technology

[0002] Quadruped robots, theoretically capable of overcoming rugged terrain, exhibit immense application potential in scenarios such as field exploration and disaster search and rescue compared to traditional wheeled or tracked robots. The core technological challenge in achieving their high mobility and adaptability lies in how the robot can intelligently perceive and understand its complex three-dimensional environment in real time, and based on this, plan a safe, stable, and efficient sequence of foot placement points. Current technologies for environmental perception and foot placement planning in quadruped robots have the following limitations:

[0003] Existing methods typically process 3D geometric point clouds directly, such as identifying the ground and obstacles through plane fitting or geometric clustering. These methods offer a superficial understanding of the environment, struggle to distinguish geometrically similar but distinct objects, and are sensitive to sensor noise. Current technologies lack an effective means to deeply fuse powerful 2D image recognition capabilities with 3D spatial information, thereby assigning accurate instance labels to 3D point clouds. Furthermore, traditional mapping methods often involve instantaneous geometric reconstruction, making it difficult to handle moving objects and sensor noise in dynamic environments. They also typically do not provide uncertainty metrics for terrain estimation, making it impossible for planners to assess the reliability of perception results. Moreover, current mainstream optimization-based planning methods (such as Model Predictive Control, MPC), while considering dynamics, are insufficient in risk avoidance. Obstacle avoidance is often achieved by setting hard constraints. This approach makes the robot behave rigidly when approaching obstacle boundaries, lacking a soft, quantifiable risk avoidance mechanism—that is, actively seeking to maintain a safe distance from obstacles within the optimization objective, thereby planning a smoother and safer path. Summary of the Invention

[0004] The technical problem this invention aims to solve is to address the shortcomings of existing technologies by providing a quadruped robot foot placement planning method based on scene decoupling and risk avoidance. It includes the following steps:

[0005] Step S100: Acquire and fuse multi-source sensor data such as depth camera, RGB camera, inertial measurement unit and joint encoder, wherein: the depth camera provides local high-resolution raw depth information and converts it into raw 3D environment point cloud information, the RGB camera provides color image information, the inertial measurement unit provides robot posture and motion state information, and the joint encoder provides information such as robot joint angle and angular velocity.

[0006] Step S200: Acquire the original depth information of the scene using a depth camera, and convert it into an original 3D environment point cloud as the basis for the main environment model using a 3D reconstruction algorithm; synchronously acquire color image information corresponding to the original depth information using an RGB camera, and use the pre-obtained camera intrinsic and extrinsic parameter calibration results to ensure that the color image information and the original 3D environment point cloud are aligned in the spatial coordinate system; use an inertial measurement unit to measure and output the angular velocity and linear acceleration of the robot body in real time at a high frequency sampling rate, and obtain its attitude and motion state in the global coordinate system through an attitude calculation algorithm; perform spatiotemporal synchronization and data fusion on the original 3D environment point cloud, the aligned color image information, and the body pose information to generate a multimodal data with rich geometric and texture information in a unified coordinate system for subsequent point cloud segmentation and scene understanding, while the joint encoder provides information such as the angle and angular velocity of the robot joints.

[0007] Step S300: Obtain a frame of synchronized color image information and original depth information, and obtain the depth scale factor depth_scale corresponding to the original depth information for unit conversion; input the color image information into a preset target detection and segmentation network, the network processes the image to identify one or more target instances in the image, and outputs its category label, detection confidence, bounding box coordinates defining its two-dimensional position, and generates a pixel-level segmentation mask for each identified target instance, the mask is used to accurately distinguish the foreground pixels and background pixels of the target instance;

[0008] Using the generated segmentation mask as a spatial index, extract all valid depth values ​​corresponding to the foreground pixels of the target instance and with values ​​greater than zero from the original depth information; perform a preset average aggregation operation on the extracted set of valid depth values ​​to obtain a robust aggregated depth value; multiply the aggregated depth value by the obtained depth scaling factor and perform unit conversion to calculate the first depth data that can characterize the three-dimensional spatial position of the target instance;

[0009] The target instance's category label, detection confidence score, bounding box coordinates, segmentation mask, and calculated first depth data are encapsulated into a structured instance target data object, and this data object is output based on the globally consistent sensor trajectory pose (T). wb ) and the external parameters (T) between the camera and the inertial measurement unit bc ), through coordinate system transformation P world =T wb ·T bc ·P camera The original 3D environment point cloud in the camera coordinate system is remapped to the world coordinate system; then, an instance segmentation mask is applied. By projecting from 3D to 2D, instance labels are assigned to the remapped point cloud, generating one or more segmented point clouds. The point cloud segmentation distinguishes between background point clouds and obstacle point clouds, and its generation process can be defined by the following model:

[0010]

[0011] Where π(·) is the projection function, τ seg T is the preset segmentation threshold. cb For T bc The inverse transform, i.e., T cb =(T bc ) -1 .

[0012] Step S400: First, an extended Kalman filter is applied to perform temporal state estimation of the elevation information of each cell in the gridded map corresponding to the segmented point cloud in step S200; wherein, the state vector of the extended Kalman filter mainly contains the elevation value (h) of the grid cell, and the uncertainty of the elevation value, i.e., the variance, is recursively estimated and maintained by updating its state covariance matrix. This generates an initial elevation map consisting of an elevation layer and an uncertainty layer;

[0013] The obstacle point cloud segmented in step S200 is projected onto the corresponding grid cells of the initial elevation map according to its three-dimensional coordinates, and these cells are specifically marked to form an obstacle differentiation layer; based on the existing elevation layer, uncertainty layer, obstacle differentiation layer and other layer information, an additional cost layer is calculated and generated; finally, the elevation layer, uncertainty layer, obstacle differentiation layer and cost layer are integrated to output a dense, smooth and clearly decoupled final multi-layer scene decoupled elevation map of the obstacle region.

[0014] Step S500: The objective function L of the optimization problem total It is a weighted sum consisting of at least four cost terms, used to seek the optimal balance among multiple conflicting objectives:

[0015] L total =L task +L kin +L env +L clearance

[0016] Including the cost of mission space trajectory tracking L task Kinematic Feasibility Cost L kin Environmental interaction cost L env and safety clearance cost L clearance The special safety gap cost Lclearance It is a distance-based penalty function used to penalize the foot landing point relative to the obstacle geometric envelope O extracted from the obstacle differentiation layer of the elevation map. i The distance between them is less than a preset minimum safety threshold d. safe To actively maintain a safe buffer zone between the robot's limbs and environmental obstacles, the formula is:

[0017]

[0018] Where d(r) foot,leg O i ) represents the distance from the foot to the boundary of the i-th low obstacle, based on the optimization variable r foot,leg Obstacle geometry information O obtained from obstacle differentiation layers i Real-time calculation; d safe This represents the preset minimum safe distance, which is a set safety parameter; w clear Represents the weight coefficient, which is an adjustment parameter set by the user. penalty(·) is a penalty function, and ∑ is the summation symbol.

[0019] The constraints of the optimization problem include: robot full dynamics model constraints, strict obstacle avoidance constraints, feasible region constraints, and physical limit constraints. Among them, the strict obstacle avoidance constraint requires that all planned foot placement points must be within the geometric envelope O of all identified low obstacles. i In addition;

[0020]

[0021] in This represents the position vector of the foot.

[0022] Step S600: First, obtain the nominal landing point and the desired trajectory of the robot's base given by the upper-level planner, and simultaneously collect real-time dynamic information from sensors. At the same time, it synchronously obtains the robot's current actual motion state in the current millisecond from sensors such as the inertial measurement unit. Before directly using the nominal landing point for control calculation, the reactive adjustment module obtains the current horizontal position r of the robot's center of mass (CoM) in real time through the state estimator. CoM，xy Horizontal surface velocity v CoM，xy and the height of the center of mass relative to the ground z CoM , where the angular frequency is Let g be the acceleration due to gravity. Based on the linear inverted pendulum dynamics model, the theoretically captureable point r that allows the robot to recover from its current motion state to static equilibrium within the next step is calculated using the following formula. CP Location:

[0023]

[0024] Calculate the theoretically captureable point r CP The nominal landing point r planned by the main optimization problem foot,nominal The deviation vector between; multiply this deviation vector by a preset gain coefficient k used to adjust the reaction intensity. cp To generate a reactive adjustment Δr pointing in the direction of stable recovery. reactive Finally, the reaction adjustment is applied to the nominal landing point, and a final adjusted landing point r after dynamic stability correction is output according to the following formula. foot,adjusted :

[0025] r foot,adjusted =r foot，nominal +k cp ·(r CP -r foot,nominal )

[0026] In this way, the final landing point not only meets the requirements of the main optimization problem, but also includes active compensation for potential dynamic instability, thereby significantly enhancing the robot's ability to maintain balance under unpredictable internal and external disturbances. Finally, the whole-body controller takes this adjusted landing point as the final tracking target and calculates the precise joint torque required to achieve the target by solving an instantaneous dynamic optimization problem.

[0027] Compared with the prior art, the advantages of the present invention are as follows:

[0028] 1) This invention creatively proposes a perception method for 3D point clouds empowered by 2D image instances. By utilizing advanced object detection and instance segmentation networks to process RGB images and accurately mapping pixel-level segmentation results to 3D point clouds, the robot not only knows the geometric shape of objects in the environment but also understands the instance meaning. This deep instance understanding capability is a fundamental prerequisite for subsequent intelligent decision-making and risk avoidance, far superior to traditional methods that rely solely on geometric analysis.

[0029] 2) High-reliability probabilistic terrain map construction: This invention employs an extended Kalman filter (EPF) to perform temporal state estimation of ground elevation, enabling real-time output of the uncertainty (variance) of the elevation estimation, forming a multi-layered (elevation, uncertainty, obstacle) scene decoupled elevation map. This allows the subsequent planner to adopt a more conservative strategy in areas where the perception results are uncertain, greatly enhancing the robot's motion robustness.

[0030] 3) Intelligent proactive risk avoidance decision-making: This invention innovatively introduces a safety gap cost term into multi-objective optimization problems. This cost term treats maintaining distance from obstacles as a soft optimization objective, rather than a simple hard constraint. This makes the robot's behavior more intelligent and human-like: it no longer walks along the edge of obstacles, but proactively and smoothly plans a path that maintains a safety buffer zone, significantly improving the safety and smoothness of movement in complex and narrow environments. Attached Figure Description

[0031] Figure 1 This is a flowchart of a quadruped robot foot placement planning method based on scene decoupling and risk avoidance according to the present invention;

[0032] Figure 2 Decouple elevation maps for multi-layered scenes;

[0033] Figure 3 A schematic diagram showing the planned landing points for a quadruped robot; Detailed Implementation

[0034] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and preferred embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without inventive effort are within the scope of protection of this invention.

[0035] This invention provides a method for planning the landing of a quadruped robot based on scene decoupling and risk avoidance, as shown in the flowchart below. Figure 1 As shown. The core of this invention lies in a base-scene decoupled elevation map to enable the quadruped robot to avoid obstacles. This method is implemented on the Unitree Robotics Go2 quadruped robot platform, which is equipped with an NVIDIA Jetson AGX Orin computing unit and a multimodal sensor suite. The detailed steps are as follows:

[0036] The multimodal perception and data fusion process performed in step S100 aims to provide a comprehensive, accurate, and spatiotemporally aligned data foundation for subsequent scene decoupling and planning steps. This process can be specifically broken down into the following parallel sub-steps of data acquisition and final fusion:

[0037] 3D geometric information acquisition: A D435i depth camera is used as the primary distance sensing sensor. This depth camera is responsible for capturing the raw depth information of the scene in real time, and through its built-in 3D reconstruction engine or the 3D reconstruction algorithm of an external computing node, the acquired frame-by-frame raw depth information is converted into a raw 3D environment point cloud containing a large number of 3D spatial coordinate points. This point cloud constitutes the main geometric data for subsequent environment modeling.

[0038] 2D Visual Information Acquisition and Registration: The D435i camera, working in conjunction with the depth camera, also includes an RGB camera responsible for synchronously acquiring color image information that is strictly time-stamped with the original depth information. To achieve the fusion of visual and geometric information, the system utilizes pre-obtained camera intrinsic and extrinsic parameter calibration results. These calibration results precisely describe the relative position and orientation between the RGB camera and the depth camera, ensuring that the acquired color image information is accurately aligned with the generated original 3D environment point cloud in the spatial coordinate system, allowing each 3D point to find its corresponding color information.

[0039] Robot External State Estimation: An inertial measurement unit (IMU) is used to sense the robot's motion state. This IMU measures and outputs the robot's angular velocity and linear acceleration in real time at a sampling frequency of 1000 Hz. This raw data is fed into a state estimation algorithm based on an extended Kalman filter (EPF). This module fuses the IMU data with other information (kinematic odometry) to accurately estimate the robot's real-time attitude and motion state (i.e., its six-DOF pose) in a preset global world coordinate system.

[0040] Robot internal state acquisition: Joint encoders are installed at each joint of the robot. These encoders are responsible for monitoring and providing precise angle and angular velocity information of all joints of the robot in real time. This internal state information is the key basis for subsequent dynamic calculations.

[0041] Based on the aforementioned data acquisition, the system performs the final spatiotemporal synchronization and data fusion operation. It rigorously aligns 3D geometric information (original 3D environment point cloud), 2D visual information (aligned color image information), external state information (global pose), and internal state information (joint states) according to their respective timestamps, and transforms all data to the same world coordinate system through the global pose. This step ultimately outputs a high-precision multimodal dataset containing rich geometric, texture, and robot internal and external state information, providing comprehensive, consistent, and reliable data input for subsequent steps such as S200 point cloud segmentation and scene understanding.

[0042] Step S200 aims to perform a 3D point cloud instance empowerment process based on 2D image instance segmentation. The core idea of ​​this step is to utilize an advanced, maturely trained YOLOv8-SEG deep learning network on 2D images to assign precise category labels to undifferentiated 3D geometric point clouds, thereby achieving intelligent recognition and separation of entities such as obstacles. This process can be broken down into the following sub-steps:

[0043] 2D Target Instance Recognition and Segmentation: First, a frame of spatiotemporally synchronized color image information and raw depth information are obtained from the preceding steps. The system inputs the color image information into a pre-set, trained YOLOv8-SEG target detection and segmentation network. The network performs depth analysis on the image to identify one or more target instances in the image, and for each successfully identified target instance, outputs a set of structured information, including: category label, detection confidence, bounding box coordinates, and pixel-level segmentation mask.

[0044] Robust 3D Depth Estimation of Target Instances: To determine the distance of each identified instance in 3D space, the system performs the following operations: Using the segmentation mask generated by identification and segmentation as a spatial index, it extracts a set of all pixels with valid (i.e., non-zero) depth values ​​corresponding to the foreground pixels of target instance i from the original depth information. To enhance the robustness of distance estimation and avoid the influence of noise from individual pixels, the system performs an average aggregation operation on the extracted set of valid depth values ​​to obtain an aggregated depth value that stably represents the overall distance of the instance. This aggregated depth value is multiplied by a pre-acquired depth scale factor (depth_scale) used for unit conversion to calculate the first depth data characterizing the 3D spatial position of the target instance.

[0045] Instance labeling and final segmentation of 3D point clouds: This step is crucial for enabling 2D instances to be visualized in 3D space. The system utilizes the globally consistent sensor trajectory pose T obtained in the preceding steps. wb and the external parameter T between the camera and the inertial measurement unit bc By transforming the coordinate system, the original 3D environment point cloud P in the camera coordinate system is transformed. camera Remapping to world coordinate system Pw orld For every three-dimensional point P in the world coordinate system world The system uses the camera projection function π(·) to project it back onto the two-dimensional image plane. Then, by examining the projection point in the instance segmentation mask... Is the value on the threshold greater than a preset segmentation threshold τ? seg The above criteria are used to determine whether a 3D point p belongs to the target instance i. All points p that satisfy the above conditions are classified into the corresponding segmentation point cloud set. In the process of segmenting point clouds to distinguish between background point clouds and obstacle point clouds, the generation process can be defined by the following model:

[0046]

[0047] Where π(·) is the projection function from three dimensions to two dimensions, τ seg To distinguish between obstacles and the background, a preset segmentation threshold, T cb For T bc The inverse transform, i.e., T cb =(T bc ) -1 By executing the above sub-steps, the present invention can ultimately output one or more segmented point clouds with precise instance category labels. These segmented point clouds clearly distinguish between passable ground and various obstacles in the environment, providing high-quality, instance-rich data input for constructing the scene decoupling elevation map in the subsequent step S300.

[0048] Step S300 aims to construct a multi-layer scene decoupled elevation map. The purpose of this step is to transform the segmented point cloud with instance labels obtained in the preceding step S200 into a structured multi-layer raster map containing rich environmental information, providing a comprehensive and reliable decision-making basis for subsequent risk avoidance and landing planning. This process can be broken down into the following core sub-steps:

[0049] Probabilistic Ground Surface Estimation (Generation of Elevation and Uncertainty Layers): The goal of this sub-step is to generate a base topographic map that is both smooth and includes reliability metrics. First, the system uses only the segmented point cloud data from step S200 as input data. Second, an Extended Kalman Filter (EPF) is applied to perform temporal state estimation of the elevation information of each cell in a gridded map. For each grid cell, its state vector primarily contains the cell's elevation value. At each time step, the EPF performs a "prediction-update" loop: Prediction step: Assuming the terrain is static, the current elevation prediction is equal to the previous best estimate. Update step: When a new ground point cloud measurement falls into the grid cell, the EPF updates the cell's elevation state based on the difference between the measurement and the prediction, and the system's confidence level in the measurement noise. Crucially, the EPF recursively updates its corresponding state covariance matrix while updating the elevation value. The diagonal elements of this matrix represent the variance of the elevation estimate, which precisely quantifies the system's uncertainty regarding the current elevation estimate. This sub-step ultimately generates an initial elevation map with two layers: a filtered, smoothed elevation layer and a corresponding uncertainty layer.

[0050] Obstacle Layer Projection and Generation: The goal of this sub-step is to clearly identify impassable areas on the map. The system uses the obstacle point cloud segmented in step S200. Each obstacle 3D point is projected onto the corresponding grid cell of the initial elevation map according to its horizontal (XY) coordinates. All grid cells occupied by the obstacle point cloud are assigned a specific Boolean logical label, thus forming an independent obstacle differentiation layer.

[0051] Elevation Map Integration and Cost Layer Calculation: This sub-step aims to gather all information and generate the final map for planning. It integrates the elevation layer, uncertainty layer, and obstacle differentiation layer generated in previous sub-steps in terms of data structure. The system can calculate and generate an additional cost layer based on existing layer information. The cost value of each grid cell in this layer can be a weighted combination, including terrain slope (calculated based on the height difference between adjacent grid cells in the elevation layer), terrain uncertainty (directly taken from the uncertainty layer), and distance to obstacles (calculated from the obstacle differentiation layer). Areas with steeper slopes, higher uncertainty, and closer proximity to obstacles have higher cost values.

[0052] After integrating the aforementioned elevation layer, uncertainty layer, obstacle differentiation layer, and cost layer, a dense, smooth, and clearly decoupled obstacle region final multi-layer scene decoupling elevation map is output, as shown below. Figure 2 As shown, this multi-layered map provides comprehensive and quantified environmental information input for the multi-objective optimization problem in step S400.

[0053] Step S400 aims to construct a multi-objective optimization problem with risk avoidance as its core based on the multi-layered scene decoupled elevation map. The process is as follows:

[0054] The objective function of the optimization problem is L total It is a weighted sum consisting of at least four cost terms, used to seek the optimal balance among multiple conflicting objectives:

[0055] L total =L task +L kin +L env +L clearance

[0056] (1) Task space trajectory tracking cost L task : Used to penalize the actual state vector ξ of the robot base over the entire prediction time domain. k (Including its three-dimensional position, attitude, linear velocity, and angular velocity) and the reference state vector ζ given by the high-level task planner. ref，k Deviation between;

[0057]

[0058] in: This represents the robot's base state vector at prediction time step k. It includes the base's position, attitude (roll, pitch, yaw), linear velocity, and angular velocity. It is obtained through state estimation (e.g., Kalman filtering) using inertial measurement unit data and the kinematics of each leg. Represents the reference base state vector. Given by a higher-level task planner or operator. k Representing the control input vector, typically referring to the desired foot velocity or acceleration, etc. It is one of the optimization variables in solving this optimization problem. ref,k This represents the reference control input vector, which is usually set to zero to minimize the control input. is the preset reference value. W ξ W v This represents the weighted diagonal matrix. Adjustment parameters are set based on task priority.

[0059] (2) Kinematic feasibility cost L kin This cost term is a penalty function used to penalize planned foot placement that leads to kinematically infeasible or undesirable leg movements, including at least excessive leg extension (i.e., link length exceeding l). max ) and penalties for being too close to regions of kinematically unusual configurations;

[0060]

[0061] in: This represents the foot position vector. It is the core optimization variable for solving this optimization problem. This represents the hip joint position vector of the corresponding leg. It is calculated based on the current base pose and the robot's fixed geometry. max This represents the maximum extension length of the leg. This is an inherent design parameter of the robot. reach This represents the weighting coefficient, which is the initial adjustment parameter.

[0062] (3) Environmental interaction cost L env This cost term is directly associated with the cost layer of the scene decoupling elevation map, and its value is related to the planned foot landing point position r. foot，leg The corresponding passage cost C at this cost layer trav It is directly proportional to the height of the landing point, guiding the robot to choose a flatter, easier-to-access landing point within the passable area; the formula is:

[0063]

[0064] Where C trav (r foot，leg ) represents the passage cost of the foot position in the cost graph, and is determined by optimizing the variable r. foot，legThe result is obtained by projecting onto the cost layer. env These are the weighting coefficients.

[0065] (4) Safety clearance cost L clearance The cost term is a distance-based penalty function used to penalize the foot landing point relative to the obstacle geometric envelope O extracted from the obstacle differentiation layer of the elevation map. i The distance between them is less than a preset minimum safety threshold d. safe This is to actively maintain a safety buffer zone between the robot's limbs and environmental obstacles. The formula is:

[0066]

[0067] Where d(r) foot,leg O i () represents the distance from the foot to the boundary of the i-th low obstacle. Based on the optimization variable r foot，leg Obstacle geometry information O obtained from obstacle differentiation layers i Real-time calculation. d safe This represents the preset minimum safe distance, which is a set safety parameter. clear This represents the weighting coefficient, which is an adjustment parameter set by the user.

[0068] The constraints of the optimization problem include: robot full dynamics model constraints, strict obstacle avoidance constraints, feasible region constraints, and physical limit constraints.

[0069] (1) Constraints of the robot's full dynamics model:

[0070]

[0071] Where: θ, Let D(θ) be the vector of joint angle, angular velocity, and angular acceleration. G(θ) represents the inertia matrix, Coriolis / centrifugal force term, and gravity term, respectively; τ, f c Joint torque and foot contact force are the optimization variables; B T J c The selection matrix and contact point Jacobian matrix are used to drive the selection.

[0072] (2) Strict obstacle avoidance and feasible domain constraints:

[0073] Walkable area constraint: All planned foot placement points must be located within a walkable ground area R. trav Inside.

[0074] r foot ∈R trav

[0075] Strict obstacle avoidance constraints: All planned foot placement points must be within the geometric envelope of all identified low obstacles. i In addition.

[0076]

[0077] (3) Physical limit constraints: including the upper and lower limits of joint position, velocity, and torque, as well as friction cone constraints.

[0078]

[0079] f normal The normal force is the contact force f at the foot. c In this context, the component perpendicular to the ground represents the strength of the supporting foot on the ground. Represents the tangential force vector, which is the contact force f at the foot. c In this context, the component parallel to the ground is friction. Robots rely on this force to propel themselves forward, backward, or sideways. ‖·‖ represents the norm, here representing the magnitude or length of the vector. Therefore... It refers to the magnitude of the total frictional force; μ represents the coefficient of friction.

[0080] In summary, the overall multi-objective optimization function is:

[0081]

[0082] Step S500 aims to perform a higher-level planning and optimization solution based on a hierarchical strategy. This step is the core of the entire control framework's decision-making process. Its main task is to efficiently solve the multi-objective, multi-constraint nonlinear optimization problem constructed in step S400 while meeting the robot's high-frequency real-time control requirements. The process is as follows:

[0083] To address the challenge of the enormous computational complexity and difficulty in real-time execution of complete nonlinear optimization problems, this embodiment employs a sequential quadratic programming (SQP) method widely used in the field of robotics. The core idea of ​​SQP is to transform the original nonlinear problem into a mathematically simpler, standardized, and efficiently solvable quadratic programming (QP) subproblem by locally approximating it in each optimization iteration cycle.

[0084] Construction of the Quadratic Programming (QP) Subproblem: At the start of each optimization iteration in the control cycle, the system performs the following key approximation steps: (a) Quadraticization of the cost function: The multi-objective cost function, typically non-quadratic, defined in step S400, is expanded using a second-order Taylor expansion near the current reference trajectory point. This operation locally approximates the complex cost function as a standard quadratic function, the form of which can be directly processed by the QP solver. (b) Linearization of the dynamic model: Simultaneously, the simplified nonlinear dynamic model describing the robot's motion is also expanded using a first-order Taylor expansion near the current state point, resulting in a linear and easily tractable system state transition equation. Through the above quadraticization and linearization processes, the original complex nonlinear problem is successfully transformed into a computationally more user-friendly QP subproblem.

[0085] Decision variables and solution of the optimization subproblem: The decision variables of the constructed QP subproblem, that is, the set of optimal values ​​that the solver needs to find within the current optimization window, mainly include: the expected base motion trajectory in the future prediction time domain: that is, a series of robot body target positions, postures, and velocities that change over time. The foot landing point position of each swinging leg: that is, the three-dimensional coordinates of the target landing point planned for the leg that is about to be lifted. The expected ground reaction force of each supporting leg: that is, the interaction force that the planned supporting leg needs to generate with the ground to maintain body balance. After comprehensively considering the terrain constraints, obstacle avoidance requirements, and task objectives defined in step S400 (which have also been linearized), the QP solver calculates the optimal solution of the QP subproblem.

[0086] The final output of step S500, which outputs the nominal planning result, is a reference instruction generated based on the solution to the aforementioned QP subproblem, guiding the robot's subsequent movements. Specifically, it includes: a future nominal foot-landing point sequence containing precise contact timing (when to lift the foot, when to place it) and three-dimensional spatial coordinates, and a foot-swing trajectory planned to achieve this foot-landing sequence; and a time-sequential robot body desired state trajectory, defining how the body should move when executing the aforementioned foot-landing sequence. This is then passed to the lower-level whole-body controller and the reactive adjustment module of step S600, serving as the basis for their high-frequency real-time tracking and dynamic correction.

[0087] Step S600 aims to execute a reactive adjustment and low-level whole-body dynamics control based on the captureable region theory. This step runs in the lower-level high-frequency control loop (e.g., 1000Hz) of the hierarchical solution strategy. Its core task is to quickly and proactively modify the "nominal plan" given by the upper-level planner based on the robot's current real-time dynamics after receiving the plan, and finally calculate the accurate and executable joint drive torque.

[0088] Real-time state awareness and captureable point (CP) calculation: At the beginning of each high-frequency control cycle (every 1 millisecond), this step first acquires two types of key information in parallel: Planning instructions: From the upper-level model predicting the controller output in step S500, the nominal landing point r for the next step is obtained. foot,nominal And the robot's desired trajectory, linear velocity, and angular velocity at the current moment; real-time dynamic state: simultaneously, the current horizontal position r of the robot's center of mass (CoM) is estimated in real time and accurately through an onboard state estimator—which is typically an extended Kalman filter (EPF) that integrates raw data from the inertial measurement unit (IMU) and kinematic information from the leg joint encoders (i.e., leg odometry). CoM,xy Horizontal surface velocity v CoM,xy and the height of the center of mass relative to the ground z CoM After acquiring the real-time status, the module calculates the theoretically captureable point r based on the linear inverted pendulum dynamics model and the following formula. CP The capture point represents, physically, the theoretical position that the robot's swinging foot must step on in order to stop its current motion trend and restore static balance.

[0089]

[0090] Center of mass height z CoM Based on the current angles of the robot's joints, the angles are calculated in real time using forward kinematics and are typically maintained at around 0.3 meters.

[0091] Reactive foot placement correction: This sub-step performs a critical dynamic stability correction on the foot placement target of the leg currently in the swing phase before directly using the nominal foot placement point for control calculations. First, the theoretically achievable point is calculated. rCP The nominal landing point r planned by the main optimization problem foot,nominal The deviation vector between () and (). Next, this deviation vector is multiplied by a preset gain coefficient k. cp To generate the reaction adjustment amount Δr reactive The gain coefficient k cp It is a dimensionless scalar between 0 and 1, which determines the strength of the reaction adjustment: when k cp When k = 0, no reactive adjustments are made, and the upper-level planning is completely trusted; when k = 0, no reactive adjustments are made, and the upper-level planning is completely trusted; cp When the value is 1, the robot will make its best effort to place its feet on the theoretically captureable point to ensure stability. In practical applications, a value of 0.4 is usually chosen to strike a balance between path tracking accuracy and maximum disturbance rejection capability. The reactive adjustment is applied to the nominal footing point, and a final adjusted footing point r after dynamic stability correction is output according to the following formula. foot,adjusted :

[0092] r foot，adjusted =r foot，nominal +k cp ·(r CP -r foot,nominal )

[0093] Full-body dynamics control and execution: After obtaining the final adjusted foot point, the system enters the final stage of low-level control. The full-body controller itself is implemented by solving a priority (or hierarchical) quadratic programming (QP) problem, the goal of which is to execute the modified motion target as perfectly as possible while satisfying the laws of physics. The structure of this QP problem is as follows:

[0094] Highest priority (hard constraints): These include strictly satisfying the Newton-Euler dynamics equations of the complete multibody system of the robot, contact constraints of the supporting feet, and limitations on friction cones, joint torques, velocity, and position.

[0095] Second highest priority (primary optimization objective): Minimize the error between the robot's actual body acceleration and the desired body acceleration, while simultaneously minimizing the difference between the actual acceleration of the swinging foot and the acceleration before reaching the adjusted foot landing point r. foot，adjusted The error between the required target acceleration and the target acceleration.

[0096] Lowest priority (secondary optimization objective): Minimize the norm ‖τ‖ of the joint driving torque. 2 And the rate of change of ground reaction force, to achieve more energy-efficient and smoother motion. By solving this hierarchical QP problem, WBC can calculate in real time (within each 1kHz cycle) the optimal ground reaction force and the precise driving force of each joint required to accurately achieve this "corrected objective". These torque signals are finally sent to the servo drives of each joint of the robot for execution. A schematic diagram of the quadruped robot's foot placement planning results is shown below. Figure 3 As shown.

[0097] In summary, step S600, by embedding a rapid reactive adjustment based on the captureable region theory into a high-frequency whole-body dynamics control loop, achieves deep synergy between high-level planning and low-level response, thereby significantly enhancing the robot's balance maintenance ability and motion robustness when facing unpredictable internal and external disturbances.

[0098] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Any aspects of the present invention not described in detail are well-known techniques to those skilled in the art.

[0099] The preferred embodiments of the present invention have been described in detail above. It should be understood that those skilled in the art can make numerous modifications and variations based on the concept of the present invention without creative effort. Therefore, all technical solutions that can be obtained by those skilled in the art based on the concept of the present invention through logical analysis, reasoning, or limited experimentation on the basis of existing technology should be within the scope of protection defined by the claims.

Claims

1. A method for planning the landing of a quadruped robot based on scene decoupling and risk avoidance, characterized in that, Includes the following steps: Step S100: Acquire and fuse multi-source sensor data such as depth camera, RGB camera, inertial measurement unit and joint encoder, wherein: the depth camera provides local high-resolution raw depth information and converts it into raw 3D environment point cloud information, the RGB camera provides color image information, the inertial measurement unit provides robot posture and motion state information, and the joint encoder provides information such as robot joint angle and angular velocity. Step S200: The acquired color image information is segmented using an object detection and performance segmentation network. Then, based on the mapping relationship between pixels and original depth information, the original 3D environment point cloud is segmented to distinguish and identify obstacles and generate segmented point clouds. Step S300: Based on the segmented point cloud and the robot's posture and motion state information provided by the inertial measurement unit, a multi-layer scene decoupled elevation map is generated. The multi-layer structure can accurately represent terrain information of different dimensions such as terrain height and obstacles, and the scene decoupling can decompose the complex scene into independently modeled components. Step S400: Based on the multi-layer scene decoupled elevation map, construct a multi-objective optimization problem with risk avoidance as the core; the optimization problem includes: a comprehensive cost function: quantifying the costs of gait efficiency, foot stability, desired speed tracking, path deviation, etc., to balance motion stability, task objectives, and safety; strict obstacle avoidance constraints: ensuring that the planned foot landing point maintains the minimum safe distance from the identified obstacles to avoid collisions; Step S500: The multi-objective optimization problem is solved in real time using a hierarchical solution strategy, wherein the hierarchical solution strategy decomposes the complex problem into sub-problems at different levels and solves them collaboratively, thereby efficiently obtaining the final optimization result; the optimal foot placement point, i.e., the nominal foot placement point, is output, which takes into account safety, stability and task objectives. This foot placement point guides the next foot placement position of the quadruped robot. Step S600: The reactive adjustment module of the captureable region theory is used to adjust the multi-objective optimization problem in real time; the adjustment module adaptively corrects the nominal landing point based on the robot's current center of mass motion state and potential dynamic instability risk, and obtains the final adjusted landing point to enhance the robustness of the robot's motion.

2. The method for planning the landing of a quadruped robot based on scene decoupling and risk avoidance according to claim 1, characterized in that, Step S100 involves acquiring and fusing data from multiple sensors, including a depth camera, an RGB camera, and an inertial measurement unit. The system utilizes a depth camera to acquire raw depth information of the scene and converts it into a raw 3D environment point cloud, which serves as the basis for the main environment model, through a 3D reconstruction algorithm. An RGB camera synchronously acquires color image information corresponding to the raw depth information, and pre-obtained camera intrinsic and extrinsic parameter calibration results ensure that the color image information and the raw 3D environment point cloud are aligned in the spatial coordinate system. An inertial measurement unit (IMU) measures and outputs the robot's angular velocity and linear acceleration in real time at a high-frequency sampling rate, and an attitude calculation algorithm obtains its attitude and motion state in the global coordinate system. The system performs spatiotemporal synchronization and data fusion on the raw 3D environment point cloud, the aligned color image information, and the robot's pose information to generate multimodal data with rich geometric and texture information in a unified coordinate system, which is used for subsequent point cloud segmentation and scene understanding. Simultaneously, a joint encoder provides information such as the robot joint angles and angular velocities.

3. The method for planning the landing of a quadruped robot based on scene decoupling and risk avoidance according to claim 1, characterized in that, Step S200 utilizes an object detection and performance segmentation network to perform instance segmentation on the acquired color image information. Then, based on the mapping relationship between pixels and original depth information, it segments the original 3D environment point cloud to distinguish and identify obstacles. The process of generating the segmented point cloud is as follows: Acquire a frame of synchronized color image information and original depth information, and obtain the depth scale factor depth_scale corresponding to the original depth information for unit conversion; input the color image information into a preset target detection and segmentation network, the network processes the image to identify one or more target instances in the image, and outputs its category label, detection confidence, bounding box coordinates defining its two-dimensional position, and generates a pixel-level segmentation mask for each identified target instance, the mask is used to accurately distinguish the foreground pixels and background pixels of the target instance; Using the generated segmentation mask as a spatial index, extract all valid depth values ​​corresponding to the foreground pixels of the target instance and with values ​​greater than zero from the original depth information; perform a preset average aggregation operation on the extracted set of valid depth values ​​to obtain a robust aggregated depth value; The aggregated depth value is multiplied by the obtained depth scaling factor, and a unit conversion is performed to calculate the first depth data that can characterize the three-dimensional spatial position of the target instance. The target instance's category label, detection confidence score, bounding box coordinates, segmentation mask, and calculated first depth data are encapsulated into a structured instance target data object, and this data object is output based on the globally consistent sensor trajectory pose (T). wb ) and the external parameters (T) between the camera and the inertial measurement unit bc ), through coordinate system transformation P world =T wb ·T bc ·P camera The original 3D environment point cloud in the camera coordinate system is remapped to the world coordinate system; then, an instance segmentation mask is applied. By projecting from 3D to 2D, instance labels are assigned to the remapped point cloud, generating one or more segmented point clouds. The point cloud segmentation distinguishes between background point clouds and obstacle point clouds, and its generation process can be defined by the following model: Where π(·) is the projection function, τ seg T is the preset segmentation threshold. cb For T bc The inverse transform, i.e., T cb =(T bc ) -1 .

4. The method for planning the landing of a quadruped robot based on scene decoupling and risk avoidance according to claim 1, characterized in that, The process of generating a multi-layer scene decoupled elevation map in step S300, based on the segmented point cloud and the robot's posture and motion state information provided by the inertial measurement unit, is as follows: First, an extended Kalman filter is applied to perform temporal state estimation of the elevation information of each cell in the gridded map corresponding to the segmented point cloud in step S200. The state vector of the extended Kalman filter mainly contains the elevation value (h) of the grid cell, and the uncertainty of the elevation value, i.e., the variance, is recursively estimated and maintained by updating its state covariance matrix. This generates an initial elevation map consisting of an elevation layer and an uncertainty layer; The obstacle point cloud segmented in step S200 is projected onto the corresponding grid cells of the initial elevation map according to its three-dimensional coordinates, and these cells are specifically marked to form an obstacle differentiation layer. Based on the existing elevation layer, uncertainty layer, obstacle differentiation layer and other layer information, an additional cost layer is calculated and generated. Finally, the elevation layer, uncertainty layer, obstacle differentiation layer and cost layer are integrated to output a dense, smooth and clearly decoupled final multi-layer scene decoupled elevation map of the obstacle region.

5. The method for planning the landing of a quadruped robot based on scene decoupling and risk avoidance according to claim 1, characterized in that, Step S400, based on the multi-layer scene decoupled elevation map, constructs a multi-objective optimization problem with risk avoidance as its core process, as follows: The objective function of the optimization problem is L total It is a weighted sum consisting of at least four cost terms, used to seek the optimal balance among multiple conflicting objectives: L total =L task +L kin +L env +L clearance Including the cost of mission space trajectory tracking L task Kinematic Feasibility Cost L kin Environmental interaction cost L env and safety clearance cost L clearance The special safety gap cost L clearance It is a distance-based penalty function used to penalize the foot landing point relative to the obstacle geometric envelope O extracted from the obstacle differentiation layer of the elevation map. i The distance between them is less than a preset minimum safety threshold d. safe To actively maintain a safe buffer zone between the robot's limbs and environmental obstacles, the formula is: Where d(r) foot,leg O i ) represents the distance from the foot to the boundary of the i-th low obstacle, based on the optimization variable r foot,leg Obstacle geometry information O obtained from obstacle differentiation layers i Real-time calculation; d safe This represents the preset minimum safe distance, which is a set safety parameter; w clear Represents the weight coefficient, which is an adjustment parameter set by the user. penalty(·) is a penalty function, and ∑ is the summation symbol. The constraints of the optimization problem include: robot full dynamics model constraints, strict obstacle avoidance constraints, feasible region constraints, and physical limit constraints. Among them, the strict obstacle avoidance constraint requires that all planned foot placement points must be within the geometric envelope O of all identified low obstacles. i In addition; in This represents the position vector of the foot.

6. The method for planning the landing of a quadruped robot based on scene decoupling and risk avoidance according to claim 1, characterized in that, Step S500 employs a hierarchical solution strategy to solve the multi-objective, multi-constraint optimization problem in real time. This strategy is based on SQP and specifically includes: By linearizing the simplified dynamics model and quadratizing the multi-objective cost function, a sequential quadratic programming (SQP) subproblem is constructed and solved. The decision variables of this subproblem mainly include: the expected trajectory of the base body over a future period, the foot landing positions of each swinging leg, and the ground reaction force of the supporting leg. After comprehensively considering terrain constraints, obstacle avoidance requirements, and mission objectives, the final output is a future nominal landing point r containing precise contact timing and spatial coordinates. foot，nominal The sequence and foot trajectory, as well as a temporally sequenced desired state trajectory of the robot body.

7. The method for planning the landing of a quadruped robot based on scene decoupling and risk avoidance according to claim 1, characterized in that, The step S600 utilizes the reactive adjustment module of the captureable region theory to perform real-time adjustments to the multi-objective optimization problem as follows: First, the nominal landing point and the desired trajectory of the robot's base state, given by the upper-level planner, are obtained, and real-time dynamic information from sensors is collected simultaneously. At the same time, the robot's actual motion state for the current millisecond is simultaneously acquired from sensors such as the inertial measurement unit. Before directly using the nominal landing point for control calculations, the reactive adjustment module obtains the current horizontal position r of the robot's center of mass (CoM) in real time through the state estimator. CoM，xy Horizontal surface velocity v CoM，xy and the height of the center of mass relative to the ground z CoM , where the angular frequency is Let g be the acceleration due to gravity. Based on the linear inverted pendulum dynamics model, the theoretically captureable point r that allows the robot to recover from its current motion state to static equilibrium within the next step is calculated using the following formula. CP Location: Calculate the theoretically captureable point r CP The nominal landing point r planned by the main optimization problem foot，nominal The deviation vector between; multiply this deviation vector by a preset gain coefficient k used to adjust the reaction intensity. cp To generate a reactive adjustment Δr pointing in the direction of stable recovery. reactive Finally, the reaction adjustment is applied to the nominal landing point, and a final adjusted landing point r after dynamic stability correction is output according to the following formula. foot，adjusted : r foot，adjusted ＝r foot，nominal +k cp ·(r CP -r foot,nominal ) In this way, the final landing point not only meets the requirements of the main optimization problem, but also includes active compensation for potential dynamic instability, thereby significantly enhancing the robot's ability to maintain balance under unpredictable internal and external disturbances. Finally, the whole-body controller takes this adjusted landing point as the final tracking target and calculates the precise joint torque required to achieve the target by solving an instantaneous dynamic optimization problem.

Images