Biped Robot Walking Centroid State Estimation Method Based on Federated Kalman Filter

Abstract

The present invention discloses a method for estimating the centroid state of a biped robot based on federated Kalman filtering. An inertial filter, a kinematic filter, and a linear inverted pendulum filter respectively process the measurement information of an inertial measurement unit, a joint encoder, and a force sensor. Through the time update and measurement update of Kalman filtering, a sub-optimal centroid state estimation vector and the corresponding error covariance matrix are obtained. The main filter performs a time update of Kalman filtering to obtain a sub-optimal centroid state estimation vector and the corresponding error covariance matrix. The main filter performs optimal fusion based on the above-mentioned sub-filters and its own sub-optimal centroid state estimation vector and the corresponding error covariance matrix to obtain a single optimal centroid state estimation vector #imgabs0# which acts on the robot. At the next moment, using #imgabs1# and the corresponding error covariance matrix P g , the information distribution coefficient is used to feedback and reset each filter, and the above steps are repeated to obtain the optimal centroid state estimation vector. The present invention has a fast calculation speed and improves the reliability of state estimation.

Description

Technical Field

[0001] The present invention relates to the technical field of biped robots, and particularly to a method for estimating the centroid state of a biped robot walking based on a federated Kalman filter. Background Art

[0002] Biped robots use leg-foot type motion mechanisms to achieve walking motion and have motion characteristics similar to those of humans. As a branch of mobile robots, compared with wheeled and tracked robots, biped robots can adopt various forms such as avoidance and crossing to adapt to complex environments during walking, and have more powerful potential for adapting to complex unstructured environments. Therefore, their application prospects are very broad.

[0003] However, biped robots have many degrees of freedom in the whole body and a high degree of non-linearity, making it difficult to control their walking stability. The centroid motion state during the walking process of a biped robot, as the closed-loop feedback information for stable control, is an important basis for realizing the stable walking of a biped robot. Due to the influence of the environment and sensor noise, the online estimation of the centroid motion state during the robot's walking process, the estimation reliability and calculation speed are the key factors affecting the stable control effect.

[0004] Currently, the online estimation of the centroid motion state during the robot's walking process mainly uses centralized Kalman filter technology, which uses a single filter to process the measurement information of multiple sensors. Although it can realize the estimation of the walking centroid state, the state vector dimension is high and the calculation amount is large, making it difficult to ensure the real-time performance of the filter; if a measurement update fails in one of the subsystems, it will directly contaminate the entire system, resulting in incorrect state estimation and large deviations. Summary of the Invention

[0005] Aiming at the deficiencies in the prior art, the present invention provides a method for estimating the centroid state of a biped robot walking based on a federated Kalman filter, which realizes fast and accurate online estimation of the centroid state of a biped robot walking, and further improves the fault tolerance performance and reliability of the system through the adaptive adjustment of the information distribution coefficient.

[0006] The present invention achieves the above technical objectives through the following technical means.

[0007] A method for estimating the centroid state of a biped robot walking based on a federated Kalman filter includes:

[0008] An inertial filter that processes the measurement information of an inertial measurement unit After Kalman filter time update and measurement update, outputs a sub-optimal centroid state estimation vector And the corresponding error covariance matrix P 1 ;

[0009] A kinematic filter that processes the measurement information of each joint encoder After the Kalman filter time update and measurement update, the sub-optimal centroid state estimation vector is output and the corresponding error covariance matrix P 2 ;

[0010] Linear inverted pendulum filter, processing the measurement information of the force sensor After the Kalman filter time update and measurement update, the sub-optimal centroid state estimation vector is output and the corresponding error covariance matrix P 3 ;

[0011] The main filter performs the Kalman filter time update to obtain the sub-optimal centroid state estimation vector and the corresponding error covariance matrix P m ;

[0012] The main filter performs optimal fusion based on the above-mentioned sub-filters and its own sub-optimal centroid state estimation vector and error covariance matrix to obtain the optimal centroid state estimation vector at the current moment and the corresponding error covariance matrix P g , which is applied to the robot;

[0013] Using the P g and the information distribution coefficient β to perform feedback reset on each filter, repeating the above steps to obtain the optimal centroid state estimation vector at the next moment, and continue to be applied to the robot;

[0014] Repeat the above process until the robot stops walking.

[0015] Furthermore, the information distribution coefficient includes the information distribution coefficients of each sub-filter and the main filter. The information distribution coefficient of the main filter is a constant value. The information distribution coefficients of each sub-filter satisfy:

[0016]

[0017] Where: is the fault-tolerant information distribution coefficient, is the precision information distribution coefficient, β m is the main filter distribution coefficient, ρ is the weight distribution factor, and 0 ≤ ρ ≤ 1;

