路径规划是机器人等人工智能领域的重要技术内容之一^[1]，其主要任务是使机器人等智能体在安全避让障碍物的同时以最短时间到达目标点位置. 机器人路径规划根据机器人周围环境是否变化可分为静态路径规划和动态路径规划^[2]. 静态规划相对简单，比如Dijkstra算法^[3-4]、A*算法^[5-6]. 动态规划对实时性要求较高，须依据环境变化进行实时动态调整，其难点之一在于如何以最少的状态扩展使路径规划时间与机器人移动时间同时达到最优. Mohd等^[7-8]对2018年以前的研究情况进行了详细介绍和总结.

随着人工智能技术的应用与发展，移动机器人更多应用在无人参与的复杂环境中，如深海探测、自动导引小车等，对动态规划的要求更高. Khatib^[9]提出人工势场法以处理机器人路径动态规划问题，其优点在于只须进行简单数学分析，无须进行大量计算即可规划出光滑路径并具有较好的实时性，但利用该方法难以获得全局最优解. Yang等^[10-12]均对人工势场法进行改进，但仍存在规划时间随障碍物复杂度的增加而增大、路径拐点较多等问题. Koenig等^[13]引入环境增量信息提出LPA*（lifelong planning A*）算法，但该算法规划效率较低. Patle等^[14]利用萤火虫算法进行不确定环境下的路径规划，但存在无效转弯、机器人移动时间成本增加的问题. Panda等^[15]提出基于遗传算法的动态网格进化搜索算法，在一定程度上提高了搜索效率，但后期收敛速度较慢.

D*算法^[16]是由Stentz提出的动态规划算法，机器人在向目标点移动过程中只检查最短路径临近节点的变化情况，因此具有较高的动态搜索效率，且可以处理任何成本参数发生变化的路径成本优化问题^[17]. 由于其通过等势线逐级扩展的方式进行搜索，搜索空间较大，尤其在初始路径规划、环境复杂程度较高时，搜索空间大的问题更加突出；在单次扩展搜索时仅考虑欧几里得距离，易出现在小范围区域内多次转弯的问题，增加移动时间成本^[18]. 为此，Stentz^[19]提出Focussed D*（focussed dynamic A*）算法，通过增加偏置函数来降低搜索空间，在一定程度上降低了路径规划时间，但机器人在小范围区域内多次转弯的问题没有得到有效解决；Koenig等^[20]提出D* Lite（dynamic A* Lite）算法，该算法在一定程度上提高了算法效率，但存在节点重复计算问题；Ganapathy等^[21]提出Enhanced D* Lite（enhanced dynamic A* Lite）算法，避免了移动机器人穿越尖角等一系列不安全路径，但机器人在小范围区域内多次转弯的问题依然没有得到有效解决.

传统移动机器人动态路径规划算法在确定子节点时遍历所有节点因而搜索效率低、存在多次转弯而增加移动时间成本. 针对上述问题提出有向D*算法，算法通过引入关键节点概念，逐级扩展确定可行路径，并结合障碍物位置信息引入导向函数缩小路径规划的搜索空间，提高搜索效率；在欧几里得距离评价标准的基础上引入平滑度函数，对规划路径与理想路径的偏移程度进行惩罚，减少路径拐点数量，提高路径平滑度，并对平滑度函数中转弯因子的确定方法、特点进行说明. 在此基础上，对有向D*算法的收敛性进行证明并给出算法流程. 在3种大小不同、障碍物复杂情况不同的环境空间内对算法进行测试，仿真实验表明所提方法能较好的兼顾局部搜索与系统最优性，尤其适用于障碍物较多的复杂环境.

移动机器人路径规划的环境建模方法主要有栅格法、几何特征法、自由空间法等. 由于栅格法数据结构简单、能有效表达空间的可变性，本研究采用如图1所示的栅格法对移动机器人的工作环境进行建模. 图中，笛卡尔坐标系中横坐标 ${x_n}$与纵坐标 ${y_n}$分别为环境地图的长、宽，两者乘积为环境地图的面积，n为栅格个数；空白栅格表示无障碍物的环境、灰色栅格表示已知障碍物的环境、黑色栅格表示存在潜在障碍物的环境.

