一种基于自适应前沿探索目标点选取的自主探索与避障方法与流程
本发明应用于移动机器人同时定位和地图构建领域，涉及到移动机器人自主探索问题，尤其是场景中存在障碍物和狭窄过道的时候，如何选择可行的区域以及如何选择探索目标点的问题，实现了一种有效的自主探索方法。
背景技术
本发明主要考虑移动机器人如何利用激光扫描仪有效地进行自主探索和场景地图构建的同时避免静态障碍物和动态障碍物。主要自主探索方法包括可行区域的判断方法、探索目标点的选取方法和动态障碍物避碰方法。虽然使用激光扫描仪构建场景地图已经非常成熟，但是让机器人自主的探索，并且构建场景地图的技术还尚未成熟，所以发明提出了一种不需要人为控制机器人运动，让机器人自主探索的一种场景地图构建方法。
在进行自主探索级地图构建的同时由于场景中不可避免的存在障碍物，如果机器人能构建完整的场景地图，则势必要绕过一些障碍物，到达未知区域，才能获得未知区域的信息。基于激光传感器扫描的信息能够有效的识别当前环境信息，从而获得局部子目标和有效的行进路线，然而传统的自主探索方法根据前沿断点信息，机器人在遇到障碍物时，无法有效的绕过障碍物，以及忽略一些狭窄的通道和重复探索的问题，导致机器人构建的场景地图不完整和耗时量大等问题，并且当运动探索目标点的过程中出现动态障碍物的情况下，还容易发生碰撞。
技术实现要素：
为解决机器人的自主探索与避障问题，本发明提供一种自适应前沿探索目标点选取方法，该方法根据场景中出现的静态障碍物，实时地选择合适的阈值，同时在动态障碍物出现时，采用最小二乘法对动态障碍物的位置进行预测，再利用自适应的前沿探索目标点选取方法避开障碍物且重新选择探索目标点。为了保证探索完整的场景地图，利用数据集记录场景中出现的所有探索目标点，并记录机器人在此探索目标点的运动方向，避免重复探索，同时，在机器人探索的过程中，实时的记录场景中的信息，构建一张储存场景信息的栅格地图，为了以后机器人在此地图上进行定位导航。此类方法数据储存量小，速度快，算法容易实现，且实施成本低等优点，能够满足移动机器人有效地避开障碍物且自主探索的应用需求。
为了解决上述技术问题本发明提供如下的技术方案：
一种基于自适应前沿探索目标点选取的自主探索与避障方法，所述方法包括以下步骤：
1)判断可行区域，过程如下：
长直过道为探索区域w，w中存在静态障碍物
dist(ot，q)为激光扫描仪位置ot与可视点q的欧式距离，定义关键角点：当前沿探索点的阈值设定为h时，根据前沿弧和激光射线围成的扇形区域为可视区域，可视区域s的边界与障碍物p相交的交点，将dist(ot，gi)较小的角点gi定义为关键角点，计算安全距离safedis＝dist(ot，gi)*sin(α)，其中α为蓝色扇形区域所对应的圆心角的一半，当safe_dis大于机器人半径r时，取扇形区域的角平分线方向作为目标方向，则将此扇形区域定为机器人的一个可行区域m；
2)自适应前沿探索目标点选取，过程如下：
在固定的激光扫描仪的最大量程无法寻找到可行区域的情况下，通过自适应的方式改变固定阈值，即根据障碍物出现的位置和机器人的最短距离为前沿探索目标点选取的阈值，根据自适应前沿探索目标点选取原理，以激光扫描仪中心点和最近障碍物d的角点g的距离值为探索的阈值，生成可视区域，此时机器人的运行方向从原来的角α方向变为角b方向，则在机器人运动的路径上，距离障碍物的最小距离值也由原来的d变为safe_dis，此时的safe_dis大于机器人的半径r，所以机器人可以安全的行驶至探索目标点处，从而避免机器人将某些狭窄的可行区域被忽略，该方法能够实时的扫描周围环境，及时判断周围障碍物的情况，从而实时的，自适应的改变前沿探索的阈值；
3)动态障碍物预测和避障，过程如下：
考虑就机器人在运行至探索目标点的过程中存在动态障碍物的情况，将每一次机器人向探索目标点运动的过程分为一段，每段的初始时刻机器人的位置为(0，0)，初始时刻，机器人连续n次采集激光扫描仪数据，根据下式计算障碍物位置信息(xi，yi)
xi＝dist(ot，gi)*cosβ+xi(2)
yi＝dist(ot，gi)*sinβ+yi
其中，β为激光扫描仪与障碍物d的连线与x轴正方向的夹角，xi为i个扫描周期内机器人在x轴方向上移动的距离，yi为i个扫描周期内机器人在y轴方向上移动的距离，xi和yi可以根据机器人的里程计信息获得，如果发现同一障碍物的位置发生变化，则根据最小二乘法原理将障碍物位置信息拟合成一条运动曲线，如下公式所示
y＝ax+b(3)
其中
根据式子(3)即可得知障碍物运动的轨迹随时间t的运动方程为：
其中，t为i个激光扫描仪扫描周期的时间，每一次机器人向探索目标点运动，机器人的运动轨迹随时刻t的变化关系如下公式所示：
xi＝0.2t(10)
yi＝0(11)
随着时间t的推移，计算激光扫描仪与障碍物的最短距离d，必然存在一个d的最小值：
如果d＜机器人的半径r，则拟以机器人与障碍物距离最短时刻的障碍物点位置假设成一个静态障碍物点，继续采用步骤2)寻找探索目标点；
4)评价函数的计算，过程如下：
基于信息增益的计算方式，通过引入机器人路径代价和时效信息来计算信息增益：
其中，a(p)为信息增益，σ为扇形可行区域的圆心角，h为选取可行区域时的阈值，θ为机器人从当前朝向转至该可行域的角平分线所需要的旋转角度，k1为具体实践时确定的参数。当f(h)越大，表示该可行区域中的目标点被探索价值越大；
5)全局探索目标点记录与选择，过程如下：
为了保证机器人可以探索到完整的地图，将探索目标点分为3类，记录在数据集中，第一类是机器人已经到达过的探索目标点，第二类是机器人已经遇到过但没有探索过的探索目标点，第三类是机器人在当前时刻下候选的探索目标点，当机器人运行至探索目标点时，发现当前位置下已经没有探索目标点时，机器人立刻搜索数据集，命令机器人运行至最近的没有探索过的探索目标点继续探索，直至将所有的探索目标点都走遍为止；
6)开启gmapping节点
通过采用机器人操作系统提供的gmapping构图程序，直接开启此节点即可记录激光扫描仪探索到的信息，并结合里程计信息实时的构建栅格形式场景地图。
本发明的有益效果表现在：1、针对移动机器人运动模型建立了具体的可行区域的判断方法，该方法计算复杂度低，运算效率高。2、通过ros这个次级系统并结合开源gmapping算法，使得本发明的建图效果精确度高，而且简单方便，易于实现，成本较低。3、在大型场合，完全自主式的探索方式，不需要人为控制机器人运动，大大节省了人力，方便人类生活。
附图说明
图1是本发明方法可行域判断图，其中，10：机器人，11：safe_dis，12:角点，13：障碍物d，14：关键角点g，15：激光射线m，16：激光射线n，17：角α，18：代表x轴正方向，19：代表y轴正方向。
图2是一种自适应前沿探索目标点选取图示，其中，20：代表机器人，21：角α，22:safe_dis，23:d，24:障碍物d，25:关键角点g，26:角β。
图3是另一种自适应前沿探索目标点选取图示，其中，30:代表机器人，31:safe_dis，32:90.0°，33:safe_dis，34:90.0°，35:探索目标点1，35:探索目标点2，37:x轴正方向，38:y轴正方向，39_1：障碍物a，39_1：障碍物b，39_1：障碍物c。
图4是动态障碍物点轨迹预测和机器人位置变化图，其中，40：t1时刻机器人，41：t2时刻机器人，42：t1时刻障碍物d，43：t2时刻障碍物d，44：t3时刻障碍物d，45：t4时刻障碍物d，46：拟合的直线y＝ax+b。
图5是探索目标点的分类图，其中，50：当前时刻下探索目标点，51：探索过的目标点，52：没有探索过的目标点。
具体实施方式
下面结合附图对本发明做进一步说明。
参照图1～图5，一种基于自适应前沿探索目标点选取的自主探索与避障方法，通过安装了北阳urg-04lx-ug01激光扫描仪的mrobot机器人，通过串口连接一台装有ubuntu14.04的linux系统的笔记本电脑，该笔记本电脑配有英特尔酷睿双核，主频为2.6ghz,内存为4g。利用机器人操作系统ros这个分布式实验平台，设计需要执行的程序，结合gmapping算法分布式执行，组成一套自主探索且构建场景地图的系统。，
本发明提供一种自适应前沿探索目标点选取方法该方法能够根据场景中出现的静态障碍物，实时的选择合适的前沿探索的阈值，同时在动态障碍物避障时，采用最小二乘法的动态障碍物预测，再利用自适应的前沿探索目标点选取方法避开障碍物且重新选择探索目标点。为了保证探索完整的场景地图，利用数据集记录场景中出现的所有探索目标点，并记录机器人在此探索目标点的运动方向，避免重复探索，同时，在机器人探索的过程中，实时的记录场景中的信息，构建一张储存场景信息的栅格地图，便于以后的导航等应用。此类方法数据储存量小，速度快，算法容易实现，且实施成本低等优点，能够满足移动机器人有效地避开障碍物且自主探索的应用需求。其工作原理如下：
1)判断可行区域，过程如下：
在说明书附图1中，长直过道为探索区域w，w中存在静态障碍物
，dist(ot，q)为激光扫描仪位置ot与可视点q的欧式距离。定义关键角点：当前沿探索点的阈值设定为h时，根据前沿弧和激光射线围成的扇形区域为可视区域(如附图1所示的蓝色虚线与红色虚线所围成的扇形区域)，可视区域s的边界与障碍物p相交的交点(由于左右两边都有可能存在障碍物，所以交点不止一个)，将dist(ot，gi)较小的角点gi定义为关键角点，如说明书附图1中的g所示，计算安全距离safedis＝dist(ot，gi)*sin(α)，其中α为蓝色扇形区域所对应的圆心角的一半，当safe_dis大于机器人半径r时，取扇形区域的角平分线方向作为目标方向，则将此扇形区域定为机器人的一个可行区域m。
步骤2)中，自适应前沿探索目标点选取，过程如下：
在说明书附图2中，根据上一小节所示的可行区域判断方法，若采用激光扫描仪最大量程5.6m(如附图2中的蓝色虚线所示)作为前沿探索的距离值，则在机器人行驶的路径上，离障碍物的最小距离值为d，而d远小于机器人的半径r，所以机器人认为存前方存在障碍物且不可通行。而实际上，障碍物d的右边明显存在可以让机器人通过的空间。考虑这种情况的发生，本发明提出了一种自适应的前沿探索目标点选取方法。该方法的主要思想为：在固定的激光扫描仪的最大量程(5.6m)无法寻找到可行区域的情况下，通过自适应的方式改变固定阈值，即根据障碍物出现的位置和机器人的最短距离为前沿探索目标点选取的阈值，如图2中绿色虚线所示，根据本文所提的自适应前沿探索目标点选取原理，以激光扫描仪中心点和最近障碍物d的角点g的距离值为探索的阈值，生成可视区域，如说明说附图2中的绿色虚线和红色虚线所围成的区域扇形所示，此时机器人的运行方向从原来的角α方向变为角β方向，则在机器人运动的路径上，距离障碍物的最小距离值也由原来的d变为safe_dis，此时的safedis大于机器人的半径r，所以机器人可以安全的行驶至探索目标点处，从而避免机器人将某些狭窄的可行区域被忽略，该方法能够实时的扫描周围环境，及时判断周围障碍物的情况，从而实时的，自适应的改变前沿探索的阈值，如附图3所示，当机器人运行至探索目标点1时，根据自适应的原则，将阈值及时的调整为激光扫描仪与障碍物c的g点的距离，从而得到新的可视区和探索目标点2，根据该方法机器人能够寻找到当前最优的可行路径。
步骤3)，动态障碍物预测和避障，过程如下：
考虑就机器人在运行至探索目标点的过程中存在动态障碍物的情况，将每一次机器人向探索目标点运动的过程分为一段，每段的初始时刻机器人的位置为(0，0)。初始时刻，机器人连续n次采集激光扫描仪数据，根据下式计算障碍物位置信息(xi，yi)
xi＝dist(ot，gi)*cosβ+xi(2)
yi＝dist(ot，gi)*sinβ+yi
其中β为激光扫描仪与障碍物d的连线与x轴正方向的夹角，xi为i个扫描周期内机器人在x轴方向上移动的距离，yi为i个扫描周期内机器人在y轴方向上移动的距离，xi和yi可以根据机器人的里程计信息获得，如果发现同一障碍物的位置发生变化，则根据最小二乘法原理将障碍物位置信息拟合成一条运动曲线，如下公式所示
y＝ax+b(3)
其中
根据式子(3)即可得知障碍物运动的轨迹随时间t的运动方程为：
其中t为i个激光扫描仪扫描周期的时间，每一次机器人向探索目标点运动，机器人的运动轨迹随时刻t的变化关系如下公式所示：
xi＝0.2t(10)
yi＝0(11)
随着时间t的推移，计算激光扫描仪与障碍物的最短距离d：
如果d＜机器人的半径r，则拟以机器人与障碍物距离最短时刻的障碍物点位置假设成一个静态障碍物点，继续采用本发明步骤2)寻找探索目标点。
步骤4)，评价函数的计算，过程如下：
当机器人处于某一位置，出现多个可行区域时，如何选择最佳的可行区域以及如何选择最佳的探索目标点一直是机器人自主探索并构建场景地图中必须要考虑的事情。本文基于信息增益的计算方式，通过引入机器人路径代价是时效信息来计算信息增益：
其中a(p)为信息增益，σ为扇形可行区域的圆心角，h为选取可行区域时的阈值，θ为机器人从当前朝向转至该可行域的角平分线所需要的旋转角度，k1为具体实践时确定的参数。当f(h)越大，表示该可行域中的目标点被探索价值越大。
步骤5)，全局探索目标点记录与选择，过程如下：
为了保证机器人可以探索到完整的地图，将探索目标点分为3类，如附图5所示，记录在数据集中，第一类是机器人已经到达过的探索目标点，第二类是机器人已经遇到过但没有探索过的探索目标点，第三类是机器人在当前时刻下候选的探索目标点，当机器人运行至探索目标点时，发现当前位置下已经没有候选的探索目标点时，机器人立刻搜索数据集，并开始运行至最近的没有探索过的探索目标点继续探索，直至将所有的探索目标点都走遍为止。
步骤6)中，开启gmapping节点
通过采用机器人操作系统提供的gmapping构图程序，直接开启此节点即可记录激光扫描仪探索到的信息，并结合里程计信息实时的构建栅格形式场景地图。
以上结合附图详细说明和陈述了本发明的实施方式，但并不局限于上述方式。在本领域的技术人员所具备的知识范围内，只要以本发明的构思为基础，还可以做出多种变化和改进。
- 还没有人留言评论。精彩留言会获得点赞！