-
摘要: 为了控制机器人完成复杂的多臂协作任务, 提出了一种基于动态时间规整−高斯混合模型(Dynamic time warping-Gaussian mixture model, DTW-GMM)的机器人多机械臂多任务协同策略. 首先, 针对机器人示教时轨迹时间长短往往存在较大差异的问题, 采用动态时间规整方法来统一时间的变化; 其次, 基于动态时间规整的多机械臂示教轨迹, 采用高斯混合模型对轨迹的特征进行提取, 并以某一机械臂的位置空间矢量作为查询向量, 基于高斯混合回归泛化输出其余机械臂的执行轨迹; 最后, 在Pepper仿人机器人平台上验证了所提出的多机械臂协同策略, 基于DTW-GMM算法控制机器人完成了双臂协作搬运任务和汉字轨迹的书写任务. 提出的基于DTW-GMM算法的多任务协同策略简单有效, 可以利用反馈信息实时协调各机械臂的任务, 在线生成平滑的协同轨迹, 控制机器人完成复杂的协作操作.Abstract: To control robot to complete complex multi-arm cooperation tasks, a multi-task collaborative strategy based on dynamic time warping-Gaussian mixture model (DTW-GMM) is proposed in this paper. Firstly, in view of the problem that demonstration trajectories are shown to be largely different in the aspects of lasting time, the amic time warping algorithm is adopted to unify the variation of the time. Secondly, after the multi-arm demonstration trajectories are aligned by amic time warping algorithm, the Gaussian mixture model is used to extract the common features. And using the position space vector of a manipulator as the query vector, the Gaussian mixture regression algorithm is adopted to generically output the remaining manipulators＇ trajectory; Finally, the multi-task collaborative strategy proposed was verified on Pepper platform. Tasks for dual-arm to collaboratively move basket and write the Chinese character are completed based on the DTW-GMM algorithm. The multi-task collaborative strategy based on the DTW-GMM method proposed in this paper is effective. The feedback information can be introduced to coordinate the robot arms＇ task in real time, and the generated coordinated trajectories are smooth, which can control the robot to complete complex cooperative operations.
-
相比单臂机器人, 多臂机器人可以通过协作完成一些更加复杂的动作, 实现各种灵巧操作任务, 如搬运、焊接和装配等. 多臂机器人各个机械臂的工作空间一般存在着相互重叠的区域, 即它们之间的协作工作区域, 在这个区域内各个机械臂可以完成协作任务[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)算法可以用来规整机器人的示教轨迹. 该方法主要的核心思想在于, 将两个长度不一的时间信号
${{X}} \in {\bf{R}}^{d \times {n_x}}$ 和${{Y}} \in {{\bf{R}}^{d \times {n_y}}}$ 通过对齐拉伸到相同长度, 且拉伸后使得两条轨迹之间的距离最短. DTW算法可以由如下目标方程描述[21]:$$ \mathop {\min }\limits_{{{{W}}_x}{\bf{,}}{{{W}}_y}} {\Phi _{{\rm{dtw}}}} = \left\| {{{X}}{{{W}}_x}} \right. - \left. {{{Y}}{{{W}}_y}} \right\|_{\rm{F}}^2 $$ (1) 式中,
${{{W}}_x} = [{a_{ij}}](1 \le i \le {n_x},1 \le j \le n,{a_{ij}} \in \left\{ {0,1} \right\})$ 和${{{W}}_y} = [{a_{ij}}](1 \le i \le {n_y},1 \le j \le n,{a_{ij}} \in \left\{ {0,1} \right\})$ 表示二值时间规整矩阵, 通过将时间信号${{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 \;\times \max ({n_x},{n_y}) ,$ m表示单调增函数基的个数, 一般m的取值为5左右. GCTW比DP算法具有较低的时间复杂度, 后文采用GCTW算法进行多机械臂示教轨迹的规整策略.表 1 算法的时间复杂度Table 1 Time complexity of algorithms轨迹规划算法 时间复杂度 DP ${n_x} {n_y}$ GCTW $2dl{{m} } + 8{{ {m^3} } }$ 图1表示两个示教轨迹序列在DTW算法规整下的结果. 图1(b)所示的规整路径将图1(a)原始序列规整成图1(c). 由此可见, DTW算法可以很好地改善原有示教轨迹存在的时间差异较大的问题, 从而统一时间的变化.
1.2 GMM轨迹特征提取
经DTW规整后的示教轨迹, 利用GMM概率模型[23-25]进行表征, 从而提取示教轨迹的共同特征. 对于多机械臂示教数据, 本文将其表示为:
$$ \left\{ {\left. {{y_i}} \right\}} \right._{i = 1}^T = \left\{ {\left. {{y_{i,s}},{y_{i,t}}} \right\}} \right._{i = 1}^T $$ (2) 式中,
$ {y_{i,s}},{y_{i,t}} $ 分别表示示教轨迹的空间信息和时间信息, T表示示教轨迹中示教点的个数.对于多维示教变量
$y = ({y_1},{y_2},\cdots,{y_T})$ , 建模GMM为:$$ \begin{split} p(y) =\, & \sum\limits_{k = 1}^K {{\pi _k}} {\rm{N}}(y;{\mu _k},{\Sigma _k}) = \\ &\sum\limits_{k = 1}^K {{\pi _k}} \frac{1}{{{{(2\pi )}^{\frac{D}{2}}}{{\left| {{\Sigma _k}} \right|}^{\frac{1}{2}}}}}{{\rm{e}}^{ - \frac{1}{2}{{(y - {\mu _k})}^{\rm{T}}}({\Sigma _k}){^{ - 1}}(y - {\mu _k})}} \end{split} $$ (3) 式中,
$ p(y) $ 表示概率密度函数, K表示高斯分布的个数,$ {\pi _k} $ 表示第k个高斯分布所占的权重,${\rm{N}}(y;{\mu _k},{\Sigma _k})$ 表示以$ {\mu _k} $ 为均值,$ {\Sigma _k} $ 为协方差矩阵的高斯概率密度函数, D代表示教轨迹的维度.相比于高斯分布的参数估计, 混合高斯分布的参数估计更加复杂. 主要原因在于隐变量的存在. 对于示教样本集
$y = ({y_1},{y_2},\cdots,{y_T})$ , 通过隐变量$ {\gamma _{t,k}} $ 的引入, 即表示$ {y_t} $ 这个样本由第k个模型生成, 可以将数据展开成完全数据:$$({y_t},{\gamma _t}_{,1},{\gamma _{t,2}},\cdots,{\gamma _{t,K}}),\;\;t = 1,2,\cdots,T $$ (4) 若
$ {y_t} $ 由第1类采样而来, 则有${\gamma _t}_{,1} = 1,\;{\gamma _{t,2}} = 0,\cdots,$ ${\gamma _{t,K}} =$ 0, 表示为$({y_t},1,0,\cdots,0).$ 故完全数据的似然函数为:
$$ \begin{split} p(y,\gamma |\mu , &\Sigma ,\pi ) =\\ & \prod\limits_{t = 1}^T {p({y_t},{\gamma _t}_{,1},{\gamma _{t,2}},\cdots,{\gamma _{t,K}}|\mu ,\Sigma ,\pi )} =\\ & \prod\limits_{t = 1}^T {\prod\limits_{k = 1}^K {{{({\pi _k}{\rm{N}}({y_t};{\mu _k},{\Sigma _k}))}^{{\gamma _{t,k}}}}} } =\\ & \prod\limits_{k = 1}^K {{{({\pi _k})}^{\sum\limits_{t = 1}^T {{{\gamma _{t,k}}}} }}} \prod\limits_{t = 1}^T {{{({\rm{N}}({y_t};{\mu _k},{\Sigma _k}))}^{{\gamma _{t,k}}}}} \end{split} $$ (5) 可以利用期望值最大(Expectation maximum, EM)算法进行迭代计算, 求取GMM模型的参数
$ {\pi _k},{\mu _k},{\Sigma _k} $ . 先定义Q函数如下:$$ \begin{split} &Q(\mu ,\Sigma ,\pi ,{\mu ^i},{\Sigma ^i},{\pi ^i}) =\\ &\qquad{{\rm{E}}_\gamma }[\ln p(y,\gamma |\mu ,\Sigma ,\pi )|y,{\mu ^i},{\Sigma ^i},{\pi ^i}]=\\ & \qquad{{\rm{E}}_\gamma }\left[\sum\limits_{k = 1}^K {\left(\sum\limits_{t = 1}^T {{\gamma _{t,k}}|{y_t},{\mu ^i},{\Sigma ^i},{\pi ^i}} \right)\ln {\pi _k}}+\right.\\ & \left.\qquad\sum\limits_{t = 1}^T {({\gamma _{t,k}}|{y_t},{\mu ^i},{\Sigma ^i},{\pi ^i})} \ln ({\rm{N}}({y_t};{\mu _k},{\Sigma _k}))\right]=\\ &\qquad \sum\limits_{k = 1}^K \left[\sum\limits_{t = 1}^T {{\rm{E}}({\gamma _{t,k}}|{y_t},{\mu ^i},{\Sigma ^i},{\pi ^i})} \ln {\pi _k}\right.+\\ &\left. \qquad\sum\limits_{t = 1}^T {{\rm{E}}({\gamma _{t,k}}|{y_t},{\mu ^i},{\Sigma ^i},{\pi ^i})} \ln ({\rm{N}}({y_t};{\mu _k},{\Sigma _k}))\right] \end{split} $$ (6) 式中,
$ {\mu ^i},{\Sigma ^i},{\pi ^i} $ 分别表示第i次迭代时GMM模型的各个高斯分布模型的均值, 协方差矩阵和权重集合;${\rm{E}}({\gamma _{t,k}}|{y_t},{\mu ^i},{\Sigma ^i},{\pi ^i})$ 表示对$ \gamma $ 的估计:$$ \begin{split} {\rm{E}}({\gamma _{t,k}}|{y_t},&{\mu ^i},{\Sigma ^i},{\pi ^i}) =\\ & p({\gamma _{t,k}} = 1|{y_t},{\mu ^i},{\Sigma ^i},{\pi ^i})=\\ & \frac{{p({\gamma _{t,k}} = 1,{y_t}|{\mu ^i},{\Sigma ^i},{\pi ^i})}}{{p({y_t})}}=\\ & \frac{{p({\gamma _{t,k}} = 1,{y_t}|{\mu ^i},{\Sigma ^i},{\pi ^i})}}{{\sum \limits_{k = 1}^K {p({\gamma _{t,k}} = 1,{y_t}|{\mu ^i},{\Sigma ^i},{\pi ^i})} }} =\\ & \frac{{\pi _k^i{\rm{N}}({y_t};\mu _k^i,\Sigma _k^i)}}{{\sum\limits_{k = 1}^K {\pi _k^i{\rm{N}}({y_t};\mu _k^i,\Sigma _k^i)} }}\\[-20pt] \end{split} $$ (7) 对Q函数进行求导, 并令其导数为0, 可得:
$$ \begin{split}\mu _k^{i + 1} =\;& \frac{{\sum\limits_{t = 1}^T {\frac{{\pi _k^i{\rm{N}}({y_t};\mu _k^i,\Sigma _k^i)}}{{\sum\limits_{k = 1}^K {\pi _k^i{\rm{N}}({y_t};\mu _k^i,\Sigma _k^i)} }}} {y_t}}}{{\sum\limits_{t = 1}^T {{\rm{E}}({\gamma _{t,k}}|{y_t},{\mu ^i},{\Sigma ^i},{\pi ^i})} }},\\ &k = 1,2,\cdots,K \end{split}$$ (8) $$ \begin{split}\Sigma _k^{i + 1} =\;& \frac{{\sum\limits_{t = 1}^T {\frac{{\pi _k^i{\rm{N}}({y_t};\mu _k^i,\Sigma _k^i)}}{{\sum\limits_{k = 1}^K {\pi _k^i{\rm{N}}({y_t};\mu _k^i,\Sigma _k^i)} }}} {{({y_t} - \mu _k^i)}^2}}}{{\sum\limits_{t = 1}^T {{\rm{E}}({\gamma _{t,k}}|{y_t},{\mu ^i},{\Sigma ^i},{\pi ^i})} }},\\ &k = 1,2,\cdots,K \end{split}$$ (9) $$ \pi _k^{i + 1} = \frac{{\sum\limits_{t = 1}^T {{\rm{E}}({\gamma _{t,k}}|{y_t},{\mu ^i},{\Sigma ^i},{\pi ^i})} }}{T},\;k = 1,2,\cdots,K $$ (10) 式中,
$\mu _k^{i + 1}、\Sigma _k^{i + 1}、\pi _k^{i + 1}$ 分别表示第i + 1次迭代, 第k个类的均值, 协方差矩阵和所占的权重.为了使得EM算法能够更快地收敛, 本文将经由DTW规整后的多机械臂示教轨迹利用K均值(K-means)聚类算法进行聚类, 划分数据的所属类别, 进行期望值最大算法的参数初始化. 同时利用赤池信息准则[26](Akaike information criterion, AIC), 通过最优化AIC指标函数对K-means的K值进行选取:
$$ \left\{\begin{aligned} &AIC = 2C(K) - 2\ln L(y)\\ &\ln L(y) = \sum\limits_{i = 1}^T {\ln p(} {y_i})\\ &C(K) = (K - 1) + K\left[ {\frac{1}{2}D(D + 1) + D} \right] \end{aligned} \right. $$ (11) 式中,
$ C(K) $ 表示GMM概率模型参数数目; D表示示教数据集的维度;$ \ln L({y}) $ 表示初始参数下示教样本的对数似然函数. 采用DTW-GMM算法对机器人的多机械臂示教轨迹进行特征提取的步骤如算法1所示.算法1. 基于DTW-GMM算法的多机械臂示教轨迹特征提取
输入. 多机械臂示教数据
$ \left\{ {\left. {{y_i}} \right\}} \right._{i = 1}^T $ ;输出. 轨迹特征, 即GMM概率模型表征参数
$\mu、\Sigma、\pi$ ;1)利用式(1)对原始示教轨迹进行时间规整;
2)利用式(11)和K-means算法对GMM设置初值
$K、{\mu ^0}、{\Sigma ^0}、{\pi ^0}$ ;3) while 参数
$ \mu ,\Sigma ,\pi $ 的更新大于阈值$ \delta $ and 迭代次数,$i < {i_{\max }}$ ;4) do;
5)利用式(7)计算
$ \gamma $ 的估计值;6)最大化Q函数, 利用式(8) ~ (10)计算
${\mu ^{i + 1}}、 {\Sigma ^{i + 1}}、$ ${\pi ^{i + 1}}$ ;7)更新参数
$\mu 、\Sigma 、\pi$ ;8) end while.
2. 基于GMR的轨迹泛化输出
第1.2节所述GMM的多机械臂样本数据可以归纳为
$ x = [{x^{\rm{I}}},{x^{\rm{O}}}] $ , 其中$ {x^{\rm{I}}} $ 是查询向量,$ {x^{\rm{O}}} $ 是编码向量, 通过建立概率分布模型$ P({x^{\rm{I}}},{x^{\rm{O}}}) $ 进行GMM的训练. 对于多机械臂多任务协同的轨迹学习, 本文设计以某一个机械臂的三维空间向量和经由DTW规整后的时间变量t, 即以$ (x_{s1}^{\rm{I}},x_{s2}^{\rm{I}},x_{s3}^{\rm{I}},t) $ 作为查询向量, 以其余机械臂的三维空间向量$(x_{s1}^{\rm{O}},x_{s2}^{\rm{O}}, $ $x_{s3}^{\rm{O}},\cdots,x_{s3n}^{\rm{O}})$ 作为编码向量, 通过多维示教数据进行模型的训练, 进行多机械臂轨迹的编码学习. 将数据点的概率分布$ P({x^{\rm{I}}},{x^{\rm{O}}}) $ 建模为GMM后, 利用GMR计算条件概率$ P({x^{\rm{O}}}{\rm{|}}{x^{\rm{I}}}) $ 的期望${\rm{E}}({x^{\rm{O}}}{\rm{|}}{x^{\rm{I}}})$ 和协方差$ Cov({x^{\rm{O}}}{\rm{|}}{x^{\rm{I}}}) $ , 将期望值作为泛化的重构轨迹点, 在协方差矩阵的约束下生成平滑的执行轨迹, 用于多机械臂协同运动的轨迹输出.对于T个D维数据点组成的数据集, 建模GMM, 该模型由K个高斯分布组成:
$$ p(x) = \sum\limits_{k = 1}^K {{\pi _k}} {\rm{N}}(x;{\mu _k},{\Sigma _k})$$ (12) 式中,
${\rm{N}}(x;{\mu _k},{\Sigma _k})$ 是以$ {\mu _k} $ 为期望、以$ {\Sigma _k} $ 为方差的高斯分布, 且有:$$ {\mu _k} = \left[ \begin{array}{l} \mu _k^{\rm{I}}\\ \mu _k^{\rm{O}} \end{array} \right],{\Sigma _k} = \left[{\begin{array}{*{20}{c}} {\Sigma _k^{\rm{I}}}&{\Sigma _k^{{\rm{IO}}}}\\ {\Sigma _k^{{\rm{OI}}}}&{\Sigma _k^{\rm{O}}} \end{array}} \right]$$ (13) 在给定
$ {x^{\rm{I}}} $ 和高斯分布k下,$ ({x^{\rm{O}}}{\rm{|}}{x^{\rm{I}}},k) $ 的条件概率也满足高斯分布, 即:$$ ({x^{\rm{O}}}{\rm{|}}{x^{\rm{I}}},k)\sim {\rm{N}}(\mu _k{'},\Sigma _k{'}) $$ (14) 式中,
$ \mu _k{'},\Sigma _k{'} $ 满足:$$ \begin{split} &\mu _k{'} = \mu _k^{\rm{O}} + \Sigma _k^{{\rm{OI}}}{(\Sigma _k^{\rm{I}})^{ - 1}}({x^{\rm{I}}} - \mu _k^{\rm{I}}) \\ &\Sigma _k{'} = \Sigma _k^{\rm{O}} - \Sigma _k^{{\rm{OI}}}{(\Sigma _k^{\rm{I}})^{ - 1}}\Sigma _k^{{\rm{IO}}} \end{split} $$ (15) 考虑整个GMM, 则有
$ ({x^{\rm{O}}}{\rm{|}}{x^{\rm{I}}}) $ 的条件概率分布为:$$ ({x^{\rm{O}}}{\rm{|}}{x^{\rm{I}}})\sim \sum\nolimits_{k = 1}^{K} {{h_k}} {\rm{N}}(\mu _k{'},\Sigma _k{'}) $$ (16) 式中,
$ {h_k} $ 满足:$$ \begin{split} {h_k} = \,& p(k|{x^{\rm{I}}})= \frac{{p(k)p({x^{\rm{I}}}|k)}}{{\sum\limits_{i = 1}^{K} {p(i)p({x^{\rm{I}}}|i)} }}=\\ & \frac{{{\pi _k}{\rm{N}}({x^{\rm{I}}};\mu _k^{\rm{I}},\Sigma _k^{\rm{I}})}}{{\sum\limits_{i = 1}^{K} {{\pi _i}{\rm{N}}({x^{\rm{I}}};\mu _k^{\rm{I}},\Sigma _k^{\rm{I}})} }} \end{split} $$ (17) 基于高斯模型具有线性转换特征, 由此可得
$ ({x^{\rm{O}}}{\rm{|}}{x^{\rm{I}}}) $ 的期望$ \mu $ 和协方差$ \Sigma $ 为:$$ \begin{split} &\mu = \sum\limits_{k = 1}^{K} {{h_k}} \mu _k{'}\\ &\Sigma = \sum\limits_{k = 1}^{K} {h_{_k}^2} \Sigma _k{'} \end{split} $$ (18) 图2是基于本文提出的算法生成的二维双臂协同轨迹泛化输出. 图2(a)为双臂协作的示教轨迹, 左、右机械臂分别负责“U”和“Z”字形的任务执行轨迹. 图2(b)则为基于DTW-GMM算法的轨迹表征图, 此处经由AIC准则确定的K取值为3. 图2(c)对应以左手臂空间矢量作为查询向量下, 右手臂的泛化输出图. 由图2可以看出, 本文算法可以对多机械臂的示教轨迹进行表征, 并在GMR回归下生成具有一定泛化性的机械臂执行轨迹, 用于完成多臂协作任务.
对机器人进行示教时, 轨迹的噪声时时存在. 图3探讨了本文采用的GMR泛化输出策略对示教噪声的抗干扰性. 图3(a)为单条示教轨迹时, 汉字轨迹“打”存在框选处的噪声时, 轨迹的表征输出结果. 而图3(b)对应3条示教轨迹时, GMR的输出情况. 由图3可以看出, 本文的轨迹泛化策略具有一定的抗干扰性, 即使存在噪声, 算法依旧可以充分提取示教轨迹的共同特征.
3. 总体系统架构
为了将本文提出的多机械臂多任务协同策略运用到机器人控制中, 设计了如图4所示的控制系统架构[27]. 其中多机械臂协同轨迹生成器主要负责多机械臂示教轨迹的特征学习与协同轨迹输出. 其通过传感器反馈的姿态信息为查询向量, 实时输出机械臂的协同轨迹. 而运动引擎主要负责对多机械臂协同输出的轨迹进行跟踪控制.
3.1 多机械臂协同轨迹生成器
多机械臂轨迹生成原器的原理框图如图5所示. 首先, 对多机械臂进行轨迹示教 然后利用DTW-GMM算法对示教轨迹进行特征提取. 其中EM算法用于GMM概率模型参数的学习, 而K-means算法对经由DTW算法规整后的示教轨迹进行聚类学习, 获得EM算法迭代的初始参数
${\mu ^0}、{\Sigma ^0}、{\pi ^0}$ . 与参数随机初始化相比, K-means可以加快EM算法的收敛. 而K-means算法中K值的选定由AIC指标函数确定, 防止模型的过拟合. 为了满足机械臂轨迹生成的实时性要求, 多机械臂协同轨迹生成器需要进行预训练, 否则轨迹的规整和参数的学习将耗费较多的时间. 预训练之后的轨迹生成器实时接收查询向量的输入, 利用GMR在线输出各个机械臂的协同轨迹.3.2 运动引擎
在机器人运动学中, 关节空间与工作空间的对应关系可以描述为:
$$ \mit{T} = F\rm({\mit{\theta}} \rm) $$ (19) 式中,
${\mit{T}} = [{T_1},{T_2},\cdots,{T_N}]$ 表示机器人末端的位置和姿态;${\mit\theta} = [{\theta _1},{\theta _2},\cdots,{\theta _M}]$ 中的$ {\theta _i} $ 表示第i个连杆相对于第i − 1个连杆的旋转角度或者位移.本文采用分解速度控制法[28] (Resolved motion rate control, RMRC)来控制机器人的关节角速度. 机器人的微分运动学和逆微分运动学方程可以表示为:
$$ {\dot {{T}}} = {{J}}({\theta} ) \cdot {\dot{{ \theta}}} $$ (20) $$ {\dot {{\theta}}} = {{J}^{ - 1}}({\theta}) \cdot{\dot {{T}}}$$ (21) 式中,
${{J}}({\theta} )、{{{J}}^{ - 1}}({\theta} )$ 分别表示雅克比矩阵和其逆矩阵.当机器人系统冗余时, 即
$ M > N $ 时, 雅可比矩阵可能存在非满秩的状态. 此时, 可以用雅可比矩阵的伪逆矩阵代替其逆矩阵[29]:$$ {{J}^\# } = {{J}^{\rm{T}}}{({J}{{J}^{{{\rm{T}}}}})^{ - 1}} $$ (22) 为了防止奇异性问题的出现, 通过引入阻尼系数
$ \lambda $ 来保证伪逆矩阵有意义:$$ {{J}^\# } = {({J}{{J}^{{\rm{T}}}+{\lambda ^2}{{I}}})^{-1}}{{{J}}^{\rm{T}}} $$ (23) 由此, 设计基于微分逆运动学的机器人运动控制引擎如图6所示. 其中,
$\mit {X_{ref}}$ 表示多机械臂协同学习系统输出的参考轨迹,$\mit{J}$ 表示雅可比矩阵,$\mit{q_{real}}$ 表示机器人的实时关节角.
图 6 基于微分逆运动学的机器人运动控制引擎设计框图Fig. 6 Block diagram of the motion control engine based on differential inverse kinematics4. 仿真与实验
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的双臂完成协作搬运.
图8所示为Pepper双臂协作完成搬运的示教过程序列, 不同人的示教过程往往存在时间长短和速度差异较大的问题. 实验时进行三次搬运示教, 将示教得到的轨迹数据传到Matlab, 并利用DTW算法规整原始示教轨迹, 结果如图9所示. 左侧表示三次示教时右手臂末端的轨迹和x、y、z三维上的变化情况. 左侧对应的原始示教轨迹序列表明, 对于三次相同动作的示教轨迹, 轨迹的时间差异明显, 而右侧经由DTW规整后的轨迹消除了示教过程中的时间不一的影响, 统一了时间维度上的变化.
4.1.2 DTW-GMM轨迹表征
基于DTW规整后的多机械臂示教轨迹, 利用算法1所述流程进行轨迹的表征学习. 实验中相关参数设置如表2所示.
表 2 DTW-GMM算法参数设置Table 2 Parameter setting of the DTW-GMM algorithm参数 参数值 $\delta$ $1 \times {10^{{\rm{ - 10}}}}$ ${i_{\max }}$ 10000 $\lambda$ 0.0001 图10是GMM模型跟本文提出的DTW-GMM模型针对搬运时右手臂的x、y、z的三维特征表征情况. 此处由式(11) AIC准则确定的K的取值为4. GMM是一种概率模型的轨迹表征策略, 图10中用椭圆来描述示教轨迹的特征提取结果. 椭圆的形状由高斯分布的协方差矩阵所决定, 其面积大小表示右手臂三维空间位置的不确定度. 面积越大, 不确定度越大, 即轨迹的特征提取效果越差. DTW-GMM的表征结果优于GMM. 表3和表4是GMM和DTW-GMM表征的同一个高斯分布的协方差矩阵表. 由协方差矩阵表的定量分析可以看出, DTW-GMM对应的各维度变量之间具有较小的协方差, 算法可以更好地提取机械臂示教轨迹的特征, 具有更好的表征结果.
表 3 GMM表征协方差矩阵表Table 3 Covariance matrix of GMM algorithm变量 t x y z t 1 0.0814 −0.0074 0.0280 x 0.0814 1 −0.0003 0.0010 y −0.0074 −0.0003 1 −0.0001 z 0.0280 0.0010 −0.0001 1 表 4 DTW-GMM表征协方差矩阵表Table 4 Covariance matrix of DTW-GMM algorithm变量 t x y z t 1 0.0078 −0.0063 0.0122 x 0.0078 1 −0.0001 0.0002 y −0.0063 −0.0001 1 0.0001 z 0.0122 0.0010 0.0001 1 4.1.3 GMR轨迹泛化输出
对机械臂示教轨迹经GMM编码后, 利用GMR进行轨迹重构, 可以得到泛化的输出轨迹. 图11是右手臂的泛化输出示意图. 经由DTW规整后的示教轨迹特征更加明显, 泛化输出时具有更低的不确定度.
得到右手臂的GMR泛化输出轨迹后, 利用第3节所述多机械臂的协同策略, 以
$ (x_{s1}^{\rm{I}},x_{s2}^{\rm{I}},x_{s3}^{\rm{I}},t) $ 作为查询向量, 实时获取左手臂的位置控制向量, 最后得到搬运实验时左、右手臂的协作执行轨迹如图12所示. 其中, 虚线表示原始的三条示教轨迹, 实线是泛化输出的执行轨迹. 原始示教轨迹由于噪声的存在, 轨迹的平滑性能较差. 本文所提出的DTW-GMM策略对噪声具有一定的抗干扰性, 能够充分提取示教轨迹的共同特征, 且最终输出的执行轨迹具有较好的平滑性.
图 12 搬运实验左右手臂轨迹泛化输出Fig. 12 Generalized dual-arm trajectory output in moving experiment4.1.4 实体机器人实验
在Pepper机器人平台上, 基于本文所提出的DTW-GMM策略, 以右手臂的空间位置信息和DTW规整后的时间信息为查询向量, 实时获取左右臂的协同执行轨迹信息. 在第3.2节设计的运动控制引擎作用下, 对轨迹进行跟踪控制. 图13是Pepper机器人双臂协作完成搬运实验的截图. 通过实验可以发现, 本文的多机械臂协同轨迹生成时间约为1 ms, 实验的硬件环境为16.00 GB RAM, Intel i7-7500U CPU, 软件环境为Matlab 2018a. 实验结果表明了基于DTW-GMM模型对控制机器人双机械臂完成协同任务的可行性.
4.2 汉字书写实验
汉字轨迹由笔画组成, 是一种离散化的轨迹[30-34]. 当前的汉字轨迹书写主要是基于预规划轨迹的执行策略. 通过将汉字轨迹分段进行规划, 并设计控制器跟踪所规划的笔画轨迹. 这种策略模式单一, 泛化性不足. 近年来, 研究人员希望通过示教学习的策略, 来学习汉字轨迹的特征, 提高轨迹生成的泛化性. 而当前基于示教学习的研究主要将汉字轨迹进行连续化处理, 这样会失去原有汉字轨迹的基本特征, 且当汉字轨迹较为复杂时进行连续化示教操作繁琐. 为此, 本节提出基于DTW-GMM的汉字轨迹书写策略.
由于汉字轨迹的书写主要是在x、y平面上的运动, 所以可以将某一个机械臂的x、y平面上的位置信息向量和经由DTW规整后的时间t, 即以
$ (x_{s1}^{\rm{I}},x_{s2}^{\rm{I}},t) $ 作为查询向量, 通过GMM和GMR的多机械臂轨迹的表征和泛化, 输出其他机械臂的协同轨迹. 图14(a)是基于本文提出的协同策略的汉字轨迹"木"的书写情况. 将“木”字按照笔画分解为“一”、“丨”、“丿”、“丶”四段轨迹, 由4个机械臂分别执行, 并以执行“一”的机械臂的位置矢量作为查询向量, 实时获取其余机械臂的协同轨迹数据. “木”字轨迹较为简单, 当汉字的笔画较为复杂时, 比如对于交叉型的轨迹, 可以适当调整策略, 通过将某些笔画分配给同一个机械臂, 可以减少需要进行协同的机械臂的数量, 减少书写任务的复杂性, 如对于汉字轨迹“打”字, 可以利用双机械臂分别对轨迹的“扌”和“丁”进行学习, 最终“打”字的轨迹表征与生成情况如图14(b)所示.
图 14 基于DTW-GMM算法的汉字轨迹书写Fig. 14 Chinese character trajectory generation based on DTW-GMM algorithm5. 结束语
本文提出了一种基于概率模型的多机械臂示教轨迹表征策略, 可以用于实现机器人的多臂协作. 利用DTW-GMM算法对原始示教轨迹进行时间规整并提取共同特征, 然后设计多机械臂协同控制策略, 最后基于GMR回归多机械臂的协同轨迹. Pepper机器人双臂协作完成搬运和汉字轨迹书写实验验证了本文所提出的基于DTW-GMM的多机械臂多任务协同策略的有效性. 下一步预期在轨迹生成时融入视觉信息反馈和基于强化学习的参数学习策略, 实现复杂环境下的多机械臂适应性协同轨迹的输出.
-
图 6 基于微分逆运动学的机器人运动控制引擎设计框图
Fig. 6 Block diagram of the motion control engine based on differential inverse kinematics
图 12 搬运实验左右手臂轨迹泛化输出
Fig. 12 Generalized dual-arm trajectory output in moving experiment
图 14 基于DTW-GMM算法的汉字轨迹书写
Fig. 14 Chinese character trajectory generation based on DTW-GMM algorithm
表 1 算法的时间复杂度
Table 1 Time complexity of algorithms
轨迹规划算法 时间复杂度 DP ${n_x} {n_y}$ GCTW $2dl{{m} } + 8{{ {m^3} } }$ 
下载: 导出CSV
表 2 DTW-GMM算法参数设置
Table 2 Parameter setting of the DTW-GMM algorithm
参数 参数值 $\delta$ $1 \times {10^{{\rm{ - 10}}}}$ ${i_{\max }}$ 10000 $\lambda$ 0.0001
下载: 导出CSV
表 3 GMM表征协方差矩阵表
Table 3 Covariance matrix of GMM algorithm
变量 t x y z t 1 0.0814 −0.0074 0.0280 x 0.0814 1 −0.0003 0.0010 y −0.0074 −0.0003 1 −0.0001 z 0.0280 0.0010 −0.0001 1
下载: 导出CSV
表 4 DTW-GMM表征协方差矩阵表
Table 4 Covariance matrix of DTW-GMM algorithm
变量 t x y z t 1 0.0078 −0.0063 0.0122 x 0.0078 1 −0.0001 0.0002 y −0.0063 −0.0001 1 0.0001 z 0.0122 0.0010 0.0001 1
下载: 导出CSV
-
[1] Kumar R, Berkelman P, Gupta P, Barnes A, Jensen P S, Whitcomb L L, et al. Preliminary experiments in cooperative human/robot force control for robot assisted microsurgical manipulation. In: Proceedings of the IEEE International Conference on Robotics and Automation. New York, USA: IEEE, 2000. 610−617 [2] Yan Y, Tang Z. Control architecture for autonomous multi-robot system: Survey and analysis. In: Proceedings of the International Conference on Intelligent Computation Technology and Automation. New York, USA: 2009. 376−379 [3] Zhi Y, Jouandeau N, Ali A. A survey and analysis of multi-robot coordination. International Journal of Advanced Robotic Systems, 2013, 10(1): 1-18 doi: 10.5772/52938 [4] Lim J, Chyung D H. On a control scheme for two cooperating robot arms. In: Proceedings of the IEEE Conference on Decision and Control. New York, USA: 1985. 334−337 [5] Ortenzi D, Muthusamy R, Freddi A, et al. Dual-arm cooperative manipulation under joint limit constraints. Robotics and Autonomous Systems, 2018, 99(1): 110-120 [6] Ramirez-Alpizar I G, Harada K, Yoshida E. Human-based framework for the assembly of elastic objects by a dual-arm robot. ROBOMECH Journal, 2017, 4(1): 20-30 doi: 10.1186/s40648-017-0088-0 [7] Tuan L A, Joo Y H, Duong P X, et al. Parameter estimator integrated-sliding mode control of dual arm robots. International Journal of Control, Automation and Systems, 2017, 15(6): 2754-2763 doi: 10.1007/s12555-017-0018-1 [8] Pastor P, Hoffmann H, Asfour T, Schaal S. Learning and generalization of motor skills by learning from demonstration. In: Proceedings of the IEEE International Conference on Robotics and Automation. Piscataway, USA: 2009. 763−768 [9] Kober J, Mülling K, Kroemer O, Lampert, C H, Schölkopf B, Peters J. Movement templates for learning of hitting and batting. In: Proceedings of the IEEE International Conference on Robotics and Automation. Alaska, USA: 2010. 853−858 [10] Li C, Yang C, Ju Z, et al. An enhanced teaching interface for a robot using DMP and GMR. International Journal of Intelligent Robotics and Applications, 2018, 2(1): 110-121 doi: 10.1007/s41315-018-0046-x [11] Zhao T, Deng M, Li Z, Hu Y. Cooperative manipulation for a mobile dual-arm robot using sequences of dynamic movement primitives. IEEE Transactions on Cognitive and Developmental Systems, to be published [12] 刘成菊, 耿烷东, 张长柱, 陈启军. 基于自学习中枢模式发生器的仿人机器人适应性行走控制. 自动化学报, 2021, 47(9): 2170− 2181Liu Cheng-Ju, Geng Wan-Dong, Zhang Chang-Zhu, Chen Qi-Jun. Adaptive locomotion control of humanoid robot based on self-learning CPG. Acta Automatica Sinica, 2021, 47 (9): 2170− 2181 [13] Khansari-Zadeh S M, Billard A. Learning stable nonlinear dynamical systems with Gaussian mixture models. IEEE Transactions on Robotics, 2011, 27(5): 943-957 doi: 10.1109/TRO.2011.2159412 [14] Hewitt A, Yang C G, Li Y, Cui R. DMP and GMR based teaching by demonstration for a KUKA LBR robot. In: Proceedings of the IEEE International Conference on Automation and Computing. Piscataway, USA: 2017. [15] Gribovskaya E, Khansari-Zadeh S M, Billard A. Learning non-linear multivariate dynamics of motion in robotic manipulators. International Journal of Robotics Research, 2011, 30(1): 80-117 doi: 10.1177/0278364910376251 [16] Chatzis S P, Korkinof D, Demiris Y. A quantum-statistical approach toward robot learning by demonstration. IEEE Transactions on Robotics, 2012, 28(6): 1371-1381 doi: 10.1109/TRO.2012.2203055 [17] Calinon S, Li Z B, Alizadeh T, Tsagarakis N G, Caldwell D G. Statistical dynamical systems for skills acquisition in humanoids. In: Proceedings of the IEEE-RAS International Conference on Humanoid Robots. Piscataway, USA: IEEE, 2012. 323−329 [18] Kim S, Gribovskaya E, Billard A. Learning motion dynamics to catch a moving object. In: Proceedings of the IEEE-RAS International Conference on Humanoid Robots. Piscataway, USA: 2010. 106−111 [19] Koc O, Maeda G, Peters J. Online optimal trajectory generation for robot table tennis. Robotics and Autonomous Systems, 2018, 105(1): 121-137 [20] Rabiner L, Juang B H. Fundamentals of Speech Recognition. Beijing: Tsinghua University Press, 1999. 200−232 [21] Zhou F, Torre F D L. Generalized canonical time warping. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2016, 38(2): 279-294 doi: 10.1109/TPAMI.2015.2414429 [22] Zhou F, Torre F D L. Generalized time warping for Multi-modal alignment of human motion. In: Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. New York, USA: 2012. 1282−1289 [23] Biship C M. Pattern Recognition and Machine Learning (Information Science and Statistics). New York: Springer-Verlag, 2007. 440−477 [24] Wu Y C. Gaussian mixture model. Journal of Neuroscience Methods, 2005, 182(1): 110-122 [25] Sekaran K, Chandana P, Krishna N M, Kadry S. Deep learning convolutional neural network (CNN) with Gaussian mixture model for predicting pancreatic cancer. Multimedia Tools and Applications, 2020, 79 (15): 10233-102 [26] Akaike H. A new look at the statistical model identification. IEEE Transactions on Automatic Control, 1974, 19(6): 716-723 doi: 10.1109/TAC.1974.1100705 [27] 田彦涛, 孙中波, 李宏扬, 王静. 动态双足机器人的控制与优化研究进展. 自动化学报, 2016, 42(8): 1142-1157Tian Yan-Tao, Sun Zhong-Bo, Li Hong-Yang, Wang Jing. A review of optimal and control strategies for dynamic walking bipedal robots. Acta Automatica Sinica, 2016, 42(8): 1142-1157 [28] Chopra N, Spong M W. On synchronization of Kuramoto oscillators. In: Proceedings of the 44th IEEE Conference on Decision and Control. Seville, Spain: 2005. 3916−3922 [29] Chiaverini S. Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators. IEEE Transactions on Robotics and Automation, 1997, 13(3): 398-410 doi: 10.1109/70.585902 [30] Xu S, Hao J, Lau F C M, et al. Computationally evaluating and reproducing the beauty of chinese calligraphy. IEEE Intelligent Systems, 2012, 27(3): 63-72 doi: 10.1109/MIS.2012.46 [31] Kulvicius T, Ning K J, Tamosiunaite M, et al. Joining movement sequences: modified dynamic movement primitives for robotics applications exemplified on handwriting. IEEE Transactions on Robotics, 2012, 28(1): 145-157 doi: 10.1109/TRO.2011.2163863 [32] Sun Y, Qian H, Xu Y. Robot learns Chinese calligraphy from demonstrations. In: Proceedings of the 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems. Chicago, USA: 2014. 4408−4413 [33] Chao F, Huang Y, Zhang X, et al. A robot calligraphy system: from simple to complex writing by human gestures. IEEE Transactions on Human-Machine Systems, 2016, 59(1): 1-14 [34] Chao F, Huang Y, Lin C M, et al. Use of automatic Chinese character decomposition and human gestures for Chinese calligraphy robots. IEEE Transactions on Human-Machine Systems, 2019, 49(1): 47-58 doi: 10.1109/THMS.2018.2882485 -
下载:
计量
- 文章访问数: 1285
- HTML全文浏览量: 329
- PDF下载量: 394
- 被引次数: 0
