CN103542851A - 一种基于水下地形高程数据库的水下航行器辅助导航定位方法 - Google Patents

Abstract

本发明公开了一种基于水下地形高程数据库的水下航行器辅助导航定位方法，其主要目的在于解决水下航行器长时间航行时由于惯性导航定位系统的误差会随着时间的积累逐渐增大的固有缺陷以至于不能满足航行器长时间精确导航定位需要的问题。本发明的主要步骤包括：主惯导系统指示航迹序列和实测高程序列的获取、匹配定位预处理、一级匹配定位、二级匹配定位以及修正过程。本发明可以解决水下航行器长时间航行所带来的初始位置误差过大致使匹配失败问题以及匹配过程中由于惯性器件的误差所引起的匹配误差问题，从而修正主惯导系统，提高导航定位精度。

Description

一种基于水下地形高程数据库的水下航行器辅助导航定位方法

技术领域

本发明涉及水下导航技术领域，具体地说是设计一种能够满足水下潜器长时间航行要求的导航方法。

背景技术

水下航行器在未来的军用和民用领域都日显其重要性，因而成为了世界军事和科技强国重点发展的领域。水下航行器要求具备高度的隐蔽性，能保持长时间的精确航行，而实现这一要求的关键技术之一是其导航技术。水下航行器的导航方式有别于陆路，GPS、无线电、天文导航等导航方式不能满足水下航行器隐蔽性的要求。惯性导航因其具有自主性、隐蔽性等优点，成为了水下潜器导航手段的首选。但惯性导航有固有的缺陷，惯性导航定位系统的误差会随着时间的积累逐渐增大以至于发散，因而在惯性导航定位系统运行一段时间后必须采用外来信息对惯性导航定位系统进行修正。随着导航技术特别是无源导航技术的发展，应用地形、重力、地磁等信息来辅助惯性导航为水下器导航研究的提供了一种新途径。由于地形高程信息测量较为方便，且陆路上的地形匹配技术已应用于实际，可为水下地形匹配导航提供借鉴，因而应用地形高程信息来辅助水下航行器的导航具备可行性。

发明内容

发明目的：本发明提供一种基于水下地形高程数据库的能够实现长时间高精度导航定位的水下航行器辅助导航定位方法。

本发明的技术方案具体如下：

一种基于水下地形高程数据库的水下航行器辅助导航定位方法，其特征为：

步骤1：获取主惯导系统指示航迹序列和实测高程序列

当水下航行器驶入匹配区时，在主惯性导航系统INS的导航下航行一段距离，与此同时每隔一个时间段Times，Times的典型值为10分钟，通过航行器的高程测量装置测量高程，由此得到惯性导航系统给出的一个惯导系统指示航迹序列 $P_i=\left(\begin{array}{c}{\mathrm{Lati}}_{P_i}\\ {\mathrm{Longti}}_{P_i}\end{array}\right),$ i=1，2…N，N代表序列点的个数，取值为20，代表惯导系统指示航迹序列P_i的纬度，

代表惯导系统指示航迹序列P_i的经度，和一个由高程测量装置给出的对应于惯导系统指示航迹序列P_i的实测高程序列D_i，i=1，2…N，考虑高程测量装置的误差可得：

$D_i={\tilde{D}}_i+\mathrm{\γ},i=\mathrm{1,2}...N$

其中

表示理想高程序列，γ表示高程测量装置的测量噪声序列，服从均值为0标准差为1的正态分布。

步骤2：匹配定位预处理

在加载地形高程数据库中，选择以惯导系统指示航迹序列P_i为中心、以惯性导航系统6倍误差为边长的区域，将此区域的高程数据存入一个预处理二维矩阵，预处理二维矩阵行标为row，1≤row≤row_max，row_max为所述区域行标的最大值，预处理二维矩阵列标为col，1≤col≤col_max，col_max为所述区域列标的最大值，矩阵中的值存为储高程数据；所述区域的起始纬度为lati₀，起始经度为longti₀，矩阵行列交点row，col处的经度、纬度分别为：lati₀+(row-1)×grid，longti₀+(col-1)×grid，grid为地形数据库中相邻高程数据的位置间距，考虑地形高程数据库建库时测量和量化所产生的误差可得：

$\mathrm{DB}(\mathrm{row},\mathrm{col})=\widetilde{\mathrm{DB}}(\mathrm{row},\mathrm{col})+\mathrm{\η}$

其中row为预处理二维矩阵的行标，col为预处理二维矩阵的列标，DB(row，col)表示预处理二维矩阵中行标为row、列标为col的高程数据，

表示对应于DB(row，col)的理想高程数据，η表示地形高程数据库建库时的测量和量化噪声，服从均值为0标准差为1的正态分布；

将实测高程序列D_i和在DB(row，col)中所提取的高程序列作相关运算，所述的相关运算采用均方差MSD算法，使用去均值后的数据进行相关运算：

$\mathrm{Ral}(\mathrm{row},\mathrm{col})=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{[\mathrm{DB}(\mathrm{row},\mathrm{col})-\mathrm{DBA}(\mathrm{row},\mathrm{col})-D_i+\mathrm{DA}]}^2$

其中，Ral(row，col)表示MSD相关运算的结果，DB(row，col)表示惯导系统指示航迹P_i平移至预处理二维矩阵row，col对应位置lati₀+(row-1)×grid，longti₀+(col-1)×grid处高程数据序列，DBA(row，col)为DB(row，col)中所提取的高程序列的均值，D_i表示实测高程序列，DA表示实测序列的均值；

运算结束后，取得使Ral(row，col)最小的row，col，则将惯导系统指示航迹P_i平移至预处理二维矩阵row，col对应位置lati₀+(row-1)×grid，longti₀+(col-1)×grid，得到预处理位置序列为 $P_i^{\′}=\left(\begin{array}{c}{\mathrm{Lati}}_{P_i^{\′}}\\ {\mathrm{Longti}}_{P_i^{\′}}\end{array}\right),$

表示得到预处理位置序列P′_i的纬度，

表示得到预处理位置序列P′_i的经度；

步骤3：一级匹配定位

将地形高程数据库中以预处理位置序列P′_i为中心，以惯性导航系统3倍误差为边长的一级匹配区域内的高程数据存入一个一级匹配二维矩阵，并利用双线性插值算法在该一级匹配区域提取高程为实测高程D_i的一级等高线C_i；

步骤3.1：

惯导系统给出的航向误差为ψ_INS，纬度和经度误差分别为t_INSlati和t_INSlongti，分别在-3ψ_INS～3ψ_INS，-3t_INSlati～3t_INSlati，-3t_INSlongti～3t_INSlongti范围内分别随机取一个航向误差、一个纬度和一个经度且值分别为：ψ_INSrand，t_INSlatirand，t_{INSlongtirand}；对预处理位置序列P′_i作一级随机旋转和一级随机平移变换，得到一级随机旋转和平移位置序列P′_irand：

$P_{\mathrm{irand}}^{\′}=\left(\begin{array}{cc}\cos {\mathrm{\ψ}}_{\mathrm{INSrand}}& -\sin {\mathrm{\ψ}}_{\mathrm{INSrand}}\\ \sin {\mathrm{\ψ}}_{\mathrm{INSrand}}& \cos {\mathrm{\ψ}}_{\mathrm{INSrand}}\end{array}\right)P_i^{\′}+\left(\begin{array}{c}{t}_{\mathrm{INSlatirand}}\\ t_{\mathrm{INSlongtirand}}\end{array}\right)$

步骤3.2：

通过一级随机旋转和平移位置序列P′_irand向一级等高线C_i作垂线得到一个一级匹配垂足序列Y，求取一个刚性变换Trans且所述刚性变换Trans包括旋转矩阵

$\mathrm{Rotation}=\left(\begin{array}{cc}\cos {\mathrm{\φ}}_{\mathrm{ICCP}}& -\sin {\mathrm{\φ}}_{\mathrm{ICCP}}\\ \sin {\mathrm{\φ}}_{\mathrm{ICCP}}& \cos {\mathrm{\φ}}_{\mathrm{ICCP}}\end{array}\right)$ 和平移矢量 $\mathrm{translation}=\left(\begin{array}{c}{t}_{\mathrm{ICCPLati}}\\ t_{\mathrm{ICCPLongti}}\end{array}\right),$ φ_ICCP为一级匹配旋转角，t_ICCPLati代表纬度偏移量，t_ICCPLongti代表经度偏移量，使得一级随机旋转和平移位置序列P′_irand逐渐逼近一级匹配垂足序列Y_i，也就是使以下目标函数最小：

$\mathrm{Objec}{t}_{\mathrm{itetation}}(Y_i,\mathrm{Trans}\·P_{\mathrm{irand}}^{\′})=\left\{\begin{array}{cc}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{weight}}_i{\left|\right|Y_i-\mathrm{Trans}\·P_{\mathrm{irand}}^{\′}\left|\right|}^2& \mathrm{iteration}\≥1\\ 0& \mathrm{iteration}=0\end{array}\right.$

${\mathrm{weight}}_i=1-\frac{\mathrm{Dist}(P_{\mathrm{irand}}^{\′},Y_i)}{{\mathrm{Dist}}_{\max }}$

其中Object为目标函数，iteration代表第iteration次迭代，iteration=0表示迭代未开始，weighti代表序列点的权值，Trans为刚性变换，Dist(P′_irand，Y_i)为P′_irand和Y_i之间的距离，Dist_max为P′_irand和Y_i序列点之间距离的最大值；

求取刚性变换Trans采用四元数法；

设四元数为quaternion：

$\mathrm{quaternion}=({\mathrm{qua}}_1,{\mathrm{qua}}_2,{\mathrm{qua}}_3,{\mathrm{qua}}_4)=(\cos \frac{{\mathrm{\φ}}_{\mathrm{ICCP}}}{2},\mathrm{0,0},\sin \frac{{\mathrm{\φ}}_{\mathrm{ICCP}}}{2})$

qua₁为四元数quaternion的第一个元素，qua₂为四元数quaternion的第二个元素，qua₃为四元数quaternion的第三个元素，qua₄为四元数quaternion的第四个元素，φ_ICCP为一级匹配旋转角；

构造矩阵MatICCp：

${\mathrm{Mat}}_{\mathrm{ICCP}}=\left[\begin{array}{cc}{\mathrm{Mat}}_{11}& {\mathrm{Mat}}_{12}\\ {\mathrm{Mat}}_{21}& {\mathrm{Mat}}_{22}\end{array}\right]=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{weight}}_i(Y_i-{\stackrel{-}{Y}}_i){(P_{\mathrm{irand}}^{\′}-{\stackrel{-}{P}}_{\mathrm{irand}}^{\′})}^T$

Mat₁₁为矩阵Mat_ICCP第一行第一列的元素，Mat₁₂为矩阵Mat_ICCP第一行第二列的元素，Mat₂₁为矩阵Mat_ICCP第二行第一列的元素，Mat₂₂为矩阵Mat_ICCP第二行第二列的元素，和

分别为一级匹配垂足序列的质心、随机旋转和平移位置序列的质心，weight_i代表序列点的权值，T代表转置；

为求解旋转矩阵Rotation而构造一个对称的哈密尔顿矩阵：

$\mathrm{Hamilton}=\left(\begin{array}{cccc}{\mathrm{Mat}}_{11}+{\mathrm{Mat}}_{22}& 0& 0& {\mathrm{Mat}}_{21}-{\mathrm{Mat}}_{12}\\ 0& {\mathrm{Mat}}_{11}-{\mathrm{Mat}}_{22}& {\mathrm{mat}}_{12}+{\mathrm{Mat}}_{21}& 0\\ 0& {\mathrm{Mat}}_{12}+{\mathrm{Mat}}_{21}& {\mathrm{Mat}}_{22}-{\mathrm{Mat}}_{11}& 0\\ {\mathrm{Mat}}_{21}-{\mathrm{Mat}}_{12}& 0& 0& {-\mathrm{Mat}}_{11}-{\mathrm{Mat}}_{22}\end{array}\right)$

求解并得到哈密尔顿矩阵Hamilton的四个特征值

记最大的特征值为

最大的特征值为

可使下式成立：

由此可得一级匹配旋转角为：

所以旋转矩阵Rotation为：

