基于电子地图的D*补偿算法AGV路径规划*

程 满 杨光永 徐天奇 黄卓群 戈一航

（云南民族大学电气信息工程学院 昆明 650500）

1 引言

自动导引车（Automated Guided Vehicle，AGV）随着智能化的发展，在智能仓储和室内搬运中发挥越来越大的作用［1］。路径规划作为AGV的核心之一，合理的路径规划是智能车辆不可或缺的一部分，决定了智能车辆的安全性和稳定。传统几何方法中的几何法，例如可视图法［2］、沃罗诺伊图法、自由空间法等计算量大、路径不是最优；单元划分法，缺点是单元分解与计算单元之间的邻接关系的开销较大；人工势场法，实际上是一种拟物方法，模拟自然界中的静电场、流体等，缺点是容易陷入局部最优而无法完成任务［3～5］。智能算法主要包括模糊逻辑算法［6］、神经网络算法、遗传算法、蚁群算法、模拟退火等，智能算法［7］普遍存在着对计算机要求高，收敛速度慢、控制变量多之类的缺陷［7］。D*算法是在图搜索的思维下发展的智能启发式算法［8］，原理简单，容易实现，D*算法是A*算法的扩展，与A*算法中有向静态弧长（结点与结点之间的代价）的定义不同，D*算法结点之间的弧长代价可以随算法的实施进程而动态变化，即动态A*算法（Dynamic A*），D*算法由此得名，不同在于D*是从目标点到起点的反向搜索。文献［9］对于地图进行分区以及子节点的改进，实现在不同区域不同搜索，并用Floyd算法平滑处理，保证了效率的同也实现了很好的避障效果。文献［10］考虑到了AGV的尺寸大小和位姿对行驶的影响，采用改进的遗传算法对A*算法产生的初始路径进行优化。文献中所提出的方法，在一定程度上实现了很好的效果，但是有一些缺陷。比如障碍物的突然出现，这些算法就不能实现很好的预期效果；以及改进后的算法相较于之前的算法而言，有时损失了例如路径最优、规划时间短等方面来满足某些方面的优势。对于以上的问题，本文提出了D*补偿算法，在不损失最优路径的同时也能很好的实现避障功能，以及考虑到了实际行驶情况，特别注意转弯次数多的问题。

2 补偿D*算法

2.1 距离表达

D*算法原始的距离计算公式是欧氏距离（Euclidean Distance）表达式如下：

由于原始距离要进行平方项和开方处理，速度慢。现在换成曼哈顿距离（Manhattan Distance）表达式如下：

2.2 启发函数

D*是一种启发式搜索算法，D*估价函数如下：

图1 节点的转弯判断

对g(n)累计代价做改进，将转弯次数这个要素也参考进去。提出g'(n)，λ和c(n)，c(n)与AGV行驶过程中转过弯的次数成正比，λ代表转过的弯次数，g'()n表示新的累计代价，本文中正比的这个比例设置为10。

2.3 平滑行驶

基于实际行驶，AGV遇到大转弯时候不可能停在原地，然后转大弯操作，这样操作性不强，对本身运行非常危险。因此在行驶中我们更希望AGV行驶的路径是一个圆滑的路径，平缓的从起点走到终点。假设平滑之前的点为［x1,x2,x3,…,xn-1,xn]，平滑之后的点为[y1,y,y3,…,yn-1,yn]。本文中α表示偏离度，β表示平滑度，为了得到最佳的效果本文的α和β都设置为0.5，迭代次数为9次，可以得到最优的平滑行驶路径。

图2 路径平滑对照图

路径平滑的实质是求解y(i)满足两种距离的最小：

最小化目标为

求解采用梯度下降法（gradient descent），多次迭代，调整y(i)使得目标函数取得最小值，初始化y(i)=x(i)，迭代：

2.4 安全行驶

长度小的障碍物，直接采用圆形进行包围，损失的可行路径比较小。外包圆直径取障碍物的顶点连线的最大值，圆心为最大连接线的中点。

障碍物在极坐标下形成的曲线表示为

图3 障碍物圆形包围膨胀

加入的电子地图障碍物是矩形，建模时膨胀矩形即可，可以节约大量的可行路径，在两障碍物中间不会判断为不可行区域。参照Costmap的栅格空间覆盖枚举的改进型方法的思想，简单的划分为Freespace（自由层）和Lethal（致命层）。膨胀的度设置为0.5个单元栅格大小。

3 总的结构流程图

图4流程图节点nij，邻近点nrs，优先队列open和closed，每一个节点nij分配的状态标志是（tnij）累计代价g（n），提出新的累计代价g'(n)。

图4 矩形障碍物矩形膨胀

4 改进算法的仿真结果分析

对比仿真实验的起点位置都设置为start=（7，3），终点位置为goal=（23，10），横坐标为x轴，纵坐标为y轴。

4.1 算法的比较

图9算法在迭代次数上面相较于图8算法来说并没有优势，但在转弯次数的减少，以及路径的减少有较明显的区别，实现了较优的改进。实际生活中的AGV的行驶，走曲线可以维持速度在一个更稳定的范围之内移动，到达目标点的耗时反而短一些。图9算法还有一个显而易见的优点就是与障碍物有一定的安全距离，满足安全行驶的要素。按照设置的预计代价函数，计算新的路径，主要考虑了转弯次数的问题，图9算法相对于图6-8算法，行驶的路径长度减少，图9算法路径长度相较于图8算法减少了25.5%，转弯次数减少了50%。

图5 结构流程

图6 Dijkstra算法规划的路径

图7 A*算法规划的路径

图8 D*算法规划的路径

图9 D*补偿算法规划的路径

4.2 临时障碍物

在遭遇临时障碍物的情况下，图11中的算法依然可以很好地避开障碍物，同时与障碍物保持一定的安全距离，相较于图10中的算法，转弯的次数减少了，行驶更短的路径。

图10 D*算法遭遇障碍物的路径

图11 补偿后的D*遭遇障碍物的路径

表1 图5～8的参数比较

5 结语

补偿后的D*算法在路径规划问题中考虑了转弯因素的影响，补偿后的算法有效地避免了行驶路径与障碍物的距离过于接近，保证了AGV的安全行驶，更好地满足了实际的行驶需求。在Matlab上完成了补偿后D*算法的仿真，并和Dijkstra、A*、D*算法进行了比较，验证了安全性和行驶路径减小。在后续的工作中将探索算法在硬件上的实现，如何更快速度地完成规划过程。