基于双目视觉的无人船障碍物探测与地图构建

23

(1.南通大学 计算机科学与技术学院，江苏 南通 226019；2.南通先进通信技术研究院，江苏 南通 226019;3.西交利物浦大学 计算机科学与软件工程系，江苏 苏州 215123)

0 引言

在智能化水质监测任务中，无人船扮演着重要角色[1-2]。无人水质监测船在其自主航行作业时需要自动巡航并自主避障，周围环境感知是先决条件，因此智能化识别周围环境是实现避障的关键技术[3]。由于现有无人水质监测船一般采用超声波避障，其只能探测船体前方障碍物，且探测距离和精度较低、避障功能弱，而雷达系统在近距离内又存在盲区，因此无人船真正实现自动巡航和自主避障需要引进视觉系统[4-6]。

如今，实时视觉测距(VisualOdometry，VO)和视觉实时定位与地图构建(Visual Simultaneous Localization And Mapping，VSLAM)已成为热门的研究课题。第一个实时VSLAM出现是在2000年左右，其核心思想是选择一系列的关键点，跟踪它们在帧间的运动，联合推断它们的3D位置和相机运动。其后，出现了性能不断提升的解决方案，包括计算硬件和算法理念的升级。例如当前主流前沿算法ORB-SLAM 算法，其能够在小尺度和大尺度、室内和室外环境实时完成位姿确定的同时构建稀疏特征点地图，执行宽基线的闭环检测和重定位，并且能够实现全自动的初始化[7]。

传统的SLAM算法是基于尝试选择关键点的，而后出现的直接法旨在直接使用图像计算几何结构和运动，跳过中间的关键点选取步骤。例如大规模单目直接法LSD-SLAM，其核心思想是跟踪相机增量运动，同时执行位姿图优化以保持轨迹的全局一致性[8]。SVO属于半直接法，仅在前端的部分使用了直接法，之后的位姿估计等则仍旧使用传统的最小化重投影误差的方式[9]。DSO是少数使用纯直接法计算视觉里程计的系统之一，其无论是在精确度还是稳健性上都超过了基于特征方法的ORB-SLAM。但DSO等直接方法在没有光度校准的情况下性能会明显下降，单目DSO存在无法估计重建场景的尺度和相机运动的单位、估计的轨迹有较大的尺度漂移等问题[10]。

本文采用的双目DSO算法相较于单目DSO，通过引入额外的传感器信息优化上述缺点，提供了更精确并且更快收敛的尺度估计，对卷帘曝光或没有光度标定敏感度降低[14]。当前该算法多运用于陆地环境，本文将其运用于水面建图，并针对水上环境的特殊情况(如光强不断变化、存在较多反光等)对算法功能进行了优化，实现了更适用于水上环境的地图构建方法。

1 双目视觉测距

1.1 双目测距原理

如图1所示，设点P沿垂直于相机中心的直线上下移动，则其在左右相机上成像点的位置将随之不断改变，即视差的大小会不断改变。点P与相机间的距离Z和视差d之间存在反比关系，视差d可通过左右相机的中心距T减去点P在左右相机上的投影点到各自中心点的距离值获得。因此，只要获得左右相机的中心距T，即可估计出点P到相机的距离。

上文所述有个前提，就是在左右相机的成像上要定位到同一点P，即要把左右两个图像上的点进行匹配，这就需要双目校正的操作。假如根据左图像上一个点的特征到右图像上匹配对应点，该过程将非常耗时。因此利用极限约束来减少搜索匹配的运算量。如图2所示，空间中任意一点在图像平面上的投影点，必然处于由该点和两个相机中心组成的对极平面上。对于图像上的某一特征点，其在另一视图上的匹配点必处于对应的极线上，称为极线约束原理[11]。极线约束使得只需在一维空间中搜索匹配对应点，从而显著提升了运算效率并减少了误匹配。

图2 极线约束原理

此时用双目校正把左右两幅图像在水平方向上严格对齐，使得左右图像的对极线在同一水平线上。根据极线约束原理，此时一幅图像上任意特征点与其在另一图像上的匹配点必然在同一行上，因此只需要在该行搜索即可匹配到对应点。

1.2 双目摄像头参数标定

