【多传感器融合定位SLAM专栏】前端里程计、IMU预积分、滤波、图优化推导与应用（3）

本专栏基于深蓝学院《多传感器融合定位》课程基础上进行拓展，对多传感器融合SLAM的学习过程进行记录

文章目录

第三章惯性导航原理及误差分析
- 惯性器件
- - 1. 机械陀螺
  - 2. 激光陀螺
  - MEMS陀螺
  - 加速度计
- 惯性器件误差分析
- - 信号误差组成
- 惯性器件内参标定
- - 内参误差模型
  - 内参误差标定
  - - 标定方法
    - 基于转台的标定
- 三维运动的描述和微分性质
- - 三维运动描述基础知识
  - - 姿态描述方法
    - 描述方法之间的关系
  - 三维运动微分性质
- 惯性导航解算
- 惯性导航误差分析

第三章惯性导航原理及误差分析

惯性器件

1. 机械陀螺

特性：

定轴性:当陀螺转子以高速旋转时，在没有任何外力矩作用在陀螺仪上时，陀螺仪的自转轴在惯性空间中的指向保持稳定不变，即指向一个固定的方向；同时反抗任何改变转子轴向的力量。
进动性:当转子高速旋转时，若外力矩作用于外环轴，陀螺仪将绕内环轴转动；若外力矩作用于内环轴，陀螺仪将绕外环轴转动。其转动角速度方向与外力矩作用方向互相垂直。

2. 激光陀螺

原理：干涉仪相对惯性空间静止时，光路A和B的光程相等，当有角速度时，光程不相等，便会产生干涉。

MEMS陀螺

科里奥利力（Coriolis force，简称为科氏力），是对旋转体系（比如自传的地球，旋转的圆盘等）中进行直线运动的质点由于惯性相对于旋转体系产生的直线运动的偏移的一种描述。

加速度计

当运载体相对惯性空间做加速度运动时，仪表壳体也随之做相对运动，质量块保持惯性，朝着与加速度方向相反的方向产生位移（拉伸或压缩弹簧）。当位移量达到一定值时，弹簧给出的力使质量块以同一加速度相对惯性空间做加速运动，加速度的大小与方向影响质量块相对位移的方向及拉伸量。

惯性器件误差分析

信号误差组成

1）量化噪声

一切量化操作所固有的噪声，是数字传感器必然出现的噪声产生原因：通过AD采集把连续时间信号采集成离散信号的过程中，精度会损失，精度损失的大小和AD转换的步长有关，步长越小，量化噪声越小。

2）角度随机游走
宽带角速率白噪声：陀螺输出角速率是含噪声的，该噪声中的白噪声成分；

产生原因：计算姿态的本质是对角速率做积分，这必然会对噪声也做了积分。白噪声的积分并不是白噪声，而是一个马尔可夫过程，即当前时刻的误差是在上一时刻误差的基础上累加一个随机白噪声得到的。

角度误差中所含的马尔可夫性质的误差，称为角度随机游走。

3）角速率随机游走

与角度随机游走类似，角速率误差中所含的马尔可夫性质的误差，称为角速率随机游走。而这个马尔可夫性质的误差是由宽带角加速率白噪声累积的结果。

4）零偏不稳定性噪声

零偏：即常说的bias，一般不是一个固定参数，而是在一定范围内缓慢随机飘移。
零偏不稳定性：零偏随时间缓慢变化，其变化值无法预估，需要假定一个概率区间描述它有多大的可能性落在这个区间内。时间越长，区间越大。

5）速率斜坡

该误差是趋势性误差，而不是随机误差。
随机误差，是指你无法用确定性模型去拟合并消除它，最多只能用概率模型去描述它，这样得到的预测结果也是概率性质的。
趋势性误差，是可以直接拟合消除的，在陀螺里产生这种误差最常见的原因是温度引起零位变化，可以通过温补来消除。

6）零偏重复性

多次启动时，零偏不相等，因此会有一个重复性误差。在实际使用中，需要每次上电都重新估计一次。
Allan方差分析时，不包含对零偏重复性的分析。

惯性器件内参标定

内参误差模型

1）零偏
误差解释：陀螺仪或加速度计输出中的常值偏移，即常说的bias。
误差特性：由于零偏存在不稳定性，因此零偏并不是固定不变的。
解决办法：实际使用中，只能一段时间内近似为常值。

加速度计的零偏表示为
ba=[baxbaybaz]b_a = [b_{ax} \ b_{ay} \ b_{az}] ba=[bax bay baz]
陀螺仪的零偏表示为
bg=[bgxbgybgz]b_g = [b_{gx} \ b_{gy} \ b_{gz}] bg=[bgx bgy bgz]
2）刻度系数误差
误差解释：器件的输出往往为脉冲值或模数转换得到的值，需要乘以一个刻度系数才能转换成角速度或加速度值，若该系数不准，便存在刻度系数误差。
误差特性：不一定是常值，它会随着输入大小的不同而发生变化，这个就是标度因数的非线性。
解决办法：如果非线性程度比较大，则需要在标定之前先拟合该非线性曲线，并补偿成线性再去做标定。

加速度计的标度因数为
Ka=[KaxKayKaz]K_a = \begin{bmatrix} K_{ax} \\ && K_{ay} \\ && && K_{az} \end{bmatrix} Ka=KaxKayKaz
陀螺仪的标度因数为
Kg=[KgxKgyKgz]K_g = \begin{bmatrix} K_{gx} \\ && K_{gy} \\ && && K_{gz} \end{bmatrix} Kg=KgxKgyKgz
3）安装误差
误差解释：如右图所示，b坐标系是正交的imu坐标系，g坐标系的三个轴是分别对应三个陀螺仪。由于加工工艺原因，陀螺仪的三个轴并不正交，而且和b坐标系的轴不重合，二者之间的偏差即为安装误差。
误差特性：实际系统中，由于硬件结构受温度影响，安装误差也会随温度发生变化。
解决办法：在不同温度下做标定，补偿温度变化量。

