一种基于GA-PSO算法的示教再现轨迹优化方法与流程

本发明涉及工业机器人操作控制领域，尤其是涉及一种基于ga-pso算法的示教再现轨迹优化方法。

背景技术：

示教再现是目前主流的工业机器人操作控制方式，常用的示教方法包括示教盒示教、操纵杆示教、主从机器人示教以及直接示教。

其中，直接示教法能以直接拖动方式实现复杂轨迹的示教，具有操作简单、示教方便等优点，在一些要求执行复杂轨迹的任务中有很高的应用价值。但是由于人手在直接示教的时候，可能会存在抖动，而且在示教过程中很难时刻满足机器人的角速度约束与角加速度约束，若直接采用示教轨迹跟踪的方式进行示教再现，机器人在运动过程中会产生抖动，或者不满足其运动学约束(主要是速度约束或加速度约束)而影响跟踪精度，甚至缩短机器人的使用寿命与机械性能。因此，需要对示教轨迹进行规划与优化。

目前在国内存在一些与机器人直接示教相关的专利，但是其中大多数都是关于使用示教盒或操纵杆进行示教，这些示教方法对操作人员有着较高的编程要求，而且示教步骤繁杂，不适用于对复杂轨迹任务的示教。还有少数专利使用直接示教的方式，但是这些专利一般只通过对采集到的轨迹序列进行滤波处理来消除高频抖动，然后根据数据复现周期与控制周期，一次发送给机器人，实现轨迹再现。目前公开的专利都没有考虑机器人本身的运动学约束(速度约束与加速度约束)。若要使得机器人能平稳运行，这就要求操作人员进行示教的同时要考虑机器人的运动学约束，操作技巧要求高。然而现实中，对于某些复杂任务，很难拖动操作人员直接示教出符合任务要求与机器人运动学约束的示教轨迹。对这些轨迹进行再现时，需要后期对示教轨迹进行人工优化。这种方式造成的时间与人力成本高，一定程度上减少了了直接示教技术适用的应用场景。

综上所述，传统的示教技术使用示教盒或操纵杆进行示教，编程要求高，步骤繁杂，不适用于复杂任务，而直接示教方法对操作人员技巧要求高，耗时大，不易得到符合实际要求的示教轨迹。

技术实现要素：

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于ga-pso算法的示教再现轨迹优化方法。

本发明的目的可以通过以下技术方法来实现：

一种基于ga-pso算法的示教再现轨迹优化方法，包括以下步骤：

1)进行直接示教时，根据设定的采样周期对机器人的轨迹进行下采样，实时记录机器人各关节的角度值，并将各关节时间序列存储在上位机中；

2)采用ga-pso算法作为优化算法，以最小化示教轨迹的运动时间为最优化目标，以机器人的运动满足小于最大加速度与最大运行速度为限制条件，引入惩罚函数进行最优化求解，获取最优的轨迹点时间间隔序列；

3)根据优化后时间间隔序列和对应的采样轨迹点进行三次样条插值，得到优化后的示教轨迹；

4)在进行示教轨迹再现时，将优化后的示教轨迹根据控制周期进行离散化，发送给机器人控制器，完整再现整个示教轨迹。

所述的步骤1)中，采样周期大于机器人的控制周期，用以去除不必要的冗余信息，消除轨迹中的高频噪声。

所述的步骤1)中，下采样得到的采样轨迹为：

其中，qr为第r个周期各关节的角度信息，n为轨迹点的总个数。

所述的步骤2)具体包括以下步骤：

21)构建ga-pso算法的目标函数为：

hi＝ti+1-ti

其中，hi为相邻的第i个和第i+1个轨迹点之间的运动时间间隔，ti、ti+1为第i个和第i+1个轨迹点对应的时间点，分别为第j关节的第i个轨迹点位置和第i+1个关键点位置之间的角速度以及角加速度，为最大角速度，为最大角加速度；

22)引入惩罚函数，根据目标函数设计适应度函数，则有：

其中，g为惩罚函数，与分别为第j个关节在第i个时间间隔内的最大角速度与最大角加速度，α与β为对角速度与角加速度超限的惩罚权重，l为关节总数；

23)将适应度函数作为优化函数求解生成最优的轨迹点时间间隔序列。

所述的步骤23)具体包括以下步骤：

231)初始化迭代次数i＝1、遗传算法种群p1及粒子群算法种群p2，根据速度约束确定搜索范围

232)对种群p1和p2中的个体进行差分计算，得到相应的各个关节的角速度与角加速度；

233)根据适应度函数计算每个个体的适应度，分别对种群p1和p2按照适应度大小进行排序；

234)对种群p1进行遗传算法更新，即进行选择、交叉以及变异操作；

