机器人运动估计——IMU运动方程与ESKF原理介绍（上）

Hello! 欢迎来到我的博客
今天的内容关于机器人中常用的传感器IMU，我们用它来实现机器人姿态、速度、位置的估计。
今天将会介绍使用低成本IMU进行机器人运动估计的一个常用方法——ESKF。

1 四元数基础

四元数，英文名称为：quaternion。正如一个单位复数可以表示在复平面上的旋转一样，单位四元数也可以用于表示三维空间中的旋转，并且由于其在更新过程中不会发生奇异现象，因此在惯性导航中被广泛使用。一般来说，四元数的表示方式有许多，其中最常用的是两种，一种是Hamilton，另一种是JPL。由于，在机器人领域中，无论是ROS，Eigen，Ceres等等项目中都用到的是Hamilton四元数，因此，这里后续文章中使用以及介绍的也是这种。

以下介绍的都是在IMU运动方程以及ESKF中必要的关于四元数的知识，因此可能会过于简略，读者可暂时先跳转至第二部分阅读，遇到不明的四元数定义时，再回来查阅。

（1）四元数定义

$$Q=a+bi+cj+dk\in \mathbb{H}$$
其中， { a , b , c , d } ∈ R \{a, b, c, d\} \in \mathbb{R} {a,b,c,d}∈R是系数， { i , j , k } \{i, j, k\} {i,j,k}是虚部单位。
一个四元数可以表示为实部和虚部，如下：
$$Q=q_w+q_{x}i+q_{y}j+q_{z}k\iff Q=q_w+{\mathbf{q}}_v$$
记作向量形式则为：
$$\mathbf{q}\triangleq \left[\begin{array}{l}{q}_w\\ {\mathbf{q}}_v\end{array}\right]=\left[\begin{array}{l}{q}_w\\ q_x\\ q_y\\ q_z\end{array}\right]$$

（2）单位四元数

单位四元数可以作为一种旋转的表示方式，它可以转换为欧拉角，也可以转换为旋转矩阵。
单位四元数的定义：
$$\mathbf{q}=\left[\begin{array}{c}\cos \theta \\ \mathbf{u}\sin \theta \end{array}\right]$$
其中，$\mathbf{u}=u_{x}i+u_{y}j+u_{z}k$是单位向量，而$\theta$为一个标量。
由此可知，单位四元数的模长为1。
$$||\mathbf{q}||=\sqrt{{\cos }^2\theta +{\sin }^2\theta (u_x^2+u_y^2+u_z^2)}=1$$

（3）乘法

两个四元数乘法的记号为：$\otimes$，乘法的具体定义为：
$$\mathbf{p}\otimes \mathbf{q}=\left[\begin{array}{l}{p}_w{q}_w-p_x{q}_x-p_y{q}_y-p_z{q}_z\\ p_w{q}_x+p_x{q}_w+p_y{q}_z-p_z{q}_y\\ p_w{q}_y-p_x{q}_z+p_y{q}_w+p_z{q}_x\\ p_w{q}_z+p_x{q}_y-p_y{q}_x+p_z{q}_w\end{array}\right]$$
值得注意的是，两个单位四元数相乘还是单位四元数。

（4）纯四元数的指数函数

纯四元数的定义为，实部为0的四元数，即：$\mathbf{v}=\left[\begin{array}{l}0\\ {\mathbf{q}}_v\end{array}\right]$

定义纯四元数的指数函数如下：
$$e^{\mathbf{v}}=e^{\mathbf{u}\theta }=\cos \theta +\mathbf{u}\sin \theta =\left[\begin{array}{c}\cos \theta \\ \mathbf{u}\sin \theta \end{array}\right]$$
其中：$\mathbf{v}=\mathbf{u}\theta$，并且$\theta =\Vert \mathbf{v}\Vert \in \mathbb{R}$，$\mathbf{u}$为单位向量。

（5）单位四元数转旋转矩阵

