>
返回

基于 IMU 的惯性导航解算及误差分析

主要整理了基于 IMU 测量数据进行位姿、速度解算的方法。

连续时间下的 IMU 运动模型

位置、速度、姿态更新

这里忽略尺度因子和轴偏差的影响,对 IMU 的运动模型定义如下:

$$
\begin{aligned}
    \tilde{\boldsymbol{\omega}}^b &= \boldsymbol{\omega}^b + \boldsymbol{b}^g + \boldsymbol{n}^g \\
    \tilde{\boldsymbol{a}}^b &= \boldsymbol{a}^b + \boldsymbol{b}^a + \boldsymbol{n}^a
\end{aligned}
$$

上式中,$\boldsymbol{b}$ 为零偏,$\boldsymbol{n}$ 为测量值中的高斯白噪声。上标 $b$ 表示 IMU 测量对象的 body 系,$a, g$ 分别对应加速度计和陀螺仪的误差值。

这里我们主要关注,在连续时间下,IMU 位置(Pose) 、速度(Velocity) 以及姿态(Attitude) 随时间的变化。要求一段时间内这些参数的变化量,思路是通过对他们的导数进行积分,位置和速度的导数很简单,分别是:

$$
\begin{aligned}
    \dot{\boldsymbol{p}}_{wb} &= \boldsymbol{v}^w_t \\
    \dot{\boldsymbol{v}}^w_t &= \boldsymbol{a}^w_t
\end{aligned}
$$

式中上标 $w$ 表示 $\boldsymbol{v, a}$ 分别表示(Body 坐标系在)参考惯性坐标系(I 系)下的速度和加速度,$\boldsymbol{p}_{wb}$ 表示 Body 坐标系在参考惯性坐标系下的位置。此外我们还需要求姿态的导数,由于姿态有旋转矩阵、四元数、欧拉角以及旋转向量等不同表示方法,需要分开讨论。在讨论求导的时候,我们一般使用四元数或者旋转向量来表示姿态,欧拉角因为有万向锁的问题实际计算中比较少使用,旋转向量实际上就是李代数,会在旋转矩阵(李群)求导时用上。根据我之前的整理,四元数和旋转向量对时间求导分别是:

四元数姿态对时间求导

$$
\dot{\boldsymbol{q}}_{wb_t} = \boldsymbol{q}\otimes\begin{bmatrix}
    0 \\
    \frac{1}{2}\boldsymbol{\omega}^{b_t}
\end{bmatrix}
$$

旋转矩阵姿态对时间求导

$$
\dot{\boldsymbol{R}}_{wb_t} = \boldsymbol{R}_{wb_t}\boldsymbol{\omega}^{b_t\wedge}
$$

上式中,$\boldsymbol{\omega}^{b_t\wedge}$ 表示 IMU 在参考惯性坐标系下的旋转角速度在 Body 系下的表示(其对应的旋转轴是在 Body 系下的),也是 IMU 中陀螺仪直接测量的参数。

下面对四元数和旋转矩阵分别进行连续时间(时间 $i$$j$ )下的运动模型推导:

基于四元数状态更新

姿态更新比较直观,$i~j$ 之间对姿态导数求积分可以求得此旗舰的姿态的变化量,前面还需要乘上原来的姿态;

$$
\begin{aligned}
    \boldsymbol{q}_{wb_j} =\boldsymbol{q}_{wb_t}\otimes\int_{t\in[i,j]}{(\boldsymbol{q}_{wb_t}\otimes\begin{bmatrix}
    0 \\
    \frac{1}{2}\boldsymbol{\omega}^{b_t}
\end{bmatrix})}\delta t \\
    \boldsymbol{q}_{wb_j} =\int_{t\in[i,j]}{(\boldsymbol{q}_{wb_t}\otimes\begin{bmatrix}
    1 \\
    \frac{1}{2}\boldsymbol{\omega}^{b_t}
\end{bmatrix})}\delta t
\end{aligned}
$$

速度更新则是在原有速度的基础上加上由于加速度产生的变化量,由于加速度计测量出的加速度测量值是在 IMU 的 Body 系下表示的(大小不变,但是加速度对应的轴是在 Body 系下),因此我们需要用旋转矩阵对其进行旋转至导航坐标系与重力加速度相减求其真实加速度,如下所示(这里的 $\boldsymbol{a, g}$ 需要齐次化转化为四元数的形式(实部为 0,虚部为对应元素)):

