捷联惯导系统学习7.1(捷联惯导粗对准 )

初始对准：确定导航参数姿态、方位、速度、位置的初始值，方位的初始化是最难的。

单独的惯导难以长时间维持高精度定位导航，需要与其他方式综合。

捷联惯导粗对准

初始对准具体就是确定导航参考坐标系的一个过程，寻找导航坐标系（一般为东北天坐标系）与载体坐标系之间的对应关系。
双矢量定姿
方法一

1. 已知两个不共线的矢量$V_1,V_2$,在坐标r（导航坐标系）系投影为$V_1^r,V_2^r$,b系（载体坐标系）下的投影为$V_1^b,V_2^b$,通过两个投影求解b系与r系之间关系，称为双矢量定姿。
   求解：即两个坐标系之间的转换关系为$C_b^r$

2. 存在：
   $$\begin{gathered} V_1^r=C_b^r{V}_1^b \\ {V}_2^r=C_b^r{V}_2^b \end{gathered}$$

3. 构造：
   $$V_1^r\times V_2^r=(C_b^r{V}_1^b)\times (C_b^r{V}_2^b)=C_b^r(V_1^b\times V_2^b)$$

4. 得到矩阵方程：
   $$\left[\begin{array}{ccc}{V}_1^r& V_2^r& V_1^r\times V_2^r\end{array}\right]=C_b^r\left[\begin{array}{ccc}{V}_1^b& V_2^b& V_1^b\times V_2^b\end{array}\right]$$

5. 得到$C_b^r$
   $$C_b^r=\left[\begin{array}{ccc}{V}_1^r& V_2^r& V_1^r\times V_2^r\end{array}\right]{\left[\begin{array}{ccc}{V}_1^b& V_2^b& V_1^b\times V_2^b\end{array}\right]}^{-1}$$

6. 因为$C_b^r$是单位正交矩阵，$C_b^r=(C_b^r{)}^{-T}$再得到：
   $$C_b^r={\left[\begin{array}{c}(V_1^r{)}^T\\ (V_2^r{)}^T\\ (V_1^r\times V_2^r{)}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{V}_1^r\\ V_2^r\\ V_1^r\times V_2^r\end{array}\right]$$

方法二
因为矢量误差往往包含模值误差和方向误差，方法一求取的模值不能严格满足单位正交化，方法2在此基础上，进行了修改。

1. 已知测量得到了两个矢量${\tilde{V}}_1^b,{\tilde{V}}_2^b$,两个${\tilde{V}}_1^r,{\tilde{V}}_2^r$

2. ${\tilde{V}}_1^b,{\tilde{V}}_2^b$求解三个单位正交矢量,(必定两两正交)$$v_1=\frac{{\tilde{V}}_1^b}{|{\tilde{V}}_1^b|};v_2=\frac{{\tilde{V}}_1^b\times {\tilde{V}}_2^b}{|{\tilde{V}}_1^b\times {\tilde{V}}_2^b|};v_3=\frac{{\tilde{V}}_1^b\times {\tilde{V}}_2^b\times {\tilde{V}}_1^b}{|{\tilde{V}}_1^b\times {\tilde{V}}_2^b\times {\tilde{V}}_1^b|}$$

3. ${\tilde{V}}_1^r,{\tilde{V}}_2^r$构造三个正交单位矢量：
   $$u_1=\frac{{\tilde{V}}_1^r}{|{\tilde{V}}_1^r|};u_2=\frac{{\tilde{V}}_1^r\times {\tilde{V}}_2^r}{|{\tilde{V}}_1^r\times {\tilde{V}}_2^r|};u_3=\frac{{\tilde{V}}_1^b\times {\tilde{V}}_2^r\times {\tilde{V}}_1^r}{|{\tilde{V}}_1^r\times {\tilde{V}}_2^r\times {\tilde{V}}_1^r|}$$

4. 构造单位阵求解$C_b^r$
   $$C_b^r={\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right]}^{-1}\left[\begin{array}{c}{v}_1\\ v_2\\ v_3\end{array}\right]=\left[\begin{array}{ccc}{u}_1& u_2& u_3\end{array}\right]\left[\begin{array}{c}{v}_1\\ v_2\\ v_3\end{array}\right]$$

