[发明专利]一种基于A*算法和RRT*算法的移动机器人路径规划方法

权利要求书

1.一种基于A*算法和RRT*算法的移动机器人路径规划方法，其特征在于，所述基于A*算法和RRT*算法的移动机器人路径规划方法，包括：

步骤S1、取移动机器人本次运动的起点坐标和终点坐标，将连接起点坐标和终点坐标的线段离散成N个点；

步骤S2、根据离散得到的N个点进行路径规划，包括：

若N个点均不在障碍物所在范围内，则采用RRT*算法进行全段路径规划；

或者，若N个点中，由起点坐标起的前floor(N/2)个点中存在1个或多个点位于障碍物所在范围内，后N-floor(N/2)个点均不在障碍物所在范围内，则先采用A*算法，以起点坐标作为A*算法的规划起始坐标，以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为A*算法的规划目标坐标进行前段路径规划，后采用RRT*算法，以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为RRT*算法的规划起始坐标，以终点坐标作为RRT*算法的规划目标坐标进行后段路径规划；其中floor()为向下取整函数；

或者，若N个点中，由起点坐标起的前floor(N/2)个点均不在障碍物所在范围内，后N-floor(N/2)个点中存在1个或多个点位于障碍物所在范围内，则先采用RRT*算法，以起点坐标作为RRT*算法的规划起始坐标，以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为RRT*算法的规划目标坐标进行前段路径规划，后采用A*算法，以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为A*算法的规划起始坐标，以终点坐标作为A*算法的规划目标坐标进行后段路径规划；

或者，若N个点中，由起点坐标起的前floor(N/2)个点中存在1个或多点在障碍物所在范围内，后N-floor(N/2)个点中也存在1个或多个点在障碍物所在范围内，则采用A*算法进行全段路径规划；

步骤S3、根据路径规划后得到的路径中的控制节点控制移动机器人由起点坐标运动至终点坐标。

2.如权利要求1所述的基于A*算法和RRT*算法的移动机器人路径规划方法，其特征在于，所述步骤S3，根据路径规划后得到的路径中的控制节点控制移动机器人由起点坐标运动至终点坐标，包括：

步骤S3.1、取全段路径规划得到的全段路径中的控制节点；

或者，整合前段路径规划得到的前段路径和后段路径规划得到的后段路径生成全段路径，取全段路径中的控制节点；

步骤S3.2、将所有控制节点按照由起点坐标至终点坐标的方向，两两以父节点至子节点的顺序保存在一个数组中；

步骤S3.3、取数组中的第一个控制节点作为第一节点，取数组中的最后一个控制节点作为第二节点；

步骤S3.4、判断第一节点和第二节点的连线是否穿过障碍物所在范围，若连线未穿过障碍物所在范围，则仅保留第一节点和第二节点，丢弃第一节点和第二节点之间的其余控制节点，并执行步骤S3.5；若连线穿过障碍物所在范围，则找到第二节点的父节点作为新的第二节点，并重新执行步骤S3.4；

步骤S3.5、判断当前找到的第二节点是否是数组中的最后一个控制节点，若是则执行步骤S3.6；否则以当前找到的第二节点作为新的第一节点，以数组中的最后一个控制节点作为新的第二节点重新执行步骤S3.4；

步骤S3.6、取最终保留的控制节点，作为移动机器人运动中的局部目标点，控制移动机器人由起点坐标运动至终点坐标。