西南石油大学学报 (自然科学版)  2017, Vol. 39 Issue (2): 163-171

Simulation and Error Analysis for Pipeline Internal Positioning Based on Integrated Navigation
WANG Zegen , TAN Jing, WANG Jinzhu
School of Civil Engineering and Architecture, Southwest Petroleum University, Chengdu, Sichuan 610500, China
Abstract: The separate application of the current pipeline internal positioning methods may fail to satisfy the actual accuracy requirements of pipeline works. Therefore, an effective method of improving the positioning accuracy is to employ the integrated navigation technology. The existing research on the pipeline internal inspection and positioning technology in China and abroad mainly focuses on a multi-theory and multi-method combination. Hence, this study proposes a positioning method based on integrated navigation, which consists of odometers and the Strap-down Inertial Navigation System (SINS), according to the internal positioning characteristics of the oil and gas pipeline. Furthermore, the simulation models of the trajectory simulator, the SINS device, and the odometer based on the Kalman filter data fusion technology are established. In addition, a simulation experiment is conducted, and the error analysis for the integrated positioning technology is performed, thereby achieving a 0.03% positioning accuracy without auxiliary equipment, such as land marks and fixed-point magnetic markers.
Key words: odometer     integrated navigation     Kalman filter     trajectory simulator     simulation model

1 里程仪速度解算及其误差模型

 图1 管道内部导航速度特征简图 Fig. 1 Velocity characteristics diagram of pipeline internal navigation

 ${\boldsymbol{ v}_{\rm n}} = \boldsymbol{ C}_{\rm b}^{\rm n}{\boldsymbol{ v}_{\rm b }}$ (1)

${\boldsymbol{ v}_{\rm b}}$——机体坐标系中里程仪速度，m/s；

$\boldsymbol{ C}_{\rm b}^{\rm n}$——姿态转换矩阵，无因次。

 $\delta {\boldsymbol{ v}_{\rm n}} =-\boldsymbol{ \phi} \times {\boldsymbol{ v}_{\rm n}} + \boldsymbol{ C}_{\rm b}^{\rm n}\delta {\boldsymbol{ v}_{\rm b}}$ (2)

$\delta {\boldsymbol{ v}_{\rm b}}$——机体坐标系中里程仪量测噪声，m/s；

$\boldsymbol{\phi}$——姿态角误差矩阵，无因次。

$\boldsymbol{ \phi}$，$\delta {\boldsymbol{ v}_{\rm b}}$由检测管道的环境以及里程仪的性能参数决定[8]，本文将其考虑成一阶马尔科夫过程，其误差满足$E\left ({\delta {\boldsymbol{ v}_{\rm b}}} \right)= 0$，$E\left ({\delta {\boldsymbol{ v}_{\rm b}}, \delta {\boldsymbol{ v}_{\rm b}}{^{\rm T}}} \right)= {{\boldsymbol{R}}}$的条件（${\boldsymbol{R}}$为正定阵）。

2 组合导航数据融合技术

2.1 数据融合方法

（1）充分利用各子系统的定位信息，获得单个子系统不具备的功能和精度。

（2）各子系统能够优势互补，扩大系统的适用范围。

（3）各子系统观测同一信息源，提高测量值的数量，增加冗余度，提高整个系统的可靠性。

2.2 卡尔曼滤波在组合定位系统中的应用 2.2.1 卡尔曼滤波基本原理

2.2.2 离散型卡尔曼滤波方程

 ${{{\boldsymbol{X}}}_k} = {\boldsymbol{ \varPhi} _{k, k-1}}{{{\boldsymbol{X}}}_{k-1}} + {\boldsymbol{ \varGamma} _{k, k-1}}{{\boldsymbol{W}}_{k-1}}$ (3)

${\boldsymbol{ \varPhi} _{k, k-1}}$——${{k-1}}$时刻至${k}$时刻的转移矩阵；