$$
\boldsymbol{v}^w_j = \boldsymbol{v}^w_i + \int_{t\in[i,j]}{(\boldsymbol{q}_{wb_t}\boldsymbol{a}(\boldsymbol{q}_{wb_t}^{b_t})^{-1} - \boldsymbol{g}^w)}\delta t
$$

位置更新则是在原有的位置上加上两部分变化量,一部分是由初始速度在给定时间范围内产生的位置变化,另一部分是由加速度产生的位置变化,需要二重积分,如下所示:

$$
\boldsymbol{p}_{wb_j} = \boldsymbol{p}_{wb_i} + \boldsymbol{v}^w_i\Delta t + \int\int_{t\in[i,j]}{(\boldsymbol{q}_{wb_t}\boldsymbol{a}(\boldsymbol{q}_{wb_t}^{b_t})^{-1} - \boldsymbol{g}^w)}\delta t^2
$$

基于旋转矩阵状态更新

基于旋转矩阵也是类似的,将上述的四元数计算部分换成旋转矩阵的对应计算即可,如下所示:

$$
\begin{aligned}
    \boldsymbol{R}_{wb_j} &= \boldsymbol{R}_{wb_i}\int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}(\boldsymbol{\omega}^{b_t})^\wedge)}\delta t \\
    \boldsymbol{v}^w_j &= \boldsymbol{v}^w_i + \int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}\boldsymbol{a} - \boldsymbol{g}^w)}\delta t\\
    \boldsymbol{p}_{wb_j} &= \boldsymbol{p}_{wb_t} + \boldsymbol{v}^w_i\Delta t + \int\int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}\boldsymbol{a} - \boldsymbol{g}^w)}\delta t^2
\end{aligned}
$$

连续积分具体运算

TODO

IMU 运动模型离散化

上述的积分是在连续时间下进行推导的,但实际使用中我们能获取的只有在离散时间内的采样值,因此有必要对积分离散化。这里有两种思路。

欧拉法离散积分

思路比较简单,从 $k$$k+1$ 的状态变化用第 $k$ 时刻的测量值来进行估算,即在采样时间内我们认为 IMU 在做以 $k$ 时刻测量值为准的定角速度定加速度运动,结果如下所示:

基于四元数的欧拉法离散积分:

$$
\begin{aligned}
    \boldsymbol{a} &= \boldsymbol{q}_{wb_k}\boldsymbol{a}^{b_k}\boldsymbol{q}_{wb_k}^{-1} - \boldsymbol{g}^w \\
    \boldsymbol{\omega} &= \boldsymbol{\omega}^{b_k}\\
    \boldsymbol{q}_{wb_{k+1}} &= \boldsymbol{q}_{wb_k}\otimes\begin{bmatrix}
        1\\
        \frac{1}{2}\boldsymbol{\omega}\Delta t
    \end{bmatrix}\\
    \boldsymbol{v}_{k+1}^w &= \boldsymbol{v}_k^w + \boldsymbol{a}\Delta t\\
    \boldsymbol{p}_{wb_{k+1}} &= \boldsymbol{p}_{wb_{k}} + \boldsymbol{v}_k^w\Delta t + \frac{1}{2}\boldsymbol{a}\Delta t^2
\end{aligned}
$$

其中前两个式子是分别对测量值减去重力角速度,注意再减去重力角速度之前要先将加速度转换至导航坐标系下。

基于旋转矩阵的欧拉法离散积分:

思路大同小异,如下所示:

$$
\begin{aligned}
    \boldsymbol{a} &= \boldsymbol{R}_{wb_k}\boldsymbol{a}^{b_k} - \boldsymbol{g}^w \\
    \boldsymbol{\omega} &= \boldsymbol{\omega}^{b_k}\\
    \boldsymbol{R}_{wb_{k+1}} &= \boldsymbol{R}_{wb_k}\boldsymbol{R}_{b_kb_{k+1}} = \boldsymbol{R}_{wb_k}\exp{((\boldsymbol{\omega}\Delta t)^\wedge)}\\
    \boldsymbol{v}_{k+1}^w &= \boldsymbol{v}_k^w + \boldsymbol{a}\Delta t\\
    \boldsymbol{p}_{wb_{k+1}} &= \boldsymbol{p}_{wb_{k}} + \boldsymbol{v}_k^w\Delta t + \frac{1}{2}\boldsymbol{a}\Delta t^2
\end{aligned}
$$

注意上式中,我们没有考虑误差造成的影响,这一部分我们留到下一步误差分析进行详细推导。此外这里利用了旋转矢量对应的李代数通过指数映射求对应旋转矩阵的过程,具体计算过程可以参考我之前整理的方法。

中值法离散积分

