基于改进D*算法的室内移动机器人路径规划

王帅军，胡立坤，王一飞

(广西大学 电气工程学院，广西 南宁 530004)

0 引 言

路径规划是移动机器人实现自主导航的关键技术之一[1]，国内外学者做了大量的研究，多种算法被应用到路径规划中。传统的路径规划方法例如人工势场法[2]，但是容易出现局部最优的缺点产生抖动和震荡现象从而无法完成任务。智能算法在路径规划上也得到了充分的应用[3-5]，以及传统算法和智能算法相结合的混合算法[6-8]，但智能算法普遍存在着收敛速度慢、搜索空间大、控制变量多等不足等缺陷。而后两种启发式算法：A*[9]和D*[10]算法由于原理简单、容易实现受到关注。D*算法与A*算法类似，不同在于是从终点到起点进行反向搜索。基于文献[11]可知，基于动态窗口法，将全局规划A*算法和局部规划D*算法结合，最大程度确保路径精确度，与此同时，还应使算法实时性在一定程度上有所提高。根据文献[12]可知，为了使路径长度有所缩减，并实现尽可能提升路径质量的目的，对D*算法进行了一系列改进，主要表现于3个方面：①在此算法中加入了视线算法；②引入懒惰更新思想；③此算法引入了距离变换。基于文献[13]的相关内容可知，以D*算法所得路径为基准，采取有效方式获得特征点路径，并有效利用遗传算法的特定功能，得知最优路径。文献[14]中明确指出，以遍历路径中的全部节点为基础，对于某两个节点，若两者间并无任何障碍物存在，则可对其中间节点采取特殊操作(即，删除之)，并使前后节点衔接在一起，从而实现平滑路径模型的基本构建。基于上述几篇文献可知，对于D*算法，文献中的方法均在一定程度上改进了其搜索效率，但依然存在一些弊端，例如①所得路径与障碍物之间具有相对较近的距离，现实生活中，机器人行走过程中，应时刻与障碍物留有相对适中的距离；②若突然改变目标点，则已有算法无法应用，应再次规划算法。

对于以上两个问题，提出改进D*算法的启发函数和子节点选取方式，使生成的路径与障碍物保持适当的距离。基于格栅法构建地图信息，通过沃罗诺伊图[15]得到局部最佳目标点，关闭无关区域节点，提升算法计算能力。将改进D*算法和沃罗诺伊路线图法相结合进行路径仿真，解决终点改变时计算量大的问题，同时考虑在机器人行进过程中突然遭遇障碍物的情形。

1 问题描述

1.1 地图建立

构建地图模型时，本设计运用的方法主要是格栅法，具体构建阶段，任何一个格栅均与现实生活中的环境位置存在一一对应的关系。在格栅地图中，最具关键性的一项参数便是分辨率，若其具有相对较高的分辨率，则意味着环境具有更高的精准性；反之，则精准性会有所降低。本设计主要基于室内环境而研究，故基于格栅法实现地图模型的合理构建具有较好的合理性。

如图1所示，白色格栅是自由空间，黑色格栅是不可达区域。假设不考虑高度约束，且机器人行进方向只有8个。将地图信息存放在二维矩阵G(i,j) 中，矩阵可表示如下

(1)

图1 仿真地图模型

1.2 D*算法分析

1.2.1 距离表现形式

对于二维空间中任意两点(x1,y1)、(x2,y2)， 可通过不同方式进行间距的度量，主要如欧几里德距离、曼哈顿距离和切比雪夫距离，这些度量标准相应的表达式分别如下

(2)

DManhattan=|x2-x1|+|y2-y1|

(3)

DChebyshev(x,y)=max(|x2-x1|,|y2-y1|)

(4)

对比分析可知后两个标准不进行平方项处理，这样处理速度显著提高。

1.2.2 启发函数

D*算法在分析时一般用到估价函数，其表达式如下

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

(5)

其中，n为当前节点；g(n) 是基本项，对应于两个节点的实际成本；h(n) 表示启发项，表示两点最小成本估计值。对这种函数而言，D*算法的选用主要基于一点，即欧氏距离，首先展开的操作是以各个节点为基准，分别对其实行平方根运算，该操作在一定程度上拉低了处理速度，此外，由于启发函数基于欧氏距离而确定，因此，角度的限制并未纳入考虑的范围内，且该距离主要指任意两点的实际距离，而本设计在地图模型的构建中，运用的方法主要是格栅法，并不能实现对两个格栅间距的详细描述，使得图2仿真路径小范围内具有相对比较高的转弯频率，且造成了相对较大的转弯角度。

