A low-cost sensor-based positioning and navigation method for quadruped robots

Abstract

The application discloses a kind of low-cost sensor-based quadruped robot positioning navigation method, belong to robot autonomous navigation technical field;First, construct hybrid internal mode, explicit regression linear velocity from ontology perception sequence and implicit encode external disturbance features by SimCLR contrast learning;Second, design sliding mode generalized momentum observer, only use joint torque to estimate foot end contact state, discard foot end contact sensor;Further construct leg inertia odometry LIO, fusion support leg kinematics constraint and IMU angular velocity, robust estimation body 6 degrees of freedom velocity;Propose IMU adaptive deep slice method, according to roll and pitch angle dynamic correction scan line, generate anti-jitter 2D scan;Finally, multi-sensor information fusion: when VIO fails, use LIO integral prediction pose reinitialization;New speed constraint factor is added in factor graph, suppresses mismatch drift.

Description

Technical Field

[0001] This invention relates to the field of autonomous robot navigation technology, specifically to a method for achieving robust localization, 2D grid map construction, and autonomous navigation in complex and dynamic indoor environments for quadruped robots equipped with low-cost sensors. Background Technology

[0002] The widespread availability of robust and reliable robotic platforms, along with significant advancements in reinforcement learning-based motion controllers, has enabled the deployment of quadruped robots in challenging outdoor and indoor environments. Meanwhile, the industry is showing increasing interest in quadruped robots for surveillance, inspection, and search and rescue missions, given their ability to navigate a variety of environments, including stairs, cluttered spaces, and uneven terrain.

[0003] To expand the real-world application of this technology, this invention only requires a robust navigation stack relying on low-cost sensors, such as depth cameras. However, practical industrial solutions typically depend on expensive robotic platforms equipped with high-precision sensors to achieve robust autonomous navigation and mapping. On low-cost platforms, most localization functions rely on inexpensive depth imaging cameras or 2D LiDAR, resulting in suboptimal performance. This causes several problems:

[0004] a. While RL-based motion strategies improve a robot's reaction speed and mobility, they often sacrifice trajectory smoothness. This can lead to severe body shaking, accompanied by high-frequency angle changes, resulting in jitter and noise in sensor data. b. Rapid movement can cause visual feature matching failures, leading to lost tracking in visual odometry. c. Body shaking causes 2D scan data generated from depth image projection to oscillate violently with terrain and motion. This makes it difficult for the SLAM backend to perform scan matching and loop closure detection, ultimately generating fragmented maps or causing localization drift. Traditional leg odometry calculations rely on foot contact sensors. However, low-cost robots often lack such sensors, or due to complex external terrain, physical contact sensors are easily damaged and provide inaccurate readings.

[0005] Furthermore, achieving robust motion control heavily relies on accurate robot state estimation. However, limited by hardware performance, most legged robots' sensors can only provide partially observable data, which is often accompanied by significant noise. This makes it difficult for the controller to calculate the accurate real-time system state, causing the robot to be highly susceptible to dynamic instability due to state estimation bias when dealing with unstructured and complex terrain, ultimately leading to irreversible interruption of the autonomous navigation mission.

[0006] To address the aforementioned issues, this invention designs a state observer based on the robot's rigid body dynamics model. This observer "senses" the forces acting on the foot by monitoring the residuals between joint torques and motion states in real time. This method, which eliminates physical contact sensors, fundamentally eliminates potential failure points in the hardware architecture, significantly improving the system's environmental adaptability and hardware robustness. To enable the robot to adapt to unknown external disturbances, this invention introduces the classic Internal Model Principle, treating the external environment as a "disturbance" acting on the system. Instead of directly measuring these complex external environmental parameters using vision or radar, this invention employs an indirect estimation strategy: by analyzing the dynamic response of the quadruped robot body under disturbances (i.e., changes in proprioceptive data), the external state is inverted. Summary of the Invention

[0007] The purpose of this invention is to provide a robust navigation framework that can eliminate dependence on expensive or fragile sensors, has low computational overhead sufficient to run on embedded devices, and is perfectly compatible with highly dynamic reinforcement learning (RL) policies, so as to enable quadruped robots to operate fully autonomously in complex environments.

[0008] To achieve the above objectives, the present invention proposes the following technical solution:

[0009] A localization and navigation method for a quadruped robot based on low-cost sensors includes the following steps:

[0010] Step 1: Propriometry and State Estimation. Obtain the robot's proprioception sequence, use hybrid internal model regression to estimate the robot's explicit linear velocity, and extract implicit latent features that reflect the characteristics of the external environment.

[0011] Step 2: Sensorless contact state determination. Based on joint torque and robot dynamics model, a hybrid mode generalized momentum GM perturbation observer is constructed to estimate foot contact force in real time and determine the contact state of each leg by combining threshold.

[0012] Step 3: Leg-inertial odometry (LIO) calculation. Based on the contact state obtained in Step 2, and combining kinematic constraints and IMU angular velocity constraints, a least squares optimization problem is constructed to solve the leg odometry of the robot body.

[0013] Step 4: Visual observation stabilization processing. Using the attitude angle measured by the IMU, the depth map acquired by the depth camera is dynamically sliced ​​and corrected to generate stabilized 2D laser scanning data that removes the influence of attitude jitter.

[0014] Step 5: Multi-source fusion localization and mapping, fusing leg odometry speed, stabilized 2D scan data and visual inertial odometry (VIO) into the SLAM system; when VIO is lost, dead reckoning is performed using leg odometry; the speed constraint factor of leg odometry is introduced into the factor map optimization to construct a high-precision 2D grid map.

[0015] Furthermore, the specific steps of step 1 are as follows:

[0016] 1.1) Explicit linear velocity regression: Utilizing the true linear velocity from the privileged information (PrivilegedInformation) provided during the simulation training phase. As a supervisory signal, the ontological perception historical observation sequence is input into the regression network, and the linear velocity estimate is output. Optimization is achieved by minimizing the mean square error (MSE) between the predicted and true values.

