067-地固系下基于等效旋转矢量的惯导更新

一、姿态更新算法

对于地固系下的方向余弦阵有：

$C_{b(m)}^{e(m)} = C_{e(m-1)}^{e(m)}C_{b(m-1)}^{e(m-1)} C_{b(m)}^{b(m-1)}$

$C_{b(m-1)}^{e(m)}， C_{b(m)}^{e(m)}$ 分别是 $t_{m-1},t_m$ 时刻的姿态矩阵。那么姿态更新问题就是求解 $C_{e(m-1)}^{e(m)}$ 和 $C_{b(m)}^{b(m-1)}$ 的问题。设在时间段 $T=t_m-t_{m-1}$ 内b系产生的等效旋转矢量为 $\phi_{ib(m)}^b$ ，其模为 $\phi$ ，反对称矩阵为 $\Phi$ ，地球自转的等效旋转矢量的反对称阵为 $\Phi_e$ ，角速度为 $\omega_e$ ，那么：

$C_{e(m-1)}^{e(m)} \approx I-\Phi_e= I - \begin{bmatrix} 0 & -\omega_e & 0 \\ \omega_e & 0 & 0\\ 0 & 0 & 0 \end{bmatrix}T$

$C_{b(m)}^{b(m-1)} = I + \frac{sin\phi}{\phi}\Phi + \frac{1-cos\phi}{\phi^2} \Phi^2$

可能有人会说： $C_{e(m-1)}^{e(m)}=I$ ，其实这是不对的。无论是机体所在的b系转动所产生的等效旋转矢量，还是地球自转的等效旋转矢量，都是以地球惯性系为参考基准的，所以不能认为 $C_{e(m-1)}^{e(m)}=I$ ，同样在速度更新中也会遇见同样的问题。
下面利用等效旋转矢量法求解 $\phi_{ib(m)}^b$ 。

1、圆锥误差补偿算法

由于该部分的算法不因选取的惯导更新坐标系的不同而不同，在此不进行逐步推导，具体算法如下所示：

假设陀螺在 $[t_{m-1},t_m]$ 内进行了两次等间隔采样(二子样算法)，角增量分别为 $\Delta \theta_{m1},\Delta \theta_{m2}$ ，采用二子样圆锥误差补偿算法，有：

$\phi_{ib(m)}^b=(\Delta \theta_{m1}+\Delta \theta_{m2})+\frac{2}{3}\Delta \theta_{m1}×\Delta \theta_{m2}$

二、速度更新算法

比力方程：

$\dot v^e(t) = C_b^e(t) f_{sf}^b(t) - 2\Omega_{ie}^e(t) v^e(t) + g^e(t)$

时间段 $[t_{m-1},t_m]$ 内积分：

$\int_{t_{m-1}}^{t_m} \dot v^e(t) dt = \int_{t_{m-1}}^{t_m} C_b^e(t) f_{sf}^b(t) - 2\Omega_{ie}^e(t) v^e(t) + g^e(t) dt$

即：

$\begin{aligned} v_m^e - v_{m-1}^e &= \int_{t_{m-1}}^{t_m} C_b^e(t) f_{sf}^b(t)dt + \int_{t_{m-1}}^{t_m} -2\Omega_{ie}^e(t) v^e(t) + g^e(t) dt \\ &= \Delta v_{sf(m)}^e + \Delta v_{cor/g(m)}^e \end{aligned}$

其中：

$\tag{2-1} \Delta v_{sf(m)}^e = \int_{t_{m-1}}^{t_m} C_b^e(t) f_{sf}^b(t)dt$

$\tag{2-2} \Delta v_{cor/g(m)}^e = \int_{t_{m-1}}^{t_m} -2\Omega_{ie}^e(t) v^e(t) + g^e(t) dt$

上述两项分别为相应时间段 $T=t_m-t_{m-1}$ 内地固系内比力速度增量、有害加速度增量。

则有：

$v_m^e = v_{m-1}^e + \Delta v_{sf(m)}^e + \Delta v_{cor/g(m)}^e$

