CN114939872B - 基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法 - Google Patents
基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法 Download PDFInfo
- Publication number
- CN114939872B CN114939872B CN202210662499.7A CN202210662499A CN114939872B CN 114939872 B CN114939872 B CN 114939872B CN 202210662499 A CN202210662499 A CN 202210662499A CN 114939872 B CN114939872 B CN 114939872B
- Authority
- CN
- China
- Prior art keywords
- path
- node
- mechanical arm
- theta
- tree
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 55
- 230000033001 locomotion Effects 0.000 title claims abstract description 45
- 238000003860 storage Methods 0.000 title claims abstract description 16
- 238000005070 sampling Methods 0.000 claims abstract description 58
- 230000008569 process Effects 0.000 claims abstract description 21
- 230000004888 barrier function Effects 0.000 claims abstract description 7
- 201000004569 Blindness Diseases 0.000 claims abstract description 5
- 239000011159 matrix material Substances 0.000 claims description 27
- 238000001514 detection method Methods 0.000 claims description 17
- 239000013598 vector Substances 0.000 claims description 13
- 210000000323 shoulder joint Anatomy 0.000 claims description 9
- 210000003857 wrist joint Anatomy 0.000 claims description 9
- 239000012636 effector Substances 0.000 claims description 8
- 210000002310 elbow joint Anatomy 0.000 claims description 8
- 238000009499 grossing Methods 0.000 claims description 8
- 238000013138 pruning Methods 0.000 claims description 8
- 238000009826 distribution Methods 0.000 claims description 6
- 230000003044 adaptive effect Effects 0.000 claims description 5
- 230000009191 jumping Effects 0.000 claims description 5
- 230000000750 progressive effect Effects 0.000 claims description 4
- 238000012952 Resampling Methods 0.000 claims description 3
- 230000002457 bidirectional effect Effects 0.000 claims description 3
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- 230000008859 change Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 7
- 230000006978 adaptation Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000011156 evaluation Methods 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000005452 bending Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000009514 concussion Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000004880 explosion Methods 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 210000001503 joint Anatomy 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
- B25J9/1666—Avoiding collision or forbidden zones
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/30—Computing systems specially adapted for manufacturing
Abstract
本发明公开了一种基于MIRRT*‑Connect算法的智能仓储冗余机械臂动态避障运动规划方法，该方法首先初始化工作空间环境，获取移动物品的起始和目标位姿并通过逆运动学求出机械臂关节变量，采用MIRRT*‑Connect算法规划一条从起始位姿到目标位姿无碰撞路径并进行平滑处理，根据所得节点序列数据控制冗余机械臂进行运动，最后利用冗余机械臂零空间自运动和局部路径规划对动态障碍物进行避障。本发明引入动态采样域概率偏置来降低扩展树生长的盲目性和双向扩展、自适应步长提高全局路径规划效率；通过最佳最近点和迭代运算优化路径；利用零空间自运动和局部路径规划有效避免动态障碍物，保证机械臂运动过程的安全性。
Description
技术领域
本发明涉及冗余机械臂路径规划技术领域，具体是一种基于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*算法引入动态采样域概率偏置来降低扩展树生长的盲目性和双向扩展、自适应步长提高全局路径规划效率；通过最佳最近点、重选父节点及布线和迭代运算优化路径；利用零空间自运动和局部路径规划有效避免动态障碍物，保证机械臂运动过程的安全性。
进一步地，所述动态采样域概率偏置具体可表述为：
其中，和/>分别为扩展树Ts和Tg的随机采样点/>和/>的均匀采样空间，Psample∈[0,1]为随机数，α为采样域概率阈值，Θ为冗余机械臂构型空间，Θ(θgoal,Rgoal)和Θ(θstart,Rstart)分别是以中心θgoal和θstart，半径为Rgoal和Rstart的超球域，这里Rgoal和Rstart可分别表示为：
上式中，γ为冗余机械臂构型空间边界系数，N1是能够作为扩展树有效采样点数量，N2是采样到障碍物空间被舍弃的点的数量，β为决定半径大小变化快慢的系数。
进一步地，所述采样自适应步长可表述为：
其中δ0为初始步长，Δδ为自适应步长增量，k为步长增益系数，δi(i＝0,1,2,…,n)为第i次采样步长，Θobs为障碍物分布空间，对于全局空间障碍物不同的分布情况，对稀疏障碍物区域采取递进式自适应步长，加快路径搜索速度；对于密集障碍物区域采用初始恒定步长增加采样点的成功率。
进一步地，所述重选父节点范围半径可表示为：
其中，m为冗余机械臂构型空间维度，Θball为m维单位超球域，ξ(·)代表高维空间的体积，为当前两扩展树上节点集，N(ΘT)为其节点数量；
当首次找到解决方案时，采样范围将缩小到超椭球域，同时对无法构成更小成本路径的节点进行剪枝处理，所述半径会被进一步更新为：
其中Θellipse为超椭球域。
进一步地，在当前扩展树上距离采样点或上次扩展树新节点的最佳最近点可根据下式得到：
式中，为当前扩展树，θtarget为采样点或者上次扩展树的新节点，wd为距离比例系数，wc为混合比例系数，Nobs(Θ(θ,2R*))为以当前扩展树上节点θ为中心，2R*为半径的超球域内障碍物的数量。
进一步地，在首次找到解决方案后，采样域进一步更新为超椭球域，其满足θellipse～μ(Θellipse)，经变换可进一步表述为：
θellipse＝ZEθball+θcenter
其中，θcenter＝(θstart+θgoal)/2为超椭球中心点，θ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。
进一步地，利用迭代法对规划路径节点序列进行平滑处理，具体表示为：
其中P(i)为步骤S3所得到的路径规划解决方案节点序列，Ps(i)为迭代后的平滑路径节点序列，ρ和分别为平滑处理前后路径节点序列距离和平滑路径节点序列临近点距离的预设参数。
进一步地，当冗余机械臂非末端执行器与动态障碍物发生碰撞时，可通过在冗余零空间自运动避开动态障碍物，所述冗余机械臂零空间自运动的旋转矢量可表述为：
其中，为冗余机械臂肩关节和腕关节的连线方向矢量，/>和/>分别为在冗余机械臂第3连杆坐标系下肩关节和肘关节与在冗余机械臂第4连杆坐标系下肘关节和腕关节的连线方向矢量，/>代表冗余机械臂第b连杆坐标系到第a连杆坐标系的旋转矩阵；
所述零空间自运动由参考面到期望的避障面的旋转矩阵可表示为：
其中，ψ为参考面与避障面之间的臂型角，为单位阵，/>为rsw的单位向量，/>为/>的反对称矩阵。
进一步地，步骤S5所述更新局部路径具体包括：当冗余机械臂末端执行器与动态障碍物发生碰撞时，检测动态障碍物所阻断的运动路径，分析被阻断涉及到的路径节点，删除所有动态障碍物运动轨迹上的节点Ps(j)～Ps(j+k)及涉及这些节点的路径段L(j-1)～L(j+k)，并根据步骤S3到步骤S5以Ps(j-1)作为路径起点，Ps(j+k+1)作为路径终点进行局部路径规划并替代原路径被阻断部分。
与现有技术相比，本发明的有益效果是：本发明提出的基于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中，连杆夹角θi和连杆距离di描述相邻连杆的关系，连杆夹角θi为关节轴线的平面内两个公垂线的夹角，连杆距离di为沿关节i轴线两个公垂线的距离；连杆长度ai和连杆扭角αi描述连杆，连杆长度ai为两关节轴线公垂线的距离，连杆扭角αi为垂直与ai所在平面内两关节轴线的夹角；
步骤S22、当冗余机械臂末端位姿确定时，此时如果固定关节3，则机械臂为非冗余机械臂。故引入当关节3角度θ3＝0时，此时肩关节2、肘关节4和腕关节6所构成的平面为参考臂平面，参考平面如图4所示；当关节3角度θ3≠0时，此时肩关节2、肘关节4和腕关节6所构成的平面可以看作参考臂平面绕肩关节2到腕关节2的矢量轴旋转一定的角度，这个角度称为臂型角ψ。故通过选定臂型角ψs和ψg，同时根据冗余机械臂的起始和目标末端位姿可求出冗余机械臂对应的关节变量θs和θg。
步骤S3、利用MIRRT*-Connect算法在冗余机械臂自由构型空间内规划出一条从初始位姿运动到目标位姿的无碰撞路径，MIRRT*-Connect算法流程图如图5所示。具体内容包括：
步骤S31、初始化扩展树Ts和Tg、树节点线集和/>树节点集/>和/>起始点θs、终止点θg、路径规划解决方案Psoln，路径最小成本cbest及相关参数；
步骤S32、调用采样函数Sample(θs,θg,cbest)，根据cbest是否为初始值来判断选取采样方式，如果是，则采用采样区域概率偏置方式来进行采样，否则在超椭球域内进行采样获取采样点θrand，并利用碰撞检测函数isCrash(θ)判断是否碰撞，如果碰撞则重新采样，如图6所示，否则跳转到步骤S33；
其中采样区域概率偏置具体可表述为：
式中，和/>分别为扩展树Ts和Tg的随机采样点/>和/>的均匀采样空间，Psample∈[0,1]为随机数，α为采样域概率阈值，Θ为冗余机械臂构型空间，Θ(θgoal,Rgoal)和Θ(θstart,Rstart)分别是以中心θgoal和θstart，半径为Rgoal和Rstart的超球域，这里Rgoal和Rstart可分别表示为：
式中，γ为冗余机械臂构型空间边界系数，N1是能够作为扩展树有效采样点数量，N2是采样到障碍物空间被舍弃的点的数量，β为决定半径大小变化快慢的系数；
如图7所示，超椭球域采样为，在首次找到解决方案后，采样域进一步更新为超椭球域，其满足θellipse～μ(Θellipse)，经变换可进一步表述为：
θellipse＝ZEθball+θcenter
其中，θcenter＝(θstart+θgoal)/2为超椭球中心点，θball＝{θ∈Θball|||θ2≤1}为在单位超球域的点，变换矩阵保证在超椭球域内均匀采样，可表示为：
其中，diag{·}为对角矩阵，cbest为当前解决方案的路径成本，超椭球以θstart和θgoal作为焦点，进一步有cmin＝||θstart-θgoal||2；
另外，旋转矩阵可表示为：
Z＝Xdiag{1,…,1,|X|·|Y|}YT
其中，|·|代表矩阵行列式，通过奇异值分解是XΣYT≡M的酉矩阵，其中M可表示为：
其中为单位矩阵的第一列；
以上即通过在单位超球域内均匀采样在经过变换、旋转和平移，可以得到超椭球域内的均匀采样点/>有：
步骤S33、调用扩展函数Extend(Ts,θrand)并判断其返回值是否不是碰撞标识，如果是，则调用函数Neareset(T,θ)遍历树节点集获取采样点θrand的最佳最近点/>最佳最近点的获取如图8所示，否则跳转到步骤S38；
最佳最近点可由下式得到：
式中，wd为距离比例系数，wc为混合比例系数，Nobs(Θ(θ,2R*))为以当前扩展树上节点θ为中心，2R*为半径的超球域内障碍物的数量；
步骤S34、如图6所示，调用生长函数Grow(θnearest,θ)从最佳最近点以自适应步长向采样点θrand进行生长到新节点/>对最佳最近点/>和新节点/>构成路径调用碰撞检测函数isCrash(θ)判断是否碰撞，如果碰撞则返回函数Extend(T,θ)碰撞标识值，跳转到步骤S38，否则执行步骤S35；
其中采样自适应步长可表述为：
其中δ0为初始步长，Δδ为自适应步长增量，k为步长增益系数，δi(i＝0,1,2,…,n)为第i次采样步长，Θobs为障碍物分布空间，对于全局空间障碍物不同的分布情况，对稀疏障碍物区域采取递进式自适应步长，加快路径搜索速度；对于密集障碍物区域采用初始恒定步长增加采样点的成功率；
其中，新节点的获取可由下式表示：
其中，Tcur代表当前扩展树，θtarget代表当前扩展目标点；
步骤S35、调用重选父节点函数ReselectParent(T,θnew,R*)，首先根据搜索附近函数Near(T,θnew,R*)，遍历以新节点为中心，重选父节点范围半径R*为半径的附近点集Θnear，调用成本函数Cost(T,Θnear,θnew)获取经过附近点集Θnear中的节点θnear从当前扩展树Ts根节点θstart到新节点/>的路径成本，得到最小路径成本附近点集Θnear中的节点θnear作为父节点，并将节点θnear和新节点/>连线添加到树节点线集/>中；
其中，重选父节点范围半径由下式表示：
其中，m为冗余机械臂构型空间维度，Θball为m维单位超球域，ξ(·)代表高维空间的体积，为当前两扩展树上节点集，N(ΘT)为其节点数量；
当首次找到解决方案时，采样范围将缩小到超椭球域，同时对无法构成更小成本路径的节点进行剪枝处理，所述半径会被进一步更新为：
其中Θellipse为超椭球域。
步骤S36、调用重布线函数Rewriting(T,ΘT,θnew)进行重新布线，首先调用成本函数Cost(T,Θnear,θnew)获取经过新节点从当前扩展树Ts根节点θstart到附近点集Θnear中的节点θnear的路径成本，如果小于从当前扩展树Ts根节点θstart到节点θnear的原路径成本且通过碰撞检测，则将新节点/>作为节点θnear的新父节点，并在树节点线集/>中删除节点θnear和原父节点的连线，添加新节点/>和节点θnear的连线，重选父节点和重布线过程如图9所示；
步骤S37、调用连接函数Connect(T,θnew)，以当前扩展树Ts的新节点为生长方向对另一扩展树Tg进行扩展，并调用函数Neareset(T,θ)获取最佳最近点/>以及碰撞检测函数isCrash(θ)判断是否碰撞，如果是则跳转步骤S38，否则依次调用重选父节点函数ReselectParent(T,θnew,R*)和重布线函数Rewriting(T,ΘT,θnew)，并判断新节点/>是否为新节点/>如果不是则执行本步骤S37继续扩展，如果是则返回连接函数Connect(T,θnew)标识符为已扩展，否则返回标识符为已连接；
步骤S38、调用函数Exchange(Ts,Tg)返回交换扩展树Ts和Tg；
步骤S39、调用是否找到路径解决方案函数isFoundPathSolution(θnew)判断新节点的扩展是否找到路径规划解决方案Psoln，如果是更新当前路径规划解决方案Psoln，否则执行步骤S310；
步骤S310、利用cbest覆盖过时最小路径成本并调用路径解决方案成本计算函数PathSolutionCost(Psoln)计算当前路径规划解决方案Psoln的路径成本并覆盖cbest，判断cbest是否小于/>如果成立则对无法构成更小成本路径的节点进行剪枝处理，否则执行步骤S311；
步骤S311、判断是否达到循环结束条件，如果是则结束循环，返回扩展树Ts和Tg以及路径规划解决方案Psoln，否则跳转步骤S32。
步骤S4、调用路径平滑处理函数PathSmoothing(P)通过迭代运算，对步骤S3所得路径解决方案节点序列进行平滑处理，利用迭代法对规划路径节点序列进行平滑处理，具体表示为：
其中P(i)为步骤S3所得到的路径规划解决方案节点序列，Ps(i)为迭代后的平滑路径节点序列，ρ和分别为平滑处理前后路径节点序列距离和平滑路径节点序列临近点距离的预设参数。
步骤S5、考虑冗余机械臂运动过程中与动态障碍物产生空间干涉，更新局部路径或改变臂型角避免碰撞，具体包括：
步骤S51，检测是否存在动态障碍物与冗余机械臂发生碰撞，并进一步判断是否为冗余机械臂末端执行器碰撞，如果是则执行步骤S52，否则执行步骤S53；
步骤S52、避免冗余机械臂末端执行器动态障碍物发生碰撞具体包括：分析被阻断涉及到的路径节点，删除所有动态障碍物运动轨迹上的节点Ps(j)～Ps(j+k)及涉及这些节点的路径段L(j-1)～L(j+k)，并根据步骤S3到步骤S5以Ps(j-1)作为路径起点，Ps(j+k+1)作为路径终点进行局部路径规划并替代原路径被阻断部分，局部路径重规划如图10所示。
步骤S53、通过在冗余零空间自运动避开动态障碍物，冗余机械臂零空间范围如图4所示，该零空间自运动的旋转矢量可表述为：
其中，为冗余机械臂肩关节和腕关节的连线方向矢量，/>和/>分别为在冗余机械臂第3连杆坐标系下肩关节和肘关节与在冗余机械臂第4连杆坐标系下肘关节和腕关节的连线方向矢量，/>代表冗余机械臂第b连杆坐标系到第a连杆坐标系的旋转矩阵；
零空间自运动由参考面到期望的避障面的旋转矩阵可表示为：
其中，ψ为参考面与避障面之间的臂型角，为单位阵，/>为/>的单位向量，/>为/>的反对称矩阵。
步骤S6、利用上位机对冗余机械臂进行控制，使其按照步骤S4所得平滑路径运动到目标位置。运动过程中检测平滑路径节点序列数据是否更新，如果是则需上位机按照最新节点序列数据对冗余机械臂进行运动控制，同时检测冗余机械臂是否到达目标位姿，如果是则停止运动等待命令，否则进行对其进行控制，直到运动到目标位姿为止。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。
Claims (5)
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+θgoal)/2为超椭球域中心点，θ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。
2.根据权利要求1所述的基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法，其特征在于，所述采样自适应步长可表述为：
其中δ0为初始步长，Δδ为自适应步长增量，k为步长增益系数，δi(i＝0,1,2,…,n)为第i次采样步长，Θobs为障碍物分布空间，对于全局空间障碍物不同的分布情况，对稀疏障碍物区域采取递进式自适应步长，加快路径搜索速度；对于密集障碍物区域采用初始恒定步长增加采样点的成功率。
3.根据权利要求1所述的基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法，其特征在于，步骤S4所述利用迭代法对规划路径节点序列进行平滑处理，具体表示为：
其中P(i)为步骤S3所得到的路径规划解决方案节点序列，Ps(i)为迭代后的平滑路径节点序列，ρ和分别为平滑处理前后路径节点序列距离和平滑路径节点序列临近点距离的预设参数。
4.根据权利要求1所述的基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法，其特征在于，当冗余机械臂非末端执行器与动态障碍物发生碰撞时，可通过在冗余零空间自运动避开动态障碍物，所述冗余机械臂零空间自运动的旋转矢量可表述为：
其中，为冗余机械臂肩关节和腕关节的连线方向矢量，/>和/>分别为在冗余机械臂第3连杆坐标系下肩关节和肘关节与在冗余机械臂第4连杆坐标系下肘关节和腕关节的连线方向矢量，/>代表冗余机械臂第b连杆坐标系到第a连杆坐标系的旋转矩阵；
所述冗余机械臂零空间自运动由参考面到期望的避障臂型面的旋转矩阵可表示为：
其中，ψ为参考面与避障臂型面之间的臂型角，为单位阵，/>为rsw的单位向量，/>为/>的反对称矩阵。
5.根据权利要求1所述的基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法，其特征在于，步骤S5所述更新局部路径具体包括：当冗余机械臂末端执行器与动态障碍物发生碰撞时，检测动态障碍物所阻断的运动路径，分析被阻断涉及到的路径节点，删除所有动态障碍物运动轨迹上的节点Ps(j)～Ps(j+k)及涉及这些节点的路径段L(j-1)～L(j+k)，并根据步骤S3到步骤S5以Ps(j-1)作为路径起点，Ps(j+k+1)作为路径终点进行局部路径规划并替代原路径被阻断部分。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210662499.7A CN114939872B (zh) | 2022-06-13 | 2022-06-13 | 基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210662499.7A CN114939872B (zh) | 2022-06-13 | 2022-06-13 | 基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114939872A CN114939872A (zh) | 2022-08-26 |
| CN114939872B true CN114939872B (zh) | 2023-08-25 |
Family
ID=82908251
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210662499.7A Active CN114939872B (zh) | 2022-06-13 | 2022-06-13 | 基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114939872B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117340890A (zh) * | 2023-11-22 | 2024-01-05 | 北京交通大学 | 一种机器人运动轨迹控制方法 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112677153A (zh) * | 2020-12-16 | 2021-04-20 | 东北林业大学 | 一种改进的rrt算法及工业机器人路径避障规划方法 |
| CN113219998A (zh) * | 2021-06-15 | 2021-08-06 | 合肥工业大学 | 一种基于改进双向informed-RRT*的车辆路径规划方法 |
| KR20210096842A (ko) * | 2020-01-29 | 2021-08-06 | 한국과학기술원 | 주어진 엔드-이펙터 경로를 정확하게 따라가기 위한 리던던트 매니퓰레이터의 충돌 없는 궤적 최적화 방법 및 장치 |
| CN113352319A (zh) * | 2021-05-08 | 2021-09-07 | 上海工程技术大学 | 基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法 |
| CN113771035A (zh) * | 2021-09-24 | 2021-12-10 | 河北工业大学 | 基于rrt*算法的冗余多自由度机械臂避障路径优化方法 |
| CN113858205A (zh) * | 2021-10-25 | 2021-12-31 | 东南大学 | 一种基于改进rrt*的七轴冗余机械臂避障算法 |
-
2022
- 2022-06-13 CN CN202210662499.7A patent/CN114939872B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20210096842A (ko) * | 2020-01-29 | 2021-08-06 | 한국과학기술원 | 주어진 엔드-이펙터 경로를 정확하게 따라가기 위한 리던던트 매니퓰레이터의 충돌 없는 궤적 최적화 방법 및 장치 |
| CN112677153A (zh) * | 2020-12-16 | 2021-04-20 | 东北林业大学 | 一种改进的rrt算法及工业机器人路径避障规划方法 |
| CN113352319A (zh) * | 2021-05-08 | 2021-09-07 | 上海工程技术大学 | 基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法 |
| CN113219998A (zh) * | 2021-06-15 | 2021-08-06 | 合肥工业大学 | 一种基于改进双向informed-RRT*的车辆路径规划方法 |
| CN113771035A (zh) * | 2021-09-24 | 2021-12-10 | 河北工业大学 | 基于rrt*算法的冗余多自由度机械臂避障路径优化方法 |
| CN113858205A (zh) * | 2021-10-25 | 2021-12-31 | 东南大学 | 一种基于改进rrt*的七轴冗余机械臂避障算法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114939872A (zh) | 2022-08-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109571466B (zh) | 一种基于快速随机搜索树的七自由度冗余机械臂动态避障路径规划方法 | |
| CN109491389B (zh) | 一种具有速度约束的机器人轨迹跟踪方法 | |
| US9411335B2 (en) | Method and apparatus to plan motion path of robot | |
| CN110228069A (zh) | 一种机械臂在线避障运动规划方法 | |
| CN107234617B (zh) | 一种避障任务无关人工势场引导的避障路径规划方法 | |
| US8825209B2 (en) | Method and apparatus to plan motion path of robot | |
| CN112223291B (zh) | 一种基于三维任务空间约束的机械臂避障方法及装置 | |
| CN108818530B (zh) | 基于改进rrt算法的机械臂抓取散乱堆放活塞运动规划方法 | |
| CN112809682B (zh) | 机械臂避障路径规划方法、系统及存储介质 | |
| WO2021165745A1 (en) | Collision avoidance motion planning method for industrial robot | |
| CN114939872B (zh) | 基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法 | |
| CN112428274B (zh) | 一种多自由度机器人的空间运动规划方法 | |
| CN110598285A (zh) | 机械手轨迹逆运动学求解方法、装置及存储介质 | |
| Jiang et al. | Path planning for robotic manipulator in complex multi-obstacle environment based on improved_RRT | |
| CN114147708B (zh) | 基于改进的天牛须搜索算法的机械臂避障路径规划方法 | |
| Liao et al. | AGV path planning model based on reinforcement learning | |
| CN115416016A (zh) | 一种基于改进人工势场法的机械臂避障路径规划方法 | |
| CN116572244A (zh) | 基于rrt*fn算法的机械臂避障路径规划方法 | |
| Liu et al. | Improved RRT path planning algorithm for humanoid robotic arm | |
| Chen et al. | Path planning of the fruit tree pruning manipulator based on improved RRT-Connect algorithm | |
| Zhao et al. | Improved manipulator obstacle avoidance path planning based on potential field method | |
| Shao et al. | Rrt-goalbias and path smoothing based motion planning of mobile manipulators with obstacle avoidance | |
| CN112902971B (zh) | 基于高斯采样和目标偏向引导的快速扩展随机树算法的机器人运动轨迹计算方法、电子设备及存储介质 | |
| CN116872212A (zh) | 一种基于A-Star算法和改进人工势场法的双机械臂避障规划方法 | |
| CN116690557A (zh) | 基于点云的仿人三维扫描运动控制方法及装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |