一种用于水下机器人的单目视觉定位方法与流程

本发明涉及视觉定位技术领域，具体为一种用于水下机器人的单目视觉定位方法，是一种在二维平面内进行单目视觉定位的简易方法，特别适用于无人水下航行器(autonomousunderwatervehicle，auv)等水下机器人的对接回收工作。

背景技术：

能够确定自身相对于特定目标的位置，是水下机器人成功执行作业任务的必要条件。但是，复杂的水下环境给机器人的精确定位带来很多难题。由于不能使用gps信号进行定位，高精度惯性导航系统融合水声定位系统是目前通常采用的水下作业定位方法。此类系统的成本很高，而且水声定位周期较长、近距离相对误差较大，不满足水下精确作业的要求。随着图像处理技术的日渐成熟，视觉定位方法越来越多地应用到水下机器人的作业任务中。与水声定位系统相比，视觉系统具有更新率高、测量精度高、成本低、系统可靠等优点，并且可以应用于非结构自然环境中。

从相机的数目来区分，视觉定位可分为单目定位与双目定位。双目定位可以直接获取目标物的深度信息，但是该定位算法比较复杂，运算周期长，而且水下环境的复杂性影响了图像特征提取与匹配的精度。因此，双目视觉定位并没有广泛地应用到水下视觉定位任务中。

很多研究者尝试改进单目视觉定位方法以获得深度信息。一般说来，常见的方法有两种：一种是几何方法，一种是扩展卡尔曼滤波(extendedkalmanfilter,ekf)算法。许多学者在理论上对ekf定位算法做了研究并改进。但是，该方法要求机器人的模型精确已知——在大多数情况下，这是很难实现的。并且，该方法的计算繁杂，不适用于对实时性要求较高的系统。

技术实现要素：

为解决现有技术存在的问题，本发明提出一种用于水下机器人的单目视觉定位方法，采用几何算法，既能够摆脱对系统模型的依赖，又能够极大地简化计算过程，而且该方法便于实施，能够用到水下机器人的实际定位任务中。

本发明实现过程中，需要两个标志物，标志物是机器人通过视觉将要识别的特征，在实验开始前应精确已知两标志物之间的距离，通过对两个标志物的识别和一定的几何运算，机器人能够获得本体相对于两个标志物连线中点的坐标。为了便于识别、减小定位误差，标志物应当与背景有足够的区分度，并且标志物的几何尺寸与两标志物的间距之比应当较小(例如小于1/20)。发明人所做实验中使用的标志物为杆状标记物，其中杆的外径为5cm，两杆间距为2.0m。两根杆分别为红色和绿色，与水体背景有足够的区分度。

水下机器人将通过搭载的相机采集图像，通过搭载的计算机进行图像特征提取及定位计算，并利用定位信息执行一定的操作。本发明采用基于几何方法的定位算法，为了取得较好的定位效果，相机应安装在机器人的中轴线上。发明人所做实验中使用的水下机器人为全驱动结构，最大航行速度1m/s。相机的分辨率为780×580，焦距5mm，最大帧率67fps(实验使用帧率5fps)。

方法实现中需要定义如图1所示的两个坐标系：全局坐标系e和载体坐标系b。本发明使用几何方法解决单目视觉定位问题。图1中点4代表所需识别的两个标志物。全局坐标系的原点oe选在两个标志物连线的中点处，载体坐标系的原点ob选在机器人的重心处。各坐标轴的正方向参见图1。ψ表示机器人在全局坐标系下的航向角，定义为xe轴与xb轴的夹角，正方向如图1所示。

图2表示该方法的几何原理图。以单目相机的位置来代表机器人当前所处的位置，以a点表示。表示相机的半视角大小(也即∠dac)，相机的视角值可以通过查阅数据手册或者相机标定等手段获得。ψ表示机器人在全局坐标系下的航向角，可以由机器人搭载的导航元件测出。m点位于ye轴，满足am⊥ye。d点表示相机视角的角平分线ad与ye轴的交点。p点及q点代表两个标志物，且p、q在全局坐标系下的坐标已知。p′q′所在直线表示成像平面，且有p′q′⊥ad。d′点、c′点、p′点、q′、o′点分别表示d点、c点、p点、q点、o点在成像平面上的投影。应注意，在图2表示的坐标系下：当ψ>0时，c点选在ye负半轴上，且q点与q′点重合；当ψ<0时，c点选在ye正半轴上，且p点与p′点重合。综上所述，基于几何方法的单目视觉定位问题可描述为：已知p点、q点在全局坐标系下的坐标，已知p′点、q′点在图像中的坐标，已知机器人当前的航向角ψ，求a点在全局坐标系下的坐标。