[0017] 1.2) Implicit Response Contrastive Learning: For external environmental parameters that cannot be directly measured, they are modeled as a latent space. Implicit response characteristics in Based on the principle of trajectory continuity, the ontology-aware features of adjacent time steps in the same trajectory are used as positive samples, while features of different environments or different trajectories are used as negative samples. A contrastive learning framework is adopted to optimize the feature space distribution, so that the feature distance of the same trajectory is shortened and the feature distance of different trajectories is lengthened.

[0018] Furthermore, the specific steps of step 2 are as follows:

[0019] 2.1) Constructing a generalized momentum GM observer model: Introducing a perturbation observer based on classical generalized momentum theory, using generalized momentum and foot contact force as state variables, and combining nonlinear dynamic compensation terms with observer error feedback to estimate the system contact force;

[0020] The observer dynamics equations based on GM are defined as follows: ;

[0021] in The generalized momentum measured by the system. Represents the joint space mass matrix. This is the joint velocity vector; The generalized momentum estimated for the observer; Let be the contact force vectors of the four legs to be estimated; , for , The first derivative; The Jacobian matrix represents the contact area of ​​the robot's feet. The observer gain matrix; This is a nonlinear dynamic compensation term, defined as follows: ,in It is the output torque of the joint motor. It is the Coriolis and centrifugal force matrix. It is the gravity compensation vector;

[0022] 2.2) Constructing a generalized superspiral momentum residual observer: Based on the generalized momentum model, a superspiral sliding mode term is introduced. Using the generalized momentum residual as the feedback signal, the observer's dynamic equation is constructed through model uncertainty compensation terms, hybrid nonlinear injection terms, and auxiliary state variables to estimate the full-dimensional perturbation torque. After estimation, the full-dimensional perturbation torque is inversely projected onto the foot Cartesian space through the Jacobian matrix, and a unilateral constraint is applied to obtain an effective estimate of the foot contact force.

[0023] The observer's dynamic equations are reconstructed as follows: ;

[0024] in For generalized momentum residuals; These are auxiliary state variables for the observer, used to absorb disturbances; , for , The first derivative; This is the model uncertainty compensation term; and The mixed nonlinear injection terms are defined as follows:

[0025]

[0026] in , The parameters of the generalized momentum observer are used to tune and optimize the observer's performance in order to more accurately estimate the dynamic behavior or response of the system; for The sign function is used to determine the direction of the residual; among which, the linear term To ensure exponential convergence when residuals are large, the square root term Ensure finite-time convergence as the residual approaches zero and eliminate chattering;

[0027] After the observer outputs the raw force estimate, the estimated full-dimensional disturbance torque will be... Inverse projection onto the foot-end Cartesian space using the Jacobian matrix, with one-sided constraints applied: ;

[0028] in The pseudo-inverse of the transpose of Jacobi. The ground plane normal vector, yes z-axis component;

[0029] 2.3) Adaptive dynamic threshold of the state:

[0030] A decision threshold function that dynamically adjusts with the joint state is established. The decision threshold function is based on the baseline noise floor threshold in the static state, combined with the norm of joint velocity and command acceleration, and corrected by the corresponding gain coefficient to achieve dynamic adjustment.

[0031] The foot contact force is filtered, and the filtered foot contact force signal is used to determine the contact state using Schmitt trigger logic. A hysteresis band is introduced to prevent signal jumps in critical states. When the contact force exceeds the upper threshold, it is determined to be ground contact; when it is below the lower threshold, it is determined to be swinging; and in intermediate states, the contact state of the previous moment is maintained.

[0032] The decision threshold function is dynamically adjusted according to the joint state. as follows: ;

[0033] in The base noise floor threshold under static conditions; Let the joint velocity norm be... The velocity gain number is used to compensate for model errors related to Coriolis force and viscous friction; Let the acceleration norm be the command. The inertial gain coefficient is used to compensate for inertial model errors during emergency stops and starts.

[0034] right First-order low-pass filtering is performed to obtain Then, Schmitt trigger logic is used for judgment to prevent signal transitions in critical states: ;

[0035] in, The hysteresis band width, =1 represents touching the ground, and the opposite represents swinging; This represents the previous updated ground state.

[0036] Furthermore, step 3 specifically includes the following steps:

[0037] 3.1) Using the contact state and joint motion information obtained in step 2, combined with the airborne IMU data, the high-frequency linear velocity and angular velocity of the robot body are calculated;

[0038] Based on the principles of rigid body kinematics, when the... When a leg is in an ideal static contact state, i.e., the foot does not slip, the velocity of the foot relative to the ground should be zero. Based on the velocity composition theorem, a constraint relationship is established between the velocity of the body's center velocity and the velocity of the foot, as follows: ;

[0039] Rearranging and simplifying, we obtain the velocity constraint equations for the body. Solving for the linear and angular velocities of the body: ;

[0040] in Let be the linear velocity of the machine body to be solved; Let ω be the angular velocity of the machine body to be solved. For the first The position vector of the foot tip of each leg in the body coordinate system; for The corresponding antisymmetric matrix; The linear velocity of the foot relative to the body, derived solely from joint movement. For Jacobian matrices, Joint angular velocity;

[0041] 3.2) Constructing a multi-source constrained overdetermined linear system: The angular velocity measurement value of the IMU is used as a strong constraint and fused with the kinematic constraints of all legs in the support phase to construct a joint linear equation system;

[0042] Joint linear equation system: ;

[0043] Where the state vector Integrate all legs currently in the support phase ,matrix sum vector The structure is as follows: ;

[0044] Among them, the former Each row block corresponds to the kinematic constraints of each support leg, and the last row block corresponds to the direct observation constraint of the IMU's angular velocity.

[0045] 3.3) Introducing a Physical Consistency Weight Matrix Based on the weights corresponding to each support leg and the weights corresponding to the IMU angular velocity measurements, a diagonal weight matrix is ​​constructed. The weighted least squares method is used to solve the overdetermined linear equations to obtain the final estimated value of the body velocity. ;

