基于改进A*与DWA算法的物流机器人路径规划

杨桂华， 卫嘉乐

(桂林理工大学机械与控制工程学院， 桂林 541004)

近年来，电子商业的飞速发展引发中国物流市场的进一步扩大，传统的物流系统已经逐渐拉低了企业的发展效率[1]。人们对智能物流系统的要求日渐迫切，当前，许多的电商采用仓库自动导引车(automated guided vehicle,AGV)取货，但是由于当今的AGV取货方式通常采用事先铺设的磁轨道进行定点取货，取货路线较为单一，并且多AGV同时进行作业的时候，AGV之间可能会有不同形式的冲突，自动导引车系统(automatic guided vehicle system，AGVS)间的避障需要进一步的研究[2]。

目前，已有多种智能算法被应用到机器人的路径规划中，常见的机器人路径规划算法有Dijkstra算法[3]、人工势场法以及蚁群算法等，也有许多学者对已有的算法进行了相应的改进，辜勇等[4]通过采用确定性选择和随机选择结合的方式改进算法转移概率，让蚁群信息素更新规则根据迭代次数进行动态适应性改变，从而得到优化路径。禹鑫燚等[5]采取线性时序逻辑理论在任务可行的网络拓扑上搜索最优路径。罗如学等[6]从改进视觉方位和拥挤度因子函数，改进人工鱼群算法，从而提高鱼群算法在机器人路径规划中的效果。除了提高单一算法的寻优效果外，部分学者选择融合多种算法进行机器人路径规划的研究，林洁等[7]采用分段的模拟退火算法优化人工势场算法路径规划中存在的易陷入局部极小值的问题。王海群等[8]则通过结合模糊智能控制与人工势场算法的方式，进行改进人工势场中的引力函数，将目标点的速度、加速度信息加入其中对目标点进行跟踪，从而实现自动避障和路径规划。王红君等[9]针对蚁群算法收敛速度慢、易陷入局部最优等问题提出了一种改进烟花-蚁群混合算法，利用改进烟花算法搜索得到的最优路径转化为蚁群算法中的信息素加强值，避免蚁群盲目的搜索，最后采用B样条插值方法进行曲线平滑得到机器人路径。

但上述文献都基本上是从减少机器人的路径出发，对机器人实际运行的流畅度方面有所忽略。鉴于此，本文研究以目前流行的ROS为核心，通过融合改进A*算法和DWA算法，减少机器人避障时候的转折次数，提高运行效率。后续利用构图和路径规划的方法到达指定的物件位置。机器人行驶过程中能够绕开动态的障碍物，并将动态局部地图与最原始的全局地图修正，最后生成一个详细的地图模型。

1 物流机器人路径规划系统

机器人路径规划系统包括环境感知、路径规划、自主避让三大模块，路径规划系统框图如图1所示。首先机器人通过所搭载的激光雷达对物流仓库进行全方位的扫描，利用ROS系统的SLAM算法扫描后生成几何特征地图信息存储在系统中。然后移动机器人具体工作时根据事先确定的任务，利用改进A*算法自主进行全局路径规划，在执行此路径的跟踪时，还要不断感知周围的局部环境信息，避开附近的移动障碍物，即再利用DWA算法进行局部规划或局部路径修正，完成障碍物路面的局部路径规划。并将规划信息送给机器人平台的底盘运动控制器，输出速度和角度信号实现机器人作业任务的自主定位导航。

图1 路径规划系统的框图Fig.1 Block diagram of path planning system

图2 系统流程图Fig.2 Flow chart of system

机器人进行存货取货的具体流程如图2所示，首先利用激光雷达传感器对仓库的现场环境扫描，获取仓库内环境信息，通过ROS系统中的SLAM算法进行几何特征地图创建，并作为先验地图存储在机器人平台系统中。然后机器人进入作业任务调度系统中，等待获取目标位置。当物流机器人在获取到目标位置信息后，机器人从机器人待命区出发，通过改进A*算法获得路径规划，移动到货架，同时在行驶过程中再结合改进DWA算法进行局部路径优化，实现动态避障，到达目标位置完成取货。完成一次任务后，等待下一步的指令，如此周而复始。

2 物流仓库环境模型

