基于改进动态系统稳定估计器的机器人技能学习方法

金聪聪 刘安东 LIU Steven 张文安

近年来,机器人产业高速发展,整体规模持续增长,在制造业和服务业等众多领域都有广泛应用.随着工业产品迭代速度日益增长,个性化需要与日俱增,传统依靠手工编程完成特定任务的方法难以适应新的需求.因此,迫切需要开发简单实用,且可以灵活适用于多种任务的机器人技能学习方法.

机器人示教学习(Learning from demonstration,LfD)的灵感最初来源于人类的模仿学习,近几年获得学术界和工业界的广泛关注[1–4].机器人通过观察用户演示来学习新技能,同时将新技能泛化到不同场景下的相似任务中去,一般包含演示、学习、复现三个阶段.演示阶段需要解决的问题是如何向机器人进行技能演示,常用的方法有视觉示教、动觉示教、遥操作示教和双臂示教.学习阶段是对技能进行建模,抽象的技能符号由示教数据具体表示,然后利用示教数据训练模型参数.复现阶段的性能主要体现在稳定性、复现精度、泛化能力和抗干扰性能4 个方面.

动态系统(Dynamical system,DS)是对机器人技能进行建模的常用方法[5],该方法将规划和执行集成到一起,并将所有可能的解决方案嵌入到模型中以实现目标[6].在非线性DS 基础上发展起来的动态运动原语只要进行一次演示就可以对运动进行建模[7],动态运动原语描述的运动模型由非线性DS和线性DS 组成,其中非线性部分保证了轨迹复现的相似度,线性部分则确保了模型全局稳定性,两者的切换依靠相位变量平稳进行.尽管动态运动原语提供了一种有效而精确的方法来对复杂的动态进行编码,但是单变量编码丢失了各自由度之间的相关信息,而且该方法本质上仍依赖于时间,在面对时间扰动时需要用启发式方法重置相位变量[8].

为弥补动态运动原语的缺陷,文献[9]提出了动态系统的稳定估计器(Stable estimator of dynamical systems,SEDS).它首先利用高斯混合模型(Gaussian mixture models,GMM)和高斯混合回归(Gaussian mixture regression,GMR)的概率学习方法对轨迹进行初步拟合.概率学习方法是轨迹编码中的常用方法,它可以保留演示的固有可变性[10],但是无法确保训练得到的动态系统具有全局稳定性.因此SEDS 在后续优化中加入了稳定性约束,确保机器人在不受扰动的情况下能够到达目标点.然而,过于严格的稳定性约束可能会在学习过程中限制模型的精度.针对SEDS 方法中稳定性和精度难以平衡的问题,文献[11]利用微分同胚变换改进了SEDS,称作τ-SEDS.该方法在保证系统稳定性的同时,很大程度上克服了模型的精度问题.但是增加了模型复杂度,导致学习更加耗时,仅限于离线学习.DS 方法的快速稳定学习[12]同时考虑了估计DS 的稳定性、准确性和学习速度三个因素.DS方法快速稳定学习的快速学习能力在很大程度上方便了它的实际应用,但在稳定性和准确性方面还不够优越.文献[13]提出了一种基于流形浸没和淹没的学习方法来解决精度与稳定性的矛盾,该方法保证了有效提取动力学特征和稳定形式的高精度,而且能处理交叉运动的情形,但模型复杂度较高.

SEDS 中另一个值得注意的问题是混合高斯分量个数的选取,但是对于该问题的相关研究较少.过多或者过少的分量个数选取都会导致模型无法有效提取演示的动力学特征,因此该问题具有一定的研究价值.通常用于确定有限混合模型的最佳分量的方法是贝叶斯信息准则[14],然而这种模型选择方法存在一些明显的缺陷[15],常常过高估计模型分量的个数,导致过拟合.贝叶斯非参数模型是一种定义在无限维参数空间上的贝叶斯模型,其利用在适当数量的模型分量密度上产生后验分布来调整模型大小,因此可以根据数据自适应聚类个数,其中狄利克雷过程混合模型是最常用的贝叶斯非参数模型之一[16−18].Figureoa 等[19]提出了一种物理一致的贝叶斯非参数混合模型,该方法可以自动估计最佳的混合分量个数,并且将相似性测度融入先验信息,提高了复现和泛化的精度.但是该方法使用吉布斯采样计算模型的后验概率,计算复杂度较高.

