徐晓苏，汤郡郡，张 涛，岳增阳

（1. 微惯性仪表与先进导航技术教育部重点实验室，南京210096；2. 东南大学 仪器科学与工程学院，南京 210096）

一种结合地形和环境特征的水下导航定位方法

徐晓苏1,2，汤郡郡1,2，张 涛1,2，岳增阳1,2

（1. 微惯性仪表与先进导航技术教育部重点实验室，南京210096；2. 东南大学 仪器科学与工程学院，南京 210096）

为了实现水下潜器长时间高精度导航定位，同时考虑到传统地形辅助导航系统在先验地形图不可得或者是地形变化不明显的海域（地形不可匹配区域），无法用来修正惯性导航位置误差的问题，提出了一种结合地形和环境特征的水下导航定位方法。在先验地形图可得且地形高程变化明显的可匹配区域，采用地形辅助导航系统来修正惯导位置误差，在先验地形图不可得或者是地形高程变化不明显的不可匹配区域，采用基于海洋环境特征的同步定位与构图算法来修正惯导位置误差。仿真结果表明，该方法在地形可匹配区域以及地形不可匹配区域得到的航迹都比纯惯导得到的轨迹更接近于理想航迹，因此可以用来修正惯导位置误差。

水下航行器；地形辅助导航系统；同步定位与构图；惯性导航系统

惯性导航系统不需要任何外来信息，也不向外辐射任何信息，仅依靠自身就能在全天候条件下，在全球范围内和任何介质环境中进行连续的导航和定位，这种同时具备自主性、隐蔽性和能获取载体完备运动信息的独特优点使其成为最受广泛使用的水下导航定位方法。但是，惯导系统也有着系统误差随时间积累的原理性缺陷，为了实现长航时高精度的导航目标，惯性导航系统需要利用外界位置信息对其进行周期的调整和校正。

地形辅助导航（Terrain-Aided Navigation，TAN）是一种利用地形高程特征来进行辅助定位的方法，它具有自主、隐蔽、连续、全天候工作、导航定位误差不随时间积累等优点，是水下潜器理想的辅助导航定位手段。然而，地形辅助导航需要有先验的地形图，且要求地形高程有明显的变化。对于一些尚未测绘或者是地形变化过于平缓的海域，传统的地形辅助导航方法是无法进行的。

基于环境特征的同步定位与构图（Simultaneous Localization and Mapping，SLAM）概念最早于1986年举办的 IEEE机器人与自动化会议上被提出，其基本思路是，将机器人放在一个未知环境的未知位置下，机器人依靠自身携带的传感器感知周围环境，增量式构建一副与环境相一致的地图，并同时根据该地图确定自身所在位置。在先验地形图不可得或者是地形变化不明显的不可匹配区域，如果存在一些明显的静止物体，如珊瑚礁、水下建筑、失事船等，则可以将这些显而易见的物体提取为二维点特征，将这些二维点特征构成一幅可描述当前水下环境的“特征地图”，同步定位与构图算法可以在构造这种特征地图的同时实现对水下潜器的辅助定位。

考虑到同步定位与构图算法的这种优势，针对地形辅助导航方法在地形不可匹配区域无法修正惯导随时间积累的误差问题，提出了一种结合地形和环境特征的水下导航定位方法，在地形可匹配区域，采用地形辅助惯性导航方法来修正惯导位置误差，在地形不可匹配区域，采用基于海洋环境特征的同步定位与构图算法来代替地形辅助导航方法修正惯导系统的位置误差。仿真结果表明，该方法在地形可匹配区域和不可匹配区域都可以实现高精度的导航定位。

1 结合地形和环境特征的水下导航定位方法原理