基本的计算思路，是通过已知的实际长度pq与图像中长度p′q′的对应关系，求算图像中其他线段对应的实际长度。具体来说，利用从实际长度pq到图像中长度p′q′的折算关系，我们能够计算出一个联系实际距离和图像距离的折算系数λ。进一步地，我们可以通过测量图像中其他线段的长度，推导出a点在全局坐标系下的坐标。

但经过实验证明，基于几何计算的定位方法有较大的定位误差(≥10％)。误差有一部分来源于算法的舍入，但是主要的误差来源是单目相机成像的非线性。也即，在真实空间中长度相等的两线段，在成像平面中对应的像未必相等。如上提出的单目视觉定位方法为了简化计算，只使用一个折算系数λ来联系实际长度和图像长度，而折算系数λ的计算只能依赖唯一已知的图像长度p′q′与真实长度pq的对应关系。由于单目相机成像的非线性特性，从pq到p′q′的折算系数并不总能代表从真实长度到图像长度的平均折算系数，而且往往会受其他因素的影响而偏大或偏小。这在很大程度上影响了解算的精度。同时，这也启发我们，可以通过补偿折算系数λ的方法来减小定位误差。具体来说，我们考虑λ^*＝mag×λ，其中mag是折算系数λ的放大倍数。通过分析，我们认为放大倍数mag是ψ与dd的函数。其中，ψ表示航向角，而dd表示在水平方向上，图像中点偏离标志物连线中点的像素l1与图像像素宽度一半l2的比值，如图3所示，dd＝l1/l2。

通过以下方法可以获得放大倍数mag的函数表达式。首先，在全局坐标系中任意选择一点作为机器人当前所处位置，并且指定航向角ψ的取值(应确保机器人在该点能够看到两个标志物)。可以选择若干数据点作为后续优化过程的样本点。第二步，计算在各点对应的λ^*值。通过之前的几何方法，计算出机器人当前所处的位置。不断修改mag的值，使得计算出的机器人的位置与真实位置之差最小，则该mag的值作为本组数据mag的最优值。实际在实验中编写了相关的matlab程序完成以上操作，并且选择机器人在全局坐标系下x、y坐标误差的平方和作为最优mag判别标准。第三步，使用matlab自带的cftool工具箱分析各点的ψ、dd与mag最优值的关系，选择拟合方法，得到放大倍数mag的函数表达式。在拟合过程中可以采用多项式插值，获得mag与dd的两次函数关系、与|ψ|的一次函数关系，拟合结果参见图4。

在解决定位问题的过程中，使用补偿后的折算系数λ^*而不是λ进行计算。如此一来，本方法在不增加原始解算过程复杂度的前提下，使用补偿的思路，有效地提高了定位结果的精度。

基于上述原理，本发明的技术方案为：

所述一种用于水下机器人的单目视觉定位方法，其特征在于：包括以下步骤：

步骤1：在水下环境中布置两个标志物，所述标志物能够被水下机器人搭载的相机拍摄，并能够被清晰识别；且以两个标志物连线的中点处作为全局坐标系的原点oe；以两个标志物连线作为全局坐标系ye轴，以水平面内垂直于ye轴的方向作为全局坐标系xe轴；

步骤2：通过以下过程计算放大倍数mag：

步骤2.1：在全局坐标系中任意选择一个全局坐标已知的点作为机器人当前所处位置a，并且指定航向角ψ的取值确保机器人搭载的相机在该点能够拍摄两个标志物；

步骤2.2：采集相机拍摄图像，并设置放大倍数mag初始值；

步骤2.3：计算从pq到p′q′的折算系数λ，以放大倍数mag修正后得到λ^*