双目标定的第一步是分别获取左右相机的内外参数，然后通过立体标定对左右两幅图像进行立体校准和对齐，最后一步是确定两个相机之间的相对位置关系，即中心距。为了消除畸变以及得到内外参数矩阵，双目相机需要标定的参数有内参数矩阵、畸变系数矩阵、本征矩阵、基础矩阵、旋转矩阵和平移矩阵。内参数矩阵与焦距相关，内参数矩阵得到镜头的信息，并消除畸变，使得到的图像更为准确，它是一个从平面到像素的转换。为了最终的测距，通过外参数矩阵得到相机相对于世界坐标的联系，实现相机坐标系与世界坐标系的转换，畸变参数一般包含在内参数矩阵中[12]。

本文采用传统的棋盘作为标定板，规格为5×7方格，棋格尺寸间距为4.2cm×4.2cm，通过左右摄像机(间距8厘米)采集图像对。双目摄像机内部参数矩阵标定结果如表1所示。

表1 内部参数标定结果

右摄像机相对于左摄像机的平移外部参数矩阵标定结果如表2所示。

表2 外部参数标定结果

由以上两表中数据可知，摄像机的旋转矩阵接近于单位矩阵，所以摄像机的位置较为理想。

1.3 立体匹配与测距

在摄像机标定结束后，开始对拍摄的图像进行匹配。立体匹配算法计算基准图像中所有像素的视差值，得到视差图，从而得到基准图像的三维信息。由于水面情况复杂(如障碍物特征不明显，低纹理区较多，水面存在倒影等)，而局部立体匹配对噪声或特征要求较高，所以采用全局立体匹配算法。

根据视差图上的深度信息及摄像机内参即可计算任一像素点与摄像机间的距离。

2 双目直接稀疏里程计

2.1 双目DSO系统

直接稀疏里程计(Direct Sparse Odometry，DSO)是近年来发展起来的一种新方法，它利用非线性优化算法，通过最小化由直接图像对齐得到的光度误差来估计相机的位姿和像素深度[13]。本文运用的是双目立体扩展的DSO算法实现无人船所处水上环境的实时三维重建。

双目DSO系统概述如图3所示。系统使用固定的双目立体匹配获得的深度估计。基于直接图像对齐公式，首先新的双目帧会对参考关键帧进行跟踪，所得到的位姿估计用于调整和改进最近选定点的深度。然后系统检查当前活动窗口是否需要插入新的关键帧，如果不需要，则将创建一个普通帧，否则将生成一个新的关键帧并添加到当前的活动窗口。活动窗口中的所有关键帧联合优化位姿，调整亮度参数以及所有可观察到的三维点的深度。为了保持活动窗口为固定尺寸，使用Schur方法边缘化旧的关键帧和三维点[14]。

图3 系统概述

2.2 直接图像对齐

系统通过直接图像对齐获得光度误差，进而通过最小化光度误差来估计相机的位姿和像素深度。假设参考帧Ii中的点集Pi在另一帧Ij中被观察到，则直接图像对齐的基本思想可表示光度损失能量函数为：

(1)

式中，‖·‖γ是Huber范数，ωp权值会减弱梯度大的地方的影响，p′是p在Ij中的投影。

双目DSO每帧选取固定数量的点，均匀分布在所有区域并且梯度明显，然后在每个点周围的很小邻域内计算光度误差。由于光度误差是直接根据像素强度计算的，对帧间光照变化比较敏感。因此对每帧图像引入了两个参数ai和bi建模亮度变化，修改后的光度损失能量函数如下：

(2)

2.3 帧管理与窗口优化

每当有新的双目帧时，系统使用直接图像对齐跟踪它和活动窗口内的最新关键帧。活动窗口内所有的点都被投影到新帧，在深度值保持固定的情况下，通过最小化能量函数计算新帧的位姿[14]。

为了构造新的关键帧，需要从图像上选择一系列的稀疏点，称为候选点。当时间较长的点从活动窗口边缘化时，候选点会被激活并加入联合优化。每一个激活点存储在一个关键帧中并且可以被活动窗口内的其它一些关键帧观测。每当一个激活点被另外的关键帧观测到时，就会创造光度损失函数内部的光度能量因子，公式如下：

(3)

将所有的因素联合起来，在窗口优化中最小化的最终能量函数为：

(4)

式中，F是当前窗口的关键帧集，obs(p)是F中的可观测到p的关键帧。

为了保持活动窗口大小不变，时间较久的帧需要Schur边缘化。在关键帧边缘化之前，需先将两个最新的关键帧未观测到的全部激活点边缘化。

3 系统功能优化

3.1 三维点云过滤

