捷联惯导系统学习4.1(惯导数值更新算法)

1 常用坐标系的定义

（1）地心惯性坐标系(i 系,inertial frame)
用$o_i{x}_i{y}_i{z}_i$表示，原点以地球为中心，
原点$o_i$在地球中心
$o_i{x}_i$在地球平面内，指向春分点
$o_i{z}_i$为地球自转轴，指向北极
$o_i{y}_i$轴在地球平面内，垂直于$o_i{x}_i$和$o_i{z}_i$

（2）地球坐标系(e 系,earth frame)（地心固坐标系）
用$o_e{x}_e{y}_e{z}_e$表示，原点为地球中心，与地球固连。
$o_g{x}_g$：在赤道平面内指向本初子午线
$o_g{y}_g$：在地球赤道平面内
$o_g{z}_g$：为地球自转轴，指向北极
地球坐标系相对于惯性坐标系的角运动就是地球自转速度，常用：
$w_{ie}=7.2921151467\times 10^{-5}rad/s$

（3）地理坐标系(g系,geographic frame)
$o_g{x}_g{y}_g{z}_g$表示“东北天”坐标系作为导航参考坐标系
$o_g$定义为运载体的中心或重心。
$o_g{x}_g$指向地理东向
$o_g{y}_g$指向地理北向
$o_g{z}_g$垂直于当地旋转椭圆面，指向天向

（4）导航坐标系(n系,mavigation frame)
$o_n{x}_n{y}_n{z}_n$表示，惯导系统求解导航参数所用坐标系
常用当地坐标系作为参考坐标系，常常为地理坐标系

（5）载体坐标系(b系,体系，body frame)
$o_b{x}_b{y}_b{z}_b$
$o_b$：原点定义为运载体的重心或中心
$o_b{x}_b$沿载体面向右
$o_b{y}_b$沿载体纵轴向前
$o_b{z}_b$沿载体立轴向上

2 姿态更新算法

选择东北天坐标系地理坐标系，作为导航参考坐标系，记为n系
则n系的微分方程为：
$w_b^n\times :坐标系n下，载体（b系）的角速度，（实际中不能直接获得）$
$C_b^n:n->b系的转换矩阵$
$${\dot{C}}_b^n=C_b^n(w_b^n\times )$$
由于：$(w_b^n\times )$不能直接测得：
$w_{bn}^n=w_{ib}^b+w_{ni}^b=w_{ib}^b-w_{in}^b$
$w_{bn}^n:n系相对于b的角速度，在n系下的转速$
$w_{ib}^b:陀螺仪的输出:载体在b系相对于i系的转速，在b系下的表示$

$${\dot{C}}_b^n=C_b^n(w_b^n\times )=C_b^n[(w_{ib}^b-w_{in}^b)\times ]=C_b^n(w_{ib}^b\times )-(w_{in}^n\times )C_b^n$$
$$w_{in}^n=w_{ie}^n+w_{en}^n$$
$w_{ie}:地球自转速度;L:地理纬度;h：高度$
$w_{ie}^n={\left[\begin{array}{ccc}0& w_{ie}cosL& w_{ie}sinL\end{array}\right]}^T$
$w_{en}^n={\left[\begin{array}{ccc}-\frac{v_N}{R_M+h}& -\frac{v_E}{R_N+h}& \frac{v_E}{R_N+h}tanL\end{array}\right]}^T$
$R_M=\frac{R_e(1-e^2)}{(1-e^{2}si{n}^{2}L{)}^{3/2}}(R_e:地球长半径，e:椭圆偏心率)$
$R_N=\frac{R_e}{\sqrt{1-e^{2}si{n}^{2}L}}$

捷联惯导导数值更新算法
$$C_{b(m)}^{n(m)}=C_{n(m-1)}^{n(m)}{C}_{b(m-1)}^{n(m-1)}{C}_{b(m)}^{b(m-1)}$$

若采用二子样法测得$\Delta {\theta }_1,\Delta {\theta }_2$
得到:$${\varphi }_{ib(m)}^b=(\Delta {\theta }_1+\Delta {\theta }_2)+\frac{2}{3}\Delta {\theta }_1\times \Delta {\theta }_2$$
$$C_{b(m)}^{b(m-1)}=M_{RV}({\varphi }_{ib(m)}^b)=I+\frac{sin|{\varphi }_{ib(m)}^b|}{|{\varphi }_{ib(m)}^b|}({\varphi }_{ib(m)}^b\times )+\frac{1-cos|{\varphi }_{ib(m)}^b|}{|{\varphi }_{ib(m)}^b{|}^2}({\varphi }_{ib(m)}^b\times {)}^2$$
$$C_{n(m-1)}^{n(m)}=M_{RV}^T({\varphi }_{in(m)}^n)\approx M_{RV}^T(T{w}_{in(m)}^n)（w_{in(m)}^n)由速度和位置引起这里我们认为是常量）$$