陀螺仪的安装误差为
Sg=[0SgxySgxzSgyz0SgyzSgzxSgzy0]S_g = \begin{bmatrix} 0 & S_{gxy} & S_{gxz} \\ S_{gyz} &0 &S_{gyz} \\ S_{gzx}& S_{gzy} &0 \end{bmatrix} Sg=0SgyzSgzxSgxy0SgzySgxzSgyz0
加速度计的安装误差为
Sa=[0SaxySaxzSayz0SayzSazxSazy0]S_a = \begin{bmatrix} 0 & S_{axy} & S_{axz} \\ S_{ayz} &0 &S_{ayz} \\ S_{azx}& S_{azy} &0 \end{bmatrix} Sa=0SayzSazxSaxy0SazySaxzSayz0

4）内参误差模型

利用以下公式，把各项误差综合到一起
W=Kg(I+Sg)ω+bg≈(Kg+Sg)ω+bgA=Ka(I+Sa)a+ba≈(Ka+Sa)ω+baW = K_g(I+S_g)\omega + b_g \approx (K_g+S_g)\omega + b_g \\ A= K_a (I+S_a)a + b_a\approx (K_a+S_a)\omega + b_a W=Kg(I+Sg)ω+bg≈(Kg+Sg)ω+bgA=Ka(I+Sa)a+ba≈(Ka+Sa)ω+ba
则陀螺仪的输出可以展开为
[WxWyWz]=[KgxSgxySgxzSgyzKgySgyzSgzxSgzyKgz][ωxωyωz]+[bgxbgybgz]\begin{bmatrix} W_x\\W_y\\W_z\end{bmatrix}= \begin{bmatrix} K_{gx} & S_{gxy} & S_{gxz} \\ S_{gyz} &K_{gy} &S_{gyz} \\ S_{gzx}& S_{gzy} &K_{gz} \end{bmatrix} \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z\end{bmatrix} + \begin{bmatrix} b_{gx} \\ b_{gy}\\b_{gz} \end{bmatrix} WxWyWz=KgxSgyzSgzxSgxyKgySgzySgxzSgyzKgzωxωyωz+bgxbgybgz
加速度计的输出可以展开为
[AxAyAz]=[KaxSaxySaxzSayzKaySayzSazxSazyKaz][axayaz]+[baxbaybaz]\begin{bmatrix} A_x\\A_y\\A_z\end{bmatrix}= \begin{bmatrix} K_{ax} & S_{axy} & S_{axz} \\ S_{ayz} &K_{ay} &S_{ayz} \\ S_{azx}& S_{azy} &K_{az} \end{bmatrix} \begin{bmatrix} a_x \\ a_y \\ a_z\end{bmatrix} + \begin{bmatrix} b_{ax} \\ b_{ay}\\b_{az} \end{bmatrix} AxAyAz=KaxSayzSazxSaxyKaySazySaxzSayzKazaxayaz+baxbaybaz

内参误差标定

标定方法

标定的本质是参数辨识，参数包括陀螺仪和加速度计各自的零偏、刻度系数误差、安装误差。
辨识方法包括：
1）解析法或最小二乘
2）迭代优化方法
3）滤波（Kalman等）

常见标定方法与上面辨识方法的对应关系为：
1）基于转台的标定：解析法、最小二乘；
2）不需要转台的标定：梯度下降迭代优化；
3）系统级标定：kalman滤波（只适用于高精度惯导）。

基于转台的标定

在IMU的误差模型中，陀螺仪和加速度计的误差方程是互相独立的，可分别标定。

以加速度计为例，其误差模型方程为：
[AxAyAz]=[KaxSaxySaxzSayzKaySayzSazxSazyKaz][axayaz]+[baxbaybaz]\begin{bmatrix} A_x\\A_y\\A_z\end{bmatrix}= \begin{bmatrix} K_{ax} & S_{axy} & S_{axz} \\ S_{ayz} &K_{ay} &S_{ayz} \\ S_{azx}& S_{azy} &K_{az} \end{bmatrix} \begin{bmatrix} a_x \\ a_y \\ a_z\end{bmatrix} + \begin{bmatrix} b_{ax} \\ b_{ay}\\b_{az} \end{bmatrix} AxAyAz=KaxSayzSazxSaxyKaySazySaxzSayzKazaxayaz+baxbaybaz
误差模型方程是一个包含12个未知参数的方程组，显然方程组没有唯一解。此时，通过改变输入，获得多个不同方程（大于12个），组成的方程组便可求解参数。

以上就是分立级标定方法的思路，具体求解方法包括解析法和最小二乘法。

**该标定方法的核心：**通过旋转lMU，改变其输入构造方程组，并且每个位置对应的加速度输入和角速度输入都必须是已知的。构建方程组时，不仅要方程组数量足够，而且要能够使误差参数可解，即系数矩阵可逆。为了满足这一点，常见的转位方案有六位置、八位置、十二位置等。在实际使用时，通过判断系数矩阵是否满秩便可判断，理论上，只要转位方案能满足这一条件，就可以使用。

三维运动的描述和微分性质

多传感器融合中三维运动的导航信息包含姿态、速度、位置，其中姿态的处理最为复杂，也最为核心。

姿态有三种表示形式：欧拉角、旋转矩阵、四元数，此外还有等效旋转矢量，但它一般在中间计算过程中使用。

三维运动描述基础知识

姿态描述方法

欧拉角

欧拉角等同于把姿态绕三次不同轴旋转。不同的旋转顺序会得到不同的欧拉角，常见的有

机器人坐标系：xyz分别对应前左上，旋转顺序为z-y-x；
惯性导航坐标系：xyz分别对应右前上，旋转顺序为z-x-y；
另一种惯性导航坐标系：xyz分别对应前右下，旋转顺序为z-y-x。

万向锁：当载体处在某个姿态时，会产生奇异性问题，导致丢失一个自由度。不同的旋转顺序下，产生万向锁时所处的姿态不同。不同坐标系定义下，选择不同旋转顺序的原因：其本质都是按照航向->俯仰->横滚”的顺序旋转，因为此时万向锁出现在俯仰为90时的情况，而多数载体出现该姿态的几率最小。

需要注意的是，欧拉角有明确的物理意义，不随坐标系定义的不同而改变：

俯仰角：载体抬头为正，低头为负；
横滚角：向右滚为正，向左滚为负；
航向角：机器人中，一般以逆时针旋转为正，顺时针旋转为负。（在与地理系相关的惯性导航中，常以北偏东为正，北偏西为负，遇到时需要注意）

