Adaptive Robust Invariant Extended Kalman filtering for Biped Robot*

The precise estimation of system states is essential for the locomotion control of biped robots to maintain balance. Currently, the estimation of system states is either based on vision data that is susceptible to the environment, or fusing data from Inertial Measurement Unit (IMU) and the kinematic calculation. Invariant Extended Kalman Filtering (IEKF) is one of the most successful fusing algorithms to estimate system state. Generally, in IEKF, the noise covariance of system state is assumed to be known. However, the noise covariance of contact point for biped is not available since the ground-contact situation normally varies and not previously known. This paper presents a new fusing algorithm-Adaptive Robust Invariant Extended Kalman Filtering (ARIEKF) to adaptively adjust the noise parameter of contact point. The proposed algorithm applied the principle of robust estimation to resist outlier effects of state, and introduced an adaptive factor for the noise covariance of state to control its outlying disturbance influences. This paper firstly completed the full state estimation of biped robot using the theory of Lie groups and invariant observer. Then, the adaptive scale factor evaluated by three-segment approach was adopted to adjust the noise covariance of contact point. Finally, both IEKF and proposed ARIEKF are applied to our biped robot-Cosmos and the accuracy of two algorithms are compared. The mean square errors of the velocity of two algorithms were evaluated using the measurements from motion capture system. Experiments demonstrated that the mean square errors of the velocity are decreased 50 percent when compared with IEKF.