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

论文标题：

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. 数学背景

这一小节介绍了论文中的一些数学符号表示。首先论文定义反对称操作符：
⌊ v ⌋ × = [ 0 − v 3 v 2 v 3 0 − v 1 − v 2 v 1 0 ] \begin{equation} \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{equation} ⌊v⌋×= ​0v3​−v2​​−v3​0v1​​v2​−v1​0​ ​​​

4.1 欧拉旋转角表示

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

R ( θ ) = [ 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 ] 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] R(θ)= ​cp​cy​cp​cy​−cp​​cy​cp​cr​−cr​cy​cr​cy​+cp​cr​cy​cp​cr​​cr​cy​+cr​cy​cp​cr​cp​cy​−cy​cr​cp​cr​​ ​ 左右滑动查看完整公式

对于IMU所直接采集到的体系加速度和角速度记作，由此世界系下的加速度以及角速度可以表示为：
$${\mathbf{a}}_w=R(\theta ){\mathbf{a}}_b-{\mathbf{g}}_w$$
θ ˙ = [ θ ˙ r θ ˙ p θ ˙ y ] = Ω ( θ ) ω b = [ 1 ( s p s r ) / c p ( c r s p ) / c p 0 c r − s r 0 s r / c p c r / c p ] ω b . \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. θ˙= ​θ˙r​θ˙p​θ˙y​​ ​=Ω(θ)ωb​= ​100​(sp​sr​)/cp​cr​sr​/cp​​(cr​sp​)/cp​−sr​cr​/cp​​ ​ωb​.左右滑动查看完整公式

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

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

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

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

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

在PO中，对于不打滑的足部触地情况，一个最基本的假设就是触地时足部速度为0。也就是上式求导为0。由此我们可以得到在触地时，世界系下的速度与角速度应该满足：
$$\mathbf{v}=-R_b^w\left[J(\varphi )\dot{\varphi }+\lfloor \omega {\rfloor }^{\times }g(\varphi )\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=\partial f/\partial {\mathbf{x}}_k,H_k=\partial h/\partial {\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中，估计器的预测模型为：

x ^ k + 1 = [ p ^ k + 1 v ^ k + 1 θ ^ k + 1 s ^ k + 1 ] = [ p ^ k + Δ t v ^ k v ^ k + Δ t ( R ( θ ^ k ) a b − g w ) θ ^ k + Δ t ( Ω ( θ ^ k ) ω b ) s ^ k ] \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] x^k+1​= ​p^​k+1​v^k+1​θ^k+1​s^k+1​​ ​= ​p^​k​+Δtv^k​v^k​+Δt(R(θ^k​)ab​−gw​)θ^k​+Δt(Ω(θ^k​)ωb​)s^k​​ ​左右滑动查看完整公式

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

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

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

系统的量测模型为：

h ( x ^ k ) = [ R ( θ ^ k ) T ( s ^ k − p ^ k ) R ( θ ^ k ) T v ^ k ] 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{\mathbf{\theta}}_k\right)^T \hat{\mathbf{v}}_k\end{array}\right] h(x^k​)= ​R(θ^k​)T(s^k​−p^​k​)R(θ^k​)Tv^k​​ ​

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

5. 涉及方法

5.1 状态定义

在所提出的方法中，将足部的速度加入了状态变量，这是由于在足部装载了额外节点的IMU，因此可以获得足部的连续状态。所提出的状态定义与预测模型如下：
x ^ k + 1 = [ p ^ k + 1 v ^ k + 1 θ ^ k + 1 s ^ k + 1 s ^ ˙ k + 1 ] = [ p ^ k + Δ t v ^ k v ^ k + Δ t ( R ( θ ^ k ) a b − g w ) θ ^ k + Δ t ( Ω ( θ ^ k ) ω b ) s ^ k + Δ t s ^ ˙ s ^ ˙ k + Δ t ( R ( θ ^ k ) R f b ( ϕ ) a f − g w ) ] \hat{\mathbf{x}}_{k+1}=\left[\begin{array}{c}\hat{\mathbf{p}}_{k+1} \\ \hat{\mathbf{v}}_{k+1} \\ \hat{\mathbf{\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{\mathbf{\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({\phi}) \mathbf{a}_f-\mathbf{g}_w\right)\end{array}\right] x^k+1​= ​p^​k+1​v^k+1​θ^k+1​s^k+1​s^˙k+1​​ ​= ​p^​k​+Δtv^k​v^k​+Δt(R(θ^k​)ab​−gw​)θ^k​+Δt(Ω(θ^k​)ωb​)s^k​+Δts^˙s^˙k​+Δt(R(θ^k​)Rfb​(ϕ)af​−gw​)​ ​左右滑动查看完整公式

5.2 触地转动模型

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

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

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

5.3 量测模型

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

h ( x ^ k , ϕ , ω f ) = [ R ( θ ^ k ) T ( s ^ k − p ^ k ) R ( θ ^ k ) T ( v ^ k − s ^ ˙ k ) s ^ ˙ k − ω × d ] h\left(\hat{\mathbf{x}}_k, {\phi}, {\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] h(x^k​,ϕ,ωf​)= ​R(θ^k​)T(s^k​−p^​k​)R(θ^k​)T(v^k​−s^˙k​)s^˙k​−ω×d​ ​

论文提出的量测模型，在标准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