图2 D*路径

1.2.3 D*算法流程

A*算法的运用具有一定的局限性，例如，仅用于静态地图的处理。而对于D*算法，从某种程度而言，可视为A*算法的进一步改进与拓展，是一种动态逆向扇形搜索算法，主要是以地图为基础，采取一系列有效措施与手段实现其格栅建模操作，并完成最小成本路径的寻找过程。D*算法具有以下几种特点，即：①以目标点为核心，扩展单个节点周围节点；②以全局地图为核心，即便局部发生一定的改变，也并不会过多影响路径；③一般而言，到达终点的路径成本均是固定的；④计算成本相对较低。正是基于D*算法的这些特点，使其表现出了自身的独特优势。D*算法在处理时对应的流程为：首先，Process-State()路线规划阶段；其次，Modify-Cost()成本更改及传播阶段，见表1。h、k代表的含义均仅有一个，具体含义依次是终点到当前节点、当前节点到终点路径的最短路径。此外，该算法的集合同样涉及两个，分别为OPEN和CLOSED集合。

表1 D*算法伪代码

基于D*路径仿真结果如图2所示，起点终点分别为(20,10)和(80,80)，特别地将地图成本信息转化为地图高度信息，高度表示到目标的距离，可以更直观的看出D*算法流程。从仿真图可以看出，D*算法拐弯角度大，部分路段和障碍物距离较近，这对机器人在实际环境中行走时是不利的。D*虽然支持地图成本改变后进行增量式规划，但与之前算法一样，也不支持终点的临时改变，因此引入沃罗诺伊路线图法来解决该问题。

1.3 沃罗诺伊图基本思想

沃罗诺伊路线图法是一种全局路径规划方法，这种方法解决了在路径规划和生成中的成本差异，同时也支持动态的起点和终点。这样，路径规划问题就类似于在地图中建立一个避开障碍物的铁路网，只需要一次计算就可以从任意起点去到任意终点。

沃罗诺伊图是一种关于空间划分的数据结构。这种图中含有一定量沃罗诺伊单元，其中黑色五角星表示站点，几个站点组合形成网格，中心站点Pi对应的多边形就是沃罗诺伊域Vi， 当前站点距这种区域中全部点的距离小于到其它点的。此多边形的边到相邻站点Pi和Pj间距相同。假设一个集合Q⊂R2中的节点q={q1,q2,…,qn}，Vi可基于如下表达式确定

(6)

沃罗诺伊图是一种关于空间划分的数据结构，如图3所示。

图3 沃罗诺伊

在MATLAB中对地图环境进行见表2处理，map变量为原始地图信息矩阵。

表2 制作沃罗诺伊路线

所得沃罗诺伊图详见图4，形成流程主要基于视障碍物为热源，当受到一定温度后，自由格栅会迅速发生膨胀，并以同等的速度将所得热能迅速传递出去[16]。此前，由自由空间转变为网络的过程中，涉及的曲线具有与障碍物等同的距离，基于此，采取一系列有效措施使路线图完成相应地平滑性处理，从而得到最终的沃罗诺伊路线图，由此可知，从安全系数角度而言，路线图中的概括性路径已充分具备此条件，然而，具有相对而言比较多的转角、且有相对而言比较长的路径，路线图的核心点在于将地图分区以及在目标点改变后视为临时路径来应用。对于D*算法，为了在一定程度上提高其搜索效率，本设计详细划分了仿真环境，涉及的局部环境共有8个，如图4所示。

图4 沃罗诺伊路线

2 算法改进与融合

2.1 启发函数的改进

通过切比雪夫距离来取代原算法中的欧氏距离来解决该问题，在二维空间中，切比雪夫距离即为棋盘距离

DChess(Goal,n)=max(|xGoal-xn|,|yGoal-yn|)

(7)

式(7)可确定出当前格栅n到终点的步数最小值，本文在研究中设置垂直水平方向成本为10，对角线方向的为14，不进行开方运算，而提高算法的实时性。在这种图形中，对角线方向机器人可移动。这样可通过如下关系式确定出当前节点到目标点的间距