由于欧拉角必然产生奇异性，因此一般只用它做人机交互的显示，而不用来做姿态解算。

旋转矩阵

旋转矩阵是描述旋转的一个三维矩阵，一个真实姿态对应一个唯一的旋转矩阵。

旋转矩阵是单位正交矩阵，行列式为1，满足p=R−1p′=RTp′p = R^{-1}p' = R^Tp'p=R−1p′=RTp′。

优点：没有奇异性，适合解算；缺点：用九个元素表示三个自由度，增加计算复杂度；为了保持正交性，一般更新完毕后要重新正交化。
四元数
- 四元数是超复数，即复数的复数。设有复数
A=a+biB=c+diA= a+bi \\ B = c+di A=a+biB=c+di

则复数的复数为
q=A+Bj=a+bi+cj+dijq = A+Bj = a+bi+cj+dij q=A+Bj=a+bi+cj+dij
令k=ijk = ijk=ij有
q=a+bi+cj+dkq = a+bi+cj+dk q=a+bi+cj+dk
即四元数。一般四元数的常见表示符号为
q=qw+qv=qw+qxi+qyj+qzkq =q_w + q_v= q_w + q_x i + q_ y j+q_zk q=qw+qv=qw+qxi+qyj+qzk
- 共轭四元数（实部相同，虚部相反）
q∗=qw−qv=qw−qxi−qyj−qzkq^* = q_w - q_v = q_w-q_xi-q_yj-q_zk q∗=qw−qv=qw−qxi−qyj−qzk
- 四元数的逆为
q−1=q∗/∣∣q∣∣q^{-1} = q^*/||q|| q−1=q∗/∣∣q∣∣
- 四元数的模长为
∣q∣=qw2+qx2+qy2+qz2|q| = \sqrt{q_w^2 + q_x^2 + q_y^2 + q_z^2} ∣q∣=qw2+qx2+qy2+qz2
- 姿态运算时，四元数为单位四元数（模长为1），即
q−1=q∗=qw−qvq^{-1} = q^* = q_w - q_v q−1=q∗=qw−qv

单位四元数可以表示旋转作用，那么单位四元数的逆就表示对这个旋转作用的抵消作用。
- 用四元数表示旋转：设有一空间三维点p=[x,y,z]∈R3p = [x,y,z]\in R^3p=[x,y,z]∈R3，以及一个单位四元数q指定的旋转。
  - 用矩阵描述：p′=Rpp' = Rpp′=Rp
  - 用四元数描述：虚四元数p=[0,x,y,z]T=[0,v]Tp=[0,x,y,z]^T=[0,v]^Tp=[0,x,y,z]T=[0,v]T 则旋转后的点p′=qpq−1p' = qpq^{-1}p′=qpq−1，p′p'p′的虚部即旋转之后点的坐标。
- 四元数乘法
  p⊗q=[pwqw−pvTqvpwqv+qwpv+pv×qv]p\otimes q = \begin{bmatrix} p_wq_w - \bold{p_v}^T\bold{q_v} \\p_w\bold{q_v} + q_w\bold{p_v} + \bold{p_v}\times \bold{q_v} \end{bmatrix} p⊗q=[pwqw−pvTqvpwqv+qwpv+pv×qv]
  如果在四元数乘法中出现三维向量，指的是和三维向量构成的纯虚四元数相乘，比如
  p⊗u=p⊗[0u1iu2ju3k]p\otimes u = p\otimes \begin{bmatrix} 0\\u_1i\\u_2j\\u_3k \end{bmatrix} p⊗u=p⊗0u1iu2ju3k
  乘法结合律
  (p⊗q)⊗r=p⊗(q⊗r)p⊗(q+r)=p⊗q+p⊗r(p+q)⊗r=p⊗r+p⊗r(p\otimes q) \otimes r = p\otimes (q\otimes r) \\ p\otimes (q+r) = p\otimes q+p\otimes r \\ (p+q)\otimes r = p\otimes r + p\otimes r (p⊗q)⊗r=p⊗(q⊗r)p⊗(q+r)=p⊗q+p⊗r(p+q)⊗r=p⊗r+p⊗r
  重要：四元数相乘，可以展开成矩阵与向量相乘的形式
  p⊗q=[p]Lq[p]L=[pw−px−py−pzpxpw−pzpypypzpw−pxpz−pypxpw]=pwI+[0−pvTpv[pv]×]p\otimes q = [p]_Lq\\ [p]_L = \begin{bmatrix} p_w & -p_x &-p_y & -p_z \\ p_x & p_w & -p_z & p_y \\ p_y & p_z & p_w & -p_x \\ p_z & -p_y & p_x &p_w \end{bmatrix} = p_w I+\begin{bmatrix} 0 & -p_v^T \\ p_v & [p_v]_\times \end{bmatrix} p⊗q=[p]Lq[p]L=pwpxpypz−pxpwpz−py−py−pzpwpx−pzpy−pxpw=pwI+[0pv−pvT[pv]×]
  其中，符号[.]×=[.]∧[.]_\times = [.]^\wedge[.]×=[.]∧，表示一个向量映射到一个反对称矩阵。
  [a⃗]×=Δ[0−azayaz0−ax−ayax0][a⃗]×b⃗=a⃗×b⃗=−b⃗×a⃗,a⃗,b⃗∈R3[\vec a]_\times \mathop{=}^\Delta \begin{bmatrix} 0 & -a_z & a_y \\ a_z & 0 & -a_x \\ -a_y & a_x &0 \end{bmatrix} \\ [\vec a]_\times \vec b = \vec a\times \vec b = -\vec b \times \vec a,\ \ \ \ \vec a,\vec b\in R^3 [a]×=Δ0az−ay−az0axay−ax0[a]×b=a×b=−b×a, a,b∈R3
  相似的，也可以展开为
  p⊗q=[q]Rp[p]R=[qw−qx−qy−qzqxqwqz−qyqy−qzqwqxqzqy−qxqw]=qwI+[0−qvTqv−[qv]×]=qwI+ΩΩ=[0−qvTqv−[qv]×]4×4p\otimes q = [q]_R p\\ [p]_R = \begin{bmatrix} q_w & -q_x &-q_y & -q_z \\ q_x & q_w & q_z & -q_y \\ q_y & -q_z & q_w & q_x \\ q_z & q_y & -q_x &q_w \end{bmatrix} = q_w I+\begin{bmatrix} 0 & -q_v^T \\ q_v & -[q_v]_\times \end{bmatrix} = q_wI+\Omega \\ \Omega = \begin{bmatrix} 0 & -q_v^T \\ q_v & -[q_v]_\times \end{bmatrix}_{4\times 4} p⊗q=[q]Rp[p]R=qwqxqyqz−qxqw−qzqy−qyqzqw−qx−qz−qyqxqw=qwI+[0qv−qvT−[qv]×]=qwI+ΩΩ=[0qv−qvT−[qv]×]4×4
  因此可以推导出一个重要性质
  (q⊗x)⊗p=[p]R[q]Lxq⊗(x⊗p)=[q]L[p]Rx(q\otimes \bold x )\otimes p = [p]_R[q]_L\bold x \\ q\otimes(\bold x \otimes p ) = [q]_L [p]_R\bold x (q⊗x)⊗p=[p]R[q]Lxq⊗(x⊗p)=[q]L[p]Rx
