[发明专利]一种考虑狭窄通道的移动机器人路径规划方法及系统

权利要求书

1.一种考虑狭窄通道的移动机器人路径规划方法，其特征在于，包括以下步骤：

S1.环境地图的构建：对机器人所处的环境包括狭窄通道进行建模描述；

S2.改进算法后的初始路径求解：改进后的RRT算法融合对狭窄通道的检测机制，从给定的起始点开始，通过对配置空间进行检索，生成新节点，不断地扩展由随机采样配置点构造的树形结构，并进行狭窄通道检测和碰撞检测，最终在扩展树中寻找到一条从初始位置到目标位置的无碰撞路径，将所有符合安全距离的新节点依次存储在初始路径的树；

S3.对初始路径进行优化：在融合狭窄通道检测机制的RRT算法的基础上改进路径节点搜索方式，对初始路径进行优化处理，为初始路径中存储的新节点寻找父节点以减少路径拐点，进行狭窄通道检测和碰撞检测并寻找出一条渐近最优的优化路径。

2.根据权利要求1所述的一种考虑狭窄通道的移动机器人路径规划方法，其特征在于，S1中环境地图的构建的具体内容包括：

S11.确定各障碍物的几何中心坐标位置；

S12.对障碍物进行膨胀处理以满足安全要求，获得安全的环境地图。

3.根据权利要求2所述的一种考虑狭窄通道的移动机器人路径规划方法，其特征在于，S11中的几何中心坐标位置为x和y；

S12中障碍物膨胀处理是以障碍物坐标为中心，向外扩张一定距离：

膨胀的距离为：

L≥α*max   (r)

其中，α为膨胀系数，0α1，r为机器人的斜长半径；

其中，w为机器人宽度,h为机器人长。

4.根据权利要求1所述的一种考虑狭窄通道的移动机器人路径规划方法，其特征在于，S2的具体内容包括：

S21.在二维空间中，将机器人量化为节点p,在p的X轴方向上分别向两侧延伸出长度为R的桥，两个端点设为p1和p2，在桥的基础上正交出新的一组垂直桥，长度为R，端点分别设为p3和p4。

5.根据权利要求4所述的一种考虑狭窄通道的移动机器人路径规划方法，其特征在于，S2的具体内容还包括：

S22.在已经建立好的环境地图中随机选择起始点Xinit和终止点Xgoal的位置，并确定初始条件，初始化随机树，确定算法参数；

S23.执行采样函数，在空间中选择一个随机点Xrand，寻找离随机点Xrand最近的临近根节点Xnear；

S24.连接随机点Xrand和最近的临近根节点Xnear，作为树生长的方向，执行树生长函数，设置步长作为树枝的长度，生长随机树，在Xrand和Xnear的连线上根据步长确定生成的新节点Xnew；

S25.对狭窄通道进行检测并执行碰撞检测函数，遍历所有障碍物，判断机器人所处空间以及是否满足安全距离，若树生长的过程中遇到障碍物或者无法通过则停止生长，否则继续生长；

S26.重复步骤S22-S25，调用连接函数将每一次生长的Xnew与Xgoal相连，判断两点之间的距离是否小于步长，若小于，将生成的节点Xnew与终点Xgoal直接连起来，并将所有符合安全距离的节点Xnew依次存储在初始路径的树中，显示规划出的初始路径长度，生成初始路径。

6.根据权利要求5所述的一种考虑狭窄通道的移动机器人路径规划方法，其特征在于，S25中判断机器人所处空间的具体内容为：

当p3和p4同时在包括边缘的障碍物内，p1和p2同时在自由空间或两点中的任意一点在障碍物内，则机器人处于狭窄通道内；当p1、p2、p3和p4均在自由空间内，则机器人点处于自由空间中。

7.根据权利要求6所述的一种考虑狭窄通道的移动机器人路径规划方法，其特征在于，S25中对判断机器人满足安全距离的具体内容为：

当移动机器人处于狭窄通道时，计算机器人所处狭窄通道两侧的距离s，则狭窄通道的距离应满足sw，w为机器人宽度；

当移动机器人处于自由空间时，为保证机器人可以自由转向或安全通过自由空间，应满足rR，r为机器人的斜长半径。