CN112486178A - 一种基于有向d＊算法的动态路径规划方法 - Google Patents

Abstract

本发明适用于路径规划技术领域，提供了一种基于有向D*算法的动态路径规划方法，针对在D*算法的基础上提出有向D*算法。该算法通过引入导向函数使路径规划具有一定的方向性，缩小搜索空间从而减少离线规划时间；引入路径平滑度函数，在考虑路径长度的同时考虑路径的平滑度，最大限度减少机器人在线移动时间，提高移动效率，保证算法的全局最优性。仿真实验结果证明本研究算法的可行性与有效性，环境越复杂，有向D*算法的规划效率、路径长度较其他传统算法的优势越明显。

Description

一种基于有向D＊算法的动态路径规划方法

技术领域

本发明主要涉及路径规划技术领域，具体涉及一种基于有向D*算法的动态路径规划方法。

背景技术

在移动机器人诸多技术的研究中，路径规划是技术研究中的重要部分之一，是机器人研究领域的热点，目的是在有障碍物的环境中按照某一种评价指标寻找一条从起始点位置到目标点位置的最优无碰撞路径，栅格法环境建模的经典方法，通过利用栅格对环境信息进行表示，栅格的大小决定了环境信息储存量的大小以机器人进行路径规划的时间长短，A*算法是路径规划算法中经典的启发式搜索算法，A*算法由Hart提出，结合Dijkstra算法和最佳优先搜索算法的优点，针对传统路径规划算法无法高效地，安全地完成动态环境下的路径规划，D*算法是A*算法的拓展，是一种动态逆向扇形搜索算法，通过将地图进行格栅建模然后寻找最小成本路径，逆向的搜索机制保留了地图成本，避免了回溯的高计算成本，这也是D*算法最大的优点。相比于A*算法，D*算法在某些动态环境下搜索效率更高，与另一些启发式算法相比，D*算法计算量小，实现起来比较简单，所以D*算法在路径规划和人工智能等方面得到了广泛的应用，在D*算法的基础上提出有向D*算法，该算法通过引入导向函数使路径规划具有一定的方向性，缩小搜索空间从而减少离线规划时间；引入路径平滑度函数，在考虑路径长度的同时考虑路径的平滑度，最大限度减少机器人在线移动时间，提高移动效率，保证算法的全局最优性。

发明内容

本发明提供一种基于有向D*算法的动态路径规划方法，在D*算法的基础上提出有向D*算法，该算法通过引入导向函数使路径规划具有一定的方向性，缩小搜索空间从而减少离线规划时间；引入路径平滑度函数，在考虑路径长度的同时考虑路径的平滑度，最大限度减少机器人在线移动时间，提高移动效率，保证算法的全局最优性，仿真实验结果证明本研究算法的可行性与有效性，环境越复杂，有向D*算法的规划效率、路径长度较其他传统算法的优势越明显。

本发明时这样实现的，一种基于改进有向D*算法的动态路径规划方法，其特点在于，所述路径规划方法包括以下步骤：

1、一种基于有向D*算法的动态路径规划方法，其特征在于，所述方法具体包括以下步骤：

S1、环境建模；

S2、D*算法原理；

S3、D*算法存在的问题；

S4、改进D*算法的搜索方式和关键节点筛选；

S5、引入平滑度函数对规划路径偏离理想路径的程度进行惩罚；

S6、有向D*算法流程；

S7、算法对比仿真。

2、根据权利要求书1所述的基于有向D*算法的动态路径规划方法，其特征在于，步骤S4具体如下：

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

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

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

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

式中：X_i为第i次路径扩展时的父节点，Y_i为第i次路径扩展时的待定子节点；

为父节点位置坐标；

为待定子节点位置坐标；横坐标x满足

α为可变参数，α＝0表示路径未穿过障碍物区域，α＝∞表示路径穿过障碍物区域。

3、根据权利要求书1所述的基于有向D*算法的动态路径规划方法，其特征在于，步骤S5具体如下：

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

h(X_i,Y_i)＝f(X_i,Y_i)+f(θ_i)

式中：f(X_i,Y_i)为父节点与待定子节点之间的距离成本，

f(θ_i)为引入的路径平滑度惩罚函数，θ_i为待定子节点与父节点之间形成的路径与理想路径之间的夹角(以下简称为路径夹角)；

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

式中：μ为环境因子，描述不同环境下已知障碍物对环境的影响信息，随着障碍物所占环境地图面积比率的不同而有所不同。

4、根据权利要求书1所述的基于有向D*算法的动态路径规划方法，其特征在于，步骤S6具体如下：

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