R = R { q } = [ q w 2 + q x 2 − q y 2 − q z 2 2 ( q x q y − q w q z ) 2 ( q x q z + q w q y ) 2 ( q x q y + q w q z ) q w 2 − q x 2 + q y 2 − q z 2 2 ( q y q z − q w q x ) 2 ( q x q z − q w q y ) 2 ( q y q z + q w q x ) q w 2 − q x 2 − q y 2 + q z 2 ] \mathbf{R}=\mathbf{R}\{\mathbf{q}\}=\left[\begin{array}{ccc} q_{w}^{2}+q_{x}^{2}-q_{y}^{2}-q_{z}^{2} & 2\left(q_{x} q_{y}-q_{w} q_{z}\right) & 2\left(q_{x} q_{z}+q_{w} q_{y}\right) \\ 2\left(q_{x} q_{y}+q_{w} q_{z}\right) & q_{w}^{2}-q_{x}^{2}+q_{y}^{2}-q_{z}^{2} & 2\left(q_{y} q_{z}-q_{w} q_{x}\right) \\ 2\left(q_{x} q_{z}-q_{w} q_{y}\right) & 2\left(q_{y} q_{z}+q_{w} q_{x}\right) & q_{w}^{2}-q_{x}^{2}-q_{y}^{2}+q_{z}^{2} \end{array}\right] R=R{q}=⎣⎡​qw2​+qx2​−qy2​−qz2​2(qx​qy​+qw​qz​)2(qx​qz​−qw​qy​)​2(qx​qy​−qw​qz​)qw2​−qx2​+qy2​−qz2​2(qy​qz​+qw​qx​)​2(qx​qz​+qw​qy​)2(qy​qz​−qw​qx​)qw2​−qx2​−qy2​+qz2​​⎦⎤​
注意，必须是单位四元数！！！

（6）单位四元数转欧拉角

由于欧拉角的定义方式也有很多，这里说明的是依据Z-Y-X定义的欧拉角，其中欧拉角与旋转矩阵的变换关系是：
$$\mathbf{R}=\left[\begin{array}{ccc}\cos \psi \cos \theta & \cos \psi \sin \theta \sin \varphi -\sin \psi \cos \varphi & \cos \psi \sin \theta \cos \varphi +\sin \psi \sin \varphi \\ \sin \psi \cos \theta & \sin \psi \sin \theta \sin \varphi +\cos \psi \cos \varphi & \sin \psi \sin \theta \cos \varphi -\cos \psi \sin \varphi \\ -\sin \theta & \cos \theta \sin \varphi & \cos \theta \cos \varphi \end{array}\right]$$
其中：$\varphi ,\theta ,\psi$为欧拉角，分别为横滚角(X)，俯仰角(Y)，偏航角(Z)。
所以，四元数转欧拉角的关系为：
$$\begin{array}{rl}\varphi & =\arctan \frac{2\left(q_y{q}_z+q_w{q}_x\right)}{q_w^2-q_x^2-q_y^2+q_z^2}\\ \theta & =-\arcsin [2\left(q_x{q}_z-q_w{q}_y\right)]\\ \psi & =\arctan \frac{2\left(q_x{q}_y+q_w{q}_z\right)}{q_w^2+q_x^2-q_y^2-q_z^2}\end{array}$$
注意，必须是单位四元数！！！

2 IMU运动方程

2.1 关于IMU测量数据

在聊到IMU测量数据的时候，我们首先需要明白两个坐标系的定义。

一是惯性系，二是IMU坐标系。

这里的惯性系就是指静止或者其速度的改变可以忽略不记的坐标系。通常在机器人的应用中，由于所用的IMU都是低成本IMU，对于地球自转角速度不够敏感，所以可以认为与地面固连的坐标系都是惯性坐标系。然而对于，航空航天应用而言，用到的IMU精度很高，对于惯性导航的要求也很严格，则需要考虑地球自转角速度，这是惯性坐标系为地球惯性坐标系。

IMU测量的实际上就是IMU坐标系相对于惯性系的加速度和角速度。值得注意的是，这里的相对于惯性系的加速度不仅包括载体的运动加速度，还包括重力加速度。
所以，我们需要特别注意，加速度计测量的不是载体的运动加速度，而是载体相对惯性空间的绝对加速度和引力加速度之和，称作“比力”。

通常，IMU的测量模型如下：
$$\begin{array}{l}{\mathbf{a}}_m={\mathbf{R}}_t^{\top }\left({\mathbf{a}}_t-{\mathbf{g}}_t\right)+{\mathbf{a}}_{bt}+{\mathbf{a}}_n\\ {\mathbf{\omega }}_m={\mathbf{\omega }}_t+{\mathbf{\omega }}_{bt}+{\mathbf{\omega }}_n\end{array}$$
其中，${\mathbf{R}}_t^{\top }$表示从惯性系到IMU坐标系的旋转矩阵。${\mathbf{a}}_{bt},{\mathbf{\omega }}_{bt}$代表随机游走噪声，可以理解为加速度计和陀螺仪的零漂。${\mathbf{a}}_n,{\mathbf{\omega }}_n$为零均值高斯白噪声。${\mathbf{g}}_t$代表重力加速度，表示在惯性系下。这里关于${\mathbf{g}}_t$的符号不用特别在意，这个负号并不是一定的，也可以写成正号，取决于${\mathbf{g}}_t$的定义。

2.2 IMU运动方程

IMU的运动方程主要描述了物体速度，位置以及姿态在IMU测量数据的激励下产生的变化。除了关于四元数的部分以外，用到的知识就是小学二年级学过的物理知识。比如，位置的导数是速度，速度的导数是加速度。
我们定义我们关心的状态量为：

• ${\mathbf{p}}_t$：表示IMU系相对惯性系的位置，在惯性系下表示的，默认是三维的
• ${\mathbf{v}}_t$：表示IMU系相对惯性系的速度，同样是在惯性系下表示的，默认是三维的
• ${\mathbf{q}}_t$：表示从IMU系到惯性系的四元数，表示从IMU系到惯性系的旋转变换

于是，IMU的运动方程可以表示如下：
$$\begin{array}{rl}{\dot{\mathbf{p}}}_t& ={\mathbf{v}}_t\\ {\dot{\mathbf{v}}}_t& ={\mathbf{a}}_t={\mathbf{R}}_t\left({\mathbf{a}}_m-{\mathbf{a}}_{bt}-{\mathbf{a}}_n\right)+{\mathbf{g}}_t\\ {\dot{\mathbf{q}}}_t& =\frac{1}{2}{\mathbf{q}}_t\otimes {\mathbf{\omega }}_t=\frac{1}{2}{\mathbf{q}}_t\otimes \left({\mathbf{\omega }}_m-{\mathbf{\omega }}_{bt}-{\mathbf{\omega }}_n\right)\\ {\dot{\mathbf{a}}}_{bt}& ={\mathbf{a}}_w\\ {\dot{\mathbf{\omega }}}_{bt}& ={\mathbf{\omega }}_w\\ {\dot{\mathbf{g}}}_t& =0\end{array}$$
其中， R t = R { q t } \mathbf{R}_{t}=\mathbf{R}\{\mathbf{q}_{t}\} Rt​=R{qt​}为基于${\mathbf{q}}_t$得到的旋转矩阵，表示了从IMU系到惯性系的旋转变换。${\mathbf{a}}_w,{\mathbf{\omega }}_w$为零均值高斯白噪声。

值得注意的是，在这组运动方程中，把${\mathbf{g}}_t$也视作变化的量，后续基于此运动方程构造的ESKF也会对${\mathbf{g}}_t$进行更新。这在许多IMU运动方程中是不常见到的，这样做有什么好处呢？

首先，我们要明白${\mathbf{g}}_t$并不等于$[0,0,9.8{]}^T$，${\mathbf{g}}_t$的值取决于IMU的初始姿态。这是因为，IMU运动方程是递推的方程，计算出来的位置和速度都是相对于初始状态的，而不是绝对的位置和速度。因此为了方便起见，我们通常都会定义初始时刻的IMU坐标系为惯性系，这样我们后面计算出来的虽然是相对于初始IMU系的位置和速度，但也可以作为绝对的位置和速度了。于是，${\mathbf{g}}_t$既然代表了惯性系下的重力加速度，也就是初始的IMU坐标系下表示的重力加速度，所以当初始IMU坐标系不是水平的时候，${\mathbf{g}}_t$自然也就不等于$[0,0,9.8{]}^T$了。

进一步地，我们也可以知道，估计${\mathbf{g}}_t$，也就是估计初始时刻IMU的姿态。既然我们通过可以${\mathbf{g}}_t$估计了初始IMU的姿态，那么初始时刻的四元数就可以直接定义为${\mathbf{q}}_0=(1,0,0,0)$。这种操作实际上就是，我们先假设初始时刻四元数为${\mathbf{q}}_0=(1,0,0,0)$，然后在此基础上估计${\mathbf{g}}_t$。当然，还有另外一种方法就是，先假设${\mathbf{g}}_t=[0,0,9.8]$，然后估计初始时刻的IMU相距水平姿态相差多少。这两种方法都可以，然后估计${\mathbf{g}}_t$的话可以使模型的线性化程度更好。

为了更好的阅读体验，本文分为上下两文，关于ESKF的内容以及MATLAB代码，请移步下一篇文章。