移动机械臂的层级聚合建模方法研究

董方方, 杨 超, 韩 江, 张新荣

(1. 合肥工业大学 机械工程学院, 合肥 230009;2. 安徽省智能数控技术及装备工程实验室, 合肥 230009;3. 长安大学 陕西省高速公路施工机械重点实验室, 西安 710064)

0 引 言

移动机械臂是通过在移动平台上加装一个或多个机械臂以实现协调控制的完整机械系统,相比于传统的固定基座机械臂有更大的工作范围,因此在工业生产、物料搬用、家政服务、抢险救援等领域有很大的应用空间．

移动机械臂虽然具有强大的功能,但由于机械臂和移动平台这两个系统各自的运动形式和运动特性存在较大差异,使得系统在作业过程中两者会存在强烈的相互作用,对各自运动状态产生相互影响,即耦合效应．耦合效应的建模分析较为困难,从而导致整体运动控制的稳定性较差．而且,这种耦合效应会伴随着机械臂-移动平台的质量比增大而愈加明显．因此,通常做法是尽可能将移动平台的质量加大来弱化这种耦合效应．表1列出了目前一些著名的机械臂-移动平台的质量比,可以看出,质量比大部分不超过0.21,少数超过0.3．但是过大的移动平台也会使得移动机械臂过于笨重,限制了其灵活性和工作能力．因此要实现对高质量比的移动机械臂的有效控制,高效而准确的建模方法就成为了一种前置条件．

表1 机械臂-移动平台质量比[1]

目前在移动机械臂的建模方法上有多种不同思路和解决办法．Liu等[9]将原本耦合的移动机械臂系统分离,将耦合效应看作外部扰动,并利用Lagrange方法分别建立移动平台和机械臂独立的动力学方程．该方法虽然简化了建模过程,但不能充分发挥移动作业机器人系统的动态作业能力．杨贺贺等[10]基于多体系统离散时间传递矩阵法,分别建立了车轮、车体柔性关节和机械臂的动力学方程,最后得到了移动柔性机械臂的整体动力学模型．该方法虽然得到了完整动力学方程,但还是要对每个分析单元进行细致的受力分析．陈良港等[11]基于单位对偶四元数法,针对冗余移动机械臂求取任务空间速度与广义空间速度映射关系困难的问题,将移动机械臂看作一个整体建立其运动学模型．该方法虽然保证了运动学精度,但需要求解微分和逆运动学．魏丽君等[12]基于D-H算法将移动平台看作成一个由2个移动关节和1个转动组成的虚拟关节,并将其纳入到机械臂中进行整体建模．该方法用在运动学分析中有其独特优势,但在动力学分析上就十分棘手．Zhong等[13]使用Lagrange方法和直接路径法(DMP)的概念对移动机械臂进行了整体建模, 兼顾了移动机械臂内在的耦合特性, 有相对较高的建模精度, 但是增加了形式和计算的复杂性, 且仍然需要求解Lagrange乘子．

针对移动机械臂建模的复杂性和耦合性,通过应用分析力学中的U-K理论[14-16]可以对移动机械臂这一非线性的机械系统进行高效而准确的建模．该理论不同于以往的力学分析方法,通过经典建模三步法就可以完成对一个复杂机械系统的动力学建模工作．即先求得一个机械系统的无约束的动力学模型,然后将约束方程转化为二阶标准形式,之后利用UKE得到解析形式的约束力,最后可得到系统受约束的动力学方程．Huang等[17]通过对无约束的单个子系统进行聚类和级联,利用UKE计算由约束引入的约束力,得到多体系统运动方程的封闭形式表达式．该方法表明U-K理论具有层级嵌套属性,多层约束可以叠加聚合,这为复杂多体机械系统建模提供了一种新的实践方法．该方法被应用到移动机械臂的建模中,不需要分析其每个子系统的受力情况,也不需要求解系统的逆解,更不需要求解Lagrange乘子,且保证了建模的准确性和简单性．董方方等[18]基于此方法构建了双臂机器人动力学模型,极大地减小了计算过程,并保证了模型的控制精度．韩江等[19]利用该方法高效、系统、快速地建立了2自由度冗余驱动并联机器人的动力学解耦模型．虽然这种方法不区分完整约束或非完整约束,且简单、通用性强,但当直接应用U-K方法时,必须保证系统在操作期间的每个时刻满足约束条件．在实际工程中,由于各种原因,在初始阶段很难满足这些约束条件．因此,为了处理初始条件对约束可能存在的偏差,需要针对约束方程进行修正[20]．

