Four-legged robot multi-source information fusion mapping method and device based on factor graph optimization

Abstract

The application belongs to the technical field of robot environment perception and three-dimensional reconstruction, and discloses a quadruped robot multi-source information fusion mapping method and device based on factor graph optimization. The foot end odometer of the quadruped robot is introduced into the factor graph optimization framework as an independent constraint factor, and in the environment with missing or degenerated laser point cloud features, the motion constraint is provided, and the robustness of the system is fundamentally improved. An adaptive noise model based on the foot end contact state is designed, the confidence of the foot end odometer factor can be dynamically adjusted according to the contact quality, strong constraints are provided during normal walking, the weight is automatically reduced when slipping, and the pollution of the system caused by the false information is effectively inhibited. The tight coupling optimization of the laser radar, the IMU and the foot end kinematics information is realized, instead of loose coupling or post-fusion, so that the multi-source sensor information is deeply complementary at the state estimation level, and the overall estimation accuracy under complex terrain and dynamic motion is significantly improved.

Description

Technical Field

[0001] This invention belongs to the field of robot environmental perception and 3D reconstruction technology, specifically relating to a method and apparatus for multi-source information fusion mapping of quadruped robots based on factor graph optimization. Background Technology

[0002] In recent years, quadruped robots have been increasingly used in large-scale, unstructured scenarios such as industrial inspection, warehousing and logistics, and disaster relief. Achieving accurate real-time positioning and map building is a core prerequisite for their autonomous navigation. Laser-synchronized localization and mapping (LSRT) technology, due to its advantages of accurate ranging and independence from lighting conditions, has become the mainstream solution in such environments.

[0003] Currently, advanced laser SLAM (Simultaneous Localization and Mapping) systems mostly employ a tightly coupled fusion framework of lidar and inertial measurement units (IMUs). Through factor graph or graph optimization techniques, they jointly optimize constraints such as laser odometry and IMU pre-integration to improve system accuracy and robustness. However, existing methods are mostly designed for wheeled or flying platforms and fail to fully consider the challenges posed by the unique motion characteristics of quadruped robots. When a quadruped robot walks, its torso experiences high-frequency shaking due to its gait, and its feet may slip when in contact with the ground. These factors cause severe motion distortion in the lidar point cloud mounted on the robot, thus affecting the accuracy of front-end matching and state estimation.

[0004] To address the motion characteristics of quadruped robots, some studies have attempted to introduce kinematic constraints. For example, they have fused foot information through planar motion assumptions or by constructing filter-based state estimators. However, these methods have significant limitations: methods based on strong assumptions are difficult to apply in complex terrain; filter-based architectures struggle to fully utilize historical observation information and typically do not deeply integrate adaptive judgment mechanisms for foot contact states into the optimization process. Therefore, when dealing with severe shaking, foot slippage, and missing environmental features (such as long corridors), the mapping accuracy and robustness of existing methods significantly decrease.

[0005] In summary, there is an urgent need for a tightly coupled mapping method that can deeply integrate the body motion perception of quadruped robots to make full use of the contact information between their feet and the ground, suppress motion distortion, and thus achieve high-precision and robust 3D environment reconstruction under complex terrain and dynamic gait. Summary of the Invention

[0006] The purpose of this invention is to provide a multi-source information fusion mapping method and device for quadruped robots based on factor graph optimization, which overcomes the problem of decreased mapping accuracy caused by body shaking, foot slippage and environmental feature degradation when existing laser inertial odometry is applied to quadruped robots.

[0007] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0008] Firstly, a multi-source information fusion mapping method for quadruped robots based on factor graph optimization is provided, including the following steps:

[0009] Set the data acquisition frequency of each sensor of the quadruped robot, and synchronously collect LiDAR point cloud data, inertial measurement unit data and joint encoder data;

[0010] The relative motion increment is obtained by pre-integrating the inertial measurement unit data, and the relative pose transformation of the lidar observation is obtained by performing motion distortion correction, feature extraction and inter-frame registration on the lidar point cloud data. The foot pose is calculated by kinematic model based on the joint encoder data.

[0011] Based on the foot contact state detection results, the foot in a stable contact state is selected, and the relative pose transformation of the robot body's foot is calculated according to the foot pose, which is used as the foot odometry measurement value.

[0012] Based on the relative motion increment, the relative pose transformation observed by lidar, and the foot odometer measurement, the error terms of adjacent state nodes in the factor diagram are determined and used as the IMU pre-integration factor, the lidar odometer factor, and the foot odometer factor.

[0013] A factor graph containing IMU pre-integration factors, laser odometry factors, and foot odometry factors is constructed, and a nonlinear optimization method is used to solve the optimization problem to obtain the optimal pose sequence of the quadruped robot.

[0014] The lidar point cloud data is registered to the world coordinate system based on the optimal pose sequence to complete the construction of a 3D environmental map.

[0015] Several alternative methods are provided below, but they are not intended as additional limitations on the overall solution above. They are merely further additions or optimizations. Provided there are no technical or logical contradictions, each alternative method can be combined individually with respect to the overall solution above, or multiple alternative methods can be combined with each other.

