CN107314768B

CN107314768B - 水下地形匹配辅助惯性导航定位方法及其定位系统

Info

Publication number: CN107314768B
Application number: CN201710548124.7A
Authority: CN
Inventors: 何林帮; 邱振戈; 杨彬
Original assignee: Shanghai Ocean University
Current assignee: Shanghai Ocean University
Priority date: 2017-07-06
Filing date: 2017-07-06
Publication date: 2020-06-09
Anticipated expiration: 2037-07-06
Also published as: CN107314768A

Abstract

本发明公开了水下地形匹配辅助惯性导航定位方法及其定位系统，主要包括以下步骤：(1)提出闭环运用卡尔曼滤波最优估计、开环运用速度回溯进行误差补偿；(2)提出顾及水下潜航器姿态角的精确三维声线跟踪模型，并给出波束脚印内采样点的提取方法；(3)提出基于水深约束和分形维理论Delauney三角网建立水下数字地形模型方法；(4)提出基于灰色决策理论的水下地形适配区评价方法；(5)提出基于自适应高斯和滤波的水下地形匹配辅助导航方法。本发明运用了闭环和开环的误差补偿方法，较大地削弱了惯导系统的累积误差，并通过构建高精度高分辨率的水下数字地形模型和提出自适应高斯和滤波算法，极大地提高了水下潜航器导航定位的精度。

Description

水下地形匹配辅助惯性导航定位方法及其定位系统

技术领域

本发明涉及水下地形匹配辅助惯性导航技术，特别涉及水下地形匹配辅助惯性导航定位方法及其定位系统。

背景技术

海洋开发和利用海洋资源，尤其是开发海底矿物和油气资源作为我国“海洋强国”战略的一个非常重要组成部分，而水下地形匹配辅助惯导技术为海底资源的探测提供高精度的位置坐标，并具有隐蔽性，便于更高效地开发海底资源，在海洋经济发展中扮演着不可或缺的基础保障作用。随着海洋产业的迅速发展，如在海底打捞、渔业资源开发、海底电缆铺设、跨海大桥建设、海底矿物和油气资源开发等方面，需要提供水下目标的精确坐标，便于更快捷、更高效地开发利用这些资源。同时，在军事领域，潜艇在整个国防战略中的地位也越来越重要，随着艇载导弹发射精度和水下工程精密性要求的提高，水下潜航器自身对位置的精度要求也就越来越高，尤其是在长时间、远距离、深海条件下航行时，如何获取高精度的载体位置也成为水下潜航器最主要的性能指标之一。目前，惯性导航系统(InertialNavigation System，INS)的定位误差随时间推移而累积，无法满足水下潜航器长时间隐蔽地在水下航行的高精度定位需求(Anonsen et al.,2007)，而海底地形体现出丰富的地形特征，可通过地形匹配辅助惯性导航系统消除长期累积的误差，因此，基于多源数据的水下高精度组合导航技术研究迫在眉睫。

地形匹配导航研究已有30多年的历史，作为INS的辅助手段，地形匹配在陆地范围内应用于飞机和导弹上较多。而水下地形匹配辅助导航是在陆地地形匹配辅助导航的基础上新发展起来的一项辅助INS进行水下导航的技术，可为AUV、ROV等水下潜航器提供导航定位服务(Zhang et al.,2012)。近几十年来，随着世界各国对海洋资源开发和海洋科学研究的力度不断加大，以及海洋测量设备的不断进步更新，地形匹配辅助惯性导航系统因其导航精度较高、无累积误差且费效比低等优点，引起了国内外学者的广泛关注。

