锚杆钻车串并联冗余机械臂的运动学研究

孙晓宇, 王恒升,2, 郭新平, 贺昕

(1. 中南大学 机电工程学院,长沙 410083;2. 中南大学 高性能复杂制造国家重点实验室,长沙 410083)

锚杆钻车是煤炭开采过程中进行巷道支护的重要工具,支护自动化作业是当下煤炭开采的迫切需求[1-2]。锚杆钻车机械臂多是重型液压驱动的多关节机械臂,本文针对的是一种用于锚杆钻车的、具有串并联混合构型的8自由度冗余机械臂。这种机械臂整体上是串联开链机构,由于臂的整体长度较大,为了增加臂的刚度,在承载最大的大臂设置了一个四边形的并联结构;考虑到巷道的作业空间限制和操作过程的灵活性,设置了8自由度(5个转动关节、3个移动关节)的冗余构型。该构型的机械臂目前装配在多款锚杆钻车上,应用于煤矿巷道的支护作业,主要是人工手动操作,因刚度大、操作灵活而应用广泛。随着矿井自动化需求的日益迫切,对锚杆钻车的自动化升级及改造成为趋势。机械臂的运动学研究是机械臂自动控制首先要解决的问题[3-4]。这种具有优良性能的机械臂由于其结构的特殊性,为其运动学建模尤其是运动学的逆解带来了困难。本文针对这一难题展开研究,提出了有效的解决方案,该方法对实现锚杆钻车自动化施工有重要意义。

除了通用的工业机械臂,一般针对特殊应用的特种机械臂,其运动学问题必须单独进行分析研究[5-6]。针对冗余机械臂逆运动学,国外学者提出的基于雅可比矩阵的伪逆法与阻尼最小二乘法为经典通用的算法[7-8],在冗余机械臂逆运动学求解得到很多应用[9-10],但该方法不能有效利用机械臂本身的结构特性和作业过程的特殊性,势必不会是优选的方法,其求解效率也相对较低。启发式算法近来发展迅猛,可将冗余机械臂逆运动学转化为最优化问题进行求解。粒子群算法结构简单且效率高,被众多学者用于机械臂逆运动学的求解[11-12]。谢习华等[13]基于改进的差分进化算法,对9自由度凿岩机械臂进行逆运动学求解。人工神经网络可无限精度逼近任意复杂的非线性函数,有学者用来解决冗余机械臂逆运动学问题[14-15]。启发式算法求解逆运动学思路虽然简单,但难以保证全局最优且参数调整对求解结果影响较大,目前较多停留在仿真阶段。文献[16-17]分析双三角凿岩机械臂的空间平移性能,通过“先定点再定位姿”的方式,解决凿岩臂的快速定位问题。Wang等[18]基于对偶四元数并结合机械臂结构,通过姿态方程与位置方程之间进行迭代,提出了一种6自由度机械臂的运动学求逆方法,有效利用了机械臂结构。

针对本文的特种机械臂,首先对其中的并联结构进行了研究,用等效开链结构来代替局部并联部分,基于D-H法建立了机械臂的运动学方程;在运动学方程的逆解方面,充分分析了机械臂的关节构型、实际作业过程钻车车体与目标锚杆孔的位姿关系和不同锚杆孔之间臂的关节运动轨迹等方面,并充分考虑目前人工操作的直观性、方便性与便捷性,提出一种位姿分离的逆运动学求解方案,通过计算机仿真验证和在模拟巷道中锚杆钻车自动打孔定位的试验,证明本文方法在求解精度、求解效率和臂的目标姿态等方面具有很大的优势,完全可以应用于臂的实时定位控制中。

1 机械臂运动学建模

1.1 结构分析

锚杆钻车机械臂整体上为串联开链机构,分为大臂、中臂和小臂,其中大臂为四边形并联机构,图1所示为机械臂的装配示意图与结构简图。

图1 机械臂结构示意图

机械臂具体运动形式与液压缸的对应情况如下:机械臂回转油缸伸缩使臂在水平面作横摆运动;机械臂支撑油缸伸缩使臂作俯仰运动;机械臂伸缩油缸和支撑杆伸缩油缸长度相等同时伸缩时,大臂保持平行四边形并作平动伸缩;当伸缩油缸和支撑杆伸缩油缸长度不等差动时,会造成臂后半部分的摆动;回转座伸缩油缸使中臂伸长或缩短;摆动油缸1和2在空间作旋转运动,改变推进器的姿态。实际工作时,机械臂以合适的目标姿态将推进器定位孔瞄准目标锚杆孔,然后钻孔作业。

