SINS/GNSS组合导航：SINS误差模型

SINS导航精度会受到多种误差所带来的影响，其中主要受到三个方面误差所造成的影响，其一是在系统的组装过程中会受到来自硬件精度，安装误差所带来的影响；其二是捷联解方法造成的算法误差，例如初始值误差，对中误差等等；其三受到外界环境因素所造成的干扰，例如信号干扰等。

若想整体性提高SINS的导航精度，则需要对系统误差进行深入研究找到解决方法。由于受到目前工艺水准的影响，使得其存在难以解决的硬件短板，造成惯性传感器的误差和初始条件中的误差在捷联解算的过程会不断的累积增加，对导航精度造成较大的误差影响。因而，需要对误差算法进行深入了解，在算法上对硬件的不足进行弥补。

0 中间量计算

利用SINS与GPS组合导航时，状态更新量是误差量，有位置误差（经纬高） $[\delta L,\delta \lambda ,\delta h]$ 、速度误差（东北天向速度） $[\delta v_E, \delta v_N, \delta v_U]$ 、姿态角误差 $\phi=[\phi_E,\phi_N, \phi_U]$ 、加表误差 $\bigtriangledown ^b=[\bigtriangledown _x,\bigtriangledown _y,\bigtriangledown _z]$ 与陀螺仪误差 $\varepsilon^b = [\varepsilon _x,\varepsilon _y, \varepsilon _z]$ 。利用这些状态量进行递推更新求解需要用到很多中间量。

1 旋转角速度误差：

地球自转角速度 $w_{ie}^n$ 分解为n系下的矢量，如下：

$w_{ie}^n=\begin{bmatrix} 0 \\ w_{ie}cosL \\ w_{ie}sinL \end{bmatrix}$ （0-1）

导航系旋转角速度 $w_{en}^n$ 如下：

$w_{en}^n=\begin{bmatrix} -\frac{v_N}{R_M+h} \\ \frac{v_E}{R_N+h} \\ \frac{v_{E}tanL}{R_N+h}\end{bmatrix}$ （0-2）

对（0-1）求微分：

$\delta w_{ie}^n=\begin{bmatrix} 0 \\ w_{ie}cosL\cdot \delta L \\ w_{ie}sinL\cdot \delta L \end{bmatrix}=\begin{bmatrix} 0 &0 &0 \\ w_{ie}cosL & 0 & 0\\ w_{ie}sinL & 0 & 0 \end{bmatrix}\cdot \begin{bmatrix} \delta L\\ \delta \lambda \\ \delta h \end{bmatrix}$ （0-3）

用表示上述矩阵得到：

$M_1=\begin{bmatrix} 0 &0 &0 \\ w_{ie}cosL & 0 & 0\\ w_{ie}sinL & 0 & 0 \end{bmatrix}$ （0-4）

对（0-2）求微分：

$\delta w_{en}^n=\begin{bmatrix} -\frac{\delta v_N}{R_M+h} +\frac{ v_N \delta h}{(R_M+h)^2} \\ \frac{\delta v_E}{R_N+h} -\frac{ v_E \delta h}{(R_N+h)^2} \\ \frac{tanL \delta v_E}{R_N+h}+\frac{v_Esec^2L\delta L}{R_N+h} -\frac{v_EtanL\delta h}{(R_N+h)^2} \end{bmatrix}$ （0-5）

可以写成：

$\delta w_{en}^n=\begin{bmatrix} 0 & -\frac{1}{R_M+h} & 0 \\ \frac{1}{R_N+h} & 0 & 0\\ \frac{tanL}{R_N +h} & 0 & 0 \end{bmatrix}\cdot \delta v^n+\begin{bmatrix} 0& 0& \frac{v_N}{(R_M+h)^2}\\ 0& 0 & \frac{-v_E}{(R_N+h)^2}\\ \frac{v_E sec^2L}{R_N+h}& 0 & -\frac{v_EtanL}{(R_N+h)^2} \end{bmatrix}\cdot \delta p^n$ （0-6）

用M表示上述矩阵：

$M_2 =\begin{bmatrix} 0 & -\frac{1}{R_M+h} & 0 \\ \frac{1}{R_N+h} & 0 & 0\\ \frac{tanL}{R_N +h} & 0 & 0 \end{bmatrix}, M_3=\begin{bmatrix} 0& 0& \frac{v_N}{(R_M+h)^2}\\ 0& 0 & \frac{-v_E}{(R_N+h)^2}\\ \frac{v_E sec^2L}{R_N+h}& 0 & -\frac{v_EtanL}{(R_N+h)^2} \end{bmatrix}$ （0-7）

2 重力矢量误差

重力公式如下：

$g = g_e(1+\beta sin^2L-\beta_1 sin^2 2L)-\beta _2 h$ （0-8）

求偏差：

$\delta g = g_e(2\beta sinL cosL \delta L-4\beta_1 sin 2L cos 2L \delta L)-\beta _2 \delta h$ （0-9）

整理得：

$\delta g = g_esin 2L(\beta-4\beta_1 cos 2L )\delta L-\beta _2 \delta h$ （0-10）

重力矢量误差转到导航座标系有：

$\delta g^n=\begin{bmatrix} 0 \\ 0 \\ -g \end{bmatrix}=\begin{bmatrix} 0& 0 &0 \\ 0& 0& 0\\ g_esin 2L(\beta-4\beta_1 cos 2L ) & 0& \beta _2 \end{bmatrix}\delta p$ （0-11）

计：

$M_4=\begin{bmatrix} 0& 0 &0 \\ 0& 0& 0\\ g_esin 2L(\beta-4\beta_1 cos 2L ) & 0& \beta _2 \end{bmatrix}$ （0-12）

1 姿态误差模型

导航计算机给出的捷联矩阵 $\widetilde{C_b^n}$ ，这个计算值与理论值捷联矩阵一定存在偏差，前者为b系至导航系n的计算值，后者为b系至导航系的理想值，将前者的n系视为 $n^{'}$ 系，捷联矩阵计算值为： $C_b^n{'}$ ，有如下关系：

$C_b^n{'} = C_n^n{'} C_b^n$ （1-1）

记n系至 $n^{'}$ 系之间的旋转矢量 $\phi _{nn^{'}}$ ， $\phi _{nn^{'}}$ 为小量，根据等效旋转矢量与方向余弦阵关系得到：

$C_{n^{'}}^n\approx I+\phi _{nn^{'} }\times$ （1-2）

转置有：

$C_{n}^n^{'}=(C_{n^{'}}^n)^T\approx I-\phi _{nn^{'} }\times$ （1-3）

则有：

$C_b^n^{'}=( I-\phi _{nn^{'} }\times) C_b^n$ （1-4）

捷联矩阵理想值计算如下：

$\dot{C_b^n}=C_b^n^{}(w_{ib}^b\times )-(w_{in}^n\times )C_b^n$ （1-5）

实际计算存在误差，表示如下：