2.1 机器人取货场地介绍

现在的物流仓库一般采用的是室内静态结构化环境设计，物流机器人在货架之间的行驶方向只有两个方向：水平和竖直。图3是物流仓库的平面结构示意图，主要包括A-X 24个不同类别的货架区，2个货物补给区，1个货物工作站、1个机器人待命区和1个物流处理区。物流处理区将到来的物件编码登记入库，登记完信息后将物件存放在货物补给区，由货物工作站将货物按照标码放置在相应的货架上。机器人从待命区出发完成取货工作，完成工作后且没有下一个任务后回到待命区。

2.2 栅格图法环境建模

在仓储环境下，物流货架较多，用拓扑图法建立拓扑网络会很复杂。可视图法虽然能直观地体现环境情况，但是同拓扑网络一样，当地图中的障碍物较多的时候，可视图的构建非常的复杂，对仓储机器人规划最优路径并不有利，且当货架位置进行变动的时候，需要重新构建可视图，灵活性不高。相比之下栅格图法在直观的表示障碍物和自由空间的前提下，对于后续的更改只需要重新对栅格进行赋值就行了。所以综合考虑下采用栅格图方法。

图4是利用栅格法建立的仓储区域图。其中黑色栅格代表的是仓储区域中边界和货架，是机器人路径规划时的静态障碍物，白色栅格是货架之间可以移动的区域。

图3 物流仓库平面图Fig.3 Logistics warehouse plan

图4 物流仓库栅格图模型Fig.4 Logistics warehouse grid diagram model

3 路径规划算法

路径规划是机器人自主完成从起始点到终点的主要过程，其中包括了全局路径规划和局部路径规划，全局路径规划是指机器人在起始点和终点之间确定一条最优路线，局部路径规划完成机器人在行驶过程中的动态避障功能，两者的结合工作完成机器人的精确定位行驶。

3.1 A*全局路径规划算法

传统的A*算法作为全局路径规划算法，结合了Dijkstra算法和广度优先算法(breadth first search, BFS)提出来的一种新型算法，兼具搜索范围广和搜索效率快双重优点，是现在较为普遍的一种算法。

传统的A*算法的路径规划的具体步骤如下。

步骤1需要定义两个列表，“open”列表和“close”列表，“open”列表内存放的是需要检查的方格，“close”列表则存放不需要检查的方格。

步骤2将起点A作为第一个方格放入“open”列表中，A能到达的方格有8格，将这8个方格放入“open”列表中，并且设置它们的父方格为起点A。然后将起点A从“open”列表中删除，并且将其放入“close”列表。

步骤3定义距离损耗评价函数[10]为

f(n)=g(n)+h(n)

(1)

式(1)中：g(n)为上一方格到此方格需要的距离消耗，这里假设横纵移动消耗10 m，斜向移动消耗14 m；h(n)表示此方格到终点的距离损耗，该函数的计算通常有3种距离算法[11]，分别为切比雪夫距离算法h1(n)、欧几里得距离算法h2(n)以及曼哈顿距离算法h3(n)。表达式为

(2)

式(2)中：(a1,b1)和(a2,b2)分别为起点位置坐标和目标位置的坐标。

步骤4计算A到达周围8个方格距离损耗值，距离最小的作为下一个起点。通过这个损耗值从而可以评价出下一个能作为起点的方格。

步骤5如果某一个临近的方格已经在“open”列表中了，需要重新计算路径消耗，如果消耗更低，则需要重新定义父方格，消耗更高的话则不需要改变。

步骤6按照上述步骤反复下去，直到“open”列表中出现了终点，将经过的方格连接出来，则连接的路径就是A*算法规划的路径，如果“open”列表中没有方格了，则说明没有路径可以到达。

3.2 改进A*路径规划算法

传统的A*算法每次对下个节点的寻找都需要遍历周围8个节点，并且随着路径规划的距离变长，每次的遍历都需要给机器人发送一次运动指令，伴随着路径的转折次数变多，机器人的移动的流畅度会降低相应的速度也会降低。

