Exploring Applications

Understanding the workings of IMUs represents just the starting point. Let’s now explore their wide-ranging applications, delving deeper into the underlying concepts.

Navigation:

• INS (Inertial Navigation Systems): Combine IMU data with mathematical calculations to determine position, attitude, angular rates, and linear velocity. This information forms the backbone for navigation and control of various vehicles.
• Kalman filtering: A mathematical technique used to fuse IMU data with GPS measurements, significantly improving accuracy and robustness in navigation, especially when GPS signals are weak or unavailable.

Robotics:

• Feedback control: IMUs provide real-time feedback on the robot’s motion and orientation, enabling precise control of robotic arms, legs, vehicles, and drones.

• Inverse kinematics: A mathematical method used to translate desired robot joint movements based on the end-effector pose obtained from IMU data. This allows for accurate and coordinated movements.

目录

• 0. 总览
• 1. IMU 运动模型
  • 1-1. 前置(1)： 旋转量求导
  • 1-2. 前置(2)： 四元数
  • 1-3. IMU 运动模型
• 2. IMU 观测和噪声模型
  • 2-1 前置(1)： 科氏加速度
  • 2-2 前置(2)： 惯性导航相关坐标系定义
  • 2-3 前置(3)： 高斯白噪声与随机游走
  • 2-4. IMU 观测模型
• 3. IMU 状态估计误差模型
  • 3-1. 前置：四元数误差小量
  • 3-2. IMU 状态估计误差模型
• 4. 小结
• 参考文献

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

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

0. 总览

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

IMU 的状态量通常表示为：

$\begin{array}{cccc}& {\mathbf{X}}_{IMU}={[}_G^I{\bar{q}}^T{\mathbf{b}}_g^T{}^G{\mathbf{v}}_I^T{\mathbf{b}}_a^T{}^G{\mathbf{p}}_I^T]& & \text{(0.0)}\end{array}$

这里我们使用和 MSCKF [1] 一样的 notation。用 {I} 表示 IMU 坐标系，{G} 表示参考坐标系。IMU 的姿态由旋转量 ${}_G^I\bar{q}$ 和平移量 ${}^G{\mathbf{p}}_I$ 表示。更具体来说，前者为将任意向量从 {G} 坐标映射到 {I} 坐标的旋转量，用单位四元数表示；后者为 IMU 在 {G} 下的三维位置。${}^G{\mathbf{v}}_I$ 表示 IMU 在 {G} 下的平移速度。另外两个量 ${\mathbf{b}}_g$ 和 ${\mathbf{b}}_a$ 表示陀螺仪（角速度计）和加速度计的 bias。可以注意一下这里除了 bias 之外的状态量的时间维度：平移量表达到速度（p 和 v，对时间的一阶导），因为 IMU 只提供到加速度（对时间的二阶导）的测量；旋转量只表达姿态量（对时间的零阶导），因为 IMU 提供到角速度（对时间的一阶导）。状态量的估计可以由 IMU 测量积分得到。

对于 IMU 状态估计问题，需要提供运动模型、观测（噪声）模型、估计误差模型：

$\begin{array}{cccc}& \dot{\mathbf{x}}=f(\mathbf{x})& & \text{(0.1)}\end{array}$$\begin{array}{cccc}& \mathbf{z}=g(\mathbf{x})+\mathbf{n}& & \text{(0.2)}\end{array}$$\begin{array}{cccc}& \delta \mathbf{x}=e(\widehat{\mathbf{x}},\mathbf{x})& & \text{(0.3)}\end{array}$

这是一个通用模型，我们用 $\mathbf{x}$ 表示真实状态量（待估计，不可知），用 $\mathbf{z}$ 表示观测量，$\mathbf{n}$ 表示观测噪声，$\widehat{\mathbf{x}}$ 表示当前的状态估计量。这篇小文主要讲 IMU （即 $\mathbf{x}:={\mathbf{X}}_{IMU}$ 时）这三个模型的推导。

1. IMU 运动模型

1-1. 前置(1)： 旋转量求导

这部分讲刚体动力学相关的前置知识，熟悉的读者可以跳过。

