基于双启发函数的机器人快速最优路径规划方法

1.本发明涉及机器人路径规划技术领域，特别是基于双启发函数的机器人快速最优路径规划方法。


背景技术：

2.路径规划技术是机器人研究领域中的一个重要分支。所谓机器人的路径规划问题，就是在其工作空间中搜寻一条从起始状态到目标状态的能避开障碍物的路径。根据对环境信息的把握程度可把路径规划划分为基于先验完全信息的全局路径规划和基于传感器信息的局部路径规划。而规划的方式可以是随机的，也可以依据某种最优准则(如工作代价最小，行走路线最短，行走时间最短等)，在工作空间中寻找一条从起始状态到目标状态的避开障碍物的最优路径。显然，避开障碍是路径规划中的一个关键问题，同时，随着机器人运动速度的不断提高以及多机协同运动日益复杂，规划速度也成了衡量规划算法优劣的一个重要指标。
3.现有技术中，常用的路径规划方法有快速随机搜索树法、人工势场法、蚁群算法、基于栅格地图的a*算法等。其中：
4.快速随机搜索树法采用随机路径搜寻方式，搜索覆盖范围大，得到的不是最优路径，且路径比较曲折，通常需要对路径进行平滑操作。
5.人工势场法是一种虚拟力法，它模仿了引力斥力下的物体运动。规划出来的路径平滑安全、描述简单，但是存在局部最优的问题，引力场的设计复杂，是算法能否成功应用的关键。
6.蚁群算法的思想来自于对蚁群觅食行为的探索，算法通过迭代来模拟蚁群觅食的行为达到目的，有良好的全局优化能力、易于用计算机实现，但计算量大、易陷入局部最优解。
7.基于栅格地图的a*算法用编码的栅格来表示地图，把包含障碍物的栅格标记为障碍栅格，反之则为自由栅格。该方法在启发函数指导下可获得最优路径，算法易于用计算机实现，但路径受障碍物影响大，导致规划时间过长。


技术实现要素：