相邻栅格的交点为路径规划的节点. 假设节点 $S$为机器人初始位置；节点 $G$为目标位置. 在路径规划时，首先将点 $S$作为父节点，通过规划算法确定下一个最优节点 ${X_A}$（图中用菱形表示）为子节点，再以其作为父节点规划确定下一个子节点 ${X_B}$，依次类推逐级确定机器人最终行走路径 $\left\{ {\left. {S,{X_A},{X_B},{X_C},G} \right\}} \right.$. 机器人按规划好的路径节点序列移动，当机器人通过传感器发现实际地图和预存地图存在环境差异时，更新地图并重新规划路径，形成新的路径节点序列；移动机器人沿新路径节点序列继续移动，直至到达目标或发现新的环境差异，重复上述过程直至目标位置.

令 $O\left( {{t_i}} \right)$为移动机器人依据预存地图或环境变化进行第 $i$次路径规划所需时间，称其为第 $i$次离线规划时间；令 $F\left( {{t_i}} \right)$为机器人依据规划路径进行第 $i$次移动的移动时间，称其为第 $i$次在线移动时间；令 $N$为机器人依据环境变化进行路径规划的总次数，则机器人路径动态规划的目标函数如下：

(1) $\min\; {\sum\limits_{i = 1}^N {\left( {O\left( {{t_i}} \right) + F\left( {{t_i}} \right)} \right)} } .$

须注意的是：不考虑设备硬件情况，仅从路径规划算法角度出发，离线规划时间由规划算法的迭代次数、障碍物的数量和环境大小等决定；在线移动时间由路径距离、路径平滑度决定；N则受未知环境中障碍物大小、数量和障碍物的复杂程度等影响. 移动机器人路径动态规划的主要难点之一在于如何协调局部搜索与全局规划，以最少的节点扩展使路径搜索离线规划时间与机器人移动时间成本均达到最小. 随着环境或障碍物复杂程度的增加，上述问题的难度及规划需要愈发突出，因此本研究提出有向D*算法来解决该问题.

传统机器人路径动态规划D*算法较其他诸多算法有较好的全局收敛性，但其存在以下问题. 1）传统D*算法在离线路径规划时采用类似等势线逐级扩展的方式，从目标点开始对地图节点进行遍历，导致搜索范围较大. 尤其当搜索地图区域较大时，问题更为突出. 本研究考虑障碍物及目标点位置信息，引入关键节点概念对节点进行区分（关键节点为障碍物所确定的相关节点），从初始点开始只对关键节点进行访问，缩小搜索空间；再结合目标点位置信息，引入导向函数以确定单次扩展时所需比较的路径节点，确定可行路径，提高路径规划速度. 2）一般D*算法在确定最优路径时使用欧几里得距离指标进行评价，忽略障碍物尤其是未知障碍物可能导致最终规划路径存在小范围区域内多次无效转弯的问题. 针对该问题，本研究引入平滑度函数对规划路线偏离理想路径的程度进行惩罚，用以控制最终规划路径的拐点数量，避免机器人在小范围区域内无效转弯.

综上所述，本研究在D*算法的基础上提出有向D*动态规划算法，该算法首先结合目标点位置及已知障碍物信息确定关键节点，并结合导向函数，通过逐级扩展搜索的方式确定可行路径；在欧几里得评价指标的基础之上，通过引入平滑度函数对可行路径进行寻优，主要系统结构如图2所示.

D* 算法在初始路径规划时使用类似等势线逐级扩展的方式，会遍历较多不必要节点，导致D*算法搜索空间较大. 为了缩小搜索空间，提出以下技术改进措施.

1）改进搜索方式. 对节点进行区分，将障碍物近似为四边形，以其4个顶点作为关键节点，主要针对关键节点进行搜索、筛选，并从初始点开始由前往后通过由父节点逐级确定子节点的方式确定可行路径. 采用逐级扩展的方式既能将每一个关键节点访问到，同时可以避免对不必要的关键节点进行路径成本计算，以提高规划效率.

