钻锚机器人钻臂轨迹规划方法研究

雷孟宇，张旭辉,2，杨文娟,2，董 征，万继成，张 超，杜昱阳,2

(1.西安科技大学 机械工程学院，陕西 西安 710054；2.陕西省矿山机电装备智能监测重点实验室，陕西 西安 710054)

当前煤矿巷道支护主要依靠人工操作单体式钻机完成，支护效率低，劳动强度大，同时由于支护过程处于空顶距下作业，安全隐患大[1-3]。为提高巷道支护自动化程度，提升支护效率，多钻臂锚杆台车应用逐渐增多。针对多钻臂支护系统，合理的钻锚孔孔序规划策略和钻臂轨迹规划方法不仅符合作业规范的要求，还能够避免多钻臂相互干涉的影响，减少能源消耗和设备磨损，提升支护效率[4-5]。

钻锚孔孔序规划问题实质上是多目标多对象任务分配问题，通过合理的任务分配提高多对象协同工作效率。针对多钻臂孔序规划问题，国内外众多专家、学者对其进行了研究。目前常用的多任务多对象任务分配方法主要包括利用蚁群算法实现孔序规划[6-8]、遗传算法实现最优规划[9-11]、优化函数方法实现孔序规划[12]和匈牙利算法实现任务分配[13]等。优化函数方法简单地将炮孔空间位置的直线距离作为代价函数，将其最优解作为孔序规划结果，该方法并未完全考虑机械臂实际运动轨迹。蚁群算法实现孔序规划以最短路径或最短时间为目标进行优化得到最优解，提高了钻孔效率，但钻臂运行过程可能会发生碰撞等问题。匈牙利算法任务分配可实现效益矩阵最大化，但多机械臂系统可能存在轨迹干涉问题。以上研究主要集中在多钻机锚杆台车孔序规划和多任务分配方法，同时由于巷道支护工艺流程复杂，以上方法在煤矿巷道支护方面研究应用较少。

针对钻臂轨迹规划方法，众多学者进行了卓有成效的研究。基于改进人工势场法[14]、多项式插值法[15]、智能算法[16-19]和深度学习方法[20]的机械臂轨迹规划方法已经在分拣、协作等场景研究。基于智能算法的机械臂轨迹规划方法能够快速收敛，确定最优轨迹，基于深度学习方法的轨迹规划在连续变动任务实验中有更高的适应性和鲁棒性。以上研究集中在机械臂轨迹规划和最优轨迹优化方法，研究对象多为工业机械臂，未涉及到煤矿钻锚装备机械臂的轨迹规划。煤矿井下钻锚作业环境中，在钻孔顺序确定的基础上，合理的轨迹规划能够减少振动冲击，保证钻臂运动过程快速平滑，有效提升钻锚效率和智能化程度。

因此，提出一种集成悬臂式掘进机和多自由度机械臂的钻锚机器人，通过自主定位、钻锚孔自动寻孔等功能，完成适应一般地质条件的巷道自动支护作业。在机器人化掘锚一体机模型样机的基础上，针对钻臂结构进行运动学分析，通过蒙特卡罗方法计算钻臂运动空间，确定钻锚机器人有效作业范围；基于支护作业工艺要求提出钻锚孔孔序规划策略；利用五次多项式插值方法进行钻臂末端轨迹规划，以期为钻锚装备自动化、智能化奠定基础。

1 钻锚机器人结构及功能

巷道成形主要包括巷道掘进和巷道支护，当前巷道成形主要有3 种方式：悬臂式掘进机完成巷道截割，锚杆台车完成巷道支护作业，该种方式支护效率有所提升，但是对巷道宽度要求较高，同时悬臂式掘进机和锚杆台车来回折返较为耗时；连续采煤机配合锚杆钻机完成巷道截割和支护作业，该种方式截割和支护作业可同时作业，且效率较高，但是仅适用于地质条件较好的巷道；目前应用最多的还是悬臂式掘进机完成巷道截割，工人操作单体锚杆钻机完成巷道支护作业任务，该种操作方式支护效率低，安全性差。因此，巷道成形存在装备自动化程度低，工人劳动强度大，工作效率无法有效提升。