${\boldsymbol{ \varGamma} _{k, k-1}}$——${{k-1}}$时刻至${k}$时刻的噪声驱动矩阵；%，为系统的激励噪声序列；

${{\boldsymbol{W}}_{k-1}}$——${{k-1}}$时刻的系统激励噪声序列。

 ${{\boldsymbol{Z}}_k} = {{\boldsymbol{H}}_k}{{\boldsymbol{X}}_k} + {{\boldsymbol{V}}_k}$ (4)

${{\boldsymbol{H}}_k}$——${k}$时刻的量测阵；

${{\boldsymbol{V}}_k}$——${k}$时刻的量测噪声。

 $\left\{ \begin{array}{l} E\left({{{{\boldsymbol{W}}}_{k-1}}} \right)= 0\\ E\left({{{{\boldsymbol{V}}}_k}} \right)= 0 \\ E\left({{{{\boldsymbol{W}}}_k}{{{\boldsymbol{W}}}_i}^{\rm T}} \right)= {{{\boldsymbol{Q}}}_k}{\delta _{ki}}\\ E\left({{{{\boldsymbol{V}}}_k}{{{\boldsymbol{V}}}_i}^{\rm T}} \right)= {{{\boldsymbol{R}}}_k}{\delta _{ki}}\\ E\left({{{{\boldsymbol{W}}}_k}{{{\boldsymbol{V}}}_i}^{\rm T}} \right)= 0 \end{array} \right.$ (5)

${{{\boldsymbol{Q}}}_k}$——系统噪声的方差阵，为零阵或正定阵；

${{{\boldsymbol{R}}}_k}$——量测噪声的方差阵，为正定阵；

${{{\boldsymbol{W}}}_i}$——$i$时刻的系统激励噪声序列；

${{{\boldsymbol{V}}}_i}$——$i$时刻的量测噪声，

${\delta _{ki}}$——Kronecker-$\delta$函数，如果$k=i$，则${\delta _{ki}} = 0$；如果$k\neq i$，则${\delta _{ki}} = 0$。

（1）计算状态一步预测值${{{\boldsymbol{X}}}_{k, k-1}}$

 ${{{\boldsymbol{X}}}_{k, k-1}} = {\boldsymbol{ \varPhi} _{k, k-1}}{{{\boldsymbol{X}}}_{k-1}}$

（2）计算一步预测均方误差${{{\boldsymbol{X}}}_k}$：

 ${{{\boldsymbol{P}}}_{k, k - 1}} = {\boldsymbol{ \varPhi} _{k, k - 1}}{{{\boldsymbol{P}}}_{k - 1}}{\boldsymbol{ \varPhi} ^{\rm T}}_{k, k - 1} + {{{\boldsymbol{Q}}}_{k - 1}}$

${{{\boldsymbol{Q}}}_{k-1}}$——$k$时刻的系统噪声方差。

（3）计算滤波增益

 ${{{\boldsymbol{K}}}_k} = {{{\boldsymbol{P}}}_{k, k-1}}{{{\boldsymbol{H}}}_k}^{\rm T}{\left[{{{{\boldsymbol{H}}}_k}{{{\boldsymbol{P}}}_{k, k-1}}{{{\boldsymbol{H}}}_k}^{\rm T} + {{{\boldsymbol{R}}}_k}} \right]^{-1}}$

（4）计算状态估计值：

 ${{{\boldsymbol{X}}}_k} = {\boldsymbol{ \varPhi} _{k, k-1}}{{{\boldsymbol{P}}}_{k-1}}{\boldsymbol{ \varPhi} ^{\rm T}}_{k, k-1} + {{{\boldsymbol{Q}}}_{k-1}}$

（5）计算估计均方误差

 ${{{\boldsymbol{P}}}_k} = \left({{{\boldsymbol{E}}}-{{{\boldsymbol{K}}}_k}{{{\boldsymbol{H}}}_k}} \right){{{\boldsymbol{P}}}_{k, k-1}}$

3 组合定位系统的数学模型 3.1 组合定位系统的设计模式 3.1.1 组合定位滤波状态量的选取

3.1.2 滤波估计的输出校正

 图2 基于输出校正的间接法滤波示意图 Fig. 2 Indirect Filter Schematic Diagram Based on Output Correction
3.2 滤波状态方程

 $\left\{ \begin{array}{l} \mathit{\boldsymbol{\dot X}} = \mathit{\boldsymbol{FX}} + \mathit{\boldsymbol{GW}}\\ \mathit{\boldsymbol{X}} = {\left[ {{\varphi _{\rm{N}}}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\varphi _{\rm{U}}}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\varphi _{\rm{E}}}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \delta {\mathit{\boldsymbol{V}}_{\rm{N}}}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \delta {\mathit{\boldsymbol{V}}_{\rm{U}}}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \delta {\mathit{\boldsymbol{V}}_{\rm{E}}}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \delta \lambda {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \delta L{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \delta h{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\nabla _{\rm{N}}}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\nabla _{\rm{U}}}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\nabla _{\rm{E}}}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\varepsilon _{\rm{N}}}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\varepsilon _{\rm{U}}}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\varepsilon _{\rm{E}}}} \right]^{\rm{T}}} \end{array} \right.$ (6)