针对上述的问题，通过对“close”列表中的行坐标的清洗，在所有的行坐标中找到一条最短的路径。从而减少原路径的转折数。在规划好整个全局路径后，再利用滑动平均平滑算法对规划好的路径进行平滑处理。进一步来提高机器人移动的流畅性，平滑函数为

(3)

式(3)中：i为需要进行平滑操作的数列；N为平滑的点数且N为奇数。根据实验效果测试，这里的N取3。

测试4个场景下的改进前后算法的对比图。如图5所示。4个场景下相关的数据对比如表1所示。

表1 算法改进前后的数据对比Table 1 Data comparison before and after algorithm improvement

从图5以及表1的数据可以看出在4个场景下改进后的A*算法路径更短，并且在转折次数上明显比之前的算法少，能一定程度上提高机器人运行过程中的流畅度，进一步提高机器人的工作效率。

3.3 DWA局部路径规划算法

局部路径规划采用的是动态窗口算法，该法通过模拟机器人在行驶过程中的速度，从而模拟机器人在一定时间内的路径。然后选取最优路径以及对应的速度来驱动机器人。

机器人的行驶路径可以看作是由一段的圆弧来形成的，这里设定机器人的运动速度为(vt,wt)，根据速度通过式(4)可以得到机器人的圆弧运动的半径为

(4)

蓝色为原算法的路径；青色为改进后算法的路径； 红色为进行平滑操作后的路径图5 改进前后的路径对比图Fig.5 Path comparison diagram before and after improvement

式(4)中：vt和wt分别为t时刻机器人的线速度和角速度，一对(vt,wt)就可以表示一个圆弧轨迹。

当机器人的旋转速度wt不为0的时候，可以得到下个采样时间的运动坐标为

(5)

式(5)中：θt为t时刻机器人朝向与水平正实轴方向之间的夹角；θt+1=θt+wtΔt。

式(5)中的一些参数可以根据图6对应可知。

图6 机器人运动坐标图Fig.6 Coordinate diagram of robot motion

DWA的算法实现具体步骤如下。

步骤1在机器人的速度阈值内对速度进行采样。

步骤2对采样的速度进行轨迹模拟，得到采样速度的模拟轨迹。

步骤3在完后速度采样后需要一个评价函数来对其模拟出的轨迹进行评价，采用的评价函数[12]为

G(v,w)=αheading(v,w)+βdist(v,w)+

γvelocity(v,w)

(6)

式(6)中：α、β、γ分别为3个子项的权重；heading(v,w)函数是用来评价机器人在当前设定的采样速度下，达到模拟轨迹末端时的朝向与目标位置之间的角度差异；dist(v,w)为机器人在当前轨迹下与最近的障碍物之间的距离，当没有障碍物的时候，将其设定为一个常数；velocity(v,w)用来评价当前轨迹的速度大小。

步骤4重复上述步骤，选出局部最优路径。

3.4 DWA局部路径规划算法优化

分析整个物流仓库环境，障碍物大致可以包括仓库设施建设、设备以及货架这些都可以在仓库环境建模中得知，为静态的已知障碍物。不确定障碍物主要为后加入物流仓库环境的工作人员，为动态的未知障碍物。DWA算法主要是为了避开那些动态的障碍物，所以将DWA算法的评价函数更改为

G(v,w)=αheading(v,w)+βdist_s(v,w)+εdist_d(v,w)+γvelocity(v,w)

(7)

式(7)中：dist_s(v,w)为机器人到静态已知障碍物的距离；dist_d(v,w)为机器人到动态未知障碍物的距离，通过实验对系数进行设定。

3.5 改进DWA算法实验

根据设想制定改进后的DWA评价函数的几组系数，然后与原DWA算法进行仿真实验对比，根据实验结果进行调整。表2是改进前后DWA算法评价函数的系数设定。序号1为DWA算法评价函数的系数，序号2～4为改进DWA算法后设定的3组参数。

为了充分验证改进算法的可行性，选择在起点和终点之间的路径中加上几个随机障碍物(黄色栅格)，图7是上述设定系数下对应的轨迹图。

其中图7(a)是原DWA算法的轨迹图，图7(b)～图7(d)是改进后3组参数设定下相对应的轨迹图，表3是相对应算法所运行的时间。

