PSINS惯性系初始对准与代码解读

码字不易，转载请注明出处

0.前言

严恭敏老师的开源代码库psins中，惯性系对准代码alingi0与《捷联惯导与组合导航》一书理论部分并没有严格对应，相应的资料也不太好找，学习中废了挺多脑筋。

本文参考“摇摆基座上基于信息的捷联惯导粗对准研究”(秦永元)，以及“严恭敏博士后研究报告”，详细解读并推导了静基座惯性系对准算法，并与PSINS代码进行对照解读。希望能对惯导初学者有所帮助，同时也欢迎各位大佬雅正。

1.坐标系定义

1. 地心惯性坐标系（$i$系）：
   $o{x}_i$轴在赤道平面内且指向春分点, $o{z}_i$轴指向地球自转方向，三轴构成右手坐标系；

2. 地球坐标系（$e$系）：
   $o{x}_e$轴在赤道平面内且指向中央子午线，$o{z}_e$轴沿地球自转方向，三轴构成右手坐标系，e系与地球固连，e系相对于i系的转动角速率即为地球自转角速率 ；

3. 导航坐标系（$n$系）:
   选取“东-北-天（$E-N-U$）”坐标系为导航坐标系（$n$系）；

4. 载体坐标系（$b$系）:
   定义“右-前-上（$R-F-U$）”坐标系为捷联惯导坐标系（$b$系），或称之为运载体坐标系；

5. 初始时刻惯性坐标系（$i_0$系）:
   在初始对准起始时刻（即当$t=t_0=0$ 时），$o{x}_{i_0}$轴在当地子午面内且平行于赤道平面，$o{z}_{i_0}$轴指向地球自转方向，三轴构成右手坐标系，初始对准开始后$i_0$系三轴方向相对惯性空间保持不动；

6. 初始时刻地球坐标系（$e_0$系）:
   原点为地球中心，$o{x}_{e_0}$轴在赤道平面内且指向初始对准开始时刻的当地子午线，$o{z}_{e_0}$沿地球自转方向，三轴构成右手坐标系，$e_0$系也与地球固连，并且$e_0$系与$i_0$系之间方位关系是前者只绕后者的$o{z}_{i_0}$轴转动了${\omega }_{ie}t$角度；

7. 初始时刻导航坐标系（$n_0$系）：
   把初始对准开始时刻的导航坐标系定义为$n_0$系，它相对地球表面固定不动，即不随捷联惯导在地球表面运动而运动；

8. 初始时刻捷联惯导惯性坐标系（$i_{b}0$系）:
   在$t_0$时刻$i_{b_0}$系重合于b系，初始对准开始后$i_{b0}$ 系不随捷联惯导转动，即在惯性空间中保持指向不变。

2.传统解析式对准算法

静基座下即捷联惯组静止不动时，加速度计的比力输出${\tilde{f}}_{sf}^b$测量的是重力加速度矢量$g$在b系中的投影$-g^b$，陀螺角速度输出${\tilde{b}}_{ib}^b$测量的是地球自转角速度矢量${\omega }_{ie}$在b系中的投影${\omega }_{ie}^b$。当对准地点的地理位置准确已知时，物理量$g$和${\omega }_{ie}$在n系中的投影$g^n$和 ${\omega }_{ie}^n$也是已知的。由姿态矩阵$C_b^n$可建立如下两个转换关系式:
$$\begin{array}{rl}-{\tilde{f}}_{sf}^b=& C_n^b{g}^n\\ {\tilde{\omega }}_{ib}^b=& C_n^b{\omega }_{ie}^n\end{array}$$式中${\left[\begin{array}{ccc}0& 0& -g\end{array}\right]}^T$，g为重力加速度大小；${\omega }_{ie}^n={\left[\begin{array}{ccc}0& {\omega }_{ie}cosL& {\omega }_{ie}sinL\end{array}\right]}^T$，L是当地地理纬度，${\omega }_{ie}$是地球自转角速度。通过公式（1）可求出得初始对准姿态矩阵$C_b^n$¹。

$$C_b^n={\left[\begin{array}{c}(g^n{)}^T\\ (g^n\times {\omega }_{ie}{)}^T\\ (g^n\times {\omega }_{ie}\times g^n{)}^T\end{array}\right]}^{-1}\left[\begin{array}{c}(-{\tilde{f}}_{sf}^b{)}^T\\ (-{\tilde{f}}_{sf}^b\times {\tilde{\omega }}_{ib}^b{)}^T\\ ({\tilde{f}}_{sf}^b\times {\tilde{\omega }}_{ib}^b\times {\tilde{f}}_{sf}^b{)}^T\end{array}\right]\text{\hspace{1em}(1)}$$

3.惯性系初始对准：