[0016] Preferably, the stable contact state is determined as follows:

[0017] The contact force on the foot in the vertical direction is detected by a three-dimensional force sensor on the sole of the foot;

[0018] If the contact force is greater than the set contact force threshold, and the standard deviation of the contact force within the time window is less than the preset proportion of the mean, and the speed of the foot in the body coordinate system is less than the speed threshold, then the foot is determined to be in a stable contact state; otherwise, the foot is determined to be in an unstable contact state.

[0019] Preferably, the method also includes determining the weighting parameters of the foot-end odometry factor based on the foot-end contact state detection results, and the specific process is as follows:

[0020] When the number of feet in stable contact is less than 2, the weight parameter of the foot odometer factor is set to zero.

[0021] Otherwise, when all four feet of the quadruped robot are in a stable contact state and the contact force fluctuation is less than the fluctuation ratio within the time window, the preset reference noise covariance matrix is ​​taken as the weight parameter of the foot end odometry factor.

[0022] Otherwise, calculate the slip index of the foot in a stable contact state, take the maximum slip index, and use the maximum slip index to linearly increase the reference noise covariance matrix. Use the increased noise covariance matrix as the weight parameter of the foot odometry factor. The weight parameter of the foot odometry factor participates in solving the optimal pose sequence of the quadruped robot.

[0023] Preferably, the slippage index is the ratio of the standard deviation to the mean of the contact force within a time window.

[0024] Preferably, the IMU pre-integration factor is represented as an error term. as follows:

[0025]

[0026] in, To obtain the relative motion increment by pre-integrating the inertial measurement unit data, for Time's up Attitude increment at any moment for Time's up The velocity increment at time, for Time's up Position increment at time, for transpose, for Time and position transpose, for Position at any given moment for The speed of time for The speed of time for Time's up The time difference between moments for Location at any given moment for Location at any given moment The gravity vector for The gyroscope is at zero bias at any given moment. for The gyroscope is at zero bias at any given moment. for The accelerometer bias at a given moment is zero. for The accelerometer shows zero bias at a given moment.

[0027] Preferably, the laser odometer factor is represented as an error term. as follows:

[0028]

[0029] in, To obtain the relative pose transformation of lidar observations through point cloud matching, For lidar observation of relative pose transformation Time's up Attitude increment at any moment For lidar observation of relative pose transformation Time's up Position increment at time, for transpose;

[0030] The foot odometer factor is represented as an error term. as follows:

[0031]

[0032] in, To calculate the relative pose transformation of the foot end using odometry, In calculating the relative pose transformation of the foot end Time's up Attitude increment at any moment In calculating the relative pose transformation of the foot end Time's up Position increment at time, for The transpose of .

[0033] Preferably, the factor graph also includes a loop closure detection factor. When the quadruped robot revisits the historical region, the loop closure detection factor is used to connect non-adjacent current state nodes with historical state nodes. The error term of the loop closure detection factor... as follows:

[0034]

[0035] in, This is the relative pose transformation obtained from lidar loop closure observations through point cloud matching. For lidar loop observation of relative pose transformation history Time to the present Attitude increment at any moment For lidar loop observation of relative pose transformation history Time to the present Position increment at time, for transpose, For history The posture of the moment For the present The posture of the moment For history Location at any given moment For the present Location at any given moment for The transpose of .

[0036] Preferably, the optimization is performed using a nonlinear optimization method, including: using the Levenberg-Marquardt algorithm, where the optimization problem is defined as minimizing the weighted sum of squares of all factors.

[0037] Preferably, the construction of the three-dimensional environmental map includes:

[0038] Apply the optimal pose sequence to the laser point cloud frame corresponding to the timestamp, and transform each laser point cloud frame to the world coordinate system.

[0039] The accumulated laser point cloud in the world coordinate system is downsampled using a voxel mesh filter, and the voxel side length is set to generate a globally consistent 3D point cloud map.

[0040] Secondly, a factor graph-based quadruped robot multi-source information fusion mapping device is provided, including a processor and a memory storing a number of computer instructions. When the computer instructions are executed by the processor, they implement the steps of the factor graph-based quadruped robot multi-source information fusion mapping method.

[0041] Compared with the prior art, the significant advantages of this invention are:

[0042] (1) The foot end odometry of the quadruped robot is introduced as an independent constraint factor into the factor graph optimization framework. In the environment where laser point cloud features are missing or degraded, it provides crucial motion constraints and fundamentally improves the robustness of the system.

[0043] (2) An adaptive noise model based on foot contact state was designed, which can dynamically adjust the confidence of foot odometer factor according to contact quality (such as whether slipping), providing strong constraints during normal walking and automatically reducing weights when slipping, effectively suppressing the pollution of the system by erroneous information.

[0044] (3) It realizes tight coupling optimization of lidar, IMU and foot kinematic information, rather than loose coupling or post-fusion, so that the information of multiple sensor sources can be deeply complementary at the state estimation level, which significantly improves the overall estimation accuracy under complex terrain and dynamic motion.

