一种基于代价地图的记忆性存储障碍物方法

1.本发明涉及机器人路径规划领域，尤其是涉及一种基于代价地图的记忆性存储障碍物方法。


背景技术：

2.路径规划是在机器人运动过程中的重要环节，其旨在从起始点和终止点之间找到一条无碰撞的路径。近些年来，用于路径规划的导航算法越来越复杂，现有的一些算法，如：dijkstra算法、a*算法等已经能很好地应用在自主机器人上，可以在复杂环境中为机器人规划一条无碰撞路径。现如今导航算法的目标已从追求无碰路径的单一目标变成多约束整合的优化问题，如在寻找无碰路径的基础上，要求机器人规划的路径考虑行人的舒适度；在规划路径时注重于机器人车辆运动时人类乘客的舒适度。
3.以上这些路径规划算法的改进都是从算法角度进行优化，都是基于得到的地图为真实地图的前提下实现的，但是在实际情况下，由于机器人会通过传感器检测障碍物，并更新地图，若在机器人无法检测到障碍物时移开障碍物，而地图上的障碍物并不会删除，这就造成与实际地图的不一致，而地图的不准确会造成路径规划与期望不符，在实际规划中可能遇到如下情况：
4.情况1，机器人在行驶过程中发现前方存在障碍物并标记，而后行驶到看不见该障碍物处后，障碍物移动或消失了，此时若进行路径规划，由于地图上障碍物并没有随着实际中障碍物的移开而修改，在该地图下规划的路径与实际地图下规划的路径很可能不一致，即导致其规划的轨迹并非最优的轨迹。
5.情况2，当障碍物处于唯一的可规划轨迹上，则无法进行路径规划，机器人无法到达指定位置。
6.当机器人所处环境为动态变化快的环境时，这两种情况经常发生，这种规划会让人们感到迷惑和不自然，并且增加了到达目标点的代价。


技术实现要素：