1、有害速度增量

一般认为有害加速度 $\Delta v_{cor/g(m)}^e$ 的被积函数是时间的缓慢量，可采用 $t_{m-1/2}=(t_{m-1}+t_m)/2$ 时刻的值进行近似代替，则公式(2-2)近似为：

$\Delta v_{cor/g(m)}^e \approx [-2\Omega_{ie(m-1/2)}^e v^e_{m-1/2} + g^e_{m-1/2}] T$

上式中 $t_{m-1/2}$ 时刻各量需使用外推法：

$x_{m-1/2} = x_{m-1} + \frac{x_{m-1}-x_{m-2}}{2} = \frac{3x_{m-1}-x_{m-2}}{2}$

其中， $x = \Omega_{ie}^e, v^e, g^e$

2、比力速度增量

将公式(2-1)进行如下分解：

$\Delta v_{sf(m)}^e = \int_{t_{m-1}}^{t_m} C_{e(m-1)}^{e(t)} C_{b(m-1)}^{e(m-1)} C_{b(t)}^{b(m-1)} f_{sf}^b(t)dt$

注意，这里 $C_{e(m-1)}^{e(t)}=I$ 也是不成立的！！

假设与 $C_{e(m-1)}^{e(t)}$ 对应的等效旋转矢量为 $\phi_{ie}^e(t,t_{m-1})$ ，角增量为 $\theta_{ie}^e(t,t_{m-1})$ (近似为地球自转角速度*时间)，与 $C_{b(t)}^{b(m-1)}$ 对应的等效旋转矢量为 $\phi_{ib}^b(t,t_{m-1})$ ，角增量为 $\theta_{ib}^b(t,t_{m-1})$ ，根据变换矩阵与等效旋转矢量之间的关系，当等效旋转矢量为小量时，可取如下一阶近似：

$\begin{aligned} C_{e(m-1)}^{e(t)} &\approx I - \phi_{ie}^e(t,t_{m-1})× \approx I - \theta_{ie}^e(t,t_{m-1})× \\ C_{b(t)}^{b(m-1)} &\approx I + \phi_{ib}^b(t,t_{m-1})× \approx I + \theta_{ib}^b(t,t_{m-1})× \\ \end{aligned}$

那么便有：

$\begin{aligned} \Delta v_{sf(m)}^e &= \int_{t_{m-1}}^{t_m} C_{e(m-1)}^{e(t)} C_{b(m-1)}^{e(m-1)} C_{b(t)}^{b(m-1)} f_{sf}^b(t)dt \\ & \approx \int_{t_{m-1}}^{t_m} [I - \theta_{ie}^e(t,t_{m-1})×] C_{b(m-1)}^{e(m-1)} [I + \theta_{ib}^b(t,t_{m-1})×] f_{sf}^b(t)dt \\ & \approx \int_{t_{m-1}}^{t_m} C_{b(m-1)}^{e(m-1)} f_{sf}^b(t) + C_{b(m-1)}^{e(m-1)} [\theta_{ib}^b(t,t_{m-1})×] f_{sf}^b(t) -[\theta_{ie}^e(t,t_{m-1})×]C_{b(m-1)}^{e(m-1)} f_{sf}^b(t)dt \\ &= C_{b(m-1)}^{e(m-1)} \int_{t_{m-1}}^{t_m} f_{sf}^b(t)dt + C_{b(m-1)}^{e(m-1)} \int_{t_{m-1}}^{t_m} [\theta_{ib}^b(t,t_{m-1})×] f_{sf}^b(t)dt - [\theta_{ie}^e(t,t_{m-1})×]C_{b(m-1)}^{e(m-1)} \int_{t_{m-1}}^{t_m} f_{sf}^b(t)dt \\ &= (I-[\theta_{ie}^e(t,t_{m-1})×])C_{b(m-1)}^{e(m-1)} \int_{t_{m-1}}^{t_m} f_{sf}^b(t)dt + C_{b(m-1)}^{e(m-1)} \int_{t_{m-1}}^{t_m} [\theta_{ib}^b(t,t_{m-1})×] f_{sf}^b(t)dt \\ \end{aligned}$ Δvsf(m)e=∫tm−1tmCe(m−1)e(t)Cb(m−1)e(m−1)Cb(t)b(m−1)fsfb(t)dt≈∫tm−1tm[I−θiee(t,tm−1)×]Cb(m−1)e(m−1)[I+θibb(t,tm−1)×]fsfb(t)dt≈∫tm−1tmCb(m−1)e(m−1)fsfb(t)+Cb(m−1)e(m−1)[θibb(t,tm−1)×]fsfb(t)−[θiee(t,tm−1)×]Cb(m−1)e(m−1)fsfb(t)dt=Cb(m−1)e(m−1)∫tm−1tmfsfb(t)dt+Cb(m−1)e(m−1)∫tm−1tm[θibb(t,tm−1)×]fsfb(t)dt−[θiee(t,tm−1)×]Cb(m−1)e(m−1)∫tm−1tmfsfb(t)dt=(I−[θiee(t,tm−1)×])Cb(m−1)e(m−1)∫tm−1tmfsfb(t)dt+Cb(m−1)e(m−1)∫tm−1tm[θibb(t,tm−1)×]fsfb(t)dt

