面向变电站智能消防机械臂的高精度运动学模型研究

汤锦慧 伍发元 辛建波 郑晓东 薛霜思

(1.国家电网 江西省电力有限公司 电力科学研究院,南昌 330096;2.国家电网 江西省电力有限公司,南昌 330096;3.西安交通大学 电气工程学院,西安 710049)

变电站是电力系统的重要组成部分,站内变压器、电容器等高低压电力设备在故障时会引起局部过热现象,如果处理不及时会进一步造成火灾,从而产生巨大的经济损失[1-2].若能对变电站的关键设备及时降温或灭火,不仅会大大降低变电站火灾风险,而且能对其安全运行提供更好的保障.变电站传统消防手段以固定式喷淋为主,但由于安装位置比较固定,所以其覆盖面及灵活性不足,无法对超温点或起火点进行准确作业,所以不能满足变电站防灾减灾的实际需求.

由于机械臂操作灵活性且工作稳定性好,所以以其为核心执行单元的智能消防系统可以实现精确的消防作业.机械臂精准控制一直以来都是工业机器人技术领域的研究热点,可以分为机械臂位姿控制与末端执行器控制两类问题.德纳维和哈登伯格首先提出了机械臂Denavit-Hartenberg(D-H)建模法[3].这一方法从刚体运动学和几何学角度出发,主要从正向运动学计算与逆向运动学控制两个方面,比较系统地阐述了机械臂位姿控制方法.其中,逆向运动学控制问题是机械臂运动控制的核心,其在满足机械臂实体一系列约束条件下,追求控制规划的时间最优,或部分时间与能量最优.为了实现控制规划最优[4-5],支持向量机、神经网络等一些智能优化算法被引入到机械臂控制中[6],不仅极大地提高了规划效率,而且推动了机械臂建模理论的完善[7-8].除了对机械臂传统控制算法进行优化,一些学者对人类手臂的控制进行了建模[9-11],并在此基础上研究了模拟神经回路与激素调节的机械臂控制方法[12]；此外,采用新型材料制造的柔性机械臂建模与控制也是研究焦点之一[13-14].近年来,机械臂控制技术取得了较大进展[15-16],且在航天器维修[17]、空间站管理[18]、仿生控制[19]、医疗[20]等工作中得到了进一步应用.但如果将机械臂直接用于变电站智慧消防系统中,则在实际运行中存在的累积误差、机械结构塑性形变等一系列问题,会直接影响机械臂的控制精度,进而会导致变电站消防系统运行效率降低.

针对上述问题,本文提出了一种高精度机械臂运动学建模方法.首先将机械臂结构简化为刚体连杆模型,基于机械臂D-H 建模法,以数学表达式描述机械臂在空间中的状态(位姿)与运动(轨迹)；其次,对实际场景所采用的四自由度机械臂进行了正向与逆向运动学分析,并提出一种利用机械臂连杆模型几何约束关系,直接求解四自由度机械臂逆运动学问题的算法,最后的实验结果验证了所建模型的精确性.

1 机械臂建模基础

机械臂运动学建模旨在将复杂的机械臂空间结构抽象为一系列参数,以运动学方法描述机械臂连杆、关节和末端执行器的位姿,进而采用数学计算方式对机械臂的运动进行解算,实现机械臂的运动轨迹规划与位姿控制.

D-H 机械臂建模法[3]是一种以刚体运动学为基础,在三维空间坐标系下对机械臂抽象化建模的系统方法.这一方法通过限制原点位置与X轴方向,用4个参数即可表达关节之间六自由度的坐标变换.对最一般情况的n(n≥1)关节机械臂进行研究.自基座到末端执行器,对关节进行编号为Axis1,Axis2,Axis3,…,Axisi-1,Axisi,…,Axisn,其中i∈[1,n]；对连杆进行编号为link1,link2,link3,…,linki-1,linki,…,linkn,其中i∈[1,n].此外,机械臂基座记为link0.关节结构分为两种：旋转关节与滑动关节,对应的运动轴线分别被称作旋转轴和平移轴,旋转轴是旋转关节旋转所围绕的轴线,平移轴是滑动关节滑动所沿的直线.

截取Axisi-1、linki-1与Axisi部分进行研究.连杆linki-1用两个参数表示：连杆长度a i-1与连杆转角αi-1.关节Axisi(Axisi-1)用两个参数表示：关节转角θi(θi-1)与连杆偏移d i(d i-1).4种参数定义如下：

(1)连杆长度：两个关节轴线之间的距离；

(2)连杆转角：某关节轴线相对于前一关节轴线绕其公共法线旋转的角度；

(3)关节转角：某关节与后一关节公共法线,和此关节与前一关节公共法线,绕此关节轴线旋转的角度；

(4)连杆偏移：某关节与后一关节公共法线,和此关节与前一关节公共法线,沿此关节轴线方向的距离.

