应用于危险气体泄漏场景的地图构建方法

本发明涉及机器人slam算法建图和自主导航，尤其涉及一种应用于危险气体环境的机器人地图构建方法。

背景技术：

1、化工厂生产环境条件复杂,危险气体泄漏极易造成起火、爆炸、毒气危害等灾难,严重危害了群众的人身和财产安全。复杂的事故现场条件极大的增加了抢救的困难、成本和风险。目前一般的应对方法是尽快撤离泄露环境污染区居民至上风地带,并及时实施隔断,严密控制出入。组织人员穿戴防护衣，在上风方向进入事故现场，并采取措施,尽可能地截断泄露源。人工应急方式需要依靠应急人员的感官和经验，气体泄漏导致工厂环境变得更加复杂，能见度降低，准确性不高；而且有毒有害气体容易威胁到应急人员的健康甚至生命安全。

2、面对气体泄漏，现场条件复杂多变，环境信息受影响，又不能进行远距离遥控的前提下，迫切需要一个可以主动收集陌生场所地图环境信息，并对毒气泄露场所实施紧急处置的方法，而化工厂的智能机器人及应急控制系统就可以通过其操作灵活、便于管理的特性，针对化工厂的危险物质的泄露情况自主实施应急的控制泄漏方法，通过收集危险物质的情况和场地条件信息，控制机构就能够按照所返回的数据采取相应的措施对策，便于后续人员进入现场进行处理。化工厂中引入的机器人自主急救系统能够大幅度提高急救的速度,同时技术人员也不必再耗费大量时间去对毒气的泄露现场进行模拟和猜测，从而使得抢救效果和服务质量都得以大大提高；更为关键的是,降低了化工厂人员在深入现场实施紧急处置时可能引起的生命健康危险。而机器人在化工厂中同样也能够起到独特而关键的功能，也因此给化工企业安全生产带来了更加有力的保证，因此，应用于危险气体泄漏场景的地图构建机器人系统具有重大的现实意义。

3、基于上述所述，构建一幅精确完整的地图是至关重要的，gmapping算法在高度相似且闭环回路较多的环境下，会出现粒子匮乏和粒子多样性丧失的情况，而机器人在仓库导航的过程中，大多数都会面临高度相似且闭环回路较多的环境，这就导致了所构建的地图精确度不足，从而导致整个导航过程存在误差和错误，效率不足。

技术实现思路

1、发明目的：针对背景技术中指出的所构建的地图精确度不足等问题，本发明提出一种应用于危险气体泄漏场景的机器人地图构建方法，提高机器人在仓库环境下导航的效率。

2、技术方案：本发明提供了一种应用于危险气体环境的机器人地图构建方法，该方法基于电动机器人设置，由两个直流电机带动驱动机器人行走，所述机器人上设置有激光雷达，包括以下步骤：

3、步骤1：通过激光雷达感知外部环境的深度信息，激光发射器负责发射激光，接收器负责接受从障碍物反射回来的光线，计算出目标物距离，直流电机负责激光雷达的旋转，从而建立二维地图，转动一圈，获得一幅点云图像；基于编码器感知直流电机的转速信息，由转速信息进而得到速度信息和位姿信息，最终得到里程计数据信息；

4、步骤2：基于采集的里程计数据和激光雷达数据，采用自适应蚁群算法优化gmapping算法来实现机器人的二维栅格地图构建和即时定位；

5、步骤3：基于全局路径规划与局部路径规划相互配合，形成机器人自主导航的路径规划，采用a*算法来实现全局路径规划，采用dwa算法来实现局部路径规划和动态避障。

6、进一步地，所述步骤2中的gmapping算法来实现机器人的二维栅格地图构建和即时定位具体方法为：

7、根据条件贝叶斯法则，有：

8、p(x1:t,m|z1:t,u1:t-1)＝p(m|x1:t,z1:t)·p(x1:t|z1:t,u1:t-1)

9、其中，p(x1:t,m|z1:t,u1:t-1)为激光雷达信息和里程计信息来估计环境地图和机器人轨迹的联合后概率；p(x1:t|z1:t,u1:t-1)为定位概率问题，利用里程计数据和激光雷达的观测对机器人的位姿进行估计；p(m|x1:t,z1:t)为地图概率估计问题，由当前机器人位姿可以对增量式m地图进行更新；

10、1)在计算提议分布时把激光雷达传感器观测考虑进去，将采样过程集中到更有意义的似然函数峰值区域l(i)进行，使用高斯分布对观测似然峰值区域近似，从而得到优化的提议分布，高斯分布参数和由在区间l(i)内的k个次采样点确定：

11、

12、

13、其中，归一化因子η(i)为：

14、

15、对于每个粒子，给予其一个权重，以便于后续的重采样步骤使用其粒子权重，权重的计算方式如下：

16、

17、2)利用有效采样尺度标准来衡量粒子集的优劣，并决定何时实施重取样过程，公式如下：

18、

19、其中，是第i个粒子的权重，粒子集对提议分布的估计越差，neff越小。