如图3所示为传统D*算法与本研究改进方法搜索方式区别举例. 图3(a)中每个栅格表示一个状态，传统D*算法在由点S确定到目标点G的路径时，须从点G开始将所有状态遍历并形成指向G的后向指针，因此栅格的数量影响D*算法在初始路径规划时须遍历的状态数，其须遍历的状态数随环境地图的扩大呈指数级增长；如图3(b)所示为采用本研究方法的搜索方式，考虑障碍物位置信息仅须对11个关键节点（用菱形表示）进行访问，有效降低搜索空间，提高路径规划效率.

2）关键节点筛选. 传统D*算法由目标点开始反向采用类似等势线逐级扩展的方式确定可行路径；本研究从出发点开始，对于关键节点采用由父节点逐级确定子节点的方式，通过引入导向函数 $p\left( x \right)$，对关键节点进行搜索和筛选. 该导向函数由2个节点构造直线方程，通过判断该直线方程是否与障碍物区域有交集从而对子节点进行筛选：若存在交集则舍弃；若不存在则保留，后续进一步进行路径寻优. 该方式能降低路径规划单次迭代的搜索空间，减少规划时间. 导向函数定义为

(2) $p(x) = \mathop {\lim }\limits_{x \to {x_{Y_i}}}\; \left(\frac{{(x - {x_{{X_i}}})({y_{{Y_i}}} - {y_{{X_i}}})}}{{{x_{{Y_i}}} - {x_{{X_i}}}}} + {y_{{X_i}}} + \alpha \right).$

式中： ${X_i}$为第 $i$次路径扩展时的父节点； ${Y_i}$为第 $i$次路径扩展时的待定子节点； $\left( {{x_{{X_i}}},{y_{{X_i}}}} \right)$为父节点位置坐标； $\left( {{x_{{Y_i}}},{y_{{Y_i}}}} \right)$为待定子节点位置坐标；横坐标 $x$满足 ${x_{{X_i}}} \leqslant x \leqslant {x_{{Y_i}}}$； $\alpha $为可变参数， $\alpha = 0$表示路径未穿过障碍物区域， $\alpha = \infty $表示路径穿过障碍物区域.

上述2种处理方式优点如下. 1）可以较好地缩小搜索空间、减小计算机数据的运算量，提高搜索效率；2）在工程实践中，障碍物位置区域一般仅为地图的小部分区域，从总体上看搜索空间相对有限，所提出的四边形的障碍物建模方式简化了搜索过程且降低了关键节点的搜索数量；3）可以通过平滑度函数控制寻优方向，能较好地兼顾局部搜索与规划路径的系统整体最优性，避免一般D*及传统路径动态规划算法易产生冗余路径的问题.

传统D*算法在对可行路径进行寻优时，未考虑移动机器人在运动中躲避障碍物时可能出现多次无效转弯的问题，导致规划路径非全局最优，增加移动时间成本. 针对该问题，本研究在欧几里得距离评价基础上引入平滑度函数对规划路径偏离理想路径（理想路径即初始点与目标点的连线）的程度进行惩罚，筛选出每一次扩展时偏移程度相对较小的路径，从而最大限度避免移动机器人的无效转弯. 建立成本函数如下：

(3) $h({X_i},{Y_i}) = f\left( {{X_i},{Y_i}} \right) + f\left( {{\theta _i}} \right){\text{.}}$

式中： $f({X_i},{Y_i})$为父节点与待定子节点之间的距离成本， $f({X_i},{Y_i}) = [ {{{({x_{{Y_i}}} - {x_{{X_i}}})}^2} + {{({y_{{Y_i}}} - {y_{{X_i}}})}^2}}]^{1/2} $； $f\left( {{\theta _i}} \right)$为引入的路径平滑度惩罚函数， ${\theta _i}$为待定子节点与父节点之间形成的路径与理想路径之间的夹角（以下简称为路径夹角）.

定义路径平滑度惩罚函数如下：

