基于泛函梯度优化的运动规划算法

傅来磊，易 阳，张兴华

(南京工业大学 电气工程与控制科学学院，江苏 南京 211816)

0 引 言

复杂环境中高维机械臂规划有效运动需要长达数秒的时间，而人体往往能够快速生成丰富多样的运动轨迹。原因在于，人类运动系统有效利用了动作技能的先验知识，能够根据不同的环境和任务直接对运动轨迹进行优化，保证了运动轨迹生成的快速性[1]。这种基于样本经验的轨迹生成机制成为了机械臂运动规划的热点研究问题[2]。难点在于如何将外界任务环境的变化映射到轨迹规划的空间并保证计算效率。

动态运动基元模型[3]能够将运动轨迹空间以参数的形式呈现，通过效用函数对动作特征的参数进行优化，直到找到满意的动作。动态运动基元对机械臂各维度单独进行优化，降低了动作空间的维度，大大简化了搜索空间，使得策略搜索的探测过程更为有安全有效。但是在连续动作空间该方法还是很难解决实际中的问题。

近年来泛函梯度算法表现出良好的计算效率，基本思路是设计基于运动轨迹的目标泛函来评价外界环境，从而在数学上可计算求取目标泛函对运动轨迹的梯度，直接采用梯度下降的方法重新规划平滑无碰撞的运动轨迹[4,5]。

受此启发，本文使用泛函梯度对运动样本进行优化，不同的是，使用动态运动基元提取运动特征，设计基于外界环境信息和内部控制输入为动作轨迹的目标泛函，通过求取目标泛函的泛函梯度快速优化运动轨迹，使其满足无碰撞及高能效等性能要求。与现有算法相比，该方法具备以下优点：

(1)无需大量样本数据存储与训练；

(2)对运动轨迹控制输入进行优化，提高了轨迹能量效率；

(3)简化了搜索空间同时提高了计算效率，能够适用于连续动作空间优化。

1 系统简介

通常外界环境与工作任务是在低维笛卡尔空间中表示，多自由度机械臂运动轨迹是在高维关节空间中描述，而运动轨迹经验是在高维隐空间中特征。本文使用动态运动基元模型将关节空间的机械臂运动轨迹编码为隐空间内的轨迹特征。然后设计以该特征的目标泛函，包含当前机械臂所处的外界物理环境成本和执行当前运动轨迹所需要的控制成本作为性能指标函数。由此，目标泛函将隐空间内的运动特征与笛卡尔空间内的工作任务联系起来。将已习得的运动特征经验作为初值，然后采用梯度下降法更新迭代直到获得局部最优策略。最后求取的最优策略通过动态运动基元解码，获得无碰撞且高能效的期望参考轨迹。系统框架如图1所示。

图1 系统框架概述

2 算法实现

2.1 动态运动基元编码

动态运动基元是轨迹模仿学习的主流方式[6]。其思想是以一类动力系统来描述机器人运动轨迹，使其确保运动的稳定性与收敛性。定义(t),(t),(t) 为单个自由度的位置、速度和加速度状态，单个动作可表示为

(1)

其中，αz(βz(g-(t))-(t)) 为弹性系统，αzβz为弹性系统，g为轨迹目标，τ为时间常数。弹性系统驱动系统状态收敛到轨迹目标。F(x) 为强迫项，为非线性的核函数的线性组合，利用非线性特征得到丰富的轨迹形状

(2)

其中，ω是可调节的线性权重，φi(x) 为基函数矩阵，是x的函数。x通过如下正则系统产生，对运动时间进行非线性归一化

φi(x)=exp(-hi(x-ci)2)

(3)

(4)

由此，设置不同的强迫项F(x)， 即动作特征ω， 系统即生成收敛于目标的不同轨迹形状。图2表示通过动态运动基元对运动轨迹和动作特征之间进行编码与解码关系。

图2 动态运动基元模型

为了将运动轨迹显示的表达为动作特征的函数，使用有限差分矩阵D1和D2

