从零开始的 IMU 状态模型推导

https://fzheng.me/2016/11/20/imu_model_eq/

从零开始的 IMU 状态模型推导

2016-11-20

提示：请使用 Firefox，Chrome，Edge 等较新的浏览器阅读，以获得完整的公式排版。IE 请使用 IE 11。

本文已授权「泡泡机器人 SLAM」微信公众号（paopaorobot_slam）发表。

0. 总览

IMU 是移动机器人、移动智能设备上常见的传感器。常见的 IMU 为六轴传感器，配备输出三轴加速度的加速度计和输出三轴角速度的陀螺仪。九轴 IMU 还会配备输出三轴姿态角的磁力计。我们这里只讨论六轴 IMU。

IMU 的状态量通常表示为：

X I M U = [I G q ¯ T b T g G v T I b T a G p T I] (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

的时间一阶导为

d r d t = ω \times r

现在考虑上图（右），坐标系 {B} 绕单位轴 u

旋转，如上所述，其三个轴的时间一阶导同样为

d i B d t = ω \times i B, d j B d t = ω \times j B, d k B d t = ω \times k B

我们知道，[iBjBkB]

实际上就是坐标系 {B} 相对于参考坐标系的旋转矩阵 R。所以 R

的时间一阶导为

R ˙ = [ω \times i B ω \times j B ω \times k B] = ω \times R (1.0)

我们知道上面的叉乘运算可以转化为负对称矩阵的乘法：

R ˙ = ω \land R (1.1)

其中负对称矩阵为

ω \land = ⎡ ⎣ ⎢ ⎢ 0 ω 3 - ω 2 - ω 3 0 ω 1 ω 2 - ω 1 0 ⎤ ⎦ ⎥ ⎥

注意这里的角速度 ω

是在参考坐标系下表达的。角速度也经常表达在体坐标系 {B} 下，记为 Bω=RTω，即 ω=RBω，于是 (1.1)

可以写作

R ˙ = (R B ω) \land R (1.2)

这里我们要利用负对称矩阵的一个很好的性质：对任意旋转矩阵 R

和三维向量 v，都有 (Rv)∧=Rv∧RT（参看《 (Rv)^ = Rv^R’ 的简单证明》），于是 (1.2)

可以写成

R ˙ = R (B ω) \land (1.3)

比较一下 (1.1)

和 (1.3)

，可以发现一个很有趣的事实，角速度如果表达在参考坐标系下，负对称矩阵写在左边；如果表达在体坐标系下，负对称矩阵写在右边。这点微小的区别，读者在阅读文献时可以特别留意。

1-2. 前置(2)：四元数

这部分讲四元数如何表示旋转的前置知识，熟悉的读者可以跳过。

用旋转矩阵来表示旋转很直观，但过于冗余，因为旋转只有三个自由度，而旋转矩阵有九个量。表征旋转还可以用欧拉角，但有万向锁问题，而且计算也不方便。旋转向量（即李代数 so(3)）和四元数是更常用的表征方法，在惯性导航中四元数似乎更普遍些。这里采用四元数。

一个四元数由一个实部和三个虚部构成，书写顺序各家不同，这里和 MSCKF [1] 一样，虚部在前实部在后：

q = q 1 i + q 2 j + q 3 k + q 4 = [v T q 4] T

虚部 v=[q1q2q3]T

。虚部三个基 i,j,k 满足 i2=j2=k2=ijk=−1。四元数仍是一种冗余表达法，为了更紧凑，通常使用使用单元四元数 q¯

，通过将四元数的模直为 1 得到。

四元数和旋转向量有很直接的转换关系。绕单位轴 u

转了 θ

角度，用四元数表达为

q = [u sin θ 2 cos θ 2] (1.4)

四元数乘法 ⊗

为类似于多项式乘法的逐项相乘：

q \otimes p = = (q 1 i + q 2 j + q 3 k + q 4) (p 1 i + p 2 j + p 3 k + p 4) (q 1 p 4 + q 2 p 3 - q 3 p 2 + q 4 p 1) i + (- q 1 p 3 + q 2 p 4 + q 3 p 1 + q 4 p 2) j + (q 1 p 2 - q 2 p 1 + q 3 p 4 + q 4 p 3) k + (- q 1 p 1 - q 2 p 2 - q 3 p 3 + q 4 p 4)

这个计算结果可以表达为多种形式：

q \otimes p = [q 4 I 3 + v \land q - v T q v q q 4] [v p p 4] = [p 4 I 3 - v \land p - v T p v p p 4] [v q q 4]

四元数乘法和其对应的两个旋转矩阵相乘物理意义是一样的，即 R(q⊗p)=R(q)R(p)

。四元数对应的旋转矩阵为：

R (q) = (2 q 24 - 1) I 3 + 2 q 4 v \land + 2 v v T (1.5)

四元数的逆为 q−1=[−vTq4]T

。易得 q⊗q−1=[0001]T:=qI，故 qI

表示旋转量为零。

四元数对时间一阶导为

q ˙ = 1 2 [ω \land - ω T ω 0] q : = 1 2 Ω (ω) q = 1 2 [q 4 I 3 - v \land - v T] ω (1.6)

读者可能注意到了 (1.6)

和 (1.1) 形式上的相似。这里 ω 的意义也是一样的。 (1.6)

的推导可以参考 [2]，这里不赘述。

1-3. IMU 运动模型

有了前置知识的铺垫之后，我们可以给出 IMU 的运动模型：

I G q ¯ ˙ b ˙ g G v ˙ I b ˙ a G p ˙ I = 1 2 Ω (ω) I G q ¯ = n w g = G a = n w a = G v I (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 ˙ B r + R B r ˙ = ω \land R B r + R B r ˙

记 P 在 {B} 下速度为 Bv

，于是

v = ω \land r + R B v = ω \times r + v r (2.0)

请注意，这里用 vr

来表达「相对速度」的概念，准确定义为 P 相对于 {B} 的速度，在惯性系 {A} 下的表达。请分清 vr、 v 以及 Bv

三者之间的区别和联系。

再对 (2.0)

求时间导：

a = v ˙ = ω ˙ \times r + ω \times r ˙ + R ˙ B v + R B v ˙ = α \times r + ω \times （ ω \times r + v r ） + ω \times R B v + R B a = α \times r + ω \times (ω \times r) + 2 ω \times v r + a r (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)] = 0 E [n (t 1) n (t 2)] = σ 2 g δ (t 1 - t 2)

其中 δ()

表示狄拉克函数。可以看出，不同时刻的高斯白噪声相互独立。 σ2g

为方差，值越大，表示噪声程度越大。

将高斯白噪声离散化，可得到：

n d [k] = σ g d w [k]

其中

w [k] \sim  (0, 1) σ g d = σ g Δ t ‾ ‾ ‾ \sqrt

其中 Δt

为采样时间。为什么离散化后分母会多出 Δt‾‾‾√ 这一项呢？我们假定在一个采样周期内 n(t)

为常数，于是

n d [k] ≜ n (t 0 + Δ t) ≃ 1 Δ t \int t 0 + Δ t t 0 n (τ) d t

E (n d [k] 2) = E (1 Δ t 2 \int t 0 + Δ t t 0 \int t 0 + Δ t t 0 n (τ) n (t) d τ d t) = E (σ 2 g Δ t 2 \int t 0 + Δ t t 0 \int t 0 + Δ t t 0 δ (t - τ) d τ d t) = E (σ 2 g Δ t)

所以有　σ2gd=σ2gΔt

，即 σgd=σgΔt√

。

接下来讨论随机游走模型。准确地讲，随机游走其实是一个离散模型，其连续模型称为维纳过程（Wiener Process）。维纳模型是高斯白噪声的积分：

b ˙ g (t) = n (t) = σ b g w (t)

其中 w

为单位高斯白噪声。将其离散化后得到随机游走模型：

b d [k] = b d [k - 1] + σ b g d w [k]

其中

w [k] \sim  (0, 1) σ g d = σ b g Δ t ‾ ‾ ‾ \sqrt

这里多出来的 Δt‾‾‾√

又是哪来的呢？仍假定一个采样周期内高斯白噪声为常数，有：

b d [k] ≜ b (t 0) + \int t 0 + Δ t t 0 n (t) d t

E ((b d [k] - b d [k - 1]) 2) = E (\int t 0 + Δ t t 0 \int t 0 + Δ t t 0 n (t) n (τ) d τ d t) = E (σ 2 b g \int t 0 + Δ t t 0 \int t 0 + Δ t t 0 δ (t - τ) d τ d t) = E (σ 2 b g Δ t)

所以有　σ2bgd=σ2bgΔt

，即 σbgd=σbgΔt‾‾‾√

。

于是我们得到随机游走模型的完整表达。实际上，观察离散模型的表达式，可以发现它生动阐释了「随机游走」的含义：每一时刻都是上一个采样时刻加上一个高斯白噪声得到的，犹如一个游走的粒子，踏出的下一步永远是随机的。在我们前面给出的 IMU 的运动模型中，bias 就设定为服从随机游走模型。

2-4. IMU 观测模型

根据上述前置知识，现在我们可以给出 IMU 的观测模型。需要注意的是，观测在不同参考坐标系下形式不同。

以 ECEF 为参考坐标系：这是 MSCKF 一代 [1] 的做法。因为 ECEF 不是惯性系，需要考虑地球自转，于是加速度模型中将会引入科氏加速度。记 ωG

为地球自转角速度， Gg 为重力加速度， ωm,am

为陀螺仪和加速度计的观测量，观测模型由以下公式给出：

ω m = ω + R (I G q ¯) ω G + b g + n g (2.2)

a m = R (I G q ¯) (G a - G g + 2 ω \land G G v I + (ω \land G) 2 G p I) + b a + n a (2.3)

观测量都是在体坐标系 {I} 下表达的，所以在参考坐标系 {G} 下表达的量都需要左乘一个旋转矩阵转化到体坐标系。每个观测量的不确定量都用一个随机游走的 bias 和一个高斯白噪声之和来表达。陀螺仪的观测模型是比较易懂的。加速度计的观测模型，我们先将其改写为形如 (2.1)

的形式：

R T (I G q ¯) (a m - b a - n a) = (ω \land G) 2 G p I + 2 ω \land G G v I + G a - G g

但这还不够，因为各个量只是在 ECEF 坐标系 {G} 下的表达，而 (2.1)

中的量都是表达在惯性坐标系下的。记 RG 为将 {G} 下坐标映射到惯性坐标系下坐标的旋转矩阵。由于 ECEF 绕固定的 z 轴匀速转动，易得 RGωG=ωG。于是上式两边左乘 RG

，可得

R G R T (I G q ¯) (a m - b a - n a) = ω G \times (ω G \times R G G p I) + 2 ω G \times (R G G v I) + R G (G a - G g)

这里我们还利用了 R(a×b)=Ra×Rb

的性质。上式对应到 (2.1) 中各项，左边为绝对加速度 a；因为地球自转是匀速的，故切向加速度项 α×r 为零。其余各项，依次为向心加速度项 ω×(ω×r)，科氏加速度项 2ω×vr，以及相对加速度项 ar

。

以 ECI 为参考坐标系：这是 MSCKF 二代 [3] 的做法。由于 ECI 为惯性系，不需要考虑地球自转，于是观测模型简单很多：

ω m = ω + R (I G q ¯) ω G + b g + n g (2.4)

a m = R (I G q ¯) (G a - G g) + b a + n a (2.5)

因为比较简单，就不多做解释了。从文献上看，现在移动机器人领域 ECI 用得更多些。

至此，我们推导完了 IMU 的观测模型。

3. IMU 状态估计误差模型

3-1. 前置：四元数误差小量

旋转量是非线性的，不宜像线性量那样使用 x̃ =x−x̂

来定义误差量。这里我们使用四元数误差小量来定义误差量。根据 (1.4)，四元数可以用旋转向量经简单的转换得到。假定绕单位轴 u 旋转了一个角度小量 δθ

，用四元数表达为：

δ q = [u sin δ θ 2 cos δ θ 2] ≃ [u δ θ 2 1] ≜ [δ θ 2 1]

于是，可以用 δq

来表示旋转的真实值和估计值之间的误差，具体关系为

q = δ q \otimes q ̂

直接使用 δθ

，可以实现参数最小化，适用于优化问题中的目标函数。

3-2. IMU 状态估计误差模型

我们直接给出和 MSCKF 一样的 IMU 状态估计误差模型：

X ̃ I M U = [δ θ T I b ̃ T g G v ̃ T I b ̃ T a G p ̃ T I] (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.