(4) $f\left( {{\theta _i}} \right) = \left\{\begin{array}{*{20}{l}} - 8\mu , & 0 \leqslant \left| \theta \right| < {\text{π}} /6;\\ - 4\mu ,&{\text{π}} /6 \leqslant \left| \theta \right| < {\text{π}} /3;\\ \mu , & {\text{π}} /3 \leqslant \left| \theta \right| < {\text{π}} /2;\\ 50\mu ,&{\text{π}} /2 \leqslant \left| \theta \right| \leqslant {\text{π}}. \end{array} \right.$

式中： $\mu $为环境因子，描述不同环境下已知障碍物对环境的影响信息，随着障碍物所占环境地图面积比率的不同而有所不同. 考虑到每一次扩展都可能产生距离成本相等的关键节点（以下简称为相似关键节点）且不同路径之间应有一定区分度，须将 $f\left( {{\theta _i}} \right)$进行分段，分段原理与 $\mu $的取值情况如下.

考虑到环境的复杂程度会随着地图的增大而变化，对不同大小的环境应采取分段处理方式，结合实践建议分段方式如下：对于小尺寸地图（小于 $250 \times 250$）、中等尺寸地图（ $250 \times 250$~ $500 \times 500$），在单次扩展时待定关键节点数较少，建议将 $f\left( {{\theta _i}} \right)$ $\left( {0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}} \right)$分为6段、 $f\left( {{\theta _i}} \right)$ $\left( {{{\text{π}} / 2} < \left| \theta \right| \leqslant \pi } \right)$为1段；对于大尺寸地图（大于 $500 \times 500$），在单次扩展时待定关键节点数较多，为了使关键节点的区分度更高，建议将 $f\left( {{\theta _i}} \right)$ $\left( {0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}} \right)$分为8段、 $f\left( {{\theta _i}} \right)$ $\left( {{{\text{π}} / 2} < \left| \theta \right| \leqslant {\text{π}} } \right)$为1段.

路径夹角θ取值范围为（0, 2π），根据第 $i$次扩展得到的子节点的横坐标 ${x_i}$、第 $i - 1$次扩展得到的子节点的横坐标 ${x_{i - 1}}$之间的大小关系，将 $\theta $分为 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$、 ${{\text{π}} / 2} < \left| \theta \right| \leqslant {\text{π}} $. 当 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$时， $x{}_i \geqslant {x_{i - 1}};$当 ${{\text{π}} / 2} < \left| \theta \right| \leqslant {\text{π}} $时， $x{}_i < {x_{i - 1}}$，路径出现倒退现象且 $\left| \theta \right|$越接近 ${\text{π}} $，倒退现象越严重，导致路径长度增加. 因此，为了避免路径出现倒退现象，在 $\mu $一定时根据经验取 $f\left( {{\theta _i}} \right){\rm{ = }}50\mu $.

另外，考虑到不同路径因具有相同距离成本而无法判定最优关键节点的情况，须对 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$再进行分段，在分段后可通过平滑度函数对关键节点进行选择，以提高长度相等路径之间的区分度. 从相似关键节点的位置和不同路径之间的区分度2个方面说明 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$的分段方法.

1）相似关键节点的位置. 考虑障碍物四边形的建模方式，可以认为障碍物的顶点均在网格交点（即路径节点）处，则路径位置为正方形对角线、栅格边、长方形对角线3种情况. 当路径位置为正方形对角线时，由于 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$区域内仅存在2个相似关键节点且两者 $\left| \theta \right|$相等即平滑度相等，两者取其一即可，故不再考虑这种情况；当路径位置为栅格边时，由于 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$区域内仅存在3个相似关键节点且其中2个关于另一个对称，故只须将 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$平均分为2段，即可避免出现相同距离成本而无法选择出最优路径的问题；当路径位置为长方形对角线时，由于 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$区域内存在4个相似关键节点且关于水平线对称，因此只须将水平线上2个相似关键节点和水平线下2个相似关键节点分开即可，故将 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$平均分为3段.

2）不同路径之间的区分度. 不同的 $\theta $表示不同路径和理想路径之间的偏移程度，为了选出具有较好平滑度的路径，须对具有不同 $\theta $的路径进行不同程度的罚值，因此要从不同路径之间的区分度角度来考虑如何分段. 在将 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$均分为 $b$段时，路径落入其中一段的概率为 ${1 / ({2b})}$、路径落入其他区域的概率为 $1 - {{1 /( {2b})}} $，且每一条路径的位置不受其他路径位置的影响. 因此，将路径角度概率问题抽象为二项分布数学模型，设路径位置在 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$区间内其中一段区的情况为事件 $A$，发生的概率为 $p$；路径位置在其他段的情况为事件 $\overline A $，发生概率为 $q$；在单次迭代中可行路径条数即为独立重复试验的次数 $z$，在 $z$次重复实验中，事件 $A$恰好发生 $k$次的概率如下：

(5) ${P_k} = {\rm C}_z^k{p^k}{q^{z - k}}\,;\;\; {k = 0,1,2, \cdots ,z}, $

(6) $P\left( A \right) = p = {1 / {\left( {2b} \right)}}\,;\;\; {0 < p < 1},$

(7) $P\left( {\overline A } \right) = q = 1 - p = 1 - {1 / {\left( {2b} \right)}}.$

$b$的取值通过实验的方式获得，实验方法如下：在不同的独立实验次数中，分别对 $b$取不同值的情况下事件 $A$发生 $k$次的概率进行统计，部分统计结果如表1所示. 可以看出，在 $z$、 $b$一定的情况下，事件 $A$发生概率的总体趋势为随着 $k$的增大而减小；在 $z$、 $k$一定的情况下，事件 $A$发生概率的总体趋势为随着 $b$的增大而减小. 当有少数几条路径同时位于同一段时，分段数大于2，不存在距离成本相同的情况，即可通过距离成本函数 $f\left( {{X_i},{Y_i}} \right)$进行筛选，达到路径成本最优，故将 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$平均分为6段.

综上所述，对于小尺寸地图、中等尺寸地图， ${{\text{π}} / 2} < \left| \theta \right| \leqslant {\text{π}} $为1段， $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$均分为6段.

平滑度函数 $f\left( {{\theta _i}} \right)$的作用是对路径偏移进行惩罚，罚值体现在路径距离上，因此，将所有待选路径距离相加后除以待选路径数，获得待选路径的平均距离（以下简称为平均距离）. 为了协调路径长度与平滑度之间的关系，不同的平均距离对应不同的 $\mu $，结合试验与实践，当障碍物覆盖率OC<10%时，建议 $\mu $=4~6，本研究取5；当障碍物覆盖率为10%~40%时，建议 $\mu $=1~3，本研究取2；当障碍物覆盖率为40%~80%时，建议 $\mu $≤1. 由于 $\mu $的取值难以从理论上证明，因此从障碍物覆盖面积一定，环境大小不同、环境大小一定，障碍物覆盖面积不同2个方面进行实验，实验结果如下.

1）当障碍物覆盖面积一定，环境大小不同时，对平均距离进行多次近似计算，受篇幅限制，现仅给出环境维数ES为50²、100²、500²、1 000²，障碍物节点数N_O为环境总节点数N_E的60%、关键节点数N_K为障碍物节点数的20%情况下平均距离的计算结果，如表2所示. 由表2及其他试验结果可以看出，在不同大小的环境下，若障碍物覆盖率相同且关键节点占障碍物节点数比率相同，地图环境大小对两相邻关键节点平均距离D_K基本无影响.

2）当环境大小一定，障碍物覆盖面积不同时，对2个关键节点距离进行多次测试、计算，结果如图4所示. 当障碍物覆盖率为10%时，两关键节点间平均距离最大；当障碍物覆盖率为80%时，两关键节点间平均距离最小. 可以看出，两关键节点间平均距离随碍物覆盖率的增加而减小.

综上所述， $\mu $的取值跟障碍物覆盖率有关，因此建议：当障碍物覆盖率小于10%时， $\mu $=4~6，本研究取5；当障碍物覆盖率为10%~40%时，µ=1~3，本研究取2；当障碍物覆盖率为40%~80%时， $\mu $≤1.

机器人路径规划的具体实现过程如下. 将地图初始化筛选出关键节点，定义OPEN列表存放关键节点；CLOSED列表存放父节点；DELETE列表存放每次迭代中除子节点以外的关键节点. 在初始路径规划时通过导向函数 $p\left( x \right)$筛选出单次迭代须访问的关键节点，若 $p\left( x \right)$不存在，则关键节点X到关键节点 $Y$的路径不存在， $h\left( {{X_i},{Y_i}} \right)$未定义；若 $p\left( x \right)$存在，则关键节点 $X$、 $Y$在初始环境中是相邻的2个关键节点， $h\left( {{X_i},{Y_i}} \right)$被定义.

在算法过程中，通过导向函数 $p\left( x \right)$在OPEN列表中筛选单次迭代须访问的关键节点,称为子节点 $t\left( {{Y_i}} \right)$，位置为 $\left( {{x_{{Y_i}}},{y_{{Y_i}}}} \right)$. 对筛选出的关键节点按 $h\left( {{X_i},Y_i} \right)$排序，将 $h\left( {{X_i},{Y_i}} \right)$ 最小的关键节点放入CLOSED列表（最小 $h\left( {{X_i},{Y_i}} \right)$表示为 ${h_{\min }}\left( {{X_i},{Y_i}} \right)$），并将此关键节点作为下一次扩展的起点即新的父节点，用 $t\left( {{X_i}} \right)$表示，位置为 $\left( {{x_{{X_i}}},{y_{{X_i}}}} \right)$. 将单次扩展筛选出的路径成本大于 ${h_{\min }}\left( {{X_i},{Y_i}} \right)$且满足 $ {x_{{Y_i}}} \leqslant {x_{{X_i}}} \cap $ ${y_{{Y_i}}} \leqslant {y_{{X_i}}}$的关键节点从OPEN列表删除，放入DELETE列表. 注意，在单次扩展时，保留具有最小 $h\left( {{X_i},{Y_i}} \right)$的关键节点，该操作定义了从点 $S$到当前位置最小成本的条件. 在算法过程中，对第 $N\left( {N \in 1,2, \cdots ,n} \right)$次扩展筛选出的关键节点按 $h\left( {{X_i},{Y_i}} \right)$进行排序，判断具有最小 $h\left( {{X_i},{Y_i}} \right)$的关键节点（用 $t\left( {{Y_i}} \right)$表示）与CLOSED列表中第 $ {l - 2} $个（l为CLOSED列表中关键节点个数）关键节点的路径是否存在. 若不存在，将 $t\left( {{Y_i}} \right)$放入CLOSED列表，令 $t\left( {{Y_i}} \right){\rm{ = }}$ $t\left( X \right)$作为下一次迭代的起点；若路径存在，则将CLOSED列表中第 $l - 1$个关键节点删除，放入DELETE列表，令 $t\left( {{Y_i}} \right) = t\left( X \right)$为下一次迭代的起点，避免 $H\left( {X,D} \right) \leqslant H\left( {X,A} \right) + h\left( {A,D} \right)$（X、A、D为关键节点）情况的出现，保证路径的最优性. 迭代通过反复删除OPEN列表中关键节点进行，搜索到目标点结束，形成初始路径的关键节点序列 $\left\{ X \right\}$.

移动机器人按初始路径的关键节点序列 $\left\{ X \right\}$移动，在检测到未知障碍物后，将受影响的关键节点放回OPEN列表. 调用导向函数 $p\left( x \right)$筛选单次迭代须访问的关键节点，并重新计算路径成本，令具有最小路径成本函数值的关键节点为 $t\left( X \right)$，当 $t\left( X \right)$等于CLOSED中任意已有的关键节点，形成新的关键节点序列 $\left\{ {\left. Y \right\}} \right.$，移动机器人沿 $\left\{ {\left. Y \right\}} \right.$继续移动，移动机器人到达目标时算法结束. 算法伪代码如下.

Function: INITIAL PATH

1 sort by the value of h(X_i,Y_i) in OPEN

2 while ^~is empty(OPEN) && t(X)^~= G

3 if correct point

4 calculation h(X_i,Y_i)

5 find h_min (X_i,Y_i)

6 if t (X )^~=h_min (X_i,Y_i) && x_Yi≤x_Xi && y_Yi≤y_Xi

7 t(X)=DELETE

8 else if t (X)=h_min (X_i,Y_i)

9 t (X)=CLOSED

10 starting from t(X)

11 else

12 t (X)=OPEN

13 end

14 else

15 t (X)=OPEN

16 end

17 end

若要证明算法收敛，只须证明每次扩展后得到的新的父节点横/纵坐标与目标点横/纵坐标的差值均趋于减小. 令 ${n \times n} $为坏境面积； $P$为障碍物个数； $E$为离散的状态搜索空间，其状态数为 $E = {4^P}$； $N$为迭代次数； ${Q_i}\left( {{Q_i} = 1,2, \cdots ,x|x \in E} \right)$为单次迭代访问的节点数， ${Q_i} < E$； $R$为单次迭代的搜索空间剩余状态数， $R = E - {Q_i}$，单调递减； ${d_i}$为父节点 $t\left( {{X_i}} \right)$到目标点的距离.

