一种基于DTW-GMM 的机器人多机械臂多任务协同策略

刘成菊 林立民 刘 明 陈启军

相比单臂机器人,多臂机器人可以通过协作完 成一些更加复杂的动作,实现各种灵巧操作任务,如搬运、焊接和装配等.多臂机器人各个机械臂的工作空间一般存在着相互重叠的区域,即它们之间的协作工作区域,在这个区域内各个机械臂可以完成协作任务[1-3].然而,多臂机器人如何在这个工作区域协调好多个任务一直是研究热点和难点.

Lim 等[4]研究了双臂机器人的抓取,通过对双臂的轨迹进行提前规划,并由微分变换算出手臂的移动增量,成功实现了对水平放置的圆柱体的抓取和搬移;Ortenzi 等[5]实现了保持末端相对位置不变的双机械臂轨迹规划策略,先对主机械臂的轨迹进行规划,然后通过建立的运动约束方程来生成从机械臂的轨迹;Ramirez-Alpizar 等[6]利用Leap Motion 运动捕捉器采集人类手臂执行零件装配任务时的运动,然后提取装配任务时轨迹关键点的手指所处的位置和姿态,用于控制Baxter 机器人生成装配任务的轨迹;Tuan 等[7]提出了一种基于姿态估计的滑模控制策略,设计了一种具有鲁棒性的自适应控制器,用于双臂机器人的3D 协作运动,但是该算法作者只将其运用到单臂自由度为2 的双臂机器人上.这些多臂协作控制策略提前规划机器人的执行轨迹,实时性有待提高,往往也需要建立复杂的机器人动力学模型.近年来,研究学者希望通过示教学习的策略,让机器人学习人类如何执行灵巧操作的任务.动态运动基元 (Dynamic movement primitive,DMP)模型[8-12]和高斯混合模型 (Gaussian mixture model,GMM)[13-19]是典型的示教学习算法.

DMP 模型由动作变化系统和标准时间系统构成[8].该模型通过对机器人的示教轨迹进行拟合,提取示教轨迹的形状特征,并通过设定期望的轨迹终点和时长生成具有泛化性的机器人轨迹.DMP 的优点在于可以控制生成轨迹的时长和幅度,但是这种示教学习模型只能应用于轨迹终点速度为0 的场景.Kober 等[9]对原始DMP 模型进行了改进,将其应用于终点速度不为0 的场景,但是如果轨迹稍有差异,都需要重新进行模型的设计,即基于DMP的学习算法只能实现单条示教轨迹的学习.如果示教轨迹存在噪声,泛化得到的输出轨迹效果非常不理想.GMM 示教学习模型利用高斯混和模型来对机器人轨迹进行建模,用概率的方法来提取轨迹之间的相关关系,从而对示教轨迹进行表征;然后利用高斯混合回归可以实现轨迹的泛化输出.基于GMM 的轨迹学习模型能够很好地保持示教轨迹的空间形状特征,但是一旦示教轨迹存在时长上的差异,GMM 模型的轨迹学习效果不佳.

针对DMP 和GMM 轨迹学习算法各自存在的问题,本文提出基于动态时间规整-高斯混合模型(Dynamic time warping-Gaussian mixture model,DTW-GMM)的多机械臂协同轨迹生成方法.论文主要内容分为三部分: 1)针对机器人示教轨迹往往存在时间长短差异较大的问题,采用DTW 算法来规整机器人的示教轨迹.文献[20]利用DTW 方法来规整人类不同语速的发音,消除语音的发音时间长短不一对语音识别的影响.本文将DTW 核心思想进行迁移,用其来规整机器人的示教轨迹.2)基于DTW 所规整的示教轨迹,利用GMM 概率模型来学习示教轨迹的共同特征,并设计多机械臂多任务协同的轨迹生成策略,利用高斯混合回归(Gaussian mixture regression,GMR)泛化输出各机械臂的执行轨迹.3)设计机器人的运动控制引擎并利用Pepper 机器人平台,完成手臂协作搬运和汉字轨迹书写实验,验证本文提出的多机械臂协同控制策略的可行性和有效性.

