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

权利要求书

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平面的投影。