$\mathrm{Rotation}=\left(\begin{array}{cc}\cos {\mathrm{\φ}}_{\mathrm{ICCP}}& -\sin {\mathrm{\φ}}_{\mathrm{ICCP}}\\ \sin {\mathrm{\φ}}_{\mathrm{ICCP}}& \cos {\mathrm{\φ}}_{\mathrm{ICCP}}\end{array}\right)$

平移量translation为：

$\mathrm{translation}={\stackrel{-}{Y}}_i-\mathrm{Rotation}\·{\stackrel{-}{P}}_{\mathrm{irand}}^{\′}$

步骤3.3：

利用步骤3.2得到的包括旋转矩阵Rotation和平移矢量translation的刚性变换Trans，对-级随机旋转和平移后的位置序列P′_irand作P′_irand=Trans·P′_irand处理，此时，若迭代次数iteration大于最大迭代次数iteration_max，表明收敛速度过慢，舍弃此次迭代的结果，返回步骤3.1；若iteration小于等于iteration_max并且

|Ogject_iteration-Ogject_iteration-1|＞τ，则返回步骤3.2，若iteration小于等于iteration_max并且|Object_iteration-Object_iteration-1|≤τ，则迭代终止并记录Object_iteration和Trans·P′_irand的值，iteration_max值为100，τ值为10^-6；

步骤3.4：

重复3.1-3.3步骤，得到50个Object_iteration 值后，找出其中最小的Object_itetation

对应的

得到一级匹配位置序列 $P_i^{\′\′}=\mathrm{Trans}\·P_{\mathrm{irand}}^{\′}=\left(\begin{array}{c}{\mathrm{Lati}}_{P^{\′\′}}\\ {\mathrm{Longti}}_{P^{\′\′}}\end{array}\right),$

代表一级匹配位置序列的纬度，

代表一级匹配序列的经度；

步骤4：二级匹配定位