秦永元老师在论文中指出，上述对准方法仅适用于微小晃动的对准环境² ，而惯性系对准的基本思想是，从惯性坐标系观察地球表面上某固定点的重力矢量，通过在不同时间对重力矢量的观测，进行双/多矢量定姿。该方法具备极强的抗角运动能力。非常适用于线运动不强但角运动明显的场景。

虽然原理如上所述，但PSINS的代码实现与书中¹ 并不完全一致，现按照PSINS代码的算法编排，推导如下：

3.1.基本原理：

将姿态矩阵作如下分解，分别求出各个变换关系然后组合。
$$C_b^n=C_e^n{C}_i^e{C}_{i_{b_0}}^i{C}_b^{i_{b_0}}\text{\hspace{1em}(2)}$$
下面分步推导涉及的变换矩阵

3.2.$C_e^n$:地球坐标系(e)到导航坐标系(n)的转换矩阵

从e系到n系的转换可分为两个步骤：
1、沿Z轴逆时针旋转$90+\lambda$，使X轴和East轴重合。
2、沿East轴（新X轴）逆时针旋转$90-\phi$ ，使Z轴和UP轴重合。

以上旋转即：
$$\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=R_3[-(\pi /2+\lambda )]R_1[-(\pi /2-\phi )]$$
其中
$$\begin{array}{rl}& R_1[\theta ]=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos(\theta )& sin(\theta )\\ 0& -sin(\theta )& cos(\theta )\end{array}\right];R_2[\theta ]=\left[\begin{array}{ccc}cos(\theta )& 0& sin(\theta )\\ 0& 1& 0\\ -sin(\theta )& 0& cos(\theta )\end{array}\right]\\ & R_3[\theta ]=\left[\begin{array}{ccc}cos(\theta )& sin(\theta )& 0\\ -sin(\theta )& cos(\theta )& 0\\ 0& 0& 1\end{array}\right]\end{array}$$
带入得：
$$R_3[-(\pi /2+\lambda )]R_1[-(\pi /2-\phi )]=\left(\begin{array}{ccc}-sin(\lambda )& cos(\lambda )& 0\\ -cos(\lambda )sin(\phi )& -sin(\lambda )sin(\phi )& cos(\phi )\\ cos(\lambda )cos(\phi )& sin(\lambda )cos(\phi )& sin(\phi )\end{array}\right)\text{\hspace{1em}(3)}$$
通常，e系的x轴是指向n系原点所在子午线与赤道交点的，即$\lambda =0$代入(3)可得：
$$C_e^n=R_3[-(\pi /2)]R_1[-(\pi /2-\phi )]=\left(\begin{array}{ccc}0& 1& 0\\ -sin(\phi )& 0& cos(\phi )\\ cos(\phi )& 0& sin(\phi )\end{array}\right)\text{\hspace{1em}(4)}$$

3.3.$C_i^e$:地球坐标系(e)到惯性系(i)的转换矩阵

e系相对i系是沿z轴的定轴转动,转动角度为${\omega }_{ie}\cdot t$旋转矩阵为:
$$C_i^e=\left[\begin{array}{ccc}cos{\omega }_{ie}\cdot t& sin{\omega }_{ie}\cdot t& 0\\ -sin{\omega }_{ie}\cdot t& cos{\omega }_{ie}\cdot t& 0\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(5)}$$

3.4.$C_b^{i_{b_0}}$:初始捷联惯导惯性系($i_{b_0}$)到捷联惯导坐标系(b)的转换矩阵

初始时刻两个坐标系是重合的，即$C_b^{i_{b_0}}(t=0)=I$，之后利用陀螺输出的角运动信息，通过捷联惯导姿态更新算法(下式)可以很容易实时求得矩阵$C_b^{i_{b_0}}(t)$:
$${\dot{C}}_t^{i_{b_0}}(t)=C_b^{i_{b_0}}(t)[{\omega }_{i_{0}b}^b(t{)}_{\times }]\text{\hspace{1em}(6)}$$
其中$[{\omega }_{i_{0}b}^b(t{)}_{\times }]$表示由陀螺测量输出的角增量构成的反对称矩阵，可通过n子样算法近似计算等效旋转矢量后求解，即ins的姿态更新。

3.5.$C_{i_{b_0}}^i$:惯性系(i)到初始捷联惯导惯性系($i_{b_0}$)的转换矩阵

