基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法

摘要

本发明公开了一种基于MIRRT*‑Connect算法的智能仓储冗余机械臂动态避障运动规划方法，该方法首先初始化工作空间环境，获取移动物品的起始和目标位姿并通过逆运动学求出机械臂关节变量，采用MIRRT*‑Connect算法规划一条从起始位姿到目标位姿无碰撞路径并进行平滑处理，根据所得节点序列数据控制冗余机械臂进行运动，最后利用冗余机械臂零空间自运动和局部路径规划对动态障碍物进行避障。本发明引入动态采样域概率偏置来降低扩展树生长的盲目性和双向扩展、自适应步长提高全局路径规划效率；通过最佳最近点和迭代运算优化路径；利用零空间自运动和局部路径规划有效避免动态障碍物，保证机械臂运动过程的安全性。

说明书

技术领域

本发明涉及冗余机械臂路径规划技术领域，具体是一种基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法。

背景技术

随着智能技术的高速发展，工业机器人凭借其生产效率、产品质量以及所带来的经济效益，已广泛应用于各行各业的不同领域。作为工业机器人的重要问题，轨迹规划旨在找出一条从起始状态到目标状态的路径，且在该路径运动过程中能够有效避开空间中所存在的障碍物。

目前常用的运动规划算法有网格搜索法(A*)、人工势场法(APF)、概率路线图法(PRM)和快速探索随机树(RRT)算法，其中网格搜索法(A*)需要对整个工作空间进行离散化建模，多用于移动机器人路径规划，对于多自由度冗余机械臂来说，存在“维度爆炸”问题；人工势场法(APF)利用障碍物斥力和目标引力思想，容易陷入局部最小值，产生震荡现象；概率路线图法(PRM)需要事先知道状态空间，效率较低；快速探索随机树(RRT)算法采用单查询、增量式的扩展路径方法，能够较好解决复杂环境和高维空间情况。针对传统RRT算法由于采样的随机性，存在低效率、盲目生长、路径曲折等问题，渐进最优RRT算法(RRT*)通过不断重选父节点及重新布线，使路径逐渐趋于最优，但其搜索；另外Informed RRT*在其基础上引入椭圆集概念，进一步限制搜索范围，但对于复杂环境仍存在适应性较差和精度较低的问题。

为了适用复杂场景和动态环境，本发明基于Informed RRT*引入双向扩展提高全局路径规划效率，采用动态采样域概率偏置来降低扩展树生长的盲目性，利用自适应步长加快在自由构型空间内扩展树的生长速度，通过距离评价函数选取最佳最近点，引入超椭球域及路径修剪进一步限制搜索范围，最后通过迭代运算对路径节点进行平滑处理，保证机械臂具有平稳的运动过程。同时，根据冗余机械臂零空间自运动和局部路径规划有效避免动态障碍物，保证机械臂运动过程的安全性。

发明内容

本发明的目的在于提供一种基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法，以解决传统技术对于复杂环境存在的效率低、适应性差和精度不高的问题。

为实现上述目的，本发明提供如下技术方案：基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法，包括以下步骤：

步骤S1：初始化工作空间环境，获取障碍物信息并确定冗余机械臂移动物品的起始位姿和目标位姿；

步骤S2：选取臂型角，通过逆运动学解算出冗余机械臂在所述起始位姿和目标位姿的关节变量；

步骤S3：利用MIRRT*-Connect算法在冗余机械臂自由构型空间内规划出一条从初始位姿运动到目标位姿的无碰撞路径；

步骤S4：通过迭代运算，对步骤S3所得路径进行平滑处理；

步骤S5：考虑冗余机械臂运动过程中与动态障碍物产生空间干涉，更新局部路径或改变臂型角避免碰撞；

步骤S6：利用上位机对冗余机械臂进行控制，保证其按照最新路径进行运动；

所述基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法，对Informed RRT*算法引入动态采样域概率偏置来降低扩展树生长的盲目性和双向扩展、自适应步长提高全局路径规划效率；通过最佳最近点、重选父节点及布线和迭代运算优化路径；利用零空间自运动和局部路径规划有效避免动态障碍物，保证机械臂运动过程的安全性。

进一步地，所述动态采样域概率偏置具体可表述为：

其中，

上式中，γ为冗余机械臂构型空间边界系数，N

进一步地，所述采样自适应步长可表述为：

其中δ