地形匹配辅助导航于20世纪70年代开始出现，在90年代迅速发展和成熟，在军事领域成为一种重要的导航技术。按照地形信息类型分类，地形辅助导航技术主要分为景象匹配区域相关技术(Scene Matching Area Correlator，SMAC) 和地形高度匹配技术(Terrain Elevation Matching，TEM)(柏菁et al.,2003)。 SMAC技术利用红外线传感器或者光学等其他传感器获取飞越地区的地形图像，然后将其与存储的基准景象进行比较，通过景象匹配算法处理，获得飞行器的精确定位。由于SMAC技术利用地面可辨认的线性特征(例如道路、河流、边界等)，而不是利用地形高度数据提供精确定位，因而在起伏不大的地形上更有效。因为地面可辨认的线性特征一般不是连续分布的，所以SMAC 技术一般不能进行连续匹配定位，通常只在相隔为几公里的距离提供离散的精确定位(赵锋伟et al.,2002)。TEM技术是通过测量飞行器获取其正下方的地形高度，与存储的参考高程地图进行比较获得飞行器的位置信息。这种技术的特点是自主、隐蔽、全天候，不受季节变化和天气条件的影响，在恶劣的天气和夜间都可以正常使用。与SMAC技术相比，TEM技术利用地形高程信息，由于地形高程相对稳定，不受季节、气候和光照等条件的影响，而且TEM技术对测绘能力的要求相对较低(冯庆堂et al.,2005)。狭义的地形辅助导航技术特指地形高度匹配技术，目前主要有TERCOM(Terrain Contour Matching)系统、 SITAN(Sandia IntertialTerrain Aided Navigation)系统和TERPROM(Terrain Profile Matching)系统三种地形高度匹配技术。TERCOM系统是美国E-Systm 公司于1958年为超音速低空导弹研制的一种导航技术，其研发目的主要是为导弹进行INS偏差校正(Golden,1980)，此项技术在美国空军应用广泛，是最早提出的解决地形高度匹配问题的方法。此种地形匹配方法，具有计算简单，性能较稳定的优点。由于这种方法使用INS观测值计算不同历元地形观测值彼此之间的平面相对位置关系，但是在匹配中却忽视了由INS观测值造成的上述相对位置的不确定性，因此其匹配精度难免受到影响，目前该技术的圆概率误差为30～100米(冯庆堂,2004)。为了解决TERCOM不能进行载体位置的连续估计这一缺点，出现了基于扩展卡尔曼滤波的SITAN系统(Hollowell,1990)。 SITAN系统全称为桑迪亚惯性地形辅助导航系统，是美国Sandia实验室于上世纪80年代末开始研制的一套地形匹配导航算法。该方法通过对地形进行线性化，使之可以应用于递推扩展卡尔曼滤波进行载体位置的连续估计。然而，对于非线性程度高的地形，SITAN的线性化方法所得到的结果并不理想。此外，该方法对于载体起始位置的误差要求也相对苛刻。这两个缺点限制了该方法的进一步应用。该系统在开始工作时，INS的定位误差不能太大，一般要求小于 200米，它的圆概率误差优于75米(冯庆堂,2004)。由于SITAN的线性化方法所引起的问题，Enns和Morrel于1995年提出了基于Viterbi算法的地形辅助导航系统VATAN(Enns and Morrell,1995)。与SITAN对地形进行线性化不同， VATAN将载体位置视为一阶马尔科夫过程，并在每一时刻估计载体的位置概率分布，并记录概率分布可能性最大的位置传递到下一时刻，从而实现序列连续估计。虽然该方法被证明取得了优于SITAN算法的实验结果，但是在实际导航中并未得到广泛的应用。与上述两种方法相反，Kamgar-Parsi于1999年提出了一种以等值线点为匹配单元的导航匹配算法，称为Iterative Closest Contour Point(ICCP)算法(Kamgar-Parsi and Kamgar-Parsi,1999)。该算法是一种基于几何学原理的匹配方法，最初源于图像配准的ICP算法，在等值线上寻找最小度量意义下的全局最优值。国内外很多学者对其进行了研究，取得了一些成果(王胜平,2011)。然而，由于该算法使用迭代法对真实航迹位置进行逼近，因此匹配结果是否为全局最优是一个值得关注的问题。此外，由于该方法匹配结果精度难以估计，因此相邻的匹配结果信息难以使用滤波算法进行融合。 TERPROM系统为英国不列颠宇航公司开发的，是目前世界上应用最广的一种地形辅助导航系统。Bae公司开发的INS/GPS/TERPROM组合导航系统的水平定位精度(CEP)小于20米，垂直均方根误差小于5米(冯庆堂,2004)。

从21世纪90年代开始，国内外学者开始考虑在序列连续估计中对载体位置的全概率分布进行追踪，并利用贝叶斯全概率公式对载体位置进行测量更新，从而提高地形匹配辅助导航的精度和稳定性。在这方面，Niclas Bergmam 率先采用点群滤波(Point MassFilter，PMF)来近似的描述飞机在飞行时的位置概率分布(Bergman and Ljung,1997)。随后，Bergmam又使用粒子滤波(Particle Filter，PF)对同一问题进行了研究，从而开启了基于贝叶斯估计的地形匹配辅助导航相关方法的研究(Bergman et al.,1999)。此后，使用数值计算方法通过贝叶斯公式进行地形匹配导航引起了学者们的广泛兴趣，点群滤波和粒子滤波被成功的应用于水下潜航器进行地形匹配辅助导航，用于实时估计水下载体的位置，例如著名的HUGIN水下潜航器和Saab水下潜航器等等。而在2003年， Karlsson等人利用粒子滤波进行了载体航行方向以及航行速度的实时估计 (Karlsson et al.,2003)。Nygren通过对水下地形批匹配算法进行研究，提高了导航的精度(Nygren,2008)。而Anonsen则对水下地形匹配导航中点群滤波和粒子滤波的特点进行了比较(Anonsen and Hallingstad,2006)，得出了有益的结论。以上研究结果都是基于精密海图(分辨率优于10米)以及高精度的INS得出的。对此，2011年斯坦福大学的Meduna等人对水下地形辅助导航中，如何使用低分辨率地图(20米至50米)和低精度INS系统进行有效地形辅助导航进行了研究(Meduna etal.,2009；Meduna,2011)，并得到了很好的实验结果。然而上述导航算法都是基于实际地形数据中的误差与预设噪声模型相符这一前提的，并未考虑实际导航环境中粗差的影响。

