065-松组合备忘之捷联惯导

相关说明

(1) 通过解析式法进行对准，模型如下：
$$\left[\begin{array}{c}{g}^e\\ \\ g^e\times {\omega }_{ie}^e\\ \\ g^e\times {\omega }_{ie}^e\times g^e\end{array}\right]=C_b^e\left[\begin{array}{c}-f_{ib}^b\\ \\ -f_{ib}^b\times {\omega }_{ie}^e\\ \\ f_{ib}^b\times {\omega }_{ie}^e\times g^e\end{array}\right]$$
(2) 解算过程中依据以下内容进行更新，并根据实时位置获取大地坐标，求取$C_b^l$，提取姿态角；
(3) $l$ 为当地水平坐标系，指向东北天方向，并且定义坐标系正向转动产生的姿态角为正；
(4) 首先通过spp解算出坐标，再进行松组合；
(5) 每一次量测更新后，皆进行误差补偿，并改正零偏；
(6) QRP等矩阵设置不规范将导致“平台”失准角出现较大误差。

一、姿态更新与误差方程

1、姿态更新

对于姿态微分方程：

$${\dot{C}}_b^e=C_b^e\ast {\Omega }_{eb}^b$$

其中${\Omega }_{eb}^b$为角速度矢量${\omega }_{eb}^b$的反对称矩阵，其求解方法：

$${\omega }_{eb}^b={\omega }_{ib}^b-C_e^b{\omega }_{ie}^e$$

对(1)进行求解：

$$C_b^e(+)=C_b^e(-)exp({\int }_{-}^{+}{\Omega }_{eb}^{b}dt)$$

其中，$-$表示前一时刻，$+$表示当前时刻。
积分区间较小，可用等效旋转矢量代替：

$${\int }_{-}^{+}{\Omega }_{eb}^{b}dt={\sigma }_{eb}^b\times$$

其中${\sigma }_{eb}^b=[{\sigma }_x,{\sigma }_y,{\sigma }_z]$，那么：

$$C_b^e(+)=C_b^e(-)exp({\sigma }_{eb}^b\times )$$

下面求解$exp({\sigma }_{eb}^b\times )$，首先展开：

$$exp(\sigma \times )=I+(\sigma \times )+\frac{(\sigma \times {)}^2}{2!}+\frac{(\sigma \times {)}^3}{3!}+\frac{(\sigma \times {)}^4}{4!}+\frac{(\sigma \times {)}^5}{5!}+\frac{(\sigma \times {)}^6}{6!}+...$$

并有：

$$(\sigma \times {)}^2=\left[\begin{array}{ccc}-({\sigma }_y^2+{\sigma }_z^2)& {\sigma }_x{\sigma }_y& {\sigma }_x{\sigma }_z\\ & & \\ {\sigma }_x{\sigma }_y& -({\sigma }_x^2+{\sigma }_z^2)& {\sigma }_y{\sigma }_z\\ & & \\ {\sigma }_x{\sigma }_z& {\sigma }_y{\sigma }_z& -({\sigma }_x^2+{\sigma }_y^2)\end{array}\right]$$

$$\begin{array}{cc}(\sigma \times {)}^3=-{\sigma }^2(\sigma \times ),& (\sigma \times {)}^4=-{\sigma }^2(\sigma \times {)}^2,\\ (\sigma \times {)}^5={\sigma }^4(\sigma \times ),& (\sigma \times {)}^6={\sigma }^4(\sigma \times {)}^2,...\end{array}$$

可得：

$$\begin{array}{rl}exp(\sigma \times )& =I+(\sigma \times )+\frac{(\sigma \times {)}^2}{2!}+\frac{(\sigma \times {)}^3}{3!}+\frac{(\sigma \times {)}^4}{4!}+\frac{(\sigma \times {)}^5}{5!}+\frac{(\sigma \times {)}^6}{6!}...\\ & =I+(\sigma \times )+\frac{(\sigma \times {)}^2}{2!}-\frac{{\sigma }^2(\sigma \times )}{3!}-\frac{{\sigma }^2(\sigma \times {)}^2}{4!}+\frac{{\sigma }^4(\sigma \times )}{5!}+\frac{{\sigma }^4(\sigma \times {)}^2}{6!}...\\ & =I+(1-\frac{{\sigma }^2}{3!}+\frac{{\sigma }^4}{5!}-...)(\sigma \times )+(\frac{1}{2!}-\frac{{\sigma }^2}{4!}+\frac{{\sigma }^4}{6!}-...)(\sigma \times {)}^2\\ & =I+\frac{sin\sigma }{\sigma }(\sigma \times )+\frac{1-cos\sigma }{{\sigma }^2}(\sigma \times {)}^2\end{array}$$

故：

$$C_b^e(+)=C_b^e(-)[I+\frac{sin\sigma }{\sigma }(\sigma \times )+\frac{1-cos\sigma }{{\sigma }^2}(\sigma \times {)}^2]$$