等效旋转矢量

把旋转当做绕空间一个固定轴转过一个角度。在不引起歧义的情况下可简称为旋转矢量或旋转向量。直接用向量ϕ⃗\vec\phiϕ表示，其方向即为转轴方向，对应的单位向量记为u⃗\vec uu，它的长度ϕ=∣ϕ⃗∣\phi = |\vec{\phi}|ϕ=∣ϕ∣即为转角。

描述方法之间的关系

欧拉角与旋转矩阵

按照机器人前(x)-左(y)-上(z)的坐标系定义，令横滚角为α\alphaα，俯仰角为β\betaβ，航向角为γ\gammaγ。
- 欧拉角—>旋转矩阵
  
  由于旋转矩阵是按照z-y-x的顺序旋转得来的，因此可以表示为
  Rwb=(Rx(α)Ry(−β)Rz(γ))TRx(α)=[1000cos⁡αsin⁡α0−sin⁡αcos⁡α]Ry(−β)=[cos⁡β0sin⁡β010−sin⁡β0cos⁡β]Rz(γ)=[cos⁡γsin⁡γ0−sin⁡γcos⁡γ0001]R_{wb} = (R_x(\alpha) R_y(-\beta) R_z(\gamma))^T \\ R_x(\alpha) = \begin{bmatrix} 1 & 0& 0 \\ 0 & \cos\alpha & \sin \alpha \\ 0& -\sin \alpha & \cos \alpha \end{bmatrix} R_y(-\beta) = \begin{bmatrix} \cos\beta & 0& \sin \beta \\ 0 & 1 & 0 \\ -\sin \beta&0 & \cos \beta \end{bmatrix} R_z(\gamma) = \begin{bmatrix} \cos\gamma & \sin \gamma & 0 \\ -\sin \gamma & \cos\gamma & 0 \\ 0& 0 & 1\end{bmatrix}Rwb=(Rx(α)Ry(−β)Rz(γ))TRx(α)=1000cosα−sinα0sinαcosαRy(−β)=cosβ0−sinβ010sinβ0cosβRz(γ)=cosγ−sinγ0sinγcosγ0001
- 旋转矩阵—>欧拉角
  
  由欧拉角得到的旋转矩阵，为
  Rwb=[cos⁡βcos⁡γ−sin⁡αsin⁡βcos⁡γ−cos⁡αsin⁡γsin⁡αsin⁡γ−cos⁡αsin⁡βcos⁡γcos⁡βsin⁡γcos⁡αcos⁡γ−sin⁡αsin⁡βsin⁡γ−cos⁡αsin⁡βsin⁡γ−sin⁡αcos⁡γsin⁡βsin⁡αcos⁡βcos⁡αcos⁡β]R_{wb} = \begin{bmatrix} \cos \beta \cos \gamma && -\sin\alpha \sin \beta\cos\gamma-\cos\alpha\sin\gamma && \sin \alpha\sin\gamma-\cos\alpha\sin\beta\cos\gamma \\ \cos\beta\sin\gamma && \cos\alpha\cos\gamma-\sin\alpha\sin\beta\sin\gamma && -\cos\alpha\sin\beta\sin\gamma-\sin\alpha\cos\gamma \\ \sin\beta && \sin\alpha\cos\beta && \cos\alpha\cos\beta \end{bmatrix} Rwb=cosβcosγcosβsinγsinβ−sinαsinβcosγ−cosαsinγcosαcosγ−sinαsinβsinγsinαcosβsinαsinγ−cosαsinβcosγ−cosαsinβsinγ−sinαcosγcosαcosβ
  观察矩阵可以推导得到
  α=arctan⁡2(Rwb(3,2),Rwb(3,3))β=arcsin⁡(Rwb(3,1))γ=arctan⁡2(Rwb(2,1),Rwb(1,1))\alpha = \arctan2(R_{wb}(3,2),R_{wb}(3,3)) \\ \beta = \arcsin (R_{wb}(3,1))\\ \gamma = \arctan2(R_{wb}(2,1),R_{wb}(1,1)) α=arctan2(Rwb(3,2),Rwb(3,3))β=arcsin(Rwb(3,1))γ=arctan2(Rwb(2,1),Rwb(1,1))
旋转矩阵与四元数
- 四元数—>旋转矩阵
  Rwb=[qw2+qx2−qy2−qz22(qxqy−qwqz)2(qxqz+qwqy)2(qxqy+qwqz)qw2−qx2+qy2−qz22(qyqz−qwqx)2(qxqz−qwqy)2(qyqz+qwqx)qw2−qx2−qy2+qz2]R_{wb} = \begin{bmatrix} q_w^2 + q_x^2 - q_y^2-q_z^2 & 2(q_xq_y-q_wq_z) & 2(q_xq_z+q_wq_y) \\ 2(q_xq_y+q_wq_z) & q_w^2 - q_x^2 + q_y^2 - q_z^2 & 2(q_yq_z - q_wq_x) \\ 2(q_xq_z-q_wq_y) & 2(q_yq_z + q_wq_x) & q_w^2 - q_x^2 - q_y^2+q_z^2 \end{bmatrix} Rwb=qw2+qx2−qy2−qz22(qxqy+qwqz)2(qxqz−qwqy)2(qxqy−qwqz)qw2−qx2+qy2−qz22(qyqz+qwqx)2(qxqz+qwqy)2(qyqz−qwqx)qw2−qx2−qy2+qz2