大臂部分有3个自由度,对应大臂俯仰、大臂两缸平动伸长与差动伸长这3种运动情况。机械臂除钻机推进油缸之外,其余每个液压缸对应一个自由度,因此共有8个自由度。

1.2 并联结构的运动等效

为便于使用D-H法建立运动学模型,需要将大臂四边形并联结构运动等效为串联结构。

大臂并联部分如图2a)所示,推导运动学表达式如式(1)所示。当已知支撑油缸、机械臂、伸缩油缸和支撑杆伸缩油缸的长度L2,L3,L4由液压缸构成的几个三角形进行运算,通过式(1)可得图中关节2和4结构处的夹角θ′2,θ′4。据此,将大臂的液压缸长转化为对应的关节角度,用等效的开链结构代替四边形并联结构,如图2b)所示。

图2 大臂运动分析示意图

(1)

1.3 运动学建模

将整个机械臂视作开链结构,并建立如图3所示的D-H坐标系,根据机械臂结构参数得到D-H参数,如表1所示。其中大臂两液压缸一般保持等长,为平行四边形。当两液压缸不等长作差动配置时,通过变量γ来表示差动造成臂后半部分的摆动角度。当大臂两缸长度相等时,γ=0;当两缸长度不等时,γ≠0。

表1 机械臂D-H参数

图3 机械臂D-H系示意图

(2)

(3)

其中,末端相对于机械臂基坐标系的位置坐标为p,姿态矩阵为R。

(4)

(5)

2 机械臂逆运动学求解

2.1 求解思路

本文将机械臂逆运动学分为姿态方程和位置方程两部分来求解[18],思路如下:

1) 对于姿态方程,根据锚杆孔目标姿态的两个自由度可整理出两个方程;为发挥机械臂末端二级推进器的有效行程,设定定位油缸8伸至最长d8max,并整理出d8的表达式。通过牛顿迭代法求解姿态方程组,可求得变量θ1,θ4,θ6,θ7,其中大臂四边形的形状需根据锚杆孔类型进行合理设定,一般设定为平行四边形。

2) 对于位置方程,机械臂在不同孔位之间移动时,优先使用中臂进行位置补偿,尽量保持大臂长度不变。基于该打孔原则,由位置方程确定变量θ2,d3,d5,至此完成机械臂逆运动学求解。

2.2 目标锚杆孔的位姿描述

锚杆钻车实际工作的巷道断面形状常见的有矩形和拱形,巷道类型虽不同,但总可根据锚杆孔的姿态将其分为3类:竖直锚杆孔、水平锚杆孔和一般姿态锚杆孔。本文以矩形巷道为例展开研究,如图4所示,建立断面坐标系{Os},本文机械臂的目标锚杆孔位为0～9号。锚杆孔的目标位姿相对于断面坐标系给出,然后将锚杆孔的位置和姿态坐标转换至机械臂基坐标系{O0}中,进行逆解计算。

图4 矩形巷道断面示意图

锚杆孔的姿态采用RPY欧拉角进行描述,根据实际工况可知,锚杆孔的姿态只需两个自由度来确定,绕其自身旋转的自由度并没有限制。因此目标姿态只需给定偏航角αs和俯仰角βs两个姿态参数,翻转角γs不需要进行设定。根据锚杆孔的姿态参数αs,βs求得目标姿态矩阵Rs,以下的讨论设定车体姿态与断面坐标系保持一致。

Rs=Rot(Z,αs)Rot(Y,βs)

(6)

(7)

式中:s=sin;c=cos。

2.3 定位油缸伸长量d8的确定

在机械臂打孔过程中,d8伸长量可根据实际情况进行调节,但为发挥推进器二级推进油缸的行程,一般设定定位油缸伸至最长d8max,因此在逆运动学求解时可将变量d8看作为定值,作为对冗余度的一处利用。下面推导d8的表达式。

由机械臂结构可知,当推进器末端保持目标姿态时,可有无数组转动关节变量的解与之对应。在目标姿态下,当机械臂的转动关节确定为某组关节变量,且末端抵达至目标位置时,此时可唯一确定该组转动关节变量所对应的d8长度。

(8)

其中,根据各关节活动范围,当θ6≠0或θ7≠π时,d8直接按照式(8)求解;当θ6=0或θ7=π时,式(8)分母为0,无法按照式(8)计算。故式(8)可作为一个姿态约束方程。

2.4 姿态方程求解

采用牛顿迭代法对构建的姿态方程组进行求解,合理设定大臂四边形形状来利用机械臂冗余度,并根据不同类型锚杆孔分别确定迭代初始值。

