专利名称：地形辅助导航方法和设备的制作方法
技术领域：
本发明提供了一种地形辅助导航方法和设备，该方法和设备 特别在初始误差较大的情况下，使用低精度惯性导航系统
(Inertial Navigation System, INS)完成匹配，实现巡航导 弹、低空无人机、飞机、潜艇、水下机器人等的低成本高精度导 航。
背景技术：
在巡航导弹等低空飞行器或潜艇的导航中， 一般需要有较高 精度的INS,但是高精度INS造价高，而且，高精度INS的导航误差 也随运行时间累积，致使INS无法独立长时间工作。所以，需要其 它辅助导航方法来修正INS的累积误差，以保持导航系统的长时间 高精度工作。相对于全球定位系统(Global Positioning System, GPS)等其它辅助导航方法，地形辅助导航具有如下优 势在低空飞行器中，无地形遮蔽，适合于贴地飞行或地形掩 蔽；在水下潜艇中，不受水体遮蔽影响。但是，目前的地形辅助 导航方法对INS精度的依赖性仍比较大。因此，研究一种在低精度 INS条件下的地形辅助导航方法在降低应用成本和小型化等方面都 是有实际意义的。
当载体运行到地形可导航区之后，INS—般具有较大的位置和 姿态误差，因而需要使用适合于大初始误差的快速定位方式，地 形轮廊匹西己(Terrain Contour Matching, TERC0M)方'法可k乂实 现大初始位置误差时的快速搜索，可以很快找到大致的匹配区 域，但其精度受INS精度影响较大，在大航迹形状误差条件下无法 正常工作；迭代最近等值点(Iterative Closest Contour Point, ICCP)算法利用高程等值线信息修正INS航迹，使INS航迹 逐渐向真实航迹靠拢，达到降低姿态误差的目的，因而减小了匹
配方法对INS精度依赖性，但ICCP算法需要较小的初始定位误差才 能稳定工作。本发明充分结合了TERCOM和ICCP算法的优点，进行 了优势互补，可以应用在巡航导弹和飞机这类空中飞行器上，也 可应用在潜艇和水下机器人等水下载体的地形辅助导航方面，适 合于低精度I NS条件下的长时间航行。
地形辅助导航方法的基本工作原理如图l所示，首先将匹配区 域的地形数字化，并以格网数字地形的形式存贮在导航计算机 中，当载体通过已经数字化的匹配地形区域时，用高程测量设备 101测量载体所在位置处的高程值，得到对应于栽体航迹的一条地 形剖面的地形高程采样值/序列，另外通过INS 103可知载体真实 位置的大体范围，在此范围内将测得的地形高程值/序列与预先存 储在导航计算机中的数字地图104—同输入导航计算机102中，进 行匹配计算，从而得到匹配的位置，再将该匹配位置反馈给INS 103, 1多正INS累积误差。
TERCOM的工作过程如图2所示。该图是以INS当前输出位置为 中心，位置估计误差方差的3倍为半边长，构成初始搜索窗口，其 中每个格网的顶点表示一个高度数据(图2中共有17xl7个高程数 据)。载体在真实航迹上运行，每隔若干个格网距离采集一个高 程数据并记录此时的INS位置，假设截至A时刻累计的高程组成一 个高程采样序列，将该高程采样序列与搜索窗口内所有可能的方 向相同、长度一致的基准地图内的假定航迹逐一进行相关分析 (图2中共有17xl7条假定航迹)，具有最佳相关值的位置即被确 定为栽体真实位置的估计，即匹配位置，如图2中的三角形点所示。
TERCOM能够进行大范围的搜索并迅速收敛；不过要求INS航迹 形状和航向都要与真实航迹相近，不能有太大的偏差，否则会因 为搜索窗口内寻找到的假定航迹与真实航迹偏差太大而导致数据 错位，匹配误差加大直至发散。
如图3所示是ICCP算法的基本原理图。载体真实航迹由f' (/ = 1, 2, L,『)点组成，其中『是航迹的长度(点数)；INS 航迹由f点组成；另外，地形测量传感器测量到当地实际地形 c,,每一高度数据对应于地形图中的一条等高线。由于INS存在测
量误差，S与f'之间不可避免地存在一定的偏差。ICCP算法的思想就是f 一定位于c,等值线上或附近，那么可以按照一定原 则，使《"靠拢到c,上，找到最优估计点,和航迹，实现对INS的误
差补偿。这实际上是一种最优化过程，对于由『个点所组成的曲 线段，唯一可以调整的是这些点都相应位于各自等值线，而且可 以在各自等值线上滑动，最终使目标函数值达到最小。以总的误 差作为目标函数，即
<formula>formula see original document page 9</formula> (1)
式中￡为目标函数，即总误差；"(P, q)为P与q之间的距离；x,
为估计点,的位置，y,为x,离等值线最近的点的位置；a,为INS指
示位置；《为刚度系数。
这里认为INS初始误差为零，如图3所示c。等高线上的真实位 置、INS位置和估计位置重合；按照最优化原理，对式(1)的目 标函数进行优化调整，即改变估计点的位置，使￡最小，并认为 该序列是对当前真实航迹的最优估计，即完成对,序列的估计。
如图4所示为载体的速度误差和方向误差对一步航行的影响， 其中A为载体速度误差，《为载体航向误差，1),+1为沿航行方向的 单位矢量，^为水平面内与b,w相垂直方向的单位矢量。由图4可 以得到式(2)。
<formula>formula see original document page 9</formula> ( 2 )
由于A和《均为小量，所以式(2)可以近似为式(3)。
<formula>formula see original document page 9</formula> (3 )
式中<formula>formula see original document page 9</formula>，其中a。表示本次匹配的前面一 点INS的位置。将式(3 )代入式(1 )可得式(4 )。
<formula>formula see original document page 9</formula> (4)
在已知a,、 a和《的前提下，x, (/ = 1， 2, L,『)可以才艮据 式(3)计算出来，因此x,是^和《的函数。另外，y,是根据x,推 算出来的，因此，y,也是A和e,的函数。
优化过程就是对式(4)进行最优化处理，由于x,和y,均是p, 和《的函数，因此优化变量就是^和《，目标函数是￡。
在《趋于0时，由于航迹的任何小的变化都可能导致目标函数
值大幅度变化，因此，只能不对航迹进行任何调整，而是通过刚 性变换(旋转和平移)使目标函数趋于最优值。
如上所述，基本ICCP算法适合于INS航迹形状偏差较大的情 况；但是该方案要求INS初始误差很小，另外刚性变换方案的ICCP 算法要求INS的形状误差很小，所以目前的ICCP算法都不能够满足 实际应用的需求。
发明内容
本发明解决的技术问题是在大初始误差和低精度INS的条件
下，研究一种能够可靠进行辅助导航的地形匹配方法和设备，优
点是能够大范围搜索，快速收敛，修正INS的航迹形状，降低导航
系统的成本，提高导航的精度。
根据本发明的一个方面，提供了一种适合于空中或水下的地
形辅助导航方法，其可用于大初始误差和低精度INS的地形辅助导
航，其特征在于包括
确定一个包含当前INS位置的窗口 ；
在所述窗口中，采用TERCOM方法计算粗匹配位置；
利用所述粗匹配位置^"正所述INS位置，在粗匹配位置上叠加
I NS航迹形状作为待调整航迹；
使用ICCP算法对该待调整航迹进行调整，使待调整航迹向真
实航迹接近。
根据本发明的一个进一步的方面，提供了一种适合于空中或 水下的地形辅助导航设备，其可用于大初始误差和低精度INS的地 形辅助导航，其特征在于包括
导航位置窗口确定装置，用于确定一个包含当前INS位置的窗
o;
粗匹配位置计算装置，用于在所述窗口中采用TERCOM方法计 算粗匹配位置；
待调整航迹生成装置，用于利用所述粗匹配位置修正所述INS 位置，在粗匹配位置上叠加INS航迹形状作为待调整航迹；
航迹调整装置，用于使用ICCP算法对该待调整航迹进行调 整，使待调整航迹向真实航迹接近。本发明的原理是利用TERC0M以及加权平均以防止误匹配的 方法选取粗匹配位置，提高匹配方法的快速性和鲁棒性，在粗匹 配位置上叠加INS航迹形状作为待调整航迹，使用ICCP算法将待调 整航迹向着真实航迹靠拢；利用等值线修正待调整航迹，即是用 高程信息修正INS航迹，利用单纯形调优法寻找到最小的目标函数 值，同时利用概率数据关联算法提高精匹配的可靠性，从而获得 最优匹配航迹。
本发明与现有技术相比的优点在于相关方法适合于航迹形 状误差较小的情况下，因而要求INS精度较高；卡尔曼滤波方法同 样要求INS精度较高；单纯的ICCP算法需要初始误差较小或航迹形 状偏差小。本发明的方法能够适应较低精度的INS航迹，能够对 INS航迹进行修正，这是以前技术中所未曾有的。很多ICCP算法釆 用刚性变换的方法实现待匹配航迹与真实航迹的配准，但是其应 用受到I NS航迹形状误差的极大制约；本发明采用非刚性变换的方 法，可以实现航迹的任意变换，可以更加逼近真实航迹的形状。
图1为地形辅助导航方法结构 图2为TERCOM方法示意图 图3为ICCP算法原理图
图4为载体速度和方向误差对位置误差的影响
图5为本发明所使用的ICCP算法原理图
图6为(A,《)选取范围示意图
图7为两次优化位置分布示意图
图8为方法实现流程图
图9为粗匹配实现流程图
图10为ICCP调整实现流程图
图11为误匹配判断实现流程图
具体实施例方式
本发明的流程如图8所示。 (1 )本发明中TERCOM方法所采用的基本算法包括:
互相关算法(Cross Correlat ion， CC或Product correlation similarity measure, Prod), 归一化互相关算法 (Normalized Product correlation similarity measure, NProd);绝对差算法(Absolute Difference, AD)，平方差算 法(Square Difference, SD)，平均绝对差算法(Mean Absolute Difference, MAD),平均平方差算法(Mean Square Difference, MSD);改进的绝对差、改进的平方差算法，例如利 用衰减系数改进的；基于Hausedorff距离的匹配算法。
(2) 本发明首先进行步骤801确定粗匹配搜索窗口采用3cr 原则，以INS位置为中心开一个6x6o"的方形搜索窗口。
(3) 本发明采用一种衰减绝对差方法作为TERCOM方法来实现 粗匹配802。图9是粗匹配802的进一步流程图。该方法是一种迭代 计算方法，将衰减绝对差方法的量测记为"AD。参考图2，步骤 901进行获取搜索窗口内数字地图的过程，然后步骤902定义yt时 刻的单点误差表示为
<formula>formula see original document page 12</formula> ( 5 )
其中附、"表示格网相对窗口中心的位置；w表示A:时刻 的测量高程；^W(A+w,A+")表示地图中的高程值；A、 A表示的 是搜索窗口的中心位置。
定义aAD如式(6 )所示，图2中，"AD为一17xl7大小的矩阵。<formula>formula see original document page 12</formula><formula>formula see original document page 12</formula>
步骤903选取矩阵aAD中最小的f个值，步骤904判断上述^个 点位置的分布方差是否小于设定的阈值，"是"则说明粗匹配方 法收敛，程序进行到步骤9 06以利用加权平均法计算粗匹配位置， "否"则说明粗匹配方法还没有收敛，程序进行到步骤905,以 INS位置作为定位输出。
设满足要求的,个点位置表示为<formula>formula see original document page 12</formula>对应权重 为A(",步骤906中的权重计算式为式(7)。
<formula>formula see original document page 13</formula>最后的估计位置表示为式(8)。
<formula>formula see original document page 13</formula>(8)
步骤803利用粗匹配位置和INS航迹形状累计待调整航迹，步 骤907判断ICCP调整的点数是否累积达到『个，"是"则进行ICCP 调整804,"否"则进行步骤908输出粗匹配位置作为最终匹配位 置。
(4)所使用的ICCP调整804如下
本发明中ICCP调整804的过程如图10所示。ICCP方法是一种批 处理方法，需要进行步骤803累计足够数目的待调整航迹点才能进 行ICCP调整804,如果还没有足够数目，此时应以粗匹配位置作为 定位输出(步骤908 ); —旦数目达到要求，进行ICCP调整804。
图5为改进后的ICCP方法，P。"表示上次匹配位置，其范围依 赖于整个地形辅助导航方法的精度；P。'为真实位置；A表示粗匹 配位置。
改进后的ICCP算法为利用粗匹配位置叠加INS航迹形状形成 待调整航迹，取代INS航迹；仍采用式(4)所示的目标函数，不 过此时的3,(/ = 1,2,...,)为待调整航迹，a。为上次最终匹配位置。
步骤1001中A和《的初值随机选取，但是由于受到粗匹配的 影响，各点的变化范围有所不同。如图6所示。户。"点表示上次最 终匹配位置(即a。)，《"与INS的关系并不显著，而(A,《) 为两者之间进行误差转移时的参数， 一般要使估计位置《"位于可 能范围之内，即图6中的圆型区域。因此(化《)的值一般比较 大，视粗匹配精度而定。此后的估计航迹应该与INS航迹具有一定 的相关性，所以(p2,《)可以根据INS速度和航向偏差方差的3cr
原则选取；其它点都是在前一点的基础上增加一个小随机量即可 (小随机数视INS短时漂移量大小而定)。如式(9)所示。
<formula>formula see original document page 13</formula>x,和y,均为A和《的函数，利用初始化的A和《可以进行步骤 1002，计算得出估计位置x,和最近点位置y,，继而利用式(4)进 行步骤1003,计算目标函数值，最后ICCP算法转化为多参数寻优 的数学问题，利用步骤1004进行改进单纯形调优法经过反射、扩 张和收缩来寻找最优解，并在步骤1005中将该最优解作为该次的 最优位置输出。
(5)在lCCP调整804过程中调整结果的i吴匹配判断中，所使 用的概率数据关联防止误匹配方法如图ll所示。
为了获得全局最优，对单纯形优化共计进行e组iccp调整，
其目标函数收敛到Emn或者迭代次数超过阈值 ^时，记录此时的 目标函数五值作为匹配的指标。
给较小的￡值优化位置以较大概率，给较大的￡值优化位置 以较小概率，并保证总的概率等于l,首次概率计算见式(IO)。
如。 (10)
其中A, = 1,2,...,2)是各次优化位置的概率。
步骤1101:将前次的优化位置的概率记为A.U'",2,…,2)，本
次的优化位置的概率记为w=i,2,.,e)，将两次的iNs位置重合，
可以把两次优化的位置放在一个图中，图7所示为合为一图的示意 图，其中共有前次和本次优化的各2个位置以及前次和本次的INS 位置点，各优化位置都对应着一个编号。
步骤1102:假定本次的每一个优化位置都可能来自于前次的 任何一个优化位置，如果INS定位误差服从N(o,)的高斯分布，那
么前次优化位置的第/点对本次优化位置的第!点作用的概率密度 如式(ll)所示。
冲|7') =+/^ (11)
其中血为/、 /两个优化位置经过INS整合后的距离，即前次优化 第)'点在本次优化中的预测位置与本次优化位置第/点之间的距 离。
步骤1103:本次的e个优化位置都可能来自于前次优化位置 中的第/点，可以根据其概率密度来决定本次各点的条件概率，
如式(12)所示。
劇=^^ (12)
步骤l 104:利用式(12)和全概率公式可以获得前次2个优化 位置对本次第/个优化位置的影响，即笫/个优化位置的联合概 率，如式(13)所示。
w)=i;/n('i)') (13)
步骤1105:在g值的基础上加入联合概率，匹配概率的计算 如式(14)所示。
A、攀' (14) 选取最大概率的优化位置作为本次最终匹配结果。
权利要求
1、一种适合于空中或水下的地形辅助导航方法，其可用于大初始误差和低精度惯性导航系统的地形辅助导航，其特征在于包括A)确定一个包含当前惯性导航位置的窗口；B)在所述窗口中，采用TERCOM方法计算粗匹配位置；C)利用所述粗匹配位置修正所述惯性导航位置，在粗匹配位置上叠加惯性导航航迹形状作为待调整航迹；D)使用ICCP算法对该待调整航迹进行调整，使待调整航迹向真实航迹接近。
2、 根据权利要求l所述的地形辅助导航方法，其特征在于 步骤B)所使用的粗匹配定位方法的包括互相关算法、归一化互 相关算法、绝对差算法、平方差算法、平均绝对差算法、平均平 方差算法、改进的绝对差、改进的平方差算法、和/或基于 Hausedorff距离的匹配算法。
3、 根据权利要求l所述的地形辅助导航方法，其特征在于 步骤B)进一步包括对于粗匹配位置因为地形相似性而出现的较大跳变，利用加 权平均方法获得比较可靠的匹配位置；利用TERC0M方法所计算得出的参数计算权值。
4、 根据权利要求l所述的地形辅助导航方法，其特征在于 步骤D)所使用的ICCP算法进一步包括把粗匹配位置叠加INS航迹形状，形成待调整航迹； 按照真实航迹位于c,等值线附近的规律，根据一个预定规 则，使待调整航迹靠拢到c,上，并确定最优估计航迹，从而实现 对INS的误差补偿，其中所述预定原则为下式所表示的目标函数￡ 取最小值式中五为目标函数即总误差；r为待调整航迹所包含的点数； x,为估计点的位置，y,为x,离等值线最近的点的位置；《为刚度系 数；a为载体速度误差；《为载体的航向误差；<formula>formula see original document page 3</formula>且<formula>formula see original document page 3</formula>；a,为待调整航迹指示位置；a。为上次最终匹配位置；其中a和《的初值随机选取，且(a,《)的范围根据粗匹配 精度决定；(a,《)根据INS速度、航向偏差方差的3cr原则选 取；其它点按照如下原则选取<formula>formula see original document page 3</formula>其中小随机数视INS短时漂移量大小而定； 其中x,和y,均为a和《的函数，其中ICCP算法转化为多参数寻优的数学问题，利用改进单纯 形调优法经过反射、扩张和收缩来寻找最优的匹配位置。
5、根据权利要求l所述的地形辅助导航方法，其特征在于 步骤D)所使用的ICCP算法进一步包括Dl)为实现全局最优，采用随机采样方式，即以待调整航迹为基准，对p,和s进行随机采样，共计采样Q组，然后对这e组优化初值分别进行ICCP最优化计算，其目标函数收敛到￡min或者迭 代次数超过阈值7^时，记录此时的目标函数￡值作为匹配的指 标；D2 )如下式计算前次优化位置上第/点对本次优化位置的第z' 点作用，其中而为前次优化第j"点在本次优化中的预测位置与本次优 化位置第!点之间的距离，^为预测位置误差方差；D3)在本次的2个优化位置都可能来自于前次优化位置中的 第尸点的情况下，根据其概率密度来决定本次各点的条件概率；D4)利用全概率公式，获得前次G个优化位置对本次第z个优 化位置的影响，即第/个优化位置的联合概率，其中A,'表示前次各优化位置的概率D5)在g值的基础上加入联合概率，匹配概率的计算表示为D6)选取最大概率的优化位置作为最终匹配结果，然后再顺 序执行上述步骤D1)、 D2)、 D3)、 D4)、 D5),从而获得一系列的更 可靠的匹配结果。
6、 一种适合于空中或水下的地形辅助导航设备，其可用于大 初始误差和低精度惯性导航系统的地形辅助导航，其特征在于包 括导航位置窗口确定装置，用于确定一个包含当前惯性导航位 置的窗口；粗匹配位置计算装置，用于在所述窗口中采用TERCOM方法计 算粗匹配位置；待调整航迹生成装置，用于利用所述粗匹配位置修正所述惯 性导航位置，在粗匹配位置上叠加惯性导航航迹形状作为待调整 航迹；航迹调整装置，用于使用ICCP算法对该待调整航迹进行调 整，使待调整航迹向真实航迹接近。
7、 根据权利要求6所述的地形辅助导航设备，其特征在于 所述所使用的粗匹配定位方法的包括互相关算法、归一化互相关算法、绝对差算法、平方差算法、平均绝对差算法、平均 平方差算法、改进的绝对差、改进的平方差算法、和/或基于 Hausedorff距离的匹配算法。
8、 根据权利要求6所述的地形辅助导航设备，其特征在于 所述粗匹配位置计算装置进一步包括跳变4务正部分，用于对于粗匹配位置因为地形相似性而出现 的较大跳变，利用加权平均方法获得比较可靠的匹配位置；权值计算部分，用于利用TERC0M方法所计算得出的参数计算 权值。
9、 根据权利要求6所述的地形辅助导航设备，其特征在于 所使用的ICCP算法进一步包括把粗匹配位置叠加INS航迹形状，形成待调整航迹； 按照真实航迹位于c,等值线附近的规律，根据一个预定规 则，使待调整航迹靠拢到c,上，并确定最优估计航迹，从而实现 对INS的误差补偿，其中所述预定原则为下式所表示的目标函数￡ 取最小值<formula>formula see original document page 5</formula>式中E为目标函数即总误差；W为待调整航迹所包含的点数； x1为估计点的位置，y1为x1离等值线最近的点的位置；Q为刚度系数；p1为栽体速度误差；Q1为载体的航向误差；<formula>formula see original document page 5</formula>,且<formula>formula see original document page 5</formula>a1为待调整航迹指示位置；a。为上次最终匹配位置；其中p1和Q1的初值随机选取，且(a,Q)的范围根据粗匹配精度决定；(A，Q)根据INS速度、航向偏差方差的3c原则选取；其它点按照如下原则选取<formula>formula see original document page 5</formula>其中小随机数视INS短时漂移量大小而定； 其中x1和y1均为p,和Q的函数，其中ICCP算法转化为多参数寻优的数学问题，利用改进单纯 形调优法经过反射、扩张和收缩来寻找最优的匹配位置。
10、 根据权利要求6所述的地形辅助导航设备，其特征在于所 述航迹调整装置所使用的ICCP算法进一步包括Dl)为实现全局最优，采用随机采样方式，即以待调整航迹为基准，对a，Q进行随机釆样，共计采样e组，然后对这e组优化初值分别进行ICCP最优化计算，其目标函数收敛到￡mm或者迭 代次数超过阈值7时，记录此时的目标函数￡值作为匹配的指 标；D2 )如下式计算前次优化位置上第/点对本次优化位置的第/ 点作用，<formula>formula see original document page 6</formula>其中""为前次优化第/点在本次优化中的预测位置与本次优 化位置第/点之间的距离，"为预测位置误差方差；D3)在本次的e个优化位置都可能来自于前次优化位置中的第尸点的情况下，根据其概率密度来决定本次各点的条件概率；D4)利用全概率公式，获得前次e个优化位置对本次第/个优 化位置的影响，即第/个优化位置的联合概率，其中A.'表示前次 各优化位置的概率D5)在A值的基础上加入联合概率，匹配概率的计算表示为A:=lD6)选取最大概率的优化位置作为最终匹配结果，然后再顺 序执行上述步骤D1)、 D2)、 D3)、 D4)、 D5)，从而获得一系列的更 可靠的匹配结果。
全文摘要
本发明设计了一种适合于空中(巡航弹、无人飞机、直升机等)或水下(潜艇、水下机器人、AUV、UUV等)的，尤其在初始定位误差和航向误差较大的情况下，使用低精度惯性导航系统实现的地形辅助导航方法和设备。该发明包括首先利用TERCOM方法以及加权平均寻找粗匹配位置修正惯导航迹形成待调整航迹；然后使用迭代最近等值点算法对具有较大航向偏差的待调整航迹进行调整；为了防止出现误匹配，将多次寻优的目标函数与匹配的位置相结合，引入概率数据关联方法提高匹配的可靠性。本发明能够在较低的惯导精度下完成匹配，降低巡航导弹和潜艇等低空飞行器和水下载体的导航成本。
文档编号G01C21/16GK101339036SQ200810118630
公开日2009年1月7日 申请日期2008年8月20日 优先权日2008年8月20日
发明者勇 杨, 王可东 申请人:北京航空航天大学