[0045] (4) It does not rely on expensive external positioning equipment, but only requires the lidar, IMU and joint encoder commonly used by quadruped robots, providing an economical and practical solution for high-precision autonomous navigation of quadruped robots. Attached Figure Description

[0046] Figure 1 This is a schematic diagram of the overall hardware configuration and data flow of the system of the present invention;

[0047] Figure 2 This is the overall flowchart of the multi-source information fusion mapping method for quadruped robots based on factor graph optimization of the present invention;

[0048] Figure 3 This is a flowchart of the foot contact state detection and odometer factor generation process of the present invention;

[0049] Figure 4 This is a schematic diagram of the tightly coupled factor graph structure constructed in this invention;

[0050] Figure 5 This is a comparison diagram of the trajectories of various schemes under the corridor degradation scenario in the experiment of this invention;

[0051] Figure 6 This is a comparison chart of the statistical results of the absolute pose error of each scheme in the corridor degradation scenario in the experiment of this invention. Detailed Implementation

[0052] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0053] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to limit the invention.

[0054] This invention provides a multi-source information fusion mapping method for quadruped robots based on factor graph optimization, which can be applied to, for example... Figure 1 The quadruped robot system shown here has the following hardware platform: a quadruped robot body platform (model Go2 selected in this embodiment), a multi-sensor data synchronization and acquisition module (based on ROS (Robot Operating System), including a solid-state LiDAR, a built-in BMI160 IMU (Inertial Measurement Unit), leg joint encoders, and foot force sensors), a multi-sensor tightly coupled state estimation and map building module, and a 3D environment point cloud map output module. All sensor data is synchronously acquired and managed through the robot operating system.

[0055] like Figure 2 As shown in this embodiment, the multi-source information fusion mapping method for quadruped robots based on factor graph optimization includes the following steps:

[0056] Step 1: Set the data acquisition frequency of each sensor of the quadruped robot, and synchronously acquire LiDAR point cloud data, inertial measurement unit data and joint encoder data.

[0057] Configure and synchronize data acquisition from various sensors: LiDAR (10Hz), IMU (100Hz), and joint encoder (100Hz). Use the message filter toolkit in the ROS system for approximate time synchronization to form a time-aligned unified data packet, with a time tolerance error set to 10ms.

[0058] Step 2: Pre-integrate the inertial measurement unit data to obtain the relative motion increment, perform motion distortion correction, feature extraction and inter-frame registration on the lidar point cloud data to obtain the relative pose transformation of lidar observation, and calculate the foot pose based on the joint encoder data through the kinematic model.

[0059] Step 2.1, IMU pre-integration.

[0060] At the key frame moments of two adjacent lidars and Between these two keyframes, high-frequency IMU data is pre-integrated to obtain the relative motion increment. The continuous-time kinematic model of the IMU is as follows:

[0061]

[0062] in, This represents the operation from a vector to a skew-symmetric matrix. , , They represent The rotation (pose) matrix (belonging to a special orthogonal group in 3D) in the world coordinate system at any given time, along with the velocity and position. The gravity vector and For IMU The gyroscope and accelerometer have zero bias at any given moment. and It is Gaussian white noise. Angular velocity measured by the IMU gyroscope for The linear acceleration measured by the IMU accelerometer at that moment. Rotation matrix The first derivative, For speed The first derivative, For position The first derivative.

[0063] The mean value integral method is used to apply the above equation to the interval Perform discrete integration to obtain the pre-integrated measurement value. , , These increments are only related to the IMU's zero bias, decoupled from the global state variables, and greatly improve optimization efficiency. The subscripts... Indicates a continuous-time variable, subscript Indicates the keyframe moment.

[0064] The pre-integration covariance is calculated using a recursive formula, which is determined by the IMU's calibration noise parameters and the pre-integration time length.

[0065] Step 2.2: Laser point cloud preprocessing and laser odometry calculation.

[0066] During a single scan cycle (100ms), the radar (e.g., the Mid360 model) will generate motion distortion in the point cloud due to the robot's own movement, which must be corrected.

[0067] Motion distortion correction: Utilizing the currently estimated body motion trajectory (initially derived from IMU integration, optimized pose is used after further optimization), assuming the robot moves at a constant speed within a single radar scan cycle, all point clouds within a frame are unified to the end of that frame. In a coordinate system. For a time... Points collected Its corrected coordinates for:

[0068]

[0069] in, It is obtained through interpolation. Time's up The transformation matrix at time step.

[0070] Feature extraction: For the corrected point cloud, the curvature of each point is calculated to extract features. For a single point in the point cloud... its local neighborhood curvature The calculation is as follows:

[0071]

[0072] in, Indicates the first The first frame of the lidar coordinate system The coordinate vector of each point For local neighborhood The base number, It is the L2 norm. Represents local neighborhood Inner The coordinate vector of each point.

[0073] Points are classified according to the magnitude of curvature: the top 20% of points with the greatest curvature are marked as sharp points, and the top 40% of points with the least curvature are marked as flat points.

