CN108981672A - 基于单目机器人与测距传感器结合的舱门实时定位方法 - Google Patents

Abstract

本发明公开了一种基于单目机器人与测距传感器结合的舱门实时定位方法，包括步骤：1)利用张正友标定算法标定摄像机的内参、畸变参数；2)建立起基于视觉深度的三维定位模型；3)视频图像实时拍摄；4)分割目标区域；5)判断视野内的舱门情况，以使其目标区域位于视野内获得ROI；6)根据3)所得结果，利用几何特征精确定位得目标点图像坐标；7)结合测距传感器的深度信息，得到舱门目标点的世界坐标；8)根据所得世界坐标，控制机器人行为。通过本发明可以提供一套方便、简单的机器人实时定位舱门的方法，该方法可应用于飞机舱门、轮船舱门等类似场景，追求舱门实时定位的准确性与智能化的同时，也保证方案的可行性且简单、易实现。

Description

基于单目机器人与测距传感器结合的舱门实时定位方法

技术领域

本发明涉及图像模式识别领域及智能机器人技术领域，尤其是指一种基于单目机器人与测距传感器结合的舱门实时定位方法。

背景技术

机器人技术经过四十多年的发展，已经成为了未来新型制造业的核心装备。如今机器人关键技术日臻成熟，在各个领域广泛应用，发展方向具有种类繁多、形式多样、功能各异等特点。不管是在服务机器人还是工业机器人的关键技术中，机器人的定位、导航和控制技术一直是一大研究热点。随着机器人的应用场景越来越广泛，同时对其智能化的要求越来越高，在工业机器人的舱门装配和服务机器人的室外应用中，舱门的实时检测与定位的问题在智能机器人的技术应用研究中也愈发有价值。因此，一套方便、简单的机器人实时检测与定位舱门的方法也愈发有意义。

本发明中采用单目视觉与激光测距传感器结合的视觉定位方法，在目前机器视觉的技术领域中，双目视觉和多目视觉一直存在立体匹配的障碍，其对于摄像机的安装也有特定要求；而单目视觉如果仅依靠一个摄像头是无法完成物体的三维定位的，必须借助其他外部已知信息。所以借助测距传感器增加辅助信息后的单目视觉定位方法，既简单补充了单目视觉的三维定位深度信息，又避免了双目视觉中立体匹配困难的问题，从而最终提高舱门实时定位的精度和可行性，达到机器人目标行为的准确性与智能化。

发明内容

本发明的目的在于针对机器人应用中的需求，提供一套方便、简单的机器人实时检测与定位舱门的方法，该方法可应用于飞机舱门、轮船舱门等类似场景，追求舱门实时定位的准确性与智能化的同时，也保证方案的可行性且简单、易实现。

为了实现上述目的，本发明所提供的技术方案为：一种基于单目机器人与测距传感器结合的舱门实时定位方法，主要通过结合激光测距传感器的单目视觉系统来实现机器人对舱门的实时定位，以达到机器人的行为目的。主要包括以下步骤：

1)利用张正友标定算法标定摄像机的内参、畸变参数；

2)利用已知信息建立起基于视觉深度的三维定位模型；

3)视频图像实时拍摄；

4)利用传统图像处理算法分割目标区域；

5)判断视野内的舱门情况，以使其目标区域位于视野内获得ROI；

6)根据3)所得结果，利用几何特征精确定位得目标点图像坐标；

7)结合测距传感器的深度信息，得到舱门目标点的世界坐标；

8)根据所得世界坐标，控制机器人行为。

在步骤1)中，采用张正友标定算法来获得摄像头的内参、畸变参数，仅需使用一个打印出来的棋盘格就可以，此方法克服了传统标定法需要的高精度标定物的缺点，同时相对于自标定而言，提高了精度且便于操作。摄像机标定的目标是建立图像坐标系和世界坐标系之间的关系，即求解摄像机的投影矩阵以及5个畸变参数，建立视觉系统几何其中投影矩阵包括了摄像机的5个内参和6 个外参。摄像机内参描述的是图像坐标系与摄像机坐标系之间的关系，只与摄像机本身有关，而摄像机外参描述的是摄像机坐标系和世界坐标系之间的关系，与摄像机的安装、世界坐标系的选取有关。主要步骤如下：

1.1)棋盘格标定板的准备

用A4纸张打印一张棋盘格(单元格宽度已知)贴在平板上，用摄像机在两个以上不同的位置对棋盘格进行多角度拍摄(一般10-20张)。

1.2)计算摄像机内参

根据摄像机模型，设三维世界坐标的点X＝[X,Y,Z,1]^T，二维图像像素坐标为 m＝[u,v,1]^T，则标定棋盘格平面到图像平面的单应性关系为：

s₀m＝K[R,T]X

其中s为世界坐标系到图像坐标系的尺度因子，K为摄像机内参，R为旋转矩阵，T为平移向量。令

其中α，β为焦距与像素横纵比的融合，γ为径向畸变参数。注意，s对于齐次坐标来说，不会改变齐次坐标值。张正友标定法中，将世界坐标系设在棋盘格平面上，棋盘格平面设为Z＝0的平面。则可得

单应性矩阵H＝K[r₁,r₂,t]，即

H＝[h₁ h₂ h₃]＝λK[r₁ r₂ t]

H是一个齐次矩阵，有8个未知数，至少需要8个方程，所以通过4个以上棋盘格对应角点的标定板图像可得到单应性矩阵H。由上式可得

由旋转矩阵r₁和r₂正交，可得

||r₁||＝||r₂||＝1

带入可得：

即每个单应性矩阵能提供两个方程，而内参矩阵包含5个参数，要求解至少需要3个单应性矩阵。为了得到3个不同的单应性矩阵，至少要使用3幅棋盘格平面的图片进行标定。通过改变摄像机和标定板之间的相对位置来得到3 张不同的图片。为了方便计算可定义如下：

B是一个对称阵，所以B的有效元素为6个，把这6个元素写成向量b，即

