基于单元分解的改进D*lite路径规划算法

张 毅，施明瑞

(重庆邮电大学 国家信息无障碍工程研发中心，重庆 400065)

0 引 言

移动机器人规划路径能力的大小，决定了机器人可胜任工作难度的高低。

在构建好的环境模型中，规划一条从初始点到目标点的无碰撞最优路径，是路径规划的主要内容。在构建环境模型时，栅格法应用较多，该方法具有直观简洁、分辨率可变、容易创建和存储等优点，适用于室内环境路径规划地图模型的建立，鲁棒性强[1]。图搜索算法是路径规划中的一类算法，它被广泛应用于栅格地图中的路径规划问题。A*算法是图搜索算法中的经典算法，它是一种启发式搜索方法。启发式搜索会评估状态空间中的每个搜索位置，找出下一步要搜索的最好位置G，再从G进行类似搜索直到确定目标位置。该搜索方法能节省大量搜索空间，提高搜索效率[2]。在A*算法的基础上，研究者们先后提出了多种改进算法，例如D*，LPA*与D*lite等算法。

D*lite算法是文献[3]提出的。该算法采用反向搜索，从目标点向当前点扩展，在进行重规划时极大地提高了效率。文献[4]通过在启发估价值中加入与障碍物相关的值，从而避免规划出靠近障碍物尖角或穿越两相邻障碍物的不安全或不可达路径。文献[5]通过融合D*lite算法和RRT*算法提高了收敛速率。文献[6]采用模型预测控制MPC，优化了D*lite算法在动态环境和特殊地形中的路径规划能力。文献[7]通过引入备选路径，提高了D*lite算法中路径重规划的速度。文献[8]通过在D*lite算法中引入搜索树的思想，在需要重规划时立即砍断搜索树，从而提高路径重规划的速度。

学者们对D*lite算法规划出的路径的安全性、平滑度和算法重规划策略进行了诸多研究，但是在路径搜索中仍然存在问题。

D*lite算法在进行路径规划时，会给网格设置优先级，每次循环中选取优先级最高的网格进行扩展，启发信息是优先级设置中的重要参考，但是启发信息只是估计代价，无法将障碍物对全局移动代价的影响完全包含。当障碍物将较大的搜索空间分隔成多个较小的自由区域时，优先级高的网格指引的搜索方向可能最终会被障碍物阻隔而无法继续扩展，导致正确的搜索方向被隐藏起来，往往需要经过多次尝试才能找到，在错误的搜索方向上扩展的网格是无效网格，增加了计算次数而对规划路径没有帮助，影响了路径规划的效率。因此本文提出一种基于单元分解的改进D*lite路径规划算法，将室内环境分解为若干相互连通的单元，这些单元的相互连通区域，即连接两个相邻单元的可通行的自由区域，是移动机器人导航时必须经过的网格，可以在这些网格中选取合适网格作为核心网格，用来引导路径规划算法的搜索方向，这样能够快速找到绕过障碍物的正确路径，从而减少无效网格的扩展，提高规划速度。

1 D*lite算法改进

1.1 改进流程

本文在原始D*lite算法的基础上进行优化，在原有的搜索模型中加入核心网格进行改进，核心网格会引导搜索算法在正确的方向上进行搜索和扩展。

改进流程如下。

1)在原有Boustrophedon[9]单元分解法的基础上设计基于新的分解规则，分割环境的栅格地图。

2)设计双向图搜索算法，找出单元顺序。

3)设计核心网格设置方法，得到核心网格组成搜索链表。

4)以搜索链表引导搜索算法完成路径规划。

改进流程如图1。

图1 改进流程Fig.1 Improved processes

1.2 单元分解

利用Boustrophedon单元分解法以及新增的分解规则，可以将移动机器人所在的室内空间分解成若干单元，这些单元是内部没有障碍物的有界区域。

Boustrophedon单元分解法是用一条垂直线从左至右扫描栅格地图。当扫描线扫入障碍空间时，扫描线的连通性增加，当扫描线扫出障碍空间时，扫描线的连通性降低。当扫描线的连通性增加时，一个旧单元结束，生成两个新单元。当扫描线的连通性降低时，两个旧单元结束，生成一个新单元。

