A legged robot body perception odometry method based on factor graph optimization

Abstract

The application claims a kind of leg robot body perception odometry method based on factor graph optimization, applied to leg robot motion perception and positioning field.Leg robot can realize the estimation of self motion state using body sensor, the application uses multiple body sensors to jointly estimate self state, obtains body relative pose factor by pre-integration of trunk inertial measurement unit (IMU), obtains hybrid foot inertial navigation system (INS) factor by pre-integration of foot IMU, obtains positive kinematics model factor and pre-integrated hybrid hinged contact model factor using joint angle sensor, fuses sensor factors under factor graph optimization framework and obtains optimal motion state estimation by distance constraint.The application uses multiple body sensors for perception, which can solve the problem of rapid drift of odometry caused by single body perception when vision and laser radar perception of leg robot are limited.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot navigation technology, and specifically relates to a method for proprioception odometry of legged robots based on factor graph optimization. Background Technology

[0002] Currently, multi-source sensor fusion navigation and localization technology based on factor graph optimization has wide applications in the field of mobile robots. For wheeled robots, IMUs and wheel velocimeters are typically used for proprioception, while LiDAR or vision methods are used for external perception, equipped with an odometry system that primarily relies on external perception and secondarily on proprioception. With the development of technology, legged robots, due to their strong adaptability to extreme environments, have great application value in logistics, military, and aerospace. To achieve navigation and localization functions, they are also equipped with odometry systems similar to those used in wheeled robots. Legged robots are susceptible to the influence of periodic gait, which increases the noise of proprioception sensors. When LiDAR and vision perception are limited, such as in environments with rain, fog, strong light, or dust, the state estimation of legged robots relies solely on proprioception, leading to severe drift in the odometry system during long-term missions. Therefore, it is necessary to propose a proprioception-based odometry method for legged robot localization to improve the accuracy and reliability of odometry when external perception is limited.

[0003] CN113267181B discloses a method for constructing a legged odometry system for legged robots. The method includes: numbering all legs of the legged robot and establishing a dynamic model; obtaining the initial position vector and posture matrix; during walking, acquiring the ground contact information and ground contact state of the feet; acquiring sensor data from the ground-contacting legs; solving for the position vector of the foot contact point to the robot body in the carrier coordinate system and the posture matrix of the legged robot; obtaining the position vector of the foot contact point in the world coordinate system based on gait planning module information; calculating the position vector of the robot body in the world coordinate system and outputting it as the odometry; obtaining the position vectors of all leg feet in the world coordinate system; repeating the above steps when walking enters the next moment, thereby obtaining the position estimation information of the legged robot at each moment and completing the construction of the legged odometry system. This invention improves the accuracy and stability of the integrated navigation system on a legged robot.

[0004] The aforementioned patent utilizes joint encoders to obtain joint angle information and establishes a kinematic model of the legged robot using the DH modeling method, thus enabling the robot to perform various actions in the legged robot's posture. and the position r of the previous moment b w Under accurate conditions, and while in a ground-contact state, the robot's position at the next moment is calculated using a kinematic model. Therefore, the accuracy of the odometry output position depends on the body posture calculated by the legged robot's body sensors. and the position r of the previous moment b w Accuracy, but in actual use, the body posture and position r b w Errors accumulate over time.

[0005] This invention integrates an IMU (Installation Unit) on the foot of a legged robot to track foot motion and sense ground contact events. When the foot touches the ground, a pseudo-zero velocity observation is generated to correct the velocity state of the hybrid foot-end INS (Inertial Measurement Unit) pose model and the hybrid articulated contact pose model, reducing foot pose errors. Then, using joint encoder data, a kinematic model is used to map the foot pose to the body position, correcting the body pose measured by the IMU, thus fully utilizing foot contact factors. A factor graph optimization method is used to fuse observed factors to estimate the optimal body pose, reducing the accumulation of body posture and position errors, and optimizing the observed factors generated by foot contact factors to reduce computational load. Summary of the Invention

[0006] This invention aims to solve the problems of the prior art. It proposes a method for proprioceptive odometry of legged robots based on factor graph optimization. The technical solution of this invention is as follows:

[0007] A method for proprioceptive odometry of a legged robot based on factor graph optimization includes the following steps:

[0008] Step 1: Establish the world coordinate system {w} and the motion odometry state variable x i An industrial-grade IMU is mounted on the back of the legged robot, and the coordinate system of the torso IMU is established as coordinate system {b}, which coincides with the coordinate system of the robot body. A consumer-grade IMU is mounted on the leg link of the robot that is in contact with the ground, and a coordinate system {c} is established at the foot end that is in contact with the ground.

[0009] Step 2: Collect body perception data and use strapdown inertial navigation algorithm and forward kinematics algorithm to obtain the posture, velocity and position state of the legged robot; at the same time, based on the keyframe data of the previous time step and the keyframe data of the current time step, perform pre-integration calculation to obtain the body relative pose factor, hybrid foot INS pose factor and hybrid articulated contact model pose factor.

[0010] Step 3: Delete the earliest keyframe node added to the graph optimization node, add the current keyframe state to the graph optimization node, connect the univariate positive motion pose factor and distance constraint factor to the new node, connect the binary body relative pose factor, hybrid foot INS pose factor, and hybrid articulated contact model pose factor between the previous keyframe node and the new node, and then perform optimal estimation of the state of each node.

[0011] Furthermore, step one, establishing the IMU sensor coordinate system and the optimized state variable set χ of the motion odometry, specifically involves:

[0012] Establish a world coordinate system {w}. The industrial-grade IMU is installed at the center of gravity of the robot's torso. A coordinate system {b} is established at the center of gravity. The consumer-grade IMU is installed at the foot link. A coordinate system {c} is established at the foot tip. Then, the coordinate system of the foot IMU is mapped to the coordinate system of the foot tip according to the installation position of the foot IMU. The installation of foot IMU can also be extended to the foot tip of all legs of the legged robot as needed. The number of foot IMUs installed is L.

[0013] The set of optimization nodes in the factor graph is denoted as:

[0014]

[0015] Where, χ m For m keyframe nodes to be optimized, K m This represents the set of all keyframes to be optimized. These represent the position, velocity, and attitude matrix of the machine at time i in the world coordinate system. Let L represent the position, velocity, and attitude matrix of the foot endpoint in the world coordinate system at time i, respectively, where L∈{1.....,N} and N represents the number of foot IMUs installed. These represent the zero bias of the machine's IMU accelerometer and gyroscope, respectively;

[0016] The error of the system state variable is denoted as:

[0017]

[0018] in, For the body attitude matrix Corresponding Euler angles Foot posture matrix Corresponding Euler angles To solve the positive kinematics for the attitude matrix of the foot relative to the body, For positive kinematics calculation of the foot position relative to the body, δ is the error notation;

[0019] The system input noise is denoted as:

[0020]

[0021] in Input noise to the IMU of the machine. Input noise to the foot-mounted IMU. The foot INS pose factor is the velocity noise at the moment of ground contact. The pose factor of the foot in the hybrid articulated contact model is the velocity noise at the moment of ground contact. The angular noise of the joint encoder all follow a Gaussian distribution.

[0022] Furthermore, in step two, the hybrid foot INS factor relies on the foot IMU for foot pose perception, and the hybrid articulated contact model pose factor relies on the foot IMU and the shutdown encoder for foot pose perception. A binary foot pose perception factor is established between two adjacent optimization keyframes. The foot pose perception factor is generated by a pre-integration method. Within the pre-integration interval [i,j], multiple ground contact events will occur. The time k of the foot ground contact event is denoted as k∈T, and the time of the first transition from swinging to ground contact is denoted as T1. 1 The moment of transition from ground contact to oscillation is represented as The last moment of transition from swing to ground is denoted as T1. n The moment of transition from ground contact to oscillation is represented as

[0023] Furthermore, the foot pose sensing foot contact event specifically includes:

[0024] Based on the foot IMU output data, the generalized likelihood ratio (GLRT) detection method is used to divide the foot movement of the legged robot into swinging moments and ground contact moments. The ground contact moment interval detection model is as follows:

[0025]

[0026] in, and For the noise variance of the accelerometer and gyroscope; and The acceleration and angular velocity information at time k; W is the sampling window width; n is the detection sequence number; γ is the average force within the sampling window; C is the GLRT detection value; let C be the ground contact time threshold. Compare the GLRT detection value with the threshold. When C < γ, the interval is determined to be the ground contact time interval.

[0027] Furthermore, step two, calculating the mixed foot INS factor, specifically involves the following steps:

[0028] This factor is a binary factor, and the residual is:

[0029]

[0030] in These are the position and attitude residuals of the factor;

[0031] The ground contact times from keyframes i to j are recorded by set T, resulting in the hybrid foot end INS pose model H1.

[0032]

[0033] in, These are foot velocity, foot attitude matrix, and foot position, respectively. For the acceleration and angular velocity measured by the foot IMU, g w Let Δt represent the gravitational acceleration in the world coordinate system, and Δt represent the sampling time of the IMU. It is Gaussian noise with a mean of 0.

[0034] The mixed foot INS factor residuals are as follows:

[0035]

[0036] in, For the mixed foot INS factor residuals; R represents the change in the true foot position and true pose matrix within the interval [i,j], respectively. L,i R L,j The foot pose matrices estimated at times i and j are p. L,i p L,j The estimated foot position at times i and j, Δp L,ij The integral within the interval [i,j] is used to estimate the change in foot position. They are the intervals [i, T1] 1 The total time;

[0037] The noise propagation of this factor is as follows:

[0038] [(δp L,ij ) T (δθ L,ij ) T ] T ~N(0,Ω) L ) (twenty two)

[0039]

[0040] Among them, Ω L Let δp be the covariance of the noise factor. L,ij δθ L,ij The cumulative noise of the foot INS pose model H1 in the interval [i,j] is the position and pose of the foot. J(·) is the cumulative noise of the foot INS pose model H1.

[0041] Furthermore, the pose factor of the hybrid articulated contact model in step two is specifically as follows:

[0042] This factor is a binary factor, and the residual is:

[0043]

[0044] in These are the position and attitude residuals of the factor;

[0045] The ground contact times from keyframes i to j are recorded by set T, resulting in the hybrid point foot articulated contact model H2.

[0046]

[0047] in, d k The arm value at the point of contact with the foot-end IMU position, The foot angular velocity, The attitude matrix of the machine in the world coordinate system, where κ is the distance from the foot contact point to the {c} frame, and φ is the position matrix. k The joint angle measured by the encoder. These represent the attitude matrix and position of the foot relative to the body, respectively, calculated using the encoder data and forward kinematics.

[0048] The pose factor residuals of the hybrid articulated contact model are as follows:

[0049]

[0050] The noise propagation of this factor is as follows:

[0051]

[0052]

[0053] Among them, L t Similar to the meaning of L, the subscript is used to distinguish between the states of model H1 and model H2. The covariance of the noise factor. The foot acceleration at time j-1 is represented by Gaussian noise. It is represented as the noise error accumulated in the [i,j-1] pose.

[0054] Furthermore, the distance constraint factor mentioned in step three is specifically as follows:

[0055] This factor is a unary factor, and the residual is:

[0056]

[0057] The noise propagation of this factor is as follows:

[0058]

[0059] Furthermore, the establishment of the optimal estimation problem in step three is specifically as follows:

[0060]

[0061] Where r0 is the prior factor residual, r b,ij For the residual of the body pose factor, r L,ij To mix the foot INS pose factor residuals, Pose factor residuals of hybrid articulated contact model The residual of the foot-to-body distance constraint factor. Let Ω be the positive motion posture factor residual, and Ω be the covariance matrix of the sensor error propagation in the residual.

[0062] An electronic device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the program, implements the legged robot body perception odometry method based on factor graph optimization as described in any one of the claims.

[0063] A non-transitory computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the legged robot body perception odometry method based on factor graph optimization as described in any one of the claims.

[0064] The advantages and beneficial effects of this invention are as follows:

[0065] This invention first installs an IMU on the foot of a legged robot to establish a foot INS and track foot movement. Under the multi-sensor fusion framework optimized by factor graph, the foot state is corrected by the observation factors generated by the optimized foot contact factors. Furthermore, the foot state is correlated with the body state through the kinematic relationship of the legged robot joints to form a distance-constrained observation factor to correct the body state, thereby improving the accuracy of the body-aware odometry. At the same time, the generation of keyframes is optimized so that they are not generated by high-frequency ground contact events but by the observation factors of the slowest sensor in the system, ensuring the real-time performance of the odometry. Attached Figure Description

[0066] Figure 1 This is a schematic diagram of a preferred embodiment of the present invention;

[0067] Figure 2 This is a schematic diagram showing the sensor installation location and sensor coordinates;

[0068] Figure 3 This is a schematic diagram of the foot contact sequence.

[0069] Figure 4 A diagram illustrating foot contact with the ground. Detailed Implementation

[0070] The technical solutions of the embodiments of the present invention will be clearly and thoroughly described below with reference to the accompanying drawings. The described embodiments are merely some embodiments of the present invention.

[0071] The technical solution of the present invention to solve the above-mentioned technical problems is:

[0072] The technical solution of the present invention to solve the above-mentioned technical problems is based on... Figure 1 As shown, a robust proprioceptive odometry system for legged robots is constructed, optimizing the observation factors generated by foot contact factors, including the following steps:

[0073] Step 1: Establish the world coordinate system {w} and the motion odometry state variable x i An industrial-grade IMU is mounted on the back of the legged robot's torso, and the torso IMU coordinate system is established as coordinate system {b}, which coincides with the robot's body coordinate system. A consumer-grade IMU is mounted on the leg link of the robot that is in contact with the ground, and a coordinate system {c} is established at the foot end that is in contact with the ground.

[0074] Step 2: Collect body perception data and use strapdown inertial navigation and forward kinematics algorithms to obtain the attitude, velocity, and position of the legged robot. Simultaneously, based on the data between the previous and current keyframes, perform pre-integration calculations to obtain the body's relative pose factor, the hybrid foot INS pose factor, and the hybrid articulated contact model pose factor.

[0075] Step 3: Delete the earliest keyframe node added to the graph optimization node, add the current keyframe state to the graph optimization node, connect the univariate positive motion pose factor and distance constraint factor to the new node, connect the binary body relative pose factor, hybrid foot INS pose factor, and hybrid articulated contact model pose factor between the previous keyframe node and the new node, and then perform optimal estimation of the state of each node.

[0076] Furthermore, the installation of the sensor and the establishment of the coordinate system in step one are referred to... Figure 2 As shown, B1 is the torso IMU mounting position, L1, L2, L3, and L4 are the foot IMU mounting positions, and K1, K2, K3, and K4 are the legs of the legged robot. Each leg has 3 degrees of freedom and 3 joint encoders. S1 is the world coordinate system {w}, S2 is the robot body coordinate system {b}, and S3, S4, S5, and S6 are the foot IMU mapping coordinate systems {c}. The state variables used in this method are as follows:

[0077]

[0078] Where, χ m For m keyframe nodes to be optimized, K mThis represents the set of all keyframes to be optimized. These represent the position, velocity, and attitude matrix of the machine at time i in the world coordinate system. Let L represent the position, velocity, and attitude matrix of the foot endpoint in the world coordinate system at time i, respectively, where L∈{1.....,N} and N represents the number of foot IMUs installed. These represent the zero bias of the machine's IMU accelerometer and gyroscope, respectively.

[0079] The error of the system state variable is denoted as:

[0080]

[0081] in, For the body attitude matrix Corresponding Euler angles Foot posture matrix Corresponding Euler angles To solve the positive kinematics for the attitude matrix of the foot relative to the body, The position of the foot relative to the body is calculated using positive kinematics, and δ is the error notation.

[0082] The system input noise is denoted as:

[0083]

[0084] in Input noise to the torso IMU. Input noise to the foot-mounted IMU. The foot INS pose factor is the velocity noise at the moment of ground contact. The pose factor of the foot in the hybrid articulated contact model is the velocity noise at the moment of ground contact. The angular noise of the joint encoder all follow a Gaussian distribution.