[0046] Among them, the first Weight of a leg It is not a fixed value, but rather the contact force estimated in step 1. Dynamic determination of kinematic residuals: ;

[0047] in For Sigmoid mapping functions; The estimated slip velocity from the previous moment is used to apply a weighted penalty to the leg that slipped. The variance of the probability of slippage occurring is used to describe the uncertainty of slippage behavior;

[0048] Final machine speed estimation The weighted least squares solution is:

[0049] .

[0050] Furthermore, step 4 consists of the following specific steps:

[0051] 4.1) Establish the geometric projection model for the virtual horizontal scan, and set the depth camera coordinate system as follows: Its optical axis is aligned with the front of the robot body; using the attitude quaternions acquired in real time by the onboard IMU, the roll angle of the robot body relative to the horizontal plane of gravity is calculated. and pitch angle To simulate a virtual 2D lidar that is always parallel to the ground plane, in the current depth image plane... Find a corresponding virtual horizontal line; based on the pinhole camera model and rigid body rotation transformation, derive the projection trajectory of the virtual laser beam on the image plane;

[0052] Assuming the virtual laser beam is located at the camera's optical center height and parallel to the ground plane, its projection trajectory on the image plane satisfies the following linear equation: ;

[0053] in Image column coordinates corresponding row coordinates ; The principal pixel coordinates of the camera; This is the equivalent focal length of the camera in the vertical direction. The pitch angle, This refers to the roll angle;

[0054] The row coordinates corresponding to the column coordinates of the image are determined by the pixel coordinates of the camera principal point, the equivalent focal length in the vertical direction, the roll angle, and the pitch angle, thereby achieving geometric correction of the horizontal scan line under attitude jitter.

[0055] 4.2) Constructing an adaptive dynamic window fusion mechanism: Define a mechanism based on the calculation row Centered sampling window Vertical bandwidth Based on the current depth value With body angular velocity Dynamic adjustment: ;

[0056] in Based on bandwidth, The coefficient of thermal expansion is the near-field coefficient. The motion blur compensation coefficient;

[0057] In the sampling window Extract all valid depth pixel sets If the set is empty, mark the angle as invalid data; if the set is not empty, calculate the median (median filtering) to remove noise at the edges and generate the final stable depth value. ;

[0058] 4.3) To prevent erroneous observations from being introduced into the SLAM system, gating logic is introduced:

[0059] If calculated Beyond the vertical range of the image Force the output to the maximum range; calculate the normal vector of the current virtual scan plane. With gravity vector The included angle; only when the included angle is within the threshold range (e.g. If the scan data is not cleared, it is considered valid; otherwise, it is determined that the system is in an extreme attitude blind zone, and the laser data update is automatically suspended. Instead, the system switches to relying solely on the leg odometry from step 3 for dead reckoning until the attitude is restored.

[0060] Furthermore, step 5 consists of the following specific steps:

[0061] 5.1) Utilize the high-frequency leg odometer (LIO) speed output from step 3. For the previous successful frame position Perform Lie group manifold integrals to predict the current time step. position : ;

[0062] in ; For the mapping operator from a vector to a Lie algebra matrix; For Lie group exponent mapping; predict pose It acts as an inertial navigator for the visual system, maintaining the continuity of the trajectory during periods of visual blindness;

[0063] 5.2) Actively predict the position of map points from the previous frame in the current frame image by predicting the pose at the current moment; thereby narrowing the search range;

[0064] Projection prediction: For known 3D map points in the previous frame Project it onto the current predicted pose. On the camera plane, the predicted pixel positions are obtained: ;

[0065] To predict pixels Set an adaptive search radius centered on the target area. ,in This represents the magnitude of the current body coordinate system linear velocity output by the leg odometer. The time difference between adjacent frames. This is the scaling factor (calibrated based on the camera's focal length and average depth of field). The basic fault-tolerant pixel threshold is used to adapt to the robot's maneuvering characteristics under different time states;

[0066] 5.3) Construct a tightly coupled nonlinear optimization system: During the operation of the SLAM system, based on the continuously input sensor data, the state variables and constraints are transformed into a factor graph in real time, and the body velocity estimated by the leg odometry is explicitly introduced as a dynamic constraint factor to force the motion between adjacent poses to conform to physical continuity and suppress abnormal pose updates caused by scan mismatch.

[0067] The system maintains a global pose graph. , where nodes This means the robot's pose has been translated more than a set displacement threshold relative to the previous keyframe. Or the rotation angle exceeds the set angle threshold. Time-based world position ,side Includes three heterogeneous constraint factors:

[0068] A. Scan matching factor: The relative pose constraint is calculated by scanning the map and matching the stabilized 2D scan data generated in step 4.

[0069] B. Visual odometry factor: Relative motion constraints between adjacent frames calculated by the visual front end;

[0070] C. Elastic Dynamics Velocity Factor: Directly constrains the average velocity between adjacent nodes, based on the leg velocity output in step 3. Construct the residual terms: ;

[0071] The Levenberg-Marquardt algorithm was used to incrementally smooth and map the factor map. The optimized pose was used to stitch the scan data from step 4 into a global 2D raster map and published to ROS2 NavigationStack in real time for path planning.

[0072] The design concept of this invention is as follows:

[0073] To address the core contradiction of perception and control mismatch in high-dynamic motion of low-cost quadruped robots, this invention addresses the issue. Traditional navigation schemes typically assume stable and accurate sensors. However, in quadruped robots driven by reinforcement learning strategies, high-frequency jitter and rapid pitch can lead to visual loss and laser scanning distortion, rendering expensive perception algorithms ineffective. To overcome the low-cost hardware limitations imposed by the lack of foot contact sensors, this invention does not rely on unreliable external sensors. Instead, it utilizes a hybrid internal model and a generalized momentum observer to softly calculate high-precision contact states and implicit environmental features from joint torques and IMU data. This essentially gives the robot "touch," allowing it to perceive the ground even when vision fails. High-frequency attitude information from the IMU is used in the image domain to "correct" the jittered depth map into a horizontal virtual laser scan, eliminating motion distortion at its source. Furthermore, an active recovery mechanism is established based on the high reliability of leg kinematics. When vision fails, leg odometry is used for manifold integration and feature reprojection to guide the vision system to quickly recover its state, rather than passively waiting for a reset, achieving complementary enhancement between control and vision. Simultaneously, an elastic velocity factor is introduced into the global mapping. The system can sense environmental degradation, and when visual degradation occurs, it automatically increases the weight of leg kinematic constraints to force the trajectory to conform to physical motion and prevent map drift.

