Biped robot state estimation method based on multi-sensor information

Abstract

The invention belongs to the technical field of robots, and discloses a biped robot state estimation method based on multi-sensor information, and the method comprises the steps: obtaining the data of an inertial measurement unit, a joint encoder and a joint sensor on a biped robot; a self-adaptive threshold contact state detection algorithm based on current sensing information is designed, and the contact state of the sole and the environment is judged in real time; determining a supporting phase of the robot according to the contact state, and applying a dynamic reference system conversion mechanism of supporting phase perception; constructing a state estimator based on invariant extended Kalman filter IEKF, defining a state vector including the posture, position and speed of the robot, performing state prediction by using IMU data, and performing state updating in combination with contact state information, kinematics information and the like; and outputting accurately estimated robot state information. According to the method, the adaptive contact detection and the IEKF are introduced, so that the state estimation precision and robustness of the biped robot in the dynamic walking process are effectively improved.

Description

Technical Field

[0001] The invention belongs to the technical field of robots, and in particular relates to a biped robot state estimation method based on multi-sensor information. Background Art

[0002] Bipedal robots, due to their anthropomorphic structure and potential for mobility in complex terrain, have become a hot topic in robotics research. To achieve stable and flexible walking control, bipedal robots must be able to accurately perceive their motion state, including their position, posture, linear velocity, and angular velocity in space. The state estimation system is a key component of bipedal robot control systems, and its performance directly impacts the effectiveness of higher-level control algorithms and the robot's overall motion performance.

[0003] However, state estimation for bipedal robots faces numerous challenges. First, bipedal robots have complex structures, high degrees of freedom, and highly nonlinear dynamic models. Second, during dynamic walking, the robot experiences intense motion, potentially subject to slippage and impact. Furthermore, the sensors onboard the robot (such as the inertial measurement unit (IMU), joint encoders, and force sensors) inherently exhibit noise, drift, and latency, making information from a single sensor insufficient to provide a complete and accurate state.

[0004] Traditional state estimation methods for bipedal robots typically rely on techniques such as the extended Kalman filter (EKF) or the unscented Kalman filter (UKF), fusing information from IMUs, joint encoders, and contact sensors. However, the EKF can introduce significant errors during linearization and is sensitive to initial states. Furthermore, fixed threshold methods are often used to determine contact states, which can lead to misjudgments when the robot's gait changes or interacts with different surfaces, compromising estimation accuracy. Furthermore, long-term integration estimation based on a fixed world coordinate system is prone to cumulative errors, resulting in position and attitude drift. The invariant extended Kalman filter (IEKF), a filtering method defined on Lie groups, exhibits superior performance when processing variables with special algebraic structures, such as rotations (attitudes), and can overcome some of the shortcomings of the traditional EKF. However, its application to bipedal robots still requires effective integration with accurate contact detection and kinematic models.

[0005] Therefore, how to effectively fuse multi-source sensor information, design a robust contact detection mechanism, and use advanced filtering algorithms to overcome the limitations of traditional methods to achieve accurate and robust state estimation of bipedal robots during complex dynamic walking is a technical problem that needs to be urgently solved in this field. Summary of the Invention

[0006] The purpose of the present invention is to provide a bipedal robot state estimation method based on multi-sensor information to solve the technical problems existing in existing bipedal robot state estimation methods, such as low accuracy, insufficient robustness to dynamic changes and sensor noise, misjudgment of contact status, and easy accumulation of errors.

[0007] To solve the above technical problems, the specific technical solutions of the biped robot state estimation method based on multi-sensor information of the present invention are as follows:

[0008] A method for estimating the state of a bipedal robot based on multi-sensor information comprises the following steps:

[0009] Step S1: Acquire sensor data: collect data from various sensors installed on the biped robot in real time;

[0010] Step S2: Adaptive contact state detection: using the current sensor data obtained in step S1, design and execute an adaptive threshold contact detection algorithm;

[0011] Step S3: Support phase determination and reference frame processing: Based on the contact state output in step S2, the current support phase of the bipedal robot is determined. Based on the support phase information, a dynamic reference frame conversion mechanism is applied to establish the estimated reference frame on the current supporting leg.

[0012] Step S4: State estimation based on IEKF: construct an invariant extended Kalman filter (IEKF) to estimate the robot state;

[0013] Step S5: Output state estimation result: output the robot state information obtained after IEKF filtering.

[0014] Furthermore, the various sensor data collected in step S1 at least include: IMU, encoders for each joint, and a current sensor installed at the knee joint.

[0015] Furthermore, step S2 includes the following steps:

[0016] The specific calculation formula for the foot contact probability is as follows:

[0017] P contact =w τ ·P τ +w v ·P v +w a ·P a

[0018] Among them, P τ 、P v and P a They represent the contact probability calculated based on current, velocity and acceleration characteristics, respectively, and wτ 、w v and w a is the corresponding weight coefficient, satisfying w τ +w v +w a =1, when the calculated contact probability P contact When it is greater than 0.8, the system determines that the foot is in contact with the ground. The calculation method of each feature probability is as follows:

[0019] Torque characteristic probability:

[0020] Joint torque is usually positively correlated with joint current. The torque characteristic probability is calculated by mapping the current value to the joint torque, comparing the joint torque with the adaptive threshold, and using the Sigmoid function to calculate the probability value:

[0021]

[0022] Where τ is the joint torque, τ threshold is the adaptive torque threshold, α τ is the sensitivity coefficient.

[0023] Velocity characteristic probability: The robot calculates the horizontal velocity of the end of the swinging leg through forward kinematics to obtain its contact situation, and maps it to an exponential function for calculation:

[0024]

[0025] Among them, v horizontal is the horizontal velocity vector of the foot end, β v is the speed sensitivity coefficient;

[0026] Acceleration shock characteristic probability: The acceleration at the foot end is calculated using the acceleration feedback from the joint encoder, and the characteristic probability of the acceleration shock is finally calculated:

[0027]

[0028] where a diff is the difference vector of acceleration at adjacent moments, β a is the acceleration sensitivity parameter;

[0029] Torque threshold τ threshold It is dynamically adjusted according to the robot's current posture and joint angles:

[0030] τ threshold =τ base ·S posture (1+k θ ·|θ knee |)·f(h)

[0031] Among them, τ base is the basic threshold, S posture is the attitude-related scaling factor, k θ is the knee joint angle influence coefficient, θ knee is the knee joint angle, and f(h) is the height-related adjustment function. For the standing posture, Sposture takes a value of 0.8, and for the standard half-squat state, S posture The value is 2.0, and the other transition postures are 1.0. The height adjustment function f(h) is defined as:

[0032]

[0033] Where h is the current height, h nominal is the nominal height, h min and h max are the minimum and maximum heights set respectively; in the specific implementation, the system dynamically updates the relevant parameters according to the currently recognized posture state, including torque threshold, speed threshold, impact threshold, smoothing factor and sensitivity parameter α τ , β a , β a .

[0034] Furthermore, step S3 includes the following steps:

[0035] The choice of reference coordinate system R depends on the support state of the robot. Let p L and p R Respectively represent the positions of the left and right foot ends in the world coordinate system, and the origin of the reference coordinate system p ref The definition is as follows:

[0036]

[0037] The orientation of the reference coordinate system is defined by three orthogonal unit vectors: the z-axis and the z Aligned vertically with the world coordinate system, the x-axis e x Point to the direction the robot is heading.

[0038]

[0039] After normalization, it is used as the x-axis direction and the y-axis direction. y Determined by the right-hand rule, the rotation matrix R from the reference coordinate system to the world coordinate system is ref Expressed as:

[0040] R ref =[e x e y e z ]

[0041] In this reference coordinate system, the robot's position state p and velocity state v can be obtained by the following transformations, which convert the position and velocity in the world coordinate system into the reference coordinate system,

[0042] Position transformation:

[0043]

[0044] Speed ​​​​change:

[0045]

[0046] Where: p com is the center of mass position calculated by forward kinematics in the world coordinate system, v com is the center of mass position calculated by forward kinematics in the world coordinate system, p ref is the position of the reference point in the world coordinate system, It is the transpose of the rotation matrix from the reference coordinate system to the world coordinate system, which represents the rotation matrix from the world coordinate system to the reference coordinate system. When the support phase of the robot switches, the position and velocity states need to be transformed. Assuming that the reference coordinate systems before and after the switch are R1 and R2 respectively, the state conversion is expressed as:

[0047] Position state transition:

[0048]

[0049] Speed ​​state transition:

[0050]

[0051] The system periodically updates the reference coordinate system definition based on the latest support state and foot position. The complete update process is as follows:

[0052] 6. Obtain the latest contact status and contact probability;

[0053] 7. Determine the new reference coordinate system definition based on the support phase;

[0054] 8. Calculate the transformation relationship between the new and old reference coordinate systems;

[0055] 9. Verify the validity of the new reference coordinate system;

[0056] 10. Perform coordinate system transformation and update state estimate.

[0057] Furthermore, step S4 includes the following steps:

[0058] Step S4.1: Building a state space model of the bipedal robot;

[0059] Step S4.2: IEKF prediction and update strategy design;

[0060] Step S4.3: Noise covariance adaptive adjustment strategy.

[0061] Furthermore, the step S4.1 includes the following steps:

[0062] The complete state vector is defined as follows:

[0063]

[0064] in:

[0065] R t ∈SO(3): rotation matrix; v t ∈R 3 : velocity vector; P t ∈R 3 : position vector; ω t ∈R 3 : true angular velocity vector; α t ∈R 3 : true acceleration vector; Accelerometer bias; gyroscope bias;

[0066] This state vector is compactly represented as a combination of elements on the SE(3) group and Euclidean space vectors:

[0067] X=[T,v,ω,a,b g ,b a ] T

[0068] in is the pose transformation matrix of the base;

[0069] S4.1.1: System Dynamics:

[0070] The continuous-time dynamic equation considering the IMU measurement model is adopted:

[0071]

[0072]

[0073] in:

[0074] g=[0,0,-9.81] T is the gravitational acceleration vector; a m and ω m are the acceleration and angular velocity measured by the IMU respectively; and These two equations describe the noise characteristics of the angular velocity and acceleration measured by the IMU. The true angular velocity ω in the state vector t and the true acceleration α t The evolution of and It is reflected in the equation of , which is combined with the bias term. The equation uses the angular velocity (ω m -b g -n g ), The equation uses the acceleration (a m -b a -n a );

[0075] [ω m -b g -n g ] × The antisymmetric matrix representing the angular velocity, n g 、n a 、n ω 、n bg 、n ba Represent the process noise of the corresponding state quantity respectively. Combining the above dynamic equations, we can get the complete state space expression:

[0076]

[0077] In actual implementation, the above continuous-time dynamics model needs to be discretized. Considering the system sampling period Δt, the evolution of the system can be represented by the following discrete-time model:

[0078]

[0079] in Represents the exponential mapping operation on the Lie group, ensuring that the discretized state still lies on the Lie group manifold, thereby maintaining the geometric integrity of the system. In actual implementation, it is necessary to discretize the continuous-time dynamics equations and use the improved Euler method for discretization:

[0080]

[0081] R k+1 =R k exp([ω m,k -b g,k ] × Δt)

[0082] v k+1 =v k +(g+R k (am,k -b a,k ))Δt

[0083] ω k+1 =ω k +w ω,k

[0084] a k+1 =a k +w a,k

[0085] b g,k+1 =b g,k

[0086] b a,k+1 =b a,k

[0087] where Δt is the sampling time interval, exp(·) represents the exponential map on SO(3);

[0088] S4.1.2: Error state definition:

[0089] The bias error of the IMU is expressed as:

[0090]

[0091] The state error at each moment is represented by a tuple, which includes the right invariant error and the general error, and corresponding measures are taken to reduce the error:

[0092]

[0093] The choice of the right invariant error reflects the geometry of the system. This error definition ensures the consistency of the estimator on the SE(3) group and makes the error dynamics independent of the current state, although the standard right invariant pose error only contains rotation errors on SE(3) and position error But since the state vector X t Also includes the speed v t and angular velocity ω k , so the state error η t Defined as a system that includes rotation error, position error, and speed error and angular velocity error The matrix form of the right invariant error η t And the general error is of the form:

[0094]

[0095] The specific definition of each error component in the right invariant error is:

[0096] Rotation matrix error:

[0097]

[0098] Position error:

[0099]

[0100] Speed ​​error:

[0101]

[0102] Angular velocity error:

[0103]

[0104] The steps for processing the right invariant error are to first calculate the right invariant error based on the state prediction at time k-1 and the measurement at time k, then combine the errors into a complete residual vector, calculate the correction amount through the Kalman gain, and finally apply the correction to the state prediction at time k, where exp(·) represents the matrix exponential mapping,

[0105]

[0106] Correction of general errors is carried out through the original parameters of the sensor and subsequent adaptive parameter adjustment of the bipedal robot.

[0107] Furthermore, step S4.2 includes the following steps:

[0108] The fourth-order Runge-Kutta RK4 method is used for numerical integration, and the state prediction equation of the system is:

[0109]

[0110] where exp(·) represents the matrix exponential mapping, which maps the elements of the Lie algebra to the Lie group, ζ k is the state increment, calculated by the following RK4 steps:

[0111]

[0112] The four coefficients of RK4 are calculated step by step through the following steps:

[0113] Phase 1 (k1): Evaluate the derivative at the current state:

[0114]

[0115] Second stage (k2): evaluate the derivative using the half-step integration of k1:

[0116]

[0117] The third stage (k3): evaluate the derivative using the k2 half-step integration:

[0118]

[0119] Stage 4 (k4): ​​Evaluate the derivative using the full-step integration of k3:

[0120]

[0121] where f(·) is the system dynamics function, which includes all the kinematic and dynamic equations defined above and describes the evolution of the state vector over time;

[0122] The constructed measurement equation is expressed as:

[0123]

[0124] The measurement vector contains the data obtained from different sensors:

[0125]

[0126] The measurement function h(·) is defined as:

[0127]

[0128] in is the predicted position, is the vector representation of the predicted rotation matrix, is the predicted speed, is the predicted angular velocity;

[0129] Using an innovative calculation method based on Lie algebra, the observation value at time k is z k , the predicted observation value is Then the observation residual (innovation) r k Expressed as:

[0130]

[0131] in, Represents the subtraction operation on the Lie group. For the position and velocity components, the difference form is directly used:

[0132]

[0133] Computational innovation through logarithmic mapping:

[0134]

[0135] where R measured is the rotation matrix obtained from the sensor data, R predis the rotation matrix in the predicted state, log(·) is the logarithmic map on SO(3), which maps the rotation matrix to its corresponding Lie algebra, and the ∨ operator maps the antisymmetric matrix back to its corresponding vector form, which represents the error from the predicted rotation to the measured rotation. Thus, the complete innovation sequence is expressed as:

[0136]

[0137] in It is an innovation in angular velocity, which directly calculates the difference between the measured and predicted values;

[0138] The state update is also performed on the SE(3) Lie group. First, the optimal gain is calculated by the standard Kalman method:

[0139]

[0140] Then, the state update is achieved using the exponential map on the Lie group:

[0141]

[0142] The covariance update uses the Joseph form:

[0143]

[0144] Furthermore, step S4.3 includes the following steps:

[0145] The adaptive measurement noise covariance matrix adjustment strategy based on the innovative sequence is introduced, and the measurement noise covariance matrix R k The update formula is as follows:

[0146]

[0147] Among them, r k is the innovation vector at time k, and the forgetting factor λ is dynamically adjusted according to the motion state: it is set to 0.95 in the static state to maintain stability, 0.85 in dynamic motion to improve response speed, and 0.75 to increase the weight during support phase transition due to the reduced observability of the system;

[0148] The adjustment of the process noise covariance matrix adopts the form of multi-factor product, which aims to adjust the size of the process noise according to the motion state characteristics of the robot and reflect the uncertainty of the system dynamics:

[0149] Q k =Q base ·λ v ·λ a ·λ c

[0150] where Qbase is the basis process noise covariance matrix, λ v is the speed-related factor, λ a is the acceleration-related factor, λ c is the contact state related factor, λ c According to the contact situation of the robot, it is divided into three situations: stable contact, no contact and transition state for parameter setting. The speed-related factor λ v The acceleration-related factor λ is obtained by normalizing the speed curve between the minimum and maximum values ​​to reflect the dynamic changes of the system at different speeds. a The calculation method is:

[0151]

[0152] Furthermore, the step S4.4 includes the following steps:

[0153] A multi-level adaptive mechanism based on motion characteristics is used to improve the robustness and accuracy of state estimation by dynamically adjusting system parameters:

[0154]

[0155] where α base is the basic weight, v th is the speed threshold, which is used to control the influence of speed on weight. Based on this adaptive weight, the state estimation obtains the final position and speed estimation through weighted fusion:

[0156] p k =α p p kin +(1-α p )p imu

[0157] v k =α v p kin +(1-α v )v imu

[0158] Weight coefficient α p and α v Dynamic adjustment is performed according to the contact state and speed state of the robot. In the static state, α p and α v Take 0.7, in dynamic motion, α p and α v Take 0.5, and use smoothing function to ensure the continuity and smoothness of state estimation at the moment of supporting phase switching. The specific method is as follows:

[0159]

[0160] where λ trans Is a proportional coefficient used to control the amplitude of the transition. t is the time from the current time to the switching moment,

[0161] τ is the time constant that controls the rate of transition.

[0162] The biped robot state estimation method based on multi-sensor information of the present invention has the following advantages:

[0163] 1. Improved state estimation accuracy and robustness: By combining the IEKF algorithm with adaptive contact detection, it can more accurately handle nonlinear motion and sensor noise, especially during dynamic walking and support phase switching, and the estimation results are more stable and reliable. IEKF has advantages in handling pose estimation, is less prone to singularities and gimbal lock, and its error dynamics are more consistent with the filter assumptions.

[0164] 2. Enhanced accuracy of contact status judgment: The contact detection method with adaptive threshold can adapt to different ground conditions and walking speeds, reducing state estimation errors caused by contact misjudgment.

[0165] 3. Effectively suppresses error accumulation: The characteristics of IEKF and the optional dynamic reference frame conversion mechanism help suppress error accumulation caused by integral drift during long-term operation, improving the long-term stability of positioning and attitude estimation.

[0166] 4. Provides more reliable feedback for the control system: Accurate and robust state estimation results can provide high-quality feedback information for upper-level control algorithms such as gait planning and balance control of the bipedal robot, thereby improving the overall motion performance and stability of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0167] Figure 1 Schematic diagram of the overall framework structure of the biped robot state estimation of the present invention;

[0168] Figure 2 is a flow chart of the biped robot state estimation method of the present invention;

[0169] Figure 3 This is a current curve diagram of the bipedal robot of the present invention during walking;

[0170] Figure 4 Establish a schematic diagram for the robot reference coordinate system of the present invention;

[0171] Figure 5 Schematic diagram of error calculation and correction of the present invention; DETAILED DESCRIPTION

[0172] In order to better understand the purpose, structure and function of the present invention, the biped robot state estimation method based on multi-sensor information of the present invention is further described in detail below with reference to the accompanying drawings.

[0173] like Figure 1 Figure 2 As shown, the present invention provides a method for estimating the state of a bipedal robot based on multi-sensor information. This method uses the current supporting foot of the robot as a dynamic reference benchmark, thereby effectively reducing the cumulative effect of position and posture estimation errors during long-term operation. This method monitors the changes in the robot's support state in real time and dynamically switches the reference coordinate system, so that the state estimation results can still maintain high accuracy and robustness during long-term operation, greatly improving the motion control performance of the bipedal robot in complex environments. The present invention innovatively combines contact dynamic detection, posture perception, and the invariant extended Kalman filter (IEKF) algorithm to achieve robust estimation of the robot's state. The method comprises the following steps:

[0174] Step S1: Acquire sensor data. Real-time data is collected from various sensors installed on the bipedal robot, including at least the IMU (providing angular velocity and linear acceleration information), joint encoders (providing joint angle information), and the current sensor installed at the knee joint (indirectly providing contact force information on the sole of the foot).

[0175] Step S2: Adaptive contact state detection. Using the current sensor data obtained in step S1, an adaptive threshold contact detection algorithm is designed and implemented. This algorithm dynamically adjusts the determination of foot contact based on the robot's current motion state (e.g., velocity, acceleration), knee joint current value, and historical contact information. Compared to fixed threshold methods, this algorithm can more accurately determine contact state, especially during stance phase transitions or when interacting with different surfaces. The algorithm outputs the contact state of each foot (e.g., contact / non-contact).