Dchess(Goal,n)=hdiagonal+hstraight

(8)

其中，h1、h2分别为

hdiagonal=min(|xGoal-xn|,|yGoal-yn|)

(9)

hstraight=|yGoal-yn|-|xGoal-xn|

(10)

将直行、斜行成本代入，得到

h(n)=10hstraight+14hdiagonal=
10|yGoal-yn|-|xGoal-xn|+
14min[|yGoal-yn|,|xGoal-xn|]

(11)

2.2 子节点选取改进

类似于A*算法，D*算法也是以当前节点为基准，采取有效方式使其8个邻居子节点完成扩展操作，并确保其以混乱顺序状态加入open list中，当然，此中可能涉及障碍物。然而，这也存在一定的问题，即：部分路径在具体的行进过程中，与障碍物有所贴近，若基于D*算法，则会在一定程度上对机器人构成威胁。本设计对D*算法做出了相应的改进，尤其体现于子节点的选取问题上。如图5所示，对于一级优先节点的选取，主要选定了垂直水平方向的子节点(2、4、6和8)，而对于二级优先节点，则主要选取了对角线方向的子节点(1、3、5和7)。对于扩展节点，主要确定为一级优先节点，然后基于此，通过下述内容选定二级优先节点：①若2为障碍物格栅，其中节点1、3不可选择。这样也排除路径0-1、0-3；②若4为此类格栅，则节点3、5不可选，排除路径0-3、0-5；③6为此类格栅情况下，则节点5、7不可选择，排除路径0-5、0-7；④若8为此类格栅，节点7、1不可选择，因而可排除路径0-7、0-1。

图5 格栅

2.3 改进算法流程

为了使上述问题得到有效改进，本文基于相应上述改进，而给出相关算法流程，具体如下：

(1)加载地图信息map(100,100)， 初始化起始格栅、目标格栅： start(20,10)、 goal(80,80)；

(2)基于沃罗诺伊路线图，采取特定的方式与操作获得地图的概括性路径，对仿真环境作出详细划分，共涉及8个局部环境，在TriplePoint集合中置入关键节点。无论是哪间房室，均应在入口及出口设定相应的关键节点，关键节点的位置主要定于两端障碍物的中端；

(3)以目标格栅为核心，采取有效方式评估其与起点格栅所处的局部环境是否相一致，若相一致，则通过改进D*算法进行规划，确定出最优路径。若不一致，则进入下一步；

(4)设置起点和目标点为中心，根据两种局部环境相关情况，选择特定方式而得到相关的概括性路径；

(5)以起点为核心，将其相关的关键节点视为局部目标点；然后以终点为基准，将其相关的关键节点视为局部起始点，采取有效手段展开路径规划操作，并将所得的局部最优路径加以保存。以改进后的D*算法为核心，详细规划途径区域，并遍历所有路径点，以节点ni为基准，当其前后节点连线不存在任何障碍物时，对节点ni采取删除操作，然后进行平滑处理。最后完成全部路径的合并；

(6)如果目标点已非原有目标点，则应将处于关闭状态的节点打开，明确新目标的具体位置，对机器人加以操作控制，使其寻找到最近的沃罗诺伊边，并以已有的概括性路径为基准，寻找新的关键节点，此后，由步骤(4)继续向下操作。

3 实验结果与分析

为了对改进D*算法的性能进行验证，而在同一台计算机(Win10，英特尔 i7，内存16 GB)上仿真分析，总共5组。对比分析：简单地图下：①S1=[20,10]，G1=[80,80]； ②S2=[80,20]，G2=[25,50] 和复杂地图下3种算法生成路径、临时遭遇障碍物相关情况。在MATLAB r2016b平台编译运行。

3.1 改进D*仿真与对比