其中p点及q点代表两个标志物，且p、q在全局坐标系下的坐标已知，p′点、q′点分别表示p点、q点在成像平面上的投影，p′点、q′点在图像中的坐标已知；

步骤2.4：根据公式

计算线段dc和ac的长度，其中c点为相机半视角边缘在全局坐标系ye轴上的点，d点表示相机视角的角平分线ad与ye轴的交点，表示相机的半视角大小，d′点、c′点分别表示d点、c点在成像平面上的投影；

步骤2.5：根据公式

计算线段am和cm的长度，其中m点位于ye轴上，满足am⊥ye；

步骤2.6：根据公式

om＝|cm-oc|

计算线段oc和om的长度，其中o′点表示o点在成像平面上的投影；

步骤2.7：根据公式

计算a点位置极坐标，其中如果ψ>0，并且oc<cm；或者ψ<0，并且oc>cm，则修正θ的值θ＝-θ；根据公式

x＝-oacosθ

y＝-oasinθ

计算a点位置在全局坐标系中的坐标；

步骤2.8：判断步骤2.7计算得到的a点位置坐标与步骤2.1设置的a点位置坐标之间的误差是否满足设定要求，若满足，则得到一组由mag、dd和ψ组成的数据点，并进行步骤3，若不满足，则修改放大倍数mag并返回步骤2.3；所述dd表示在水平方向上，图像中点偏离标志物连线中点的像素长度l1与图像像素宽度一半l2的比值；

步骤3：重新在全局坐标系中任意选择一个全局坐标已知的点作为机器人当前所处位置a，并且指定航向角ψ的取值确保机器人搭载的相机在该点能够拍摄两个标志物，重复步骤2，得到新的一组数据点；当得到设定组数的数据点后，进入步骤4；

步骤4：根据得到的若干组数数据点，拟合得到以mag因变量，dd以及ψ为自变量的函数关系；

步骤5：实际定位过程中，采集相机图像，得到机器人在全局坐标系下的航向角ψ以及当前状态下的dd，根据步骤4得到的拟合函数，得到当前状态下的放大倍数mag；

步骤6：根据步骤5采集的相机图像，利用步骤5得到的当前状态下的放大倍数mag，计算机器人实际坐标：

步骤6.1：计算从pq到p′q′的折算系数λ，以放大倍数mag修正后得到λ^*

其中p点及q点代表两个标志物，且p、q在全局坐标系下的坐标已知，p′点、q′点分别表示p点、q点在成像平面上的投影，p′点、q′点在图像中的坐标已知；

步骤6.2：根据公式

计算线段dc和ac的长度，其中c点为相机半视角边缘在全局坐标系ye轴上的点，d点表示相机视角的角平分线ad与ye轴的交点，表示相机的半视角大小，d′点、c′点分别表示d点、c点在成像平面上的投影；

步骤6.3：根据公式

计算线段am和cm的长度，其中m点位于ye轴上，满足am⊥ye；

步骤6.4：根据公式

om＝|cm-oc|

计算线段oc和om的长度，其中o′点表示o点在成像平面上的投影；

步骤6.5：根据公式

计算a点位置极坐标，其中如果ψ>0，并且oc<cm；或者ψ<0，并且oc>cm，则修正θ的值θ＝-θ；根据公式

x＝-oacosθ

y＝-oasinθ

计算机器人a点在全局坐标系中的坐标。

有益效果

本发明的有益效果是，设计了一套简便易行、精度较高的单目视觉定位方法。该方法应用范围较广，通过合理地布置两个标志物的位置，该方法能够应用于水下机器人的定点回收、目标跟踪、动力定位等任务中。所用方法在确保较高定位精度的情况下，极大降低了单目视觉定位方法的复杂度，提高了时效性，使得其可以应用到没有精确数学模型的系统中，或者对实时性要求较高的系统中，或者价格低廉、处理性能有限的硬件平台上。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为全局坐标系e和载体坐标系b的示意图。

图2为几何方法的原理图。

图3为图像中点与标志物连线中点在水平方向上偏离的示意图。

图4为放大倍数mag的拟合结果。

