面向自主靠泊的船舶航迹估计方法

邹 雄, 肖长诗,b, 文元桥,b, 周春辉,b, 詹文强

(武汉理工大学 a.航运学院； b.内河航运技术湖北省重点实验室， 武汉 430063)

目前，随着无人航行器和自动驾驶越来越受到关注，无人艇(Unmanned Surface Vehicle，USV)作为航行器的一个分支，应用也日趋广泛，不仅应用在军事行动上，还应用在民用领域中。USV非常适合执行危险和耗时的任务，例如海上勘测、危险水域的搜索和救援、海岸线巡逻和水质检测等。[1]为使用USV执行这些任务，精确导航是至关重要的能力。车载导航广泛使用的工具之一是组合惯性导航系统(Inertial Navigation System，INS)和全球定位系统(Global Positioning System，GPS)的组合导航系统。INS通常以高频率运行，但不可避免地具有漂移误差，而GPS可提供绝对位置，其集成为海洋导航应用提供理想的互补组合。然而，在大型建筑物附近，如桥梁、建筑物周围以及内河水域，GPS的使用常常受到限制。[2]

针对这些局限，激光里程计和视觉里程计(Visual Odometry，VO)迅速发展，与轮式里程计一样，传感器数据已被用来计算其相对运动轨迹。激光雷达方法的缺点是所使用的传感器非常昂贵。此外，其测量范围一般较小，不适合检测远距离海洋标志物。[3]BANDYOPHADYAY等[4]提出由于水面波动的影响，激光传感器在大于20 m的范围内相对无效。VO被定义为通过观察其环境的图像序列来估计相机的相对运动的过程，如果将单个或多个摄像机固定到载体上(车辆、人和机器人)，估计的位姿则为载体的相对运动。[5]而且，VO视觉方向性强，在视距好的情况下具有覆盖距离很远等诸多优势，所以VO在水上环境尤其是在近岸水域具有很好的发展潜力。

1 相关工作

MORAVEC提出利用视觉来估计无人车(Unmanned Ground Vehicle，UGV)的运动。MAIMONE等[6]提出视觉里程计的概念，并在美国国家航空航天局(National Aeronautics and Space Administration,NASA)开发的火星探测器上使用，其装备两个向下的相机组成双目视觉，通过提取兴趣点并采用跟踪的方式完成相邻帧的特征匹配，最后采用最大似然估计算法得到运动估计，并在30 m的轨道上能获得2%的精度。NISTÉR等[7]也对VO进行研究，首先检测Harris角点，提取专用描述符，然后在一定的搜索区域内完成特征匹配，在运动估计时提出单目和立体视觉两种方案，而且在无人车上实施并验证可行。随后，KONOLIGE等[8]针对大尺度崎岖不平的野外场景，提出双目实时VO，融合惯性测量单元(Inertial Measurement Unit，IMU)来减少角度偏差的增长，最低误差能达到0.36%。GEIGER等[9]提出双目视觉里程计算法并将其开源，其采用自定义的检测子和描述子并用专用指令集进行优化，在前后帧的匹配上采用运动模型引导搜索，在“外点”去除上采用“环形”匹配予以验证，最后通过特征点在左右图像的重投影误差和的最小化得到相机的运动估计。

