朱明鎏 ，李 梁

（1.国家能源集团宁夏煤业公司 枣泉煤矿，宁夏 灵武 750499；2.中煤科工机器人科技有限公司，广东 深圳 518000）

现阶段，煤矿井下管道的常规安装仍是采用人力和辅助设备相结合的传统方法[1]。首先把起吊架固定在梭矿车上，用手拉葫芦起吊煤矿管道，然后工人爬到固定好的管路上进行连接施工。此种接管工艺完全依靠人力，导致安全性差，工作效率低，具有很大的制约性及安全隐患。煤矿抓管机器人是近年来兴起的一种新型的井下辅助作业机器人[2-3]，主要由履带式移动底盘搭载重载液压机械臂，通过车-臂协同控制，来实现管路的搬运、码放、安装、拆卸等多种功能，作业对象可以为煤矿的瓦斯管、水管、风管等。

机械臂作为抓管机器人目标物抓取作业的重要组成，其有效的连杆坐标系间的运动关系决定了机械臂的运动控制精度和平稳可靠运行。机械臂的逆运动学求解是已知末端坐标系在惯性参考系下的位置和姿态描述，求解各个关节的变量值。目前求机械臂运动学逆解的方法主要有解析法和数值法[4-6]。串联机械臂用逆运动学解析解的充分条件是满足Pieper 准则。解析法比数值方法求解精度高、速度快，而且容易区分所有可能解，是机器人逆运动学求解的首选方法。因此，一般只有在机器人构型不满足封闭解求解条件时，才会采用数值方法求解。对于数值解法来说，目前国内研究热点是设计基于优化算法的逆运动学求解算法，并取得了大量成果[7-10]。然而数值解法存在的主要问题是求解效率低、不稳定、可能求解失败等，显然与煤矿应用的高可靠性原则相悖。

为了适应煤矿管路安装作业的特点以及液压驱动机构的动作特性，管路抓取作业机械臂的结构设计无法满足Pieper 准则，因此不存在封闭解；另一方面，为了增加机械臂的操作灵活性，抓管机器人的液压机械臂还具有1 个冗余自由度，导致在逆运动学求解时存在无穷多种解。为了解决上述难题，基于固定角度法和搜索方法，设计了具有冗余自由度的煤矿抓管机器人液压机械臂的逆运动学解析解法，保证了求解过程的高可靠性、高效率和结果的准确性。同时，解析解因具有显式数学表达式，相比于雅克比矩阵法、遗传算法等数值算法更易于部署到单片机、PLC 等嵌入式控制器，极大地降低了算法部署的成本和难度。

1 煤矿抓管机器人

针对国内某煤矿辅助作业场景需求，研制出了一款煤矿抓管机器人。该机器人具有遥控行走、电子围栏等功能，能够有效辅助人员完成煤矿巷道内的不同规格尺寸的管道搬运、提升、安装等工序，且可根据现场实际情况选择半自主作业，远程遥控作业、人工就地作业等多种作业模式，可有效降低井下作业人员的人身安全，同时大大提高工作效率，实现了煤矿井下巷道危重场景全方位的智能化、无人化辅助作业，保障煤矿安全生产。

煤矿抓管机器人的系统构成如图1。

履带式移动底盘上搭载了抓管机械臂，抓管机械臂为液压机械臂，通过履带式移动底盘上搭载的液压泵站驱动。煤矿抓管机器人通过自身搭载的激光测距仪的引导实现在井下巷道的自主导航和运动，并且通过自身搭载的视觉系统对管道进行识别并引导抓管机械臂进行管道安装作业。

煤矿抓管机器人能够在空间狭小、倾角大等复杂井下条件下实现管道的安全、高效安装。抓管机械臂主要技术参数为：①抓管机械臂末端最大速度：0.1 m/s；②作业半径：3 m；③最大抓举质量：0.5 t；④机械臂自由度数量：6；⑤遥控距离：30 m；⑥管道安装精度：≤6 mm；⑦管道识别角度误差：≤±3°；⑧标志物识别率：≥95%。除了管路的抓取、搬运及安装等作业功能外，煤矿抓管机器人还具有如下功能：