上述4种参数即为机械臂的D-H 参数.此外,常引入参数σi(σi-1),用以区分关节i(i-1)是旋转关节或是滑动关节.

为将上述参数以数学表达式形式表示,在机械臂各关节处建立三维笛卡尔直角坐标系O1X1Y1Z1,O2X2Y2Z2,…,O i-1X i-1Y i-1Z i-1,O i X i Y i Z i,…,O n X n Y n Z n,其中i∈[1,n].此外,机械臂基座坐标系O0X0Y0Z0可视为机械臂的公共坐标系,统一记为OXYZ.以O i X i Y i Z i为例,说明坐标系建立方法：Z i为关节i轴线方向；X i为Z i-1轴线与Z i轴线的公共法线,正方向为Z i-1指向Z i,最后由右手螺旋法则确定Y i方向.特别的,当相邻两关节的Z轴平行,则有无数条公垂线,可挑选与前一关节公垂线共线的一条作为X轴.

2 运动学计算求解与控制

2.1 机械臂运动学计算求解方法

机械臂正向运动学计算是指已知机械臂全部关节运动位矢(旋转角度及方向),解算机械臂末端位置与姿态.机械臂正向运动学计算有且仅有唯一解,求解过程如下.

基于参数在笛卡尔直角坐标系中的定义,可以进一步求解O i-1X i-1Y i-1Z i-1与O i X i Y i Z i相邻两坐标系间的变换关系.由坐标系O i-1X i-1Y i-1Z i-1转换到坐标系O i X i Y i Z i的转换矩阵见式(1).

其中：R(z,θi)和R(x,αi)表征旋转变换,T(0,0,d i)和T(a i,0,0)表征平移变换,具体见式(2)～(5).

求解机械臂末端位置和姿态,就是求解机械臂末端连杆linkn在基座坐标系OXYZ下的空间姿态.取机械臂末端连杆linkn上一质点P n(x n,y n,z n),将其在O n X n Y n Z n下的坐标依次变换到O n-1X n-1Y n-1Z n-1、O n-2X n-2Y n-2Z n-2、O n-3X n-3Y n-3Z n-3…坐标系中,最终坐标将变换到O0X0Y0Z0坐标系,即OXYZ坐标系中,如此便推导出linkn上质点P在机械臂公共参考系中坐标P(x,y,z)的表达式,见式(6).

其中,

方程(7)称作机械臂运动学方程.只要任取末端连杆上两个不同质点,用上述方法求解其在公共参考系中的坐标表示,即可表示出机械臂末端位姿.

2.2 机械臂逆向运动学控制

机械臂逆向运动学控制是指,已知机械臂末端执行器位姿(其上任一点在公共坐标系下的坐标),求解机械臂全部关节运动位矢.

机械臂逆向运动学控制问题就是已知A,对运动学方程(7)中的进行求解,对应于每个合理存在的A,可能有多组解集合.

方程的解空间转化为

变量v一般有严格的限制条件：当v i表示旋转角度时,要根据机械臂i关节实际可以旋转的角度范围设定限制条件；当v i表示移动距离时,要根据机械臂i关节实际可以平移的范围设定限制条件.这样,机械臂逆运动学控制问题,转化成为在n维有限解空间内的方程求解问题.

上述方程的求解可以从方程两侧的矩阵特征入手,A可表示为式(11).

其中：x,y,z为机械臂末端位置坐标,即坐标系O n X n Y n Z n原点O n在OXYZ坐标系下的坐标,t ij(i,j∈[1,3])为表征末端连杆linkn偏转的常数.

f(v)可表示为式(12)形式.

其中：M ij(i∈[1,3],j∈[1,4])为含有机械臂D-H参数多项式.由于两矩阵相等,因此矩阵中的元素对应相等,由此可以得到12个方程组成的方程组.当机械臂自由度小于12时,逆运动学控制问题为解不定方程组问题；当自由度大于12时,由于r ij多项式中含有三角函数,逆运动学控制问题为解超越方程组问题,可能有多组解存在.

此处从三自由度机械臂逆运动学解算开始讨论.三自由度机械臂逆向运动学控制问题为方程(13).方程(13)两侧左乘得方程(14).

方程(14)共有3个未知数v1、v2、v3,而上式左侧化简展开后得到的4×4矩阵中(1,4)、(2,4)、(3,4)位置的多项式元素恰不含不易得知的t ij,此时只需使得两侧矩阵中(1,4)、(2,4)、(3,4)位置的元素对应相等即可列出3个方程对问题进行求解.机械臂自由度＞3 时,可以采用迭代方法求一组近似解

3 四自由度机械臂运动学建模

3.1 四自由度机械臂D-H 模型建立

本文所研究的机械臂共含有6个舵机,其中2个舵机负责输出灭火泵组相关动作,对机械臂末端位置和仰角控制有贡献的舵机为底座部位旋转轴线垂直水平面的舵机和臂体铰接处3个旋转轴线平行于水平面的舵机,因此可以将其视为四自由度机械臂.

在实验中所用机械臂实体采用伺服电机配谐波减速机结构,最大转速为2 500 r/min,各轴可做正负角度运动.水平轴最大运动范围为±180°,轴线平行于水平面各轴运动范围为±170°,末端轴运动范围为±360°,实体图如图1所示.

图1 六自由度机械臂实体

在机械臂所处的水平地面上建立笛卡尔直角坐标系OXYZ,水平地面为OXY平面,Z轴垂直于OXY平面,与关节1处舵机转轴共线.之后依次在4个关节处按照要求建立坐标系.机械臂连杆模型如图2所示,且其D-H 参数列表见表1.

图2 四自由度机械臂连杆模型

表1 机械臂D-H 参数表

3.2 四自由度机械臂正向运动建模

根据D-H 参数列表可得各相邻坐标系转换矩阵,见式(15)～(19).

将上述4个矩阵依次相乘即可得到机械臂正向运动方程为：

3.3 四自由度机械臂逆向运动解算

由于四自由度机械臂逆向运动学解算矩阵解法十分繁琐,算法无法简洁地编写成代码嵌入机械臂上位机控制程序中,故此处摈弃繁复的矩阵算法,从逆向运动学控制的定义入手,提出一种由机械臂连杆模型几何约束关系直接求解逆运动学问题的算法.标注有机械臂参数的模型如图3所示.

图3 标注有逆运动学计算变量的连杆模型

四自由度机械臂逆运动学计算转化为以下几何问题：已知机械臂末端P点位置为P(x,y,z),仰角为φ(link4与OXY面夹角为φ),求解θ1,θ2,θ3,θ4.

首先,可以十分容易计算θ1

由于link2、link3和link4运动过程中始终处于同一平面,且关节旋转角度值与坐标系选择无关,因此可将机械臂连杆模型投影到二维平面求解.在OXY平面建立W轴与X轴呈θ1,平面OWZ即为link2、link3和link4运动的平面,机械臂末端点P在OWZ平面坐标系中位置为P(w,z),link4与W 轴夹角等于φ,其中w=

问题进一步简化为：平面内已知机械臂末端P点位置为P(w,z),仰角为φ,求解θ2,θ3,θ4.

由几何约束关系可得方程组(22).

此方程组的解表示为方程(23).

其中

θ2,θ3,θ4满足实际角度约束条件.

综上所述,算法的计算机程序流程如下：

1)计算θ1和w；

2)计算m,n,t1,t2；

3)判断是否满足式(22)的两个条件,

两条件都满足执行步骤3),只要有一个不满足则无解,即机械臂理论上无法到达该位姿；

4)计算式(23)中两个正弦取值

两式均取值在[-1,1]范围内,舍去超出的值.

5)用式(24)计算满足如下条件的θ2,θ3,θ4,

此时(θ1,θ2,θ3,θ4)可能有多组解.

根据机械臂各关节实际可以到达的角度范围,对解取舍,最终结果可能无解、有一组解或有多组解.这种几何算法适用于大多数四自由度以下(包括四自由度)机械臂的逆运动学计算.

4 实验结果

根据表1的D-H 参数,建立机械臂连杆模型[21],初始状态如图4所示.θ1限制在[-90°,90°]间变化,θ2,θ3,θ4三个参数均限制在[-170°,170°]之间变化.

图4 机械臂初始状态

利用仿真实验对逆运动学解算结果进行验证.假设机械臂末端预期达到位姿是：末端坐标为(236,0,186),末端执行器仰角为0°.则使用嵌入在机械臂控制程序的逆运动学解算结果为(θ1,θ2,θ3,θ4)=(0°,66.93°,-70.91°,3.98°).将数据代入到MATLAB建立好的模型中进行正向运动学计算,机械臂此时位姿如图5所示,可求解机械臂正向运动学方程如式(33)所示.

图5 在(0°,66.93°,-70.91°,3.98°)状态机械臂位姿

因此末端坐标为(236.5,0,185.6),这说明本文提出的逆运动学解算方法是精确有效的.

5 结论

为了应对变电站智能消防前端执行机构灵活性不足、定位精确度低等问题,本文研究了面向变电站智能消防机械臂高精度运动学模型的建立方法.首先使用D-H 建模法完成了四自由度机械臂正向与逆向运动学模型构建,并得到了正向运动学转换矩阵,以及逆向运动学机械臂末端位姿与关节转角的关系方程式,接着提出了一种利用机械臂连杆模型几何约束关系,直接求解四自由度机械臂逆运动学问题的算法,可以在解算前判断机械臂能否到达预期位姿,而且计算过程回避了复杂的矩阵运算,可高效精确地解决四自由度机械臂逆运动学控制问题.最后,本文搭建了机械臂的三维模型,并完成了仿真实验,实验结果验证了模型的精确性.