中值法顾名思义则是利用 $k$$k+1$ 的测量值分别作为采样时间内的加速度和角速度进行积分,积分过程和上面一样,加速度和角速度计算如下:

基于四元数:

$$
\begin{aligned}
    \boldsymbol{a} &= \frac{1}{2}[\boldsymbol{q}_{wb_k}\boldsymbol{a}^{b_k} \boldsymbol{q}_{wb_k}^{-1} - \boldsymbol{g}^w + \boldsymbol{q}_{wb_{k+1}}(\boldsymbol{a}^{b_{k+1}} - \boldsymbol{b}_{k}^a)\boldsymbol{q}_{wb_{k+1}}^{-1} - \boldsymbol{g}^w]\\
    \boldsymbol{\omega} &= \frac{1}{2}[\boldsymbol{\omega}^{b_k} + \boldsymbol{\omega}^{b_{k+1}} ]
\end{aligned}
$$

基于旋转矩阵:

$$
\begin{aligned}
    \boldsymbol{a} &= \frac{1}{2}(\boldsymbol{R}_{wb_k}\boldsymbol{a}^{b_k} - \boldsymbol{g}^w + \boldsymbol{R}_{wb_{k+1}}\boldsymbol{a}^{b_{k+1}} - \boldsymbol{g}^w) \\
    \boldsymbol{\omega} &= \frac{1}{2}[\boldsymbol{\omega}^{b_k} + \boldsymbol{\omega}^{b_{k+1}} ]
\end{aligned}
$$

姿态更新中的坐标系讨论

在上述分析中,大部分时候我们都用世界坐标系来作为参考坐标系,但实际过程主要涉及到到三个坐标系:

  • IMU 参考的惯性坐标系,一般以地心惯性系为参考,这个坐标系和 ECEF 系不同不会随着地球自转旋转
  • 导航坐标系:一般以当地的东北天坐标系作为导航坐标系
  • 机体坐标系:这里可以认为是 IMU 的 Body 系

不难发现载体和惯性系之间有一个相对旋转角速度 $\boldsymbol{\omega}_{in}$,由两部分组成:地球的自转引起的导航系和惯性系的旋转$\boldsymbol{\omega}_{ie}$、由于地球球体不是规则的球体,因此载体在运动的时候也会对当地的导航系形成一定的角速度 $\boldsymbol{\omega}_{en}$,如下所示。这一部分参考《捷联惯导算法与组合导航原理讲义》,这里不进行深入探讨。

$$
\begin{aligned}
    \boldsymbol{\omega}_{in} &= \boldsymbol{\omega}_{ie} + \boldsymbol{\omega}_{en}\\
    \boldsymbol{\omega}_{ie} &= \begin{bmatrix}
        0 & \omega_{ie}\cos{L} & \omega_{ie}\cos{L}
    \end{bmatrix}^T\\
    \boldsymbol{\omega}_{en} &= \begin{bmatrix}
        -\frac{v_N}{R_M+h} & \frac{v_E}{R_N + h} & \frac{v_E}{R_N + h}\tan{L}
    \end{bmatrix}^T
\end{aligned}
$$

而 IMU 测量值是对于惯性系的,因此我们如果要得到导航系(东北天)下的加速度和角速度还需要做出旋转补偿,这里以旋转矩阵为例,如下所示:

$$
\begin{aligned}
    \boldsymbol{R}_{nb} &= \boldsymbol{R}_{ni}\boldsymbol{R}_{ib}\\
    &= \boldsymbol{R}_{n_tn_{t-1}}\boldsymbol{R}_{n_{t-1}i}\boldsymbol{R}_{ib_{t-1}}\boldsymbol{R}_{b_tb_{t-1}}
\end{aligned}
$$

这里面由于导航系(N 系)和载体系(B 系)相对于惯性系而言都是动坐标系,因此需要标注时间。

误差分析

上面整理了怎么通过 IMU 的测量数据进行状态的更新,接下来看一下在更新过程误差是怎么变化的。

对于误差分析的过程一般如下所示,以下面例子为例,假设理想状态下:

$$
\begin{aligned}
    \dot{z} &= x + y \\
\end{aligned}
$$

考虑误差为线性相加,则微分方程和各个真实值和理想值之间的关系:

$$
\begin{aligned}
    \dot{\tilde{z}} &= \tilde{x} + \tilde{y}\\
    \tilde{z} &= z + \delta z\\
    \tilde{x} &= x + \delta x\\
    \tilde{y} &= y + \delta y
\end{aligned}
$$

由于误差形式已知,很容易对考虑误差的状态值写出微分方程,并代入各个状态量:

$$
\begin{aligned}
    \dot{z} + \delta\dot{z} = x + \delta x + y + \delta y
\end{aligned}
$$

而理想状态下的 $z$ 求导结果是已知的,可以直接代入并消掉状态量得:

$$
\begin{aligned}
    x + y + \delta\dot{z} &= x + \delta x + y + \delta y\\
    \Rightarrow \delta\dot{z} &= \delta x + \delta y
\end{aligned}
$$

通过最后的式子我们知道怎么不同状态量之间误差的传递方式。接下来按照这个步骤对姿态、速度和位置的误差更新进行分析。

姿态误差分析

第一步,写出理想状态下姿态微分方程:

$$
\dot{\boldsymbol{R}}_{nb} = \boldsymbol{R}_{nb}((\boldsymbol{\omega}_{nb}^{b})^\wedge)
$$

根据上一部分的分析,我们知道陀螺仪直接测量的是相对于惯性系(i 系)的角速度,因此这里要进行一步转换:

$$
\begin{aligned}
    \dot{\boldsymbol{R}}_{nb} &= \boldsymbol{R}_{nb}((\boldsymbol{\omega}_{nb}^{b})^\wedge)\\
    &= \boldsymbol{R}_{nb}((\boldsymbol{\omega}_{ib}^{b} - \boldsymbol{\omega}_{in}^{b})^\wedge)\\
    &=  \boldsymbol{R}_{nb}(\boldsymbol{\omega}_{ib}^{b})^\wedge - \boldsymbol{R}_{nb}(\boldsymbol{\omega}_{in}^{b})^\wedge\\
    &= \boldsymbol{R}_{nb}(\boldsymbol{\omega}_{ib}^{b})^\wedge - (\boldsymbol{\omega}_{in}^{n})^\wedge\boldsymbol{R}_{nb}
\end{aligned}
$$

上式中,$\boldsymbol{\omega}^n_{in}$ 是导航系相对于惯性系的角速度向量在导航系下的表示方法;最后一步利用了之前在推导旋转矩阵微分方程的一个性质,如下所示:

$$
\boldsymbol{R}_{ab}(\boldsymbol{\omega}^b)^\wedge = (\boldsymbol{\omega}^a)^\wedge\boldsymbol{R}_{ab}
$$

这里我们之所以转换的这个形式,是因为上式中最后一步的状态量都是已知的。$\boldsymbol{R}_{nb}$ 是要估计的状态量,$\boldsymbol{\omega}_{ib}^b$ 是陀螺仪的测量量,$\boldsymbol{\omega}_{in}^n$ 可以根据上一部分的公式计算出来。

第二步,写出考虑误差时的微分方程:

$$
\dot{\tilde{\boldsymbol{R}}}_{nb} = \tilde{\boldsymbol{R}}_{nb}(\tilde{\boldsymbol{\omega}}_{ib}^{b})^\wedge - (\tilde{\boldsymbol{\omega}}_{in}^{n})^\wedge\tilde{\boldsymbol{R}}_{nb}
$$

第三步,写出真实值和测量值的误差构成形式,这里一共涉及三个变量,姿态 $\boldsymbol{R}_{nb}$,IMU 角速度 $\boldsymbol{\omega}_{ib}^b$,导航系相对惯性系的角速度 $\boldsymbol{\omega}_{in}^n$,下面一一进行分析。

首先是姿态 $\boldsymbol{R}_{nb}$ 的误差,这里的误差有可能是因为 Body 系或者导航系的误差构成,计算时我们一般假设误差都在导航系上,将这个带有误差的导航系称为 $n'$ 系,因此误差关系为:

$$
\tilde{\boldsymbol{R}}_{nb} = \boldsymbol{R}_{n'b} = \boldsymbol{R}_{n'n}\boldsymbol{R}_{nb}
$$