该机械臂大臂为四边形结构,实际作业过程中,大臂两个液压缸一般伸长量相同为平行四边形,保持平动。大臂两液压缸等长时,θ2,θ4并不影响推进器末端的姿态。当大臂两液压缸不等长时,不再为平行四边形,双缸的差动使得推进器部分“抬头”或“低头”动作。此时θ2,θ4满足关系

(9)

式中变量γ表示大臂两液压缸差动时,造成推进器末端的上下摆动。变量γ在机械臂逆运动学求解中视为一个冗余自由度。该自由度的合理使用:首先设定大臂两缸等长γ=0进行求解,然后根据其余关节变量的求解结果进行讨论。当θ6,θ7不能满足锚杆孔的目标姿态时,可使用变量γ进行姿态补偿。

由D-H系可知,8号系的Z轴方向表示推进器的打孔姿态,将θ2,θ4的关系式(9)代入姿态表达式(5)中,整理得:

(10)

保证推进器与锚杆孔的目标姿态一致,只需联立式(7)第一列与式(10),整理得:

(11)

式中Rs(1,1)表示矩阵Rs的第一行第一列,其余同理。

上述两方程联立式(8),当sθ6sθ7≠0,在机械臂的关节活动范围内即θ6≠0或θ7≠π时,整理非线性方程组为:

(12)

该方程组中有θ1,γ,θ6,θ7共4个变量,为冗余方程。针对不同姿态的锚杆孔,需对其中某个变量进行合理设定,将其转化为含有3个未知变量的方程组。

(13)

牛顿迭代法的求解主要分为三方面来讨论:

1) 初始值X应尽量靠近解的附近;

2) 其迭代表达式为

X(k+1)=X(k)-F′(X(k))-1F(X(k))k=0,1,…

(14)

式中F′(X(k))-1为非线性方程组Jacobi矩阵的逆矩阵。

3) 循环结束条件为当迭代前后两变量的误差小于设定值时,迭代循环结束,认为找到可行解。

因此针对不同姿态类型的锚杆孔,主要做两方面讨论:一是对θ1,γ,θ6,θ7进行合理设定;二是选取各自初始值。

2.4.1 竖直锚杆孔

图5 竖直锚杆孔机械臂投影图

图中O8O′7所在虚线的直线方程为

sθ1x-cθ1y=a8-d7-d4

(15)

当机械臂推进器末端抵达至锚杆孔目标位置处时,将O′7(x′7,y′7)代入虚线方程,计算得θ1为

arctan2(-y′7,x′7)

(16)

式中:L为机械臂偏置长度,L=a8-d4-d7;θxy为锚杆孔轴线与OXY面的夹角;θx为锚杆孔轴线在OXY面上投影线与X轴的夹角,O′7(x′7,y′7)坐标计算为:

(17)

(18)

对于竖直向上的锚杆孔,当设定大臂两液压缸等长,即γ=0,由于θ7位于范围边界,实际可能由于车体姿态导致逆解θ7超限, 无法满足锚杆孔的目标姿态。此时可通过大臂两液压缸差动,由变量γ来实现姿态补偿。因此在实际求解时,姿态方程变量设定θ7=90°,未知变量为θ1,γ,θ6。

设定θ7=90°,同时将式(16)计算的θ1代入姿态方程(11),求得θ6后根据求解结果进行讨论。

θ6=arcsin[Rs(1,1)sθ1-Rs(2,1)cθ1]

(19)

当θ6=0,即机械臂处于奇异位型,θ1直接由式(16)来确定。d8不能通过式(8)确定,此时不需要通过牛顿迭代法进行求解,姿态方程的求解结果为:

(20)

当θ6≠0,说明机械臂临近奇异位型,θ1无法根据式(16)直接确定。设定θ7=90°为已知,通过牛顿迭代法求解变量θ1,γ,θ6。将式(16)作为θ1的初始值,通过式(11)推导γ,θ6关于θ1的表达式,整理初始值公式为:

(21)

雅可比矩阵的求解为

J1=

(22)

2.4.2 水平锚杆孔

对于水平锚杆孔,当大臂保持为平行四边形,由机械臂结构可知要使推进器摆至水平,其中θ6位于关节范围边界±90°附近。此时机械臂在基坐标系OXY面上的投影分析如图6所示,其中Oi表示机械臂各关节处D-H坐标系的原点,O8O7为推进器定位油缸伸最长d8max在OXY面上的投影。

图6 水平锚杆孔机械臂投影图

图6中虚线方程为

sθ1x-cθ1y=-d4

