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、取最终保留的控制节点,作为移动机器人运动中的局部目标点,控制移动机器人由起点坐标运动至终点坐标。
3.如权利要求1所述的基于A*算法和RRT*算法的移动机器人路径规划方法,其特征在于,所述A*算法,包括:
1)将规划起始坐标作为起始节点a,将规划目标坐标作为终节点b,从起始节点a开始,把起始节点a作为待处理的方格,存入一个open表中,open表存放的是待检查的节点;
2)寻找起始节点a相邻并可以直接到达的节点,将寻找到的节点都放入open表中,并将寻找到的节点的父节点设置为起始节点a;
3)从open表中删除起始节点a,并将起始节点a加入close表中,close表中存放的是不需要再次检查的节点;
4)从open表中找出F值最小的节点c,其中F值的计算如下:
F=G+H
其中G表示从起始节点a移动到当前计算节点的移动代价,H表示从当前计算节点到终节点的估值代价;
5)把节点c从open表中删除,放入close表中;
6)检查节点c所有相邻并且可以直接到达的节点,若所述节点还不在open表中,则将所述节点加入open表,并将所述节点的父节点设置为节点c;
7)如果节点c所有相邻并且可以直接到达的节点中的部分节点已经在open表中,则计算已经在open表中的节点新的路径的G值,若G值更低则将当前计算节点的父节点设置为节点c,并重新计算当前计算节点的F值;否则不作处理;
8)重复步骤4)~7)直至将终节点加入open表,说明路径已经找到,路径规划完成;或者重复步骤4)~7)直至查找终节点失败,并且open表为空,表明不存在路径,路径规划完成。
4.如权利要求1所述的基于A*算法和RRT*算法的移动机器人路径规划方法,其特征在于,所述RRT*算法,包括:
1)在规划起始坐标和规划目标坐标之间基于RRT算法找到一条可行的初始路径;
2)以规划起始坐标和规划目标坐标作为椭圆的两个焦点,以所述初始路径的总长度为椭圆上的点到两个焦点的距离之和,构建一个椭圆;
3)在所述椭圆中进行重采样,并对重采样得到的节点重新选择父节点;
4)为随机树进行重新布线。
5.如权利要求4所述的基于A*算法和RRT*算法的移动机器人路径规划方法,其特征在于,所述在椭圆中进行重采样,包括:
3.1)连接椭圆的两个顶点得到线段l1,将线段l1进行M等分,记M-1个等分点为m0,m1,…mM-3,mM-2;
3.2)分别过各等分点做线段l1的垂线,记垂线交于椭圆的上半部分的交点为并保存在数组v1中,记垂线交于椭圆的下半部分的交点为并保存在数组v2中;
3.3)判断数组v1和v2中的交点在障碍物所在范围内的情况,选取数组v1和v2中交点在障碍物所在范围内较少的一组所对应的一半椭圆作为重采样区域,在所述重采样区域中进行重采样。