b＝[B₁₁ B₁₂ B₂₂ B₁₃ B₂₃ B₃₃]^T

推导可得

ν_ij＝[h_i1h_j1 h_i1h_j2+h_i2h_j1 h_i2h_j2 h_i3h_j1+h_i1h_j3 h_i3h_j2+h_i2h_j3 h_i3h_j3」

利用约束条件可得到：

通过上式可知至少需要3幅棋盘格平板的图像，才能计算得B，然后通过cholesky分解，得到摄像机的内参矩阵K。

1.3)最大似然估计

上述的推导结果是基于理想情况下的解，但由于可能存在高斯噪声，所以需要使用最大似然估计进行优化。设采集了n副包含棋盘格的图像进行标定，每副图像里有棋盘格角点m个。令第i副图像上的角点M_j在上述计算得到的摄像机矩阵下图像上的投影点为：

其中R_i和t_i是第i副图对应的旋转矩阵和平移向量，K是内参矩阵。则角点 m_ij的概率密度函数为：

构造似然函数：

让L取得最大值，即让该式最小。这里使用的是多参数非线性系统优化问题的Levenberg-Marquardt算法进行迭代求最优解。

1.4)径向畸变估计

张正友标定法只关注了影响最大的径向畸变。其数学表达式为：

其中，(u,v)是理想无畸变的像素坐标，是实际畸变后的像素坐标。(u₀,v₀)代表主点，(x,y)是理想无畸变的连续图像坐标，是实际畸变后的连续图像坐标。k₁和k₂为前两阶的畸变参数。

化成矩阵形式：

记为：Dk＝d，则可得：k＝[k₁ k₂]^T＝(D^TD)^-1D^Td，计算得畸变参数k。使用最大似然的思想优化结果，即同1.3)，LM法计算下列函数值最小的参数值：

在步骤2)中三维定位模型的建立需要凭借已知三维世界坐标的棋盘格标定板标定得到该位置的外参，建立起指定世界坐标系下的定位模型，在之后的实时定位中，结合激光测距传感器的距离深度信息，就可获得指定世界坐标系下的三维坐标。包括以下几步：

2.1)标定已知位置下的外参

首先，外参由旋转矩阵与平移向量构成，即[R|t]＝[r₁ r₂ r₃ t]。由1)中的推导可得：

则r₃＝r₁×r₂，t＝λK^-1h₃可得该位置下的一组外参 [R₀|t₀]。

由于外参描述的是摄像机、世界坐标系之间的位置关系，本发明中摄像机的安装是固定不变的，且最终机器人的行为目标皆可以机器人为参考下的舱门的相对坐标为本方案中的世界坐标系，故而把世界坐标系选定在机器人上，即摄像机与世界坐标系是相对静止的，也即它们之间的位置关系是固定不变的。

基于上述前提，我们定义之后用到的几个坐标系：

·坐标系{C}：摄像机坐标系，原点与光心重合，Z轴为摄像机的光轴；

·坐标系{S}：机器人上的已知位置三维坐标系，该坐标系需与外参标定板的位置关系已知；

·坐标系{W}：机器人上的最终世界坐标系，与坐标系{S}的位置关系已知；

·坐标系{D}：引入一个与坐标系{C}原点重合，即该坐标系与坐标系{C} 之间的位置关系只有旋转无平移；且与摄像机坐标系{S}的坐标轴方向一致，即该坐标系与坐标系{S}之间的位置关系只有平移无旋转。通过该坐标系来计算目标点的三维坐标。

2.2)坐标换算模型

坐标的整体模型如下：

根据上述张正友标定法得到已知位置P₀下的一组外参[R₀|t₀]。首先，从坐标系的定义可知，结合该外参标定板和坐标系{S}的已知关系，可获得坐标系{C} 下坐标系{S}位置关系的旋转矩阵和平移矩阵

此时，可以计算得到摄像机光心相对于坐标系{S}原点的Z方向上的距离由于坐标系{D}相对于坐标系{C}原点重合，即只有旋转无平移；坐标系{D}相对于坐标系{S}只是原点偏移，即只有平移无旋转，所以也可得到坐标系{D}跟坐标系{S}的关系。

可以通过坐标系位置关系的传递可得坐标系{D}与坐标系{C}的关系，如下：

最后就可得到归一化的坐标从坐标系{C}转换到坐标系{D}，

即可计算出对应点的在{D}坐标系下的归一化坐标P_D＝[X_D,Y_D,Z_D]^T。

在步骤3)中，采用摄像头进行图像的实时拍摄，该摄像头安装在机器人部件上，与舱门底边相近高度较好，且其拍摄方向尽量与舱门平面垂直，以保证摄像头的视场覆盖整个舱门目标区域且镜头畸变较小。

在步骤4)中利用传统图像处理算法进行步骤3)中单帧图像的目标区域分割，基于舱门矩形边宽的几何特征进行舱门的提取与定位，总体思路为先根据横向相邻像素灰度值差的统计特征进行舱门两侧边的提取，得到目标附近点，从而进行舱门图像的ROI(感兴趣区域、region of interest)矩形区域定位、分割。主要步骤如下：

4.1)分割舱门的侧边与底边

首先机器人的摄像机安装固定，且由于摄像机视场的限制本方案的应用场景为在距离舱门一定范围内拍摄。根据所得图像的像素灰度值进行统计分析，观察得舱门边框的像素分布规律，如针对具有明显标志线的舱门边框，具有同行(列)相邻前一像素灰度值减后一像素的灰度值的灰度差，在舱门边框附近呈类正弦函数的正负峰值分布。故而分割算法可分别计算灰度图的横向、纵向相邻像素灰度差结果，根据舱门侧边、底边的梯度宽度特征介于固定范围内，进行灰度差结果图的阈值二值化运算，可分别得舱门两侧边和底边的提取结果二值图，也即可分别初步提取出舱门的两侧边、底边。

