1.一种三维空间路径规划方法,其特征在于,包括:依据双树的扩展方向,获得预选新节点;其中,双树包括起点树Tree1和终点树Tree2;
依据预选新节点,进行父节点重选;
依据父节点重选结果,分别对双树预选新节点的父节点进行新节点的生长,获得新节点;
对新节点的合格性和终止性进行判断,获得最终路径;
所述依据双树的扩展方向,获得预选新节点,包括:
当第一次迭代N=1时,如果随机概率点大于等于目标偏置阈值时,以对方树的第一个节点作为采样方向点x当迭代次数N>1时,如果随机概率点大于等于目标偏置阈值时,以对方树的第N个新节点为采样方向点x所述依据预选新节点,进行父节点重选,包括:
分别以起点树Tree1和终点树Tree2的预选新节点x如果圆内找到的点数大于2,构建重选父节点集合;判断我方树起点经过重选父节点集合中任一点到预选新节点的路径代价是否比起点经过原父节点到预选新节点路径代价小;如果存在任一点使路径代价小,则把使路径代价小的点作为预选新节点的重选父节点;如果不存在,则预选新节点的父节点不变;使路径最小的重选父节点命名为最小父节点x否则,则预选新节点的父节点不变;
所述依据父节点重选结果,分别对双树预选新节点的父节点进行新节点的生长,获得新节点,包括:在父节点上引入人工势场的思想,计算父节点的引力向量F所述对新节点的合格性和终止性进行判断,获得最终路径,包括:依据生成的新节点Xnew,判断新节点的父节点和新节点Xnew的连线是否与障碍物发生碰撞:如果发生碰撞,则删除此新节点并且跳到“依据双树的扩展方向,获得预选新节点”直到找到无碰撞新节点Xnew,反之,将新节点加入各自树的点集合;判断两棵树生成的新节点是否为同一个,若为同一个则结束迭代,找到最终路径,反之迭代次数加1,接着执行“依据双树的扩展方向,获得预选新节点”步骤。
2.一种三维空间路径规划系统,其特征在于,包括:第一获得模块,用于依据双树的扩展方向,获得预选新节点;其中,双树包括起点树Tree1和终点树Tree2;
重选模块,用于依据预选新节点,进行父节点重选;
第二获得模块,用于依据父节点重选结果,分别对双树预选新节点的父节点进行新节点的生长,获得新节点;
第三获得模块,用于对新节点的合格性和终止性进行判断,获得最终路径;
所述依据双树的扩展方向,获得预选新节点,包括:
当第一次迭代N=1时,如果随机概率点大于等于目标偏置阈值时,以对方树的第一个节点作为采样方向点x当迭代次数N>1时,如果随机概率点大于等于目标偏置阈值时,以对方树的第N个新节点为采样方向点x所述依据预选新节点,进行父节点重选,包括:
分别以起点树Tree1和终点树Tree2的预选新节点x如果圆内找到的点数大于2,构建重选父节点集合;判断我方树起点经过重选父节点集合中任一点到预选新节点的路径代价是否比起点经过原父节点到预选新节点路径代价小;如果存在任一点使路径代价小,则把使路径代价小的点作为预选新节点的重选父节点;如果不存在,则预选新节点的父节点不变;使路径最小的重选父节点命名为最小父节点x否则,则预选新节点的父节点不变;
所述依据父节点重选结果,分别对双树预选新节点的父节点进行新节点的生长,获得新节点,包括:在父节点上引入人工势场的思想,计算父节点的引力向量F所述对新节点的合格性和终止性进行判断,获得最终路径,包括:依据生成的新节点Xnew,判断新节点的父节点和新节点Xnew的连线是否与障碍物发生碰撞:如果发生碰撞,则删除此新节点并且跳到“依据双树的扩展方向,获得预选新节点”直到找到无碰撞新节点Xnew,反之,将新节点加入各自树的点集合;判断两棵树生成的新节点是否为同一个,若为同一个则结束迭代,找到最终路径,反之迭代次数加1,接着执行“依据双树的扩展方向,获得预选新节点”步骤。
3.一种处理器,其特征在于,所述处理器用于运行程序,其中,所述程序运行时执行权利要求1所述的三维空间路径规划方法。
4.一种三维空间路径规划方法的应用,其特征在于,用于机械臂放置工件作业路径规划,包括:权利要求1所述的一种三维空间路径规划方法,还包括:依据最终路径,删除冗余点,获得最短路径;
依据最短路径,进行曲线拟合,获得平缓路径;
依据平缓曲线路径进行碰撞检测,获得无碰撞路径。
5.根据权利要求4所述的三维空间路径规划方法的应用,其特征在于,所述依据最终路径,删除冗余点,获得最短路径,包括:依据最终路径,使用冗余点连续删除策略,路径点T={T
6.根据权利要求4所述的三维空间路径规划方法的应用,其特征在于,对路径进行三次均匀B样条曲线拟合。