SLAM整个过程中没有一个确知的绝对位置，所以它实际上是一种相对定位，然而对于水下潜器这样的载体来说，得到相对位置并不是目的。当潜器在地形可匹配区域航行时，采用地形辅助导航修正惯导位置误差的同时，对环境进行构图，此时潜器的绝对位置是经过修正的，通过转换得到的特征的绝对位置精度也相对较高。当潜器从地形可匹配区域进入地形不可匹配区域时，可以利用地形辅助导航得到的最后一个绝对位置，通过SLAM的相对定位以及对路标的实时观测实现潜器和路标绝对位置的传递。当潜器再次进入地形可匹配区域时，由于惯性导航系统的位置误差已经得到了及时的修正，潜器初始位置误差很小，地形匹配算法可以在很小的范围内寻找匹配点，这不仅提高了算法的效率，也同时提高了导航定位的精度。

2 地形可匹配区域

在先验地形图可得且地形变化明显的可匹配区域，采用基于ICCP算法的地形辅助导航系统来修正惯性导航系统的位置误差。

2.1 基于ICCP算法的地形匹配流程

① 由惯性导航系统提供航迹测量的N个位置值pi（其中i=1,2,…,N）；实时测深传感器提供惯导指示航迹序列点pi相应的水深值hi（其中i=1,2,…,N），并且从已知参考地图中抽取出相应的等值线Ci（其中i=1,2,…,N）。

③ 假设pi在相应等值线上的最近点为yi，寻找随机旋转和平移变换T，使集合之间的距离最小，也即使下面的目标函数最小

⑤ 判断是否满足最终的迭代终止条件，如果满足便退出。此时得到的匹配点即为最佳匹配点，可用来修正惯导位置误差。

2.2 SINS/TAN系统模型的构建

2.2.1 状态方程

SINS/TAN系统的状态方程就是SINS系统的状态方程，采用间接法估计 S INS系统状态的误差。选取东北天坐标系为导航坐标系，载体坐标系x轴沿潜器横轴指向右舷，y轴沿潜器纵轴指向前，z轴垂直于x轴和y轴所确定的平面构成右手坐标系。选择位置误差、速度误差、姿态误差、陀螺漂移和加速度计偏置作为状态量，有：

式中，

2.2.2 观测方程

系统量测量由SINS输出的位置信息LSINS、λSINS减去ICCP算法得到的位置信息LTAN、λTAN形成。

式中，Vk为观测噪声, Hk如下：

2.2.3 误差修正

将惯性导航系统输出的位置信息与ICCP匹配得到的位置信息的差值作为观测量，进行卡尔曼滤波，并将滤波得到的位置误差量反馈到主惯性导航系统就可以实现对主惯性导航系统位置信息的校正。

3 地形不可匹配区域

在先验地形图不可得或者是地形变化不明显的不可匹配区域，采用基于扩展卡尔曼滤波的同步定位与构图算法来修正惯性导航系统的位置误差。

3.1 系统状态向量和协方差矩阵

水下潜器的状态用它在地理坐标系下纬度方向的位置分量xv、经度方向的位置分量yv以及航向φv表示，其估计均值和方差分别定义为

设潜器观测到的特征均为静止的2维点特征，并用这些点特征在同一个坐标系下的位置来形成地图。

该向量维数随着新特征的加入而扩充，其协方差为

系统状态向量为

系统状态协方差为

式中，Pvv表示潜器位姿估计的不确定性，Pvm表示潜器位姿与环境特征估计之间的相关性，Pmm中对角线元素表示每个环境特征位置估计的不确定性，非对角线元素表示某一特征与其他特征估计间的相关性。

3.2 水下航行器EKF-SLAM算法的具体执行过程

EKF-SLAM算法的执行过程大体可以分为三个阶段：预测、观测和更新/扩充。首先将潜器姿态和地图特征存储在一个独立的状态向量中，然后通过先观测再观测的迭代地推过程来估计系统状态，实现对潜器的定位和特征地图的创建。

3.2.1 预测阶段

图1 潜器姿态变换示意图Fig.1 Attitude transformation of vehicle

3.2.2 观测阶段

设当前时刻声纳测量得到一个特征，经过数据关联，它与存储在地图中的估计值为的特征关联成功，观测值和观测协方差分别为

式中，r表示载体坐标系下特征相对于潜器的距离，θ表示载体坐标系下特征相对于潜器的方位。

如果在一个扫描周期内声纳观测到了m( m＞1)个特征，则观测向量和观测方差可以写成：