多矢量定姿
当有$m(m>2)$个不共面的矢量,在$b$系和$r$系对这些矢量进行测量，由于误差存在，可以近似满足如下关系：
$${\tilde{V}}_i^r\approx C_b^r{\tilde{V}}_i^b(i=1,2,...,m)$$

1. 这里求解变换矩阵变为了最优问题，指标函数为：
   $$\begin{gathered} J^{\ast }(C_b^r)=\frac{1}{2}\sum _{i=1}^m{w}_i|{\tilde{V}}_i^r-C_b^r{\tilde{V}}_i^b{|}^2=min \\ \sum _{i=1}^m{w}_i=1,加权平均时w_i=1或w_i=1/m \end{gathered}$$

2. 求$|{\tilde{V}}_i^r-C_b^r{\tilde{V}}_i^b{|}^2$:
   $$\begin{gathered} |{\tilde{V}}_i^r-C_b^r{\tilde{V}}_i^b{|}^2=({\tilde{V}}_i^r-C_b^r{\tilde{V}}_i^b{)}^T({\tilde{V}}_i^r-C_b^r{\tilde{V}}_i^b) \\ =[({\tilde{V}}_i^r{)}^T-({\tilde{V}}_i^b{)}^T(C_b^r{)}^T]({\tilde{V}}_i^r-C_b^r{\tilde{V}}_i^b) \\ =|{\tilde{V}}_i^r{|}^2+|{\tilde{V}}_i^b{|}^2-2({\tilde{V}}_i^r)C_b^r{\tilde{V}}_i^b \end{gathered}$$

3. 带入得到：
   $$J^{\ast }(C_b^r)=\frac{1}{2}\sum _{i=1}^m{w}_i|{\tilde{V}}_i^r-C_b^r{\tilde{V}}_i^b{|}^2=\frac{1}{2}\sum _{i=1}^m{w}_i(|{\tilde{V}}_i^r{|}^2+|{\tilde{V}}_i^b{|}^2)-\sum _{i=1}^m{w}_i({\tilde{V}}_i^r{)}^T{C}_b^r{\tilde{V}}_i^b$$

4. 想要$J^{\ast }(C_b^r)$取最小，即${\sum }_{i=1}^m{w}_i({\tilde{V}}_i^r{)}^T{C}_b^r{\tilde{V}}_i^b$取最大值
   令 J ( C b r ) = { ∑ i = 1 m w i ( V ~ i r ) T C b r V ~ i b } m a x 令J(C^r_b)=\{\sum^m_{i=1}w_i(\tilde V^r_i)^TC^r_b\tilde V^b_i\}_{max} 令J(Cbr​)={i=1∑m​wi​(V~ir​)TCbr​V~ib​}max​

5. 仅以化简：
   J ( C b r ) = { ∑ i = 1 m w i ( V ~ i r ) T C b r V ~ i b } m a x = t r [ [ w 1 ( V ~ 1 r ) T w 2 ( V ~ 2 r ) T . . . w m ( V ~ m r ) T ] C b r ] [ V ~ 1 b V ~ 2 b . . . V ~ m b ] ] = t r ( C b r ∑ i = 1 m w i C ~ i b ( C ~ i b ) T ) = t r ( C b r A T ) J(C^r_b)=\{\sum^m_{i=1}w_i(\tilde V^r_i)^TC^r_b\tilde V^b_i\}_{max}\\ =tr[\left[\begin{matrix}w_1(\tilde V_1^r)^T\\w_2(\tilde V_2^r)^T\\...\\w_m(\tilde V_m^r)^T \end{matrix}\right]C^r_b]\left[\begin{matrix}\tilde V_1^b&\tilde V_2^b&...&\tilde V_m^b \end{matrix}\right]]\\ =tr(C^r_b\sum^m_{i=1}w_i\tilde C_i^b(\tilde C_i^b)^T)\\ =tr(C^r_b A^T) J(Cbr​)={i=1∑m​wi​(V~ir​)TCbr​V~ib​}max​=tr[⎣⎢⎢⎡​w1​(V~1r​)Tw2​(V~2r​)T...wm​(V~mr​)T​⎦⎥⎥⎤​Cbr​][V~1b​​V~2b​​...​V~mb​​]]=tr(Cbr​i=1∑m​wi​C~ib​(C~ib​)T)=tr(Cbr​AT)