鉴于SEDS 存在的上述缺陷,本文提出了改进的SEDS (Improved SEDS,i-SEDS).该方法有效地解决了SEDS 中稳定性和精度无法兼顾的问题,并且可以自动确定合适的分量个数.仿真以及Franka-panda协作机器人的实验结果验证了本文方法的有效性和优越性.本文的主要贡献有:1)使用狄利克雷过程高斯混合模型(Dirichlet process GMM,DPGMM)代替GMM 拟合演示,并利用变分推断(Variational inference,VI)训练模型,该模型可以根据演示数据自动确定合适的混合分量个数.仿真分析超参数对DPGMM 的影响,为超参数的选择提供了指导意义;2)采用参数化的李雅普诺夫函数修改了原SEDS 中的稳定性约束条件,从而提高了学习轨迹的精度,解决了稳定性和精度难以兼顾的问题.

1 问题描述

将机器人离散运动公式化为由自治动态系统驱动的控制律.考虑一个状态变量ξ∈Rd,ξ通常表示笛卡尔空间中末端执行器的坐标.有时ξ也可用于定义机器人系统的广义坐标(例如,机械手的关节角度或轮式机器人的姿势)

式中,f(ξ) 是一个非线性连续可微函数,用于编码特定行为,例如拾取搬运物品,伸手拿杯子等.从初始位置ξ0开始,机器人运动ξt,t∈[0,+∞) 由式(1)积分得到,运动最后收敛至单一吸引子ξ∗,f(ξ∗)0.本文需要估计该吸引子上全局渐近稳定的动态系统f(ξ).

通常,将基于一组M条演示轨迹的数据对f(ξ) 进行估计,Tm表示第m条演示轨迹的序列长度,估计得到的DS 为一般采用回归算法进行估计,如高斯过程回归[20]、局部加权映射回归算法[21]和混合高斯回归等.但这些标准的回归技术在训练过程中没有考虑DS 的渐近稳定性,很可能是不稳定的或收敛至伪吸引子.因此SEDS 方法将不稳定DS 的建模转换为非线性优化问题,构造了全局稳定的运动估计,具体流程如图1 所示.

图1 基于SEDS 的机器人示教学习流程图Fig.1 Flow chart of LfD based on SEDS

上述方法有足够的灵活性来建模各种运动,但是无法确保DS 的渐近稳定性,为此,SEDS 给出了在保证系统稳定性下的参数学习方法,即通过最小化带非线性项约束的目标函数来估计参数 Θ.

式中,≺0表示矩阵的负定,是全部训练数据的总数量.目标函数J(Θ) 的选取有均方误差(Mean square error,MSE) 和对数似然(Log likelihood,LL)两种.

式中,对数ln 也可换成以其他常数为底的对数,上式的约束是全局稳定收敛的充分条件,这些条件由Lyapunov 第二稳定性方法推导得到,见定理1.

定理1.如果存在径向无界且满足式(5) 的Lyapunov 函数V(ξ):RdR,则DS 在ξ∗点处全局渐近稳定.

SEDS 方法可以有效地从演示中估计出具有全局渐近稳定性的DS,是LfD 领域中的常用方法,但是该方法存在两个问题:1) 用于估计初始DS 的GMM-GMR 方法需要人工指定混合分量个数K,当状态ξ维度升高或演示轨迹变得复杂时,通过经验确定超参数K将变得很困难;2)在SEDS 中,V(ξ)是二次李雅普诺夫函数(Quadratic Lyapunov function,QLF),即V(ξ)(ξ −ξ∗)T(ξ −ξ∗).在几何学上,QLF 只允许L2范数(即 ‖ξ −ξ∗‖2)距离是单调递减的轨迹[11].这导致了SEDS 在包含高曲率或非单调(暂时远离吸引子)的高度非线性运动中表现不佳,不能同时保证稳定性和精度性.