235)对种群p2进行粒子群算法操作，即统计出种群最优解以及个体历史最优解，根据两个最优解，更新每个粒子的位置信息及速度信息；

236)分别对种群p1和p2按照其适应度大小进行排序，将排序后的p1中的前γ个体替换掉p2的后γ个体，并将p2中的前γ个体替换掉p1的后γ个体，实现遗传算法与粒子群算法的信息交互融合；

237)迭代次数i＝i+1，判断是否找到全局最优解或达到了最大迭代次数，若是，则输出最优解，若否，则返回步骤232)。

所述的步骤3)具体为：

根据优化后时间间隔序列将对应的采样轨迹点分为多个分段区间，分别对每个关节进行三次样条曲线插值，其中，每个关节的角度轨迹均为一三次多项式，且满足以下条件：

轨迹起点和终点均通过采样轨迹点；

相邻采样轨迹点之间速度连续且加速度连续；

轨迹起点和终点速度均为0。

与现有技术相比，本发明具有以下优点：

1、相比于后续对采样轨迹滤波的方式，本发明通过下采样加插值的方式消除由于人手在直接示教时引入的高频抖动，能在保证运行精度的条件下，有效地去除冗余的轨迹信息，减小了存储的轨迹点数量。

2、对采集到的轨迹，在考虑机器人运动学约束下，利用ga-pso算法进行示教轨迹优化，使得机器人示教再现时能平稳运行，不会出现速度加速度突变，并能保持较高的跟踪精度。并且能提高机器人的使用寿命和机械性能。同时，本方案易于操作，降低了拖动示教操作人员的要求，在示教过程中不需要考虑机器人的机械结构限制。

3、采用三次样条曲线的插值方式，使得关键点之间的插值更加符合运动学规律，使得机器人运行更为平稳，不出现停顿或突然加速，增加了整个运行过程的安全性。

附图说明

图1为本发明的整体流程图。

图2为ga-pso算法流程图。

图3为示教重现轨迹与原示教轨迹对比。

图4为优化前各关节角度。

图5为优化后各关节角度。

图6为优化前各关节角速度。

图7为优化后各关节角速度。

图8为优化前各关节角加速度。

图9为优化后各关节角加速度。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

如图1所示，本发明提出了一种基于ga-pso算法的示教再现轨迹优化方法，对轨迹点间的时间间隔进行优化，使得示教轨迹既能满足机器人的运动学约束，又能达到时间最优，降低了对操作人员的要求，使直接示教变得简单易行。

本发明对示教轨迹进行下采样，采用ga-pso优化算法对轨迹点间的时间间隔进行优化，并采用三次样条曲线对轨迹点进行插值，得到时间最优且符合角速度与角加速度约束的示教轨迹。

具体包括以下步骤：

1)示教轨迹采样：操作人员拖动机器人末端进行示教时，根据设定好的采样周期，实时记录机器人各关节的角度值，并将这些关节时间序列存储在上位机中；

本发明对示教轨迹采取下采样策略，即对轨迹的采样周期大于机器人的控制周期，这是因为在直接示教过程中，若采样周期等于控制周期，获得的稠密示教轨迹数据存在着大量冗余信息，这些冗余信息会可能存在着高频噪声，在示教重现时容易引起机器人的抖动，影响运动精度，降低机器人的寿命。采用下采样的策略可以去除不必要的冗余信息，消除轨迹中的高频噪声，同时轨迹点的减少可以很大程度地降低轨迹优化算法的优化难度，提高处理效率。

本方法针对的机器人为一款七自由度的协作机器人，该机器人控制系统的控制周期为10ms，因此在直接示教中，将采样周期设置为100ms，该采样周期较为合适，既能去除大部分的高频噪声，同时能保留轨迹中的关键信息，确保示教重现的轨迹精度。在每个采样时刻，记录机器人各关节的角度信息，得到的采样轨迹为：

其中，qr为第r个周期各关节的角度信息，n为轨迹点的总个数，表示第i个周期的第1至第7个关节的角度信息。

本发明记录的是各关节角度信息而非末端位姿信息，这是因为对于本方法的七自由度协作机器人而言，由于其冗余特性，使得其逆运动学是多解的，即一个末端位姿可以对应无穷多个关节角度，而正运动学是唯一的，不存在多解，即一个关节角度唯一对应着一个末端位姿，不存在多解，这样在进行轨迹重现时就不需要考虑逆运动学的多解性，再现的轨迹在关节空间与笛卡尔空间上都将是唯一且可以重复再现的。