2、姿态误差方程

载体真实姿态：$C_b^e$，载体通过INS推算得到的姿态${\tilde{C}}_b^e$，定义:
$${\tilde{C}}_b^e=(I-\Phi )C_b^e$$

可推得：
$${\tilde{C}}_b^e{C_b^e}^T=I-\Phi \text{\hspace{1em}(1)}$$
$${\tilde{C}}_b^e=C_b^e-\Phi C_b^e\text{\hspace{1em}(2)}$$

其中，$\Phi$为姿态失准角${\phi }^e$构成的斜对称矩阵。易得：
$$\Phi =I-\widetilde{C_b^e}{C_b^e}^T$$

$T$表转置，求导：

$$\dot{\Phi }=-\dot{{\tilde{C}}_b^e}{C_b^e}^T-\widetilde{C_b^e}{{\dot{C}}_b^e}^T$$

又有反对称阵的相似变换矩阵：
$${\Omega }_{ie}^e=C_b^e{\Omega }_{ie}^b{C}_e^b$$
可得：
$${\Omega }_{ie}^e{C}_b^e=C_b^e{\Omega }_{ie}^b$$
根据上式，对于姿态微分方程可得：
$$\begin{array}{rl}{\dot{C}}_b^e& =C_b^e\ast {\Omega }_{eb}^b=C_b^e({\Omega }_{ib}^b-{\Omega }_{ie}^b)\\ & =C_b^e\ast {\Omega }_{ib}^b-{\Omega }_{ie}^e\ast C_b^e\end{array}\text{\hspace{1em}(3)}$$
同时也有：
$$\dot{\widetilde{C_b^e}}={\tilde{C}}_b^e\ast {\tilde{\Omega }}_{ib}^b-{\tilde{\Omega }}_{ie}^e\ast {\tilde{C}}_b^e\text{\hspace{1em}(4)}$$

顾及(3)(4)，此时可得姿态误差方程：

$$\dot{\Phi }=-({\tilde{C}}_b^e{\tilde{\Omega }}_{ib}^b-{\tilde{\Omega }}_{ie}^e{\tilde{C}}_b^e){C_b^e}^T-{\tilde{C}}_b^e(C_b^e{\Omega }_{ib}^b-{\Omega }_{ie}^e{C}_b^e{)}^T$$

定义：陀螺加计的输出
$$\begin{array}{rl}{\omega }_{ib}^b& ={\tilde{\omega }}_{ib}^b-\delta {\omega }_{ib}^b\\ f_{ib}^b& ={\tilde{f}}_{ib}^b-\delta f_{ib}^b\end{array}$$
所以有：
$${\tilde{\Omega }}_{ib}^b={\Omega }_{ib}^b+\delta {\Omega }_{ib}^b\text{\hspace{1em}(5)}$$

顾及(1)(2)(5)，忽略地球自转角速度的误差及二阶误差项，得：

$$\begin{array}{rl}\dot{\Phi }& =-{\tilde{C}}_b^e{\tilde{\Omega }}_{ib}^b{C_b^e}^T+{\tilde{\Omega }}_{ie}^e{\tilde{C}}_b^e{C_b^e}^T+{\tilde{C}}_b^e{\Omega }_{ib}^b{C_b^e}^T-{\tilde{C}}_b^e{C_b^e}^T{\Omega }_{ie}^e\\ & \approx -(I-\Phi )C_b^e({\Omega }_{ib}^b+\delta {\Omega }_{ib}^b){C_b^e}^T+{\Omega }_{ie}^e(I-\Phi )C_b^e{C_b^e}^T+(I-\Phi )C_b^e{\Omega }_{ib}^b{C_b^e}^T-(I-\Phi )C_b^e{C_b^e}^T{\Omega }_{ie}^e\\ & =-(I-\Phi )C_b^e({\Omega }_{ib}^b+\delta {\Omega }_{ib}^b){C_b^e}^T+{\Omega }_{ie}^e(I-\Phi )+(I-\Phi )C_b^e{\Omega }_{ib}^b{C_b^e}^T-(I-\Phi ){\Omega }_{ie}^e\\ & =-C_b^e{\Omega }_{ib}^b{C_b^e}^T-C_b^e\delta {\Omega }_{ib}^b{C_b^e}^T+\Phi C_b^e{\Omega }_{ib}^b{C_b^e}^T+\Phi C_b^e\delta {\Omega }_{ib}^b{C_b^e}^T+{\Omega }_{ie}^e-{\Omega }_{ie}^e\Phi \\ & +C_b^e{\Omega }_{ib}^b{C_b^e}^T-\Phi C_b^e{\Omega }_{ib}^b{C_b^e}^T-{\Omega }_{ie}^e+\Phi {\Omega }_{ie}^e\\ & \approx -C_b^e\delta {\Omega }_{ib}^b{C_b^e}^T-{\Omega }_{ie}^e\Phi +\Phi {\Omega }_{ie}^e\\ & =-({\Omega }_{ie}^e\Phi -\Phi {\Omega }_{ie}^e)-C_b^e\delta {\Omega }_{ib}^b{C_b^e}^T\end{array}$$

