基于图像声纳SLAM算法在AUV组合导航中的应用

刘 明, 张 严, 徐德民



基于图像声纳SLAM算法在AUV组合导航中的应用

刘 明1, 张 严2, 徐德民1

(1. 西北工业大学 航海学院, 陕 西西安, 710072; 2. 中国船舶重工集团公司第705研究所 昆明分部, 云南 昆明, 650118)

为了在有限的体积、功耗及成本范围内提高自主水下航行器(AUV)远程、深海导航定位精度, 提出了将基于图像声纳的同时定位与地图创建(SLAM)方法应用于AUV水下组合导航系统, 利用图像声纳获取AUV与地形特征点之间的距离和3D方位数据, 结合捷联惯性导航系统(SINS)得到的导航数据, 通过扩展卡尔曼滤波(EFK)方法对载体状态和地图状态进行连续并行估计和量测, 将得到的误差估计反馈回SINS进行修正, 可抑制其随航行时间和距离增加的姿态、速度和位置误差。此外, 在地形特征点向量中加入声学尺寸元素, 可提高特征识别的准确性。仿真结果表明, 在持续观测到有效的地形特征点条件下, 惯导误差得到了较好抑制, 特别是在AUV返程或往复巡航过程中, 重复观测到同一地标时, 可大幅提高水下组合导航的定位精度。

自主水下航行器; 同时定位与地图创建; 图像声纳; 捷联惯性导航系统

0 引言

精确水下导航是远程自主水下航行器 (autonomous underwater vehicles, AUV)有效应用和成功回收的关键技术之一, 要求导航系统必须在有限的体积、功耗和成本范围内长时间保持较高的位置、速度及姿态精度。但由于恶劣的水下环境无法接收GPS信号, 导航误差随着航行时间和距离的增加而发散的问题长期无法有效解决。

各国科学家对此展开了积极探索并取得了卓有成效的进展, 其中很重要的一个突破来自机器人导航领域的同时定位与地图创建(simultaneous localization and mapping, SLAM)技术。SLAM的出现使得在未知区域的地形辅助导航成为可能, 其基本思路是, 当机器人被放置在一个未知的位置和陌生的环境中时, 在不断构造与周围环境具有相合性(consistent)的增量式地图的同时确定自身在地图中的位置[1]。由于地形辅助导航具有与GPS一样误差不随时间增长的优良特性, 可以同惯性导航系统构成很好的组合。这一特性为AUV水下长航程或者长航时自主式导航过程中抑制惯性系统误差的发散提供了新的思路。研究基于SLAM方法的远程AUV组合导航技术, 对于抑制惯性导航系统随时间和距离增长的误差, 提高导航精度具有实际意义。

SLAM技术在过去的十几年中取得了很多实用性的成果[2], 如基于摄像机的SLAM算法在无人飞机上已获得成功应用[3], 悉尼大学水下机器人Oberon在近海测试结果证明SLAM算法生成的估计是连续一致的[4], 麻省理工学院2000年利用真实海洋声纳进行SLAM算法测试证明其误差增长比INS＋DVL慢得多, 而对于室外,特别是海底的非结构化未知环境中采用这一技术目前还处于试验阶段。

1 SLAM在AUV组合导航中的应用

1.1 地形传感器性能

本文以Imagenex 技术公司837型“Delta T”民用多波束图像声纳作为地形传感器, 其基本性能指标如下[5]: 频率260 kHz, 扫描范围在120°(水平)× 20°(垂直), 单波束水平宽度为3°, 距离分辨率是0.2%(为距离), 探测距离为5~100 m, 输出接口为10 M以太网。

该传感器输出的数据比较复杂, 主要用于图像显示, 本文直接采集其各个波束输出的原始数据, 利用特征提取方法实时获得满足要求的地图特征点相对于AUV的方位和距离数据。

1.2 SLAM基本原理

SLAM方法并不需要象地形匹配导航中那样事先存储地图信息, 而是通过避碰声纳实时构造一个增量式地图, 并行估计AUV在这个地图中的位置, 从而形成一个真正自持的自主式导航系统, 系统框图如图1所示。

图1 系统框图

文献[6]已证明SINS、DVL和SLAM系统利用联邦卡尔曼滤波进行有效融合, 可使水下航行器的水平速度误差、姿态角误差、航向角误差和位置误差都在较短时间内以较高精度达到稳定。

SLAM方法中, 当地形传感器检测到一个有效的地形特征时, SLAM估计器将该特征点位置增加到地理坐标系下的地图数据库中, 然后通过连续的量测并行估计AUV和地图的状态, 得到的误差估计值反馈回INS进行修正, 使其保持高于纯惯导的姿态、速度和位置精度。然而问题是随着地标数目的增加, 地图状态变量维数以地标数目的平方关系递增, 算法很快发散, 突破来自Durrant-Whyte给出的SLAM问题的结构描述和收敛性分析结果:如果把地图创建和定位估计同时进行, 算法是收敛的。以往试图消除的地标之间的关联度其实是解决问题的关键, 而且关联度越大, 收敛性越好[1]。

