基于虚拟连杆机构的类圆柱体目标位姿获取

罗磊，陈恳，马振书，穆希辉

(1. 清华大学 机械工程系，北京100084;2. 总装备部 军械技术研究所，河北 石家庄050000)

0 引言

随着国际反恐形势的日趋严峻和反恐斗争的深入，世界各国和军队十分关注危险弹药处理问题。由于危险弹药的保险状态、质量状况不明、放置的位置与姿态随机性较大，搬运过程中随时有爆炸的可能［1］，因此，采用危险弹药处理机器人进行弹药处理作业，降低危险弹药的处理难度，避免工作人员的伤亡，对提高弹药处理技术水平和作业效率具有重要意义［2-3］。国内外一些公司、大学及研究院所，如美国的Remotec、iRobot，英国的Allen、ABP，德国的Telerob，中国科学院沈阳自动化研究所，上海交通大学，华南理工大学等，都对排爆机器人相关技术进行了大量研究，并取得了突破性的进展，在不适于人类作业的危险环境作业、爆炸物处理过程中，均发挥了重要作用。时至今日，国际上已有数十种排爆机器人问世［4-5］，但普遍存在目标位姿获取和自主抓取的智能化程度不够、作业效率不高、对操作人员要求高等问题。

1 虚拟连杆位姿测量机构设计

作为被抓取物通常是弹药弹丸等类圆柱体目标的排爆机器人，其目标位姿确定是实现机械手路径规划与自主抓取的基础和关键环节［6］。

1.1 类圆柱体目标位姿获取研究现状

目前，排爆机器人对类圆柱体目标的位姿获取主要通过以下途径实现:

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

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

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

最新研究进展表明，将具有角度反馈的云台和激光测距传感器综合应用于位姿测量［9-10］，甚至用于物体三维表面形状的粗检测是可行的［11］，该方法不但有效，而且定位效率高。

1.2 虚拟连杆位姿测量机构设计

位姿获取机构主要由测量机构底座、一个具有两个转动自由度的数字云台(含两个角度编码器)和一个激光测距传感器组成。在本机构设计中，将激光测距传感器发出的光线作为虚拟连杆，实现位姿测量，如图1 所示。

图1 虚拟连杆位姿测量机构Fig.1 Structure and constitution of measurement mechanism

如图2 所示，该位姿获取机构2 的底座通常与机器人本体1 固联，从而与机械手3 建立相对固定的坐标关系。由于排爆机器人通常需要远程遥控，为了将激光斑点打到需要的位置，通常需要在云台上安装双目摄像头。为了实现危险场所的远距离作业和操纵的方便性，后端控制台6 通常位于安全距离外，通过无线传输设备5 进行云台的转动和俯仰遥操作控制。云台的旋转和俯仰角度、激光测距传感器的数值等信息可以得到实时反馈，从而得到类圆柱体目标4 相对于位姿获取机构的位姿信息。位姿获取机构和机械手均固联在机器人机体上，其相对位置固定，所以可通过坐标变换得到抓取目标相对于机械手的位姿，从而实现自主抓取。

该位姿获取机构可看作是具有两个旋转自由度(即云台旋转和俯仰)和一个伸缩连杆的机械臂(虚拟连杆)。两个角度可以通过云台的角度编码器反馈的数值确定，连杆长度可通过激光测距传感器数值确定。因此，落在被测目标物上的光斑的三维空间位置坐标可以通过该机构的运动学方程求得。

图2 位姿测量机构与排爆机器人的结合应用Fig.2 Measurement mechanism and EOD robot

显然，一个光斑点可以确定三维空间位置，两个不重合的光斑点可以确定一条直线，3 个不在一条直线上的光斑点可以确定一个平面。对于类圆柱体目标物，其轴向旋转自由度可以不加限制。也就是说，用两个光斑点就能够确定弹丸等类圆柱体目标物的空间位置和姿态。

2 位姿获取原理与计算

2.1 机构运动学方程建立

1)建立坐标系:固定坐标系在云台的底座上，以云台正面为y 轴正向，垂直向上为z 轴正向，以底座与云台旋转轴交点O 为坐标原点，建立坐标系;

