基于双目视觉定位技术的无人船航迹跟踪控制室内模型实验

雷 洁，徐文华，许国冬，李明爵

（1.重庆前卫科技集团有限公司，重庆 401121；2.哈尔滨工程大学船舶工程学院，哈尔滨 150001）

1 引言

海洋的开发利用与货物运输一般依靠水面航行器。得益于传感器技术与无人技术的发展，船舶逐渐向智能化方向发展。通过集成运动控制系统自动跟踪预设航迹，实现无人驾驶是智能化的首要目标。室内的模型实验是研究船舶航迹跟踪控制系统的重要手段，但船舶的运动控制需要定位系统进行辅助。实船上通常使用的GPS 定位系统与水声定位系统在室内不能有效地实现定位与跟踪。因此，船舶航迹跟踪控制的室内模型实验需要采用合适的室内定位技术。

当前已有的室内定位技术多通过硬件设备实现，包括红外线定位技术、Wi-Fi 定位技术以及超声波定位技术等。这些定位技术由信号发射器和信号接收器配合实现[1-6]。提前将多个信号接收器布置在室内场景下，信号发射器安装在目标物上随物体运动并定时向外发射固定信号，在多个接收器接收到信号后根据多边测量原理计算目标物的实时坐标。由于布置多个信号接收器，造价以及功耗导致这些定位技术的成本较高。此外，信号容易发生遮挡、折射与衰减，信号传播极易受到场地环境的影响。

双目视觉定位技术是近年来比较热门的定位技术，已经被应用到多个领域，如火星探测器[7]、无人驾驶汽车[8]等，同时它在室内场景下有很强的适用性，比如餐厅的服务机器人可以通过双目视觉技术，将餐品准确送到目标桌号[9]。双目视觉定位技术是一种基于三角测距原理的定位技术，通过一组固定且各项参数信息已知的摄像机拍摄物体图像，配合电脑程序实现目标的定位[10]。相较于常规的室内定位技术，双目视觉定位技术成本较低，定位精度高。双目视觉技术的处理对象是摄像机拍摄的图像，信息保留较好，位置信息反馈快，定位结果的可信度高。本文采用双目视觉定位技术，应用到船模室内航迹跟踪控制实验中。实验表明，该方法实用可靠、稳定性好。

2 双目视觉定位技术原理与室内船模定位系统

2.1 双目视觉技术原理

一般情况下，双目视觉定位系统使用平行双目模式，两台摄像机的光轴平行，成像平面共面并且水平高度一致，光心之间的连线平行于成像平面[11]，具体模型如图1所示。图中，lO和rO是左右摄像机的光心，Z l和Zr轴是两个摄像机的光轴，A和B是两个空间点，点A距离成像平面较远，它们在两个成像平面上的投影是lA和rA以及lB和rB。由图可知，空间点在左右成像平面上的投影点的横坐标存在偏差，在双目视觉系统中，这个偏差叫做视差，通常用d表示[11-14]。可以发现，点A的视差AlAr比点B的视差Bl Br要大。

图1 双目视觉模型Fig.1 Binocular vision model

为了方便推导空间点坐标与视差之间的具体数学关系，现给出了双目视觉模型的俯视图[15]，如图2所示。其中，和是左右摄像机的光心，线段叫做双目视觉系统的基线，长度为B。和代表两个成像平面，和表示两个主点，f是焦距。点P是空间中的一个任意点，与光心平面的距离是Z，在左右成像平面上的投影分别为和，d是点P的视差。很显然，在图2 中，三角形相似于三角形，所以有：

图2 双目视觉模型俯视图Fig.2 Top view of binocular vision model

上式可简化为：

根据方程（2）可知，如果已知摄像机的焦距f、基线B以及空间点的视差d，则可以计算出该点的空间坐标信息。

2.2 双目视觉摄像机图像标定与畸变校正

由双目视觉定位的原理可知，空间点的坐标计算需要知道摄像机的焦距、基线等参数，摄像机的出厂铭牌中一般不完全包含。此外，由于生产装配过程中的误差，摄像机拍摄的图像存在一定的畸变，而实际的双目视觉模型通常如图3所示。和是左右摄像机的光心，和为点P在不同摄像机成像平面上的位置。通过摄像机标定可以得到摄像机的内、外参数，以此可将实际模型校正为双目平行模式，并得到焦距f和基线B。同时，摄像机标定还能得到畸变参数，对图像畸变进行校正。

图3 双目视觉实际模型Fig.3 Practical model of binocular vision

本文基于张正友标定法[16]对摄像机进行标定。在标定过程中，双目摄像机保持不动，拍摄不同位置下的棋盘格标定板图像，为了保证标定结果精确，采集了70 组共140 张图像。根据标定结果对左右图像位置和畸变进行校正，结果如图4所示。

