第三章-(2)-惯性导航解算原理

Catalogue
  1. 1. 惯导解算
  2. 2. 基础部分
    1. 2.1. 旋转矩阵微分
      1. 2.1.1. 其他求旋转矩阵微分的方法
    2. 2.2. 四元数及其微分
      1. 2.2.1. 四元数微分-方法一
      2. 2.2.2. 四元数微分-方法二(推荐)
    3. 2.3. 等效旋转矢量
    4. 2.4. 旋转矩阵、四元数、等效旋转矢量之间的关系
  3. 3. 姿态更新
    1. 3.1. 基于旋转矩阵的更新
    2. 3.2. 基于四元数的更新
      1. 3.2.1. 推导方法1
      2. 3.2.2. 推导方法2
  4. 4. 等效旋转矢量的计算(关键)
  5. 5. 解算部分
  6. 6. 误差分析
    1. 6.1. 基本方法——误差分析的思路
    2. 6.2. 姿态误差分析
    3. 6.3. 速度误差分析
    4. 6.4. 位置误差分析
    5. 6.5. 总结

惯导解算

基础部分

旋转矩阵微分

关于\(\dot{r}^{b}=-\omega_{ib}^{b} \times r^b\)的推导

其他求旋转矩阵微分的方法

考虑任意旋转矩阵\(R^TR=I\)

对于时变情况,有\(R^T(t) R(t)=I\)

对上式两边同时求导:

\[ \begin{aligned} \dot{R}^{T}R+R^T\dot{R}=0 \end{aligned} \]

整理,有:

\[ \begin{aligned} \dot{R}^{T}R&=-R^T\dot{R} \\ &=-(\dot{R}^{T}R)^T \end{aligned} \]

可以发现,\(\dot{R}^{T}R\)是一个反对称矩阵,因此可以使用一个三维向量的反对称矩阵来表示(实际上就是角速度)

\[ \dot{R}^{T}(t)R(t)=\phi(t)^\wedge \]

继续整理,有:

\[ \begin{aligned} \dot{R}^{T}(t)R(t)=\phi(t)^\wedge \\ \rightarrow \dot{R}^{T}(t)=\phi(t)^\wedge R(t)^T \\ \rightarrow \dot{R}(t)=(\phi(t)^\wedge R(t)^T)^T \\ \rightarrow \dot{R}(t)=R(t)\phi(t)^\wedge \end{aligned} \]

四元数及其微分

四元数微分-方法一

四元数微分-方法二(推荐)

具体可参见四元数微分

等效旋转矢量

旋转矩阵、四元数、等效旋转矢量之间的关系

姿态更新

基于旋转矩阵的更新

思想:把小周期内的角度增量看作是等效旋转矢量,然后进行姿态更新

基于四元数的更新

推导方法1

补充: \[ \begin{aligned} M_{w_{ib}^b}'= q_0 I + (\phi*)_2 &= \begin{bmatrix} q_0 & -\phi_x & -\phi_y & -\phi_z \\ \phi_x & q_0 & \phi_z & -\phi_y \\ \phi_y & -\phi_z & q_0 & \phi_x \\ \phi_z & \phi_y & -\phi_x & q_0 \end{bmatrix} \\ &= \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} \end{aligned} \]

常系数微分方程求解

推导方法2

等效旋转矢量的计算(关键)

这里探讨的是 等效旋转矢量与角速度(角增量)之间的关系,实际上,等效旋转矢量\(\phi\)一般不等于角增量\(\Delta \theta\),当且近当载体做定轴旋转时(即等效旋转矢量与角速度矢量方向平行时),两者才相等。

解算部分

上图中圈起来的部分:

  • \(C_{b(m)}^{n(m)}\) 表示\(t_m\)时刻下,载体相对于导航坐标系的姿态
  • \(C_i^{n(m-1)}C_{b(m-1)}^i = C_{b(m-1)}^{n(m-1)}\) 表示\(t_{m-1}\)时刻下,载体相对于导航坐标系的姿态
  • \(C_{b(m)}^{b(m-1)}=\exp([\phi_{ib(m)}^b]\times)\) 表示以i系作为参考基准,b系从\(t_{m-1}\)\(t_m\)时刻的旋转变化,此量可由载体陀螺仪角速度\(\omega_{ib}^b\)确定
  • \(\phi_{ib(m)}^b=(\Delta \theta_1+\Delta \theta_2)+\frac{2}{3}\Delta \theta_1 \times \Delta \theta_2\) 这是误差补偿项,用以补偿 等效旋转矢量与陀螺仪得到的角度增量之间的误差(不可交换误差),采用二子样圆锥误差补偿算法。(单子样+前一周期的补偿算法见《捷联惯导算法与组合导航原理》-P30)
  • \(C_{n(m-1)}^{n(m)}=(C_{n(m)}^{n(m-1)})^T=[\exp(\phi_{in(m)}^n)]^T=[\exp(T_{[m-1,m]}\omega_{in(m)}^n)]^T\) 通常在导航更新周期\([t_{m-1},t_m]\)内,可以认为速度和位置引起的\(w_{in}^n\)变化很小,可作为常数,直接乘以周期即可。


误差分析

基本方法——误差分析的思路

误差方程的形式:

假设给定微分方程:

\[ \begin{aligned} \dot{z}=x+y \end{aligned} \tag{1} \]

且:

\[ \begin{aligned} \tilde{z}&=z+\delta z \\ \tilde{x}&=x+\delta x \\ \tilde{y}&=y+\delta y \end{aligned} \tag{2} \]

则误差方程的形式为:

\[ \begin{aligned} \delta \dot{z}= ??? \end{aligned} \]

  1. 写出考虑误差时的微分方程

    即把\(\dot{z}=x+y\)使用带有误差的变量代替,得到:

\[ \begin{aligned} \tilde{\dot{z}}=\tilde{x}+\tilde{y} \end{aligned} \tag{3} \]

  1. 把(2)的代入到等式(3)

\[ \begin{aligned} \dot{z}+\delta{\dot{z}}=x+\delta x+ y+ \delta y \end{aligned} \tag{4} \]

  1. 取原微分方程(1),代入(4)

\[ \begin{aligned} \because \dot{z}&=x+y \\ \therefore x+y + \delta \dot{z}&=x + y+ \delta x+\delta y \\ \therefore \delta \dot{z}&=\delta x+ \delta y \end{aligned} \tag{5} \]

姿态误差分析

速度误差分析

位置误差分析

总结