而：

对于特征数量很多的海底环境而言，SLAM算法中的雅可比矩阵中大部分项都是 0，这使得新息协方差矩阵以及滤波增益的计算成为可能。

3.2.3 更新/扩充阶段

对于关联正确的特征，可以对潜器状态和地图进行更新，更新如下：

随着声纳对海底环境的探测，必然会扫描到一些新的特征，当某个特征第一次被观测时，必须通过一个初始化过程将特征加入到地图中去，使地图可以随着潜器探索区域的增大而不断扩展。对新特征的初始化方法如下：

首先，将新观测值z和观测协方差R扩充到系统状态向量和状态协方差中，测量是在载体坐标系下获得的。扩充后的系统状态和状态协方差如下所示：

新的观测值z与潜器当前姿态的关系如图2所示。

图2 新的特征观测与航行器关系图Fig.2 Relationship between new feature and vehicle

由于雅可比矩阵大部分项为 0，变换仅影响到协方差矩阵对角线上的矩阵块和新特征所在的行和列上与地图其它状态的互相关项，因此扩充后的状态协方差矩阵可以如下表示：

4 仿真实验

为了验证算法的有效性，本文选取了2块网格（网格间距为0.00125°，也即139 m）大小一致的地形进行了算法仿真。地形 1起点为（东经 38°，北纬120.0375°），如图 3(a)所示；地形 2起点为（东经38.075°，北纬120°），如图3(b)所示。

设定惯导初始位置误差为：经度方向1个网格，纬度方向2个网格，陀螺漂移0.02 (°)/h，加速度计偏置为0.0005 m/s2。测深传感器采样时间为60 s，在两块地形上分别采样10个点后进行ICCP算法匹配，匹配结果如图4(a)、图4(b)所示。

图3(a) 地形1Fig.3(a) Terrain 1

图3(b) 地形2Fig.3(b) Terrain 2

图4(a) 地形1匹配结果Fig.4(a) Matching result of terrain 1

图4(b) 地形2匹配结果Fig.4(b) Matching result of terrain 2

对地形1和地形2在经纬度方向的最大误差和平均误差进行统计，统计结果如表1所示。

表1 两块匹配误差统计结果Tab.1 Matching errors of two terrains

若要求潜器经纬度方向定位精度小于一个网格，也即139 m，则从表1可以简单判定地形1为可匹配地形，地形2为不可匹配地形。

在地形2上对同步定位与构图算法的有效性进行仿真验证。分别为水下潜器设置20 m × 20 m、300 m × 200 m的运动空间，设定声纳的扫描范围为20 m，惯性仪表参数设置同上。如图5(a)、图5(b)所示分别为潜器在小、大两个运动空间上以不同的航行路径所得到的仿真结果图。

图5(a) 20 m × 20 m仿真结果Fig.5(a) Simulation result of 20 m × 20 m

如图5(a)、图5(b)中，“-o”线所示为纯惯导指示航迹，“-”线所示为潜器理想航迹，“-口”线所示SLAM算法得到的航迹。从图中可以明显地看到，当纯惯导得到的航迹与潜器理想航迹存在较大偏差时，SLAM算法得到的航迹很接近于潜器的理想航迹。

图5(b) 300 m × 200 m仿真结果Fig.5(b) Simulation result of 300 m × 200 m

5 结 论

综上所述，算法不仅可以在地形可匹配区域，采用ICCP算法实现高精度的匹配定位，也可以在地形不可匹配区域，采用SLAM算法实现精确的定位，这对于实现水下潜器长时间高精度导航具有很重要的意义。

（References）：

[1] Bryson M. Active airborne localisation and exploration in unknown environments using inertial SLAM[C]//IEEE Aerospace Conference. 2006: 229-241.

[2] 徐晓苏, 吴剑飞, 徐胜保, 等. 基于仿射修正技术的水下地形 ICCP匹配算法[J]. 中国惯性技术学报, 2014, 22(3): 362-367. Xu Xiao-su, Wu Jian-fei, Xu Sheng-bao, et al. ICCP algorithm for underwater terrain matching navigation based on affine correction[J]. Journal of Chinese Inertial Technology, 2014, 22(3): 362-367.