当障碍空间的外形产生变化时，此时扫描线的连通性不变，但单元内仍然存在阻挡网格扩展的障碍，算法难以快速找到绕过障碍的正确路径。针对此问题设计了新的分解规则，当扫描线的连通性不变但某段连通线长度发生了变化，若变化的大小超过当前单元最大宽度的一半，则旧单元结束，新单元生成如图2。

图2 连通线长度变化较大时产生新单元Fig.2 Generating new unit when the length of the slice changes greatly

其余情况只增加单元大小，而不产生或结束单元。扫描结束后，环境地图被分割成若干独立的有界单元，每个单元内部都没有障碍物，如图3。

图3 地图分割成若干单元Fig.3 Map is segmented into several units

可以把这些单元看作节点，单元之间如果有直接的连通区域则可以看作相应节点间有边连接，这些边和节点可以组成图。需要将两单元间的移动代价作为代价值赋给相应的边，在二维栅格地图中曼哈顿距离的计算简单，且能够较准确地比较两段距离长短，所以可以将单元补全为长方形，计算边的两端单元中心之间的曼哈顿距离，此距离作为边的代价值。若两单元之间没有直接的自由连通区域，则两单元节点之间没有边连接，两单元之间的代价值设置为无穷大。计算完成后可得到单元节点图如图4。

图4 单元节点图Fig.4 Node graph of units

把节点图中的代价值用矩阵表示可以得到邻接矩阵edge，其中，edge[i][j]代表节点i与节点j之间的直接代价值。

1.3 节点图双向搜索

设定起始网格所在节点为起始节点，目标网格所在节点为目标节点，需要找到起始节点到目标节点代价总值最短的路径，此路径表示了从起始网格到目标网格的最短路径需要依次经过哪些单元。

利用图搜索方法可以在节点图中找到两个节点之间的最短路径。给每个节点设置两个值，cost值表示与搜索起始节点的代价值，pre表示父节点，每次从中取出cost值最小的节点，然后更新其余节点的cost和pre，直至取出搜索目标节点。此搜索方法每次只选取代价值最小的节点，所以在找到起始节点到目标节点的最短路径前，会先计算一些无法到达目标节点但代价值更小的路径。为了提高算法的速度，可以采用双向搜索的方法。同时进行两个搜索，分别以起始节点和目标节点作为搜索起始节点。

节点图双向搜索方法的算法步骤如下。

1)初始化。用集合c保存已经确认最短路径的节点，把搜索起始节点s放入c中。用集合w保存还没有确认最短路径的节点，把其余节点都放在w中。将w中所有节点j的cost值设为edge[s][j]，pre设为s。

2)从集合w中选出cost最小的节点i，由于cost非负，所以此cost为s到i的最小代价值，将i移出w，移入c。

3)对于w中的每个节点j，若有costj>costi+edge[i][j]，则更新costj为costi+edge[i][j]，并把prej设为i。

4)在两个搜索中分别重复步骤2、步骤3，直到两个搜索中的集合c出现交集,或者任意一个搜索中集合w中节点的cost值都为无穷大，此时循环结束。

5)若两个搜索中的集合c有交集，则根据c中节点的pre值可以得到起始节点到目标节点代价总值最短的路径，即可得到起始网格到目标网格最短路径经过的单元顺序，否则就说明起始节点无法到达目标节点。

1.4 基于核心网格的路径规划

D*lite算法采用反向搜索方法，即在路径规划时，从目标网格开始搜索，向起始网格扩展。在D*lite算法中每个网格s需要维护3个值：g(s)为目标网格到s网格的实际代价；h(s)是启发值，为s网格到起始网格的估计代价，取s网格与起始网格横坐标之差和纵坐标之差中的最大值，计算如(1)式；rhs(s)保存s网格的父网格s′的g值加上父网格到s网格的代价值c(s′,s)，rhs(s)的计算如(2)式。由于机器人在栅格地图中，从所在网格可以向相邻的8个网格中任意一个直接移动，所以D*lite算法中父网格到8个子网格的代价值都为1，(1)式的计算结果可以作为网格s的启发值h(s)。

h(s)=max(abs(xstart-xs),abs(ystart-ys))

(1)

(2)

(2)式中，pred(s)表示s网格的父网格，是从目标网格向起始网格搜索时搜索方向上的父网格。