其中,$\boldsymbol{R}_{n'n}$ 为误差项。这里我们假设从 $n$$n'$ 系的等效旋转向量(失准角)为 $\boldsymbol{\phi}$,可以知道这个旋转矢量和 $\boldsymbol{R}_{n'n}$ 的等效旋转矢量 $\boldsymbol{\phi}'$相反(旋转方向相反),旋转矩阵 $\boldsymbol{R}_{n'n}$ 和失准角的关系可以近似为(这个误差项一般是微小量):

$$
\begin{aligned}
\boldsymbol{R}_{n'n} &= \exp{(\phi'^\wedge)}\\
&\approx \boldsymbol{I} + \boldsymbol{\phi}'^\wedge\\
&= \boldsymbol{I} -\boldsymbol{\phi}^\wedge
\end{aligned}
$$

这里利用了罗格里德斯公式在小量下的近似,可以参考我之前整理的文章。

因此,带误差的矩阵表示为:

$$
\tilde{\boldsymbol{R}}_{nb} = (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_{nb}
$$

接下来看 IMU 相对于惯性系的角速度的误差模型,这里在之前的传感器误差模型中已经大致提到过,测量值的误差模型中大致包括尺度因子,轴偏差以及零偏。实际中一般不会考虑这么多误差参数,会根据需求取一部分变量作为误差估计,这里取尺度因子和部分轴偏差以及零偏如下所示:

$$
\begin{aligned}
    \tilde{\boldsymbol{\omega}}_{ib}^b &= \boldsymbol{\omega}_{ib}^b + \delta\boldsymbol{\omega}_{ib}^b\\
    &= \begin{bmatrix}
        s_x^g & \beta_{yz}^g & \beta_{zy}^g\\
        \beta_{xz}^g & s_y^g & \beta_{xz}^g\\
        \beta_{xy}^g & \beta_{yx} & s_z^g
    \end{bmatrix}\boldsymbol{\omega}_{ib}^b + \begin{bmatrix}
        b_x^g\\
        b_y^g\\
        b_z^g
    \end{bmatrix}\\
    &= \boldsymbol{\omega}_{ib}^b + 
    \begin{bmatrix}
        s_x^g - 1 & \beta_{yz}^g & \beta_{zy}^g\\
        \beta_{xz}^g & s_y^g - 1 & \beta_{xz}^g\\
        \beta_{xy}^g & \beta_{yx} & s_z^g - 1
    \end{bmatrix}\boldsymbol{\omega}_{ib}^b + 
    \begin{bmatrix}
        b_x^g\\
        b_y^g\\
        b_z^g
    \end{bmatrix}\\
    \Rightarrow \delta\boldsymbol{\omega}_{ib}^b &= \begin{bmatrix}
        s_x^g - 1 & \beta_{yz}^g & \beta_{zy}^g\\
        \beta_{xz}^g & s_y^g - 1 & \beta_{xz}^g\\
        \beta_{xy}^g & \beta_{yx} & s_z^g - 1
    \end{bmatrix}\boldsymbol{\omega}_{ib}^b + 
    \begin{bmatrix}
        b_x^g\\
        b_y^g\\
        b_z^g
    \end{bmatrix}
\end{aligned}
$$

最后是导航系相对于惯性系的角速度误差模型,根据上一部分的说明,如下所示:

$$
\begin{aligned}
    \tilde{\boldsymbol{\omega}}_{in}^n &= \tilde{\boldsymbol{\omega}}_{ie} + \tilde{\boldsymbol{\omega}}_{en}\\
    &= \boldsymbol{\omega}_{ie} + \delta\boldsymbol{\omega}_{ie} + \boldsymbol{\omega}_{en} + \delta\boldsymbol{\omega}_{en}
\end{aligned}
$$

实际中,这两项误差和载体运动的速度和在地球的位置有关。在不是很高精度的导航中,这两项误差的量级相对于器件误差太小;并且速度和位置误差会修正使得这两项误差的量级更小,所以一般不考虑这两项误差,因此 $\tilde{\boldsymbol{\omega}}_{in}^n = \boldsymbol{\omega}_{in}^n$

第四步,将误差模型代入含误差的微分方程中:

误差模型如下:

$$
\begin{aligned}
    \tilde{\boldsymbol{R}}_{nb} &= (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_{nb}\\
    \tilde{\boldsymbol{\omega}}_{ib}^b &= \boldsymbol{\omega}_{ib}^b + \delta\boldsymbol{\omega}_{ib}^b\\
    \tilde{\boldsymbol{\omega}}_{in}^n &= \boldsymbol{\omega}_{ie} + \delta\boldsymbol{\omega}_{ie} + \boldsymbol{\omega}_{en} + \delta\boldsymbol{\omega}_{en}
\end{aligned}
$$

在第二步中得到带误差的微分方程为:

$$
\dot{\tilde{\boldsymbol{R}}}_{nb} = \tilde{\boldsymbol{R}}_{nb}(\tilde{\boldsymbol{\omega}}_{ib}^{b})^\wedge - (\tilde{\boldsymbol{\omega}}_{in}^{n})^\wedge\tilde{\boldsymbol{R}}_{nb}
$$

代入误差模型得:

$$
\begin{aligned}
    \mathrm{左边} &= \frac{d((\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_{nb})}{dt} \\
    &= (-\dot{\boldsymbol{\phi}}^\wedge)\boldsymbol{R}_{nb} + (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\dot{\boldsymbol{R}}_{nb}\\
    \mathrm{右边} &= ((\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_{nb})((\boldsymbol{\omega}_{ib}^b + \delta\boldsymbol{\omega}_{ib}^b)^\wedge) - (\boldsymbol{\omega}_{in}^{n})^\wedge(\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_{nb}
\end{aligned}
$$

第五步,将理想情况下的微分方程代入上式中,可得:

$$
\begin{aligned}
    \mathrm{左边} &= (-\dot{\boldsymbol{\phi}}^\wedge)\boldsymbol{R}_{nb} + (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)(\boldsymbol{R}_{nb}(\boldsymbol{\omega}_{ib}^{b})^\wedge - (\boldsymbol{\omega}_{in}^{n})^\wedge\boldsymbol{R}_{nb})\\
\mathrm{右边} &= ((\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_{nb})((\boldsymbol{\omega}_{ib}^b + \delta\boldsymbol{\omega}_{ib}^b)^\wedge) - (\boldsymbol{\omega}_{in}^{n})^\wedge(\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_{nb}
\end{aligned}
$$

最后整理化简,这里不难看出可以通过右乘 $\boldsymbol{R}_{bn}$ 来化简:

$$
\begin{aligned}
    \mathrm{左边} &= (-\dot{\boldsymbol{\phi}}^\wedge) + (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)(\boldsymbol{R}_{nb}(\boldsymbol{\omega}_{ib}^{b})^\wedge\boldsymbol{R}_{bn} - (\boldsymbol{\omega}_{in}^{n})^\wedge)\\
    &= (-\dot{\boldsymbol{\phi}}^\wedge) + (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)((\boldsymbol{\omega}_{ib}^{n})^\wedge - (\boldsymbol{\omega}_{in}^{n})^\wedge)\\
\mathrm{右边} &= (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_{nb}(\boldsymbol{\omega}_{ib}^b + \delta\boldsymbol{\omega}_{ib}^b)^\wedge\boldsymbol{R}_{bn} - (\boldsymbol{\omega}_{in}^{n})^\wedge(\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\\
&= (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)(\boldsymbol{\omega}_{ib}^n + \delta\boldsymbol{\omega}_{ib}^b)^\wedge - (\boldsymbol{\omega}_{in}^{n})^\wedge(\boldsymbol{I} -\boldsymbol{\phi}^\wedge)
\end{aligned}
$$

上式中左右边的进一步化简都利用到了 $\boldsymbol{R}_{ab}\boldsymbol{v}^b\boldsymbol{R}_{ba} = \boldsymbol{v}^a$ 这个性质同样可以从旋转矩阵推导中得到($\boldsymbol{R}_{ab}\boldsymbol{v}^b = \boldsymbol{v}^a\boldsymbol{R}_{ab}$),具体参考我之前的博客。

进一步展开并忽略二阶小量(小量与小量的乘积),可得:

$$
\begin{aligned}
\mathrm{左边} &= (-\dot{\boldsymbol{\phi}}^\wedge) +(\boldsymbol{\omega}_{ib}^{n})^\wedge - (\boldsymbol{\omega}_{in}^{n})^\wedge -\boldsymbol{\phi}^\wedge(\boldsymbol{\omega}_{ib}^{n})^\wedge + \boldsymbol{\phi}^\wedge(\boldsymbol{\omega}_{in}^{n})^\wedge\\
\mathrm{右边} &= (\boldsymbol{\omega}_{ib}^n + \delta\boldsymbol{\omega}_{ib}^b)^\wedge  -\boldsymbol{\phi}^\wedge(\boldsymbol{\omega}_{ib}^n + \delta\boldsymbol{\omega}_{ib}^b)^\wedge - (\boldsymbol{\omega}_{in}^{n})^\wedge  + (\boldsymbol{\omega}_{in}^{n})^\wedge\boldsymbol{\phi}^\wedge \\
&= (\boldsymbol{\omega}_{ib}^n)^\wedge + (\delta\boldsymbol{\omega}_{ib}^b)^\wedge  -\boldsymbol{\phi}^\wedge(\boldsymbol{\omega}_{ib}^n)^\wedge -\boldsymbol{\phi}^\wedge(\delta\boldsymbol{\omega}_{ib}^b)^\wedge - (\boldsymbol{\omega}_{in}^{n})^\wedge  + (\boldsymbol{\omega}_{in}^{n})^\wedge\boldsymbol{\phi}^\wedge\\
&\approx (\boldsymbol{\omega}_{ib}^n)^\wedge + (\delta\boldsymbol{\omega}_{ib}^b)^\wedge  -\boldsymbol{\phi}^\wedge(\boldsymbol{\omega}_{ib}^n)^\wedge - (\boldsymbol{\omega}_{in}^{n})^\wedge  + (\boldsymbol{\omega}_{in}^{n})^\wedge\boldsymbol{\phi}^\wedge\\
\mathrm{消去共同项}\Rightarrow(-\dot{\boldsymbol{\phi}}^\wedge)&= -\boldsymbol{\phi}^\wedge(\boldsymbol{\omega}_{in}^{n})^\wedge + (\delta\boldsymbol{\omega}_{ib}^b)^\wedge + (\boldsymbol{\omega}_{in}^{n})^\wedge\boldsymbol{\phi}^\wedge\\
\dot{\boldsymbol{\phi}}^\wedge&= \boldsymbol{\phi}^\wedge(\boldsymbol{\omega}_{in}^{n})^\wedge - (\boldsymbol{\omega}_{in}^{n})^\wedge\boldsymbol{\phi}^\wedge - (\delta\boldsymbol{\omega}_{ib}^b)^\wedge
\end{aligned}
$$

对于反对称矩阵,有这样一个性质 $\boldsymbol{v}_1^\wedge\boldsymbol{v}_2^\wedge - \boldsymbol{v}_2^\wedge\boldsymbol{v}_1^\wedge = (\boldsymbol{v}_1\times\boldsymbol{v}_2)^\wedge$,上式进一步整理得:

$$
\begin{aligned}
    \dot{\boldsymbol{\phi}}^\wedge&= \boldsymbol{\phi}^\wedge(\boldsymbol{\omega}_{in}^{n})^\wedge - (\boldsymbol{\omega}_{in}^{n})^\wedge\boldsymbol{\phi}^\wedge - (\delta\boldsymbol{\omega}_{ib}^b)^\wedge\\
    &= (\boldsymbol{\phi}\times\boldsymbol{\omega}_{in}^n)^\wedge - (\delta\boldsymbol{\omega}_{ib}^b)^\wedge\\
    &= (\boldsymbol{\phi}\times\boldsymbol{\omega}_{in}^n - \delta\boldsymbol{\omega}_{ib}^b)^\wedge\\
    \dot{\boldsymbol{\phi}}&= \boldsymbol{\phi}\times\boldsymbol{\omega}_{in}^n - \delta\boldsymbol{\omega}_{ib}^b
\end{aligned}
$$

这样我们就获得姿态误差的转移方程,可以从导航系失准角的等效旋转向量 $\boldsymbol{\phi}$、导航系相对惯性系的旋转角速度 $\boldsymbol{\omega}_{in}^n$ 以及陀螺仪测量误差 $\delta\boldsymbol{\omega}_{ib}^b$ 来获得姿态误差。

速度误差分析

同样对速度误差进行分析,第一步写出理想情况微分方程:

$$
\dot{\boldsymbol{v}}^n = \boldsymbol{R}_{nb}\boldsymbol{a}^b - \boldsymbol{g}^n
$$

第二步写出考虑误差的微分方程:

$$
\dot{\tilde{\boldsymbol{v}}}^n = \tilde{\boldsymbol{R}}_{nb}\tilde{\boldsymbol{a}}^b - \tilde{\boldsymbol{g}}^n
$$

第三步写出各个量的误差模型,这里一共涉及四个量:

$$
\begin{aligned}
    \tilde{\boldsymbol{v}}&=\boldsymbol{v}+\delta\boldsymbol{v}\\
    \tilde{\boldsymbol{R}}_{nb} &= (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_{nb}\\
    \tilde{\boldsymbol{a}}^b &= \boldsymbol{a}^b + \delta\boldsymbol{a}^b\\
\tilde{\boldsymbol{g}}^n&= \boldsymbol{g}^n + \delta\boldsymbol{g}^n
\end{aligned}
$$

其中,$\delta\boldsymbol{a}^b$ 是 IMU 对加速度的测量误差,分析方法和角速度测量误差类似,可以参考内参标定部分。对于导航系下重力加速度的误差 $\delta\boldsymbol{g}^n$ 一般忽略不计。

第四步,将误差模型代入含误差的微分方程中

$$
\begin{aligned}
    \dot{\tilde{\boldsymbol{v}}}^n &= \tilde{\boldsymbol{R}}_{nb}\tilde{\boldsymbol{a}}^b - \tilde{\boldsymbol{g}}^n\\
    \frac{d(\boldsymbol{v}+\delta\boldsymbol{v})}{dt} &=  (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_{nb}(\boldsymbol{a}^b + \delta\boldsymbol{a}^b) -\boldsymbol{g}\\
    \dot{\boldsymbol{v}} + \dot{\delta\boldsymbol{v}}&=  (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_{nb}(\boldsymbol{a}^b + \delta\boldsymbol{a}^b) -\boldsymbol{g}
\end{aligned}
$$

第五步,将理想微分方程代入上式得:

$$
\begin{aligned}
    \dot{\boldsymbol{v}} + \dot{\delta\boldsymbol{v}}&=  (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_{nb}(\boldsymbol{a}^b + \delta\boldsymbol{a}^b) -\boldsymbol{g}\\
    \boldsymbol{R}_{nb}\boldsymbol{a}^b - \boldsymbol{g}^n + \dot{\delta\boldsymbol{v}}&=  (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_{nb}(\boldsymbol{a}^b + \delta\boldsymbol{a}^b) -\boldsymbol{g}\\
    \dot{\delta\boldsymbol{v}}&=  (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_{nb}(\boldsymbol{a}^b + \delta\boldsymbol{a}^b) - \boldsymbol{R}_{nb}\boldsymbol{a}^b
\end{aligned}
$$

最后一步,化简并消去二阶小量得:

$$
\begin{aligned}
    \delta\dot{\boldsymbol{v}} &= (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_{nb}(\boldsymbol{a}^b + \delta\boldsymbol{a}^b) - \boldsymbol{R}_{nb}\boldsymbol{a}^b\\
    &= \boldsymbol{R}_{nb}(\boldsymbol{a}^b + \delta\boldsymbol{a}^b) - \boldsymbol{\phi}^\wedge\boldsymbol{R}_{nb}(\boldsymbol{a}^b + \delta\boldsymbol{a}^b) - \boldsymbol{R}_{nb}\\
    &= \boldsymbol{R}_{nb}\delta\boldsymbol{a}^b - \boldsymbol{\phi}^\wedge\boldsymbol{R}_{nb}(\boldsymbol{a}^b + \delta\boldsymbol{a}^b)\\
    &\approx\boldsymbol{R}_{nb}\delta\boldsymbol{a}^b - \boldsymbol{\phi}^\wedge\boldsymbol{R}_{nb}\boldsymbol{a}^b\\
    &= \delta\boldsymbol{a}^n - \boldsymbol{\phi}^\wedge\boldsymbol{a}^n\\
    &= \delta\boldsymbol{a}^n + (\boldsymbol{a}^n)^\wedge\boldsymbol{\phi}
\end{aligned}
$$

位置误差分析

位置的误差分析更加简单,如下所示:

第一步,理想微分方程:

$$
\dot{\boldsymbol{p}} = \boldsymbol{v}
$$

第二步,含误差微分方程:

$$
\dot{\tilde{\boldsymbol{p}}} = \tilde{\boldsymbol{v}}
$$

第三步,误差模型:

$$
\tilde{\boldsymbol{p}} = \boldsymbol{p} + \delta\boldsymbol{p}\\
\tilde{\boldsymbol{v}} = \boldsymbol{v} + \delta\boldsymbol{v}
$$

第四步,误差模型代入含误差微分方程中:

$$
\dot{\boldsymbol{p}} + \delta\dot{\boldsymbol{p}} = \boldsymbol{v} + \delta\boldsymbol{v}
$$

第五步,理想微分方程代入上式:

$$
\boldsymbol{v} + \delta\dot{\boldsymbol{p}} = \boldsymbol{v} + \delta\boldsymbol{v}
$$

第六步,整理化简,消去二阶小量:

$$
\delta\dot{\boldsymbol{p}} = \delta\boldsymbol{v}
$$

误差分析的目的

误差分析主要可以用来推导状态和观测方程,并且可以通过导航任务精度的要求,通过误差转移关系反推出所需传感器的精度来辅助器件选型。(另一种选型的方法是通过仿真来选型)

补充

误差分析的内容很大一部分参考了《捷联惯导算法与组合导航原理讲义》,书里面的旋转矩阵(方向余弦矩阵)用 $\boldsymbol{C}^n_b$ 这种方式来表示,因为我习惯用 $\boldsymbol{R}_{nb}$ 来表示从 $b$ 系坐标转换为 $n$ 系坐标的旋转矩阵,所以都统一用这种表示方法了。

参考资料

Built with Hugo
Theme Stack designed by Jimmy