捷联惯导系统模型及仿真（四）

2. 编程实现–速度解算

2.2 速度解算

进行姿态解算的目标之一就是为 加速度计提供测量基准，即，将加速度计在机体坐标系下的输出 变换到 导航坐标中去。
数学表示： Cnbfb C b n f b
【注】：本编程实现中，速度更新周期 [tm,tm+1] [ t m , t m + 1 ] 与姿态更新周期 [tk,tk+1] [ t k , t k + 1 ] 相同，均为6倍的采样时间间隔。

2.2.1 算法原理

目标就是求得速度方程（或比力方程）的数值解：

V˙n=Cnbfb−(2ωnie+ωnen)×Vn+gn V ˙ n = C b n f b − ( 2 ω i e n + ω e n n ) × V n + g n

对该式子在一个速度更新周期 [tm,tm+1] [ t m , t m + 1 ] 做积分得，速度增量：

Vnm−Vnm−1=Cn(m−1)b(m−1)∫tmtm−1Cb(m−1)b(t)fbdt+∫tmtm−1[−(2ωnie+ωnen)×Vn+gn]dt V m n − V m − 1 n = C b ( m − 1 ) n ( m − 1 ) ∫ t m − 1 t m C b ( t ) b ( m − 1 ) f b d t + ∫ t m − 1 t m [ − ( 2 ω i e n + ω e n n ) × V n + g n ] d t

• Cn(m−1)b(m−1) C b ( m − 1 ) n ( m − 1 ) 是 tm−1 t m − 1 时刻姿态矩阵，且简记为 Cm−1 C m − 1

• Cb(m−1)b(t) C b ( t ) b ( m − 1 ) 反映 t(m−1) t ( m − 1 ) 时刻至 t(t) t ( t ) 时刻坐标系的变化，而这种变化也可以由等效旋转矢量表示，关系如下（推导见P253）：
  

  Cb(m−1)b(t)=I+sinϕϕ(Φ×)+1−cosϕϕ2(Φ×)(Φ×) C b ( t ) b ( m − 1 ) = I + s i n ϕ ϕ ( Φ × ) + 1 − c o s ϕ ϕ 2 ( Φ × ) ( Φ × )
  • 上式中的等效旋转矢量 Φ Φ 本应该由上一节中推导的旋转矢量微分方程解得，但此处取近似，( Φ Φ 为小量)：
    
    Cb(m−1)b(t)≈I+(Φ×) C b ( t ) b ( m − 1 ) ≈ I + ( Φ × )
    
    
    Φ≈ΔΘ=∫ttm−1ω(τ)dτ Φ ≈ Δ Θ = ∫ t m − 1 t ω ( τ ) d τ

这样， tm t m 时刻运载体在导航坐标系内的速度：

Vnm=Vnm−1+Cm−1ΔVsfm+ΔVg/corm=Vnm−1+Cm−1∫tmtm−1(I+(ΔΘ×))fbdt+∫tmtm−1[−(2ωnie+ωnen)×Vn+gn]dt V m n = V m − 1 n + C m − 1 Δ V s f m + Δ V g / c o r m = V m − 1 n + C m − 1 ∫ t m − 1 t m ( I + ( Δ Θ × ) ) f b d t + ∫ t m − 1 t m [ − ( 2 ω i e n + ω e n n ) × V n + g n ] d t

• 其中， ΔVg/corm Δ V g / c o r m 是由有害加速度引起的速度补偿量；

• ΔVsfm Δ V s f m 是由比力引起的速度补偿量；
  ΔVsfm=ΔVm+ΔVrotm+ΔVsculm Δ V s f m = Δ V m + Δ V r o t m + Δ V s c u l m

  • 其中， ΔVm=∫tmtm−1fb(t)dt Δ V m = ∫ t m − 1 t m f b ( t ) d t 为加计在 [tm,tm+1] [ t m , t m + 1 ] 输出的速度增量；
    ΔVrotm=12ΔΘm×ΔVm Δ V r o t m = 1 2 Δ Θ m × Δ V m 为速度的旋转效应补偿项；
    ΔVsculm=12∫tmtm−1[ΔΘ(t)×f(t)+ΔV(t)×ω(t)]dt Δ V s c u l m = 1 2 ∫ t m − 1 t m [ Δ Θ ( t ) × f ( t ) + Δ V ( t ) × ω ( t ) ] d t 为速度的划桨效应补偿项。

---

下面，分析对最后一项，即，划桨效应补偿项的补偿算法。
与姿态解算时同样地，运载体的角速度和加速度随时间变化的曲线复杂且未知，此处，采用直线拟合 ω(t) ω ( t ) 和 f(t) f ( t ) .
推导过程略，给出最后的划桨效应补偿项公式：

ΔVsculm=23[ΔVm(1)×ΔΘm(2)−ΔVm(2)×ΔΘm(1)] Δ V s c u l m = 2 3 [ Δ V m ( 1 ) × Δ Θ m ( 2 ) − Δ V m ( 2 ) × Δ Θ m ( 1 ) ]

但是，实际中运载体的角速度和比力 与 给定的拟合曲线 并不一致。于是下面给出 在划桨运动下，划桨效应补偿项的公式：
双子样，系数相同；
三子样、四子样公式详见P298.

2.2.2 算法的编程实现

实现功能：根据陀螺和加计的输出，计算运载体的速度。
函数参数输入：（此处采用三子样算法）
增量的输出对应的时间间隔： T T
陀螺的角度增量输出： ΔΘ1,ΔΘ2,ΔΘ3 Δ Θ 1 , Δ Θ 2 , Δ Θ 3
加计的速度增量输出： ΔV1,ΔV2,ΔV3 Δ V 1 , Δ V 2 , Δ V 3
当地的地球物理量信息： ωnie,ωnen,gn ω i e n , ω e n n , g n
tm−1 t m − 1 时刻的运载体速度： Vnm−1 V m − 1 n
函数的输出参数：
tm t m 时刻的运载体速度： Vnm V m n
代码如下：

function Vm1=CalculateVm(T,Cm,delTheta,delV,Vm,Wie,Wen,G)
% This function is to calculate the Vm1
% delTheta : 3-by-3 matrix \delta \Theta
% delV     : 3-by-3 matrix \delta V
% By changshen_xu
% Date 2018-07-19

delTheta1 = delTheta(:,1);
delTheta2 = delTheta(:,2);
delTheta3 = delTheta(:,3);
delV1 = delV(:,1);
delV2 = delV(:,2);
delV3 = delV(:,3);

% Vm1 = Vm + Cm \delta Vsfm + \delta Vgcorm
% ====\delta Vgcorm========
harmful_force = G-cross(2*Wie+Wen,Vm);% be treated as a constant
delVg = harmful_force*T;  

% \delta Vsfm = \delta Vm + \delta Vrotm + \delta Vsculm
% ====\delta Vm===========
delVm = delV1+delV2+delV3;
% ====\delta Vrotm=======
delThetam = delTheta1+delTheta2+delTheta3;
delVrotm = 1/2*cross(delVm,delThetam);
% ====\delta Vsculm========
delVsculm = 9/20*(cross(delTheta1,delV3) + cross(delV1,delTheta3)) +...
            27/40*(cross(delTheta1,delV2)+ cross(delTheta2,delV3) + ...
            cross(delV1,delTheta2) + cross(delV2,delTheta3));

delVsfm = delVm+delVrotm+delVsculm;

Vm1 = Vm+Cm*delVsfm+delVg;
end