3 比力方程

比力：加速度计测量的载体相对惯性空间的绝对加速度和引力加速度之差
运载体在导航系下的惯导结算方程：
运载体在地球表面运动时比力方程，可达$10^{-7}g$对于惯性级导航可忽略。
$g^n:重力加速度$
$f_{sf}^b:加速度计测量的比力$
$w_{en}^n\times v_{en}^n：由载体引起的对地向心加速度$
$2w_{ie}^n\times v_{en}^n：由载体和地球自转引起的哥氏加速度$
${\dot{v}}_{en}^n:运载体在导航系下的运动加速度$
$C_b^n:b系到n系的转换矩阵$
$${\dot{v}}_{en}^n=C_b^n{f}_{sf}^b-(2w_{ie}^n+w_{en}^n)\times v_{en}^n+g^n$$

4 速度更新算法

利用比力方程在$[t_{m-1},t_m]$内积分，求得速度的递推更新公式为：
$v_m^{n(m)}:t_m时刻惯导速度$
$v_{m-1}^{n(m-1)}:t_{m-1}时刻惯导速度$
$\Delta v_{sf(m)}^n:导航比力速度增量$
$\Delta v_{cor/g(m)}^n:有害加速度增量$

$$v_m^{n(m)}=v_{m-1}^{n(m-1)}+\Delta v_{sf(m)}^n+\Delta v_{cor/g(m)}^n$$
$$\Delta v_{sf(m)}^n={\int }_{t_{m-1}}^{t_m}{C}_b^n(t)f_{sf}^b(t)dt$$
Δ v c o r / g ( m ) n = ∫ t m − 1 t m { − [ 2 w i e n ( t ) + w e n n ( t ) ] × v n ( t ) + g n ( t ) } d t \Delta v_{cor/g(m)}^n=\int_{t_{m-1}}^{t_m}\{-[2w_{ie}^n(t)+w_{en}^n(t)]×v^n(t)+g^n(t)\}dt Δvcor/g(m)n​=∫tm−1​tm​​{−[2wien​(t)+wenn​(t)]×vn(t)+gn(t)}dt
$\Delta v_{cor/g(m)}^n$的计算()：
Δ v c o r / g ( m ) n ≈ { − [ 2 w i e ( m − 1 / 2 ) n + w e n ( m − 1 / 2 ) n ] × v m − 1 / 2 n + g m − 1 / 2 n } T \Delta v_{cor/g(m)}^n\approx \{ -[2w_{ie(m-1/2)}^n+w_{en(m-1/2)}^n]×v_{m-1/2}^{n}+g_{m-1/2}^{n}\}T Δvcor/g(m)n​≈{−[2wie(m−1/2)n​+wen(m−1/2)n​]×vm−1/2n​+gm−1/2n​}T
使用外推法计算：
$$x_{m-1/2}=x_{m-1}+\frac{x_{m-1}-x_{m-2}}{2}(x=w_{ie}^n,w_{en}^n,v^n,g^n)$$
$\Delta v_{sf(m)}^n$的计算（）：
${\theta }_{ib}^b(t,t_{m-1}):角增量$
$$\Delta v_{sf(m)}^n\approx C_{b(m-1)}^{n(m-1)}{\int }_{t_{m-1}}^{t_m}{f}_{sf}^b(t)dt-{\int }_{t_{m-1}}^{t_m}{\theta }_{in}^n(t,t_{m-1})\times [C_{b(m-1)}^{n(m-1)}{f}_{sf}^b(t)]dt+C_{b(m-1)}^{n(m-1)}{\int }_{t_{m-1}}^{t_m}{\theta }_{ib}^b(t,t_{m-1})f_{sf}^b(t)dt$$
${\int }_{t_{m-1}}^{t_m}{\theta }_{ib}^b(t,t_{m-1})f_{sf}^b(t)dt=\Delta v_{rot(m)}^{b(m-1)}+\Delta v_{scul(m)}^{b(m-1)}$:
$$\{\begin{array}{ll}\Delta v_{rot(m)}^{b(m-1)}=\frac{1}{2}\Delta {\theta }_m\times \Delta v_m& 速度的旋转误差补偿\\ \Delta v_{scul(m)}^{b(m-1)}=\frac{1}{2}{\int }_{t_{m-1}}^{t_m}{\theta }_{ib}^b(t,t_{m-1})\times f_{sf}^b(t)+v_{sf}^b(t,t_{m-1})\times w_{ib}^b(t)dt& 划桨误差补偿\\ \Delta {\theta }_m={\int }_{t_{m-1}}^m{w}_{ib}^b(t)dt& 陀螺仪采样角增量\\ \Delta v_m={\int }_{t_{m-1}}^{t_m}{f}_{sf}^b(t)dt& 加速计采样比力速度增量\end{array}$$
假设陀螺仪角速度和加速度比力测量输出均为线性模型
即：
$$\{\begin{array}{ll}{w}_{ib}^b(t)=a+2b(t-t_{m-1})& 陀螺仪采样角增量\\ f_{sf}^b(t)=A+2B(t-t_{m-1})& 加速计采样比力速度增量\end{array}$$
得到
$$\{\begin{array}{ll}\Delta {\theta }_m={\int }_{t_{m-1}}^{t_m}{w}_{ib}^b(t)dt=a(t-t_{m-1})+b(t-t_{m-1}{)}^2& 陀螺仪采样角增量\\ \Delta v_m={\int }_{t_{m-1}}^{t_m}{f}_{sf}^b(t)dt=A(t-t_{m-1})+B(t-t_{m-1}{)}^2& 加速计采样比力速度增量\end{array}$$
可以得到：
$$\{\begin{array}{ll}\Delta v_{rot(m)}^{b(m-1)}=\frac{1}{2}\Delta {\theta }_m\times \Delta v_m& 速度的旋转误差补偿\\ \Delta v_{scul(m)}^{b(m-1)}=(a\times B+A\times b)\frac{(t_m-t_{m-1}{)}^3}{6}& 划桨误差补偿\end{array}$$
若采用2子样法，在$[t_{m-1},t_m]$时间两次等间隔采样数据为:
$\Delta {\theta }_{m1},\Delta {\theta }_{m2},\Delta v_{m1},\Delta v_{m2},h=\frac{T}{2}=\frac{t_m-t_{m-1}}{2}$
可以得到：
$a=\frac{3\Delta {\theta }_{m1}-\Delta {\theta }_{m2}}{2h},b=\frac{{\theta }_{m2}-{\theta }_{m1}}{2h^2}$
$A=\frac{3\Delta v_{m1}-\Delta v_{m2}}{2h},B=\frac{v_{m2}-v_{m1}}{2h^2}$
$$\{\begin{array}{ll}\Delta v_{rot(m)}^{b(m-1)}=\frac{1}{2}\Delta {\theta }_m\times \Delta v_m& 速度的旋转误差补偿\\ \Delta v_{scul(m)}^{b(m-1)}=\frac{2}{3}(\Delta {\theta }_{m1}\times \Delta v_{m2}+\Delta v_{m1}\times \Delta {\theta }_{m2})& 划桨误差补偿\end{array}$$
${\int }_{t_{m-1}}^{t_m}{\theta }_{in}^n(t,t_{m-1})\times [C_{b(m-1)}^{n(m-1)}{f}_{sf}^b(t)]dt\approx \frac{T}{6}{w}_{in(m-1/2)}^n\times [C_{b(m-1)}^{n(m-1)}(\Delta v_{m1}+\Delta v_{m2})]$

最后得到：
$$\Delta v_{sf(m)}^n=C_{b(m-1)}^{n(m-1)}\Delta v_m-\frac{T}{6}{w}_{in(m-1/2)}^n\times [C_{b(m-1)}^{n(m-1)}(\Delta v_{m1}+5\Delta v_{m2})]+C_{b(m-1)}^{n(m-1)}(\Delta v_{rot(m)}^{b(m-1)}+\Delta v_{scul(m)}^{b(m-1)})$$
若在$[t_{m-1},t_m]$内的比力变化不大，可以近似看作$\Delta v_{m1}\approx \Delta v_{m2}\approx \frac{1}{2}\Delta v_m$
$$\Delta v_{sf(m)}^n\approx [I-\frac{T}{2}(w_{in(m-1/2)}^n\times )]C_{b(m-1)}^{n(m-1)}(\Delta v_m+\Delta v_{rot(m)}^{b(m-1)}+\Delta v_{scul(m)}^{b(m-1)})$$

5 划桨误差补偿

（1）使用二子样法，测到划桨误差在多项式条件下的误差补偿如下（已在上方求解）：
$\Delta v_{scul(m)}^{b(m-1)}=\frac{2}{3}(\Delta {\theta }_{m1}\times \Delta v_{m2}+\Delta v_{m1}\times \Delta {\theta }_{m2})$
（2）理想条件下的划桨误差补偿
划桨运动：假设坐标系（b系：物体坐标系）：绕其x轴做角振动，绕其y轴做线振动，两者振动频率相同为$\Omega$，单相差$90^{。}$
则其角度速度和比例分别为：
$$w_{ib}^b(t)=\left[\begin{array}{ccc}a\Omega tsin\Omega t& 0& 0\end{array}\right]$$
$$f_{sf}^b(t)=\left[\begin{array}{ccc}0& \beta \Omega cos\Omega t& 0\end{array}\right]$$
在时间$[t_{m-1},t]$内的角增量和比力增量为：
${\theta }_{ib}^b(t_{m-1},t)={\int }_{t_{m-1}}^t{w}_{ib}^b(\tau )d\tau ={\left[\begin{array}{ccc}-a(\sin \Omega t-\cos \Omega t_{m-1})& 0& 0\end{array}\right]}^T$
$v_{sf}^b(t_{m-1},t_m)={\int }_{t_{m-1}}^t{f}_{sf}^b(\tau )d\tau ={\left[\begin{array}{ccc}0& \beta (\sin \Omega t-sin\Omega t_{t_{m-1}})& 0\end{array}\right]}^T$
带入:
$$\Delta v_{scul(m)}^{b(m-1)}=\frac{1}{2}{\int }_{t_{m-1}}^{t_m}{\theta }_{ib}^b(t,t_{m-1})\times f_{sf}^b(t)+v_{sf}^b(t,t_{m-1})\times w_{ib}^b(t)dt$$
$$=\frac{1}{2}{\int }_{t_{m-1}}^{t_m}[{\theta }_{ib}^b(t,t_m)+v_{sf}^b(t,t_{m-1})]\times [f_{sf}^b(t)+w_{ib}^b(t)]dt$$
划桨误差与圆锥误差的不可交换式形式上一致，因此将圆锥误差的补偿算法应用于划桨误差算法，圆锥误差求解，
求得划桨误差的补偿公式如下：
$$\Delta v_{scul(m)}^{b(m-1)}=\sum _{i=1}^{N-1}{k}_{N-i}\Delta {\theta }_{mi}\times \Delta v_{mN}+\sum _{i=1}^{N-1}{k}_{N-i}\Delta v_{mi}\times \Delta {\theta }_{mN}$$
剩余误差为（未补偿的高阶误差项）：${\rho }_n与圆锥误差补偿{\rho }_n相同$
$▽n=\frac{1}{T}[\Delta {\overline{v}}_{scul(m)}^{b(m-1)}-\Delta v_{scul(m)}^{b(m-1)}{]}_z={\rho }_n\frac{\alpha \beta \Omega (\Omega T{)}^{2N+1}}{T}(N\ge 1)$

6 位置更新算法

$公式（1）$：计算等效旋转矢量
$${\varphi }_{ib(m)}^b=(\Delta {\theta }_1+\Delta {\theta }_2)+\frac{2}{3}\Delta {\theta }_1\times \Delta {\theta }_2$$
$公式（2）$：计算误差补偿值
$$\Delta v_{rot(m)}^{b(m-1)}=\frac{1}{2}\Delta {\theta }_m\times \Delta v_m$$
$$\Delta v_{scul(m)}^{b(m-1)}=\frac{2}{3}(\Delta {\theta }_{m1}\times \Delta v_{m2}+\Delta v_{m1}\times \Delta {\theta }_{m2})$$
$公式（3）$：捷联惯导导数值更新算法
$$C_{b(m)}^{n(m)}=C_{n(m-1)}^{n(m)}{C}_{b(m-1)}^{n(m-1)}{C}_{b(m)}^{b(m-1)}$$
$公式（4）$速度更新算法
$$\Delta v_{sf(m)}^n\approx [I-\frac{T}{2}(w_{in(m-1/2)}^n\times )]C_{b(m-1)}^{n(m-1)}(\Delta v_m+\Delta v_{rot(m)}^{b(m-1)}+\Delta v_{scul(m)}^{b(m-1)})$$
$$v_m^{n(m)}=v_{m-1}^{n(m-1)}+\Delta v_{sf(m)}^n+\Delta v_{cor/g(m)}^n$$
$公式（5）$位置更新算法
$$p_m=p_{m-1}+M_{pv(m-1/2)}(v_{m-1}^{n(m-1)}+v_m^{n(m)})\frac{T}{2}$$
$R_{Nh}=R_n+h;R_{Mh}=R_M+h$
$$M_{pv}={\left[\begin{array}{ccc}0& 1/R_{Mh}& 0\\ secL/R_{Nh}& 0& 0\\ 0& 0& 1\end{array}\right]}^T$$
$公式（6）$外推公式
$$x_{m-1/2}=x_{m-1}+\frac{x_{m-1}-x_{m-2}}{2}(x=w_{ie}^n,w_{en}^n,v^n,g^n)$$