图4 摄像机标定结果Fig.4 Camera calibration result

2.3 室内船模运动定位系统搭建

实验在室内的水池内进行，主要包含船模、计算机以及双目摄像头等实验设备，各设备的布置如图5所示。

图5 实验示意图Fig.5 Schematic diagram of experiment

实验选用的摄像机为USB 免驱双目摄像机，如图6所示。摄像机通过一条USB3.0 接口的数据线与控制电脑主机相连实现图像传输，帧率最高可达60 fps，单个摄像头的分辨率为1280×720像素。双目摄像机通过钢材架设在水池上方，成像平面与水面平行，在经过图像处理后可得出特征点的三维坐标信息，成像平面与水面平行可以保证相机有最大的视野范围。由于场地限制，摄像机与水面距离为2 m，摄像机视野中的水池面积约为2.4 m×1.8 m。

图6 双目摄像机Fig.6 Binocular camera

船模定位系统采用无线控制模式。由Arduino 2560 开发板、HC-05 蓝牙模块、直流电机、L298N电机驱动器、舵机、干电池以及船模实现。HC-05蓝牙模块包含两块，分别连接在电脑主机和Arduino 2560 开发板上，实现信号的无线传输；Arduino 2560 开发板根据HC-05 蓝牙模块接收的信号控制船模的运动，一方面直接控制舵机角度，另一方面采用PWM 技术通过L298N 电机驱动器控制直流电机与船模螺旋桨转速，进而控制船模速度；船模中所有硬件设备均由干电池供电。

室内定位系统基于Qt Creator 开发船模运动控制程序。采用视线导向法（Line of Sight Guidance，LOS）[17-19]作为船模的航迹跟踪算法，采用PID 控制器[20-21]对船模的推进系统进行控制，采用帧差法[22]根据船模在前后两帧上的位置与艏向角计算纵向速度、横向速度和艏向角速度，依据多帧的位置和艏向角计算纵向加速度、横向加速度和艏向角加速度。程序中关于双目视觉定位的代码均基于OpenCV 2.4 计算机视觉库，程序界面及工作模块如图7所示。

图7 船模运动控制程序Fig.7 Ship model motion control program

2.4 图像预处理与定位

确定空间点三维坐标的前提是准确找到该点在左右图像上的投影坐标，获得视差，这在双目视觉技术中叫做左右图像匹配。视觉定位部分的流程图如图8所示。实验中使用船模重心和纵剖面上一点即可反映船模的位置和朝向。为准确提取上述两点的投影坐标，实现左右图像的快速匹配，根据实验水池背景为蓝色这一特征，在船模的重心以及纵剖面上粘贴大小不同的白色剪纸，作为图像匹配的特征物，如图9所示。

图8 视觉定位部分流程图Fig.8 Flow chart of visual positioning

图9 船模特征物布置示意图Fig.9 Layout of ship model features

特征物仅占据了摄像机采集图像的极小部分，其余图像均为干扰。为快速从图中提取特征物，提前对图像进行了预处理，将图像中的干扰区域完全剔除掉。由于特征物是依据颜色选择的，图像预处理的主要方法和步骤均围绕颜色特征设计，具体的处理过程为：（1）图像灰度化；（2）图像滤波去噪；（3）图像二值化；（4）形态学处理。具体处理结果如图10所示，从图10（a）与图10（e）可以看出，图中的干扰区域已经完全剔除，设置在船模上的特征物被准确地保留下来。

图10 图像预处理Fig.10 Image preprocessing

以两个特征物区域的面积大小作为判别依据，可区分出不同的点，并实现左右图像的匹配。设正方形区域形心点为P1，三角形区域形心点为P2，程序的匹配结果如图11所示。从最终的匹配结果来看，程序成功提取了船模特征物，并根据船模特征物大小对其进行识别，实现了左右船模图像中同一像素点之间的匹配。在本系统中，方形中心P1 为船体重心位置，视为船模位置坐标；三角形中心P2 为船艏位置，通过P1、P2 点坐标，即可算出艏向角，从而得到船舶模型相应的位置与艏向角信息。

图11 左右图像匹配结果Fig.11 Image matching results

3 室内船模航迹跟踪控制实验

3.1 双目视觉定位验证与误差分析

在开展船模运动与定位之前，测试所设计的双目视觉定位系统的精度。

双目视觉定位的基础原理是提取特征物的形心坐标，通过计算得出目标的相关姿态信息。上节中所使用的三角形及方形特征物，本质上是为了依靠不同形状物体的特征面积区分不同点的坐标信息，得到模型的位置与艏向。因此，为了测试定位精度，本节使用圆形（形心即圆心）进行精度的测量。