众所周知，一个刚体在同一个惯性坐标系下进行平移运动，其平移量对时间的一阶导和二阶导即速度和加速度：

$\dot{\mathbf{p}}=\mathbf{v},\dot{\mathbf{v}}=\mathbf{a}$

对于旋转量以及非惯性系参考坐标系，情况稍微复杂些。

首先，如下图（左）所示，考虑一个从原点出发的向量 $\mathbf{r}$ 绕单位轴 $\mathbf{u}$ 旋转，角速度大小为 $\dot{\theta }$。

角速度矢量可以表示为 $\mathit{\omega }=\dot{\theta }\mathbf{u}$。易得向量 $\mathbf{r}$ 末端点 P 的速度矢量，即 $\mathbf{r}$的时间一阶导为

$\frac{d\mathbf{r}}{dt}=\mathit{\omega }\times \mathbf{r}$

现在考虑上图（右），坐标系 {B} 绕单位轴 $\mathbf{u}$ 旋转，如上所述，其三个轴的时间一阶导同样为

$\frac{d{\mathbf{i}}_B}{dt}=\mathit{\omega }\times {\mathbf{i}}_B,\frac{d{\mathbf{j}}_B}{dt}=\mathit{\omega }\times {\mathbf{j}}_B,\frac{d{\mathbf{k}}_B}{dt}=\mathit{\omega }\times {\mathbf{k}}_B$

我们知道，$[{\mathbf{i}}_B{\mathbf{j}}_B{\mathbf{k}}_B]$ 实际上就是坐标系 {B} 相对于参考坐标系的旋转矩阵 $\mathbf{R}$。所以 $\mathbf{R}$ 的时间一阶导为

$\begin{array}{cccc}& \dot{\mathbf{R}}=[\mathit{\omega }\times {\mathbf{i}}_B\mathit{\omega }\times {\mathbf{j}}_B\mathit{\omega }\times {\mathbf{k}}_B]=\mathit{\omega }\times \mathbf{R}& & \text{(1.0)}\end{array}$

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

$\begin{array}{cccc}& \dot{\mathbf{R}}={\mathit{\omega }}^{\wedge }\mathbf{R}& & \text{(1.1)}\end{array}$

其中负对称矩阵为

${\mathit{\omega }}^{\wedge }=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]$

注意这里的角速度 $\mathit{\omega }$ 是在参考坐标系下表达的。角速度也经常表达在体坐标系 {B} 下，记为 ${}^B\mathit{\omega }={\mathbf{R}}^T\mathit{\omega }$，即 $\mathit{\omega }=\mathbf{R}{}^B\mathit{\omega }$，于是 $(1.1)$ 可以写作

$\begin{array}{cccc}& \dot{\mathbf{R}}=(\mathbf{R}{}^B\mathit{\omega }{)}^{\wedge }\mathbf{R}& & \text{(1.2)}\end{array}$

这里我们要利用负对称矩阵的一个很好的性质：对任意旋转矩阵 $\mathbf{R}$ 和三维向量 $\mathbf{v}$，都有 $(\mathbf{R}\mathbf{v}{)}^{\wedge }={\mathbf{R}{\mathbf{v}}^{\wedge }\mathbf{R}}^T$（参看《(Rv)^ = Rv^R’ 的简单证明》），于是 $(1.2)$ 可以写成

$\begin{array}{cccc}& \dot{\mathbf{R}}=\mathbf{R}({}^B\mathit{\omega }{)}^{\wedge }& & \text{(1.3)}\end{array}$

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

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

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

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

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

$\mathbf{q}=q_{1}i+q_{2}j+q_{3}k+q_4=[{\mathit{v}}^T{q}_4{]}^T$

虚部 $\mathit{v}=[q_1{q}_2{q}_3{]}^T$。虚部三个基 $i,j,k$ 满足 $i^2=j^2=k^2=ijk=-1$。 四元数仍是一种冗余表达法，为了更紧凑，通常使用使用单元四元数 $\bar{\mathbf{q}}$，通过将四元数的模直为 1 得到。

四元数和旋转向量有很直接的转换关系。绕单位轴 $\mathbf{u}$ 转了 $\theta$ 角度，用四元数表达为