为解决以上难题，提出集掘进和支护流程于一体的钻锚机器人，如图1 所示。钻锚机器人主要由悬臂式掘进机和两个六自由度机械臂组成。悬臂式掘进机完成巷道掘进作业任务；两个六自由度机械臂分别集成在悬臂式掘进机机身两侧，该机械臂是针对煤矿巷道支护工艺及当前支护装备存在问题等专门设计的，两个钻臂结构相同，该钻臂包含5 个旋转关节和一个移动关节，前两个旋转关节和第3 个移动关节决定了钻机末端钻臂在空间中的大致位置，后3 个旋转关节类似于手腕关节，决定了钻机末端钻臂在空间中的姿态，实现钻机的对准，理论上可以到达空间任意一点。根据巷道尺寸不同选择机械臂，以完成自动钻锚作业。

图1 钻锚机器人结构Fig.1 Structure of anchor drilling robot

钻机作为末端执行器布置在钻臂末端，临时支护装置能够提供安全的作业环境，每个旋转关节和移动关节分别配备拉绳位移传感器和倾角仪，实时测量各个关节运动变化量。钻锚机器人配置多套视觉系统，能够实现钻锚孔识别定位及构成闭环的视觉伺服控制系统，有效提高支护装备自动化程度，提高支护效率。

钻锚孔间排距等支护参数根据巷道尺寸和地质条件确定，但在实际支护过程，由于顶板凹凸不平且存在施工误差，钻锚孔并不能完全严格按照设计的间排距排列，因此在钻锚机器人机身上布置一套双目视觉系统，用于采集钢带图像并确定钻锚孔中心空间坐标并实现钻锚机器人基于位置的视觉伺服控制，钻臂根据双目视觉系统反馈信息运动至目标位置，实现钻臂粗略控制；同时在钻机末端配置一套单目视觉系统，用于采集钻锚孔图像信息并实现钻臂基于图像的视觉伺服控制，钻臂根据视觉系统反馈信息实现钻臂的精确控制到达钻锚孔中心，完成钻孔及锚固作业任务。临时支护装置能够将钢带固定在顶板上，提供安全作业环境。

钻锚机器人系统方案为：构建钻锚机器人运动学模型，进行运动学分析；通过系统搭载的双目视觉测量系统识别钻锚孔，并确定钻锚孔中心空间坐标，将其作为目标位置，求解钻锚机器人各关节变化量；根据钻臂结构参数及钻锚孔排列方式，构建钻锚孔孔序规划策略；基于孔序规划策略完成双钻臂钻锚顺序，利用五次多项式插值方法完成轨迹规划；构建基于位置的视觉伺服控制系统实现钻臂的粗略运动控制和基于图像的视觉伺服控制系统实现钻臂的精确运动控制，使钻臂运动至目标位置，实现自动钻锚作业任务。钻锚机器人系统方案如图2 所示。

图2 钻锚机器人系统方案Fig.2 Scheme of anchor drilling robot system

2 钻机目标位置及钻臂关节变量分析

2.1 钻臂运动学建模

两个六自由度钻臂对称布置在钻锚机器人机身两侧，因此只需要选取一个钻臂进行研究分析。依据改进的DH 坐标系构建法则对此六自由度钻臂进行分析，首先找出六个关节轴，沿着关节轴i的方向为轴zi的方向，以关节轴i和i+1 的交点或公垂线与关节轴i的交点作为连杆坐标系xiyizi的原点；规定xi轴沿公垂线的指向，若关节轴i和i+1 相交，则xi轴垂直于关节轴i和i+1 所在的平面；最后按照右手定则确定yi轴的方向。图3 所示xiyizi坐标系为基于此原则建立的钻臂各个关节坐标系，其中x0y0z0坐标系与x1y1z1坐标系重合，固定在基座上。d3为连杆3 的偏移量，θ1—θ6为对应关节转动角度。

图3 基于改进DH 方法的机械臂运动坐标系Fig.3 Motion coordinate system of manipulator based on modified DH method

根据建立的DH 坐标系，得到六自由度钻臂的DH参数，见表1。

表1 六自由度钻臂DH 参数Table 1 D-H coordinate system parameters of the six-degreeof-freedom manipulator

2.2 钻机目标位置