1 基于DTW-GMM 的示教轨迹特征提取

1.1 动态时间规整算法

动态时间规整(Dynamic time warping,DTW)算法可以用来规整机器人的示教轨迹.该方法主要的核心思想在于,将两个长度不一的时间信号通过对齐拉伸到相同长度,且拉伸后使得两条轨迹之间的距离最短.DTW算法可以由如下目标方程描述[21]:

式中,Wx=[aij](1≤i ≤nx,1≤j ≤n,aij ∈{0,1})和Wy=[aij](1≤i ≤ny,1≤j ≤n,aij ∈{0,1}) 表示二值时间规整矩阵,通过将时间信号X和Y中的相关向量进行拉伸,最终将两个轨迹矩阵规整到相同长度.

时间规整矩阵必须同时满足以下三个约束条件:

1)X和Y的初始和终止向量点必须对齐;

2)规整矩阵本质上表示的是时间序列,因此规整矩阵中取值为1 的点所依次连成的路径(即规整路径)必须是单调的;

3) 由于信号需要保持有序性,因此矩阵X和Y中对应向量的前后顺序必须保持不变.

求解目标方程(1)中的时间规整矩阵可以使用动态规划(Dynamical programming,DP)算法或广义典型时间规整(Generalized canonical time warping,GCTW)算法[21-22],两种求解算法的时间复杂度的对比如表1 所示,其中l用于设置GCTW算法规整后轨迹信号的长度,原则上选取l=1.2×max(nx,ny),m表示单调增函数基的个数,一般m的取值为5 左右.GCTW 比DP 算法具有较低的时间复杂度,后文采用GCTW 算法进行多机械臂示教轨迹的规整策略.

表1 算法的时间复杂度Table 1 Time complexity of algorithms

图1 表示两个示教轨迹序列在DTW 算法规整下的结果.图1(b)所示的规整路径将图1(a)原始序列规整成图1(c).由此可见,DTW 算法可以很好地改善原有示教轨迹存在的时间差异较大的问题,从而统一时间的变化.

1.2 GMM 轨迹特征提取

经DTW 规整后的示教轨迹,利用GMM 概率模型[23-25]进行表征,从而提取示教轨迹的共同特征.对于多机械臂示教数据,本文将其表示为:

式中,yi,s,yi,t分别表示示教轨迹的空间信息和时间信息,T表示示教轨迹中示教点的个数.

对于多维示教变量y=(y1,y2,···,yT),建模GMM 为:

式中,p(y)表示概率密度函数,K表示高斯分布的个数,πk表 示第k个高斯分布所占的权重,N(y;µk,Σk)表示以µk为均值,Σk为协方差矩阵的高斯概率密度函数,D代表示教轨迹的维度.

相比于高斯分布的参数估计,混合高斯分布的参数估计更加复杂.主要原因在于隐变量的存在.对于示教样本集y=(y1,y2,···,yT),通过隐变量γt,k的引入,即表示yt这个样本由第k个模型生成,可以将数据展开成完全数据:

若yt由 第1类采样而来,则有γt,1=1,γt,2=0,···,γt,K=0,表示为(yt,1,0,···,0).

故完全数据的似然函数为:

可以利用期望值最大(Expectation maximum,EM) 算法进行迭代计算,求取GMM模型的参数πk,µk,Σk.先定义Q 函数如下:

式中,µi,Σi,πi分别表示第i次迭代时GMM 模型的各个高斯分布模型的均值,协方差矩阵和权重集合;表示对γ的估计:

对Q 函数进行求导,并令其导数为0,可得:

为了使得EM 算法能够更快地收敛,本文将经由DTW 规整后的多机械臂示教轨迹利用K 均值(K-means)聚类算法进行聚类,划分数据的所属类别,进行期望值最大算法的参数初始化.同时利用赤池信息准则[26](Akaike information criterion,AIC),通过最优化AIC 指标函数对K-means 的K值进行选取:

式中,C(K)表示GMM 概率模型参数数目;D表示示教数据集的维度;l nL(y)表示初始参数下示教样本的对数似然函数.采用DTW-GMM 算法对机器人的多机械臂示教轨迹进行特征提取的步骤如算法1 所示.

算法1.基于DTW-GMM 算法的多机械臂示教轨迹特征提取

2 基于GMR 的轨迹泛化输出

图2 是基于本文提出的算法生成的二维双臂协同轨迹泛化输出.图2(a)为双臂协作的示教轨迹,左、右机械臂分别负责 “U”和 “Z”字形的任务执行轨迹.图2(b)则为基于DTW-GMM 算法的轨迹表征图,此处经由AIC 准则确定的K取值为3.图2(c)对应以左手臂空间矢量作为查询向量下,右手臂的泛化输出图.由图2 可以看出,本文算法可以对多机械臂的示教轨迹进行表征,并在GMR 回归下生成具有一定泛化性的机械臂执行轨迹,用于完成多臂协作任务.

图2 双臂协同轨迹泛化输出Fig.2 Generalized dual-arm collaborative trajectory output

对机器人进行示教时,轨迹的噪声时时存在.图3 探讨了本文采用的GMR 泛化输出策略对示教噪声的抗干扰性.图3(a)为单条示教轨迹时,汉字轨迹 “打”存在框选处的噪声时,轨迹的表征输出结果.而图3(b)对应3 条示教轨迹时,GMR 的输出情况.由图3 可以看出,本文的轨迹泛化策略具有一定的抗干扰性,即使存在噪声,算法依旧可以充分提取示教轨迹的共同特征.

3 总体系统架构

为了将本文提出的多机械臂多任务协同策略运用到机器人控制中,设计了如图4 所示的控制系统架构[27].其中多机械臂协同轨迹生成器主要负责多机械臂示教轨迹的特征学习与协同轨迹输出.其通过传感器反馈的姿态信息为查询向量,实时输出机械臂的协同轨迹.而运动引擎主要负责对多机械臂协同输出的轨迹进行跟踪控制.

图4 总体系统架构设计Fig.4 The system architecture block diagram

3.1 多机械臂协同轨迹生成器

多机械臂轨迹生成原器的原理框图如图5 所示.首先,对多机械臂进行轨迹示教 然后利用DTWGMM 算法对示教轨迹进行特征提取.其中EM 算法用于GMM 概率模型参数的学习,而K-means算法对经由DTW 算法规整后的示教轨迹进行聚类学习,获得EM 算法迭代的初始参数µ0、Σ0、π0.与参数随机初始化相比,K-means 可以加快EM 算法的收敛.而K-means算法中K值的选定由AIC指标函数确定,防止模型的过拟合.为了满足机械臂轨迹生成的实时性要求,多机械臂协同轨迹生成器需要进行预训练,否则轨迹的规整和参数的学习将耗费较多的时间.预训练之后的轨迹生成器实时接收查询向量的输入,利用GMR 在线输出各个机械臂的协同轨迹.

图5 多机械臂协同轨迹生成器框图Fig.5 Multi-arm collaborative trajectory generator block diagram

3.2 运动引擎

在机器人运动学中,关节空间与工作空间的对应关系可以描述为:

式中,T=[T1,T2,···,TN] 表示机器人末端的位置和姿态;θ=[θ1,θ2,···,θM] 中的θi表示第i个连杆相对于第i -1 个连杆的旋转角度或者位移.

本文采用分解速度控制法[28](Resolved motion rate control,RMRC)来控制机器人的关节角速度.机器人的微分运动学和逆微分运动学方程可以表示为:

式中,J(θ)、J-1(θ)分别表示雅克比矩阵和其逆矩阵.

当机器人系统冗余时,即M＞N时,雅可比矩阵可能存在非满秩的状态.此时,可以用雅可比矩阵的伪逆矩阵代替其逆矩阵[29]:

为了防止奇异性问题的出现,通过引入阻尼系数λ来保证伪逆矩阵有意义:

由此,设计基于微分逆运动学的机器人运动控制引擎如图6 所示.其中,Xref表示多机械臂协同学习系统输出的参考轨迹,J表示雅可比矩阵,qreal表示机器人的实时关节角.

图6 基于微分逆运动学的机器人运动控制引擎设计框图Fig.6 Block diagram of the motion control engine based on differential inverse kinematics

4 仿真与实验

4.1 双臂搬运实验

4.1.1 双臂搬运示教与DTW 规整

如图7 所示,本文选用Pepper 仿人机器人作为实验验证平台.Pepper 高1.2 m,装有加速度计和陀螺仪等多种传感器,共有20 个自由度,本文只考虑其左、右手臂各有的5 个自由度.其带有的操作系统NAOqi 支持python SDK 和C++SDK.本文利用Matlab 良好的数据处理特性进行DTW多机械臂示教轨迹的规整、GMM 概率模型的轨迹表征训练以及Pepper 正逆运动学的计算等工作,然后将泛化输出的多机械臂协同轨迹数据存储为.mat 格式的文件,最后调用这些文件控制Pepper 的双臂完成协作搬运.

图7 Pepper 实验平台Fig.7 Pepper experimental platform

图8 所示为Pepper 双臂协作完成搬运的示教过程序列,不同人的示教过程往往存在时间长短和速度差异较大的问题.实验时进行三次搬运示教,将示教得到的轨迹数据传到Matlab,并利用DTW算法规整原始示教轨迹,结果如图9 所示.左侧表示三次示教时右手臂末端的轨迹和x、y、z三维上的变化情况.左侧对应的原始示教轨迹序列表明,对于三次相同动作的示教轨迹,轨迹的时间差异明显,而右侧经由DTW 规整后的轨迹消除了示教过程中的时间不一的影响,统一了时间维度上的变化.

图8 双臂协作搬运示教Fig.8 Demonstrations of dual-arm collaborative moving trajectory

图9 右手臂轨迹DTW 规整Fig.9 DTW output of right arm trajectory

4.1.2 DTW-GMM 轨迹表征

基于DTW 规整后的多机械臂示教轨迹,利用算法1 所述流程进行轨迹的表征学习.实验中相关参数设置如表2 所示.

表2 DTW-GMM 算法参数设置Table 2 Parameter setting of the DTW-GMM algorithm

图10 是GMM 模型跟本文提出的DTWGMM 模型针对搬运时右手臂的x、y、z的三维特征表征情况.此处由式(11) AIC 准则确定的K的取值为4.GMM 是一种概率模型的轨迹表征策略,图10 中用椭圆来描述示教轨迹的特征提取结果.椭圆的形状由高斯分布的协方差矩阵所决定,其面积大小表示右手臂三维空间位置的不确定度.面积越大,不确定度越大,即轨迹的特征提取效果越差.DTW-GMM 的表征结果优于GMM.表3 和表4是GMM和DTW-GMM 表征的同一个高斯分布的协方差矩阵表.由协方差矩阵表的定量分析可以看出,DTW-GMM 对应的各维度变量之间具有较小的协方差,算法可以更好地提取机械臂示教轨迹的特征,具有更好的表征结果.

图10 右手臂轨迹表征Fig.10 Right arm trajectory characterization

表3 GMM 表征协方差矩阵表Table 3 Covariance matrix of GMM algorithm

表4 DTW-GMM 表征协方差矩阵表Table 4 Covariance matrix of DTW-GMM algorithm

4.1.3 GMR 轨迹泛化输出

对机械臂示教轨迹经GMM 编码后,利用GMR进行轨迹重构,可以得到泛化的输出轨迹.图11 是右手臂的泛化输出示意图.经由DTW 规整后的示教轨迹特征更加明显,泛化输出时具有更低的不确定度.

图11 右手臂轨迹泛化输出Fig.11 Generalized right arm trajectory output

