CN102809376A - 一种基于等值线的辅助导航定位方法 - Google Patents

Abstract

本发明公开了一种基于等值线的辅助导航定位方法，具体地说，当航行器开始进行地形/重力/地磁匹配时，在较大的初始匹配误差下，利用基于价值函数的等值线匹配算法在置信区域内找到一条离实际航迹较为接近的航迹来降低惯性导航系统(INS)的初始定位误差；然后，利用ICCP算法的旋转和平移变换获得航迹向及最近点序列信息；最后，利用得到的航迹向及最近点信息在等值线上采用加密的方法得到最佳的匹配航迹。本发明可以解决当前辅助导航系统在大的初始定位误差情况下易发散的问题，提高辅助导航系统的精度及可靠性，尤其适用于存在探测盲区情况下的辅助导航定位。

Description

一种基于等值线的辅助导航定位方法

技术领域

本发明涉及的是一种基于等值线的辅助导航定位方法，属于导航定位技术领域，适用于航空或航海中INS精度不高或INS存在较大位置误差情况下的导航定位。

背景技术

无源自主导航是目前国内外航行器发展的主要趋势之一。惯性导航系统(InertialNavigation System,INS)能够全天候提供较全面的导航信息，且具有较强的工作自主性和实时性、无源性好、不向外部发送信号、抗干扰能力强等诸多优点，使得INS成为航行器中必备的导航设备之一。但是由于INS存在位置误差积累的问题，无法满足潜器水下长期高精度的导航需求。因此为提高导航精度、获得高可靠性的位置信息，必须对INS进行定期修正。

辅助导航系统能很好地解决INS因误差积累而发散的问题，其系统主要由测量装置、INS、存储在数字计算机中的先验地形/重力/地磁数据库及辅助导航算法组成，其中辅助导航算法是辅助导航系统中最关键的技术之一。

目前，常用的辅助导航算法主要分为基于地形/重力/地磁轮廓的匹配算法和基于最优滤波估计的匹配算法两大类。最近几年对轮廓匹配算法的研究主要集中在对等值线迭代最近点算法(ICCP)的研究上，ICCP算法在INS初始位置误差不大的情况下导航精度较高，但在初始匹配误差超出ICCP算法容许范围时，容易导致发散甚至误匹配。在这种情况下，申请号为200810118630.3的发明专利中应用TERCOM(The Terrain Contour Matching)和ICCP两种算法实现了一种组合方法，即用TERCOM算法进行粗匹配，这样可以很大程度上降低INS的积累误差；在此基础上，利用ICCP算法进行精匹配，从而修正惯性导航系统的导航误差。该组合方法取得了很好的效果，并能在很大程度上抑制ICCP算法在大的匹配误差情况下发散的问题。但该组合方式是有局限性的，这是因为TERCOM方法对INS的航向(这里的航向是指航迹向)偏差较为敏感，图3示出在INS没有航向偏差时的TERCOM方法匹配示意图，匹配航迹是与INS指示航迹相平行的一系列高程剖面。实际上，TERCOM方法在匹配过程中隐含了一个条件，即假设匹配时INS的航向偏差为0或很小的角度，但在潜器的实际航行过程中，INS的航向偏差不可能为0，这就对其精度有较高的要求。当航向偏差较大时，INS指示航迹偏离了实际航迹，这时所得到的最佳匹配位置是平行于惯导系统指示航迹的，从而偏离了实际航迹。

发明内容

本发明的目的是为了解决上述问题，针对传统辅助导航算法在INS初始误差较大情况下容易发散的问题，提出一种基于等值线的辅助导航定位方法，本发明能大幅度抑制这种发散并提高INS导航精度的组合导航方法。

一种基于等值线的辅助导航定位方法，具体步骤如下：

步骤一：根据INS的误差置信范围划定搜索区域；

步骤二：在搜索区域内，根据测量装置实时测得的数据提取出等值线；

步骤三：利用价值函数等值线匹配算法，在等值线上找到离真实航迹较为接近的航迹；

步骤四：利用ICCP算法不断地进行旋转和平移变换，获得最终的航迹向和最近点；

步骤五：采用最近点加密的方法，获得最佳匹配航迹，进行导航定位。

本发明的优点在于：