[0074] Edge point matching: Using extracted edge points and planar points, the relative pose transformation of lidar observations between adjacent keyframes is estimated through inter-frame registration. This transformation will serve as the observed value for the laser odometry factor in subsequent factor map optimization. The specific registration method is as follows:

[0075] For each edge point in the current frame, search for the nearest points in the edge point set of the previous frame, fit a spatial straight line, and calculate the distance from the current point to the straight line as the residual.

[0076] Planar point matching: for the current frame Each planar point in the previous frame Search for the nearest points in the set of points on the plane, fit a spatial plane, and calculate the distance from the current point to the plane as the residual.

[0077] By minimizing the weighted sum of squares of all residuals, and using the Gauss-Newton or Levenberg-Marquardt algorithm for iterative optimization, the relative pose transformation of the lidar observation is obtained. .

[0078] Step 2.3, foot position calculation.

[0079] The leg kinematics model based on the Go2 robot (modeled using standard DH (Denavit-Hartenberg) parameters, as shown in Table 1) and real-time joint encoder readings are presented. Each foot tip is calculated using forward kinematics. In the body coordinate system The three-dimensional position below.

[0080] Table 1: DH parameters of Go2 robot legs

[0081]

[0082] The formula for calculating forward kinematics is:

[0083]

[0084] in, Lower foot end of the body coordinate system The position of the foot tip, For positive kinematic functions, For the first DH transformation matrix of each joint Let be the homogeneous coordinates of the foot, where It is expressed as follows:

[0085]

[0086] in, For the first The joint angles of each joint. For the first The torsion angle of each joint For the first The length of the link in each joint For the first Linkage offset of each joint.

[0087] Step 3: Based on the foot contact state detection results, select the foot in a stable contact state, calculate the relative pose transformation of the robot body's foot based on the foot pose, and use it as the foot odometry measurement value.

[0088] This embodiment uses foot-based odometer generation and adaptive noise modeling, such as Figure 3 As shown, the steps are as follows.

[0089] (1) Foot contact state detection: Using the three-dimensional force sensor on the bottom of the robot's foot, the contact force of each foot in the vertical direction (Z axis) is read in real time. Set a contact force threshold. When the foot satisfy Contact force within the time window The standard deviation within is less than 5% of the mean (preset ratio, can be adjusted as needed) and the velocity of the foot in the body coordinate system (Speed ​​threshold, adjustable as needed) Determine if the foot is in a stable contact state, and denote the set of indices of all feet in a stable contact state as follows: .

[0090] (2) Calculation of foot odometer measurements: Only when the number of feet in stable contact is... Only then can the relative pose transformation of the computer body be calculated. The principle is based on rigid connections. Assumption: within a short time interval... Inside, all feet in stable contact remain stationary relative to the ground. Therefore, the organism from time [time missing]... arrive The relative pose transformation can be obtained by solving an absolute orientation problem:

[0091]

[0092] in and The foot end calculated using forward kinematics exist and The position of the machine body in the coordinate system at any given time. The time from which the odometer is calculated arrive The relative pose transformation is calculated from the foot end. For the increment of the rotation matrix, This represents the translation vector increment. The optimization problem is solved using the Umeyama algorithm.

[0093] (3) Adaptive Noise Model: In this embodiment, an adaptive noise model was designed for the foot-end odometer factor. This model dynamically adjusts the confidence level (i.e., covariance matrix) of the factor based on the quality of foot contact. ( ), to deal with abnormal situations such as slipping.

[0094] When all four legs of the quadruped robot are in stable contact and the contact force fluctuation is less than 5% (the fluctuation ratio can be adjusted as needed), a lower fixed noise parameter is assigned. ,in This is the reference (or minimum) noise covariance matrix of the foot odometer factor under ideal stable contact conditions. The first three elements of the matrix correspond to the position (three degrees of freedom) in the state vector, and the last three elements correspond to the attitude (three degrees of freedom) in the state vector.

[0095] When slippage is detected in parts of the foot, the noise covariance matrix is ​​linearly increased according to the degree of slippage:

[0096]

[0097] in, This is an empirical scaling factor, set to 10 in this embodiment. The maximum slip index among all stable contact feet; when the number of feet in stable contact is less than two, the foot odometer factor is discontinued.

[0098] Slippage index calculation: Definition of the first Each foot is within the time window Internal contact force slippage index The ratio of the standard deviation to the mean of the contact force is expressed by the following formula:

[0099]

[0100] in, and They represent contact forces respectively. The standard deviation and mean of the contact force reflect the degree of fluctuation in contact force; a higher value indicates more unstable foot contact and a higher probability of slippage. However, due to the presence of actual sensor noise and minute fluctuations, it is not possible to simply use the standard deviation and mean of the contact force. A slippage detection threshold is set to determine if slippage has occurred. .when It is assumed that the foot tip has stable contact; when It is believed that there is a clear tendency for the foot to slip; After experimental calibration, the optimal value is 0.05.

[0101] When the number of feet in stable contact is less than two, the foot odometry factor is suspended because the relative pose transformation of the body cannot be reliably solved. In this embodiment, suspension means that the corresponding foot odometry factor is not constructed at the current moment, or equivalently, the weight of the factor is reset to zero so that it does not participate in the joint optimization solution.

