破拆机器人机械臂自主运动控制系统软件设计*

何 锋，章小建，赵江海

(1．常州先进制造技术研究所 机器人系统实验室， 江苏 常州 213164;2.中国科学院合肥物质科学研究院先进制造技术研究所 机器人系统实验室，江苏 常州 213164)

破拆机器人机械臂自主运动控制系统软件设计*

何 锋1，2，章小建1，2，赵江海1，2

(1．常州先进制造技术研究所 机器人系统实验室， 江苏 常州 213164;2.中国科学院合肥物质科学研究院先进制造技术研究所 机器人系统实验室，江苏 常州 213164)

针对现有破拆机器人手工定位时间长、定位不精确的问题，研发了一个基于激光定位的破拆机器人机械臂自主运动的控制系统软件。设计了运动学模块、闭环反馈调节模块、液压装置控制模块、手眼标定模块和运动控制模块。实际使用效果表明，该软件系统能完成控制系统设计指标，提高工作效率。

破拆机器人；运动学；自主运动

0 引言

目前，施工现场作业的破拆机器人采用手动方式进行作业[1]。由于破拆机器人机械臂运动过程产生的惯性力大，操作人员手工操控需要反复调整才能运动到大致位置，因此，研发一种破拆机器人机械臂自主运动控制系统，设计该控制系统的上位软件，来实现机械臂目标导引的自主运动控制[2]功能是十分有必要的。

本文设计了控制系统软件的各个模块，给出了各模块的设计方法，最后，通过实验证明了控制系统软件能完成破拆机器人机械臂目标导引[3]的自主控制功能。

1 破拆机器人总体硬件架构

图1 破拆机器人整机实物图

本文实验平台在惊天液压股份公司的 GTRC-15 型破拆机器人基础上开展功能部件研究，机器人整机实物图见图1。由图1可看出，机器人由机械臂、移动回转平台、行走履带和作业支架组成。机械臂主要由大臂、二臂、三臂和末端执行器构成。回转平台由回转马达驱动且安装有角度编码器以获得当前回转平台偏置角度，机械臂的各个关节由液压缸驱动，安装了角度编码器传感器，可检测当前关节角的角度值。行走履带使用液压马达驱动，可以驱使机器人进行前进、后退、旋转等动作。机器人的通信使用 CAN 总线模式[4]。控制系统主要包括机械臂各关节的电液比例控制系统、无线视频采集与传输系统、激光定位平台控制系统及机器人主控PC。电液比例控制系统主要是通过控制比例阀阀口开度大小来控制液压缸体内液体的流速，而流速与液压缸体的运动速度及关节角的即时速度成近似线性关系。无线视频采集与传输系统是用于操作员远离作业现场环境下，通过远程无线视频来观察定位作业点，进而自动驱动激光定位平台找到作业目标点，使液压机械臂自动运动到指定目标位置。

2 破拆机器人软件设计

破拆机器人的控制系统软件选用的平台是 Windows XP 系统，程序运行框架是用 VC6 的 MFC 类库开发。在控制系统中设计了运动学计算模块、闭环反馈调节模块、液压装置控制模块、运动控制模块、手眼标定模块、无线视频采集与处理模块等。控制系统软件界面如图2。

图2 破拆机器人控制系统软件界面

破拆机器人激光定位下机械臂自主运动整体控制系统工作原理为：操作员通过远程视频监控系统观察作业现场，并根据现场环境确定所要破碎的目标点，然后操作激光定位云台进行俯仰和旋转运动，直至激光点落在定位的目标点位置上。此时控制系统根据激光定位平台在两个方向上旋转的角度和激光测距仪当前的读数，运用前面给出的激光云台坐标系求解公式很容易计算出作业目标点在激光定位平台坐标系下的空间坐标，该坐标通过刚性转置可转换为机械臂坐标系下的作业目标的空间坐标值。通过运动学逆解[5]或在线轨迹规划方法[6]将空间运动量转为关节运动输出量，进而驱动液压执行元件使机械臂实现多轴联动动作，快速准确到达指定破拆目标。

自主破拆的算法步骤为：(1)根据标定装置测出的作业目标距离，经过旋转及偏置后得到视觉云台坐标系下三维坐标。激光测距传感器安装在云台坐标系下的Y轴原点，且激光器发射方向与Y轴正向一致，若当前激光测距传感器读数是dm，则作业目标在第五坐标系下的坐标表示为 [0,d,0]。(2)通过基坐标系与末端执行器坐标系之间的齐次转换矩阵计算获取激光测距系统下的目标物体坐标。(3)通过运动学逆解将第一坐标系下目标点坐标及设定的末端执行器位姿转换为液压机械臂五个关节角的角度值。(4)通过关节角与液压缸长度的映射关系得到液压缸运动的目标长度。(5)使用 PID 算法控制液压元件的速度信号，使其快速准确地到达目标点，完成破拆定位。

3 运动学模块设计

