CN103522291B - 一种排爆机器人的目标抓取系统及方法 - Google Patents

Abstract

本发明提出了一种排爆机器人的目标抓取系统及方法，所述系统包含：位姿获取机构，用于测量激光测距传感器发射到目标物体上的激光束的长度值，并通过云台的旋转和俯仰运动采集所述激光束的旋转角和俯仰角，进而通过建立机器人运动学方程获得所述激光束到达目标物体上的激光点的三维坐标；信息处理单元，用于根据目标在位姿获取机构底座上的坐标信息，以及位姿获取机构和机械手的相对位置关系，计算目标物相对于机械手的位姿；机械手，用于基于目标相对于机械手的位姿信息抓取目标；机器人机体，用于固定机械手和位姿获取机构；其中，所述激光测距传感器固定于云台上，且所述激光测距传感器能够与云台同步旋转和俯仰。

Description

一种排爆机器人的目标抓取系统及方法

技术领域

本发明涉及自动控制、无线传输、遥操作、激光测距、空间坐标变换和机器人运动学技术，尤其涉及一种能够实现类圆柱体目标位姿获取的测量机构，该机构固定于排爆机器人机体，与排爆机器人的机械手构成确定的位置关系，从而通过坐标变换计算出目标物相对于机械手的位姿，引导机械手实现自主抓取。

背景技术

随着国际反恐形势的日趋严峻和反恐斗争的深入，危险弹药处理与搬运已成为世界各国和军队共同关注的课题。由于危险弹药的保险状态、质量状况不明、放置的位置与位姿随机性较大，搬运过程中随时有爆炸的可能，因此，采用排爆机器人进行弹药处理作业，降低危险弹药的处理难度，避免工作人员的伤亡，对提高弹药处理技术水平和作业效率具有重要意义。世界上已有的排爆机器人包括履带式“手推车”、“超级手推车”MPR-800型多功能智能机器人、威霸龙排爆机器人、安德鲁斯HD-I机器人、Rode排爆机器人、“灵蜥A”和“灵蜥B”排爆机器人、RAPTOR-EOD中型排爆机器人等。目前，这些排爆机器人普遍存在目标位姿获取和自主抓取的智能化程度不够、作业效率不高、对操作人员要求高等问题。危险弹药等类圆柱体目标的位姿获取是实现排弹机械手路径规划与自主抓取的基础和关键环节，已成为当前制约排爆机器人作业效率的“瓶颈”环节之一。

目前，国内外已有十余种排爆机器人，对弹药等类圆柱体目标的位姿获取主要通过以下途径实现：

（1）通过多个不同角度摄像头的2D图像信息，依靠人的目测与经验进行机械手末端执行器的控制。该方法对操作者的经验和心理素质要求较高，末端执行器的位置与实际偏差不能满足需求，抓取效果不够理想；由于该方法无法获得目标物的位姿信息，因此不能实现自主抓取。

（2）在手爪或腕部等部位安装测距传感器，辅助自主抓取。这种方法能够得到机器人手爪与抓取目标之间的距离信息，但无法得到其姿态信息，由于测距传感器安装在机械手上，对准目标物的操作存在一定困难，其初始位姿误差受到机械手多关节累积误差影响较大，需要在抓取过程中进行多次测量和调整；虽然该方法无法引导机器人的自主抓取，但能够在一定程度上消除操作人员的紧张程度。

（3）采用双目立体视觉系统，进行位姿确定，作为机械手自主抓取闭环控制的反馈环节，实现自动抓取和路径规划。该方法由于需要进行摄像机标定、目标图像分割处理、抓取点分析等处理，算法复杂，精度不够理想，特别是在复杂背景中提取出目标物并准确识别较为困难，对操作人员要求也较高。

发明内容

本发明的目的是提供一种用于排爆机器人的类圆柱体目标位姿获取机构，采用该机构获取弹药等目标物的位姿，通过该机构与机械手的相对坐标关系，计算出目标物相对于机械手的位姿，从而实现自主抓取。

为实现上述目的，本发明提供了一种排爆机器人的目标抓取系统，所述系统包含：

位姿获取机构，用于测量激光测距传感器发射到目标物体上的激光束的长度值，并通过云台的旋转和俯仰运动采集所述激光束的旋转角和俯仰角，进而通过运动学方程获得所述激光束到达目标物体上的激光点的三维坐标；

信息处理单元，用于根据目标在位姿获取机构底座上的坐标信息，以及位姿获取机构和机械手的相对位置关系，计算目标物相对于机械手的位姿；

机械手，用于基于目标相对于机械手的位姿信息抓取目标；

机器人机体，用于固定机械手和位姿获取机构；

其中，所述激光测距传感器固定于云台上，且所述激光测距传感器能够与云台同步旋转和俯仰。

上述系统还包含：

后端控制台，用于依据得到的双目视觉图像，进而操作者遥控机器人行进到达作业区域，并通过发送控制指令实现自主抓取；和

无线传输设备，用于实现后端控制台和机器人之间的音频、视频、控制信号和数据的传输。主要包括音频和控制信号的双向传输，将双路视频信号、云台角度反馈值和激光测距反馈数值传输到后端控制台。

上述位姿获取机构从下向上依次包含：

底座，用于将云台固定于机器人机体上，且底座所在的位置为位姿获取机构的坐标基准；

云台，用于为激光测距传感器发出的激光提供旋转和俯仰两个旋转自由度，并能够反馈该云台的旋转角和俯仰角的值；

调节机构，该调节机构固定在云台上，能够相对于云台进行角度和位置微调；通过该调节机构，还能够调整激光测距传感器和双目摄像机相对于云台的安装位置关系并能够紧固；

双目摄像机，安装于调节机构上方，用于采集机械手的作业区域的双目视觉图像，并将采集的双目视觉图像传输到后端控制台，从而引导遥操作位姿获取和目标自主抓取；和

激光测距传感器，安装在云台上，用于测量发射到目标物体上的激光束的长度。

其中，本发明还可以通过如下策略调整上述云台、双目摄像机和激光测距传感器之间的安装位置：

以云台旋转角度为0时的前向为X坐标方向，旋转轴向上为Z坐标方向，通过调节机构进行角度微调、双目摄像机和激光测距传感器的预紧，使得双目摄像机视线方向、激光测距传感器发出的激光束虚拟连杆均与X坐标轴方向平行；

在云台旋转角度和俯仰角度均控制为0的初始状态，记录两个摄像机中心点、激光测距器发射点构成的安装三角形三个边长；沿X坐标轴方向任意取平行于YZ平面的一个平面，在该平面上标定出双目摄像机两幅图像的中心点和激光测距传感器发出的光斑点，则三点构成一个三角形；取另一个平行于YZ平面的平面，采用上述同样的方法构成三角形；若上述三个三角形均为全等三角形，则调节完毕，此时若再取不同的平行于YZ平面的平面进行验证，应仍能构成全等三角形；若不全等，则重新调节，逐步接近全等；

其中，调整误差可用测量三角形与安装三角形最大边长差值和YZ平面与云台距离的比值来表示。

基于上述抓取系统，本发明还提供了一种排爆机器人的目标抓取方法，所述方法包含：

步骤101）固定坐标系在云台的底座上，以云台正面为X轴正向，垂直向上为Z轴正向，以底座与云台旋转轴交点为坐标原点，建立坐标系；

步骤102）D-H参数获取，具体为：当机构参数包括：云台水平旋转角度θ₁与俯仰角度θ₂，云台俯仰中心距坐标系原点Z向距离d₁,激光传感器与云台俯仰中心Z向距离d₂，激光束虚拟连杆距离参数d₃，激光传感器相对于云台旋转中心Y向侧移距离m，则该位姿获取机构的D-H参数为：

D-H参数表

	αi-1	ai-1	di	θi	变量
						1	0	0	d1	θ1	θ1
2	-90	0	m	θ2	θ2
						3	0	d3	d2	0	d3

步骤103）根据D-H参数表，建立本机构的机器人运动学方程，可得激光束在目标物体上的光斑点的三维坐标：