- 旋转矩阵—>四元数
  qw=1+Rwb(1,1)+Rwb(2,2)+Rwb(3,3)2qx=Rwb(3,2)−Rwb(2,3)4qwqy=Rwb(1,3)−Rwb(3,1)4qwqz=Rwb(2,1)−Rwb(1,2)4qwq_w = \frac{\sqrt{1+R_{wb}(1,1) + R_{wb}(2,2) + R_{wb}(3,3)}}{2} \\ q_x = \frac{R_{wb}(3,2)-R_{wb}(2,3)}{4q_w} \\ q_y = \frac{R_{wb}(1,3) - R_{wb}(3,1)}{4q_w} \\ q_z = \frac{R_{wb}(2,1) - R_{wb}(1,2)}{4q_w} qw=21+Rwb(1,1)+Rwb(2,2)+Rwb(3,3)qx=4qwRwb(3,2)−Rwb(2,3)qy=4qwRwb(1,3)−Rwb(3,1)qz=4qwRwb(2,1)−Rwb(1,2)
  需要满足
  qw≠01+Rwb(1,1)+Rwb(2,2)+Rwb(3,3)>0q_w \neq 0\\ 1+R_{wb}(1,1) +R_{wb}(2,2) +R_{wb}(3,3)>0 qw=01+Rwb(1,1)+Rwb(2,2)+Rwb(3,3)>0
旋转矩阵与旋转矢量
- 旋转矢量—>旋转矩阵
  Rwb=I+sin⁡ϕϕ(ϕ⃗∧)+1−cos⁡ϕϕ2(ϕ⃗∧)2R_{wb} = I +\frac{\sin \phi}{\phi}(\vec\phi^\wedge) + \frac{1-\cos\phi}{\phi^2}(\vec\phi^\wedge)^2 Rwb=I+ϕsinϕ(ϕ∧)+ϕ21−cosϕ(ϕ∧)2
  此公式也被称为罗德里格斯公式，并且与旋转矢量的指数运算结果相同。
- 旋转矩阵—>旋转矢量
  ϕ=arccos⁡tr(Rwb)−12u⃗=(Rwb−(Rwb)T)∨2sin⁡ϕ\phi =\arccos\frac{tr(R_{wb})-1}{2} \\ \vec u = \frac{(R_{wb}-(R_{wb})^T)^\vee}{2\sin\phi} ϕ=arccos2tr(Rwb)−1u=2sinϕ(Rwb−(Rwb)T)∨
  其中，符号.∨.^\vee.∨表示从反对称矩阵得到对应的矢量。
四元数与旋转矢量
- 旋转矢量—>四元数
  q=cos⁡ϕ2+sin⁡ϕ2ϕϕ⃗q=\cos\frac{\phi}{2} + \frac{\sin \frac{\phi}{2}}{\phi}\vec \phi q=cos2ϕ+ϕsin2ϕϕ
- 四元数—>旋转矢量
  ϕ=2arctan⁡(∣∣qv∣∣,qw)u⃗=qv/∣∣qv∣∣\phi = 2\arctan(||q_v||,q_w) \\ \vec u = q_v /||q_v|| ϕ=2arctan(∣∣qv∣∣,qw)u=qv/∣∣qv∣∣

三维运动微分性质

旋转矩阵微分方程

假设世界坐标系（w）中有一个固定不动的矢量rwr^wrw，在载体坐标系（b）下的表示为rbr^brb，即有
rw=Rwbrbr^w = R_{wb}r^b rw=Rwbrb
两边同时对时间求微分有
r˙w=R˙wbrb+Rwbr˙b\dot r^w = \dot R_{wb}r^b + R_{wb}\dot r^b r˙w=R˙wbrb+Rwbr˙b
绕定轴转动时向量对时间的导数为
dr⃗bdt=−w⃗wb×r⃗b=w⃗bw×r⃗b\frac{d\vec r^b}{dt }= -\vec w_{wb} \times \vec r^b = \vec w_{bw} \times \vec r^b dtdrb=−wwb×rb=wbw×rb
最终可得
R˙wb=Rwb[ω⃗wbb]×\dot R_{wb} = R_{wb} [\vec \omega_{wb}^b]_\times R˙wb=Rwb[ωwbb]×
其中ω⃗wbb\vec \omega_{wb}^bωwbb代表载体旋转角速度在b系下的显示，实际使用时就是陀螺仪的角速度输出。

四元数微分方程

rwr^wrw和rbr^brb两个矢量之间可以用四元数乘法(⊗\otimes⊗)转换如下
rw=qwb⊗rb⊗qwb∗r^w = q_{wb} \otimes r^b\otimes q^*_{wb} rw=qwb⊗rb⊗qwb∗
可得到
q˙wb=qwb⊗12[0ω⃗wbb]=12[0ω⃗wbb]Rqwb=12Ω(ω⃗wbb)qwb\dot q_{wb} = q_{wb} \otimes \frac{1}{2} \begin{bmatrix}0 \\ \vec \omega_{wb}^b \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 \\ \vec \omega_{wb}^b \end{bmatrix} _R q_{wb} = \frac{1}{2} \Omega(\vec \omega_{wb}^b)q_{wb} q˙wb=qwb⊗21[0ωwbb]=21[0ωwbb]Rqwb=21Ω(ωwbb)qwb

等效旋转矢量微分方程

在旋转矩阵微分方程中，把旋转矩阵用等效旋转矢量表示，则可以求出等效旋转矢量的微分方程。同样地，在四元数微分方程中也可以按此方式得到。
此处直接给出结论
ϕ⃗˙=ω⃗wbb+12ϕ⃗×ω⃗wbb\dot{\vec \phi} = \vec \omega_{wb}^b + \frac{1}{2}\vec \phi \times \vec \omega_{wb}^b ϕ˙=ωwbb+21ϕ×ωwbb

惯性导航解算

惯性导航解算概述