(1) 旋转和划桨误差

对于上式的第二项中 $\int_{t_{m-1}}^{t_m} [\theta_{ib}^b(t,t_{m-1})×] f_{sf}^b(t)dt$ ，我们称为旋转与划船误差。下面对其进行分析，由于

$\begin{aligned} \frac{d[\theta_{ib}^b(t,t_{m-1}) × v_{sf}^b(t,t_{m-1})]}{dt} &= \omega_{ib}^b(t) × v_{sf}^b(t,t_{m-1}) + \theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) \\ &= -v_{sf}^b(t,t_{m-1}) × \omega_{ib}^b(t) - \theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) + 2\theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) \end{aligned}$

移项整理可得：

$\theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) \\= \frac{1}{2} \frac{d[\theta_{ib}^b(t,t_{m-1}) × v_{sf}^b(t,t_{m-1})]}{dt} + \frac{1}{2} [\theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) + v_{sf}^b(t,t_{m-1}) × \omega_{ib}^b(t)]$

时间段 $[t_{m-1},t_m]$ 内积分：

$\int_{t_{m-1}}^{t_m} \theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) dt \\= \frac{1}{2} \theta_{ib}^b(t_m,t_{m-1}) × v_{sf}^b(t_m,t_{m-1}) + \frac{1}{2} \int_{t_{m-1}}^{t_m} \theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) + v_{sf}^b(t,t_{m-1}) × \omega_{ib}^b(t) dt \\ = \Delta v_{rot(m)}^{b(m-1)} + \Delta v_{scul(m)}^{b(m-1)}$

其中：

$\Delta v_{rot(m)}^{b(m-1)} = \frac{1}{2}\Delta \theta_m × \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}) × f_{sf}^b(t) + v_{sf}^b(t,t_{m-1}) × \omega_{ib}^b(t) dt$

$\begin{cases} \Delta \theta_m = \theta_{ib}^b(t_m,t_{m-1}) = \int_{t_{m-1}}^{t_m} \omega_{ib}^b(t) dt \\ \Delta v_m = v_{sf}^b(t_m,t_{m-1}) = \int_{t_{m-1}}^{t_m} f_{sf}^b(t) dt \\ \end{cases}$

$\Delta v_{rot(m)}^{b(m-1)}$ 称为速度的旋转误差补偿量；
$\Delta v_{scul(m)}^{b(m-1)}$ 称为划桨误差补偿量；

(2) 划桨误差补偿算法

假设动坐标系(b)绕其x轴做角振动，同时绕y轴做线振动，两者的频率相同但相位正好差90°，即角速度和比力分别为：