6. 得到$A^T={\sum }_{i=1}^m{w}_i{\tilde{C}}_i^b({\tilde{C}}_i^b{)}^T$

7. 对A进行奇异值分解即：$A=UD{V}^T$

8. 得到 ${\hat{C}}_b^r=U{V}^T$

粗对准实现

初始对准时运载体一般实在精致条件下进行的，重力矢量和地球自转角速度矢量$w_{ie}$是已知的，分别如下:
$$g^n=\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right],w_{ie}^n=\left[\begin{array}{c}0\\ w_{ie}cosL\\ w_{ie}sinL\end{array}\right]=\left[\begin{array}{c}0\\ w_N\\ w_U\end{array}\right]$$

1. 地球角速度测量关系和比力方程
   $w_{ib}^b:b系（载体坐标系）与i系（惯性坐标系）之间的旋转，在b系下的表示$
   $w_{ib}^n：n系（惯导坐标系）与i系（惯性坐标系）之间的旋转（自转）在b系下的表示$
   $w_{en}^n:n系（惯导坐标系）与e系（地球坐标系）之间的夹角在n系下的表示$
   $w_{nb}^n:基静坐晃动干扰角速度$
   ${\dot{v}}^n:寂静做晃动角速度$
   $f_{sf}^b:比力加速度测得数据$
   $$\begin{gathered} C_b^n{w}_{ib}^b=w_{ib}^n=w_{ie}^n+w_{en}^n+w_{nb}^n \\ {\dot{v}}^n=C_b^n{f}_{sf}^b-(2w_{ie}^n+w_{en}^n)\times v^n+g^n \end{gathered}$$

2. 静基座状态下$(2w_{ie}^n+w_{en}^n)\times v^n$和$w_{en}^n$可以忽略不计，再加入陀螺仪量测误差$\delta w_{ib}^b$和加速度计测量误差$\delta f_{sf}^b$,得到：
   $$\begin{gathered} C_b^n({\tilde{w}}_{ib}^b-\delta w_{ib}^b)-w_{nb}^n=w_{ie}^n \\ {C}_b^n({\tilde{f}}_{sf}^b-\delta f_{sf}^b)-{\dot{v}}^n=-g^n \end{gathered}$$

3. 可以简写为：
   ${\overline{\xi }}^n=C_b^b{\tilde{w}}_{ib}^b+w_{nb}^n$
   ${\overline{▽}}^n=C_b^n\delta f_{sf}^b+{\dot{v}}^n$
   $$\begin{gathered} C_b^n{\tilde{w}}_{ib}^b-{\overline{\xi }}^n=w_{ie}^n \\ {C}_b^n{\tilde{f}}_{sf}^b-{\overline{▽}}^n=-g^n \end{gathered}$$

4. 当测量误差远小于有用信号时，即$|{\overline{\xi }}^n|<\frac{1}{10}|w_{ie}^n|=\frac{w_{ie}}{10}$且$|{\overline{▽}}^n|<\frac{|-g^n|}{100}=\frac{g}{100}$,上式可以近似估计为：
   $$\begin{gathered} {\tilde{C}}_b^n{\tilde{w}}_{ib}^b=w_{ie}^n \\ {\tilde{C}}_b^n{\tilde{f}}_{sf}^b=-g^n \end{gathered}$$