[0176] Accurately identifying the contact state between the foot and the ground during walking is a key prerequisite for state estimation. Traditional methods rely on foot-based sensors, which increase cost and complexity, and have limited reliability in complex motion states. This paper proposes an adaptive threshold contact detection algorithm based on multi-feature fusion. This algorithm does not require additional external sensors and can adapt to different postures and motion states.

[0177] Based on a multi-feature fusion framework, this algorithm comprehensively considers three key features: joint torque, foot end velocity, and acceleration impact generated at the moment of contact. Through weighted fusion, it calculates the probability of foot end contact with the ground. This fusion strategy effectively utilizes the complementary information of different physical quantities, improving detection accuracy and reliability, and providing greater robustness in dynamic walking and complex environments. The specific formula for calculating foot end contact probability is as follows:

[0178] P contact =w τ ·P τ +w v ·P v +w a ·P a

[0179] Among them, P τ 、P v and P a They represent the contact probability calculated based on current, velocity and acceleration characteristics, respectively, and w τ 、w v and w a is the corresponding weight coefficient, satisfying w τ +w v +w a = 1. When the calculated contact probability P contact When it is greater than 0.8, the system determines that the foot is in contact with the ground. The calculation method of each feature probability is as follows:

[0180] 1. Torque characteristic probability: When a biped robot walks, the current values ​​of the ankle joints of its left and right feet will show periodic changes. The knee joint current of the robot walking in the GAZEBO simulation environment is as follows: Figure 3 shown.

[0181] Joint torque is usually positively correlated with joint current. The torque characteristic probability is calculated by mapping the current value to the joint torque, comparing the joint torque with the adaptive threshold, and using the Sigmoid function to calculate the probability value:

[0182]

[0183] Where τ is the joint torque, τ threshold is the adaptive torque threshold, α τ is the sensitivity coefficient.

[0184] 2. Velocity characteristic probability: The robot calculates the horizontal velocity of the end of the swinging leg through forward kinematics to obtain its contact situation, and maps it to an exponential function for calculation:

[0185]

[0186] Among them, v horizontal is the horizontal velocity vector of the foot end, β v is the speed sensitivity coefficient.

[0187] 3. Characteristic probability of acceleration impact: When the robot is moving, its foot will be subjected to a certain acceleration impact at the moment of landing. The research of this invention calculates the acceleration of the foot through the acceleration feedback from the joint encoder, and finally calculates the characteristic probability of acceleration impact.

[0188]

[0189] where a diff is the difference vector of acceleration at adjacent moments, β a is the acceleration sensitivity parameter.

[0190] Since the robot joint torque is closely related to the robot walking posture, the torque threshold τ threshold It is dynamically adjusted according to the robot's current posture and joint angles:

[0191] τ threshold =τ base ·S posture (1+k θ ·|θ knee |)·f(h)

[0192] Among them, τ base is the basic threshold, S posture is the attitude-related scaling factor, k θ is the knee joint angle influence coefficient, θ knee is the knee joint angle, and f(h) is the height-dependent adjustment function. For the standing posture, S posture Take the value 0.8, for the standard half squat state, S posture The value is 2.0, and the other transition postures are 1.0. The height adjustment function f(h) is defined as:

[0193]

[0194] Where h is the current height, h nominal is the nominal height (0.5m), h min (0.3m) and h max (0.63m) are the minimum and maximum heights set respectively.

[0195] In the specific implementation, the system will dynamically update the relevant parameters according to the current recognized posture state, including torque threshold, speed threshold, impact threshold, smoothing factor and sensitivity parameter α τ , β a , β a .

[0196] Step S3: Support phase determination and reference frame processing. Based on the contact state output from step S2, the current support phase of the bipedal robot (e.g., single support phase, double support phase, or airborne phase) is determined. Based on this support phase information, a dynamic reference frame conversion mechanism is applied to establish the estimated reference frame on the current supporting leg to reduce the cumulative error caused by integration in the fixed world coordinate system.

[0197] In bipedal robot state estimation, this paper employs a dynamic reference coordinate system based on the supporting foot to more accurately describe the robot's motion state and effectively address the challenges posed by support phase switching. Compared to the traditional fixed world coordinate system, this dynamic reference coordinate system more intuitively reflects the robot's relative motion in the current support state and significantly simplifies state transitions during support phase switching, avoiding potential estimation jumps.

[0198] The choice of reference coordinate system R depends on the support state of the robot. Let p L and p R Respectively represent the positions of the left and right foot ends in the world coordinate system, and the origin of the reference coordinate system p ref The definition is as follows:

[0199]

[0200] The orientation of the reference coordinate system is defined by three orthogonal unit vectors: the z-axis and the z Aligned vertically with the world coordinate system, the x-axis e x Pointing to the robot's forward direction (determined by the center of mass position and the projection of the supporting foot on the horizontal plane),

[0201]

[0202] After normalization, it is used as the x-axis direction and the y-axis direction. y Determined by the right-hand rule. Therefore, the rotation matrix R from the reference coordinate system to the world coordinate system is ref It can be expressed as:

[0203] R ref =[e x e y e z ]

[0204] In this reference coordinate system, the robot's position state p and velocity state v can be obtained by the following transformations. These transformations convert the position and velocity in the world coordinate system into the reference coordinate system.

[0205] Position transformation:

[0206]

[0207] Speed ​​​​change:

[0208]

[0209] Where: p com is the center of mass position calculated by forward kinematics in the world coordinate system, v com is the center of mass position calculated by forward kinematics in the world coordinate system, p ref is the position of the reference point in the world coordinate system, It is the transpose of the rotation matrix from the reference coordinate system to the world coordinate system, which represents the rotation matrix from the world coordinate system to the reference coordinate system. When the support phase of the robot switches, in order to maintain the continuity of the state, the position and velocity states need to be transformed. Assuming that the reference coordinate systems before and after the switch are R1 and R2 respectively, the state transition can be expressed as:

[0210] Position state transition:

[0211]

[0212] Speed ​​state transition:

[0213]

[0214] Using a state representation based on the support foot allows the robot's position and motion characteristics relative to the support foot to be directly reflected in the state variables, greatly facilitating the design and implementation of the controller. Especially in situations where there is uncertainty in the robot's motion, when switching from one support phase to another, the state transition in this manner has a clear geometric interpretation, making the transition process more intuitive and reliable. To ensure the accuracy of the reference coordinate system definition, the system periodically updates the reference coordinate system definition based on the latest support state and foot position. The complete update process is as follows:

[0215] 11. Get the latest contact status and contact probability;

[0216] 12. Determine the new reference coordinate system definition based on the support phase;

[0217] 13. Calculate the transformation relationship between the new and old reference coordinate systems;

[0218] 14. Verify the validity of the new reference coordinate system;

[0219] 15. Perform coordinate system transformation and update state estimate.

[0220] This periodic update mechanism ensures the consistency of the reference coordinate system with the actual support state, while also guaranteeing the continuity of the state estimate through verification and transformation steps. By directly using the contact point as the origin of the new reference coordinate system, this method not only simplifies the formulation of the contact constraints but also significantly improves the stability of the state estimate. This helps enhance the adaptability and accuracy of the control system in the face of complex environmental changes, which is crucial for improving the stable walking performance of bipedal robots and their ability to navigate complex terrain.