[0074] The beneficial effects of this invention are as follows:

[0075] Compared with existing methods, this invention can significantly improve the positioning accuracy and navigation success rate of quadruped robots in fast-moving and complex indoor environments while ensuring low system cost. By introducing scan stabilization and leg odometry, the core problem of SLAM performance degradation caused by motion jitter is effectively solved. At the same time, to minimize jitter caused by external interference, a hybrid internal model is used at the body motion control level of the quadruped, further improving the robustness of the quadruped. Attached Figure Description

[0076] Figure 1 This is a flowchart of the present invention;

[0077] Figure 2 A comparison chart is generated for stable operation without scanning and with scanning. Detailed Implementation

[0078] The present invention will be further described below with reference to the accompanying drawings.

[0079] A localization and navigation method for a quadruped robot based on low-cost sensors, comprising the following steps:

[0080] Step 1: Propriometry and State Estimation. Obtain the robot's proprioception sequence, use hybrid internal model regression to estimate the robot's explicit linear velocity, and extract implicit latent features that reflect external environmental characteristics (such as terrain and friction).

[0081] 1.1) Explicit linear velocity regression: Utilizing the true linear velocity from the privileged information provided during the simulation training phase. As a supervisory signal, the historical observation sequence of the ontology perception is input into the regression network, and the estimated linear velocity is output. This branch optimizes by minimizing the mean squared error (MSE) between the predicted and true values, aiming to enable the robot to obtain accurate kinematic state estimates.

[0082] 1.2) Implicit Response Contrastive Learning: For external environmental parameters that cannot be directly measured (such as ground friction and terrain undulation), they are modeled as latent spaces. Implicit response characteristics in Based on the principle of trajectory continuity, the ontology-aware features of adjacent time steps within the same trajectory should have high similarity and serve as positive samples, while features from different environments or different trajectories should have significant differences and serve as negative samples. This step employs a contrastive learning framework to optimize the feature space distribution, bringing features belonging to the same trajectory closer together and widening the distance between features from different trajectories, thereby endowing the reinforcement learning controller with the ability to indirectly identify responses to different external environments.

[0083] Step 2: Sensorless contact state determination. Based on joint torque and robot dynamics model, a hybrid mode generalized momentum (GM) perturbation observer is constructed to estimate foot contact force in real time and determine the contact state of each leg by combining threshold values, without the need for physical contact sensors.

[0084] 2.1) Constructing the Generalized Momentum (GM) Observer Model This step introduces a perturbation observer based on classical generalized momentum theory as the core of the contact state detector. Unlike traditional acceleration-based observers, this method does not require differentiation of the acceleration signal, thus avoiding noise amplification. The dynamic equations of the GM-based observer are defined as follows: ;

[0085] in The generalized momentum measured by the system. Represents the joint space mass matrix. This is the joint velocity vector; The generalized momentum estimated for the observer; Let be the contact force vectors of the four legs to be estimated; The Jacobian matrix represents the contact area of ​​the robot's feet. The observer gain matrix; This is a nonlinear dynamic compensation term, defined as follows: ,in It is the output torque of the joint motor. It is the Coriolis matrix and the centrifugal force matrix. It is the gravity compensation vector.

[0086] 2.2) Constructing a Generalized Super-Twisting Momentum Residual Observer: To overcome the inherent contradiction (phase lag) between noise suppression and response speed in traditional linear high-gain observers, this invention innovatively introduces a super-twisting sliding mode term based on the generalized momentum model. This not only performs sliding mode control on the position error, but also directly controls the momentum residual. Nonlinear dynamic evolution is performed to achieve a finite-time convergent estimate of the external contact force.

[0087] The observer's dynamic equations are reconstructed as follows: ;

[0088] in For generalized momentum residuals; Predict torques for models that incorporate both gravity and Coriolis forces; This is the model uncertainty compensation term; These are auxiliary state variables for the observer, used to absorb disturbances; and The hybrid nonlinear injection term is defined as: ;

[0089] Linear terms are introduced here. To ensure exponential convergence when residuals are large, the square root term... Ensure finite-time convergence when the residual approaches zero and eliminate chattering.

[0090] Once the observer outputs the initial force estimate, it needs to be transformed into a reliable binary contact signal through physical constraints and adaptive logic. Since the physical contact between the foot and the ground can only generate thrust and not tension, and we are primarily concerned with the supporting effect, the estimated full-dimensional disturbance torque will be... Inverse projection onto the foot-end Cartesian space using the Jacobian matrix, with one-sided constraints applied: ;

[0091] in The pseudo-inverse of the transpose of Jacobi. The ground plane normal vector, yes The z-axis component.

[0092] 2.3) State-Dependent Adaptive Threshold: Existing techniques typically use a fixed threshold. However, low-cost robots, when swinging their legs at high speeds, may generate false "observed forces" due to errors in the dynamic model, such as motor inertia and friction, leading to misjudgments of landing in mid-air. To address this, this invention proposes a judgment threshold function that dynamically adjusts according to the joint state. : ;

[0093] in The base noise floor threshold under static conditions; Let the joint velocity norm be... The velocity gain number is used to compensate for model errors related to Coriolis force and viscous friction; For the command acceleration norm, The inertial gain coefficient is used to compensate for inertial model errors during emergency stops and starts.