进一步地，所述重选父节点范围半径可表示为：

其中，m为冗余机械臂构型空间维度，Θ

当首次找到解决方案时，采样范围将缩小到超椭球域，同时对无法构成更小成本路径的节点进行剪枝处理，所述半径会被进一步更新为：

其中Θ

进一步地，在当前扩展树上距离采样点或上次扩展树新节点的最佳最近点可根据下式得到：

式中，

进一步地，在首次找到解决方案后，采样域进一步更新为超椭球域，其满足θ

θ

其中，θ

其中，diag{·}为对角矩阵，c

另外，旋转矩阵

Z＝Xdiag{1,…,1,|X|·|Y|}Y

其中，|·|代表矩阵行列式，

其中

以上即通过在单位超球域内均匀采样

进一步地，所述步骤S3中利用MIRRT*-Connect算法进行路径规划过程具体包括：

步骤S31、初始化扩展树T

步骤S32、根据c

步骤S33、所述最佳最近点的搜寻，遍历树节点集

步骤S34、沿

步骤S35、遍历以新节点

步骤S36、获取经过新节点

步骤S37、以当前扩展树T

步骤S38、交换扩展树T

步骤S39、判断新节点

步骤S310、利用c

步骤S311、判断是否达到循环结束条件，如果是则结束循环，返回扩展树T

进一步地，利用迭代法对规划路径节点序列进行平滑处理，具体表示为：

其中P(i)为步骤S3所得到的路径规划解决方案节点序列，P

进一步地，当冗余机械臂非末端执行器与动态障碍物发生碰撞时，可通过在冗余零空间自运动避开动态障碍物，所述冗余机械臂零空间自运动的旋转矢量可表述为：

其中，

所述零空间自运动由参考面到期望的避障面的旋转矩阵可表示为：

其中，ψ为参考面与避障面之间的臂型角，

进一步地，步骤S5所述更新局部路径具体包括：当冗余机械臂末端执行器与动态障碍物发生碰撞时，检测动态障碍物所阻断的运动路径，分析被阻断涉及到的路径节点，删除所有动态障碍物运动轨迹上的节点P

与现有技术相比，本发明的有益效果是：本发明提出的基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法，针对传统Informed RRT*算法存在的局限性进行了改进，引入双向扩展提高全局路径规划效率；采用采样域概率偏置降低初始路径规划解决方案获取前采样的盲目性；利用自适应步长加快在自由构型空间内扩展树的生长速度；通过距离评价函数选取最佳最近点；引入超椭球域及路径修剪进一步限制搜索范围；利用迭代运算对路径节点进行平滑处理，保证冗余机械臂具有平稳的运动过程，同时考虑动态障碍物的影响，采用冗余机械臂的零空间自运动和局部路径规划保证机械臂运动过程中的安全性。

附图说明

图1是本发明基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法的流程图；

图2是本发明智能仓储冗余机械臂系统结构图；

图3是本发明智能仓储冗余机械臂各关节结构图；

图4是本发明机械臂冗余零空间自运动示意图；

图5是本发明MIRRT*-Connect算法避障运动规划流程图；

图6是本发明MIRRT*-Connect算法中超椭球域采样示意图；

图7是本发明MIRRT*-Connect算法自适应步长及双向扩展示意图；

图8是本发明MIRRT*-Connect算法最佳最近点的选取示意图；

图9是本发明MIRRT*-Connect算法重选父节点和布线示意图；

图10是本发明局部路径重规划示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-10，本发明提供一种技术方案：

本发明提供一种基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法，其整体流程图如图1所示。该方法在系统硬件装置和软件控制的基础上，首先初始化冗余机械臂工作空间环境，获取工作空间障碍物信息和冗余机械臂移动物品的起始位姿和目标位姿，并根据选定臂型角利用逆运动学求解冗余机械臂在位姿上的关节变量；运动规划系统基于MIRRT*-Connect算法规划出一条从起始位姿到目标位姿的无碰撞路径并进行平滑处理；将最终路径节点序列数据发送至上位机，上位机根据数据对冗余机械臂进行运动控制；运动过程进行动态碰撞检测，判断是否为末端执行器发生碰撞，如果是则更新局部路径，并利用上位机对冗余机械臂沿更新后路径进行运动控制；如果不是则改变臂型角，完成避障后恢复原状态，继续按原路径进行运动；检测当前位姿是否到达目标位姿，如果没有则继续运动，直到运动至目标位姿。方法具体包括如下步骤：

