[发明专利]移动机器人全局路径规划与局部路径规划融合方法及设备

权利要求书

1.一种移动机器人全局路径规划与局部路径规划融合方法，其特征在于，包括：

利用改进后的全局路径规划A*算法在栅格地图上规划全局路径；所述改进后的全局路径规划A*算法为在全局路径规划A*算法的成本评价函数中集成有节点角度信息和节点距离信息的算法；

采用平滑策略对所述全局路径进行平滑处理，得到平滑后的全局路径；所述平滑策略为梯度下降法；

利用融合算法从所述平滑后的全局路径上选取局部路径规划的目标，使得规划路径接近所述平滑后的全局路径，完成全局路径规划与局部路径规划融合；所述融合算法为所述改进后的全局路径规划A*算法与局部路径规划动态窗口法融合得到的算法；

所述改进后的全局路径规划A*算法的评价函数表达式为：

其中，

公式中，x和y分别代表横坐标和纵坐标；f(n)为当前位置n的成本评价函数；g(n)为机器人从初始位置到当前位置n的实际成本；h(n)为机器人从当前位置n到目标位置g的估计成本；d_nt是当前节点n与目标之间的长度；d_st是起点与目标之间的长度；h(p)是当前节点n的父节点的启发式函数；β为节点方向角，表示起始点到当前节点n的向量与节点n指向目标点的向量之间的夹角；

所述利用改进后的全局路径规划A*算法在栅格地图上规划全局路径，具体为：

步骤1.1，设定open列表和close列表，分别存储没有访问过的节点信息和已经访问过的节点信息；

步骤1.2，开始搜索时，close列表为空，访问过开始节点后，将所述开始节点从open列表中删除，将所述开始节点加入close列表中，利用所述改进后的全局路径规划A*算法反复搜索，寻找目标点；

步骤1.3，在到达目标点之前，继续搜索所述开始节点的相邻节点，判断搜索到的新节点是否在open列表中，如果不在open列表中，则将该新节点加入open列表中，采用成本评价函数计算该新节点的代价值f，将该新节点设置为当前搜索点的父节点并加入close列表中；如果在open列表中，则执行步骤1.4；

步骤1.4，采用成本评价函数计算该新节点的代价值f，并与前一次计算出来的相同节点的代价值f进行对比，选取代价值f最小的节点作为下一次搜索点的父节点，并将所述代价值f最小的节点加入close列表中，然后继续更新open列表中的节点信息；

步骤1.5，循环步骤1.1至步骤1.4，直到达到目标位置，当遍历到目标点，并且将目标点添加到close列表中时，循环结束；如果此时open列表为空，则表示没有路径；

所述采用平滑策略对所述全局路径进行平滑处理，得到平滑的全局路径，具体包括：

步骤2.1，假设有原始全局路径v和平滑的全局路径s，最小化式d₁和d₂；

式中，i为路径中的点；d₁为平滑后的全局路径与原平滑的全局路径的偏移量；d₂是平滑后的全局路径一个节点到下一个节点的偏移量；平滑后的全局路径的起点与终点与原始全局路径相同，即起点v₁＝s₁和指定目标v_n＝s_n；

步骤2.2，设定误差阈值e，利用梯度下降法更新和从而达到优化的目的，在梯度下降法优化过程中，

式中，α和β为在梯度方向上的搜索步长；

步骤2.3，考虑与前一状态偏差，则有：

步骤2.4，更新和直到梯度下降的距离小于误差阈值，则停止更新，得到更加平滑的全局路径；

所述融合算法的评价函数的表达式为：

G(v,w)＝GoalCost(v,w)-ObstacleCost(v,w)

其中，

GoalCost(v,w)＝k₁·[D_target(t-1)-D_target(t)]

ObstacleCost(v,w)＝max(k₂·[D_safe-D_obs(t)],0)

式中，k₁为前向增益；D_target(t-1)为t-1时刻机器人与目标之间的距离；D_target(t)是t时刻机器人与局部目标点之间的距离；D_target(t-1)-D_target(t)用于测量机器人与目标之间的距离增量；k₂为障碍增益；D_safe为制动距离，即机器人在最大加速度下将最大线速度减速为零所需的欧氏距离；D_obs(t)是t时刻机器人到最近障碍物的距离；

所述利用融合算法从所述平滑后的全局路径上选取局部路径规划的目标，使得所述规划路径接近所述平滑后的全局路径，完成全局路径规划与局部路径规划融合，具体为：

步骤3.1，对平滑后的全局路径进行初始化；

步骤3.2，机器人在速度空间(v,w)中进行采样，生成模拟轨迹；

步骤3.3，在考虑全局最优路径的基础上，在步骤3.2生成的模拟轨迹中，根据所述融合算法的评价函数选择最优路径；

步骤3.4，机器人沿着步骤3.3中得到的最优路径移动；

步骤3.5，判断是否移动到终点，如果移动到终点则结束，如果不是终点，则返回步骤3.2。

2.一种移动机器人全局路径规划与局部路径规划融合设备，其特征在于，基于权利要求1所述的一种移动机器人全局路径规划与局部路径规划融合方法，包括：

路径规划模块，用于利用改进后的全局路径规划A*算法在栅格地图上规划全局路径；所述改进后的全局路径规划A*算法为在全局路径规划A*算法的成本评价函数中集成有节点角度信息和节点距离信息的算法；

平滑处理模块，用于采用平滑策略对所述全局路径进行平滑处理，得到平滑后的全局路径；

路径融合模块，用于利用融合算法从所述平滑后的全局路径上选取局部路径规划的目标，使得规划路径接近所述平滑后的全局路径，完成全局路径规划与局部路径规划融合；所述融合算法为所述改进后的全局路径规划A*算法与局部路径规划动态窗口法融合得到的算法。