即：

$${\dot{\phi }}^e=-{\Omega }_{ie}^e{\phi }^e-C_b^e\delta {\omega }_{ib}^b$$

二、速度更新与误差方程

1、速度更新

速度微分方程：

$${\dot{v}}^e=f_{ib}^e-2{\Omega }_{ie}^e{v}^e+g^e$$
对于比力：

$$f_{ib}^e(+)\approx \frac{1}{2}[C_b^e(-)+C_b^e(+)]\ast f_{ib}^b(+)$$

可得速度更新：
$$v^e\approx v^e(-)+[f_{ib}^e(+)-2{\Omega }_{ie}^e{v}^e(-)+g^e(-)]\ast \Delta t$$

2、速度误差方程

定义：
$$\delta v^e={\tilde{v}}^e-v^e$$

忽略重力及地球自转角速度误差，可得：

$$\begin{array}{rl}\delta {\dot{v}}^e& =\dot{{\tilde{v}}^e}-{\dot{v}}^e\\ & =-2{\Omega }_{ie}^e\delta v^e+[(C_b^e{f}_{ib}^b)\times ]{\varphi }^e+C_b^e\delta f_{ib}^b\end{array}$$

三、位置更新与误差方程

1、位置更新

$$r^e(+)=r^e(-)+\frac{1}{2}[v^e(-)+v^e(+)]\Delta t$$

2、位置误差方程

$$\delta {\dot{r}}^e=\delta v^e$$

易知， $\delta r^e={\tilde{r}}^e-r^e$

四、零偏反馈

前面定义陀螺加计的输出真值
$$\begin{array}{rl}{\omega }_{ib}^b& ={\tilde{\omega }}_{ib}^b-\delta {\omega }_{ib}^b\\ f_{ib}^b& ={\tilde{f}}_{ib}^b-\delta f_{ib}^b\end{array}$$

近似有：

$$\begin{array}{rl}\delta {\omega }_{ib}^b& =d+{\epsilon }_g\\ \delta f_{ib}^b& =b+{\epsilon }_a\end{array}$$

五、状态方程

根据以上内容，可得姿态误差方程：
$${\dot{\phi }}^e=-{\Omega }_{ie}^e{\phi }^e-C_b^e\delta {\omega }_{ib}^e=-{\Omega }_{ie}^e{\phi }^e-C_b^{e}d-C_b^e{\epsilon }_g$$

速度误差方程：
$$\begin{array}{rl}\delta {\dot{v}}^e& =-2{\Omega }_{ie}^e\delta v^e+[(C_b^e{f}_{ib}^b)\times ]{\phi }^e+C_b^e\delta f_{ib}^b\\ & =-2{\Omega }_{ie}^e\delta v^e+[(C_b^e{f}_{ib}^b)\times ]{\phi }^e+C_b^{e}b+C_b^e{\epsilon }_a\end{array}$$

故可得线性化后的状态方程为：
$$\left[\begin{array}{c}\delta {\dot{r}}^e\\ \\ \delta {\dot{v}}^e\\ \\ {\dot{\phi }}^e\\ \\ d\\ \\ b\end{array}\right]=\left[\begin{array}{ccccc}O& I& O& O& O\\ & & & & \\ O& -2{\Omega }_{ie}^e& [(C_b^e{f}_{ib}^b)\times ]& O& C_b^e\\ & & & & \\ O& O& -{\Omega }_{ie}^e& -C_b^e& O\\ & & & & \\ O& O& O& O& O\\ & & & & \\ O& O& O& O& O\end{array}\right]\left[\begin{array}{c}\delta r^e\\ \\ \delta v^e\\ \\ {\phi }^e\\ \\ d\\ \\ b\end{array}\right]+\left[\begin{array}{cc}O& O\\ & \\ O& C_b^e\\ & \\ -C_b^e& O\\ & \\ O& O\\ & \\ O& O\end{array}\right]\left[\begin{array}{c}{\epsilon }_g\\ \\ {\epsilon }_a\end{array}\right]$$

与单点定位进行组合，其中GPS数据只利用解算得到的点位坐标作为量测值进行更新：
$$\left[\begin{array}{c}{r}_{ins}-r_{gps}\end{array}\right]=\left[\begin{array}{ccccc}I& O& O& O& O\end{array}\right]\left[\begin{array}{c}\delta r^e\\ \\ \delta v^e\\ \\ {\phi }^e\\ \\ d\\ \\ b\end{array}\right]+\left[\begin{array}{c}{\epsilon }_r\end{array}\right]$$

---

参考：刘帅. 模糊度固定解PPP/INS紧组合理论与方法[D].解放军信息工程大学,2017.点此进入资源页下载