2)基于ga-pso的时间最优轨迹优化：采用ga-pso算法作为优化算法，以最小化示教轨迹的运动时间为最优化目标，以机器人运动学约束(即机器人的运动需满足小于最大加速度与最大运行速度)为限制条件，优化出最优的轨迹点时间间隔序列，具体为：

21)适应度函数设计

设t1,t2,…tn为各关键点(轨迹点)对应的时间序列，h1,h2,...hn-1为相邻的两个关键点之间的运动时间，即hi＝ti+1-ti，该变量即为我们所要优化的变量。对于本方法而言，需要优化出一条时间最优的轨迹，即在满足运动学约束的情况下，合理调节每个关键点之间的时间间隔，使得机器人完成轨迹的时间最低，因此，优化的目标函数为：

其中分别为第j关节的第i个关键点和第i+1个关键点之间的角速度以及角加速度，该角速度要小于该关节的最大角速度角加速度要小于该关节的最大角加速度

为了能使用最优化算法进行优化，需要对公式(3-1)进行调整，设计适应度函数为：

其中：

其中g为惩罚函数，与分别为第j个关节在第i个时间间隔内的最大角速度与最大角加速度，α与β为对角速度与角加速度超限的惩罚权重，h表示为所有关节的时间间隔序列集合，g表示所有符合最大加速度和最大速度约束限制的时间间隔序列的集合。

可以看到，该适应度函数分为两个部分，第一部分为关键点间时间间隔之和，即运动时间之和，第二部分为一个惩罚函数g，若轨迹没有违反运动学约束，则惩罚项为0，否则当某一关节的最大角速度或者最大角加速度超过了机器人的运动学约束，那么就施以惩罚，超出越多，则惩罚越大，并成二次函数关系，适应度越小，从而促使算法搜索出符合运动学约束的时间最小解。通过引入该惩罚函数，将运动学约束融入到适应度函数中，使得适应度为一个连续函数，而且在无效解区域也拥有梯度，提高了优化算法的搜索效率与收敛速度，有效解决有约束的优化问题。

对于式(3-2)中的适应度函数，本例中采用最优算法进行优化求解。

22)遗传算法与粒子群算法

遗传算法(ga)是一种基于自然选择原理和遗传机制的优化算法，它的核心为：选择、交叉以及变异操作.它通过这三种操作来模拟自然界中优胜劣汰以及物种进化的机理，驱使算法向问题的全局最优值收敛，最终找到最优解。

粒子群算法(pso)也是一种高效的随机搜索优化算法，它以一系列的粒子来模拟鸟类的捕食行为，对优化问题的最优解进行并行搜索.每次迭代的过程中，全体粒子都会更新自身的最优位置pbest，并推举出群体的最优位置gbest，根据这两个最优值，适当调整自身的速度参数以及位置参数，进行快速高效的搜索.粒子群算法的这种机制实现了粒子之间的相互通信以及反馈，极大地提高了算法的搜索效率以及收敛速度.

ga算法有着很好的全局搜索能力，因为它的变异操作有效地保证了种群的多样性，避免算法陷入局部最优，但是它的搜索效率较低，收敛速度较慢；而pso算法通过引入粒子间的通信反馈机制，追随着当前的全局最优值以及个体最优值进行搜索，使得其收敛速度快，通用性强，但是该算法容易陷入局部最优值

综上所述可见，ga与pso的优点具有互补性，因此，本方法将这两种算法进行结合，通过引入信息交流机制，最大程度地利用两种算法的优点.该信息交流机制为：在算法的每次迭代过程中，将遗传算法种群中的适应度最大的部分个体替换掉粒子群算法种群中适应度最小的部分个体，同时，将粒子群算法种群中适应度最大的部分个体替换掉遗传算法中适应度最小的部分个体.通过这样的机制，能够实现两种算法之间的信息交互，充分发挥各自的优势，从而快速地收敛到全局最优解。该算法的流程图如图2所示。

23)轨迹优化

将上述ga-pso算法应用于时间最优轨迹的生成，其优化变量为h＝{h1,h2,...,hn-1}，优化函数为适应度函数，即公式(3-2)。该算法的具体应用步骤可描述为：

(1)初始化迭代次数i＝1、遗传算法种群p1及粒子群算法种群p2，根据速度约束确定搜索范围提高搜索效率；

(2)对种群p1和p2中的个体进行差分计算，得到相应的各个关节的角速度与角加速度，即

(3)根据公式(3-2)计算每个个体的适应度，分别对种群p1和p2按照其适应度大小进行排序；

(4)对种群p1进行遗传算法更新，即进行选择、交叉以及变异操作；

(5)对种群p2进行粒子群算法操作，即统计出种群最优解gbest以及个体历史最优解pbest，根据两个最优解，更新每个粒子的位置信息及速度信息；