$T=\left[\begin{array}{cccc}c{\mathrm{\θ}}_{1}c{\mathrm{\θ}}_2& -c{\mathrm{\θ}}_{1}s{\mathrm{\θ}}_2& -s{\mathrm{\θ}}_1& c{\mathrm{\θ}}_{1}c{\mathrm{\θ}}_2{d}_3-c{\mathrm{\θ}}_{1}s{\mathrm{\θ}}_2{d}_2-s{\mathrm{\θ}}_{1}m\\ s{\mathrm{\θ}}_{1}c{\mathrm{\θ}}_2& -s{\mathrm{\θ}}_{1}s{\mathrm{\θ}}_2& c{\mathrm{\θ}}_1& s{\mathrm{\θ}}_{1}c{\mathrm{\θ}}_2{d}_3-s{\mathrm{\θ}}_{1}s{\mathrm{\θ}}_2{d}_2+c{\mathrm{\θ}}_{1}m\\ -s{\mathrm{\θ}}_2& -c{\mathrm{\θ}}_2& 0& -s{\mathrm{\θ}}_2{d}_3-c{\mathrm{\θ}}_2{d}_2+d_1\\ 0& 0& 0& 1\end{array}\right]$

因此,激光束在目标物体上的光斑点坐标为：

步骤104）根据目标物相对于位姿获取机构底座的位姿信息，以及位姿获取机构和机械手的相对位置关系，计算目标物相对于机械手的位姿。

当目标物体为类圆柱体时，所述光斑点坐标采用如下方法得到：

首先，以云台正面为X轴正向，垂直向上为Z轴正向，以测量机构底座与云台旋转轴交点为坐标原点，建立坐标系，并建立本机构的机器人运动学方程，可得：