$\begin{array}{cccc}& \mathbf{q}=[\mathbf{u}\sin \frac{\theta }{2}\cos \frac{\theta }{2}]& & \text{(1.4)}\end{array}$

四元数乘法 $\otimes$ 为类似于多项式乘法的逐项相乘：

$\begin{array}{rl}{\displaystyle \mathbf{q}\otimes \mathbf{p}=}& {\displaystyle (q_{1}i+q_{2}j+q_{3}k+q_4)(p_{1}i+p_{2}j+p_{3}k+p_4)}\\ {\displaystyle =}& {\displaystyle (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+}\\ {\displaystyle }& {\displaystyle (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)}\end{array}$

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

$\begin{array}{rl}{\displaystyle \mathbf{q}\otimes \mathbf{p}}& {\displaystyle =\left[\begin{array}{cc}{q}_4{\mathbf{I}}_3+{\mathit{v}}_q^{\wedge }& {\mathit{v}}_q\\ -{\mathit{v}}_q^T& q_4\end{array}\right]\left[\begin{array}{c}{\mathit{v}}_p\\ p_4\end{array}\right]}\\ {\displaystyle }& {\displaystyle =\left[\begin{array}{cc}{p}_4{\mathbf{I}}_3-{\mathit{v}}_p^{\wedge }& {\mathit{v}}_p\\ -{\mathit{v}}_p^T& p_4\end{array}\right]\left[\begin{array}{c}{\mathit{v}}_q\\ q_4\end{array}\right]}\end{array}$

四元数乘法和其对应的两个旋转矩阵相乘物理意义是一样的，即 $\mathbf{R}(\mathbf{q}\otimes \mathbf{p})=\mathbf{R}(\mathbf{q})\mathbf{R}(\mathbf{p})$。四元数对应的旋转矩阵为：

$\begin{array}{cccc}& \mathbf{R}(\mathbf{q})=(2q_4^2-1){\mathbf{I}}_3+2q_4{\mathit{v}}^{\wedge }+2{\mathrm{vv}}^T& & \text{(1.5)}\end{array}$

四元数的逆为 ${\mathbf{q}}^{-1}=[-{\mathit{v}}^T{q}_4{]}^T$。易得 $\mathbf{q}\otimes {\mathbf{q}}^{-1}=[0001{]}^T:={\mathbf{q}}_I$，故 ${\mathbf{q}}_I$ 表示旋转量为零。

四元数对时间一阶导为

$\begin{array}{cccc}& \begin{array}{rl}{\displaystyle \dot{\mathbf{q}}}& {\displaystyle =\frac{1}{2}\left[\begin{array}{cc}{\mathit{\omega }}^{\wedge }& \mathit{\omega }\\ -{\mathit{\omega }}^T& 0\end{array}\right]\mathbf{q}:=\frac{1}{2}\mathit{\Omega }(\mathit{\omega })\mathbf{q}}\\ {\displaystyle }& {\displaystyle =\frac{1}{2}\left[\begin{array}{c}{q}_4{\mathbf{I}}_3-{\mathit{v}}^{\wedge }\\ -{\mathit{v}}^T\end{array}\right]\mathit{\omega }}\end{array}& & \text{(1.6)}\end{array}$

读者可能注意到了 $(1.6)$ 和 $(1.1)$ 形式上的相似。这里 $\mathit{\omega }$ 的意义也是一样的。$(1.6)$ 的推导可以参考 [2]，这里不赘述。

1-3. IMU 运动模型

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

$\begin{array}{cccc}& \begin{array}{rl}{\displaystyle {}_G^I\dot{\bar{q}}}& {\displaystyle =\frac{1}{2}\mathit{\Omega }(\mathit{\omega }){}_G^I\bar{q}}\\ {\displaystyle {\dot{\mathbf{b}}}_g}& {\displaystyle ={\mathbf{n}}_{wg}}\\ {\displaystyle {}^G{\dot{\mathbf{v}}}_I}& {\displaystyle ={}^G\mathbf{a}}\\ {\displaystyle {\dot{\mathbf{b}}}_a}& {\displaystyle ={\mathbf{n}}_{wa}}\\ {\displaystyle {}^G{\dot{\mathbf{p}}}_I}& {\displaystyle ={}^G{\mathbf{v}}_I}\end{array}& & \text{(1.7)}\end{array}$

${}_G^I\dot{\bar{q}}$ 由 $(1.6)$ 直接得到。注意这里角速度 $\mathit{\omega }$ 是在体坐标系 {I} 下表达的，与 $(1.1)$ 处相反。原因是 ${}_G^I\bar{q}$ 表示的旋转方向与 $(1.1)$ 处的 $\mathbf{R}$ 是相反的。其他的四项，速度和加速度都很简单，bias 两项在下面观测模型部分讲。

2. IMU 观测和噪声模型

2-1 前置(1)： 科氏加速度

这部分在 1-1 的基础上，讨论参考坐标系不是惯性系的情况，熟悉科氏加速度的读者可以跳过。我们仍利用 1-1 中的图，但这次把绕惯性系 {A} 中固定单位轴 $\mathbf{u}$ 旋转的 {B} 作为参考坐标系。考虑下图， 点 P 相对于 {B} 运动，记 ${}^B\mathbf{r}$ 分别为 P 在 {B} 下的坐标，$\mathbf{r}$ 为 P 的绝对坐标（即 {A} 下坐标）， $\mathbf{R}$ 仍为 {B} 相对于 {A} 的旋转矩阵，易知 $\mathbf{r}={\mathbf{R}}^B\mathbf{r}$。

求一阶时间导，并利用公式 $(1.1)$：

$\mathbf{v}=\dot{\mathbf{r}}=\dot{\mathbf{R}}{}^B\mathbf{r}+{\mathbf{R}}^B\dot{\mathbf{r}}={\mathit{\omega }}^{\wedge }\mathbf{R}{}^B\mathbf{r}+{\mathbf{R}}^B\dot{\mathbf{r}}$

记 P 在 {B} 下速度为 ${}^B\mathbf{v}$，于是

$\begin{array}{cccc}& \begin{array}{rl}{\displaystyle \mathbf{v}}& {\displaystyle ={\mathit{\omega }}^{\wedge }\mathbf{r}+{\mathbf{R}}^B\mathbf{v}}\\ {\displaystyle }& {\displaystyle =\mathit{\omega }\times \mathbf{r}+{\mathbf{v}}_r}\end{array}& & \text{(2.0)}\end{array}$

请注意，这里用 ${\mathbf{v}}_r$ 来表达「相对速度」的概念，准确定义为 P 相对于 {B} 的速度，在惯性系 {A} 下的表达。请分清 ${\mathbf{v}}_r$、$\mathbf{v}$ 以及 ${}^B\mathbf{v}$ 三者之间的区别和联系。

再对 $(2.0)$ 求时间导：

$\begin{array}{cccc}& \begin{array}{rl}{\displaystyle \mathbf{a}=\dot{\mathbf{v}}}& {\displaystyle =\dot{\mathit{\omega }}\times \mathbf{r}+\mathit{\omega }\times \dot{\mathbf{r}}+\dot{\mathbf{R}}{}^B\mathbf{v}+\mathbf{R}{}^B\dot{\mathbf{v}}}\\ {\displaystyle }& {\displaystyle =\mathit{\alpha }\times \mathbf{r}+\mathit{\omega }\times \text{（}\mathit{\omega }\times \mathbf{r}+{\mathbf{v}}_r\text{）}+\mathit{\omega }\times \mathbf{R}{}^B\mathbf{v}+\mathbf{R}{}^B\mathbf{a}}\\ {\displaystyle }& {\displaystyle =\mathit{\alpha }\times \mathbf{r}+\mathit{\omega }\times (\mathit{\omega }\times \mathbf{r})+2\mathit{\omega }\times {\mathbf{v}}_r+{\mathbf{a}}_r}\end{array}& & \text{(2.1)}\end{array}$

我们来逐项分析上面这个式子。第一项中 $\mathit{\alpha }$ 为 {B} 的角加速度，所以第一项的物理意义是 {B} 旋转所造成的 P 的切向加速度。第二项是 {B} 旋转所造成的向心加速度。第四项为 P 相对于 {B} 的加速度，但在惯性系 {A} 下表达——类似于 ${\mathbf{v}}_r$，定义相对加速度 ${\mathbf{a}}_r$。第三项比较特殊，为 {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)$，满足以下两个条件

$$\begin{gathered} E[n(t)]=0 \\ E[n(t_1)n(t_2)]={\sigma }_g^2\delta (t_1-t_2) \end{gathered}$$

其中 $\delta ()$ 表示狄拉克函数。可以看出，不同时刻的高斯白噪声相互独立。${\sigma }_g^2$ 为方差，值越大，表示噪声程度越大。

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

$n_d[k]={\sigma }_{gd}w[k]$

其中

$$\begin{gathered} w[k]\sim \mathcal{N}(0,1) \\ {\sigma }_{gd}=\frac{{\sigma }_g}{\sqrt{\mathrm{\Delta }t}} \end{gathered}$$​σg​​

其中 $\mathrm{\Delta }t$ 为采样时间。为什么离散化后分母会多出 $\sqrt{\mathrm{\Delta }t}$

​ 这一项呢？我们假定在一个采样周期内 $n(t)$ 为常数，于是

$n_d[k]\triangleq n(t_0+\mathrm{\Delta }t)\simeq \frac{1}{\mathrm{\Delta }t}{\int }_{t_0}^{t_0+\mathrm{\Delta }t}n(\tau )dt$$\begin{array}{rl}{\displaystyle E(n_d[k{]}^2)}& {\displaystyle =E(\frac{1}{\mathrm{\Delta }{t}^2}{\int }_{t_0}^{t_0+\mathrm{\Delta }t}{\int }_{t_0}^{t_0+\mathrm{\Delta }t}n(\tau )n(t)d\tau dt)}\\ {\displaystyle }& {\displaystyle =E(\frac{{\sigma }_g^2}{\mathrm{\Delta }{t}^2}{\int }_{t_0}^{t_0+\mathrm{\Delta }t}{\int }_{t_0}^{t_0+\mathrm{\Delta }t}\delta (t-\tau )d\tau dt)}\\ {\displaystyle }& {\displaystyle =E(\frac{{\sigma }_g^2}{\mathrm{\Delta }t})}\end{array}$

所以有　${\sigma }_{gd}^2=\frac{{\sigma }_g^2}{\mathrm{\Delta }t}$，即 ${\sigma }_{gd}=\frac{{\sigma }_g}{\sqrt{\mathrm{\Delta }t}}$

​σg​​。

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

${\dot{b}}_g(t)=n(t)={\sigma }_{bg}w(t)$

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

$b_d[k]=b_d[k-1]+{\sigma }_{bgd}w[k]$

其中

$$\begin{gathered} w[k]\sim \mathcal{N}(0,1) \\ {\sigma }_{gd}={\sigma }_{bg}\sqrt{\mathrm{\Delta }t} \end{gathered}$$​

这里多出来的 $\sqrt{\mathrm{\Delta }t}$

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

$b_d[k]\triangleq b(t_0)+{\int }_{t_0}^{t_0+\mathrm{\Delta }t}n(t)dt$$\begin{array}{rl}{\displaystyle E((b_d[k]-b_d[k-1]{)}^2)}& {\displaystyle =E({\int }_{t_0}^{t_0+\mathrm{\Delta }t}{\int }_{t_0}^{t_0+\mathrm{\Delta }t}n(t)n(\tau )d\tau dt)}\\ {\displaystyle }& {\displaystyle =E({\sigma }_{bg}^2{\int }_{t_0}^{t_0+\mathrm{\Delta }t}{\int }_{t_0}^{t_0+\mathrm{\Delta }t}\delta (t-\tau )d\tau dt)}\\ {\displaystyle }& {\displaystyle =E({\sigma }_{bg}^2\mathrm{\Delta }t)}\end{array}$

所以有　${\sigma }_{bgd}^2={\sigma }_{bg}^2\mathrm{\Delta }t$，即 ${\sigma }_{bgd}={\sigma }_{bg}\sqrt{\mathrm{\Delta }t}$

​。

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

2-4. IMU 观测模型

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

以 ECEF 为参考坐标系：这是 MSCKF 一代 [1] 的做法。因为 ECEF 不是惯性系，需要考虑地球自转，于是加速度模型中将会引入科氏加速度。记 ${\mathit{\omega }}_G$ 为地球自转角速度， ${}^G\mathbf{g}$ 为重力加速度， ${\mathit{\omega }}_m,{\mathbf{a}}_m$ 为陀螺仪和加速度计的观测量，观测模型由以下公式给出：

$\begin{array}{cccc}& {\mathit{\omega }}_m=\mathit{\omega }+\mathbf{R}({}_G^I\bar{q}){\mathit{\omega }}_G+{\mathbf{b}}_g+{\mathbf{n}}_g& & \text{(2.2)}\end{array}$$\begin{array}{cccc}& {\mathbf{a}}_m=\mathbf{R}({}_G^I\bar{q})({}^G\mathbf{a}-{}^G\mathbf{g}+2{\mathit{\omega }}_G^{\wedge }{}^G{\mathbf{v}}_I+({\mathit{\omega }}_G^{\wedge }{)}^2{}^G{\mathbf{p}}_I)+{\mathbf{b}}_a+{\mathbf{n}}_a& & \text{(2.3)}\end{array}$

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

${\mathbf{R}}^T({}_G^I\bar{q})({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a)=({\mathit{\omega }}_G^{\wedge }{)}^2{}^G{\mathbf{p}}_I+2{\mathit{\omega }}_G^{\wedge }{}^G{\mathbf{v}}_I+{}^G\mathbf{a}-{}^G\mathbf{g}$

但这还不够，因为各个量只是在 ECEF 坐标系 {G} 下的表达，而 $(2.1)$ 中的量都是表达在惯性坐标系下的。记 ${\mathbf{R}}_G$ 为将 {G} 下坐标映射到惯性坐标系下坐标的旋转矩阵。由于 ECEF 绕固定的 z 轴匀速转动，易得 ${\mathbf{R}}_G{\mathit{\omega }}_G={\mathit{\omega }}_G$。于是上式两边左乘 ${\mathbf{R}}_G$，可得

${\mathbf{R}}_G{\mathbf{R}}^T({}_G^I\bar{q})({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a)={\mathit{\omega }}_G\times ({\mathit{\omega }}_G\times {\mathbf{R}}_G{}^G{\mathbf{p}}_I)+2{\mathit{\omega }}_G\times ({\mathbf{R}}_G{}^G{\mathbf{v}}_I)+{\mathbf{R}}_G({}^G\mathbf{a}-{}^G\mathbf{g})$

这里我们还利用了 $\mathbf{R}(\mathbf{a}\times \mathbf{b})=\mathbf{R}\mathbf{a}\times \mathbf{R}\mathbf{b}$ 的性质。上式对应到 $(2.1)$ 中各项，左边为绝对加速度 $\mathbf{a}$；因为地球自转是匀速的，故切向加速度项 $\mathit{\alpha }\times \mathbf{r}$ 为零。其余各项，依次为向心加速度项 $\mathit{\omega }\times (\mathit{\omega }\times \mathbf{r})$，科氏加速度项 $2\mathit{\omega }\times {\mathbf{v}}_r$，以及相对加速度项 ${\mathbf{a}}_r$。

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

$\begin{array}{cccc}& {\mathit{\omega }}_m=\mathit{\omega }+{\mathbf{b}}_g+{\mathbf{n}}_g& & \text{(2.4)}\end{array}$$\begin{array}{cccc}& {\mathbf{a}}_m=\mathbf{R}({}_G^I\bar{q})({}^G\mathbf{a}-{}^G\mathbf{g})+{\mathbf{b}}_a+{\mathbf{n}}_a& & \text{(2.5)}\end{array}$

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

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

3. IMU 状态估计误差模型

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

旋转量是非线性的，不宜像线性量那样使用 $\tilde{\mathbf{x}}=x-\widehat{x}$ 来定义误差量。这里我们使用四元数误差小量来定义误差量。根据 $(1.4)$，四元数可以用旋转向量经简单的转换得到。假定绕单位轴 $\mathbf{u}$ 旋转了一个角度小量 $\delta \theta$，用四元数表达为：

$\begin{array}{rl}{\displaystyle \delta \mathbf{q}}& {\displaystyle =\left[\begin{array}{c}\mathbf{u}\sin \frac{\delta \theta }{2}\\ \cos \frac{\delta \theta }{2}\end{array}\right]}\\ {\displaystyle }& {\displaystyle \simeq \left[\begin{array}{c}\mathbf{u}\frac{\delta \theta }{2}\\ 1\end{array}\right]\triangleq \left[\begin{array}{c}\frac{\mathrm{\delta \theta }}{2}\\ 1\end{array}\right]}\end{array}$

于是，可以用 $\delta \mathbf{q}$ 来表示旋转的真实值和估计值之间的误差，具体关系为

$\mathbf{q}=\delta \mathbf{q}\otimes \widehat{\mathbf{q}}$

直接使用 $\mathrm{\delta \theta }$，可以实现参数最小化，适用于优化问题中的目标函数。

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

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

$\begin{array}{cccc}& {\tilde{\mathbf{X}}}_{IMU}=[{\mathrm{\delta \theta }}_I^T{\tilde{\mathbf{b}}}_g^T{}^G{\tilde{\mathbf{v}}}_I^T{\tilde{\mathbf{b}}}_a^T{}^G{\tilde{\mathbf{p}}}_I^T]& & \text{(3.0)}\end{array}$

其中旋转量按照四元数误差小量给出，其余直接由真实值和估计值相减得到。

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.

1、坐标系方向

       std::tuple<Eigen::Vector3d, int, bool> result = citibot::common::math::WGS84ToUTM(gnss_pos);

       GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, x, y, gmma, k);

      上述两个函数的转换坐标系方向为: ENU

2、坐标系原点

        上述两个函数的转换后的坐标系原点在：本经度所在的Zone对应的中央子午线与赤道的交点。

       

详细讲解了什么是无人车的自定位系统，以及无人车为什么需要精准的定位系统。最后详细讲解了激光定位、视觉定位和惯性导航。由于单一定位方式有缺陷，因此产生了以卡尔曼滤波器为中心的多传感器融合定位系统。

 

 

 

本周阿波君将与大家分享Apollo自动定位技术——三维几何变换和坐标系介绍。下面，我们一起进入进阶课程第14期。

 

以下，ENJOY

 

本节主要介绍自定位的一些基础知识，主要包括三维几何变换和几个坐标系。

根据各个轴位置关系的不同，空间中的坐标系可以分为左手坐标系和右手坐标系。如下图所示。

空间坐标系

旋转是指在一个坐标系下，把一个点旋转到另一个位置的几何变换。

二维旋转

 

如上所示：将P旋转θ角到P'的位置,在二维坐标系下，由一个2×2的矩阵表示，如图中的R(θ)。点的旋转其实也可以理解为两个坐标系的一个旋转。对于三维而言，多了一个维度，可以用3×3矩阵来表示。

 

分解来看，可以通过X轴Y轴Z轴分别去旋转，对应的分量分别是RX，RY和RZ。三维旋转矩阵可以由对应三个方向的基本旋转矩阵相乘来表示。三维旋转矩阵存在九个元素，对九个元素进行优化是非常复杂。通常情况下会使用是欧拉角或者四元数的方式进行优化，如下图所示。

三维旋转

平移是当前点相对于坐标原点的位置变化，通常由当前位置的坐标表示，由一个3x1的向量表示，如下所示。

三维平移

刚体的位置和朝向

 

刚体是一种有限尺寸，可以忽略形变的固体。自然界不存在完美的刚体，但物体通常可以假定为完美刚体。自动驾驶中汽车可以认为是一个刚体。

 

刚体坐标系通常是取刚体内的一点P为原点建立的局部坐标系。刚体的位置可以看作刚体原点P相对于参考坐标系原点O所做的平移，可以使用三维平移向量表示。刚体的朝向可以由刚体原点P的朝向来表示。

下面介绍一下定位里面常用的坐标系。

ECI地心惯性坐标系，也称i系，这个坐标系如下图所示：

地心惯性坐标系ECI

 

它圆心就是地球的原点，Z轴沿地轴方向朝向北极， X轴和Y轴位于赤道平面内，与Z轴满足右手法则，并且X轴和Y轴分别指向两个恒星。也就是说不随着地球的自转而发生变化。它是一个非常固定的坐标系。IMU测量得到的加速度，角速度都是相对于这个坐标系的。

ECEF地心地固坐标系也称为e系，它的原点也是地球的原点， Z轴指向北极， 它与前面的ECI的区别就在于它的XY随着地球的自转而转动，它是以地球为基准的。它的X轴指向格林威治的子午面的交线， Y轴在赤道平面内与X轴、Z轴满足右手系法则，常用的如WGS84坐标系。其特点是与地球固定在一起，随地球一起转动。

地心地固坐标系ECEF

当地水平坐标系，一般也称为L系。如下图所示，它的原点位于载体所在的地球表面，X轴和Y轴在当地水平面内，分别指向东向和北向，Z轴垂直向上，与X轴和Y轴满足右手法则。在导航解算过程中通常也把该坐标系选取为导航坐标系（n系），也称为“东-北-天（E-N-U）”坐标系。

当地水平坐标系

UTM(UNIVERSAL TRANSVERSE MERCARTOR GRIDSYSTEM，通用横轴墨卡托格网系统)坐标是一种平面直角坐标，这种坐标格网系统及其所依据的投影已经广泛用于地形图。其投影是一种“等角横轴割圆柱投影”，椭圆柱割地球与南纬80度，北纬84度两条等高圈，投影后两条相割的经线上没有变形，而中央经线上长度比为0.9996。该投影方法按经度把地球划分成了60个区域，那么每6度一个区域，例如北京其实在第50区。南北又做了划分，相当于把地球划分成很多块。

UTM投影

车体坐标系原点在载体质量中心与载体固连处（相对于车载，选取原点位于后轴中心位置），X轴沿载体轴向指向右，Y轴指向前，Z轴与X轴和Y轴满足右手法则指向天向。该坐标系通常称为“右-前-上（R-F-U）”坐标系。它是一个局部坐标系，它与N系或者导航坐标系、当地水平坐标系之间的旋转关系表示现在车的姿态。

车体坐标系

IMU坐标系其实和车体坐标系基本上是一样，它的原点在陀螺仪和加速度计的坐标原点，XYZ三个轴方向分别与陀螺仪和加速度计对应的轴向平行。在不考虑安装误差角的情况下，载体坐标系和IMU坐标系是一样的。

IMU坐标系特点

相机坐标系以相机光心为原点，Xc轴和Yc轴与成像平面坐标系的x轴和y轴平行，Zc轴为摄像机的光轴，和图像平面垂直。由点O与Xc、Yc、Zc轴组成的坐标系成为相机的坐标系，如下图右边所示。通常情况下并不会将相机坐标系和其他的全局坐标系直接连接起来，因为已经选IMU作为汽车原点。通过一个外参，把相机和IMU关联起来，也就是标定的外部参数。当知道IMU在世界坐标系的姿态和位置就可以得到相机坐标系到世界坐标系的转换。

相机坐标系

下图为激光雷达坐标系以及其俯视图，从图中可以看出激光雷达坐标系的坐标原点位于多线束中点旋转轴的交点处，Z轴沿轴线向上，X和Y轴如俯视图所示。其测量的点坐标是在激光雷达坐标系下的三维坐标。

 

要转换到世界坐标系，可以使用相机坐标系下点转换到世界坐标系的类似方法。通过IMU坐标系到激光雷达坐标系的外參，以及IMU坐标系的姿态，得到激光雷达坐标系到世界坐标系的转换。

激光雷达坐标系及其俯视图

下图给出了定位模块中输出的各个坐标系下的信息以及不同坐标系之间的关系。

无人车定位信息中涉及的坐标系

 

定位输出信息主要包括UTM坐标系的XY,IMU的姿态四元数， ENU坐标系下的速度。另外还输出一些和车体相关信息，例如车体姿态的四元数，车体坐标系下的加速度和角速度。这些输出的信息可以给感知和PNC使用。各个坐标系之间是一些基本的旋转关系。