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

发布时间:2024年01月24日

论文标题:

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 欧拉旋转角表示

论文中定义 θ = [ θ 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(θ)= ?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所直接采集到的体系加速度和角速度记作,由此世界系下的加速度以及角速度可以表示为:
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?(sp?sr?)/cp?cr?sr?/cp??(cr?sp?)/cp??sr?cr?/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+Rbw?pf?=p+Rbw?g(?)

在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+1k?),则计算完卡尔曼增益后,更新的最优状态为:
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+1k?+Kk+1?zk+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+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中,只有当足部触地的时刻才能够更新体系的速度,因此,状态变量中的对足部位置的量测模型协方差通常设为以下切换形式:
σ 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] y?k?=[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+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的距离,方向为足部低端到机器人的质心距离。其计算如下:

ω ( 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?左右滑动查看完整公式

图1 | 左图描述了机器狗足部触地时的转动模型。右图为实验验证,证明了足部触地转动模型的有效性。??【深蓝AI】编译
图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中使用足部接触传感器的触地检测方式改进为如下马氏距离的形式:

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以及足部触地转动模型确实能够有效降低定位漂移。而且对计算资源的消耗也没有显著增加。

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

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

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

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

7. 总结

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

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

文章来源:https://blog.csdn.net/soaring_casia/article/details/135815125
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。