国内目前对水下地形匹配导航的研究尚处于起步阶段。哈尔滨工程大学的刘承香于2003最早发表了关于水下地形匹配导航研究的博士论文(刘承香, 2003)。现阶段，仅西北工业大学，中国海洋大学、北京航空航天大学、海军工程大学、武汉大学、国防科技大学、东南大学以及哈尔滨工程大学等几家单位开展了基于地形匹配导航系统相关方面的研究。然而，目前的研究大多基于仿真实验的研究结果，而进入试验阶段的完整地形匹配导航系统尚未出现。

由此可见，现有技术仍存在以下缺陷：

(1)当使用卡尔曼滤波进行水下地形导航时，现有误匹配检测方法没有考虑地图误差影响与实测数据误差影响的区别。在导航中，很可能因此出现误匹配漏判的现象，造成滤波结果不稳定。

(2)目前水下地形匹配算法都是针对正常的噪声环境。然而，背景和实测海床地形数据中常常出现难以避免的粗差。如果粗差的影响不能得到有效探测和排除，会造成导航结果的精度下降，甚至引起滤波发散。

(3)水下地形匹配导航中，当水下地形较为平缓时，由于地形信息的不足，可能会使地形匹配结果的质量受到影响。因此在导航算法中，很有必要将这一因素加以考虑，以排除贫信息地形在滤波结果中造成的影响，以提高导航算法的稳定性。

发明内容

本发明要解决的技术问题是针对惯性导航系统误差随时间推移累积的问题，首先在惯性导航系统中进行闭环和开环误差补偿；然后利用多波束测量的地形数据构建高精度高分辨率的数字地形模型；再次将惯导系统输出的位置估值信息和地形匹配结果信息同时输入滤波器，进行滤波计算，获得潜器当前位置的后验估值；最后将潜器当前位置的后验估值修正惯性导航系统的运动状态参数，从而进一步提高惯性导航系统的导航定位精度。

为达到上述目的，本发明提供的水下地形匹配辅助惯性导航定位方法，包括以下步骤：

(1)建立误差模型；

(2)闭环运用卡尔曼滤波最优估计对惯性导航系统进行闭环误差补偿，并且开环运用速度回溯补偿进行开环误差补偿以便校正由于随时间推移累积产生的误差；

(3)建立顾及水下潜航器姿态角的精确三维声线跟踪模型，获取高精度的波束脚印坐标，并给出波束脚印内采样点的提取方法，以获得高分辨率的多波束采样点；

(4)根据基于水深约束和分形维理论Delauney三角网建立水下数字地形模型，建立高分辨率高精度的水下数字地形模型，为地形匹配提供可靠的依据；

(5)基于灰色决策理论的水下地形适配区评测，建立可靠的适导性指标矩阵，以便后期的最优地形匹配；

(6)在上述水下高精度微地形匹配基础上，基于自适应高斯和滤波的水下地形匹配辅助导航，将地形匹配结果和惯导系统输出的位置估值进行滤波，获取位置的最优误差估计值，以修正最初惯导系统输出的定位参数，从而提高地形匹配辅助导航的精度和效率。

所述误差模型建立包括姿态误差模型建立、速度误差模型建立和位置误差模型建立。

所述闭环误差补偿采用下列算法：

并且，修正后获得t_k时刻的四元数将迭代进行t_k+1时刻的姿态矩阵更新计算中。

所述开环误差补偿采用分为速度回溯拟合补偿算法和速度回溯预测补偿算法，其中，

所述三维声线跟踪模型采用下列算法：

本发明要解决的技术问题主要包括如下几个方面：

(1)提出闭环运用卡尔曼滤波最优估计进行误差补偿，开环运用速度回溯补偿进行误差补偿；

(2)提出顾及水下潜航器姿态角的精确三维声线跟踪模型，并给出波束脚印内采样点的提取方法，获取高精度高分辨率的多波束采样点；

(3)提出基于水深约束和分形维理论Delauney三角网建立水下数字地形模型方法，建立高分辨率高精度的水下数字地形模型；

(4)提出基于灰色决策理论的水下地形适配区评价方法，建立可靠性较高的适导性指标矩阵，提供最优地形匹配方案；

(5)在实现水下高精度微地形匹配基础上，提出基于自适应高斯和滤波的水下地形匹配辅助导航方法，将地形匹配结果和惯导系统输出的位置估值进行滤波，获取位置的最优误差估计值，以期修正最初惯导系统输出的定位参数，提高地形匹配辅助导航的精度和效率。

通过上述技术方案，本发明的有益效果是：

(1)提出顾及水下潜航器姿态角的精确三维声线跟踪模型，并给出波束脚印内采样点的提取方法，同时基于水深约束和分形维理论Delauney三角网建立高分辨率高精度的水下数字地形模型；

(2)在实现水下高精度微地形匹配基础上，提出基于自适应高斯和滤波的水下地形匹配辅助导航方法，将地形匹配结果和惯导系统输出的位置估值进行滤波，获取位置的最优误差估计值，极大地提高地形匹配辅助导航的精度和效率。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明的水下地形匹配辅助惯性导航系统关键技术总体路线；

图2是本发明的惯性导航系统估计/回溯补偿方法原理；

图3是本发明的换能器基阵旋转角度模型；

图4是本发明的波束点空间旋转示意图；

图5是本发明的常梯度声线跟踪示意图；

图6是本发明的水下三维数字地形模型建立流程；

图7是本发明的水下地形匹配辅助导航中AGSF算法流程。

具体实施方式

为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体图示，进一步阐述本发明。

首先，本发明涉及如下技术术语：

惯性导航系统

惯性导航系统(INS，以下简称惯导)是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统(丁国强,2010)。其工作环境不仅包括空中、地面，还可以在水下。惯导的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。

卡尔曼滤波

卡尔曼滤波(Kalman filtering)一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法(祁芳,2003)。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

声线跟踪法

声线跟踪是建立在声速剖面基础上的一种波束脚印(投射点)相对船体坐标系坐标的计算方法(何林帮et al.,2015)。声线跟踪通常采用层追加方法，即将声速剖面内相邻两个声速采样点划分为一个层，层内声速变化可假设为常值 (零梯度)或常梯度。对于前者，计算思想和过程简单，后者相对比较复杂。在后者的声线跟踪计算过程中，声速变化函数采用Harmonic平均声速。

分形维理论

如果一个数据集在所有的观察尺度下都具有自相似性，即一个数据集的部分分布有着与整体分布相似的结构或属性，称该数据集是分形的。自相似原则和迭代生成原则是分形理论的重要原则(陈国雄,2016)。它表征分形在通常的几何变换下具有不变性，即标度无关性。由自相似性是从不同尺度的对称出发，也就意味着递归。分形形体中的自相似性可以是完全相同，也可以是统计意义上的相似。标准的自相似分形是数学上的抽象，迭代生成无限精细的结构，如科赫曲线(Koch snowflake)、谢尔宾斯基地毯(Sierpinski carpet)等。根据自相似性的程度，分形可以分为有规分形和无规分形，有规分形是指具体有严格的自相似性，即可以通过简单的数学模型来描述其相似性的分形，比如三分康托集、Koch曲线等；无规分形是指具有统计学意义上的自相似性的分形，比如曲折连绵的海岸线，漂浮的云朵等。

Delauney三角网

Delaunay三角网是Voronoi图的对偶图，由对应Voronoi多边形共边的点连接而成(李涛,2012)。Delaunay三角形由三个相邻点连接而成，这三个相邻点对应的Voronoi多边形有一个公共的顶点，此顶点同时也是Dlaunay三角形外接圆的圆心。下图显示了Voronoi图与Dlaunay三角网的关系，其中，实线为Delaunay三网，虚线为Voronoi图。Delaunay三角网具有两个重要的性质： (1)在由点集V所形成的Delaunay三角网中，其中每个三角形的外接圆均不包含点集V中的其它任意点；(2)在由两相邻三角形构成的凸四边形中，交换此四边形的两条对角线，不会增加这两个三角形六个内角和的最小值。 Delaunay三角网具备了三点不共线，四点不共圆的特点。而且它所形成的三角形具有最优的特性，即三角形的形状最好，避免了狭长三角形的产生。这些决定了它是二维平面三角网中唯一的、最好的三角网。

数字地形模型

数字地形模型(DTM)是测绘工作中，又称数字高程模型。即在一个区域内，以密集的地形模型点的坐标X、Y、Z表达地面形态(胡金星et al.,2003)。这样的地形模型点，就其平面位置来说，可以是随机分布的(包括像片上取规则格网的情况在内),也可以是规则分布的。规则分布时，只须记录和存贮点的高程，应用比较方便。数字地形模型主要用于描述地面起伏状况，可以用于提取各种地形参数，如坡度、坡向、粗糙度等，并进行通视分析、流域结构生成等应用分析。因此，DTM在各个领域中被广泛使用。DTM可以有多种表达方法，包括网格、等高线、三角网等。

