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

论文标题：

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}?\mathbf{v}{?}^{\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}\text{\hspace{1em}?}$$

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$$
$$\dot{\theta }=\left[\begin{array}{c}{\dot{\theta }}_r\\ {\dot{\theta }}_p\\ {\dot{\theta }}_y\end{array}\right]=\Omega (\theta ){\omega }_b=\left[\begin{array}{ccc}1& \left(s_p{s}_r\right)/c_p& \left(c_r{s}_p\right)/c_p\\ 0& c_r& ?s_r\\ 0& s_r/c_p& c_r/c_p\end{array}\right]{\omega }_b.$$左右滑动查看完整公式

4.2 前向运动学与腿部里程计

论文定义$?$为包含所有机器人关节角度的向量。记前向运动学函数为${\mathbf{p}}_f=g(?)\in {\mathbb{R}}^3$。

其中$P_f$是机器人足部在体系下的位置。记$J(?)$为关节角度对于速度的雅可比矩阵，由此可以得到：
$${\mathbf{v}}_f={\dot{\mathbf{p}}}_f=J(?)\dot{?}$$

进一步，我们得到机器人足部位置与身体在世界系下的位置关系如下：

$${\mathbf{p}}_f^w=\mathbf{p}+R_b^w{\mathbf{p}}_f=\mathbf{p}+R_b^{w}g(?)$$

在PO中，对于不打滑的足部触地情况，一个最基本的假设就是触地时足部速度为0。也就是上式求导为0。由此我们可以得到在触地时，世界系下的速度与角速度应该满足：
$$\mathbf{v}=?R_b^w\left[J(?)\dot{?}+?\omega {?}^{\times }g(?)\right]$$

这个速度就被称为腿部里程计的速度。

4.3 扩展卡尔曼滤波框架

扩展卡尔曼滤波主要由预测模型和量测模型构成，通过计算卡尔曼增益达到最优估计。论文中做了简要介绍。论文中预测模型记为：$${\mathbf{x}}_{k+1}=f\left({\mathbf{x}}_k\right)+{\mathbf{n}}_k$$
量测模型为：$${\mathbf{y}}_k=h\left({\mathbf{x}}_k\right)+{\mathbf{w}}_k$$
扩展卡尔曼滤波是卡尔曼滤波的线性化，则需要对预测和量测模型进行雅可比矩阵求解，记：$$F_k=?f/?{\mathbf{x}}_k,H_k=?h/?{\mathbf{x}}_k$$
记量测残差为${\mathbf{z}}_{k+1}={\bar{\mathbf{y}}}_k?h\left({\hat{\mathbf{x}}}_{k+1\mid k}\right)$，则计算完卡尔曼增益后，更新的最优状态为：
$${\hat{\mathbf{x}}}_{k+1}={\hat{\mathbf{x}}}_{k+1\mid k}+K_{k+1}{\mathbf{z}}_{k+1}$$

4.4 标准单IMU本体PO

标准PO定义状态为：$$\mathbf{x}=\left[\mathbf{p};\mathbf{v};\theta ;{\mathbf{s}}_1,\dots ,{\mathbf{s}}_j,\dots ,{\mathbf{s}}_L\right]$$

包含机器人的世界系位置、速度、旋转以及$L$条腿的足部位置。

在标准PO中，估计器的预测模型为：

$${\hat{\mathbf{x}}}_{k+1}=\left[\begin{array}{c}{\hat{\mathbf{p}}}_{k+1}\\ {\hat{\mathbf{v}}}_{k+1}\\ {\hat{\theta }}_{k+1}\\ {\hat{\mathbf{s}}}_{k+1}\end{array}\right]=\left[\begin{array}{c}{\hat{\mathbf{p}}}_k+\Delta t{\hat{\mathbf{v}}}_k\\ {\hat{\mathbf{v}}}_k+\Delta t\left(R\left({\hat{\theta }}_k\right){\mathbf{a}}_b?{\mathbf{g}}_w\right)\\ {\hat{\theta }}_k+\Delta t\left(\Omega \left({\hat{\theta }}_k\right){\omega }_b\right)\\ {\hat{\mathbf{s}}}_k\end{array}\right]$$左右滑动查看完整公式

在标准的PO中，只有当足部触地的时刻才能够更新体系的速度，因此，状态变量中的对足部位置的量测模型协方差通常设为以下切换形式：
$${\sigma }_s=c{\sigma }_c+(1?c){\sigma }_n$$

其中$c$为触地标志，若触地，那么协方差正常设置，若不触地则设置协方差很大，使得量测模型对更新不起作用。

对于观测值，标准PO可由关节角度与体系角速度计算得到：
$${\overline{\mathbf{y}}}_k=\left[\begin{array}{c}g(?)\\ ?J(?)\dot{?}+{?{\omega }_b?}^{\times }g(?)\end{array}\right]$$

系统的量测模型为：

$$h\left({\hat{\mathbf{x}}}_k\right)=\left[\begin{array}{c}R{\left({\hat{\theta }}_k\right)}^T\left({\hat{\mathbf{s}}}_k?{\hat{\mathbf{p}}}_k\right)\\ R{\left({\hat{\theta }}_k\right)}^T{\hat{\mathbf{v}}}_k\end{array}\right]$$

值得注意的是，标准PO中的观测假设为触地点速度为0，若此假设不满足，则整体量测模型就会失效。

5. 涉及方法

5.1 状态定义

