申请/专利权人：合肥工业大学

申请日：2022-06-13

公开（公告）日：2023-08-25

公开（公告）号：CN114939872B

主分类号：B25J9/16

分类号：B25J9/16

优先权：

专利状态码：有效-授权

法律状态：2023.08.25#授权;2022.09.13#实质审查的生效;2022.08.26#公开

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

主权项：1.基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法，其特征在于，包括以下步骤：步骤S1：初始化工作空间环境，获取障碍物信息并确定冗余机械臂移动物品的起始位姿和目标位姿；步骤S2：选取臂型角，通过逆运动学解算出冗余机械臂在所述起始位姿和目标位姿的关节变量；步骤S3：利用MIRRT*-Connect算法在冗余机械臂自由构型空间内规划出一条从初始位姿运动到目标位姿的无碰撞路径；步骤S4：通过迭代运算，对步骤S3所得路径进行平滑处理；步骤S5：考虑冗余机械臂运动过程中与动态障碍物产生空间干涉，更新局部路径或改变臂型角避免碰撞；步骤S6：利用上位机对冗余机械臂进行控制，使其按照最新路径进行运动；所述基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法，对InformedRRT*算法引入动态采样域概率偏置来降低扩展树生长的盲目性和双向扩展、自适应步长和提高全局路径规划效率；通过最佳最近点、重选父节点及布线和迭代运算优化路径；利用零空间自运动和局部路径规划有效避免动态障碍物，保证机械臂运动过程的安全性；所述动态采样域概率偏置具体可表述为： 其中，和分别为扩展树Ts和Tg的随机采样点和的均匀采样空间，Psample∈[0,1]为随机数，α为动态采样域概率阈值，Θ为冗余机械臂构型空间，Θθgoal,Rgoal和Θθstart,Rstart分别是以中心θgoal和θstart，半径为Rgoal和Rstart的超球域，这里Rgoal和Rstart可分别表示为： 上式中，γ为冗余机械臂构型空间边界系数，N1是能够作为扩展树有效采样点数量，N2是采样到障碍物区域被舍弃的点的数量，β为决定半径大小变化快慢的系数；所述重选父节点范围半径可表示为： 其中，m为冗余机械臂构型空间维度，Θball为m维单位球域，ξ·代表高维空间的体积，为当前两扩展树上节点集，NΘT为其节点数量；当首次找到解决方案时，采样范围将缩小到超椭球域，同时对无法构成更小成本路径的节点进行剪枝处理，所述半径会被进一步更新为： 其中Θellipse为超椭球域；在当前扩展树上距离采样点或上次扩展树新节点的所述最佳最近点可根据下式得到： 式中，为当前扩展树，θtarget为采样点或者上次扩展树的新节点，wd为距离比例系数，wc为混合比例系数，NobsΘθ,2R*为以当前扩展树上节点θ为中心，2R*为半径的超球域内障碍物的数量；在首次找到解决方案后，采样域进一步更新为超椭球域，其满足θellipse～μΘellipse，经变换可进一步表述为：θellipse＝ZEθball+θcenter其中，θcenter＝θstart+θgoal2为超椭球域中心点，θball＝{θ∈Θball|||θ||2≤1}为在单位超球域的点，变换矩阵保证在超椭球域内均匀采样，可表示为： 其中，diag{·}为对角矩阵，cbest为当前解决方案的路径成本，超椭球以θstart和θgoal作为焦点，进一步有cmin＝||θstart-θgoal||2；另外，旋转矩阵可表示为：Z＝Xdiag{1,…,1,|X|·|Y|}YT其中，|·|代表矩阵行列式，通过奇异值分解是XΣYT≡M的酉矩阵，其中M可表示为： 其中为单位矩阵的第一列；以上即通过在单位超球域内均匀采样在经过变换、旋转和平移，可以得到超椭球域内的均匀采样点进一步描述为： 步骤S3中所述利用MIRRT*-Connect算法进行路径规划过程具体包括：步骤S31、初始化扩展树Ts和Tg、树节点线集和树节点集和起始点θs、终止点θg、路径规划解决方案Psoln，路径最小成本cbest及相关参数；步骤S32、根据cbest是否为初始值来判断选取采样方式，并通过采样函数获取当前扩展树Ts的采样点并检测是否通过碰撞检测，如果是则跳转到步骤S33，否则重新采样；步骤S33、所述最佳最近点的搜寻，遍历树节点集获取采样点的最佳最近点步骤S34、沿到指定方向进行扩展的到新节点对最佳最近点和新节点构成路径进行碰撞检测，如果未通过检测则跳转到步骤S38，否则执行步骤S35；步骤S35、遍历以新节点为中心，R*为半径的附近点集Θnear，获取经过附近点集Θnear中的节点θnear从当前扩展树Ts根节点θstart到新节点的路径成本，得到最小路径成本附近点集Θnear中的节点θnear作为父节点，并将节点θnear和新节点连线添加到树节点线集中；步骤S36、获取经过新节点从当前扩展树Ts根节点θstart到附近点集Θnear中的节点θnear的路径成本，如果小于从当前扩展树Ts根节点θstart到节点θnear的原路径成本，则将新节点作为节点θnear的新父节点，并在树节点线集中删除节点θnear和原父节点的连线，添加新节点和节点θnear的连线；步骤S37、以当前扩展树Ts的新节点为生长方向对另一扩展树Tg进行扩展，并获取最佳最近点并进行碰撞检测，未通过碰撞检测则跳转到步骤S38，否则重选父节点、重布线、并判断新节点是否为新节点如果不是则继续扩展，直到扩展未通过碰撞检测，执行步骤S38，否则连接两树；步骤S38、交换扩展树Ts和Tg；步骤S39、判断新节点的加入是否找到路径规划解决方案Psoln，如果是更新当前路径规划解决方案Psoln，否则执行步骤S310；步骤S310、利用cbest覆盖过时最小路径成本并计算当前路径规划解决方案Psoln的路径成本并覆盖cbest，判断cbest是否小于如果成立则对无法构成更小成本路径的节点进行剪枝处理，否则执行步骤S311；步骤S311、判断是否达到循环结束条件，如果是则结束循环，返回扩展树Ts和Tg以及路径规划解决方案Psoln，否则跳转至步骤S32。

全文数据：

权利要求：

百度查询： 合肥工业大学 基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD