专利名称：一种基于等值线的辅助导航定位方法
技术领域：
本发明涉及的是一种基于等值线的辅助导航定位方法，属于导航定位技术领域，适用于航空或航海中INS精度不高或INS存在较大位置误差情况下的导航定位。
背景技术：
无源自主导航是目前国内外航行器发展的主要趋势之一。惯性导航系统(Inertial Navigation 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算法进一步提高匹配精度的方法，本发明所述方法在大的初始匹配误差下仍能达到很高的定位精度且增加了系统的可靠性。
图I是本发明的方法流程图；图2是本发明的最近点加密方法原理图；图3是背景技术中TERCOM方法匹配示意图；图4是在初始匹配误差为6. 0'时，ICCP算法仿真图；图5是在初始匹配误差为6. 0'时，ICCP算法最终仿真图；图6是在初始匹配误差为6. 0'时，本发明所述方法的仿真图。
具体实施例方式下面将结合附图和实施例对本发明作进一步的详细说明。本发明的一种基于等值线的辅助导航定位方法，原理是当航行器开始进行辅助导航时，在较大的初始匹配误差下，利用基于价值函数的等值线匹配算法在置信区域内进行一次大范围搜索，找到一条离实际航迹较为接近的航迹来降低INS的位置误差，从而形成待匹配航迹；在此基础上利用ICCP算法进行进一步匹配，获得精确的航迹向和最近点信息；加密的方法是利用上述得到的航迹向和最近点信息在等值线上连续地寻找使价值函数最小的航迹作为最终匹配航迹，从而很大程度上提高了辅助导航精度及可靠性，可以满足航行器在INS大的位置误差情况下的导航需求。本发明是一种基于等值线的辅助导航定位方法，流程如图I所示，包括以下几个步骤步骤一根据INS (惯性导航系统)的误差置信范围划定搜索区域；搜索区域是以X为中心，以3倍的INS估计位置误差为半边长的正方形区域，其中，X表示由INS输出的当前的地形/重力/地磁位置点。步骤二 在搜索区域内，根据测量装置实时测得的数据提取出等值线；具体为航行器匀速行驶进某一匹配区Q后，测量装置实时测得的地形/重力/地磁数据经过数据处理后，获得M个地形/重力/地磁数据点序列，利用该M个数据点序列从预存的地形/重力/地磁数据库中提取出相应的等值线，并在步骤一的搜索区域内截取等值线，记为 C”i = 1，2，…，M。同时，由INS获得M个地形/重力/地磁位置点Xi, i = 1，2，…，M和前M_1个位置点对应的航向信息，这里M个地形/重力/地磁位置点分别对应于M条等值线。将M个相邻的位置点做差获得M-I个相对距离,与前M-I个位置点的航向信息一起作为价值函数等值线匹配算法的输入量。步骤三利用价值函数等值线匹配算法，在等值线上找到离真实航迹接近的航迹，从而形成待匹配航迹；具体为在等值线间通过搜索、跟踪和决策来连续的寻找使价值函数最小的两点作为最佳匹配位置点，在等值线上找到离真实航迹接近的航迹，形成待匹配航迹。所述的价值函数等值线匹配算法是依据INS在短时间内两点间相对距离与实际的相对距离偏差较小和航向精度很高的特点而来。价值函数等值线匹配算法不依赖于INS 初始匹配位置，由此能在INS大的初始位置误差下工作，同时该算法需要航行器在两个匹配点保持直线航行。孙枫等人提出价值函数等值线匹配算法并给出了基本原理及可行性分析可参见，孙枫，王文晶，高伟等.用于无源重力导航的等值线匹配算法.仪器仪表学报，2009。本发明利用基于价值函数等值线匹配算法对初始位置调优，从而降低INS初始定位误差，形成待匹配航迹，在此基础上，利用ICCP算法进行精匹配。步骤四利用ICCP算法不断地进行旋转和平移变换，获得最终的航迹向和最近占.利用ICCP算法在初始误差较小时能给出精确的航迹向及最近点信息，从而利用该信息为最近点加密方法做好准备。所述的利用ICCP算法获得最终的航迹向和最近点具体方法为根据ICCP算法利用四元数进行旋转变换的基本原理，对整条待匹配航迹进行旋转平移变换，旋转角度为ak，其公式如下
A、，= R(Ch)WlWlk
I, =X
(cosor, -Sinori')
Riak)=..
I ^I M
Ph = Uyw hp_h X ^-Yw lX , g, Pf/ 今 '* I” 『今 a f Jr
=p^T T|式中Xi,k表示待匹配航迹经过第k次旋转变换后的第i个点所在的位置，Xijk+1表示待匹配航迹经过第k+1次旋转变换后的第i个点所在的位置，Ti, k表示经过第k次旋转后第i个点的平移量，Pg,k、Xg,k分别表示经过第k次旋转后的最近点和匹配航迹点的重心，R(ak)为由a,组成的反对称旋转矩阵，Pi,,表示经过第k次旋转后第i个位置点所对应的最近点，权值Wi, k为经过第k次旋转后待匹配航迹点Xi, k到相应最近点Pi, k距离的倒数，W为权值Wi, k之和。经过最后一次旋转变换后，获得最终的位置点Xi, k所对应的最近点Pi, k和相邻两点Xi^XiIk所形成的航迹向；
步骤五采用最近点加密的方法，获得最佳匹配航迹。具体包括以下几个步骤(I)计算相邻距离，根据INS给定的位置坐标点Xi,Q Ci = 0,1,2,…，M)，分别计算Xij0到Xi+u间的距离Li,其中M表示所需匹配的总点数，即Li = I I Xpli0-Xi,01(2)分段等值线，在搜索区域所截取的等值线Ci上找到利用ICCP算法最后一次旋转变换的位置点Xi,k所对应的最近点Pi,k，然后采用加密的方法以Pu为中心，以A为步长将等值线Ci划分为2Xm段，确保真实位置点包含在mX A范围内，每个分段点记为4(/ = 0丄2,-%2/ ),然后以圮为起始点，沿1到\+1,1;的方向(最后一次旋转后的航迹向Hm)向等值线Ci+1分别作射线，如图2所示，相交点记为，其中j表示存在相交点的段数，满足j < I。 其中，所述的求解相交点/仏,—的方法为构造两条直线，即经过分段点& 方向为尤的一条直线和由等值线Ci+1上的两点顺次连接所组成的任意一条直线(已用角度判别法去除不相交的线段)，利用上述两条直线联立方程组，从而求得相交点d— o其中，所述的步长A的选取方法为由于方位陀螺漂移和北向陀螺漂移，引起INS在经度方向上的位置误差随时间增长而发散，若取一般精度的INS (陀螺漂移为0.01° /h)，其位置误差约为In mile/h，选取测量装置采样周期为5min，则所选取的INS在5min之内位置误差大约为0.083'，本发明在该直线上取小间隔的间距时，即以该位置误差为基准，采取折中的办法，考虑到INS呈震荡性发散，则取每个间距为0. 0415'，间隔连续地取为10个，S卩上下共取20个数据点，这样可以保证定位误差包含在该区域内，从而降低误匹配的可能。(3)求取价值函数，求取匹配时刻所形成的每一条航迹的价值函数入；化4) 其中a表示可能形成的航迹条数，Np表示所需采样点的总个数。判断使人、《AM最小的价值函数Da (Pi, Li)，其公式如下式所示
权利要求
1.一种基于等值线的辅助导航定位方法，其特征在于，包括以下几个步骤 步骤一根据INS的误差置信范围划定搜索区域； 搜索区域是以X为中心，以3倍的INS估计位置误差为半边长的正方形区域，其中，X表示由INS输出的当前的地形/重力/地磁位置点； 所述的INS为惯性导航系统； 步骤二 在搜索区域内，根据测量装置实时测得的数据提取出等值线； 具体为 航行器匀速行驶进某一匹配区Q后，测量装置实时测得的地形/重力/地磁数据经过数据处理后，获得M个地形/重力/地磁数据点序列，利用该M个数据点序列从预存的地形/重力/地磁数据库中提取出相应的等值线，并在步骤一的搜索区域内截取等值线，记为Ci, i = 同时，由INS获得M个地形/重力/地磁位置点Xi, i = 1，2，…，M和前M-I个位置点对应的航向信息，M个地形/重力/地磁位置点分别对应于M条等值线个相邻的位置点做差获得M-I个相对距离,与前M-I个位置点的航向信息一起作为步骤三中价值函数等值线匹配算法的输入量； 步骤三利用价值函数等值线匹配算法，在等值线上找到离真实航迹接近的航迹，从而形成待匹配航迹； 在等值线间通过搜索、跟踪和决策来连续的寻找使价值函数最小的两点作为最佳匹配位置点，在等值线上找到离真实航迹接近的航迹，形成待匹配航迹； 步骤四利用ICCP算法不断地进行旋转和平移变换，获得最终的航迹向和最近点； 所述的ICCP为等值线迭代最近点算法，步骤四具体包括 根据ICCP算法利用四元数进行旋转变换的基本原理，对整条待匹配航迹进行旋转平移变换，旋转角度为ak，其公式如下 式中Xi，k表示待匹配航迹经过第k次旋转变换后的第i个点所在的位置，Xm表示待匹配航迹经过第k-l次旋转变换后的第i个点所在的位置，Tu表示经过第k次旋转后第i个点的平移量，Pg,k> Xg,k分别表示经过第k次旋转后的最近点和匹配航迹点的重心，R(ak)为由a k组成的反对称旋转矩阵，Pu表示经过第k次旋转后第i个位置点所对应的最近点，权值Wu为经过第k次旋转后待匹配航迹点Xu到相应最近点Pu距离的倒数，W为权值Wi, k之和； 经过最后一次旋转变换后，获得最终的位置点所对应的最近点Pi, k和相邻两点Xi,k、xi+1,k所形成的航迹向t ; 步骤五采用最近点加密的方法，获得最佳匹配航迹； 具体包括以下几个步骤 (1)计算相邻距离，根据INS给定的位置坐标点XiMi = 1，2，…，M，分别计算Xi,。到Xi+1,0间的距离Li,其中M表示所需匹配的总点数，即 Li = I Xi+lj0-Xi；0 (2)分段等值线，在搜索区域所截取的等值线Ci上找到利用ICCP算法最后一次旋转变换的位置点Xi,k所对应的最近点Pi,k，然后采用加密的方法以Pu为中心，以A为步长将等值线Ci划分为2Xm段，确保真实位置点包含在mX A范围内，每个分段点记为/ . 1=O，I, 2，…，2m，然后以/匕为起始点，沿Xu到Xi+1,k的方向，即最后一次旋转后的航迹向A^u^,；，向等值线Ci+1分别作射线，相交点记为/^;ti.，其中j表示存在相交点的段数，满足j ^ I ; (3)求取价值函数，求取匹配时刻所形成的每一条航迹的价值函数人^(6，4)，其中a表示可能形成的航迹条数，Np表示所需采样点的总个数；判断使最小的价值函数Da (Pi, Li),其公式如下式所示
2.根据权利要求I所述的一种基于等值线的辅助导航定位方法，其特征在于，步骤五(2)中，所述的求解相交点/丨.^的方法为构造两条直线，即经过分段点圮,方向的一条直线和由等值线Ci+1上的两点顺次连接所组成的任意一条直线，其中已用角度判别法去除不相交的线段，利用上述两条直线联立方程组，从而求得相交点/丨^
3.根据权利要求I所述的一种基于等值线的辅助导航定位方法，其特征在于，步骤五(2)中，所述的步长A的选取方法为取步长A为0.0415'，间隔连续地取为10个，即上下共取20个数据点。
4.根据权利要求I所述的一种基于等值线的辅助导航定位方法，其特征在于，步骤五(3)中，在所有的匹配航迹中，当使价值函数人、(尽最小的匹配航迹不止一条时，选取最接近最近点的位置作为最佳匹配点，去除相对较远的点。
全文摘要
本发明公开了一种基于等值线的辅助导航定位方法，具体地说，当航行器开始进行地形/重力/地磁匹配时，在较大的初始匹配误差下，利用基于价值函数的等值线匹配算法在置信区域内找到一条离实际航迹较为接近的航迹来降低惯性导航系统(INS)的初始定位误差；然后，利用ICCP算法的旋转和平移变换获得航迹向及最近点序列信息；最后，利用得到的航迹向及最近点信息在等值线上采用加密的方法得到最佳的匹配航迹。本发明可以解决当前辅助导航系统在大的初始定位误差情况下易发散的问题，提高辅助导航系统的精度及可靠性，尤其适用于存在探测盲区情况下的辅助导航定位。
文档编号G01C21/16GK102809376SQ201210276928
公开日2012年12月5日 申请日期2012年8月6日 优先权日2012年8月6日
发明者李宁, 张红伟, 张勇刚, 刘利强, 齐昭 申请人:哈尔滨工程大学