1）避障功能。机器人搭载超声避障传感器，当传感器探测到障碍物时，机器人按照预先设定好的包含速度和加速度约束的轨迹平滑地实现停车。

2）通信功能。机器人通过无线路由器与本地设备的有线通信，通过OPC 系统实现了部分参数井上管控平台和本地通信。

3）路径规划导航功能。机器人搭载3D 激光测距传感器，通过纠偏算法实现移动过程中机器人本体与目标侧巷道壁的距离始终保持不变。

4）车辆姿态感知功能。机器人通过倾角传感器、编码器获得自身的当前位姿，超过安全限度即时报警。

5）视觉功能。机器人通过视觉传感器识别管路，完成管路坐标的识别，将坐标数据发送给控制系统。

6）语音提示功能。机器人具有语音提示功能，可以合理对故障及隐患进行预报警，提高作业时的安全性。

2 冗余液压机械臂逆运动学

机械臂的控制系统采用层级模块化设计，具备关节编码器信号采集、运动学解算、控制指令接收与下达及液压伺服控制等功能。工作原理具体为：控制系统采用机器视觉等传感技术获得目标物体的位姿，并将其通过以太网（TCP）发送给运动控制器，控制器自主完成轨迹规划；当机械臂末端处于轨迹上某点时，逆运动学算法将从编码器读取机械臂的当前关节角序列作为搜索算法的初始值，并根据轨迹上的下1 个运动点对应的末端位姿完成逆运动学解算，获得下1 个运动点对应的机械臂关节角序列，并将其发送给液压伺服控制系统来驱动机械臂末端运动到轨迹上的下1 点。

2.1 基于固定角度法的封闭解

机械臂的各关节旋转轴分布如图2，基于标准D-H 方法建立的机械臂连杆坐标系如图3。

图2 机械臂的各关节旋转轴分布Fig.2 Configuration of each joint rotation axis of robotic arm

图3 机械臂坐标系Fig.3 Robotic arm coordinate system

由图2 可知：液压机械臂共有6 个自由度，分别是基座绕垂直轴的旋转自由度（轴1）、4 个连杆绕关节轴的旋转自由度（轴2～轴5），以及机械臂末端的法兰盘旋转自由度（轴6）。

由图3 可知：机械臂各关节旋转角qj绕zj-1旋转，旋转方向遵守右手法则。记抓手中心点T相对于坐标系 {0}的 坐标为（xOT,yOT,zOT）， 即坐标系 {6}相对于 {0}的 平移。图中： θi为关节旋转角，是xi-1沿zi转向xi轴 的有向转角；di为 连杆偏距，是xi-1与zi的交点到连杆i坐标系的原点沿zi方向的有向距离；ai为 连杆长度，是zi轴 与zi+1轴 的公法线长度； αi为连杆扭转角，是zi轴 沿xi轴 转向zi+1轴的有向转角。

抓管机械臂的标准D-H 参数模型各参数为：①杆1：θ1=q1rad，d1=0.135 mm，a1=0.190 mm，α1=π/2；②杆2：θ2=q2rad，d2=0 mm，a2=1.150 mm，α2=p2rad；③杆3：θ3=q3rad，d3=0 mm，a3=1.450 mm，α3=0；④杆4：θ4=q4rad，d4=0 mm，a4=0.970 mm，α4=0；⑤杆5：θ5=（q5-π/2） rad，d5=0.466 mm，a5=0.138 mm，α1=-π/2；⑥杆6：θ6=q6rad，d6=0.215 mm，a6=0 mm，α6=0。

基于上述坐标系和D-H 模型参数，建立的煤矿抓管机器人的前向运动学公式如式（1）：

式中：0AT为从机器人机座到机械臂末端的坐标变换矩阵；0A1、1A2、2A3、3A4、4A5、5A6为4×4阶坐标变换矩阵。

令：sin（x）简写为Sx、 cos（x）简写为Cx（以下皆采用简写），则0A1、1A2、2A3、3A4、4A5、5A6坐标变换矩阵可表示为：