结合表2序号1和序号2的参数设置以及图7轨迹图分析可知，在静态距离项系数设定相同的情况下，增加动态距离项的设定在路径上有了更好的选择，表3的数据也说明了，算法改进后时间更少，进一步验证了改进DWA算法的可行性。序号3参数设定在序号2的基础上进行了动态距离项系数的增加，但是对应的算法时间增加了不少。序号4通过减少静态距离项系数，与序号2进行对比实验，从图7(b)和图7(d)轨迹图来看，后者轨迹更加平滑，且根据表3数据，序号4的算法运行时间更少，效果更优。由此分析可知通过减少机器人到已知障碍物的距离加权系数β，提高到未知障碍物的距离加权系数ε来增加DWA算法在物流环境下的避障性能，从而加快路径规划速度。

表2 DWA算法参数配置优化Table 2 DWA algorithm parameter configuration optimization

表3 算法运行的时间Table 3 The running time of the algorithm

图7 路径对比图Fig.7 Path contrast diagram

4 实验

4.1 实验调试

本次应用的建图算法为激光SLAM，主流的激光SLAM算法有Gmapping、Karto、Hector、Cartographer等。本文研究采用基于Rao-Blackwellized粒子滤波(Rao-Blackwellized particle filter,RBPF)的Gmapping算法，相比Hector算法对雷达频率高[13]，Gmapping对激光雷达的频率要求低且鲁棒性高[14]。而相比Cartographer在构建小场景地图时，Gmapping不需要太多的粒子并且没有回环检测因此计算量小。Gmapping算法结合了里程计信息和最新的观测信息，两者的提议分布更能符合目标分布，也能更快地达到似然函数的峰值。

机器人建图的具体步骤如下。算法总共包括4个步骤：采样-计算权重-重采样-地图估计。

步骤1先将上位机(本文采用笔记本)和机器人安置在同一局域网中，打开一个terminal窗口，在窗口输入ssh clbrobot@robot连接机器人，利用WiFi实现上位机与机器人之间的通信。

步骤2输入roslaunch clbrobot bringup.launch打开小车主节点。

步骤3新建一个窗口输入ssh clbrobot@robot启动另一个工程输入roslaunch clbrobot lidar_slam.la-unch开启小车的雷达。

步骤4新建一个窗口输入rosrun rviz rviz打开Rviz仿真平台，新开一个窗口输入rosrun teleop_t-wist_keyboard teleop_twist_keyboard.py打开键盘控制，移动机器人，更新地图信息，继续移动机器人直至地图信息完整，房间边界显示明确，完成建图。

图8 Gmapping算法构图Fig.8 The map constructed by the Gmapping algorithm

图9 机器人完成避障任务到达终点位置Fig.9 The robot completes the obstacle avoidance task and arrives at the destination position

步骤5保存地图。

现实环境以及利用Gmapping算法构建的房间示意图如图8所示。实验测试使用创乐博激光雷达机器人在(5 m×8 m)房间模拟物流机器人，实验平台操作系统采用为Ubuntu版本5.3.1，Ros操作系统版本1.12.6，机器人搭载的传感器为RPLIDAR A1激光雷达传感器，将算法应用到机器人中进行调试。

在机器人行驶的路线中途设置障碍物，机器人在放置障碍物后的路线规划如图9(a)所示，机器人到达终点位置图如图9(b)所示。

4.2 实验结果

通过上述进行的创乐博机器人自主导航实验，可以看出在机器人行驶途中设有障碍物后，机器人能及时地识别并且更改原始的路径规划，避开设置的障碍物，最终到达预定的任务位置。从机器人的路径规划以及实际的运行情况来看，搭载改进算法的机器人能减少避障时候的冗余转折，使得避障路线更加的平滑，从而一定程度上减少避障时间。

5 结论

对传统的A*算法和DWA算法进行了一定的改进，并且在MATLAB中做了相关的实验进行改进前后的效果对比，实验表明改进算法在机器人行驶路径以及转折次数上有优化作用。后续通过对创乐博机器人的调试运行融合算法，模拟物流机器人在任务过程中的路径规划和避障功能。实现了机器人的自主导航和实时避障以及最后的姿态调整工作。结果表明了融合算法在机器人路径规划方面的可行性。