CN115946117B - 一种三维空间路径规划方法 - Google Patents
一种三维空间路径规划方法 Download PDFInfo
- Publication number
- CN115946117B CN115946117B CN202211628076.XA CN202211628076A CN115946117B CN 115946117 B CN115946117 B CN 115946117B CN 202211628076 A CN202211628076 A CN 202211628076A CN 115946117 B CN115946117 B CN 115946117B
- Authority
- CN
- China
- Prior art keywords
- node
- new
- new node
- preselected
- path
- 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 41
- 238000012797 qualification Methods 0.000 claims abstract description 10
- 230000012010 growth Effects 0.000 claims abstract description 9
- 238000005070 sampling Methods 0.000 claims description 23
- 230000009191 jumping Effects 0.000 claims description 5
- 238000001514 detection method Methods 0.000 claims description 4
- 238000004422 calculation algorithm Methods 0.000 abstract description 50
- 230000033001 locomotion Effects 0.000 abstract description 17
- 230000008569 process Effects 0.000 abstract description 4
- 230000002457 bidirectional effect Effects 0.000 abstract description 3
- 238000013138 pruning Methods 0.000 abstract description 2
- 238000004088 simulation Methods 0.000 description 11
- 238000003801 milling Methods 0.000 description 10
- 238000010586 diagram Methods 0.000 description 8
- 230000006870 function Effects 0.000 description 8
- 230000002035 prolonged effect Effects 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 230000008859 change Effects 0.000 description 3
- 238000011161 development Methods 0.000 description 3
- 108091028140 FREP Proteins 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 2
- 238000012217 deletion Methods 0.000 description 2
- 230000037430 deletion Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 230000001788 irregular Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 101100273916 Schizosaccharomyces pombe (strain 972 / ATCC 24843) wip1 gene Proteins 0.000 description 1
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 239000012636 effector Substances 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000007773 growth pattern Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 230000001681 protective effect Effects 0.000 description 1
Classifications
-
- 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
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
本发明公开了一种三维空间路径规划方法、***及应用，方法包括：依据双树的扩展方向，获得预选新节点；依据预选新节点，进行父节点重选；依据父节点重选结果，分别对双树预选新节点的父节点进行新节点的生长，获得新节点；对新节点的合格性和终止性进行判断，获得最终路径。本发明建立概率目标阈值控制新节点的扩展方向，使搜索过程具有一定导向性；其次，结合双树思想，由起点单向搜索变成始末点双向搜索，提高了算法的搜索效率；在此基础上，通过人工势场思想对新节点生长方式的引导，使新节点的生长方向既避开了障碍物又进一步接近目标点；最后对规划路径进行剪枝处理，删除冗余点，利用三次B样条曲线进行路径平滑拟合，减少机械臂在运动时发生的震动，使路径运动更加平稳。
Description
技术领域
本发明涉及一种三维空间路径规划方法、***及应用，属于运动规划技术技术领域。
背景技术
随着智能制造技术的不断发展，机械臂广泛应用于工业发展，太空探索，医疗辅助等行业，减轻了人为因素对工作的影响，提高了工作效率，给制造业带来了许多便利。然而，当前机械臂的自动化和智能化程度依旧发展缓慢，在多数环境狭小作业场合，需要依赖人为的示教，工作人员难以直接参与该环境，导致了机械臂发展受到局限性。因此，机械臂自主的，高效的规划出一条可行路径成为当前的研究热点。
机械臂路径规划是指寻找一条从起始位姿避开障碍物到达终点位姿的可行路径。学者们已对机械臂路径规划算法做了大量研究，算法思想主要分为三类。其一，用启发式算法进行路径规划，主要有A*算法，蚁群算法，粒子群算法等，启发式算法虽然比较直观易于修改，能在较短时间内给出最优解，但是存在不稳定性，性能取决于具体问题和设计者的经验；其二，势场思想进行规划，主要为人工势场法，特点是一种反馈控制策略，对控制和传感误差具有一定鲁棒性，但是很容易陷入局部最小值，不一定得到最优解；其三，采样思想进行规划，主要有快速随机树算法(Rapidly-Exploring Random Trees)和概率地图法(Probailistic Roadmap Method)算法，适用于高维度空间，它们以概率完备性来代替完备性，从而提高搜索效率，但是路径曲折程度较大，不一定为最优值。因此，有必要设计优质的规划法实现三维空间路径规划。
发明内容
本发明提供了一种三维空间路径规划方法、***及应用，实现了三维空间路径规划，并进一步能有效地用于的机械臂放置工件作业路径规划。
本发明的技术方案是：
根据本发明的一方面，提供了一种三维空间路径规划方法，包括：依据双树的扩展方向，获得预选新节点；其中，双树包括起点树Tree1和终点树Tree2；依据预选新节点，进行父节点重选；依据父节点重选结果，分别对双树预选新节点的父节点进行新节点的生长，获得新节点；对新节点的合格性和终止性进行判断，获得最终路径。
所述依据双树的扩展方向，获得预选新节点，包括：当第一次迭代N＝1时，如果随机概率点大于等于目标偏置阈值时，以对方树的第一个节点作为采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew；否则，在初始化三维空间随机寻找一个采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew；当迭代次数N>1时，如果随机概率点大于等于目标偏置阈值时，以对方树的第N个新节点为采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew；否则在初始化三维空间随机寻找一个采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew。
所述依据预选新节点，进行父节点重选，包括：分别以起点树Tree1和终点树Tree2的预选新节点xnew为圆心，r为半径做圆，找到圆内所有的我方树中存在的点；依据圆内找到的点数进行父节点重选：如果圆内找到的点数大于2，构建重选父节点集合；判断我方树起点经过重选父节点集合中任一点到预选新节点的路径代价是否比起点经过原父节点到预选新节点路径代价小；如果存在任一点使路径代价小，则把使路径代价小的点作为预选新节点的重选父节点；如果不存在，则预选新节点的父节点不变；使路径最小的重选父节点命名为最小父节点xmin，并将最小父节点与预选新节点连线；其中，重选父节点集合中的节点为圆内找到的除起点和预选新节点原父节点之外的点；否则，则预选新节点的父节点不变。
所述依据父节点重选结果，分别对双树预选新节点的父节点进行新节点的生长，获得新节点，包括：在父节点上引入人工势场的思想，计算父节点的引力向量Fatt、斥力向量Frep和指向预选新节点的向量Frand，求出引力向量和斥力向量的和向量F，向量F与指向预选新节点的向量Frand求和得到新向量F1，在此基础上，父节点沿着新向量方向F1扩展一个步长则得到第N个新节点Xnew，用新节点Xnew代替预选新节点xnew。
所述对新节点的合格性和终止性进行判断，获得最终路径，包括：依据生成的新节点Xnew，判断新节点的父节点和新节点Xnew的连线是否与障碍物发生碰撞：如果发生碰撞，则删除此新节点并且跳到“依据双树的扩展方向，获得预选新节点”直到找到无碰撞新节点Xnew，反之，将新节点加入各自树的点集合；判断两棵树生成的新节点是否为同一个，若为同一个则结束迭代，找到最终路径，反之迭代次数加1，接着执行“依据双树的扩展方向，获得预选新节点”步骤。
根据本发明的另一方面，提供了一种三维空间路径规划***，包括：第一获得模块，用于依据双树的扩展方向，获得预选新节点；其中，双树包括起点树Tree1和终点树Tree2；重选模块，用于依据预选新节点，进行父节点重选；第二获得模块，用于依据父节点重选结果，分别对双树预选新节点的父节点进行新节点的生长，获得新节点；第三获得模块，用于对新节点的合格性和终止性进行判断，获得最终路径。
根据本发明的另一方面，提供了一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行上述中任意一项所述的三维空间路径规划方法。
根据本发明的另一方面，提供了一种三维空间路径规划方法的应用，用于机械臂放置工件作业路径规划，包括上述中任一项所述的三维空间路径规划方法，还包括：依据最终路径，删除冗余点，获得最短路径；依据最短路径，进行曲线拟合，获得平缓路径；依据平缓曲线路径进行碰撞检测，获得无碰撞路径。
所述依据最终路径，删除冗余点，获得最短路径，包括：依据最终路径，使用冗余点连续删除策略，路径点T＝{T1,T2…Tn}，若存在点Tk(k>1)，使得T1和Tk的连线与障碍物没有交集，并且T1和Tk+1之间的连线与障碍物存在交集，则去掉T1和Tk中间的所有点，直接将点T1和Tk连线；继续从Tk开始进行冗余点删除，直到遍历完所有路径点，去除所有冗余点。
对路径进行三次均匀B样条曲线拟合。
本发明的有益效果是：本发明建立概率目标阈值控制新节点的扩展方向，使搜索过程具有一定导向性；其次，结合双树思想，由起点单向搜索变成始末点双向搜索，提高了算法的搜索效率；在此基础上，通过人工势场思想对新节点生长方式的引导，使新节点的生长方向既避开了障碍物又进一步接近目标点；最后对规划路径进行剪枝处理，删除冗余点，利用三次B样条曲线进行路径平滑拟合，减少机械臂在运动时发生的震动，使路径运动更加平稳。另外，进行多次不同环境下不同算法路径规划实验对比，证明本发明算法的高效性，并且根据真实物理场景搭建机械臂放置工件仿真实验平台，进行机械臂改进算法的路径规划仿真实验，实验结果证明了本发明算法的应用性。
附图说明
图1是本发明实施例的流程示意图；
图2为双向扩展图；
图3为重画线重布线过程图；
图4为新节点生长方式图；
图5为第一种布局环境算法对比图；
图6为第二种布局环境算法对比图；
图7为第三种布局环境算法对比图；
图8为冗余点删除策略图；
图9为本发明的机械臂放置工件作业路径；
图10为各关节角度、关节角速度和关节角加速度变化曲线图。
具体实施方式
下面结合附图和实施例，对发明做进一步的说明，但本发明的内容并不限于所述范围。
实施例1：一种三维空间路径规划方法，包括：依据双树的扩展方向，获得预选新节点；其中，双树包括起点树Tree1和终点树Tree2；依据预选新节点，进行父节点重选；依据父节点重选结果，分别对双树预选新节点的父节点进行新节点的生长，获得新节点；对新节点的合格性和终止性进行判断，获得最终路径。
可选地，所述依据双树的扩展方向，获得预选新节点，包括：当第一次迭代N＝1时，如果随机概率点大于等于目标偏置阈值时，以对方树的第一个节点作为采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew；否则，在初始化三维空间随机寻找一个采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew；当迭代次数N>1时，如果随机概率点大于等于目标偏置阈值时，以对方树的第N个新节点为采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew；否则在初始化三维空间随机寻找一个采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew。
可选地，所述依据预选新节点，进行父节点重选，包括：分别以起点树Tree1和终点树Tree2的预选新节点xnew为圆心，r为半径做圆，找到圆内所有的我方树中存在的点；依据圆内找到的点数进行父节点重选：如果圆内找到的点数大于2，构建重选父节点集合；判断我方树起点经过重选父节点集合中任一点到预选新节点的路径代价是否比起点经过原父节点到预选新节点路径代价小；如果存在任一点使路径代价小，则把使路径代价小的点作为预选新节点的重选父节点；如果不存在，则预选新节点的父节点不变；使路径最小的重选父节点命名为最小父节点xmin，并将最小父节点与预选新节点连线；其中，重选父节点集合中的节点为圆内找到的除起点和预选新节点原父节点之外的点；否则，则预选新节点的父节点不变。
可选地，所述依据父节点重选结果，分别对双树预选新节点的父节点进行新节点的生长，获得新节点，包括：在父节点上引入人工势场的思想，计算父节点的引力向量Fatt、斥力向量Frep和指向预选新节点的向量Frand，求出引力向量和斥力向量的和向量F，向量F与指向预选新节点的向量Frand求和得到新向量F1，在此基础上，父节点沿着新向量方向F1扩展一个步长则得到第N个新节点Xnew，用新节点Xnew代替预选新节点xnew。
可选地，所述对新节点的合格性和终止性进行判断，获得最终路径，包括：依据生成的新节点Xnew，判断新节点的父节点和新节点Xnew的连线是否与障碍物发生碰撞：如果发生碰撞，则删除此新节点并且跳到“依据双树的扩展方向，获得预选新节点”直到找到无碰撞新节点Xnew，反之，将新节点加入各自树的点集合；判断两棵树生成的新节点是否为同一个，若为同一个则结束迭代，找到最终路径，反之迭代次数加1，接着执行“依据双树的扩展方向，获得预选新节点”步骤。
实施例2：一种三维空间路径规划***，包括：第一获得模块，用于依据双树的扩展方向，获得预选新节点；其中，双树包括起点树Tree1和终点树Tree2；重选模块，用于依据预选新节点，进行父节点重选；第二获得模块，用于依据父节点重选结果，分别对双树预选新节点的父节点进行新节点的生长，获得新节点；第三获得模块，用于对新节点的合格性和终止性进行判断，获得最终路径。
实施例3：一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行上述中任意一项所述的三维空间路径规划方法。
实施例4：如图1-10所示，一种三维空间路径规划方法的应用，用于机械臂放置工件作业路径规划，包括上述中任一项所述的三维空间路径规划方法，还包括：依据最终路径，删除冗余点，获得最短路径；依据最短路径，进行曲线拟合，获得平缓路径；依据平缓曲线路径进行碰撞检测，获得无碰撞路径。
可选地，所述依据最终路径，删除冗余点，获得最短路径，包括：依据最终路径，使用冗余点连续删除策略，路径点T＝{T1,T2…Tn}，若存在点Tk(k>1)，使得T1和Tk的连线与障碍物没有交集，并且T1和Tk+1之间的连线与障碍物存在交集，则去掉T1和Tk中间的所有点，直接将点T1和Tk连线；继续从Tk开始进行冗余点删除，直到遍历完所有路径点，去除所有冗余点。
可选地，对路径进行三次均匀B样条曲线拟合。
进一步地，给出可选地实施过程如下：
(1)初始化三维空间，建立障碍物信息点Obs，初始化起点位置xstart并且加入起点树Tree1作为起点树Tree1的第一个节点，初始化目标位置xgoal并且加入终点树Tree2作为终点树Tree2的第一个节点，迭代步长xstepsize，目标偏置阈值xvalue，人工势场引力系数ξ＝0.01，斥力系数ξr＝200，斥力场作用半径dmin＝100，人工势场的迭代步长λ＝3；初始化迭代次数N＝1且N取正整数。
(2)对起点树Tree1和终点树Tree2同时采用概率偏置的方式引导双树的扩展方向：
当第一次迭代时，起点树Tree1采样点xrand1的目标偏置概率公式为：
终点树Tree2采样点xrand2的目标偏置概率公式为：
当迭代次数大于等于2时，更换两棵树的采样目标点。起点树Tree1采样时，当随机概率点prandom大于等于偏置阈值时，目标采样点改为Tree2点集的最新节点；终点树Tree2同理，当随机概率点更大时，目标采样点为Tree1点集中的最新节点；公式为：
(3)分别以起点树Tree1和终点树Tree2的预选新节点xnew为圆心，r为半径做圆，找到圆内所有的我方树中存在的点；依据圆内找到的点数进行父节点重选：r一般取步长的2到3倍，本实施例中取2倍步长。
如果圆内找到的点数大于2，构建重选父节点集合；判断我方树起点经过重选父节点集合中任一点到预选新节点的路径代价是否比起点经过原父节点到预选新节点路径代价小；如果存在任一点使路径代价小，则把使路径代价小的点作为预选新节点的重选父节点；如果不存在，则预选新节点的父节点不变；使路径最小的重选父节点命名为最小父节点xmin，并将最小父节点与预选新节点连线；其中，重选父节点集合中的节点为圆内找到的除起点和预选新节点原父节点之外的点；需要说明的是，对于存在使路径代价小的任一点指的是对重选父节点集合中所有的点都经判断后所选择的使路径代价最小的点，如果判断后，存在路径代价比起点经过原父节点到预选新节点路径代价小的节点数据大于等于2，则任意选择一个节点作为重选父节点；否则，即如果圆内找到的点数等于2，则预选新节点的父节点不变；如图3所示，以一棵树为例，以预选新节点xnew为圆心，r为半径做圆，找到圆内所有的我方树中存在的点，共计4个，构建重选父节点集合(重选父节点集合中的节点为圆内找到的除起点和预选新节点原父节点之外的点，即共计2个)，根据计算，图中xmin使路径代价小于其它，因此使路径最小的重选父节点命名为最小父节点xmin，并将最小父节点与预选新节点连线。
(4)在父节点上引入人工势场的思想，计算父节点的引力向量Fatt、斥力向量Frep和指向预选新节点的向量Frand，求出引力向量和斥力向量的和向量F，向量F与指向预选新节点的向量Frand求和得到新向量F1，在此基础上，父节点沿着新向量方向F1扩展一个步长则得到第N个新节点Xnew，用新节点Xnew代替预选新节点xnew。
人工势场引力势函数为：
Uatt(x)＝1/2ξd2(x,xgoal)
人工势场引力函数为：
Fatt(x)＝▽Uatt(x)＝ξd(x,xgoal)
其中：ξ为引力常数；x和xgoal分别为规定区域中起点位置和目标位置；d(x,xgoal)表示两个位置之间的欧氏距离，为设定的距离阈值。
人工势场的斥力势函数为：
人工势场的斥力函数为：
其中：ξr为斥力常数；dmin为斥力场作用半径；为斥力势力场作用半径。
人工势场法的合力势函数为：
U(x)＝Uatt(x)+Urep(x)
人工势场法的和力函数为：
F(x)＝Fatt(x)+Frep(x)
新节点Xnew的生长公式为：
Xnew＝xmin+xstepsize(F/||F||+λFrand/||Frand||)
式中：xmin为最小父节点，xstepsize为迭代步长，λ为人工势场的迭代步长，F为人工势场和向量；Frand为指向向量，由父节点指向预选新节点xnew；
如图4所示，xnew为点xmin沿xmin—xrand方向扩展一个步长得到的点；向量Frep为障碍物对重选父节点xmin的排斥力，向量Fatt为目标点xgoal对点xmin的引力，F为点xmin在搜索区域内所受的人工势场和向量；Frand代表由点xmin指向xnew的向量。F1为F和Frand的和向量；点Xnew表示点xmin在向量F1的方向上迭代一次得到的新节点。
(5)依据生成的新节点Xnew，判断新节点的父节点和新节点Xnew的连线是否与障碍物发生碰撞：如果发生碰撞，则删除此新节点并且跳到步骤(2)直到找到无碰撞新节点Xnew，反之，将新节点加入各自树的点集合；判断两棵树生成的新节点是否为同一个，若为同一个则结束迭代，即xnew1＝xnew2，找到最终路径，反之迭代次数加1，接着执行步骤(2)；
(6)验证算法的高效性，本发明提出三种不同的高密度异型障碍物避障环境仿真实验。第一种环境为八边角球体大障碍物，这种布局模式描述了大型障碍物占据六面搜索区内四角的情况，分析了起末点存在较大障碍物是算法的融合性，仿真结果图如图5所示；第二种环境为规律对角型串行障碍物，该布局模式选取较小障碍物在始末连接点直线上，考虑了融合算法中人工势场容易在障碍物中心和始末点连线上发生震荡情况，仿真结果图如图6所示。第三种环境为不规则布局障碍物仿真实验，仿真结果图如图7所示。使用MATLAB2021b平台，设置搜索区域大小为[01000；01000；01000]，为搜索区域建立栅格地图，起始点为原点[000],目标点为[100010001000]，障碍物颜色为浅灰色。如下表中，本发明提出了四种算法的对比，分别是传统快速随机树算法(RRT)，目标概率偏置双向渐进最优快速随机树算法(Goal-bias BRRT*)，人工势场引导节点生长的快速随机树算法(APF-RRT)和本发明方法(GB-APFB-RRT*)；实验结果表明，四种自定义参数下，本发明算法规划时间、迭代步长、节点数、路径长度皆为最优，证明了本发明算法的准确性和高效性。
具体地，表1针对八边角球体大障碍物布局，取xstepsize＝5，xvalue＝0.5做运动规划图，如图5，分析表1数据可得，本发明算法与传统RRT算法相比规划时间减少了83％，迭代次数降低了97％，路径节点数降低了39％，路径长度减少了31％；与Goal-bias BRRT*算法相比比较指标分别减少了75％，94％，35％，22％；与APF-RRT算法相比比较指标分别减少了75％，94％，35％，22％。
针对本发明算法不同参数的对比，当迭代步长xstepsize＝5不变，目标偏置阈值xvalue从0.5上升到0.8时，规划时间延长了58％，路径长度增加了15％。保持目标偏置阈值xvalue不变，迭代步长xstepsize从5增加到10，规划时间减少了24％，但是路径长度有3％的增幅。
表1布局一环境算法仿真实验结果：
具体地，表2针对对角线串型障碍物布局，取xstepsize＝5，xvalue＝0.5做运动规划图，如图6，分析表2数据可得，本发明算法与传统RRT算法相比规划时间减少了82％，迭代次数降低了91％，路径节点数降低了32％，路径长度减少了33％；与Goal-bias BRRT*算法相比比较指标分别减少了81％，85％，11％，25％；与APF-RRT算法相比比较指标分别减少了60％，90％，30％，30％。
针对本发明算法不同参数的对比，当迭代步长xstepsize＝5不变，目标偏置阈值xvalue从0.5上升到0.8式时，规划时间延长了30％，路径长度增加了10％。保持目标偏置阈值xvalue不变，迭代步长xstepsize从5增加到10，规划时间减少了39％，但是路径长度有4％的增幅。
表2布局二环境算法仿真实验结果：
具体地，表3针对无规律始对角线障碍物布局，取xstepsize＝5，xvalue＝0.5做运动规划图，如图7，分析表3数据可得，本发明算法与传统RRT算法相比规划时间减少了95％，迭代次数降低了97％，路径节点数降低了28％，路径长度减少了33％；与Goal-bias BRRT*算法相比比较指标分别减少了75％，86％，20％，24％；与APF-RRT算法相比比较指标分别减少了53％，80％，35％，9％；
针对本发明算法不同参数的对比，当迭代步长xstepsize＝5不变，目标偏置阈值xvalue从0.5上升到0.8式时，规划时间延长了59％，路径长度增加了9％。保持目标偏置阈值xvalue＝0.5不变，迭代步长xstepsize从5增加到10，规划时间减少了39％，但是路径长度有1％的增幅。
表3布局三环境算法仿真实验结果：
综上，本发明从三种布局环境仿真实验结果分析可知，本发明算法与另外三种算法相比，规划时间、迭代次数、节点数、路径长度皆为最优并且实验图路径相对平缓程度更高，无用枝叶节点的扩展更少。规划时间为改善最优值，其次是迭代次数，在相同计算机配置下，可以有效地降低运行内存，提升搜索效率。
针对算法的最优自定义参数分析，当迭代步长xstepsize＝5不变，提高目标偏置阈值，会提升规划时间，增加路径长度。当目标偏置阈值xvalue＝0.5不变时，增加迭代步长，可有效降低规划时间，但是路径长度会有增幅，伴随着拐点数量的增加，基于算法路径的最有优性，选取迭代步长xstepsize＝5，目标偏置阈值xvalue＝0.8。
(7)找到最终路径后，路径中存在极少数冗余点，使用冗余点连续删除策略，路径点T＝{T1,T2…Tn}，若存在点Tk(k>1)，使得T1和Tk的连线与障碍物没有交集，并且T1和Tk+1之间的连线与障碍物存在交集，则去掉T1和Tk中间的所有点，直接将点T1和Tk连线。继续从Tk开始进行冗余点删除，直到遍历完所有路径点，去除所有冗余点。如图8所示。
(8)删除冗余点后，为了减少机械臂在运动过程中的机械冲击和末端抖动，需要对算法所规划出的路径进行三次B样条曲线进行拟合实现平滑处理，B样条曲线是贝塞尔(Bezier)曲线的基础上发展起来的一种或样条曲线，它克服了贝塞尔曲线整体性能带来的不便。
K次B样条曲线方程为：
式中：K表示B样条的次数，K+1表示曲线阶数，i表示B样条的序号，控制点di个数为n+1。Ni,k(u)b表示i个K次B样条基函数，使用Cox-deBoor递推公式获取；
因此，三次非均匀B样条基函数为：
三次非均匀B样条曲线定义为：
p0,3(u)＝d0*N0,3(u)+d1*N1,3(u)+d2*N2,3(u)+d3*N3,3(u)u∈[0,1]
当给定控制点di后，利用上式就可以求出一条三次B样式的平缓曲线点。
(9)在Matlab平台搭建运动规划场景。起始点为机械臂末端执行器机械手抓取执行器位置，目标点为数控铣床工作台工件装夹定位中心。障碍物环境为数控铣床防护挡板，工作台，夹具和铣刀。采用包围盒的方式对机械臂与障碍物进行碰撞检测，将经过步骤8所得路径后，ERF-900机械臂在关节空间中采样规划则可以得到一条从起点到终点的无碰撞路径。记录了机械臂通过本文算法规划路径中的的六个运行位姿，分别是起始位姿(a)，机械臂位于零点状态；转向铣床位姿(b)，机械臂开始转向运动，通过运动使机械臂末端指向铣床正面；面向铣床防护板位姿(c)，机械臂完成转向运动，末端面向铣床夹具；准备进入铣床防护板位姿(d)，机械臂开始进行放置运动；经过铣床防护板位姿(f)，机械臂经过铣床防护板运动状态；终点位姿(e)，机械臂到达夹具装夹工件位置，完成工件的放置，如图9所示。
(10)、图10所示为本文机械臂放置工件运动规划得到的各关节角度、关节角速度和关节角加速度变化曲线。由图可以看出，通过对机械臂末端轨迹进行三次B样条曲线拟合后，得到了平滑的关节角变化曲线，速度与加速度没有发生突变。在末端得到平缓的轨迹并且减少了机械的抖动程度，保障了机械臂在运动规划得到的路径上平稳运行并且成功避障。
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
上面结合附图对本发明的具体实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。
Claims (6)
1.一种三维空间路径规划方法，其特征在于，包括：
依据双树的扩展方向，获得预选新节点；其中，双树包括起点树Tree1和终点树Tree2；
依据预选新节点，进行父节点重选；
依据父节点重选结果，分别对双树预选新节点的父节点进行新节点的生长，获得新节点；
对新节点的合格性和终止性进行判断，获得最终路径；
所述依据双树的扩展方向，获得预选新节点，包括：
当第一次迭代N=1时，如果随机概率点大于等于目标偏置阈值时，以对方树的第一个节点作为采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew；否则，在初始化三维空间随机寻找一个采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew；
当迭代次数N>1时，如果随机概率点大于等于目标偏置阈值时，以对方树的第N个新节点为采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew；否则在初始化三维空间随机寻找一个采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew；
所述依据预选新节点，进行父节点重选，包括：
分别以起点树Tree1和终点树Tree2的预选新节点xnew为圆心，r为半径做圆，找到圆内所有的我方树中存在的点；依据圆内找到的点数进行父节点重选：
如果圆内找到的点数大于2，构建重选父节点集合；判断我方树起点经过重选父节点集合中任一点到预选新节点的路径代价是否比起点经过原父节点到预选新节点路径代价小；如果存在任一点使路径代价小，则把使路径代价小的点作为预选新节点的重选父节点；如果不存在，则预选新节点的父节点不变；使路径最小的重选父节点命名为最小父节点xmin，并将最小父节点与预选新节点连线；其中，重选父节点集合中的节点为圆内找到的除起点和预选新节点原父节点之外的点；
否则，则预选新节点的父节点不变；
所述依据父节点重选结果，分别对双树预选新节点的父节点进行新节点的生长，获得新节点，包括：
在父节点上引入人工势场的思想，计算父节点的引力向量Fatt、斥力向量Frep和指向预选新节点的向量Frand，求出引力向量和斥力向量的和向量F，向量F与指向预选新节点的向量Frand求和得到新向量F1，在此基础上，父节点沿着新向量方向F1扩展一个步长则得到第N个新节点Xnew，用新节点Xnew代替预选新节点xnew；
所述对新节点的合格性和终止性进行判断，获得最终路径，包括：
依据生成的新节点Xnew，判断新节点的父节点和新节点Xnew的连线是否与障碍物发生碰撞：如果发生碰撞，则删除此新节点并且跳到“依据双树的扩展方向，获得预选新节点”直到找到无碰撞新节点Xnew，反之，将新节点加入各自树的点集合；判断两棵树生成的新节点是否为同一个，若为同一个则结束迭代，找到最终路径，反之迭代次数加1，接着执行“依据双树的扩展方向，获得预选新节点”步骤。
2.一种三维空间路径规划***，其特征在于，包括：
第一获得模块，用于依据双树的扩展方向，获得预选新节点；其中，双树包括起点树Tree1和终点树Tree2；
重选模块，用于依据预选新节点，进行父节点重选；
第二获得模块，用于依据父节点重选结果，分别对双树预选新节点的父节点进行新节点的生长，获得新节点；
第三获得模块，用于对新节点的合格性和终止性进行判断，获得最终路径；
所述依据双树的扩展方向，获得预选新节点，包括：
当第一次迭代N=1时，如果随机概率点大于等于目标偏置阈值时，以对方树的第一个节点作为采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew；否则，在初始化三维空间随机寻找一个采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew；
当迭代次数N>1时，如果随机概率点大于等于目标偏置阈值时，以对方树的第N个新节点为采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew；否则在初始化三维空间随机寻找一个采样方向点xrand进行一个步长扩展，获得第N个预选新节点xnew；
所述依据预选新节点，进行父节点重选，包括：
分别以起点树Tree1和终点树Tree2的预选新节点xnew为圆心，r为半径做圆，找到圆内所有的我方树中存在的点；依据圆内找到的点数进行父节点重选：
如果圆内找到的点数大于2，构建重选父节点集合；判断我方树起点经过重选父节点集合中任一点到预选新节点的路径代价是否比起点经过原父节点到预选新节点路径代价小；如果存在任一点使路径代价小，则把使路径代价小的点作为预选新节点的重选父节点；如果不存在，则预选新节点的父节点不变；使路径最小的重选父节点命名为最小父节点xmin，并将最小父节点与预选新节点连线；其中，重选父节点集合中的节点为圆内找到的除起点和预选新节点原父节点之外的点；
否则，则预选新节点的父节点不变；
所述依据父节点重选结果，分别对双树预选新节点的父节点进行新节点的生长，获得新节点，包括：
在父节点上引入人工势场的思想，计算父节点的引力向量Fatt、斥力向量Frep和指向预选新节点的向量Frand，求出引力向量和斥力向量的和向量F，向量F与指向预选新节点的向量Frand求和得到新向量F1，在此基础上，父节点沿着新向量方向F1扩展一个步长则得到第N个新节点Xnew，用新节点Xnew代替预选新节点xnew；
所述对新节点的合格性和终止性进行判断，获得最终路径，包括：
依据生成的新节点Xnew，判断新节点的父节点和新节点Xnew的连线是否与障碍物发生碰撞：如果发生碰撞，则删除此新节点并且跳到“依据双树的扩展方向，获得预选新节点”直到找到无碰撞新节点Xnew，反之，将新节点加入各自树的点集合；判断两棵树生成的新节点是否为同一个，若为同一个则结束迭代，找到最终路径，反之迭代次数加1，接着执行“依据双树的扩展方向，获得预选新节点”步骤。
3.一种处理器，其特征在于，所述处理器用于运行程序，其中，所述程序运行时执行权利要求1所述的三维空间路径规划方法。
4.一种三维空间路径规划方法的应用，其特征在于，用于机械臂放置工件作业路径规划，包括：权利要求1所述的一种三维空间路径规划方法，还包括：
依据最终路径，删除冗余点，获得最短路径；
依据最短路径，进行曲线拟合，获得平缓路径；
依据平缓曲线路径进行碰撞检测，获得无碰撞路径。
5.根据权利要求4所述的三维空间路径规划方法的应用，其特征在于，所述依据最终路径，删除冗余点，获得最短路径，包括：
依据最终路径，使用冗余点连续删除策略，路径点T={T1,T2…Tn}，若存在点Tk（k>1），使得T1 和Tk的连线与障碍物没有交集，并且T1 和Tk+1之间的连线与障碍物存在交集，则去掉T1 和Tk中间的所有点，直接将点T1 和Tk连线；继续从Tk开始进行冗余点删除，直到遍历完所有路径点，去除所有冗余点。
6.根据权利要求4所述的三维空间路径规划方法的应用，其特征在于，对路径进行三次均匀B样条曲线拟合。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211628076.XA CN115946117B (zh) | 2022-12-16 | 2022-12-16 | 一种三维空间路径规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211628076.XA CN115946117B (zh) | 2022-12-16 | 2022-12-16 | 一种三维空间路径规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115946117A CN115946117A (zh) | 2023-04-11 |
| CN115946117B true CN115946117B (zh) | 2024-06-11 |
Family
ID=87286830
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211628076.XA Active CN115946117B (zh) | 2022-12-16 | 2022-12-16 | 一种三维空间路径规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115946117B (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101339480B1 (ko) * | 2012-12-14 | 2013-12-10 | 고려대학교 산학협력단 | Ｒｒｔ 기반의 듀얼 트리 구조를 이용한 이동 로봇의 궤적 계획 방법 |
| CN108981704A (zh) * | 2018-07-13 | 2018-12-11 | 昆明理工大学 | 一种基于动态步长的目标引力双向rrt路径规划方法 |
| CN110083165A (zh) * | 2019-05-21 | 2019-08-02 | 大连大学 | 一种机器人在复杂狭窄环境下路径规划方法 |
| CN113084811A (zh) * | 2021-04-12 | 2021-07-09 | 贵州大学 | 一种机械臂路径规划方法 |
| CN114161416A (zh) * | 2021-12-06 | 2022-03-11 | 贵州大学 | 基于势函数的机器人路径规划方法 |
-
2022
- 2022-12-16 CN CN202211628076.XA patent/CN115946117B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101339480B1 (ko) * | 2012-12-14 | 2013-12-10 | 고려대학교 산학협력단 | Ｒｒｔ 기반의 듀얼 트리 구조를 이용한 이동 로봇의 궤적 계획 방법 |
| CN108981704A (zh) * | 2018-07-13 | 2018-12-11 | 昆明理工大学 | 一种基于动态步长的目标引力双向rrt路径规划方法 |
| CN110083165A (zh) * | 2019-05-21 | 2019-08-02 | 大连大学 | 一种机器人在复杂狭窄环境下路径规划方法 |
| CN113084811A (zh) * | 2021-04-12 | 2021-07-09 | 贵州大学 | 一种机械臂路径规划方法 |
| CN114161416A (zh) * | 2021-12-06 | 2022-03-11 | 贵州大学 | 基于势函数的机器人路径规划方法 |
Non-Patent Citations (3)
| Title |
|---|
| Biased Sampling Potentially Guided Intelligent Bidirectional RRT∗ Algorithm for UAV Path Planning in 3D Environment;Xiaojing Wu;Mathematical Problems in Engineering;20191119;第2019卷;全文 * |
| 六轴机械臂路径规划及孪生实现;尹庆文;中国优秀硕士学位论文全文数据库;20240415(第4（2024）期);I140-693 * |
| 动态约束采样RRT*-Connect算法的移动机器人路径规划;陈忠;机械科学与技术;20231225;12 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115946117A (zh) | 2023-04-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Zhang et al. | Path planning of industrial robot based on improved RRT algorithm in complex environments | |
| Chen et al. | Path planning for manipulators based on an improved probabilistic roadmap method | |
| CN110962130B (zh) | 基于目标偏向寻优的启发式rrt机械臂运动规划方法 | |
| CN108356819B (zh) | 基于改进a*算法的工业机械臂无碰撞路径规划方法 | |
| CN110703768B (zh) | 一种改进型动态rrt*的移动机器人运动规划方法 | |
| CN110231824B (zh) | 基于直线偏离度方法的智能体路径规划方法 | |
| CN111761582B (zh) | 一种基于随机采样的移动机械臂避障规划方法 | |
| Jiang et al. | Path planning for robotic manipulator in complex multi-obstacle environment based on improved_RRT | |
| CN114147708B (zh) | 基于改进的天牛须搜索算法的机械臂避障路径规划方法 | |
| CN112428274B (zh) | 一种多自由度机器人的空间运动规划方法 | |
| CN113858205A (zh) | 一种基于改进rrt*的七轴冗余机械臂避障算法 | |
| CN114939872B (zh) | 基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法 | |
| CN116572244A (zh) | 基于rrt*fn算法的机械臂避障路径规划方法 | |
| CN115416016A (zh) | 一种基于改进人工势场法的机械臂避障路径规划方法 | |
| Dai et al. | Novel potential guided bidirectional RRT* with direct connection strategy for path planning of redundant robot manipulators in joint space | |
| US20210323154A1 (en) | Disassembly based assembly planning | |
| Ji et al. | E-RRT*: Path Planning for Hyper-Redundant Manipulators | |
| Shao et al. | Rrt-goalbias and path smoothing based motion planning of mobile manipulators with obstacle avoidance | |
| He et al. | Robot path planning using improved rapidly-exploring random tree algorithm | |
| JP2003280710A (ja) | ロボットハンドの作業軌道の生成と制御方法 | |
| CN115946117B (zh) | 一种三维空间路径规划方法 | |
| CN117260735A (zh) | 一种机器人深框抓取的路径规划方法 | |
| CN114986501A (zh) | 一种机械臂路径规划方法、***及机械臂 | |
| CN116852367A (zh) | 一种基于改进RRTstar的六轴机械臂避障路径规划方法 | |
| Jarvis | Robot path planning: complexity, flexibility and application scope |
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 |