[0221] Step S4: State estimation based on IEKF: Construct an Invariant Extended Kalman Filter (IEKF) to estimate the robot state.

[0222] S4.1: Bipedal robot state space model construction:

[0223] In view of the special motion characteristics and control requirements of the bipedal robot, the state vector designed in this invention includes key state quantities such as the base's position information, velocity, acceleration, and sensor bias. The complete state vector is defined as follows:

[0224]

[0225] in:

[0226] R t ∈SO(3): rotation matrix; v t ∈R 3 : velocity vector; p t ∈R 3 : position vector; ω t ∈R 3 : true angular velocity vector (gyroscope bias removed); α t ∈R 3 : True acceleration vector (accelerometer bias removed, excluding gravity); Accelerometer bias; Gyroscope bias.

[0227] This state vector can be compactly represented as a combination of elements on the SE(3) group and Euclidean space vectors:

[0228] X=[T,v,ω,a,b g ,b a ] T

[0229] in is the pose transformation matrix of the base.

[0230] S4.1.1: System Dynamics:

[0231] The dynamic model of the biped robot describes the evolution of the state vector over time. In the IEKF framework, the system dynamics model needs to be expressed based on Lie group theory. This paper adopts the continuous-time dynamics equation considering the IMU measurement model:

[0232]

[0233] in:

[0234] g=[0,0,-9.81] T is the gravitational acceleration vector; a m and ω m are the acceleration and angular velocity measured by the IMU respectively; and These two equations describe the noise characteristics of the angular velocity and acceleration measured by the IMU. The true angular velocity ω in the state vector t and the true acceleration α t The evolution of and It is reflected in the equation of . The equation uses the angular velocity (ω m -b g -n g ), The equation uses the acceleration (a m -b a -n a ).

[0235] [ω m -b g -n g ] × The antisymmetric matrix representing the angular velocity, n g 、n a 、n ω 、n bg 、n ba Respectively represent the process noise of the corresponding state quantity. Combining the above dynamic equations, we can get the complete state space expression:

[0236]

[0237] This dynamic model not only maintains the inherent structural characteristics of the SO(3) group and avoids the common singularity problem when using Euler angle representation, but also fully considers the influence of IMU bias and improves the accuracy and reliability of state estimation.

[0238] In practical implementation, in order to facilitate numerical calculation and real-time processing, the above continuous-time dynamic model needs to be discretized. Considering the system sampling period Δt, we can express the evolution of the system through the following discrete-time model:

[0239]

[0240] in Represents the exponential mapping operation on the Lie group, ensuring that the discretized state still lies on the Lie group manifold, thereby maintaining the geometric integrity of the system. In practical implementation, it is necessary to discretize the continuous-time dynamic equations. Considering the particularity of the rotation part, this study uses the improved Euler method for discretization:

[0241]

[0242] R k+1 =R k exp([ω m,k -b g,k ] × Δt)

[0243] v k+1 =v k +(g+R k (a m,k -b a,k ))Δt

[0244] ω k+1 =ω k +w ω,k

[0245] a k+1 =a k +w a,k

[0246] b g,k+1 =b g,k

[0247] b a,k+1 =b a,k

[0248] Where Δt is the sampling interval, and exp(·) represents the exponential map on SO(3). This discretization method maintains the Lie group structure and avoids the singularity problem that may arise from directly using Euler angle updates. Position and velocity updates use measurements after removing the accelerometer bias. Rotation updates use measurements after removing the gyroscope bias. Angular velocity and acceleration state updates take process noise into account. Process noise terms are also added to the bias update to simulate random walk characteristics.

[0249] S4.1.2: Error state definition:

[0250] In inertial navigation, due to sensor drift and other factors, the state of the system will gradually deviate from the actual state. Therefore, a filter is needed to estimate the state error and reduce the error by correcting the results of the navigation system. State variables usually include information such as position, velocity, and acceleration. The state error refers to the difference between these variables and the true value. To better describe this difference, the state error can be decomposed into two parts: right invariant error and general error. Among them, the right invariant error refers to the error caused by the error of the inertial measurement unit (IMU). These errors can be reduced or eliminated by calibrating the IMU. The bias error of the IMU is expressed as:

[0251]

[0252] General errors are caused by other factors, such as atmospheric drag and gravity gradient. These errors are difficult to eliminate, but they can be modeled and compensated for using appropriate models and algorithms. The state error at each moment can be represented as a tuple, consisting of the right-hand invariant error and the general error. This allows for a more accurate estimation of the state error and the implementation of appropriate measures to mitigate it.

[0253]

[0254] The choice of the right invariant error reflects the geometric structure of the system. This error definition ensures the consistency of the estimator on the SE(3) group and makes the error dynamics independent of the current state. Although the standard right invariant pose error only contains the rotation error on SE(3) and position error But since the state vector X t Also includes the speed v t and angular velocity ω t , so we set the state error η t Defined as a system that includes rotation error, position error, and speed error and angular velocity error This definition makes it easier for us to process the entire state error in the subsequent IEKF derivation and calculation. The specific right invariant error η t And the general error is of the form:

[0255]

[0256] The specific definition of each error component in the right invariant error is:

[0257] Rotation matrix error:

[0258]

[0259] Position error:

[0260]

[0261] Speed ​​error:

[0262]

[0263] Angular velocity error:

[0264]

[0265] The steps for processing the right invariant error are to first calculate the right invariant error based on the state prediction at time k-1 and the measurement at time k. The errors are then combined into a complete residual vector, and the correction is calculated using the Kalman gain. Finally, the correction is applied to the state prediction at time k, where exp(·) represents the matrix exponential mapping.

[0266]

[0267] The specific execution order is as follows Figure 5 As shown, the present invention corrects general errors by adjusting the original parameters of the sensor and the subsequent adaptive parameters of the biped robot.

[0268] S4.2: IEKF prediction and update strategy design:

[0269] In the implementation of IEKF, it is necessary to calculate the Jacobian matrix of the system state equation with respect to the state vector for the prediction of the error state. For the Lie group state representation, the calculation of the Jacobian matrix needs to be performed based on Lie algebra. Taking into account the nonlinear characteristics of the system, the present invention adopts the fourth-order Runge-Kutta (RK4) method for numerical integration. Compared with the Euler method, RK4 not only has better numerical stability and higher accuracy, but also can meet the requirements of a larger integration step size in the state estimation system of the biped robot to meet the real-time requirements. The state prediction equation of the system is:

[0270]

[0271] where exp(·) represents the matrix exponential mapping, which maps the elements of the Lie algebra to the Lie group, ζ k is the state increment, calculated by the following RK4 steps:

[0272]

[0273] The four coefficients of RK4 are calculated step by step through the following steps:

[0274] Phase 1 (k1): Evaluate the derivative at the current state

[0275]