[0102] Step 4: Based on the relative motion increment, the relative pose transformation observed by the lidar, and the foot odometry measurement, determine the error terms of adjacent state nodes in the factor graph, which serve as the IMU pre-integration factor, the lidar odometry factor, and the foot odometry factor; construct a factor graph containing the IMU pre-integration factor, the lidar odometry factor, and the foot odometry factor, and use a nonlinear optimization method to optimize and solve the problem to obtain the optimal pose sequence of the quadruped robot.

[0103] This embodiment constructs a tightly coupled factor graph optimization model, the structure of which is as follows: Figure 4 As shown. Unlike traditional loosely coupled methods (which calculate the odometer readings of each sensor independently before fusing them), tightly coupled methods directly incorporate the original observation model as a constraint into the optimization, achieving deep fusion of information at the state estimation level.

[0104] (1) Definition of state variables and factor graph: Constructing factor graph ,in: Represents the sequence of robot states to be estimated, and indicates the sequence of states from the 1st to the 2nd state contained in the current optimization window. The robot state to be estimated Each state Include: ,in For position vectors, The pose is represented by a unit quaternion. Represents a three-dimensional sphere. For velocity vectors, For IMU zero bias, Represents a three-dimensional real vector space. Factor This represents an observation model that connects nodes and provides constraints. In this embodiment, robot pose is used as a node, and laser odometry factor, IMU pre-integration factor, foot odometry factor, and loop closure detection factor are used as edges to construct a unified factor graph model.

[0105] (2) Definitions of various factors:

[0106] IMU pre-integration factor: used to connect adjacent state nodes. and Its error term for:

[0107]

[0108] in, for IMU pre-integration factor at time step To obtain the relative motion increment by pre-integrating the inertial measurement unit data, for Time's up Attitude increment at any moment for Time's up The velocity increment at time, for Time's up Position increment at time, for transpose, for Time and position transpose, for Position at any given moment for The speed of time for The speed of time for Time's up The time difference between moments for Location at any given moment for Location at any given moment The gravity vector for The gyroscope is at zero bias at any given moment. for The gyroscope is at zero bias at any given moment. for The accelerometer bias at a given moment is zero. for The accelerometer shows zero bias at a given moment.

[0109] Laser odometry factor: used to connect adjacent state nodes and Its error term for:

[0110]

[0111] in, for Laser odometry factor at time, Based on the relative transformation matrix between adjacent keyframes obtained through inter-frame registration of laser point clouds after point cloud preprocessing, the Gauss-Newton algorithm is used for iterative calculation. The final transformation matrix after iterative convergence is... ; For lidar observation of relative pose transformation Time's up Attitude increment at any moment For lidar observation of relative pose transformation Time's up Position increment at time, for The transpose of .

[0112] Covariance matrix of laser odometer factor The weighting parameters for the laser odometry factor in joint optimization are used. When the number of effective feature matches between the current laser keyframe and the previous keyframe is greater than a threshold and the registration residual is less than a preset threshold, the baseline covariance matrix is ​​used. ;in, This is the baseline covariance matrix for the laser odometry factor under normal observation conditions. Its specific value is not limited and can be preset based on the lidar measurement accuracy, statistical results of inter-frame registration errors in the point cloud, and experimental calibration results. A diagonal covariance matrix is ​​preferred. Otherwise (when feature degradation in the laser point cloud is detected, the number of effective matching points is insufficient, or the registration residual exceeds a preset threshold), the covariance matrix is ​​enlarged to... in, The preset amplification factor can be determined based on system calibration and experimental tuning results. By employing this method, a higher weight is maintained when laser observation is reliable, while the optimization weight of the laser odometry factor is reduced when laser degradation occurs, thereby improving the system's robustness under characteristic degradation environments.

[0113] Foot-end odometer factor: used to connect adjacent state nodes and Its error term for:

[0114]

[0115] The solution employs the Umeyama algorithm, which uses Singular Value Decomposition (SVD) to directly solve for the optimal transformation matrix from two corresponding 3D point clouds in a closed loop. for Foot-to-foot odometer factor at any moment The relative pose transformation is calculated using an odometry system. In calculating the relative pose transformation of the foot end Time's up Attitude increment at any moment In calculating the relative pose transformation of the foot end Time's up Position increment at time, for The transpose of .

[0116] Loop closure detection factor: When the system detects that the robot has revisited a historical region (i.e., triggered a loop), this factor is used to connect non-adjacent current state nodes. and historical state nodes Its error term has the same form as the laser odometry factor:

[0117]

[0118] in, Non-adjacent state node index pairs The loop detection factor, This is the relative pose transformation obtained from lidar loop closure observations through point cloud matching. For lidar loop observation of relative pose transformation history Time to the present Attitude increment at any moment For lidar loop observation of relative pose transformation history Time to the present Position increment at time, for transpose, For history The posture of the moment For the present The posture of the moment For history Location at any given moment For the present Location at any given moment for The transpose of .