$\dot {{\boldsymbol{X}}}$——状态向量微分形式；

$\varphi$——姿态误差，rad；

$V$——速度误差，m/s；

$\lambda$——经度；

$L$——纬度；

$h$——高度，m；

$\nabla$——加速度计误差；

$\varepsilon$——和陀螺仪误差；

λ经度；

L纬度；

h高度，m；

$\nabla$加速度计误差；

ε和陀螺仪误差；

 $\boldsymbol{ F} = \left[{\begin{array}{*{20}{c}} {{\boldsymbol{ F}_{11}}}&{{\boldsymbol{ F}_{12}}}&{{\boldsymbol{ F}_{13}}}&{-\boldsymbol{ C}_b^n}&{{0_{3 \times 3}}}\\ {{\boldsymbol{ F}_{21}}}&{{\boldsymbol{ F}_{22}}}&{{\boldsymbol{ F}_{23}}}&{{0_{3 \times 3}}}&{\boldsymbol{ C}_b^n}\\ {{\boldsymbol{ F}_{31}}}&{{\boldsymbol{ F}_{32}}}&{{\boldsymbol{ F}_{33}}}&{{0_{3 \times 3}}}&{{0_{3 \times 3}}}\\ {{0_{3 \times 3}}}&{{0_{3 \times 3}}}&{{0_{3 \times 3}}}&{{\boldsymbol{ F}_{\rm g}}}&{{0_{3 \times 3}}}\\ {{0_{3 \times 3}}}&{{0_{3 \times 3}}}&{{0_{3 \times 3}}}&{{0_{3 \times 3}}}&{{\boldsymbol{ F}_{\rm a}}} \end{array}} \right]； \boldsymbol{ G} = \left[{\begin{array}{*{20}{c}} {{0_{3 \times 3}}}&{{0_{3 \times 3}}}\\ {{0_{3 \times 3}}}&{{0_{3 \times 3}}}\\ {{0_{3 \times 3}}}&{{0_{3 \times 3}}}\\ {{\boldsymbol{ I}_{3 \times 3}}}&{{0_{3 \times 3}}}\\ {{0_{3 \times 3}}}&{{\boldsymbol{ I}_{3 \times 3}}} \end{array}} \right]；\\ {\boldsymbol{ F}_{11}} = \left[{\begin{array}{*{10}{c}} 0&{{w_{\rm E}}}&{ \!-\! {\Omega _{\rm U}} \!-\! {w_{\rm N}}\tan L}\\ { \!-\! {w_{\rm E}}}&0&{{\Omega _{\rm N}} \!+\! {w_{\rm N}}}\\ {{\Omega _{\rm U}} \!+\! {w_{\rm N}}\tan L}&{ \!-\! {\Omega _{\rm N}} \!-\! {w_{\rm N}}}&0 \end{array}} \right]； {\boldsymbol{ F}_{12}} = \left[{\begin{array}{*{20}{c}} 0&0&1\\ 0&0&{\tan L}\\ {-1}&0&0 \end{array}} \right]；\\ {\boldsymbol{ F}_{13}} = \left[{\begin{array}{*{20}{c}} 0&{-{\Omega _{\rm{U}}}}&{{w_{\rm{N}}}/{R_{\rm 0}}}\\ 0&{{\Omega _{\rm{U}}} + {w_{\rm{N}}}{{\sec }^2}L}&{{w_{\rm{N}}}\tan L/{R_{\rm 0}}}\\ 0&0&{-{w_{\rm{E}}}/{R_{\rm 0}}} \end{array}} \right]； {\boldsymbol{ F}_{21}} = \left[{\begin{array}{*{20}{c}} 0&{-{f_{\rm{E}}}}&{{f_{\rm{U}}}}\\ {{f_{\rm{E}}}}&0&{-{f_{\rm{N}}}}\\ {-{f_{\rm{U}}}}&{{f_{\rm{N}}}}&0 \end{array}} \right]；\\ {\boldsymbol{ F}_{22}} = \left[{\begin{array}{*{20}{c}} {{V_{\rm{U}}}/{R_{\rm 0}}}&{{w_{\rm{E}}}}&{- 2({\Omega _{\rm{U}}} + {w_{\rm{N}}}\tan L)}\\ {- 2{w_{\rm{E}}}}&0&{2({\Omega _{\rm{N}}} \!+\! {w_{\rm{N}}})}\\[4pt] {2{\Omega _{\rm{U}}} \!+\! {w_{\rm{N}}}\tan L}&{ \!-\! 2{\Omega _{\rm{N}}} \!-\! {w_{\rm{N}}}}&{\dfrac{{{V_{\rm{N}}}\tan L \!-\! {V_{\rm{U}}}}}{{{R_{\rm 0}}}}} \end{array}} \right]；\\ {\boldsymbol{ F}_{31}} = \left[{\begin{array}{*{20}{c}} 0&0&0\\ 0&0&0\\ 0&0&0 \end{array}} \right]； {\boldsymbol{ F}_{32}} = \left[{\begin{array}{*{20}{c}} 0&0&{\sec L/{R_{\rm 0}}}\\ {1/{R_{\rm 0}}}&0&0\\ 0&1&0 \end{array}} \right]；\\ {\boldsymbol{ F}_{23}} = \left[{\begin{array}{*{20}{c}} 0&{-(2{\Omega _{\rm N}} + {w_{\rm N}}{{\sec }^2}L){V_{\rm E}}}&{{w_{\rm N}}^2\tan L-{w_{\rm E}}{V_{\rm U}}/{R_{\rm 0}}}\\ 0&{-2{\Omega _{\rm U}}{V_{\rm E}}}&{ - {w_{\rm N}}^2 - {w_{\rm E}}^2}\\ 0&{2{\Omega _{\rm N}}{V_{\rm N}} + 2{\Omega _{\rm U}}{V_{\rm U}} + {w_{\rm N}}{V_{\rm N}}{{\sec }^2}L}&{{w_{\rm N}}{w_{\rm E}}\tan L + {w_{\rm N}}{V_{\rm U}}/{R_{\rm 0}}} \end{array}} \right]；\\ {\boldsymbol{ F}_{33}} = \left[{\begin{array}{*{20}{c}} 0&0&{\sec L/{R_{\rm 0}}}\\ {1/{R_{\rm 0}}}&0&0\\ 0&1&0 \end{array}} \right]；$