[0276] Second stage (k2): evaluate the derivative using the half-step integration of k1

[0277]

[0278] Stage 3 (k3): Evaluate the derivative using the k2 half-step integration

[0279]

[0280] Stage 4 (k4): ​​Evaluate the derivative using the full-step integration of k3

[0281]

[0282] where f(·) is the system dynamics function, encompassing all the kinematic and dynamic equations defined previously, and describing the time evolution of the state vector. By performing multiple function evaluations at each time step, the RK4 method can more accurately approximate the true trajectory of a nonlinear system.

[0283] In order to realize the state estimation of the biped robot, it is also necessary to build a measurement model of the system. The measurement model of the system needs to comprehensively consider the information of multiple sources of sensors. The robot of the present invention is equipped with an inertial measurement unit (IMU) and a joint encoder. The IMU can directly measure the angular velocity and linear acceleration of the robot. By integrating the IMU data, the posture and speed of the robot can be preliminarily estimated. The joint encoder measures the angles of each joint of the robot. These angle information can be converted into the posture information of the robot base through the kinematic model of the robot. The constructed measurement equation can be expressed as:

[0284]

[0285] The measurement vector contains the data obtained from different sensors:

[0286]

[0287] In order to map the system state into the observation space, the measurement function h(·) is defined as:

[0288]

[0289] in is the predicted position, is the vector representation of the predicted rotation matrix, is the predicted speed, is the predicted angular velocity.

[0290] In order to measure the accuracy between the predicted value and the true value, it is crucial to calculate the innovation sequence (observation residual). In the state estimation on the Lie group, the innovation sequence refers to the time series of the observation residual. This paper adopts an innovation calculation method based on Lie algebra. The observation value at time k is z k , the predicted observation value is Then the observation residual (innovation) r k It can be expressed as:

[0291]

[0292] in, Represents the subtraction operation on the Lie group. For the position and velocity components, due to their good properties in Euclidean space, the differential form can be directly used:

[0293]

[0294] However, for the rotation part, in order to maintain the structure of the SO(3) group, it is necessary to calculate the innovation through a logarithmic mapping:

[0295]

[0296] where R measured is the rotation matrix obtained from the sensor data, R pred is the rotation matrix in the predicted state. log(·) is the logarithmic map on SO(3), which maps the rotation matrix to its corresponding Lie algebra (an antisymmetric matrix that can be represented as a rotation vector). The ∨ operator maps the antisymmetric matrix back to its corresponding vector form, which represents the error from the predicted rotation to the measured rotation. In this way, the complete innovation sequence can be expressed as:

[0297]

[0298] in is the innovation of angular velocity, directly calculating the difference between the measured and predicted values.

[0299] In order to maintain the geometric structure of the state, the state update also needs to be performed on the SE(3) Lie group. First, the optimal gain is calculated by the standard Kalman method:

[0300]

[0301] Then, the state update is achieved using the exponential map on the Lie group:

[0302]

[0303] This completes a state update, after which a covariance update is required. To ensure the stability of numerical calculations, the covariance update adopts the Joseph form:

[0304]

[0305] The state estimation flow chart of the present invention is as follows: Figure 2 As shown, through this Lie group-based state update mechanism combined with an adaptive noise adjustment strategy, the method proposed in the present invention can effectively handle various challenges in the motion process of a bipedal robot and ensure the accuracy and reliability of state estimation.

[0306] S4.3: Noise covariance adaptive adjustment strategy:

[0307] Considering the differences in dynamic characteristics of bipedal robots under different motion states, this paper introduces an adaptive measurement noise covariance matrix adjustment strategy based on an innovative sequence. This strategy can dynamically adjust the confidence level of the measurement noise according to the actual performance of the filter, thereby improving the robustness of state estimation. The measurement noise covariance matrix R k The update formula is as follows:

[0308]

[0309] Among them, r k is the innovation vector at time k. The forgetting factor λ is dynamically adjusted based on the motion state: 0.95 is used in the static state to maintain stability, 0.85 is used during dynamic motion to improve response speed, and 0.75 is used to increase the weight during support phase transitions due to reduced system observability. This adaptive mechanism improves the robustness of state estimation.

[0310] The adjustment of the process noise covariance matrix adopts the form of multi-factor product, which aims to adjust the size of the process noise according to the motion state characteristics of the robot and reflect the uncertainty of the system dynamics:

[0311] Q k =Q base ·λ v ·λ a ·λ c

[0312] where Q base is the basis process noise covariance matrix, λ v is the speed-related factor, λ a is the acceleration-related factor, λ c is the contact state related factor. c According to the contact situation of the robot, it is divided into three situations: stable contact, no contact and transition state for parameter setting. The speed-related factor λ vThe acceleration factor λ is obtained by normalizing the speed curve between the minimum and maximum values ​​to reflect the dynamic changes of the system at different speeds. a The calculation method is:

[0313]

[0314] S4.4: Adaptive measurement parameter adjustment:

[0315] The motion characteristics of bipedal robots are highly nonlinear and time-varying, and their motion states frequently switch between various modes, such as standing, walking, and support phase switching. This complex dynamic characteristic makes it difficult for a fixed-parameter state estimator to maintain optimal performance in all motion states. To this end, this paper adopts a multi-level adaptive mechanism based on motion characteristics to improve the robustness and accuracy of state estimation by dynamically adjusting system parameters:

[0316]

[0317] where α base is the basic weight, v th is the speed threshold, which is used to control the influence of speed on weight. Based on this adaptive weight, the state estimation obtains the final position and speed estimation through weighted fusion:

[0318] p k =α p p kin +(1-α p )p imu

[0319] v k =α v p kin +(1-α v )v imu

[0320] Weight coefficient α p and α v Dynamic adjustment is performed based on the robot's contact state and speed state. In a stationary state, kinematic measurement is more reliable because it has no integral drift problem, and the noise of the joint encoder and the error of the kinematic model are less affected. p and α v Take 0.7. However, IMU data is more reliable during dynamic motion because IMU can capture fast motion. During fast motion, kinematic model error and joint compliance will increase simultaneously. p and α v Take 0.5. At the moment of supporting phase switching, the system needs to use a smoothing function to ensure the continuity and smoothness of the state estimation. The specific method is:

[0321]

[0322] where λ trans is a proportional coefficient that controls the magnitude of the transition. t is the time from the current time to the switching moment, and τ is a time constant that controls the rate of the transition. This smooth transition mechanism effectively avoids state estimation jumps caused by sudden changes in the contact state during support phase switching, while ensuring that the weights gradually return to their normal values.

[0323] Step S5: Output state estimation results. Output the robot state information obtained after IEKF filtering, including but not limited to: the robot's posture, center of mass position, center of mass linear velocity, center of mass angular velocity, etc. in the world coordinate system.

[0324] Another aspect of the present invention provides a biped robot state estimation system based on an invariant extended Kalman filter and adaptive contact detection, the system being configured to execute the above method, comprising:

[0325] Sensor module: used to obtain IMU data, joint encoder data, and knee joint current sensor data.

[0326] Contact detection module: connected to the sensor module, used to execute the contact state detection algorithm based on the adaptive threshold.

[0327] Support phase determination module: connected to the contact detection module, used to determine the support phase according to the contact state.

[0328] State estimation module: usually includes a processor connected to the sensor module, contact detection module and support phase determination module, and is configured to run the IEKF algorithm to perform state prediction and update steps, where the update step utilizes contact state and kinematic information.

[0329] Output module: connected to the state estimation module to output the final estimated robot state.

[0330] It will be understood that the present invention is described by way of some embodiments, and it will be appreciated by those skilled in the art that various changes or equivalent substitutions may be made to these features and embodiments without departing from the spirit and scope of the present invention. In addition, under the teachings of the present invention, these features and embodiments may be modified to adapt to specific circumstances and materials without departing from the spirit and scope of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed herein, and all embodiments falling within the scope of the claims of this application are intended to be protected by the present invention.

Claims

1. A method for estimating the state of a bipedal robot based on multi-sensor information, characterized in that: The following steps are involved: Step S1: Acquire sensor data: collect data from various sensors installed on the biped robot in real time; Step S2: Adaptive contact state detection: using the current sensor data obtained in step S1, design and execute an adaptive threshold contact detection algorithm; Step S3: Support phase determination and reference frame processing: Based on the contact state output in step S2, the current support phase of the bipedal robot is determined. Based on the support phase information, a dynamic reference frame conversion mechanism is applied to establish the estimated reference frame on the current supporting leg. Step S4: State estimation based on IEKF: Construct an invariant extended Kalman filter (IEKF) to estimate the robot state; Step S5: Output state estimation result: output the robot state information obtained after IEKF filtering.

2. The method for estimating the state of a bipedal robot based on multi-sensor information according to claim 1, characterized in that: The various sensor data collected in step S1 at least include: IMU, encoders of each joint, and current sensors installed on the knee joint.

3. The method for estimating the state of a bipedal robot based on multi-sensor information according to claim 1, characterized in that: The step S2 comprises the following steps: The specific calculation formula for the foot contact probability is as follows: P contact =w τ ·P τ +w v ·P v +w a ·P a Among them, P τ 、P v and P a They represent the contact probability calculated based on current, velocity and acceleration characteristics, respectively, and w τ 、w v and w a is the corresponding weight coefficient, satisfying w τ +w v +w a =1, when the calculated contact probability P contact When it is greater than 0.8, the system determines that the foot is in contact with the ground. The calculation method of each feature probability is as follows: Torque characteristic probability: Joint torque is usually positively correlated with joint current. The torque characteristic probability is calculated by mapping the current value to the joint torque, comparing the joint torque with the adaptive threshold, and using the Sigmoid function to calculate the probability value: Where τ is the joint torque, τ threshold is the adaptive torque threshold, α τ is the sensitivity coefficient. Speed ​​characteristic probability: The robot calculates the horizontal velocity of the end of the swing leg through forward kinematics to obtain its contact situation, and maps it to the exponential function for calculation: Among them, v horizontal is the horizontal velocity vector of the foot end, β v is the speed sensitivity coefficient; Acceleration shock characteristic probability: The acceleration at the foot end is calculated using the acceleration feedback from the joint encoder, and the characteristic probability of the acceleration shock is finally calculated: where a diff is the difference vector of acceleration at adjacent moments, β a is the acceleration sensitivity parameter; Torque threshold τ threshold It is dynamically adjusted according to the robot's current posture and joint angles: t threshold =t base ·S posture ·(1+k θ ·|θ knee |)·f(h) Among them, τ base is the basic threshold, S posture is the attitude-related scaling factor, k θ is the knee joint angle influence coefficient, θ knee is the knee joint angle, f(h) is the height-related adjustment function, and for the standing posture, S posture Take the value 0.8, for the standard half squat state, S posture The value is 2.0, and the other transition postures are 1.

0. The height adjustment function f(h) is defined as: Where h is the current height, h nominal is the nominal height, h min and h max are the minimum and maximum heights set respectively; in the specific implementation, the system dynamically updates the relevant parameters according to the currently recognized posture state, including torque threshold, speed threshold, impact threshold, smoothing factor and sensitivity parameter α τ , β a , β a .

4. The method for estimating the state of a bipedal robot based on multi-sensor information according to claim 1, characterized in that: The step S3 comprises the following steps: The choice of reference coordinate system R depends on the support state of the robot. Let p L and p R Respectively represent the positions of the left and right foot ends in the world coordinate system, and the origin of the reference coordinate system p ref The definition is as follows: The orientation of the reference coordinate system is defined by three orthogonal unit vectors: the z-axis and the z Aligned vertically with the world coordinate system, the x-axis e x Point to the direction the robot is heading. After normalization, it is used as the x-axis direction and the y-axis direction. y Determined by the right-hand rule, the rotation matrix R from the reference coordinate system to the world coordinate system is ref Expressed as: R ref =[and x And y And z ] In this reference coordinate system, the robot's position state p and velocity state v are obtained by the following transformations, which convert the position and velocity in the world coordinate system into the reference coordinate system, Position transformation: Speed ​​​​change: Where: p com is the center of mass position calculated by forward kinematics in the world coordinate system, v com is the center of mass position calculated by forward kinematics in the world coordinate system, p ref is the position of the reference point in the world coordinate system, It is the transpose of the rotation matrix from the reference coordinate system to the world coordinate system, which represents the rotation matrix from the world coordinate system to the reference coordinate system. When the support phase of the robot switches, the position and velocity states need to be transformed. Assuming that the reference coordinate systems before and after the switch are R1 and R2 respectively, the state conversion is expressed as: Position state transition: Speed ​​state transition: The system periodically updates the reference coordinate system definition based on the latest support state and foot position. The complete update process is as follows:

1. Obtain the latest contact status and contact probability; 2. Determine the new reference coordinate system definition based on the support phase; 3. Calculate the transformation relationship between the new and old reference coordinate systems; 4. Verify the validity of the new reference coordinate system; 5. Perform coordinate system transformation and update state estimate.

5. The method for estimating the state of a bipedal robot based on multi-sensor information according to claim 1, characterized in that: The step S4 comprises the following steps: Step S4.1: Building a state space model of the bipedal robot; Step S4.2: IEKF prediction and update strategy design; Step S4.3: Noise covariance adaptive adjustment strategy.