本文根据移动机械臂的运动特点,将移动机械臂划分为多个子系统．该方法既考虑系统所固有的耦合效应,又不失简单性和可操作性．该方法可以获得在不考虑任务要求下动力学建模的结构约束和任务约束下跟踪指定轨迹的性能约束,然后便可以通过求解UKE获得控制力的显式的解析表达式．同时,为了解决当初始条件不满足给定约束条件时的问题,基于Lyapunov稳定性理论,构造了修正的约束方程．最后通过仿真,验证了建模的正确性和应对初始条件偏差的有效性．

1 Udwadia-Kalaba理论简介

1.1 Udwadia-Kalaba基本方程

U-K方法是一种求解受约束系统约束力解析解的方法．假设一个无约束的机械系统有n个状态变量q=[q1,q2,q3,…,qn]T,该系统无约束条件下的动力学方程可以描述为如下形式[14]:

(1)

若该系统存在m(m≤n)个约束:

(2)

m个约束可以被划分为两类,即完整约束和非完整约束．其包括h个完整约束

φi(q,t)=0,i=1,2,…,h,

(3)

和m-h个非完整约束

(4)

基于一致性的假设,我们可以将非完整约束(4)对时间t微分一次,将完整约束(3)对时间t微分两次,以矩阵方程的形式推导出一组约束方程．可得到约束的二阶Pfaffian标准微分形式如下[14]:

(5)

(6)

(7)

式中B(q,t)=A(q,t)M1/2(q,t),B+(q,t)为B(q,t)的Moore-Penrose广义逆矩阵．对比式(1)和(6),可得由施加约束产生的约束力为

(8)

从上述约束力的求解过程可以看出,该方法既不需要先确定存在的具体约束条件,也不需要求解Lagrange乘子,其求解的约束力为解析解,免去了大量的求解过程,且形式简单整洁．

1.2 Udwadia-Kalaba基本方程的层级属性

依据式(8),外加约束力Qc中有包含Q的成分,即Qc是基于Q而得到的解析化结果．这就使得当给系统施加多个约束时,UKE可以不断层级化地生成对应约束所产生的约束力．例如一个无约束的系统的动力学方程如下:

(9)

(10)

其中

(11)

(12)

其中

(13)

当再有别的附加约束时,这种过程可以持续迭代下去．假设当到某层级时,该无约束动力学方程为

(14)

其中

(15)

1.3 建模示例

我们通过一个算例来对比层级聚合方法与传统Lagrange方法的建模过程．假设有以下单摆系统,如图1所示,小球的质量为m0,绳子长度为l0,绳子系在坐标系O0xy原点处,小球的坐标为(x0,y0)．小球摆动的速度为v,绳子角速度为ω,绳子与水平的夹角为β,绳子对小球施加的拉力为F．

图1 单摆系统Fig. 1 A single pendulum system

1.3.1 层级聚合建模方法

现将单摆上的小球视为一个质量为m0做自由落体运动的质点,则运动方程为

(16)

将式(16)改写为矩阵形式:

(17)

根据单摆系统的运动特点,小球受到的约束为

(18)

写成二阶Pfaffian标准微分形式为

(19)

其中

利用UKE求得约束力,并将约束力施加到式(17)的系统中,因此单摆系统动力学方程为

(20)

1.3.2 Lagrange方法

首先写出该系统的Lagrange函数:

(21)

将式(21)代入Lagrange方程:

(22)

F-m0gsinβ=mω2l0．

(23)

因此可知F为

F=m0gsinβ+mω2l0,

(24)

计算可以得到

(25)

对应到Q0为

(26)

最后可以得到其动力学方程为

(27)

其中