在算法过程中，通过导向函数p(x)在OPEN列表中筛选单次迭代须访问的关键节点，称为子节点t(Y_i)，位置为

对筛选出的关键节点按h(X_i,Y_i)排序，将h(X_i,Y_i)最小的关键节点放入CLOSED列表(最小h(X_i,Y_i)表示为h_min(X_i,Y_i))，并将此关键节点作为下一次扩展的起点即新的父节点，用t(X_i)表示，位置为

将单次扩展筛选出的路径成本大于h_min(X_i,Y_i)且满足

的关键节点从OPEN列表删除，放入DELETE列表，注意，在单次扩展时，保留具有最小h(X_i,Y_i)的关键节点，该操作定义了从点S到当前位置最小成本的条件；

进一步的，对第N(N∈1,2,...,n)次扩展筛选出的关键节点按h(X_i,Y_i)进行排序，判断具有最小h(X_i,Y_i)的关键节点(用t(Y_i)表示)与CLOSED列表中第i-2个(l为CLOSED列表中关键节点个数)关键节点的路径是否存在，若路径不存在，将t(Y_i)放入CLOSED列表，令t(Y_i)＝t(X)作为下一次迭代的起点；若路径存在，则将CLOSED列表中第i-1个关键节点删除，放入DELETE列表，令t(Y_i)＝t(X)为下一次迭代的起点，避免H(X,D)≤H(X,A)+h(A,D)(X、A、D为关键节点)情况的出现，保证路径的最优性，迭代通过反复删除OPEN列表中关键节点进行，搜索到目标点结束，形成初始路径的关键节点序列{X}；

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

本发明有以下有益效果：

(1)可以较好地缩小搜索空间、减小计算机数据的运算量，提高搜索效率；

(2)在工程实践中，障碍物位置区域一般仅为地图的小部分区域，从总体上看搜索空间相对有限，所提出的四边形的障碍物建模方式简化了搜索过程且降低了关键节点的搜索数量；

(3)可以通过平滑度函数控制寻优方向，能较好地兼顾局部搜索与规划路径的系统整体最优性，避免一般D*及传统路径动态规划算法易产生冗余路径的问题；

(4)在欧几里得距离评价基础上引入平滑度函数对规划路径偏离理想路径(理想路径即初始点与目标点的连线)的程度进行惩罚，筛选出每一次扩展时偏移程度相对较小的路径，从而最大限度避免移动机器人的无效转弯。

附图说明

图1为传统D*算法与本研究算法的搜索方法图；

图2为平均距离与障碍物覆盖率的关系图；

图3为地图50*50环境示意图；

图4为地图50*50环境下三种算法路径对比；

图5为地图100*100环境示意图；

图6为地图100*100环境下三种算法路径对比；

图7为地图500*500环境示意图；

图8为地图500*500环境下三种算法路径对比。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，参照附图，对本发明进一步详细说明。

S1、路径规划的第一步是环境建模，本文采用栅格地图的方式对地图信息进行建模，假设机器人工作环境为一个封闭的二维空间，设S为栅格序列，n为栅格阶数，从左往右从上至下进行序列增加，序列总数为n2，那么机器人在栅格地图中的实际位置和地图坐标的对应关系如下：

其中mod代表取余运算，ceil代表求整运算，根据上式就可以得到机器人在当前格栅地图中的真实坐标，将地图信息存放在二维矩阵G(i,j)中，矩阵可以表示如下：

至此，环境建模完成。

S2、D*算法是路径规划中流行且高效的算法，通过计算占用网格图来找到一条最佳路径，D*算法在实际应用中有着诸多优点，对于栅格地图，需要采用一种计算机制来对两个栅格的距离进行描述，对于二维空间中的任意两点和，常用度量标准一般分为：欧几里德距离、曼哈顿距离和切比雪夫距离，对应如下公式：

D_Manhattan＝|x₂-x₁|+|y₂-y₁|

D_Chebyshev＝max(|x₂-x₁|,|y₂-y₁|)

由于不涉及到平方项处理，通常来说，曼哈顿距离和切比雪夫距离比欧式距离处理速度快；

D*算法采用如下估价函数来进行计算：

f(n)＝g(n)+h(n)

