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）