[0119] (3) Joint optimization solution: The nonlinear optimization method adopts the Levenberg-Marquardt algorithm. The optimization problem is defined as minimizing the weighted sum of squares of all factor errors. The optimization is performed within a sliding window to balance accuracy and real-time performance.

[0120]

[0121] in, This is the optimal pose sequence for the quadruped robot. The square of the Mahalanobis distance. For Huber robust kernel function, For a set of cyclic pairs, For the effective foot odometer factor set, The weighting parameters represent the IMU pre-integration factors. The weighting parameter represents the laser odometry factor. The weighting parameter represents the loop closure detection factor. Indicates time Weighting parameters for the foot odometer factor. This represents the time index between adjacent state nodes within the sliding window; the IMU pre-integration factor and the laser odometry factor construct constraints for each adjacent state node within the window, while the foot odometry factor is only applied at time... The system is constructed only when at least two foot tips are in stable contact; therefore, its summation range is the set of effective foot odometer factors. ; Denotes the set of cyclic pairs. To detect non-adjacent state node index pairs that form a loop. The covariance matrix of the loop closure detection factor is calculated using a recursive formula, determined by the IMU's calibration noise parameters and pre-integration time. Set as a fixed diagonal matrix: Empirical fixed values ​​were used, with a position standard deviation of 0.02m and an attitude standard deviation of 0.01rad.

[0122] Foot odometry factors are constructed and added to the set only when there are at least two stable foot contacts; otherwise, the use of these factors is suspended. A sliding window method is used to limit the optimization scale, with the window size set to 20 keyframes.

[0123] Step 5: Register the LiDAR point cloud data to the world coordinate system according to the optimal pose sequence to complete the construction of the 3D environmental map.

[0124] The optimal pose sequence The process is applied to the corresponding laser point cloud frame, transforming each frame of laser point cloud to the world coordinate system; a voxel mesh filter is used to downsample the accumulated laser point cloud in the world coordinate system, the voxel side length is set, and a globally consistent 3D point cloud map is generated.

[0125] This invention addresses the issue of decreased accuracy in laser inertial odometry mapping of quadruped robots due to body shaking, foot slippage, and missing environmental features during movement. A novel solution is proposed by tightly coupling foot kinematics information into a multi-sensor fusion framework: First, data from lidar, IMU, and joint encoders are simultaneously acquired, and IMU pre-integration, laser point cloud distortion correction and feature extraction, and foot forward kinematics calculation are performed separately. Based on stable foot contact detected by plantar force sensors, the relative pose transformation of the robot body is calculated as the foot odometry measurement. An adaptive noise model is established based on real-time assessment of contact quality (e.g., slippage), dynamically adjusting the confidence weight of this measurement in the optimization process. Finally, a unified factor map is constructed, including laser point cloud observation factors, IMU pre-integration factors, foot odometry factors, and loop closure detection factors. A nonlinear optimization method is used for tightly coupled joint state estimation, thereby obtaining a high-precision robot pose sequence, which is then used to construct a 3D environmental map. This invention introduces foot-end odometry as an independent constraint factor and designs an adaptive weighting mechanism, which effectively enhances the robustness and accuracy of the system under laser degradation environment and dynamic disturbance, providing quadruped robots with accurate autonomous mapping capabilities that do not rely on external devices.

[0126] To verify the effectiveness of the method of this invention, experiments were conducted on a simulation platform configured with Ubuntu 20.04 as the underlying operating system and Noetic as the robot operating system. The simulation environment used the Gazebo physics engine to simulate the motion of the Go2 quadruped robot in a typical degenerate scenario. The sensor configuration included: a 10Hz 3D LiDAR, a 200Hz six-axis IMU, a 100Hz joint encoder, and a foot force sensor. The experiment compared the following four configuration schemes:

[0127] 1. Scheme G0 (baseline): Only use tightly coupled lidar and IMU for mapping (similar to the existing LIO-SAM scheme, derived from the literature "Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping"), without introducing foot odometry factors.

[0128] 2. Scheme G1: Based on Scheme G0, introduce a foot-based odometry factor, but do not enable degenerate sensing fusion and consistency gating (i.e., adaptive noise model), and set a fixed covariance matrix. .

[0129] 3. Scheme G2: Based on Scheme G1, a laser odometer factor is introduced, and the covariance matrix of the laser odometer factor and the foot odometer factor is dynamically adjusted.

[0130] 4. Solution G3: That is, to fully implement the method of the present invention.

[0131] The scenario is a degraded corridor environment: the corridor is a straight corridor 50m long and 3m wide, with smooth and repetitive walls on both sides. The laser point cloud shows obvious geometric degradation in the lateral translation and yaw angle directions. The robot travels back and forth along the corridor at a constant speed once. The comparison indicators are the mean APE (Absolute Pose Error), root mean square APE, maximum APE, and mean RPE (Relative Pose Error). The error statistics are shown in Table 2.

[0132] Table 2. Error statistics of each scheme in the corridor degradation scenario.

[0133]