一般为了更好的检测效果，也需进行一些图像预处理(视图像采集环境而定)。为了解决高光现象问题，国内外文献提出了许多的解决方法，代表性的有以直方图均衡化为代表的灰度变换法、基于照明-反射的同态滤波发、Retinex增强方法以及梯度域图像增强方法、Poisson编辑和颜色偏移率自动校正法等。其中RGB空间中个分量颜色偏移率自动校正法计算效率较高，且对高光区域的校正效果也较好，能够校正图像中的高光区域，本发明采用该方法对舱门图像的高光区域的校正处理。使用舱门图像的像素与空间分布特征，检测正常光照区域，然后利用各个分量的颜色偏移率，自动校正图像中的高光区域。图像增强的目的是提高对比度，突出有助于识别的特征，抑制不感兴趣的特征，使之改善图像质量、丰富信息量，加强图像判读和识别效果。可采用直方图均衡算法、同态滤波等常规的图像增强方法。同时考虑到舱门目标的形状特征较显著，舱门的形状中线状结构所占比例较高，故本发明采用Frangi滤波器增强线状目标。

4.2)分割ROI

在侧边、底边提取结果的基础上进行中值滤波，并根据Seed-filling种子填充法进行连通域分析。一个连通区域是由具有相同像素值的相邻像素组成像素集合，因此，我们就可以通过这两个条件在图像中寻找连通区域，对于找到的每个连通区域，我们赋予其一个唯一的标识label，以区别其他连通区域。主要思路为，选取一个前景像素点作为种子，然后根据连通区域的两个基本条件(像素值相同、位置相邻)，将与种子相邻的前景像素合并到同一个像素集合中，最后得到的该像素集合则为一个连通区域。步骤如下：

①按行或按列访问扫描图像，直到当前像素点B(x,y)＝＝1：

a.将B(x,y)作为种子(像素位置)，并赋予其一个label，然后将该种子相邻的所有前景像素都压入栈中；

b.弹出栈顶像素，赋予其相同的label，然后再将与该栈顶像素相邻的所有前景像素都压入栈中；

c.重复b步骤，直到栈为空；

此时，便找到了图像B中的一个连通区域，该区域内的像素值被标记为 label；

②重复第①步，直到扫描结束；

扫描结束后，就可以得到图像B中所有的连通区域；

基于连通域分析后的结果图，除去一些太窄/太宽的连通域后，并分别找两条高度最高的连通域connected-D1,connected-D2，以及一条宽度最宽的连通域 connected-D3，即为舱门两侧边、底边的提取结果，获得这三条连通域的两端点、高、宽数据，并返回两侧边连通域connected-D1,connected-D2的最底点P_{connected-D1.b}、 P_{connected-D2.b}，以及底边连通域connected-D3的两端点P_{connected-D3.t}、P_{connected-D3.b}，这四点即为目标附近点；后续基于该步骤的检测结果判断舱门各边(左侧边、右侧边、底边)的检测情况，并划分舱门图像ROI(region of interest)。

在步骤5)中根据步骤4)中所得的四个点P_{connected-D1.b}、P_{connected-D2.b}、P_{connected-D3.t}、P_{connected-D3.b}判断视野内的舱门情况。由于舱门在机器人摄像机的视野范围内的不确定性，且有可能只拍摄到舱门的部分区域，故而可分为以下7中情况(此处假设舱门目标区域为下半部分分析)。

5.1)情况分类

①情况1：舱门的整个下半部分均不在视野范围内。即满足要求的三条连通域在步骤4)中都未被检测到，返回结果以使机器人随机移动，并进行下一帧的图像处理；

②情况2：只拍摄到舱门的左/右半侧边。即在步骤4)中只检测到侧边连通域connected-D1/connected-D2中的一条，根据P_{connected-D1.b}、P_{connected-D2.b}可判断其为左 /右侧边，进而判断此时机器人是太靠近左/右边界，此时将所得P_{connected-D1.b}或者 P_{connected-D2.b}存入Kalman跟踪器的对应点集中，并返回结果以使机器人向右/左，同时进行下一帧的处理；

③情况3：只拍摄到舱门的底边。即在步骤4)中只检测到底边的一条连通域connected-D3，进而判断此时机器人是太靠近下边界，返回结果以使机器人向上移动，并进行下一帧的处理；

④情况4：只拍摄到了舱门的两条侧边。即在步骤4)中只检测到侧边的两条连通域connected-D1、connected-D2，但未检测到底边连通域connected-D3，进而判断此时机器人位置太靠上，此时将P_{connected-D1.b}、P_{connected-D2.b}存入Kalman跟踪器的对应点集中，并返回结果以使机器人向下移动，同时进行下一帧的处理；

⑤情况5：只拍摄到了舱门的左/右半部分。即在步骤4)中只检测到侧边连通域connected-D1/connected-D2中的一条，以及底边连通域connected-D3，类似②情况2首先根据P_{connected-D1.b}、P_{connected-D2.b}可判断检测到的侧边为左/右侧边，进而判断此时机器人是太靠近左/右边界，此时将所得P_{connected-D1.b}或者P_{connected-D2.b}存入 Kalman跟踪器的对应点集中，并返回结果以使机器人向右/左，同时进行下一帧的处理；

⑥情况6：舱门的整个下半部分均在视野范围内。即在步骤4)中满足要求的三条连通域都被检测到了，那么先将所得P_{connected-D1.b}或者P_{connected-D2.b}存入Kalman 跟踪器的对应点集中，并根据P_{connected-D3.t}、P_{connected-D3.b}判断机器人是否太靠近舱门底边，若机器人太靠近舱门底边，则返回结果意思机器人向下移动；否则即为三边初步检测结果符合后续定位要求，接下来可进行目标点的精确定位，即可进行步骤7)利用舱门侧边、底边的拟合结果，获得两个目标点的图像坐标。

此时根据P_{connected-D1.b}、P_{connected-D2.b}两点向外画矩形，分割图像区域的ROI；

⑦其他情况：其他异常情况。返回结果并进行下一帧的处理；