(k)=A(k)ω+Bg

其中

(5)

(6)

将轨迹用K个离散点表示，即

=[(1),…(k),…(K)]

离散轨迹特征为

(ω)=Aω+Bg

(7)

其中，A=[A(1),…A(k),…A(K)]T，B=IKB。

2.2 目标泛函设计

图3 目标泛函设计

将目标泛函C() 设计为关节限制约束下无碰撞项和节能项的加权和

C[]=λ1Clim+λ2Cobs[]+Ceff[]

(8)

第一个必须满足的条件是关节角度的限制Clim，设计为

(9)

第二项对于无碰撞目标泛函Cobs， 由于障碍物则位于世界坐标系下的笛卡尔空间，关节轨迹位于机械臂内部关节空间，使用正向运动学计算机械臂本体在给某一姿态时刻的世界坐标位置：K((i),b)， 其中b∈Brob为机械臂本体上某一点。则在运动轨迹某一时刻，设计机械臂本体与障碍物距离之间的惩罚函数c((k))∶R3→R可设计为

c(

(10)

其中，d() 是机器人运动轨迹与最近的障碍物边界的距离，0指障碍物的最小安全缓冲距离。考虑整体运动轨迹和机械臂本体所有离散点，τ()={(k,b),k∈[1,K],b∈Brob} 使用最值约化算子，无障碍的目标泛函可设计为

(11)

(12)

这里M为机械臂系统惯性矩阵，C为科氏矩阵，G为重力项。使用线性反馈控制器对目标参考轨迹(t),(t),(t) 进行追踪，控制u为

u=diag(Kpos)(-y)+diag(Kvel)(

(13)

能量损耗为控制输入的总和，则

(14)

综上所定义的目标泛函，则理想的无碰撞高能效的运动轨迹为该目标泛函的最优值

(15)

2.3 运动轨迹优化

使用泛函梯度对运动轨迹进行优化，是将运动轨迹整体看作一个几何对象，在无限维空间中即为一个点[7]。泛函梯度沿着泛函C[] 下降速度最快的方向移动轨迹∈Ξ。 这里泛函梯度是针对整体轨迹而言，并非某个参数的梯度而言。因此，泛函梯度可能沿着对参数变化较小，某些参数变化较大的方向进行，与一般的参数梯度有所区别。定义为泛函梯度。为了说明轨迹空间Ξ的几何结构，在运动轨迹中加入微小的扰动量φ后，看轨迹如何变化

(16)

本文使用统计学中Kullback-Leibler(KL)散度距离进行表示轨迹的变化

DKL(+εφ)=Ε[log()-log(+εφ)]

DKL(+εφ)≈-εTΕ
TΕ

作为一阶项

Ε
DKL(+εφ)≈-εTGε

定义二次型的费雪信息矩阵评估扰动程度

F=-Ε

(17)

可见费雪信息矩阵有助于描述基于KL距离变化的最陡下降方向，而与具体的参数无关。也就是说，轨迹空间有着自身的几何形状，泛函梯度指向几何变换最快的方向，并不是某个参数的变化方向。实践中，费雪信息矩阵可以通过数值计算。

给定式(8)目标泛函，可由迭代接近法求取式(15)中的优化值。在当前轨迹i附近的目标泛函处线性化，近似得基于泛函梯度的目标泛函迭代更新值

C(i)(-i)+ο((-i)2)

(18)

每次迭代i中，对目标泛函进行一阶泰勒近似，并取最大值

(19)

(20)

则迭代更新规则变为

(21)

由此，更新规则式(21)将局部梯度变化扩展至轨迹的其余部分。解决该优化问题后，带入式(7)可得动作特征的迭代式

(22)

最后根据动态运动基元模型解码为期望参考轨迹并实现机械臂的实时控制。

3 实验设置和结果

首先在matlab中验证在平面轨迹优化中泛函梯度优化性能。接着在v-rep中使用六自由度机械臂(kinova Jacob2)来验证算法的可行性，采用matlab和v-rep通讯的方式进行实验。目的是在有障碍环境和高能效要求等约束条件下快速生成可行的参考轨迹。如第2节所述，初始动作特征用ω0和常数φ来表示。高斯基函数的数量为Nbasi=10， 中心点为ci=e-0.9ni,ni=1,2,…Nbasi， 宽度为hi=0.03，τ=1， 时间步数为Ntimestep=10。 所有常数φ是一个10×10的矩阵，而ω0为10×1的向量。动态运动基元的参数设置如下：αz=2,βz=0.5,αt=0.9。 实验中设置目标的位置g后，通过简单计算即可得到初始轨迹 ((t),(t),(t))。

3.1 平面轨迹优化

在matlab中设计一条平面轨迹，通过DMP模型，将两条轨迹参数化，并设计两个障碍物，分别通过普通的梯度优化方式和泛函梯度优化的方式对轨迹进行优化，结果如图4所示。

图4 平面避障轨迹对比

相比与泛函梯度优化，普通的梯度优化方法，虽然也能够完成避障的任务，但是通常是对于避障点局部优化，没有考虑轨迹的整体性，轨迹整体的变化较大。而泛函梯度优化能够考虑轨迹的整体性，在满足避障要求的情况下，轨迹的变化较小，保留了轨迹原有的经验特征，在以后的任务中，仍然可以当作经验学习。

3.2 机械臂快速避障

在桌面工作区随机摆放物体，创建随机工作场景。目标位置为(0,0,0)m，而机器人初始末端执行器位置为(0.1，0.4,0.1)m。在图5中，下方穿越障碍物方块的为原始轨迹，上方机械臂的末端抓手运行的轨迹为优化以后的轨迹。获取目标g的初始轨迹会撞到障碍物。这种情况下，末端执行器应该向上移动才能避开障碍物，然后再向下移动获取目标。运用式(8)的目标泛函优化运动轨迹避开碰撞同时满足关节限制约束。如图6所示，以变化较大的关节-3为例，初始轨迹几乎是直线，以固定步长η=0.4对动作特征ωi进行4次迭代，各个关节的轨迹就能在图5中表现为无碰撞的期望运动轨迹。

图5 机械臂kinova Jacob2抓取了桌上设有障碍的杯子

图6 关节-3 关节角度迭代

3.3 机械臂高能效运动

本实验中更新的机械臂的运动目标，机械臂需更新运动轨迹实现高能效的性能指标。如式(13)所示，设置控制器参数为Kpos=diag-10,-500,-100,-80,-50和Kvel=diag-1,-20,-20,-10,-5。 在能效约束的目标泛函下对初始轨迹进行重新规划，如图7所示，左侧轨迹为原始轨迹，右侧机械臂末端抓手的运行轨迹，为能耗优化以后的轨迹。图8以能耗最大的关节-2为例，关节-2的能耗占整体50%左右，可以看出，优化以后的关节-2能耗下降十分明显。

图7 通过动作特征优化形成高能效的运动轨迹

图8 关节-2力矩迭代

另外，对该算法的泛化能力进行了测试。在整个工作区间随机生成25个目标点实验，记录能效降低程度和迭代次数。如表1所示，大部分优化的轨迹能够在小于6次迭代级情况下，能耗下降在20%以上。

表1 迭代次数与能耗减少

4 结束语

本文提出了基于泛函梯度优化的运动规划算法，结合了动态运动基元的轨迹特征提取方法和费雪信息矩阵的泛函梯度描述，通过对初始经验轨迹的优化，使得多自由度机械臂能够快速生成无碰撞且高能效的运动轨迹，体现出了良好的性能。但是在本次实验中，所有的经验轨迹都是通过人为选取，机械臂系统仍然无法通过所执行的任务和外部环境来自我判断选择经验轨迹，没有达到真正意义上的智能化。所以如何建立机械臂系统对于运动轨迹和外部环境之间的学习机制将会是一个重要的研究方向。