$\omega_{ib}^b(t) = \begin{bmatrix} \alpha \Omega sin \Omega t \\ 0 \\ 0 \\ \end{bmatrix}, f_{sf}^b(t) = \begin{bmatrix} 0 \\ \beta \Omega cos \Omega t \\ 0 \\ \end{bmatrix}$

其中， $\alpha$ 为角振动的角度幅值， $\beta$ 为线振动的比力增量幅值， $\Omega$ 为震动频率。
积分得其角增量和比力增量：

$\theta_{ib}^b(t,t_{m-1}) = \int_{t_{m-1}}^{t_m} \omega_{ib}^b(\tau) d\tau = \begin{bmatrix} -\alpha(cos\Omega t - cos\Omega t_{m-1})\\ 0 \\ 0 \\ \end{bmatrix}$

$v_{sf}^b(t,t_{m-1}) = \int_{t_{m-1}}^{t_m} f_{sf}^b(\tau) d\tau = \begin{bmatrix} 0 \\ \beta(sin\Omega t - sin\Omega t_{m-1})\\ 0 \\ \end{bmatrix}$

那么：

$\begin{aligned} \Delta v_{scul(m)}^{b(m-1)} &= \frac{1}{2} \int_{t_{m-1}}^{t_m} \theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) + v_{sf}^b(t,t_{m-1}) × \omega_{ib}^b(t) dt\\ &= \frac{1}{2} \int_{t_{m-1}}^{t_m} \begin{bmatrix} -\alpha(cos\Omega t - cos\Omega t_{m-1}) \\ 0 \\ 0 \\ \end{bmatrix} × \begin{bmatrix} 0 \\ \beta \Omega cos \Omega t \\ 0 \\ \end{bmatrix} \\ &+ \begin{bmatrix} 0 \\ \beta(sin\Omega t - sin\Omega t_{m-1})\\ 0 \\ \end{bmatrix} × \begin{bmatrix} \alpha \Omega sin \Omega t \\ 0 \\ 0 \\ \end{bmatrix} dt\\ &= \frac{1}{2} \int_{t_{m-1}}^{t_m} \begin{bmatrix} -\alpha(cos\Omega t - cos\Omega t_{m-1}) \\ \beta(sin\Omega t - sin\Omega t_{m-1}) \\ 0 \\ \end{bmatrix} × \begin{bmatrix} \alpha \Omega sin \Omega t \\ \beta \Omega cos \Omega t \\ 0 \\ \end{bmatrix} dt \\ &= \frac{1}{2} \int_{t_{m-1}}^{t_m} [\theta_{ib}^b(t,t_{m-1}) + v_{sf}^b(t,t_{m-1})] × [f_{sf}^b(t) + \omega_{ib}^b(t)] dt \end{aligned}$ Δvscul(m)b(m−1)=21∫tm−1tmθibb(t,tm−1)×fsfb(t)+vsfb(t,tm−1)×ωibb(t)dt=21∫tm−1tm⎣⎡−α(cosΩt−cosΩtm−1)00⎦⎤×⎣⎡0βΩcosΩt0⎦⎤+⎣⎡0β(sinΩt−sinΩtm−1)0⎦⎤×⎣⎡αΩsinΩt00⎦⎤dt=21∫tm−1tm⎣⎡−α(cosΩt−cosΩtm−1)β(sinΩt−sinΩtm−1)0⎦⎤×⎣⎡αΩsinΩtβΩcosΩt0⎦⎤dt=21∫tm−1tm[θibb(t,tm−1)+vsfb(t,tm−1)]×[fsfb(t)+ωibb(t)]dt

若设 $U_{mi}=\Delta \theta_{mi}+\Delta v_{mi}$ ，其中 $\Delta \theta_{mi}=\int_{t_{m-1}+(i-1)h}^{t_{m-1}+ih} \omega_{ib}^b(t)dt, \Delta v_{mi}=\int_{t_{m-1}+(i-1)h}^{t_{m-1}+ih} f_{sf}^b(t)dt$ ，类似于圆锥误差补偿公式，有划桨误差补偿算法(估计公式)：