基于改进的DH 坐标系构建方法建立的参数表及钻臂相应连杆的参数，通过相邻连杆的坐标系转换可得到钻臂末端执行器(钻机)相对于钻臂基坐标系的转换关系，即为钻臂正运动学模型，得到钻机目标位置相对钻臂基坐标系的空间位姿。坐标系{i}和坐标系{i-1}的转换关系可以用变换矩阵表示[21]：

将钻锚机器人钻臂DH 参数及对应连杆参数代入式(1)，可得：

经解算，末端执行器(钻机)坐标系相对于基坐标 系转换关系为：

其中：

式中：a和o分别为接近向量和方向向量；n=o×a为法向量，p为位置向量。单位正交向量n、o和a描述了钻机相对于钻臂基坐标系的姿态，p描述了钻机相对于钻臂基坐标系的位置。θ1、θ2、d3、θ4、θ5和 θ6分别表示钻臂5 个旋转变量和一个平移变量，根据以上分析，将各个变量对应值代入式(2)，即可得到钻机相对于钻臂基坐标系的空间位姿，通过坐标系转换可得到其相对于钻锚机器人机身坐标系空间位姿。钻锚机器人实际工作时通过自动定位模块获取钻锚孔空间位姿，求解钻臂各个关节变量，因此，还需研究到达空间某一点时钻臂各个关节变量。

2.3 钻臂关节变量

机械臂正运动学分析能够求解钻臂各关节在一定运动变化范围内，钻机在空间坐标系中的位姿；逆运动学分析主要是钻机空间位姿已知条件下求解各个关节运动量。本系统中，布置在钻机上的双目视觉模块能够实现钻锚孔空间位姿的解算，在此基础上求解钻臂逆运动学得到各关节运动变化量是实现钻臂自动控制、智能锚固的重点。

将式(2)含有θ1的部分移到方程左边，则有：

其中：

令式(3)左右两边元素(2,4)分别相等，则有：

利用三角恒等变换对上式进行处理可得：

同理，令式(3)两边元素(1,4)和(3,4)分别相等，则有：

对式(7)、式(8)进行整理可得：

将式(2)含有θ1、θ2和d3的部分移到方程左边，则有：

令等式(11)两边元素(2,3)、(1,3)、(3,3)、(2,1)和(2,2)分别相等，则有：

同理可解得：

通过以上分析可得双钻臂到达运动空间位置时各个关节变化量，但是到达空间一点各个关节变化量可能会存在多组解，在轨迹规划过程中应选取关节运动量最小或运动时间最短的路径点。

3 钻锚机器人双钻臂运动空间

工作空间是指机械臂末端执行器能到达空间点的集合，钻机的工作空间即为钻机所能到达的空间位置。《煤矿安全规程》和《采矿工程设计手册》明确规定了支护作业要求，其中包括：锚杆孔实际钻孔角度与设计角度偏差小于5°，因此，对于巷道顶板和侧帮支护时需要保证钻机垂直顶板平面或侧帮板平面，保证实际钻孔角度与设计角度在偏差范围内。蒙特卡罗方法是基于统计理论的数值计算方法，借助随机抽样来解决数学问题，基于蒙特卡罗方法利用Matlab 机器人工具箱解算该钻锚机器人双钻臂运动空间。

计算钻臂运动空间时，在各个关节变化范围内随机取值求解末端点在空间坐标系下位置，随机选取大量的采样点，尽可能构建出钻臂完整的运动空间。图4为蒙特卡罗方法求解钻臂运动空间方案，首先构建钻臂运动模型，设定随机次数为Q，设置各个关节变化量，机械臂各个关节变量 θi(di)取 值范围分别为 [θimin,θimax],利用随机函数rand 生成Q个[0,1]的随机点，以(θimax-θimin)∗rand为随机步长，求解各个关节变量的随机值，即：

图4 蒙特卡罗方法求解钻臂运动空间方案Fig.4 Scheme of manipulator workspace based on Monte Carlo method

将式(14)所得关节变量随机值代入式(3)即可求得钻机末端空间坐标，将此步骤重复Q次，即可得到双钻臂运动空间轮廓，其中，Q取值越大，运动空间轮廓越接近真实情况。钻臂各个关节取值范围见表2，随机次数Q取值为20 000 次，基于Matlab 仿真双钻臂钻锚机器人运动空间如图5 所示。

表2 六自由度钻臂各关节运动范围Table 2 Range of motion of each joint of six-degree-of-freedom manipulator

