068-地固系下的粗对准方案

首先设定如下坐标系：地心地固坐标系(e系)、载体坐标系(b系)、初始时刻的与e系重合并凝滞的坐标系e0系以及初始时刻与b系重合并凝滞的坐标系b0系，则b系到e系的方向余弦阵可做如下链式分解：

$$C_b^e=C_{e0}^e\ast C_{b0}^{e0}\ast C_b^{b0}$$

因为：

$${\dot{C}}_e^{e0}=C_e^{e0}({\omega }_{ie}^e\times )=C_e^{e0}{\Omega }_{ie}^e$$

可得：

$$C_e^{e0}=I+\frac{sin({\omega }_{ie}t)}{{\omega }_{ie}t}{\Omega }_{ie}^{e}t+\frac{1-cos({\omega }_{ie}t)}{({\omega }_{ie}t{)}^2}({\Omega }_{ie}^{e}t{)}^2$$

其中：${\omega }_{ie}=|{\omega }_{ie}^e|$，t为从开始到现在的时间长度。
因为：

$${\dot{C}}_b^{b0}=C_b^{b0}({\omega }_{b0b}^b\times )=C_b^{b0}({\omega }_{ib}^b\times )=C_b^{b0}{\Omega }_{ib}^b$$

可得：

$$C_b^{b0}=I+\frac{sin({\omega }_{ib}t)}{{\omega }_{ib}t}{\Omega }_{ib}^{b}t+\frac{1-cos({\omega }_{ib}t)}{({\omega }_{ib}t{)}^2}({\Omega }_{ib}^{b}t{)}^2$$

其中：${\omega }_{ib}=|{\omega }_{ib}^b|$，t为从开始到现在的时间长度。但是应用中：

$$C_b^{b0}(t)=C_b^{b0}(t-1)[I+\frac{sin({\omega }_{ib}\Delta t)}{{\omega }_{ib}\Delta t}{\Omega }_{ib}^b\Delta t+\frac{1-cos({\omega }_{ib}\Delta t)}{({\omega }_{ib}\Delta t{)}^2}({\Omega }_{ib}^b\Delta t{)}^2]$$

此时等效旋转矢量也为$\Delta t$时间内的。不需限制${\omega }_{ib}^b$大小，具有很强的抗角运动的能力；

下面计算$C_{b0}^{e0}$
将e系中的重力矢量投影到e0l系：

$$g^{e0}=C_e^{e0}{g}^e$$

将b系中的比力投影到b0系：

$$f_{sf}^{b0}=C_b^{b0}{f}_{sf}^b$$

容易得到：

$$C_{b0}^{e0}{f}_{sf}^{b0}=-g^{e0}$$

即：

$$C_{b0}^{e0}{C}_b^{b0}{f}_{sf}^b=-C_e^{e0}{g}^e$$

实际上加计测量的比力总是含有误差，对准过程中多多少少会有载体线加速度误差，所以上式应为：

$$C_{b0}^{e0}(C_b^{b0}{f}_{sf}^b-{\nabla }^{b0})=-C_e^{e0}{g}^e$$

为了消除上述误差的影响，考虑对其进行积分，于是一次积分：

$$C_{b0}^{e0}{F}^{b0}=G^{e0}\text{\hspace{1em}(1)}$$

其中：$F^{b0}={\int }_0^{\Delta t}(C_b^{b0}{f}_{sf}^b-{\nabla }^{b0})dt$，$G^{e0}=-{\int }_0^{\Delta t}{C}_e^{e0}{g}^{e}dt$
从开始到现在经过的时间为t，选取0.5t为另一矢量：$F_{0.5t}^{b0},G_{0.5t}^{e0}$，得到基于速度的双矢量定姿：

$$C_{b0}^{e0}\left[\begin{array}{ccc}\frac{F^{b0}}{|F^{b0}|}& \frac{F^{b0}\times F_{0.5t}^{b0}}{|F^{b0}\times F_{0.5t}^{b0}|}& \frac{F^{b0}\times F_{0.5t}^{b0}\times F^{b0}}{|F^{b0}\times F_{0.5t}^{b0}\times F^{b0}|}\end{array}\right]=\left[\begin{array}{ccc}\frac{G^{e0}}{|G^{e0}|}& \frac{G^{e0}\times G_{0.5t}^{e0}}{|G^{e0}\times G_{0.5t}^{e0}|}& \frac{G^{e0}\times G_{0.5t}^{e0}\times G^{e0}}{|G^{e0}\times G_{0.5t}^{e0}\times G^{e0}|}\end{array}\right]$$

得到：

$$C_{b0}^{e0}=\left[\begin{array}{ccc}\frac{G^{e0}}{|G^{e0}|}& \frac{G^{e0}\times G_{0.5t}^{e0}}{|G^{e0}\times G_{0.5t}^{e0}|}& \frac{G^{e0}\times G_{0.5t}^{e0}\times G^{e0}}{|G^{e0}\times G_{0.5t}^{e0}\times G^{e0}|}\end{array}\right]{\left[\begin{array}{ccc}\frac{F^{b0}}{|F^{b0}|}& \frac{F^{b0}\times F_{0.5t}^{b0}}{|F^{b0}\times F_{0.5t}^{b0}|}& \frac{F^{b0}\times F_{0.5t}^{b0}\times F^{b0}}{|F^{b0}\times F_{0.5t}^{b0}\times F^{b0}|}\end{array}\right]}^{-1}$$

另外，还可以对公式(1)再进行一次积分，得到基于位置的双矢量定姿，在此不再过多介绍。可以看出，两次积分使得误差项${\nabla }^{b0}$几乎可以忽略，即增强了抗线晃动的能力。

---

测试

另将对准期间的IMU数据作平均处理后,选取e系的重力与地球自转角速度作为双矢量，选取b系的比力与角速度作为双矢量，进行对准。载体处于准静止状态，即没有进行晃动模拟，对准时长为20分钟，对准结束后继续静止250s，进行惯导解算。本方案的对准结果如下：

基于惯性系的对准方案解算结果如下：

  对比两幅图可以看出，基于惯性系的对准方案并没有取得很好的对准效果，甚至在X方向的漂移误差很大，所以对于准静态的载体而言，基于惯性系的对准方案并不合适，另外该方案计算量较大，降低了可用性。但是对于晃动的载体而言，基于惯性系的这种抗晃动的对准方案应该会具有很大的优势，但是没有实践过便没有发言权。
  另外，这两种对准方法只是粗对准的方案，对于精度要求高的惯导，还是要进行精对准。看过一些文献，基于速度量测的卡尔曼滤波算法会有比较高的精度，后面会进行尝试。

---

参考：
[1]严恭敏,翁浚.捷联惯导算法与组合导航原理讲义.
[2]严恭敏,白亮,翁浚,秦永元.基于频域分离算子的SINS抗晃动干扰初始对准算法[J].宇航学报,2011,32(07):1486-1490.