2)D-H 参数:机构参数包括云台水平旋转角度θ1与俯仰角度θ2，云台俯仰中心距坐标系原点z 向距离d1，激光传感器与云台俯仰中心z 向距离d2，激光束虚拟连杆距离参数d3，激光传感器相对于云台旋转中心x 向侧移距离m，如图3 所示。该位姿获取机构的D-H 参数见表1。

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

图3 位姿测量机构参数Fig.3 Structure parameters of measurement mechanism

表1 位姿测量机构的D-H 参数表Tab.1 D-H parameters of measurement mechanism

则有

可得光斑点坐标为

当机构确定时，d1、d2、m 为定值。(1)式、(2)式中的参数d3、θ1、θ2值由激光测距传感器、云台角度编码器反馈值确定。

2.2 姿态计算

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

设实际测量得到的A、B 两点对应的水平角度θ1、俯仰角度θ2和激光测距d4值分别为(θ1A，θ2A，d4A)、(θ1B，θ2B，d4B)，代入(2)式可得，两点相对于测量坐标系的坐标值PA(xA，yA，zA)、PB(xB，yB，zB)。

则有

转化为单位长度矢量:

该矢量表示了目标物轴线的姿态。

2.3 位置计算

A、B 两点的中点D 在目标物轴线上的投影C点为机械手自主抓取中心，求解C 点坐标。显然，C点位于OAB 平面内，距离实测A、B 点相等且距离AB 线段的中点D 距离为目标物的半径R，如图4 所示。

图4 位置计算示意图Fig.4 Position calculation

即满足:

满足上述条件的解有两个，应取y 坐标较大的一个解。

2.4 反向验证

为确保操作的安全性，通常在抓取前需对位姿测量结果进行验证。对于排爆机器人，通常取两个测量点的中点进行验证。其方法是:通过反向计算中点对应的云台水平角度、俯仰角度和激光测距值，将云台转动到相应角度，观察激光束斑点是否落在期望位置。

若已知期望点坐标P(x，y，z)，机构参数d1、d2、d3、m，则任意空间点对应的机构参数可通过以下公式计算:

3 应用实例

3.1 测量平台搭建

采用FY-SP2515 型室外数字云台、DLS-C 型激光测距传感器、两个日立VK-S888N 型一体化摄像机、无线数字音视频传输系统等设备进行系统搭建，该系统由现场数据采集子系统、无线传输子系统、远程监控子系统三部分组成，如图5 所示。由于排爆机器人作业过程中有发生爆炸的可能，因此需要在视觉图像引导下进行遥操作测量。通过该系统，可实现双目视觉引导下的三维空间点遥操作位置测量、类圆柱体位姿获取，进而引导机械手进行自主抓取。

建立的测量机构结构参数为d1=326.3 mm，d2=266.3 mm，d3=d0+d4，其中:d0是指激光测距传感器前端平面(发射点)到云台俯仰轴垂直平面的水平距离，是测距距离的偏置量，为安装参数(60 mm);d4为激光测距传感器反馈数值。激光光轴偏离俯仰轴中心的距离m 为0 mm，θ1取值范围为0 ～2π rad，θ2取值范围为

3.2 位姿测量与误差

图5 应用系统总体结构图Fig.5 Overall structure of practical system

3.2.1 初始变换与位姿理论值将口径为152 mm(R=76 mm)某型弹药弹丸垂直放置，然后绕y 轴旋转1.329 rad，再绕x 轴旋转0.64 rad，最后进行的平移量为(- 72.70 mm，1 447.90 mm，-967.10 mm). 此时z 向单位矢量(0，0，1)经过以上变换后对应的标矢为nz=(0.971，-0.143，0.192);该型弹丸两个定心位置高度分别为125 mm、304 mm，则其中点C 坐标为PC(0 mm，0 mm，214.50 mm)，经旋转和平移变换后理论坐标值为 PCt(- 20.90 mm，1 440.90 mm，-958.30 mm)。

3.2.2 姿态测量与误差

经过上述坐标变换后，对其轴向两处直径为152 mm 的部位进行测量，并使得激光束与弹药轴线基本相交(允许存在一定误差)，如图1 所示。实际测得的A、B 两点对应的水平角度θ1、俯仰角度θ2和激光测距d4值分别为(2.36°，42.45°，1 550.80 mm)、(355.22°，42.80°，1 596.20 mm)，代入(2)式可得，两点相对于测量坐标系的坐标为PA(56.34 mm，1 367.13 mm，- 890.71 mm)，PB(-116.34 mm，1 391.28 mm，-929.90 mm)。