选择以加载地形高程数据库中以一级匹配位置序列P″_i为中心、3×grid为边长的区域，将此区域的高程数据存入一个二级匹配二维矩阵，并利用双线性插值算法在该区域提取高程为实测高程D_i的二级等高线C′_i，在二级等高线C′_i上寻找一级匹配位置序列P″_i的对应垂足得到一个二级垂足序列 $Y_i^{\′}=\left\{\begin{array}{c}{\mathrm{Lati}}_{Y_{i^{\′}}}\\ {\mathrm{Longti}}_{Y_{i^{\′}}}\end{array}\right.,$

代表二级垂足序列的纬度，

代表二级垂足序列的经度；对二级垂足序列Y′_i到一级匹配位置序列P″_i作仿射变换G且所述仿射变换包括在纬度方向上的平移因子t_affine，在经度方向上的平移因子t_affine，缩放因子α_afine’旋转因子θ_affine，e_i为二级垂足序列Y′_i与二级匹配定位的输入序列P″_i的经仿射变换后的坐标的差：

$\begin{array}{c}{e}_i=\left(\begin{array}{c}{t}_{\mathrm{affinex}}\\ t_{\mathrm{affiney}}\end{array}\right)+\left(\begin{array}{cc}{\mathrm{\α}}_{\mathrm{affine}}\cos {\mathrm{\θ}}_{\mathrm{affine}}& -{\mathrm{\α}}_{\mathrm{affine}}\sin {\mathrm{\θ}}_{\mathrm{affine}}\\ {\mathrm{\α}}_{\mathrm{affine}}\sin {\mathrm{\θ}}_{\mathrm{affine}}& {\mathrm{\α}}_{\mathrm{affine}}\cos {\mathrm{\θ}}_{\mathrm{affine}}\end{array}\right)P_i^{\text{'}\text{'}}-Y_i^{\text{'}}\\ =\left(\begin{array}{cccc}1& 0& {\mathrm{Lati}}_{P_i^{\text{'}\text{'}}}& -{\mathrm{Longti}}_{P_i^{\text{'}\text{'}}}\\ 0& 1& {\mathrm{Longti}}_{P_i^{\text{'}\text{'}}}& {\mathrm{Lati}}_{P_i^{\text{'}\text{'}}}\end{array}\right)\left(\begin{array}{c}{t}_{\mathrm{affinex}}\\ t_{\mathrm{affiney}}\\ {\mathrm{\α}}_{\mathrm{affine}}\cos {\mathrm{\θ}}_{\mathrm{affine}}\\ {\mathrm{\α}}_{\mathrm{affine}}\sin {\mathrm{\θ}}_{\mathrm{affine}}\end{array}\right)-\left(\begin{array}{c}{\mathrm{Lati}}_{Y_i^{\text{'}}}\\ {\mathrm{Longti}}_{Y_i^{\text{'}}}\end{array}\right)\end{array}$

令仿射变换求解向量为t_affine，仿射变换求解矩阵为

$t_{\mathrm{affine}}=\left(\begin{array}{c}{t}_{\mathrm{affinex}}\\ t_{\mathrm{affiney}}\\ {\mathrm{\α}}_{\mathrm{affine}}\cos {\mathrm{\θ}}_{\mathrm{affine}}\\ {\mathrm{\α}}_{\mathrm{affine}}\sin {\mathrm{\θ}}_{\mathrm{affine}}\end{array}\right)$

$C_{P_i^{\′\′}}=\left(\begin{array}{cccc}1& 0& {\mathrm{Lati}}_{P_i^{\′\′}}& -{\mathrm{Longti}}_{P_i^{\′\′}}\\ 0& 1& {\mathrm{Longti}}_{P_i^{\′\′}}& {\mathrm{Lati}}_{P_i^{\′\′}}\end{array}\right)$

则 $e_i=C_{P_i^{\′\′}}{t}_{\mathrm{affine}}-Y_i^{\′};$

欧式平方距离ESD_affine为：

${\mathrm{ESD}}_{\mathrm{affine}}=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{e_i}^T{e}_i={t_{\mathrm{affine}}}^T{C_{P_i^{\′\′}}}^T{C}_{P_i^{\′\′}}{t}_{\mathrm{affine}}-{2Y}_i^{\′T}{C}_{P_i^{\′\′}}{t}_{\mathrm{affine}}+Y_i^{\′T}{Y}_i^{\′}$

T代表转置；

将使ESD_affine最小的t_affine为

对ESD_affine求导可得：

$\frac{{\mathrm{dESD}}_{\mathrm{affine}}}{{\mathrm{dt}}_{\mathrm{affine}}}={{2C}_{P_i^{\′\′}}}^T{C}_{P_i^{\′\′}}{t}_{\mathrm{affine}}-{{2C}_{P_i^{\′\′}}}^T{Y}_i^{\′}$

令 $\frac{{\mathrm{dESD}}_{\mathrm{affine}}}{d{t}_{\mathrm{affine}}}=0$ 可得

${\hat{t}}_{\mathrm{affine}}={\left({C_{P_i^{\text{'}\text{'}}}}^T{C}_{P_i^{\text{'}\text{'}}}\right)}^{-1}{C_{P_i^{\text{'}\text{'}}}}^T{Y}_i^{\text{'}}$

当ESD_affine对t_affine的二阶导数

时，

就是使ESD_affine最小的解：

$\begin{array}{c}{\hat{t}}_{\mathrm{affine}}={\left({C_{P_i^{\′\′}}}^T{C}_{P_i^{\′\′}}\right)}^{-1}{C_{P_i^{\′\′}}}^T{Y}_i^{\′}\\ =\frac{1}{\det }\left(\begin{array}{cccc}{l}_p& 0& -{\mathrm{\μ}}_{\mathrm{xp}}& {\mathrm{\μ}}_{\mathrm{yp}}\\ 0& l_p& {-\mathrm{\μ}}_{\mathrm{yp}}& {-\mathrm{\μ}}_{\mathrm{xp}}\\ {-\mathrm{\μ}}_{\mathrm{xp}}& {-\mathrm{\μ}}_{\mathrm{yp}}& N& 0\\ {\mathrm{\μ}}_{\mathrm{yp}}& {-\mathrm{\μ}}_{\mathrm{xp}}& 0& N\end{array}\right)\left(\begin{array}{c}{\mathrm{\μ}}_{\mathrm{xq}}\\ {\mathrm{\μ}}_{\mathrm{yq}}\\ l_{p+q}\\ l_{p-q}\end{array}\right)\end{array}$

其中：

代表一级匹配位置序列的纬度的之和，

代表二级垂足序列的纬度之和，

代表一级匹配位置序列的经度的之和，

代表二级垂足序列的经度之和，

代表一级匹配位置序列和二级垂足序列的点积之和，

代表一级匹配位置序列和二级垂足序列的叉积之和，

代表一级匹配位置序列的模的平方之和， $\det =N\×l_p-{\mathrm{\μ}}_{\mathrm{xp}}^2-{\mathrm{\μ}}_{\mathrm{yp}}^2$ 代表矩阵

的模；

由此就可以得到：

${\hat{t}}_{\mathrm{affine}}={({\hat{t}}_{\mathrm{affinex}},{\hat{t}}_{\mathrm{affiney}},{\widehat{\mathrm{\α}}}_{\mathrm{affine}}\cos {\widehat{\mathrm{\θ}}}_{\mathrm{affine}},{\widehat{\mathrm{\α}}}_{\mathrm{affine}}\sin {\widehat{\mathrm{\θ}}}_{\mathrm{affine}})}^T$

其中

为求解所得的平移因子，

为求解所得的缩放因子，为求解所得的旋转因子；

对一级匹配位置序列作仿射变换得二级匹配位置序列P″′_i：

$P_i^{\text{'}\text{'}\text{'}}=\left(\begin{array}{c}{\mathrm{Lati}}_{P_i^{\text{'}\text{'}\text{'}}}\\ {\mathrm{Longti}}_{P_i^{\text{'}\text{'}\text{'}}}\end{array}\right)=\left(\begin{array}{c}{\hat{t}}_{\mathrm{affinex}}\\ {\hat{t}}_{\mathrm{affiney}}\end{array}\right)+\left(\begin{array}{cc}{\widehat{\mathrm{\α}}}_{\mathrm{affine}}\cos {\widehat{\mathrm{\θ}}}_{\mathrm{affine}}& -{\widehat{\mathrm{\α}}}_{\mathrm{affine}}\sin {\widehat{\mathrm{\θ}}}_{\mathrm{affine}}\\ {\widehat{\mathrm{\α}}}_{\mathrm{affine}}\sin {\widehat{\mathrm{\θ}}}_{\mathrm{affine}}& {\widehat{\mathrm{\α}}}_{\mathrm{affine}}\cos {\widehat{\mathrm{\θ}}}_{\mathrm{affine}}\end{array}\right)P_i^{\text{'}\text{'}}$

代表二级匹配位置序列的纬度，代表二级匹配位置序列的经度；

步骤5：将二级匹配位置序列P″′_i中的最新位置信息P″′₂₀作为观测信息，利用卡尔曼滤波器中的间接滤波法估计并修正主惯性导航系统位置误差，卡尔曼滤波器接收惯性导航系统的当前位置信息和最新位置信息P″′₂₀的差值，经过滤波计算，估计出惯性导航系统位置误差量，用估计出的位置误差去修正惯性导航系统输出的位置信息，作为导航位置信息输出。

与现有技术相比，本发明具有如下优点：

1)本发明对主惯导系统进行了匹配定位预处理，匹配定位预处理所采用的算法在大初始位置误差情况下定位速度快，但定位精度较低，一级匹配所采用的算法则是在匹配定位预处理的定位结果上进行匹配，具有较小的搜索范围，因而能加快定位速度并提高定位精度；

2)本发明二级匹配定位对主惯导系统的定位误差建模采用了仿射模型，仿射模型中引入了旋转因子和缩放因子，可更加真实地反映航迹匹配过程中惯性仪器陀螺仪和加速度计的误差特性，因而比现有的匹配算法有更高的精度；

3)本发明采用两级匹配定位方式，在求解仿射参数过程中，由于一级匹配定位的航迹已在理想航迹的附近，因此并不需要采用复杂的最优搜索算法，可直接采用简单的最小二乘法来求解仿射参数，这种求解方式大幅降低了算法的复杂度，同时从实施例的结果得出本发明的匹配方法精度比现有匹配方法精度大约提高20％。

附图说明

图1为本发明所描述的地形辅助导航定位的原理图；

图2为双线性插值算法示意图；

图3为等高线提取方法示意图；

图4为本发明所描述的仿射变换示意图；

图5为实施例中所采用的地形图；

图6为实施例中TERCOM/ICCP算法匹配和本发明的匹配结果图；

图7为实施例中TERCOM／ICCP算法匹配和本发明匹配误差的对比图。

具体实施方式

下面结合具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域普通技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。

一种基于水下地形高程数据库的水下航行器辅助导航定位方法：

步骤1：获取主惯导系统指示航迹序列和实测高程序列

当水下航行器驶入匹配区时，在主惯性导航系统INS的导航下航行一段距离，与此同时每隔一个时间段Times，Times的典型值为10分钟，通过航行器的高程测量装置测量高程，由此得到惯性导航系统给出的一个惯导系统指示航迹序列

$P_i=\left(\begin{array}{c}{\mathrm{Lati}}_{P_i}\\ {\mathrm{Longti}}_{P_i}\end{array}\right),$ i＝1，2…N，N代表序列点的个数，取值为20，

代表惯导系统指示航迹序列P_i的纬度，代表惯导系统指示航迹序列P_i的经度，和一个由高程测量装置给出的对应于惯导系统指示航迹序列P_i的实测高程序列D，i＝1，2…N，考虑高程测量装置的误差可得：

$D_i={\tilde{D}}_i+\mathrm{\γ},i=\mathrm{1,2}...N$

其中表示理想高程序列，γ表示高程测量装置的测量噪声序列，服从均值为0标准差为1的正态分布。

步骤2：匹配定位预处理

在加载地形高程数据库中，选择以惯导系统指示航迹序列P_i为中心、以惯性导航系统6倍误差为边长的区域，将此区域的高程数据存入一个预处理二维矩阵，预处理二维矩阵行标为row，1≤row≤row_max，row_max为所述区域行标的最大值，预处理二维矩阵列标为col，1≤col≤col_max，col_max为所述区域列标的最大值，矩阵中的值存为储高程数据；所述区域的起始纬度为lati₀，起始经度为longti₀，矩阵行列交点row，col处的经度、纬度分别为：lati₀+(row-1)×grid，longti₀+(col-1)×grid，grid为地形数据库中相邻高程数据的位置间距，考虑地形高程数据库建库时测量和量化所产生的误差可得：

$\mathrm{DB}(\mathrm{row},\mathrm{col})=\widetilde{\mathrm{DB}}(\mathrm{row},\mathrm{col})+\mathrm{\η}$

其中row为预处理二维矩阵的行标，col为预处理二维矩阵的列标，DB(row，col)表示预处理二维矩阵中行标为row、列标为col的高程数据，

表示对应于DB(row，col)的理想高程数据，η表示地形高程数据库建库时的测量和量化噪声，服从均值为0标准差为1的正态分布；

将实测高程序列D_i和在DB(row，col)中所提取的高程序列作相关运算，所述的相关运算采用均方差MSD算法，使用去均值后的数据进行相关运算：

$\mathrm{Ral}(\mathrm{row},\mathrm{col})=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{[\mathrm{DB}(\mathrm{row},\mathrm{col})-\mathrm{DBA}(\mathrm{row},\mathrm{col})-D_i+\mathrm{DA}]}^2$

其中，Ral(row，col)表示MSD相关运算的结果，DB(row，col)表示惯导系统指示航迹P_i平移至预处理二维矩阵row，col对应位置lati₀+(row-1)×grid，longti₀+(col-1)×grid处高程数据序列，DBA(row，col)为DB(row，col)中所提取的高程序列的均值，D_i表示实测高程序列，DA表示实测序列的均值；

运算结束后，取得使Ral(row，col)最小的row，col，则将惯导系统指示航迹P_i平移至预处理二维矩阵row，col对应位置lati₀+(row-1)×grid，longti₀+(col-1)×grid，得到预处理位置序列为 $P_i^{\′}=\left(\begin{array}{c}{\mathrm{Lati}}_{P_i^{\′}}\\ {\mathrm{Longti}}_{P_i^{\′}}\end{array}\right),$

表示得到预处理位置序列P′_i的纬度，

表示得到预处理位置序列P′_i的经度；

步骤3：一级匹配定位

将地形高程数据库中以预处理位置序列P′_i为中心，以惯性导航系统3倍误差为边长的一级匹配区域内的高程数据存入一个一级匹配二维矩阵，并利用双线性插值算法在该一级匹配区域提取高程为实测高程D_i的一级等高线C_i；

双线性插值算法示意图如图2所示，其计算方法为：

坐标轴Oposx和坐标轴Oposy构成平面直角坐标系，坐标轴Oele垂直于坐标轴Oposx和坐标轴Oposy构成平面直角坐标系构成右手坐标系在坐标posy方向：

$\left\{\begin{array}{c}\mathrm{ele}(\mathrm{posl},\mathrm{posm})=(1-\mathrm{scale}2)\mathrm{ele}(\mathrm{pos}1,\mathrm{pos}2)+\mathrm{scale}2\×\mathrm{ele}(\mathrm{pos}1,\mathrm{pos}2+1)\\ \mathrm{ele}(\mathrm{pos}1+1,\mathrm{posm})=(1-\mathrm{scale}2)\mathrm{ele}(\mathrm{pos}1+1,\mathrm{pos}2)+\mathrm{scale}2\×\mathrm{ele}(\mathrm{pos}1+1,\mathrm{pos}2+1)\end{array}\right.$

scale2=posm--pos2

在坐标posx方向：

$\left\{\begin{array}{c}\mathrm{ele}(\mathrm{posn},\mathrm{pos}2)=(1-\mathrm{scale}1)\mathrm{ele}(\mathrm{pos}1,\mathrm{pos}2)+\mathrm{scale}1\×\mathrm{ele}(\mathrm{pos}1+1,\mathrm{pos}2)\\ \mathrm{ele}(\mathrm{posn},\mathrm{pos}2+1)=(1-\mathrm{scale}1)\mathrm{ele}(\mathrm{pos}1,\mathrm{pos}2+1)+\mathrm{scale}1\×\mathrm{ele}(\mathrm{pos}1+1,\mathrm{pos}2+1)\end{array}\right.$

scale1=posn-pos1

posn，posm的高程值为：

$\mathrm{ele}(\mathrm{posn},\mathrm{posm})=\left[\begin{array}{cc}1-\mathrm{scale}1& \mathrm{scale}\end{array}\right]\left[\begin{array}{cc}\mathrm{ele}(\mathrm{pos}1,\mathrm{pos}2)& \mathrm{ele}(\mathrm{pos}1,\mathrm{pos}2+1)\\ \mathrm{ele}(\mathrm{pos}1+1,\mathrm{pos}2)& \mathrm{ele}(\mathrm{pos}1+1,\mathrm{pos}2+1)\end{array}\right]\left[\begin{array}{c}1-\mathrm{scale}2\\ \mathrm{scale}2\end{array}\right]$ 其中pos1，pos2为高程数据库中的网格点，scale2代表坐标posm方向上的比例，scale1代表坐标posn方向上的比例，posn，posm为pos1，pos2、pos1+1，pos2、pos1+1，pos2+1、pos1，pos2+1四个网格点所围成的矩形区域内的任意一点，ele(pos1，posm)表示pos1，posm处的高程值，ele(pos1+1，posm)表示pos1+1，posm处的高程值，ele(posn，pos2)表示posn，pos2处的高程值，ele(posn，pos2+1)表示posn，pos2+1处的高程值，ele(posn，posm)表示posn，posm处的高程值，ele(pos1，pos2)表示pos1，pos2处的高程值，ele(pos1+1，pos2)表示pos1+1，pos2处的高程值，ele(pos1，pos2+1)表示pos1，pos2+1处的高程值，ele(pos1+1，pos2+1)表示pos1+1，pos2+1处的高程值；

提取等高线的方法为：

设实测高程为D_temp，四个网格点中部分高程小于D_temp，而其它大于D_temp时等高线穿过该网格单元，当等高线穿过该网格单元时，四个网格点所组成的矩形的四条边上高程为D_temp的点有2个点或者是4个点；当是2个点的情况时，将两点所连直线近似为该网格单元内高程为D_temp的等高线；当是如图3所示4个点的情况时，网格的四条边上都有出入点，所以首先必须得确定成对的出入点。

令标记高程ele_flag为：

${\mathrm{ele}}_{\mathrm{flag}}=\frac{\mathrm{ele}(\mathrm{pos}1,\mathrm{pos}2+1)\×\mathrm{ele}(\mathrm{pos}1+1,\mathrm{pos}2)-\mathrm{ele}(\mathrm{pos}1,\mathrm{pos}2)\×\mathrm{ele}(\mathrm{pos}1+1,\mathrm{pos}2+1)}{\mathrm{ele}(\mathrm{pos}1+1,\mathrm{pos}2)+\mathrm{ele}(\mathrm{pos}1,\mathrm{pos}2+1)-\mathrm{ele}(\mathrm{pos}1,\mathrm{pos}2)-\mathrm{ele}(\mathrm{pos}1+1,\mathrm{pos}2+1)}$ 如图3所示，pA，pB，pC，pD为网格的四条边上出入点，当D_temp＞ele_flag时pA，pB成对，pC，pD成对；当D_temp＜ele_flag时pA，pD成对，pB，pC成对；当D_temp=ele_flag时pA，pC成对，pB，pD成对；两条成对点所连直线近似为该网格单元内高程为D_temp的等高线。

步骤3.1：

惯导系统给出的航向误差为ψ_INS，纬度和经度误差分别为t_INSlati和t_INSlongti，分别在-3ψ_INS～3ψ_INS，-3t_INSlati～3t_INSlati，-3t_INSlongti～3t_INSlongti范围内分别随机取一个航向误差、一个纬度和一个经度且值分别为：ψ_INSrand，t_INSlatirand，t_{INSlongtirand}；对预处理位置序列P′_i作一级随机旋转和一级随机平移变换，得到一级随机旋转和平移位置序列P′_irand：

$P_{\mathrm{irand}}^{\′}=\left(\begin{array}{cc}\cos {\mathrm{\ψ}}_{\mathrm{INSrand}}& -\sin {\mathrm{\ψ}}_{\mathrm{INSrand}}\\ \sin {\mathrm{\ψ}}_{\mathrm{INSrand}}& \cos {\mathrm{\ψ}}_{\mathrm{INSrand}}\end{array}\right)P_i^{\′}+\left(\begin{array}{c}{t}_{\mathrm{INSlatirand}}\\ t_{\mathrm{INSlongtirand}}\end{array}\right)$

步骤3.2：

通过一级随机旋转和平移位置序列P′_irand向一级等高线C_i作垂线得到一个一级匹配垂足序列Y_i。

获取垂足序列的方法为：由等高线的提取方法可知等高线是由一系列线段组成。求一点到线段的垂足有三种情况：第一种是该点落在线段上，则该点就是垂足。第二种是由该点向线段作垂线，且垂线和线段相交在线段之内，则相交点就是垂足。第三种是由该点向线段作垂线，但垂线和线段的延长线相交，或者该点落在线段的延长线上，在这种情况下，本发明选取线段的两个端点中距离该点较近的点作为垂足。由于等高线由一系列线段组成，则一级随机旋转和平移位置序列P′_irand中的点在其对应等高线上可以求得不止一个的垂足，本发明中采用的策略是在垂足中选取与一级随机旋转和平移位置序列P′_irand中的点的距离最近点组成一级匹配垂足序列Y_i。

求取一个刚性变换Trans且所述刚性变换Trans包括旋转矩阵

$\mathrm{Rotation}=\left(\begin{array}{cc}\cos {\mathrm{\φ}}_{\mathrm{ICCP}}& -\sin {\mathrm{\φ}}_{\mathrm{ICCP}}\\ \sin {\mathrm{\φ}}_{\mathrm{ICCP}}& \cos {\mathrm{\φ}}_{\mathrm{ICCP}}\end{array}\right)$ 和平移矢量 $\mathrm{translation}=\left(\begin{array}{c}{t}_{\mathrm{ICCPLati}}\\ t_{\mathrm{ICCPLongti}}\end{array}\right),$ φ_ICCP为一级匹配旋转角，t_ICCPLati代表纬度偏移量，t_ICCPLongti代表经度偏移量，使得一级随机旋转和平移位置序列P′_irand逐渐逼近一级匹配垂足序列Y_i，也就是使以下目标函数最小：

$\mathrm{Objec}{t}_{\mathrm{itetation}}(Y_i,\mathrm{Trans}\·P_{\mathrm{irand}}^{\′})=\left\{\begin{array}{cc}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{weight}}_i{\left|\right|Y_i-\mathrm{Trans}\·P_{\mathrm{irand}}^{\′}\left|\right|}^2& \mathrm{iteration}\≥1\\ 0& \mathrm{iteration}=0\end{array}\right.$

${\mathrm{weight}}_i=1-\frac{\mathrm{Dist}(P_{\mathrm{irand}}^{\′},Y_i)}{{\mathrm{Dist}}_{\max }}$

其中Object为目标函数，iteration代表第iteration次迭代，iteration=0表示迭代未开始，weighti代表序列点的权值，Trans为刚性变换，Dist(P′_irand，Y_i)为P′_irand和Y_i之间的距离，Dist_max为P′_irand和Y_i序列点之间距离的最大值；

求取刚性变换Trans采用四元数法；

设四元数为quaternion：

$\mathrm{quaternion}=({\mathrm{qua}}_1,{\mathrm{qua}}_2,{\mathrm{qua}}_3,{\mathrm{qua}}_4)=(\cos \frac{{\mathrm{\φ}}_{\mathrm{ICCP}}}{2},\mathrm{0,0},\sin \frac{{\mathrm{\φ}}_{\mathrm{ICCP}}}{2})$

qua₁为四元数quaternion的第一个元素，qua2为四元数quaternion的第二个元素，qua₃为四元数quaternion的第三个元素，qua₄为四元数quaternion的第四个元素，φ_ICCP为一级匹配旋转角；

构造矩阵Mat_ICCP：

${\mathrm{Mat}}_{\mathrm{ICCP}}=\left[\begin{array}{cc}{\mathrm{Mat}}_{11}& {\mathrm{Mat}}_{12}\\ {\mathrm{Mat}}_{21}& {\mathrm{Mat}}_{22}\end{array}\right]=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{weight}}_i(Y_i-{\stackrel{-}{Y}}_i){(P_{\mathrm{irand}}^{\′}-{\stackrel{-}{P}}_{\mathrm{irand}}^{\′})}^T$

Mat₁₁为矩阵Mat_ICCP第一行第一列的元素，Mat₁₂为矩阵Mat_ICCP第一行第二列的元素，Mat₂₁为矩阵Mat_ICCP第二行第一列的元素，Mat₂₂为矩阵Mat_ICCP第二行第二列的元素，

和

分别为一级匹配垂足序列的质心、随机旋转和平移位置序列的质心，weighti代表序列点的权值，T代表转置；

为求解旋转矩阵Rotation而构造一个对称的哈密尔顿矩阵：

$\mathrm{Hamilton}=\left(\begin{array}{cccc}{\mathrm{Mat}}_{11}+{\mathrm{Mat}}_{22}& 0& 0& {\mathrm{Mat}}_{21}-{\mathrm{Mat}}_{12}\\ 0& {\mathrm{Mat}}_{11}-{\mathrm{Mat}}_{22}& {\mathrm{mat}}_{12}+{\mathrm{Mat}}_{21}& 0\\ 0& {\mathrm{Mat}}_{12}+{\mathrm{Mat}}_{21}& {\mathrm{Mat}}_{22}-{\mathrm{Mat}}_{11}& 0\\ {\mathrm{Mat}}_{21}-{\mathrm{Mat}}_{12}& 0& 0& {-\mathrm{Mat}}_{11}-{\mathrm{Mat}}_{22}\end{array}\right)$

求解并得到哈密尔顿矩阵Hamilton的四个特征值

：

记最大的特征值为

最大的特征值为

可使下式成立：

由此可得一级匹配旋转角为：

所以旋转矩阵Rotation为：

$\mathrm{Rotation}=\left(\begin{array}{cc}\cos {\mathrm{\φ}}_{\mathrm{ICCP}}& -\sin {\mathrm{\φ}}_{\mathrm{ICCP}}\\ \sin {\mathrm{\φ}}_{\mathrm{ICCP}}& \cos {\mathrm{\φ}}_{\mathrm{ICCP}}\end{array}\right)$

平移量translation为：

$\mathrm{translation}={\stackrel{-}{Y}}_i-\mathrm{Rotation}\·{\stackrel{-}{P}}_{\mathrm{irand}}^{\′}$

步骤3.3：

利用步骤3.2得到的包括旋转矩阵Rotation和平移矢量translation的刚性变换Trans，对一级随机旋转和平移后的位置序列P′_irand作P′_irand=Trans·P′_irand处理，此时，若迭代次数iteration大于最大迭代次数iteration_max，表明收敛速度过慢，舍弃此次迭代的结果，返回步骤3.1；若iteration小于等于iteration_max并且|O勿ect_iteration-Object_iteration-1|＞τ，则返回步骤3.2，若iteration小于等于iteration_max并且|Object_iteration-Object_iteration-1|≤τ，则迭代终止并记录Object_itetation和Trans·P′_irand的值，iteration_max值为100，τ值为10^-6；

步骤3.4：

重复3.1-3.3步骤，得到50个Object_itetation(Y_i，Trans·P′_irand)值后，找出其中最小的Object_itetation(Y_i，Trans·P′_irand)对应的Trans·P′_irand，得到一级匹配位置序列 $P_i^{\′\′}=\mathrm{Trans}\·P_{\mathrm{irand}}^{\′}=\left(\begin{array}{c}{\mathrm{Lati}}_{P^{\′\′}}\\ {\mathrm{Longti}}_{P^{\′\′}}\end{array}\right),$

代表一级匹配位置序列的纬度，

代表一级匹配序列的经度；

步骤4：二级匹配定位

加载地形高程数据库中以一级匹配位置序列P″_i为中心，3×grid为边长区域，将此区域的高程数据存入一个二级匹配二维矩阵，并利用双线性插值算法在该区域提取高程为实测高程D_i的二级等高线C_i′，在二级等高线C_i′上寻找一级匹配位置序列P″_i的对应垂足得到一个二级垂足序列 $Y_i^{\′}=\left\{\begin{array}{c}{\mathrm{Lati}}_{Y_{i^{\′}}}\\ {\mathrm{Longti}}_{Y_{i^{\′}}}\end{array}\right.,$

代表二级垂足序列的纬度，

代表二级垂足序列的经度；对二级垂足序列Y′_i到一级匹配位置序列P”_i作仿射变换G且所述仿射变换包括在纬度方向上的平移因子t_affinx，在经度方向上的平移因子t_affinx，缩放因子α_affine，旋转因子θ_affine，e_i为二级垂足序列Y′_i与二级匹配定位的输入序列P″_i的经仿射变换后的坐标的差：

$\begin{array}{c}{e}_i=\left(\begin{array}{c}{t}_{\mathrm{affinex}}\\ t_{\mathrm{affiney}}\end{array}\right)+\left(\begin{array}{cc}{\mathrm{\α}}_{\mathrm{affine}}\cos {\mathrm{\θ}}_{\mathrm{affine}}& -{\mathrm{\α}}_{\mathrm{affine}}\sin {\mathrm{\θ}}_{\mathrm{affine}}\\ {\mathrm{\α}}_{\mathrm{affine}}\sin {\mathrm{\θ}}_{\mathrm{affine}}& {\mathrm{\α}}_{\mathrm{affine}}\cos {\mathrm{\θ}}_{\mathrm{affine}}\end{array}\right)P_i^{\text{'}\text{'}}-Y_i^{\text{'}}\\ =\left(\begin{array}{cccc}1& 0& {\mathrm{Lati}}_{P_i^{\text{'}\text{'}}}& -{\mathrm{Longti}}_{P_i^{\text{'}\text{'}}}\\ 0& 1& {\mathrm{Longti}}_{P_i^{\text{'}\text{'}}}& {\mathrm{Lati}}_{P_i^{\text{'}\text{'}}}\end{array}\right)\left(\begin{array}{c}{t}_{\mathrm{affinex}}\\ t_{\mathrm{affiney}}\\ {\mathrm{\α}}_{\mathrm{affine}}\cos {\mathrm{\θ}}_{\mathrm{affine}}\\ {\mathrm{\α}}_{\mathrm{affine}}\sin {\mathrm{\θ}}_{\mathrm{affine}}\end{array}\right)-\left(\begin{array}{c}{\mathrm{Lati}}_{Y_i^{\text{'}}}\\ {\mathrm{Longti}}_{Y_i^{\text{'}}}\end{array}\right)\end{array}$

令仿射变换求解向量为t_affine，仿射变换求解矩阵为

$t_{\mathrm{affine}}=\left(\begin{array}{c}{t}_{\mathrm{affinex}}\\ t_{\mathrm{affiney}}\\ {\mathrm{\α}}_{\mathrm{affine}}\cos {\mathrm{\θ}}_{\mathrm{affine}}\\ {\mathrm{\α}}_{\mathrm{affine}}\sin {\mathrm{\θ}}_{\mathrm{affine}}\end{array}\right)$

$C_{P_i^{\′\′}}=\left(\begin{array}{cccc}1& 0& {\mathrm{Lati}}_{P_i^{\′\′}}& -{\mathrm{Longti}}_{P_i^{\′\′}}\\ 0& 1& {\mathrm{Longti}}_{P_i^{\′\′}}& {\mathrm{Lati}}_{P_i^{\′\′}}\end{array}\right)$

则 $e_i=C_{P_i^{\′\′}}{t}_{\mathrm{affine}}-Y_i^{\′};$

则欧式平方距离ESD_affine为：

${\mathrm{ESD}}_{\mathrm{affine}}=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{e_i}^T{e}_i={t_{\mathrm{affine}}}^T{C_{P_i^{\′\′}}}^T{C}_{P_i^{\′\′}}{t}_{\mathrm{affine}}-{2Y}_i^{\′T}{C}_{P_i^{\′\′}}{t}_{\mathrm{affine}}+Y_i^{\′T}{Y}_i^{\′}$

T代表转置；

将使ESDD_affine最小的t_affine记为对ESD_affine求导可得：

$\frac{{\mathrm{dESD}}_{\mathrm{affine}}}{{\mathrm{dt}}_{\mathrm{affine}}}={{2C}_{P_i^{\′\′}}}^T{C}_{P_i^{\′\′}}{t}_{\mathrm{affine}}-{{2C}_{P_i^{\′\′}}}^T{Y}_i^{\′}$

令 $\frac{{\mathrm{dESD}}_{\mathrm{affine}}}{d{t}_{\mathrm{affine}}}=0$ 可得

${\hat{t}}_{\mathrm{affine}}={\left({C_{P_i^{\text{'}\text{'}}}}^T{C}_{P_i^{\text{'}\text{'}}}\right)}^{-1}{C_{P_i^{\text{'}\text{'}}}}^T{Y}_i^{\text{'}}$

当ESD_affine对t_affine的二阶导数

时，就是使ESD_affine最小的解：

$\begin{array}{c}{\hat{t}}_{\mathrm{affine}}={\left({C_{P_i^{\′\′}}}^T{C}_{P_i^{\′\′}}\right)}^{-1}{C_{P_i^{\′\′}}}^T{Y}_i^{\′}\\ =\frac{1}{\det }\left(\begin{array}{cccc}{l}_p& 0& -{\mathrm{\μ}}_{\mathrm{xp}}& {\mathrm{\μ}}_{\mathrm{yp}}\\ 0& l_p& {-\mathrm{\μ}}_{\mathrm{yp}}& {-\mathrm{\μ}}_{\mathrm{xp}}\\ {-\mathrm{\μ}}_{\mathrm{xp}}& {-\mathrm{\μ}}_{\mathrm{yp}}& N& 0\\ {\mathrm{\μ}}_{\mathrm{yp}}& {-\mathrm{\μ}}_{\mathrm{xp}}& 0& N\end{array}\right)\left(\begin{array}{c}{\mathrm{\μ}}_{\mathrm{xq}}\\ {\mathrm{\μ}}_{\mathrm{yq}}\\ l_{p+q}\\ l_{p-q}\end{array}\right)\end{array}$

其中：代表一级匹配位置序列的纬度的之和，

代表二级垂足序列的纬度之和，

代表一级匹配位置序列的经度的之和，

代表二级垂足序列的经度之和，代表一级匹配位置序列和二级垂足序列的点积之和，

代表一级匹配位置序列和二级垂足序列的叉积之和，

代表一级匹配位置序列的模的平方之和， $\det =N\×l_p-{\mathrm{\μ}}_{\mathrm{xp}}^2-{\mathrm{\μ}}_{\mathrm{yp}}^2$ 代表矩阵

的模；

由此就可以得到

${\hat{t}}_{\mathrm{affine}}={({\hat{t}}_{\mathrm{affinex}},{\hat{t}}_{\mathrm{affiney}},{\widehat{\mathrm{\α}}}_{\mathrm{affine}}\cos {\widehat{\mathrm{\θ}}}_{\mathrm{affine}},{\widehat{\mathrm{\α}}}_{\mathrm{affine}}\sin {\widehat{\mathrm{\θ}}}_{\mathrm{affine}})}^T$

其中

为求解所得的平移因子，

为求解所得的缩放因子，为求解所得的旋转因子；

对一级匹配位置序列作仿射变换得二级匹配位置序列P″′_i：

$P_i^{\text{'}\text{'}\text{'}}=\left(\begin{array}{c}{\mathrm{Lati}}_{P_i^{\text{'}\text{'}\text{'}}}\\ {\mathrm{Longti}}_{P_i^{\text{'}\text{'}\text{'}}}\end{array}\right)=\left(\begin{array}{c}{\hat{t}}_{\mathrm{affinex}}\\ {\hat{t}}_{\mathrm{affiney}}\end{array}\right)+\left(\begin{array}{cc}{\widehat{\mathrm{\α}}}_{\mathrm{affine}}\cos {\widehat{\mathrm{\θ}}}_{\mathrm{affine}}& -{\widehat{\mathrm{\α}}}_{\mathrm{affine}}\sin {\widehat{\mathrm{\θ}}}_{\mathrm{affine}}\\ {\widehat{\mathrm{\α}}}_{\mathrm{affine}}\sin {\widehat{\mathrm{\θ}}}_{\mathrm{affine}}& {\widehat{\mathrm{\α}}}_{\mathrm{affine}}\cos {\widehat{\mathrm{\θ}}}_{\mathrm{affine}}\end{array}\right)P_i^{\text{'}\text{'}}$

代表二级匹配位置序列的纬度，

代表二级匹配位置序列的经度；

步骤5：将二级匹配位置序列P″′_i中的最新位置信息P″′₂₀作为观测信息，利用卡尔曼滤波器中的间接滤波法估计并修正主惯性导航系统位置误差，卡尔曼滤波器接收惯性导航系统的当前位置信息和最新位置信息P″′₂₀的差值，经过滤波计算，估计出惯性导航系统位置误差量，用估计出的位置误差去修正惯性导航系统输出的位置信息，作为导航位置信息输出。

惯性导航系统位置误差估计修正步骤如下：

步骤5.1：建立惯性导航系统状态方程，选取东北天坐标系O_ENU作为导航坐标系，其中OE为东向轴，ON为北向轴，OU表示天向轴，载体坐标系Oxyz，其中Ox轴沿航行器横轴指向右舷，Oy轴沿航行器纵轴指向前，Oz轴垂直于Ox与Oy轴所确定的平面构成右手坐标系，选取状态向量X_INS为：

其中，δV_E、δV_N、δV_U分别是东向、北向和天向速度误差；φ_E、φ_N、φ_U分别是东向、北向和天向失准角；δLati、δLongti、δh分别是纬度、经度和天向误差；Δ_bx、Δ_by、Δ_bz分别是Ox、Oy、Oz向的加速度计偏置；ε_bx、ε_by、ε_bz分别是Ox、Oy、Oz向的陀螺漂移；

建立惯性导航系统线性状态方程为：

${\stackrel{\·}{X}}_{\mathrm{INS}}=F_{\mathrm{INS}}{X}_{\mathrm{INS}}+W_{\mathrm{INS}}$

其中：

$F_{\mathrm{INS}}=\left[\begin{array}{ccccccccccccccc}{F}_{11}& F_{12}& F_{13}& 0& -f_U& f_N& F_{17}& 0& F_{19}& c_{11}& c_{21}& c_{31}& 0& 0& 0\\ F_{21}& -\frac{V_U}{{\mathrm{Radius}}_n+h}& -\frac{V_N}{{\mathrm{Radius}}_n+h}& f_U& 0& -f_E& F_{27}& 0& F_{29}& c_{21}& c_{22}& c_{32}& 0& 0& 0\\ F_{31}& \frac{{2V}_U}{{\mathrm{Radius}}_n+h}& 0& -f_N& f_E& 0& -2{\mathrm{\ω}}_{\mathrm{ie}}{V}_E\sin \mathrm{Longti}& 0& F_{39}& c_{13}& c_{23}& c_{33}& 0& 0& 0\\ 0& -\frac{1}{{\mathrm{Radius}}_n+h}& 0& 0& F_{45}& F_{46}& 0& 0& \frac{V_N}{{({\mathrm{Radius}}_n+h)}^2}& 0& 0& 0& -c_{11}& -c_{21}& -c_{31}\\ \frac{1}{{\mathrm{Radius}}_n+h}& 0& 0& F_{54}& 0& -\frac{V_N}{{\mathrm{Radius}}_n+h}& -{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{Longti}& 0& \frac{V_E}{{({\mathrm{Radius}}_n+h)}^2}& 0& 0& 0& -c_{12}& -c_{22}& -c_{23}\\ \frac{\tan \mathrm{Longti}}{{\mathrm{Radius}}_n+h}& 0& 0& F_{64}& \frac{V_N}{{\mathrm{Radius}}_n+h}& 0& F_{67}& 0& \frac{V_E\tan \mathrm{Longti}}{{({\mathrm{Radius}}_n+h)}^2}& 0& 0& 0& -c_{13}& -c_{23}& -c_{33}\\ 0& \frac{1}{{\mathrm{Radius}}_n+h}& 0& 0& 0& 0& 0& 0& -\frac{V_N}{{({\mathrm{Radius}}_n+h)}^2}& 0& 0& 0& 0& 0& 0\\ \frac{1}{\left({\mathrm{Radius}}_n+h\right)\cos \mathrm{Longti}}& 0& 0& 0& 0& 0& \frac{V_E\tan \mathrm{Longti}}{(R_e+h)\cos \mathrm{Longti}}& 0& -\frac{V_E}{{({\mathrm{Radius}}_n+h)}^2\cos \mathrm{Longti}}& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ & & & & & & & 0_{6\×15}& & & & & & & \end{array}\right]$

$W_{\mathrm{INS}}={\left[\begin{array}{ccccccccccccccc}{\mathrm{\ω}}_{V_E}& {\mathrm{\ω}}_{V_N}& {\mathrm{\ω}}_{V_U}& {\mathrm{\ω}}_{{\mathrm{\φ}}_E}& {\mathrm{\ω}}_{{\mathrm{\φ}}_N}& {\mathrm{\ω}}_{{\mathrm{\φ}}_U}& {\mathrm{\ω}}_{\mathrm{\δLongti}}& {\mathrm{\ω}}_{\mathrm{\δLati}}& {\mathrm{\ω}}_{\mathrm{\δh}}& 0& 0& 0& 0& 0& 0\end{array}\right]}^T$

$F_{11}=\frac{V_N\tan \mathrm{Longti}}{{\mathrm{Radius}}_n+h}-\frac{V_U}{{\mathrm{Radius}}_e+h},F_{12}=2{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{longti}+\frac{V_E\tan \mathrm{longti}}{{\mathrm{Radius}}_e+h}$

$F_{13}=-(2{\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Longti}+\frac{V_E}{{\mathrm{Radius}}_e+h}),F_{17}=2{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{Longti}\×V_U+2{\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Longti}\×V_N+\frac{V_E{V}_N{\left(\sec \mathrm{Longti}\right)}^2}{{\mathrm{Radius}}_e+h}$

$F_{19}=\frac{V_E{V}_U}{{({\mathrm{Radius}}_e+h)}^2}-\frac{V_E{V}_V\tan \mathrm{Longti}}{{({\mathrm{Radius}}_e+h)}^2},F_{21}=-2{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{Longti}-\frac{V_E\tan \mathrm{Longti}}{{\mathrm{Radius}}_e+h}$

$F_{27}=-(2{\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Longti}{V}_E+\frac{V_E^2{\left(\sec \mathrm{Longti}\right)}^2}{{\mathrm{Radius}}_e+h}),F_{29}=\frac{V_N{V}_U}{{({\mathrm{Radius}}_n+h)}^2}+\frac{V_E^2\tan \mathrm{Longti}}{{(R{\mathrm{adius}}_e+h)}^2}$

$F_{31}=2{\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Longti}+\frac{2V_E}{{\mathrm{Radius}}_e+h},F_{39}=-(\frac{V_N^2}{{(R_n+h)}^2}+\frac{V_E^2\tan \mathrm{Longti}}{{({\mathrm{Radius}}_e+h)}^2})$

$F_{45}={\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{Longti}+\frac{V_E\tan \mathrm{Longti}}{{\mathrm{Radius}}_e+h},F_{46}=-({\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Longti}+\frac{V_E{\left(\sec \mathrm{Longti}\right)}^2}{{\mathrm{Radius}}_e+h})$

$F_{54}=-({\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{Longti}+\frac{V_E\tan \mathrm{Longti}}{{\mathrm{Radius}}_e+h}),F_{64}={\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Longti}+\frac{V_E{\left(\sec \mathrm{Longti}\right)}^2}{{\mathrm{Radius}}_e+h}$

$F_{67}={\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Longti}+\frac{V_E{\left(\sec \mathrm{Longti}\right)}^2}{{\mathrm{Radius}}_e+h}$

其中F₁₁为矩阵F_INS的第1行第1列元素，F₁₂为矩阵F_INS的第1行第2列元素，F₁₃为矩阵F_INS的第1行第3列元素，F₁₇为矩阵F_IVS的第1行第7列元素，F₁₉为矩阵F_INS的第1行第9列元素，F₂₁为矩阵F_IVS的第2行第1列元素，F₂₇为矩阵F_INS的第2行第7列元素，F₂₉为矩阵F_INS的第2行第9列元素，F₃₁为矩阵F_INS的第3行第1列元素，F₃₉为矩阵F_INS的第3行第9列元素，F₄₅为矩阵F_INS的第4行第5列元素，F₄₆为矩阵F_INS的第4行第6列元素，F₅₄为矩阵F_INS的第5行第4列元素，F₆₄为矩阵F_INS的第6行第4列元素，F₆₇为矩阵F_INS的第6行第7列元素，c_ij为姿态转移矩阵

的元素，F_INS为系统状态转移矩阵；W_INS为系统随机干扰向量，ω_ie为地球自转角速度，Radius_n、Radius_e分别为沿子午圈和卯酉圈的主曲率半径，V_E、V_N、及V_U为载体真实东向、北向和天向速度，Longti、Lati及h表示载体所处的真实经度、纬度及高程；卡尔曼滤波用离散化模型来描述系统，采用离散化后的差分方程描述连续系统，离散化后的系统状态方程为：

$X_k={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+W_{k-1}$

其中X_k为k时刻的系统状态向量，Φ_k，k-1为k-1到k时刻的一步转移矩阵，

为系统状态向量的估计，W_k-1为k-1时刻的系统噪声，且

${\mathrm{\Φ}}_{k,k-1}=I+\mathrm{\ΔPer}\underset{j=1}{\overset{\mathrm{\σ}}{\mathrm{\Σ}}}{F}_{j-1}$

I为单位矩阵，ΔPer为将滤波周期Per分成σ个时间间隔ΔPer，F_j-1为F_INS在时刻k-1+(j-1)ΔPer时的值；

步骤5.2：建立观测方程，将二级匹配位置序列P″′_i中的最新位置信息P″′₂₀与惯性导航系统输出的经度Longti_INS、纬度Lati_INS位置信息的差值，高程测量装置实测高程D_i的最新实测高程D₂₀与主惯导系统输出高程h_INS的差值构成位置量测向量：

$Z_k=\left[\begin{array}{c}{\mathrm{Longti}}_{\mathrm{INS}}-{\mathrm{Longti}}_{{P_{20}}^{\text{'}\text{'}\text{'}}}\\ {\mathrm{Lati}}_{\mathrm{INS}}-{\mathrm{Lati}}_{{P_{20}}^{\text{'}\text{'}\text{'}}}\\ h_{\mathrm{INS}}-D_{20}\end{array}\right]=H_k{X}_k-{\mathrm{\υ}}_k$

其中： $H_k=\left[\begin{array}{cccccc}& 1& 0& 0& & 0\\ 0_{3\×6}& 0& 1& 0& 0_{3\×6}& 0\\ & 0& 0& 1& & -1\end{array}\right]$ 为观测矩阵，υ_k为观测噪声；

步骤5.3：卡尔曼滤波器误差估计：

步骤5.3.1时间更新：

$\left\{\begin{array}{c}{\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T\end{array}\right.$

步骤5.3.1量测更新：

$\left\{\begin{array}{c}{K}_{k-1}=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^T\\ {\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})\\ P_k=(I-K_k{H}_k)P_{k,k-1}\end{array}\right.$

T代表转置，

为对系统状态向量的估计，P_k为状态估计误差的协方差阵，K_k为滤波增益，

表示对量测值的一步预测，

为新息序列，Q_k-1、R_k分别为Wk-1、u_k的方差阵；给定初始条件X₀和P₀，由Z_k即可得到每一步的状态最优估计

步骤5.4利用当前卡尔曼滤波器得出的误差量最优估计向量

中的位置误差量，修正惯性导航系统位置信息，得到修正后的位置信息

$\tilde{L}{\mathrm{ongti}}_{\mathrm{INS}}={\mathrm{Longti}}_{\mathrm{INS}}+\mathrm{\δ}\hat{L}\mathrm{ongti},{\tilde{L}\mathrm{atin}}_{\mathrm{INS}}={\mathrm{Latin}}_{\mathrm{INS}}+\mathrm{\δ}\hat{L}\mathrm{ati}$

其中

分别是卡尔曼滤波器输出的纬度、经度误差。

本发明所描述的地形辅助导航定位的原理图如图1所示；

实施例：

仿真实验中地形高程数据库的范围为(北纬38.0°，东经120.0°)至(北纬38.5°，东经120.5°)的矩形区域，数据库地形的精度为0.075′×0.075′，在该区域内设置一条理想航迹位置序列，位置序列点数为20点，起始位置点为(北纬38.2°，东经120.2°)，初始航向为38.7°，航向变化范围为±7°，初始航速为11.5节，航速变换范围为±1.3节。水下航行器惯导系统的陀螺仪漂移为0.02°/h，加速度计零偏为0.0005m／s²，初始位置误差在经度方向取为0.75′，纬度方向取为0.15′，采样时间为30s，测深装置的测量误差取为幅值为0.2的白噪声。表1为现有的ICCP迭代最近等值线匹配定位算法，现有的TERCOM／ICCP地形轮廓／迭代最近点匹配定位算法和本发明的匹配定位仿真的统计结果：

表1：

参数	参数值	单位
			ICCP匹配纬度误差均值误差	436.2	m
现有的TERCOM／ICCP匹配纬度均值误差	69.2	m
			本发明的匹配纬度均值误差	47.6	m
ICCP匹配经度误差均值误差	448.1	m
			现有的TERCOM／ICCP匹配经度均值误差	50.1	m
本发明匹配经度均值误差	28.9	m
			ICCP匹配时间	203.5	s
现有的TERCOM／ICCP匹配时间	7.568	s
			本发明匹配时间	7.594	s

从仿真结果来可以看出，在较大的初始位置误差下，ICCP匹配的误差很大，属于误匹配，这是由于初始位置过大而不满足ICCP匹配算法正确执行所需满足的初始航迹在理想航迹附近的假设前提。由于TERCOM算法对初始位置不敏感，因而采用TERCOM／ICCP的方法进行匹配时，可以得到较为准确的匹配结果。而采用本发明的方法，在匹配时间相当的情况下，相比于TERCOM／ICCP匹配方法拥有更高的匹配精度。因此，本发明可以解决水下航行器长时间潜航所产生大初始位置情况下易出现误匹配的问题，同时以极小时间代价提高了导航定位的精度。

Claims (2)

1.一种基于水下地形高程数据库的水下航行器辅助导航定位方法，其特征为：

步骤1：获取主惯导系统指示航迹序列和实测高程序列

当水下航行器驶入匹配区时，在主惯性导航系统INS的导航下航行一段距离，与此同时每隔一个时间段Times，Times的典型值为10分钟，通过航行器的高程测量装置测量高程，由此得到惯性导航系统给出的一个惯导系统指示航迹序列 $P_i=\left(\begin{array}{c}{\mathrm{Lati}}_{P_i}\\ {\mathrm{Longti}}_{P_i}\end{array}\right),$ i=1，2…N，N代表序列点的个数，取值为20，

代表惯导系统指示航迹序列P_i的纬度，

代表惯导系统指示航迹序列P_i的经度，和一个由高程测量装置给出的对应于惯导系统指示航迹序列P_i的实测高程序列D_i，i=1，2…N，考虑高程测量装置的误差可得：

$D_i={\tilde{D}}_i+\mathrm{\γ},i=\mathrm{1,2}...N$

其中

表示理想高程序列，γ表示高程测量装置的测量噪声序列，服从均值为0标准差为1的正态分布。

步骤2：匹配定位预处理

在加载地形高程数据库中，选择以惯导系统指示航迹序列P_i为中心、以惯性导航系统6倍误差为边长的区域，将此区域的高程数据存入一个预处理二维矩阵，预处理二维矩阵行标为row，1≤row≤row_max，row_max为所述区域行标的最大值，预处理二维矩阵列标为col，1≤col≤col_max，col_max为所述区域列标的最大值，矩阵中的值存为储高程数据；所述区域的起始纬度为lati₀，起始经度为longti₀，矩阵行列交点row，col处的经度、纬度分别为：lati₀+(row-1)×grid，longti₀+(col-1)×grid，grid为地形数据库中相邻高程数据的位置间距，考虑地形高程数据库建库时测量和量化所产生的误差可得：

$\mathrm{DB}(\mathrm{row},\mathrm{col})=\widetilde{\mathrm{DB}}(\mathrm{row},\mathrm{col})+\mathrm{\η}$

其中row为预处理二维矩阵的行标，col为预处理二维矩阵的列标，DB(row，col)表示预处理二维矩阵中行标为row、列标为col的高程数据，

表示对应于DB(row，col)的理想高程数据，η表示地形高程数据库建库时的测量和量化噪声，服从均值为0标准差为1的正态分布；

将实测高程序列D_i和在DB(row，col)中所提取的高程序列作相关运算，所述的相关运算采用均方差MSD算法，使用去均值后的数据进行相关运算：

$\mathrm{Ral}(\mathrm{row},\mathrm{col})=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{[\mathrm{DB}(\mathrm{row},\mathrm{col})-\mathrm{DBA}(\mathrm{row},\mathrm{col})-D_i+\mathrm{DA}]}^2$

其中，Ral(row，col)表示MSD相关运算的结果，DB(row，col)表示惯导系统指示航迹P_i平移至预处理二维矩阵row，col对应位置lati₀+(row-1)×grid，longti₀+(col-1)×grid处高程数据序列，DBA(raw，col)为DB(row，col)中所提取的高程序列的均值，D_i表示实测高程序列，DA表示实测序列的均值；

运算结束后，取得使Ral(row，col)最小的row，col，则将惯导系统指示航迹P_i平移至预处理二维矩阵row，col对应位置lati₀+(row-1)×grid，longti₀+(col-1)×grid，得到预处理位置序列为 $P_i^{\′}=\left(\begin{array}{c}{\mathrm{Lati}}_{P_i^{\′}}\\ {\mathrm{Longti}}_{P_i^{\′}}\end{array}\right),$ 表示得到预处理位置序列P′_i的纬度，

表示得到预处理位置序列P′_i的经度；

步骤3：一级匹配定位

将地形高程数据库中以预处理位置序列P′_i为中心，以惯性导航系统3倍误差为边长的一级匹配区域内的高程数据存入一个一级匹配二维矩阵，并利用双线性插值算法在该一级匹配区域提取高程为实测高程D_i的一级等高线C_i；

步骤3.1：

惯导系统给出的航向误差为ψ_INS，纬度和经度误差分别为t_INSlati和t_INSlongti，分别在-3ψ_INS～3ψ_INS，--3t_INSlati～3t_INSlati，--3t_INSlongti～3t_INSlongti范围内分别随机取一个航向误差、一个纬度和一个经度且值分别为：ψ_INSrand，t_INSlatirand，t_{INSlongtirand}；对预处理位置序列P′_i作一级随机旋转和一级随机平移变换，得到一级随机旋转和平移位置序列P′_iand：

$P_{\mathrm{irand}}^{\′}=\left(\begin{array}{cc}\cos {\mathrm{\ψ}}_{\mathrm{INSrand}}& -\sin {\mathrm{\ψ}}_{\mathrm{INSrand}}\\ \sin {\mathrm{\ψ}}_{\mathrm{INSrand}}& \cos {\mathrm{\ψ}}_{\mathrm{INSrand}}\end{array}\right)P_i^{\′}+\left(\begin{array}{c}{t}_{\mathrm{INSlatirand}}\\ t_{\mathrm{INSlongtirand}}\end{array}\right)$

步骤3.2：

通过一级随机旋转和平移位置序列P′_irand向一级等高线C_i作垂线，得到一个一级匹配垂足序列Y_i，求取一个刚性变换Trans且所述刚性变换Trans包括旋转矩阵 $\mathrm{Rotation}=\left(\begin{array}{cc}\cos {\mathrm{\φ}}_{\mathrm{ICCP}}& -\sin {\mathrm{\φ}}_{\mathrm{ICCP}}\\ \sin {\mathrm{\φ}}_{\mathrm{ICCP}}& \cos {\mathrm{\φ}}_{\mathrm{ICCP}}\end{array}\right)$ 和平移矢量 $\mathrm{translation}=\left(\begin{array}{c}{t}_{\mathrm{ICCPLati}}\\ t_{\mathrm{ICCPLongti}}\end{array}\right),$ φ_ICCP为一级匹配旋转角，t_ICCPLati代表纬度偏移量，t_ICCPLongti代表经度偏移量，使得一级随机旋转和平移位置序列P′_irand逐渐逼近一级匹配垂足序列Y_i，也就是使以下目标函数最小：

$\mathrm{Objec}{t}_{\mathrm{itetation}}(Y_i,\mathrm{Trans}\·P_{\mathrm{irand}}^{\′})=\left\{\begin{array}{cc}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{weight}}_i{\left|\right|Y_i-\mathrm{Trans}\·P_{\mathrm{irand}}^{\′}\left|\right|}^2& \mathrm{iteration}\≥1\\ 0& \mathrm{iteration}=0\end{array}\right.$

${\mathrm{weight}}_i=1-\frac{\mathrm{Dist}(P_{\mathrm{irand}}^{\′},Y_i)}{{\mathrm{Dist}}_{\max }}$

其中Object为目标函数，iteration代表第iteration次迭代，iteration=0表示迭代未开始，weighti代表序列点的权值，Trans为刚性变换，Dist(P′_irand，Y_i)为P′_irand和Y_i之间的距离，Dist_max为P′_irand和Y_i序列点之间距离的最大值；

求取刚性变换Trans采用四元数法；

设四元数为quaternion：

$\mathrm{quaternion}=({\mathrm{qua}}_1,{\mathrm{qua}}_2,{\mathrm{qua}}_3,{\mathrm{qua}}_4)=(\cos \frac{{\mathrm{\φ}}_{\mathrm{ICCP}}}{2},\mathrm{0,0},\sin \frac{{\mathrm{\φ}}_{\mathrm{ICCP}}}{2})$

qua₁为四元数quaternion的第一个元素，qua₂为四元数quaternion的第二个元素，qua₃为四元数quaternion的第三个元素，qua₄为四元数quaternion的第四个元素，φ_ICCP为一级匹配旋转角；

构造矩阵Mat_ICCP：

${\mathrm{Mat}}_{\mathrm{ICCP}}=\left[\begin{array}{cc}{\mathrm{Mat}}_{11}& {\mathrm{Mat}}_{12}\\ {\mathrm{Mat}}_{21}& {\mathrm{Mat}}_{22}\end{array}\right]=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{weight}}_i(Y_i-{\stackrel{-}{Y}}_i){(P_{\mathrm{irand}}^{\′}-{\stackrel{-}{P}}_{\mathrm{irand}}^{\′})}^T$

Mat₁₁为矩阵Mat_ICCP第一行第一列的元素，Mat₁₂为矩阵Mat_ICCP第一行第二列的元素，Mat₂₁为矩阵Mat_ICCP第二行第一列的元素，Mat₂₃为矩阵Mat_ICCP第二行第二列的元素，

和

分别为一级匹配垂足序列的质心、随机旋转和平移位置序列的质心，weight_i代表序列点的权值，T代表转置；

为求解旋转矩阵Rotation而构造一个对称的哈密尔顿矩阵：

$\mathrm{Hamilton}=\left(\begin{array}{cccc}{\mathrm{Mat}}_{11}+{\mathrm{Mat}}_{22}& 0& 0& {\mathrm{Mat}}_{21}-{\mathrm{Mat}}_{12}\\ 0& {\mathrm{Mat}}_{11}-{\mathrm{Mat}}_{22}& {\mathrm{mat}}_{12}+{\mathrm{Mat}}_{21}& 0\\ 0& {\mathrm{Mat}}_{12}+{\mathrm{Mat}}_{21}& {\mathrm{Mat}}_{22}-{\mathrm{Mat}}_{11}& 0\\ {\mathrm{Mat}}_{21}-{\mathrm{Mat}}_{12}& 0& 0& {-\mathrm{Mat}}_{11}-{\mathrm{Mat}}_{22}\end{array}\right)$

求解并得到哈密尔顿矩阵Hamilton的四个特征值

记最大的特征值为最大的特征值为

可使下式成立：

由此可得一级匹配旋转角为：

所以旋转矩阵Rotation为：

$\mathrm{Rotation}=\left(\begin{array}{cc}\cos {\mathrm{\φ}}_{\mathrm{ICCP}}& -\sin {\mathrm{\φ}}_{\mathrm{ICCP}}\\ \sin {\mathrm{\φ}}_{\mathrm{ICCP}}& \cos {\mathrm{\φ}}_{\mathrm{ICCP}}\end{array}\right)$

平移量translation为：

$\mathrm{translation}={\stackrel{-}{Y}}_i-\mathrm{Rotation}\·{\stackrel{-}{P}}_{\mathrm{irand}}^{\′}$

步骤3.3：

利用步骡3.2得到的包括旋转矩阵Rotation和平移矢量translation的刚性变换Trans，对一级随机旋转和平移后的位置序列P′_irand作P′_irand=Trans·P′_irand处理，此时，若迭代次数iteration大于最大迭代次数iteration_max，表明收敛速度过慢，舍弃此次迭代的结果，返回步骤3.1；若iteration小于等于iteration_max并且|Object_iteration-Object_iretration-1|＞τ，则返回步骤3.2，若iteration小于等于iteration_max并且|Object_iteation-Object_ireration-1|≤τ，则迭代终止并记录Object_itetation和Trans·P′_irand的值，iteration_max值为100，τ值为10^-6；

步骤3.4：

重复3.1-3.3步骤，得到50个Object_itetation(Y_i，Trans·P′_irand)值后，找出其中最小的Object_itetation(Y_i，Trans·P′_irand)对应的Trans·P′_irand，得到一级匹配位置序列 $P_i^{\′\′}=\mathrm{Trans}\·P_{\mathrm{irand}}^{\′}=\left(\begin{array}{c}{\mathrm{Lati}}_{P^{\′\′}}\\ {\mathrm{Longti}}_{P^{\′\′}}\end{array}\right),$

代表一级匹配位置序列的纬度，代表一级匹配序列的经度；

步骤4：二级匹配定位

选择以加载地形高程数据库中以一级匹配位置序列P″_i为中心、3×grid为边长的区域，将此区域的高程数据存入一个二级匹配二维矩阵，并利用双线性插值算法在该区域提取高程为实测高程D_i的二级等高线C′_i，在二级等高线C′_i上寻找一级匹配位置序列P″_i的对应垂足得到一个二级垂足序列 $Y_i^{\′}=\left\{\begin{array}{c}{\mathrm{Lati}}_{Y_{i^{\′}}}\\ {\mathrm{Longti}}_{Y_{i^{\′}}}\end{array}\right.,$

代表二级垂足序列的纬度，

代表二级垂足序列的经度；对二级垂足序列Y′_i到一级匹配位置序列P″_i作仿射变换G且所述仿射变换包括在纬度方向上的平移因子t_affinex，在经度方向上的平移因子t_affiney，缩放因子α_affine，旋转因子θ_affine，e_i为二级垂足序列Y′_i与二级匹配定位的输入序列P″_i的经仿射变换后的坐标的差：

$\begin{array}{c}{e}_i=\left(\begin{array}{c}{t}_{\mathrm{affinex}}\\ t_{\mathrm{affiney}}\end{array}\right)+\left(\begin{array}{cc}{\mathrm{\α}}_{\mathrm{affine}}\cos {\mathrm{\θ}}_{\mathrm{affine}}& -{\mathrm{\α}}_{\mathrm{affine}}\sin {\mathrm{\θ}}_{\mathrm{affine}}\\ {\mathrm{\α}}_{\mathrm{affine}}\sin {\mathrm{\θ}}_{\mathrm{affine}}& {\mathrm{\α}}_{\mathrm{affine}}\cos {\mathrm{\θ}}_{\mathrm{affine}}\end{array}\right)P_i^{\text{'}\text{'}}-Y_i^{\text{'}}\\ =\left(\begin{array}{cccc}1& 0& {\mathrm{Lati}}_{P_i^{\text{'}\text{'}}}& -{\mathrm{Longti}}_{P_i^{\text{'}\text{'}}}\\ 0& 1& {\mathrm{Longti}}_{P_i^{\text{'}\text{'}}}& {\mathrm{Lati}}_{P_i^{\text{'}\text{'}}}\end{array}\right)\left(\begin{array}{c}{t}_{\mathrm{affinex}}\\ t_{\mathrm{affiney}}\\ {\mathrm{\α}}_{\mathrm{affine}}\cos {\mathrm{\θ}}_{\mathrm{affine}}\\ {\mathrm{\α}}_{\mathrm{affine}}\sin {\mathrm{\θ}}_{\mathrm{affine}}\end{array}\right)-\left(\begin{array}{c}{\mathrm{Lati}}_{Y_i^{\text{'}}}\\ {\mathrm{Longti}}_{Y_i^{\text{'}}}\end{array}\right)\end{array}$

令仿射变换求解向量为t_affine，仿射变换求解矩阵为

$t_{\mathrm{affine}}=\left(\begin{array}{c}{t}_{\mathrm{affinex}}\\ t_{\mathrm{affiney}}\\ {\mathrm{\α}}_{\mathrm{affine}}\cos {\mathrm{\θ}}_{\mathrm{affine}}\\ {\mathrm{\α}}_{\mathrm{affine}}\sin {\mathrm{\θ}}_{\mathrm{affine}}\end{array}\right)$

$C_{P_i^{\′\′}}=\left(\begin{array}{cccc}1& 0& {\mathrm{Lati}}_{P_i^{\′\′}}& -{\mathrm{Longti}}_{P_i^{\′\′}}\\ 0& 1& {\mathrm{Longti}}_{P_i^{\′\′}}& {\mathrm{Lati}}_{P_i^{\′\′}}\end{array}\right)$

则 $e_i=C_{P_i^{\′\′}}{t}_{\mathrm{affine}}-Y_i^{\′};$

欧式平方距离ESD_affine为：

${\mathrm{ESD}}_{\mathrm{affine}}=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{e_i}^T{e}_i={t_{\mathrm{affine}}}^T{C_{P_i^{\′\′}}}^T{C}_{P_i^{\′\′}}{t}_{\mathrm{affine}}-{2Y}_i^{\′T}{C}_{P_i^{\′\′}}{t}_{\mathrm{affine}}+Y_i^{\′T}{Y}_i^{\′}$

T代表转置；

将使ESD_affine最小的t_affine记为

对ESD_affine求导可得：

$\frac{{\mathrm{dESD}}_{\mathrm{affine}}}{{\mathrm{dt}}_{\mathrm{affine}}}={{2C}_{P_i^{\′\′}}}^T{C}_{P_i^{\′\′}}{t}_{\mathrm{affine}}-{{2C}_{P_i^{\′\′}}}^T{Y}_i^{\′}$

令 $\frac{{\mathrm{dESD}}_{\mathrm{affine}}}{{\mathrm{dt}}_{\mathrm{affine}}}=0$ 可得

${\hat{t}}_{\mathrm{affine}}={\left({C_{P_i^{\′\′}}}^T{C}_{P_i^{\′\′}}\right)}^{-1}{C_{P_i^{\′\′}}}^T{Y}_i^{\′}$

当ESD_affine对t_affine的二阶导数

时，

就是使ESD_affine最小的解：

$\begin{array}{c}{\hat{t}}_{\mathrm{affine}}={\left({C_{P_i^{\′\′}}}^T{C}_{P_i^{\′\′}}\right)}^{-1}{C_{P_i^{\′\′}}}^T{Y}_i^{\′}\\ =\frac{1}{\det }\left(\begin{array}{cccc}{l}_p& 0& -{\mathrm{\μ}}_{\mathrm{xp}}& {\mathrm{\μ}}_{\mathrm{yp}}\\ 0& l_p& {-\mathrm{\μ}}_{\mathrm{yp}}& {-\mathrm{\μ}}_{\mathrm{xp}}\\ {-\mathrm{\μ}}_{\mathrm{xp}}& {-\mathrm{\μ}}_{\mathrm{yp}}& N& 0\\ {\mathrm{\μ}}_{\mathrm{yp}}& {-\mathrm{\μ}}_{\mathrm{xp}}& 0& N\end{array}\right)\left(\begin{array}{c}{\mathrm{\μ}}_{\mathrm{xq}}\\ {\mathrm{\μ}}_{\mathrm{yq}}\\ l_{p+q}\\ l_{p-q}\end{array}\right)\end{array}$

其中：

代表一级匹配位置序列的纬度的之和，

代表二级垂足序列的纬度之和，

代表一级匹配位置序列的经度的之和，

代表二级垂足序列的经度之和，

代表一级匹配位置序列和二级垂足序列的点积之和，代表一级匹配位置序列和二级垂足序列的叉积之和，代表一级匹配位置序列的模的平方之和， $\det =N\×l_p-{\mathrm{\μ}}_{\mathrm{xp}}^2-{\mathrm{\μ}}_{\mathrm{yp}}^2$ 代表矩阵

的模；

由此就可以得到：

${\hat{t}}_{\mathrm{affine}}={({\hat{t}}_{\mathrm{affinex}},{\hat{t}}_{\mathrm{affiney}},{\widehat{\mathrm{\α}}}_{\mathrm{affine}}\cos {\widehat{\mathrm{\θ}}}_{\mathrm{affine}},{\widehat{\mathrm{\α}}}_{\mathrm{affine}}\sin {\widehat{\mathrm{\θ}}}_{\mathrm{affine}})}^T$

其中

为求解所得的平移因子，

为求解所得的缩放因子，

为求解所得的旋转因子；

对一级匹配位置序列作仿射变换得二级匹配位置序列P_i″′：

$P_i^{\text{'}\text{'}\text{'}}=\left(\begin{array}{c}{\mathrm{Lati}}_{P_i^{\text{'}\text{'}\text{'}}}\\ {\mathrm{Longti}}_{P_i^{\text{'}\text{'}\text{'}}}\end{array}\right)=\left(\begin{array}{c}{\hat{t}}_{\mathrm{affinex}}\\ {\hat{t}}_{\mathrm{affiney}}\end{array}\right)+\left(\begin{array}{cc}{\widehat{\mathrm{\α}}}_{\mathrm{affine}}\cos {\widehat{\mathrm{\θ}}}_{\mathrm{affine}}& -{\widehat{\mathrm{\α}}}_{\mathrm{affine}}\sin {\widehat{\mathrm{\θ}}}_{\mathrm{affine}}\\ {\widehat{\mathrm{\α}}}_{\mathrm{affine}}\sin {\widehat{\mathrm{\θ}}}_{\mathrm{affine}}& {\widehat{\mathrm{\α}}}_{\mathrm{affine}}\cos {\widehat{\mathrm{\θ}}}_{\mathrm{affine}}\end{array}\right)P_i^{\text{'}\text{'}}$

代表二级匹配位置序列的纬度，

代表二级匹配位置序列的经度；

步骤5：将二级匹配位置序列P_i″′中的最新位置信息、P₂₀″′作为观测信息，利用卡尔曼滤波器中的间接滤波法估计并修正主惯性导航系统位置误差，卡尔曼滤波器接收惯性导航系统的当前位置信息和最新位置信息P₂₀″′的差值，经过滤波计算，估计出惯性导航系统位置误差量，用估计出的位置误差去修正惯性导航系统输出的位置信息，作为导航位置信息输出。

2.根据权利要求1所述的一种基于水下地形高程数据库的水下航行器辅助导航定位方法，惯性导航系统位置误差量的估计采用如下方法：

步骤5.1：建立惯性导航系统状态方程，选取东北天坐标系O_ENU作为导航坐标系，其中OE为东向轴，ON为北向轴，OU表示天向轴，载体坐标系Oxyz，其中Ox轴沿航行器横轴指向右舷，Oy轴沿航行器纵轴指向前，Oz轴垂直于Ox与Oy轴所确定的平面构成右手坐标系，选取状态向量X_INS为：

其中，δV_E、δV_N、δV_U分别是东向、北向和天向速度误差；φ_E、φ_N、φ_U分别是东向、北向和天向失准角；δLati、δLongti、δh分别是纬度、经度和天向误差；

分别是Ox、Oy、Oz向的加速度计偏置；ε_bx、ε_by、ε_bz分别是Ox、Oy、Oz向的陀螺漂移；

建立惯性导航系统线性状态方程为：

${\stackrel{\·}{X}}_{\mathrm{INS}}=F_{\mathrm{INS}}{X}_{\mathrm{INS}}+W_{\mathrm{INS}}$

其中：

$F_{\mathrm{INS}}=\left[\begin{array}{ccccccccccccccc}{F}_{11}& F_{12}& F_{13}& 0& -f_U& f_N& F_{17}& 0& F_{19}& c_{11}& c_{21}& c_{31}& 0& 0& 0\\ F_{21}& -\frac{V_U}{{\mathrm{Radius}}_n+h}& -\frac{V_N}{{\mathrm{Radius}}_n+h}& f_U& 0& -f_E& F_{27}& 0& F_{29}& c_{21}& c_{22}& c_{32}& 0& 0& 0\\ F_{31}& \frac{{2V}_U}{{\mathrm{Radius}}_n+h}& 0& -f_N& f_E& 0& -2{\mathrm{\ω}}_{\mathrm{ie}}{V}_E\sin \mathrm{Longti}& 0& F_{39}& c_{13}& c_{23}& c_{33}& 0& 0& 0\\ 0& -\frac{1}{{\mathrm{Radius}}_n+h}& 0& 0& F_{45}& F_{46}& 0& 0& \frac{V_N}{{({\mathrm{Radius}}_n+h)}^2}& 0& 0& 0& -c_{11}& -c_{21}& -c_{31}\\ \frac{1}{{\mathrm{Radius}}_n+h}& 0& 0& F_{54}& 0& -\frac{V_N}{{\mathrm{Radius}}_n+h}& -{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{Longti}& 0& \frac{V_E}{{({\mathrm{Radius}}_n+h)}^2}& 0& 0& 0& -c_{12}& -c_{22}& -c_{23}\\ \frac{\tan \mathrm{Longti}}{{\mathrm{Radius}}_n+h}& 0& 0& F_{64}& \frac{V_N}{{\mathrm{Radius}}_n+h}& 0& F_{67}& 0& \frac{V_E\tan \mathrm{Longti}}{{({\mathrm{Radius}}_n+h)}^2}& 0& 0& 0& -c_{13}& -c_{23}& -c_{33}\\ 0& \frac{1}{{\mathrm{Radius}}_n+h}& 0& 0& 0& 0& 0& 0& -\frac{V_N}{{({\mathrm{Radius}}_n+h)}^2}& 0& 0& 0& 0& 0& 0\\ \frac{1}{\left({\mathrm{Radius}}_n+h\right)\cos \mathrm{Longti}}& 0& 0& 0& 0& 0& \frac{V_E\tan \mathrm{Longti}}{(R_e+h)\cos \mathrm{Longti}}& 0& -\frac{V_E}{{({\mathrm{Radius}}_n+h)}^2\cos \mathrm{Longti}}& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ & & & & & & & 0_{6\×15}& & & & & & & \end{array}\right]$

$W_{\mathrm{INS}}={\left[\begin{array}{ccccccccccccccc}{\mathrm{\ω}}_{V_E}& {\mathrm{\ω}}_{V_N}& {\mathrm{\ω}}_{V_U}& {\mathrm{\ω}}_{{\mathrm{\φ}}_E}& {\mathrm{\ω}}_{{\mathrm{\φ}}_N}& {\mathrm{\ω}}_{{\mathrm{\φ}}_U}& {\mathrm{\ω}}_{\mathrm{\δLongti}}& {\mathrm{\ω}}_{\mathrm{\δLati}}& {\mathrm{\ω}}_{\mathrm{\δh}}& 0& 0& 0& 0& 0& 0\end{array}\right]}^T$

$F_{11}=\frac{V_N\tan \mathrm{Longti}}{{\mathrm{Radius}}_n+h}-\frac{V_U}{{\mathrm{Radius}}_e+h},F_{12}=2{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{longti}+\frac{V_E\tan \mathrm{longti}}{{\mathrm{Radius}}_e+h}$

$F_{13}=-(2{\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Longti}+\frac{V_E}{{\mathrm{Radius}}_e+h}),F_{17}=2{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{Longti}\×V_U+2{\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Longti}\×V_N+\frac{V_E{V}_N{\left(\sec \mathrm{Longti}\right)}^2}{{\mathrm{Radius}}_e+h}$

$F_{19}=\frac{V_E{V}_U}{{({\mathrm{Radius}}_e+h)}^2}-\frac{V_E{V}_V\tan \mathrm{Longti}}{{({\mathrm{Radius}}_e+h)}^2},F_{21}=-2{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{Longti}-\frac{V_E\tan \mathrm{Longti}}{{\mathrm{Radius}}_e+h}$

$F_{27}=-(2{\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Longti}{V}_E+\frac{V_E^2{\left(\sec \mathrm{Longti}\right)}^2}{{\mathrm{Radius}}_e+h}),F_{29}=\frac{V_N{V}_U}{{({\mathrm{Radius}}_n+h)}^2}+\frac{V_E^2\tan \mathrm{Longti}}{{(R{\mathrm{adius}}_e+h)}^2}$

$F_{31}=2{\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Longti}+\frac{2V_E}{{\mathrm{Radius}}_e+h},F_{39}=-(\frac{V_N^2}{{(R_n+h)}^2}+\frac{V_E^2\tan \mathrm{Longti}}{{({\mathrm{Radius}}_e+h)}^2})$

$F_{45}={\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{Longti}+\frac{V_E\tan \mathrm{Longti}}{{\mathrm{Radius}}_e+h},F_{46}=-({\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Longti}+\frac{V_E{\left(\sec \mathrm{Longti}\right)}^2}{{\mathrm{Radius}}_e+h})$

$F_{54}=-({\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{Longti}+\frac{V_E\tan \mathrm{Longti}}{{\mathrm{Radius}}_e+h}),F_{64}={\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Longti}+\frac{V_E{\left(\sec \mathrm{Longti}\right)}^2}{{\mathrm{Radius}}_e+h}$

$F_{67}={\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Longti}+\frac{V_E{\left(\sec \mathrm{Longti}\right)}^2}{{\mathrm{Radius}}_e+h}$

其中F₁₁为矩阵F_INS的第1行第1列元素，F₁₂为矩阵F_INS的第1行第2列元素，F₁₃为矩阵F_INS的第1行第3列元素，F₁₇为矩阵F_INS的第1行第7列元素，F₁₉为矩阵F_INS的第1行第9列元素，F₂₁为矩阵F_INS的第2行第1列元素，F₂₇为矩阵F_INS的第2行第7列元素，F₂₉为矩阵F_INS的第2行第9列元素，F₃₁为矩阵F_INS的第3行第1列元素，F₃₉为矩阵F_INS的第3行第9列元素，F₄₅为矩阵F_INS的第4行第5列元素，F₄₆为矩阵F_INS的第4行第6列元素，F₅₄为矩阵F_INS的第5行第4列元素，F₆₄为矩阵F_INS的第6行第4列元素，F₆₇为矩阵F_INS的第6行第7列元素，c_ij为姿态转移矩阵

的元素，F_INS为系统状态转移矩阵；W_INS为系统随机干扰向量，ω_ie为地球自转角速度，Radius_n、Radius_e分别为沿子午圈和卯四圈的主曲率半径，V_E、V_N及V_U为载体真实东向、北向和天向速度，Longti、Lati及h表示载体所处的真实经度、纬度及高程；卡尔曼滤波用离散化模型来描述系统，采用离散化后的差分方程描述连续系统，离散化后的系统状态方程为：

$X_k={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+W_{k-1}$

其中X_k为k时刻的系统状态向量，Φ_k,k-1为k-1到k时刻的一步转移矩阵，

为系统状态向量的估计，W_k-1为k-1时刻的系统噪声，且

${\mathrm{\Φ}}_{k,k-1}=I+\mathrm{\ΔPer}\underset{j=1}{\overset{\mathrm{\σ}}{\mathrm{\Σ}}}{F}_{j-1}$

I为单位矩阵，ΔPer为将滤波周期Per分成σ个时间间隔ΔPer，F_j-1为F_INS在时刻k-1+(j-1)ΔPer时的值；

步骤5.2：建立观测方程，将二级匹配位置序列P_i′″中的最新位置信息P₂₀′″与惯性导航系统输出的经度Longti_INS、纬度Lati_INS位置信息的差值，高程测量装置实测高程D_i的最新实测高程D₂₀与主惯导系统输出高程h_INS的差值构成位置量测向量：

$Z_k=\left[\begin{array}{c}{\mathrm{Longti}}_{\mathrm{INS}}-{\mathrm{Longti}}_{{P_{20}}^{\text{'}\text{'}\text{'}}}\\ {\mathrm{Lati}}_{\mathrm{INS}}-{\mathrm{Lati}}_{{P_{20}}^{\text{'}\text{'}\text{'}}}\\ h_{\mathrm{INS}}-D_{20}\end{array}\right]=H_k{X}_k-{\mathrm{\υ}}_k$

其中： $H_k=\left[\begin{array}{cccccc}& 1& 0& 0& & 0\\ 0_{3\×6}& 0& 1& 0& 0_{3\×6}& 0\\ & 0& 0& 1& & -1\end{array}\right]$ 为观测矩阵，υ_k为观测噪声；

步骤5.3：卡尔曼滤波器误差估计：

步骤5.3.1时间更新：

$\left\{\begin{array}{c}{\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T\end{array}\right.$

步骤5.3.1量测更新：

$\left\{\begin{array}{c}{K}_{k-1}=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^T\\ {\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})\\ P_k=(I-K_k{H}_k)P_{k,k-1}\end{array}\right.$

T代表转置，

为对系统状态向量的估计，P_k为状态估计误差的协方差阵，K_k为滤波增益，

表示对量测值的一步预测，

为新息序列，Qk-1、R_k分别为W_k-1、υ_k的方差阵；给定初始条件X₀和P₀，由Z_k即可得到每一步的状态最优估计

步骤5.4利用当前卡尔曼滤波器得出的误差量最优估计向量

中的位置误差量，修正惯性导航系统位置信息，得到修正后的位置信息

$\tilde{L}{\mathrm{ongti}}_{\mathrm{INS}}={\mathrm{Longti}}_{\mathrm{INS}}+\mathrm{\δ}\hat{L}\mathrm{ongti},{\tilde{L}\mathrm{atin}}_{\mathrm{INS}}={\mathrm{Latin}}_{\mathrm{INS}}+\mathrm{\δ}\hat{L}\mathrm{ati}$ 其中

分别是卡尔曼滤波器输出的纬度、经度误差。

Images