目录
提示:请使用 Firefox,Chrome,Edge 等较新的浏览器阅读,以获得完整的公式排版。IE 请使用 IE 11。
本文已授权「泡泡机器人 SLAM」微信公众号(paopaorobot_slam)发表。
0. 总览
IMU 是移动机器人、移动智能设备上常见的传感器。常见的 IMU 为六轴传感器,配备输出三轴加速度的加速度计和输出三轴角速度的陀螺仪。九轴 IMU 还会配备输出三轴姿态角的磁力计。我们这里只讨论六轴 IMU。
IMU 的状态量通常表示为:
XIMU=[IGq¯TbTgGvTIbTaGpTI](0.0)
这里我们使用和 MSCKF [1] 一样的 notation。用 {I} 表示 IMU 坐标系,{G} 表示参考坐标系。IMU 的姿态由旋转量 IGq¯
和平移量 GpI 表示。更具体来说,前者为将任意向量从 {G} 坐标映射到 {I} 坐标的旋转量,用单位四元数表示;后者为 IMU 在 {G} 下的三维位置。GvI 表示 IMU 在 {G} 下的平移速度。另外两个量 bg 和 ba
表示陀螺仪(角速度计)和加速度计的 bias。可以注意一下这里除了 bias 之外的状态量的时间维度:平移量表达到速度(p 和 v,对时间的一阶导),因为 IMU 只提供到加速度(对时间的二阶导)的测量;旋转量只表达姿态量(对时间的零阶导),因为 IMU 提供到角速度(对时间的一阶导)。状态量的估计可以由 IMU 测量积分得到。
对于 IMU 状态估计问题,需要提供运动模型、观测(噪声)模型、估计误差模型:
x˙=f(x)(0.1)
z=g(x)+n(0.2)
δx=e(x̂ ,x)(0.3)
这是一个通用模型,我们用 x
表示真实状态量(待估计,不可知),用 z 表示观测量,n 表示观测噪声,x̂ 表示当前的状态估计量。这篇小文主要讲 IMU (即 x:=XIMU
时)这三个模型的推导。
1. IMU 运动模型
1-1. 前置(1): 旋转量求导
这部分讲刚体动力学相关的前置知识,熟悉的读者可以跳过。
众所周知,一个刚体在同一个惯性坐标系下进行平移运动,其平移量对时间的一阶导和二阶导即速度和加速度:
p˙=v,v˙=a
对于旋转量以及非惯性系参考坐标系,情况稍微复杂些。
首先,如下图(左)所示,考虑一个从原点出发的向量 r
绕单位轴 u 旋转,角速度大小为 θ˙
。
角速度矢量可以表示为 ω=θ˙u
。易得向量 r 末端点 P 的速度矢量,即 r
的时间一阶导为
drdt=ω×r
现在考虑上图(右),坐标系 {B} 绕单位轴 u
旋转,如上所述,其三个轴的时间一阶导同样为
diBdt=ω×iB,djBdt=ω×jB,dkBdt=ω×kB
我们知道,[iBjBkB]
实际上就是坐标系 {B} 相对于参考坐标系的旋转矩阵 R。所以 R
的时间一阶导为
R˙=[ω×iBω×jBω×kB]=ω×R(1.0)
我们知道上面的叉乘运算可以转化为负对称矩阵的乘法:
R˙=ω∧R(1.1)
其中负对称矩阵为
ω∧=⎡⎣⎢⎢0ω3−ω2−ω30ω1ω2−ω10⎤⎦⎥⎥
注意这里的角速度 ω
是在参考坐标系下表达的。角速度也经常表达在体坐标系 {B} 下,记为 Bω=RTω,即 ω=RBω,于是 (1.1)
可以写作
R˙=(RBω)∧R(1.2)
这里我们要利用负对称矩阵的一个很好的性质:对任意旋转矩阵 R
和三维向量 v,都有 (Rv)∧=Rv∧RT(参看《(Rv)^ = Rv^R’ 的简单证明》),于是 (1.2)
可以写成
R˙=R(Bω)∧(1.3)
比较一下 (1.1)
和 (1.3)
,可以发现一个很有趣的事实,角速度如果表达在参考坐标系下,负对称矩阵写在左边;如果表达在体坐标系下,负对称矩阵写在右边。这点微小的区别,读者在阅读文献时可以特别留意。
1-2. 前置(2): 四元数
这部分讲四元数如何表示旋转的前置知识,熟悉的读者可以跳过。
用旋转矩阵来表示旋转很直观,但过于冗余,因为旋转只有三个自由度,而旋转矩阵有九个量。表征旋转还可以用欧拉角,但有万向锁问题,而且计算也不方便。旋转向量(即李代数 so(3))和四元数是更常用的表征方法,在惯性导航中四元数似乎更普遍些。这里采用四元数。
一个四元数由一个实部和三个虚部构成,书写顺序各家不同,这里和 MSCKF [1] 一样,虚部在前实部在后:
q=q1i+q2j+q3k+q4=[vTq4]T
虚部 v=[q1q2q3]T
。虚部三个基 i,j,k 满足 i2=j2=k2=ijk=−1。 四元数仍是一种冗余表达法,为了更紧凑,通常使用使用单元四元数 q¯
,通过将四元数的模直为 1 得到。
四元数和旋转向量有很直接的转换关系。绕单位轴 u
转了 θ
角度,用四元数表达为
q=[usinθ2cosθ2](1.4)
四元数乘法 ⊗
为类似于多项式乘法的逐项相乘:
q⊗p==(q1i+q2j+q3k+q4)(p1i+p2j+p3k+p4)(q1p4+q2p3−q3p2+q4p1)i+(−q1p3+q2p4+q3p1+q4p2)j+(q1p2−q2p1+q3p4+q4p3)k+(−q1p1−q2p2−q3p3+q4p4)
这个计算结果可以表达为多种形式:
q⊗p=[q4I3+v∧q−vTqvqq4][vpp4]=[p4I3−v∧p−vTpvpp4][vqq4]
四元数乘法和其对应的两个旋转矩阵相乘物理意义是一样的,即 R(q⊗p)=R(q)R(p)
。四元数对应的旋转矩阵为:
R(q)=(2q24−1)I3+2q4v∧+2vvT(1.5)
四元数的逆为 q−1=[−vTq4]T
。易得 q⊗q−1=[0001]T:=qI,故 qI
表示旋转量为零。
四元数对时间一阶导为
q˙=12[ω∧−ωTω0]q:=12Ω(ω)q=12[q4I3−v∧−vT]ω(1.6)
读者可能注意到了 (1.6)
和 (1.1) 形式上的相似。这里 ω 的意义也是一样的。(1.6)
的推导可以参考 [2],这里不赘述。
1-3. IMU 运动模型
有了前置知识的铺垫之后,我们可以给出 IMU 的运动模型:
IGq¯˙b˙gGv˙Ib˙aGp˙I=12Ω(ω)IGq¯=nwg=Ga=nwa=GvI(1.7)
IGq¯˙
由 (1.6) 直接得到。注意这里角速度 ω 是在体坐标系 {I} 下表达的,与 (1.1) 处相反。原因是 IGq¯ 表示的旋转方向与 (1.1) 处的 R
是相反的。其他的四项,速度和加速度都很简单,bias 两项在下面观测模型部分讲。
2. IMU 观测和噪声模型
2-1 前置(1): 科氏加速度
这部分在 1-1 的基础上,讨论参考坐标系不是惯性系的情况,熟悉科氏加速度的读者可以跳过。我们仍利用 1-1 中的图,但这次把绕惯性系 {A} 中固定单位轴 u
旋转的 {B} 作为参考坐标系。考虑下图, 点 P 相对于 {B} 运动,记 Br 分别为 P 在 {B} 下的坐标,r 为 P 的绝对坐标(即 {A} 下坐标), R 仍为 {B} 相对于 {A} 的旋转矩阵,易知 r=RBr
。
求一阶时间导,并利用公式 (1.1)
:
v=r˙=R˙Br+RBr˙=ω∧RBr+RBr˙
记 P 在 {B} 下速度为 Bv
,于是
v=ω∧r+RBv=ω×r+vr(2.0)
请注意,这里用 vr
来表达「相对速度」的概念,准确定义为 P 相对于 {B} 的速度,在惯性系 {A} 下的表达。请分清 vr、v 以及 Bv
三者之间的区别和联系。
再对 (2.0)
求时间导:
a=v˙=ω˙×r+ω×r˙+R˙Bv+RBv˙=α×r+ω×(ω×r+vr)+ω×RBv+RBa=α×r+ω×(ω×r)+2ω×vr+ar(2.1)
我们来逐项分析上面这个式子。第一项中 α
为 {B} 的角加速度,所以第一项的物理意义是 {B} 旋转所造成的 P 的切向加速度。第二项是 {B} 旋转所造成的向心加速度。第四项为 P 相对于 {B} 的加速度,但在惯性系 {A} 下表达——类似于 vr,定义相对加速度 ar
。第三项比较特殊,为 {B} 的旋转运动与 P 相对 {B} 的平移运动耦合产生的加速度,称为「科氏加速度」。可以看到,除了第四项外,另外三项都和 {B} 的旋转有关。
2-2 前置(2): 惯性导航相关坐标系定义
这部分讲惯性导航中经常出现的几个坐标系的定义 [5]。
Earth-Centered-Earth-Fixed (ECEF) Frame:地心地固坐标系 ECEF。以地心为坐标原点,向北为 z 轴,x-y 平面为赤道平面,x 轴指向经纬度 (0,0) 点。ECI 固连在地球上,跟随地球自转,非惯性坐标系。MSCKF 一代 [1] 使用 ECEF 为参考坐标系 {G}。
Earth-Centered-Inertial (ECI) Frame:地心惯性坐标系 ECI。以地心为坐标原点,向北为 z 轴,x-y 平面为赤道平面,x 轴指向春分点(vernal equinox point,即每年春分时日心-地心连线与赤道的交点)。ECI 不跟随地球自转,在惯性导航中视为惯性坐标系。MSCKF 二代 [3] 使用 ECI 为参考坐标系 {G}。
Body Frame:体坐标系。原点在导航体的质心,固连在导航体上,用来表示导航体的姿态。在本文前置推导部分为 {B},在 MSCKF 中为 {I}。
2-3 前置(3): 高斯白噪声与随机游走
这部分讲高斯白噪声和随机游走(random walk)模型,及其离散化。这部分在kalibr 库中的 IMU noise model [4] 有简单的介绍,这里在其基础上添加了离散化的推导,因为离散化中部分内容还是有些令人疑惑的。离散化的推导部分参考自 [5]。
先讲高斯白噪声。一个连续时间的高斯白噪声 n(t)
,满足以下两个条件
E[n(t)]=0E[n(t1)n(t2)]=σ2gδ(t1−t2)
其中 δ()
表示狄拉克函数。可以看出,不同时刻的高斯白噪声相互独立。σ2g
为方差,值越大,表示噪声程度越大。
将高斯白噪声离散化,可得到:
nd[k]=σgdw[k]
其中
w[k]∼(0,1)σgd=σgΔt‾‾‾√
其中 Δt
为采样时间。为什么离散化后分母会多出 Δt‾‾‾√ 这一项呢?我们假定在一个采样周期内 n(t)
为常数,于是
nd[k]≜n(t0+Δt)≃1Δt∫t0+Δtt0n(τ)dt
E(nd[k]2)=E(1Δt2∫t0+Δtt0∫t0+Δtt0n(τ)n(t)dτdt)=E(σ2gΔt2∫t0+Δtt0∫t0+Δtt0δ(t−τ)dτdt)=E(σ2gΔt)
所以有 σ2gd=σ2gΔt
,即 σgd=σgΔt√
。
接下来讨论随机游走模型。准确地讲,随机游走其实是一个离散模型,其连续模型称为维纳过程(Wiener Process)。维纳模型是高斯白噪声的积分:
b˙g(t)=n(t)=σbgw(t)
其中 w
为单位高斯白噪声。将其离散化后得到随机游走模型:
bd[k]=bd[k−1]+σbgdw[k]
其中
w[k]∼(0,1)σgd=σbgΔt‾‾‾√
这里多出来的 Δt‾‾‾√
又是哪来的呢?仍假定一个采样周期内高斯白噪声为常数,有:
bd[k]≜b(t0)+∫t0+Δtt0n(t)dt
E((bd[k]−bd[k−1])2)=E(∫t0+Δtt0∫t0+Δtt0n(t)n(τ)dτdt)=E(σ2bg∫t0+Δtt0∫t0+Δtt0δ(t−τ)dτdt)=E(σ2bgΔt)
所以有 σ2bgd=σ2bgΔt
,即 σbgd=σbgΔt‾‾‾√
。
于是我们得到随机游走模型的完整表达。实际上,观察离散模型的表达式,可以发现它生动阐释了「随机游走」的含义:每一时刻都是上一个采样时刻加上一个高斯白噪声得到的,犹如一个游走的粒子,踏出的下一步永远是随机的。在我们前面给出的 IMU 的运动模型中,bias 就设定为服从随机游走模型。
2-4. IMU 观测模型
根据上述前置知识,现在我们可以给出 IMU 的观测模型。需要注意的是,观测在不同参考坐标系下形式不同。
以 ECEF 为参考坐标系:这是 MSCKF 一代 [1] 的做法。因为 ECEF 不是惯性系,需要考虑地球自转,于是加速度模型中将会引入科氏加速度。记 ωG
为地球自转角速度, Gg 为重力加速度, ωm,am
为陀螺仪和加速度计的观测量,观测模型由以下公式给出:
ωm=ω+R(IGq¯)ωG+bg+ng(2.2)
am=R(IGq¯)(Ga−Gg+2ω∧GGvI+(ω∧G)2GpI)+ba+na(2.3)
观测量都是在体坐标系 {I} 下表达的,所以在参考坐标系 {G} 下表达的量都需要左乘一个旋转矩阵转化到体坐标系。每个观测量的不确定量都用一个随机游走的 bias 和一个高斯白噪声之和来表达。陀螺仪的观测模型是比较易懂的。加速度计的观测模型,我们先将其改写为形如 (2.1)
的形式:
RT(IGq¯)(am−ba−na)=(ω∧G)2GpI+2ω∧GGvI+Ga−Gg
但这还不够,因为各个量只是在 ECEF 坐标系 {G} 下的表达,而 (2.1)
中的量都是表达在惯性坐标系下的。记 RG 为将 {G} 下坐标映射到惯性坐标系下坐标的旋转矩阵。由于 ECEF 绕固定的 z 轴匀速转动,易得 RGωG=ωG。于是上式两边左乘 RG
,可得
RGRT(IGq¯)(am−ba−na)=ωG×(ωG×RGGpI)+2ωG×(RGGvI)+RG(Ga−Gg)
这里我们还利用了 R(a×b)=Ra×Rb
的性质。上式对应到 (2.1) 中各项,左边为绝对加速度 a;因为地球自转是匀速的,故切向加速度项 α×r 为零。其余各项,依次为向心加速度项 ω×(ω×r),科氏加速度项 2ω×vr,以及相对加速度项 ar
。
以 ECI 为参考坐标系:这是 MSCKF 二代 [3] 的做法。由于 ECI 为惯性系,不需要考虑地球自转,于是观测模型简单很多:
ωm=ω+R(IGq¯)ωG+bg+ng(2.4)
am=R(IGq¯)(Ga−Gg)+ba+na(2.5)
因为比较简单,就不多做解释了。从文献上看,现在移动机器人领域 ECI 用得更多些。
至此,我们推导完了 IMU 的观测模型。
3. IMU 状态估计误差模型
3-1. 前置:四元数误差小量
旋转量是非线性的,不宜像线性量那样使用 x̃ =x−x̂
来定义误差量。这里我们使用四元数误差小量来定义误差量。根据 (1.4),四元数可以用旋转向量经简单的转换得到。假定绕单位轴 u 旋转了一个角度小量 δθ
,用四元数表达为:
δq=[usinδθ2cosδθ2]≃[uδθ21]≜[δθ21]
于是,可以用 δq
来表示旋转的真实值和估计值之间的误差,具体关系为
q=δq⊗q̂
直接使用 δθ
,可以实现参数最小化,适用于优化问题中的目标函数。
3-2. IMU 状态估计误差模型
我们直接给出和 MSCKF 一样的 IMU 状态估计误差模型:
X̃ IMU=[δθTIb̃ TgGṽ TIb̃ TaGp̃ TI](3.0)
其中旋转量按照四元数误差小量给出,其余直接由真实值和估计值相减得到。
4. 小结
本文从基础出发推导了 IMU 的运动模型(1.7)
、观测和噪声模型(2.2)−(2.5)、估计误差模型(3.0)
,适用于用 IMU 来做状态估计的场合。至于以上这些模型如何再经过线性化、离散化等处理进入具体状态估计问题的框架中,这里不做赘述,留待读者阅读和探索。
参考文献
[1] Mourikis, Anastasios I., and Stergios I. Roumeliotis. “A multi-state constraint Kalman filter for vision-aided inertial navigation.” Proceedings 2007 IEEE International Conference on Robotics and Automation. IEEE, 2007.
[2] Trawny, Nikolas, and Stergios I. Roumeliotis. “Indirect Kalman filter for 3D attitude estimation.” University of Minnesota, Dept. of Comp. Sci. & Eng., Tech. Rep 2 (2005): 2005.
[3] Li, Mingyang. “Visual-inertial odometry on resource-constrained systems.” (2014).
[4] IMU noise model https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model
[5] Crassidis, John L., and John L. Junkins. Optimal estimation of dynamic systems. CRC press, 2011.
留言请用 Github Issues
聊天请在 Gitter/fan-farm