Factor graph optimization-based legged robot body sensing odometer method

Abstract

The invention discloses a legged robot body sensing odometer method based on factor graph optimization, and is applied to the field of legged robot motion sensing and positioning. According to the method, the legged robot can estimate the motion state of the legged robot by means of the body sensors, the state of the legged robot is jointly estimated by means of the multiple body sensors, a body relative pose factor is obtained by means of pre-integration of a trunk inertial measurement unit (IMU), and a mixed foot inertial navigation system (INS) factor is obtained by means of pre-integration of a foot IMU. A forward kinematics model factor and a pre-integration mixed hinge contact model factor are obtained through a joint angle sensor, the sensor factors are fused under a factor graph optimization framework, and optimal motion state estimation is obtained through distance constraint. The multi-body sensor is used for sensing, and the problem that when vision and laser radar sensing of a legged robot are limited, a speedometer rapidly drifts due to single-body sensing can be solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of mobile robot navigation, and particularly relates to a method for body perception odometer of a legged robot based on factor graph optimization. Background Art

[0002] At present, the multi-source sensor fusion navigation and positioning technology based on factor graph optimization has been widely used in the field of mobile robots. For wheeled robots, IMU and wheel speedometers are usually used for body state perception, and lidar or vision methods are used for external perception, equipped with an odometer system mainly based on external perception and supplemented by body perception. With the development of technology, legged robots have great application value in logistics, military and aerospace due to their strong adaptability to extreme environments. In order to achieve the navigation and positioning function, an odometer system similar to that of wheeled robots is also equipped. Legged robots are easily affected by periodic gaits, which increases the noise of body perception sensors. When lidar and vision perception are limited, such as in environments of rain, fog, strong light, dust, etc., the state estimation of legged robots relies only on body perception, and the odometer system will have serious drift in long-term tasks. Therefore, it is necessary to propose a body perception odometer method for legged robot positioning to improve the accuracy and reliability of the odometer when external perception is limited.

[0003] CN113267181B, a method for constructing a foot odometer suitable for a legged robot, includes: numbering all legs of the legged robot and establishing a dynamic model to obtain the position vector and attitude matrix at the initial moment; during walking, obtaining the touchdown information and touchdown state of the foot end; obtaining the sensor data on the touchdown leg, and solving the position vector from the touchdown point of the foot end to the fuselage and the attitude matrix of the legged robot in the carrier coordinate system; obtaining the position vector of the touchdown point of the foot end in the world coordinate system according to the information of the gait planning module; obtaining the position vector of the fuselage in the world coordinate system and using it as the odometer output; obtaining the position vectors of all leg foot ends in the world coordinate system; when walking enters the next moment, repeating the above steps to obtain the position estimation information of the legged robot at each moment and complete the construction of the foot odometer. The present invention improves the accuracy and stability of the integrated navigation system on the legged robot.

[0004] The above patent uses joint encoders to obtain joint angle information, and establishes a kinematic model of the legged robot through the D-H modeling method, and in the attitude and the position at the previous moment being accurate and in the touchdown state, the body position at the next moment is calculated through the kinematic model. Therefore, the accuracy of the odometer output position depends on the body attitude and the position at the previous moment Precision, but during actual use, the body posture and position errors will accumulate over time.

[0005] In the present invention, an IMU is installed at the foot of the legged robot to track the foot motion state and sense the touchdown event. When the foot touches the ground, a pseudo-zero velocity observation is generated to correct the velocity state of the hybrid foot-end INS pose model and the hybrid articulated contact pose model, reducing the foot-end pose error. Then, using the joint encoder data, the foot-end pose is mapped to the body position through the kinematic model to correct the body pose measured by the body IMU, making full use of the foot contact factor. Through the factor graph optimization method, the observation factors are fused to estimate the optimal body pose, reducing the accumulation of body posture and position errors, and optimizing the observation factors generated by the foot contact factor to reduce the computational amount. Summary of the Invention