针对问题1),本文利用贝叶斯非参数模型能根据数据自动确定最佳聚类个数的特性,采用了DPGMM 拟合演示数据,并推导GMR 估计初始DS;针对问题2),本文选择参数化QLF (Parametrized QLF,P-QLF)来确保估计DS 的稳定性,即V(ξ)(ξ −ξ∗)TP(ξ −ξ∗).其中P的作用是将简单的QLF重塑为“椭圆”形式,放松了约束条件,从而能够精确复现不收缩(高曲率且朝向目标具有非单调性)的演示轨迹.

2 DPGMM-GMR 算法

DPGMM-GMR 算法由DPGMM 的建模过程、利用变分推断进行模型求解和推导GMR 估计初始DS 三部分组成.

2.1 狄利克雷过程高斯混合模型

狄利克雷过程(Dirichlet process,DP)一般用DP(G0,α)表示,其中G0是连续的基分布,α是集中参数,用来控制从DP 中抽取的分布的离散化程度[22].假设从DP 中随机抽取一个样本分布G,然后再从G中独立地抽取N个随机变量

通过SBP,可以将独立于G0绘制的原子θk视为混合模型分量的分布参数,该模型包含无限数量的分量,并且每个分量的权重为πk(v),从而建立了狄利克雷过程混合模型.

式中,Cat(·) 表示分类分布.

在狄利克雷过程混合模型的基础上明确定义基分布G0为Normal-Wishart 分布,即θk～NIW(θ0),就得到了DPGMM,这意味着每个高斯分量的均值和精度都服从一个独立的Normal-Wishart 分布,根据式(8),DPGMM 的公式化表示如下:

式中,µk和Λk分别表示第k个高斯分量的均值和精度,m0和β0分别是µk的高斯先验的均值和方差,W0和ν0分别是 Λk的Wishart 先验的精度矩阵和自由度.

式(9)是给定的共轭先验分布,式(10)是模型的似然函数.DPGMM 的概率结构如图2 所示.然而在无限维的设定下,实际中模型不易处理.因此本文采用一种常见的策略,即基于DP 的截断SBP[24].预定义最大的混合分量数,并且令即对于所有k >,πk(v) 等于零.

图2 DPGMM 概率结构图Fig.2 DPGMM probability structure diagram

2.2变分推断

第2.1 节已为所有随机变量明确定义了共轭先验和模型的似然函数,本节采用变分贝叶斯方法得出参数可靠的后验分布.将隐变量和未知参数表示为 Θ{v,Z,µ,Λ},超参数表示为Ξ{α,m0,β0,W0,ν0}.由于实际后验p(Θ|X,Ξ) 难以计算,因此变分推断方法引入任意分布q(Θ) 以逼近p(Θ|Ξ,X).在此假设下,导出模型的对数似然值 lnp(X),即:

其中

KL(q||p)表示变分后验q(Θ) 和实际后验p(Θ|X,Ξ)之间的KL(Kullback-Leibler)散度,刻画了两个分布之间的相似程度.为降低求解难度,一般采用平均场理论限制概率分布q(Θ) 的范围,然后寻找这个类别中使得KL 散度最小化的概率分布[25].因此需要将q(Θ) 分解为:

同时,考虑DPGMM 的共轭先验配置,根据共轭的定义,期望后验q(Θ) 与先验p(Θ) 具有相同的函数形式[26],可得:

指示变量zt满足[27]:

式中,〈·〉表示数学期望.

采用变分贝叶斯期望最大化(Expectation maximization,EM)算法.对该模型参数进行迭代求解.该算法类似于EM 算法,在E 步中,计算指示变量的期望;在M 步中,该期望值用于重新计算其他参数的变分分布.

期望通过变分推断得到DPGMM 模型参数的后验分布后,可以得到如下的模型联合概率密度:

式中,St(·)是学生t分布,各分布的权重πk(v) 按下式计算:

式中,〈vk〉1/(1+α).

2.3 GMR 估计初始DS

与基于GMM 的GMR 一样,先对式(16)的DPGMM 中的均值和方差进行划分.

给定输入位置ξt下输出速度的条件概率分布为:

其中

DPGMM 在拟合演示轨迹时,自动确定最佳分量个数K,其余不相关高斯分量权重趋向于0,因此可以丢弃πk(v)〉 值过低的高斯分量来简化计算.同时将式(20a)代入式(21)得到动态系统估计.

3 基于P-QLF 的稳定DS 估计

第2 节利用DPGMM-GMR 从演示中估计初始动态系统同其他的回归算法一样,训练得到的不具有全局稳定性.SEDS 方法针对该问题构造了具有稳定性约束的非线性规划问题,如式(3)所示.其不等式约束Ak+(Ak)T≺0 通过QLF 推导得到,作为DS 稳定收敛的充分条件,该约束过于严格,导致学习的DS 为了确保稳定性而使生成的轨迹偏离了演示,尤其是面对不收缩(高曲率且朝向目标具有非单调性)的演示轨迹.本节使用PQLF 代替QLF 进行稳定性约束的推导,从而解决SEDS 中存在的稳定性和精度无法平衡的问题.

P-QLF 通常用于确保线性时不变(Linear timeinvariant,LTI)系统的稳定性,如命题1 所述.

命题1.形式为+b的线性DS 在吸引子ξ∗处全局渐近收敛,当且仅当:

证明.见文献[19].

命题1 易扩展到式(22)表示的线性参数可变系统中,即Ak为LTI 参数,混合系数hk(ξ) 是状态相关的时变参数.如命题2 所述.

命题2.式(22)中定义的非线性DS 在吸引子ξ∗处全局渐近收敛,当且仅当

式中,P为对称正定矩阵.

证明.如果存在连续且可微的李雅普诺夫函数V(ξ)满足定理1,即可证明命题2.通过考虑以下形式的P-QLF 候选函数:

由于其二次形式,可以确保V(ξ)>0,第2 个条件是取V(ξ) 的时间导数:

将ξξ∗代入式(25)和(26),可以确保V(ξ∗)0,0.因此,如果满足条件(24),则式(22)相对于吸引子ξ∗是全局渐近稳定的.□

不难发现式(4)的约束条件是式(24)的一种特殊情况,即当P为单位矩阵时,式(24)就等同于式(4).SEDS 中稳定性和精度的两难问题本质上是因为稳定性约束过于严格,使得可行的参数空间无法满足精度要求.而基于P-QLF 推导得到的稳定性约束放松了对P的限制,只要满足P为对称正定矩阵即可,从而扩展了原问题的可行域.解决了SEDS学习不收缩轨迹时精度过低的问题.

4 实验

仿真实验在人类手写数据库(LASA 数据集)和Franka-panda 机器人上进行,实验结果验证了本文提出方法的有效性.

4.1 DPGMM-GMR 算法性能分析

尽管利用DPGMM 对演示进行拟合可以自动确定合适的聚类个数,但该方法引入了更多的超参数 Ξ{α,m0,β0,W0,ν0}.因此需要研究这些超参数对模型性能的影响,从而来指导超参数的选择.训练数据从LASA 数据集(该数据集是二维平面手写轨迹数据)中随机选取10 组演示.ξ表示二维笛卡尔位置,即ξ{ξ1,ξ2},演示数据集合X通常根据对称性,超参数m0会选择为X的均值.而超参数W0是一个 4×4 的矩阵,难以对所有矩阵元素进行分析,因此令W0ω×I,其中ω为缩放系数,是一个正标量.I是 4×4 的单位矩阵.只需要对α、β0、ω、ν0四个标量超参数进行研究.对10 组演示训练得到10 个DPGMM,用对数似然值给模型打分,数值越高则说明模型拟合效果越好.10 个模型的对数似然值相对于各个超参数(固定其他超参数只变化一个超参数)的变化趋势如图3 所示.由图3 可以看出:1)模型对于超参数的变化不敏感,一般ν0取数据维数即可,β0和ω取值不宜过大,α对模型影响最大,但只有在lnα>2时似然对数值才发生明显下降;2)对于任意一个超参数,10 个模型的对数似然值对于该超参数具有相同变化趋势,这意味着DPGMM 超参数具有可复用性,一组超参数能够适用于相同场景下的不同演示数据集,例如LASA 数据集,同一个机械臂操作空间下的不同任务演示等.DPGMM 超参数的不敏感性和可复用性使其在实际任务中的使用更加便捷.

