程建华,丁惠倩,常 乐,葛靖宇

(哈尔滨工程大学智能科学与工程学院,哈尔滨 150001)

0 引言

自主水下航行器(autonomous underwater vehicle,AUV)在不频繁上浮的情况下进行长航时水下航行时,必须有精确可靠的水下导航和定位。作为AUV的基本导航系统,捷联惯性导航系统(strapdowninertial na-vigation system,SINS)的一个主要缺点是存在时间累积误差,这就需要依靠其他导航系统来频繁地修改和校正误差。目前,常见的水下辅助惯性导航系统的导航方式主要包括声学导航[1]、地球物理信息导航[2]和航位推算导航[3]等,其中,地球物理信息导航又可分为地磁辅助导航[4]、重力辅助导航[5]以及地形辅助导航[6-7]。水下地形辅助导航(underwater terrain aided navigation,UTAN)是一种利用先进的水下数字地图和测得的水下地形高程来估计AUV位置的技术,可以实现完全自主式导航定位,从而得到高精度的水下位置信息,用以修正捷联惯性导航系统的误差。随着多波束测深系统的发展和逐步应用,水下地形测量的精度越来越高,范围也越来越广,其工作时发射的多个波束呈现扇面开角,相对于单波束测深系统只能发射一个波束来说,所测量得到的地形信息大大增加,AUV航行时所测量的地形信息整体呈现为曲面,而非单波束系统呈现的曲线,这使得AUV可以更有效率地得到精度更高的水下数字地图和实时扫测地形分布。多波束测深系统的这些优势为水下地形辅助匹配导航创造了良好的条件[8]。

国内外学者针对基于相关性匹配的经典地形匹配算法有不同程度的改进。赵龙等[9]提出了结合地形轮廓匹配(terrain contour matching,TERCOM)和粒子滤波(particle filter,PF)的地形辅助惯导算法,初始位置信息在采集模式下由TERCOM获取,连续跟踪在跟踪模式下由PF算法实现,不同模式之间的切换由模式控制逻辑控制,在初始误差较大的情况下能够快速、准确的定位;李培娟等[10]提出了结合迭代最近等值线(iterated closest contour point,ICCP)算法和卡尔曼滤波器的地形导航系统,以ICCP匹配位置与SINS的差值作为卡尔曼滤波器的测量,利用滤波结果反馈校正SINS输出,得到最优估计,可以满足AUV长时间保持精确导航的要求,大大降低了位置误差;王汉兵等[11]分析了多波束测深数据特征,选择条带中心和边缘两侧数据点作为三条并行路径,基于中心光束基准面多波束测深数据原理,设置路径权重,实现了多路径并行的ICCP算法,可以在具有更多地形特征的区域实现更高的定位精度;赵建虎等[12]指出,TERCOM可靠性与准确性受到单一匹配序列的影响,因此采用两个匹配序列来增强算法,通过TERCOM得到新的匹配区域后,使用基于链码的多等值线地形匹配(multi-conto-urs terrain matching,MCTM)算法精确匹配,两种算法克服了彼此的缺点,获得了更高的水下导航精度和可靠性。以上文献对经典的地形匹配方法进行了不同程度的改进与结合,但是改进后的方法依然是面向线对线的匹配,没有利用到多波束测深系统扫测得到地形面的特点。

ICCP算法借助等值线数字地图来定位匹配,目前,已应用在地球物理信息导航领域,即地磁辅助导航、重力辅助导航以及地形辅助导航中。传统的地球物理信息导航所使用的测量物理信息仪器分别为地磁传感器、重力仪和单波束测深系统,理论上能测量的仅仅为AUV当前垂直位置的物理信息。因此,当AUV航行时,只能获得一条线信息。但是,目前地形匹配导航中开始广泛使用多波束测深系统,该系统在航行时可以获得地形面信息,而ICCP算法通常用来处理线信息,这就要求引入其他的算法来处理信息,从而完成地形匹配。迭代最近点(iterative closest point,ICP)[13]算法是ICCP算法的基础,目前常用于点云配准领域,该方法可以应用于多波束测深数据与水下数字地图的匹配问题。基于以上分析,本文提出了一种TERCOM与ICP算法联合的水下地形辅助导航算法,利用多波束测深系统扫测的地形面,解决UTAN中TERCOM算法精度不高、ICP算法在初始误差较大情况下迭代次数过多的问题。