[0018] The fault-tolerant information distribution coefficient satisfies:

[0019]

[0020] where cond(R i ) is the measurement noise variance matrix R of the i-th sub-filteri Condition number;

[0021] The precision information distribution coefficient satisfies:

[0022]

[0023] where ‖P i ‖ F is the Frobenius norm of the error covariance matrix P i of the i-th sub-filter.

[0024] Furthermore, the sub-optimal centroid state estimation vector is obtained by the following method:

[0025]

[0026] where: is the Kalman gain of each sub-filter at time t k+1 , is the sub-optimal state estimation of each sub-filter at time t k+1 , is the error covariance matrix corresponding to the sub-optimal state estimation of each sub-filter at time t k+1 , is the measurement matrix of each sub-filter at time t k+1 , is the measurement noise variance matrix of each sub-filter at time t k+1 , is the measurement information of the corresponding sensor of each sub-filter at time t k+1 , and are the prior estimation of the centroid state and the prior estimation error covariance matrix of the centroid state of each sub-filter at time t k+1 , respectively.

[0027] Even further, the optimal fusion is performed as follows:

[0028]

[0029] where: is the global error covariance matrix of the centroid state estimator at time t k+1 , is the global optimal centroid state estimation vector of the centroid state estimator at time t k+1 .

[0030] Furthermore, the measurement information of the inertial measurement unit is:

[0031]

[0032] where and for The components in the X and Y directions of the world coordinate system, and:

[0033]

[0034] in: t k The acceleration measurement vector in the world coordinate system after moment filtering, t k-1 The acceleration measurement vector in the world coordinate system after moment filtering, w a k is the world coordinate system t k The acceleration measurement vector of the inertial measurement unit at the moment, μ is the first-order inertial filter coefficient of the measured acceleration, and 0<μ<1.

[0035] Furthermore, the measurement information of each joint code disc is:

[0036]

[0037] in and for The components in the X and Y directions, and for In the X and Y directions, the is the measured value of the centroid position, is the measured value of the center of mass velocity, and:

[0038]

[0039]

[0040] in: w P foot (t k ) is t k The position of the supporting leg foothold in the world coordinate system at all times, R k Yes k is the attitude rotation matrix of the local coordinate system of the supporting leg foothold relative to the world coordinate system at the moment, T is the discrete period, and ForwardKinematic(·) is the forward kinematic calculation function of the single leg of the biped robot obtained based on the Denavit-Hartenberg method.

[0041] Furthermore, the measurement information of the force sensor is:

[0042]

[0043] Where: g is the constant value of gravitational acceleration, zc is the centroid height constant value, is the measurement of the centroid position of the planar component, w p zmp (t k ) is the position of the zero moment point in the world coordinate system at time t. k Furthermore, the ZMP position in the world coordinate system at time t satisfies:

[0044] Furthermore, the ZMP position in the world coordinate system at time t satisfies: k where:

[0045]

[0046] where: is the position of the left foot sole in the world coordinate system at time t k , is the position of the right foot sole in the world coordinate system at time t, α(t k ) is the distribution ratio of the vertical ground reaction forces on both feet at time t, and: k ) is the distribution ratio of the vertical ground reaction forces on both feet at time t, and: k where:

[0047]

[0048] where: and are the numerical values of the ground reaction forces in the Z direction of the left and right feet in the world coordinate system after first-order inertial filtering at time t k respectively.

[0049] Furthermore, the main filter and each sub-filter perform time update:

[0050]

[0051] where is the prior estimate of the centroid state of each filter at time t, P k+1 i k+1，k is the prior estimate error covariance matrix of the centroid state of each filter at time t, F k+1 i k+′，k i k+′，k is the system matrix of each filter, G i k+′，k is the control matrix of each filter, is the control input of each filter at time t k , is k the error covariance matrix corresponding to the sub-optimal state estimate of each filter at time t, Γ i k is the error covariance matrix corresponding to the sub-optimal state estimate of each filter at time t, Γ kThe noise-driven matrix at a moment is the process noise variance matrix of each filter.

[0052] Furthermore, the system matrix of each filter is:

[0053]

[0054] where: T is the discrete period;

[0055] The control matrix of each filter is:

[0056]

[0057] The beneficial effects of the present invention are:

[0058] (1) The present invention uses three sub-filters, namely an inertial filter, a kinematic filter, and a linear inverted pendulum filter, to process the measurement information of an inertial measurement unit, a joint encoder, and a force sensor respectively. It is a decentralized filtering structure with a low state dimension and a fast calculation speed, which can ensure the real-time performance of the centroid state estimation;