(23)

同样当机械臂推进器末端抵达锚杆孔目标位置处时,点O7(x7,y7)为直线上一点,代入方程计算得θ1的初始值为

arctan2(-y7,x7)

(24)

式中O7(x7,y7)计算与式(18)相同。

对于水平锚杆孔,首先设定大臂两液压缸等长,即γ=0。非线性方程组的变量为θ1,θ6,θ7,通过牛顿迭代法求解非线性方程组。

当γ=0时,代入式(11)推导出θ6与θ7关于θ1的表达式,将式(24)作为θ1的初始值。设定高度H,当锚杆孔高于H时,推进器不翻转处于远地端进行打孔作业;当锚杆孔低于H时,推进器翻转至近地端进行打孔作业。

(25)

(26)

雅可比矩阵求解为

(27)

由实际巷道和锚杆钻车的位置,可知θ7≠90°,根据上述确定初始值,然后通过牛顿迭代法进行求解,再由θ6的求解结果进行讨论。

(28)

(29)

2.4.3 一般姿态锚杆孔

对于一般姿态锚杆孔,当大臂保持为平行四边形,由机械臂结构可知要使推进器摆至该姿态,θ6不在关节范围边界,因此很难直接确定接近逆解结果的θ1。为求解接近θ1逆解的初始值,本文根据竖直水平两类锚杆孔对应机械臂在水平面上的投影分析,当推进器定位油缸伸至最长d8max时,求解出对于一般姿态锚杆孔的θ1的最大范围,取该范围的中间值作为θ1的初始值。

对于一般姿态的锚杆孔,机械臂推进器定位油缸伸至最长d8max,当按照水平锚杆孔的θ1计算公式,可求得该目标孔位下机械臂θ1可能的最小值θ1min。机械臂在基坐标系OXY面上的投影如图7所示,O8O7为推进器定位油缸d8max在OXY面上的投影。当按照竖直锚杆孔的θ1计算公式,可求得该目标孔位下机械臂θ1可能的最大值θ1max。

图7 一般姿态锚杆孔机械臂投影图

(30)

对于一般姿态锚杆孔,首先设定大臂两液压缸等长,即γ=0。非线性方程组的变量为θ1,θ6,θ7,通过牛顿迭代法求解非线性方程组。

当γ=0时,代入姿态方程推导出θ6与θ7关于θ1的表达式,θ1的初始值设定为式(30)范围中间值。设定高度H,当锚杆孔高于H时,推进器处于远地端进行打孔作业;当锚杆孔低于H时,推进器翻转至近地端进行打孔作业。

(31)

(32)

确定初始值之后,按照牛顿迭代法的步骤进行求解。该姿态的锚杆孔求解出来θ6与θ7一般不会超限,可直接求解成功。雅可比矩阵求解见式(27),与水平锚杆孔一致。

综上所述,姿态方程求解完毕。

2.5 位置方程求解

2.5.1 求解原则

位置方程求解过程中体现机械臂一处冗余度,通过分析人工操作过程与臂的结构特点,确定以下求解准则合理利用冗余度。

由人工打孔的实际操作可知,打孔顺序是由内向外、由上向下进行,分别通过机械臂的横摆缸与俯仰缸实现。当大臂作横摆或者俯仰操作时,末端在空间的轨迹是一段圆弧。锚杆孔排列在垂直断面上,因此在外摆和俯仰过程中需要有长度的补偿。在机械臂结构设计上,中臂d5的伸长优先起到长度补偿作用,而不是直接通过调节大臂两油缸d3来实现。大臂两液压缸在调节过程中,一般需要保证同步性,否则差动会造成末端推进器的晃动;并且大臂长度改变,还要配合俯仰缸的长度变化,因此调节中臂d5操作更加方便。

位置方程求解时,对于1号孔优先伸长大臂d3和支撑缸θ2来满足目标位置,中臂d5在允许范围内最小。对于其余孔位,机械臂在外摆与俯仰过程中大臂长度尽量保持不变。在位置方程求解时,先代入当下孔位上一锚杆孔的大臂长度d3,求解计算变量d5和θ2。当计算出的d5超限时,再通过调整d3来实现位置方程求解。

2.5.2θ2,d3,d5的确定

(33)

(34)

(35)

第一个孔位求解,根据位置方程的求解原则,首先设定d5为关节范围最小值,求解θ2与d3。求解方程(35)结果如下:

(36)

(37)

剩余孔位求解时,将上一孔位的大臂长度d3视为已知量,求解θ2与d5。根据方程(35),推导过程与结果为

(38)