$\left\{\begin{array}{c}{P}_x=(l_0\sin \mathrm{\β}+l\cos \mathrm{\β})\cos \mathrm{\α}\\ P_y=-(l_0\sin \mathrm{\β}+l\cos \mathrm{\β})\sin \mathrm{\α}\\ P_z=l_1+l_0\cos \mathrm{\β}-l\sin \mathrm{\β}\end{array}\right.$ 式（1）

通过式（1）计算P_A,P_B点坐标，进而确定两点的中点位置和两点确定的直线，从而确定类圆柱体被抓取物的姿态；

以A、B两点的中点C为抓取目标点，其坐标为：

$\left\{\begin{array}{c}{x}_c=(x_A+x_B)/2\\ y_c=(y_A+y_B)/2\\ z_c=(z_A+z_B)/2\end{array}\right.$ 式（2）

给定X、Y、Z坐标，还能反求C点对应的距离、云台水平角度与俯仰角度，具体公式如下：

$\left\{\begin{array}{c}l=\sqrt{x^2+y^2+{(z-l_1)}^2-l_0^2}\\ \mathrm{\β}=\mathrm{ta}{\stackrel{-}{n}}^1\left(\frac{l_0}{l}\right)-\mathrm{si}{\stackrel{-}{n}}^1\left(\frac{z-l_1}{\sqrt{l_0^2+l^2}}\right)\\ \mathrm{\α}=-\mathrm{ta}{\stackrel{-}{n}}^1\left(\frac{y}{x}\right)\end{array}\right.$ 式（3）

至此，获得到了抓取点的位姿信息。

与现有技术相比，本发明的技术优势在于：

在本发明提供的位姿获取机构与机械手相对坐标系确定情况下,可直接获取被抓取目标的位姿,自动化程度高、作业效率高；与双目视觉和无线视频传输结合十分方便，可通过遥操作、临场感操作实现目标抓取；在爆炸波及范围外进行遥操作，可大幅度降低操作人员的紧张程度和疲劳度。

附图说明

图1是本机构的总体组成与结构示意图。

图2是位姿获取机构组成结构与测量原理示意图。

图3是位姿获取机构的安装参数示意图。

图4是实施例的系统框图。

附图标识：

1、排爆机器人机体 2、位姿获取机构 3、机械手

4、类圆柱体目标 5、无线传输设备 6、后端控制台

2-1、底座 2-2云台 2-3、调节机构

2-4、双目摄像机 2-5、激光测距传感器 2-6激光束虚拟连杆（即，照射目标的激光束）

具体实施方式

下面结合附图对本发明进行详细的描述。

本发明包括排爆机器人机体1、位姿获取机构2、机械手3无线传输设备5和后端控制台6，如图1所示。

所述排爆机器人机体1是能够实现行走并容纳控制系统、驱动系统、无线传输设备、电源等设备的机器人主体；

所述位姿获取机构2是固定安装在机器人机体1上，能够接受来自机体1的控制指令并能将位姿测量结果反馈给机器人控制系统的测量机构；

所述机械手3固定安装在机器人机体1上，能够接受来自机体1的控制指令并实现目标抓取的执行机构；

所述类圆柱体目标4是指具有爆炸危险的弹药等类圆柱体目标物；

所述无线传输设备5是指为了保证操作人员安全，实现排爆机器人机体1与后端控制台6分离，具有音频、视频、控制信号和数据无线传输功能的设备；

所述后端控制台6是通过无线传输设备5与机体1进行通讯、位于爆炸安全距离外的操作台，能够实现立体视觉显示和遥操作位姿获取、自主抓取。

本发明包括的位姿获取机构2由底座2-1、具有旋转和俯仰光电编码器角度反馈的云台2-2、调节机构2-3、双目摄像机2-4、激光测距传感器2-5和激光束虚拟连杆2-6组成，如图2所示。

所述底座2-1是位姿获取机构2的支撑座，通过该底座将位姿获取机构2固连在排爆机器人机体1上，也是位姿获取机构2的坐标基准；

所述具有俯仰和旋转光电编码器角度反馈的云台2-2是能够通过直接操作或遥操作控制其旋转和俯仰角度，并能够将两个角度值反馈到控制端6的云台装置；

所述调节机构2-3是固定在云台2-2上，能够相对于云台进行角度和位置微调的装置。通过调节机构，调整激光测距传感器2-3和双目摄像机2-5相对于云台安装位置关系并能够紧固。

所述双目摄像机2-4是固定安装在调节机构2-3上，能够随云台旋转和俯仰，可获取图像信号并通过无线传输设备5发送到后端控制台6，产生立体视觉；

所述激光测距传感器2-5是固定安装在调节机构2-3上，能够随云台旋转和俯仰，并能够通过发射激光束进行距离测量的装置；

所述激光束虚拟连杆2-6是指由激光测距传感器2-3发射出的、光斑点落在目标物的激光束，相当于一个长度可伸缩的虚拟连杆，其长度可通过激光测距传感器2-3反馈值获得。

如图1所示，排爆机器人实现自主抓取作业的过程为：（1）通过双目摄像机2-5传输到后端控制台6的双目视觉图像，操作者遥控机器人行进，到达作业区域；（2）通过后端控制台6对位姿获取机构2的控制，实现对类圆柱体目标物4的位姿信息获取；（3）根据目标物相对于位姿获取机构2的位姿信息，以及位姿获取机构2和机械手3的相对位置关系，计算出目标物4相对于机械手3的位姿；（4）机器人进行路径规划，实现自主抓取。

基于上述系统本发明还提供一种排爆机器人的目标抓取方法，用于抓取任意形状的目标，所述方法包含：

步骤101）固定坐标系在云台的底座上，以云台正面为X轴正向，垂直向上为Z轴正向，以底座与云台旋转轴交点为坐标原点，建立坐标系；

步骤102）D-H参数获取，具体为：机构参数包括云台水平旋转角度θ₁与俯仰角度θ₂，云台俯仰中心距坐标系原点Z向距离d₁,激光传感器与云台俯仰中心Z向距离d₂，激光束虚拟连杆距离参数d₃，激光传感器相对于云台旋转中心Y向侧移距离m，则该位姿获取机构的D-H参数为：

D-H参数表

	αi-1	ai-1	di	θi	变量
						1	0	0	d1	θ1	θ1
2	-90	0	m	θ2	θ2
						3	0	d3	d2	0	d3

步骤103）根据D-H参数表，建立本机构的机器人运动学方程，可得激光束在目标物体上的光斑点的三维坐标：

$T=\left[\begin{array}{cccc}c{\mathrm{\θ}}_{1}c{\mathrm{\θ}}_2& -c{\mathrm{\θ}}_{1}s{\mathrm{\θ}}_2& -s{\mathrm{\θ}}_1& c{\mathrm{\θ}}_{1}c{\mathrm{\θ}}_2{d}_3-c{\mathrm{\θ}}_{1}s{\mathrm{\θ}}_2{d}_2-s{\mathrm{\θ}}_{1}m\\ s{\mathrm{\θ}}_{1}c{\mathrm{\θ}}_2& -s{\mathrm{\θ}}_{1}s{\mathrm{\θ}}_2& c{\mathrm{\θ}}_1& s{\mathrm{\θ}}_{1}c{\mathrm{\θ}}_2{d}_3-s{\mathrm{\θ}}_{1}s{\mathrm{\θ}}_2{d}_2+c{\mathrm{\θ}}_{1}m\\ -s{\mathrm{\θ}}_2& -c{\mathrm{\θ}}_2& 0& -s{\mathrm{\θ}}_2{d}_3-c{\mathrm{\θ}}_2{d}_2+d_1\\ 0& 0& 0& 1\end{array}\right]$

因此,激光束在目标物体上的光斑点坐标为：

步骤104）根据两个不重合目标点相对于位姿获取机构底座的三维坐标信息，以及位姿获取机构和机械手的相对位置关系，计算目标物相对于机械手的位姿。

以下对本发明的位姿获取机构2中的双目摄像机2-4、激光测距传感器2-5的初始安装位置调节过程进行详细的描述。

如图2所示，以云台旋转角度为0时的前向为X坐标方向，旋转轴向上为Z坐标方向，通过调节机构2-3进行角度微调、双目摄像机2-4和激光测距传感器2-5的预紧，使得双目摄像机视线方向、激光测距传感器2-5发出的激光束虚拟连杆2-6均与X坐标轴方向平行。调节精度可采用本发明提出的多平行面全等三角形法进行核定和验证。

以下对多平行面全等三角形方法的调整过程进行详细描述：在云台2-2旋转角度和俯仰角度均控制为0的初始状态，记录两个摄像机中心点、激光测距器发射点构成的安装三角形三个边长；沿X坐标轴方向任意取平行于YZ平面的一个平面，在该平面上标定出双目摄像机两幅图像的中心点和激光测距传感器2-5发出的光斑点，则三点构成一个三角形；取另一个平行于YZ平面的平面，采用上述同样的方法构成三角形。若上述三个三角形均为全等三角形，则调节完毕，此时若再取不同的平行于YZ平面的平面进行验证，应仍能构成全等三角形；若不全等，则重新调节，逐步接近全等。调整误差可用测量三角形与安装三角形最大边长差值和YZ平面与云台距离的比值来表示。例测量三角形与安装三角形最大边长误差为2毫米，YZ平面与云台的距离为2米，则调整误差为0.1%。

以下对本发明的位姿获取机构2实现目标物位姿测量的运行过程进行描述。

如图2和图1所示，本发明包括的位姿获取机构2由底座2-1、具有旋转和俯仰光电编码器角度反馈的云台2-2、调节机构2-3、双目摄像机2-4、激光测距传感器2-5和激光束虚拟连杆2-6组成。

测量机构底座2-1安装在机器人机体1上，云台2-2固定安装在测量机构底座2-1上，双目摄像机2-4和激光测距传感器2-5通过调节机构2-3紧固在云台2-2上，可随云台旋转和俯仰。类圆柱体目标4放置在激光测距传感器可探测范围内的任意位置，使得激光测距传感器发出的激光束虚拟连杆2-6的光斑能够落在类圆柱体目标4上。

如图2和图3所示,固定坐标系在云台底座，被测物（即，被抓取物）为类圆柱体，通过测量被测物体的两个位置点P_A,P_B对应的激光束虚拟连杆距离信息l_A,l_B，根据云台分别对应的方位信息（包括水平角度α_A,α_B与俯仰角度β_A,β_B），结合激光测距传感器相对于云台坐标系的位置信息（云台俯仰中心距坐标系原点Z向距离为l₁,激光传感器与云台俯仰中心Z向距离为l₀），就可以计算出被测物体的两个位置点相对于云台坐标系的三维坐标(x_A,y_A,z_A)与(x_B,y_B,z_B)，从而计算出目标物体相对于云台坐标系的位姿。具体求解如下：

首先，以云台正面为X轴正向，垂直向上为Z轴正向，以测量机构底座与云台旋转轴交点为坐标原点，建立坐标系，并建立本机构的机器人运动学方程，可得：

$\left\{\begin{array}{c}{P}_x=(l_0\sin \mathrm{\β}+l\cos \mathrm{\β})\cos \mathrm{\α}\\ P_y=-(l_0\sin \mathrm{\β}+l\cos \mathrm{\β})\sin \mathrm{\α}\\ P_z=l_1+l_0\cos \mathrm{\β}-l\sin \mathrm{\β}\end{array}\right.$ 式（1）

通过式（1）可计算出P_A,P_B点坐标，就可以确定两点的中点位置和两点确定的直线，从而确定类圆柱体被抓取物的姿态。

以A、B两点的中点C为抓取目标点，其坐标为： $\left\{\begin{array}{c}{x}_c=(x_A+x_B)/2\\ y_c=(y_A+y_B)/2\\ z_c=(z_A+z_B)/2\end{array}\right.$ 式（2）

给定X、Y、Z坐标，还可反求C点对应的距离、云台水平角度与俯仰角度：

$\left\{\begin{array}{c}l=\sqrt{x^2+y^2+{(z-l_1)}^2-l_0^2}\\ \mathrm{\β}=\mathrm{ta}{\stackrel{-}{n}}^1\left(\frac{l_0}{l}\right)-\mathrm{si}{\stackrel{-}{n}}^1\left(\frac{z-l_1}{\sqrt{l_0^2+l^2}}\right)\\ \mathrm{\α}=-\mathrm{ta}{\stackrel{-}{n}}^1\left(\frac{y}{x}\right)\end{array}\right.$ 式（3）

至此，就得到了抓取点的位姿。

实施例

本实施例中，采用了FY-SP2515型室外数字云台（定位精度为±0.1°）、DLS-C30型激光测距传感器、两个日立VK-S888N型一体化摄像机、无线数字音视频传输系统等设备搭建位姿获取机构，系统框图如图4所示。实现了双目视觉引导下的遥操作三维空间点位置测量、类圆柱体位姿获取，并引导机械手进行自主抓取。将双目视觉技术与本位姿获取机构相结合，还可以实现双目视觉测距与虚拟连杆位姿测量的有机结合。采用本机构原理的实施例，在机械手可达的0.2～2米范围内，目标位置综合误差小于2毫米，空间姿态角度误差小于1度，完全满足实际需求，应用效果良好。

总之，本发明将双目立体摄像机、无线遥操作云台、激光测距传感器等设备综合应用于类圆柱体目标位姿获取，可以遥操作实现目标位姿获取，从而引导机械手自主抓取，自动化程度和效率较高，代表了工业机器人位姿获取的一个发展方向。

最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (1)

1.一种用于调整排爆机器人的目标抓取系统的方法，所述方法通过如下策略调整云台、双目摄像机和激光测距传感器之间的安装位置：

以云台旋转角度为0时的前向为X坐标方向，旋转轴向上为Z坐标方向，通过调节机构进行角度微调、双目摄像机和激光测距传感器的预紧，使得双目摄像机视线方向、激光测距传感器发出的激光束虚拟连杆均与X坐标轴方向平行；

在云台旋转角度和俯仰角度均控制为0的初始状态，记录两个摄像机中心点、激光测距传感器发射点构成的安装三角形三个边长；沿X坐标轴方向任意取平行于YZ平面的一个平面，在该平面上标定出双目摄像机两幅图像的中心点和激光测距传感器发出的光斑点，则三点构成一个三角形；取另一个平行于YZ平面的平面，采用上述同样的方法构成三角形；若上述三个三角形均为全等三角形，则调节完毕，此时若再取不同的平行于YZ平面的平面进行验证，应仍能构成全等三角形；若不全等，则重新调节，逐步接近全等；

其中，调整误差可用测量三角形与安装三角形最大边长差值和YZ平面与云台距离的比值来表示。