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$ 表示上述矩阵得到：

$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}$ ，这个计算值与理论值捷联矩阵 $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）

捷联矩阵理想值 $C_b^n$ 计算如下：

$\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）

$R_M,R_N$ 变化很小，视为常值，求偏差得到：

$\\ \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）

4 惯性仪表误差

4.1 陀螺仪误差模型

陀螺仪是敏感载体角运动的元件，它本身就具有误差。陀螺仪的输出

$\hat{w_{ib}^b}=w_{ib}^b+\varepsilon ^b$ （4-1）

$\varepsilon ^b$ 表示陀螺仪所有误差之和。只考虑随时间的漂移误差。假设三个陀螺误差模型相同，陀螺仪漂移是：

$\varepsilon =\varepsilon_b +\varepsilon _r+w_g$ （4-2）

$\varepsilon_b$ 为常值误差， $\varepsilon _r$ 一阶马尔科夫漂移项， $w_g$ 高斯白噪声。误差模型如下：

$\\ \dot{\varepsilon_b}=0\\ \dot{\varepsilon_r}=\frac{1}{\tau _g}\varepsilon_r+w_r$ （4-3）

$w_r$ 为驱动白噪声。 $\tau _g$ 为马尔科夫过程驱动相时间，如下：

$\tau _g= \begin{bmatrix} -\frac{1}{\tau _{gx}} & & \\ & -\frac{1}{\tau _{gy}} & \\ & & -\frac{1}{\tau _{gz}} \end{bmatrix}$ （4-4）

4.2 加速度计误差模型

加速度计主要用于输出运载体的比力，本文只考虑加速度计的主要的误差项，它的误差模型是：

$\hat{f^b}=f^b+\bigtriangledown ^b$ （4-5）

$\bigtriangledown^b$ 为加速度计所有误差之和，一般建模为白噪声项 $w_a$ 及一阶马尔科夫漂移项 $\bigtriangledown_r$ 之和。

$\bigtriangledown ^b=\bigtriangledown _r+w_a$ （4-6）

加速度计误差模型如下：

$\dot{\bigtriangledown_r }= -\frac{1}{\tau _{r}}\cdot \bigtriangledown _r+w_r$ （4-7）

$\tau _r$ 为马尔科夫过程时间，方程如下：

$\frac{1}{\tau _{r}} = \begin{bmatrix} -\frac{1}{\tau _{rx}} & & \\ & -\frac{1}{\tau _{ry}} & \\ & & -\frac{1}{\tau _{rz}} \end{bmatrix}$ （4-8）

$w_{r}$ 为马尔科夫驱动相关时间。

SINS/GNSS组合导航：SINS误差模型相关推荐

基于PSINS工具箱的卡尔曼滤波与SINS/GNSS组合导航
文章目录卡尔曼滤波与SINS/GNSS组合导航典型的SINS/GNSS组合滤波 POS处理卡尔曼滤波与SINS/GNSS组合导航用于Kalman滤波的函数有: psinstypedef(nnm ...
SINS/GNSS组合导航仿真应用详细版(基于PSINS工具箱 )
文章目录轨迹仿真生成轨迹数据绘制仿真数据惯导仿真纯惯导仿真 SINS/GPS组合导航总结 1 2 3 4 5 轨迹仿真生成轨迹数据首先,打开demos\test_trj.m文件,运行仿 ...
SINS/GNSS组合导航：捷联惯导静基座下初始对准（一）粗对准（Matlab）
SINS初始对准要测定系统的姿态变换矩阵,分为粗对准和精对准两个部分,粗短准阶段利用重力和地球自转量粗略计算姿态矩阵:在精对准阶段,不仅依靠前面的姿态矩阵测量值及重力和地球自转量,还要依据惯性器件的输 ...
SINS/NHC组合导航（一）
SINS/NHC组合导航(一) 前言数据 SINS/NHC基本原理数据验证与分析参考论文前言一般在车载SINS/GNSS组合导航中,为了提升GNSS失锁性能,可以采用融合车辆运动学约束条件辅 ...
SINS/DR组合导航(一)
1.1 关于SINS/DR组合导航 SINS/DR组合导航是一种很常见的,也是很传统的一种组合导航方式,这里的DR叫做航位推算,定义是利用姿态.航向和行驶里程信息来推算机器人相对于起始点的相对位置.对 ...
MTI-G-710/GNSS组合导航代码分析
这里写自定义目录标题前言代码分析主函数 sinsgps核心函数 IMU参数辨识结果分析 1.原始数据分析 2.陀螺仪零偏估计 3.加速度计零偏估计 4.自适应卡尔曼滤波 5.组合导航下的方位角 ...
INS/GNSS组合导航（三）松耦合、紧耦合、深度耦合
0 INS/GNSS组合导航概述所谓INS/GNSS组合导航,就是利用INS和GNSS两者信息进行融合,从而综合利用两者的优点,实现优势互补,求解实现pose的解算,得到PVA(位置.速度.姿态)的 ...
INS/GNSS组合导航（七）-SINS的微分方程的推导
(三)中对SINS的机械编排进行了初步可行性的介绍,并未对机械编排进行原理性介绍.那么在详细介绍机械编排之前,需要先对SINS的微分方程进行详细的推导. 无论是机械编排,还是后面误差方程的建立,SIN ...
INS/GNSS组合导航（六）-惯性器件的主要误差
在SINS的各种误差源中,影响最大的是惯性传感器误差,这其中又包括加速度计和陀螺仪的误差.从误差的性质上可以分为系统误差和随机误差,而从误差源上又可以分为零偏误差.尺度因子误差.安装误差.非线性误差以 ...

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

SINS/GNSS组合导航：SINS误差模型相关推荐

最新文章

热门文章