图中1.机器人，2.载体坐标系原点，3.单目相机，4.特征点，5.全局坐标系原点。

具体实施方式

下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

为解决现有技术存在的问题，本发明提出一种用于水下机器人的单目视觉定位方法，采用几何算法，既能够摆脱对系统模型的依赖，又能够极大地简化计算过程，而且该方法便于实施，能够用到水下机器人的实际定位任务中。

一、系统组成

本发明用于水下机器人的单目视觉定位系统主要由两个标志物和水下机器人两部分组成。标志物是机器人通过视觉将要识别的特征，在实验开始前应精确已知两标志物之间的距离，通过对两个标志物的识别和一定的几何运算，机器人能够获得本体相对于两个标志物连线中点的坐标。为了便于识别、减小定位误差，标志物应当与背景有足够的区分度，并且标志物的几何尺寸与两标志物的间距之比应当较小(例如小于1/20)。发明人所做实验中使用的标志物为杆状标记物，其中杆的外径为5cm，两杆间距为2.0m。两根杆分别为红色和绿色，与水体背景有足够的区分度。

水下机器人将通过搭载的相机采集图像，通过搭载的计算机进行图像特征提取及定位计算，并利用定位信息执行一定的操作。本发明采用基于几何方法的定位算法，为了取得较好的定位效果，相机应安装在机器人的中轴线上。发明人所做实验中使用的水下机器人为全驱动结构，最大航行速度1m/s。相机的分辨率为780×580，焦距5mm，最大帧率67fps(实验使用帧率5fps)。

二、基于几何方法的单目视觉定位模型

方法实现中需要定义如图1所示的两个坐标系：全局坐标系e和载体坐标系b。本发明使用几何方法解决单目视觉定位问题。图1中点4代表所需识别的两个标志物。全局坐标系的原点oe选在两个标志物连线的中点处，载体坐标系的原点ob选在机器人的重心处。各坐标轴的正方向参见图1。ψ表示机器人在全局坐标系下的航向角，定义为xe轴与xb轴的夹角，正方向如图1所示。

图2表示该方法的几何原理图。以单目相机的位置来代表机器人当前所处的位置，以a点表示。表示相机的半视角大小(也即∠dac)，相机的视角值可以通过查阅数据手册或者相机标定等手段获得。ψ表示机器人在全局坐标系下的航向角，可以由机器人搭载的导航元件测出。m点位于ye轴，满足am⊥ye。d点表示相机视角的角平分线ad与ye轴的交点。p点及q点代表两个标志物，且p、q在全局坐标系下的坐标已知。p′q′所在直线表示成像平面，且有p′q′⊥ad。d′点、c′点、p′点、q′、o′点分别表示d点、c点、p点、q点、o点在成像平面上的投影。应注意，在图2表示的坐标系下：当ψ>0时，c点选在ye负半轴上，且q点与q′点重合；当ψ<0时，c点选在ye正半轴上，且p点与p′点重合。综上所述，基于几何方法的单目视觉定位问题可描述为：已知p点、q点在全局坐标系下的坐标，已知p′点、q′点在图像中的坐标，已知机器人当前的航向角ψ，求a点在全局坐标系下的坐标。

基本的计算思路，是通过已知的实际长度pq与图像中长度p′q′的对应关系，求算图像中其他线段对应的实际长度。具体来说，利用从实际长度pq到图像中长度p′q′的折算关系，我们能够计算出一个联系实际距离和图像距离的折算系数λ。进一步地，我们可以通过测量图像中其他线段的长度，推导出a点在全局坐标系下的坐标。

三、误差补偿思路及方法

经过实验证明，基于几何计算的定位方法有较大的定位误差(≥10％)。误差有一部分来源于算法的舍入，但是主要的误差来源是单目相机成像的非线性。也即，在真实空间中长度相等的两线段，在成像平面中对应的像未必相等。如上提出的单目视觉定位方法为了简化计算，只使用一个折算系数λ来联系实际长度和图像长度，而折算系数λ的计算只能依赖唯一已知的图像长度p′q′与真实长度pq的对应关系。由于单目相机成像的非线性特性，从pq到p′q′的折算系数并不总能代表从真实长度到图像长度的平均折算系数，而且往往会受其他因素的影响而偏大或偏小。这在很大程度上影响了解算的精度。同时，这也启发我们，可以通过补偿折算系数λ的方法来减小定位误差。具体来说，我们考虑λ^*＝mag×λ，其中mag是折算系数λ的放大倍数。通过分析，我们认为放大倍数mag是ψ与dd的函数。其中，ψ表示航向角，而dd表示在水平方向上，图像中点偏离标志物连线中点的像素l1与图像像素宽度一半l2的比值，如图3所示，dd＝l1/l2。