$\begin{aligned} \Delta \hat v_{scul(m)}^{b(m-1)} &= \sum_{i=1}^{N-1} k_{N-i} U_{mi} × U_{mN} \\ &= \sum_{i=1}^{N-1} k_{N-i} (\Delta \theta_{mi}+\Delta v_{mi})× (\Delta \theta_{mN}+\Delta v_{mN}) \end{aligned}$

在划桨运动***意到 $\Delta \theta_{mi} × \Delta \theta_{mN} = \Delta v_{mi} × \Delta v_{mN} = 0$ ，故：

$\Delta \hat v_{scul(m)}^{b(m-1)} = \sum_{i=1}^{N-1} k_{N-i} \Delta \theta_{mi} × \Delta v_{mN} + \sum_{i=1}^{N-1} k_{N-i} \Delta v_{mi} × \Delta \theta_{mN}$

其中系数 $k_{N-i}$ 同圆锥误差补偿系数。
整理比力速度增量：

$\begin{aligned} \Delta v_{sf(m)}^e & \approx (I-[\theta_{ie}^e(t,t_{m-1})×]) C_{b(m-1)}^{e(m-1)} \int_{t_{m-1}}^{t_m} f_{sf}^b(t)dt + C_{b(m-1)}^{e(m-1)} \int_{t_{m-1}}^{t_m} [\theta_{ib}^b(t,t_{m-1})×] f_{sf}^b(t)dt \\ & \approx (I-[\theta_{ie}^e(t,t_{m-1})×]) C_{b(m-1)}^{e(m-1)} \Delta v_m + C_{b(m-1)}^{e(m-1)} (\Delta v_{rot(m)}^{b(m-1)} + \Delta \hat v_{scul(m)}^{b(m-1)}) \\ \end{aligned}$

其中， $\int_{t_{m-1}}^{t_m} f_{sf}^b(t)dt \approx f_{sf}^b(t) * T \approx \Delta v_m$

3、公式汇总

重写速度更新如下：

$\begin{aligned} v_m^e &= v_{m-1}^e + \Delta v_{sf(m)}^e + \Delta v_{cor/g(m)}^e \\ \Delta v_{sf(m)}^e & \approx (I-[\theta_{ie}^e(t,t_{m-1})×]) C_{b(m-1)}^{e(m-1)} \Delta v_m + C_{b(m-1)}^{e(m-1)} (\Delta v_{rot(m)}^{b(m-1)} + \Delta \hat v_{scul(m)}^{b(m-1)}) \\ \Delta v_{rot(m)}^{b(m-1)} &= \frac{1}{2}\Delta \theta_m × \Delta v_m \\ \Delta \hat v_{scul(m)}^{b(m-1)} &= \sum_{i=1}^{N-1} k_{N-i} \Delta \theta_{mi} × \Delta v_{mN} + \sum_{i=1}^{N-1} k_{N-i} \Delta v_{mi} × \Delta \theta_{mN} \\ \Delta v_{cor/g(m)}^e & \approx [-2\Omega_{ie(m-1/2)}^e v^e_{m-1/2} + g^e_{m-1/2}] T \end{aligned}$ vmeΔvsf(m)eΔvrot(m)b(m−1)Δv^scul(m)b(m−1)Δvcor/g(m)e=vm−1e+Δvsf(m)e+Δvcor/g(m)e≈(I−[θiee(t,tm−1)×])Cb(m−1)e(m−1)Δvm+Cb(m−1)e(m−1)(Δvrot(m)b(m−1)+Δv^scul(m)b(m−1))=21Δθm×Δvm=i=1∑N−1kN−iΔθmi×ΔvmN+i=1∑N−1kN−iΔvmi×ΔθmN≈[−2Ωie(m−1/2)evm−1/2e+gm−1/2e]T

参考：
严恭敏.捷联惯导算法与组合导航原理讲义