[0094] Ultimately First-order low-pass filtering is performed to obtain Then, a Schmitt trigger logic is used for judgment to prevent signal transitions in critical states: ;

[0095] in This refers to the hysteresis band width. =1 represents touching the ground, and the opposite represents swinging.

[0096] Step 3: Leg-inertial odometry (LIO) calculation. Based on the contact state obtained in Step 2, and combining kinematic constraints and IMU angular velocity constraints, a least squares optimization problem is constructed to solve the leg odometry of the robot body.

[0097] 3.1) This step aims to utilize the contact state and joint motion information obtained in step 2, combined with onboard IMU data, to calculate the high-frequency linear velocity and angular velocity of the robot body. To address the foot slippage problem experienced by low-cost robots on slippery or uneven surfaces, this invention proposes a weighted least squares framework to dynamically suppress the influence of unreliable legs on state estimation. Based on the principles of rigid body kinematics, when the... When a leg is in an ideal static contact state, meaning the foot does not slip, the velocity of the foot relative to the ground should be zero. According to the velocity composition theorem, the velocity of the body center and the velocity of the foot are subject to the following constraint relationship: ;

[0098] Rearranging and simplifying, we obtain the velocity constraint equations for the machine body: ;

[0099] in Let be the linear velocity of the machine body to be solved; Let ω be the angular velocity of the machine body to be solved. For the first The position vector of the foot tip of each leg in the body coordinate system; for The corresponding antisymmetric matrix; The linear velocity of the foot relative to the body, derived solely from joint movement. For Jacobian matrices, This represents the joint angular velocity.

[0100] 3.2) Constructing a multi-source constrained overdetermined linear system, where single-leg constraints cannot uniquely determine the 6-DOF velocities of the system. To ensure the observability of the system under arbitrary gait, this invention integrates angular velocity measurements from an IMU (Integrated Measurement Unit). As a strong constraint, a joint system of linear equations is constructed: ;

[0101] Where the state vector For the set of all legs currently in the support phase. ,matrix sum vector The structure is as follows: ;

[0102] Among them, the former Each row block corresponds to the kinematic constraints of each support leg, and the last row block corresponds to the direct observation constraint of the IMU's angular velocity.

[0103] 3.3) In real-world complex terrain, some legs may be in a "near-touching" or "slightly slipping" state. Therefore, a physical consistency weight matrix is ​​introduced. : ;

[0104] Among them, the first Weight of a leg It is not a fixed value, but rather the contact force estimated in step 1. Dynamic determination of kinematic residuals: ;

[0105] in The Sigmoid mapping function ensures that the greater the contact force (the more solid the support), the higher the weight. The estimated slip velocity from the previous moment is used to apply a weighted penalty to the leg that slipped.

[0106] Final machine speed estimation The weighted least squares solution is: ;

[0107] 3.4) To provide an accurate information matrix for the factor graph optimization in subsequent step 5, this step not only outputs the velocity estimate but also analytically calculates the estimated covariance. .

[0108] Assume the sensor noise covariance is According to the law of error propagation: ;

[0109] The covariance matrix By directly passing the dynamic weights to the backend SLAM system, a closed-loop management of uncertainty from leg perception to global mapping is achieved.

[0110] Step 4: Visual observation stabilization processing. Using the attitude angle measured by the IMU, the depth map acquired by the depth camera is dynamically sliced ​​and corrected to generate stabilized 2D laser scanning data that removes the influence of attitude jitter.

[0111] 4.1) This step aims to solve the problem that when a quadruped robot moves in a highly dynamic manner, the drastic change in the body's attitude angle causes the imaging plane of the airborne depth camera to tilt relative to the ground plane, resulting in geometric distortion of the generated 2D laser scanning data.

[0112] Establish a geometric projection model for virtual horizontal scanning, and set the depth camera coordinate system as follows: Its optical axis is aligned with the front of the robot body. Using the attitude quaternions acquired in real time by the onboard IMU, the roll angle of the robot relative to the horizontal plane of gravity is calculated. and pitch angle To simulate a virtual 2D lidar that is always parallel to the ground plane, it is necessary to [do something] in the current depth image plane. Find a corresponding virtual horizontal line. Based on the pinhole camera model and rigid body rotation transformation, assuming the virtual laser beam is located at the camera's optical center height and parallel to the ground plane, its projection trajectory on the image plane satisfies the following linear equation: ;

[0113] in Image column coordinates corresponding row coordinates ; The principal pixel coordinates of the camera; Let be the equivalent focal length of the camera in the vertical direction; this equation intuitively shows the roll angle. It determines the slope of the scan line and the pitch angle. It determines the vertical intercept of the scan line.

[0114] 4.2) Constructing an adaptive dynamic window fusion mechanism. Existing technologies typically extract only the single-row pixels calculated by formula (15) as distance readings. This method is highly susceptible to the effects of depth camera speckle noise and depth loss. To address this, this invention proposes a statistically based dynamic window fusion algorithm, defining a method for calculating row... Centered sampling window Its vertical bandwidth It is not a fixed value, but rather depends on the current depth value. With body angular velocity Dynamic adjustment: ;

[0115] in Based on bandwidth, This is the near-field dilation coefficient (near objects require more pixels for smoothing). For motion blur compensation coefficients.

[0116] In the sampling window Extract all valid depth pixel sets within the inner layer. If the set is empty, the angle is marked as invalid data; if the set is not empty, its value is calculated to remove ghosting noise at the edges, generating the final stable depth value. .

[0117] 4.3) Due to the limited vertical field of view of the camera, when the robot's pitch angle... When the angle is too large (e.g., looking up while climbing a slope or looking down while going down stairs), the theoretically calculated scan line... It may move out of the image boundary. To prevent the SLAM system from introducing erroneous observations, gating logic is introduced: if the calculated... Beyond the vertical range of the image Force the output to the maximum range; calculate the normal vector of the current virtual scan plane. With gravity vector The included angle. Only if the included angle is within the threshold range, such as Only when the scan data is valid is the system considered valid; otherwise, it is determined that the system is in an extreme attitude blind zone, and the system automatically pauses laser data updates and switches to pure dead reckoning based solely on the leg odometry from step 2 until the attitude is restored.