1 地形匹配算法分析

目前,地形匹配常用的算法大致分为两种:相关性匹配方法和基于滤波的方法。相关性匹配方法主要有TERCOM算法,该方法在进行匹配前需要采集一定的数据点才能匹配,是一种批相关处理的方法。桑迪亚惯性地形辅助导航(Sandia inertial terrain-aided navigation,SITAN)[14]算法不同于利用相关分析法的TERCOM算法,它是一种递推的扩展卡尔曼滤波系统,利用数字地图和传感器实时测得的数据,通过卡尔曼滤波器对惯导系统实时不间断地修正。本文将着重介绍TERCOM算法和ICP算法。

1.1 TERCOM算法原理

TERCOM算法从数字地图中选取一组和SINS指示航迹平行的序列,将真实高程序列与选取的高程序列按照某种算法度量进行匹配相关分析,依据相关分析结果就可以得到AUV的真实位置偏差,从而修正SINS的参数,减小累积误差。

目前有3个算法常用于相关分析,分别是交叉相关(correlation,COR)算法、平均绝对差(mean absolute differences,MAD)算法和平均标准差(mean square differences,MSD)算法。这3个算法的定义如式(1)、式(2)、式(3)

(1)

(2)

(3)

其中,hr代表真实高程序列;hm代表选取的高程序列。

在上面的相关分析指标中,MAD和MSD是距离决策算法,取最小值时获得最佳匹配位置,而COR是一种形状决策算法,取最大值时获得最佳匹配位置[1]。这两种算法的定位结果相互独立,同时满足距离决策和形状决策算法的定位结果可以视为真实位置。如果没有测量误差,这两种类型的算法得到相同的定位结果。经过实验与分析发现,MSD算法的准确性略高于COR和MAD算法,因此在研究中多采用MSD算法[6]。由于TERCOM算法的相关性分析是基于网格来计算的,所以始终存在着半个网格间距的量化误差,即TERCOM算法精度最高只能达到网格长度的一半。

1.2 ICP算法原理

ICP算法一直被认为是点云数据配准中最经典的算法,并且在相关领域的应用已经十分成熟。ICP 算法是一种基于最小二乘法的点云精确配准方法,通过最小化相应点对之间的距离,不断地迭代更新以获得配准参数的估计值。

已知原点云P={p1,p2,…,pn}和目标点云Q={q1,q2,…,qn},通过不断迭代,将P点云中的pi与Q点云中的qi距离最短的点作为对应点求解变换矩阵,当误差损失函数达到设定阈值或者达到迭代次数时停止迭代,获得最佳旋转矩阵R和平移矩阵t,达到最佳配准效果。

一般来说,在没有误差的情况下点云数据满足式(4)

qi=Rpi+t

(4)

(5)

(6)

设R*、t*为最优解,可以将目标函数优化问题分为两部分

(7)

t*=μq-Rμp

(8)

R*=UVT

(9)

t*=μq-R*μp

(10)

将变换矩阵应用到点云P中的所有点上,使所有点都进行坐标变换,如式(11)所示完成点云P的坐标更新

Pnew=R*P+t*

(11)

ICP算法最大的优点是结果比较稳定,但缺点也很明显,例如计算效率太低、两个数据集的重叠比高及容易落入局部最优解。

2 TERCOM-ICP联合匹配算法