运动学模块主要包括关节角求解部分、正运动学求解部分和逆运动学求解部分[7]。其中关节角求解部分主要完成液压缸的变化量与关节角变化量的线性映射关系的建立。正运动学求解部分完成各关节角的当前值与末端破碎锤的空间坐标和姿态的对应关系建立。逆运动学求解部分完成在指定目标位置和指定末端破碎锤姿态的情况下求出满足能量最优条件下[8]的各关节角的目标值。通过运动学模块设计，可以在线获得当前机械臂的姿态和末端执行器的位置，也能根据指定目标点和末端破碎锤姿态求得各关节角的目标值[9]，并将目标值转化为液压缸运动的长度。

4 闭环反馈调节模块设计

闭环反馈调节模块主要是用于对各液压缸的位置运动误差进行调节。使用PID控制方法进行闭环调节，PID控制参数通过经验法进行设计，获取合适的各液压缸的PID控制参数。通过PID闭环反馈调节后，每个液压缸关节的位置运动误差在0.01 mm以内。PID控制模型图见图3。

图3 PID控制结构图

5 液压装置控制模块设计

液压装置控制模块通过CAN总线与各关节的电液比例控制系统进行通信，控制各个液压缸的阀门开度、运动方向和运动速度。通过液压装置控制模块设计，可以控制机械臂各关节的运动。

6 手眼标定模块设计

控制系统软件使用激光定位平台获取视觉的坐标系点，为将视觉坐标系和机械臂坐标系进行整合，需要进行手眼标定模块设计，手眼标定模块使用最小二乘算法来计算两坐标系之间的齐次变换矩阵。通过齐次变换矩阵可以将视觉坐标系的目标坐标和目标姿态转化为机械臂坐标系下的坐标，进一步通过运动学模块求出各液压缸的变化量，通过液压装置控制模块和闭环反馈调节模块运动到指定的目标位置[10]。

7 运动控制模块设计

运动控制模块主要完成机械臂的多轴运动控制，在软件中使用了三次样条曲线插值的方法，将逆解求出的各关节角目标值、各关节编码器反馈的各关节角当前值和约定的整体运动时间作为插值输入参量代入，求得每个定时器周期下各关节角的位置、速度和加速度值。利用液压缸的运动特性，进行机械臂多轴运动控制。

8 结论

使用本文设计的控制系统软件，通过激光定位装置获取6组目标点，取末端破碎锤与地面垂直的姿态为最终姿态，通过实验数据表1可发现定位误差在5 cm内，定位时间为10 s，且可调。通过实验可证明该控制系统软件满足了破拆机器人机械臂运动自动化的目标。

表1 控制系统软件实验数据

[1] 刘庆运. 破拆救援机器人关键技术[J]. 现代制造工程, 2009,30(7):149-153.

[2] 杨政, 尚建忠，王彪，等. 液压驱动机械臂的轨迹规划[J].

机械研究与应用,2009,25(4):47-51.

[3] 周孟然,吴永祥.基于红外激光定位技术的矿井提升机位置跟踪系统的研究[J].煤炭学报,2002,27(6):658-660.

[4] 胡敏,梁聪慧，郑庆华，等.多关节液压臂架实时控制软件的设计与实现[J].中国工程机械学报,2013,11(3):243-247.

[5] 蔡自兴. 机器人学[M]. 北京：清华大学出版社, 2000.

[6] CRAIG J. Introduction to robotics:mechanics and control (third edition)[M].Pearson Education,Inc.,2005.

[7] 陆震. 冗余自由度机器人原理及应用[M]. 北京：机械工业出版社,2006.

[8] XIANG J, ZHONG C, WEI W. A varied weights method for the kinematic control of redundant manipulators with multiple constraints[J]. IEEE Transactions on Robotics, 2012,28(2):330-340.

[9] CARRILER W F, KHOSLA P K， KROGH B H. Path planning for mobile manipulators for multiple task exceution[J]. IEEE Transactions on Robotics and Automation, 1991,7(3):403-408.

[10] FINZEL R, HELDUSER S, JANG D S. Electro-hydraulic control systems for mobile machinery with low energy consumption[C].Proceeings of the Seventh International Conference on Fluid Power Transmission and Control, 2009:214-219.

Software design of demolish robot manipulator autonomous motion control system

He Feng1，2，Zhang Xiaojian1，2，Zhao Jianghai1，2

(1.Robotic Lab, Institute of Advanced Manufacturing Technology, Changzhou 213164, China;2.Robotic Lab, Institute of Advanced Manufacturing Technology, Hefei Institutes of Physical Science, Chinese Academy of Sciences, Changzhou 213164，China)

Aiming at the problems that the existing demolish robot has long manual positioning time and inaccurate positioning, the control system software of the demolish manipulator autonomous motion which based on laser-positioning is given. The paper has designed the kinematic model, close-loop model, hydraulic device controller model, hand-eye calibration model and motion controller model. The application results show that the software system can complete the design indexes of the control system and improve the work efficiency.

demolish robot; kinematic; autonomous motion

国家科技支撑计划(2015BAK06B02)；江苏省产学研合作计划(BY2016040-02)

TP273

A

10.19358/j.issn.1674- 7720.2017.09.030

何锋，章小建，赵江海.破拆机器人机械臂自主运动控制系统软件设计[J].微型机与应用，2017,36(9)：103-105.

2016-12-14)

何锋(1977-)，男，硕士，工程师，主要研究方向：机器人。