灰色决策理论

灰色决策模型是借用模糊数学、运筹学、系统工程学中的一些高等数学模型。由于研究对象的不确定性上才派生这种具有特色的不确定性科学，但是事物本身可能不止含有一种不确定性，而是同时含有两种或更多种不确定性(王霞,2011)。因此，在数学上不仅应该研究事物的各种单一不确定性，也应该研究各种复合不确定性。因此，学者提出了灰色模糊集合的概念，这种集合是一种模糊集合，但其隶属函数却是在信息不完全的情况下确定的，即隶属函数带有灰度。给出的定义是：如果一个模糊集合的隶属函数带有灰度，那么就称为灰色模糊集合。

高斯和滤波

高斯和滤波通过使用高斯混合模型(Gauss Multiple Model，GMM)来表示状态向量的概率密度分布函数(王磊et al.,2017)。

水下地形匹配导航

为实现这种导航，需先用声呐探测手段，测绘出水下潜航器路线的地形数据并制成数字地图，存储在潜器导航系统中。在水下潜航器航行过程中，潜器装载的声呐测量设备实际测得的地形数据与存储潜器的数字地图进行比较，确定潜器对应的坐标位置，如果出现偏差，导航系统发出控制信号，修正潜器的航行路线。水下地形匹配导航方式的优点是精度高，隐蔽性好。

本发明水下地形匹配辅助惯性导航系统关键技术主要包括以下步骤：

(1)针对惯性导航系统误差随时间推移累积的问题，提出闭环运用卡尔曼滤波最优估计进行误差补偿，开环运用速度回溯补偿进行误差补偿；

(2)提出顾及水下潜航器姿态角的精确三维声线跟踪模型，获取高精度的波束脚印坐标，并给出波束脚印内采样点的提取方法，获得了高分辨率的多波束采样点；

(3)提出基于水深约束和分形维理论Delauney三角网建立水下数字地形模型方法，建立高分辨率高精度的水下数字地形模型，为地形匹配提供可靠的依据；

(4)提出基于灰色决策理论的水下地形适配区评价方法，建立可靠性较高的适导性指标矩阵，为后期的最优地形匹配提供有效的决策；

参见图1至图7所示，现在对本发明的具体实施方式进行如下详细描述：

(1)总体技术方案

图1为本发明的总体技术方案原理图，首先通过分析惯性导航系统自身的误差来源，建立相关的误差模型，并对惯导系统自身进行闭环和开环误差补偿；其次，通过精确的三维声线跟踪模型和波束脚印采样点提取方法，获取高精度高分辨率的多波束采样点；然后通过基于水深约束和分形维理论Delauney三角网建立高分辨率高精度的水下数字地形模型，并通过地形匹配结果和惯性导航系统最初输出的位置估计值进行滤波获得最优误差估计值，最后对惯导系统的运动状态参数进行修正，以期获得更高精度的水下潜航器运动状态参数。

(2)惯性导航系统的误差模型建立及其补偿

(a)误差模型建立

①姿态误差模型：

假设数学平台坐标系为p系,地理坐标系为t系,则平台误差角的变化规律为:

为数学平台相对于惯性坐标系的实际转动角速度向量在数学平台上的投影，

为地理坐标系相对于惯性坐标系的转动角速度向量在数学平台上的投影，

陀螺漂移为ε＝[ε_x,ε_y,ε_z]^T，将平台误差角变化方程在三轴上展开,得到数学平台的姿态误差为：

δV_i为速度误差,V_i为速度在地理坐标系上的投影,

为地理纬度,δφ为地理纬度误差,i＝x,y,z。

②速度误差模型

由于平台姿态误差及加速度计误差源等的影响，捷联惯性导航系统的速度输出与水下潜航器的真实速度直接也存在着误差。假设捷联惯导系统的输出速度为

则速度误差为：

δV_i＝V_i ^p-V_i (11)

将速度误差分解到x,y轴，并对等式两边求全微分并进行扩展，考虑到加速度计误差▽_i对速度的影响，得到以下速度误差方程：

③位置误差模型

由于东向、北向速度存在误差，由速度积分计算得到位置也具有误差，位置误差方程如下：

(b)误差补偿

针对捷联惯性导航系统和多普勒测速仪的水下潜航器的运动状态参数误差，补偿方案通常采用卡尔曼滤波对导航实时误差加以消除，但是由于卡尔曼滤波的状态转移矩阵由一阶近似分量组成，本身具有一定的近似性误差，通过卡尔曼滤波后惯导系统的导航误差并不能完全消除，而且由于卡尔曼滤波状态转移矩阵结构复杂、计算量大、只能间隔一定时间间隔进行，在卡尔曼滤波工作的时间间隔中，由于速度误差的存在，经过导航迭代解算，惯性导航位置误差还是有累积。因此，本课题研究提出实时误差补偿与误差回溯补偿相结合、反馈补偿与开环补偿相结合的误差补偿方法。具体原理如图2所示。