[0134] As shown in Table 2, baseline scheme G0 exhibits severe drift in the degraded environment (maximum APE of 12.73m). Scheme G1, which introduces foot constraints, reduces the mean APE by approximately 60%. The complete method G3 of this invention further combines degradation-aware fusion and consistency gating, further reducing the mean APE by approximately 26% compared to scheme G1, and lowering the mean RPE to 0.089m, effectively suppressing local estimation fluctuations and extreme errors.

[0135] Figure 5 This is a comparison chart of the trajectories of various schemes under the scenario of corridor degradation. Figure 5 The true trajectory GT(ref) is represented by a solid line, and the estimated trajectories of schemes G0, G1, G2, and G3 are represented by lines of different colors, respectively. Figure 5 As can be seen, the estimated trajectory of scheme G0 exhibits significant lateral drift and overall misalignment in the middle and later sections, with the largest deviation from the true trajectory. The estimated trajectory of scheme G1 is closer to the true trajectory than that of scheme G0, but still exhibits jitter in local areas. The estimated trajectory of scheme G2 shows improved smoothness and reduced local abnormal offsets. The estimated trajectory of scheme G3 has the highest overlap with the true trajectory, maintaining good consistency in the straight section, turning section, and return section, without significant drift or local divergence. The above trajectory comparison results indicate that in a degraded corridor environment with a single geometric structure, the method of this invention (scheme G3), by introducing a foot-end odometry factor, a degradation perception fusion mechanism, and consistency gating, effectively suppresses the accumulation of positioning errors and significantly improves the global consistency and robustness of the estimated trajectory.

[0136] Experiments show that this invention significantly improves the accuracy and robustness of quadruped robot localization and mapping in both normal and geometrically degraded environments by constructing a factor graph containing foot odometry factors, introducing degradation detection and adaptive weight adjustment based on observability analysis, and a foot-laser consistency gating strategy. Especially in laser feature degradation scenarios such as corridors, the method of this invention can dynamically enhance foot constraints and suppress abnormal observations, demonstrating outstanding technical effectiveness.

[0137] Figure 6 The chart compares the statistical results of absolute pose errors for each scheme in a corridor degradation scenario. Scheme G0 has a mean absolute pose error of 2.538 m, a root mean square error of 3.611 m, and a maximum absolute pose error of 12.732 m. This indicates that in a corridor environment with insufficient geometric constraints, relying solely on lidar and inertial measurement units easily leads to significant cumulative drift. Scheme G1 reduces the mean absolute pose error to 1.016 m, the root mean square error to 1.258 m, and the maximum error to 4.327 m, indicating that introducing the foot-end odometry factor provides additional motion constraints and effectively suppresses global drift. Schemes G2 and G3 further optimize the error statistics. The method of this invention (Scheme G3) has a mean absolute pose error of 0.746 m, a root mean square error of 0.870 m, and a maximum absolute pose error of 2.346 m, with all error indicators being the lowest among the four schemes. Therefore, the method of the present invention significantly reduces the absolute pose error in the corridor degradation environment by integrating foot odometry factor, degradation perception adaptive adjustment and consistency gating, thereby improving positioning accuracy and robustness.

[0138] In another embodiment, the present invention also provides a quadruped robot multi-source information fusion mapping device based on factor graph optimization, including a processor and a memory storing a number of computer instructions, wherein the computer instructions are executed by the processor to implement the steps of the quadruped robot multi-source information fusion mapping method based on factor graph optimization.

[0139] The memory and processor are electrically connected directly or indirectly to enable data transmission or interaction. For example, these components can be electrically connected to each other via one or more communication buses or signal lines. The memory stores a computer program that can run on the processor, which implements the method of the present invention by running the computer program stored in the memory.

[0140] The memory may be, but is not limited to, Random Access Memory (RAM), Read Only Memory (ROM), Programmable Read-Only Memory (PROM), Erasable Programmable Read-Only Memory (EPROM), Electrically Erasable Programmable Read-Only Memory (EEPROM), etc. The memory stores the program, and the processor executes the program upon receiving an execution instruction.

[0141] The processor may be an integrated circuit chip with data processing capabilities. The aforementioned processor can be a general-purpose processor, including a Central Processing Unit (CPU), a Network Processor (NP), etc. It can implement or execute the methods, steps, and logic block diagrams disclosed in the embodiments of this invention. The general-purpose processor can be a microprocessor or any conventional processor.

[0142] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0143] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the appended claims.

Claims

