关键词：排爆机器人 运动控制 双目视觉定位 MATLAB/RTW
Abstract：A control system scheme of a Bomb-disposal Robot based on the binocular stereo vision location and navigation is proposed in this paper. The principle of the binocular vision location and the design of a motion control system are described. Realtime control system of the robot based on xPC target through the MATLAB/RTW run successfully in test prototype furnished with PC/104, and satisfactory result is obtained.
Keywords：Bomb-disposal Robot Motion Control Binocular Vision Location MatLab/RTW
1. 前言
排爆机器人作为一种特殊环境下使用的机器人在很多国家都已开展了研制，有的已有产品面世，但大都是为一种遥控机器人，抓起目标物的过程中需要人的干预，由于有多个关节需要协调控制，且抓起目标物时的定位精度通常在5mm以下，故操作者没有长期的训练是难于胜任的。本文提出了一种基于双目视觉定位导航的手眼协调系统来完成自动控制抓起目标物的过程，该控制方案在实验样机上得到了验证，取得了较满意的结果。
2. 系统设计
排爆机器人由机械系统、双目视觉定位系统和运动控制系统三部分组成，可对目标物进行准确测距定位并自动完成抓取功能。
2.1机械结构
机器人整体结构分为车体和机械手两部分，车体两侧各安装一个电机，采用链传动实现四轮驱动，通过控制两侧电机的转速和转向可使小车实现任意半径的转弯。
本机器人机械手具有5个自由度，分别对应为腰、肩、肘、腕关节和手爪转位关节，其结构示意图见图1。各关节由直流电机驱动，均采用位置闭环伺服控制。两摄像头安装在小臂上，为机器人双目视觉。
图1 机械手机构示意图
2.2双目视觉定位和手眼协调
双目视觉定位[2]系统是由安装在小臂上的两个摄像头摄取同一目标物两幅图像，利用三角测距原理，经图像匹配和计算得出目标物与机械手坐标系的精确相对位置。通过手爪运动轨迹规划使得手爪在接近目标物的过程中，摄像头一直对准目标物，利用这种手眼协调方式以使目标物始终处于图像监控之下;然后在手爪离目标物较近的位置（手爪路径规划时确定）时再进行一次更为精确的测距，最后准确抓取目标。整个抓取过程由机器人控制系统自动完成。
2.2.1三角测距原理
如果已知两台摄像枪的几何位置，且同时知道同一物体在2个摄像枪中的成像位置，便可以利用三角原理计算物体在空间的位置，即通过三角测距法获取深度信息。
如图2所示。考虑最简单的情形，L和R分别为2个参数相同的针孔摄像枪，f为2个摄像枪的焦距，L和R光轴平行，与X轴重合，Y轴垂直于纸面。以L的摄像枪坐标系为世界坐标系，R的原点或投影中心的偏移量为b，b成为立体视觉系统的基线。目标点为P，P在左右摄像枪中投影的x坐标分别为x1,x2。根据空间几何的原理，有如下公式：
已知（x1,y1）,（x2,y2），焦距f和基线长度b即可以得到P点的三维坐标（X，Y，Z）
图2 三角测距示意图
2.3运动控制系统
机器人运动控制系统采用全数字式伺服方式，即除电机驱动及以后的部分为硬件外，其余的如插补算法、比较单元、控制器等均数字化、软件化后由计算机来完成。其突出优点是可灵活运用各种复杂的控制规律，并降低硬件成本。
2.3.1手爪路径规划和插补
要完成手眼协调控制，即在自动抓取目标物的前期（手爪接近目标物）摄像头一直对准目标物，则从手爪起始点到精确测距点的轨迹应为一直线。为实现手爪近似水平伸向目标物，从精确测距点到最终抓取目标点应为另一直线，故手爪路径由两段直线组成。
手爪路径规划在直角坐标空间[3]进行，手爪设计移动速度为50mm/s，由于机器人手爪运动轨迹精度要求并不高，故采用ΔL=5mm作定距插补得到密集化的数据系列，相当于粗插补周期100ms。得到一系列目标点后利用逆运动学算法算出各关节角度系列，各关节采用位置闭环伺服控制达到所要求的角度，功能框图如图3所示。由于机械手为开链悬臂结构，为了使各手臂运动平稳，必须对手臂运动进行速度控制，故在关节空间进一步插补作速度规划，插补速度曲线采用梯形速度曲线模式，插补周期为10ms，控制规律采用数字PID算法，电机由PWM波调速，PWM波由软件产生，周期为0.5ms，即PWM频率为2kHz。另外，若反馈环节开路或出现故障会造成偏差过大甚至越来越大，这就使得手臂过速及到达目标位置而不能停止，故为安全起见，增加了偏差检测保护模块。各关节闭环位置控制系统框图见图4。
图3 机械手运动控制系统框图
图4 各关节位置闭环伺服控制系统框图
2.3.2 逆运动学算法的简化
五个自由度以上的逆运动学算法[4]复杂，要求计算机的计算时间长。从工程实现的角度考虑完全可以适当简化，按最通常的情况，手爪是水平抓取目标物，而手爪的转动方位取决于目标物抓起时的着力位置，抓起目标时可先让腰关节转动使手爪对准目标物，这样处理实际上就在逆运动算法时减少了三个自由度，只剩下两个自由度即两个关节角度需要计算，大大简化了计算过程，减少了计算机的处理时间。另外，由于位置反馈使用的是增量式编码器，所以开机后机械手各关节必须确定一个位置（置位位置），该位置时写入可逆计数器一个确定值，各关节角度示意如图1。
2.3.3 xPC目标实时控制系统
本机器人运动控制系统是基于MATLAB/RTW[1]（Real-Time Workshop）的xPC目标[5]实时控制系统。xPC目标采用了宿主机-目标机的技术途径，宿主机用于运行Simulink[6]，建立控制模型，生成目标代码后下载至目标机。宿主机还运行控制界面程序和发送操作控制命令。目标PC机则用于执行所生成的代码。xPC提供了一个高度减缩型的实时操作核，运行于目标PC机上。本机器人采用了PC/104作为实时目标机，配合一块I/O扩展卡作编码器信号输入和电机控制信号输出。
图5为控制系统局部Simulink模型，由视觉系统传送来目标点先经验算是否可及，不可及则报错，可及则计算分配腰关节应转过的角度和其他关节经手爪轨迹规划（粗插补）得到应转过的角度，角度均对应为编码器的脉冲数，目标脉冲数与编码器反馈脉冲数之差经数字PID算法输出至PWM波产生模块，PWM波输出至电机驱动器驱动直流电机。图6为机器人现场抓取一水瓶照片。
图5 局部控制Simulink模型
图6 现场照片
3.结束语
本设计创新点在于把双目视觉定位系统应用于机器人目标导航抓取，其定位精度高，抓取时无需人的干预，抓取目标精度达到5mm以内。控制系统是基于MATLAB/RTW生成的xPC目标实时系统，该方法开发周期短，易于调试，是控制系统产品开发和验证的一个有力工具。
另外，由于负载的变化，使得机械手臂在运动时某些位置会产生抖颤，速度变化不圆滑，这都有待于从控制规律（如增加前馈控制或采用自适用控制）和改善硬件（如采用高脉冲数的编码器、减小机械传动间隙等）等方面来解决。
4.参考文献
1.杨涤，李立涛，杨旭等编著 系统实时仿真开发环境与应用.北京：清华大出版社，2002;
2.Tsai R. Y An efficient and accurate camera calibration technique for 3D machine vision. In Proc. IEEE Conf, on Computer Vision and Pattern Recognition, Miami Beach. FL, 1986, 364374;
3.丛爽，李泽湘 编著 实用运动控制技术.北京：电子工业出版社，2006.1;
4.殷际英，何广平 编著 关节型机器人.北京：化学工业出版社，工业装备与信息工程出版中心，2003;
5.徐国政，陈勇 基于Matlab/xPC Target的数据采集系统.微计算机信息[J]，20051033，63-64;
6.李颖，朱伯立，张威 编 Simulink动态系统建模与仿真基础.西安：西安电子科技大学出版社，2004.7。