[0059] (2) The information distribution coefficient during the feedback reset of each filter of the present invention can be adaptively adjusted. By adjusting the weight distribution factor, the fault tolerance performance and estimation accuracy of the state estimation can be dynamically adjusted. When a sensor fails, the fault tolerance performance of the state estimation can be improved, and when the sensor works normally, the estimation accuracy of the state estimation can be improved, ensuring the reliability of the state estimation. Description of the Drawings

[0060] Figure 1 is a schematic structural diagram of the walking centroid state estimator of the present invention;

[0061] Figure 2 is an explanatory diagram of the on-board pose and coordinate system definition of the sensors used for the centroid state estimation of the present invention;

[0062] Figure 3 is the flow chart of the walking centroid state estimation of the present invention. Detailed Embodiment

[0063] The present invention will be further described below in conjunction with the drawings and specific embodiments, but the protection scope of the present invention is not limited thereto.

[0064] The federated Kalman filter belongs to the decentralized filtering technology. The federated Kalman filter is a two-level filter, which includes sub-filters and a main filter. The sub-filters are independent and parallel to each other, and respectively process the measurement information of different sensors, and local sub-optimal state estimates can be obtained. The main filter performs optimal fusion on each local estimate to obtain the global optimal state estimate. The federated Kalman filter uses the information distribution principle to ensure the optimality of the global estimate, and uses the variance upper bound technology to ensure the uncorrelation of the estimates of each sub-filter.

[0065] The structure of the centroid state estimator for the biped robot walking of the present invention is as Figure 1 shown. The input of the estimator is the measurement information calculated according to the real-time measurement data of the on-board sensors, including the measurement information of the inertial measurement unit (IMU) the measurement information of the joint encoders of the lower limbs of the biped robot and the measurement information of the foot-end force sensor Adopting the decentralized filtering method, based on the Kalman filtering principle, three sub-filters are respectively designed to independently process the measurement information of the three sensors. Among them, the inertial filter (the first sub-filter) processes the measurement information of the inertial measurement unit and outputs a sub-optimal centroid state estimation vector and the corresponding error covariance matrix P 1 ; the kinematic filter (the second sub-filter) processes the measurement information of each joint encoder and outputs a sub-optimal centroid state estimation vector and the corresponding error covariance matrix P 2 ; the linear inverted pendulum (LIPM) filter (the third sub-filter) processes the measurement information of the force sensor and outputs a sub-optimal centroid state estimation vector and the corresponding error covariance matrix P 3 ; the main filter does not receive the measurement information and only performs time update to obtain a sub-optimal centroid state estimation vector and the corresponding error covariance matrix P m . Subsequently, the main filter performs optimal fusion based on the sub-optimal state estimation vectors and the corresponding error covariance matrices of each sub-filter and itself to obtain the optimal centroid state estimation vector and the corresponding error covariance matrix P g at the current moment. Before the sub-filter and the main filter perform state estimation, based on the optimal centroid state estimation vector of the previous moment and the corresponding error covariance matrix P g and the information distribution coefficient β, the sub-filters are feedback reset. The information distribution coefficient of the inertial filter is β 1, the information distribution coefficient of the kinematic filter is β 2 , the information distribution coefficient of the linear inverted pendulum filter is β 3 , the information allocation coefficient of the main filter is β m .

[0066] The walking center of mass state estimator of the present invention is used to online estimate the position, velocity and acceleration of the center of mass in the forward (X) direction and the lateral (Y) direction in the world coordinate system during the walking process of the bipedal robot. A three-dimensional linear inverted pendulum (3D-LIPM) model is used to characterize the walking dynamics of the bipedal robot, and the lateral movement and forward movement of the bipedal robot are decoupled. Based on the walking center of mass state estimator proposed by the present invention, the movement states in the X and Y directions are merged, and the center of mass movement states in the X and Y directions are estimated simultaneously.

[0067] Therefore, the center of mass state vector of each filter in the walking center of mass state estimator is defined as:

[0068]

[0069] in: represents the field of real numbers, represents the 6-dimensional linear space over the real number field, and the rest are similar; They are the estimated positions of the center of mass in the X and Y directions in the world coordinate system of the corresponding filter, are the estimated velocities in the X and Y directions of the center of mass in the world coordinate system of the corresponding filter, They are the estimated accelerations in the X and Y directions of the center of mass in the world coordinate system of the corresponding filter.