${\Omega _{\rm N}} = {w_{\rm ie}}\cos L$；

${\Omega _{\rm U}} = {w_{\rm ie}}\sin L$；

${w_{\rm N}} = {V_{\rm E}}/({R_{\rm 0}} + h)$；

${w_{\rm E}} = {V_{\rm N}}/({R_{\rm N}} + h)$；

$R_{\rm 0}$——地球半径，m；

${\boldsymbol{ F}_{\rm g}} = \left ({-1/{T_{\rm g}}} \right){I_{3 \times 3}}$；

${\boldsymbol{ F}_{\rm a}} = \left ({-1/{T_{\rm a}}} \right){I_{3 \times 3}}$；

$w_{\rm ie}$——地理坐标系相对于地球坐标系的转动角速度，rad/s；

$R_{\rm N}$——卯酉圈曲率半径，m；

${T_{\rm g}}$——陀螺仪相关时间系数；

${T_{\rm a}}$——加速度计相关时间系数；

$\boldsymbol{ I}$——单位矩阵；

$f$——检测器加速度分量，

3.3 滤波量测方程

 ${{\boldsymbol{Z}}} = {{{\boldsymbol{V}}}_{\rm SINS}}-{{{\boldsymbol{V}}}_{\rm n}} = {{\boldsymbol{H}}}{{\boldsymbol{X}}} + \delta {{{\boldsymbol{V}}}_{\rm n}}$ (7)

${{\boldsymbol{H}}}$——观测矩阵；

${{{\boldsymbol{V}}}_{\rm SINS}}$——SINS的速度，m/s。

 $\begin{array}{l} C_{\rm{b}}^{\rm{n}}\left[ \begin{array}{l} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\rm{cos}}\gamma c{\rm{os}}\psi - {\rm{sin}}\gamma {\rm{sin}}\theta {\rm{sin}}\psi {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} - \sin \psi \cos \theta {\kern 1pt} \\ \cos \gamma \sin \psi + \sin \gamma \cos \psi \sin \theta {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \cos \psi \cos \theta \\ {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} - \sin \gamma \cos \theta {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \sin \theta {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \end{array} \right.\\ \left. \begin{array}{l} \sin \gamma \cos \psi + \cos \gamma \sin \psi \sin \theta \\ \sin \gamma \sin \psi - \cos \gamma \cos \psi \sin \theta \\ {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \cos \gamma \cos \theta \end{array} \right] \end{array}$ (8)

$\theta$——俯仰角，rad；

$\gamma$——横滚角，rad。

3.4 卡尔曼滤波方程离散化

 $\left\{ \begin{array}{l} \dot {{\boldsymbol{X}}} = {{\boldsymbol{F}}}{{\boldsymbol{X}}} + {{\boldsymbol{W}}}\\ {{{\boldsymbol{Z}}}} = {{\boldsymbol{V}}}{{\boldsymbol{X}}} + \delta {{{\boldsymbol{V}}}_{\rm n}} \end{array} \right.$ (9)

 $\left\{ \begin{array}{l} {{{\boldsymbol{X}}}_k} = {\boldsymbol{ \varPhi} _{k, k-1}}{{{\boldsymbol{X}}}_{k-1}} + {\boldsymbol{ \varGamma} _{k, k-1}}{{{\boldsymbol{W}}}_{k-1}}\\ {{{\boldsymbol{Z}}}_k} = {{{\boldsymbol{H}}}_k}{{{\boldsymbol{X}}}_k} + \delta {{{\boldsymbol{V}}}_{\rm n}}_k \\ {\boldsymbol{ \varPhi} _{k, k-1}} = E + F\left({{t_k}} \right)\Delta T + \dfrac{1}{2}{\left[{F\left({{t_k}} \right)\cdot \Delta T} \right]^2} +\\{\kern 20pt} \dfrac{1}{{3!}}{\left[{F\left({{t_k}} \right)\cdot \Delta T} \right]^3} +\cdots \\[6pt] \dfrac{\boldsymbol{ \varGamma} _{k, k-1}}{G\left({{t_k}} \right)\cdot \Delta T} = {{\boldsymbol{E}}} + \dfrac{1}{2}\left[{F\left({{t_k}} \right)\cdot \Delta T} \right]+ \\[4pt]{\kern 20pt} \dfrac{1}{{3!}}{{\left[{F\left({{t_k}} \right)\cdot \Delta T} \right]}^2} +... \end{array} \right.$ (10)

$t_k$——$k$时刻对应的时间，s；

$\Delta T$——离散时间步长，s。

 $\left\{ \begin{array}{l} {{\hat {{\boldsymbol{X}}}}_{k, k-1}} = {\boldsymbol{ \varPhi} _{k, k-1}}{{\hat {{\boldsymbol{X}}}}_{k-1}}\\ {{\hat {{\boldsymbol{X}}}}_k} = {{\hat {{\boldsymbol{X}}}}_{k, k-1}} + {{{\boldsymbol{X}}}_k}\left({{{{\boldsymbol{X}}}_k}-{{{\boldsymbol{X}}}_k}{{\hat {{\boldsymbol{X}}}}_{k, k-1}}} \right)\\[4pt] {{{\boldsymbol{K}}}_k} = {{{\boldsymbol{P}}}_{k, k-1}}{{{\boldsymbol{H}}}_k}^{\rm T}{\left({{{{\boldsymbol{H}}}_k}{{{\boldsymbol{P}}}_{k, k-1}}{{{\boldsymbol{H}}}_k}^{\rm T} + {{{\boldsymbol{R}}}_k}} \right)^{-1}}\\[4pt] {{{\boldsymbol{P}}}_{k, k-1}} = {\boldsymbol{ \varPhi} _{k, k-1}}{{{\boldsymbol{P}}}_{k-1}}{\boldsymbol{ \varPhi} ^{\rm T}}_{k, k-1} + {{{\boldsymbol{Q}}}_{k-1}}\\[4pt] {{{\boldsymbol{P}}}_k} = \left({{{\boldsymbol{E}}}-{{{\boldsymbol{K}}}_k}{{{\boldsymbol{H}}}_k}} \right){{{\boldsymbol{P}}}_{k, k-1}} \end{array} \right.$ (11)

${{\hat {{\boldsymbol{X}}}}_{k}}$——状态最优预测值；

${{{\boldsymbol{P}}}_{k, k-1}}$——一步预测均方矩阵；

${{{\boldsymbol{P}}}_k}$——估计均方误差。

4 组合定位系统的仿真设计

4.1 轨迹发生器仿真

 $\left[{\begin{array}{*{20}{c}} {{\omega _{{\rm{b}}x}}}\\ {{\omega _{{\rm{b}}y}}}\\ {{\omega _{{\rm{b}}z}}} \end{array}} \right] = \left[{\begin{array}{*{20}{c}} {\cos \gamma }&0&{\sin \gamma \cos \theta }\\ 0&1&{-\sin \theta }\\ {\sin \gamma }&0&{\cos \gamma \cos \theta } \end{array}} \right]\left[{\begin{array}{*{20}{c}} {\dot \theta }\\ {\dot \gamma }\\ {\dot \psi } \end{array}} \right]$ (12)

$\omega _{{\rm b}y}$——机体坐标系中$y$轴姿态角速率，rad/s；

$\omega _{{\rm b}z}$——机体坐标系中$z$轴姿态角速率，rad/s；

${\dot \theta }$——俯仰角$\theta$的一阶导数，rad/s；

${\dot \gamma }$——俯仰角$\gamma$的一阶导数，rad/s；

${\dot \psi }$——俯仰角$\psi$的一阶导数，rad/s。

 ${a^{\rm{n}}} = {a_{\rm{t}}} - {w_{\rm{t}}} \times {v_{\rm{t}}} = {\boldsymbol{{C}}}_{\rm{e}}^{\rm{t}} \times \left[{\begin{array}{*{20}{c}} {{a_x}}\\ {{a_y}}\\ {{a_z}} \end{array}} \right] - {w_{\rm{t}}} \times {v_{\rm{t}}}$ (13)

${a_{\rm{t}}}$——修正前加速度在地理系中的投影，m/s$^2$；

${v_{\rm{t}}}$——地理坐标系中速度投影值，m/s；

${w_{\rm{t}}}$——地理坐标系相对于地球坐标系的转动角速度，rad/s；

${\boldsymbol{{C}}}_{\rm{e}}^{\rm{t}}$——地球坐标系到地理坐标系的转换矩阵，

 ${\boldsymbol{{C}}}_{\rm{e}}^{\rm{t}} = \left[{\begin{array}{*{20}{c}} {-\sin \lambda }&{\cos \lambda }&0\\ {-\sin L\cos \lambda }&{-\sin L\sin \lambda }&{\cos L}\\ {\cos L\cos \lambda }&{\cos L\sin \lambda }&{\sin L} \end{array}} \right]$

$a_x$——地球坐标系下$x$轴方向的加速度，m/s$^2$；

$a_y$——地球坐标系下$y$轴方向的加速度，m/s$^2$；

$a_z$——地球坐标系下$z$轴方向的加速度，m/s$^2$。

4.2 惯导器件的仿真设计

 ${\omega _{\rm{b}}} = {\bar \omega _{\rm{b}}} + \bar \varepsilon$ (14)

${\bar \omega _{\rm{b}}}$——理想陀螺仪输出量，rad/s；

$\bar \varepsilon$——陀螺仪本身的误差，rad/s。

 ${f_{\rm{b}}} = {\bar f_{\rm{b}}} + {\nabla _{\rm{b}}}$ (15)

${\bar f_{\rm{b}}}$——理想检测器加速度值，m/s$^2$；

${\nabla _{\rm{b}}}$——加速度计本身的误差，m/s$^2$。

4.3 里程仪仿真模型

 ${{{v}}_{\rm{b}}} = {\bar {{{v}}_{\rm{b}}}} + {{ \kappa} _{\rm{b}}}$ (16)

$\bar {{{v}}_{\rm{b}}}$——理想检测器的速度值，m/s；

${{ \kappa} _{\rm{b}}}$——里程仪的速度误差，m/s。

$\bar {{{v}}_{\rm{b}}}$是由惯性器件的仿真模型计算得到检测器加速度计的理想值（${{{\bar f}_{\rm{b}}}}$）后，将加速度计值积分得到检测器的理想速度值，可以通过式（17）得到。把${{ \kappa} _{\rm{b}}}$看作是里程仪的刻度系数误差，根据相关研究成果，将其考虑成一阶马尔科夫过程，如式（18）所示。

 ${\bar v_{\rm{b}}} = \int_0^{{T_{\rm{b}}}} {{{\bar f}_{\rm{b}}}} {\rm d}t$ (17)
 ${\kappa _{\rm{b}}} = - \dfrac{1}{{{T_{\rm{b}}}}}{\kappa _{\rm{b}}} + {w_{\rm{b}}}$ (18)

${w_{\rm b}}$——里程仪的马尔科夫噪声，m/s。

4.4 仿真试验与误差分析 4.4.1 仿真分析条件

4.4.2 试验结果分析

（1）设定组合导航系统解算模型初值以及部分模型参数的取值。

（2）设计并编写轨迹仿真器、捷联惯导器件的仿真程序，用于组合定位系统数据生成。

（3）设计并编写里程仪仿真模型可执行程序，用于组合导航系统里程仪仿真数据生成。

（4）设计并编写卡尔曼滤波组合定位系统的数据融合数学模型的可执行程序，用于仿真滤波数据的生成。

（5）采用上述初值、参数、仿真模型和误差模型输出组合导航系统的误差结果。

 图3 组合系统经度误差曲线图 Fig. 3 Longitude error curve of integrated system
 图4 组合系统纬度误差曲线图 Fig. 4 Latitude error curve of integrated system

5 结语