水面建图的一个重要问题是水面反光的影响。河岸、船只或杂草的倒影会严重误导建图过程，造成“虚拟障碍”问题，在实验中会发现有些点出现在河流下方，如图4所示。为了消除河岸建筑在水面上的倒影的影响，系统在生成点云之前对点进行了过滤。由于相机只能观察水面，所以所有实际物体的点都应该在水面上。因此，任何具有负y坐标的点都可以确定为反射点。通过比较点云的坐标，可以估算出曲面的y坐标。然后，通过删除所有相关值较低的点来过滤点云[15]。实际实验表明该方法有效且不会影响表面障碍物的探测。

图4 水面倒影造成“虚拟障碍”

3.2 生成二维网格图

水面建图的另一个问题是，不同于无人机等运动范围相对较大的自动巡航设备，无人船仅在固定河道航行，因此可以假设无人船是在二维平面(x，z)上运动的，具有相同(x，z)坐标而y坐标不相同的点对于无人船的障碍意义相同。所以将三维点云图转换成二维网格图可以简化计算，并且不影响无人船实现避障功能[16]。

对于二维网格图而言，超出船体高度的点不仅会增加计算量，有时(尤其是穿越桥体时)会对建图以及标记障碍物产生误导。因此将高出船体一定距离的空间忽略，仅取船体前方10×10×2的空间，将该空间垂直分割成20×20个的方格，若方格内存在关键点，则标记此空间为存在障碍物，最终得到一个20×20的二维数组，存在障碍物的区域标记为1，不存在障碍物的区域标记为0，即为二维网格图。

4 实验结果与分析

4.1 实验结果

图5为无人船在河上行驶时某一时刻的左右摄像机拍摄的图像以及此时刻的关键帧选点图。

图5 左右摄像机拍摄图像(左图)和关键帧选点(右图)

以图中拍摄到的桥为例，此时无人船距离桥较远，为23 m，系统未完整构建桥的空间模型，如图6所示。

图7为无人船继续向前行驶后左右摄像机拍摄的图像以及此时刻的关键帧选点图，由图可知此时与桥的距离缩短，为8.5 m。

图7 左右摄像机拍摄图像(左图)和关键帧选点(右图)

如图8所示，此时系统基本构建完成桥的三维模型，并估计出了这段距离船的行驶轨迹。

图8 三维重建点云图(2)

与图8相对应，系统输出此时的二维网格地图，运用二维数组M存储二维网格地图的信息。其中每个元素表示该空间是否被占用，即在相机坐标系下的坐标为(x，z)其对应二维网格中的M(i，j)，示例输出如图9所示。

图9 二维网格地图(1)

二维地图中船体行进方向为由左至右，图中二维数组中1用红色标记，表示有障碍物。建图是动态的且只收集关键帧所有活动点的信息，而不是每次都计算整张地图，这意味着数组将在每一组关键帧插入之后输出和刷新，如图10即为图9下一组关键帧输出的二维地图。

由图9与图10可以看出，由于预先过滤了超出船体一定高度(本文设定为2 m)的点云，在生成的二维网格图中，对应桥洞位置的M(7,17)、M(8,17)、M(9,17)的值均为0，并未被标记为障碍物，因此二维地图在传输数据给避障系统后，避障系统会判定此处为可通行，证明了此方法的可行性。

图10 二维网格地图(2)

4.2 实验分析

本文系统对真实河流环境进行重建。通过优化改进算法，过滤点云，解决了水面反光造成的“虚拟障碍”问题，最终生成了准确、鲁棒的相关点云，使其适用于水上环境重建。其次根据三维点云图生成二维网格图，优化了避障所需的计算量，同时解决了二维建图所带来的“障碍误导”问题，即在压缩过程中将整个桥体压缩至平面导致避障系统判定此路不通的情况。最终该系统生成的二维地图以二维数组的形式传输给避障系统用来进行规避障碍物与路径规划。实验结果证明了本文系统的可行性以及稳定性。

5 结束语

本文采用双目DSO系统，主要用于无人船自动巡航。作为一种可视化的、实时的环境重建方法，其在深度的精确化、光线的鲁棒性等方面都具备了较好的性能。在此基础上，本文优化了系统功能，解决了水上环境构建存在的“虚拟障碍”等问题，并且优化了地图信息，由三维点云图生成二维网格图，简化了计算量，使其更适用于水上环境的地图构建，最终为避障系统提供数据信息。

在未来的研究中，可以将重点放在优化从图像中选择关键点的算法，通过光度校准来减少来自不同光照强度下的动态物体和预处理图像。