基于以上7种情况分别进行相应的机器人控制，以使得最终舱门的目标区域位于摄像机的视野范围内。

5.2)Kalman跟踪器

Kalman跟踪器是利用Kalman滤波器进行视频目标点的跟踪，是一种拥有状态估计功能的滤波器，通过时间更新与状态更新的五个核心方程，预测下一时刻的状态，从而完成对目标点的跟踪。

设以下动态系统是线性的：

对中心坐标P_p＝(x_p,y_p)，x_p和y_p分别是它的横纵坐标， x(k)＝[x_p y_p Δx_p Δy_p]^T为系统在k时刻的状态变量，x(k-1)为系统在k-1时刻的状态变量，Δx_p和Δy_p为x_p和y_p的变化量，y(k)＝[x_p y_p]^T为系统在k时刻的观测变量，A为状态估计矩阵，H为观测矩阵，q和r分别为估计噪声和观测噪声，符合高斯分布；

Kalman滤波器时间与状态共五个核心更新方程如下：

其中，xg(k)为k时刻的状态预测值，x(k-1)为k-1时刻的状态值，P(k)'为k时刻的先验误差协方差矩阵，P(k-1)为k-1时刻的后验误差协方差矩阵，Q为q的协方差矩阵，K(k)为k时刻的增益，R为r的协方差矩阵，P(k)为k时刻的后验误差协方差矩阵；

因此，选择P_{connected-D1.b}、P_{connected-D2.b}两个目标点的检测特征向量作为Kalman滤波器的状态变量，通过迭代计算五条核心方程，状态变量最终将收敛至最优估计，就能够达到对目标点进行跟踪的目的。

在步骤6)中先根据步骤5)所得ROI，先进行舱门目标区域的精确定位，再在该目标区域内进行目标点的图像坐标获取。

6.1)目标区域的精确定位

先进行ROI结果图的全局自适应阈值化后，进行寻找轮廓处理，得到舱门目标区域的轮廓二值图。再根据舱门轮廓形态学上的长度、外接矩、质心等几何特征，以及与所得P_{connected-D1.b}、P_{connected-D2.b}两点的位置关系，滤去干扰轮廓以精确得到舱门目标区域的最小外接矩，从而精确提取舱门的完整轮廓，得到目标区域的轮廓结果图。

6.2)获取目标点的图像坐标

基于步骤6.1)所得的舱门目标区域的轮廓结果图，在P_{connected-D1.b}、P_{connected-D2.b}两点之间的圆角以外范围，自下而上寻找第一个非零像素点，即为舱门底边的外侧点，如此间隔采样舱门底边框的外侧像素点集，并用直线拟合舱门底边框的外侧；同时利用该直线来分别采样舱门左、右侧边框的外侧点集，即跳过舱门 P_{connected-D1.b}、P_{connected-D2.b}两点的圆角范围平行等间距地向上平移该直线，每次分别从该直线的左、右端点向右、向左寻找第一个非零像素点，分别获得该直线上的舱门左、右边框的外侧点，如此分别间隔采样舱门左、右边框的外侧像素点集，并分别用二次多项式曲线拟合两侧边框的外侧，最终得到左、右侧边曲线和底边直线。两条曲线分别底边直线的存在两个交点，这两个交点即为最终舱门的左、右目标点，返回左、右目标点的图像坐标。

在步骤7)中根据步骤2)所建立的基于视觉深度的三维定位模型，结合测距传感器的深度信息，最终得到舱门目标点的世界坐标。在步骤2)最后计算出来的P_D＝[X_D,Y_D,Z_D]^T，可以理解为摄像头光心到目标点连线射线的方向向量。所求目标点的三维世界坐标，也即求舱门平面与该射线交点的三维世界坐标。根据该射线的方向向量，和该射线上一点即坐标系{D}的原点(0,0,0)，可以确定这条直线在坐标系{D}下的方程。接下来根据左右两个测距传感器的深度信息可得到舱门平面的两个不同点A、B，再可通过这两点折中取点也可很容易得到舱门平面上的另一不同点C，且这三点为舱门平面三个不共线的点，此时确定该平面在坐标系{D}下的方程，进而根据直线方程与平面方程联立的方程组求解，即可得到平面跟直线的交点坐标，也即舱门目标点在坐标系{D}下的三维世界坐标。又由于坐标系{W}与坐标系{S}的位置关系已知，结合坐标系{D}与坐标系{S}的位置关系，最终可得坐标系{W}下目标点的三维世界坐标。

在步骤8)中基于步骤7)最后所得舱门目标点的实时世界坐标，以及最终机器人行为目的来控制机器人此时的行为，重复步骤3)至步骤8)以到达最终目标。

本发明与现有技术相比，具有如下优点与有益效果：

本发明适用于飞机舱门、轮船舱门等类似的多种应用场景中，也可以满足工业机器人和服务机器人的多种机器人移动需求，通过简单快速的图像处理算法得到舱门目标点的图像坐标，并通过方便可行的三维定位模型最终获得指定世界坐标系下舱门目标点的世界坐标，最终达到机器人控制的行为目的。本方案仅利用单个摄像头，采用测距传感器增加定位的辅助信息，既弥补了单目视觉的三维定位深度信息的缺乏，又避免了双目视觉中立体匹配困难的问题，从而最终提高舱门实时定位的精度和可行性，达到机器人目标行为的准确性与智能化，在追求舱门实时定位的准确性与智能化的同时，也保证方案的可行性和简单、易实现。

附图说明

图1为本发明方法流程图。

图2为本发明中三维定位模型的位置关系示意图。

图3为舱门边框初步检测结果图。

图4为分割ROI后的舱门轮廓二值图。

图5为舱门两侧边、底边拟合结果图。

具体实施方式

下面结合具体实施例对本发明作进一步说明。