图3 DPGMM 对数似然值关于超参数的变化趋势Fig.3 The change trend of DPGMM log-likelihood value on hyperparameters

接着,在整个LASA 数据集上定量评估基于VI 的DPGMM-GMR 算法拟合初始DS 的效果,超参数均设置为αβ0ω1,ν04.对比算法包括基于贝叶斯信息准则的GMM-GMR (EM 算法)、狄利克雷分布GMM-GMR (VI)和DPGMM-GMR,其中贝叶斯信息准则高斯混合模型最大评估分量设定为15 个,其余方法的截断分量也设置为15.性能指标采用以下形式均方根误差(Root mean square error,RMSE):

式中,M和T分别表示一组演示中轨迹的条数和所有的轨迹点个数.

4.2 i-SEDS 学习方法性能分析

本节i-SEDS 的性能分析同样在数据集LASA上进行,如果算法能够使复现的轨迹紧密(准确)地跟随演示轨迹,则表明该算法可以令人满意地解决稳定性和精度的难题.

由图4 可以看出,所有复现轨迹紧密跟随了演示轨迹,仿真结果说明了i-SEDS 的有效性.图5给出了SEDS 和i-SEDS 在部分不收缩轨迹上的复现结果.可以看出,SEDS 生成的轨迹完全偏离了演示,这是由于遵循QLF 稳定性结果导致的.图5定性地说明了i-SEDS 可以较好地解决SEDS 稳定性和精度的难题.

图4 i-SEDS 方法在LASA 数据集上的复现效果Fig.4 The reproductions of i-SEDS method on LASA dataset

图5 SEDS 和i-SEDS 在不收缩轨迹上的复现结果Fig.5 i-SEDS and SEDS reproductions on the non-contractive demonstrations

为了定量分析该方法的有效性,使用文献[28]提出的扫描误差区域(Swept error area,SEA).SEA 方法计算了演示轨迹和复现轨迹之间的面积,计算公式如下:

图6 扫描误差区域(SEA)示意图Fig.6 Schematic diagram of Swept error area (SEA)

考虑到原始SEDS 中2 种不同的目标函数MSE 和LL,以及i-SEDS 在学习不收缩演示轨迹时具有明显优势.因此利用SEA 方法定量分析i-SEDS (MSE)、i-SEDS (LL)、SEDS (MSE)、SEDS(LL)四种方法分别在收缩和不收缩演示轨迹上的性能.从LASA 数据集中取收缩轨迹和不收缩轨迹各10 组,分别使用上述4 种方法学习稳定DS,计算相应的SEA,结果如结果如图7所示.从图7(a)可以看出,四种方法的SEA 在收缩轨迹上差别并不明显,i-SEDS 仅略优于SEDS 方法.但在不收缩轨迹组的图7(b)中,i-SEDS 方法的复现精度要明显优于SEDS 方法.基于LL 的方法精度要略高于MSE,但MSE 的计算速度更快,因此,在实际使用中可以根据需求进行选择.

图7 4 种方法在收缩和不收缩轨迹上的SEA 分析Fig.7 Performance analysis of four methods on contraction and non-contraction demonstrations with SEA

4.3 机器人实验

为了验证本文方法的有效性,在7 自由度的机械臂Franka-panda 机器人上进行了实验验证.