1）目的：利用IMU测量的角速度、加速度，根据上一时刻导航信息，求解出当前时刻导航信息，包括姿态解算、速度解算、位置解算。
2）方法：由姿态、速度、位置的微分方程，推导出它们的解，并转变成离散时间下的近似形式，从而可以在离散时间采样下，完成导航信息求解。
3）优点：IMU不受外界信号干扰，可以得到稳定、平滑的导航结果。
4）缺点：误差随时间累计，一般时间越长，累计误差越大。因此需要融合，而导航解算（本小节）及其误差分析（下一小节），是融合的基础。
姿态更新
- 基于旋转矩阵的姿态更新
  
  根据旋转矩阵的微分方程
  R˙wb=Rwb[ω]×\dot R_{wb} = R_{wb} [\omega]_\times R˙wb=Rwb[ω]×
  由微分方程的求解方法，可以写出由k-1时刻求解k时刻的旋转矩阵的公式
  Rwbk=Rwbk−1e∫tk−1tk[ω]×dτR_{wb_k} = R_{wb_{k-1}}e^{\int_{t_k-1}^{t_k}[\omega]_\times d\tau} Rwbk=Rwbk−1e∫tk−1tk[ω]×dτ
  指数上的积分项其实就是k-1时刻到k时刻之间的相对旋转对应的等效旋转矢量构成的反对称矩阵。因此上式可重新书写为
  Rwbk=Rwbk−1eϕ×R_{wb_k} = R_{wb_{k-1}}e^{\phi_\times} Rwbk=Rwbk−1eϕ×
  根据罗德里格斯公式上式可以写为
  Rwbk=Rwbk−1Rbk−1bkR_{wb_k} = R_{wb_{k-1}} R_{b_{k-1}b_k} Rwbk=Rwbk−1Rbk−1bk
  其中
  Rbk−1bk=I+sinϕϕ(ϕ×)+1−cosϕϕ2(ϕ×)2R_{b_{k-1}b_k} = I + \frac{sin\phi}{\phi}(\phi_\times) + \frac{1-cos\phi}{\phi^2}(\phi_\times)^2 Rbk−1bk=I+ϕsinϕ(ϕ×)+ϕ21−cosϕ(ϕ×)2
  可以看出，解算过程中需要求解等效旋转矢量ϕ\phiϕ，需要借助其微分方程实现。在旋转矢量的微分方程 ϕ˙≈ω+12ϕ×ω\dot \phi \approx \omega + \frac{1}{2}\phi \times \omegaϕ˙≈ω+21ϕ×ω 中，对于右侧第二项，根据叉乘的定义可知，当叉乘的两个矢量之间方向重合时，结果为0，在k-1时刻到k时刻的极短时间内，可认为接近重合，因此，旋转矢量微分方程可进一步化简为
  ϕ˙≈ω≈∫tk−1tkω(τ)dτ\dot \phi \approx \omega\approx \int _{t_{k-1}} ^{t_k} \omega(\tau) d\tau ϕ˙≈ω≈∫tk−1tkω(τ)dτ
  欧拉法：ϕ=ωk−1(tk−tk−1)\phi = \omega_{k-1} (t_k - t_{k-1})ϕ=ωk−1(tk−tk−1)
  
  中值法：ϕ=ωk−1+ωk2(tk−tk−1)\phi = \frac{\omega_{k-1} + \omega_k}{2}(t_k - t_{k-1})ϕ=2ωk−1+ωk(tk−tk−1) 精度更高
- 基于四元数的姿态更新
  
  根据推导，四元数的微分方程为
  q˙wb=qwb⊗12[0ωwbb]\dot q_{wb} = q_{wb} \otimes \frac{1}{2} \begin{bmatrix}0 \\ \omega_{wb}^b \end{bmatrix} q˙wb=qwb⊗21[0ωwbb]
  其矩阵形式为
  q˙wb=12[0ω]Rqwb\dot q_{wb} = \frac{1}{2} \begin{bmatrix} 0 \\ \omega \end{bmatrix} _R q_{wb} q˙wb=21[0ω]Rqwb
  同样按照解微分方程的方法，并对指数项进行积分，得到
  q˙wb=e12⊝qwb\dot q_{wb} = e^{\frac{1}{2} \circleddash} q_{wb} q˙wb=e21⊝qwb
  其中
  ⊝=[0−ϕx−ϕy−ϕzϕx0ϕz−ϕyϕy−ϕz0ϕxϕzϕy−ϕx0]\circleddash = \begin{bmatrix} 0 & -\phi_x & -\phi_y & -\phi_z\\ \phi_x&0& \phi_z & -\phi_y \\ \phi_y & -\phi_z & 0 & \phi_x \\ \phi_z & \phi_y & -\phi_x & 0 \end{bmatrix} ⊝=0ϕxϕyϕz−ϕx0−ϕzϕy−ϕyϕz0−ϕx−ϕz−ϕyϕx0
  为了求解该方程，要对指数函数进行泰勒展开，同样包含高次幂
  e12⊝(T)=Icosϕ2+⊝ϕsinϕ2e^{\frac{1}{2} \circleddash(T)} = Icos\frac{\phi}{2} + \frac{\circleddash}{\phi} sin\frac{\phi}{2} e21⊝(T)=Icos2ϕ+ϕ⊝sin2ϕ
  因此有
  qwbk=[Icosϕ2+⊝ϕsinϕ2]qwbk−1q_wb_k = [Icos\frac{\phi}{2} + \frac{\circleddash}{\phi} sin\frac{\phi}{2}]q_{wb_{k-1}} qwbk=[Icos2ϕ+ϕ⊝sin2ϕ]qwbk−1
  则
  qbk−1bk=[cosϕ2,Φϕsinϕ2]Tq_{b_{k-1}b_k} = [cos\frac{\phi}{2}, \frac{\Phi}{\phi} sin\frac{\phi}{2}]^T qbk−1bk=[cos2ϕ,ϕΦsin2ϕ]T
  则
  qwbk=qwbk−1⊗qbk−1bkq_{wb_k} = q_{wb_{k-1}} \otimes q_{b_{k-1}b_k} qwbk=qwbk−1⊗qbk−1bk
  其中旋转矢量的计算可采用中值法计算。
速度更新