[0118] Step 5: Multi-source fusion localization and mapping, fusing leg odometry velocity, stabilized 2D scan data, and visual inertial odometry (VIO) into the SLAM system. When VIO is lost, leg odometry is used for dead reckoning and resetting; a velocity constraint factor from leg odometry is introduced into the factor map optimization to construct a high-precision 2D grid map;

[0119] 5.1) This step aims to address the tracking failure problem of quadruped robots in highly dynamic or weakly textured areas, caused by the loss of feature points or image motion blur at the visual front end. This invention abandons the traditional passive recovery mode of "detection failure-stop-reset" and proposes an active recovery mechanism of prediction-guidance-seamless stitching. Once the recovery mechanism is triggered, the high-frequency leg odometry (LIO) speed output in step 2 is utilized. For the previous successful frame position Perform Lie group manifold integration. Compared to traditional Euler angle integration, this is based on Lie algebras. The integral can avoid gimbal lock and ensure the orthogonality of the rotation matrix. Predict the current time step. position : ;

[0120] in ; For the mapping operator from vector to Lie algebra matrix; This is a Lie group exponent mapping. The predicted pose is... It acts as an inertial navigator for the visual system, maintaining the continuity of the trajectory during periods of visual blindness.

[0121] 5.2) Traditional resetting involves blindly searching for new features throughout the entire image. This invention utilizes... Actively predict the location of map points from the previous frame in the current frame image, thereby narrowing the search range. Projection prediction: For known 3D map points from the previous frame... Project it onto the current predicted pose. On the camera plane: ;

[0122] To predict pixels Set a very small search radius around the center. Within this window, matching features are searched. Utilizing leg kinematic information significantly reduces the search space for visual matching, enabling the system to quickly capture potential feature points even in severely blurred images.

[0123] During extreme periods when vision is completely unavailable, the system does not insert visual constraints into the SLAM factor graph; instead, it inserts nodes with purely kinematic constraints. Temporary nodes are created in the factor graph, connected only by the pre-integrated factors from the leg odometry and the IMU factors. Once vision is restored, a new visual keyframe is connected to these blind zone nodes via prior edges, thus ensuring the temporal continuity of the global map and avoiding map fragmentation or ghosting phenomena often seen in traditional methods.

[0124] 5.3) This step aims to construct a tightly coupled nonlinear optimization system, fusing visual odometry (VIO), leg odometry (LIO), and stabilized laser scanning into a unified factor graph framework. It breaks through the limitation of traditional SLAM relying solely on perception data for optimization, innovatively utilizing leg kinematics information as a physical anchor point to dynamically take over pose estimation when the perceived environment degrades, and verifying the physical feasibility of loop closure detection. To alleviate this problem, this paper proposes explicitly introducing the body velocity estimated by leg odometry as a dynamic constraint factor into the factor graph, forcing the motion between adjacent poses to conform to physical continuity, thereby suppressing abnormal pose updates caused by scan mismatch and ensuring that the optimization results are consistent with the robot's actual motion state. The system maintains a global pose graph. , where nodes Represents the world-system pose of the robot at keyframe moments. The edges in the diagram Includes three heterogeneous constraint factors:

[0125] A. Scan matching factor: The relative pose constraint is calculated by scanning-to-map matching from the stabilized 2D scan data generated in step 4.

[0126] B. Visual odometry factor: Relative motion constraints between adjacent frames calculated by the visual front end.

[0127] C. Elastic Dynamics Velocity Factor: Unlike traditional position constraints, this factor directly constrains the average velocity between adjacent nodes. Based on the leg velocity output from step 3. Construct the residual terms: ;

[0128] The Levenberg-Marquardt algorithm was used to incrementally smooth and map the aforementioned factor map. The optimized pose was then used to stitch the scan data from step 4 into a global 2D raster map, which was then published in real time to ROS2 NavigationStack for path planning.

[0129] Example:

[0130] 10,000 trajectory data points were collected in the Isaac Gym simulation environment. Privileged information includes the actual linear velocity output by the simulator. coefficient of friction with the ground Construct the input vector Including the most recent 5 frames, The joint velocity, IMU acceleration, and motor torque are input into a pre-trained MLP network, which outputs an explicit linear velocity estimate at a frequency of 50 Hz. and 16-dimensional implicit environmental features This implicit feature is used for subsequent adaptive parameter adjustment of the controller.

[0131] Next, the foot contact state is estimated based on the robot's current joint position. and speed The joint space mass matrix is ​​calculated in real time using the URDF model. And the expected torque, which includes gravity and Coriolis force. Compare the expected torque with the measured motor torque. The difference is input to the generalized momentum observer. The observer gain is set to... And introduce parameters as The super-helical sliding mode term was used to calculate the estimated value of the foot contact force. This configuration ensures that the observer converges within 10ms after the contact impact. To avoid misinterpreting the inertial force during leg swing as contact, the system calculates a dynamic threshold in real time. Set a base threshold. N. If the robot is in a rapid trotting state and the average joint speed reaches 10 rad / s, then calculate the current threshold. Estimation power and Comparison. If If the leg is in the supporting phase, then it is determined to be in the swinging phase; otherwise, it is determined to be in the swinging phase.

[0132] Next, the leg odometer is calculated, iterating through all four legs, and the physical consistency weight is calculated based on the results of step 2. .

[0133] Assuming the left foreleg is detected as the support phase, estimate the contact force. N, and the foot sliding speed detected in the previous moment was relatively small. m / s. The system substitutes the values ​​into the formula to calculate the weights. If one leg is in a skating position For legs with a speed of m / s, the calculated weight will automatically drop below 0.01, thus removing that leg's data. Then, the kinematic constraints of all high-weight legs are combined with the IMU's angular velocity constraints to construct... overdetermined matrix The weighted least squares method is used to solve the equations, and the linear velocity of the aircraft in the fuselage coordinate system is output. and velocity covariance matrix .

