CN105091744B - 一种基于视觉传感器和激光测距仪的位姿检测装置与方法 - Google Patents

Abstract

本发明公开了一种基于视觉传感器和激光测距仪的位姿检测装置与方法，该装置使用一个视觉传感器和三个激光测距仪共同采集目标信息；该方法包括：从目标图像中选择特征点和特征直线，得到特征点的图像坐标和特征直线的角度，进而得到特征点的图像偏差和特征直线的角度偏差；求取当前像素当量，根据当前图像偏差、角度偏差和像素当量，得到当前位姿在三个自由度上的偏差；根据三个激光测距仪的读数及相对位置关系，得到当前姿态在对应两个旋转自由度上的角度偏差和当前位姿在深度方向上的偏差，由此实现目标的六自由度位姿检测。

Description

一种基于视觉传感器和激光测距仪的位姿检测装置与方法

技术领域

本发明属于传感器检测技术领域，更具体地涉及一种基于视觉传感器和激光测距仪的位姿检测装置和方法。

背景技术

位姿检测作为重要环节，广泛的应用于机器人控制、航空对接、跟踪制导等诸多领域。传感器种类丰富，功能多样，是获取外界环境信息的重要工具。通过传感器提供的信息实现位姿检测吸引了众多学者的关注。

视觉传感器能够感知丰富的环境信息，且安装使用方便，是较常使用的一种外部传感器。通过对视觉传感器获取的图像信息进行处理，可以得到目标的轮廓、形状、颜色，还可以实现目标的运动检测、相对定位等。根据视觉传感器提供的图像信息得到目标位姿信息是常用的位姿检测方法，这类方法(例如参考SangJoo Kwon，Haemm Jeong，and JaewoongHwang.Kalman Filter-Based Coarse-to-Fine Control for Display Visual AlignmentSystems[J].IEEE Transactions on Automation Science and Engineering，2012，9(3)：621-628；Biao Zhang，Jianjun Wang，Gregory Rossano，Carlos Martinez andKock.Vision-guided Robot Alignment for Scalable，Flexible Assembly Automation[C].IEEE International Conference on Robotics and Biomimetic.Phuket，Thailand，2011：944-951.)一般基于单目视觉或双目视觉，先从视觉图像中提取关键特征，然后结合视觉测量技术，经过多个坐标变换，得到目标的位置和姿态信息，实现目标的位姿检测。这类方法仅使用视觉传感器，传感器配置方便，检测过程简单高效。但是这类方法要求目标在检测过程中一直完全可见，因此只适用于小尺寸目标的位姿检测。

当不能得到充分目标信息如目标尺寸较大、部分视觉特征不可见时，就需要借助其他传感器来完成目标检测。目前，常用的方法主要有借助激光跟踪仪(例如参见Zhi Liu，Ying Xie，Jing Xu，Ken Chen.Laser tracker based robotic assembly system forlarge scale peg-hole parts[C].IEEE International Conference on CyberTechnology in Automation，Control and Intelligent Systems，Hong Kong，China，2014：574-578.)或者激光测距仪(例如参见Young-Keun Kim，Yonghun Kim，Kyung-sooKim，Yun Sub Jung.Developing a Robust Sensing System for Remote Relative 6-DOFMotion Using 1-D Laser Sensors[C].IEEE International Systems Conference，Vancouver，CANADA.2012.)来实现大尺寸目标的位姿检测。激光跟踪仪非常适合用于大尺寸零部件的位姿测量，但价格昂贵。激光测距仪测量精度高，性能稳定，抗干扰能力强，体积小，便于安装。目前的借助激光测距仪和视觉传感器进行位姿检测的方法将视觉传感器安装在目标物体上，检测时需要先手动调整激光测距仪的位姿，使其打到目标物体的指定位置，这类方法检测过程繁琐，且检测精度较低。

发明内容

基于上述背景，本发明的主要目的在于提供一种综合视觉传感器和激光测距仪两者优点的位姿检测装置和方法。

为达到上述目的，作为本发明的一个方面，本发明提供了一种基于视觉传感器和激光测距传感器的位姿检测方法，包括步骤如下：

步骤S0，使用一个视觉传感器和三个激光测距仪共同获取目标信息；

步骤S1：从所述视觉传感器获得的目标图像中选择视觉传感器敏感的特征点和特征直线，离线得到特征点期望图像坐标和特征直线期望角度，在线提取所述特征点和特征直线，得到所述特征点当前图像坐标和所述特征直线当前角度，比较当前值和期望值，得到当前特征点和期望特征点之间的图像偏差，以及当前特征直线角度和期望特征直线角度之间的偏差；

步骤S2：根据在线提取的所述目标图像的特征和目标空间尺寸，得到当前像素当量；

步骤S3：根据所述图像偏差和当前像素当量，得到当前位姿和目标位姿在视觉传感器敏感的三个自由度的偏差；

步骤S4：根据所述三个激光测距仪的读数及它们的相对位置关系，离线得到所述三个激光测距仪构成的平面的法向量期望值，在线得到当前平面法向量，将当前法向量和期望法向量分别投影到视觉不敏感的两个旋转自由度对应的平面上，并根据投影到各平面的分量，得到当前姿态和期望姿态在对应自由度上的角度偏差；

步骤S5：根据所述三个激光测距仪的读数得到当前位姿和期望位姿在深度方向的偏差；

步骤S6：结合从所述目标图像得到的三个自由度的偏差和从激光测距仪读数得到的三个自由度的偏差，以及当前位姿，得到目标的六自由度信息，实现目标的位姿检测。

其中，所述视觉传感器安装在便于获取目标物体图像的位置，所述三个激光测距仪呈等腰三角形排列。

其中，步骤S1中所述特征点的当前图像偏差和特征直线的当前角度偏差是通过下式来计算的：

其中，Δu、Δv分别表示特征点在U方向和V方向的图像坐标偏差，θ_z表示特征直线角度偏差。(u_d，v_d)表示特征点期望图像坐标，(u，v)表示特征点当前图像坐标，θ_d表示特征直线期望角度，θ表示特征直线当前角度。

其中，步骤S2中所述的当前像素当量是通过下式来计算的：

t_s＝S/S₀；

其中，t_s表示当前的像素当量，S表示目标在图像中的尺寸，S₀表示目标的空间尺寸。

其中，步骤S3中所述的当前位姿和目标位姿在视觉传感器敏感的三个自由度的偏差表示如下：

其中，Δx表示当前位姿和期望位姿在X方向的位置偏差，Δy表示当前位姿和期望位姿在X方向的位置偏差，Δθ_z表示当前位姿和期望位姿在绕光轴方向的旋转角度偏差。

其中，步骤S4中所述的当前位姿和目标位姿在视觉传感器不敏感的两个自由度的偏差表示如下：

其中，Δθ_x表示当前位姿和目标位姿在绕X轴方向的旋转角度偏差，Δθ_y示当前位姿和目标位姿在绕Y轴方向的旋转角度偏差。V_qyoz和V_qzox是激光平面的期望法向量V_q分别在YOZ平面和ZOX平面的投影，V_yox和V_zox是激光平面法向量V分别在YOZ平面和ZOX平面的投影。

其中，步骤S5中所述的当前位姿和目标位姿在视觉传感器不敏感的深度方向上的位置偏差表示如下：

Δz＝(d₁+d₂+d₃)/3；

其中，Δz表示当前位姿和目标位姿在Z方向的位置偏差，d_i(i＝1，2，3)表示三个激光测距仪的读数。

其中，所述的目标位姿表示如下：

其中，[x，y，z，θ_x，θ_y，θ_z]表示通过视觉传感器和激光测距仪检测得到的目标位姿，[x₀，y₀，z₀，θ_x0，θ_y0，θ_z0]表示当前位姿，[Δx，Δy，Δz，Δθ_x，Δθ_y，Δθ_z]表示当前位姿和目标位姿之间的偏差，[Δx，Δy，Δθ_z]是通过视觉图像得到的位姿偏差，视觉传感器对这三个自由度的位姿变化敏感，[Δz，Δθ_x，Δθ_y]是根据激光测距仪的测量值得到的位姿偏差，通过激光测距仪来得到视觉传感器敏感的三个自由度之外的位姿信息。

作为本发明的另一个方面，本发明还提供了一种基于视觉传感器和激光测距仪的位姿检测装置，包括一个视觉传感器、三个激光测距仪和控制单元，其中所述视觉传感器安装在便于获取目标物体图像的位置，所述三个激光测距仪呈等腰三角形排列，以及

所述控制单元执行如权利要求1至8任意一项所述的基于视觉传感器和激光测距仪的位姿检测方法来控制所述视觉传感器和激光测距仪的输入，并计算得到目标的六自由度信息，实现目标的位姿检测。

其中，所述视觉传感器为AVT GC1600H，激光测距仪为CASTAL PT50220S。