如图1所示，本实施例所述的基于单目机器人与测距传感器结合的舱门实时定位方法，主要是通过安装在机器人上的单个摄像头进行在舱门距离 1.5m-2.5m时(根据摄像头的视场大小所得)进行实时拍摄，获取图中舱门目标点的二维图像坐标，最后结合测距传感器的深度信息得到舱门目标点的三维世界坐标，从而控制机器人的一次移动行为，如此反复以达到机器人控制的最终目的。如图2所示，摄像头与左右两个激光测距传感器均安装在机器人的同一模块上，同时建立的几个坐标系如下：

·坐标系{C}：摄像机坐标系，原点与光心重合，Z轴为摄像机的光轴；

·坐标系{S}：机器人上的已知位置三维坐标系，该坐标系的原点位于外参标定板中线的下端点；

·坐标系{W}：机器人上的最终世界坐标系，与坐标系{S}的位置关系已知；

·坐标系{D}：与坐标系{C}原点重合，即该坐标系与坐标系{C}之间的位置关系只有旋转无平移；且与摄像机坐标系{S}的坐标轴方向一致，即该坐标系与坐标系{S}之间的位置关系只有平移无旋转。通过该坐标系来计算目标点的三维坐标。

其中坐标系{C}、坐标系{S}和坐标系{W}均在机器人的同一模块上，即在机器人的移动过程中是相对静止的。最终所得舱门目标点的世界坐标是指在坐标系{W}下的表示值。本实例中所用的摄像机是Navitar 2.5mm短焦距广角镜头工业相机，两个激光测距传感器是ZYT-0100-1激光测距传感器。

1)标定摄像机的内参、畸变参数

首先进行内参、畸变参数的标定准备，用A4纸打印一张内角点为8*11的黑白棋盘格，贴在一个硬平板上制作标定所需的棋盘标定板，用摄像机在不同位置、不同角度拍摄20张照片。根据张正友标定法计算摄像机内参、畸变参数。

1.1)计算摄像机内参

根据摄像机模型，设三维世界坐标的点X＝[X,Y,Z,1]^T，二维图像像素坐标为 m＝[u,v,1]^T，则标定棋盘格平面到图像平面的单应性关系为：

s₀m＝K[R,T]X

其中s为世界坐标系到图像坐标系的尺度因子，K为摄像机内参，R为旋转矩阵，T为平移向量。令

其中α，β为焦距与像素横纵比的融合，γ为径向畸变参数。注意，s对于齐次坐标来说，不会改变齐次坐标值。将世界坐标系设在棋盘格平面上，棋盘格平面设为Z＝0的平面。则可得

单应性矩阵H＝K[r₁,r₂,t]，即

H＝[h₁ h₂ h₃]＝λK[r₁ r₂ t]

H是一个齐次矩阵，有8个未知数。由上式可得

由旋转矩阵r₁和r₂正交，可得

||r₁||＝||r₂||＝1

带入可得：

即每个单应性矩阵能提供两个方程，为了方便计算可定义如下：

B是一个对称阵，所以B的有效元素为6个，把这6个元素写成向量b，即

b＝[B₁₁ B₁₂ B₂₂ B₁₃ B₂₃ B₃₃]^T

推导可得

ν_ij＝[h_i1h_j1 h_i1h_j2+h_i2h_j1 h_i2h_j2 h_i3h_j1+h_i1h_j3 h_i3h_j2+h_i2h_j3 h_i3h_j3」

利用约束条件可得到：

最后通过cholesky分解，得到摄像机的内参矩阵

1.2)最大似然估计

上述的推导结果是基于理想情况下的解，但由于可能存在高斯噪声，所以需要使用最大似然估计进行优化。设采集了n副包含棋盘格的图像进行标定，每副图像里有棋盘格角点m个。令第i副图像上的角点M_j在上述计算得到的摄像机矩阵下图像上的投影点为：

其中R_i和t_i是第i副图对应的旋转矩阵和平移向量，K是内参矩阵。则角点 m_ij的概率密度函数为：

构造似然函数：

让L取得最大值，即让该式最小。这里使用的是多参数非线性系统优化问题的Levenberg-Marquardt算法进行迭代求最优解。

1.3)径向畸变估计

张正友标定法只关注了影响最大的径向畸变。其数学表达式为：

其中，(u,v)是理想无畸变的像素坐标，是实际畸变后的像素坐标。(u₀,v₀)代表主点，(x,y)是理想无畸变的连续图像坐标，是实际畸变后的连续图像坐标。k₁和k₂为前两阶的畸变参数。

化成矩阵形式：

记为：Dk＝d，则可得：k＝[k₁ k₂]^T＝(D^TD)^-1D^Td，计算得畸变参数k。使用最大似然的思想优化结果，即同1.3)，LM法计算下列函数值最小的参数值：

最终得到畸变参数k＝[-4.1327308257154893e-001, 1.2965083049830844e-001,-1.4412726750038607e-003,7172713255872575e-003, 1.5444288296932739e-002]。

2)建立基于视觉深度的三维定位模型

标定物0：在标定时，我们会以坐标系{S}原点为中心的Z＝0平面，放置一块标定板。该标定板平面底部位于X轴上，且标定板平面的的中线位于Y轴的负半轴上。定义该标定物为标定物0。

2.1)标定已知位置的一组外参

外参由旋转矩阵与平移向量构成，即[R|t]＝[r₁ r₂ r₃ t]。由1)中的推导可得：

2.2)计算坐标换算模型

本实例中的坐标系{S}与坐标系{W}之间的已知关系为

其中chessboard_row、chessboard_col为外参标定板的行数、列数，square_H、square_W为外参标定板单位棋盘格的高度、宽度。且chessboard_row＝8、 chessboard_col＝11、square_H＝65mm、square_W＝65mm。

根据已知位置的一组外参值，可得本实例中坐标系{S}与坐标系{C}之间的关系为：

坐标系{D}跟坐标系{S}的已知关系：

可知本实例中坐标系{S}与坐标系{D}之间的关系为：

根据坐标换算模型：

可得坐标系{C}与坐标系{D}之间的关系为：

