惯性导航原理（3）：姿态微分方程及初始对准

QAQ 本章的知识点相对于前两章比较少，而且也是正式进入惯导解算了

姿态微分方程

1、欧拉角微分方程

  若导航坐标系为东北天坐标系，IMU的载体坐标系为右前上，欧拉角的旋转顺序为：Z->X->Y (偏航(yaw)、俯仰(pitch)，滚转(roll))。

  表示$C_{\psi }$绕Z轴的旋转矩阵，$C_{\theta }$表示绕X轴的旋转矩阵，$C_{\varphi }$表示绕Y轴的旋转矩阵。$C_n^b$为导航系到载体坐标系的转换矩阵, $X_b$表示载体坐标系下的坐标，$X_n$表示导航坐标系下的坐标，则有以下公式：$X_n=C_n^b\ast X_b$
  第一步：
  假设已经完成绕三个轴的旋转，则最后绕Y轴的角速度在载体坐标系和导航坐标系都是一样的，所以有如下公式：
             $w_b(roll)=[d_{(pitch)}/dt,0,0{]}^T$
  第二步：
  假设已经完成了前两组旋转，最后一组旋转没有完成，如果接着完成最后一步旋转，则和第一步一样，绕X轴的角速度在两个坐标系下是一样的，则有如下公式
            $w_b(pitch)=C_{\varphi }\ast [0,d_{(roll)}/dt,0{]}^T$
  第三步：
  分析和第二步类似。假设已经完成了第一组旋转，最后两组旋转没有完成，如果接着完成最后两组旋转，则绕Z轴的角速度在两个坐标系下是一样的，则有如下公式：
           $w_b(pitch)=C_{\varphi }\ast C_{\theta }\ast [0,0,d_{(yaw)}/dt{]}^T$
  最后有：
  $[w_{nbx}^b,w_{nby}^b,w_{nbz}^b{]}^T=[d_{(pitch)}/dt,0,0{]}^T+C_{\varphi }\ast [0,d_{(roll)}/dt,0{]}^T+C_{\varphi }\ast C_{\theta }\ast [0,0,d_{(yaw)}/dt{]}^T$
  整理后可得欧拉角微分方程。
  当导航坐标系是北东地，载体坐标系是前右下时，欧拉角微分方程为：

 注意 当俯仰角=90度时，是无解的，所以欧拉角法不能用在全姿态飞行器中。

2、方向余弦阵微分方程

  $[{\Omega }_{nb}^b]$是反对陈矩阵，有：$[{\Omega }_{nb}^b{]}^T$=$-[{\Omega }_{nb}^b]$

3、四元数微分方程

  假设n系到b系的四元数为q，则：


  这里的具体推导可参考这篇文章

  姿态微分方程的求解有点复杂，公式太多啦，，，，这里就不求解了，只把微分方程的形式列出来了。

粗对准

  对准按照不同的参照，可分为不同类别，如：动基座对准&静基座对准、自对准&传递对准、粗对准&精对准等等。
  在运动基座上进行对准，需要外界提供参考信息，比如速度、加速度、姿态、位置等等，而精对准有多数要涉及卡尔曼滤波，因此这里只介绍下静基座状态下的粗对准，精对准留到后面章节再更新。
  下面两种方法粗对准必须满足陀螺能够敏感地球自转！！！！！！！！

1、北东地坐标系下的解析粗对准

2、北东地坐标系下的罗经粗对准

  首先是水平调平：





  接下来是求解航向：




  其实还有双矢量定姿法、多矢量定姿法 等等，但是因为粗对准后面一般接有精对准，最终姿态会收敛，所以这里就不介绍了（实际上是因为我懒）。