通过上述算法分析可知,TERCOM算法简单、计算量小,但是在有偏转的情况下误差较大,精度只有网格长度的一半;ICP算法能够有效地处理旋转偏差,但是容易落入局部最优解,计算量大。基于以上原因,本文提出了一种联合匹配算法,充分利用多波束测深设备测得的水下地形面,先通过TERCOM算法粗略匹配,将匹配后的大致位置作为指示输入ICP算法中进行精确匹配。

当AUV经过可匹配地形区时,利用多波束测深系统测得AUV距离水下地面的深度,用压力传感器获得所处深度,进而得到当前AUV位置下方真实的地形高程。之后根据SINS输出的位置从数字地图中确定TERCOM算法的匹配搜索区域,该区域是一个矩形区域,以SINS指示的最终位置作为中心,6σ作为边的长度,σ表示SINS导航误差的标准差。使用该方法确定的搜索范围包含AUV真实航行的最终位置,然后从SINS导航航迹中提取若干个地形高程剖面图,这些位置应该与真实地形高程剖面图平行。将真实高程序列与选取的高程序列按照MSD算法进行匹配相关分析,得到MSD值最小的高程序列所指示的位置即为TERCOM算法匹配位置,也就是ICP算法的指示位置。

在进行ICP算法地形匹配时,将通过多波束测深系统以及指示位置计算出的地形坐标数据视为ICP算法中的原点云,从水下地形数据中提取相匹配的格网数据作为目标点云。水下地形格网数据通过K-D树(K-Dimensional tree)存储[15]。

K-D树[16]是一种基于二叉搜索的数据结构,解决在k维空间为数据集建立索引的问题。它可以有效地存储、管理和搜索数据,并用于提高ICP算法的运行效率。K-D树将待匹配区域地形坐标在x、y、z这3个维度投影中分成若干个半平面,使得每个点都位于自己的空间中,这种将空间划分为子空间的方法是使用空间中值计算来实现的。整个模型基于一个根节点,在树的下方层层递进地分为若干个叶节点,然后使用最临近搜索找出在树中与输入点最接近的点。将要搜索的节点与划分维度的空间中值进行比较,如果搜索节点对应值小于或等于划分维度的中位数,则进入左侧子分支,相反则进入右侧子分支。循环到二叉树的叶节点,并沿着路径找出在要查询的点的同一子空间内的近点;如果在搜索路径节点的其他子空间节点上有另一个较近的点,则转到子空间的节点以搜索距离中的近点。重复上述过程直到搜索路径无效,结束搜索。

通过K-D树搜索到地形坐标数据的最临近点数据,再经过ICP算法得到两组数据的旋转矩阵和平移矩阵,变换后得到修正后的地形坐标数据。重复上述过程直至达到迭代次数,最后得到的修正地形坐标数据即为最终匹配结果。整个算法的流程如图1所示。

图1 TERCOM-ICP联合算法流程图Fig.1 Flow chart of TERCOM-ICP joint algorithm

3 仿真实验分析

本文采用Python进行仿真实验分析,实验区域三维地形图和等高线图分别如图2和图3所示,X、Y表示经度、纬度方向格网编号,格网长度为10 m。假设AUV以4 m/s的速度行驶,速度误差为0.05 m/s,陀螺仪常值漂移误差为0.001(°)/h,每隔20 s进行一次采样,经过10次采样进行一次匹配,共进行10次匹配。每完成一次匹配后,给予一个均值为0、标准差为5°的随机角度,从而使AUV的航向发生变化。本文采用TERCOM-ICP联合匹配方法,在图示区域内选择一条较平缓的轨迹作为实验一和一条较崎岖的轨迹作为实验二进行仿真实验。

图2 实验区域三维地形图Fig.2 3D topographic map of the experimental area

图3 实验区域等高线图Fig.3 Contour map of the experimental area

实验一匹配结果如图4所示。图5所示分别为第2个匹配点和第9个匹配点的结果。AUV真实轨迹的起始点为(400 m,8 000 m),初始误差为(400 m,200 m),初始航向角为110°。

图4 实验一匹配结果Fig.4 Matching results of experiment one