3)实时拍摄一张舱门模型原始图像。

4)分割舱门目标区域ROI

利用传统图像处理算法进行舱门模型原始图像的目标区域分割，基于舱门矩形边宽的几何特征进行舱门的提取与定位，总体思路为先根据横向相邻像素灰度值差的统计特征进行舱门两侧边的提取，得到目标附近点，从而进行舱门图像的ROI(region ofinterest)矩形区域定位、分割。主要步骤如下：

4.1)分割舱门的侧边与底边

分别计算灰度图的横向、纵向相邻像素灰度差结果，根据舱门侧边、底边的梯度宽度特征介于固定范围内，进行灰度差结果图的阈值二值化运算，可分别得舱门两侧边和底边的提取结果二值图，也即可分别初步提取出舱门的两侧边、底边。图3为舱门边框初步检测结果图。

4.2)分割ROI

在侧边、底边提取结果的基础上进行中值滤波，根据Seed-filling种子填充法进行连通域分析。除去一些太窄/太宽的连通域后，并分别找两条高度最高的连通域connected-D1,connected-D2，以及一条宽度最宽的连通域connected-D3，即为舱门两侧边、底边的提取结果，获得这三条连通域的两端点、高、宽数据，并返回两侧边连通域connected-D1,connected-D2的最底点P_{connected-D1.b}、P_{connected-D2.b}，以及底边连通域connected-D3的两端点P_{connected-D3.t}、P_{connected-D3.b}，这四点即为目标附近点；后续基于该步骤的检测结果判断舱门各边(左侧边、右侧边、底边)的检测情况，并划分舱门图像ROI(region of interest)。

5)判断视野内的舱门情况

由P_{connected-D1.b}＝(773,901)、P_{connected-D2.b}＝(1209,923)、P_{connected-D3.t}＝(773,904)、 P_{connected-D3.b}＝(1203,927)四点的检测结果，均满足坐标要求且检测标记位 flag_cca[0]＝1、flag_cca[1]＝1，根据情况划分定义可判断满足情况6(舱门的整个下半部分均在视野范围内)，那么先将P_{connected-D1.b}＝(773,901)、P_{connected-D2.b}＝(1209,923)、P_{connected-D3.t}＝(773,904)、P_{connected-D3.b}＝(1203,927)四点存入Kalman跟踪器的对应点集中，且src.rows-Max(901,923,904,927)＝1080-927＝153＞30，可判断机器人不会靠近舱门底边，此时可进行目标点的精确定位。

6)精确定位目标点的图像坐标

将4)中所得ROI分割出来后，先进行ROI结果图的全局自适应阈值化，再进行寻找轮廓处理，得到舱门目标区域的轮廓二值图。再根据舱门轮廓形态学上的长度、外接矩、质心等几何特征，以及与所得P_{connected-D1.b}＝(773,901)、 P_{connected-D2.b}＝(1209,923)两点的位置关系，滤去下部、两侧外部的干扰轮廓以精确得到舱门目标区域的最小外接矩，从而精确提取舱门的完整轮廓，得到目标区域的轮廓结果图。图4为本实例分割ROI后的舱门轮廓二值图。

基于图4的轮廓二值图，在P_{connected-D1.b}＝(773,901)、P_{connected-D2.b}＝(1209,923)两点之间的圆角以外范围，自下而上寻找第一个非零像素点，即为舱门底边的外侧点，如此间隔采样舱门底边框的外侧像素点集(图5底部橙色点集)，并用直线拟合舱门底边框的外侧；同时利用该直线来分别采样舱门左、右侧边框的外侧点集，即跳过舱门P_{connected-D1.b}＝(773,901)、P_{connected-D2.b}＝(1209,923)两点的圆角范围平行等间距地向上平移该直线，每次分别从该直线的左、右端点向右、向左寻找第一个非零像素点，分别获得该直线上的舱门左、右边框的外侧点，如此分别间隔采样舱门左、右边框的外侧像素点集，并分别用二次多项式曲线拟合两侧边框的外侧，最终得到左、右侧边曲线和底边直线(图5中的直线和曲线)。两条曲线分别底边直线的存在两个交点，这两个交点即为最终舱门的左、右目标点，返回左、右目标点的图像坐标。图5为本实例的舱门两侧边、底边拟合结果图，图中两个红色标记点即为ROI区域内拟合结果所得舱门的左、右目标点，最终得到原图的舱门两目标点最终结果。

7)结合测距传感器的深度信息，得到舱门目标点的世界坐标

根据步骤2)所建立的基于视觉深度的三维定位模型，其中所得坐标系{C} 与坐标系{D}之间的关系为：

结合本实例中测距传感器的深度信息distance_lr＝[20402010](单位：mm)，计算图像坐标P_l＝(768,914)的三维世界坐标。

首先根据步骤2)先计算出摄像头光心到点P_l＝(768,914)连线射线在坐标系{D}下的归一化方程系数矩阵为[0.1212 0.2536 -0.9838]^T，接下来根据左右两个测距传感器的深度信息distance_lr＝[2040 2010](单位：mm)，获取舱门平面上三个不共线的点A、B、C，如图2所示，本实例中A选取左边测距对应的点，B选取右边测距对应的点，C选取B上方的一个点，得到舱门平面的两个不同点 P_A＝(125,0,3402.9)、P_B＝(-125,0,1392.9)，再可通过这两点折中取点也可很容易得到舱门平面上的另一不同点P_C＝(125,125,3402.9)，且这三点为舱门平面三个不共线的点，可确定该平面在坐标系{D}下的归一化方程系数矩阵为[0.99240-0.1234]^T，进而根据直线方程与平面方程联立的方程组求解，即可得到平面跟直线的交点坐标，也即舱门目标点在坐标系{D}下的三维世界坐标。又由于坐标系{W}与坐标系{S}的位置关系已知，结合坐标系{D}与坐标系{S}的位置关系，最终可得坐标系{W}下目标点的三维世界坐标Pl_W＝(-243.499,-252.622,2597.5)。同理可得到本实例中另一目标点的三维世界坐标Pr_W＝(-260.123,-628.322,2463.84)。