在所提出的方法中，将足部的速度加入了状态变量，这是由于在足部装载了额外节点的IMU，因此可以获得足部的连续状态。所提出的状态定义与预测模型如下：
$${\hat{\mathbf{x}}}_{k+1}=\left[\begin{array}{c}{\hat{\mathbf{p}}}_{k+1}\\ {\hat{\mathbf{v}}}_{k+1}\\ {\hat{\theta }}_{k+1}\\ {\hat{\mathbf{s}}}_{k+1}\\ {\dot{\hat{\mathbf{s}}}}_{k+1}\end{array}\right]=\left[\begin{array}{c}{\hat{\mathbf{p}}}_k+\Delta t{\hat{\mathbf{v}}}_k\\ {\hat{\mathbf{v}}}_k+\Delta t\left(R\left({\hat{\theta }}_k\right){\mathbf{a}}_b?{\mathbf{g}}_w\right)\\ {\hat{\theta }}_k+\Delta t\left(\Omega \left({\hat{\theta }}_k\right){\omega }_b\right)\\ {\hat{\mathbf{s}}}_k+\Delta t\dot{\hat{\mathbf{s}}}\\ {\dot{\hat{\mathbf{s}}}}_k+\Delta t\left(R\left({\hat{\theta }}_k\right)R_f^b(?){\mathbf{a}}_f?{\mathbf{g}}_w\right)\end{array}\right]$$左右滑动查看完整公式

5.2 触地转动模型

对于足式机器人来说，在足部触地瞬间，机器人的身体是绕着接触点转动的，无论触地的足部是静止还是转动的。因此，触地足部的线速度应当为足部角速度向量与转动向量的叉积，转动向量的长度为足部末端到足部IMU的距离，方向为足部低端到机器人的质心距离。其计算如下：

$$\begin{array}{rl}\omega \left({\hat{\mathbf{x}}}_k,?,{\omega }_f\right)& =R\left({\hat{\theta }}_k\right)R_f^b(?){\omega }_f,\\ \mathbf{d}\left({\hat{\mathbf{x}}}_k,?\right)& =?d_0?\mathbf{n}/\Vert \mathbf{n}\Vert \end{array}$$左右滑动查看完整公式

图1 | 左图描述了机器狗足部触地时的转动模型。右图为实验验证，证明了足部触地转动模型的有效性。??【深蓝AI】编译

5.3 量测模型

对于足部转动模型的量测，论文设计如下：

$$h\left({\hat{\mathbf{x}}}_k,?,{\omega }_f\right)=\left[\begin{array}{c}R{\left({\hat{\theta }}_k\right)}^T\left({\hat{\mathbf{s}}}_k?{\hat{\mathbf{p}}}_k\right)\\ R{\left({\hat{\theta }}_k\right)}^T\left({\hat{\mathbf{v}}}_k?{\dot{\hat{\mathbf{s}}}}_k\right)\\ {\dot{\hat{\mathbf{s}}}}_k?\omega \times \mathbf{d}\end{array}\right]$$

论文提出的量测模型，在标准PO的基础上，考虑了足部触地时的转动速度。量测的最后一项理论上应当为0，因此在此作为隐式的量测融合在系统中。

5.4 足部触地与打滑检测

论文将标准PO中使用足部接触传感器的触地检测方式改进为如下马氏距离的形式：

$$\sqrt{z^T{S}^{?1}z}<\sigma$$

其中$z=\dot{\hat{\mathbf{s}}}?\omega \times \mathbf{d}$，$S$为卡尔曼滤波中的协方差。如果足部转动模型成立，则马氏距离较小，可以认为足部接触地面并没有打滑，正常更新。若马氏距离过大，说明此时足部并未着地或有显著打滑，不用此量测更新。

6. 实验结果

论文实验的硬件平台为宇树机器狗Go1，体IMU使用的时Go1体内的内置MEMS IMU，关节处编码器数据可以由以太网传输获得。此外，本文在机器狗的足端安装四个了MPU9250 IMU。实验表明，论文所提出的多节点IMU以及足部触地转动模型确实能够有效降低定位漂移。而且对计算资源的消耗也没有显著增加。

图2｜图中展示了标准PO与论文所提出的MIPO在室内定位的结果，可以观察到所提出的MIPO可以显著降低定位漂移。轨迹全长10.5 m. 标准PO定位平均漂移为11.39%. 所提出的MIPO定位平均漂移仅为 2.31%。??【深蓝AI】编译

表1｜表中展示了标准PO以及MIPO的计算开销与定位漂移结果，表明了MIPO计算开销增加很少但是能够大幅降低定位漂移。??【深蓝AI】编译

此外，论文还进行了消融实验。分别对所提出的多节点策略、足部触地转动模型以及触地打滑检测进行了消融验证，验证了其模块的有效性。

图3 | 图中展示了消融实验结果。除了对比标准PO外，论文还对所提出模块消融进行了对比，结果表明所提出的MIPO具有最小的定位漂移。??【深蓝AI】编译

7. 总结

本论文提出了一种多节点IMU本体感知足式机器人里程计，利用额外的足部IMU节点，构建了足部触地转动模型与触地打滑检测方法，提升了本体感知的定位精度。

编译｜Frank Xiong
审核｜Los
移步公众号【深蓝AI】，第一时间获取自动驾驶、人工智能与机器人行业最新最前沿论文和科技动态👇
“盲狗”具身定位：最新本体感知足式机器人里程计｜深蓝AI