[发明专利]基于RRT-Connect改进的医用移动机器人路径规划方法

权利要求书

1.一种基于RRT-Connect改进的医用移动机器人路径规划方法，其特征在于，包括如下步骤：

步骤1：在已知环境地图中，设置移动机器人路径规划的原始起始点和原始目标点；

步骤2：依据距离代价函数，获取起始点和目标点之间的距离，并基于中节点采样函数，在已知环境地图中进行等距采样，获得第二起始点和第二目标点；

步骤3：将原始起始点和原始目标点分别作为第一随机树和第二随机树的根节点，将第二起始点作为第三随机树、第四随机树的起始点，将第二目标点作为第五随机树、第六随机树的起始点，第一随机树至第六随机树均通过随机采样函数在已知环境地图中进行节点初始拓展；

其中第一随机树与第三随机树为彼此的拓展目标，第四随机树与第五随机树为彼此的拓展目标，第六随机树与第二随机树为彼此的拓展目标；

步骤4：对第一随机树至第六随机树均按步骤5-步骤6进行重新拓展，获得规划路径；

步骤5：对于每棵随机树，从环境地图中随机选择一个采样节点Q_rand，并计算同一随机树中其余节点与所选节点之间的距离，找出与所选节点的最近节点Q_near；

步骤6：从随机树的最近节点Q_near与采样节点Q_rand之间，按照节点步长距离D，获取离最近节点最近的新节点Q_new，并判断所获取的新节点、新节点与最近节点之间的路径是否与障碍物发生碰撞，若发生，则从随机树中删除该节点，并返回步骤5，否则，将新节点加入到随机树的路径中，直到当前随机树的新节点与相邻随机树的所有节点之间的距离均小于节点步长距离D，完成随机树的路径规划，进入步骤7；

步骤7：从重新拓展后的六棵随机树中的路径点，选择一条从原始起始点到原始目标点之间的最优路径；

所述中节点采样函数按以下公式计算：

其中，Q_init为原始起始点、Q_goal为原始目标点、Q_init2为第二起始点、Q_goal2为第二目标点；

最近节点Q_near的计算公式如下所示；

其中，Q_rand表示随机树T₁通过随机采样函数选择一个随机采样点，K_i表示随机树T₁中的节点，n表示随机树T₁中节点的数量。

2.根据权利要求1所述的方法，其特征在于，所述步骤6中节点之间的距离为欧式距离。

3.根据权利要求1所述的方法，其特征在于，以六棵随机树中对应的最短路径作为从原始起始点到原始目标点之间的最优路径。