改进差分进化算法优化的机器人时间最优轨迹规划算法

郭明明，刘满禄,2，张 华，王 姮，霍建文，朱晓明

(1.西南科技大学信息工程学院,四川 绵阳 621010；2.中国科学技术大学信息科学技术学院，安徽 合肥 230026)

0 引言

近年来，协作型工业机器人被广泛应用于生产线焊接、装配等任务。机器人快速、稳定的控制与轨迹规划方法备受学者的关注。因此，在保证协作工业机器人运行过程“人-机-环”安全的前提下，利用智能算法优化轨迹规划对于提高作业效率具有重要意义。

目前，国内外学者采用智能算法进行轨迹规划优化。Frederik Debrouwere等人[1]针对机器人的优化控制问题，引入非凸的加加约束，解决由凸框架产生的加速度跳跃问题。居鹤华等学者[2]提出了基于遗传算法的3-5-3多项式插值轨迹规划算法，通过与粒子群优化(particle swarm optimization,PSO)算法的3-5-3多项式机械臂轨迹规划对比，证明了所提算法在优化时间上的优越性。李小为等研究人员[3]在速度约束的前提下，采用PSO的时间最优3-5-3多项式插值的轨迹规划方法，证明了该方法能够准确地实现任意速度约束的时间最优轨迹规划。王学琨等学者[4]提出一种差分进化(differential evolution，DE)算法优化的时间最优3-5-3多项式插值机械臂轨迹规划方法，解决了传统轨迹规划方法效率不高的问题。但该方法未考虑人和机器人协作过程中的安全性问题，而且优化时间约束条件考虑不全面，优化时间未达到最优。

不同于以往未考虑协作型工业机器人在作业过程中的安全性问题，本文在确保“人-机-环”安全的基础上，采用改进DE时间最优3-5-3多项式插值的协作型机器人轨迹规划算法。该算法在满足运动学约束、动力学约束与负载约束的前提下，解决了传统的多项式插值轨迹规划方法效率低的问题。最后，通过UR5协作型机器人平台，验证了该算法的有效性和可行性。

1 UR5机器人的建模及获取工作空间

本文以协作型工业机器人UR5为试验对象。UR5机器人的位置由前三个关节确定，机器人的姿态由后三个关节确定。

本文利用随机概率方法获取协作型工业机器人的工作空间。蒙特卡洛方法[5-6]是一种使用随机数来解决计算问题的数值方法。采用蒙特卡洛方法求解UR5机器人的工作空间主要包括以下步骤。

①计算UR5机器人的正运动学方程的解，根据该解求出机器人末端参考点在机器人坐标系中的位置向量。

②求解其末端执行器的空间位置集合WΩ为：

WΩ(xi,yi,zi)=Γ[f(qi)]i=1,2,…,n

(1)

式中：f(x)为UR5机器人的正运行学；Γ(x)为机器人末端的空间位置。

③根据蒙特卡洛算法，由RAND函数随机产生一个值作为随机变量步长，则关节变量公式为：

(2)

根据上述步骤，可以获得UR5机器人的工作空间WΩ。

2 碰撞检测算法

协作型工业机器人在作业过程中，有可能与工作空间的人发生碰撞。在碰撞过程中，机器人的动量会发生较大变化[7-9]，因此，采用动量观测器方式检测是否发生碰撞。机器人在碰撞时紧急停止。

根据欧拉-拉格朗日方法，建立的UR5机器人的动力学方程如下：

(3)

基于广义动量的UR5机器人的系统描述如下：

(4)

(5)

则有：

(6)

结合式(3)、式(5)和式(6)可得：

(7)

对式(7)两边积分：

(8)

因此，定义残余向量：

(9)

式中:r为观测的外力矩;k为增益矩阵。

如果UR5机器人在工作空间与人发生碰撞，即存在一个未知的扰动τf，因此，动力学方程可以表示为：

(10)

则：

(11)

因此，对式(11)进行拉普拉斯变换可得：

(12)

所以，可根据r观测外部力矩τf。

3 基于改进DE的轨迹规划问题

3.1 优化目标函数选取

以3-5-3多项式插值为基础，通过改进DE方法选择一组最优的插值时间，使协作型机器人在执行某些焊接、装配任务时，保证运行时间最短，同时满足运动学约束[10]、动力学约束[11]和负载约束。依据第2节观测的外部力矩τf设计动力学约束，优化目标函数如下：

(13)

①运动学约束。

位置约束：

(14)

速度约束：

(15)

②动力学约束。

关节力矩约束：

(16)

如果机器人与人未发生碰撞，则τf=0。

负载约束：

Fgmin≤Fk≤Fgmaxk=1,2

(17)