针对单目的尺度不确定性问题，SCARAMUZZA等[10]在假设地面平坦的基础上，将全景相机放在车辆顶部并结合车辆的运动学模型，提出一种基于随机抽样一致(Random Sample Consensus, RANSAC)的单点算法，能够最大程度地降低迭代次数和运行时间。KITT等[11]利用相机高度和车辆附近地面的平面假设得到尺度信息并恢复运动估计。相机由于具有体积、重量、成本等诸多优点，其是无人机(Unmanned Aerial Vehicle，UAV)中最具竞争力的传感设备。BROCKERS等[12]修改第一个多线程实时的同步定位与建图系统(Parallel Tracking and Mapping，PTAM)用于无人机的自主导航，得以在机载嵌入式计算机上以30帧/s的速度估计无人机的6D姿态。FORSTER等[13]提出一种半直接视觉里程计算法(Semi-Direct Monocular Visual Odometry，SVO)，在一台嵌入式计算机上以55帧/s的速度运行，并输出稀疏的3D环境地图。FU等[14]采用嵌入式系统实现无人机的立体视觉预处理系统，其中双目视觉里程计由一个基线为12 cm的单独模块实现，该系统可实时估计六自由度无人机姿态以及重构无人机的周围环境，在室内0.8 m/s的速度下，估计精度小于3 cm。 HUANG等[15]将深度相机用于四旋翼无人机的自主飞行，采用FAST特征提取和9×9大小的像素块作为描述符通过绝对误差和算法找到匹配特征点，实现基于深度相机的VO，能够在静态室内环境中估计自身位姿并生成丰富的环境地图。STEINBRÜCKER等[16]针对深度相机提出能量最小化方法得到图像间相机的相对运动，该方法在较小的相机位移和静态环境下的性能良好。KERL等[17]在此基础上提出一种更高精度和更广用途的扩展版本(Depth Visual Odometry，DVO)，通过最小化光度误差得到两个图像间的相机运动关系。然而与视觉导航在UGV和UAV的应用如此广泛有所不同，关于VO与USV结合的研究，尤其是在海事环境中，面向USV的视觉导航的实现还有很多未知领域，而且针对UGV和UAV开发的视觉里程计算法均不能高效地适合USV的水域环境。VO算法需要检测每帧图像中的特征点，但是这不适合于特征很少的开放海洋环境。[18]在水域环境中VO的性能受到快速变化的水面、反射等情况的严重限制。[19]面对这些困难，相关学者研究船舶的视觉导航和水域环境的重建。HUNTSBERGER等[20]针对海洋环境设计一种具有4个摄像机的立体视觉系统，以增加视觉范围，实现对水中目标和障碍物的检测。LEEDEKERKEN等[21]提出既能在水面又能在水下场景都能建图和导航的方法，该方法利用开放水域中的GPS数据和GPS受限区域的其他传感器数据。

本文主要研究利用视觉传感器在无需外部位置信息的情况下,在水域环境中生成USV的轨迹和三维稀疏点云。特别地，在VO算法中引入水岸线检测算法，以提高轨迹精度。本文的创新之处在于不像传统的VO需要在所有图像帧中完成特征检测，本算法只需在关键帧中检测特征，帧间通过特征点的跟踪匹配进行关联，不仅可提高精度，还可加快初始化速度。

2 算法设计

本文的目的是面对靠近岸边的水域环境，利用船载视觉传感器获取图像，提取和跟踪图像中比较稳定的特征点，从而估计出船舶的航迹。提出的算法框架见图1，其算法框架分为两部分：一部分为水岸线检测；另一部分为航迹估计也叫运动估计。其中水岸线检测的目的是将快速变化的水面给予去除，运动估计通过VO的方式实现。

图1 算法框架

2.1 水岸线检测

面对近岸水域，获取的水岸线图像分为3部分见图2:图像上方是天空背景，往往最亮(白天)，其灰度变化平缓；图像下方是水面背景，其灰度明显小于天空背景，而且水面区域往往有岸边景物的倒影和受风浪流影响的波纹；图像中间是岸边区域，主要是草地、树木和建筑，其灰度值最低。岸上区域和水面区域的交界线称为水岸线，其是梯度较大值的连线。由于艇载视觉的高度加上水面的水平特性，所以通常水岸线在图像中为直线，但由于无人艇航行和艇载视觉的朝向，水岸线有一定的倾角。

a) 长江水域

b) 东湖水域

本文借用文献[22]的思想，提出的水岸线检测算法分为3步如图1所示。

2.1.1边缘检测