机器人实验是在操作空间中学习物块搬运任务,如图8 所示.这是一个点到点运动的任务,机械臂需要学会如何从机械臂初始位置到达物块放置位置,然后拾取物块将其放入目标容器中,整个过程由拾取物块−放置物块2 部分组成,并利用Kinect 摄像头对物体进行跟踪.

图8 物品搬运任务示教流程图Fig.8 The process of object transport task teaching

1)示教:首先设置panda 机械臂为示教模式,再由演示人员拖动机械臂完成拾取物块−放置物块2 个步骤,整个示教过程如图8 所示.然后不断调整物块起始位置重复上述示教过程共8 次,同时以10 Hz频率记录机械臂末端位置和速度,获取8 条演示轨迹数据,记作

2)学习:在对演示轨迹建模前,需要对数据进行一些预处理.a)轨迹平滑:使用移动均值滤波对采集到的演示数据进行平滑处理.b)任务分割:为了降低任务的学习成本,将物品搬运任务分解成拾取物块−放置物块2 个子任务.根据2 个子任务切换过程中机械臂末端速度为零这一特点将演示轨迹分成2 段,然后各自进行学习.c)目标位置原点化:为保证动态系统的参数不会随目标位置的变化而变化,需要将参考坐标系的原点设在目标位置处.2 个子任务的演示轨迹都转化为以原点为终点的轨迹.预处理后的演示轨迹如图9 所示.

图9 预处理后的物品搬运任务的演示轨迹Fig.9 Preprocessed demonstrations of object transport task

使用i-SEDS 方法对预处理后的演示数据进行建模,用于2 个子任务学习的超参数均设置为α0.1,β0ω1,ν06.

3)复现.机械臂从初始位置开始,通过模型不断迭代得到当前位置下的期望速度将其作为逆运动学的输入驱动机械臂按照期望轨迹到达目标位置.记录每次生成的轨迹,获得的结果如图10 所示.所有复现轨迹都与演示具有相似的位置和速度轮廓,说明两者均遵循相似的动力学.

图10 物品搬运任务复现位置和速度轨迹Fig.10 Position and velocity trajectories of reproduction in object transport task

a)目标位置的变化.图11 显示了模型对环境变化的鲁棒性,其中虚线表示原始复现轨迹,实线表示在t4 s 时对目标位置进行转移后由动态系统重新规划的轨迹,可以看到该模型可以快速适应目标位置的变化,重新规划轨迹使机械臂到达目标.

图11 机械臂运行时改变目标位置后的轨迹变化Fig.11 Trajectory changes after moving the target container when the robot arm is running

b)抗扰动能力.图12 显示了模型对外界扰动的鲁棒性,在t4 s 时,施加扰动使机械臂脱离原始轨迹,其中虚线表示原始轨迹,实线表示机械臂在脱离原始轨迹后模型重新规划的轨迹,可以看到机械臂在受到外界扰动后仍能平稳到达目标位置,因此该模型具备一定的抗扰动能力.

图12 对机械臂施加外界扰动时的轨迹变化Fig.12 Trajectory changes when external disturbance is applied to the robot arm

上述机器人实验展示了i-SEDS 方法在确保稳定性的前提下,仍具有良好的复现精度以及泛化能力和抗扰动能力.

5 结束语

本文分析了SEDS 方法中存在的缺陷,并给出了合理的解决方案.首先利用DPGMM 对演示数据进行拟合,并使用变分推断求解模型参数,推导GMR 拟合初始DS.解决了基于EM 算法的GMM难以确定混合分量个数的难题.然后利用P-QLF推导新的宽稳定性约束取代原有约束,最后结合目标函数优化得到能精确复现的稳定DS.在LASA数据集中验证了DPGMM 超参数的性质和其推导的GMR 算法的性能,以及i-SEDS 方法在不收缩轨迹上的优越性能.最后通过Franka-panda协作机器人上的实验,验证了本文方法在实际场景的有效性.在未来工作中,将降低本文方法模型的复杂度.