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

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

Optimal trajectory planning method for industrial robot in obstacle environment

The invention discloses a trajectory planning method for optimal dynamic performance of industrial robot in obstacle environment, which comprises the following steps: S1, input environment map and robot ontology information through input layer, output trajectory space-time information by output layer to drive the robot to complete a given task; S2, efficient RRT through C space; Connect algorithm, get collision-free path, and then prune and smooth the path; S3, through the dynamic optimizer, optimize the path planned in the second stage into dynamic executable trajectory. In the complex obstacle environment, the trajectory planning method of the invention has high efficiency and practicability.

【技术实现步骤摘要】
障碍环境下的工业机器人动力学性能最优轨迹规划方法
本专利技术涉及机器人轨迹规划领域，具体涉及一种障碍环境下的工业机器人动力学性能最优轨迹规划方法。
技术介绍
障碍环境下的工业机器人动力学轨迹规划，比较复杂，因为，当机器人完成任务时，既要使机器人本体不与外界世界发生碰撞或穿透，又要满足机器人本身内在系统的运动学/动力学约束。目前，主要采用静态动力学规划方法，解决运动学，非完整和动力学约束情况下的轨迹规划，有两类规划方法：一类是解耦将路径规划和动力学约束分离，即在传统路径规划时，不考虑动力学因素，按照路径的性能指标，进行路径规划。当路径规划完后，再进行动力学约束下的轨迹规划，最终规划出最优轨迹。此类方法在非完整系统和仿人机器人领域，获得广泛应用；二类是将路径规划和动力学约束一起考虑，此时，机器人轨迹规划变成关于轨迹的泛函求极值问题，即变量为轨迹，目标函数和约束函数为轨迹的泛函，在复杂障碍下，轨迹规划变得极其复杂，因为障碍物约束作用，最优值存在广泛的局部多解性。
技术实现思路
为解决上述问题，本专利技术提供了一种障碍环境下的工业机器人动力学性能最优轨迹规划方法。为实现上述目的，本专利技术采取的技术方案为：障碍环境下的工业机器人动力学性能最优轨迹规划方法，包括如下步骤：S1、通过输入层输入环境地图和机器人本体信息，输出层输出轨迹的时空信息，以驱动机器人完成给定的任务；S2、通过C空间高效的RRT-Connect算法，获得无碰撞路径，然后对路径进行修剪与光滑操作；S3、通过动力学优化器，将第二阶段规划好的路径优化成动力学可执行轨迹。进一步地，所述环境地图通过激光雷达扫描技术快速获取。进一步地，所述步骤S2通过以下步骤完成路径的修剪操作：对于采样节点序列路径Q＝(q1，q2，…，qk)，如果qi-1和qi+1之间能够直接连通，通过删除位姿点坐标qi剪短路径；对于相隔位姿点之间的位姿，通过线性插值计算中间位姿点，然后采用碰撞检测函数，对每个中间位姿点进行检测，最终保证所有的中间位姿点属于无障碍构型空间Cfree。进一步地，所述步骤S2中的路径光滑操作采用B样条拟合位姿点序列，如果检测出拟合后的轨迹发生碰撞，则还需返回路径采样，继续采样操作，如果检测光滑后的路径没有碰撞，则表示光滑操作成功，可以直接进入动力学规划。进一步地，所述步骤S3包括如下步骤：S31、假设机器人的轨迹Q＝(q1，q2，…，qk，T)，其中，qk为路径结点，T为轨迹时间，则定义目标函数C(Q)：式中，第一项为关节力矩的平方和，表示轨迹能量消耗；第二项为关节跃度的平方和，表示轨迹的平滑性；第三项表示轨迹里程时间，三项的加权总和作为目标函数，其中，τ为关节扭矩矢量，可以通过机器人逆动力学公式计算得到；为关节跃度矢量，T为轨迹时间变量，通过参数化轨迹直接得到和T的值，Ke、Kj、Kt为各个目标函数的权重因子；S32、根据目标函数(1)式，得到以下优化问题：minC(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所示，首先进行初始化，从qint、qgoa分别构造两棵树Ta和Tb，然后，进入双树交替扩展的迭代过程，其中每一步迭代需要完成下面的Extend扩展步骤；调用子程序RandomConfig()，在C空间中均匀随机地选取一个配置点qrand，调用Extend算法，使Ta朝向qrand扩展一个λ步长，产生新节点qnew，然后调用Connect算法，使Tb朝向qnew扩展多个λ步长；如果Tb上的节点与qnew能够完全连接，则返回解路径path(Ta，Tb)；否则，调用SWAP子程序交换两棵树Ta和Tb，重新执行上述过程，直至算法返回一条联通路径，或者到达迭代次数上限k，则退出。表1RRT-Connect算法本实施例中，通过以下路径修剪算法和路径光滑算法完成路径修剪和光滑操作，为动力学轨迹规划提供基础。路径修剪对于采样节点序列路径Q＝(q1，q2，…，qk)，运用修剪算法prune(Q)，如果qi-1和qi+1之间能够直接连通，则删除位姿点坐标qi剪短路径，这样依次修剪路径。对于相隔的位姿点，通过线性插值中间位姿点，依次采用碰撞检测函数，对每个位姿点进行检测，最终保证所有的位姿点属于Cfree(无障碍构型空间)。修剪算法prune(Q)如表2所示，首先输入轨迹Q，然后逐次循环检测是否碰撞，任意两位姿点之间，如果检测到无碰撞，则可以删除该任意两位姿点之间所夹位姿点，直接连通该两位姿点，以达到删除路径冗余位姿点的目的。基于碰撞检测的路径修剪算法在表2中概述，其结果如图2(b)粗线所示，对比修剪前的间断线，可以发现，修剪后的路径效率相对要高，路径长度明显减短。修剪以后，虽然路径缩短，但依旧是折线，效率不够高，因此，本文档来自技高网...

【技术保护点】
1.障碍环境下的工业机器人动力学性能最优轨迹规划方法，其特征在于，包括如下步骤：S1、通过输入层输入环境地图和机器人本体信息，输出层输出轨迹的时空信息，以驱动机器人完成给定的任务；S2、通过C空间高效的RRT‑Connect算法，获得无碰撞路径，然后对路径进行修剪与光滑操作；S3、通过动力学优化器，将第二阶段规划好的路径优化成动力学可执行轨迹。

【技术特征摘要】
1.障碍环境下的工业机器人动力学性能最优轨迹规划方法，其特征在于，包括如下步骤：S1、通过输入层输入环境地图和机器人本体信息，输出层输出轨迹的时空信息，以驱动机器人完成给定的任务；S2、通过C空间高效的RRT-Connect算法，获得无碰撞路径，然后对路径进行修剪与光滑操作；S3、通过动力学优化器，将第二阶段规划好的路径优化成动力学可执行轨迹。2.如权利要求1所述的障碍环境下的工业机器人动力学性能最优轨迹规划方法，其特征在于，所述环境地图通过激光雷达扫描技术快速获取。3.如权利要求1所述的障碍环境下的工业机器人动力学性能最优轨迹规划方法，其特征在于，所述步骤S2通过以下步骤完成路径的修剪操作：对于采样节点序列路径Q＝(q1，q2，…，qk)，如果qi-1和qi+1之间能够直接连通，通过删除位姿点坐标qi剪短路径；对于相隔位姿点之间的位姿，通过线性插值计算中间位姿点，然后采用碰撞检测函数，对每个中间位姿点进行检测，最终保证所有的中间位姿点属于无障碍构型空间Cfree。4.如权利要求1所述的障碍环境下的工业机器人动力学性能最优轨迹规划方法，...

【专利技术属性】
技术研发人员：南文虎，张淑珍，郑海霞，李春玲，杨萍，
申请(专利权)人：兰州理工大学，
类型：发明
国别省市：甘肃,62