得到右手臂的GMR 泛化输出轨迹后,利用第3 节所述多机械臂的协同策略,以作为查询向量,实时获取左手臂的位置控制向量,最后得到搬运实验时左、右手臂的协作执行轨迹如图12所示.其中,虚线表示原始的三条示教轨迹,实线是泛化输出的执行轨迹.原始示教轨迹由于噪声的存在,轨迹的平滑性能较差.本文所提出的DTW-GMM策略对噪声具有一定的抗干扰性,能够充分提取示教轨迹的共同特征,且最终输出的执行轨迹具有较好的平滑性.

图12 搬运实验左右手臂轨迹泛化输出Fig.12 Generalized dual-arm trajectory output in moving experiment

4.1.4 实体机器人实验

在Pepper 机器人平台上,基于本文所提出的DTW-GMM 策略,以右手臂的空间位置信息和DTW 规整后的时间信息为查询向量,实时获取左右臂的协同执行轨迹信息.在第3.2 节设计的运动控制引擎作用下,对轨迹进行跟踪控制.图13 是Pepper 机器人双臂协作完成搬运实验的截图.通过实验可以发现,本文的多机械臂协同轨迹生成时间约为1 ms,实验的硬件环境为16.00 GB RAM,Intel i7-7500U CPU,软件环境为Matlab 2018a.实验结果表明了基于DTW-GMM 模型对控制机器人双机械臂完成协同任务的可行性.

图13 双臂协作搬运实验截图Fig.13 Snapshots of dual-arm to collaboratively move basket

4.2 汉字书写实验

汉字轨迹由笔画组成,是一种离散化的轨迹[30-34].当前的汉字轨迹书写主要是基于预规划轨迹的执行策略.通过将汉字轨迹分段进行规划,并设计控制器跟踪所规划的笔画轨迹.这种策略模式单一,泛化性不足.近年来,研究人员希望通过示教学习的策略,来学习汉字轨迹的特征,提高轨迹生成的泛化性.而当前基于示教学习的研究主要将汉字轨迹进行连续化处理,这样会失去原有汉字轨迹的基本特征,且当汉字轨迹较为复杂时进行连续化示教操作繁琐.为此,本节提出基于DTW-GMM 的汉字轨迹书写策略.

由于汉字轨迹的书写主要是在x、y平面上的运动,所以可以将某一个机械臂的x、y平面上的位置信息向量和经由DTW 规整后的时间t,即以作为查询向量,通过GMM 和GMR 的多机械臂轨迹的表征和泛化,输出其他机械臂的协同轨迹.图14(a)是基于本文提出的协同策略的汉字轨迹"木"的书写情况.将 “木”字按照笔画分解为“一”、“丨”、“丿”、“丶”四段轨迹,由4 个机械臂分别执行,并以执行 “一”的机械臂的位置矢量作为查询向量,实时获取其余机械臂的协同轨迹数据.“木”字轨迹较为简单,当汉字的笔画较为复杂时,比如对于交叉型的轨迹,可以适当调整策略,通过将某些笔画分配给同一个机械臂,可以减少需要进行协同的机械臂的数量,减少书写任务的复杂性,如对于汉字轨迹 “打”字,可以利用双机械臂分别对轨迹的 “扌”和 “丁”进行学习,最终 “打”字的轨迹表征与生成情况如图14(b)所示.

图14 基于DTW-GMM 算法的汉字轨迹书写Fig.14 Chinese character trajectory generation based on DTW-GMM algorithm

5 结束语

本文提出了一种基于概率模型的多机械臂示教轨迹表征策略,可以用于实现机器人的多臂协作.利用DTW-GMM 算法对原始示教轨迹进行时间规整并提取共同特征,然后设计多机械臂协同控制策略,最后基于GMR 回归多机械臂的协同轨迹.Pepper 机器人双臂协作完成搬运和汉字轨迹书写实验验证了本文所提出的基于DTW-GMM 的多机械臂多任务协同策略的有效性.下一步预期在轨迹生成时融入视觉信息反馈和基于强化学习的参数学习策略,实现复杂环境下的多机械臂适应性协同轨迹的输出.