速度微分方程为
v˙=Rwba−g\dot v = R_{wb} a -g v˙=Rwba−g
其中a=[ax,ay,az]a = [a_x, a_y, a_z]a=[ax,ay,az]，为测量加速度，g=[0,0,g0]g = [0, 0, g_0]g=[0,0,g0]为重力加速度。该微分方程的通解形式为
Δv=(Rwba−g)Δt\Delta v = (R_{wb}a-g)\Delta t Δv=(Rwba−g)Δt
对应的基于中值法的速度更新形式为
vk=vk−1+(Rwbkak+Rwbk−1ak−12−g)(tk−tk−1)v_k = v_{k-1} + (\frac{R_{wb_k} a_k + R_{wb_{k-1}}a_{k-1} }{2} - g)(t_k - t_{k-1}) vk=vk−1+(2Rwbkak+Rwbk−1ak−1−g)(tk−tk−1)
位置更新

位置微分方程为
p˙=v\dot p = v p˙=v
通解形式为Δp=vΔt\Delta p = v \Delta tΔp=vΔt，此处的v指的是该时间段内的平均速度，该形式对应的基于中值法的离散形式为
pk=pk−1+vk+vk−12(tk−tk−1)p_k = p_{k-1} + \frac{v_k + v_{k-1}}{2}(t_k - t_{k-1}) pk=pk−1+2vk+vk−1(tk−tk−1)
另外通解还可以写为
Δp=vΔt+12aΔt2\Delta p = v\Delta t + \frac{1}{2} a \Delta t ^2 Δp=vΔt+21aΔt2
需要注意的是，此处的v指的是该时间段起始的时刻的速度，该形式对应基于中值法的离散形式为
pk=pk−1+vk−1(tk−tk−1)+12(Rwbkak+Rwbk−1ak−12−g)(tk−tk−1)2p_ k = p_{k-1} + v_{k-1}(t_k - t_{k-1}) + \frac{1}{2} (\frac{R_{wb_k}a_k + R_{wb_{k-1}}a_{k-1} }{2} - g)(t_k - t_{k-1})^2 pk=pk−1+vk−1(tk−tk−1)+21(2Rwbkak+Rwbk−1ak−1−g)(tk−tk−1)2

惯性导航误差分析

误差方程推导方法：

误差方程是状态量（速度误差、位置误差、姿态误差、bias误差等）误差形式的表示。误差方程及其推导方法，是后续滤波、图优化等融合方案的基础。误差方程的推导有固定的套路，举例说明具体步骤：

（1）写出不考虑误差时的微分方程
z˙=x+y\dot z = x+y z˙=x+y
（2）写出考虑误差时的微分方程
z~˙=x~+y~\dot{\tilde z} = \tilde x + \tilde y z~˙=x~+y~
（3）写出真实值(⋅)~\tilde {(\cdot)}(⋅)~与理想值之间的关系
z~=z+δzx~=x+δxy~=y+δy\tilde z = z + \delta z \\ \tilde x = x + \delta x \\ \tilde y = y + \delta y z~=z+δzx~=x+δxy~=y+δy
（4）把（3）中的关系带入（2）
z˙+δz˙=x+δx+y+δy\dot z + \delta \dot z = x + \delta x + y + \delta y z˙+δz˙=x+δx+y+δy
（5）把（1）中的关系带入（4）
x+y+δz˙=x+δx+y+δyx+y+\delta \dot z = x + \delta x + y + \delta y x+y+δz˙=x+δx+y+δy
（6）化简方程
δz˙=δx+δy\delta \dot z = \delta x + \delta y δz˙=δx+δy
姿态误差方程

（1）写出不考虑误差的微分方程
q˙t=qt⊗12[0ω~t−bw~t]\dot q_{t} = q_{t} \otimes \frac{1}{2} \begin{bmatrix}0 \\ \tilde \omega_{t} - b_{\tilde w_t} \end{bmatrix} q˙t=qt⊗21[0ω~t−bw~t]
（2）写出考虑误差的微分方程
q~t˙=q~t⊗12[0ω~t−b~wt−nw]\dot {\tilde q_{t}} = \tilde q_{t} \otimes \frac{1}{2} \begin{bmatrix}0 \\ \tilde \omega_{t} - \tilde b_{w_t} - n_w \end{bmatrix} q~t˙=q~t⊗21[0ω~t−b~wt−nw]
（3）写出带误差的值与理想值之间的关系
q~t=qt⊗δqω~t=ωt+nwb~wt=bwt+δbwt\tilde q_t = q_t \otimes \delta q \\ \tilde \omega_t = \omega_t + n_w \\ \tilde b_{w_t} = b_{w_t} + \delta b_{w_t} q~t=qt⊗δqω~t=ωt+nwb~wt=bwt+δbwt
其中nwn_wnw为陀螺仪白噪声，
δq=[cos(∣δθ2∣)δθ∣δθ∣sin(∣δθ∣2)]≈[1δθ2]\delta q = \begin{bmatrix} cos(|\frac{\delta \theta}{2}|) \\ \frac{\delta \theta}{|\delta \theta|} sin(\frac{|\delta \theta|}{2}) \end{bmatrix} \approx \begin{bmatrix}1 \\ \frac{\delta \theta}{2}\end{bmatrix} δq=[cos(∣2δθ∣)∣δθ∣δθsin(2∣δθ∣)]≈[12δθ]
$\delta \theta $是姿态误差对应的旋转矢量（失准角）。

（4）将（3）中的关系带入（2）
(qt⊗δq)=12qt⨂δθ⊗[0ω^t−nω−bωt−δbωt](q_t \otimes \delta q ) = \frac{1}{2} q_t\bigotimes \delta \theta \otimes \begin{bmatrix} 0 \\ \hat \omega_t - n_\omega - b_{\omega_t} -\delta b_{\omega_t}\end{bmatrix} (qt⊗δq)=21qt⨂δθ⊗[0ω^t−nω−bωt−δbωt]
（5）将（1）中的关系带入（4）并化简得
δθ˙=−[ω^t−bωt]×δθ−nω−δbωt\delta \dot \theta = -[\hat \omega_t - b_{\omega_t}]_\times \delta \theta - n _\omega -\delta b_{\omega t} δθ˙=−[ω^t−bωt]×δθ−nω−δbωt
速度误差方程