本发明针对目前常用的辅助导航算法不允许INS有较大的初始匹配误差的局限性，提出利用价值函数等值线匹配算法降低INS初始匹配误差，利用加密的ICCP算法进一步提高匹配精度的方法，本发明所述方法在大的初始匹配误差下仍能达到很高的定位精度且增加了系统的可靠性。

附图说明

图1是本发明的方法流程图；

图2是本发明的最近点加密方法原理图；

图3是背景技术中TERCOM方法匹配示意图；

图4是在初始匹配误差为6.0′时，ICCP算法仿真图；

图5是在初始匹配误差为6.0′时，ICCP算法最终仿真图；

图6是在初始匹配误差为6.0′时，本发明所述方法的仿真图。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明的一种基于等值线的辅助导航定位方法，原理是：当航行器开始进行辅助导航时，在较大的初始匹配误差下，利用基于价值函数的等值线匹配算法在置信区域内进行一次大范围搜索，找到一条离实际航迹较为接近的航迹来降低INS的位置误差，从而形成待匹配航迹；在此基础上利用ICCP算法进行进一步匹配，获得精确的航迹向和最近点信息；加密的方法是利用上述得到的航迹向和最近点信息在等值线上连续地寻找使价值函数最小的航迹作为最终匹配航迹，从而很大程度上提高了辅助导航精度及可靠性，可以满足航行器在INS大的位置误差情况下的导航需求。

本发明是一种基于等值线的辅助导航定位方法，流程如图1所示，包括以下几个步骤：

步骤一：根据INS（惯性导航系统）的误差置信范围划定搜索区域；

搜索区域是以X为中心，以3倍的INS估计位置误差为半边长的正方形区域，其中，X表示由INS输出的当前的地形/重力/地磁位置点。

步骤二：在搜索区域内，根据测量装置实时测得的数据提取出等值线；

具体为：

航行器匀速行驶进某一匹配区Ω后，测量装置实时测得的地形/重力/地磁数据经过数据处理后，获得M个地形/重力/地磁数据点序列，利用该M个数据点序列从预存的地形/重力/地磁数据库中提取出相应的等值线，并在步骤一的搜索区域内截取等值线，记为C_i,i＝1,2,…,M。

同时，由INS获得M个地形/重力/地磁位置点X_i,i＝1,2,…,M和前M-1个位置点对应的航向信息，这里M个地形/重力/地磁位置点分别对应于M条等值线。将M个相邻的位置点做差获得M-1个相对距离，与前M-1个位置点的航向信息一起作为价值函数等值线匹配算法的输入量。

步骤三：利用价值函数等值线匹配算法，在等值线上找到离真实航迹接近的航迹，从而形成待匹配航迹；

具体为：在等值线间通过搜索、跟踪和决策来连续的寻找使价值函数最小的两点作为最佳匹配位置点，在等值线上找到离真实航迹接近的航迹，形成待匹配航迹。

所述的价值函数等值线匹配算法是依据INS在短时间内两点间相对距离与实际的相对距离偏差较小和航向精度很高的特点而来。价值函数等值线匹配算法不依赖于INS初始匹配位置，由此能在INS大的初始位置误差下工作，同时该算法需要航行器在两个匹配点保持直线航行。孙枫等人提出价值函数等值线匹配算法并给出了基本原理及可行性分析可参见，孙枫，王文晶，高伟等.用于无源重力导航的等值线匹配算法.仪器仪表学报，2009。本发明利用基于价值函数等值线匹配算法对初始位置调优，从而降低INS初始定位误差，形成待匹配航迹，在此基础上，利用ICCP算法进行精匹配。

步骤四：利用ICCP算法不断地进行旋转和平移变换，获得最终的航迹向和最近点；

利用ICCP算法在初始误差较小时能给出精确的航迹向及最近点信息，从而利用该信息为最近点加密方法做好准备。

所述的利用ICCP算法获得最终的航迹向和最近点具体方法为：

根据ICCP算法利用四元数进行旋转变换的基本原理，对整条待匹配航迹进行旋转平移变换，旋转角度为α_k，其公式如下：