通过以下方法可以获得放大倍数mag的函数表达式。首先，在全局坐标系中任意选择一点作为机器人当前所处位置，并且指定航向角ψ的取值(应确保机器人在该点能够看到两个标志物)。可以选择若干数据点作为后续优化过程的样本点。第二步，计算在各点对应的λ^*值。通过之前的几何方法，计算出机器人当前所处的位置。不断修改mag的值，使得计算出的机器人的位置与真实位置之差最小，则该mag的值作为本组数据mag的最优值。实际在实验中编写了相关的matlab程序完成以上操作，并且选择机器人在全局坐标系下x、y坐标误差的平方和作为最优mag判别标准。第三步，使用matlab自带的cftool工具箱分析各点的ψ、dd与mag最优值的关系，选择拟合方法，得到放大倍数mag的函数表达式。在拟合过程中可以采用多项式插值，获得mag与dd的两次函数关系、与|ψ|的一次函数关系，拟合结果参见图4。

在解决定位问题的过程中，使用补偿后的折算系数λ^*而不是λ进行计算。如此一来，本方法在不增加原始解算过程复杂度的前提下，使用补偿的思路，有效地提高了定位结果的精度。

本发明所述一种用于水下机器人的单目视觉定位方法，包括以下步骤：

步骤1：在水下环境中布置两个标志物，所述标志物能够被水下机器人搭载的相机拍摄，并能够被清晰识别；且以两个标志物连线的中点处作为全局坐标系的原点oe；以两个标志物连线作为全局坐标系ye轴，以水平面内垂直于ye轴的方向作为全局坐标系xe轴；

步骤2：通过以下过程计算放大倍数mag：

步骤2.1：在全局坐标系中任意选择一个全局坐标已知的点作为机器人当前所处位置a，并且指定航向角ψ的取值确保机器人搭载的相机在该点能够拍摄两个标志物；

步骤2.2：采集相机拍摄图像，并设置放大倍数mag初始值；

步骤2.3：计算从pq到p′q′的折算系数λ，以放大倍数mag修正后得到λ^*

其中p点及q点代表两个标志物，且p、q在全局坐标系下的坐标已知，p′点、q′点分别表示p点、q点在成像平面上的投影，p′点、q′点在图像中的坐标已知；

步骤2.4：根据公式

计算线段dc和ac的长度，其中c点为相机半视角边缘在全局坐标系ye轴上的点，d点表示相机视角的角平分线ad与ye轴的交点，表示相机的半视角大小，d′点、c′点分别表示d点、c点在成像平面上的投影；

步骤2.5：根据公式

计算线段am和cm的长度，其中m点位于ye轴上，满足am⊥ye；

步骤2.6：根据公式

om＝|cm-oc|

计算线段oc和om的长度，其中o′点表示o点在成像平面上的投影；

步骤2.7：根据公式

计算a点位置极坐标，其中如果ψ>0，并且oc<cm；或者ψ<0，并且oc>cm，则修正θ的值θ＝-θ；根据公式

x＝-oacosθ

y＝-oasinθ

计算a点位置在全局坐标系中的坐标；

步骤2.8：判断步骤2.7计算得到的a点位置坐标与步骤2.1设置的a点位置坐标之间的误差是否满足设定要求，若满足，则得到一组由mag、dd和ψ组成的数据点，并进行步骤3，若不满足，则修改放大倍数mag并返回步骤2.3；所述dd表示在水平方向上，图像中点偏离标志物连线中点的像素长度l1与图像像素宽度一半l2的比值；