（1）不考虑误差时的微分方程
v˙t=Rt(a~t−bat)\dot v_t = R_t(\tilde a_t - b_{a_t}) v˙t=Rt(a~t−bat)
（2）考虑误差时的微分方程
v~t˙=R~t(a~t−na−b~at)\dot{\tilde v_t} = \tilde R_t(\tilde a_t - n_a - \tilde b_{a_t}) v~t˙=R~t(a~t−na−b~at)
（3）真实值与理想值之间的关系
v~=v+δvR~t=Rtexp([δθ]×)≈Rt(I+[δθ]×)a~t=at+nab~at=bat+δbat\tilde v = v+\delta v \\ \tilde R_t = R_t exp([\delta \theta]_\times) \approx R_t(I+[\delta \theta]_\times)\\ \tilde a_t = a_t + n_a \\ \tilde b_{a_t} = b_{a_t} + \delta b_{a_t} v~=v+δvR~t=Rtexp([δθ]×)≈Rt(I+[δθ]×)a~t=at+nab~at=bat+δbat
其中nan_ana为加速度计白噪声。

（4）将（3）的关系带入（2）
v˙+δv˙=Rt(I+[δθ]×)(a~t−na−bat−δbat)\dot v + \delta \dot v = R_t(I+[\delta \theta]_\times)(\tilde a_t - n_a-b_{a_t}-\delta b_{a_t}) v˙+δv˙=Rt(I+[δθ]×)(a~t−na−bat−δbat)
（5）将（1）的关系带入（4）
Rt(at−bat)+δv˙=Rt(I+[δθ]×)(a^t−na−bat−δbat)R_t(a_t-b_{a_t}) + \delta \dot v = R_t(I+[\delta \theta]_\times)(\hat a_t - n_a-b_{a_t}-\delta b_{a_t}) Rt(at−bat)+δv˙=Rt(I+[δθ]×)(a^t−na−bat−δbat)
（6）化简方程，忽略二阶小量
δv˙=−Rt[a^t−bat]×δθ−Rt(na−δbat)\delta \dot v = -R_t[\hat a_t - b_{a_t}]_\times \delta \theta - R_t(n_a - \delta b_{a_t}) δv˙=−Rt[a^t−bat]×δθ−Rt(na−δbat)
位置误差方程

（1）不考虑误差时的微分方程
p˙=v\dot p = v p˙=v
（2）考虑误差时的微分方程
p~˙=v~\dot{\tilde p } = \tilde v p~˙=v~
（3）真实值与理想值之间的关系
v~=v+δvp~=p+δp\tilde v = v + \delta v\\ \tilde p = p + \delta p v~=v+δvp~=p+δp
（4）将（3）的关系带入（2）
p˙+δp˙=v+δv\dot p + \delta \dot p = v+\delta v p˙+δp˙=v+δv
（5）将（1）的关系带入（4）
v+δp˙=v+δvv+\delta \dot p = v + \delta v v+δp˙=v+δv
（6）化简方程
δp˙=δv\delta \dot p = \delta v δp˙=δv
bias误差方程

在IMU精度较高时，bias可以认为是常值，即
δb˙at=0δb˙ωt=0\delta \dot b_{a_t} = 0 \\ \delta \dot b_{\omega_t} = 0 δb˙at=0δb˙ωt=0
但自动驾驶和机器人领域所用的MEMS多数达不到这种精度，因为角速度随机游走和加速度的随机游走交到，因此误差方程常写为
δb˙at=nbaδb˙ωt=nbω\delta \dot b_{a_t} = n_{b_a}\\ \delta \dot b_{\omega_t} = n_{b\omega} δb˙at=nbaδb˙ωt=nbω
其中nban_{b_a}nba和nbωn_{b_\omega}nbω分别为加速度计和陀螺仪的随机游走对应的白噪声。
惯性导航误差分析总结
δp˙=δvδv˙=−Rt[a^t−bat]×δθ−Rt(na+δbat)δθ˙=−[ω^t−bωt]×δθ−nω−δbωtδb˙at=nbaδb˙ωt=nbω[δp˙δv˙δθ˙δb˙atδb˙wt]5×3=[0I00000−Rt[a^t−bat]×−Rt000−[ω^t−bwt]×0−I0000000000][δpδvδθδbatδbwt]+[0000−Rt0000−I0000I0000I][nanwnbanbw]4×3\delta \dot p = \delta v \\ \delta \dot v = -R_t[\hat a_t - b_{a_t}]_\times \delta \theta - R_t(n_a + \delta b_{a_t})\\\delta \dot \theta = -[\hat \omega_t - b_{\omega_t}]_\times \delta \theta - n _\omega -\delta b_{\omega t} \\\delta \dot b_{a_t} = n_{b_a}\\\delta \dot b_{\omega_t} = n_{b\omega}\\\begin{bmatrix} \delta \dot p \\ \delta \dot v \\ \delta \dot \theta \\ \delta \dot b_{a_t} \\ \delta \dot b_{w_t} \end{bmatrix}_{5\times 3} = \begin{bmatrix} 0 & I & 0& 0 & 0 \\ 0 & 0 & -R_t[ \hat a_t - b_{a_t}]_\times & -R_t & 0 \\ 0 & 0 & -[ \hat \omega_t - b_{w_t}]_\times & 0 & -I \\ 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} \delta p\\ \delta v\\ \delta \theta \\ \delta b_{a_t} \\ \delta b_{w_t} \end{bmatrix} + \begin{bmatrix} 0 & 0& 0 & 0 \\ -R_t& 0 & 0 & 0 \\ 0 & -I & 0 & 0 \\ 0 & 0 & I & 0 \\0 & 0 & 0 & I\end{bmatrix} \begin{bmatrix} n_a \\ n_w \\ n_{b_a} \\ n_{b_w} \end{bmatrix}_{4\times 3} δp˙=δvδv˙=−Rt[a^t−bat]×δθ−Rt(na+δbat)δθ˙=−[ω^t−bωt]×δθ−nω−δbωtδb˙at=nbaδb˙ωt=nbωδp˙δv˙δθ˙δb˙atδb˙wt5×3=00000I00000−Rt[a^t−bat]×−[ω^t−bwt]×000−Rt00000−I00δpδvδθδbatδbwt+0−Rt00000−I00000I00000Inanwnbanbw4×3