[0070] Taking the BHR6P biped robot of Beijing Institute of Technology as an example, the position distribution of each sensor and the definition of the coordinate system used for center of mass state estimation are explained. Figure 2As shown in the figure, the BHR6P biped robot has a total of 23 degrees of freedom in its whole body, including 8 degrees of freedom in the upper limbs, 3 degrees of freedom in the waist, and 12 degrees of freedom in the lower limbs. The six-axis force sensor is located at the ankle joints of the two legs of the robot, the inertial measurement unit (IMU) is located at the neck and shoulders of the robot, and the joint encoders are located at each joint, and the number is the same as the number of joints. The origin of the center of mass (CoM) coordinate system is located at the center of the two hips. The X direction of the center of mass coordinate system is towards the front of the robot, the Y direction is towards the left of the robot, and the Z direction is vertically upward. This coordinate system is fixedly connected to the robot. The origin of the World coordinate system is the vertical projection of the origin of the center of mass coordinate system onto the ground before the robot starts moving, and is located at the center of the two feet. The coordinate system direction is the same as the orientation of the initial center of mass coordinate system, and this coordinate system is fixedly connected to the ground. The origin of the IMU coordinate system is located at the center of the IMU measurement unit, and its orientation is the same as the orientation of the initial center of mass coordinate system. This coordinate system is fixedly connected to the robot. The relative pose between the center of mass coordinate system and the IMU coordinate system can be calibrated by measurement tools. The origins of the left foot landing point (LFoot) coordinate system and the right foot landing point (RFoot) coordinate system are respectively located at the vertical projections of the left and right ankle joints onto their respective foot soles. The left foot landing point coordinate system and the right foot landing point coordinate system are fixedly connected to the robot, and their initial orientations are the same as the World coordinate system.

[0071] The state estimation process of the biped robot walking center of mass state estimator of the present invention is as Figure 3 shown. The state estimation process is mainly divided into five steps, namely system initialization, information distribution, time update, measurement update, and optimal fusion.

[0072] (1) In the system initialization part, it is necessary to configure the center of mass optimal state estimation vector corresponding to the error covariance matrix P g , the initial values of the information distribution coefficients β i (i = 1, 2, 3, m), the global process noise matrix Q g and the measurement noise matrix R i (i = 1, 2, 3) measured values; before the robot starts moving, the center of mass is directly above the World coordinate system, so the initial value of the center of mass optimal state estimation vector is:

[0073]

[0074] For the measurement noise of each sensor, based on the actual measured data of the sensor and measurement calculation for calibration, the measured value of the inertial measurement unit measurement noise matrix the measured value of the joint encoder measurement noise matrix and the measured value of the force sensor measurement noise matrix The error covariance matrix P g and the process noise matrix Q gConfigure the initial values based on engineering experience, and the parameters can be adjusted through multiple groups of experiments until the estimation results converge;

[0075] For the information distribution coefficient β of each filter i (i = 1, 2, 3, m), adopt the variable proportion mode of the federated Kalman filter to evenly distribute the coefficients, then the initial values of the information distribution coefficients of each filter are:

[0076]

[0077] (2) Information distribution part, based on the optimal centroid state estimation result or the estimation initial value at the previous moment, perform feedback reset on the sub-filters, and use the optimal centroid state estimation vector to reset the sub-optimal state estimation vectors of each filter

[0078]

[0079] where is the sub-optimal state estimation vector of each filter at time t k , and is the optimal centroid state estimation vector at time t k ;

[0080] Based on the information distribution coefficient and the optimal estimation result at the previous moment (or the initial value of the optimal centroid state estimation vector), feedback reset the error covariance matrix P i and the process noise matrix Q i of each filter:

[0081]

[0082]

[0083] The information distribution coefficient always follows the principle of information conservation, that is:

[0084]

[0085] The traditional federated Kalman filter uses a constant information distribution coefficient, which is difficult to cope with complex environmental changes. When a sensor fails, it is easy to cause the estimation to be contaminated and diverge, and it is difficult to ensure the accuracy of the centroid state estimation. Therefore, the present invention proposes a method for adaptively adjusting the information distribution coefficient, which can reduce the impact of faulty sensors on the global optimal centroid state estimation, improve the fault tolerance and reliability of the centroid state estimation, and ensure the accuracy of the state estimation.

[0086] ​The main filter only performs time updates and does not perform measurement updates. Its estimation accuracy is not affected by sensor failures. Therefore, the information distribution coefficient of the main filter is set to a constant value according to the degree of conformity between the main filter process model and the actual motion of the biped robot, that is:

[0087]

[0088] For the sub - filters, the following adaptive adjustment rules for the information distribution coefficient are designed:

[0089]

[0090] is the fault - tolerance information distribution coefficient, and its calculation formula is:

[0091]

[0092] where cond(R i ) is the condition number of the measurement noise matrix R i of the i - th sub - filter, and its calculation formula is:

[0093]

[0094] where ‖R i ‖ is the norm of the measurement noise matrix R i , is the inverse matrix of the measurement noise matrix R i , and the norm of cond -1 (R i ) is the reciprocal of the condition number of the measurement noise matrix R i .