其中g(n)是基本项，代表终点(Goal)至当前节点的实际成本，为h(n)启发项，代表当前节点到起点(Start)的最小成本估计值，n为当前节点，D*算法采用欧氏距离作为启发函数，首先需要对每个节点进行平方根运算，降低了处理速度，其次欧氏距离不考虑角度的限制，是二维或三维空问中任意两点的真实距离，而本文采用格栅法建立地图模型，对于地图中两个格栅之问的距离不能准确描述，总的来说，D*算法的搜索效率直接取决于估价函数的选取，可以根据实际环境选择不同的估价函数。

S3、传统D*算法在离线路径规划时采用类似等势线逐级扩展的方式，从目标点开始对地图节点进行遍历，导致算法搜索范围较大，尤其当搜索地图区域较大时，问题更为突出，考虑障碍物及目标点位置信息，引入关键节点概念对节点进行区分(关键节点为障碍物所确定的相关节点)，从初始点开始只对关键节点进行访问，缩小搜索空间，再结合目标点位置信息，引入导向函数以确定单次扩展时所需比较的路径节点，确定可行路径，提高路径规划速度；

一般D*算法在确定最优路径时使用欧几里得距离指标进行评价，忽略障碍物尤其是未知障碍物可能导致最终规划路径存在小范围区域内多次无效转弯的问题，针对该问题，可以引入平滑度函数对规划路线偏离理想路径的程度进行惩罚，用以控制最终规划路径的拐点数量，避免机器人在小范围区域内无效转弯；

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

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

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

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

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

式中：X_i为第i次路径扩展时的父节点，Y_i为第i次路径扩展时的待定子节点；

为父节点位置坐标；

为待定子节点位置坐标；横坐标x满足

α为可变参数，α＝0表示路径未穿过障碍物区域，α＝∞表示路径穿过障碍物区域。

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

h(X_i,Y_i)＝f(X_i,Y_i)+f(θ_i)

式中：f(X_i,Y_i)为父节点与待定子节点之间的距离成本，

f(θ_i)为引入的路径平滑度惩罚函数，θ_i为待定子节点与父节点之间形成的路径与理想路径之间的夹角(以下简称为路径夹角)；

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

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

平滑度函数的分段，考虑到环境的复杂程度会随着地图的增大而变化，对不同大小的环境应采取分段处理方式，结合实践建议分段方式如下：对于小尺寸地图(小于250*250)、中等尺寸地图(250*250～500*500)，由于在单次扩展时待定关键节点数较少，因此建议将f(θ_i)(0≤|θ|≤π/2)分为6段、f(θ_i)(π/2＜|θ|≤π)分为1段，对于大尺寸地图(大于500*500)，单次扩展时待定关键节点数较多，为使关键节点的区分度更高，建议将f(θ_i)(0≤|θ|≤π/2)分为8段、f(θ_i)(π/2＜|θ|≤π)为1段；

路径夹角取值范围为(0,2π)，根据第i次扩展得到的子节点的横坐标x_i、第i-1次扩展得到的子节点的横坐标x_i-1之间的大小关系，将分为0≤|θ|≤π/2、π/2＜|θ|≤π，当0≤|θ|≤π/2时，x_i≥x_i-1；当π/2＜|θ|≤π时，x_i＜x_i-1，路径出现倒退现象且|θ|越接近π，倒退现象越严重，导致路径长度增加，因此，为了避免路径出现倒退现象，在μ一定时根据经验取f(θ_i)＝50μ；

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

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

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

综上所述，μ的取值跟障碍物覆盖率有关，因此建议：当障碍物覆盖率小于10％时，μ＝4～6，本研究取5；当障碍物覆盖率为10％～40％时，μ＝1～3，本研究取2；当障碍物覆盖率为40％～80％时，μ≤1。

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

在算法过程中，通过导向函数p(x)在OPEN列表中筛选单次迭代须访问的关键节点，称为子节点t(Y_i)，位置为

对筛选出的关键节点按h(X_i,Y_i)排序，将h(X_i,Y_i)最小的关键节点放入CLOSED列表(最小h(X_i,Y_i)表示为h_min(X_i,Y_i))，并将此关键节点作为下一次扩展的起点即新的父节点，用t(X_i)表示，位置为

将单次扩展筛选出的路径成本大于h_min(X_i,Y_i)且满足

的关键节点从OPEN列表删除，放入DELETE列表，注意，在单次扩展时，保留具有最小h(X_i,Y_i)的关键节点，该操作定义了从点S到当前位置最小成本的条件；