（此处推导有区别于秦永元论文与严恭敏博士后报告论文，重点在于静基座、微小晃动）
在静止基座上，加速度计输出的比力在坐标系$i_{b_0}$的投影为：
$$C_b^{i_{b_0}}{\tilde{f}}_{sf}^b=C_b^{i_{b_0}}(-g^b+{\nabla }^b)\text{\hspace{1em}(7.1)}$$
其中${\nabla }^b=\delta f_{sf}^b+{\dot{v}}^b$是比力测量误差和线速度干扰。
对(7.1)在$[t_0,t_k]$内积分
$${\tilde{v}}_{b_0}^i={\int }_{t_0}^{t_k}{C}_b^{i_{b_0}}{\tilde{f}}_{sf}^b\mathrm{d}t=-C_i^{i_{b_0}}{\int }_{t_0}^{t_k}{g}^i\mathrm{d}t+{\int }_{t_0}^{t_k}{\nabla }^{i_{b_0}}\mathrm{d}t\text{\hspace{1em}(7.2)}$$
记
$$v_{b_0}^i=-C_i^{i_{b_0}}{\int }_{t_0}^{t_k}{g}^i\mathrm{d}t$$
其中
$$g^i=C_e^i{C}_n^e\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]=\left[\begin{array}{c}-gcosLcos{\omega }_{ie}(t-t_0)\\ -gcosLsin{\omega }_{ie}(t-t_0)\\ -gsinL\end{array}\right]\text{\hspace{1em}(7.3)}$$
积分得
$$v^i(t_k)=\left[\begin{array}{c}\frac{gcosL}{{\omega }_{ie}}sin{\omega }_{ie}{t}_k\\ \frac{gcosL}{{\omega }_{ie}}(1-cos{\omega }_{ie}{t}_k)\\ t_{k}gsinL\end{array}\right]\text{\hspace{1em}(7.4)}$$
再次积分得（此处第二个元素原来多写了个负号，现已修改，感谢@Williams_lwq）：
$$P^i(t_k)=\left[\begin{array}{c}\frac{gcosL}{{\omega }_{ie}^2}(1-cos{\omega }_{i}e{t}_k)\\ \frac{gcosL}{{\omega }_{ie}^2}({\omega }_{ie}{t}_k-sin{\omega }_{ie}{t}_k)\\ \frac{t_k^{2}gsinL}{2}\end{array}\right]\text{\hspace{1em}(7.5)}$$
当线运动极小时：
$$\begin{gathered} {\tilde{v}}_{b_0}^i\approx v_{b_0}^i=C_i^{i_{b_0}}{v}^i \\ {\tilde{P}}_{b_0}^i\approx P^{i_{b_0}}=C_i^{i_{b_0}}{P}^i \end{gathered}$$
${\tilde{v}}_{b_0}^i,{\tilde{P}}_{b_0}^i$为真实的速度和位移量，可以由INS速度、位置更新求解。
采取两个不同时间间隔的数据$t_{k_1},t_{k_2}$构建双矢量定姿方程(参考书上¹P194)：
$${\hat{C}}_i^{i_{b_0}}={\left[\begin{array}{c}[v^i(t_{k_1}){]}^T\\ [v^i(t_{k_2}){]}^T\\ [v^i(t_{k_1}){]}^T\times [v^i(t_{k_2}){]}^T\end{array}\right]}^{-1}\cdot \left[\begin{array}{c}[{\tilde{v}}^{i_{b_0}}(t_{k_1}){]}^T\\ [{\tilde{v}}^{i_{b_0}}(t_{k_2}){]}^T\\ [{\tilde{v}}^{i_{b_0}}(t_{k_1}){]}^T\times [{\tilde{v}}^{i_{b_0}}(t_{k_2}){]}^T\end{array}\right]\text{\hspace{1em}(7.6)}$$
位置矢量的求解与上式原理相同。
最后将解得的(4)(5)(6)(7.6)带入(2)求得$C_b^n$，完成初始对准。
该方法对角运动有很好的抗干扰能力，在线运动不明显的时候，短时间即可达到较高精度对准。

4.代码解析

CAlign0.m

35行—计算等效旋转矢量与补偿后的速度增量
36行—计算初始比力
40行—速度更新，对应(7.6)中的 ${\tilde{v}}^{i_{b_0}}$
41行—位置更新，对应(7.6)中的 ${\tilde{P}}^{i_{b_0}}$
45行—计算惯性系观测速度位移，对应(7.*)中的 $g^i,v^i,P^i$ 。代码解析见下文
46行—初始捷联惯导惯性系到捷联惯导坐标系(t)的转换矩阵，对应(6)
52行—对应(4)乘以(5)
47行—选取$t_{(}{k}_1)=0.5t_{(}{k}_2)$时刻，为参与定姿的矢量变量赋值
56行—对应(7.6)
57行—对应(2)完成求解

i0fvp

21行—对应(7.3)
22行—对应(7.4)
23行—对应(7.5)

码字不易，转载请注明出处

---

1. 《捷联惯导算法与组合导航》（严恭敏著）第七章第一节。 ↩︎ ↩︎ ↩︎

2. “摇摆基座上基于信息的捷联惯导粗对准研究”(秦永元) ↩︎