1.3 数据融合算法模型

1.3.1 AUV运动模型

式中:为横滚角;为俯仰角;为航向角。

1.3.2 SLAM算法框架

SLAM算法基本框架是利用卡尔曼滤波器作为状态估计器, 在给出AUV运动模型和AUV与特征点之间的相对位置量测之后, 估计出地图的结构以及AUV在地图中的位置、速度及方位[7]。

算法以AUV及地图的上述变量的误差作为状态变量

每当有新地标进入则地图状态变量更新

每个特征点向量包括该点在导航坐标系下的位置和声学尺寸,为当前地图中的特征点数目。

连续时间的误差模型为

式中:f ,为来自INS的加速度及转动速率; f ,为对应的误差值。

线性化后SLAM误差离散时间状态方程为[7]

其中，()为惯导系统白噪声, 其噪声强度为()。

噪声强度

1.3.3 地形特征点方位/距离/高度量测

量测方程为非线性模型[7]

式中，()为量测噪声向量。

由于该方程为载体和第个地形特征向量的函数, 因此方程

式中:()为第次量测；v()是方差为()的白噪声。

第个地形特征点地理系中的位置为

式中:,,分别为地形传感器量测得到的特征点相对距离, 方位角及高度角。

因此, 式(16)可写为

此处

1.3.4 量测误差方程

以该误差作为量测状态向量建立量测方程

式中:()为量测的雅可比矩阵;()是强度为()的量测噪声。

量测方程包含传感器坐标系到地理坐标系、直角系到极坐标系的转换, 经过线性化处理后得到观测矩阵和噪声输入矩阵为

1.3.5 Kalman滤波估计

其中

1.3.6 误差修正

地图位置修正

上述公式递推执行构成了一个完整的SLAM/INS估计的闭环运算过程。

1.4 数据关联

数据关联是指不同时刻对环境的观测是否来源于同一特征。对于扩展卡尔曼滤波器(extended Kalman filter, EKF)模型来说, 数据关联不准确将导致算法无法收敛。数据关联目的是找出时刻的量测与地图中地形特征点之间的对应关系, 将量测与地形特征点之间进行准确的关联是构造实时地图的关键步骤, 直接决定SLAM算法的成败。

文献[8]采用正则化新息方差(normalised innovation square, NIS)进行关联计算

假设新息及其协方差均满足正态分布, 则为2分布。在预先确定的置信度水平下, 对2用查表法得到关联阀值, 每次量测计算得到的小于该阀值, 则该次新息可与特征点关联。

2 算法数学仿真结果

数学仿真以837型“Delta T”多波束成像声纳作为地形传感器对方位和距离进行量测, 各传感器参数按民用级别设定如下[5]。

仿真边界条件: 假设AUV在水下贴近海底200 m高度范围内航行, 根据国外常见的AUV参数假设平均航速≤10 kn, 航行时间2 000 s, DVL对INS漂移的补偿不予考虑, 仅考虑INS和声纳的量测误差, 噪声水平见表1, 可关联的地形特征点分26个和54个2种情况进行仿真。

表1 仿真参数

Table 1 Simulation parameters

开始时刻假定地图中有5个利用GPS确定的地形特征点, 之后GPS信号无效, INS误差和地图误差通过新量测到的地形特征点各自独立并行估计, 随着时间推移, INS误差不断增长, 导致地图误差随之增大, 但在具有SLAM相关性的闭环作用下, INS误差被有效抑制, 进而消除了地图中因INS导致的误差项。每观测到一个特征点, 地图估计方差单调下降, 反过来抑制了INS误差增长, 实现了提高INS定位和测姿精度的目标。数学仿真得到的轨迹见图2。

观测新地标时, SLAM反馈回路可起到类似GPS误差修正的作用, 特别是在回旋路径上反复观测到同一地标时, 效果更加明显。图3为放大的轨迹图, 修正的部分效果加入SLAM反馈前后东向和北向速度误差对比见图4和图5。从图中可知, 误差增长得到有效控制, 这一特性对于提高返程阶段或在同一海域反复巡航的AUV导航精度有一定意义。

图2 AUV运动轨迹图

图3 SLAM回路效果放大图

图4 东向速度误差

图5 北向速度误差

从26个特征点和54个特征点这2种情况下速度误差曲线可见, 在数学仿真理想的边界条件下, 地形特征点的数量对于导航经度的影响主要表现在噪声强度上, 对于绝对定位精度没有数量级的提高。