8)将定位结果Pl_W＝(-243.499,-252.622,2597.5)、Pr_W＝(-260.123,-628.322,2463.84) 传入机器人控制单元，以完成本次机器人的移动。同时根据本次所得目标点图像坐标在Kalman跟踪器的跟踪结果，优化下一帧的图像处理部分，并继续下一帧的目标点定位和机器人的移动，如此重复以达到机器人移动的最终位置。

以上所述实例只为本发明之实验实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。

Claims (7)

1.基于单目机器人与测距传感器结合的舱门实时定位方法，其特征在于：通过结合激光测距传感器的单目视觉系统来实现机器人对舱门的实时定位，以达到机器人的行为目的，包括以下步骤：

1)利用张正友标定算法标定摄像机的内参、畸变参数；

2)建立起基于视觉深度的三维定位模型；

3)视频图像实时拍摄；

4)分割目标区域；

5)判断视野内的舱门情况，以使其目标区域位于视野内获得感兴趣区域(ROI)；

6)根据3)所得结果，利用几何特征精确定位得目标点图像坐标；

7)结合测距传感器的深度信息，得到舱门目标点的世界坐标；

8)根据所得世界坐标，控制机器人行为。

2.根据权利要求1所述的基于单目机器人与测距传感器结合的舱门实时定位方法，其特征在于步骤2)包括以下几步：

2.1)标定已知位置下的外参

首先，外参由旋转矩阵与平移向量构成，即[R|t]＝[r₁ r₂ r₃ t]，其中s为世界坐标系到图像坐标系的尺度因子，K为摄像机内参，R为旋转矩阵，T为平移向量，H是一个齐次矩阵，根据张正友标定法原理的推导可得：

则r₃＝r₁×r₂，t＝λK^-1h₃可得该位置下的一组外参[R₀|t₀]；

2.2)坐标换算模型

需要定义以下几个坐标系：

坐标系{C}：摄像机坐标系，原点与光心重合，Z轴为摄像机的光轴；

坐标系{S}：机器人上的已知位置三维坐标系，该坐标系需与外参标定板的位置关系已知；

坐标系{W}：机器人上的最终世界坐标系，与坐标系{S}的位置关系已知；

坐标系{D}：引入一个与坐标系{C}原点重合，即该坐标系与坐标系{C}之间的位置关系只有旋转无平移；且与摄像机坐标系{S}的坐标轴方向一致，即该坐标系与坐标系{S}之间的位置关系只有平移无旋转。通过该坐标系来计算目标点的三维坐标；

坐标的整体模型如下：

根据上述张正友标定法得到已知位置P₀下的一组外参[R₀|t₀]，首先，从坐标系的定义可知，结合该外参标定板和坐标系{S}的已知关系，可获得坐标系{C}下坐标系{S}位置关系的旋转矩阵和平移矩阵

计算得到摄像机光心相对于坐标系{S}原点的Z方向上的距离由于坐标系{D}相对于坐标系{C}原点重合，即只有旋转无平移；坐标系{D}相对于坐标系{S}只是原点偏移，即只有平移无旋转，所以也可得到坐标系{D}跟坐标系{S}的关系

通过坐标系位置关系的传递可得坐标系{D}与坐标系{C}的关系，如下：

最后就可得到归一化的坐标从坐标系{C}转换到坐标系{D}，

即可计算出对应点的在{D}坐标系下的归一化坐标P_D＝[X_D,Y_D,Z_D]^T。

3.根据权利要求1所述的基于单目机器人与测距传感器结合的舱门实时定位方法，其特征在于：在步骤3)中，采用摄像头进行图像的实时拍摄，该摄像头安装在机器人部件上，与舱门底边相近高度，且其拍摄方向尽量与舱门平面垂直，以保证摄像头的视场覆盖整个舱门目标区域且镜头畸变较小。

4.根据权利要求1所述的基于单目机器人与测距传感器结合的舱门实时定位方法，其特征在于步骤4)中主要步骤如下：

4.1)分割舱门的侧边与底边

首先机器人的摄像机安装固定，且由于摄像机视场的限制本方案的应用场景为在距离舱门一定范围内拍摄，根据所得图像的像素灰度值进行统计分析，观察得舱门边框的像素分布规律，如针对具有明显标志线的舱门边框，具有同行或同列相邻前一像素灰度值减后一像素的灰度值的灰度差，在舱门边框附近呈类正弦函数的正负峰值分布，故而分割算法可分别计算灰度图的横向、纵向相邻像素灰度差结果，根据舱门侧边、底边的梯度宽度特征介于固定范围内，进行灰度差结果图的阈值二值化运算，可分别得舱门两侧边和底边的提取结果二值图，也即可分别初步提取出舱门的两侧边、底边。

4.2)分割ROI

在侧边、底边提取结果的基础上进行中值滤波，根据Seed-filling种子填充法进行连通域分析，除去一些太窄/太宽的连通域后，并分别找两条高度最高的连通域connected-D1,connected-D2，以及一条宽度最宽的连通域connected-D3，即为舱门两侧边、底边的提取结果，获得这三条连通域的两端点、高、宽数据，并返回两侧边连通域connected-D1,connected-D2的最底点P_{connected-D1.b}、P_{connected-D2.b}，以及底边连通域connected-D3的两端点P_{connected-D3.t}、P_{connected-D3.b}，这四点即为目标附近点；后续基于该步骤的检测结果判断舱门各边的检测情况，并划分舱门图像感兴趣区域(ROI)。

5.根据权利要求4所述的基于单目机器人与测距传感器结合的舱门实时定位方法，其特征在于：在步骤5)中，根据步骤4)中所得的四个点P_{connected-D1.b}、P_{connected-D2.b}、P_{connected-D3.t}、P_{connected-D3.b}判断视野内的舱门情况，由于舱门在机器人摄像机的视野范围内的不确定性，且有可能只拍摄到舱门的部分区域，故而可分为以下7中情况：

