用于移动机器人的路径规划方法、装置及计算机存储介质

1.本发明涉及机器人技术领域，尤其涉及一种用于移动机器人的路径规划方法、装置及计算机存储介质。


背景技术：

2.由于移动机器人具有自主运行、移动灵活等特点，所以在国防科技、生活服务、生产建设等重要领域具有广阔的开发利用前景。随着移动机器人不断被普及应用，移动机器人的路径规划算法成为移动机器人的关键技术之一。
3.随着移动机器人的应用领域越来越广泛，传统单一的a*、rrt*等路径规划算法存在寻找最优路径的效果不佳、路径搜索速度慢、路径不平滑的技术问题，已难以适用于复杂多变的环境。另外，基于智能算法的路径规划算法虽然寻优效果有所提升，但效率较低，极易陷入局部最优。


技术实现要素：

4.本发明所要解决的技术问题在于提供一种用于移动机器人的路径规划方法、装置及计算机存储介质，解决传统路径规划方法存在的路径搜索速度慢、路径不平滑的技术问题，提高搜索效率。
5.为解决上述技术问题，本发明采用如下所述的技术方案：
6.一种用于移动机器人的路径规划方法，其包括有如下步骤：获取移动机器人的初始位置、目标位置、位于初始位置与目标位置之间的中间位置以及障碍物信息；以初始位置、中间位置分别作为根节点构建两棵搜索树t1、t3相向搜索，以目标位置、中间位置分别作为根节点构建两棵搜索树t2、t4相向搜索；基于预设的节点拓展策略为四棵搜索树拓展新节点；重复前一步骤，直至搜索树t1与搜索树t3相连且搜索树t2与搜索树t4相连，生成从初始位置到目标位置的路径；通过梯度下降优化策略对前一步骤规划所得的路径进行平滑处理，获得适合移动机器人的最佳运动路径。
7.一种用于移动机器人的路径规划装置，其包括有数据获取模块、搜索树构建模块、搜索树扩展模块、路径生成模块和路径平滑模块，数据获取模块用于获取移动机器人的初始位置、目标位置、位于初始位置与目标位置之间的中间位置以及障碍物信息；搜索树构建模块用于以初始位置、中间位置分别作为根节点构建两棵搜索树t1、t3，以及用于以目标位置、中间位置分别作为根节点构建两棵搜索树t2、t4，其中，搜索树t1、t3相向搜索，搜索树t2、t4相向搜索；搜索树扩展模块用于基于预设的节点拓展策略为四棵搜索树拓展新节点；路径生成模块用于当搜索树t1与搜索树t3相连且搜索树t2与搜索树t4相连时，生成从初始位置到目标位置的路径；路径平滑模块用于通过梯度下降优化策略对路径生成模块生成的路径进行平滑处理，获得适合移动机器人的最佳运动路径。
8.一种计算机存储介质，其内部存储有程序指令，所述程序指令被处理器执行时实现上述的用于移动机器人的路径规划方法。
9.本发明的有益技术效果在于：上述的用于移动机器人的路径规划方法，采用了两处双向采样的策略，生成四棵搜索树，利用四棵搜索树同时搜索，缩短了路径规划时间，提高了路径规划效率；此外，采用梯度下降的优化策略对生成的路径进行平滑优化处理，使路径更符合移动机器人的动力模型，进而使机器人运动更平稳。
附图说明
10.图1为本发明的用于移动机器人的路径规划方法的流程示意图；
11.图2为本发明的基于预设的节点拓展策略为四棵搜索树拓展新节点步骤的流程示意图；
12.图3为本发明的用于移动机器人的路径规划装置的结构示意图；
13.图4为本发明的搜索树扩展模块的结构示意图。
具体实施方式
14.为使本领域的普通技术人员更加清楚地理解本发明的目的、技术方案和优点，以下结合附图和实施例对本发明做进一步的阐述。
15.如图1所示，在本发明一个实施例中，用于移动机器人的路径规划方法包括有如下步骤：
16.s110、获取移动机器人的初始位置、目标位置、位于初始位置与目标位置之间的中间位置以及障碍物信息。
17.s120、以初始位置、中间位置分别作为根节点构建两棵搜索树t1、t3相向搜索，以目标位置、中间位置分别作为根节点构建两棵搜索树t2、t4相向搜索。
18.s130、基于预设的节点拓展策略为四棵搜索树拓展新节点。
19.s140、判断是否搜索树t1与搜索树t3相连且搜索树t2与搜索树t4相连，若是，执行步骤s150，若否，返回步骤s130。
20.s150、生成从初始位置到目标位置的路径。
21.s160、通过梯度下降优化策略对步骤s150规划所得的路径进行平滑处理，获得适合移动机器人的最佳运动路径。
22.所述步骤s110包括：通过移动机器人的激光雷达传感器采集移动机器人工作环境信息；建立栅格地图；基于移动机器人工作环境信息，在栅格地图上确定移动机器人的初始位置x
start
、目标位置x
goal
、中间位置x
mid
以及障碍物信息。
23.其中，所述中间位置x
mid
位于初始位置x
start
和目标位置x
goal
之间，且中间位置x
mid
到初始位置x
start
的距离和中间位置x
mid
到初始位置x
start
的距离相等。
24.若初始位置x
start
和目标位置x
goal
的连线中点处无障碍物，则取初始位置x
start
和目标位置x
goal
的连线中点为所述中间位置x
mid
；若初始位置x
start
和目标位置x
goal
的连线中点处存在障碍物，则将初始位置x
start
和目标位置x
goal
的连线中点沿与初始位置x
start
和目标位置x
goal
的连线垂直的方向平移若干个单位距离后作为所述中间位置x
mid
。在平移初始位置x
start
和目标位置x
goal
的连线中点时，可以沿初始位置x
start
和目标位置x
goal
的连线垂直的方向朝初始位置x
start
和目标位置x
goal
的连线的任意一侧平移，直至离开障碍物。
25.在一些优选实施例中，将初始位置x
start
和目标位置x
goal
的连线中点朝初始位置
x
start
和目标位置x
goal
的连线的第一侧平移至离开障碍物，需要平移a个单位距离，将初始位置x
start
和目标位置x
goal
的连线中点朝初始位置x
start
和目标位置x
goal
的连线的第二侧平移至离开障碍物，需要平移b个单位距离。若a≥b，则将初始位置x
start
和目标位置x
goal
的连线中点沿与初始位置x
start
和目标位置x
goal
的连线垂直的方向朝初始位置x
start
和目标位置x
goal
的连线的第二侧平移b个单位距离后作为所述中间位置x
mid
；若a＜b，则将初始位置x
start
和目标位置x
goal
的连线中点沿与初始位置x
start
和目标位置x
goal
的连线垂直的方向朝初始位置x
start
和目标位置x
goal
的连线的第一侧平移a个单位距离后作为所述中间位置x
mid
。
26.所述步骤s120包括：初始化四棵搜索树t1、t2、t3、t4，并设初始路径信息列表l为空，其中，初始位置x
start
、目标位置x
goal
分别作为两棵搜索树t1、t2的根节点，中间位置x
mid
同时作为两棵搜索树t3、t4的根节点。
27.当搜索树t1从初始位置x
start
向中间位置x
mid
搜索时，搜索树t2也从目标位置x
goal
向中间位置x
mid
搜索；同时，搜索树t3从中间位置x
mid
向初始位置x
start
搜索，搜索树t4从中间位置x
mid
向目标位置x
goal
搜索，四棵搜索树同时搜索，缩短了路径规划时间，提高了路径规划效率。
28.如图2所示，所述步骤s130进一步包括如下步骤：
29.s131、对障碍物进行膨胀化处理，基于膨胀后的障碍物在地图上记出障碍区域和无障碍区域；
30.s132、在障碍物上施加一个斥力场对随机采样点进行约束，在无障碍区域内生成随机采样点x
rand
；
31.s133、在搜索树的区域范围内寻找与随机采样点距离最近的最近节点x
nearest
；
32.s134、判断随机采样点x
rand
和最近节点x
nearest
的连线是否与障碍区域接触，若否，则执行步骤s135，若是，则返回步骤s132；
33.s135、在随机采样点x
rand
和最近节点x
nearest
连线上选取一个节点作为新节点x
new
插入搜索树；
34.s136、基于预设的重连策略对新节点x
new
及新节点x
new
邻域范围内的所有节点xi进行重连。
35.在所述步骤s131中，通过设置膨胀系数k对障碍物进行膨胀化处理，并将膨胀后的障碍物在格栅地图上所处的位置标记为障碍区域，格栅地图上无障碍物的位置标记为无障碍区域。其中，所述膨胀系数k为膨胀后障碍物边和原障碍物边之间的欧式距离，k值可根据移动机器人的尺寸进行调整。本步骤对障碍物进行了膨胀化处理，搜索树在障碍物放大后的环境中拓展新节点，避免了生成的路径紧贴障碍物。
36.在所述步骤s132中，在障碍物上施加一个斥力场对随机采样点进行约束，从而在无障碍区域内生成四棵搜索树各自的随机采样点x
rand
。障碍物对随机采样点x
rand
的斥力函数可以表示为：
[0037][0038]
其中，m表示障碍物的斥力增益系数，xi表示随机采样点的坐标，xo表示障碍物的坐
标，||x
i-xo||表示随机采样点到障碍物的欧式距离，ρ0为距离阈值，表示障碍物产生斥力作用的最大影响距离。本步骤利用障碍物产生的斥力场对采样点的生成进行约束，使采样点的生成具有方向性，使得采样点远离障碍物，在无障碍区域内生成随机采样点x
rand
。
[0039]
由于步骤s130中，四棵搜索树同时拓展各自的新节点。因此，在所述步骤s133中，分别在搜索树t1、t2、t3、t4的区域范围内寻找与对应随机采样点距离最近的节点x
nearest
。换言之，每一棵搜索树拓展新节点时，都在无障碍区域内生成对应的随机采样点x
rand
，每一棵搜索树的区域范围内都存在与其随机采样点x
rand
距离最近的节点x
nearest
。
[0040]
在所述步骤s136中，采用如下方法对搜索树插入的新节点x
new
及新节点x
new
邻域范围内的所有节点xi进行重连。
[0041]
遍历新节点x
new
邻域范围内的所有节点xi，令代价值之和最小的节点xi为最小代价值节点x
min
，所述代价值之和表示为c||x
new-xi|| c||x
i-x
base
||，其中，||x
new-xi||表示节点xi到新节点x
new
的欧式距离，||x
i-x
base
||表示节点xi到搜索树根节点的欧式距离。x
base
表示搜索树根节点的坐标，例如，在搜索树t1中，x
base
＝x
start
；在搜索树t2中，x
base
＝x
goal
；在搜索树t3、t4中，x
base
＝x
mid
。
[0042]
判断最小代价值节点x
min
与新节点x
new
的连线与障碍区域是否发生碰撞，若否，则将新节点x
new
与其原父节点断开连接，以最小代价值节点x
min
作为新节点x
new
的父节点进行重连，若是，保持新节点x
new
原有的连接关系。
[0043]
对于新节点x
new
邻域范围内除最小代价值节点x
min
外的所有节点xi，若c||x
new-x
base
|| c||x
i-x
new
||＜c||x
i-x
base
||，且节点xi与新节点x
new
的连线与障碍区域没有发生碰撞，则将节点xi与其原先父节点断开连接，使新节点x
new
成为节点xi新的父节点进行重连，而节点xi的子代节点的连接进行递归改变，其中，||x
new-x
base
||表示新节点x
new
到搜索树根节点的欧式距离，||x
i-x
new
||表示节点xi到新节点x
new
的欧式距离；若c||x
new-x
base
|| c||x
i-x
new
||≥c||x
i-x
base
||，或节点xi与新节点x
new
的连线与障碍区域发生碰撞，保持节点xi原有的连接关系。
[0044]
在步骤s140中，当搜索树t1扩展到搜索树t3的一个节点时，或者搜索树t3扩展到搜索树t1的一个节点，则判定搜索树t1、与搜索树t3相连；当搜索树t2扩展到搜索树t4的一个节点时，或者搜索树t4扩展到搜索树t2的一个节点时，则判定搜索树t2与搜索树t4相连；当搜索树t1、与搜索树t3相连且搜索树t2与搜索树t4相连时，执行步骤s150，若否，返回步骤s130。
[0045]
所述步骤s160进一步包括有步骤：
[0046]
将步骤s40规划所得的路径看成曲线，用函数s'(n)表示并对函数s(n)求导，
[0047]
s(n)＝ω0x0 ω1x1 ω2x2 ... ωnxnꢀꢀꢀꢀꢀ
(1)
[0048][0049]
其中，xn为处理数据中的特征值，ωn为权重系数，h
ω
(x)为函数s(n)求导后的表达式；
[0050]
用均方差表示函数s(n)和函数s(n)求导后的误差，
[0051]
[0052]
其中，g(ω)为均方损失差，包含了所有样本数据，m表示样本数量；
[0053]
建立微分迭代公式，
[0054][0055]
其中，ω表示权重值，l表示ω的取值范围宽度，β表示微分步长；
[0056]
对微分迭代公式(4)求解，通过迭代使每次的损失最小，使得目标函数|s'(n)-s(n)|取得最小值，从而达到对路径进行平滑优化的效果。
[0057]
本发明的用于移动机器人的路径规划方法，利用四棵搜索树同时搜索，缩短了路径规划时间，提高了路径规划效率；在搜索树拓展节点时，利用障碍物产生的斥力场对采样点的生成进行约束，使采样点的生成具有方向性，使得采样点远离障碍物，同时对障碍物进行了膨胀化处理，避免了生成的路径紧贴障碍物；采用梯度下降的优化策略对生成的路径进行平滑优化处理，从而获得适合移动机器人的最佳运动路径。
[0058]
如图3所示，基于图1、图2所示用于移动机器人的路径规划方法，本发明提供了一种用于移动机器人的路径规划装置，其包括有数据获取模块110、搜索树构建模块120、搜索树扩展模块130、路径生成模块140和路径平滑模块150。
[0059]
数据获取模块110，用于获取移动机器人的初始位置、目标位置、位于初始位置与目标位置之间的中间位置以及障碍物信息，即用于执行图1所示实施例中的用于移动机器人的路径规划方法中的步骤s110。
[0060]
搜索树构建模块120，用于以初始位置、中间位置分别作为根节点构建两棵搜索树t1、t3，以及用于以目标位置、中间位置分别作为根节点构建两棵搜索树t2、t4，其中，搜索树t1、t3相向搜索，搜索树t2、t4相向搜索，即用于执行图1所示实施例中的用于移动机器人的路径规划方法中的步骤s120。
[0061]
搜索树扩展模块130，用于基于预设的节点拓展策略为四棵搜索树拓展新节点，即用于执行图1所示实施例中的用于移动机器人的路径规划方法中的步骤s130。
[0062]
路径生成模块140，用于当搜索树t1与搜索树t3相连且搜索树t2与搜索树t4相连时，生成从初始位置到目标位置的路径，即用于执行图1所示实施例中的用于移动机器人的路径规划方法中的步骤s140、s150。
[0063]
路径平滑模块150，用于通过梯度下降优化策略对路径生成模块生成的路径进行平滑处理，获得适合移动机器人的最佳运动路径，即用于执行图1所示实施例中的用于移动机器人的路径规划方法中的步骤s160。
[0064]
如图4所示，所述搜索树扩展模块130包括有障碍物膨胀单元131、采样点生成单元132、最近节点搜寻单元133、新节点插入单元134和节点重连单元135。
[0065]
障碍物膨胀单元131，用于对障碍物进行膨胀化处理，并基于膨胀后的障碍物在地图上标记出障碍区域和无障碍区域，即用于执行图2所示实施例中的用于移动机器人的路径规划方法中的步骤s131。
[0066]
采样点生成单元132，用于在障碍物上施加一个斥力场对随机采样点进行约束，在无障碍区域内生成随机采样点x
rand
，即用于执行图2所示实施例中的用于移动机器人的路径规划方法中的步骤s132。
[0067]
最近节点搜寻单元133，用于在搜索树的区域范围内寻找与随机采样点距离最近
的最近节点x
nearest
，即用于执行图2所示实施例中的用于移动机器人的路径规划方法中的步骤s133。
[0068]
新节点插入单元134，用于当随机采样点x
rand
和最近节点x
nearest
的连线与障碍区域无接触时，在随机采样点x
rand
和最近节点x
nearest
连线上选取一个节点作为新节点x
new
插入搜索树；即用于执行图2所示实施例中的用于移动机器人的路径规划方法中的步骤s134、s135。
[0069]
节点重连单元135，用于基于预设的重连策略对新节点x
new
及新节点x
new
邻域范围内的所有节点xi进行重连，即用于执行图2所示实施例中的用于移动机器人的路径规划方法中的步骤s136。
[0070]
本发明还提供了一种计算机存储介质，所述计算机存储介质内存储有程序指令，该程序指令被处理器执行时实现图1或图2所示实施例中的用于移动机器人的路径规划方法。
[0071]
以上所述仅为本发明的优选实施例，而非对本发明做任何形式上的限制。本领域的技术人员可在上述实施例的基础上施以各种等同的更改和改进，凡在权利要求范围内所做的等同变化或修饰，均应落入本发明的保护范围之内。