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

2. 编程实现–速度解算

2.2 速度解算

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

2.2.1 算法原理

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

$$\dot V^n=C^n_bf^b-(2\omega^n_{ie}+\omega^n_{en})\times V^n+g^n$$
对该式子在一个速度更新周期 $[t_m,t_{m+1}]$做积分得，速度增量：
$$V^n_m-V^n_{m-1}=C^{n(m-1)}_{b(m-1)}\int ^{t_m}_{t_{m-1}}C^{b(m-1)}_{b(t)}f^bdt\\+\int ^{t_m}_{t_{m-1}}[ -(2\omega^n_{ie}+\omega^n_{en})\times V^n+g^n]dt$$

• $C^{n(m-1)}_{b(m-1)}$是$t_{m-1}$时刻姿态矩阵，且简记为 $C_{m-1}$

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

  $$C^{b(m-1)}_{b(t)}=I+\frac{sin\phi}{\phi}(\Phi\times)+\frac{1-cos\phi}{\phi^2}(\Phi\times)(\Phi\times)$$
  • 上式中的等效旋转矢量$\Phi$本应该由上一节中推导的旋转矢量微分方程解得，但此处取近似，($\Phi$为小量)：
    $$C^{b(m-1)}_{b(t)}\approx I+(\Phi\times)$$
    $$\Phi\approx \Delta\Theta=\int^{t}_{t_{m-1}}\omega(\tau)d\tau$$

这样，$t_m$时刻运载体在导航坐标系内的速度：

$$V^n_m=V^n_{m-1}+C_{m-1}\Delta V_{sfm}+\Delta V_{g/corm} \\=V^n_{m-1}+C_{m-1}\int ^{t_m}_{t_{m-1}}(I+(\Delta\Theta\times))f^bdt\\+\int ^{t_m}_{t_{m-1}}[ -(2\omega^n_{ie}+\omega^n_{en})\times V^n+g^n]dt$$

• 其中，$\Delta V_{g/corm}$是由有害加速度引起的速度补偿量；

• $\Delta V_{sfm}$是由比力引起的速度补偿量；
  $\Delta V_{sfm}=\Delta V_m+\Delta V_{rotm}+\Delta V_{sculm}$

  • 其中，$\Delta V_m=\int ^{t_m}_{t_{m-1}}f^b(t)dt$为加计在$[t_m,t_{m+1}]$输出的速度增量；
    $\Delta V_{rotm}=\frac{1}{2}\Delta \Theta_m\times \Delta V_m$为速度的旋转效应补偿项；
    $\Delta V_{sculm}=\frac{1}{2}\int ^{t_m}_{t_{m-1}}[\Delta \Theta(t)\times f(t)+\Delta V(t)\times \omega(t)]dt$为速度的划桨效应补偿项。

---

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

$$\Delta V_{sculm}=\frac{2}{3}[\Delta V_m(1)\times\Delta\Theta_m(2) -\Delta V_m(2)\times\Delta\Theta_m(1)]$$

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

2.2.2 算法的编程实现

实现功能：根据陀螺和加计的输出，计算运载体的速度。
函数参数输入：（此处采用三子样算法）
增量的输出对应的时间间隔：$T$
陀螺的角度增量输出：$\Delta\Theta_1,\Delta\Theta_2,\Delta\Theta_3$
加计的速度增量输出：$\Delta V_1,\Delta V_2,\Delta V_3$
当地的地球物理量信息：$\omega^n_{ie},\omega^n_{en},g^n$
$t_{m-1}$时刻的运载体速度：$V^n_{m-1}$
函数的输出参数：
$t_m$时刻的运载体速度：$V^n_m$
代码如下：

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