[发明专利]一种非均匀采样FMT*的移动机器人路径规划方法

权利要求书

1.一种非均匀采样FMT*的移动机器人路径规划方法，其特征在于：包括如下步骤：

步骤S1、地图初始化，其方法为：将环境中所有的已知障碍物用均匀分布的点集合进行离散化表示，并假设障碍物数量为k，组成集合{C_i}，即

步骤S2、利用增量构造方法建立广义Voronoi图，即GVG，由广义Voronoi边和顶点组成；

步骤S3、寻找初始化路径，其方法为：设置机器人的初始位置、目标位置，在路线图GVG中寻找初始路径，并离散化路径，实现最优路径与初始路径同伦；

步骤S4、安全隧道建立，其方法为通过GVG建立过程得到距离障碍物的距离d，将其作为半径R，并以圆心在路径离散点，半径R的连续圆形区域，建立覆盖初始路径的安全隧道；

步骤S5、非均匀采样，其方法为：设置总采样数N，在安全隧道内进行随机采样，得到采样集合V_unvisited；

步骤S6、构造采样集合V_unvisited、探索集合V_open、已探索集合V_closed，＝将机器人路径的目标点x_goal放入V_unvisited，将初始点x_start放入探索集合V_open，代价初始化为0，记作c(x_start)＝0，已探索集合

步骤S7、寻找探索集合V_open中代价最小点作为节点Z，以Z为中心，r_n为半径范围内搜索采样集合V_unvisited的节点，组成集合X_near，以函数Near(V_unvisited,Z,r_n)表示；

步骤S8、对于每个点x∈X_near，以x为中心，半径r_n探索V_open中点，组成集合Y_near，以函数Near(V_open,x,r_n)表示；

步骤S9、计算所有点y∈Y_near到点x的代价，选取代价最小点y_min，表示为使树中各个节点到子节点代价最小，从而实现算法的渐进最优；

步骤S10、判断y_min与x是否有障碍物，若无则将x从V_unvisited放入V_open中，其代价为c(x)＝c(y_min)+Cost(y_min,x)，y_min为x的父节点；

步骤S11、探索完X_near后，将Z从V_open放入V_closed；

步骤S12、判断Z是否是目标点，若是则返回路径解，若不是则返回步骤S7继续；

步骤S13、机器人接收到算法规划的可行路径解，并把可行路径解中的路径转化为所需线速度和角速度，控制机器人移动到目标位置，路径规划结束。

2.根据权利要求1所述的一种非均匀采样FMT*的移动机器人路径规划方法，其特征在于：所述步骤S2具体实现方式为：距离d_i(x)表示点x到第i个障碍物C_i的距离，定义为：定义方向为点x指向c₀，由梯度表示，其中c₀表示x到C_i的距离最近点；

若点x周围有许多障碍物，则定义x到环境的距离为所定义的距离均为可视距离且不考虑被遮挡障碍物；

所述GVG的基本组成部分是每个点到障碍物C_i和C_j的等距集合，将集合定义为等距边：

由公式一获得的所有等距边的交汇点为广义Voronoi顶点；进一步地，GVG由广义Voronoi边和顶点组成；每个顶点和Voronoi边都保留距离d，GVG形成的路线图分布在所有的障碍物周围，并且保证路线图中的任意两点之间存在一条有GVG边缘和交汇点形成的路径将两点连接。