7.本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于代价地图的记忆性存储障碍物方法。
8.本发明的目的可以通过以下技术方案来实现：
9.一种基于代价地图的记忆性存储障碍物方法，包括以下步骤：
10.1)在移动机器人行驶过程中，将传感器检测到的障碍物数据经过处理加入到代价地图中障碍物层中的二维栅格地图内；
11.2)分别构建一个与栅格地图相同规模的时间地图和次数地图，用以分别记录栅格地图相应位置的更新时间以及障碍物的删除次数；
12.3)根据传感器检测区域和时间地图确定障碍物层的更新区域；
13.4)在更新区域内更新栅格地图，并根据设定的衰减函数更新对应的代价值；
14.5)在代价地图的膨胀层中基于代价值对记忆性存储的障碍物进行膨胀。
15.所述的步骤1)中，移动机器人通过激光雷达作为传感器检测行驶过程中的障碍物。
16.所述的步骤2)中，时间地图通过数组储存相应栅格地图中加入障碍物的时间，其初始值为开始运行时间。
17.所述的步骤2)中，次数地图通过数组储存相应栅格地图中障碍物的删除次数，其初始值为0，当移动机器人首次检测到障碍物时，次数地图数组存储的删除次数为初始值，当障碍物的删除次数达到设定上限时，则认定为固定障碍物。
18.所述的步骤3)中，将移动机器人激光雷达扫描到的栅格以及在时间地图中时间不为初始值且存在时间超过设定的维持时间的栅格共同加入更新区域内。
19.对于更新区域内存在激光雷达扫描到的新增障碍物和超时障碍物，根据时间地图搜索到障碍物的存在时间超过设定的维持时间的栅格，同时根据次数地图搜索到障碍物的删除次数未达设定上限的栅格，并更新对应栅格的代价值，当更新后栅格的代价值c
′
为零时，次数地图相应位置的障碍物的删除次数加一。
20.对于首次检测到的新增障碍物，在首次检测后，新增障碍物对应栅格的代价值下降公式为：
[0021][0022]
δt＝t
now-t
re-t
[0023]
其中，c为栅格的初始代价值，c
′
为更新后栅格的代价值，且代价值为整数，t
now
为当前时间，t
re
为存储障碍物的更新时间，ts为初始值，t为设定的维持时间。
[0024]
当在障碍物的删除次数加一后，通过衰减公式降低代价值的下降速度，当删除次数达到设定的上限时，则代价值不再变化，所述的衰减公式为：
[0025][0026]
其中，n为相应位置保存的障碍物的删除次数，n为设定的删除次数上限。
[0027]
对于存在时间超过设定的维持时间的超时障碍物对应的栅格，采用通过衰减公式降低代价值的下降速度，所述的衰减公式为：
[0028][0029]
δt＝t
now-t
re-t。
[0030]
所述的步骤5)具体为：
[0031]
代价地图对更新区域内的障碍物进行膨胀，记忆性障碍物膨胀栅格的代价值与固定障碍物不同，在膨胀时，先将固定障碍物向周围膨胀一圈，再与记忆性障碍物一起膨胀，固定障碍物周围栅格的代价值通过距离以及记忆性障碍物的代价值确定，则有：
[0032]
cost
′
＝cost*scost/obstacle
[0033]
其中，cost为根据距离计算的代价值，scost为最近障碍物的代价值，obstacle为固定障碍物的代价值，cost
′
为最终栅格的代价值。
[0034]
与现有技术相比，本发明具有以下优点：
[0035]
一、在动态变化较大的环境中，如人来人往的博物馆中，本发明能够动态删除传感器观测到的障碍物，防止由于环境中动态障碍物过多导致规划路径与实际最优规划不符。
[0036]
二、删除障碍物的方法采用类似于人类记忆曲线，代价值下降速度随时间减小，且观测次数越多，其相同时间下降速度也越慢。
[0037]
三、当传感器观测到同一障碍物次数达到一定时，则将其认为是固定障碍物，代价值不再变化，能够考虑到在环境中确实存在一些不会变化位置的障碍物的情况。
[0038]
四、该方法仅对代价地图做出改进，不针对特定的机器人、路径规划方法、传感器等。
[0039]
五、该方法中的维持时间、删除次数上限均可以根据需要在配置文件中修改，便于根据环境变化而修改参数。
附图说明
[0040]
图1为本发明整体地图的更新流程。
[0041]
图2为记忆性障碍物层地图的更新流程。
[0042]
图3为传感器检测到移动障碍物时的地图。
[0043]
图4为删除次数上限为3时观测到障碍物三次的代价值变化曲线。
[0044]
图5为移动障碍物衰减过程中的地图。
[0045]
图6为移动障碍物在地图上被删除后的地图。
具体实施方式
[0046]
下面结合附图和具体实施例对本发明进行详细说明。
[0047]
本发明提供一种基于代价地图的记忆性存储障碍物方法实现了障碍物的储存，类似艾宾浩斯记忆曲线，障碍物的代价值在一定时间范围内保持，但超过该时间范围后随时间而变小，但当多次观察到同一障碍物，其下降变慢，当观察次数达到一定时，认为该障碍物为固定障碍物，其代价值不再跟随时间变化，本发明实现障碍物的储存过程如下：
[0048]
(1)在移动机器人行驶过程中，传感器检测到的障碍物数据经过处理加入到二维栅格地图中。
[0049]
代价地图是通过使用不同层管理不同语义信息的障碍物，包括管理原始地图的静态层、管理激光雷达等传感器检测障碍物的障碍物层、管理障碍物膨胀的膨胀层等，移动机器人行驶过程中传感器检测到的障碍物会在代价地图中的障碍物层进行处理添加到障碍物层的地图中，在整合成一张地图时由膨胀层对障碍物进行膨胀。
[0050]
(2)使用数组储存相应栅格地图中加入障碍物的时间；使用数组储存删除障碍物的次数，当删除次数达到上限，认定为固定障碍物。
[0051]
建立一个与二维栅格地图相同规模的时间地图，用于存储栅格地图相应位置的更新时间，每一栅格的初始值为程序运行的初始时间，当障碍物层地图更新时，相应位置的时
间更新为当前时间，再建立一个与二维栅格地图相同规模的次数地图，用于存储二维栅格地图相应位置的障碍物删除的次数，当障碍物被删除后其次数加一，当多次删除达到上限时，将其认为是固定障碍物，不再根据时间更新其代价值。
[0052]
(3)根据传感器检测区域和存储时间确定更新区域。
[0053]
代价地图是将不同层的地图处理合成一个整体地图，先确定需要更新的区域，再将每一层障碍物根据规则加入，在确定障碍物层的更新区域时，首先包括当前传感器所检测的区域，还需要查找时间地图中栅格时间不为初始值且存在时间超过设定的维持时间的栅格，将其加入更新区域中。
[0054]
(4)在更新区域内更新栅格地图，根据给定的衰减函数更新其代价值。
[0055]
在实际更新代价值时，判断障碍物存在时间，对于超时障碍物根据其过时时间以及被删除的次数来更新其代价值。
[0056]
(5)对记忆性存储的障碍物进行膨胀。
[0057]
代价地图将从其他层得到的障碍物地图对更新区域内的障碍物进行膨胀，而记忆性障碍物层的代价值与固定障碍物的代价值不同，其膨胀栅格的代价值也与固定障碍物不同，在膨胀时，先将固定障碍物膨胀一圈，再与记忆性障碍物一起膨胀。
[0058]
实施例
[0059]
本实施例为一种基于代价地图的记忆性存储障碍物方法，对于传感器检测到的障碍物会按照记忆性存储方式进行存储，如图1所示，该方法的地图更新整体流程分包括以下五个步骤：
[0060]
(1)在移动机器人行驶过程中，传感器检测到的障碍物数据经过处理加入到二维栅格地图中，此部分由ros中的costmap2d包完成。
[0061]
本实例中用激光雷达作为传感器检测移动机器人行驶过程中的障碍物，经过代价地图的障碍物层将障碍物数据经过处理加入到二维栅格地图中。如图3所示，检测到移动障碍物，将其加入障碍物层地图中，障碍物层相应位置栅格的代价值记为占据。
[0062]
(2)使用数组储存相应栅格地图中加入障碍物的时间，并使用数组储存删除障碍物的次数，当删除次数达到上限，认定其为固定障碍物。
[0063]
本实例中新建与栅格地图相同规模的时间地图，初始值均为程序起始的运行时间。在地图加入移动障碍物时，将当前加入时间放入时间地图相应位置，新建与栅格地图相同规模的次数地图，初始值均为零，本实例中，当前为第一次检测到该障碍物，删除次数为初始值。
[0064]
(3)确定更新区域。在障碍物层确定更新区域时，不仅考虑激光雷达所扫描到的区域，还需要找到在时间地图中时间不为初始值且存在时间超过设定的维持时间的栅格加入更新区域。
[0065]
(4)在更新区域内更新栅格地图时，查找已经过时的障碍物，根据给定的函数更新其代价值。
[0066]
在此实例中，对更新区域的栅格更新的具体流程如图2所示，由于更新区域内的障碍物存在激光雷达扫描的新增障碍物和超时障碍物，首先寻找存在时间超过维持时间的栅格，再寻找其中删除次数未达上限的栅格，按照公式根据时间和删除次数对栅格的代价值进行更新，若更新后代价值为零则删除次数加一。
[0067]
设置第一次观测到障碍物后，其代价值下降公式如下：
[0068][0069]
δt＝t
now-t
re-t
[0070]
其中，c为栅格的初始代价值，c
′
为更新后栅格的代价值，且代价值为整数，t
now
为当前时间，t
re
为存储障碍物的时间，ts为初始值，t为设定的障碍物维持时间。
[0071]
当更新后栅格的代价值c
′
变为零时，次数地图相应位置的值加一，以后代价值下降变慢，删除次数越大下降也越慢，当次数超过设定上限值后，其代价值不再变化，其衰减公式如下：
[0072][0073]
其中n为相应位置保存的删除障碍物次数，n为设定的删除次数上限。
[0074]
根据以上分析，在障碍物层确定更新区域时，不仅考虑当前激光雷达所检测的区域，还需要考虑时间地图，判断当前是否存在障碍物存在时间超过设定的维持时间，若是超过维持时间，则查看当前障碍物被删除的次数，按照如下公式修改其代价值：
[0075][0076]
δt＝t
now-t
re-t
[0077]
本实例中将n设置为3，图4为障碍物经过三次观测与删除时其代价值的效果。
[0078]
(5)对记忆性存储的障碍物进行膨胀。
[0079]
对更新区域内的障碍物进行膨胀时，其可能的修改区域除更新区域外还包括膨胀半径向外膨胀的一圈。原始膨胀层的膨胀方法是在更新区域以及向外膨胀的部分内找到障碍物，即代价值为固定障碍物(255)的栅格，从此开始从距离障碍物由近到远的栅格进行膨胀，访问过的栅格进行标记，由此栅格的代价值由离最近的障碍物的距离确定。
[0080]
但记忆性障碍物的代价值不一定为255，按照原方法记忆性障碍物不会进行膨胀，为此需要更改膨胀方法：由于更新区域内只存在障碍物，且障碍物还未进行膨胀，代价值不为空闲区域(0)的均为障碍物。又由于与固定障碍物膨胀相同距离栅格的代价值会大于记忆性障碍物的，因此寻找最近的障碍物是不合理的；若是让障碍物任意膨胀，冲突时选择较高代价值的方法能得到最准确的代价值，但经过实验发现会花费大量时间，无法实现要求的地图更新频率；故在本实例中让记忆性障碍物的重要性等同于距离固定障碍物一定距离的栅格，即其膨胀是在固定障碍物膨胀到某一距离开始的，其周围栅格的代价值通过距离以及记忆性障碍物的代价值确定，公式如下：
[0081]
cost
′
＝cost*scost/obstacle
[0082]
其中cost是按照原方法根据距离计算的代价值，scost为其最近障碍物的代价值，obstacle为固定障碍物的代价值，cost
′
为最终栅格的代价值。
[0083]
在本实例中，图3为检测到障碍物并加入地图后经过膨胀的地图；图5为移动机器
人离开后将其判定为超时障碍物，代价值逐渐下降过程中障碍物经过膨胀后的地图；图6为删除该障碍物后的地图。