从上面两种方法的推导及分析过程可知,层级聚合方法直接对约束进行标准化处理后代入U-K方程可获得解析形式的约束力,再与无约束系统结合即可写出受约束系统的动力学方程．整个过程的分析简单,无需推导,易于编程实现．而拉氏方法不仅需要复杂的运动分析,还需要对得到的Lagrange函数方程进行求解,进而得到最终的动力学方程．因此从求解全过程可知,从数据处理和运动分析的角度来看,层级聚合方法具有简明直观的特点．

2 基于层级聚合建模方法的移动机械臂建模

基于U-K方法的层级属性,将所研究的移动机械臂划分为3个子系统,即移动平台系统和两个机械关节系统,如图2所示．为了方便在移动机器人上建立坐标系,下文将移动机器人抽象成简略图．对于3个子系统均为无约束系统时,可以更容易地得到其动力学方程．将移动平台和机械臂切分开来是考虑到移动平台和机械臂的差异性．此外将机械臂切分为第一、 二关节和其余关节系统, 这样划分的目的是减少建模过程的复杂性．

从以上介绍可知,移动机器臂是由移动平台和机械臂所构成的复合系统．我们将选用由Mecanum轮驱动的全方向移动平台和三关节机械臂构成的移动机械臂系统来研究移动机械臂的建模过程．选用全向移动平台是考虑到其灵活性和保证底盘的小巧性;选用的三关节机械臂是将常规的六关节机械臂简化为只有旋转、下臂和上臂的三轴机械臂,而舍去其他3个对移动机械臂平台整体控制精度和工作能力影响较小的腕关节,同时也不失研究的代表性和一般性．

图2 移动机械臂子系统分割图Fig. 2 Partition of the mobile manipulator subsystem

移动机械臂运动学分析的主要任务是给出关节空间变量与位姿空间变量之间的转换关系,即运动学正问题与运动学逆问题．图3中的机械系统的工作装置为三关节机械臂,因此工作装置有三自由度．对移动机器臂进行运动学分析,首先在地面和机械装置上建立一些坐标系．OWxWyWzW为基坐标系,OMxMyMzM为车辆坐标系,其原点OM被设置在车辆在地面上的投影,ORxRyRzR为固定在机械臂基座的坐标系,O1x1y1z1为固定在第一关节的坐标系,O2x2y2z2为固定在第二关节的坐标系,O3x3y3z3为固定在第三关节的坐标系,O4x4y4z4为固定在末端执行器的坐标系．选取机械臂的关节空间变量为:第一关节和底座之间转角为θ1,第二关节和底座之间转角为θ2,第三关节和第二关节之间转角为θ3．

(28)

(29)

其中,c(θi-1)=cosθi-1,s(θi-1)=sinθi-1,c(αi-1)=cosαi-1,s(αi-1)=sinαi-1,i=1,2,3,4,θi-1为坐标系Oixiyizi相对于坐标系Oi-1xi-1yi-1zi-1的旋转角,αi-1为坐标系Oi-1xi-1yi-1zi-1旋转轴相对于坐标系Oixiyizi旋转轴的旋转角,ai-1为在xi-1方向上Oi-2xi-2yi-2zi-2和Oi-1xi-1yi-1zi-1之间的距离,di-1为在zi-1方向上Oi-2xi-2yi-2zi-2和Oi-1xi-1yi-1zi-1之间的距离．机械臂几何参数如表2所示．

图3 移动机械臂坐标示意图Fig. 3 Coordinate systems for the mobile manipulator

表2 机械臂几何参数

因此,可得在坐标系O4x4y4z4中的末端执行器尖端向量p4在机械臂坐标系O1x1y1z1中表达式p0为

(30)

(31)

其中,c(θi)=cosθi,s(θi)=sinθi,c(θi-θj)=cos(θi-θj),s(θi-θj)=sin(θi-θj),i=1,2,3,j=2,3．机器人运动学分析中,Jacobi矩阵用来表示机器人末端执行器的线速度和角速度与各关节速度之间的转换关系．上面求得末端位置与各个关节角度的关系后,则可得到末端执行器速度和各个关节速度的关系为

(32)

(33)

2.1 移动平台子系统的建模

移动平台采用了由伺服电机通过减速齿轮箱进行独立驱动每个Mecanum轮的设置,将其置于空间坐标中对其进行运动学和动力学建模,如图4所示．