证明：

令 $\left( {{x_{{X_i}}},{y_{{X_i}}}} \right)$为第 $i$次迭代后父节点 $t\left( {{x_i}} \right)$的位置，令 $\left( {{x_{{X_{i + 1}}}},{y_{{X_{i + 1}}}}} \right)$为第 $i + 1$次迭代后父节点 $t\left( {{x_{i + 1}}} \right)$的位置， $\left( {{x_G},{y_G}} \right)$为目标点 $G$的位置.

在第 $i$次迭代后父节点横/纵坐标距目标点横/纵坐标的差值分别为 $\Delta {x_i} = {x_G} - {x_{{X_i}}}$， $\Delta {y_i} = {y_G} - {y_{{X_i}}}$.

在第 $i + 1$次迭代后父节点横/纵坐标距目标点横/纵坐标的差值分别为 $\Delta {x_{i + 1}} \!\!= \!\!{x_G} \!-\! {x_{{X_{i + 1}}}}$， $ \Delta {y_{i + 1}} \!\!=$ $ {y_G} \!-\! {y_{{X_{i + 1}}}}$.

根据算法的搜索策略，只要存在离目标更近的关键节点，则一定有 ${x_i}\!\! \leqslant\!\! {x_{{X_{i + 1}}}}$且 ${y_i} \!\!\leqslant\!\! {y_{{X_{i + 1}}}}$，则 $ \Delta {x_{i + 1}} \!\!\leqslant$ $\Delta {x_i}$， $ \Delta {y_{i + 1}} \leqslant \Delta {y_i}$.

因此，当单次迭代的搜索空间剩余状态数趋近于零时，有 $\mathop {\lim }\nolimits_{R \to 0}\; {\Delta {x_i}} = 0$， $\mathop {\lim }\nolimits_{R \to 0} \;{\Delta {y_i}} = 0$.

综上所述，算法具有收敛性.

1）初始化栅格地图，设置起点、终点以及障碍物. 2）依据地图中的障碍物位置信息选取关键节点. 3）所有关键节点分为3组：CLOSED列表、OPEN列表、DELETE列表. 4）通过导向函数 $p\left( x \right)$获得单次扩展须访问的关键节点，选取须访问关键节点中的最优关键节点. 迭代通过OPEN列表中反复删除位置状态进行，直到当终点G加入CLOSED列表或起点S所能到达的所有节点都包含于CLOSED、DELETE列表中，搜索结束. 5）移动机器人沿初始最优路径前进，若传感器探测到实际地图信息与离线地图信息不符时，将移动机器人的当前位置设为起始节点，进入第4）步，对路径进行重新规划找到避开障碍物的安全路径，当移动机器人移动到目标节点 $G$时，算法结束. 有向D*算法流程图如图5所示.

仿真环境如下：Intel(R)Core(TM)i3-3240 CPU &3.40 GHz双核处理器、Win7旗舰版32位系统以及MATLAB 2010a软件.

为了保证仿真实验结果的客观性、避免特定环境可能带来的影响，采用不同数据在同一台计算机上分别对Focussed D*、D* Lite和有向D*算法进行多次运算，且仿真实验分别从不同环境大小、不同障碍物覆盖率两方面进行. 如图6(a)所示为环境面积小且障碍物数目少的简单环境，如图6(b)所示为环境面积一般且障碍物数目较多的环境，如图6(c)所示为环境面积大且障碍物数目较多的复杂环境. 以这3种环境为例进行对比分析，初始节点、目标节点如表3所示.

如表4所示，从路径距离方面对实验结果进行分析. 可以看出，在 $50 \times 50$的环境中，有向D*算法路径长度比D* Lite、Focussed D*算法分别缩短6.5%、4.2%；在 $100 \times 100$的环境中，有向D*算法的路径长度比D* Lite、Focussed D*算法分别缩短3.8%、2.2%；在 $500 \times 500$的环境中，有向D*算法的路径长度比D* Lite、Focussed D*算法分别缩小3.7%、27.0%. 环境越复杂，有向D*算法相对于Focussed D*算法的优势越明显，原因在于有向D*算法在欧几里得评价指标的基础上引入了平滑度函数，最大限度地减少路径发生无效转弯的次数，进而缩短路径长度.