基于上述技术方案可知，本发明具有以下有益效果：传统的基于视觉图像的位姿检测方法要求目标在检测过程中一直完全可见，只适用于小尺寸目标的位姿检测，对于不能得到充分目标图像信息如目标尺寸较大、部分视觉特征不可见等场景，其难以检测目标的精确位姿信息。本发明针对视觉传感器不能得到充分目标图像的场景，借助视觉传感器和激光测距仪共同获取目标信息，从视觉图像得到视觉传感器敏感的三个自由度的位姿信息，从激光距离信息得到剩余三个自由度的位姿信息，融合两种传感器的检测结果，实现目标的位姿检测。该发明能够在无法得到充分目标图像的场景下，根据视觉图像信息和激光距离信息，得到目标的六自由度位姿。大量的试用数据也证明了本发明的有效性。由此可见，本发明借助精心布局的视觉传感器和激光传感器，解决了仅靠视觉传感器难以得到充分目标信息的问题；基于视觉传感器和激光传感器提供的信息，实现了目标的六自由度位姿检测；检测精度高，稳定性和实时性好。

附图说明

图1为本发明的基于视觉传感器和激光测距仪的位姿检测装置的传感器布局图，其中深灰色方形表示激光测距仪，深灰色圆形表示视觉传感器；

图2为本发明的基于视觉传感器和激光测距仪的位姿检测方法的流程图；

图3为本发明的基于视觉传感器和激光测距仪的位姿检测装置传感器的检测结果。

具体实施方式

下面结合附图对本发明的实施例作详细的说明：本实施例在以本发明技术方案为前提下进行实施，结合详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述实施例。

本发明公开了一种基于视觉传感器和激光测距仪的位姿检测装置和方法，该装置使用精心布局的多个传感器共同采集目标信息，对不同类型传感器提供的信息分别处理，并将处理结果进行融合，实现目标的六自由度位姿检测。

更具体地，作为本发明的一个优选实施例，如图1示出本发明的基于视觉传感器和激光测距仪的位姿检测装置的传感器布局图，该装置中使用了四个传感器：一个视觉传感器和三个激光测距仪。视觉传感器安装在便于获取目标物体图像的位置，三个激光测距仪呈等腰三角形排列。针对方形目标，传感器支架也为方形，三个激光测距仪分别安装在方形支架的左边、右边和下边的中点处，视觉传感器安装在方形支架的上边。其中，所述视觉传感器例如为AVT GC1600H，激光测距仪例如为CASTAL PT50220S。

如图2示出本发明的基于视觉传感器和激光测距仪的位姿检测方法的流程图，在检测过程中，从视觉图像得到当前特征和期望特征的图像偏差，进而得到当前位姿和期望位姿在视觉敏感的三个自由度的信息，从激光距离信息得到当前位姿和期望位姿在深度方向的位置偏差，以及当前法向量和期望法向量的角度偏差，从而得到当前位姿和期望位姿在视觉不敏感的三个自由度上的信息，融合视觉图像处理结果和激光距离信息结果，实现目标的位姿检测，该方法包括以下步骤：

第一步：从目标图像中选择视觉传感器敏感的特征点和特征直线，离线得到特征点期望图像坐标和特征直线期望角度，在线提取特征点和特征直线，得到特征点当前图像坐标和特征直线当前角度，比较当前值和期望值，得到当前特征点和期望特征点之间的图像偏差，以及当前特征直线角度和期望特征直线角度之间的偏差；

第二步：根据在线提取的图像特征和目标空间尺寸，得到当前像素当量；

第三步：根据当前图像偏差和像素当量，得到当前位姿和目标位姿在视觉传感器敏感的三个自由度的偏差；

第四步：根据三个激光测距仪的读数及它们的相对位置关系，离线得到三个激光测距仪构成的平面的法向量期望值，在线得到当前平面法向量，将当前法向量和期望法向量分别投影到视觉不敏感的两个旋转自由度对应的平面上，并根据投影到各平面的分量，得到当前姿态和期望姿态在对应自由度上的角度偏差；

第五步：根据三个激光传感器的读数得到当前位姿和期望位姿在深度方向的偏差；

第六步：结合从视觉图像得到的三个自由度的偏差和从激光距离信息得到的三个自由度的偏差，以及当前位姿，得到目标的六自由度信息，实现目标的位姿检测。

所述第一步，具体如下：

根据视觉传感器获取的目标图像，提取关键特征，得到特征点的当前图像偏差和特征直线的当前角度偏差如下：

其中，Δu、Δv分别表示特征点在U方向和V方向的图像坐标偏差，θ_z表示特征直线角度偏差。(u_d，v_d)表示特征点期望图像坐标，(u，v)表示特征点当前图像坐标。θ_d表示特征直线期望角度，θ表示特征直线当前角度。

