技术领域
[0001] 本
发明属于特种机器人
定位导航领域，特别涉及了石化工厂巡检机器人环境建模与地图构建装置和方法。
背景技术
[0002] 石化企业、油库、储运公司等工厂的工作区域面积通常较大，安全隐患不易排查。目前只能通过人工对各个罐区、
泵区、
变电所等工作场合的仪器仪表进行徒步现场检测，对输油管路、储油罐底部、泵站、计量间、流量间等工作场合的各类
阀门、仪表、设备进行重点逐一检查。人工巡检的工作强度大、复杂度高，需要巡检的区域繁杂极容易漏检，存在较大的安全隐患。采用自主巡检机器人代替人工执行巡检任务，可以极大的降低工人工作强度，降低工作
风险；容易扩展为多机器人协作方式，昼夜不间断工作，提高巡检效率；可以通过事先标识和自主
感知发现，对需要重点巡检的场合进行程序化排查，排除人为漏检错检失误，安全性更高。
[0003] 目前，巡检机器人在变电站、室内工作区域等结构化条件较好的环境已经取得应用和推广，在巡检工作自动化和智能化方面上有了一定的提高。相比于变电站，石化工厂的环境非结构化程度更高，需要巡检的内容和场所类型更复杂。例如，巡检机器人除了需要在厂区室外环境对罐区、泵区等设备进行巡检，同时还要进入变电所、流量间、计量间等室内环境。由于巡检任务的复杂性，给机器人定位和自主导航带来巨大的困难。通过石化工厂的环境建模和地图构建可以为机器人提供高
精度的环境模型，基于地图可以大幅度降低机器人定位的难度，极大提高定位精度。同时环境模型的建立可以预先对重要仪器和事故多发地区的
位置进行标识，对于制定巡检策略和后期
机器人自主导航具有重要的作用，其
质量直接影响路径规划、避障的难度和精度，决定巡检效率。另外环境模型的建立也为
可视化监控机器人运行状况和环境情况带来巨大便利。
[0004] 为了满足石化工厂自主巡检机器人的精确定位和自主导航的功能要求，需要对石化工厂室内和室外环境进行精确的环境建模和地图构建。目前石化行业应用的巡检机器人较少。其他领域巡检机器人常用的环境建模方法有以下几种：
[0005] (1)人工测绘构建
电子地图：通过专业测绘人员采用全站仪、米尺、激光测距仪等设备进行人工测绘。此类方式主要缺点是工作量大，自动化程度低，测绘精度较差，测绘误差对于
移动机器人定位和自主导航性能影响较大，同时不利于厂区环境变化后地图的实时更新；
[0006] (2)采用激光
扫描仪等建模设备在厂区内进行多点测量，后期通过地图拼接方法绘制三维地图。此类方法使用的设备造价高，同时也不利于厂区环境变化后地图的实时更新。
[0007] (3)采用卫星影像或无人机测绘获得区域的地理信息构建环境地图：此类方法造价高，误差大，获得的地图仅适用于适用采用GPS等全局定位方式的机器人定位使用，无法应对大量的室内和有遮蔽环境。
[0008] (4)机器人携带如二维
激光雷达、视觉相机等
传感器采用在线或离线方式构建环境地图的方法。此类方法通常采用二维激光雷达或相机使用同步定位和建图方法构建二维环境地图，结合其他传感器提高定位精度。此类方法通常适用于室内环境，要求地面平坦场景简单，对于斜坡、颠簸场景应用效果差。
[0009] 同时，上述建模方法仅是包含对于环境的范围描绘信息，没有精确的设备和仪器仪表标识信息，对于工作量极大的石化工厂巡检机器人执行具体巡检任务没有实质帮助，仍然只能依赖其他传感器如相机等进行大场景下的仪器仪表目标识别与监测。另外重要的一点，石化工厂使用的设备还需要满足防爆安全要求，对于现阶段使用的各类测量设备，都难以满足直接应用到此类环境区域。
发明内容
[0010] 为了解决上述背景技术提出的技术问题，本发明旨在提供石化工厂巡检机器人环境建模与地图构建装置和方法，通过多类型传感器的不同
位姿信息方法，获取高精度的环境模型和地图，提高系统对于环境的适应性和精确性，同时系统满足防爆安全要求。
[0011] 为了实现上述技术目的，本发明的技术方案为：
[0012] 一种石化工厂巡检机器人环境建模与地图构建装置，该装置安装或集成在巡检机器人平台上，包括防爆壳体，该防爆壳体包含3个腔体：主控腔、接线腔和电源腔，主控腔内设有工控机、差分GPS模
块、惯性测量模块、
信号隔离安全栅模块、通讯模块和电源转换模块，接线腔用于转接各供电和通信
电缆，电源腔内设有电源模块，防爆壳体的外部设有视觉模块、激光雷达、差分GPS模块天线和里程测量模块，视觉模块、激光雷达和里程测量模块的输出端分别经信号隔离安全栅模块与通讯模块的输入端连接，差分GPS模块天线的输出端经信号隔离安全栅模块与差分GPS模块的输入端连接，差分GPS模块的输出端连接通讯模块的输入端，惯性测量模块和通讯模块的输出端与工控机连接，电源模块的输出端与电源转换模块的输入端连接，电源转换模块的输出端输出各模块所需的供电
电压，工控机根据采集的视觉信息、激光雷达信息、差分GPS信息、里程信息和惯性信息，实现机器人精确定位，并基于定位结果构建厂区的环境模型和地图。
[0013] 基于上述装置的环境建模与地图构建方法，在工控机上建立第一位姿估计单元、第二位姿估计单元、第三位姿估计单元、第四位姿估计单元、坐标转换模块、信息综合模块、建图模块和集成估计模块，步骤如下：
[0014] (1)通过第一位姿估计单元、第二位姿估计单元、第三位姿估计单元、第四位姿估计单元分别获得四种定位方式得到的位姿信息，并输入到坐标转换模块；其中，第一位姿估计单元通过差分GPS模块与惯性测量模块组合导航获得位姿信息，输出惯导中心相对于世界
坐标系的第一位姿估计；第二位姿估计单元根据激光雷达信息，通过扫描匹配的
运动估计方法获得位姿信息，输出激光雷达中心相对于世界坐标系的第二位姿估计；第三位姿估计单元根据视觉信息，采用视觉
里程计方法获得位姿信息，输出视觉模块中心相对于世界坐标系的第三位姿估计；第四位姿估计单元通过里程信息与惯性测量模块获取的方位
角信息组合，通过
扩展卡尔曼
滤波器融合获得位姿信息，输出惯导中心相对于世界坐标系的第四位姿估计；
[0015] (2)坐标转换模块将四种位姿估计结果统一到以机器人质心为坐标系原点的机器人坐标系下，并将经过坐标变换的四个位姿估计结果输入到信息综合模块；
[0016] (3)信息综合模块利用自适应加权评估方法进行机器人位姿信息的综合评估，将评估后的位姿结果分别输入到建图模块和集成估计模块；
[0017] (4)建图模块根据机器人当前位姿，经过坐标转换得到激光雷达位姿，结合激光扫描获得的点
云信息匹配到已构建地图，实现增量式地图构建；
[0018] (5)集成估计模块结合信息综合模块的位姿信息和建图模块的点云地图信息，计算相对于地图的精确机器人位姿估计；
[0019] (6)存储环境模型，对建模数据进行人工标识；
[0020] (7)当环境发生局部变化时，对地图进行动态更新。
[0021] 进一步地，在步骤(2)中，利用第一～第四位姿估计Li以及对应的位姿传感器相对于机器人质心的安装位置变换矩阵Ti，得到机器人质心相对于世界坐标系的位姿估计pi：
[0022]
[0023] 上式中，下标i＝1,2,3,4，分别表示四种位姿估计。
[0024] 进一步地，在步骤(3)中，机器人位姿信息的综合评估值P：
[0025]
[0026] 上式中，αi为pi对应的权重，其初始值由人为设定，在机器人运行过程中，αi的取值根据环境变化自适应地改变。
[0027] 进一步地，第一位姿估计的权重α1的取值由下式确定：
[0028]
[0029] 上式中，t为GPS信号连续丢失的
采样数，tG为设定的
阈值；若观测卫星数量少于4颗或位置精度因子小于4，则判断GPS信号丢失；
[0030] 第二位姿估计的权重α2的取值由下式确定：
[0031]
[0032] 上式中，ΔL为公倍数采样周期T内第四位姿估计的变化ΔL4与第二位姿估计的变化ΔL2的绝对差值，ΔLS为设定的阈值；
[0033] 第三位姿估计的权重α3的取值由下式确定：
[0034]
[0035] 上式中， 为人工设定的初始值，n为视觉里程计相邻
帧间检测到的特征点匹配成功的数目，nV为设定的阈值；
[0036] 第四位姿估计的权重α4的取值由下式确定：
[0037]
[0038] 上式中， 为人工设定的初始值，a为滤波后的机器人z轴平均
加速度，aAcc为设定的阈值。
[0039] 进一步地，在步骤(4)中，信息综合模块在k时刻获得机器人相对于世界坐标系{W}的精确位姿Pk，对应k时刻的已完成构建的点云地图Qk，利用特征点提取和特征关联方法获得从k到k+1时刻新采集的点云数据{qk+1}，同时通过信息综合模块获得的k+1时刻的位姿为Pk+1，则新构建的点云地图Qk+1：
[0040]
[0041] 上式中， 为广义加，表示地图的增量式构建过程， 表示相邻时刻旋转平移过程。
[0042] 进一步地，在步骤(5)中，利用扫描匹配方法在建图模块输出的点云地图中进行位姿估计，得到结果PSM，将PSM利用
时间线性插值的方法插入到信息综合模块输出的位姿估计结果P中，获得相对于点云地图的定位信息。
[0043] 进一步地，在步骤(6)中，对构建的点云地图模型以八叉树的形式存储，并进行滤波、压缩、栅格划分处理，栅格用于存储被占用情况和备用信息；采用人工标识，对模型中的关注区域的区域和目标种类进行不同种类的标识和存储，获得环境中待检测目标的精确坐标和语义信息。
[0044] 进一步地，在步骤(7)中，当出现环境局部变化的情况时，机器人运行到变化后的环境后，仅利用建图模块和集成估计模块，实现动态更新局部地图。
[0045] 进一步地，在动态更新局部地图过程中，从未更新环境开始运行，集成估计模块获得的相对于地图的位姿估计结果仅包含扫描匹配方法获得的位姿估计，将此定位结果输入建图模块，继续用增量式建图方法更新局部变化的环境；当更新到原环境模型中未更新的区域时，扫描匹配方法自动重定位到原环境模型下，完成地图更新。
[0046] 采用上述技术方案带来的有益效果：
[0047] (1)本发明可以满足石化工厂全场景、全工况条件下工作，通过多类型传感器的不同位姿信息获取方法，利用环境自适应的定位权重评价机制，实现了兼顾室内室外环境及设备稠密遮挡情况、环境特征数量情况、光照条件、地面颠簸程度等石化工厂存在的环境情况，在各种环境中都能获得最优的定位结果，基于精确的定位制作出高精度的环境模型和地图，提高了系统对于环境的适应性和精确性，克服了由于单一类型传感器定位效果差导致建图质量下降的弊端；
[0048] (2)本发明满足石化工厂对于检测监测设备的防爆安全性能要求，通过本安传感器的使用和与隔爆腔体中元器件的电气与信号隔离，满足隔爆兼本安的防爆要求，保障了系统的安全性和可靠性，突破了现有环境建模和地图构建设备不防爆、无法直接应用到石化工厂进行建模构图的技术应用现状，系统原理简洁易于实现、实用性强；
[0049] (3)本发明采用环境自适应的定位权重评价方法，通过利用传感器特性和各种定位方式特点，利用程序自动更改权重大小；初始值可由人工进行设置，根据不同厂区的环境特征、不同精度的传感器配置通过经验判断对各定位方法的信任程度，人为
修改权值，综合适应性强；在初始值设定后，机器人运行过程中无需人工干预，即可自动适应各类环境实现各种环境下的最优位姿估计，因此进一步提高了基于位姿估计的增量式环境建模的精度；同时该评价方法免去了目前常用的基于滤波的多传感器融合时当某一类型传感器失效后，滤波器随即失效或精度下降的情况，计算量小，
算法原理设计简单有效；
[0050] (4)本发明采用局部地图动态更新方法，克服了一旦环境发生局部变化后就需要整体环境重新建模的弊端；动态更新时，基于未变化环境地图开始工作，利用基于扫描匹配获得的位姿进行增量式地图构建更新局部变化的环境模型，这样的方法保证了在更新过程再一次到达原环境模型中未更新的区域时，可以自动校正构图的结果，克服传统方法因为定位精度与初次建模时定位精度不一致导致建图结果与原始模型不完全一致，带来更新地图与原地图
混叠的问题；而且地图动态更新过程只需要关闭部分程序模块，不需要引入其他新的建模方法，简单实用；
[0051] (5)本发明通过人工标识厂区内的待检测仪器仪表设备，可以在机器人到达巡检工位之前就知道待检测目标的全局位置信息，有利于帮助机器人在后续路径规划时减小机器人搜索范围，准确巡航；同时后续便于与仪器仪表目标检测方法对接，通过预先标识的位置，缩小视觉目标检测的搜索范围，为准确识别仪器仪表读数和状态信息奠定
基础，提高机器人巡检工作效率。
附图说明
[0052] 图1为本发明装置内部结构图；
[0053] 图2为本发明装置外部传感器布置图；
[0054] 图3为本发明环境建模与地图构建方法功能模块图；
[0055] 图4为本发明环境建模与地图构建步骤图；
[0056] 标号说明：1-
电池电源；2-工控机；3-惯性测量模块；4-差分GPS模块；5-电源转换模块；6-信号隔离安全栅模块；7-通讯模块；8-主控腔；9-接线腔；10-电源腔；11-隔爆壳体；12-视觉模块；13-激光雷达；14-差分GPS模块天线；15-里程测量模块；16-第一位姿估计单元；17-第二位姿估计单元；18-第三位姿估计单元；19-第四位姿估计单元；20-坐标转换模块；21-信息综合模块；22-建图模块；23-集成估计模块。
具体实施方式
[0057] 以下将结合附图，对本发明的技术方案进行详细说明。
[0058] 一种石化工厂巡检机器人环境建模与地图构建装置，如图1所示，包括电源模块1、工控机2、惯性测量模块3、差分GPS模块4，电源转换模块5、信号隔离安全栅模块6、通讯模块7和隔爆壳体11，隔爆壳体11分为三个腔体：主控腔8、接线腔9和电源腔10，工控机2、惯性测量模块3、差分GPS模块4，电源转换模块5、信号隔离安全栅模块6和通讯模块7安装于主控腔
8，接线腔9转接各类供电和通信电缆，电源腔10安装电源模块1。
[0059] 图2为机器人外部传感器布置关系图，包括视觉模块12、激光雷达13、差分GPS模块天线14、里程测量模块15。所述视觉模块12、多线激光雷达13、差分GPS模块天线14安装于隔爆壳体11外部上方。
[0060] 在本
实施例中，视觉模块12通过网络
接口利用矿用阻燃网络通讯电缆连接到信号隔离安全栅模块6输出端，采集信号经信号隔离安全栅模块6信号隔离、通讯模块7中转后发送到工控机2进行位姿估计。作为优选的，视觉模块采用隔爆型双目网络摄像机，用于接收双路图像信号，采用
双目视觉里程计方法提供基于视觉的位姿信息。
[0061] 在本实施例中，激光雷达13通过网络接口利用矿用阻燃通讯电缆连接到信号隔离安全栅模块6输出端，采集信号经信号隔离安全栅模块6信号隔离、通讯模块7中转后发送到工控机2进行位姿估计。作为优选的，激光雷达13使用本质安全型激光雷达，用于采集激光点云数据，采用扫描匹配方法实现位姿估计和建立高精度点云地图。
[0062] 在本实施例中，差分GPS模块天线14通过矿用阻燃电缆连接到信号隔离安全栅模块6输出端，采集信号经信号隔离安全栅模块6信号隔离发送给差分GPS模块4进行位姿估计。作为优选的，差分GPS模块天线14使用本质安全型天线，用于增强接收到的GPS信号。差分GPS模块4通过网络接口利用网线连接到通讯模块7，用于接收、处理、发送GPS定位信号最终发送到工控机2。
[0063] 在本实施例中，里程测量模块15通过矿用阻燃通信电缆连接到信号隔离安全栅模块6输出端，采集信号经信号隔离安全栅模块6信号隔离后经通讯模块7发送给工控机2进行位姿估计。作为优选的，里程测量模块使用本质安全型光电
编码器，用于测量机器人平台的运行速度和里程。
[0064] 在本实施例中，惯性测量模块3通过USB线缆连接到工控机2的USB接口，用于辅助GPS和里程计进行组合定位，提供机器人
姿态角和加速度信息。
[0065] 在本实施例中，电源转换模块5输入端连接到电源模块1的输出端，其中的电源转换模块5输出端分别转变为不同电压的非安电源端和本安电源端，隔爆壳体11内部设备包括工控机2、差分GPS模块4、信号隔离安全栅模块6、通讯模块7的供电端分别连接到匹配各自电源要求的非安电源端，隔爆壳体11外部传感器包括视觉模块12、激光雷达13、里程测量模块15的供电端分别连接到匹配各自电源要求的本安电源端。电源转换模块的主要功能是为在隔爆壳体11内工作的设备提供非安电源，为在隔爆壳体11外工作的设备提供本安电源，实现电气隔离，保证安全性。信号隔离安全栅6的功能是保证工作在隔爆壳体内外的
电路之间的信号隔离，保证安全性。工控机2通过网络接口利用网线连接到通讯模块7，用于综合处理各类传感器信息，并进行环境建模和地图构建算法的执行。
[0066] 基于上述装置的环境建模与地图构建方法，如图3所示在工控机上建立第一位姿估计单元16、第二位姿估计单元17、第三位姿估计单元18、第四位姿估计单元19、坐标转换模块20、信息综合模块21、建图模块22、集成估计模块23。其中，第一位姿估计单元16、第二位姿估计单元17、第三位姿估计单元18、第四位姿估计单元19的四种定位方式获得的位姿信息，分别输入到坐标转换模块20，从而将四种位姿估计结果统一到以机器人质心为坐标系原点的机器人坐标系下。经过坐标变换的四个位姿估计结果输入到信息综合模块21，利用自适应加权评估方法进行机器人位姿信息的综合评估。将评估后的位姿结果分别输入到建图模块22和集成估计模块23。建图模块22根据机器人当前位姿，经过从平台到激光雷达的坐标转换得到激光雷达位姿，结合激光扫描获得的点云信息匹配到已构建地图，实现增量式地图构建。集成估计模块23结合信息综合模块21的位姿信息和建图模块22的三维地图信息，计算获得相对于地图的机器人位姿。
[0067] 环境建模与地图构建步骤如图4所示：
[0068] 步骤S41、四个位姿估计单元分别计算位姿信息：通过所述的第一位姿估计单元16、第二位姿估计单元17、第三位姿估计单元18、第四位姿估计单元19的四种定位方式分别获得位姿信息，输入到坐标转换模块20。
[0069] 步骤S42、四种位姿估计结果进行坐标转换统一到机器人坐标系下：所述的坐标转换模块20将四种位姿估计结果统一到以机器人质心为坐标系原点的机器人坐标系下，经过坐标变换的四个位姿估计结果输入到信息综合模块21。
[0070] 步骤S43、自适应加权评估方法对位姿进行评估：所述的信息综合模块21利用自适应加权评估方法进行机器人位姿信息的综合评估，将评估后的位姿结果分别输入到所述的建图模块22和集成估计模块23。
[0071] 步骤S44、基于定位完成增量式地图构建：所述的建图模块22根据机器人当前位姿，经过从平台到激光雷达的坐标转换得到激光雷达位姿，结合激光扫描获得的点云信息匹配到已构建地图，实现增量式地图构建。
[0072] 步骤S45、所述的集成估计模块23结合信息综合模块21的位姿信息和建图模块22的三维地图信息，计算获得相对于地图的精确机器人位姿估计。
[0073] 步骤S46、人工标识：在完成厂区内不同环境的建模后，对建模数据进行人工标识。
[0074] 步骤S47、局部环境模型与地图动态更新：当环境发生局部变化时，对环境地图进行动态更新。
[0075] 在步骤S41中，所述第一位姿估计单元16，通过差分GPS模块4与惯性测量模块3构成组合导航获得位姿估计结果，输出惯导中心相对于机器人所处世界坐标系{W}的第一位姿估计L1、
频率为f1；作为优选的，频率f1为100Hz。
[0076] 在步骤S41中，所述第二位姿信息估计单元17，通过激光雷达13通过边缘和平面特征点提取和特征关联，通过扫描匹配的运动估计方法获得位姿信息，输出激光雷达13中心相对于机器人所处世界坐标系{W}第二位姿估计L2、频率为f2Hz；作为优选的，频率f2为10Hz。
[0077] 在步骤S41中，所述第三位姿信息估计单元18，通过视觉模块12利用视觉里程计方法获得位姿信息，输出视觉模块12中心相对于机器人所处世界坐标系{W}的第三位姿信息L3、频率为f3Hz。作为优选的，所述视觉里程计算法使用SOFT(Stereo Odometry with Feature Tracking)算法进行位姿估计L3，频率f3为20Hz。
[0078] 在步骤S41中，所述第四位姿信息估计单元19，通过里程测量模块15获得的里程数据和惯性测量模块3测量的方位角数据组合，利用扩展卡尔曼滤波器融合获得位姿信息，输出相对于惯导中心相对于机器人所处世界坐标系{W}的第四位姿估计L4、频率为f4Hz；作为优选的，频率f4为20Hz。
[0079] 在步骤S41中，所述坐标转换模块20，利用各自传感器坐标相对于世界坐标系{W}的位姿Li，以及各位姿估计单元传感器相对于机器人质心的安装位置变换矩阵Τi，经过刚体坐标变换得到各种位姿估计方法获得的各种定位方法获得的机器人质心相对于世界坐标系{W}的位姿估计pi：
[0080]
[0081] 在步骤S42中，信息综合模块21用于提供精确的机器人位姿估计，采用环境自适应的定位权重评价方法对上述四种定位方式获得的结果进行综合评估，计算公式：
[0082]
[0083] 上式中，下标i,j表示第i,j个位姿信息单元。
[0084] 所述环境自适应的定位权重评价方法，通过人工预先判断机器人所处环境，包括室内室外环境及设备稠密遮挡情况、环境特征多少情况、光照情况、地面颠簸程度等信息，以及使用的传感器精度特点，人为
指定相应的初始权重 机器人运行过程中，αi的取值根据环境变化通过自适应判断的方法改变，在初值设定后无需后续的人工干涉。环境自适应的定位权重判断如下：
[0085] 1)第一位姿信息估计单元16测量过程中，差分GPS模块4测量的GPS信号在进入房间或由于周围设备较多有遮挡或造成多路径反射问题时，随信号中断时间的增长定位精度会下降。根据此特性，信息综合模块21判断GPS信号接收状况，根据GPS信号(观测卫星数量不少于4颗、位置精度因子小于4)连续丢失采样数t与设定阈值tG的关系决定第一位姿信息权重α1的大小。使用线性插值计算权重α1(tG为设定阈值)：
[0086]
[0087] 2)第二位姿信息估计单元17测量过程中，激光雷达13利用扫描匹配进行定位，当机器人运行到室内长直走廊、室外防火通道等角点、平面特征较少的场合，定位精度降低，而航迹推算不受此环境的影响。根据此特性，信息综合模块21判断设定公倍数采样周期T内第四位姿信息的变化ΔL4与第二位姿信息的变化ΔL2的绝对差值ΔL决定第二位姿信息权重α2。使用线性插值计算权重α2(ΔLS为设定阈值)：
[0088]
[0089] 作为优选的，所述扫描匹配定位采用Levenberg-Marquardt方法进行非线性优化寻找最优的位姿匹配。
[0090] 3)第三位姿信息估计单元18测量过程中，视觉模块12利用双目视觉里程计方法进行定位，当机器人运行到光线较差的室内环境，或正对阳光和灯光等高曝光场景时适应性较差，定位精度降低。根据此特性，信息综合模块21判断视觉里程计相邻帧间检测到的特征点匹配成功的数目决定第三位姿信息权重α3。使用下式计算权重α3(nV为设定阈值)：
[0091]
[0092] 4)所述的第四位姿信息估计单元19测量过程中，当机器人在颠簸路面运行时航迹推算的定位精度下降。根据此特性，信息综合模块21记录惯性测量模块3测量的机器人z轴(垂直机器人平台方向)加速度值历史数据，对z轴加速度进行
带通滤波以去除重
力和平台振动的影响，剩余加速度造成的振幅即为地面起伏造成的。信息综合模块21判断滤波后的平均加速度a决定对振动情况的反应权重α4(aAcc为设定阈值)：
[0093]
[0094] 在步骤S42中，所述信息综合模块21获得的精确位姿P分别以f5Hz输入建图模块22、以f6Hz输入集成估计模块23。作为优选的，频率f5为1Hz，f6为10Hz。
[0095] 在步骤S43中，所述建图模块22，将新增的激光扫描获得的特征点点云数据投影到世界坐标系下，利用特征点点云匹配和注册过程，实现增量式地图构建。具体方法如下：信息综合模块21在k时刻获得机器人相对于世界坐标系{W}的精确位姿Pk，对应k时刻的已完成构建的点云地图Qk。利用特征点提取和特征关联方法获得从k到k+1时刻新采集的点云数据{qk+1}，同时通过信息综合模块21获得的k+1时刻的位姿为Pk+1。则新构建的点云地图Qk+1计算公式为：
[0096]
[0097] 上式中， 为广义加，表示地图的增量式构建过程； 表示相邻时刻旋转平移过程。建图发布频率f7Hz。作为优选的，频率f7为1Hz。
[0098] 在步骤S44中，所述集成估计模块23结合信息综合模块21的定位信息和建图模块(22)的三维点云地图，获得相对于已构建地图的机器人位姿。具体做法是：利用扫描匹配方法在建图模块22输出的三维点云图中进行位姿估计，结果为PSM，频率与建图发布频率f7一致。信息综合模块21输出的位姿估计结果为P，频率为f5。将PSM利用时间线性插值的方法插入到P定位结果中，获得最终频率为f5的相对于地图的定位信息。作为优选的，扫描匹配采用NDT(Normal Distributions Transform)算法获得基于地图的位姿信息。
[0099] 利用上述机器人定位和建图过程，实现石化工厂厂区包括室内室外的全范围环境建模和地图构建。在完成厂区内不同环境的建模后，对建模数据进行人工标识，当机器人巡检工作过程中使用目标识别方法进行仪器仪表数据读取、主动感知危险环境等，可以大幅度缩小搜索范围并提高检测效率和精度。
[0100] 在步骤S45中，对构建的点云地图模型以八叉树的形式存储，进行滤波、压缩、栅格划分处理，栅格用于存储被占用情况和备用信息，采用人工标识，对模型中的关注区域的区域和目标种类进行不同的标识，获得环境中待检测目标的精确坐标和语义信息。作为优选的，采用VoxelGrid滤波器对点云进行
下采样滤波，选用octomap方法对环境模型进行压缩和
栅格化处理。仪器仪表及其他待检测设备使用人工手动标识，为重点关注的仪器和危险性较大的设备的当前位置及周围区域赋予逐渐降低的占用概率权值，对关心的区域进行不同
颜色的标记。
[0101] 在步骤S46中，当出现更换仪表设备等环境局部变化的情况时，机器人运行到变化后的环境时，仅利用上述环境建模和地图构建系统的建图模块22和集成估计模块23，实现动态更新局部地图。具体做法是：从未更新环境开始运行，利用集成估计模块获得的相对于地图的位姿估计结果仅包含依赖于扫描匹配过程获得的位姿估计PSM。基于此定位结果输入建图模块22，继续用所述的增量式建图方法更新局部变化的环境。当更新到原环境模型中未更新的区域时，扫描匹配方法自动重定位到原环境模型下，完成地图更新。
[0102] 以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。