20、进一步地，在利用gmapping算法来实现机器人的二维栅格地图构建和即时定位时，利用蚁群算法改进gmapping算法的重采样过程，具体操作如下：

21、通过已知随机动态系统先验概率密度分布p(x0)，进行n次采样得到t＝0时刻的初始粒子每个粒子对应的初始权值为

22、对于i＝1,…,n，根据采样新粒子

23、根据当前的观测值yt，计算每个粒子的权值：

24、

25、剔除估计值与真实值异号的粒子：

26、当时，则令权值反之，权值保持不变；

27、再归一化粒子权值：

28、在第t时刻第i个粒子转移到第j个粒子的转移概率集比较获得转移概率最大值并设置转移概率阈值为λ；当时，令当时，令更新粒子集每个粒子所对应的权值集为输出状态估计值同时使t＝t+1。

29、进一步地，在利用蚁群算法改进gmapping算法的重采样过程中，还对蚁群算法进行了改进，提出自适应蚁群算法：采用伪随机状态转移规则，新的状态转移公式如下：

30、

31、其中：q是一个值在[0,1]之内的随机数；q0是前两次迭代中最短路径长度的比值，当q≤q0时，蚂蚁将从其当前所在节点i的相邻节点中选择启发式函数值和信息素浓度积最大值的节点作为移动的下一节点；当q＞q0时，该蚂蚁则首先计算出下一可选节点集合中各节点启发式函数值和信息素浓度的积，再利用轮盘赌的方式确定下一节点。

32、进一步地，利用自适应蚁群算法优化gmapping算法的具体步骤如下

33、1)对里程计所接受的数据信息和激光雷达数据信息进行预处理，对于所需参数进行初始化；

34、2)激光雷达数据转化成粒子，多粒子形成了点云信息，对点云信息进行线性拟合，判断点云波动量是否大于所设定的阀值；

35、3)判断点云波动量是否大于所设定的阀值，若判断点云波动量大于所设定的阀值，则进行下一步；反之，若判断点云波动量小于所设定的阀值，则减少粒子数，转至步骤5)；

36、4)进行自适应优化，采用权利要求4中的伪随机状态转移规则，增加粒子多样性，提高蚁群的全局搜索能力；

37、5)扫描匹配，从优化提议分布中采样，并计算出当前粒子的权重；

38、6)输出全局极值点和最优个体值，并重新计算粒子权重和归一化处理；

39、7)根据有效粒子数进行选择性重采样，即设定阀值；当有效粒子数小于阀值时，则进行重采样；反之，则不进行此操作；

40、8)依据粒子的权重，将当前粒子划分为高权重区和低权重区；

41、9)判断高权重区的粒子数n是否大于设定的阀值，若小于阀值，则进行下一步的寻优操作；反之，则直接跳转到步骤11)；

42、10)计算全局最优值，进行蚁群优化处理，同时计算各粒子与最优粒子间的吸引度，进而进行寻优，将权值较小的粒子在粒子的最优预测区域迁移；若达到迭代次数或者精度，则停止寻优，否则继续执行步骤10)；

43、11)更新地图。

44、有益效果：

45、1、本发明gmapping算法通过重采样技术来避免粒子缺失，方法是在淘汰原连接权值较小的粒子后，将原权值较大的粒子加以重复。这个技术的好处在于操作简便，但进行反复迭代时，会导致粒子没有了多样化。而采用蚁群优化的方法，将权值较小的粒子在粒子的最优预测区域迁移，但并非全部复制权值的粒子，并维持权值最大的粒子位置不变，会使粒子良好的均匀分布，从而保证了粒子的稳定多样化。

46、2、本发明提出了一种自适应蚁群算法，首先设置自适应条件，将采样粒子数和现实环境的复杂程序进行线性拟合，当二维激光点云波动量大于某个阀值时，采用自适应的蚁群算法进行算法优化，反之则适当减少采样数，优化算法的采样处理时间。自适应蚁群算法于普通的蚁群算法不同点在于，普通的蚁群算法采用传统的状态转移规则和轮盘赌选择方式，促使蚂蚁选择当前解中的较优值，但当点云波动量超过阀值时，认定外界环境为复杂环境，采样粒子不一定均匀地分布在整个解空间，导致粒子多样性不足，若是这时再采用普通的蚁群优化gmapping算法，将权值较小的粒子在粒子的最优预测区域迁移，随着迭代的进行，种群容易陷入局部最优解，出现算法停滞的现象。而自适应蚁群算法采用了伪随机状态转移规则，提高算法的搜索效率和质量。

47、3、本发明全局路线规划要求了解全部的自然环境资讯，通过环境地图的各种资讯完成路线规划；局部路线规划只要求由传感器实时收集周围环境信号，了解周围环境地图信息，进而判断出所在地图的方位以及局部的障碍物分布状况，由此能够选择从当前节点至某一子目标结点的最佳路线，采用全局路线规划和局部路线规划互相融合的方法，使地图构建机器人在危险气体泄漏环境条件下，利用全局路线规划，能够迅速的找到最佳的全局地图路径，同时通过局部路径规划，使得地图构建机器人可以进行动态避障。