图5 钻臂运动空间Fig.5 Workspace of the manipulator

图5a 为钻锚机器人双钻臂在三维空间下运动空间，图中X轴、Y轴和Z轴分别表示沿巷道掘进方向、巷道宽度方向和巷道垂直高度方向，单位均为mm；图5b、图5c 分别为钻臂运动空间在XOZ和XOY平面的投影，钻锚机器人两钻臂平行分布在机身两侧，两钻臂之间距离1.2 m，由钻臂运动空间在XOZ和XOY平面的投影可以看出，钻锚机器人两钻臂针对巷道顶板锚固工作空间适用于巷道宽度不超过6 000 mm。钻臂布置在钻锚机器人机身上，距离巷道地面高度约2 m，工作过程机身受巷道底板情况影响，存在俯仰、横滚与航向角，但一般情况下其变化幅度不大，可近似认为钻锚机器人机身保持不变。侧帮支护过程中，低于钻锚机器人机身位置，钻臂与机身发生干涉无法垂直侧帮进行支护，侧帮支护工作高度区间为2 000～4 500 mm。因此，钻锚机器人可适用于巷道6 000 mm×4 500 mm 以内的巷道顶板和侧帮支护作业。

4 钻锚孔孔序规划及钻臂轨迹规划

4.1 钻锚孔孔序规划策略

孔序规划主要是将基于双目视觉的钻锚孔空间位姿信息及其排列信息传输至控制系统，系统根据钻锚孔空间位姿确定该钻锚孔位于左侧钻臂或右侧钻臂作业空间范围内，分配锚固作业任务给相应钻臂；根据钻臂起始位置和目标位置，基于笔者提出的五次多项式插值方法规划钻臂轨迹，控制各个关节运动，钻臂到达目标位置，完成锚固作业任务。

图6 所示为根据巷道结构尺寸构建双钻臂工作空间数学模型，XOY为煤矿巷道横截面绝对坐标系。钻臂在完成锚固作业任务时，可以分别独立完成各自工作空间范围内目标点的锚固作业，若锚固点为奇数，则中间锚固点最后由左侧钻臂完成锚固作业。为了高效快速完成锚固作业任务，钻臂需要协同作业。为避免两钻臂作业过程产生干涉，相互影响，需要考虑两钻臂协同控制问题，避免钻臂干涉，提高锚固效率。两钻臂对称分布在钻锚机器人机身两侧，两侧帮锚固作业任务分别由对应侧钻臂单独完成，协同控制模型主要针对顶板锚固作业任务，需要对左右钻臂完成顶板锚固作业点个数和作业顺序进行规划，使得两钻臂能够协同作业，高效完成巷道支护作业任务。

图6 双钻臂工作空间数学模型Fig.6 Mathematical model of workspace of two manipulator

按照规范要求，巷道支护需要先完成顶板支护再进行侧帮支护，其中顶板支护要求从中间向两帮进行顺序施工。因此，钻锚机器人两钻臂在锚固作业时，在顶板中间位置锚固作业时最容易发生干涉，相互影响，随着顶板锚固作业任务从中间向两帮进行，两钻臂距离越来越远，不会发生干涉。因此，主要研究巷道顶板中间位置附近锚固点协同作业任务问题，通过左右钻臂锚固点欧氏距离判断是否可能发生干涉。

图7 所示为机械臂作业次序，主要研究中间位置锚固点作业次序及分配原则，图7a 表示顶板锚固点数为奇数，图7b 表示顶板锚固点数为偶数。1—4 分别表示巷道左右侧帮从上往下钻锚孔序号，(i-1)—(i+3)分别为巷道顶板从左往右钻锚孔序号。假设相邻锚固点之间距离为d0，钻机工作半径为R，为保证两钻臂锚固作业过程不发生干涉，必须保证左右两钻机工作时，左右钻机的工作半径之和大于两个钻锚孔之间的距离，即：

图7 顶板锚固点孔序规划Fig.7 Drilling sequence planning for roof anchor points

式中：m为左右钻锚孔之间包含钻锚孔个数。

1)顶板锚固点为2i+1 个