①闭环最优估计误差补偿

根据惯性导航系统的误差模型，离散的捷联惯性导航系统误差状态方程为：

其中，

速度误差、位置误差的反馈补偿，直接采用误差消除的方法，也就是将t_k时刻卡尔曼滤波器给出的导航系统速度误差、位置误差的最优估计值，直接从系统中消去，可直接达到控制速度、位置误差的目的。经过反馈补偿后的纬度、经度和速度分别为：

获得t_k时刻的经度、纬度和速度后将迭代进入t_k+1时刻的惯性导航计算中。

由于姿态角误差与姿态角大小量之间反复计算会造成计算误差，因此，根据转动四元数定理，可利用姿态角误差直接修正四元数，经过补偿后的四元数为：

其中，

展开后得到四元数误差更新的具体算法为：

修正后获得t_k时刻的四元数将迭代进行t_k+1时刻的姿态矩阵更新计算中。

对闭环进行误差补偿后，由于卡尔曼滤波本身的误差造成惯导系统输出的运动状态参数仍然存在误差，因此需要进行开环速度回溯补偿。速度回溯补偿分为拟合补偿和预测补偿两部分。

②开环速度回溯误差补偿

开环速度回溯误差补偿分为速度回溯拟合补偿算法和速度回溯预测补偿算法两部分。

速度回溯拟合补偿：

在t_k-2，t_k-1，t_k三个时间点对惯性导航系统速度与水下多普勒测速仪速度差的测量，获得惯性导航系统速度误差的近似值，如下式所示：

对速度误差δV_{i_k}(t_k-2)，δV_{i_k}(t_k-1)，δV_{i_k}(t_k)进行曲线拟合，得到在t_k-2～t_k区间的速度误差拟合曲线方程为：

δV_{i_k}(t)＝a_{i_k0}t+a_{i_k1}t+a_{i_k2}t² (19)

然后在t_k-1～t_k区间对δV_k(t)积分得到这段时间内的位置累积误差：

从惯导系统开始工作到t_k时刻，总共累积的位置误差为：

速度回溯预测补偿：

使用t_k-2～t_k这个区间内的惯导速度误差预测曲线δV_{i_k}(t)＝a_{i_k0}+a_{i_k1}t+a_{i_k2}t²,对在t_k～t_k+1区间的惯性导航系统速度误差进行预测,并将惯性导航系统实际速度输出减去预测的速度误差,得到优化的速度输出值：

通过对速度误差预测曲线积分预测，区间内任意时刻的惯导位置误差：

将预测获得的惯导位置误差对惯性导航系统的位置进行实时误差修正补偿：

作为惯导系统的导航数据中的位置输出,输入到控制计算机上使用,而不再反馈回到惯导计算程序中。

(3)顾及姿态角的三维声线跟踪

由于船体坐标系的中心通常是以换能器为中心，因此以换能器理想水平状态作为基准面来分析船体姿态对波束初始入射角的影响，如图3所示的换能器基阵坐标系中，水平状态的换能器基准面位于OABC平面内，O为换能器中心， OA为基准面纵轴正方向，OC为基准面横轴正方向。设OA长度为a，OC长度为c，A、B两点的坐标分别为(a,0,0)和(0,c,0)。在某一姿态(横摇、纵摇角分别为r和p)影响下基准面变化为OA₁B₁C₁，即基阵面由水平先绕OX轴旋转角度α(α≠r)，再绕OY轴旋转角度β形成。A点、C点经过两次旋转后分别转到A₁和C₁位置，A₁、C₁两点在水平面OXY上的投影分别为A₂和C₂。在此状态下，OA₁与水平面夹角∠A₁OA₂即为纵摇角p，OC₁与水平面夹角∠C₁OC₂即为横摇角r。根据纵摇角、横摇角和旋转角定义，r和α符号一致，p和β符号一致。

由以上过程可知，基准面OABC经过α和β两次旋转得到OA₁B₁C₁，则有：

则旋转后A₁点坐标为：

旋转后C₁点坐标为：

由式(26)得到旋转后A₁点坐标再根据三角形正弦定理可计算出基阵面的纵摇角p(即∠A₁OA₂)：

式(28)中Z_A1为A₁点在Z轴上的坐标，根据β与p符号一致可得：

β＝p (29)

类似地，由式(27)得到旋转后C₁点坐标再根据三角形正弦定理可计算出基阵面的横摇角r(即∠C₁OC₂)：

式(30)中Z_C1为C₁点在Z轴上的坐标，r和α符号一致，并将β＝p带入式(30) 得：