[0095] cond(R i ) can reflect the degree of contamination of the global optimal centroid state estimation by the faulty sensor. The smaller cond(R i ), the greater the global contamination degree. To improve the fault - tolerance performance of the sub - filters, the fault - tolerance information distribution coefficient is set to be inversely proportional to cond(R i ). When a sensor fails, causing the global contamination to increase and cond(R i ) to become smaller, the fault - tolerance information distribution coefficient should be increased, so that the information distribution coefficients of the sub - filters corresponding to other non - faulty sensors are reduced, enhancing the system's fault - tolerance performance.

[0096] is the accuracy information distribution coefficient, and its calculation formula is:

[0097]

[0098] where ‖P i‖ F is the error covariance matrix \(P\) of the \(i\)-th sub-filter i of the Frobenius norm, and its calculation formula is:

[0099]

[0100] is the matrix trace of, is the error covariance matrix \(P\) i reciprocal of the Frobenius norm of.

[0101] ‖\(P\) i ‖ F can measure the magnitude of the estimation error reflected by the error covariance matrix. The larger the estimation error, the lower the estimation accuracy of the sub-filter, and the smaller the allocated information coefficient, that is, the accuracy information allocation coefficient is inversely proportional to the estimation error, thereby improving the accuracy of the global optimal centroid state estimation.

[0102] For the fault-tolerant information allocation coefficient and the accuracy information allocation coefficient Obviously, there is:

[0103]

[0104]

[0105] That is, the information allocation coefficient adaptive adjustment method proposed by the present invention still satisfies the information conservation principle (Equation (7)).

[0106] In Equation (9), \(\rho\) is the weight allocation factor, and its value range is \(0\leq\rho\leq1\), which is used to adjust the fault-tolerant information allocation coefficient and the accuracy information allocation coefficient in the information allocation coefficient \(\beta\) i proportion. When \(\rho\rightarrow0\), the information allocation coefficient adaptive adjustment method tends to improve the fault tolerance of the system. When \(\rho\rightarrow1\), the information allocation coefficient adaptive adjustment method tends to improve the estimation accuracy of the system. The weight allocation factor \(\rho\) can be determined according to the failure rates of each sensor and the actual test situation.

[0107] (3) Time update part, based on the three-dimensional linear inverted pendulum (3D-LIPM) model for time update, each filter performs a one-step prediction based on the centroid state estimation result, error covariance matrix and process model at the previous moment to obtain the prior estimation of the centroid state and its prior error covariance matrix at the current moment:

[0108]

[0109] where is the \(t\) of each filterk+1 Prior estimate of the centroid state at a moment, P i k+1,k For each filter at time t k+1 Prior estimate error covariance matrix of the centroid state at time t for each filter, F i k+1,k System matrix for each filter, G i k+1,k Control matrix for each filter For each filter at time t k Control input at time t Is at time t k Error covariance matrix corresponding to the sub - optimal state estimate of each filter at time t, Γ i k For each filter at time t k Noise driving matrix at time t Is the process noise variance matrix of each filter

[0110] For the walking motion of a biped robot, adopting the classical three - dimensional linear inverted pendulum dynamics model, the state equation (process model) of the system can be obtained as follows:

[0111]

[0112] x, y are the positions of the centroid of the biped robot in the X and Y directions in the world coordinate system Are the velocities of the biped robot in the X and Y directions in the world coordinate system Are the accelerations of the biped robot in the X and Y directions in the world coordinate system, u x And u y Are the control inputs in the X and Y directions of the system

[0113] Taking this state equation as the process model of the sub - filter and the main filter, and discretizing the state equation, we can obtain:

[0114]

[0115]

[0116] Where: T is the discrete period, that is, t k+1 - t k ;

[0117] Since each sub - filter and the main filter have the same estimated state, the noise driving matrix is the identity matrix, that is:

[0118]

[0119] (4) Measurement update part. Each sub - filter first calculates the measurement information based on its own sensor information, then calculates the Kalman gain, based on tk Prior estimate of the centroid state at a moment and its prior estimate error covariance matrix Measurement update gives t k+1 Sub-optimal state estimates at respective moments and the corresponding error covariance matrices

[0120] For the inertial filter, the measured acceleration of the IMU is used as the measurement information for the measurement update of the filter. The inertial filter is used to measure the updated acceleration, and its Kalman filter measurement model is:

[0121]

[0122] Where is the measurement vector of the inertial filter, is the measurement matrix of the inertial filter, is the centroid state vector of the inertial filter, is the measurement noise matrix of the inertial filter, and