以上公式中：i=1,2,…,n，由于UR5机器人有6个自由度，因此n=6；tij为第i个关节的运行轨迹的第1段、第2段和第3段的运行时间；f(t)为第i个关节完成三段总的运行时间；j为1,2,3；Fk为末端执行器的夹持力；Fgmin为协作型工业机器人和环境交互过程中的最小夹持力；Fgmax为最大夹持力。

3.2 多项式插值函数轨迹规划

3-5-3次多项式插值法是结合三次多项式插值和五次多项式插值提出的一种新的轨迹规划方法。该方法是在起始点和终止点中间选取两个中间点，将运行过程分成三段。第一段和第三段轨迹采用三次多项式插值，第二段轨迹采用五次多项式插值。第i个关节的3-5-3多项式插值的各段轨迹方程如下。

第一段三次曲线：

(18)

第二段五次曲线:

(19)

第三段三次曲线：

(20)

式中：θi1、θi2和θi3分别为第i个关节的第一段三次多项式的运行轨迹、第二段五次多项式的运行轨迹和第三段三次多项式的运行轨迹；ai1j、ai2j和ai3j分别为多项式系数；ti1、ti2和ti3分别为第i个关节的运行轨迹的第一段、第二段和第三段的运行时间。

为求出各多项式系数，已知条件为第i个关节各段的初始点xi0,中间点xi1、xi2和末端点xi3，初始点和终点的加速度和速度为0，路径点之间的速度和加速度连续。

根据以上推导条件，可以得出：

b=[0 0 0 0 0 0θi30 0θi00 0θi2θi1]T

(21)

a=A-1b

(22)

式中：A为关于时间的矩阵;A-1为A的逆矩阵。

a=[ai13ai12ai11ai10ai25ai24ai23ai22ai21ai20ai33ai32ai31ai30]T

3.3 基于改进DE的轨迹规划

本文在3-5-3多项式插值轨迹规划的基础上，利用给定的UR5机器人各个关节角度状态的起始点、终点、中间路径点，各关节的初始速度、加速度、最大运行速度，采用改进DE算法，以优化函数式(13)最小为目的，满足约束。

考虑到种群的多样性，在UR5机器人的工作空间，采用统一的概率分布。通过初始化种群，染色体采用实数编码[12]，随机产生每个个体的染色体为：

Xji=lowji+rand×(highji-lowji)

(23)

式中：highji为遗传基因染色体的上限；lowji为遗传基因染色体的下限；rand为[0,1]内的随机数；j∈[1,NP],NP为种群个体个数;i∈[1,M],M为遗传代数。

当选择的个体使式(13)值为最小时，选择的个体达到最优。

进化操作时，首先计算每个个体对应的交叉因子CR和变异因子F，然后按照DE/rand计算每个新个体，可以得到：

Vkj=Wr1G+F×(Vr2G-Vr3G)

(24)

式中：r1≠r2≠r3≠j，分别为[1，NP]内的随机整数；新个体V由W生成。

基于以上分析，本文提出的改进DE交互式工业机器人的时间最优轨迹规划算法流程图如图1所示。

图1 轨迹规划算法流程图

算法具体步骤如下。

①给出轨迹规划任务中的初始点、两个中间点，以及终止点的位置、速度和加速度，设置每个关节的最大允许位置、速度和加速度，对这三段轨迹的时间分别优化，设置优化目标函数的最大速度Vmax和最大关节力矩τmax。为了便于验证本文提出的轨迹规划方法，令τf=0。

②初始化差分进化算法的参数，设置N、M、F和CR。

③分别设置ti1、ti2、ti3的可行性范围，通过3-5-3多项式轨迹方程,求得每个关节的三段轨迹规划的时间，构成初始种群。

④对初始种群进行交叉、变异，得到试验种群。

⑤对得到的初始种群和试验种群进行适应度计算，查看是否超过设定的最大值。如果初始种群和试验种群的粒子都满足速度要求，则选择较小的t；如果有一个粒子没有满足，则选择满足的粒子；如果都不满足，则选择超出约束量小的粒子。

⑥判断是否达到终止条件。如没有达到条件，则转到步骤④。

⑦重复步骤②到步骤⑥的操作，分别得到三段轨迹的最优时间，把它们相加得到整个轨迹规划最优时间。

4 试验结果与分析

本文以六自由度协作型工业机器人UR5为研究对象，采用D-H法建立UR5机器人模型，在MATLAB平台下，验证本文所提出的基于改进DE的时间最优的轨迹规划算法。为方便验证，本文以UR5机器人的前三个关节为研究对象，后三个关节的验证和前三个关节一致。在前文中已经建立了UR5机器人的模型，并求取了工作空间。在工作空间下，获得UR5机器人的初始位置和末端任务位置，在确保无碰撞的前提下，选取初始位置和末端任务位置的中间点。前三个关节对应的角度状态如表1所示。