[0134] Electronic image stabilization and scan generation for visual observation involve simultaneously reading the IMU's attitude angle upon receiving the depth image. Assume the robot is currently in a head-up, hill-climbing state, with the pitch angle... Roll angle Calculate the center intercept offset of the virtual horizontal ground plane in the image based on the geometric projection formula. And based on the currently measured average depth meters and body angular velocity rad / s, calculate the sampling window bandwidth: In the image number Depth data is extracted within the range of the row, and the median of each column of pixels is taken to generate a stable row of virtual 2D laser scan data. If the robot violently tilts its head, the calculated scan line row number is... The system determines that the viewer is currently looking at the sky and automatically outputs an inf value to prevent the sky from being mistaken for an obstacle in front of them; for example... Figure 2 As shown, the left side is a map generated stably without scanning ( Figure 2 Part a), the right side is the scan-stable 2D image ( Figure 2 Part b).

[0135] The active monitoring and recovery of VIO is performed in the visual odometry backend thread, during which the robot executes a rapid turning motion. The speed of rad / s causes image blurring and a sharp drop in the number of feature points. The system determines that VIO is about to be lost and immediately suspends the regular matching process. It then calls the leg odometer speed output from step 3. In the current frame interval Within milliseconds, SE(3) manifold integration is performed to predict that the current pose has rotated by approximately 3.5° relative to the previous frame. Using this predicted pose, the 3D feature points of the previous keyframe are projected onto the current blurred image. Only around the predicted projection points... Feature search is performed within a tiny window of pixels. Compared to full-image search, the time was reduced from 22ms to 2ms, successfully recovering 83% of feature points, maintaining VIO continuity, and avoiding resets. Subsequently, global mapping is performed based on the elasticity factor. When the robot enters a long corridor, the left and right walls are clear, but there are no features in front or behind, leading to the scanning matching of the minimum Hessian eigenvalue in the forward direction. The system determines that the environment has "degraded." The system automatically adjusts the weights of each constraint in the factor graph. The leg speed factor... Information matrix Instant improvement. The iSAM2 solver is invoked for optimization. At this point, the optimizer ignores unreliable laser front-to-back matching constraints and instead fully trusts the forward speed measured by the leg odometry. The resulting corridor map has a length error of less than 2%, and a 2D occupancy grid map with a resolution of 0.05m is published at a frequency of 5Hz for the navigation stack to perform path planning.

[0136] This invention addresses the problems of severe body shaking, easy loss of visual odometry (VIO), 2D scan jumps, and map drift caused by high-speed movement in existing low-cost quadruped platforms. It proposes a multi-source collaborative enhancement framework: First, a hybrid internal model is constructed, using SimCLR contrastive learning to explicitly regress linear velocity from the ontology perception sequence and implicitly encode external perturbation features. Second, a sliding mode generalized momentum observer is designed, using only joint torque to estimate foot contact state, eliminating the need for foot contact sensors. Then, a leg inertial odometry (LIO) is constructed, fusing supporting leg kinematic constraints and IMU angular velocity to robustly estimate the body's 6-DOF velocity. An IMU adaptive depth slicing method is proposed, dynamically correcting the scan line based on roll and pitch angles to generate anti-shake 2D scans. Finally, multi-sensor information is fused: when VIO fails, LIO integral prediction is used for pose reinitialization; a velocity constraint factor is added to the factor map to suppress mismatch drift. Experiments show that this method only requires an IMU, an articulated encoder, and an RGB-D camera to achieve stable 2D mapping and navigation on low-cost platforms such as Go2, significantly improving localization robustness and success rate.

[0137] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit the scope of protection of the present invention. Various equivalent substitutions or modifications made by those skilled in the art without departing from the spirit and substance of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for localization and navigation of a quadruped robot based on low-cost sensors, characterized in that, Includes the following steps: Step 1: Propriometry and State Estimation: Obtain the robot's proprioception sequence, use hybrid internal model regression to estimate the robot's explicit linear velocity, and extract implicit latent features that reflect the characteristics of the external environment. Step 2: Sensorless contact state determination: Based on joint torque and robot dynamics model, a hybrid mode generalized momentum GM perturbation observer is constructed to estimate foot contact force in real time and determine the contact state of each leg by combining threshold. Step 2 is detailed below: 2.1) Constructing a generalized momentum observer model: Introducing a perturbation observer based on classical generalized momentum theory, using generalized momentum and foot contact force as state variables, and combining nonlinear dynamic compensation terms with observer error feedback to estimate the system contact force; 2.2) Constructing a generalized superspiral momentum residual observer: Based on the generalized momentum model, a superspiral sliding mode term is introduced. Using the generalized momentum residual as the feedback signal, the observer's dynamic equation is constructed through model uncertainty compensation terms, hybrid nonlinear injection terms, and auxiliary state variables to estimate the full-dimensional perturbation torque. After estimation, the full-dimensional perturbation torque is inversely projected onto the foot Cartesian space through the Jacobian matrix, and a unilateral constraint is applied to obtain an effective estimate of the foot contact force. 2.3) Adaptive dynamic threshold of state: Establish a judgment threshold function that dynamically adjusts with the joint state. The judgment threshold function is based on the basic noise floor threshold in the static state, combined with the norm of joint velocity and command acceleration, and corrected by the corresponding gain coefficient to achieve dynamic adjustment. 2.3) also includes filtering the foot contact force. The filtered foot contact force signal is used to determine the contact state using Schmitt trigger logic. A hysteresis band is introduced to prevent signal jumps in critical states. When the contact force exceeds the upper threshold, it is determined to be ground contact. When it is below the lower threshold, it is determined to be swinging. In the intermediate state, the contact state of the previous moment is maintained. Step 3: Leg-Inertial Odometry (LIO) Solution: Based on the contact state obtained in Step 2, and combining kinematic constraints and IMU angular velocity constraints, a least squares optimization problem is constructed to solve the leg odometry of the robot body; Step 4: Visual observation stabilization processing: Using the attitude angle measured by the IMU, the depth map acquired by the depth camera is dynamically sliced ​​and corrected to generate stabilized 2D laser scan data that removes the influence of attitude jitter. Step 5: Multi-source fusion localization and mapping, fusing leg odometry speed, stabilized 2D scan data and visual inertial odometry (VIO) into the SLAM system; when VIO is lost, dead reckoning is performed using leg odometry; the speed constraint factor of leg odometry is introduced into the factor map optimization to construct a high-precision 2D grid map.

