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

论文标题：

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−v30v1v2−v10

4.1 欧拉旋转角表示

论文中定义 θ = [ θ r ; θ p ; θ y ] \mathbf{\theta}=\left[{\theta}_r ; \theta_p ; \theta_y\right] θ=[θr;θp;θy]为欧拉角，分别为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(θ)= cpcycpcy−cpcycpcr−crcycrcy+cpcrcycpcrcrcy+crcycpcrcpcy−cycrcpcr 左右滑动查看完整公式

对于IMU所直接采集到的体系加速度和角速度记作，由此世界系下的加速度以及角速度可以表示为：
a w = R ( θ ) a b − g w \mathbf{a}_w=R({\theta}) \mathbf{a}_b-\mathbf{g}_w aw=R(θ)ab−gw
θ ˙ = [ θ ˙ 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(spsr)/cpcrsr/cp(crsp)/cp−srcr/cp ωb.左右滑动查看完整公式

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

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

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

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

p f w = p + R b w p f = p + R b w g ( ϕ ) \mathbf{p}_f^w=\mathbf{p}+R_b^w \mathbf{p}_f=\mathbf{p}+R_b^w g({\phi}) pfw=p+Rbwpf=p+Rbwg(ϕ)

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

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

4.3 扩展卡尔曼滤波框架

扩展卡尔曼滤波主要由预测模型和量测模型构成，通过计算卡尔曼增益达到最优估计。论文中做了简要介绍。论文中预测模型记为： x k + 1 = f ( x k ) + n k \mathbf{x}_{k+1}=f\left(\mathbf{x}_k\right)+\mathbf{n}_k xk+1=f(xk)+nk

量测模型为： y k = h ( x k ) + w k \mathbf{y}_k=h\left(\mathbf{x}_k\right)+\mathbf{w}_k yk=h(xk)+wk

扩展卡尔曼滤波是卡尔曼滤波的线性化，则需要对预测和量测模型进行雅可比矩阵求解，记： F k = ∂ f / ∂ x k , H k = ∂ h / ∂ x k F_k=\partial f / \partial \mathbf{x}_k, H_k=\partial h / \partial \mathbf{x}_k Fk=∂f/∂xk,Hk=∂h/∂xk

记量测残差为 z k + 1 = y ˉ k − h ( x ^ k + 1 ∣ k ) \mathbf{z}{k+1}=\bar{\mathbf{y}}k-h\left(\hat{\mathbf{x}}{k+1 \mid k}\right) zk+1=yˉk−h(x^k+1∣k)，则计算完卡尔曼增益后，更新的最优状态为：
x ^ k + 1 = x ^ k + 1 ∣ k + K k + 1 z k + 1 \hat{\mathbf{x}}{k+1}=\hat{\mathbf{x}}{k+1 \mid k}+K{k+1} \mathbf{z}_{k+1} x^k+1=x^k+1∣k+Kk+1zk+1

4.4 标准单IMU本体PO

标准PO定义状态为： x = [ p ; v ; θ ; s 1 , ... , s j , ... , s L ] \mathbf{x}=\left[\mathbf{p} ; \mathbf{v} ; {\theta} ; \mathbf{s}_1, \ldots, \mathbf{s}_j, \ldots, \mathbf{s}_L\right] x=[p;v;θ;s1,...,sj,...,sL]

包含机器人的世界系位置、速度、旋转以及 L L 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+1v^k+1θ^k+1s^k+1 = p^k+Δtv^kv^k+Δt(R(θ^k)ab−gw)θ^k+Δt(Ω(θ^k)ωb)s^k 左右滑动查看完整公式

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

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

对于观测值，标准PO可由关节角度与体系角速度计算得到：
y ‾ k = [ g ( ϕ ) − J ( ϕ ) ϕ ˙ + ⌊ ω b ⌋ × g ( ϕ ) ] \overline{\mathbf{y}}_k=\left[\begin{array}{c}g({\phi}) \\ -J({\phi}) \dot{{\phi}}+\left\lfloor{\omega}_b\right\rfloor^{\times} g({\phi})\end{array}\right] yk=[g(ϕ)−J(ϕ)ϕ˙+⌊ωb⌋×g(ϕ)]

系统的量测模型为：

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+1v^k+1θ^k+1s^k+1s^˙k+1 = p^k+Δtv^kv^k+Δt(R(θ^k)ab−gw)θ^k+Δt(Ω(θ^k)ωb)s^k+Δts^˙s^˙k+Δt(R(θ^k)Rfb(ϕ)af−gw) 左右滑动查看完整公式

5.2 触地转动模型

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

ω ( x ^ k , ϕ , ω f ) = R ( θ ^ k ) R f b ( ϕ ) ω f , d ( x ^ k , ϕ ) = − d 0 ⋅ n / ∥ n ∥ \begin{aligned} {\omega}\left(\hat{\mathbf{x}}_k, {\phi}, {\omega}_f\right) & =R\left(\hat{{\theta}}_k\right) R_f^b({\phi}) {\omega}_f, \\ \mathbf{d}\left(\hat{\mathbf{x}}_k, {\phi}\right) & =-d_0 \cdot \mathbf{n} /\|\mathbf{n}\|\end{aligned} ω(x^k,ϕ,ωf)d(x^k,ϕ)=R(θ^k)Rfb(ϕ)ωf,=−d0⋅n/∥n∥左右滑动查看完整公式

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中使用足部接触传感器的触地检测方式改进为如下马氏距离的形式：

z T S − 1 z < σ \sqrt{z^T S^{-1} z}<\sigma zTS−1z <σ

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

6. 实验结果

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

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

7. 总结

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

编译｜Frank Xiong

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