步骤3：重新在全局坐标系中任意选择一个全局坐标已知的点作为机器人当前所处位置a，并且指定航向角ψ的取值确保机器人搭载的相机在该点能够拍摄两个标志物，重复步骤2，得到新的一组数据点；当得到设定组数的数据点后，进入步骤4；

步骤4：根据得到的若干组数数据点，拟合得到以mag因变量，dd以及ψ为自变量的函数关系；

步骤5：实际定位过程中，采集相机图像，得到机器人在全局坐标系下的航向角ψ以及当前状态下的dd，根据步骤4得到的拟合函数，得到当前状态下的放大倍数mag；

步骤6：根据步骤5采集的相机图像，利用步骤5得到的当前状态下的放大倍数mag，计算机器人实际坐标：

步骤6.1：计算从pq到p′q′的折算系数λ，以放大倍数mag修正后得到λ^*

其中p点及q点代表两个标志物，且p、q在全局坐标系下的坐标已知，p′点、q′点分别表示p点、q点在成像平面上的投影，p′点、q′点在图像中的坐标已知；

步骤6.2：根据公式

计算线段dc和ac的长度，其中c点为相机半视角边缘在全局坐标系ye轴上的点，d点表示相机视角的角平分线ad与ye轴的交点，表示相机的半视角大小，d′点、c′点分别表示d点、c点在成像平面上的投影；

步骤6.3：根据公式

计算线段am和cm的长度，其中m点位于ye轴上，满足am⊥ye；

步骤6.4：根据公式

om＝|cm-oc|

计算线段oc和om的长度，其中o′点表示o点在成像平面上的投影；

步骤6.5：根据公式

计算a点位置极坐标，其中如果ψ>0，并且oc<cm；或者ψ<0，并且oc>cm，则修正θ的值θ＝-θ；根据公式

x＝-oacosθ

y＝-oasinθ

计算机器人a点在全局坐标系中的坐标。

基于上述技术方案，下面给出两个实施实例：

坐标系定义、各参数定义及正方向定义都参照图1和图2。已知pq＝2.0m，相机的像素宽度为800pixel。依赖具体的实验条件，以上参数可以不同。

放大倍数mag的拟合函数为：

mag＝1.013+1.105×10^-2dd+1.175×10^-2|ψ|-7.832×10^-2dd²-2.426×10^-2dd|ψ|

【实施例1】

设相机当前采集的图像中，p′＝156pixel，q′＝315pixel，机器人上搭载的惯性器件测得当前时刻的航向角ψ＝5°。下面计算机器人在全局坐标系中的坐标。

第一步：dd＝0.411，根据拟合函数计算得到mag＝1.013；

第二步：计算得到λ^*＝80.55；

第三步：计算得到dc＝4.97，ac＝8.22；

第四步：计算得到am＝6.97，cm＝4.36；

第五步：计算得到oc＝2.92，om＝1.43；

第六步：计算得到oa＝7.12，θ＝11.61°；

第七步：此时，ψ>0，并且oc<cm，因此，θ＝-11.61°；

第八步：计算得到机器人在全局坐标系中的坐标为x＝-6.97，y＝1.43。

误差分析：本例中，机器人在全局坐标系中的真实坐标为(-7.0,1.5)，因此，相对定位误差为(0.42％,4.7％)。

【实施例2】

设相机当前采集的图像中，p′＝251pixel，q′＝462，机器人上搭载的惯性器件测得当前时刻的航向角ψ＝10°。下面计算机器人在全局坐标系中的坐标。

第一步：dd＝0.109，根据拟合函数计算得到mag＝1.104；

第二步：计算得到λ^*＝116.51；

第三步：计算得到dc＝3.43，ac＝5.62；

第四步：计算得到am＝5.01，cm＝2.55；

第五步：计算得到oc＝3.06，om＝0.51；

第六步：计算得到oa＝5.03，θ＝5.81°；

第七步：此时，ψ>0，并且oc>cm，因此不需对θ进行修正；

第八步：计算得到机器人在全局坐标系中的坐标为x＝-5.01，y＝-0.51。

误差分析：本例中，机器人在全局坐标系中的真实坐标为(-5.0,-0.5)，因此，相对定位误差为(0.2％,2％)。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。