CN108621165B - 障碍环境下的工业机器人动力学性能最优轨迹规划方法 - Google Patents

Abstract

本发明公开了一种障碍环境下的工业机器人动力学性能最优轨迹规划方法，包括如下步骤：S1、通过输入层输入环境地图和机器人本体信息，输出层输出轨迹的时空信息，以驱动机器人完成给定的任务；S2、通过C空间高效的RRT‑Connect算法，获得无碰撞路径，然后对路径进行修剪与光滑操作；S3、通过动力学优化器，将第二阶段规划好的路径优化成动力学可执行轨迹。在复杂障碍环境中，本发明的轨迹规划方法具有高效性和实用性。

Description

障碍环境下的工业机器人动力学性能最优轨迹规划方法

技术领域

本发明涉及机器人轨迹规划领域，具体涉及一种障碍环境下的工业机器人动力学性能最优轨迹规划方法。

背景技术

障碍环境下的工业机器人动力学轨迹规划，比较复杂，因为，当机器人完成任务时，既要使机器人本体不与外界世界发生碰撞或穿透，又要满足机器人本身内在***的运动学/动力学约束。

目前，主要采用静态动力学规划方法，解决运动学，非完整和动力学约束情况下的轨迹规划，有两类规划方法：一类是解耦将路径规划和动力学约束分离，即在传统路径规划时，不考虑动力学因素，按照路径的性能指标，进行路径规划。当路径规划完后，再进行动力学约束下的轨迹规划，最终规划出最优轨迹。此类方法在非完整***和仿人机器人领域，获得广泛应用；二类是将路径规划和动力学约束一起考虑，此时，机器人轨迹规划变成关于轨迹的泛函求极值问题，即变量为轨迹，目标函数和约束函数为轨迹的泛函，在复杂障碍下，轨迹规划变得极其复杂，因为障碍物约束作用，最优值存在广泛的局部多解性。

发明内容

为解决上述问题，本发明提供了一种障碍环境下的工业机器人动力学性能最优轨迹规划方法。

为实现上述目的，本发明采取的技术方案为：

障碍环境下的工业机器人动力学性能最优轨迹规划方法，包括如下步骤：

S1、通过输入层输入环境地图和机器人本体信息，输出层输出轨迹的时空信息，以驱动机器人完成给定的任务；

S2、通过C空间高效的RRT-Connect算法，获得无碰撞路径，然后对路径进行修剪与光滑操作；

S3、通过动力学优化器，将第二阶段规划好的路径优化成动力学可执行轨迹。

进一步地，所述环境地图通过激光雷达扫描技术快速获取。

进一步地，所述步骤S2通过以下步骤完成路径的修剪操作：

对于采样节点序列路径Q＝(q₁，q₂，…，q_k)，如果q_i-1和q_i+1之间能够直接连通，通过删除位姿点坐标q_i剪短路径；

对于相隔位姿点之间的位姿，通过线性插值计算中间位姿点，然后采用碰撞检测函数，对每个中间位姿点进行检测，最终保证所有的中间位姿点属于无障碍构型空间C_free。

进一步地，所述步骤S2中的路径光滑操作采用B样条拟合位姿点序列，如果检测出拟合后的轨迹发生碰撞，则还需返回路径采样，继续采样操作，如果检测光滑后的路径没有碰撞，则表示光滑操作成功，可以直接进入动力学规划。

进一步地，所述步骤S3包括如下步骤：

S31、假设机器人的轨迹Q＝(q₁，q₂，…，q_k，T)，其中，q_k为路径结点，T为轨迹时间，则定义目标函数C(Q)：

式中，第一项为关节力矩的平方和，表示轨迹能量消耗；第二项为关节跃度的平方和，表示轨迹的平滑性；第三项表示轨迹里程时间，三项的加权总和作为目标函数，其中，τ为关节扭矩矢量，可以通过机器人逆动力学公式计算得到；

为关节跃度矢量，T为轨迹时间变量，通过参数化轨迹直接得到

和T的值，K_e、K_j、K_t为各个目标函数的权重因子；

S32、根据目标函数(1)式，得到以下优化问题：

min C(Q) (2)

式(2)为目标函数，式(3)为约束函数，依次为关节速度、关节加速度和关节扭矩约束；

S33、采用SQP算法解算上述约束函数，即得最优轨迹。

与现有技术相比，本发明具有以下有益效果：

采用RRT-Connect算法，建立了路径修剪算法和路径光滑算法；将路径规划和动力学规划集成到一起，在复杂障碍工况下，获取了工业机器人动力学最优轨迹；在工业机器人抓取实验中，与传统参数轨迹优化方法对比分析，证明所提的方法在轨迹规划效率和质量上，具有明显的优势。对工业机器人复杂环境下的动力学轨迹规划具有较强的指导意义和参考价值。