为了表征移动平台的位置,使用广义坐标x,y,φ来描述其位置信息．因为该移动平台为全向驱动,每个轮子对应了一个角位移,分别为θω1,θω2,θω3,θω4．同时由图4可知平台的长度和宽度分别为2L,2l,车轮半径为Rω．其运动方程可以表示为

(34)

定义移动平台子系统为S11,并构建其Lagrange函数为

(35)

图4 移动平台结构示意图Fig. 4 Schematic diagram of the mobile platform structure

将式(35)代入到Lagrange方程中可得

(36)

通过求解方程(36),可以得到S11的动力学方程:

(37)

M11和Q11的具体结构如下:

(38)

其中,μ表示摩擦因数．

2.2 机械臂子系统的建模

上面对机械臂部分进行了运动学分析,下面对其进行动力学建模．根据本文前面所阐述的将机械臂划分为第一、二关节和其余关节子系统的建模思路,下面对其具体建模过程进行演示．将第一、二关节子系统定义为系统S21,其余关节子系统定义为系统S31,图5为机械臂关节子系统示意图．

对于系统S21,选取广义坐标为q21=[x01y01θ01θ2]T,(x01,y01,z01)为坐标系O1x1y1z1的原点在基座标系OMxMyMzM中的空间位置．其中z01为定值,不作为其状态变量．选取(α01,β01,θ01)为坐标系O1x1y1z1的Euler角,Euler角选取为XYZ表示方法,其中α01=0,β01=0．其子系统的Lagrange函数为L21为

(39)

其中,xc2=x01+r2cosθ01cosθ2,yc2=y01+r2sinθ01cosθ2,zc2=z01+l1+r2sinθ2,I1和I2分别为第一关节和第二关节的转动惯量．

(a) 第一、二关节子系统(子系统S21) (b) 其余关节子系统(子系统S31) (a) The 1st and 2nd joint subsystem (subsystem S21) (b) The remaining joint subsystem (subsystem S31)

代入Lagrange方程可得

(40)

可得系统S21在无约束条件下的动力学方程,其表达式为

(41)

其中

(42)

(43)

对于系统S31,选取广义坐标为q31=[x03y03z03γθ03]T,同样(x03,y03,z03)为坐标系O3x3y3z3的原点在基座标系OMxMyMzM中的空间位置．选取(γ,ψ,-θ03)为坐标系O3x3y3z3的Euler角,Euler角选取为ZXZ表示方法,其中ψ=90°．

子系统S31的Lagrange函数L31可以表示为

(44)

其中

xc3=x03+r3cosθ03cosγ,yc3=y03+r3cosθ03sinγ,

zc3=z03-r3sinθ03,xc4=x03+(l3+r4)cosθ03cosγ,

yc4=y03+(l3+r4)cosθ03sinγ,zc4=z03-(l3+r4)sinθ03,

I3和I4分别为第三关节和末端执行器的转动惯量．

将式(44)代入Lagrange方程,得

(45)

求解式(45),可得系统S31的无约束动力学方程为

(46)

式中

(47)

(48)

其中

c(θ03)=cosθ03,s(θ03)=sinθ03,c(γ)=cosγ,s(γ)=sinγ．

2.3 构建约束和系统整合

移动机械臂可以通过堆聚子系统S11,S21和S31再辅以物理上的结构约束将其重构成一个有机的完整系统．结合空间的位置关系,可以很容易得到其约束关系如下:

(49)

由式(49)可得其约束方程为

(50)

(51)

(52)

其中

q12=[xyφx01y01θ01θ2x03y03z03γθ03]T．

同时可以将子系统S11,S21和S31的矩阵M和Q集中写成另外两个矩阵M12和Q12,其具体构造如下:

(53)

Q12=[Q11Q21Q31]T．

(54)

利用UKE可以得到结构约束力,并获得完整的受约束的动力学方程:

(55)

(56)

因为式(55)不是最简形式,所以需要将其化简．这就需要找到状态变量q12与系统真正需要的状态变量q=[xyφθ1θ2θ3]T之间的变换关系．除了知道式(49)所得到的关系之外,还知道θ01=γ=φ+θ1和θ03=θ3-θ2,综合这些条件,可以得到以下关系式:

(57)

其中

(58)

(59)

则对式(55)进行化简可得

(60)

当系统受到外部运动约束时,会对系统施加外部约束力τ,从而最后可得式(60)的最简形式如下:

(61)

其中

τ为运动约束力．

3 初始条件偏差

在仿真中使用的系统初始条件必须满足所需的轨迹跟踪约束．但是,由于各种因素的存在,这种情况未必真会发生．它可能只在初始时刻被近似满足．如果初始条件不相容,则模拟结果会出现发散．处理初始条件问题的一种方法是使用Lyapunov稳定性理论,如文献[20]所示．

所期望的轨迹由式(2)描述．基于轨迹稳定方法,我们现在修改约束方程为

(62)

式中f(φ,t,α)是包含向量p、参数α的m维向量．式(62)必须满足以下要求:

注1 特别地,如果约束方程是完整的,如式(3)所示,那么方程可以修改为

(63)

现在用修正式(3)作为期望的轨迹要求,通过微分过程,得到矩阵方程的形式为

(64)

经过修改,控制力为

(65)

4 仿 真

为了验证本文所提出的层级建模方法的准确性,以及UK方法对于移动机械臂的协调控制的可实现性和适应性,将通过以下仿真过程予以说明和阐述．模拟的对象是图2所示的移动机械臂,该机械装置主要包括由4个Mecanum轮驱动的全向移动平台和由3个关节和末端夹爪构成的机械臂,其具体参数如表3所示．

表3 系统动力学参数表

为了贴近实际使用场景,便于直观地理解仿真结果,本文采用了常见的机械臂-移动平台质量比参数．质量比参数并不影响本文的建模和仿真过程,只影响稳定性控制的难易程度．

为了验证本文层级聚合建模方法的建模精度和准确性,对拉氏方法和层级聚合建模方法得到的模型同时施加约束(66)(约束满足初始条件):

(66)

并比较在施加约束后每个状态变量的数值是否一致．

图6表示在受约束条件下两种建模方法获得的移动平台轨迹随时间的变化情况,图7(a)和7(b)分别为层级聚合建模方法和Lagrange方法所得的机械臂各关节轨迹随时间的变化情况,通过对比可以发现各自的运动轨迹是一致的．图8为两种模型下各个状态变量的数值误差,图8(a)为移动平台x方向和y方向的轨迹误差,图8(b)为机械臂各关节的轨迹误差．可以发现所有结果始终为零,这说明两种建模方法得到的计算结果完全一致,充分说明了本文建模方法与拉氏方法在建模精度和准确性上是一致的．

图6 移动平台轨迹

为对比算法的效率,采用MATLAB记录了两种方法的计算耗时,如图9所示．从图9中可以看出,层级聚合方法比拉氏方法所获得的模型在计算效率上略有提高,但提升的幅度有限．然而,本文提出的层级聚合建模方法对于不同类型约束(完整、非完整约束)处理具有更好的一致性,都是将约束转化为二阶微分形式,再利用UKE写出约束力的解析解．整个建模过程简洁明确,分析步骤较少．而拉氏方法需要先求解Lagrange乘子,无法获得解析形式的解．此外,利用层级叠加的属性,当有新的约束增加后,本文方法只需将新的约束转化为二阶微分形式后代入UKE,不影响其他分析步骤．

图8 移动平台与机械臂各自轨迹误差Fig. 8 The respective trajectory errors of the mobile platform and the manipulator

图9 层级聚合方法与拉氏方法计算效率对比Fig. 9 Comparison of computation efficiency between the hierarchical aggregation method and the Lagrange method

同时为了证明本文算法对于处理初始条件偏差的有效性,为移动机械臂选定一组轨迹约束,让移动平台走正弦曲线,并且机械臂进行相对于移动平台的画斜圆运动,约束条件如表4所示(xmp(t),ymp(t)和zmp(t)分别为末端执行器相对于移动平台在坐标系ORxRyRzR中三个方向上的轨迹约束时间函数)．

表4 约束条件参数