[0085] Furthermore, the body pose factor in step two is a prior art technique in the field of inertial navigation. Referring to the paper "On-Manifold Preintegration for Real-Time Visual-Inertial Odometry", this factor is a binary factor, and the residual is:

[0086]

[0087] The noise propagation of this factor is as follows:

[0088]

[0089] Furthermore, the positive kinematic factor in step three is a prior art technique in the field of legged robot control. Referring to the paper "Legged Robot State-Estimation Through Combined Forward Kinematic and Preintegrated Contact Factors", this factor is a univariate factor, and the residual is:

[0090]

[0091] The noise propagation of this factor is as follows:

[0092]

[0093]

[0094] in, It is a mapping function for the posture and position of each joint of the legged robot.

[0095] Furthermore, in step two, the data acquisition of the body specifically involves connecting the four IMUs at the foot and the torso IMU to the USB expansion port of the legged robot via serial communication. The internal computer of the robot acquires inertial data while also acquiring angle and angular velocity data from the 12 joint encoders. The sampling rate is 200Hz, and the data is stored in the cache.

[0096] Furthermore, in step two, the hybrid foot INS factor relies on the foot IMU for foot pose perception, while the hybrid articulated contact model pose factor relies on the foot IMU and the shutdown encoder for combined foot pose perception. When external sensing such as vision and LiDAR is added to the system, the optimization keyframes are generated by the sensor with the slowest update frequency in the system, and the generation time of the optimization keyframes is not fixed. In order to incorporate foot pose perception, traditional methods use the foot contact moment as the keyframe. When the legged robot has multiple feet touching the ground, a large number of optimization keyframes will be generated, which will greatly reduce the optimization efficiency. Optimizing the observation factor generated by the foot contact factor allows the system's optimization keyframes to be generated by the sensor with the slowest update frequency in the system. In this example, sensors with slower update frequencies such as vision and LiDAR are not included. Therefore, to improve optimization efficiency, a keyframe is generated every 1.8 seconds to improve optimization efficiency.

[0097] Establish a binary foot pose awareness factor between two adjacent optimization keyframes, as shown in the timing diagram. Figure 3As shown, S7 is the IMU data timing, S8 is the joint motor timing, S9 is the support timing, and S10 is the contact and swing phase relationship. The foot pose sensing factor is generated through pre-integration. Within the pre-integration interval [i,j], multiple ground contact events may occur. The time k of the foot ground contact event can be represented as k∈T, and the time of the first transition from swing to ground contact is represented as T1. 1 The moment of transition from ground contact to oscillation is represented as The last moment of transition from swing to ground is denoted as T1. n The moment of transition from ground contact to oscillation is represented as

[0098] Furthermore, to detect the moment of ground contact, legged robots are typically equipped with pressure sensors at the foot. However, these sensors are prone to wear and tear due to prolonged contact with complex ground surfaces, leading to decreased sensitivity. Since foot-mounted IMUs can track the acceleration and angular velocity of foot movements, the output data can be used to assist in detecting foot ground contact. The Generalized Likelihood Ratio Test (GLRT) method is used to distinguish between the swinging moment and the ground contact moment of the legged robot's foot movement. The ground contact moment detection model is as follows:

[0099]

[0100] in, and For the noise variance of the accelerometer and gyroscope; and The specific force and angular velocity information at time k; W is the sampling window width; n is the detection sequence number; γ is the average force within the sampling window; C is the GLRT detection value; let C be the ground contact time threshold. Compare the GLRT detection value with the threshold. When C < γ, the interval is determined to be the ground contact time interval.

[0101] Furthermore, in step two, the foot INS factor is mixed. This factor is a binary factor, and the residual is:

[0102]

[0103] The ground contact times from keyframes i to j are recorded by set T, resulting in a hybrid foot INS pose model H1, which is then corrected for the velocity of the hybrid foot INS factors.

[0104]

[0105] The mixed foot INS factor posture residual is similar to the body pose factor posture residual, specifically:

[0106]

[0107] exist Figure 3 The contact interval shown is used to reduce the velocity of this factor to zero. However, since slippage may occur at the foot, zero velocity is replaced with white noise with zero mean. The specific velocity of the foot tip INS is:

[0108]

[0109] The velocity noise transmission of the foot-end INS is as follows:

[0110]

[0111] After velocity correction in the contact region, the displacement residual of this factor is specifically as follows:

[0112]

[0113] in, Let be the changes in the true foot position and true pose matrix, respectively, within the interval [i,j]. For [i,T1] 1 The total length of time in the interval.

[0114] The noise propagation of this factor is as follows:

[0115] [(δp L,ij ) T (δθ L,ij ) T ] T ~N(0,Ω) L ) (twenty two)

[0116]

[0117] Among them, Ω L The covariance of the noise factor, δp L,ij δθ L,ij The cumulative noise of the foot INS pose model H1 in the interval [i,j] is the position and pose of the foot. J(·) is the cumulative noise of the foot INS pose model H1.

[0118] Furthermore, in step two, the pose factor of the hybrid articulated contact model is a binary factor, and the residual is:

[0119]

[0120] in These are the position and attitude residuals of the factor;

[0121] Figure 4As shown, when the legged robot's foot contacts the ground, without slippage at the foot tip, the linkage rotates around this point. At this time, the leg velocity is not strictly zero, but rather possesses a linear velocity generated by the rotation, which affects the foot tip displacement. This model incorporates positive kinematics factors to jointly estimate the foot tip posture instead of relying solely on the foot tip gyroscope for posture calculation. The hybrid point-foot articulated contact model H2 is as follows:

[0122]

[0123] Caused by encoder noise An error is generated, which is related to the number of joints and does not change significantly over time. Therefore, the influence of encoder noise on foot pose estimation is ignored here, and can be expressed by the following formula:

[0124]

[0125] The pose factor and attitude residual of the hybrid articulated contact model are as follows:

[0126]

[0127] The pose factor velocity of the hybrid articulated contact model is as follows:

[0128]

[0129] Where κ is the distance from the foot contact point to the {c} system, φ k The joint angle measured by the encoder. These represent the foot's posture matrix and position relative to the body, respectively, calculated using the encoder data and forward kinematics.

[0130]

[0131] Velocity noise transmission, specifically:

[0132]

[0133] The position residuals of the pose factor in the hybrid articulated contact model are as follows:

[0134]

[0135]

[0136] The noise propagation of this factor is as follows:

[0137]

[0138] Furthermore, the distance constraint factor in step three is a unary factor, specifically:

[0139]

[0140] The foot position is mapped to the body position through the body attitude, resulting in:

[0141]

[0142] The noise propagation of this factor is as follows:

[0143] δd L,i ~N(0,Ω) d (38)

[0144]

[0145] Furthermore, the optimal estimation problem described in step three, such as... Figure 1 As shown, the problem can be formalized as follows:

[0146]

[0147] Where r0 is the prior factor residual, r b,ij For the residual of the body pose factor, r L,ij To mix the foot INS pose factor residuals, Pose factor residuals of hybrid articulated contact model The residual of the foot-to-body distance constraint factor. Let Ω be the positive motion posture factor residual, and Ω be the covariance matrix of the sensor error propagation in the residual.

[0148] Furthermore, Ω is the covariance matrix of the residuals, which is determined by the original noise after noise propagation and state error. Here, we take... For example, the noise propagation factor is represented by formula (35), which can be written in the following form:

[0149]

[0150] Where, ζ t,j This is the error variable for the factor. The initial value can be 0, Ω η Depend on Variance determines.

[0151] Furthermore, for Figure 1Solving the constructed graph optimization problem is essentially solving the nonlinear least squares problem of formula (40). In this example, the open-source g2o framework based on graph optimization is used to solve the problem. Based on the error propagation and residuals of the above factors, the nonlinear least squares optimization is constructed using the node structure and edge structure defined by g2o to achieve state marginalization and batch smooth estimation.