固定大臂旋转角，即令q2为已知量，对式（7）方程组进行求解得到封闭解如下：

2.2 基于搜索方法的逆运动学求解

逆运动学算法流程图如图4。

图4 逆运动学算法流程图Fig.4 Flow chart of inverse kinematics algorithm

逆运动学算法基本思想是固定大臂倾角q2，令其值等于运动前的初始值q2_original，假如由式（8）求得的封闭解满足机械臂的各个角度约束，则无需搜索新的q2，直接得到1 组解，供运动控制系统使用。简单来说就是在满足末端位姿要求时，先保持大臂不动，动其他臂。如果上述情况不成立，则在大臂倾角的初始值q2_original附近搜索1 个离其最近的q2，使得q1、q2、q3、q4、q5同时满足各自角度约束,并由（q1,q2,q3,q4,q5）构成1 组可行解。搜索步长为0.1°（步长可根据机器人现场调试效果在程序中调整）。简单来说，就是在保证大臂动得最小的前提下，求取封闭解，来使得满足末端位姿要求时各个运动轴的位移量总体来说尽量最小。

3 仿真与实验

基于上述给出的机械臂逆运动学求解公式和算法流程，采用MATLAB 进行了逆运动学算法的仿真验证。根据图4 所示算法流程图，编写出MATLAB 函数[ik_q1,ik_q2,ik_q3,ik_q4,ik_q5]=

pipe_grab_ikine（x_oT,y_oT,z_oT,beta_T,q2_original）。函数的输入参数为抓手中心点三维坐标x_oT,y_oT,z_oT、抓手倾角 beta_T及大臂倾角的初始值q2_original， 输出参数为各旋转轴的角度ik_q1，ik_q2，ik_q3，ik_q4，ik_q5。

假设目标末端位姿为T_in，当大臂倾角初始值为q2_original 时，通过逆运动学算法求得1 组逆解ik_q。再以逆解ik_q作为输入，进行前向运动学计算得到末端位姿T_ik。由仿真结果可知T_ik=T_in，即根据目标位姿进行逆运动学求解得到的关节角序列能够驱动机械臂末端运动到目标位姿，说明逆运动学算法有效。

基于该逆运动学函数，在笛卡尔空间内规划1 条直线轨迹，令机械臂末端从T1运动到T2，基于MATLAB 的机械臂逆运动学仿真实验如图5。图中的T1点与T2点之间的直线段为机械臂在仿真环境中实际划过的轨迹，说明基于本文设计的逆运动学算法，管路安装机器人能够实现机械臂末端的精准轨迹控制。

图5 基于MATLAB 的机械臂逆运动学仿真实验Fig.5 Inverse kinematics simulation experiment of robotic arm based on MATLAB

此外，该算法已在煤矿抓管机器人样机上部署，并在煤矿井下进行管路抓取作业测试。额定负载管路直径300 mm，质量500 kg，设定机械臂初始位姿为（1 860，900，0），目标位姿（650，0，0），进行机械臂重复运行管路抓取动作定位精度测试。试验结果显示：冗余机械臂可以实现重负载抓管操作，抓取过程中机械臂操控性良好，运行稳定、可靠，绝对误差最大值为7.81 mm，满足煤矿井下机械臂辅助作业精度设计要求。

4 结 语

针对现有煤矿井下管路安装作业需求痛点，完成了基于模块化技术的煤矿抓管机器人整体系统方案设计，并完成机器人的系统组成、核心参数和主要功能分析；研究了基于解析法的机械臂逆运动学求解算法，推导了冗余机械臂的逆运动学求解原理，给出了算法的具体实现流程；采用matlab 软件完成了冗余机械臂的运动学仿真模拟。仿真结果表明，逆运动学求解得到的关节角序列能够驱动机械臂末端运动到目标位姿，能够实现机械臂的高精度作业控制，验证了本研究算法的有效性。煤矿抓管机器人机械臂的运动控制精度测试结果表明：机械臂运行平稳、可靠，抓管精度绝对误差最大值为7.81 mm，满足设计需求。