情况1：舱门的整个下半部分均不在视野范围内。即满足要求的三条连通域在步骤4)中都未被检测到，返回结果以使机器人随机移动，并进行下一帧的图像处理；

情况2：只拍摄到舱门的左/右半侧边，即在步骤4)中只检测到侧边连通域connected-D1/connected-D2中的一条，根据P_{connected-D1.b}、P_{connected-D2.b}可判断其为左/右侧边，进而判断此时机器人是太靠近左/右边界，此时将所得P_{connected-D1.b}或者P_{connected-D2.b}存入Kalman跟踪器的对应点集中，并返回结果以使机器人向右/左，同时进行下一帧的处理；

情况3：只拍摄到舱门的底边，即在步骤4)中只检测到底边的一条连通域connected-D3，进而判断此时机器人是太靠近下边界，返回结果以使机器人向上移动，并进行下一帧的处理；

情况4：只拍摄到了舱门的两条侧边，即在步骤4)中只检测到侧边的两条连通域connected-D1、connected-D2，但未检测到底边连通域connected-D3，进而判断此时机器人位置太靠上，此时将P_{connected-D1.b}、P_{connected-D2.b}存入Kalman跟踪器的对应点集中，并返回结果以使机器人向下移动，同时进行下一帧的处理；

情况5：只拍摄到了舱门的左/右半部分，即在步骤4)中只检测到侧边连通域connected-D1/connected-D2中的一条，以及底边连通域connected-D3，类似②情况2首先根据P_{connected-D1.b}、P_{connected-D2.b}可判断检测到的侧边为左/右侧边，进而判断此时机器人是太靠近左/右边界，此时将所得P_{connected-D1.b}或者P_{connected-D2.b}存入Kalman跟踪器的对应点集中，并返回结果以使机器人向右/左，同时进行下一帧的处理；

情况6：舱门的整个下半部分均在视野范围内，即在步骤4)中满足要求的三条连通域都被检测到了，那么先将所得P_{connected-D1.b}或者P_{connected-D2.b}存入Kalman跟踪器的对应点集中，并根据P_{connected-D3.t}、P_{connected-D3.b}判断机器人是否太靠近舱门底边，若机器人太靠近舱门底边，则返回结果意思机器人向下移动；否则即为三边初步检测结果符合后续定位要求，接下来可进行目标点的精确定位，即可进行步骤7)利用舱门侧边、底边的拟合结果，获得两个目标点的图像坐标。

此时根据P_{connected-D1.b}、P_{connected-D2.b}两点向外画矩形，分割图像区域的感兴趣区域(ROI)；

其他情况：其他异常情况，返回结果并进行下一帧的处理；

基于以上7种情况分别进行相应的机器人控制，以使得最终舱门的目标区域位于摄像机的视野范围内。

6.根据权利要求5所述的基于单目机器人与测距传感器结合的舱门实时定位方法，其特征在于：在步骤6)中，先根据步骤5)所得感兴趣区域(ROI)，先进行舱门目标区域的精确定位，再在该目标区域内进行目标点的图像坐标获取：

6.1)目标区域的精确定位

先进行感兴趣区域(ROI)结果图的全局自适应阈值化后，进行寻找轮廓处理，得到舱门目标区域的轮廓二值图，再根据舱门轮廓形态学上的长度、外接矩、质心等几何特征，以及与所得P_{connected-D1.b}、P_{connected-D2.b}两点的位置关系，滤去干扰轮廓以精确得到舱门目标区域的最小外接矩，从而精确提取舱门的完整轮廓，得到目标区域的轮廓结果图；

6.2)获取目标点的图像坐标

基于步骤6.1)所得的舱门目标区域的轮廓结果图，在P_{connected-D1.b}、P_{connected-D2.b}两点之间的圆角以外范围，自下而上寻找第一个非零像素点，即为舱门底边的外侧点，如此间隔采样舱门底边框的外侧像素点集，并用直线拟合舱门底边框的外侧；同时利用该直线来分别采样舱门左、右侧边框的外侧点集，即跳过舱门P_{connected-D1.b}、P_{connected-D2.b}两点的圆角范围平行等间距地向上平移该直线，每次分别从该直线的左、右端点向右、向左寻找第一个非零像素点，分别获得该直线上的舱门左、右边框的外侧点，如此分别间隔采样舱门左、右边框的外侧像素点集，并分别用二次多项式曲线拟合两侧边框的外侧，最终得到左、右侧边曲线和底边直线，两条曲线分别底边直线的存在两个交点，这两个交点即为最终舱门的左、右目标点，返回左、右目标点的图像坐标。

7.根据权利要求6所述的基于单目机器人与测距传感器结合的舱门实时定位方法，其特征在于：在步骤7)中，根据步骤2)所建立的基于视觉深度的三维定位模型，结合测距传感器的深度信息，最终得到舱门目标点的世界坐标。在步骤2)最后计算出来的P_D＝[X_D,Y_D,Z_D]^T，可以理解为摄像头光心到目标点连线射线的方向向量。所求目标点的三维世界坐标，也即求舱门平面与该射线交点的三维世界坐标，根据该射线的方向向量，和该射线上一点即坐标系{D}的原点(0,0,0)，可以确定这条直线在坐标系{D}下的方程，接下来根据左右两个测距传感器的深度信息可得到舱门平面的两个不同点A、B，再可通过这两点折中取点也可很容易得到舱门平面上的另一不同点C，且这三点为舱门平面三个不共线的点，此时确定该平面在坐标系{D}下的方程，进而根据直线方程与平面方程联立的方程组求解，即可得到平面跟直线的交点坐标，也即舱门目标点在坐标系{D}下的三维世界坐标，根据坐标换算

坐标系{W}与坐标系{S}的位置关系已知，结合坐标系{D}与坐标系{S}的位置关系，最终可得坐标系{W}下目标点的三维世界坐标。