所述第二步，具体如下：

根据目标在图像中的尺寸和目标的空间尺寸，得到当前像素当量如下：

t_s＝S/S₀

其中，t_s表示当前的像素当量，S表示目标在图像中的尺寸，S₀表示目标的空间尺寸。

所述第三步，具体如下：

根据第一步得到的图像偏差和角度偏差，以及第二步得到的像素当量，得到当前位姿和目标位姿在视觉传感器敏感的三个自由度的偏差如下：

其中，Δx表示当前位姿和期望位姿在X方向的位置偏差，Δy表示当前位姿和期望位姿在X方向的位置偏差，Δθ_z表示当前位姿和期望位姿在绕光轴方向的旋转角度偏差。

所述第四步，具体如下：

将激光传感器当成质点，则这三个激光传感器构成一个平面。记三个激光传感器质点为A，B，C，则三个质点构成的平面法向量V如下：

根据三个激光测距仪的测量值及它们之间的位置关系，离线得到期望的平面法向量V_q，然后在线计算当前的平面法向量V，将V和V_q投影到YOZ平面和ZOX平面，则两个法向量在YOZ平面的投影之间的夹角即为当前位姿与期望位姿在绕X轴方向的角度偏差，两个法向量在ZOX平面的投影之间的夹角即为当前位姿与期望位姿在绕Y轴方向的角度偏差。因此，得到角度偏差如下：

其中，Δθ_x表示当前位姿和目标位姿在绕X轴方向的旋转角度偏差，Δθ_y表示当前位姿和目标位姿在绕Y轴方向的旋转角度偏差。V_qyoz和V_qzox是激光平面的期望法向量V_q分别在YOZ平面和ZOX平面的投影，V_yox和V_zox是激光平面法向量V分别在YOZ平面和ZOX平面的投影。

所述第五步，具体如下：

根据三个激光测距仪的测量值，得到当前位姿和目标位姿在视觉传感器不敏感的深度方向上的位置偏差如下：

Δz＝(d₁+d₂+d₃)/3

其中，Δz表示当前位姿和目标位姿在Z方向的位置偏差，d_i(i＝1，2，3)表示三个激光测距仪的测量值。

所述第六步，具体如下：

其特征在于，所述的目标位姿表示如下：

其中，[x，y，z，θ_x，θ_y，θ_z]表示通过视觉传感器和激光测距仪检测得到的目标位姿，[x₀，y₀，z₀，θ_x0，θ_y0，θ_z0]表示当前位姿，[Δx，Δy，Δz，Δθ_x，Δθ_y，Δθ_z]表示当前位姿和目标位姿之间的偏差，[Δx，Δy，Δθ_z]是通过视觉图像得到的位姿偏差，视觉传感器对这三个自由度的位姿变化敏感，[Δz，Δθ_x，Δθ_y]是根据激光测距仪的测量值得到的位姿偏差，通过激光测距仪来得到视觉传感器敏感的三个自由度之外的位姿信息。

为了验证本发明的方法，对目标进行位姿检测。图3为本发明的传感器检测结果，(a)为目标图像处理结果，(b)为三个激光测距仪的测量值。从图3可以看出，所述方法能够准确的对目标图像进行检测，且三个激光测距仪测量值基本一致，此时检测装置与目标的位姿偏差已基本消除，根据装置的当前位姿，可以得到目标的位姿信息。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于视觉传感器和激光测距仪的位姿检测方法，包括以下步骤：

步骤S0，使用一个视觉传感器和三个激光测距仪共同获取目标信息；

步骤S1：从所述视觉传感器获得的目标图像中选择视觉传感器敏感的特征点和特征直线，离线得到特征点期望图像坐标和特征直线期望角度，在线提取所述特征点和特征直线，得到所述特征点当前图像坐标和所述特征直线当前角度，比较当前值和期望值，得到当前特征点和期望特征点之间的图像偏差，以及特征直线当前角度和特征直线期望角度之间的偏差；

步骤S2：根据在线提取的所述目标图像的特征和目标空间尺寸，得到当前像素当量；

步骤S3：根据所述图像偏差和当前像素当量，得到当前位姿和目标位姿在视觉传感器敏感的三个自由度的偏差；

步骤S4：根据所述三个激光测距仪的读数及它们的相对位置关系，离线得到所述三个激光测距仪构成的平面的法向量期望值，在线得到当前平面法向量，将当前法向量和期望法向量分别投影到视觉不敏感的两个旋转自由度对应的平面上，并根据投影到各平面的分量，得到当前姿态和期望姿态在对应自由度上的角度偏差；