附图说明

图1为分层规划示意图。

图2为路径优化示意图。

图3为SQP算法路径示意图。

图4为RRT-Connect算法路径示意图。

图5为集成轨迹规划路径示意图。

图6为规划时间对比图。

图7为轨迹时间对比图。

图8为关节速度对比图：

图中：(a)为关节1的速度曲线图；(b)为关节2的速度曲线图；(c)为关节3的速度曲线图；(d)为关节4的速度曲线图。

图9为关节跃度比较图：

图中：(a)为关节1的关节跃度图；(b)为关节2的关节跃度图、(c)为关节3的关节跃度图；(d)为关节4的关节跃度图。

图10为关节扭矩比较图。

图中：(a)为关节1的关节扭矩图；(b)为关节2的关节扭矩图；(c)为关节3的关节扭矩图；(d)为4的扭矩曲线图。

具体实施方式

为了使本发明的目的及优点更加清楚明白，以下结合实施例对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明实施例提出了一个普遍和实用极强的有障碍轨迹规划框架，该框架分为三层，第一层为信息输入输出层，输入层主要提供已知的环境地图和机器人本体信息，环境地图通过激光雷达扫描技术快速获取。输出层输出轨迹的时空信息，以驱动机器人完成给定的任务；第二层与第三层分别是针对几何空间与能量空间的轨迹规划，这两个阶段是整个轨迹规划的基础部分。在第二阶段中，如图1所示，在构型空间搜索固定位姿间的路径，并通过路径修剪和光滑操作，增强路径平滑性和效率；第三阶段，根据动力学特征函数，规划出动力学最优轨迹。由于动力学规划，可能导致路径与障碍物发生碰撞，动力学规划后的路径，需要再次返回到第一阶段，进行碰撞检测，以保证路径的安全性。这两个过程重复，直到获得动力学最优轨迹为止。下面主要讲述路径规划与动力学轨迹规划的具体方案。

本实施例中采用RRT-Connect算法进行路径规划，并将该算法集成到工业机器人动力学轨迹规划算法中。RRT-Connect算法的伪代码如表1所示，首先进行初始化，从q_int、q_goa分别构造两棵树T_a和T_b，然后，进入双树交替扩展的迭代过程，其中每一步迭代需要完成下面的Extend扩展步骤；调用子程序RandomConfig()，在C空间中均匀随机地选取一个配置点q_rand，调用Extend算法，使T_a朝向q_rand扩展一个λ步长，产生新节点q_new，然后调用Connect算法，使T_b朝向q_new扩展多个λ步长；如果T_b上的节点与q_new能够完全连接，则返回解路径path(T_a，T_b)；否则，调用SWAP子程序交换两棵树T_a和T_b，重新执行上述过程，直至算法返回一条联通路径，或者到达迭代次数上限k，则退出。

表1 RRT-Connect算法

本实施例中，通过以下路径修剪算法和路径光滑算法完成路径修剪和光滑操作，为动力学轨迹规划提供基础。

路径修剪

对于采样节点序列路径Q＝(q₁，q₂，…，q_k)，运用修剪算法prune(Q)，如果q_i-1和q_i+1之间能够直接连通，则删除位姿点坐标q_i剪短路径，这样依次修剪路径。对于相隔的位姿点，通过线性插值中间位姿点，依次采用碰撞检测函数，对每个位姿点进行检测，最终保证所有的位姿点属于C_free(无障碍构型空间)。

修剪算法prune(Q)如表2所示，首先输入轨迹Q，然后逐次循环检测是否碰撞，任意两位姿点之间，如果检测到无碰撞，则可以删除该任意两位姿点之间所夹位姿点，直接连通该两位姿点，以达到删除路径冗余位姿点的目的。基于碰撞检测的路径修剪算法在表2中概述，其结果如图2(b)粗线所示，对比修剪前的间断线，可以发现，修剪后的路径效率相对要高，路径长度明显减短。修剪以后，虽然路径缩短，但依旧是折线，效率不够高，因此，需进行下面的路径光滑操作。

表2路径修剪算法

路径光滑

路径光滑一般采用样条曲线拟合修剪后的位姿点，本发明采用B样条拟合位姿点序列，但是，如果检测出拟合后的轨迹发生碰撞，则还需返回路径采样，继续采样操作，如果检测光滑后的路径没有碰撞，则表示光滑操作成功，可以直接进入动力学规划。光滑函数Smooth(Q)如表3所示，其光滑操作的结果如图2(c)中实线所示，可以看出，经路径优化后，轨迹效率大幅度提高。

表3路径光滑算法