边缘是图像中周围像素灰度值显著变化的像素点的集合，边缘检测就是提取这些显著变化的点，常用的边缘检测方法有一阶微分算子Roberts、Sobel和二阶微分算子Laplacian、Marr等。这些方法的优点是原理简单，速度快，但是对噪声敏感。诸多的边缘检测算子中，Canny算子是最佳边缘检测算子，具有非常好的检测效果，其具体步骤为

(1) 采用高斯滤波器对图像进行平滑;

(2) 通过邻域范围的有限差分，计算梯度幅值和方向;

(3) 对梯度幅值图像进行非极大值抑制，只保留局部梯度幅值最大的点;

(4) 采用双阈值法不断地收集边缘直到将图像边缘闭合。

2.1.2线段提取

通过采集和分析水岸线图像，其有如下特点：

(1) 水岸线往往贯穿整个图像，即使有时被遮挡，但其长度、连续性还是好于水面波纹。

(2) 岸边建筑物在图像中呈直线状，且与水岸线垂直。

(3) 统计来说，水面波纹呈现的直线段长度大概率小于5个像素(图像为640×480)。

(4) 统计来说，水岸线往往与图像水平夹角在45°之内。也就是说：通过角度和线段长度的判断可把绝大多数的岸上建筑物和水面波纹的干扰消除。所以本方法采用霍夫变换提取线段，其中只考虑长度为5像素以上并且直线斜率在[-1，1]之间的线段。

2.1.3直线拟合

采用RANSAC算法进行直线拟合，其迭代次数为

(1)

式(1)中：p为正确概率；w为每次从数据集中选取一个局内点的概率。

为保证工程应用的实时性和可靠性，该方法针对RANSAC算法做了细节上的修改：受CHUM等[23]的启发，从最可信的匹配中挑选样本，在样本的选择上采取如下规则：

(1) 第一个点的选取，越靠近图像中心水平线选择优先级越高;

(2) 第二个点的选取，以第一个点为原点，水平移动一定距离(100像素)得到中心点，越靠近该中心点优先级越高；抽样次数，不需要遍历所有可能情况，保证至少有一个优样本(样本数据都是“内点”的样本)即可。据统计计算量可降低60%。

2.2 运动估计

利用去除水面区域后的图像来估计船舶航迹，分为3个部分：首先对图像特征检测并跟踪，得到两个关键帧的特征对应关系;然后估计两相机的相对运动;最后采用滑动窗口局部优化。

2.2.1特征匹配

到目前为止，研究者陆续提出许多局部特征方法，如Harris、FAST、SURF、SIFT、ORB等，其中前两种是角点，后几种特征包含关键点和描述符。通常有匹配特征点的描述子和跟踪两种方式得到图像间的特征点对应关系。本文采取多尺度FAST特征加上Lucas光流(Kanade-Lucas-Tomasi,KLT)跟踪的方式实现图像间的特征匹配。其中尺度因子取1.2，当检测到的特征数量太少时，将阈值(默认值20)减半，以确保检测到足够多的特征点数量。KLT属于一种光流法，假设：相邻帧之间的亮度恒定；相邻帧之间的运动微小；相邻点有相似运动。

针对研究的水域环境，与UGV和UAV相比，船舶的运动较慢，而且水域环境中的运动物体较少，所以这种匹配方式较为适合，而且最为关键的是不需要每一帧图像都进行水岸线检测，这对算法的运行效率相对较高。

2.2.2位姿估计

在初始化完成之后，已经具备全局坐标系和初始化的场景结构，每当提取一个关键帧，通过特征跟踪，得到特征点的匹配关系，以及在当前图像中是可见的初始场景点，这构成2D-3D的对应，可通过n点透视问题(Perspective-n-Point, PnP)求解。换句话说，已经知道的场景点(X1,X2,…,Xj,…,Xn)和其在图像中的特征位置以及在目前的图像帧的对应位置，能够估计当前的相机在全局坐标系中的位姿。