sinr＝sinαcosp (31)

则有：

α＝arcsin(sinr/cosp) (32)

由式(29)和式(32)可知，在旋转变换中，绕OY轴旋转角β等于纵摇角p，而绕OX轴旋转角α并不等于横摇角r。因此，在目前较为精细的声线跟踪计算中，即使顾及姿态影响，定义初始入射角为θ₀+r显然是不正确的。

为了得到姿态(r、p)影响下真实的波束入射角，下面推导实际波束初始入射角θ′₀的计算模型。

由以上推导可知，实际声线可由理想状态下的声线经α、β旋转变换R后得到。设理想状态下，第i个波束分配初始入射角为θ_i，在不失精度的情况下，假设经历第一个水层以常声速传播，传播距离为R_i，则波束在第一水层下界的落点P_i坐标为(0，R_isinθ_i,R_icosθ_i)，而在姿态影响下的实际坐标(x_i,y_i,z_i)为：

式(33)可借助图4解释。假设换能器基阵水平时，第i号波束的波束角度为θ_i，斜距为R，则点A坐标为(0，R_isinθ_i,R_icosθ_i)，换能器基阵在横摇r和纵摇p的影响下，A点旋转到了B点，第i号波束的实际入射角度为θ′_i(即∠BOD)，定义经旋转后第i号波束的水平角度

即为波束横距BD与 OY轴的夹角，其表达式为：

由式(34)可获得姿态影响下的波束实际初始入射角，之后根据改进的精确声线跟踪方法进行三维声线精确跟踪(参见图5)。

(4)水下数字地形模型建立

水下三维数字地形建模流程如图6所示，获得实测条带所有采样点的坐标后，首先对波束脚印指向点建立低分辨率的Delaunay三角网；再获取波束脚印内采样点的地形分形维特征，并运用LOD(细节层次)技术获取高分辨率地形；最后根据波束脚印内的回波采样点对三维地形模型中插值出来的水深点进行水深约束，获得精度更高的高分辨率水下三维数字地形模型。

(5)水下地形匹配辅助导航算法

目前导航领域的非线性滤波方法除了点群滤波和粒子滤波，还包括高斯和滤波算法(Gauss Sum Filter，GSF)。高斯和滤波通过使用高斯混合模型(Gauss Multiple Model，GMM)来表示状态向量的概率密度分布函数。将d维高斯概率密度函数记为：

其中Σ表示状态向量的协方差矩阵。

在PDF表示为高斯分布的和之后，两个不同的PDF信息可以很容易的通过合并高斯分布来进行融合：

N(u¹,Σ¹)·N(u²,Σ²)＝αN(u³,Σ³) (37)

其中：

因此，后验分布p(x_n|y_0:n)可以表示为高斯分布之和:k＝r·s。

其中k＝r·s。

本课题研究拟采用自适应高斯和滤波(Adaptive Gauss Sum Filter，AGSF) 将基于实测多波束数据建立的三维数字地形模型与背景场多波束数据模型进行匹配，基于自适应因子的AGSF的滤波更新策略为：

自适应因子α调整策略为：

其中，

自适应因子β调整策略为：

其中，

基于AGSF的水下地形匹配辅助导航过程如图7所示，首先根据惯性导航初步给出的位置，搜索出该区域的地形背景场数据，与实测数据进行匹配，根据匹配结果给出自适应因子，然后将匹配误差与惯性导航误差进行自适应高斯和滤波，最后将获取的最优位置估计修正惯性导航输出值。

由此可见，本发明中解决了下列技术问题：

另外，本发明的技术特点如下：

(2)在惯性导航自身系统中，提出闭环运用卡尔曼滤波最优估计进行误差补偿，开环运用速度回溯补偿进行误差补偿方法，同时提出基于自适应高斯和滤波的水下地形匹配辅助导航方法，在实现水下高精度微地形匹配的基础上，并将匹配结果和最初惯性导航系统的输出值进行滤波，获取最优估计值以期修正惯性导航系统运动状态参数，极大地提高地形匹配辅助导航的精度和效率。

参考文献：

Anonsen,K.B.,Hallingstad,O.,2006.Terrain aided underwater navigationusing point mass and particle filters, Proceedings of the IEEE/ION PositionLocation and Navigation Symposium.

Anonsen,K.B.,Hallingstad,O.,Hagen,O.K.,2007.Bayesian Terrain-BasedUnderwater Navigation Using an Improved State-Space Model,Symposium onUnderwater Technology and Workshop on Scientific Use of Submarine Cables andRelated Technologies,pp.499-505.

Bergman,N.,Ljung,L.,1997.Point-mass filter and Cramer-Rao bound forterrain-aided navigation,Decision and Control,1997.,Proceedings of the 36thIEEE Conference on,IEEE,pp.565-570.