整理得到关于d5的一元二次方程为

(39)

当求解出的d5在关节范围内,则求解成功,然后求解出变量θ2,θ4;当求解出d5超上限,则设定d5为最大值再求解d3与θ2;当求解出d5超下限,则设定d5为最小值再求解d3与θ2。至此,位置方程求解结束。

综上所述,本文根据位姿分离的思想,完成机械臂逆运动学求解。

3 仿真分析

对图4矩形巷道中的锚杆孔进行逆运动学求解,然后将得到的逆解关节结果代入正运动学方程,与目标位置进行对比验证逆解的准确性。

用于仿真的PC配置如下:操作系统为64位Windows10专业版,处理器为Intel(R) Core(TM) i5-4210M CPU,2.60GHz;仿真软件为MATLAB 2016b。

3.1 逆解计算

表2 锚杆孔相对于断面坐标系的位姿坐标

针对上述竖直、水平和一般姿态锚杆孔,设定牛顿迭代法求解精度为Δ=0.000 5 rad,求得逆解整理见表3。结合逆解的求解思路对上述结果进行分析。

1) 位置方程求解分析:观察各锚杆孔逆解的d3与d5,大臂长度d3在逆解计算时,以上一个锚杆孔的大臂伸长量为基准,长度不足则通过中臂d5来补偿。保证机械臂实际打孔操作时大臂尽量少的运动,充分发挥了中臂d5的补偿作用,符合机械臂的结构设计。

2) 姿态方程求解分析:在上述车体姿态下,观察各锚杆孔逆解的θ2与θ4,可知上述逆解中大臂两液压缸等长,保持为平行四边形。观察锚杆孔逆解的θ6与θ7,可知7、8、9这3个锚杆孔由于高度较低,机械臂推进器翻转至近地端;对于其余孔位,推进器位于远地端。

3.2 逆解结果误差分析

每个锚杆孔的逆解进行误差分析:将各锚杆孔的逆解代入运动学方程,求解末端实际位置,与给定的目标位置对比,如表4所示,对该表误差分析如下:

表4 锚杆孔逆解误差分析

1) 产生误差的原因:误差的来源在于姿态方程是通过牛顿迭代法求解,因此求解出来的转动关节角度存在一定的偏差,导致误差的产生。当姿态方程求解出各关节角度为准确值,位置方程求解理论上不会存在偏差。

仿真实验表明,设定迭代精度为Δ=0.000 5 rad,本文提出的逆解方案最终末端位置坐标的误差保持在10-7mm以下,算法精确高;该精度要求下,牛顿迭代法求解姿态方程迭代次数在2～3次,求解上述9个锚杆孔逆解共耗时65 ms,平均每个孔耗时7.3 ms。在实际工作时只要输入目标孔,立即就可得到逆解结果,操作人员感觉不到计算延迟。该逆解算法精确性与实时性满足实际工程需要。

4 结论

本文针对一类应用于锚杆钻车的串并联混合结构、具有冗余自由度的液压重型机械臂的运动学问题展开分析研究,解决了该特殊结构的机械臂的运动学逆解难题,为进一步实现钻臂的自动定位控制打下了基础。利用本文方法,针对某矩形巷道中9个锚杆孔目标位姿数据,在MATLAB平台进行逆运动学求解计算,获得充分的计算精度和计算速度;将算法部署到PLC中,在模拟巷道中进行机械臂的定位控制实验,也满足实际工程需求。

1) 对于具有局部并联结构的串联机械臂,可以通过运动等效,将其转化为串联开链机构,然后采用传统的D-H法建立运动学模型。针对文中机械臂的大臂的四边形并联结构,将油缸2、3和4的长度L2,L3,L4变量转化为等效杆件的角度变量,实现了并联到串联的等效结构转化。

2) 对于冗余自由度机械臂,结合臂的结构与工作环境的工况特点,对关节变量提出附加约束,以简化逆解运算;尽可能实现位置与姿态的分离求解。针对文中机械臂,为保证推进器获得最大行程,设定定位油缸d8为最大伸长量;合理设定大臂四边形形状,可通过变量γ实现姿态补偿;尽量保持大臂长度d3不变,优先使用中臂d5进行位置补偿。这些附加约束大大缩小了逆解的可能空间,简化了求解运算;同时充分利用了冗余关节的自由度,保证了逆解结果在运动的直观性、无干涉、运动轨迹短的优势。

本文逆解方案中的运动学参数为机械臂理想结构参数,未考虑装配精度和控制精度等误差对逆解造成的影响,今后会在此基础上进一步研究相应的误差补偿算法。