8.针对上述问题，本发明旨在提供一种计算量小、规划时间少且易于实现的最优路径规划方法。
9.本发明的目的采用以下技术方案来实现：
10.第一方面，本发明提出基于双启发函数的机器人快速最优路径规划方法，包括：
11.s1定义启发函数，其中启发函数包括最优路径启发函数s(p,ps))和目标指引启发函数g(p,pg)，其中最优路径启发函数s(p,ps))表示从起始结点到当前结点的最小代价，用于建立相邻节点间的父子关系；目标指引启发函数g(p,pg)表示从当前节点到目标节点的启发式评估代价，用于引导路径向目标方向扩展；其中ps表示栅格地图中起始节点坐标，pg表示目标节点坐标；p表示当前节点坐标；
12.s2初始化开集合和闭集合，其中开集合用于保存待检测节点，闭集合用于保存已经搜索过的节点；将起始节点加入开集合，且指定起始节点的父节点为一个虚拟节点，其父节点的目标指引启发函数值为无限大；
13.s3从开集合中选取一个目标指引启发函数值g(p,pg)最小的节点，判断该节点是否为目标点，若是，则跳转至步骤s11；否则进一步判断该节点g(p,pg)值是否比其父节点小，若是，则跳转至步骤s4，执行八邻域引导式搜索；否则，剔除开集合中不与障碍物直接相邻的节点，并跳转至步骤s8，执行四邻域障碍绕行搜索；
14.s4对当前节点的八邻域空间进行搜索，若某个相邻节点不为障碍物且不在闭集合内，则计算相邻节点的最优路径启发函数值和目标指引启发函数值；
15.s5根据最优路径启发函数值更新当前节点与相邻节点的父子关系；若某个相邻节点还未建立父子关系，则该相邻节点的父节点为当前节点，并把该相邻节点加入开集合；若某个相邻节点已经在开集和内，即已经与其它节点建立了父子关系，则保留最优路径启发函数值较小的父子关系；
16.s6将当前节点加入到闭集合中，并返回步骤s3；
17.s7从开集合中选取一个目标指引启发函数值g(p,pg)最小的节点，判断该节点是否为目标点，若是，则跳转至步骤s11；否则进一步判断该节点的g(p,pg)值是否比其父节点小，若则清空开集合，并跳转至步骤s4，执行八邻域引导式搜索；否则，跳转至步骤s8，执行四邻域障碍绕行搜索；
18.s8对当前节点的四邻域空间进行搜索，若某个相邻节点与障碍物直接或间接相邻，则计算该相邻节点的最优路径启发函数值和目标指引启发函数值；
19.s9根据最优路径启发函数值更新当前节点与相邻节点的父子关系，若某个相邻节点还未建立父子关系，则该相邻节点的父节点为当前节点，并把该相邻节点加入开集合；若某个相邻节点已经在开集和内，即该相邻节点已经与其它节点建立了父子关系，则保留最优路径启发函数值较小的父子关系；
20.s10将当前节点加入到闭集合中，并返回步骤s7；
21.s11从目标节点开始，根据节点间的父子关系回溯到起始节点，得到初步路径；
22.s12获取初步路径中的所有拐点；
23.s13沿着初步路径方向，遍历所有拐点：对于当前遍历的拐点，找到与该拐点距离最近的非相邻拐点重新规划这两个拐点之间的路径，若两拐点间的新路径比原路径短，则用新路径代替这两拐点间的原路径；
24.s14重复第十三步，直到路径长度不再变化，得到最优路径。
25.优选的，栅格地图包括二维栅格地图或三维栅格地图。
26.优选的，步骤s1中，最优路径启发函数s(p,ps))的计算方式为：
[0027][0028]
式中表示当前节点的相邻节点；
[0029]
目标指引启发函数g(p,pg)的计算方式为：
[0030]
g(p,pg)＝(r-rg)2 (c-cg)2[0031]
式中，(r,c)表示当前节点坐标p(r,c)；(rg,cg)表示目标节点坐标pg(rg,cg)。
[0032]
优选的，步骤s4中，若相邻节点为障碍物或已经在闭集合内，则不对它进行任何处理。
[0033]
优选的，步骤s8中，若相邻节点不与障碍物直接或间接相邻，则不对它进行任何处理。
[0034]
优选的，步骤s11中，节点间的父子关系为由父节点指向子节点。
[0035]
优选的，步骤s12中，当初步路径中相邻的三个节点不在同一直线上，则称三个节点中间位置的节点为拐点，并把起始节点以及目标节点也当作拐点。
[0036]
优选的，步骤s13中，重新规划两个拐点之间的路径时，重新规划的搜索距离不超过这两个拐点在初步路径中的距离。
[0037]
第二方面，本发明提出基于双启发函数的机器人快速最优路径规划装置，该装置用于实现如上述第一方面中任一种实施方式所述的基于双启发函数的机器人快速最优路径规划方法。
[0038]
本发明的有益效果为：
[0039]
1、引入最优路径启发函数，使规划的路径更短，引入目标指引启发函数，可以引导规划进程向目标节点扩展，减少无用的路径尝试。两个单独的启发函数不存在量化单位相互约束的问题，每个启发函数都可以使用最准确的代价计算方式和计算度量，计算量小，启发效果更理想。
[0040]
2、通过父、子节点的目标指引启发函数值判断当前搜索进程是否遇到了大范围障碍物。当子节点的目标指引启发函数值比父节点大，说明遇到了大范围障碍物，需要绕行。反之，则说明未遇到大范围障碍物或已经绕过障碍物，有助于提高最有路径规划的准确性。
[0041]
3、同时采用了八邻域引导式搜索、四邻域障碍绕行搜索两种搜索方式。八邻域引导式搜索可以最快的速度、最短的路径搜索目标节点，当遇到大范围障碍物时，采用四邻域障碍绕行搜索，只处理与障碍物直接相邻的节点，可以快速绕过大范围障碍物，有效地减小搜索范围。
[0042]
4、提取初步路径中的拐点信息，对某个拐点以及与它距离最近的非相邻拐点进行拐点间的路径重规划，可以以最小的代价优化路径，得到从起始节点到目标节点的最短路径。
附图说明
[0043]
利用附图对本发明作进一步说明，但附图中的实施例不构成对本发明的任何限制，对于本领域的普通技术人员，在不付出创造性劳动的前提下，还可以根据以下附图获得其它的附图。
[0044]
图1为本发明实施例所示的二维栅格地图示意图；
[0045]
图2为本发明基于双启发函数的机器人快速最优路径规划方法的流程示意图；
[0046]
图3为本发明实施例所示的相邻节点示意图；
[0047]
图4为本发明实施例所示的当前节点邻域示意图；
[0048]
图5为本发明实施例所示的初步路径示意图；
具体实施方式
[0049]
结合以下应用场景对本发明作进一步描述。
[0050]
本发明方案能够在二维或三维栅格地图中使用，为了方便描述，本实施例以一幅可能的二维栅格地图举例，来进一步说明本发明提出的基于双启发函数的机器人快速最优路径规划方法。
[0051]
其中，参见图1所示的距离二维栅格地图中，白色方格为自由空间，黑色方格为障碍物，白色箭头表示起始位置节点，黑色旗子为目标位置节点，黑色锚点为当前位置节点。其中栅格地图以行列作为坐标，坐标值均为正整数，则起始节点坐标为ps(rs,cs)，目标节点坐标为pg(rg,cg)，当前节点坐标为p(r,c)，其中r和c是栅格地图的坐标，可定义坐标原点为地图的左上角，r表示行坐标，c表示列坐标。
[0052]
参见图2，其示出本实施例基于双启发函数的机器人快速最优路径规划方法的流程示意图，其中，为了避免进行过多的无用路径搜索，本实施例制定了两种路径搜索方式，八邻域引导式搜索可以最快的速度、最短的路径搜索目标节点，当遇到大范围障碍物时，采用四邻域障碍绕行搜索，可以快速绕过大范围障碍物，有效地减小搜索范围。为了解决最速与最优问题，本实施例引入了两个启发函数，最优路径启发函数用以保证能找到一条较短路径，目标指引启发函数用以引导路径向目标方向扩展，减少无用的路径尝试。在得到初步路径后，提取路径的拐点信息，进行拐点间路径重规划，以最小的代价优化路径，得到从起始节点到目标节点的最短路径。具体步骤如下：
[0053]
第一步，定义启发函数。本实施例使用两个启发函数对路径规划进程进行引导，包括：最优路径启发函数s(p,ps))、目标指引启发函数g(p,pg)。最优路径启发函数s(p,ps))表示从起始结点到当前结点的最小代价，用以建立相邻节点间的父子关系，保证能找到一条较短路径。例如，参见图3，当前节点的相邻节点为如图3的灰色方格，显然，当前节点到达直接相邻节点的代价比间接相邻节点低，则最优路径启发函数s(p,ps))的计算方式如下：
[0054][0055]
目标指引启发函数g(p,pg)表示从当前节点到目标节点的启发式评估代价，用以
[0056]
引导路径向目标方向扩展，减少无用的路径尝试。它的定义如下：
[0057]
g(p,pg)＝(r-rg)2 (c-cg)2[0058]
第二步，初始化开集合、闭集合，开集合用以保存待检测节点，闭集合用来保存已经搜索过的节点，避免重复搜索。将起始节点加入开集合，且指定起始节点的父节点为一个虚拟节点，其父节点的目标指引启发函数值为无限大(例如，在编程时，可以使用数据类型的最大值表示)。
[0059]
第三步，从开集合中选取一个目标指引启发函数值g(p,pg)最小的节点，判断该节点是否为目标点，若是，则从第十一步开始。否则进一步判断该节点g(p,pg)值是否比其父节点小，若是，则从第四步开始，执行八邻域引导式搜索。否则，说明当前规划进程遇到了大范围障碍物，需要绕行以避免进行无用搜索，先剔除开集合中不与障碍物直接相邻的节点，并从第八步开始，执行四邻域障碍绕行搜索。
[0060]
第四步，对当前节点的八邻域空间(如图4(a)所示灰色区域)进行搜索，若某个相邻节点为障碍物或已经在闭集合内(即已经被搜索过了)，则不对它进行任何处理。否则，计
算相邻节点的最优路径启发函数值、目标指引启发函数值。
[0061]
第五步，根据最优路径启发函数值更新当前节点与相邻节点的父子关系。若某个相邻节点还未建立父子关系，则该相邻节点的父节点为当前节点，并把该相邻节点加入开集合；若某个相邻节点已经在开集和内，即已经与其它节点建立了父子关系，则此时保留最优路径启发函数值较小的父子关系。
[0062]
第六步，将当前节点加入到闭集合中，并返回第三步。
[0063]
第七步，从开集合中选取一个目标指引启发函数值g(p,pg)最小的节点，判断该节点是否为目标点，若是，则从第十一步开始。否则进一步判断该节点的g(p,pg)值是否比其父节点小，若是，说明当前规划进程已经绕过障碍物，这时，清空开集合，并从第四步开始，执行八邻域引导式搜索。否则，从第八步开始，执行四邻域障碍绕行搜索。
[0064]
第八步，对当前节点的四邻域空间(如图4(b)所示灰色区域)进行搜索，若某个相邻节点与障碍物直接或间接相邻，则计算该相邻节点的最优路径启发函数值、目标指引启发函数值。否则，不对它进行任何处理。
[0065]
第九步，根据最优路径启发函数值更新当前节点与相邻节点的父子关系。若某个相邻节点还未建立父子关系，则该相邻节点的父节点为当前节点，并把该相邻节点加入开集合；若某个相邻节点已经在开集和内，即已经与其它节点建立了父子关系，则此时保留最优路径启发函数值较小的父子关系。
[0066]
第十步，将当前节点加入到闭集合中，并返回第七步。
[0067]
第十一步，如图5所示，黑色箭头表示节点间的父子关系，从父节点指向子节点。据此，从目标节点开始，根据节点间的父子关系回溯到起始节点，得到初步路径。
[0068]
第十二步，找出初步路径中的所有拐点。如图5黑色圆点所示，当初步路径中相邻的三个节点不在同一直线上，则称三个节点中间位置的节点为拐点，并把起始节点以及目标节点也当作拐点。
[0069]
第十三步，沿着初步路径方向，遍历所有拐点，对于某个拐点，找到与它距离最近的非相邻拐点(如图5所示，对于起始节点，与它相邻的拐点为虚线圆框所示拐点，距离它最近的非相邻拐点为虚线方框所示拐点)。重新规划这两个拐点之间的路径，若两拐点间的新路径比原路径短，则用新路径代替这两拐点间的原路径；否则，继续处理下一拐点。规定重新规划的搜索距离不能超过这两个拐点在初步路径中的距离，因为重新规划的目的是寻找更优的路径，若搜索长度已经超过原路径，则没必要继续搜索了，这可以有效地缩小搜索范围，节省规划时间。
[0070]
第十四步，重复第十三步，直到路径长度不再变化，即得到了最优路径。
[0071]
本发明上述实施方式，
[0072]
第一，引入最优路径启发函数，使规划的路径更短，引入目标指引启发函数，可以引导规划进程向目标节点扩展，减少无用的路径尝试。两个单独的启发函数不存在量化单位相互约束的问题，每个启发函数都可以使用最准确的代价计算方式和计算度量，启发效果更理想。
[0073]
第二，通过父、子节点的目标指引启发函数值判断当前搜索进程是否遇到了大范围障碍物。当子节点的目标指引启发函数值比父节点大，说明遇到了大范围障碍物，需要绕行。反之，则说明未遇到大范围障碍物或已经绕过障碍物。
[0074]
第三，同时采用了八邻域引导式搜索、四邻域障碍绕行搜索两种搜索方式。八邻域引导式搜索可以最快的速度、最短的路径搜索目标节点，当遇到大范围障碍物时，采用四邻域障碍绕行搜索，只处理与障碍物直接相邻的节点，可以快速绕过大范围障碍物，有效地减小搜索范围。
[0075]
第四，提取初步路径中的拐点信息，对某个拐点以及与它距离最近的非相邻拐点进行拐点间的路径重规划，可以以最小的代价优化路径，得到从起始节点到目标节点的最短路径。
[0076]
同时，与上述提出的基于双启发函数的机器人快速最优路径规划方法相对应，本发明实施例还提出基于双启发函数的机器人快速最优路径规划装置，其中该装置用于实现所示的基于双启发函数的机器人快速最优路径规划方法以及各方法步骤对应的具体实施例，本发明在此不重复叙述。
[0077]
需要说明的是，在本发明各个实施例中的各功能单元/模块可以集成在一个处理单元/模块中，也可以是各个单元/模块单独物理存在，也可以是两个或两个以上单元/模块集成在一个单元/模块中。上述集成的单元/模块既可以采用硬件的形式实现，也可以采用软件功能单元/模块的形式实现。
[0078]
通过以上的实施方式的描述，所属领域的技术人员可以清楚地了解应当理解，可以以硬件、软件、固件、中间件、代码或其任何恰当组合来实现这里描述的实施例。对于硬件实现，处理器可以在一个或多个下列单元中实现：专用集成电路(asic)、数字信号处理器(dsp)、数字信号处理设备(dspd)、可编程逻辑器件(pld)、现场可编程门阵列(fpga)、处理器、控制器、微控制器、微处理器、设计用于实现这里所描述功能的其他电子单元或其组合。对于软件实现，实施例的部分或全部流程可以通过计算机程序来指令相关的硬件来完成。实现时，可以将上述程序存储在计算机可读介质中或作为计算机可读介质上的一个或多个指令或代码进行传输。计算机可读介质包括计算机存储介质和通信介质，其中通信介质包括便于从一个地方向另一个地方传送计算机程序的任何介质。存储介质可以是计算机能够存取的任何可用介质。计算机可读介质可以包括但不限于ram、rom、eeprom、cd-rom或其他光盘存储、磁盘存储介质或者其他磁存储设备、或者能够用于携带或存储具有指令或数据结构形式的期望的程序代码并能够由计算机存取的任何其他介质。
[0079]
最后应当说明的是，以上实施例仅用以说明本发明的技术方案，而非对本发明保护范围的限制，尽管参照较佳实施例对本发明作了详细地说明，本领域的普通技术人员应当分析，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的实质和范围。