[3] Kim J, Sukkarieh S. Real-time implementation of airborne inertial-SLAM[J]. Robotics and Autonomous Systems, 2007, 55(1): 62-71.

[4] 杨绘弘. 基于ICCP的水下潜器地形辅助导航方法研究[D]. 哈尔滨: 哈尔滨工程大学, 2009. Yang Hong-hui. Research on terrain matching algori- thm based on ICCP for underwater vehicles[D]. Harbin: Harbin Engineering University, 2009.

[5] Zheng Tong, Bian Shao-feng. Study on the algorithm of sea bottom terrain match[C]//Manchester, United Kingdom, 2009: 179-182.

[6] 王文晶. 基于重力和环境特征的水下导航定位方法研究[D]. 哈尔滨: 哈尔滨工程大学, 2009. Wang Wen-jing. Underwater navigation methods based on gravity and environmental features[D]. Harbin: Harbin Engineering University, 2009.

[7] Durrant H F, Bailey T. Simultaneous location and mapping: PartⅡ[J]. IEEE Robotics and Automation Magazine, 2006, 13(3): 108-117.

[8] Ribas D, Ridao P, Tardos J D, et al. Visual SLAM for 3D large-scale seabed acquisition employing underwater vehicles[C]//Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems. Nice, France: IEEE Press, 2008: 1011-1016.

[9] 李佩娟, 徐晓苏, 张涛. 信息融合技术在水下组合导航系统中的应用[J]. 中国惯性技术学报, 2009, 17(3): 344-349. Li Pei-juan, Xu Xiao-su, Zhang Tao. Application of information fusion to integrated navigation system of underwater vehicle[J]. Journal of Chinese Inertial Technology, 2009, 17(3): 344-349.

[10] Deng Z, Ge Y, Guan W, et al. Underwater map-matching aided inertial navigation system based on multigeophysical information[J]. Frontiers of Electrical and Electronic Engineering in China, 2010, 5(4): 496-500.

[11] 王向磊, 苏牡丹, 刘培根, 等. 利用改进的ICCP算法辅助导航[J]. 测绘科学, 2013, 38(1): 36-39, 50. Wang Xiang-lei, Su Mu-dan, Liu Pei-gen, et al. Application of improved ICCP algorithm in gravity matching aided navigation[J]. Science of Surveying and Mapping, 2013, 38(1): 36-39, 50.

Underwater navigation method based on terrain and environmental features

XU Xiao-su1,2,Tang Jun-jun1,2, ZHANG Tao1,2, YUE Zeng-yang1,2
(1. Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology, Ministry of Education, Southeast University, Nanjing 210096, China; 2. School of Instrument Science & Engineering, Southeast University, Nanjing 210096, China.)

The main drawback in traditional terrain aided navigation(TAN)/SINS systems is that it becomes useless in some area when without any terrain data or with a featureless terrain field. In order to realize long-distance and high-precision navigation for an underwater vehicle, a new navigation algorithm is presented which combines terrain data with environmental features. For the areas where terrain data is available and terrain is of great change, TAN is used to correct errors of SINS; for the areas where terrain data is not available or terrain is of less change, a feature-based SLAM method is used to correct the error of SINS. Simulation results show that the proposed method can achieve a better positioning precision than that of pure inertial navigation both in matchable areas and unmatchable areas.

underwater vehicle; terrain aided navigation; simultaneous localization and mapping; strapdowm inertial navigation system

U666.1

：A

2015-05-23；

：2015-08-28

国家自然科学基金项目（51175082，61473085，51375088）、微惯性仪表与先进导航技术教育部重点实验室基金，优秀青年教师教学科研资助计划（2242015R30031）

徐晓苏（1961—），男，博士生导师，从事测控技术与导航定位领域的研究。E-mail：xxs@seu.edu.cn

1005-6734(2015)05-0590-07

10.13695/j.cnki.12-1222/o3.2015.05.007