[0006] The present invention aims to solve the above problems of the prior art. A body perception odometry method for a legged robot based on factor graph optimization is proposed. The technical solution of the present invention is as follows:

[0007] A body perception odometry method for a legged robot based on factor graph optimization, which includes the following steps:

[0008] Step 1: Establish the world coordinate system {w} and the motion odometry state variable x i , install an industrial-grade IMU on the back torso of the legged robot, and establish the IMU coordinate system of the torso to coincide with the robot body coordinate system as the coordinate system {b}. Install a consumer-grade IMU on the leg link of the robot in contact with the ground, and establish the coordinate system {c} at the foot end in contact with the ground;

[0009] Step 2: Collect body perception data, and use the strapdown inertial navigation algorithm and the forward kinematics algorithm to obtain the attitude, velocity, and position states of the legged robot; at the same time, according to the data of the previous key frame and the current key frame, perform pre-integration calculation to obtain the body relative pose factor, the hybrid foot INS pose factor, and the hybrid articulated contact model pose factor;

[0010] Step 3: Delete the key frame node that was added earliest in the graph optimization node, add the current key frame state to the graph optimization node, connect the unary forward kinematic pose factor and the distance constraint factor to the new node, and connect the binary body relative pose factor, the hybrid foot INS pose factor, and the hybrid articulated contact model pose factor between the previous key frame node and the new node, and then perform optimal estimation on the states of each node.

[0011] Further, in Step 1, the IMU sensor coordinate system and the motion odometry optimization state variable set χ are established, specifically:

[0012] Establish the 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 position. The consumer-grade IMU is installed at the end-link of the foot, and a coordinate system {c} is established at the foot tip. Then, according to the installation position of the foot IMU, the coordinate system of the foot IMU is mapped to the foot tip coordinate system. The installation of the foot IMU can also be extended to the foot tip positions of all legs of the legged robot as needed. The number of foot IMUs installed is L;

[0013] The factor graph optimization node set is denoted as:

[0014]

[0015]

[0016] Among them, χ m is m key frame nodes to be optimized, and K m represents the set of all key frames to be optimized. respectively represent the position, velocity, and attitude matrix of the body at time i in the world coordinate system. respectively represent the position, velocity, and attitude matrix of the foot tip at time i in the world coordinate system, where L ∈ {1....., N}, and N represents the number of foot IMUs installed. respectively represent the biases of the accelerometer and gyroscope of the body IMU;

[0017] The system state variable error is denoted as:

[0018]

[0019] Among them, is the body attitude matrix corresponding to the Euler angles, is the foot tip attitude matrix corresponding to the Euler angles, is the attitude matrix of the foot tip relative to the body obtained by forward kinematic solution, is the position of the foot tip relative to the body obtained by forward kinematic solution, and δ is the error notation;

[0020] The system input noise is denoted as:

[0021]

[0022] Among them is the input noise of the body IMU, is the input noise of the foot tip IMU, is the velocity noise at the touchdown moment of the foot INS pose factor, is the velocity noise at the touchdown moment of the foot hybrid hinge contact model pose factor, is the joint encoder angle noise, both obeying Gaussian distribution.

[0023] Furthermore, the hybrid foot INS factor in step 2 relies on the foot IMU for foot end pose perception, and the hybrid articulated contact model pose factor relies on the combination of the foot IMU and the joint encoder for foot end pose perception. A binary foot end pose perception factor is established between two adjacent optimized key frames. The foot end pose perception factor is generated by the pre-integration method. During the pre-integration interval [i, j], multiple touchdown events will occur. The touchdown event occurrence time k of the foot is expressed as k ∈ T. The first time from swing to touchdown is expressed as The time from touchdown to swing is expressed as The last time from swing to touchdown is expressed as The time from touchdown to swing is expressed as

[0024] Furthermore, the foot touchdown event of the foot end pose perception specifically includes:

[0025] According to the output data of the foot IMU, the generalized likelihood ratio test method GLRT is used to divide the swing time and touchdown time of the legged robot's foot. The touchdown time interval detection method model is:

[0026]

[0027] Among them, and are the noise variances of the accelerometer and gyroscope; and are the acceleration and angular velocity information at time k; W is the sampling window width; n is the detection serial number; is the mean specific force in the sampling window; C is the GLRT detection value; Let C be the touchdown time threshold, compare the GLRT detection value with the threshold, and when C < γ, judge this interval as the touchdown time interval.

[0028] Furthermore, the specific steps for calculating the hybrid foot INS factor in step 2 are as follows:

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

[0030]

[0031] Among them are the position residual and attitude residual of this factor;

[0032] The touchdown times from key frame i to j are recorded by the set T, and the hybrid foot end INS pose model Η 1 ,

[0033]

[0034] Among them, are the foot-end speed, the foot-end attitude matrix, and the foot-end position respectively, are the acceleration and angular velocity measured by the foot-end IMU, and g w is the gravitational acceleration in the world coordinate system, and Δt represents the sampling time of the IMU; is Gaussian noise with a mean of 0.

[0035] The hybrid foot INS factor residual is specifically:

[0036]

[0037] Among them, r ΔθL,ij is the hybrid foot INS factor residual; are the changes in the true foot-end position and the true attitude matrix within the interval [i, j] respectively, and R L,i , R L,j are the estimated foot-end attitude matrices at times i and j respectively, p L,i , p L,j are the estimated foot-end positions at times i and j respectively, and Δp L,ij is the estimated change in the foot-end position integrated within the interval [i, j], are the total lengths of the intervals respectively;

[0038] The noise transfer of this factor is:

[0039] [(δp L,ij ) T (δθ L,ij ) T T ~N(0,Ω L )(22)

[0040]

[0041] Among them, Ω L is the covariance of the noise of this factor, and δp L,ij , δθ L,ij are the cumulative position and attitude noises of the foot-end INS pose model Η 1 within the interval [i, j] respectively, and J(·) is

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

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

[0044] ​

[0045] wherein are the position residual and attitude residual of this factor;

[0046] The touchdown moments from key frame i to j are recorded by set T, and the hybrid point-foot articulated contact model Η is obtained 2 ,

[0047]

[0048] wherein d k is the arm value from the touchdown point to the position of the foot-end IMU, is the angular velocity of the foot end, is the attitude matrix of the fuselage in the world coordinate system, κ is the distance from the foot contact point to the {c} system, and φ k is the joint angle measured by the encoder, respectively represent the attitude matrix and position of the foot relative to the fuselage deduced by forward kinematics using encoder data;

[0049] The pose factor residual of the hybrid articulated contact model is specifically:

[0050]

[0051] The noise transfer of this factor is:

[0052]

[0053]

[0054] wherein, L t has the same meaning as L, and the subscript is used to distinguish model Η 1 from model Η 2 state, is the covariance of the noise of this factor. represents the Gaussian noise of the foot-end acceleration at time j-1, represents the noise error accumulated by the attitude from [i, j-1].

[0055] Furthermore, the distance constraint factor described in step three is specifically:

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

[0057]

[0058] The noise transfer of this factor is:

[0059]

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

[0061]

[0062] Among them, r 0 is the prior factor residual, and r b,ij is the body pose factor residual, and r L,ij is the hybrid foot INS pose factor residual, the hybrid articulated contact model pose factor residual, is the foot-end body distance constraint factor residual, is the forward kinematics pose factor residual, and Ω is the covariance matrix of sensor error transmission in the residual.

[0063] An electronic device includes a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, it implements the leg-type robot body perception odometer method based on factor graph optimization as described in any one of the above.

[0064] A non-transitory computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, it implements the leg-type robot body perception odometer method based on factor graph optimization as described in any one of the above.

[0065] The advantages and beneficial effects of the present invention are as follows:

[0066] The present invention first installs an IMU on the foot of the leg-type robot to establish a foot INS to track the foot movement. Under the factor graph optimization-based multi-sensor fusion framework, the observation factors generated by the optimized foot contact factors are used to correct the foot state, and the foot state is associated with the body state through the kinematic relationship of the leg-type robot joints to form a distance constraint observation factor to correct the body state, thereby improving the accuracy of the body perception odometer. At the same time, the generation of the optimized key frames is not generated by high-frequency touchdown events but by the observation factors of the slowest sensor in the system, ensuring the real-time performance of the odometer. Description of the Drawings

[0067] Figure 1 is a schematic structural diagram of a preferred embodiment provided by the present invention;

[0068] Figure 2 is a schematic diagram of the sensor installation position and sensor coordinates;

[0069] Figure 3 is a schematic diagram of the foot touchdown timing;

[0070] Figure 4 Schematic diagram of the foot touchdown movement. Detailed Embodiments

[0071] The technical solutions in the embodiments of the present invention will be clearly and detailedly described below with reference to the accompanying drawings in the embodiments of the present invention. The described embodiments are only a part of the embodiments of the present invention.

[0072] The technical solution of the present invention to solve the above technical problem is:

[0073] The technical solution of the present invention to solve the above technical problem is to refer to Figure 1 as shown, to construct a robust body perception odometer system for a legged robot, and optimize the observation factors generated by the foot contact factors, including the following steps:

[0074] Step 1: Establish the world coordinate system {w} and the motion odometer state variable x i , install an industrial-grade IMU on the back torso of the legged robot, and establish the body IMU coordinate system to coincide with the robot body coordinate system as the coordinate system {b}. Install a consumer-grade IMU on the leg link of the robot in contact with the ground, and establish the coordinate system {c} at the foot end in contact with the ground.

[0075] Step 2: Collect body perception data, and use the strapdown inertial navigation algorithm and the forward kinematics algorithm to obtain the states of the legged robot such as attitude, speed, and position. At the same time, according to the data between the previous key frame and the current key frame, perform pre-integration calculations to obtain the body relative pose factor, the hybrid foot INS pose factor, and the hybrid articulated contact model pose factor.

[0076] Step 3: Delete the earliest added key frame node in the graph optimization node, add the current key frame state to the graph optimization node, connect the unary forward kinematics pose factor and the distance constraint factor to the new node, and connect the binary body relative pose factor, the hybrid foot INS pose factor, and the hybrid articulated contact model pose factor between the previous key frame node and the new node, and then perform an optimal estimation of the states of each node.

[0077] Furthermore, for the installation of the sensors and the establishment of the coordinate system in Step 1, refer to Figure 2 as shown, B1 is the installation position of the body IMU, L1, L2, L3, L4 are the installation positions of the foot end IMUs, K1, K2, K3, K4 are the moving legs of the legged robot, each leg has 3 degrees of freedom, and there are 3 joint encoders. S1 is the world coordinate system {w}, S2 is the robot body coordinate system {b}, and S3, S4, S5, S6 are the foot end IMU mapping coordinate systems {c}. The state variables used in this method are constructed as follows:

[0078]

[0079]

[0080] where χm are m key frame nodes to be optimized, K m represents the set of all key frames to be optimized, respectively represent the position, velocity, and attitude matrix of the body in the world coordinate system at time i, respectively represent the position, velocity, and attitude matrix of the foot end point in the world coordinate system at time i, where L ∈ {1....., N}, and N represents the number of foot IMU installations, respectively represent the biases of the body IMU accelerometer and gyroscope.

[0081] The system state variable error is denoted as:

[0082]

[0083] where, is the body attitude matrix corresponding to the Euler angles, is the foot end attitude matrix corresponding Euler angles, is the attitude matrix of the foot end relative to the body obtained by forward kinematic solution, is the position of the foot end relative to the body obtained by forward kinematic solution, and δ is the error notation.

[0084] The system input noise is denoted as:

[0085]

[0086] where is the input noise of the torso IMU, is the input noise of the foot end IMU, is the velocity noise at the touchdown moment of the foot INS pose factor, is the velocity noise at the touchdown moment of the foot hybrid articulated contact model pose factor, is the joint encoder angle noise, all of which follow a Gaussian distribution.

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

[0088]

[0089] The noise transfer of this factor is:

[0090]

[0091] Furthermore, the forward kinematic factor in Step 3 is a prior art 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 unary factor, and the residual is:

[0092]

[0093] The noise transfer of this factor is:

[0094]

[0095]

[0096] where is a mapping function regarding the attitude and pose of each joint of the legged robot.

[0097] Furthermore, the specific process of collecting body data in Step 2 is as follows: four IMUs at the foot end and the torso IMU are connected to the USB expansion port of the legged robot through serial communication. While the internal computer of the robot collects inertial data, it also collects the angle and angular velocity data of 12 joint encoders. The sampling rate is 200Hz for all, and the data is stored in the buffer.

[0098] Furthermore, the hybrid foot INS factor in Step 2 relies on the foot IMU to sense the foot end pose, and the hybrid articulated contact model pose factor relies on the combination of the foot IMU and the joint encoder to sense the foot end pose. When external sensors such as vision and lidar are added to the system, the optimization key frame is generated by the sensor with the slowest update frequency in the system, and the generation time of the optimization key frame is not fixed. To add foot end pose perception, the traditional method takes the foot contact moment as the key frame. When the legged robot has multiple feet on the ground, a large number of optimization key frames will be generated, which will greatly reduce the optimization efficiency. By optimizing the observation factor generated by the foot contact factor, the optimization key frame of the system can 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, for the sake of optimization efficiency, key frames are generated at a fixed time interval of every 1.8s to improve the optimization efficiency.

[0099] A binary foot end pose perception factor is established between two adjacent optimization key frames. The timing diagram is referred to Figure 3As shown, S7 is the IMU data time sequence, S8 is the joint motor time sequence, S9 is the support time sequence, and S10 is the contact and swing phase relationship. The foot-end pose perception factor is generated by the pre-integration method. During the pre-integration interval [i, j], multiple touchdown events may occur. The touchdown event occurrence time k of the foot can be expressed as k ∈ T. The first time from swing to touchdown is expressed as The time from touchdown to swing is expressed as The last time from swing to touchdown is expressed as The time from touchdown to swing is expressed as

[0100] Furthermore, in order to sense the touchdown moment, generally, legged robots are equipped with pressure sensors at the foot end. However, due to long-term contact with complex ground, these pressure sensors are extremely prone to wear and their sensitivity decreases. Since the foot IMU can track the acceleration and angular velocity of the foot movement, the foot-end touchdown can be assisted in detection based on the output data of the foot IMU. The generalized likelihood ratio test (GLRT) is used to divide the swing moment and touchdown moment of the foot movement of the legged robot. The touchdown moment detection method model is:

[0101]

[0102] Among them, and are the noise variances of the accelerometer and gyroscope; and are the specific force and angular velocity information at time k; W is the sampling window width; n is the detection serial number; is the mean value of the specific force in the sampling window; C is the GLRT detection value; Let C be the touchdown moment threshold. Compare the GLRT detection value with the threshold. When C < γ, this interval is judged as the touchdown moment interval.

[0103] Furthermore, in the second step, the hybrid foot INS factor is mentioned. This factor is a binary factor, and the residual is:

[0104]

[0105] The touchdown moments from key frame i to j are recorded by the set T, and the hybrid foot-end INS pose model Η 1 is obtained, and the speed of the hybrid foot INS factor is corrected.

[0106]

[0107] The attitude residual of the hybrid foot INS factor is similar to that of the body pose factor. Specifically:

[0108]

[0109] In Figure 3 the shown contact interval, the speed of this factor is made zero. However, since there may be slippage of the foot, white noise with a mean of zero is used to replace the zero speed. The specific speed of the foot-end INS is as follows:

[0110]

[0111] The speed noise transfer of the foot-end INS is specifically as follows:

[0112]

[0113] After the speed correction in the contact interval, the displacement residual of this factor is specifically as follows:

[0114]

[0115] Among them, are respectively the change amounts of the true position of the foot-end and the true attitude matrix integrated within the [i, j] interval, is the total time length of the interval.

[0116] The noise transfer of this factor is:

[0117] [(δp L,ij ) T (δθ L,ij ) T T ~N(0,Ω L )(22)

[0118]

[0119] Among them, Ω L is the covariance of the noise of this factor, δp L,ij , δθ L,ij are respectively the position and attitude cumulative noises of the foot-end INS pose model Η 1 within the [i, j] interval, and J(·) is

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

[0121]

[0122] Among them are the position residual and attitude residual of this factor;

[0123] Figure 4 ​As shown, when the foot of the legged robot contacts the ground and there is no slip at the foot end point, the connecting rod rotates around this point. At this time, the leg speed is not strictly zero, but there is a linear velocity generated by the rotation, and this linear velocity will affect the foot end displacement. In this model, positive kinematic factors are added to jointly estimate the foot end attitude instead of relying only on the foot end gyroscope to solve the attitude, and the hybrid point-foot articulated contact model Η 2 , specifically:

[0124]

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

[0126]

[0127] The pose factor attitude residual of the hybrid articulated contact model, specifically:

[0128]

[0129] The pose factor velocity of the hybrid articulated contact model, specifically:

[0130]

[0131] Among them, κ is the distance from the foot contact point to the {c} system, and φ k is the joint angle measured by the encoder, respectively represent the attitude matrix and position of the foot relative to the body calculated by positive kinematics using encoder data.

[0132]

[0133] Velocity noise transfer, specifically:

[0134]

[0135] The pose factor position residual of the hybrid articulated contact model, specifically:

[0136]

[0137]

[0138] The noise transfer of this factor is:

[0139]

[0140] Further, the distance constraint factor in step three is a unary factor, specifically:

[0141]

[0142] The foot end position is mapped to the body position through the body attitude, obtaining:

[0143]

[0144] The noise transfer of this factor is:

[0145]

[0146] Further, the establishment of the optimal estimation problem described in step three, as Figure 1 shown, is formulated specifically as:

[0147]

[0148] where r 0 is the prior factor residual, r b,ij is the body pose factor residual, r L,ij is the hybrid foot INS pose factor residual, the hybrid articulated contact model pose factor residual, is the foot end body distance constraint factor residual, is the forward kinematics pose factor residual, and Ω is the covariance matrix of the sensor error transfer in the residual.

[0149] Further, Ω, the covariance matrix of the residual, is jointly determined by the original noise through noise transfer and state error. Here, taking as an example, the noise transfer of this factor is formula (35) and can be written in the following form:

[0150]

[0151] where ζ t,j is the error variable of this factor, the initial value can be 0, and Ω η is determined by the variance.

[0152] Further, for Figure 1 the graph optimization problem constructed, essentially solving the non - linear least - squares problem of formula (40). In this example, the open - source g2o framework based on graph optimization is used for solving. According to the error transfer and residuals of the above factors, the non - linear least - squares optimization is constructed using the node structure and edge structure defined by g2o to achieve state marginalization and batch smoothing estimation.

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

[0154] It should also be noted that the term "including", "comprising" or any other variant thereof is intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements not only includes those elements, but also includes other elements not expressly listed, or elements inherent to such process, method, commodity or device. Without further limitation, an element defined by the statement "including one..." does not exclude the existence of another identical element in the process, method, commodity or device including the said element.

[0155] The above embodiments should be understood as being only used to illustrate the present invention and not to limit the protection scope of the present invention. After reading the content described in the present invention, those skilled in the art can make various changes 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 proprioceptive odometer method for a legged robot based on factor graph optimization, characterized in that: The following steps are involved: Step 1: Establish the world coordinate system {w} and the motion odometer state variable x i , an industrial-grade IMU is installed on the back trunk of the legged robot, and the trunk IMU coordinate system coincides with the robot body coordinate system to establish a coordinate system {b}, and a consumer-grade IMU is installed on the leg link of the robot that contacts the ground, and a coordinate system {c} is established at the foot end that contacts the ground; Step 2: Collect proprioceptive data, and use the 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 data of the key frame at the previous moment and the key frame at the current moment, perform pre-integration calculation to obtain the relative posture factor of the body, the hybrid foot INS posture factor and the hybrid articulated contact model posture factor; Step 3: Delete the earliest keyframe node added in the graph optimization node, add the current keyframe state to the graph optimization node, connect the one-dimensional positive kinematics degree pose factor and distance constraint factor on the new node, connect the two-dimensional body relative pose factor, hybrid foot INS pose factor, hybrid articulated contact model pose factor between the previous keyframe node and the new node, and then make the optimal estimate of each node state.

2. A method for proprioceptive odometer of a legged robot based on factor graph optimization according to claim 1, characterized in that: The step 1 establishes the IMU sensor coordinate system and the motion odometer optimization state variable set x, specifically: Establish a world coordinate system {w}, install the industrial-grade IMU at the center of gravity of the robot's trunk, establish a coordinate system {b} at the center of gravity, install the consumer-grade IMU at the foot-end connecting rod, establish a coordinate system {c} at the foot-end point, and then map the foot IMU's coordinate system to the foot-end point coordinate system according to the installation position of the foot IMU. The foot IMU installation can also be extended to the foot-end positions of all legs of the legged robot as needed. The number of foot IMU installations is L; The factor graph optimization node set is recorded as: Among them, χ m are m key frame nodes to be optimized, K m Represents the set of all key frames to be optimized. Respectively represent the position, velocity, and posture matrix of the body in the world coordinate system at time i, Respectively represent the position, velocity, and attitude matrix of the foot endpoint in the world coordinate system at time i, where L∈{1.....,N}, N represents the number of foot IMUs installed, Represent the zero bias of the IMU accelerometer and gyroscope of the body respectively; The system state variable error is recorded as: in, is the body posture matrix Corresponding to the Euler angle, is the foot end posture matrix The corresponding Euler angles, Solve the posture matrix of the foot end relative to the body for positive kinematics, is the position of the foot relative to the body in the forward kinematic solution, and δ is the error sign; The system input noise is recorded as: in is the body IMU input noise, is the foot-end IMU input noise, is the velocity noise of the foot INS posture factor at the moment of touching the ground, is the velocity noise of the foot hybrid articulated contact model posture factor at the touchdown moment, is the angle noise of the joint encoder, which obeys Gaussian distribution.

3. The method for proprioceptive odometer of a legged robot based on factor graph optimization according to claim 2 is characterized in that: The hybrid foot INS factor of step 2 relies on the foot IMU to perceive the foot-end posture, and the hybrid articulated contact model posture factor relies on the foot IMU and the shutdown encoder to perceive the foot-end posture. A binary foot-end posture perception factor is established between two adjacent optimized key frames. The foot-end posture perception factor is generated by a pre-integration method. In the pre-integration interval [i, j], multiple touchdown events will occur. The moment k when the foot touchdown event occurs is represented by k∈T, and the first transition from swing to touchdown is represented by T1 1 , the transition from touchdown to swing is expressed as The last transition from swing to contact is denoted as T1 n , the transition from touchdown to swing is expressed as 4. The method for proprioceptive odometer of a legged robot based on factor graph optimization according to claim 3 is characterized in that: The foot end posture sensing foot contact event specifically includes: According to the output data of the foot IMU, the generalized likelihood ratio test method GLRT is used to divide the legged robot foot movement swing moment and the ground contact moment. The ground contact moment interval detection method model is: in, and is the noise variance of the accelerometer and gyroscope; and is the acceleration and angular velocity information at time k; W is the sampling window width; n is the detection sequence number; is the mean value of the specific force in the sampling window; C is the GLRT detection value; let C be the touchdown time threshold, compare the GLRT detection value with the threshold, when C<γ, the interval is judged as the touchdown time interval.

5. The method for proprioceptive odometer of a legged robot based on factor graph optimization according to claim 4 is characterized in that: The step 2 of calculating the mixed foot INS factor is as follows: This factor is a binary factor, and the residual is: in are the position residual and attitude residual of the factor; The touchdown time from key frame i to j is recorded by set T, and the hybrid foot-end INS pose model H1 is obtained. in, are the foot end velocity, foot end posture matrix and foot end position respectively, is the acceleration and angular velocity measured by the foot IMU, g w is the gravitational acceleration in the world coordinate system, Δt represents the sampling time of IMU; is Gaussian noise with a mean of 0; The residual of the mixed foot INS factor is: in, is the residual of the mixed foot INS factor; are the changes of the foot end true value position and true value posture matrix in the interval [i, j], R L,i , R L,j are the foot posture matrices estimated at time i and j, respectively, and p L,i , p L,j are the estimated foot positions at time i and j, Δp L,ij is the estimated foot position change integrated in the interval [i,j], They are respectively the interval [i,T1 1 ] total length of time; The factor noise transfer is: Among them, Ω L is the covariance of the factor noise, δp L,ij ,δθ L,ij are the position and posture cumulative noise of the foot-end INS pose model H1 in the interval [i, j], and J(·) is 6. The method for proprioceptive odometer of a legged robot based on factor graph optimization according to claim 5, characterized in that: The hybrid articulated contact model pose factor of step 2 is specifically: This factor is a binary factor, and the residual is: in are the position residual and attitude residual of the factor; The touchdown time from key frame i to j is recorded by set T, and the hybrid point-foot articulated contact model H2 is obtained. in, d k is the arm value from the touchdown point to the IMU position at the foot end, is the foot end angular velocity, The posture matrix of the body in the world coordinate system, κ is the distance from the foot contact point to the {c} system, φ k is the joint angle measured by the encoder, They represent the posture matrix and position of the foot relative to the body calculated using the forward kinematics of the encoder data; The pose factor residual of the mixed articulated contact model is: The factor noise transfer is: Among them, L t Same meaning as L, the subscript is used to distinguish the state of model H1 and model H2. is the covariance of the factor noise, It is represented by the foot acceleration Gaussian noise at time j-1, It is expressed as the accumulated noise error of [i,j-1] posture.

7. The method for proprioceptive odometer of a legged robot based on factor graph optimization according to claim 6 is characterized in that: The distance constraint factor described in step 3 is specifically: This factor is a univariate factor, and the residual is: The factor noise transfer is:

8. The method for proprioceptive odometer of a legged robot based on factor graph optimization according to claim 7, characterized in that: The step 3 of establishing the optimal estimation problem is specifically as follows: Among them, r0 is the prior factor residual, r b,ij is the residual error of the body posture factor, r L,ij is the residual of the mixed foot INS pose factor, r Lt,ij Mixed articulated contact model pose factor residual, is the residual of the foot-end body distance constraint factor, is the residual of the positive kinematic degree pose factor, and Ω is the covariance matrix of the sensor error transmission in the residual.

9. An electronic device, characterized in that: It includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the legged robot body perception odometer method based on factor graph optimization as described in any one of claims 1 to 8 is implemented.

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

Images