Contact-aided invariant extended Kalman filtering for robot state estimation
Contact-aided invariant extended Kalman filtering for robot state estimation
复制标题
DOI：
10.1177/0278364919894385
复制
发表时间：
2020-01-16
影响因子：
9.2
通讯作者：
Grizzle, Jessy W.
中科院分区：
文献类型：
--
作者：
Hartley, Ross;Ghaffari, Maani;Grizzle, Jessy W.
腿部机器人需要知道姿势和速度，才能保持稳定并执行行走路径。目前的解决方案要么依赖视觉数据，这对环境和照明条件很敏感，要么将运动学和接触数据与惯性测量单元(IMU)的测量融合在一起。在这项工作中，我们利用李群理论和不变观测器设计，提出了一种接触辅助不变扩展卡尔曼滤波(InEKF)。该过滤器将接触惯性动力学与正向运动学校正相结合，以估计姿态和速度以及所有当前接触点。我们证明了误差动力学遵循对数线性自治微分方程且具有几个重要结果：(A)可观测状态变量可以收敛于独立于系统轨迹的吸引域；(B)与标准扩展卡尔曼滤波不同，线性化的误差动力学和线性化的观测模型都不依赖于当前状态估计，这(C)导致了更好的收敛性质；(D)局部可观测性矩阵与潜在的非线性系统一致。此外，我们还演示了如何包含IMU偏差、添加/删除联系人以及制定以世界为中心的版本和以机器人为中心的版本。通过仿真和在CASSIE串联两足机器人上的实验，比较了InEKF和常用的基于四元数的扩展卡尔曼滤波(EKF)的收敛性能。使用运动捕捉分析了滤波精度，并通过LiDAR映射实验提供了一个实际应用案例。总体而言，由于利用了系统中存在的对称性，所开发的接触式辅助InEKF比基于四元数的EKF具有更好的性能。
Legged robots require knowledge of pose and velocity in order to maintain stability and execute walking paths. Current solutions either rely on vision data, which is susceptible to environmental and lighting conditions, or fusion of kinematic and contact data with measurements from an inertial measurement unit (IMU). In this work, we develop a contact-aided invariant extended Kalman filter (InEKF) using the theory of Lie groups and invariant observer design. This filter combines contact-inertial dynamics with forward kinematic corrections to estimate pose and velocity along with all current contact points. We show that the error dynamics follows a log-linear autonomous differential equation with several important consequences: (a) the observable state variables can be rendered convergent with a domain of attraction that is independent of the system's trajectory; (b) unlike the standard EKF, neither the linearized error dynamics nor the linearized observation model depend on the current state estimate, which (c) leads to improved convergence properties and (d) a local observability matrix that is consistent with the underlying nonlinear system. Furthermore, we demonstrate how to include IMU biases, add/remove contacts, and formulate both world-centric and robo-centric versions. We compare the convergence of the proposed InEKF with the commonly used quaternion-based extended Kalman filter (EKF) through both simulations and experiments on a Cassie-series bipedal robot. Filter accuracy is analyzed using motion capture, while a LiDAR mapping experiment provides a practical use case. Overall, the developed contact-aided InEKF provides better performance in comparison with the quaternion-based EKF as a result of exploiting symmetries present in system.