[0152] The systems, devices, modules, or units described in the above embodiments can be implemented by computer chips or entities, or by products with certain functions.

[0153] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0154] The above embodiments should be understood as illustrative only and not as limiting the scope of protection of the present invention. After reading the description of the present invention, those skilled in the art can make various alterations or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

Claims

1. A method for proprioceptive odometry of a legged robot based on factor graph optimization, characterized in that, Includes the following steps: Step 1: Establish the world coordinate system {w} and the motion odometry state variable x i An industrial-grade IMU is mounted on the back of the legged robot, and the coordinate system of the torso IMU is established as coordinate system {b}, which coincides with the coordinate system of the robot body. A consumer-grade IMU is mounted on the leg link of the robot that is in contact with the ground, and a coordinate system {c} is established at the foot end that is in contact with the ground. Step 2: Collect body perception data and use strapdown inertial navigation algorithm and forward kinematics algorithm to obtain the posture, velocity and position state of the legged robot; at the same time, based on the keyframe data of the previous time step and the keyframe data of the current time step, perform pre-integration calculation to obtain the body relative pose factor, hybrid foot INS pose factor and hybrid articulated contact model pose factor. Step 3: Delete the earliest keyframe node added to the graph optimization node, add the current keyframe state to the graph optimization node, connect the univariate positive motion pose factor and distance constraint factor to the new node, connect the binary body relative pose factor, hybrid foot INS pose factor, and hybrid articulated contact model pose factor between the previous keyframe node and the new node, and then perform optimal estimation of the state of each node.

2. The method for proprioceptive odometry of a legged robot based on factor graph optimization according to claim 1, characterized in that, Step one, establishing the IMU sensor coordinate system and the optimized state variable set χ of the motion odometry, specifically involves: Establish a world coordinate system {w}. The industrial-grade IMU is installed at the center of gravity of the robot's torso. A coordinate system {b} is established at the center of gravity. The consumer-grade IMU is installed at the foot link. A coordinate system {c} is established at the foot tip. Then, the coordinate system of the foot IMU is mapped to the coordinate system of the foot tip according to the installation position of the foot IMU. The installation of foot IMUs is extended to the foot tips of all legs of the legged robot. The number of foot IMUs installed is L. The set of optimization nodes in the factor graph is denoted as: Where, χ m For m keyframe nodes to be optimized, K m This represents the set of all keyframes to be optimized. These represent the position, velocity, and attitude matrix of the machine at time i in the world coordinate system. Let L represent the position, velocity, and attitude matrix of the foot endpoint in the world coordinate system at time i, respectively, where L∈{1.....,N} and N represents the number of foot IMUs installed. These represent the zero bias of the in-flight IMU accelerometer and gyroscope, respectively. The error of the system state variable is denoted as: in, For the body attitude matrix Corresponding Euler angles Foot posture matrix Corresponding Euler angles To solve the positive kinematics for the attitude matrix of the foot relative to the body, For positive kinematics calculation of the foot position relative to the body, δ is the error notation; The system input noise is denoted as: in Input noise to the IMU of the machine. Input noise to the foot-mounted IMU. The foot INS pose factor is the velocity noise at the moment of ground contact. The pose factor of the foot in the hybrid articulated contact model is the velocity noise at the moment of ground contact. The angular noise of the joint encoder all follow a Gaussian distribution.

3. The method for proprioceptive odometry of a legged robot based on factor graph optimization according to claim 2, characterized in that, In step two, the hybrid foot INS factor relies on the foot IMU for foot pose perception, while the hybrid articulated contact model pose factor relies on the foot IMU and the shutdown encoder for combined foot pose perception. A binary foot pose perception factor is established between two adjacent optimization keyframes. The foot pose perception factor is generated through pre-integration. Within the pre-integration interval [i,j], multiple ground contact events will occur. The time k of the foot ground contact event is denoted as k∈T, and the time of the first transition from swinging to ground contact is denoted as... The moment of transition from ground contact to oscillation is represented as The last moment of transition from swing to ground is represented as The moment of transition from ground contact to oscillation is represented as 4. The method for proprioceptive odometry of a legged robot based on factor graph optimization according to claim 3, characterized in that, The foot pose sensing foot ground contact event specifically includes: Based on the foot IMU output data, the generalized likelihood ratio (GLRT) detection method is used to divide the foot movement of the legged robot into swinging moments and ground contact moments. The ground contact moment interval detection model is as follows: in, and For the noise variance of the accelerometer and gyroscope; and The acceleration and angular velocity information at time k; W is the sampling window width; n is the detection sequence number; γ is the average force within the sampling window; C is the GLRT detection value; let C be the ground contact time threshold. Compare the GLRT detection value with the threshold. When C < γ, the interval is determined to be the ground contact time interval.

5. The method for proprioceptive odometry of a legged robot based on factor graph optimization according to claim 4, characterized in that, Step two, which calculates the mixed foot INS factor, involves the following steps: This factor is a binary factor, and the residual is: in These are the position and attitude residuals of the factor; The ground contact times from keyframes i to j are recorded by set T, resulting in the hybrid foot end INS pose model H1. in, These are foot velocity, foot attitude matrix, and foot position, respectively. For the acceleration and angular velocity measured by the foot IMU, g w Let Δt represent the gravitational acceleration in the world coordinate system, and Δt represent the sampling time of the IMU. It is Gaussian noise with a mean of 0; The mixed foot INS factor residuals are as follows: in, For the mixed foot INS factor residuals; R represents the change in the true foot position and true pose matrix within the interval [i,j], respectively. L,i R L,j The foot pose matrices estimated at times i and j are p. L,i p L,j The estimated foot position at times i and j, respectively, Δp L,ij The integral within the interval [i,j] is used to estimate the change in foot position. They are intervals The total time is long; The noise propagation of this factor is as follows: Among them, Ω L Let δp be the covariance of the noise factor. L,ij δθ L,ij The cumulative noise of the foot INS pose model H1 in the interval [i,j] is the position and pose of the foot. J(·) is the cumulative noise of the foot INS pose model H1.

6. The method for proprioceptive odometry of a legged robot based on factor graph optimization according to claim 5, characterized in that, The pose factor of the hybrid articulated contact model in step two is specifically as follows: This factor is a binary factor, and the residual is: in These are the position and attitude residuals of the factor; The ground contact times from keyframes i to j are recorded by set T, resulting in the hybrid point foot articulated contact model H2. in, d k =-κ·n / ||n||, d k The arm value at the point of contact with the foot-end IMU position, The foot angular velocity, The attitude matrix of the machine in the world coordinate system, where κ is the distance from the foot contact point to the {c} frame, and φ is the position matrix. k The joint angle measured by the encoder. These represent the attitude matrix and position of the foot relative to the body, respectively, calculated using the encoder data and forward kinematics. The pose factor residuals of the hybrid articulated contact model are as follows: The noise propagation of this factor is as follows: Among them, L t Similar to L, the subscript is used to distinguish between the states of model H1 and model H2, Ω. Lt The covariance of the noise factor. The foot acceleration at time j-1 is represented by Gaussian noise. It is represented as the noise error accumulated in the [i,j-1] pose.

7. The method for proprioceptive odometry of a legged robot based on factor graph optimization according to claim 6, characterized in that, The distance constraint factor mentioned in step three is specifically as follows: This factor is a unary factor, and the residual is: The noise propagation of this factor is as follows:

8. The method for proprioceptive odometry of a legged robot based on factor graph optimization according to claim 7, characterized in that, The third step, establishing the optimal estimation problem, specifically involves: Where r0 is the prior factor residual, r b,ij For the residual of the body pose factor, r L,ij To mix the foot INS pose factor residuals, Pose factor residuals of hybrid articulated contact model The residual of the foot-to-body distance constraint factor. Let Ω be the positive motion posture factor residual, and Ω be the covariance matrix of the sensor error propagation in the residual.

9. An electronic device, characterized in that, The method includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the program, implements the legged robot body perception odometry method based on factor graph optimization as described in any one of claims 1 to 8.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the legged robot body perception odometry method based on factor graph optimization as described in any one of claims 1 to 8.

Images