[发明专利]一种适用于机械臂关节空间路径规划的新型双向RRT*方法

权利要求书

1.一种适用于机械臂关节空间路径规划的新型双向RRT*方法，其特征在于，包括下列步骤：

步骤1，分别在初始关节角q_start与目标关节角q_goal处各建立一颗搜索树T₁与T₂，并将这两组关节角作为各自的根节点；

步骤2，计算搜索树T₁的最新节点q_current与搜索树T₂中所有节点之间的距离，然后挑选出与节点q_current距离最近的节点q_closest，并对两者进行直连判断；若结果显示两者无法直连，则舍弃该局部路径并执行步骤3的操作；反之，则将这两个节点直接相连，并结束整个规划过程；

步骤3，随机选取搜索树T₂中的一个节点作为局部目标点q_localgoal，然后判断随机概率p_rand是否小于给定的阈值概率p_threshold，若小于，则在机械臂关节运动范围内随机选取一组关节角q_rand作为采样点q_sample；反之，则将局部目标点q_localgoal作为采样点q_sample；

步骤4，在树T₁中搜寻距离采样点q_sample最近的节点q_near；

步骤5，若采样点q_sample为随机点q_rand，则在采样点q_sample与节点q_near的连线上拓展一个新节点q_new；若采样点q_sample为局部目标点q_localgoal，则通过人工势场法确定拓展方向，并在该方向上拓展一个新节点q_new；其中点q_new与节点q_near之间的距离为拓展步长δ；然后对点q_new对应的机械臂的空间状态进行碰撞检测，若机械臂不发生碰撞，则将点q_new作为新节点加入树T₁中；反之，则将该点作为描述障碍物的特征点进行保存，且不将其加入树T₁，然后跳转到步骤9；

步骤6，在点q_new的邻域内重选其父节点，依次将邻域内的各节点当作点q_new的父节点并计算此时的路径代价，再从中选取路径代价最小的节点作为其新的父节点；

步骤7，在点q_new的邻域内进行改线操作，将点q_new作为各节点的父节点并计算此时的路径代价，若其小于原路径代价，则舍弃该节点原有的父节点而将点q_new作为其新的父节点；反之，则保留原有的父节点；

步骤8，检测新节点q_new是否包含在树T₂中或与树T₂的某个节点间的距离小于给定阈值，若满足上述条件，则代表树T₁与树T₂彼此相连，规划过程结束；反之，执行步骤9；

步骤9，判断当前搜索树是T₁还是T₂，若是树T₁，则将操作对象切换至树T₂；若是树T₂，则将操作对象切换至树T₁；然后返回执行步骤2到步骤8的操作。