图10—13为移动平台和移动机械臂各自设定轨迹约束并附加任意初始条件的情况下对于理想轨迹的跟踪情况．通过观察可知我们选定的约束均为完整约束,所以选择式(63)这种修正方案．表4所有约束均可写为式(63)这种形式,这样便可以将式子Fi(i=1,2,…,6)改写为6个修正的约束等式．其中两参数设置为,κm=2.5(m=1,2,3对应F1,F2,F3的修正方程),κn=2(n=4,5,6对应F4,F5,F6的修正方程),εi=1.5 (i=1,2,…,6对应F1,F2,…,F6的修正方程)．图10(b)为机械臂相对于移动平台的画圆运动,可以发现在宏观上末端执行器从较大范围的初始条件偏差下逐渐回归到离线轨迹上．图10(a)为移动平台和机械臂相对于移动平台画圆展开后的空间轨迹,符合一般移动机械臂协调运动模式(p1和p3分别为末端执行器初始位置和终止位置;p2和p4分别为移动平台初始位置和终止位置)．

(a) 移动平台、末端执行器空间轨迹 (b) 末端执行器相对移动平台轨迹 (a) Spatial trajectories of the mobile platform and the end-effector (b) End-effector trajectories relative to the mobile platform

图11为移动平台的轨迹跟踪情况,可以发现移动平台在从在较大的初始条件偏差的情况下,在修正后的约束方程的约束下渐进地使得偏离的轨迹收敛于理想轨迹上．图11(b)、(c)和(d)分别是移动轨迹在x,y方向和航向角上的跟踪情况,也都吻合图11(a)宏观上所显现的收敛情况,都在一定的时间段后达到了收敛．图12(a)和(b)分别为移动平台和末端执行器两部分轨迹跟踪误差变化情况,可以发现移动平台在5 s后逐渐收敛,机械臂在8 s后逐渐收敛．其中移动平台的轨迹跟踪误差为4×10-4m,末端执行器移动平台的轨迹跟踪误差为5×10-4m,符合轨迹跟踪精度要求(图12显示的轨迹误差均为综合误差)．初始时刻之所以会出现跟踪误差较大情况,首先因为本文特意将初始条件选取在理想轨迹之外的一点,所以使得初始时刻轨迹误差较大;其次也是利用修正方程对理想轨迹的跟踪使得初始时刻出现了较大的超调量,目的是使系统更快的收敛．图13(a)中Fx和Fy分别为关节1施加在移动平台x,y方向上的约束力, 图13(b)中T1,T2和T3分别为关节1,2,3的内部约束力．观察图中数值可知,约束力没有出现奇大情况,贴合现实的使用需求．

(a) 移动平台x,y轨迹 (b) 移动平台x方向轨迹 (a) Mobile platform x,y trajectories (b) Mobile platform x-direction trajectories

(c) 移动平台y方向轨迹 (d) 移动平台航向角 (c) Mobile platform y-direction trajectories (d) Mobile platform heading angle

(a) 移动平台轨迹误差 (b) 末端执行器相对移动平台轨迹误差 (a) Mobile platform trajectory errors (b) End-effector trajectory errors relative to the mobile platform

(a) 关节1施加在移动平台x,y方向上的约束力 (b) 关节1,2,3内部约束力 (a) The forces applied to joint 1 in the x,y-directions (b) Internal constraints of joints 1,2,3 of the moving platform

5 结 论

本文首先应用了不同以往的建模方法,依据移动机械臂本身的运动特点,将移动机械臂划分为多个子系统．该方法既考虑系统所固有的耦合效应,又不失简单性和可操作性．该方法包含在不考虑任务要求下动力学建模的结构约束和跟踪指定轨迹的性能约束条件下,通过求解UKE可以获得控制力的显式、闭合形式的解析表达式．对于机械系统在一般条件中初始条件不满足的情况下,通过基于Lyapunov稳定性理论,将原来设定的轨迹约束规约化为修正的约束方程,以得到新的约束矩阵,并将其施加在所建立的动力学模型上达到补偿初始条件偏差的目的．最后仿真结果验证了移动平台和机械臂在同时进行运动时,均可以满足收敛到理想轨迹的性能需求,并实现了较高的精度要求．

致谢本文作者衷心感谢陕西省高速公路施工机械重点实验室(长安大学)开放基金(300102252505)对本文的资助．