由图6知：A*算法的缺陷主要体现于下述几点，即：具有相对较高的转弯频率，相对较大的转弯角度，同时，某些路径与障碍物之间的距离相对较近，并不具有较高的路径安全性；在节点扩展方面，D*算法与上述算法具有一定的相似性，故依然存在上述问题；如图6(c)所示，对于改进后的D*算法，主要基于优先级子领域的选定准则确定了最优路径，使上述问题有了明显的改进，并在较大程度上提高了路径安全性。此中，路径长度的识别是基于图像处理技术来完成，即，而整个操作中确保具有等同的像素比例尺下(简单地图：589×523，复杂地图：955×698)。由表3能够得知，相比于A*算法，D*算法在地图分区时，做出了相应的改进，主要以沃罗诺伊图为基准，某些无用节点首先放入CLOSED LIST中，且D*算法规划循环迭代次数在已有基础上有所增加，增加了11%，已提高至76%；且其它方面的性能均有所提升。表4可以得知，第二组起讫点下路径长度有一定幅度增加，另3组数据取得较好优化效果。在一些特定状况下，为了使路径安全性在一定程度上有所提升，本文算法会以路径长度的牺牲为代价[17]。

图6 简单地图下两组起讫点3种算法路径仿真

表3 第一组

表4 第二组

在复杂地图模型下，A*算法的转角次数明显减少，然而，具有相对而言比较长的路径，尽管D*算法对此做出了相应的优化处理，但却导致转角次数在原有基础上有所增加。根据表5可以得知，相比于上述两种算法，本文的改进主要体现于迭代次数和处理时间方面。在图7前两图对应的圆圈标记区，两种算法规划的路径与障碍物间距较近，都穿过图中的桌椅，图7(c)对应的规划路径和障碍物距离适中，同时也避开桌椅，因而在路径安全性方面相对高。

本文建立的改进D*算法可有效的避免A*和D*算法的实时性差，转弯次数多的缺陷，在路径长度方面，优化不显著，不过安全性显著高于传统算法的，有效提高了路径质量。

图7 复杂地图下3种算法路径仿真

表5 第三组

3.2 临时遭遇障碍物

某些状况下，有可能突然碰到未知的障碍物，据此，本文的改进D*算法则可对其进行较好的处理。以D*算法为基础，在其现有优势的基础上，根据现有的信息，在图8所示的状况中，极大地缩减了二次规划的迭代次数，并将具体次数缩减至83次。

3.3 目标点改变

无论是使用哪种传统算法，若路径规划的目标点与原有目标点不一致，则这些算法均不能较好地完成后续的工作。对于这种改进D*算法，在实际应用过程中可基于沃罗诺伊路线图，而有效的避免了上述问题，有较高的应用价值。假定机器人运行过程中，到达某位置(70,41)时，基于内部操作可定义新目标点位置，并由此生成沃罗诺伊路线图，以此为基础，机器人会以最适合的路径运行至合适的路线图，并由内部特定的操作确定合适的临时目标点。目前，目标点则由(80,80)发生了相应的改变，转变为(25,80)，如图9所示，将A*和D*算法及改进后的D*算法均列于图中。仿真数据结果对比如下：在机器人行进到(70,41)，目标点发生了相应的转变，变换为(25，80)，对应时间依次是：TA*=4.998s、TD*=5.706s、TID*=2.974s。 相比之下，改进D*算法的规划时间在较大程度上有所缩短。由此可知，这种路径规划下，机器人运行过程中驶向最近的沃罗诺伊边，并沿路线图相关路径不断前进一直到目标区对应的局部节点，使运行速度在较大程度上有所提升。

图8 遭遇障碍物

图9 目标点改变后3种算法路径仿真

4 结束语

A*算法、D*算法所得路径并不能满足具体的需求。因此，本文对D*算法进行了相应的改进，基于格栅地图模型，结合沃罗诺伊图得到一条具有相对较高安全性、高质量性的路径。以已知环境为基础，采取有效措施对其展开局部分解，并选定相应的局部关键节点，关闭局部区域，这样可提高算法的处理速度，更好满足实时性要求。同时，针对启发函数的选取方式，本文也做出了相应的改进，并改进了本有的子节点选取方式，引入优先级概念，使各方面的性能均有所提升，例如，路径长度有所减小、处理时间极大地缩短等。基于沃罗诺伊图，若目标点发生改变，则可在计算量相对较小的前提下，实现机器人到达新目标点的目的。最后，在MATLAB上完成了相应的仿真处理，结果可知，无论是对于生成路径质量的改善方面，还是安全性的提升方面，本文提出的改进D*算法均发挥了一定的优势，且此项改进工作使室内移动机器人可更加出色地完成工作内容，对于多数室内环境有较好的适用性。