代入(3)式、(4)式得AB = (172.68 mm，-24.15 mm，39.19 mm)，n = (0.966，- 0.135，0.219).

3.2.3 位置测量与误差

将A、B 点坐标值代入(5)式，并取y 值较大解得实测值:PCm= (- 12.37 mm，1 442.06 mm，-949.23 mm).

位置误差:δ=PCt-PCm=(-8.53 mm，-1.16 mm，-9.07 mm).

位置综合误差:|δ| =12.50 mm.

3.3 误差分析

1)实测姿态误差为2.87%，1 400 mm 测量距离时的位置综合误差为12.50 mm，完全能够满足排爆机器人的使用要求。

2)影响测量的因素主要包括:激光测距传感器和云台反馈角度误差，属系统误差;理论位置和姿态通过其他测量手段获得，具有一定误差;激光束延长线不通过弹药轴线，是误差主要因素，属偶然误差;弹药表面的锈蚀也会带来测量误差。

3)减小误差的措施:提高激光测距传感器精度;通过操作训练，使得激光束延长线偏离弹药轴线较少;可通过多次测量取平均值，降低偶然误差。

4 结论

本文设计的基于虚拟连杆的目标位姿获取机构，结构简单，测量精度较高，算法易于编程实现。该机构和遥操作、临场感等技术相结合，具有良好的应用前景与广阔的发展空间。通过该机构能够获取弹药弹丸等类圆柱体目标物的位姿信息，可直接用于排爆机器人机械手的路径规划与自主抓取，能够极大提高排爆机器人的作业效率和智能化程度。

References)

［1］孔新立，金丰年，蒋美蓉. 恐怖爆炸袭击方式及规模分析［J］.爆破，2007，24(3):88 -92.KONG Xin-li，JIN Feng-nian，JIANG Mei-rong. Analysis of way and scale of terroristic raid［J］. Blasting，2007，24(3):88 -92.(in Chinese)

［2］范路桥，姚锡凡，祁亨年. 排爆机器人的研究现状及其关键技术［J］.机床与液压，2008，36(6):139 -143.FAN Lu-qiao，YAO Xi-fan，QI Heng-nian. Research progress and key techniques of explosive ordnance disposal robot［J］. Machine Tool ＆ Hydraulics，2008，36(6):139 -143. (in Chinese)

［3］金周英，白英.我国机器人发展的政策研究报告［J］.机器人技术与应用，2009(2):1 -7.JIN Zhou-ying，BAI Ying. Policy research report of robot development［J］. Robot Technique and Application，2009(2):1 -7.(in Chinese)

［4］Dakis，Ann. Human presence detection:the navy has tri-service responsibility for EOD-related science and technology development［J］.CHIPS，2011，29(4):50 -52.

［5］Jean，Grace V. New robots planned for bomb disposal teams［J］.National Defense，2011，96(692):28 -31.

［6］Zuniga A L A，Pedraza O J C，Gorrostieta E，et al. Design and manufacture of a mobile robot applied to the manipulation of explosives［C］∥Proceedings of the 11th IEEE International Conference on Power Electronics Congress，CIEP 2008. Morelos，Mexico:IEEE，2008:84 -89.

［7］Zeng Jian-jun，Yang Ru-qing，Zhang Wei-jun. Research on semiautomatic bomb fetching for an EOD robot［J］. International Journal of Advanced Robotic Systems，2008，4(2):247 -252.

［8］Gonzalez-Galvan，Emilio J，Cesar A. Precise industrial robot positioning and path-tracking over large surfaces using non-calibrated vision［C］∥2011 IEEE International Conference on Robotics and Automation. Shanghai，China:IEEE，2011:5160 -5166.

［9］Ming-Chih Lu，Chen-Chien Hsu，Yin-Yu Lu. Image-based system for measuring objects on an oblique plane and its applications in 2-D localization［J］. IEEE Sensors Journal，2012，12(6):2249 -2261.

［10］Tom Epton，Adam Hoover. Improving odometry using a controlled point laser［J］. Autonomous Robots，2012(32):165 -172.

［11］Harvent J，Coudrin B，Brethes L，et al. Multi-view dense 3D modelling of untextured objects from a moving projector-cameras system［J］. Machine Vision and Applications，2013，24(8):1645 -1659.