由于在位姿估计中通常会有少量的“外点”干扰，采用RANSAC方案得到最大的一致集以及位姿估计的初值。随后通过场景点在图像中的重投影误差构建最小二乘优化问题，通过光速法平差(Bundle Adjustment, BA)对估计值进行调整。

2.2.3局部优化

每当提取一个关键帧，一方面为得到更加精细的位姿估计，需要对前面的相机位姿和场景点进行优化，另一方面为控制一定的计算量，所以设定只在一定的滑动窗口内执行优化操作[24]，具体如下：取窗口大小M和最新关键帧Nk，其他的相机位姿保持固定不变，使用LOURAKIS等[25]提出的稀疏束调整进行实现。在局部优化完成后，通过当前关键帧与前后的两个关键帧，采用三视角的三角化方案[26]得到场景3D点。

3 稀疏集中调整

3.1 关键帧的选择

随着程序的运行，因为有些场景移出视线，所以能够跟踪成功的特征点的数量越来越少。当该数量下降到一定阈值时，选择一个新的关键帧。但是，如果关键帧过于密集，不仅会影响程序的运行速度，还会增加空间点三角测量的不确定性。[27]所以选择关键帧，既希望他们之间有尽可能多的相机运动，又需要有足够的匹配。[11]

3.1.1初始关键帧的选择

除去场景模糊等之外的情况，通常取第一帧为关键帧K1，进行水岸线检测，去除水面区域，然后提取L0个特征点，采取持续跟踪的方式选择第二关键帧K2，使得在K1和K2之间至少存在L1个匹配点，然后选择第三个关键帧K3，使得在K1和K3之间至少有L2个匹配点。

3.1.2关键帧的选择

在初始化完成之后，对最近的关键帧图像提取L0个特征点，之后每获取一帧图像，都采用KLT跟踪并统计特征点数量，当特征点数量降为L2个，设置当前帧为关键帧，并进行去除水面操作，然后提取L0个特征，依次往复。

3.2 初始化过程

初始化过程在初始关键帧中实现，其步骤如下：

1) 得到3个初始关键帧K1、K2、K3，在去除水面之后，K1与K2、K1与K3之间各有L1和L2个对应特征点。

2) 设定K1为全局坐标系，在K1与K2和K1与K3之间通过对极几何在RANSAC框架下分别计算相对位姿。

3) 基于本质矩阵E(K1与K3)，得到K3与全局坐标的相对位姿(R，t)，其中R为相机的旋转，t为相机的位移。

4) 使用K3的相机位姿和对应的特征点，先三角化得到三维点，再利用其与K2中的对应特征采用PnP算法得到K2帧的相机位姿。

5) 在这3个关键帧中，通过场景点在图像中的投影位置构建重投影误差为

(2)

至此，初始化完成后，位姿估计算法的过程为

(1) 对于每一帧，判断是否为关键帧，如果不是，重复(1);

(2) 采用PnP算法估计相机位姿的初值，三角化得到场景点，将该帧加入窗口;

(3) 在窗口中构建重投影误差，采用(2)中的初值进行优化，并返回(1)。

4 试 验

试验分为水岸线检测试验和船舶运动估计试验两部分。试验设备有：iNav-II型USV和相关设备，视觉系统由工业相机(200 W pixel，3.2 μm像素尺寸)和工业透镜(2.8～12.0 mm，1.0∶1.4)组成，捕获的图像以30帧/s分辨率记录，分辨率为1 280×720像素。PC机具有双核英特尔I5处理器和4 GB存储器，Win 7系统，Visual C++编辑器等。