2. The quadruped robot localization and navigation method based on low-cost sensors according to claim 1, characterized in that, Step 1: The specific steps are as follows: 1.1) Explicit linear velocity regression: Utilizing the true linear velocities from the privileged information provided during the simulation training phase. As a monitoring signal; Input the historical observation sequence of the proprioception into the regression network, and output the linear velocity estimate. Optimization is achieved by minimizing the mean square error (MSE) between the predicted and true values. 1.2) Implicit Response Contrastive Learning: For external environmental parameters that cannot be directly measured, they are modeled as a latent space. Implicit response characteristics in Based on the principle of trajectory continuity, the ontology-aware features of adjacent time steps in the same trajectory are used as positive samples, while features of different environments or different trajectories are used as negative samples. A contrastive learning framework is adopted to optimize the feature space distribution, so that the feature distance of the same trajectory is shortened and the feature distance of different trajectories is lengthened.

3. The quadruped robot localization and navigation method based on low-cost sensors according to claim 1, characterized in that, Step 3 is as follows: 3.1) Using the contact state and joint motion information obtained in step 2, combined with the airborne IMU data, calculate the linear velocity and angular velocity of the robot body; 3.2) Constructing a multi-source constrained overdetermined linear system: The angular velocity measurement value of the IMU is used as a strong constraint and fused with the kinematic constraints of all legs in the support phase to construct a joint linear equation system; 3.3) Introducing a physical consistency weight matrix: Based on the weights corresponding to each support leg and the weights corresponding to the IMU angular velocity measurements, a diagonal weight matrix is ​​constructed. The weighted least squares method is used to solve the overdetermined linear equations to obtain the final estimated value of the body velocity.

4. The quadruped robot localization and navigation method based on low-cost sensors according to claim 1, characterized in that, Step 4 is detailed below: 4.1) Establish the geometric projection model for the virtual horizontal scan, and set the depth camera coordinate system as follows: Its optical axis is aligned with the front of the robot body; using the attitude quaternions acquired in real time by the onboard IMU, the roll and pitch angles of the robot relative to the horizontal gravity plane are calculated; in the current image plane Find a corresponding virtual horizontal line, and derive the projection trajectory of the virtual laser beam on the image plane based on the pinhole camera model and rigid body rotation transformation. 4.2) Construct an adaptive dynamic window fusion mechanism: Define a sampling window centered on the virtual horizontal line pixel row. The vertical bandwidth of the window is dynamically adjusted according to the current depth value and the body angular velocity. Extract all valid depth pixels within the sampling window. If there is no valid data, mark it as invalid. If there is valid data, remove edge noise through median filtering to generate the final stable depth value. 4.3) Introduce gating logic: When the calculated position of the virtual horizontal line exceeds the vertical range of the image, the maximum range is forced to be output; at the same time, the angle between the virtual scanning plane and the direction of gravity is checked. Only when the angle is within the preset threshold range is the scanning data considered valid; otherwise, it is determined that the current state is in an extreme attitude blind zone, the laser data update is suspended, and the system switches to pure dead reckoning based solely on the leg odometry until the attitude returns to normal.

5. A quadruped robot localization and navigation method based on low-cost sensors according to claim 1, characterized in that, Step 5 is detailed below: 5.1) Using the high-frequency leg odometry velocity output in step 3, perform Lie group manifold integration on the pose of the previous frame to predict the pose at the current moment. 5.2) Actively predict the position of map points from the previous frame in the current frame image by using the current pose; project the known 3D map points from the previous frame onto the camera plane corresponding to the current predicted pose to obtain the predicted pixel position; then, with the predicted pixel position as the center, combine the body speed and time difference output by the leg odometry to set an adaptive search radius to adapt to the robot's maneuvering characteristics under different time states. 5.3) Construct a tightly coupled nonlinear optimization system: During SLAM system operation, state variables and constraint terms are transformed into factor graphs, and the body velocity estimated by leg odometry is explicitly introduced as a dynamic constraint; the system maintains a global pose graph, where nodes are robot world system poses and edges contain three types of constraint factors: scan matching factors based on 2D scan data, visual odometry factors based on the visual front end, and elastic dynamic velocity factors based on leg velocity; the factor graph is incrementally smoothed and mapped using the Levenberg-Marquardt algorithm, and a 2D grid map is output for navigation.

6. A quadruped robot localization and navigation method based on low-cost sensors according to claim 3, characterized in that, The process described in 3.1) is as follows: Based on the principles of rigid body kinematics, when the... When a leg is in an ideal static contact state, that is, the foot does not slip and the velocity of the foot relative to the ground is zero; according to the velocity composition theorem, the constraint relationship between the velocity of the body center and the velocity of the foot is established, and the velocity constraint equation of the body is obtained by simplification, and the linear velocity and angular velocity of the body are solved.

7. A quadruped robot localization and navigation method based on low-cost sensors according to claim 4, characterized in that, In section 4.1), the projection trajectory satisfies a linear equation relationship, where the row coordinates corresponding to the image column coordinates are determined by the camera principal point pixel coordinates, the equivalent focal length in the vertical direction, the roll angle, and the pitch angle, thereby achieving geometric correction of the horizontal scan line under attitude jitter.

8. A quadruped robot localization and navigation method based on low-cost sensors according to claim 5, characterized in that, The pose predicted at the current moment in 5.1) can serve as an inertial navigator for the vision system, maintaining the continuity of the trajectory during vision failure.

Images