(6)分别对种群p1和p2按照其适应度大小进行排序，将排序后的p1中的前γ个体替换掉p2的后γ个体，并将p2中的前γ个体替换掉p1的后γ个体，实现遗传算法与粒子群算法的信息交流；

(7)修改迭代次数i：i＝i+1，判断是否找到了全局最优解或者达到了最大迭代次数，若是则输出最优解，结束算法；否则，返回步骤(2)。

3)基于三次样条曲线的轨迹插值：利用优化后时间间隔序列和对应的采样轨迹点进行三次样条插值，得到合适的示教轨迹，具体为：

经过步骤2)所示的优化算法，得到了各轨迹点之间的时间间隔，根据该时间间隔，我们需要对轨迹点进行插值，得到连续平滑的轨迹曲线。本方案采用三次样条插值算法对轨迹点进行插值，因为该插值方法可以保证轨迹是二阶连续的，即得到的轨迹在速度与加速度上都是连续的，利于机器人的平滑运动。

设优化算法优化得到的时间间隔为：t1,t2,…tn，对应时刻的关节角度为q1,q2,…qn。其中分别对每个关节进行三次样条曲线插值。根据三次样条插值，有：

在每个分段区间[ti,ti+1],i＝1,2,...n，每个关节的角度轨迹si(t)都是一个三次多项式。

满足s(ti)＝qi。

关节角度函数si(t)，关节角速度函数关节角加速度函数在区间[ti,ti+1]上都是连续的，即曲线是光滑的。所以对于这n-1个区间，三次多项式可以写为：

si(t)＝ai+bi(t-ti)+ci(t-ti)²+di(t-ti)³

其中，ai,bi,ci,di即为要求解的系数。

根据插值函数的二阶连续性，可得：

si(t)＝qi

si(ti+1)＝qi+1

将时间间隔hi＝ti+1-ti代入上述三个条件中，可得：

(1)由si(t)＝qi可以推出ai＝qi。

(2)由si(ti+1)＝qi+1可以推出

(3)由s′i(ti+1)＝s′i+1(ti+1)可以推出

s′i+1(ti+1)＝bi+1+2ci+1(ti+1-ti+1)+3di+1(ti+1-ti+1)²＝bi+1

可以得到：

(4)由s″i(ti+1)＝s″i+1(ti+1)推出：

2ci+6hidi-2ci+1＝0

(5)设mi＝s″i(ti)＝2ci，则：

2ci+6hidi-2ci+1＝0可写为：mi+6hidi-mi+1＝0，推出：

(6)将ci,di代入可得：

(7)将bi,ci,di代入可得：

除了以上的条件，我们还需要对轨迹的两个端点的速度进行限制，在示教重现中，机器人应当是从静止开始加速运动，最后减速至终点并静止，所以端点的速度为0，即m1＝0,mn＝0，因此推出最后要求解的方程组为：

通过求解上述方程组，就能将每个时间区间中的三次多项式求解出来，进而得到整体的示教轨迹。最后将这些轨迹按照10ms控制周期进行离散化，发送给机器人控制器，使其完成示教再现功能。

4)进行示教轨迹再现时，将优化后的示教轨迹根据控制周期进行离散化，发送给机器人控制器，完整再现整个示教轨迹。

为证明本方法的可行性，在一款七自由度协作机器人上设置示教再现实现实验。在本实验中，由操作人员拖动出一条较为复杂的运动轨迹，机器人末端运动轨迹如图3为通过本方法的优化方法前后重新规划得到的机器人末端运行轨迹。

图4为通过本方法优化前的机器人各关节角度值。图5为通过本方法优化前的机器人各关节角度值。优化前后仅改变了轨迹的拖动时间，并未改变末端的运行位姿。

图6为通过本方案优化前的机器人各关节角速度值。图7为通过本方案优化前的机器人各关节角速度值。优化前，关节速度变化剧烈，有些时刻速度过大，远超过机器人的角速度限制。优化后，显著的改善了关节的运动情况，使得机器人关节运动符合实际电机性能。

图8为通过本方案优化前的机器人各关节角加速度值。图9为通过本方案优化前的机器人各关节角加速度值。优化前，机器人各关节加速度变化剧烈，且最大加速度过大，远超过机器人电机最大力矩产生的加速度。优化后，使得机器人加速度变化满足实际机器人关节电机能的性能。

从以上的效果图可以看出，若在重现轨迹时直接按照人手拖动的轨迹进行运动，那么各关节的角速度和角加速度将会远超出机器人的运动学约束，严重影响重现精度，容易使得机器人发生抖动，损伤机械性能，而经过了本方案的优化，成功地将各关节的角速度与角加速度限制在最大值内，保证了重现精度，减小机械损耗。