$\dot{C_b^n^{'}}=C_b^n^{'}(\widetilde{w_{ib}^b}\times )-(\widetilde{w_{in}^n}\times )C_b^n^{'}$ （1-6）

其中：

$\\ \dot{w_{ib}^b}=w_{ib}^b+\delta w_{ib}^b\\ \widetilde{w_{in}^n}=w_{in}^n+\delta w_{in}^n$ （1-7）

$\delta w_{ib}^b$ 为陀螺误差， $\delta w_{in}^n$ 为导航系计算误差。

（1-4）两边求导数并等于（1-6）右端， $\phi _{nn^{'} }$ 用 $\phi$ 表示得到：

$(\dot{-\phi }\times) C_b^n+(I-\phi\times )\dot{C_b^n}=C_b^n^{'}(\widetilde{w_{ib}^b}\times )-(\widetilde{w_{in}^n\times} )C_b^n^{'}$ （1-8）

展开略去二次小量得到：

$(\dot{\phi }\times)=[(\phi\times )(w_{in}^n\times)-(w_{in}^n\times)(\phi\times )]+(\delta w_{in}^n\times )-C_b^n(\delta w_{ib}^b\times )C_n^b$ （1-9）

右边第一项根据 $(V_1\times) (V_2\times) -(V_2\times) (V_1\times)=[(V_1\times V_2)\times]$ 得到：

$(\dot{\phi }\times)=[(\phi\times w_{in}^n)\times ]+(\delta w_{in}^n\times )-(\delta w_{ib}^b\times )=[((\phi\times w_{in}^n)+(\delta w_{in}^n)-(\delta w_{ib}^b))\times ]$ （1-10）

得到：

$\dot{\phi }=(\phi\times w_{in}^n)+(\delta w_{in}^n)-(\delta w_{ib}^n)$ （1-11）

至此得到了姿态角误差模型，表示了n系至 $n^{'}$ 系之间失准角变化规律。

可以写成：

$\dot{\phi }=(\phi\times w_{in}^n)+(\delta w_{ie}^n+\delta w_{en}^n)-(C_b^n\delta w_{ib}^b)$ （1-12）

代入（0-3）（0-5）得到：

$\dot{\phi }=(-w_{in}^n\times\phi )+(M_1\delta p + M_2 \delta v_N + M_3 \delta p)-(C_b^n \varepsilon ^b)$ （1-13）

整理如下：

$\dot{\phi }=M_{aa}\phi +M_{av} \delta v^n + M_{ap}\delta p -C_b^n \varepsilon ^b$ （1-14）

其中， $M_{aa}=(-w_{in}^n\times),M_{av}=M_2,M_{ap}=M_1+M_3$ 。至此得到了姿态角的误差方程。

2 速度误差：

速度误差模型描述了导航计算机计算的速度与理想速度之间的误差，同位置误差模型，n系与 $n^{'}$ 的区别。导航计算机计算的速度表示为 $\widetilde{v}_{en^{'}}^n^{'}$ ，记为 $\widetilde{v}$ ，误差公式如下：

$\delta v^n = \widetilde{v}^{n^{'}}-v^n$ （2-1）

求微分得到：

$\delta \dot{v}^n = \dot{\widetilde{{v}}}^{n^{'}}-\dot{v}^n$ （2-2）

比例方程如下：

$\dot{{v}}^n={C}_b^n{f}_{sf}^b-(2{w}_{ie}^n+{w}_{en}^n)\times {v}^n+{g}^n$ （2-3）

实际导航计算机计算如下：

$\dot{\widetilde{v}}^n=\widetilde{C}_b^n\widetilde{f}_{sf}^b-(2\widetilde{w}_{ie}^n+\widetilde{w}_{en}^n)\times \widetilde{v}^n+\widetilde{g}^n$ （2-4）

其中，

$\\ \widetilde{f}_{sf}^b=f_{sf}^b+\delta f_{sf}^b\\ \widetilde{w}_{ie}^n=w_{ie}^n+\delta w_{ie}^n\\ \widetilde{w}_{en}^n=w_{en}^n+\delta w_{en}^n\\ \widetilde{g}^n=g^n+\delta g^n$ （2-5）

$\delta f_{sf}^b$ 为加速度计测量误差， $\delta w_{ie}^n$ 为地球自转角速度误差（0-3）， $\delta w_{en}^n$ 为导航系旋转计算误差（0-5）， $\delta g$ 为重力误差。

根据（2-2），用式（2-4）减去（2-3）得到：

$\delta \dot{v}^n =(\widetilde{C}_b^n\widetilde{f}_{sf}^b-{C}_b^n{f}_{sf}^b)-((2\widetilde{w}_{ie}^n+\widetilde{w}_{en}^n)\times \widetilde{v}^n - (2{w}_{ie}^n+{w}_{en}^n)\times {v}^n)+\widetilde{g}^n -{g}^n$ （2-6）

代入（1-4）及（2-5）得到速度误差模型：

$\delta \dot{v}^n =f_{sf}^n\times\phi+v^n\times(2\delta w_{ie}^n+\delta w_{en}^n)-(2\delta w_{ie}^n+\delta w_{en}^n)\times v^n+\delta f_{sf}^n+\delta g^n$ （2-7）

代入（0-3）（0-5）得到：

$\delta \dot{v}^n =M_{va}\phi+M_{vv}\delta v^n+M_{vp}\delta p^n+C_b^n\bigtriangledown ^b$ （2-8）

其中 $M_{va}=(f_{sf}^n\times), M_{vv}=(v^n\times)M_2-(2w_{ie}^n+w_{en}^n)\times$ , $M_{vp}=(v^n\times)(2M_1+M_3)+M_4$ 。

3 位置误差

经纬高的导数如下：

$\begin{bmatrix} \dot{L} \\ \dot{\lambda} \\ \dot{h} \end{bmatrix} =\begin{bmatrix} \frac{v_N^{n}}{R_M+h} \\ \frac{v_E^n}{(R_N+h)cosL} \\ v_U^n \end{bmatrix}$ （3-1）

变化很小，视为常值，求偏差得到：

$\\ \delta \dot{L}=\frac{1}{R_M+h}\delta v_N-\frac{v_N}{(R_M+h)^2}\delta h\\ \delta \dot \lambda = \frac{secL}{R_N+h}\delta v_E+\frac{v_E secL tanL}{R_N+h}\delta L -\frac{v_E secL}{(R_N+h)^2}\delta h\\ \delta \dot h = \delta v_U$ （3-2）

即：

$\delta \dot{p} = \begin{bmatrix} 0 & \frac{1}{R_M+h} & 0 \\ \frac{secL}{R_N + h} & 0 & 0\\ 0 & 0 & 1 \end{bmatrix} \delta v +\begin{bmatrix} 0 & 0 & \frac{v_N}{(R_M+h)^2}\\ \frac{v_E secL tanL}{R_N +h}& 0& -\frac{v_EsecL}{(R_N + h)^2}\\ 0& 0 & 0 \end{bmatrix}\delta p$ （3-3）

计：

$M_{pv} = \begin{bmatrix} 0 & \frac{1}{R_M+h} & 0 \\ \frac{secL}{R_N + h} & 0 & 0\\ 0 & 0 & 1 \end{bmatrix},M_{pp}=\begin{bmatrix} 0 & 0 & \frac{v_N}{(R_M+h)^2}\\ \frac{v_E secL tanL}{R_N +h}& 0& -\frac{v_EsecL}{(R_N + h)^2}\\ 0& 0 & 0 \end{bmatrix}$ （3-4）