5. 在一般情况下，线运动干扰相对误差小于角运动，参考双矢量标定，以$(-g^n)$作为主参考矢量，可以得到姿态估计阵为：
   $${\hat{C}}_b^n=\left[\begin{array}{ccc}\frac{(-g^n)}{|(-g^n)|}& \frac{(-g^n)\times w_{ie}^n}{|(-g^n)\times w_{ie}^n|}& \frac{(-g^n)\times w_{ie}^n\times (-g^n)}{|(-g^n)\times w_{ie}^n\times (-g^n)|}\end{array}\right]\times \left[\begin{array}{c}\frac{({\tilde{f}}_{sf}^n{)}^T}{|{\tilde{f}}_{sf}^n|}\\ \frac{{\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b}{|{\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b|}\\ \frac{({\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b\times {\tilde{f}}_{sf}^n)}{|{\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b\times {\tilde{f}}_{sf}^n|}\end{array}\right]$$

6. 将$g^n,w_{ie}^n$带入，求得${\hat{C}}_b^n$
   $${\hat{C}}_b^n=\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& 1\\ 1& 0& 0\end{array}\right]\left[\begin{array}{c}\frac{({\tilde{f}}_{sf}^n{)}^T}{|{\tilde{f}}_{sf}^n|}\\ \frac{{\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b}{|{\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b|}\\ \frac{({\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b\times {\tilde{f}}_{sf}^n)}{|{\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b\times {\tilde{f}}_{sf}^n|}\end{array}\right]=\left[\begin{array}{c}-\frac{({\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b{)}^T}{|{\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b|}\\ \frac{({\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b\times {\tilde{f}}_{sf}^n{)}^T}{|{\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b\times {\tilde{f}}_{sf}^n|}\\ \frac{({\tilde{f}}_{sf}^n{)}^T}{|{\tilde{f}}_{sf}^n|}\end{array}\right]$$

7.实际中：$\angle ({\tilde{w}}_{ib}^b,{\tilde{f}}_{sf}^b)\approx \frac{\pi }{2}-L$
假设角增量为在$[0,T]$时间内$\Delta \theta (T)$速度增量为$\Delta V(T)$，当$[0,T]$时间内低频晃动小于$W_{ie}/10$且速度变化小于$gT/100$时能够得到一定经度的结果
令${\tilde{w}}_{ib}^b={\overline{w}}_{ib}^b=\frac{\Delta \theta (T)}{T},{\tilde{f}}_{sf}^b={\overline{f}}_{sf}^b=\frac{\Delta V(T)}{T}$

粗对准方法的误差分析
$${\hat{C}}_b^n=\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& 1\\ 1& 0& 0\end{array}\right]\left[\begin{array}{c}\frac{({\tilde{f}}_{sf}^n{)}^T}{|{\tilde{f}}_{sf}^n|}\\ \frac{{\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b}{|{\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b|}\\ \frac{({\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b\times {\tilde{f}}_{sf}^n)}{|{\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b\times {\tilde{f}}_{sf}^n|}\end{array}\right]=\left[\begin{array}{c}-\frac{({\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b{)}^T}{|{\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b|}\\ \frac{({\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b\times {\tilde{f}}_{sf}^n{)}^T}{|{\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b\times {\tilde{f}}_{sf}^n|}\\ \frac{({\tilde{f}}_{sf}^n{)}^T}{|{\tilde{f}}_{sf}^n|}\end{array}\right]$$
上述分母可以近似为：
$|{\tilde{f}}_{sf}^n)\times {\tilde{w}}_{ib}^b|\approx |(-g^n)\times w_{ie}^n|=g{w}_{ie}cosL=g{w}_n$
$|{\tilde{f}}_{sf}^n\times {\tilde{w}}_{ib}^b\times {\tilde{f}}_{sf}^n|\approx |(-g^n)\times w_{ie}^n\times (-g^n)|=g^2{w}_N$
$|{\tilde{f}}_{sf}^n|\approx |(-g^n)|=g$
${\hat{C}}_b^n$忽略误差的高阶小量：
$$\begin{gathered} {\hat{C}}_b^n\approx \left[\begin{array}{c}-[(f_{sf}^b+\delta f_{sf}^b)\times (w_{ib}^b+\delta w_{ib}^b)]/(g{w}_N)\\ -[(f_{sf}^b+\delta f_{sf}^b)\times (w_{ib}^b+\delta w_{ib}^b)\times (f_{sf}^b+\delta f_{sf}^b){]}^T/(g^2{w}_N)\\ (f_{sf}^b+\delta f_{sf}^b{)}^T/g\end{array}\right] \\ \approx [I+\left[\begin{array}{ccc}\ast & -\delta w_{ib,E}^n/w_N+\delta f_{sf,E}^{n}tanL/g& \ast \\ \ast & \ast & \ast \\ \delta f_{sf,E}^n/g& \delta f_{sf,N}^n/g& \ast \end{array}\right]]C_b^n \end{gathered}$$
又因为${\hat{C}}_b^n=(I-\varphi \times )C_b^n(\varphi 为失准角)$
求得失准角为：
$$\varphi =\left[\begin{array}{c}-\delta f_{sf,N}^n/g\\ \delta f_{sf,E}^n/g\\ -\delta w_{ib,E}^n/w_N+\delta f_{sf,E}^{n}tanL/g\end{array}\right]\approx \left[\begin{array}{c}-\delta f_{sf,N}^n/g\\ \delta f_{sf,E}^n/g\\ -\delta w_{ib,E}^n/w_N\end{array}\right]$$
可以观察到失准角的主要无影响。

间接粗对准方法

与直接对准方法相比，短时间内抗扰能力弱，长时间（分重量级）间接粗对准方法更加准确

1. 设：
   初始时刻载体惯性系$b_0$,与对准时刻载体惯性系重合
   初始时刻导航惯性系$n_0$，与对准时刻导航惯性系重合
   常值矩阵$C_{b_0}^{n_0}$为$b_0$系与$n_0$系的方位关系

2. 重力矢量，在$n_0$系的投影：
   $g^n={\left[\begin{array}{ccc}0& 0& -g\end{array}\right]}^T$
   $$\begin{gathered} g^{n_0}=C_n^{n_0}{g}^n \\ {\dot{C}}_n^{n_0}=C_n^{n_0}(w_{n_{0}n}^n\times )=C_n^{n_0}{w}_{ie}^n\times \end{gathered}$$

3. 解得$$\begin{gathered} C_n^{n_0}=e^{i{w}_{ie}^n\times }=I+\frac{sin{w}_{ie}t}{w_{ie}t}(t{w}_{ie}^n\times )+\frac{1-cos{w}_{ie}t}{(w_{ie}t{)}^2}(t{w}_{ie}^n\times {)}^2 \\ =\left[\begin{array}{ccc}cos{w}_{ie}t& -sin{w}_{ie}tsinL& sin{w}_{ie}tcosL\\ sin{w}_{ie}tsinL& 1-(1-cos{w}_{ie}t)si{n}^{2}L& (1-cos{w}_{ie}t)sinLcosL\\ -sin{w}_{ie}tcosL& (1-cos{w}_{ie}t)sinLcosL& 1-(1-cos{w}_{ie}t)co{s}^{2}L\end{array}\right] \end{gathered}$$

4. 得到$g^{n_0}=-g\left[\begin{array}{c}sin{w}_{ie}t\\ (1-cos{w}_{ie}t))sinLcosL\\ 1-(1-cos{w}_{ie}t))co{s}^{2}L\end{array}\right]$

5. 加速度计的比例输出在$b_0$系投影为：
   $$\begin{gathered} w_{ib}^b:为陀螺仪测量值 \\ {C}_b^{b_0}(0)=I:初始值为I，其他时刻值使用姿态更新算法（二子样法或者其他） \end{gathered}$$
   $$\begin{gathered} f_{sf}^{b_0}=C_b^{b_0}{f}_{sf}^b \\ {\dot{C}}_b^{b_0}=C_b^{b_0}(w_{b_{0}b}^b\times )=C_b^{b_0}(w_{ib}^b\times ) \end{gathered}$$

6. 得到加速度与加速度计比力测量之间关系：
   ${\overline{▽}}^{b_0}:表示在b_0系的加速度计测量误差及线性干扰$
   $$\begin{gathered} C_{b_0}^{n_0}(C_b^{b_0}{\tilde{f}}_{sf}^b-{\overline{▽}}^{b_0})=-g^{n_0} \\ {\overline{▽}}^{b_0}=C_b^{b_0}\delta f_{sf}^b+{\dot{v}}^{b_0} \end{gathered}$$

7. 在初始对准过程中进行定积分得到：
   $$\{\begin{array}{l}{\tilde{F}}_i^{b_0}={\int }_0^{t_i}{C}_b^{b_0}{\tilde{f}}_{sf}^{b}dt\\ G_i^{n_0}=-{\int }_0^{t_i}{g}^{n0}dt\end{array}$$
   得到$G_i^{n_0}=C_{b0}^{b_0}{\tilde{F}}_i^{b_0}-{\int }_0^{t_i}{C}_{b_0}^{n_0}{\overline{▽}}^{b_0}dt$

8. 使用双重矢量法对准：
   在$t_2$时间内结束对准：$t_2=2t_1$最后得到:
   $$\begin{gathered} {\hat{C}}_{b_0}^{n_0}=\left[\begin{array}{ccc}\frac{G_1^{n_0}}{|G_1^{n_0}|}& \frac{G_1^{n_0}\times G_2^{n_0}}{|G_1^{n_0}\times G_2^{n_0}|}& \frac{G_1^{n_0}\times G_2^{n_0}\times G_1^{n_0}}{|G_1^{n_0}\times }\end{array}\right]\times \\ \left[\begin{array}{c}\frac{({\tilde{F}}_1^{b_0}{)}^T}{|{\tilde{F}}_1^{b_0}|}\\ \frac{({\tilde{F}}_1^{b_0}\times {\tilde{F}}_2^{b_0}{)}^T}{|{\tilde{F}}_1^{b_0}\times {\tilde{F}}_2^{b_0}|}\\ \frac{({\tilde{F}}_1^{b_0}\times {\tilde{F}}_2^{b_0}\times {\tilde{F}}_1^{b_0}{)}^T}{|{\tilde{F}}_1^{b_0}\times {\tilde{F}}_2^{b_0}\times {\tilde{F}}_1^{b_0}T|}\end{array}\right] \end{gathered}$$

9. 使用多重矢量法标定：
   $$A=-{\int }_0^{t_2}{g}^{n_0}({\tilde{f}}_{sf}^{b_0}{)}^{T}dt$$
   再进行求解

10. 得到最后对准时刻矩阵${\hat{C}}_{b_0}^{n_0}$
    $$\begin{gathered} {\dot{C}}_n^{n_0}=C_n^{n_0}(w_{n_{0}n}^n\times )=C_n^{n_0}{w}_{ie}^n\times \\ {\dot{C}}_b^{b_0}=C_b^{b_0}(w_{b_{0}b}^b\times )=C_b^{b_0}(w_{ib}^b\times ) \end{gathered}$$
    $${\hat{C}}_{b_0}^{n_0}=C_{n0}^n{\hat{C}}_{b_0}^{n_0}{C}_b^{b_0}$$