当顶板锚固点为2i+1 时，若2R≤ 2d0，即R≤d0，则左侧钻臂从锚固点i开始向锚固点i-1 方向完成锚固作业，同时右侧钻臂从锚固点i+2 开始向锚固点i+3 方向完成锚固作业，最后由左侧钻臂完成顶板锚固点i+1 的锚固作业任务。

若2d0≤ 2R≤ 3d0，即d0≤R≤ 1.5d0，则左侧钻臂从锚固点i开始向锚固点i-1 方向完成锚固作业，同时右侧钻臂从锚固点i+3 开始向锚固点i+1 方向完成锚固作业，之后继续从锚固点i+4 向锚固点2i+1 完成顶板锚固作业任务；若2R≥ 3d0，则依此类推完成顶板锚固作业任务。

2)顶板锚固点为2i个

当顶板锚固点为2i时，若2R≤d0，即R≤ 0.5d0，则左侧钻臂从锚固点i开始向锚固点i-1 方向完成锚固作业，同时右侧钻臂从锚固点i+1 开始向锚固点i+2 方向完成锚固作业。

若d0≤ 2R≤ 2d0，即0.5d0≤R≤d0，则左侧钻臂从锚固点i开始向锚固点i-1 方向完成锚固作业，同时右侧钻臂从锚固点i+2 开始向锚固点i+1 方向完成锚固作业，之后继续从锚固点i+3 向锚固点2i完成顶板锚固作业任务。

若2d0≤ 2R≤ 3d0，即d0≤R≤ 1.5d0，则左侧钻臂从锚固点i开始向锚固点i-1 方向完成锚固作业，同时右侧钻臂从锚固点i+3 开始向锚固点i+1 方向完成锚固作业，之后继续从锚固点i+4 向锚固点2i完成顶板锚固作业任务；若2R≥ 3d0，则依此类推完成顶板锚固作业任务。在掘进工作面进行巷道掘进时，锚索与锚杆支护会交错进行，对于锚索的工作任务，其策略与锚杆一致，服从锚杆的最优匹配策略。

4.2 钻臂轨迹规划方法

轨迹规划是运动规划的主要内容，主要是通过在起始点和终点之间插入中间点序列，实现钻臂沿着轨迹平稳运动[22]。为保证钻臂从起始点到目标点运动轨迹平滑，速度和加速度无突变，三次多项式插值方法轨迹规划只能够保证速度和位移连续，并不能保证加速度连续，加速度突变可能会导致振动甚至冲击，因此，提出采用五次多项式插值方法进行钻臂轨迹规划，五次多项式插值方法解决了关节角速度变化不平滑且加速度存在跳变的情况。两钻臂对称布置在钻锚机器人机身两侧，因此，设定左右钻臂分别完成左右各部分顶板和侧帮锚固作业任务。本文以右侧钻臂为研究对象，研究钻臂轨迹规划问题。

设钻臂关节关于时间的运动函数为：

式中：ci为五次多项式的系数，其角速度和角加速度函数表达式分别为：

对初始位置和目标位置、角速度和角加速度代入式(17)和式(18)有：

式中：t0、tf分别为关节运动的起始时间和中止时间；s(t0)、s(tf)分别为关节的初始位置和目标位置。

将上式写成矩阵的形式，则有：

将上式记为AB=P，则B=A-1P，求解上式可得五次多项式各个系数。

5 实验结果与分析

5.1 基于数字孪生的钻锚孔孔序规划实验

采用SolidWorks 按比例构建钻锚机器人虚拟模型和煤矿巷道虚拟场景，建立钻锚机器人运动学模型，采用Unity3D 完成虚拟模型动作编程及虚拟模型与虚拟场景的耦合，实现基于数字孪生的钻锚孔孔序规划仿真实验。

在孔序规划策略基础上，根据钻机参数结构、钻锚孔个数及相对位置对钻锚孔进行任务分配及作业顺序规划。如图8 所示，钻锚机器人及巷道数字孪生模型中，钻机作业半径为0.8 m，每个钻锚孔间隔0.6 m。图8a 中，钢带上共计9 个钻锚孔，左侧钻臂完成钻锚孔1—5，钻锚顺序为4—3—2—1—5，右侧钻臂完成钻锚孔6—9，钻锚顺序为7—6—8—9；图8b 中，钢带上共计8 个钻锚孔，左侧钻臂完成钻锚孔1—4，钻锚顺序为4—3—2—1，右侧钻臂完成钻锚孔5—8，钻锚顺序为7—6—5—8。仿真结果表明，该孔序规划策略能避免两钻臂工作过程发生干涉，有效提升锚固作业效率。