如表5所示，从路径规划时间角度对实验结果进行分析. 可以看出，在 $50 \times 50$的环境中，有向D*算法的路径规划离线时间比D* Lite、Focussed D*算法分别缩短69%、75%；在 $100 \times 100$的环境中，有向D*算法的路径离线规划时间比D* Lite、Focussed D*算法分别缩短89%、92%；在 $500 \times 500$的环境中，有向D*算法的路径规划时间比D* Lite、Focussed D*算法分别缩短97%、98%. D* Lite算法、Focussed D*算法规划时间长的原因是当环境面积较小、障碍物个数较少时，规划最优路径所需搜索的节点数较少，因此与本研究算法路径规划时间差距相对较小，但是随着环境面积的增加及障碍物数目的增加，所需搜索的节点数会呈指数增长，导致 $O\left( T \right)$离线规划时间及 $F\left( T \right)$在线移动时间大幅增加，计算效率受到影响. 在有向D*算法中，为了缩小移动机器人路径规划时的搜索空间，在对移动机器人路径规划时引入导向函数 $p\left( x \right)$，在导向函数中考虑移动机器人和关键节点的相对位置，使规划具有一定的方向性，有效缩小移动机器人在进行路径规划时的搜索空间，最大限度减少状态扩展和路径规划时间. 环境维数越大，有向D*算法的规划效率优势越明显.

如图7(a)~(c)所示为不同环境下相关算法所规划的路径，其拐点数量如表6所示. 如图7(a)所示，当地图面积较小、障碍物数量较少时，由于Focussed D*算法在规划路径时仅使用欧几里得评价指标，规划的路径平滑度不高；D* Lite算法主要在缩小搜索范围方面进行了改进，因此路径平滑度与Focussed D*算法基本持平；本研究所提出的有向D*算法的平滑度明显优于前2种算法. 如图7(b)所示，当地图面积与障碍物等环境复杂程度相较于图7(a)而言都有一定程度的增加时，有向D*算法的路径拐点个数比D* Lite、Focussed D*算法分别减少23%、50%，可以有效减少机器人的在线移动时间. 如图7(c)所示，当环境面积、环境复杂程度相较于图7(b)均更高时，利用有向D*算法规划的路径在拐点数量上比D* Lite、Focussed D*算法分别减少29%、52%，在平滑度方面的优势更加突出. 原因在于所提算法在选取子节点时引入路径平滑度函数 $f\left( {{\theta _i}} \right)$，对转弯进行处罚，将拐点的影响考虑进整体优化中，而传统算法并没有考虑拐点对规划路径的影响. 另外，在本研究所提方法中，较大的转弯因子 $\mu $使规划出的路径具有更好的平滑度，较小的转弯因子 $\mu $使规划出的路径距离更短. 由于路径规划的最终结果对转弯因子 $\mu $较敏感，因此恰当设置 $\mu $可使拐点数与路径距离实现综合最优，最终实现离线规划时间及在线移动时间最小的目标.

综上可知，随着环境地图的增大及障碍物的增加，有向D*算法相较于其他2种传统经典算法所规划的路径拐点数量更少、路径更短；离线规划时间及在线移动时间更少，并且可以在较小的迭代次数内获得最优解.

本研究在D* 算法的基础上提出有向D*算法. 该算法通过引入导向函数使路径规划具有一定的方向性，缩小搜索空间从而减少离线规划时间；引入路径平滑度函数，在考虑路径长度的同时考虑路径的平滑度，最大限度减少机器人在线移动时间，提高移动效率，保证算法的全局最优性. 仿真实验结果证明本研究算法的可行性与有效性，环境越复杂，本研究算法的规划效率、路径长度较其他传统算法的优势越明显. 该算法仍存在有待改进之处，例如，如何进一步提高路径的动态更新效率；在特殊环境下由父节点确定子节点时，仅比较第N、N−1代父节点与第N代子节点之间的路径长度，可能存在路径无效转弯导致达不到全局最优的问题，这也是下一阶段的研究工作.