图2说明路径优化的例子，长方形表示高维C空间，图2(a)中实线为RRT-Connect采样规划后的路径，可以看出，这条路径的效率不高，曲折蜿蜒于障碍空间内，采用修前算法，经修剪以后的路径，如图2(b)粗线所示，对比修剪前的间断线，可以发现，修剪后的路径效率相对要高，路径长度明显减短。图2(c)所示为光滑以后的路径，对比光滑前的间断线，光滑后的路径曲率较小，效率更高，适合机器人控制。下面在第二节基础上，进行动力学轨迹规划，使轨迹更加符合工业机器人物理设备的驱动特性。

动力学规划***

假设机器人的轨迹Q＝(q₁，q₂，…，q_k，T)(q_k为路径结点，T为轨迹时间)，则定义目标函数C(Q)：

式(1)中第一项为关节力矩的平方和，表示轨迹能量消耗；第二项为关节跃度的平方和，表示轨迹的平滑性；第三项表示轨迹里程时间，三项的加权总和作为目标函数，其中，τ为关节扭矩矢量，可以通过机器人逆动力学公式计算得到。

为关节跃度矢量，T为轨迹时间变量，通过参数化轨迹直接得到

和T的值，K_e、K_j、K_t为各个目标函数的权重因子，根据目标函数(1)式，可以写出以下优化问题：

min C(Q) (2)

上式(2)为目标函数，式(3)为约束函数，依次为关节速度、关节加速度和关节扭矩约束。上述优化问题是典型的非线性优化问题。考虑到实际应用中问题的大规模以及复杂性，本发明采用计算效率较高的SQP算法，解决上面约束优化问题，以离线计算的方式，解决动力学性能轨迹优化问题。通过上面工业机器人的路径和动力学集成规划***，得到最优轨迹，下面实验验证本发明提出的方法。

实验研究

实验的建立，以工业上下料，锻造产业为背景，专门针对抓取操作。抓取对象识别和目标识别可选择专门的视觉开发公司(Scape Technologies A/S³)提供的眼睛系，该眼睛***的特点是，开发一个固定在工具坐标系的三维栅格扫描仪，扫描仪提供致密的点云，可以发现障碍物点云，采用文献[14]所示点云构建多面体的方法，构建障碍物的简化几何体，然后以高次曲面包络简化后的集合体，或几何模型匹配的方式，以达到减小避障计算量的目的。路径规划、碰撞检测和轨迹规划，采用本发明提出的集成规划***，离线规划产生机器人轨迹。

实验机器人采用国产华数机器人，其控制器型号为：MC800(HNC)，采用工业以太网与外部计算机通信。该机器人配备吸盘和气动夹持器，以抓取对象。目标识别和规划软件，在外部计算机上，其配置为：Pentium(R)G620处理器，主频2.66Hz，内存为3.41GB，操作***使用Windows XP，轨迹规划采用单线程编程实现。

为增加碰撞检测算法的效率，将桌子和箱子等障碍采用超曲面包络方法，超曲面方程为：

Γ(x，y，z)：(x/ξ₁)⁴+(y/ξ₂)⁸+(z/ξ₃)⁴-1 (4)

其中，ξ₁、ξ₂、ξ₃为超曲面的系数，根据凸面体的大小，选取适当系数即可，则满足不与超曲面碰撞的条件为：

Γ(x，y，z)＞0 (5)

以六自由度工业机器人从箱子里拿出工件，中间绕过桌子等障碍物，最后将工件放到工作台为工作目标，采用本发明所设计的算法，进行试验验证。下面将对轨迹的路径和规划时间进行对比分析。

轨迹路径与时间对比

与本发明轨迹规划相比，传统的示教模式下，机器人的运动本质上由人脑所决策，如碰撞检测与轨迹模式，虽然可靠性很高，但是效率很低。一般抓取操作过程为：抓取目标对象后，在笛卡尔空间，机器人手抓直线移动到箱子上面的安全位姿，然后，采用多个点对点的直线运动，以避免碰撞，最后，到达机器人运动的目标位姿。显然，由于每条线段都需要加减速，以致效率较低。采用非线性优化的惩罚函数，只需给定机器人的起点位姿、终点位姿和障碍物空间位置，然后优化，得到最优解，效率比示教高，但是可靠性较低。采用本发明所提的集成规划方法，可以得到效率和成功率较高的轨迹。下表4所示，为采用三种非线性优化算法：SQP、PSO、GA算法所规划的轨迹，与本发明集成规划方式产生的轨迹进行对比，分别对20个位姿坐标之间的轨迹进行试验，其测试结果如表4所示。

表4三种轨迹比较

Tab.4 The comparison of three trajectory

从表4可以看出，从平均规划时间、轨迹里程时间、成功率和平均路径长度四方面看，本发明所提方法，均优于其余三种算法。不难发现，基于梯度优化的SQP算法，在规划速度和轨迹时间上优于两种进化算法(PSO和GA)，从算法本身来讲，梯度优化算法速度要快于种群进化算法，但是，SQP算法，需要较好初值，若初值不当，则可能导致算法失败。