6. The method for estimating the state of a bipedal robot based on multi-sensor information according to claim 5, characterized in that: The step S4.1 includes the following steps: The complete state vector is defined as follows: in: R t ∈SO(3): rotation matrix; v t ∈R 3 : velocity vector; p t ∈R 3 : position vector; ω t ∈R 3 : true angular velocity vector; α t ∈R 3 : true acceleration vector; Accelerometer bias; gyroscope bias; This state vector is compactly represented as a combination of elements on the SE(3) group and Euclidean space vectors: X=[T,v,ω,a,b g ,b a ] T in is the pose transformation matrix of the base; S4.1.1: System Dynamics: The continuous-time dynamic equation considering the IMU measurement model is adopted: in: g=[0,0,-9.81] T is the gravitational acceleration vector; a m and ω m are the acceleration and angular velocity measured by the IMU respectively; and These two equations describe the noise characteristics of the angular velocity and acceleration measured by the IMU. The true angular velocity ω in the state vector t and the true acceleration α t The evolution of and It is reflected in the equation of , which is combined with the bias term. The equation uses the angular velocity (ω m -b g -n g ), The equation uses the acceleration (a m -b a -n a ); [ω m -b g -n g ] × The antisymmetric matrix representing the angular velocity, n g 、n a 、n ω 、n bg 、n ba Represent the process noise of the corresponding state quantity respectively. Combining the above dynamic equations, we can get the complete state space expression: In actual implementation, the above continuous-time dynamics model needs to be discretized. Considering the system sampling period Δt, the evolution of the system can be represented by the following discrete-time model: X k+1 =X k ⊕(f(X k ,u k )Δt) Where ⊕ represents the exponential mapping operation on the Lie group, which ensures that the discretized state still lies on the Lie group manifold, thereby maintaining the geometric integrity of the system. In actual implementation, it is necessary to discretize the continuous-time dynamics equations and use the improved Euler method for discretization: R k+1 =R k exp([ω m,k -b g,k ] × Δt) v k+1 =v k +(g+R k (a m,k -b a,k ))Δt oh k+1 =ω k +w ω,k a k+1 =a k +w a,k b g,k+1 =b g,k b a,k+1 =b a,k where Δt is the sampling time interval, exp(·) represents the exponential map on SO(3); S4.1.2: Error state definition: The bias error of the IMU is expressed as: The state error at each moment is represented by a tuple, which includes the right invariant error and the general error, and corresponding measures are taken to reduce the error: The choice of the right invariant error reflects the geometry of the system. This error definition ensures the consistency of the estimator on the SE(3) group and makes the error dynamics independent of the current state, although the standard right invariant pose error only contains rotation errors on SE(3) and position error But since the state vector X t Also includes the speed v t and angular velocity ω t , so the state error η t Defined as a system that includes rotation error, position error, and speed error and angular velocity error The matrix form of the right invariant error η t And the general error is of the form: The specific definition of each error component in the right invariant error is: Rotation matrix error: Position error: Speed ​​error: Angular velocity error: The steps for processing the right invariant error are to first calculate the right invariant error based on the state prediction at time k-1 and the measurement at time k, then combine the errors into a complete residual vector, calculate the correction amount through the Kalman gain, and finally apply the correction to the state prediction at time k, where exp(·) represents the matrix exponential mapping, Correction of general errors is carried out through the original parameters of the sensor and subsequent adaptive parameter adjustment of the bipedal robot.

7. The method for estimating the state of a bipedal robot based on multi-sensor information according to claim 5, characterized in that: The step S4.2 includes the following steps: The fourth-order Runge-Kutta RK4 method is used for numerical integration, and the state prediction equation of the system is: where exp(·) represents the matrix exponential mapping, which maps the elements of the Lie algebra to the Lie group, ζ k is the state increment, calculated by the following RK4 steps: The four coefficients of RK4 are calculated step by step through the following steps: Phase 1 (k1): Evaluate the derivative at the current state: Second stage (k2): evaluate the derivative using the half-step integration of k1: The third stage (k3): evaluate the derivative using the k2 half-step integration: Stage 4 (k4): ​​Evaluate the derivative using the full-step integration of k3: where f(·) is the system dynamics function, which includes all the kinematic and dynamic equations defined above and describes the evolution of the state vector over time; The constructed measurement equation is expressed as: The measurement vector contains the data obtained from different sensors: The measurement function h(·) is defined as: in is the predicted position, is the vector representation of the predicted rotation matrix, is the predicted speed, is the predicted angular velocity; Using an innovative calculation method based on Lie algebra, the observation value at time k is z k , the predicted observation value is Then the observation residual (innovation) r k Expressed as: in, Represents the subtraction operation on the Lie group. For the position and velocity components, the difference form is directly used: Computational innovation through logarithmic mapping: where R measured is the rotation matrix obtained from the sensor data, R pred is the rotation matrix in the predicted state, log(·) is the logarithmic map on SO(3), which maps the rotation matrix to its corresponding Lie algebra, and the ∨ operator maps the antisymmetric matrix back to its corresponding vector form, which represents the error from the predicted rotation to the measured rotation. Thus, the complete innovation sequence is expressed as: in It is an innovation in angular velocity, which directly calculates the difference between the measured and predicted values; The state update is also performed on the SE(3) Lie group. First, the optimal gain is calculated by the standard Kalman method: Then, the state update is achieved using the exponential map on the Lie group: The covariance update uses the Joseph form:

8. The method for estimating the state of a bipedal robot based on multi-sensor information according to claim 5, characterized in that: The step S4.3 includes the following steps: The adaptive measurement noise covariance matrix adjustment strategy based on the innovative sequence is introduced, and the measurement noise covariance matrix R k The update formula is as follows: Among them, r k is the innovation vector at time k, and the forgetting factor λ is dynamically adjusted according to the motion state: it is set to 0.95 in the static state to maintain stability, 0.85 in dynamic motion to improve response speed, and 0.75 to increase the weight during support phase transition due to the reduced observability of the system; The adjustment of the process noise covariance matrix adopts the form of multi-factor product, which aims to adjust the size of the process noise according to the motion state characteristics of the robot and reflect the uncertainty of the system dynamics: Q k =Q base ·l v ·l a ·l c where Q base is the basis process noise covariance matrix, λ v is the speed-related factor, λ a is the acceleration-related factor, λ c is the contact state related factor, λ c According to the contact situation of the robot, it is divided into three situations: stable contact, no contact and transition state for parameter setting. The speed-related factor λ v The acceleration-related factor λ is obtained by normalizing the speed curve between the minimum and maximum values ​​to reflect the dynamic changes of the system at different speeds. a The calculation method is:

9. The method for estimating the state of a bipedal robot based on multi-sensor information according to claim 5, characterized in that: The step S4.4 includes the following steps: A multi-level adaptive mechanism based on motion characteristics is used to improve the robustness and accuracy of state estimation by dynamically adjusting system parameters: where α base is the basic weight, v th is the speed threshold, which is used to control the influence of speed on weight. Based on this adaptive weight, the state estimation obtains the final position and speed estimation through weighted fusion: p k =α p p kin +(1-α p )p imu v k =a v p kin +(1-a v )v imu Weight coefficient α p and α v Dynamic adjustment is performed according to the contact state and speed state of the robot. In the static state, α p and α v Take 0.7, in dynamic motion, α p and α v Take 0.5, and use smoothing function to ensure the continuity and smoothness of state estimation at the moment of supporting phase switching. The specific method is as follows: where λ trans is a proportional coefficient that controls the magnitude of the transition. t is the time from the current time to the switching moment, and τ is a time constant that controls the rate of the transition.

Images