Bergman,N.,Ljung,L.,Gustafsson,F.,1999.Terrain navigation usingBayesian statistics.Control Systems, IEEE 19,33-40.

Enns,R.,Morrell,D.,1995.Terrain-aided navigation using the viterbialgorithm.Journal of Guidance,Control, and Dynamics 18,1444-1449.

Golden,J.P.,1980.Terrain contour matching(TERCOM):a cruise missileguidance aid,24th Annual Technical Symposium,International Society for Opticsand Photonics,pp.10-18.

Hollowell,J.,1990.Heli/SITAN:a terrain referenced navigationalgorithm for helicopters,Position Location and Navigation Symposium,1990.Record.The 1990's-A Decade of Excellence in the Navigation Sciences.IEEE PLANS'90.,IEEE,IEEE,pp.616-625.

Kamgar-Parsi,B.,Kamgar-Parsi,B.,1999.Vehicle localization on gravitymaps,AeroSense'99,International Society for Optics and Photonics,pp.182-191.

Karlsson,R.,Gusfafsson,F.,Karlsson,T.,2003.Particle filtering andCramer-Rao lower bound for underwater navigation,Acoustics,Speech,and SignalProcessing,2003.Proceedings.(ICASSP'03).2003IEEE International Conference on,IEEE,pp.VI-65-68vol.66.

Meduna,D.,Rock,S.M.,McEwen,R.,2009.Auv terrain relative navigationusing coarse maps,Unmanned Untethered Submersible Technology Conference.

Meduna,D.K.,2011.Terrain relative navigation for sensor-limitedsystems with application to underwater vehicles.Stanford University.

Nygren,I.,2008.Robust and efficient terrain navigation of underwatervehicles,Position,Location and Navigation Symposium,2008IEEE/ION,IEEE,pp.923-932.

Zhang,F.,Geng,J.,Cheng,P.,2012.Design of Underwater PassiveNavigation Simulation System Based on Information Fusion.

柏菁,刘建业,熊智,2003.地形辅助/惯性/GPS组合导航系统的可视化仿真研究.中国惯性技术学报11, 23-28.

陈国雄,2016.基于分形与小波理论的成矿复杂信息提取与识别方法研究,中国地质大学.

丁国强,2010.惯性导航系统传递对准技术关键问题研究,哈尔滨工程大学.

冯庆堂,2004.地形匹配新方法及其环境适应性研究,国防科学技术大学.

冯庆堂,沈林成,常文森,叶媛媛,2005.基于HMM的地形高度匹配算法.自动化学报31,960-964.

何林帮,赵建虎,张红梅,王晓,严俊,2015.顾及姿态角的多波束声线精确跟踪方法.哈尔滨工程大学学报,46-50.

胡金星,潘懋,马照亭,吴焕萍,2003.高效构建Delaunay三角网数字地形模型算法研究.北京大学学报自然科学版39,736-741.

李涛,2012.Delaunay三角网构建及可视化方法与实现,东华理工大学.

刘承香,2003.水下潜航器的地形匹配辅助定位技术研究,哈尔滨工程大学.

祁芳,2003.卡尔曼滤波算法在GPS非差相位精密单点定位中的应用研究,武汉大学.

王磊,程向红,李双喜,2017.高斯和高阶无迹卡尔曼滤波算法.电子学报45,424-430.

王胜平,2011.海洋地磁匹配导航关键技术研究,武汉大学.

王霞,2011.灰色决策理论若干问题探讨,华北水利水电学院.

赵锋伟,李吉成,沈振康,2002.景象匹配技术研究.系统工程与电子技术24,110-113。

Claims

1.一种水下地形匹配辅助惯性导航定位方法，其特征在于，包括以下步骤：

(1)建立误差模型；

(6)在水下高精度微地形匹配基础上，基于自适应高斯和滤波的水下地形匹配辅助导航，将地形匹配结果和惯导系统输出的位置估值进行滤波，获取位置的最优误差估计值，以修正最初惯导系统输出的定位参数，从而提高地形匹配辅助导航的精度和效率。

2.如权利要求1所述的水下地形匹配辅助惯性导航定位方法，其特征在于，所述误差模型建立包括姿态误差模型建立、速度误差模型建立和位置误差模型建立。

3.如权利要求1所述的水下地形匹配辅助惯性导航定位方法，其特征在于，所述闭环误差补偿采用下列算法：

4.如权利要求1所述的水下地形匹配辅助惯性导航定位方法，其特征在于，所述开环误差补偿采用速度回溯拟合补偿算法和速度回溯预测补偿算法，其中，

所述三维声线跟踪模型采用下列算法：

5.使用如权利要求1-4任一项所述的水下地形匹配辅助惯性导航定位方法所构建的水下地形匹配辅助惯性导航定位系统。