观察图3、图4和图5，可以看出，图3中，采用非线性优化技术的SQP算法，虽然得到可行解，但是路径的效率并不高，路径较为曲折，因此，消耗更多的时间和控制能量，图4和5采用本发明方法所得路径，图4中，虽然通过采样得到联通可行路径，但路径效率很低，经过修剪、光滑、动力学优化后，路径比较平滑，位姿分布比较均匀。

轨迹规划时间如图6所示，选取5个位姿点，作为对比，可以看出，本发明所提出的方法，轨迹规划所需的时间最少，SQP算法次之，PSO和GA消耗的时间最多。因此，本发明提出的方法，在规划时间上具有明显的优势。

轨迹运行时间如图7所示，选取5个位姿点，作为对比，可以看出，本发明所提出的方法，轨迹运行所需的时间最小，除第一个位姿点，SQP算法计算的轨迹时间与本发明方法相近。在其余位姿点处，本发明所提出的集成规划方案，轨迹时间都小于其它三种算法，轨迹时间较小，说明机器人完成任务的效率较高，因此所提出的轨迹规划算法具有高效性，下面将分析轨迹的动力学性能。

通过以上基于动力学的轨迹规划，得到工业机器人的状态曲线，下面将分析机器人运行过程中的状态特性。

对比图8所示的关节速度，在相同的时间内，采用本发明规划算法的速度曲线极值都小于SQP算法优化所得的结果，在有障碍情况下，速度变化较为平稳，说明本发明规划算法产生的速度较为平稳。

对比图9所示的关节跃度，不难发现，除3关节外，其余关节的跃度值，采用综合规划方法，相比于SQP算法，跃度减小很大幅度，关节2较为明显，具有较小的跃度值，说明加速度变化较小，有利于机器人降低振动，使机器人动态特性更为稳定。

分析图10，可以发现，相比于SQP规划算法，采用本发明集成优化算法，除关节1中，集成规划的扭矩稍大于SQP算法规划的扭矩外。其余三个关节的扭矩，本发明算法所得的扭矩最大幅值远小于SQP算法规划所得的扭矩，这说明扭矩变化平稳，轨迹的动力学特性较好，有利于伺服位置精确控制。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (4)

1.障碍环境下的工业机器人动力学性能最优轨迹规划方法，其特征在于，包括如下步骤：

S1、通过输入层输入环境地图和机器人本体信息，输出层输出轨迹的时空信息，以驱动机器人完成给定的任务；

S2、通过C空间高效的RRT-Connect算法，获得无碰撞路径，然后对路径进行修剪与光滑操作；

S3、通过动力学优化器，将第二阶段规划好的路径优化成动力学可执行轨迹；

S31、假设机器人的轨迹Q＝(q₁，q₂，…，q_k，T)，其中，q_k为路径结点，T为轨迹时间，则定义目标函数C(Q)：

式中，第一项为关节力矩的平方和，表示轨迹能量消耗；第二项为关节跃度的平方和，表示轨迹的平滑性；第三项表示轨迹里程时间，三项的加权总和作为目标函数，其中，τ为关节扭矩矢量，可以通过机器人逆动力学公式计算得到；

为关节跃度矢量，T为轨迹时间变量，通过参数化轨迹直接得到

和T的值，K_e、K_j、K_t为各个目标函数的权重因子；

S32、根据目标函数(1)式，得到以下优化问题：

min C(Q) (2)

式(2)为目标函数，式(3)为约束函数，依次为关节速度、关节加速度和关节扭矩约束；

S33、采用SQP算法解算上述约束函数，即得最优轨迹。

2.如权利要求1所述的障碍环境下的工业机器人动力学性能最优轨迹规划方法，其特征在于，所述环境地图通过激光雷达扫描技术快速获取。

3.如权利要求1所述的障碍环境下的工业机器人动力学性能最优轨迹规划方法，其特征在于，所述步骤S2通过以下步骤完成路径的修剪操作：

对于采样节点序列路径Q＝(q₁，q₂，…，q_k)，如果q_i-1和q_i+1之间能够直接连通，通过删除位姿点坐标q_i剪短路径；

对于相隔位姿点之间的位姿，通过线性插值计算中间位姿点，然后采用碰撞检测函数，对每个中间位姿点进行检测，最终保证所有的中间位姿点属于无障碍构型空间C_free。

4.如权利要求1所述的障碍环境下的工业机器人动力学性能最优轨迹规划方法，其特征在于，所述步骤S2中的路径光滑操作采用B样条拟合位姿点序列，如果检测出拟合后的轨迹发生碰撞，则还需返回路径采样，继续采样操作，如果检测光滑后的路径没有碰撞，则表示光滑操作成功，可以直接进入动力学规划。

Images