步骤S5：根据所述三个激光测距仪的读数得到当前位姿和期望位姿在深度方向的偏差；

步骤S6：结合从所述目标图像得到的三个自由度的偏差和从激光测距仪读数得到的三个自由度的偏差，以及当前位姿，得到目标的六自由度信息，实现目标的位姿检测。

2.根据权利要求1所述的基于视觉传感器和激光测距仪的位姿检测方法，其中所述视觉传感器安装在便于获取目标物体图像的位置，所述三个激光测距仪呈等腰三角形排列。

3.根据权利要求1所述的基于视觉传感器和激光测距仪的位姿检测方法，其中步骤S1中所述特征点的当前图像偏差和特征直线的当前角度偏差是通过下式来计算的：

其中，Δu、Δv分别表示特征点在U方向和V方向的图像坐标偏差，θ_z表示特征直线角度偏差，(u_d，v_d)表示特征点期望图像坐标，(u，v)表示特征点当前图像坐标，θ_d表示特征直线期望角度，θ表示特征直线当前角度。

4.根据权利要求1所述的基于视觉传感器和激光测距仪的位姿检测方法，其中步骤S2中所述的当前像素当量是通过下式来计算的：

t_s＝S/S₀；

其中，t_S表示当前的像素当量，S表示目标在图像中的尺寸，S₀表示目标的空间尺寸。

5.根据权利要求1所述的基于视觉传感器和激光测距仪的位姿检测方法，其中步骤S3中所述的当前位姿和目标位姿在视觉传感器敏感的三个自由度的偏差表示如下：

其中，Δx表示当前位姿和期望位姿在X方向的位置偏差，Δy表示当前位姿和期望位姿在X方向的位置偏差，Δθ_z表示当前位姿和期望位姿在绕光轴方向的旋转角度偏差。

6.根据权利要求1所述的基于视觉传感器和激光测距仪的位姿检测方法，其中步骤S4中所述的当前位姿和目标位姿在视觉传感器不敏感的两个自由度的偏差表示如下：

其中，Δθ_x表示当前位姿和目标位姿在绕X轴方向的旋转角度偏差，Δθ_y示当前位姿和目标位姿在绕Y轴方向的旋转角度偏差，V_qyoz和V_qzox是激光平面的期望法向量V_q分别在YOZ平面和ZOX平面的投影，V_yox和V_zox是激光平面法向量V分别在YOZ平面和ZOX平面的投影。

7.根据权利要求1所述的基于视觉传感器和激光测距仪的位姿检测方法，其中步骤S5中所述的当前位姿和目标位姿在视觉传感器不敏感的深度方向上的位置偏差表示如下：

Δz＝(d₁+d₂+d₃)/3；

其中，Δz表示当前位姿和目标位姿在Z方向的位置偏差，d_i(i＝1，2，3)表示三个激光测距仪的读数。

8.根据权利要求1所述的基于视觉传感器和激光测距仪的位姿检测方法，其中所述的目标位姿表示如下：

其中，[x，y，z，θ_x，θ_y，θ_z]表示通过视觉传感器和激光测距仪检测得到的目标位姿，[x₀，y₀，z₀，θ_x0，θ_y0，θ_z0]表示当前位姿，[Δx，Δy，Δz，Δθ_x，Δθ_y，Δθ_z]表示当前位姿和目标位姿之间的偏差，[Δx，Δy，Δθ_z]是通过视觉图像得到的位姿偏差，视觉传感器对这三个自由度的位姿变化敏感，[Δz，Δθ_x，Δθ_y]是根据激光测距仪的测量值得到的位姿偏差，通过激光测距仪来得到视觉传感器敏感的三个自由度之外的位姿信息。

9.一种基于视觉传感器和激光测距仪的位姿检测装置，包括一个视觉传感器、三个激光测距仪和控制单元，其中所述视觉传感器安装在便于获取目标物体图像的位置，所述三个激光测距仪呈等腰三角形排列，以及

所述控制单元执行如权利要求1至8任意一项所述的基于视觉传感器和激光测距仪的位姿检测方法来控制所述视觉传感器和激光测距仪的输入，并计算得到目标的六自由度信息，实现目标的位姿检测。

10.根据权利要求9所述的基于视觉传感器和激光测距仪的位姿检测装置，其中所述视觉传感器为AVT GC1600H，激光测距仪为CASTAL PT50220S。