iNav-I型USV是我们自主设计的4.15 m长和1.6 m宽的无人艇见图3。该USV采用单吊舱推进器，偏转角限制在-25°～25°。整个船舶动力和设备动力由两组48伏锂电池提供。USV系统由两部分组成：USV子系统和岸基支撑系统。USV子系统主要包括差分GPS移动台、惯性导航、电子罗盘、船载无线电、风速风向仪、应急操纵装置和控制计算机。该子系统使用差分GPS移动台的每秒脉冲(Pulse Per Second,PPS)来实现精确定时。船载系统中各传感器和设备的数据通过MavLink协议进行封装，然后通过交换机和无线电台进行传输和通信。岸基支持系统主要包括岸基无线电台、岸基差分基站和岸基地面站等3个部分，前两个对应于USV子系统，岸基地面站负责船舶运行状态数据的显示和存储以及控制命令的发布。

图3 iNaV-I型USV

选择3个位点作为试验场所。地点1是中国第二大城市湖泊武汉东湖水域，场地水域宽阔畅通无阻。地点2和地点3分别在武汉长江大桥附近和二桥附近。由于长江的严格管理，USV被禁止，因此，视频是在渡船上使用视觉系统采集的，差分全球定位系统(Differential Global Position System,DGPS)信息通过基准站R6和移动站R93T组合得到，可达到厘米级精度。

4.1 水岸线检测试验

为验证本文水岸线检测算法的有效性和实用性，采取实船验证的方式：第一列为利用iNav-I型USV在东湖获取的水岸线图像，试验中将摄像机传感器放置在无人艇上，其方向和无人艇的船头方向保持一致；第二列和第三列分别是通过视觉传感器在轮渡上采集的，地点分别为武汉长江大桥附近和二桥附近。水岸线检测结果见图4。

图4中:图4a、图4b和图4c为采集的原始图像;图4d、图4e和图4f为边缘检测后的图像;图4g、图4h和图4i为提取线段后的图像放在原图中的效果;图4j、图4k和图4l为更加清晰显示的二值图像;图4m、图4n和图4o为直线拟合后的结果。由图4可知：本文的水岸线检测算法基本可满足要求，也为后续位姿估计提供较好的条件。

a) 东湖原始图像

b) 一桥原始图像

c) 二桥原始图像

图4 水岸线检测结果

4.2 运动估计试验

本文研究的最终目的是针对内河领域，估计船舶在靠泊过程中的运动轨迹。所提出的VO系统利用OpenCV SDK3等库在Visual C++2015中实现。试验是在中华路码头到武汉港码头的渡船上，将视觉传感器放置渡船侧面，采集岸边视频数据。轮渡的航行轨迹和采集的图像见图5，水面上的路径是渡船的轨迹，黑白图像是对应点捕获的图像。

图5 轮渡的航行轨迹和采集的图像

系统运行图见图6，右上角为实时的船舶运动估计和三维场景点，右下角为船舶轨迹。针对本实验，特征点个数和L0、L1、L3分别取200、180、160。滑动窗口中的窗口大小M和最新关键帧N分别取7和3。

图6 系统运行图

估计轨迹和实际轨迹的对比见图7。图7中显示的是估计的船舶航迹节点(小方框)和真实数据(Real Time Kinematic,RTK)值(实线)的对比。由图7可知:在近80 m范围之内精度很高，本文算法可正常运行，目测精度良好，在超过该距离(图7中的小叉点)后，船舶已经转向，相机对着开阔水域，算法无法获得精确估计，误差已逐渐变大。这也是后续需要研究的方面。本试验初始化耗时3 s，而ORB-SLAM无法完成初始化。其中，本文算法没有尺度信息，80 m是通过RTK与算法估计轨迹对比得到的，在这段期间，相机采集1 418帧，有26个关键帧，特征点平均生存6.5个关键帧，这也从另一方面说明滑动窗口取7比较适合。

图7 估计轨迹和实际轨迹的对比

5 结束语

本文针对船舶在靠泊过程中自身轨迹估计和周围环境重建的需求，提出一种基于水面区域去除的船舶航迹估计方法。通过现场试验以及与RTK数据的对比结果表明，该方法切实可行，但是针对船舶具体靠泊会涉及转向，而单目普通相机无论是视野还是尺度都无法满足这种需求，这也是下一步研究的方向。