$\left\{\begin{array}{c}{X}_{i,k+1}=R\left({\mathrm{\α}}_k\right)\×X_{i,k}+T_{i,k}\\ T_{i,k}=P_{g,k}-R\left({\mathrm{\α}}_k\right)\×X_{g,k}\\ R\left({\mathrm{\α}}_k\right)=\left(\begin{array}{cc}\cos {\mathrm{\α}}_k& -\sin {\mathrm{\α}}_k\\ \sin {\mathrm{\α}}_k& \cos {\mathrm{\α}}_k\end{array}\right)\\ P_{g,k}=\frac{1}{W}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{w}_{i,k}{P}_{i,k},X_{g,k}=\frac{1}{W}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{w}_{i,k}{X}_{i,k}\\ w_{i,k}=\frac{1}{\left|\right|X_{i,k}-P_{i,k}\left|\right|}\end{array}\right.$

式中X_i，k表示待匹配航迹经过第k次旋转变换后的第i个点所在的位置，X_i，k+1表示待匹配航迹经过第k+1次旋转变换后的第i个点所在的位置，T_i，k表示经过第k次旋转后第i个点的平移量，P_g，k、X_g，k分别表示经过第k次旋转后的最近点和匹配航迹点的重心，R(α_k)为由α_k组成的反对称旋转矩阵，P_i，k表示经过第k次旋转后第i个位置点所对应的最近点，权值w_i，k为经过第k次旋转后待匹配航迹点X_i，k到相应最近点P_i，k距离的倒数，W为权值w_i，k之和。

经过最后一次旋转变换后，获得最终的位置点X_i，k所对应的最近点P_i，k和相邻两点X_i，k、X_i+1，k所形成的航迹向

步骤五：采用最近点加密的方法，获得最佳匹配航迹。

具体包括以下几个步骤：

（1）计算相邻距离，根据INS给定的位置坐标点X_i,0（i＝0,1,2,…,M），分别计算X_i，0到X_i+1，0间的距离L_i，其中M表示所需匹配的总点数，即：

L_i＝||X_i+1，0-X_i,0||

（2）分段等值线，在搜索区域所截取的等值线C_i上找到利用ICCP算法最后一次旋转变换的位置点X_i，k所对应的最近点P_i，k，然后采用加密的方法以P_i,k为中心，以Δ为步长将等值线C_i划分为2×m段，确保真实位置点包含在m×Δ范围内，每个分段点记为

然后以

为起始点，沿X_i,k到X_i+1，k的方向(最后一次旋转后的航迹向

)向等值线C_i+1分别作射线，如图2所示，相交点记为

其中j表示存在相交点的段数，满足j≤l。

其中，所述的求解相交点

的方法为：构造两条直线，即经过分段点

方向为

的一条直线和由等值线C_i+1上的两点顺次连接所组成的任意一条直线(已用角度判别法去除不相交的线段)，利用上述两条直线联立方程组，从而求得相交点

其中，所述的步长Δ的选取方法为：由于方位陀螺漂移和北向陀螺漂移，引起INS在经度方向上的位置误差随时间增长而发散，若取一般精度的INS（陀螺漂移为0.01°/h），其位置误差约为1n mile/h，选取测量装置采样周期为5min，则所选取的INS在5min之内位置误差大约为0.083′，本发明在该直线上取小间隔的间距时，即以该位置误差为基准，采取折中的办法，考虑到INS呈震荡性发散，则取每个间距为0.0415′，间隔连续地取为10个，即上下共取20个数据点，这样可以保证定位误差包含在该区域内，从而降低误匹配的可能。

（3）求取价值函数，求取匹配时刻所形成的每一条航迹的价值函数

其中a表示可能形成的航迹条数，N_p表示所需采样点的总个数。判断使最小的价值函数D_a(P_i,L_i)，其公式如下式所示：

$\left\{\begin{array}{c}{D}_a(P_i,L_i)=\underset{a\∈\{\mathrm{1,2},...,N_h\}}{\min }\left\{J_{a,N_p}(P_i,L_i)\right\}\\ J_{a,N_p}(P_i,L_i)=\underset{i=2}{\overset{N_p}{\mathrm{\Σ}}}{\left|\right|P_i-L_i\left|\right|}^2\\ P_i=\left|\right|P_{i+1,k}^j-P_{i,k}^j\left|\right|\end{array}\right.$

式中：N_h表示将N_p个等值线分段后，各段线段所能形成的航迹的总个数，满足N_h≤2m×(N_p-1)。由此D_a(P_i,L_i)所对应的匹配点即为最佳匹配点，D_a(P_i,L_i)所形成的航迹即为最佳的匹配航迹。

其中，所述在所有的匹配航迹中，使价值函数

最小的匹配航迹可能不止一条，判断依据是选取最接近最近点的位置作为最佳匹配点，而去除相对较远的点。

最后，将最佳的匹配航迹输出至组合导航滤波器，组合导航滤波器导航输出，进行导航定位。

实施例：

仿真实验采用分辨率为0.5′×0.5′，网格大小为72×82的数字地图。任意设一航迹作为实际航迹，INS指示航迹为是由实际航迹旋转15°后经过若干平移变换而得到的，在纬度方向上取初始匹配位置误差为6.0′，ICCP算法采样点数取为9点，如图4所示。图中方形虚线为实际航迹，粗菱形线为INS指示航迹，细菱形线表示ICCP算法的匹配航迹。图5为最终的ICCP算法匹配结果，表1给出初始位置误差为6.0′时ICCP匹配误差统计结果。

表1

	min/(′)	max/(′)	mean	var	std
						经度误差	0.1846	2.5131	1.3240	0.7026	0.8382
纬度误差	1.0317	7.9765	3.8426	5.0953	2.2573

在上述同样的条件下，采用本发明的基于等值线的辅助导航定位方法对其进行仿真实验，加密时在相应等值线最近点取小间隔的间距，取每个小间距为0.0415′，间隔连续地取为10个，上下共取20个数据点，仿真结果如图6所示。表2为初始匹配位置误差为6.0′时等值线的辅助导航定位方法误差统计表。

表2

	min/(′)	max/(′)	mean	var	std
						经度误差	0.0174	0.5383	0.2347	0.0265	0.1629

纬度误差

0.1047

0.4065

0.2004

0.0075

0.0864

从以上的仿真结果可以看出，在较大的初始匹配误差下，本发明的基于等值线的辅助导航定位方法的匹配精度要比传统的ICCP算法精度要高。从而可以解决当前辅助导航系统在大的初始定位误差情况下易发散的问题，提高辅助导航系统的精度及可靠性，尤其适用于存在探测盲区情况下的辅助导航定位，能够满足一般航行器的导航要求。

Claims (4)

1.一种基于等值线的辅助导航定位方法，其特征在于，包括以下几个步骤：

步骤一：根据INS的误差置信范围划定搜索区域；

搜索区域是以X为中心，以3倍的INS估计位置误差为半边长的正方形区域，其中，X表示由INS输出的当前的地形/重力/地磁位置点；

所述的INS为惯性导航系统；

步骤二：在搜索区域内，根据测量装置实时测得的数据提取出等值线；

具体为：

航行器匀速行驶进某一匹配区Ω后，测量装置实时测得的地形/重力/地磁数据经过数据处理后，获得M个地形/重力/地磁数据点序列，利用该M个数据点序列从预存的地形/重力/地磁数据库中提取出相应的等值线，并在步骤一的搜索区域内截取等值线，记为C_i,i＝1,2,…,M；

同时，由INS获得M个地形/重力/地磁位置点X_i,i＝1,2,…,M和前M-1个位置点对应的航向信息，M个地形/重力/地磁位置点分别对应于M条等值线；将M个相邻的位置点做差获得M-1个相对距离，与前M-1个位置点的航向信息一起作为步骤三中价值函数等值线匹配算法的输入量；

步骤三：利用价值函数等值线匹配算法，在等值线上找到离真实航迹接近的航迹，从而形成待匹配航迹；

在等值线间通过搜索、跟踪和决策来连续的寻找使价值函数最小的两点作为最佳匹配位置点，在等值线上找到离真实航迹接近的航迹，形成待匹配航迹；

步骤四：利用ICCP算法不断地进行旋转和平移变换，获得最终的航迹向和最近点；

所述的ICCP为等值线迭代最近点算法，步骤四具体包括：

根据ICCP算法利用四元数进行旋转变换的基本原理，对整条待匹配航迹进行旋转平移变换，旋转角度为α_k，其公式如下：

$\left\{\begin{array}{c}{X}_{i,k}=R\left({\mathrm{\α}}_k\right)\×X_{i,k-1}+T_{i,k}\\ T_{i,k}=P_{g,k}-R\left({\mathrm{\α}}_k\right)\×X_{g,k}\\ R\left({\mathrm{\α}}_k\right)=\left(\begin{array}{cc}\cos {\mathrm{\α}}_k& -\sin {\mathrm{\α}}_k\\ \sin {\mathrm{\α}}_k& \cos {\mathrm{\α}}_k\end{array}\right)\\ P_{g,k}=\frac{1}{W}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{w}_{i,k}{P}_{i,k},X_{g,k}=\frac{1}{W}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{w}_{i,k}{X}_{i,k}\\ w_{i,k}=\frac{1}{\left|\right|X_{i,k}-P_{i,k}\left|\right|}\end{array}\right.$

式中：X_i,k表示待匹配航迹经过第k次旋转变换后的第i个点所在的位置，X_i，k-1表示待匹配航迹经过第k-1次旋转变换后的第i个点所在的位置，T_i,k表示经过第k次旋转后第i个点的平移量，P_g,k、X_g,k分别表示经过第k次旋转后的最近点和匹配航迹点的重心，R(α_k)为由α_k组成的反对称旋转矩阵，P_i,k表示经过第k次旋转后第i个位置点所对应的最近点，权值w_i,k为经过第k次旋转后待匹配航迹点X_i,k到相应最近点P_i,k距离的倒数，W为权值w_i，k之和；

经过最后一次旋转变换后，获得最终的位置点X_i,k所对应的最近点P_i，k和相邻两点X_i，k、X_i+1，k所形成的航迹向

步骤五：采用最近点加密的方法，获得最佳匹配航迹；

具体包括以下几个步骤：

（1）计算相邻距离，根据INS给定的位置坐标点X_i，0，i＝1,2,…,M，分别计算X_i，0到X_i+1，0间的距离L_i，其中M表示所需匹配的总点数，即：

L_i＝||X_i+1，0-X_i,0||

（2）分段等值线，在搜索区域所截取的等值线C_i上找到利用ICCP算法最后一次旋转变换的位置点X_i，k所对应的最近点P_i，k，然后采用加密的方法以P_i,k为中心，以Δ为步长将等值线C_i划分为2×m段，确保真实位置点包含在m×Δ范围内，每个分段点记为

l＝0,1,2,…,2m，然后以

为起始点，沿X_i,k到X_i+1，k的方向，即最后一次旋转后的航迹向

向等值线C_i+1分别作射线，相交点记为

其中j表示存在相交点的段数，满足j≤l；

（3）求取价值函数，求取匹配时刻所形成的每一条航迹的价值函数

其中a表示可能形成的航迹条数，N_p表示所需采样点的总个数；判断使

最小的价值函数D_a(P_i,L_i)，其公式如下式所示：

$\left\{\begin{array}{c}{D}_a(P_i,L_i)=\underset{a\∈\{\mathrm{1,2},...,N_h\}}{\min }\left\{J_{a,N_p}(P_i,L_i)\right\}\\ J_{a,N_p}(P_i,L_i)=\underset{i=2}{\overset{N_p}{\mathrm{\Σ}}}{\left|\right|P_i-L_i\left|\right|}^2\\ P_i=\left|\right|P_{i+1,k}^j-P_{i,k}^j\left|\right|\end{array}\right.$

式中：N_h表示将N_p个等值线分段后，各段线段所能形成的航迹的总个数，满足N_h≤2m×(N_p-1)；由此D_a(P_i,L_i)所对应的匹配点即为最佳匹配点，D_a(P_i,L_i)所形成的航迹即为最佳的匹配航迹；

最后，将最佳的匹配航迹输出至组合导航滤波器，组合导航滤波器导航输出，进行导航定位。

2.根据权利要求1所述的一种基于等值线的辅助导航定位方法，其特征在于，步骤五（2）中，所述的求解相交点

的方法为：构造两条直线，即经过分段点

方向为

的一条直线和由等值线C_i+1上的两点顺次连接所组成的任意一条直线，其中已用角度判别法去除不相交的线段，利用上述两条直线联立方程组，从而求得相交点

3.根据权利要求1所述的一种基于等值线的辅助导航定位方法，其特征在于，步骤五（2）中，所述的步长Δ的选取方法为：取步长Δ为0.0415′，间隔连续地取为10个，即上下共取20个数据点。

4.根据权利要求1所述的一种基于等值线的辅助导航定位方法，其特征在于，步骤五（3）中，在所有的匹配航迹中，当使价值函数

最小的匹配航迹不止一条时，选取最接近最近点的位置作为最佳匹配点，去除相对较远的点。

Images