[0123] Taking t k moment as an example, the acceleration measurement vector k in the IMU coordinate system read by the inertial measurement unit (IMU) at the t IMU a k is transformed to be described in the world coordinate system through attitude transformation, and the acceleration measurement vector k at the t w moment in the world coordinate system is obtained k a

[0124] w a k = w R IMU IMU a k (22)

[0125] Where w R IMU is the rotation matrix of the IMU coordinate system relative to the world coordinate system, and

[0126] For w a k perform first-order inertial filtering to obtain the filtered acceleration measurement vector k in the world coordinate system at the t That is:

[0127]

[0128] Where For t k-1 The acceleration measurement vector in the world coordinate system after filtering at time (the previous time), μ is the first-order inertial filtering coefficient of the measured acceleration, and 0 < μ < 1,

[0129] Then the inertial filter at time t k The measurement information of the inertial measurement unit at time is:

[0130]

[0131] Where and are The components in the X and Y directions in the world coordinate system.

[0132] For the kinematic filter, the single-leg forward kinematics is used to calculate the position of the floating base relative to the supporting foot and the differential linear velocity as the measurement information for measurement update; the kinematic filter is used to measure and update the position and velocity of the center of mass, and its Kalman filter measurement model is:

[0133]

[0134] Where Is the measurement vector of the kinematic filter, Is the measurement matrix of the kinematic filter, Is the center-of-mass state vector of the kinematic filter, Is the measurement noise matrix of the kinematic filter, and

[0135] When a biped robot walks, it is similar to a human, and the two feet alternately contact the ground, there are single-leg support periods and swing periods. When the supporting leg remains in contact with the ground, the pose of the floating base (center of mass) and its motion increment can be measured according to the pose of the supporting leg and the encoder data of each joint of the leg.

[0136] Taking time t k-1 And time t k As an example, denote the joint angle vector of the supporting leg read from the joint encoder at time t k-1 As q k-1 , t k The joint angle vector of the supporting leg read from the joint encoder at time is q k , The BHR6P robot has a total of 6 joint angles for a single leg, then The relational expression between the landing point position and the floating base position in the world coordinate system is:

[0137] w P foot (t k-1 ) = pk-1 +R k-1 *ForwardKinematic(q k-1 ) (26)

[0138] w P foot (t k ) = p k +R k *ForωardKinematic(q k ) (27)

[0139] where w P foot (t k-1 ) is the position of the landing point of the supporting leg at time t in the world coordinate system, p k-1 is the position of the floating base at time t in the world coordinate system, R k-1 is the attitude rotation matrix of the local coordinate system of the landing point of the supporting leg relative to the world coordinate system at time t, k-1 k-1 k-1 w foot k k P foot (t k ) is the position of the landing point of the supporting leg at time t in the world coordinate system, p k is the position of the floating base at time t in the world coordinate system, R k is the attitude rotation matrix of the local coordinate system of the landing point of the supporting leg relative to the world coordinate system at time t, and k k k w foot k-1 ForwardKinematic(·) is a biped robot single-leg forward kinematics calculation function obtained based on the Denavit-Hartenberg method (which is prior art). This function takes the biped robot single-leg joint angle vector as input and outputs the three-dimensional position vector of the landing point of the supporting leg relative to the centroid coordinate system.

[0140] When the supporting leg maintains stable contact with the ground, during this support period, the position of the landing point in the world coordinate system remains unchanged, that is:

[0141] w P foot (t k-1 ) = w P foot (t k ) (28)

[0142] The centroid velocity measurement can be obtained by position difference, that is:

[0143]

[0144] Substituting equations (27)-(28) into (29), the measurement formula for the centroid velocity can be obtained as follows:

[0145]

[0146] is the measured value of the centroid velocity,

[0147] The measurement of the centroid position can be obtained from equation (27), and its formula is:

[0148]

[0149] is the measured value of the centroid position, and R k are obtained from an external positioning sensor or biped robot motion planning technology (prior art).

[0150] From equations (30) and (31), the kinematic filter t k The measured information of the joint encoders at time is:

[0151]

[0152] where and are the components in the X and Y directions, and are the components in the X and Y directions.

[0153] For the LIPM filter, the centroid acceleration is measured and updated using the zero moment point (ZMP) dynamics based on a linear inverted pendulum. Its Kalman filter measurement model is:

[0154]

[0155] where is the measurement vector of the LIPM filter, is the measurement matrix of the LIPM filter, is the centroid state vector of the LIPM filter, is the measurement noise matrix of the LIPM filter, and

[0156] The LIPM filter needs to use the measured information of the six-dimensional force sensors at the ankles of both legs to calculate the ZMP. Assuming the biped robot has point feet, the ZMP position of each foot of the robot is the same as the landing point position. The two-dimensional ZMP position during the biped robot's walking is:

[0157]

[0158] in w p zmp (t k ) is t k The ZMP position in the world coordinate system at that moment, t k The position of the left foot sole in the world coordinate system at this moment, t k The position of the sole of the right foot in the world coordinate system at this moment, α(t k ) is t k The distribution ratio of the vertical force of the ground on the two feet at any moment is calculated as follows:

[0159]

[0160] and t k The ground force values ​​of the left and right feet in the Z direction (vertically upward) of the world coordinate system after first-order inertial filtering at any moment are calculated as follows:

[0161]

[0162] in and t k The ground force values ​​in the Z direction (vertically upward) of the world coordinate system measured by the left and right foot force sensors at all times. and t k-1 The ground force values ​​of the left and right feet in the Z direction (vertically upward) of the world coordinate system after first-order inertial filtering at all times. σ is the first-order inertial filter coefficient of the force sensor, and 0<σ<1.

[0163] Based on the zero moment point (ZMP) dynamics of the linear inverted pendulum, the measured acceleration is calculated as the measurement information of the force sensor:

[0164]

[0165] in t k The measurement information of the force sensor of the LIPM filter at the moment, g is the constant value of gravity acceleration, z c is the constant value of the centroid height, The center of mass position is measured The plane component of Right now:

[0166]

[0167] After each sub-filter calculates its own measurement information based on the sensor data, the t k A priori estimate of the center of mass state at time and its prior error covariance matrix P i k+1，k , perform measurement update:

[0168]

[0169] in Yes k+1 The Kalman gain of each sub-filter at the moment, Yes k+1 The suboptimal state estimate of each sub-filter at time t, Yes k+1 The error covariance matrix corresponding to each sub-filter and the suboptimal state estimate at the moment, Yes k+1 The measurement matrix of each sub-filter at time instant, t k+1 The noise variance matrix of each sub-filter measurement at the moment is: For each sub-filter t k+1 The measurement information of the corresponding sensor at the time.

[0170] For the main filter, since only the Kalman filter time update is performed, no measurement update is required. The centroid state prior estimate of the main filter obtained by time update is taken. and its prior error covariance matrix As the suboptimal state estimate of the main filter and the error covariance matrix corresponding to the suboptimal state estimate Right now:

[0171]

[0172] (5) Optimal fusion part, based on the suboptimal state estimation of each filter and the corresponding error covariance matrix The global optimal quality center state estimation vector and the corresponding global error covariance matrix can be calculated as follows:

[0173]

[0174] in t k+1 The global error covariance matrix of the centroid state estimator at time instant, t k+1The global optimal centroid state estimation vector of the centroid state estimator at a moment.

[0175] It is the output of the centroid state estimator, which can be used as the feedback input for the stable control of the biped robot's walking to maintain the stability of the robot's walking, and can also be used to correct the actual motion state of the robot in real time during the motion navigation path planning of the biped robot.

[0176] When the biped robot is still walking, as time progresses forward, the information distribution, time update, measurement update, and optimal fusion steps for the next moment are carried out to obtain the global optimal centroid state estimation vector for the next moment. When the biped robot stops walking, the walking centroid state estimator stops working.

[0177] The described embodiments are the preferred embodiments of the present invention, but the present invention is not limited to the above embodiments. Without departing from the essential content of the present invention, any obvious improvements, substitutions, or variations that those skilled in the art can make all fall within the protection scope of the present invention.

Claims

1. A method for estimating the centroid state of a biped robot walking based on federated Kalman filter, characterized in that, including: Inertial filter, which processes measurement information of an inertial measurement unit Through Kalman filter time update and measurement update, a sub-optimal centroid state estimation vector is obtained and the corresponding error covariance matrix P 1 ; Kinematics filter, which processes the measurement information of each joint encoder Through the time update and measurement update of the Kalman filter, a sub-optimal centroid state estimation vector is obtained and the corresponding error covariance matrix P 2 ; Linear inverted pendulum filter, which processes the measurement information of the force sensor Through Kalman filter time update and measurement update, obtain the sub-optimal centroid state estimation vector and the corresponding error covariance matrix P 3 ; The main filter performs the Kalman filter time update to obtain the sub-optimal centroid state estimation vector and the corresponding error covariance matrix P m ; The main filter performs optimal fusion based on the above-mentioned sub-filters, its own sub-optimal centroid state estimation vector, and error covariance matrix to obtain the optimal centroid state estimation vector at the current moment and the corresponding error covariance matrix P g , the acts on the robot; Using the said P g and the information distribution coefficient β to perform feedback reset on each filter, repeating the above steps to obtain the optimal centroid state estimation vector at the next moment, and continuing to act on the robot; Repeat the above process until the robot stops walking. The information distribution coefficients include the information distribution coefficients of each sub-filter and the information distribution coefficient of the main filter. The information distribution coefficient of the main filter is a constant value, and the information distribution coefficients of each sub-filter satisfy: Wherein: is the fault tolerance information allocation coefficient, is the precision information allocation coefficient, β m is the main filter allocation coefficient, ρ is the weight allocation factor, and 0 ≤ ρ ≤ 1; The fault-tolerant information distribution coefficient satisfies: where cond(R i ) is the condition number of the measurement noise variance matrix R i ; The accuracy information distribution coefficient satisfies: where ||P i || F is the Frobenius norm of the error covariance matrix P i of the i-th sub-filter.