当网格的g值等于rhs值时，称为局部一致，反之则为局部不一致，只有局部一致的网格才是扩展完毕可以通行的。D*lite算法的优势在于动态环境中可以通过g值和rhs值的关系判断网格的状态，从而找出受环境变化影响的网格。本文暂时只讨论初始环境下的路径规划。

算法使用一个优先队列保存待更新的局部不一致网格，以k值作为优先级，k值越小优先级越高，k值由k1和k2组成，k(s)=(k1(s),k2(s))，k1和k2的计算方法为

(3)

比较k值的大小时，优先比较k1，如果k1值相等再比较k2值。

D*lite算法的核心步骤如下。

1)初始化所有网格，将所有网格的g值和rhs值设置为无穷大，然后将目标网格的rhs值设置为0，根据(3)式计算其k值后插入优先队列。

2)从优先队列中移出优先级最高的网格，设置其g值等于rhs值，使该网格成为局部一致。然后根据(2)式计算该网格的邻近网格的rhs值，若这些网格的rhs值与g值不等，则计算它们的k值并将它们插入优先队列。

3)重复第2步直到起始网格成为局部一致。从起始网格开始按照g值最小的规则搜索四周邻近网格，直到到达目标网格，即可得到最终路径。

D*lite算法以k值作为优先级来决定从优先队列中取出哪个网格，以求得8个邻域方向的最优解。然而很多优先级较高的网格引导的搜索方向并不能进入正确路径必须经过的单元中，通过这些网格无法找到正确路径，势必造成算法在计算过程中耗费大量时间做无效工作。若在起始网格到目标网格的路径必须经过的单元中设置核心网格，首先从目标网格向核心网格进行扩展，则在一开始就能找到绕过障碍物的正确路径。

为了找出核心网格，需要先设置核心区域，再在核心区域中设置核心网格。核心网格的设置算法如下。

1)为每一个网格添加一个编号n，n等于网格所在单元的编号。因为D*lite算法采用反向搜索，需要把节点图双向搜索算法得到的单元顺序颠倒方向，使之成为从目标单元到起始单元的单元顺序。

2)根据单元顺序，从第2个单元开始，找出与前一个单元相邻的网格作为核心区域，直到起始单元。设当前单元为u，前一单元为u′，核心区域设置如(4)式，其中，Ss.n=u.n表示单元u中的网格，succ(s′)表示与网格s′相邻的网格，(4)式表示在当前单元u中，所有与前一单元u′相邻的网格共同组成了单元u的核心区域。以xmax，xmin，ymax，ymin表示核心区域内的网格中的最大和最小横纵坐标，则核心区域中心点可由(5)式得到。

(4)

(5)

3)设置目标网格为第1个核心网格，起始网格为最后一个核心网格。给每一个核心区域内的网格设置一个kc值，由kc1和kc2组成，kc(s)=(kc1(s),kc2(s))，kc1和kc2的计算如 (6) 式。其中s为当前核心区域内的网格，cgrid为上一个核心网格，coremid为下一个核心区域的中心点。每个核心区域中选取kc值最小的网格作为核心网格，比较kc值大小时，首先比较kc1值，如果kc1相同再比较kc2值。设置核心网格如图5。

(6)

图5 设置核心网格Fig.5 Main grids are set

图5中星标志为目标网格，圆标志为起始网格，由节点图双向搜索算法可计算出目标单元到起始单元的最短路径为5→2→1→4→9，灰色网格为核心区域，圆环标志为核心网格。

将目标网格、起始网格和设置完成的核心网格按照已经计算完成的单元顺序依次插入搜索链表中，用搜索链表引导D*lite算法完成多段路径规划。首先设置一个地址P指向搜索链表的首个网格。执行每段路径规划时，以地址P当前指向的网格作为单次搜索中的目标网格，以P的下一个地址指向的网格作为单次搜索中的起始网格，为了增加搜索效率，限定只有编号n与当前搜索目标网格或者当前搜索起始网格的编号相同的网格才能被遍历，当本次搜索中的起始网格成为局部一致后，将P向后移一位，继续执行下一段路径规划。当搜索链表的最后一个网格即真正的起始网格成为局部一致时，从起始网格开始循着邻接网格中g值最小的网格移动，直到到达目标网格，就可完成从起始网格到目标网格的路径规划。

2 实验结果与分析

为了验证算法的有效性，以MATLAB R2016b作为软件平台进行仿真，以Intel Core i3-6100 CPU作为硬件平台，主频为3.70 GHZ，4 GByte内存。

在相同的硬件平台和软件平台下，对本文算法和DLD*lite算法[10]进行相同栅格地图下的对比实验,DLD*lite算法在D*lite算法的基础上引入了视线算法和距离变换作为改进。实验采用15×15的栅格地图进行仿真，得到如图6的路径规划对比实验图。

图6 对比实验Fig.6 Comparative experiment

图6中星形标志为目标网格，圆形标志为起始网格，黑色网格为障碍物。深灰色网格和浅灰色网格为更新函数遍历过的网格，计算了rhs值和k值并插入优先队列中。深灰色网格为搜索函数从优先队列中取出，计算完g值的局部一致网格。

图6a为DLD*lite算法仿真图，该算法在启发函数中加入障碍物距离值，用视线算法更新父节点，使得规划出的路径平滑且安全，但是由于该算法每次只选取k值最小的网格进行扩展，从而扩展了很多无效网格，对这些网格的g值和rhs值的计算耗费了大量时间却收效甚微，这样大大降低了搜索效率。图6b为本文算法的仿真图，方形标志为算法计算出的核心网格，通过单元分解、双向图搜索和设置核心网格得到了搜索链表，用搜索链表引导分段搜索，最终找到起始网格到目标网格的路径。由于以核心网格组成的搜索链表作为导向，使得搜索算法朝着正确的方向进行扩展，大大降低了遍历网格的数量和计算次数，从而提高了路径规划的速度。

表1为路径规划对比实验中已遍历网格数与最终路径长度的统计。本文算法中更新函数已遍历的网格数比DLD*lite算法中减少了43%，搜索函数已计算的网格数减少了70%。可以看出本文算法减少了网格的遍历次数和计算次数。

表1 路径规划对比实验相关数据

为了进一步验证本文算法提高路径规划效率的有效性，统计本文算法、DLD*lite算法和改进多步长蚁群算法[11]路径规划所用时间并进行对比，为了消除偶然现象，提高实验结果的精确性，统计了多次对比实验。改进多步长蚁群算法路径规划实验如图7。3种算法规划出的路径长度通过计算比较并没有明显差别。

改进多步长蚁群算法将每次迭代产生的最优路径作为引导路径，提高了算法的收敛速度，该算法规划路径时需要进行多次迭代，当其收敛后即完成了路径规划，所以统计收敛用时作为改进多步长蚁群算法的所用时间。表2为10次对比实验中3种算法的规划时间统计。由于改进多步长蚁群算法具有一定的随机性，每次收敛时的迭代次数有起伏造成了所用时间有波动。DLD*lite算法平均用时701.2 ms，改进多步长蚁群算法平均用时654 ms，本文算法平均用时504.4 ms。可以看出，虽然本文算法需要进行节点图双向搜索计算以及核心网格计算，但是总规划平均时间仍然比DLD*lite算法减少了大约28%，比改进多步长蚁群算法减少了大约23%，验证了本文算法在提高路径规划效率上的有效性。

图7 改进多步长蚁群算法Fig.7 Improved multi-step ant colony algorithm

表2 多次对比实验用时统计

3 结束语

本文提出了一种基于地图分割的移动机器人路径规划方法，在原始D*lite算法的基础上改进了搜索策略，通过单元分解和双向图搜索方法找出从起始网格到目标网格的必经单元，按照核心网格设置策略在这些必经单元中选出核心网格，将若干核心网格依次插入目标网格和起始网格之间组成搜索链表，用D*lite算法的反向搜索针对搜索链表完成分段搜索，即可得出起始网格到目标网格的路径。将原始D*lite算法中直接的、低效的搜索策略改进为分段的、高效的搜索策略，能够在室内环境中快速地找到绕过障碍物的正确路径。通过实验比较证实了本文算法对于提高路径规划效率的有效性。然而移动机器人所在环境一般是动态的，已规划好的路径可能因为移动过程中出现新的障碍物而被阻断，此时需要路径重规划。D*lite算法进行路径重规划时可以保留初次规划中未受环境变化影响的网格的全部信息，但是为了找到导向新起点的受环境变化影响的网格并将它们处理成能够继续扩展的状态，程序需要进行多次循环，将会耗费大量的时间。如何快速找到这部分网格并插入优先队列以此来提高D*lite算法路径重规划的速率将是未来的研究目标。