图5 实验一局部匹配结果Fig.5 Local matching results of experiment one

图6所示为TERCOM算法和TERCOM-ICP联合算法的误差曲线图。

图6 实验一匹配误差Fig.6 Matching errors of experiment one

表1中,D表示匹配位置与真实位置之间的直线距离误差。从图示结果以及表格中的数据来看,TERCOM-ICP算法能够有效减小单纯使用TERCOM算法的匹配误差,平均匹配误差能达到20 m以内。从图表可以看出,TERCOM算法的匹配结果在一定程度上影响着后续ICP算法的结果,一方面可能是因为该匹配点处地形变化不明显或存在其他相似地形;另一方面是因为TERCOM算法的匹配结果作为指示输入到ICP算法中,从而对最终匹配结果产生一定的影响。

表1 实验一匹配误差参数对比Tab.1 Comparison of matching error parameters in experiment one

实验二匹配结果如图7所示。图8所示分别为第2个匹配点和第9个匹配点的结果。AUV真实轨迹的起始点为(1 000 m,2 000 m),初始误差为(-200 m,400 m),初始航向角为45°。

图7 实验二匹配结果Fig.7 Matching results of experiment two

图8 实验二局部匹配结果Fig.8 Local matching results of experiment two

图9所示为TERCOM算法和TERCOM-ICP联合算法的误差曲线图。

图9 实验二匹配误差Fig.9 Matching errors of experiment two

从图示结果以及表2的数据来看,TERCOM-ICP算法能够有效减小使用TERCOM算法的匹配误差,平均匹配误差能达到20 m以内。在传统的ICCP算法中,通常AUV的航迹所经过的区域较为崎岖时,即在地形变化较明显的区域,匹配结果会明显好于AUV的航迹经过的区域较为平缓时,也就是地形变化不明显的区域[17],但是本文提出的TERCOM-ICP算法在两处区域内匹配结果精度并没有明显差别。产生以上情况的原因是ICCP算法是一种线匹配方法,当AUV在平缓区域内移动时,所能获取的水下地形信息量远不如地形变化明显的区域,因而易产生匹配误差较大的情况;而ICP算法是一种面匹配方法,在同样平缓的区域内,ICP算法通过多波束测深系统所能获得的水下地形信息量远大于ICCP算法,从而在匹配时能有效减小产生较大误差的可能。

表2 实验二匹配误差参数对比Tab.2 Comparison of matching error parameters in experiment two

表3数据表示实验一和实验二分别使用ICP算法和TERCOM-ICP联合算法进行同样路径的地形匹配所用时间。根据表中数据分析可知,TERCOM算法计算简单,在地形匹配时先进行粗匹配,之后再使用ICP算法精匹配,所用时间要小于单纯使用ICP算法的匹配时间。由此可验证本文提出的TERCOM-ICP算法能够避免ICP算法效率较低的缺陷,减少整个匹配过程的时间。

表3 实验所用时间比较Tab.3 Comparison of time spent in experiments

4 结论

鉴于点云数据配准和地形匹配算法等有关概念的相关性,将ICP算法引入到地形匹配算法中,与传统的TERCOM地形匹配算法相结合,提出了一种TERCOM-ICP联合算法,经仿真实验可得到以下结论:

1)通过仿真实验验证了TERCOM-ICP算法在地形匹配领域的可行性,与TERCOM算法相比,能够提高匹配导航的整体精度,与ICP算法比较,能够减少匹配时间。

2)引入ICP算法能够有效利用多波束测深系统扫测得到的地形面优势,增加所能获得的水下地形数据信息量,降低产生较大匹配误差的可能。

3)本文仅验证了TERCOM-ICP算法的可行性,未对AUV航行过程中可能遇到的各种误差的影响进行分析。此外,TERCOM算法和ICP算法都需要累积一定的路程后才可进行匹配,所以导航初期属于纯惯性导航阶段,在对实时性要求较高的情况下,该方法还有待改进提升。