图8 顶板锚固点孔序规划仿真结果Fig.8 Simulation results of drilling sequence planning for roof anchor points

5.2 基于五次多项式的钻臂轨迹规划仿真实验

在Matlab 中利用机器人工具箱对钻臂基于五次多项式插值方法进行轨迹规划仿真。设定其初始位置和目标位置，初始位置各个关节量分别为[-π/3,-π/3,0,-π/6,-π/2,-π/2]，对应的钻臂末端空间坐标为[250,-433,866]，目标位置各个关节量分别为[π/3,0,400,π/6,0,π/2]，对应的钻臂末端空间坐标为[700,1 212,0]。

图9 为基于五次多项式的钻臂轨迹规划所得轨迹、位移、速度、加速度曲线。图9a 红色曲线表示钻臂完成试验起点到终点的轨迹，轨迹平滑无波动；图9b 表示钻臂各个关节位移变化曲线，关节3 为移动关节，其位移量从0 逐渐连续平滑增加至400 mm，其余旋转关节均从起始位置关节角度连续平滑至目标位置各关节角度，整体连续平滑运动，各关节变量平稳运动无突变，说明两点之间6 个关节都能够平滑地进行轨迹运动，图9c 表示钻臂各个关节速度变化曲线，关节2 和关节4 速度变化情况一致，两条速度变化曲线重合，整体从起始点位置开始逐渐增大，各个关节速度到达峰值后逐渐减小，在目标位置时各个关节速度为零，整体均平稳变化，未发生速度突变现象，图9d 表示钻臂各个关节加速度变化曲线，其变化规律类似于正弦曲线，从起始点位置开始加速度逐渐增大，加速度到最大值之后逐渐减小，加速度减小为零时各个关节速度达到最大值，随着加速度继续减小，速度逐渐减小，其整体变化过程平稳未发生突变。基于五次多项式线性插值轨迹规划方法所得轨迹平滑，速度和加速度曲线连续、平滑变化说明机械臂在运动过程中不会产生速度突变和加速度突变现象，提高了钻臂运动效率。

5.3 结果分析

利用三次多项式插值方法对钻臂进行轨迹规划，所得结果如图10 所示。图10a 红色曲线表示钻臂末端执行器轨迹，图10b 表示钻臂各关节随时间变化情况，从轨迹规划起始点到目标点，各关节变量均呈连续 平滑增大，与五次多项式插值方法轨迹规划所得位移变化曲线图相似，图10c 表示钻臂各关节速度变化曲线图，关节2 和关节4 速度变化曲线一致，其余曲线均平滑增大，图10d 表示钻臂各关节加速度变化曲线，关节2 与关节4 加速度变化曲线一致，其余关节加速度均线性变化，变化速率一致，但各关节加速度在起始点和目标点加速度值均不为零，存在加速度突变情况。对比分析可知，基于五次多项式插值方法的轨迹规划所得位移、速度和加速度变化曲线均连续且平滑，在起始点和目标点加速度从零开始逐渐变化，无突变发生。

图10 基于三次多项式插值方法的轨迹规划Fig.10 Trajectory planning based on cubic polynomial interpolation method

6 结论

a.根据巷道支护工艺和钻锚孔数量等提出一种孔序规划策略，在运动学分析基础上提出采用五次多项式插值方法进行钻臂轨迹规划，仿真结果表明本文所述方法能有效避免双钻臂干涉，且钻臂能够快速平稳到达目标位置。

b.设计的钻锚机器人对巷道地质条件适应性强，钻臂集成在掘进机两侧能够减少掘进设备与支护设备交替调度时间。能够自动完成孔序规划和钻臂轨迹规划，提高设备自动化程度，减少支护作业人员数量。

c.钻锚机器人较强的环境适应性和较高的自动化程度，在各种地质环境的煤矿巷道有广阔的应用前景。后续研究工作可集中在钻锚机器人视觉伺服控制方法研究，将钻锚孔自动定位技术、孔序规划策略、轨迹规划方法和视觉伺服控制技术等结合起来构建智能支护系统，实现巷道支护全流程智能化，提高巷道支护效率。