“盲狗”具身定位：最新本体感知足式机器人里程计

论文标题：

Multi-IMU Proprioceptive Odometry for Legged Robots

论文作者：

Shuo Yang, Zixin Zhang, Benjamin Bokser, Zachary Manchester

github仓库：

https://github.com/ShuoYangRobotics/Multi-IMU-Proprioceptive-Odometry

1. 摘要

此论文提出了一种四足机器狗的多节点IMU本体感知定位方法。在传统的单一体IMU与机器狗关节编码器的基础上，论文作者添加了足部多节点IMU辅助估计。论文使用了扩展卡尔曼滤波的信息融合方法进行状态估计。使用额外的足部多节点IMU，系统可以估计机器狗足部触地的多种模式以及打滑情况，而不需要足部的压力传感器。所提出的方法在硬件平台上进行了验证，表明了该方法相较于传统方法确实可以大幅降低定位漂移，所增加的计算量也十分有限。

2. 引言

为了响应速度指令以及在高挑战场景下的路径规划，足式机器人需要精确的位姿与速度估计。在很多应用场景中，状态估计只能依赖机器人上装载的传感器，这是由于外部的GPS以及动捕系统常常会失效。

一个常见的足式机器人在线状态估计系统是本体感知。本体感知指的是用IMU以及机器人腿部的传感器，例如关节处的编码器以及足部的接触传感器，去估计机器人的体位姿与速度状态。然而，在传感器精度有限的足式机器人平台上，本体感知的定位漂移很严重，往往定位误差会超过10%。这是由于低成本传感器的噪声以及错误的足部触地检测。

在最近的研究中，其他例如视觉、激光雷达传感器被应用以提升机器人的状态估计精度。视觉、激光雷达传感器可以有效降低定位漂移，有的方法可以将定位漂移降低至 0.5%。然而，这些方法会带来很大的额外成本、系统复杂度的提升以及处理图像以及点云数据的计算资源消耗。在低成本的轻量足式机器人平台上，这些视觉、雷达传感器方法并不适用。

论文的目标就是开发一种本体感知方法，它能够在增添有限的硬件以及计算资源上实现高精度定位。为了实现这个目标，论文提出了多节点IMU本体感知里程计，这个方法使用了多节点IMU以及机器人的关节编码器去提升定位精度。相较于传统的视觉以及激光雷达传感器方法，IMU的成本更低，消耗的能量更低，体积较小。因此，在机器人足部添加额外节点IMU传感器，并不会对机器人的整体设计造成很大的影响。为了融合多节点IMU信息，论文将机器人足部的位置以及速度加入了扩展卡尔曼滤波框架中，并设计了相应的预测以及量测模型。更加重要的是，论文使用足部的IMU进行触地以及打滑检测，克服了在传统本体感知中足部触地时速度为0的不合理假设。

论文的状态估计方法在四足机器人平台上进行了验证，并且与多种已有的方法进行了比较。此论文的具体贡献点如下：

●提出了一种多节点IMU本体感知足式机器人里程计方法。

●提出了针对足部IMU节点的预测以及量测模型，能够检测触地以及打滑情况。

●在硬件平台了进行了消融实验验证，验证了此论文方法降低定位漂移的有效性。

3. 相关工作

足式机器人的状态估计问题最近收到了广泛关注。随着低成本四足机器人的商业化程度越来越高，对于在有限资源平台上进行可靠的感知定位方法不可或缺。

最早的足式机器人状态估计方法使用的是低成本消费级传感器，包括一个IMU、关节编码器以及足部触地传感器。由于这些方法的传感器全部包含在机器人体内，因此这些方法被称为本体感知里程计（PO）。一个简化版本的PO应用在了MIT Cheetah 3 robot上，此状态估计器只估计了身体速度以及足部的位置，因此该估计器是线性的。为了改善旋转估计，一种不变滤波的框架被剔除。尽管很多不同形式的滤波方法被提出，但是他们全部都使用了同样形式和状态的传感器。

对于本体感知的传感器，速度估计往往能够满足稳定的闭环控制需求，但是定位的漂移仍然会达到10%-15%。本体感知定位误差的主要来源就是机器人足部触地时的零速假设。然而，在实际情况中，在触地时，机器人的足部可能会打滑、变形或者滚动。但是，由于不是所有的足式机器人控制应用都需要高精度的位置估计，因此PO还是被广泛应用。

除了本体上的传感器，相机和激光雷达在SLAM领域中的性能效果很好。以外部视觉为基础的定位方法已经成功应用于足式机器人上：一种松耦合的视觉惯性及足式里程计被应用于波士顿动力的LS3机器人上，他们使用的是战术级IMU、高分辨率的相机以及基于FPGA的同步板，能够达到小于 1%的定位漂移。另外，一种基于因子图的融合IMU、编码器以及视觉观测的方法也被提出，而且在加入激光雷达传感器后，定位漂移可以被进一步降低至0.2%-0.4%。相机也被证明可以消除PO中的一些误差从而提升定位精度。但是，由于视觉和雷达会产生高带宽的数据，因此这些方法的计算复杂度天然非常高。

使用多节点IMU改善状态估计在机器人领域非常常见。例如在行人定位系统、AR/VR领域，在这些系统中，多节点的IMU往往装载在同一刚体上。最后，使用多节点IMU状态在人形机器人的不同关节部位上的传感器网络被提出，然后它所提出的目的是为了更好的关节速度估计，并不是为了位置估计。

4. 数学背景

这一小节介绍了论文中的一些数学符号表示。首先论文定义反对称操作符：
$$\begin{array}{c}\lfloor \mathbf{v}{\rfloor }^{\times }=\left[\begin{array}{ccc}0& -v_3& v_2\\ v_3& 0& -v_1\\ -v_2& v_1& 0\end{array}\right]\end{array}$$

4.1 欧拉旋转角表示

论文中定义$\theta =\left[{\theta }_r;{\theta }_p;{\theta }_y\right]$为欧拉角，分别为roll，pitch，yaw角。其对应的旋转矩阵定义为：

$$R(\theta )=\left[\begin{array}{ccc}{c}_p{c}_y& c_y{c}_p{c}_r-c_r{c}_y& c_r{c}_y+c_r{c}_y{c}_p\\ c_p{c}_y& c_r{c}_y+c_p{c}_r{c}_y& c_r{c}_p{c}_y-c_y{c}_r\\ -c_p& c_p{c}_r& c_p{c}_r\end{array}\right]$$ 左右滑动查看完整公式

对于IMU所直接采集到的体系加速度和角速度记作，由此世界系下的加速度以及角速度可以表示为：
$${\mathbf{a}}_w=R(\theta ){\mathbf{a}}_b-{\mathbf{g}}_w$$
θ ˙ = [ θ ˙ r θ ˙ p θ ˙ y ] = Ω ( θ ) ω b = [ 1