步骤S1、智能仓储冗余机械臂系统构成如图2所示，其中搬运硬件装置包括移动地盘1、导航装置2、物品暂放装置3、冗余机械臂4和末端夹爪5，软件控制系统包括物品信息处理系统、运动规划系统和冗余机械臂控制系统。首先初始化冗余机械臂工作空间环境，获取工作空间内障碍物信息以及臂移动物品的起始位姿和目标位姿。

步骤S2、根据选定臂型角并利用逆运动学求解冗余机械臂在物品的起始位姿和目标位姿的关节变量，具体包括：

步骤S21、为冗余机械臂各连杆建立坐标系，其关节结构如图3所示，根据机械臂各连杆之间关系和各连杆尺寸，得出冗余机械臂的DH参数如表1所示。

表1冗余机械臂DH参数。

表1中，连杆夹角θ

步骤S22、当冗余机械臂末端位姿确定时，此时如果固定关节3，则机械臂为非冗余机械臂。故引入当关节3角度θ

步骤S3、利用MIRRT*-Connect算法在冗余机械臂自由构型空间内规划出一条从初始位姿运动到目标位姿的无碰撞路径，MIRRT*-Connect算法流程图如图5所示。具体内容包括：

步骤S31、初始化扩展树T

步骤S32、调用采样函数Sample(θ

其中采样区域概率偏置具体可表述为：

式中，

式中，γ为冗余机械臂构型空间边界系数，N

如图7所示，超椭球域采样为，在首次找到解决方案后，采样域进一步更新为超椭球域，其满足θ

θ

其中，θ

其中，diag{·}为对角矩阵，c

另外，旋转矩阵

Z＝Xdiag{1,…,1,|X|·|Y|}Y

其中，|·|代表矩阵行列式，

其中

以上即通过在单位超球域内均匀采样

步骤S33、调用扩展函数Extend(T

最佳最近点

式中，w

步骤S34、如图6所示，调用生长函数Grow(θ

其中采样自适应步长可表述为：

其中δ

其中，新节点

其中，T

步骤S35、调用重选父节点函数ReselectParent(T,θ

其中，重选父节点范围半径由下式表示：

其中，m为冗余机械臂构型空间维度，Θ

当首次找到解决方案时，采样范围将缩小到超椭球域，同时对无法构成更小成本路径的节点进行剪枝处理，所述半径会被进一步更新为：

其中Θ

步骤S36、调用重布线函数Rewriting(T,Θ

步骤S37、调用连接函数Connect(T,θ

步骤S38、调用函数Exchange(T

步骤S39、调用是否找到路径解决方案函数isFoundPathSolution(θ

步骤S310、利用c

步骤S311、判断是否达到循环结束条件，如果是则结束循环，返回扩展树T

步骤S4、调用路径平滑处理函数PathSmoothing(P)通过迭代运算，对步骤S3所得路径解决方案节点序列进行平滑处理，利用迭代法对规划路径节点序列进行平滑处理，具体表示为：

其中P(i)为步骤S3所得到的路径规划解决方案节点序列，P

步骤S5、考虑冗余机械臂运动过程中与动态障碍物产生空间干涉，更新局部路径或改变臂型角避免碰撞，具体包括：

步骤S51，检测是否存在动态障碍物与冗余机械臂发生碰撞，并进一步判断是否为冗余机械臂末端执行器碰撞，如果是则执行步骤S52，否则执行步骤S53；

步骤S52、避免冗余机械臂末端执行器动态障碍物发生碰撞具体包括：分析被阻断涉及到的路径节点，删除所有动态障碍物运动轨迹上的节点P

步骤S53、通过在冗余零空间自运动避开动态障碍物，冗余机械臂零空间范围如图4所示，该零空间自运动的旋转矢量可表述为：

其中，

零空间自运动由参考面到期望的避障面的旋转矩阵可表示为：

其中，ψ为参考面与避障面之间的臂型角，

步骤S6、利用上位机对冗余机械臂进行控制，使其按照步骤S4所得平滑路径运动到目标位置。运动过程中检测平滑路径节点序列数据是否更新，如果是则需上位机按照最新节点序列数据对冗余机械臂进行运动控制，同时检测冗余机械臂是否到达目标位姿，如果是则停止运动等待命令，否则进行对其进行控制，直到运动到目标位姿为止。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。