进一步的，对第N(N∈1,2,...,n)次扩展筛选出的关键节点按h(X_i,Y_i)进行排序，判断具有最小h(X_i,Y_i)的关键节点(用h(X_i,Y_i)表示)与CLOSED列表中第i-2个(l为CLOSED列表中关键节点个数)关键节点的路径是否存在，若路径不存在，将t(Y_i)放入CLOSED列表，令t(Y_i)＝t(X)为下一次迭代起点；若路径存在，则将CLOSED列表中第i-1个关键节点删除，放入DELETE列表，令t(Y_i)＝t(X)为下一次迭代的起点，避免H(X,D)≤H(X,A)+h(A,D)(X、A、D为关键节点)情况的出现，保证路径的最优性，迭代通过反复删除OPEN列表中关键节点进行，搜索到目标点结束，形成初始路径的关键节点序列{X}；

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

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

表1不同环境下路径长度对比

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

表2不同环境下离线规划时间及在线移动时间对比

仿真实验分别从不同环境大小、不同障碍物覆盖率两方面进行，如图3所示为环境面积小且障碍物数目少的大小为50*50的简单环境，如图5所示为环境面积一般且障碍物数目较多的大小为100*100的一般环境，如图7所示为环境面积大且障碍物数目较多的大小为500*500的复杂环境，在此强调一下，100*100的地图是50*50的地图的4倍大，图4是三种算法在图3的环境中经行的路径规划对比，图6是三种算法在图5的环境中经行的路径规划对比，图8是三种算法在图7的环境中经行的路径规划对比；

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

表3不同环境下拐点数目对比

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

Claims (4)

1.一种基于有向D*算法的动态路径规划方法，其特征在于，所述方法具体包括以下步骤：

S1、环境建模；

S2、D*算法原理；

S3、D*算法存在的问题；

S4、改进D*算法的搜索方式和关键节点筛选；

S5、引入平滑度函数对规划路径偏离理想路径的程度进行惩罚；

S6、有向D*算法流程；

S7、算法对比仿真。

2.根据权利要求书1所述的基于有向D*算法的动态路径规划方法，其特征在于，步骤S4具体如下：

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

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

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

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

式中：X_i为第i次路径扩展时的父节点，Y_i为第i次路径扩展时的待定子节点；

为父节点位置坐标；

为待定子节点位置坐标；横坐标x满足

α为可变参数，α＝0表示路径未穿过障碍物区域，α＝∞表示路径穿过障碍物区域。

3.根据权利要求书1所述的基于有向D*算法的动态路径规划方法，其特征在于，步骤S5具体如下：

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

h(X_i,Y_i)＝f(X_i,Y_i)+f(θ_i)

式中：f(X_i,Y_i)为父节点与待定子节点之间的距离成本，

f(θ_i)为引入的路径平滑度惩罚函数，θ_i为待定子节点与父节点之间形成的路径与理想路径之间的夹角(以下简称为路径夹角)；

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

式中：μ为环境因子，描述不同环境下已知障碍物对环境的影响信息，随着障碍物所占环境地图面积比率的不同而有所不同。

4.根据权利要求书1所述的基于有向D*算法的动态路径规划方法，其特征在于，步骤S6具体如下：

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

在算法过程中，通过导向函数p(x)在OPEN列表中筛选单次迭代须访问的关键节点，称为子节点t(Y_i)，位置为

对筛选出的关键节点按h(X_i,Y_i)排序，将h(X_i,Y_i)最小的关键节点放入CLOSED列表(最小h(X_i,Y_i)表示为h_min(X_i,Y_i))，并将此关键节点作为下一次扩展的起点即新的父节点，用t(X_i)表示，位置为

将单次扩展筛选出的路径成本大于h_min(X_i,Y_i)且满足

的关键节点从OPEN列表删除，放入DELETE列表，注意，在单次扩展时，保留具有最小h(X_i,Y_i)的关键节点，该操作定义了从点S到当前位置最小成本的条件；

进一步的，对第N(N∈1,2,...,n)次扩展筛选出的关键节点按h(X_i,Y_i)进行排序，判断具有最小h(X_i,Y_i)的关键节点(用t(Y_i)表示)与CLOSED列表中第i-2个(l为CLOSED列表中关键节点个数)关键节点的路径是否存在，若路径不存在，将t(Y_i)放入CLOSED列表，令t(Y_i)＝t(X)作为下一次迭代的起点；若路径存在，则将CLOSED列表中第i-1个关键节点删除，放入DELETE列表，令t(Y_i)＝t(X)为下一次迭代的起点，避免H(X,D)≤H(X,A)+h(A,D)(X、A、D为关键节点)情况的出现，保证路径的最优性，迭代通过反复删除OPEN列表中关键节点进行，搜索到目标点结束，形成初始路径的关键节点序列{X}；

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

Images