1. A multi-source information fusion mapping method for quadruped robots based on factor graph optimization, characterized in that, Includes the following steps: Set the data acquisition frequency of each sensor of the quadruped robot, and synchronously collect LiDAR point cloud data, inertial measurement unit data and joint encoder data; The relative motion increment is obtained by pre-integrating the inertial measurement unit data, and the relative pose transformation of the lidar observation is obtained by performing motion distortion correction, feature extraction and inter-frame registration on the lidar point cloud data. The foot pose is calculated by kinematic model based on the joint encoder data. Based on the foot contact state detection results, the foot in a stable contact state is selected, and the relative pose transformation of the robot body's foot is calculated according to the foot pose, which is used as the foot odometry measurement value. Based on the relative motion increment, the relative pose transformation observed by lidar, and the foot odometry measurement, error terms of adjacent state nodes in the factor graph are determined as IMU pre-integration factors, lidar odometry factors, and foot odometry factors. The weighting parameters of the foot odometry factor are determined based on the foot contact state detection results. The specific process is as follows: When the number of feet in stable contact is less than 2, the weight parameter of the foot odometer factor is set to zero. Otherwise, when all four feet of the quadruped robot are in a stable contact state and the contact force fluctuation is less than the fluctuation ratio within the time window, the preset reference noise covariance matrix is ​​taken as the weight parameter of the foot end odometry factor. Otherwise, calculate the slip index of the foot in a stable contact state, take the maximum slip index, and use the maximum slip index to linearly increase the reference noise covariance matrix. Use the increased noise covariance matrix as the weight parameter of the foot odometry factor. The weight parameter of the foot odometry factor participates in solving the optimal pose sequence of the quadruped robot. A factor graph containing IMU pre-integration factors, laser odometry factors, and foot odometry factors is constructed, and a nonlinear optimization method is used to solve the optimization problem to obtain the optimal pose sequence of the quadruped robot. The lidar point cloud data is registered to the world coordinate system based on the optimal pose sequence to complete the construction of a 3D environmental map.

2. The method for multi-source information fusion mapping of quadruped robots based on factor graph optimization according to claim 1, characterized in that, The process for determining the stable contact state is as follows: The contact force of the foot in the vertical direction is detected by a three-dimensional force sensor on the sole of the foot; When the contact force is greater than the set contact force threshold, and the standard deviation of the contact force within the time window is less than the preset proportion of the mean, and the speed of the foot in the body coordinate system is less than the speed threshold, the foot is determined to be in a stable contact state. Otherwise, the foot is determined to be in a state of unstable contact.

3. The method for multi-source information fusion mapping of quadruped robots based on factor graph optimization according to claim 1, characterized in that, The slippage index is the ratio of the standard deviation to the mean of the contact force within a time window.

4. The method for multi-source information fusion mapping of quadruped robots based on factor graph optimization according to claim 1, characterized in that, The IMU pre-integration factor is represented as an error term. as follows: ； in, To obtain the relative motion increment by pre-integrating the inertial measurement unit data, for Time's up Attitude increment at any moment for Time's up The velocity increment at time, for Time's up Position increment at time, for transpose, for Time and position transpose, for Position at any given moment for The speed of time for The speed of time for Time's up The time difference between moments for Location at any given moment for Location at any given moment The gravity vector for The gyroscope is at zero bias at any given moment. for The gyroscope is at zero bias at any given moment. for The accelerometer shows zero bias at a given moment. for The accelerometer shows zero bias at a given moment.

5. The method for multi-source information fusion mapping of quadruped robots based on factor graph optimization according to claim 1, characterized in that, The laser odometer factor is represented as an error term. as follows: ； in, To obtain the relative pose transformation of lidar observations through point cloud matching, For lidar observation of relative pose transformation Time's up Attitude increment at any moment For lidar observation of relative pose transformation Time's up Position increment at time, for Transpose of; The foot odometer factor is represented as an error term. as follows: ； in, To calculate the relative pose transformation of the foot end using odometry, In calculating the relative pose transformation of the foot end Time's up Attitude increment at any moment In calculating the relative pose transformation of the foot end Time's up Position increment at time, for The transpose of .

6. The method for multi-source information fusion mapping of quadruped robots based on factor graph optimization according to claim 1, characterized in that, The factor graph also includes a loop closure detection factor. When the quadruped robot revisits the historical region, the loop closure detection factor is used to connect non-adjacent current state nodes with historical state nodes. The error term of the loop closure detection factor... as follows: ； in, This is the relative pose transformation obtained from lidar loop closure observations through point cloud matching. For lidar loop observation of relative pose transformation history Time to the present Attitude increment at any moment For lidar loop observation of relative pose transformation history Time to the present Position increment at time, for transpose, For history The posture of the moment For the present The posture of the moment For history Location at any given moment For the present Location at any given moment for The transpose of .

7. The method for multi-source information fusion mapping of quadruped robots based on factor graph optimization according to claim 1, characterized in that, The optimization solution using a nonlinear optimization method includes: employing the Levenberg-Marquardt algorithm, where the optimization problem is defined as minimizing the weighted sum of squares of all factors.

8. The method for multi-source information fusion mapping of quadruped robots based on factor graph optimization according to claim 1, characterized in that, The construction of the 3D environmental map includes: Apply the optimal pose sequence to the laser point cloud frame corresponding to the timestamp, and transform each laser point cloud frame to the world coordinate system. The accumulated laser point cloud in the world coordinate system is downsampled using a voxel mesh filter, and the voxel side length is set to generate a globally consistent 3D point cloud map.

9. A quadruped robot multi-source information fusion mapping device based on factor graph optimization, comprising a processor and a memory storing a plurality of computer instructions, characterized in that, When the computer instructions are executed by the processor, they implement the steps of the multi-source information fusion mapping method for quadruped robots based on factor graph optimization as described in any one of claims 1 to 8.

Images