表1 角度状态

使用3-5-3多项式插值的轨迹规划进行点到点的运动，多项式插值轨迹规划图如图2所示。单独使用3-5-3多项式插值进行轨迹规划，设置第一段轨迹运行时间为2 s，第二段轨迹运行时间为4 s，第三段轨迹运行时间为2 s，起始点终止点的速度、加速度均为0。由三个关节的位置曲线、速度曲线和加速度曲线可以看出，单独使用3-5-3多项式插值法规划机器人的运行轨迹的位置、速度和加速度都连续，但运行速度并未接近最大速度Vmax，加速度的变化幅度也较大。加速度的抖动也会导致UR5机器人在作业过程中对设备本身产生不良影响。

图2 多项式插值轨迹规划图

使用基于改进DE优化的3-5-3多项式插值法，根据3.3节中的步骤优化。设置N为50，M为150，F为0.5，CR为0.7，时间取值范围为0.5～5 s，三个关节对应的角度状态同表1。设置UR5机器人运行最大角速度设置为3.14 rad/s，UR5机器人的最大关节力矩为100 N，协作型工业机器人和环境交互的夹持力的最小值为0 N，最大值为50 N。总体进化代数最优时间曲线如图3所示。

图3 总体进化代数最优时间曲线

经过优化得到第一段轨迹最优时间为1.151 s，第二段轨迹最优时间为2.351 s，第三段最优时间为0.800 s，所以总的运行时间为4.302 s，比未进行优化的3-5-3多项式轨迹规划节省了3.698 s。经过改进DE优化后的UR5机器人的位置曲线、速度曲线、加速度曲线均连续，能够在满足运动学约束、动力学约束和负载约束的前提下，使UR5机器人在不损坏设备的情况下，执行任务时间更短。改进机器人关节性能曲线如图4所示。

图4 改进机器人关节性能曲线

5 结束语

在执行某些焊接、装配任务时，协作型工业机器人在运行过程的关节状态可能因没有达到最大约束而影响效率。在确保“人-机-环”安全的前提下，提出了碰撞检测方法和基于改进DE的机器人时间优化方法。在3-5-3多项式插值的协作型工业机器人的轨迹规划的基础上，基于改进DE的协作型工业机器人的作业时间可以减少3.699 s，优于传统方法。通过试验验证了该方法的可行性和有效性。

与现有的基于差分进化算法等智能算法的机器人轨迹规划不同，本文方法能够根据DE的优化能力对时间进行优化，该方法的不足之处在于DE的收敛速度相对较慢。

[1] DEBROUWERE F,LOOCK W V,PIPELEERS G,et al.Time-optimal path following for robots with trajectory jerk constraints using sequential convex programming[C]//IEEE International Conference on Robotics and Automation.2013:1916-1921.

[2] 居鹤华,付荣.基于GA的时间最优机械臂轨迹规划算法[J].计算机应用研究,2011,19(9):3275-3278.

[3] 李小为,胡立坤,王琥.速度约束下PSO的六自由度机械臂时间最优轨迹规划[J].智能系统学报,2015(3):393-398.

[4] 王学琨,李刚,周东凯,等.基于DE的时间最优6-DOF机械臂轨迹规划算法[J].计算机仿真,2015,32(8):332-337.

[5] 印峰,王耀南,余洪山.基于蒙特卡罗方法的除冰机器人作业空间边界提取[J].控制理论与应用,2010(7):891-896.

[6] 王婷.RoboCup人形组足球机器人的蒙特卡罗自主定位算法研究[D].长春：吉林大学,2012.

[7] ZHIJU M A,HAIBIN W U.Sensorless collision detection for robot manipulator[J].Journal of Test & Measurement Technology,2013,27(3):254-259.

[8] HE S,YE J,LI Z,et al.A momentum-based collision detection algorithm for industrial robots[C]// IEEE International Conference on Robotics and Biomimetics,IEEE,2015:1253-1259.

[9] 吴海彬,李实懿,吴国魁.基于动量偏差观测器的机器人碰撞检测算法[J].电机与控制学报,2015,19(5):97-104.

[10]FARES J,FRANCISCO V,VICENTE M.Evolutionary path planning algorithm for industrial Robots[J].Advanced Robotics,2012,26(11-12):1369-1392.

[11]LUO L P,YUAN C,YAN R J,et al.Trajectory planning for energy minimization of industry robotic manipulators using the Lagrange interpolation method[J].International Journal of Precision Engineering & Manufacturing,2015,16(5):911-917.

[12]李飞,刘建昌,石怀涛,等.基于分解和差分进化的多目标粒子群优化算法[J].控制与决策,2017(3):403-410.