3 结束语

837型“Delta T”多波束成像声纳作为地形传感器, 可生成3D声纳图像, 地形信息含量更为丰富, 且输出频率较高。本文针对这一特点, 在提高地形特征点密度前提下仿真了AUV高速(10 kn)航行时速度误差估计的效果, 结果表明, 使用该型声纳可使AUV在更大的航速范围内使用SLAM算法进行组合导航。同时本文在地形特征向量中加入地标声学尺寸元素, 有利于AUV返航过程中再次观测到同一地标时更加准确快速的作出判断, 数学仿真表明, 回旋过程导航精度得到了一定程度的提高。

随着声纳技术的发展, 在绘制地图范围和地图特征提取准确度上也已取得进展, 这给提高AUV水下远程深海组合导航精度展现了光明前景。当然, 利用声纳作为地形传感器仍然面临很多问题, 例如海底环境特征非结构化特征导致制图困难; 声纳成像速度和AUV航行速度的匹配; 水中动态障碍物和静态障碍物作为地形特征点的甄别和遴选[9]等都对SLAM算法成败起决定性作用。

[1] Durrant-Whyte H, Batley T. Simultaneous Localization and Mapping: Part I[J]. IEEE Robotics & Automation Magazine, 2006, 13(2): 99-110.

[2] Nüchter A, Lingemann K, Hertzberg J. 6D SLAM—3D Mapping Outdoor Environments[J]. Journal of Field Ro- botics, 2007, 24(8/9): 699-722.

[3] Steder B, Grisetti G, Stachniss C, et al. Visual SLAM for Flying Vehicles[J]. IEEE Transactions on Robotics, 2008, 24(5): 1088-1093 .

[4] Majumder S, Dissanayake G, Durrant-Whyte H. Multi- Sensor Data Fusion for underwater Navigation[J]. Jour- nal of Robotics and Autonomous System, 2001, 35(2): 97-108.

[5] Imagenex Technology Corp. Delta T Imaging Manual [EB/OL].[2006-07].http://www.imagenex.com/docs/prod/folders/print/Delta T.html.

[6] 李佩娟, 徐晓苏, 张涛. 信息融合技术在水下组合导航系统中的应用[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 Tech- nology, 2009, 17(3): 344-349.

[7] Kim J, Sukkarieh S. 6DoF SLAM Aided GNSS/INS Navigation in GNSS Denied and Unknown Environ- ments[J]. Journal of Global Positioning Systems, 2005, 4(1-2): 120-128.

[8] Kim J. Autonomous Navigation for Airborne Appli- cations[D]. Sydney:The University of Sydney, 2004.

[9] 蔡自兴, 肖正, 于金霞. 动态环境中移动机器人地图构建的研究进展[J]. 控制工程, 2007, 14(3): 231-235. Cai Zi-xing, Xiao Zheng, Yu Jin-xia. Advances on Map Building with Mobile Robots in Dynamic Environments [J]. Control Engineering of China, 2007, 14(3): 231-235.

Application of SLAM Algorithm Based on Image Sonar to AUV Integrated Navigation

LIU Ming1, ZHANG Yan2, XU De-min

(1. College of Marine Engineering, Northwestern Polytechnical University, Xi¢an 710072, China; 2. Kunming Branch of the 705 Research Institute, China Shipbuilding Industry Corporation, Kunming 650118, China)

To improve the long-distance or deep-sea navigation and localization precision for an autonomous underwater vehicles(AUV) within limited volume, power and cost, an algorithm of simultaneous localization and mapping(SLAM) based on image sonar is presented for AUV underwater integrated navigation system. According to the data of real-time distance between each feature on map and an AUV, the data of three-dimensional bearing from image sonar and the data of navigation from strapdown inertial navigation system(SINS), the carrier state and map state can be estimated and measured simultaneously and continuously by extended Kalman filter(EKF). The errors are fed back to SINS for correction, hence the accumulated errors of attitude, velocity and position increasing with running distance and time can be effectively inhibited. Furthermore, the accuracy of features identification can be improved by putting acoustics size element into map features vector. Simulation result shows that this method can effectively inhibit errors of inertial navigation by continuously measuring valid features on map. Particularly, the localization accuracy of an underwater integrated navigation is significantly enhanced when same landmarks are observed repeatedly in cruise or return voyage of an AUV.

autonomous underwater vehicles (AUV); simultaneous localization and mapping (SLAM); image sonar; strapdown inertial navigation system (SINS)

TJ630.33; TP24

A

1673-1948(2011)04-0276-06

2011-01-17;

2011-02-17.

刘 明(1973-), 男, 高工, 博士研究生, 研究领域为水下航行器组合导航.

(责任编辑: 杨力军)