使用3D 打印技术制作两个大小不同的白色圆形薄片，较小圆的圆心固定在直尺0 刻度位置，较大圆的圆心固定在600mm 刻度位置，即两圆心之间的距离为600mm，如图12所示。为保证误差分析结果有效可靠，将带有两圆的标尺摆放在摄像头视线范围内的不同位置进行测量，测得两个圆心的平面坐标并计算圆心之间的距离，对比分析定位系统的视觉图像测量精度。误差分析以及后续的定位工作中需要根据坐标系统确定关键点的具体坐标值。在本实验中，以图像的正中心为坐标原点，水平向右为X轴正向，竖直向上为Y轴正向。此外，在后续计算船模的艏向角时，以Y轴正向为0°，顺时针递增至360°。

图12 测量物体Fig.12 Measuring object

摄像机捕捉图像与圆心提取结果如图13所示，具体测得的圆心平面坐标和误差值见表1。由表1 可知，当前设计的双目视觉定位系统测量的长度最小误差为0.68%，最大误差为2.29%。

表1 圆心空间坐标与误差值Table 1 Spatial coordinates and error values of circle center

图13 不同位置的拍摄图像及其圆心提取结果（左侧图像为摄像机拍摄原图，右侧图像为实现图像匹配后的图像）Fig.13 Captured images at different positions and their circle center extraction results(The left image is the original image taken by the camera,and the right image is the image after image matching)

实际测量数据表明，双目视觉获得的位置精度高，完全可以满足室内实验的精度要求。分析发现标尺在图像中心区域的误差最小，在视线边缘区域的误差较大。

3.2 船模定艏向控制实验

对船模的航速与艏向角速度不做限制，进行船模的定艏向控制实验。实验中图像竖直向上方向为0°方向，角度顺时针增加。船模的航行轨迹如图14所示。为保证足够的运动时长，初始时刻船模均静置在靠近摄像机视野边界的位置。图14（a）为目标艏向角为60°的船模运动轨迹图，初始时刻船模的艏向角为90°；图14（b）为目标艏向角为270°的船模运动轨迹图，初始时刻船模的艏向角为0°。从船模的实际航行轨迹来看，船艏最终都转向了设计航向，达到了控制目标。

图14 定艏向控制实验Fig.14 Fixed heading control experiment

3.3 船模航迹跟踪控制实验

航迹跟踪控制主要使用PID 控制器结合LOS算法对船模的航向角进行控制，对船模的航行速度不做要求。航迹的设计主要通过设置航迹点坐标实现。设置若干数量的航迹点，再使用直线将航迹点依次连接即可得到设计航迹。本文设计了航迹点数分别为3 点和6 点的两种航迹，控制船模对其进行跟踪，具体的实验结果如图15所示。图中黑色虚线为设计航迹，黑色实线为船模的实际航迹，红色虚线为路径点的切换圆。切换圆以航迹点为圆心，一定数值为半径，当船模跟踪航迹点行驶至切换圆内，船模自动按顺序跟踪下一个航迹点。在本实验中，3 航迹点跟踪实验的切换圆半径为0.2 m，6 航迹点跟踪实验的切换圆半径为0.1 m。

图15 航迹跟踪控制实验Fig.15 Track tracking control experiment

从图15 可以看出，船模实现了对预设航迹的稳定跟踪，证明本文设计的双目视觉定位系统在响应速度和定位精度上都能够满足室内船舶航迹跟踪控制系统设计的要求。

4 结论

室内模型实验是研究船舶航迹跟踪控制系统的重要手段，针对实船常用定位技术无法在室内环境下使用的难点，本文提出将双目视觉技术作为定位工具，向控制系统实时反馈船模的位置信息，辅助船舶航迹跟踪控制的室内模型研究。从船模的位置跟踪与航迹控制实验结果看，设计的控制系统对船模的控制效果达到了预期目标，双目视觉定位系统在船模航迹跟踪控制的室内模型研究中可行、可靠。

在实验过程中，图像的预处理方法及流程仅考虑了颜色特征，处理结果受环境影响较大，进一步研究还可以考虑在不预设目标颜色和形状等特征的室外更加复杂的情况，以满足更多应用。当前采用双目摄像机分辨率较低，采用高分辨率相机可提供更高精度的位置信息。

5 展望

本文所使用的双目视觉定位技术，其基本原理是通过提取特征图形的三维坐标信息，计算并得出相关的姿态信息，以进行后续操作。实验水池背景色较为单一，因此该方法可以用于水池实验环境中。目前，双目视觉技术还处于发展阶段，通过规则标记物定位所得出的坐标精度更高。对于复杂环境，该双目视觉定位技术在今后的发展中可进行相应的改进，如可通过与深度学习结合，使用卷积神经网络与图像处理相结合的方式进行定位，便可不需通过特征物直接将被测物体识别出来，实现对无明显标记点的无人船进行姿态的提取，这将大大提高该双目视觉定位技术的应用范围。