2. The method for estimating the centroid state of a biped robot walking according to claim 1, characterized in that, The sub-optimal centroid state estimation vector is obtained by the following method: Wherein: is the Kalman gain of each sub-filter at time t k+1 ; is the sub-optimal state estimate of each sub-filter at time t k+1 ; is the error covariance matrix corresponding to the sub-optimal state estimate of each sub-filter at time t k+1 ; is the measurement matrix of each sub-filter at time t k+1 ; is the measurement noise variance matrix of each sub-filter at time t k+1 ; is the measurement information of the corresponding sensor of each sub-filter at time t k+1 ; and are respectively the prior estimate of the centroid state and the prior estimate error covariance matrix of the centroid state of each sub-filter at time t k+1 .

3. The method for estimating the centroid state of a biped robot walking according to claim 2, characterized in that, The optimal fusion is carried out in the following way: Wherein: is the global error covariance matrix of the centroid state estimator at time t, k+1 and is the globally optimal centroid state estimation vector of the centroid state estimator at time t. k+1 ​ 4. The method for estimating the centroid state of a biped robot walking according to claim 1, characterized in that, The measurement information of the inertial measurement unit is: wherein and are components in the X and Y directions of the world coordinate system, and: Wherein: is the acceleration measurement vector in the world coordinate system after filtering at time t k ; is the acceleration measurement vector in the world coordinate system after filtering at time t k-1 ; w a k is the acceleration measurement vector of the inertial measurement unit at time t in the world coordinate system, μ is the first-order inertial filtering coefficient of the measured acceleration, and 0 < μ < 1. k ​ 5. The method for estimating the centroid state of a biped robot walking according to claim 1, characterized in that, The measurement information of each joint encoder is: wherein and are components in the X and Y directions, and are components in the X and Y directions, the is the measured value of the centroid position, is the measured value of the centroid velocity, and: in: w P foot (t k ) is t k The position of the supporting leg foothold in the world coordinate system at all times, R k Yes k is the attitude rotation matrix of the local coordinate system of the supporting leg foothold relative to the world coordinate system at the moment, T is the discrete period, and ForwardKinematic(·) is the forward kinematic calculation function of the single leg of the biped robot obtained based on the Denavit-Hartenberg method.

6. The method for estimating the centroid state of a biped robot walking according to claim 1, characterized in that, The measurement information of the force sensor is: where: g is the constant value of gravitational acceleration, z c is the constant value of the height of the center of mass, is the planar component of the measurement of the position of the center of mass , w p zmp (t k ) is the position of the zero moment point in the world coordinate system at time t k .

7. The method for estimating the centroid state of a biped robot walking according to claim 6, characterized in that, The t k The ZMP position in the world coordinate system at the moment satisfies: Wherein: is the position of the left foot sole in the world coordinate system at time t, k is the position of the right foot sole in the world coordinate system at time t, α(t k k ) is the distribution ratio of the vertical ground reaction forces on both feet at time t, and: k is at time t k and the vertical ground reaction forces on both feet at this time, and: Wherein: and are respectively the numerical values of the ground reaction forces in the Z direction of the world coordinate system of the left foot and the right foot after first-order inertial filtering at time t k ​ 8. The method for estimating the centroid state of a biped robot walking according to claim 1, characterized in that, The main filter and each sub-filter perform time update: where is the prior estimate of the centroid state of each filter at time t, P k+1 i k+1,k is the prior estimate of the centroid state of each filter at time t k+1 and P is the prior estimate error covariance matrix of the centroid state of each filter at time t, F i k+1,k is the system matrix of each filter, G i k+1,k is the control matrix of each filter, is the control input of each filter at time t k is t k and Γ is the error covariance matrix corresponding to the sub-optimal state estimate of each filter at time t i k is the noise driving matrix of each filter at time t k is the process noise variance matrix of each filter.​​​ 9. The method for estimating the centroid state of a biped robot walking according to claim 8, characterized in that, The system matrix of each filter is: where: T is the discrete period; The control matrix of each filter is:

Images