一种石化工厂巡检机器人定位系统和方法与流程

本发明属于特种机器人定位领域，特别涉及了一种石化工厂巡检机器人定位系统和方法。

背景技术：

石化企业、油库、储运公司等工厂的工作区域面积大、环境形式复杂，安全隐患不易排查。目前只能通过人工对各个罐区、泵区、变电所等工作场合的仪器仪表进行徒步现场检测。石化工厂需要巡检的内容多样，通常包括输油管路、储油罐底部、泵站、计量间、流量间、防火通道等各类工作场合，对各种阀门、仪表、设备进行安全排查等。这些待检内容存在于室内与室外、结构化与非结构化、白天和夜间、照明情况较好和较差、阴雨风雪，甚至可能出现有烟尘、水汽等恶劣环境中，给巡检任务带来巨大的工作强度和危险性。采用自主巡检机器人代替人工执行巡检任务，可以极大的降低工人工作强度，降低工作风险。

现阶段巡检机器人在变电站、室内工作区域等结构化条件较好的环境已经取得应用和推广，但是在石化行业的应用却进展缓慢，原因是相较于变电站与室内环境，石化工厂环境非结构化程度更高，需要巡检的内容和场所类型和环境更复杂。另外还要满足白天和夜间、雨雪等恶劣天气的巡检工作。由于巡检任务的复杂性，机器人必须具有很强的定位能力，既要可以适应各类复杂场合和环境条件，同时必须严格保证定位精度，从而确保机器人在自主导航和运动过程中不会与危险设备或其他物体碰撞。在运行到场景变化、环境特征发生较大改变的区域时，定位效果不能发生震荡或失效情况。石化工厂自主巡检机器人在各类复杂环境的高精度、高可靠性定位对于机器人的自主性和安全性起着至关重要的作用。

目前石化行业应用的巡检机器人较少。其他领域巡检机器人常用的定位方法有以下几种：

1.磁条引导加射频识别RFID辅助定位：此类方法为目前工厂AGV导引车和变电站巡检机器人常用的定位方式，通过预先铺设磁轨和RFID标签实现机器人定位，主要缺点是需要预先布置磁条，施工工作量和后期维护成本高，巡检路径固定不灵活。而且只适应与平坦路面，对于地形复杂区域适应性差。石化工厂厂区面积大，后期改造难度大，而且地形复杂，使用磁条导航成本高，且无法满足复杂地形下的精确定位。

2.GNSS/INS组合定位：此类方法在车载系统、户外移动机器人和无人机广泛应用，利用高精度GPS与IMU进行组合定位，主要缺点是设备成本高，而且由于GPS存在多路径效应，其对于设备和建筑物林立的巡检场所适应性差，在室内等有遮挡环境下无法使用。石化工厂流量间、计量间、变电所等室内巡检环境多，而且储油罐、泵区设备等林立，单纯使用GPS定位精度难以保证。

3.航迹推算定位：此类方法主要应用于室内移动机器人等领域，利用里程计或惯性测量单元建立运动模型通过航迹推算进行位姿估计，主要缺点是随着运动范围的扩大，航迹推算的累积误差增大，逐渐丧失定位能力，而且对于地面崎岖不平的适应性差。石化工厂存在崎岖、斜坡等路面，单独利用航迹推算定位累积误差大。

4.二维激光SLAM定位：此类方法主要应用于室内移动机器人和变电站巡检机器人上，利用激光SLAM方法实现同步定位和地图构建，主要缺点是环境适应能力不强，对特征稀疏的场合无法实现精确定位，对于崎岖路面适应性差。石化工厂存在室内较长范围走廊、室外防火堤内等特征少的场合，二维激光SLAM方法无法适应此类环境，同时激光对于雨天工作的适应性差。

5.视觉SLAM定位：此类方法主要应用于室内扫地机器人等场合，主要缺点是对光照敏感，无法在夜间工作，在机器人快速旋转等场合容易定位失败。石化工厂巡检机器人需要在夜间工作，使用视觉SLAM实现困难。

此外，也有部分研究针对多传感器融合的方法实现机器人定位，但是仍然是针对特定场合的组合定位应用，通常应用于传感器信号稳定、传输状况较好的条件下，对于信号丢失情况下的组合定位，其稳定定位能力不强。

总之，目前对于石化工厂巡检机器人在的定位问题，尚无可以同时在室内与室外、平坦与崎岖路面、白天与夜间、特征稀疏与明显、雨雪天气等各类复杂场合实现精确、稳定的定位的巡检机器人定位系统。另外重要的一点，石化工厂使用的设备还需要满足防爆安全要求，对于现阶段使用的各类定位设备，都难以满足直接应用到此类环境区域。

技术实现要素：

为了解决上述背景技术提出的技术问题，本发明旨在提供一种石化工厂巡检机器人定位系统和方法，补充研究巡检机器人在石化工厂各种复杂区域运行时的无缝定位问题，保证定位过程的连续性和定位结果的稳定性。

为了实现上述技术目的，本发明的技术方案为：

一种石化工厂巡检机器人定位系统，包括GPS观测单元、UWB观测单元、视觉观测单元、激光观测单元、里程观测单元、组合决策模块、惯性状态估计模块、可用定位观测/SINS组合模块和定位信息融合模块；GPS观测单元、UWB观测单元、视觉观测单元、激光观测单元和里程观测单元分别采集各自的观测数据，并以特定频率发送给组合决策模块；组合决策模块接收环境模型先验信息模块存储的环境地图模型，结合各观测数据进行基于多层地图的定位，判断可用观测类型，并将可用观测数据发送给可用定位观测/SINS组合模块；可用定位观测/SINS组合模块将可用观测数据与惯性状态估计模块建立的对应运动方程进行组合，并将组合结果发送给定位信息融合模块，进行最终的集成位姿估计。

基于上述石化工厂巡检机器人定位系统的定位方法，包括以下步骤：

(1)当机器人开机或人为移动位置时，初始化5个观测单元的观测值zi(0)以及惯性状态估计模块的状态变量空间xi(0)；5个观测单元分别采集观测数据zi(ki)，并以特定频率发送到组合决策模块；下标i＝1,2,3,4,5，代表5个观测单元；

(2)组合决策模块结合各观测单元的观测信息和环境模型先验信息模块存储的环境地图模型进行位姿初步估计，确定可用的观测类型i，然后将可用的观测数据以频率Si发送观测数据zi(ki)以及对应的观测矩阵Ci(ki)到可用定位观测/SINS组合模块；

(3)惯性状态估计模块构建对应各可用观测数据zi(ki)的运动状态变量xi(k)，将xi(k)以相同频率S0发送到可用定位观测/SINS组合模块；

(4)可用定位观测/SINS组合模块将各运动状态变量xi(k)与对应的观测矩阵Ci(ki)分别进行组合，采用虚拟观测方法通过内插和外推将各观测矩阵Ci(ki)统一至频率为S0的观测矩阵Ci(k)，并构建局部滤波器；

(5)定位信息融合模块采用自适应联合滤波融合算法将各局部滤波器的估计结果进行集成组合，获得最终的组合位姿估计结果。

进一步地，环境模型先验信息模块经过原始地图对齐得到GIS电子地图、视觉特征地图和激光点云地图三种类型叠加的地图模型。

进一步地，在步骤(2)中，确定可用观测类型的方法如下：

组合决策模块接收GPS观测单元采集的GPS卫星定位信息，在环境模型先验信息模块提供的GIS电子地图中进行坐标定位，若GPS观测单元连续t1时间内正常在GIS电子地图中定位，即观测卫星数量不少于4颗、位置精度因子小于4，则认为GPS观测单元采集的观测数据为可用观测类型；

组合决策模块接收UWB观测单元采集的UWB无线定位信息，在环境模型先验信息模块提供的GIS电子地图室内环境中进行坐标定位，若UWB观测单元连续t2时间内正常在GIS电子地图中定位，即RSSI接收信号强度PL(d)＞Rmin，Rmin为设定的信号强度阈值，则认为UWB观测单元采集的观测数据为可用观测类型；

组合决策模块接收视觉观测单元采集的图像特征信息，在环境模型先验信息模块提供的视觉特征地图中进行特征匹配定位，若视觉观测单元检测到的环境中经过剔除错误匹配后的特征关键点对数目大于设定阈值N3，则认为视觉观测单元采集的观测数据为可用观测类型；

组合决策模块接收激光观测单元采集的环境点云信息，在环境模型先验信息模块提供的激光点云地图中进行点云地图配准定位，若激光观测单元在激光点云匹配过程中，经过噪声去除错误匹配后，成功匹配的特征点对数量大于设定阈值N4，则认为激光观测单元采集的观测数据为可用观测类型。

进一步地，惯性状态估计模块测量垂直于机器人平台的z轴加速度输入组合决策模块，判断里程观测单元的观测数据是否为可用观测类型：

组合决策模块接收里程观测单元采集编码器反馈的里程信息，在环境模型先验信息模块提供的GIS电子地图中根据航迹推算方法进行定位，若惯性状态估计模块输出的z轴加速度平均变化率在ΔT时间内小于设定阈值则认为里程观测单元的观测数据是可用观测类型。

进一步地，在步骤(4)中，若GPS观测单元采集的观测数据为可用观测类型，相应的局部滤波器使用GPS与SINS定位的位置差和速度差构建观测方程z1；若UWB观测单元采集的观测数据为可用观测类型，相应的局部滤波器使用UWB与SINS定位的位置信息差构建观测方程z2；若视觉观测单元采集的观测数据为可用观测类型，相应的局部滤波器使用视觉与SINS定位的位置差和姿态差值构建观测方程z3；若激光观测单元采集的观测数据为可用观测类型，相应的局部滤波器使用激光与SINS定位的位置差和姿态差构建观测方程z4；若里程观测单元的观测数据是可用观测类型，相应的局部滤波器使用航迹推算与SINS定位的位置差和姿态差构建观测方程z5。

进一步地，步骤(5)的具体步骤如下：

(51)根据组合决策模块判断的可用观测类型，确定组合信息分配系数βi，设置各可用观测类型局部滤波器初值；

(52)将全局状态估计协方差矩阵Pf及公共噪声矩阵Qf按信息分配原则分配到各个局部滤波器中；

(53)经过时间同步后的各可用观测方程zi分别与惯性状态估计模块输出的运动状态变量xi进行局部位姿状态估计，得到局部滤波结果其中为局部状态估计，Pi为对应的协方差矩阵，i∈[1,5]；

(54)将各局部滤波定位结果从局部坐标系转换到机器人质心坐标系下，结合各可用局部位姿估计结果与主滤波器反馈结果，利用主滤波器进行组合估计，得到全局最优估计

(55)重置各局部滤波器和主滤波器的状态估计和协方差矩阵，进行下一迭代周期的位姿估计。

进一步地，在步骤(51)中，所述组合信息分配系数βi满足信息守恒原理：

其中，βm为主滤波器的信息分配系数，根据场地特点进行设定；

对于组合决策模块判断的不可用观测类型，对应的βi＝0，可用观测类型对应的βi按照信息守恒等式平均分配。

进一步地，在步骤(55)中，局部坐标系到机器人质心坐标系的转换是利用各观测单元的安装位置到机器人质心坐标系的坐标转换关系确定的。

采用上述技术方案带来的有益效果：

本发明通过各种定位子系统的组合，实现了巡检机器人在石化工厂的室内与室外、平坦与崎岖路面、白天与夜间、特征稀疏与明显、雨雪天气等各类复杂场合实现精确、稳定的应用，保证了机器人的全场景全工况下的无缝定位。通过将各类观测传感器的数据先与惯性状态估计模块集成估计，获得了频率均为惯性状态估计模块输出频率的观测方程，保证了不同频率传感器观测的同步。利用自适应联合滤波算法实现了在不同环境下自动判断可用定位观测类型，利用此场景下恰当的传感器进行组合定位，获得了小于单独使用其中任一定位方法误差的定位效果，保证了无缝定位的精确性。

附图说明

图1是本发明的系统组成框图；

图2是本发明的方法流程图；

图3是面向巡检任务的自适应联合滤波融合算法原理图；

图4是自适应联合滤波算法的步骤流程图；

主要标号说明：

1、GPS观测单元；2、UWB观测单元；3、视觉观测单元；4、激光观测单元；5、里程观测单元；6、环境模型先验信息模块；7、组合决策模块；8、惯性状态估计模块；9、可用定位观测/SINS组合模块；10、定位信息融合模块。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

如图1所示，本发明设计的石化工厂巡检机器人定位系统，包括GPS观测单元1、UWB观测单元2、视觉观测单元3、激光观测单元4、里程观测单元5、环境模型先验信息模块6、组合决策模块7、惯性状态估计模块8、可用定位观测/SINS组合模块9和定位信息融合模块10。

组合决策模块7接收环境模型先验信息模块6存储的环境地图模型，结合各观测数据进行基于多层地图的定位，判断可用观测类型，并将可用观测数据发送给可用定位观测/SINS组合模块9；可用定位观测/SINS组合模块9将可用观测数据与惯性状态估计模块8建立的对应运动方程进行组合，并将组合结果发送给定位信息融合模块10，进行最终的集成位姿估计。

如图2所示，基于上述定位系统的定位方法，具体步骤如下。

步骤S21、机器人开机或者人为移动位置时，初始化观测单元1-5的观测值zi(0)以及惯性状态估计模块8的状态变量空间xi(0)。各观测单元1-5分别采集观测信息zi(ki)，以特定频率发送到组合决策模块7。

GPS观测单元1在机器人运行在室外和无遮挡环境且GPS信号可连续接收时，自动采集GPS信号并发送给组合决策模块7。在本实施例中，发送频率1Hz。

UWB观测单元2在机器人运行在预先布设UWB无线传感网络的室内环境且UWB信号可连续接收时，自动采集UWB信号并发送给组合决策模块7。在本实施例中，发送频率设定为1Hz。

视觉观测单元3在机器人运行过程中，自动采集图像信息并发送给组合决策模块7。在本实施例中，发送频率20Hz。

激光观测单元4在机器人运行过程中，自动采集激光点云信息并发送给组合决策模块7。在本实施例中，采集频率设定为10Hz。

里程观测单元5在机器人运行过程中，自动采集里程数据并发送给组合决策模块7。在本实施例中，采集频率设定为20Hz。

环境模型先验信息模块6经过原始地图对齐得到GIS电子地图、视觉特征地图、激光点云地图三种类型地图叠加的地图模型。在本实施例中，GIS电子地图利用ARCGIS软件二次开发构建厂区三维空间数据库，在遥感航测影像GIS电子地图基础上，将室内UWB锚节点信号覆盖范围下的环境使用人工测绘数据进行存储并叠加到GIS电子地图；其中的稀疏特征点地图通过使用视觉ORB-SLAM进行稀疏点环境建模获得并叠加到GIS电子地图上；其中的激光点云地图通过使用激光里程计LOAM(Lidar Odometry and Mapping)方法获得并叠加到GIS电子地图上。制图时采集的各类型传感器数据同步进行并通过坐标转换统一到世界坐标系下，以此获得三层地图叠加并对齐的环境模型。

步骤S22、组合决策模块7结合各观测单元的观测信息和环境模型先验信息进行位姿初次估计，确定可用的观测类型i(i表示可用观测单元序号)，然后将可用观测i以频率Si发送观测zi(ki)到可用定位观测/SINS组合模块9。

组合决策模块7根据接收到的各个定位观测单元获得的传感器数据，以及环境模型先验信息模块6的多层地图信息，确定当前可用的传感器类型。具体方法如下：

组合决策模块7接收GPS观测单元1采集的GPS卫星定位信息，在环境模型先验信息模块6提供的GIS电子地图中进行坐标定位，判断组合方案是否包含GPS观测信息。若GPS观测单元连续t1时间内正常在GIS电子地图中定位(观测卫星数量不少于4颗、位置精度因子小于4)，则认为当前运动位置的组合方案包含GPS观测信息。组合决策模块以频率S1发送观测z1(k1)，对应观测矩阵C1(k1)、观测噪声v1(k1)。

组合决策模块7接收UWB观测单元2采集的UWB无线定位信息，在环境模型先验信息模块6提供的GIS电子地图室内环境中进行坐标定位，判断组合方案是否包含UWB观测信息。若UWB观测单元连续t2时间内正常在GIS电子地图中定位(RSSI接收信号强度PL(d)＞Rmin)，Rmin为设定的信号强度阈值，则认为当前运动位置的组合方案包含UWB观测信息。组合决策模块以频率S2发送观测z2(k2)，对应观测矩阵C2(k2)、观测噪声v2(k2)。

组合决策模块7接收视觉观测单元3采集的图像特征信息，在环境模型先验信息模块6提供的特征地图中进行特征匹配定位，判断组合方案是否包含视觉观测信息。若视觉观测单元3检测到的环境中经过剔除错误匹配后的特征关键点数目大于指定阈值N3，则认为当前运动位置的组合方案包含视觉观测信息。组合决策模块以频率S3观测z3(k3)，对应观测矩阵C3(k3)、观测噪声v3(k3)。作为优选的，特征匹配采用ORB特征点，利用随机采样一致性算法(RANSAC)剔除错误的配准点对，利用ORB-SLAM的定位模式实现机器人位姿估计。

组合决策模块7接收所述的激光观测单元4采集的环境点云信息，在环境模型先验信息模块6提供的点云地图中进行点云地图配准定位，判断组合方案是否包含激光观测信息。若激光观测单元4在激光点云匹配过程中，经过噪声去除错误匹配后，成功匹配的特征点对数量大于指定阈值N4，则认为当前运动位置的组合方案包含激光观测信息。组合决策模块以频率S4发送观测z4(k4)，对应观测矩阵C4(k4)、观测噪声v4(k4)。在本实施例中，特征匹配时采用查询点邻域法向量作为特征，使用kd树最近邻查询方法进行匹配过程，点云地图配准使用迭代最近点(ICP)方法实现机器人位姿估计。

组合决策模块7接收里程观测单元5采集的编码器反馈的里程信息，在环境模型先验信息模块6提供的GIS电子地图中根据航迹推算方法进行定位，判断组合方案是否包含里程观测信息。若惯性状态估计模块8输出的z轴加速度平均变化率在ΔT时间内小于设定阈值则认为当前运动位置的组合方案包含里程观测信息。组合决策模块以频率S5观测z5(k5)，对应观测矩阵C5(k5)、观测噪声v5(k5)。

步骤S23、惯性状态估计模块分别构建对应各个观测观测zi(ki)的状态空间变量xi(k)，状态转移矩阵Ai(k)，系统噪声项wi(k)。将各个运动预测xi(k)以相同频率S0发送到可用定位观测/SINS组合模块(9)。

惯性状态估计模块8根据不同的观测构建机器人运动预测状态信息xi(k)，输入到多传感器同步模块9。同时测量垂直于机器人平台的z轴加速度输入组合决策模块7，判断组合方案是否包含里程观测信息。在本实施例中，惯性状态估计发送频率S0＝100Hz。

步骤S24、可用定位观测/SINS组合模块(9)将惯性状态估计模块发送的各SINS运动预测xi(k)，与各个不同频率可用观测单元发送的Ci(ki)分别进行比较，利用虚拟观测法获得尺度均为S0的同频率的观测结果Ci(k)，构建局部滤波器。

可用定位观测/SINS组合模块9将各可用观测单元的观测数据Ci(ki)分别与惯性状态估计模块(8)的xi(k)进行组合构建局部滤波器，经过离散化后，系统为：

xi(k+1)＝Ai(k)xi(k)+Γi(k)wi(k)

zi(ki)＝Ci(ki)xi(ki)+vi(ki)

其中，xi(ki)＝x(ni(ki-1)+1)；ni为SINS与各个观测单元频率的采样率比值；xi(k)为对应第i种估计方法的被估计位姿状态，wi(k)为过程噪声矩阵，Γi(k)为噪声输入矩阵，对应过程噪声方差阵为Qi(k)；zi(ki)为观测方程，Ci(ki)表示观测矩阵；观测误差vi(ki)是零均值高斯白噪声，方差为Ri(ki)。

采用虚拟观测方法通过内插和外推将各观测单元的观测频率统一到SINS下，得到各自观测下的定位估计方程：

xi(k+1)＝Ai(k)xi(k)+Γi(k)wi(k)

zi(k)＝Ci(k)xi(k)+vi(k)

其中，为各种观测构建的各子滤波器的公共状态的估计，即SINS中定位位置、速度和姿态误差以及陀螺仪和加速度计零偏的状态空间。为第i个滤波器特有的状态估计。xc通过联合滤波器两级次优滤波估计得到最优全局估计，xsi在子滤波器中进行一次滤波输出估计值。

局部滤波器系统的状态方程可以表示为：

其中，

SINS系统的平台姿态误差速度误差位置误差陀螺常值零位漂移ε^T，加速度计常值误差陀螺仪白噪声加速度计白噪声

局部滤波器1使用GPS与SINS形成的位置差与速度差构建观测方程z1：

z1(k)＝C1(k)x1(k)+v1(k)

x1(k)＝xc(k)

其中，为GPS的速度误差，为位置误差。

局部滤波器2使用UWB与SINS定位获得的位置信息差构建观测方程z2：

z2(k)＝C2(k)x2(k)+v2(k)

x2(k)＝xc(k)

其中，为UWB姿态误差。

局部滤波器3使用视觉定位与SINS定位的位置差和姿态差值构建观测方程z3：

z3(k)＝C3(k)x3(k)+v3(k)

其中，为视觉尺度因子，为相机定位估计的姿态误差，为相机定位估计的位置误差。

局部滤波器4使用激光定位与SINS定位的位置差和姿态差构建观测方程z4：

z4(k)＝C4(k)x4(k)+v4(k)

其中，为激光雷达定位估计的姿态误差，为激光雷达定位估计的位置误差。

局部滤波器5使用航迹推算与SINS的姿态差和位置差构建观测方程z5：

z5(k)＝C5(k)x5(k)+v5(k)

其中，为航迹推算姿态误差，为位置误差，δK为里程计刻度系数误差。

步骤S25、定位信息融合模块10采用一种面向巡检任务的自适应联合滤波(Federated Filter)融合算法，将各观测构建的局部滤波器估计的进行集成组合，获得最终的组合位姿估计结果

图3为面向巡检任务的自适应联合滤波融合算法原理图，通过各观测单元1-5发送的观测信息z1～z5，与惯性状态估计模块8发送的运动状态估计信息x1～x5分别进行组合，输入到对应局部滤波器。局部滤波器i结合运动方程xi、观测方程zi以及反馈的主滤波器信息进行局部滤波定位估计。输出的局部滤波结果通过主滤波器进行联合滤波，获得最终的位姿估计输出

图4为自适应联合滤波算法的步骤流程图，具体过程为：

步骤S41、根据组合决策模块7判断的可用观测类型，确定组合信息分配系数βi，设置各可用观测局部滤波器初值。

组合信息分配系数βi满足信息守恒原理：

其中βm为主滤波器的信息分配系数，根据实验场地的特点进行人工指定(0.1＜βm＜0.3)。对于组合决策模块7判断的不可用观测，对应的βi＝0，其余可用观测对应的βi按照信息守恒等式进行平均分配。

各可用局部滤波器和主滤波器的协方差P、噪声方差R初值设定为：

步骤S42、将全局状态估计协方差矩阵Pf及公共噪声矩阵Qf按信息分配原则分配到各个局部滤波器中：

步骤S43、经过时间同步后的各可用观测zi分别与SINS的运动状态xi进行局部位姿状态估计得到可用局部滤波结果

局部滤波器和主滤波器分别进行状态预测和更新：

预测过程：

更新过程：

Pi(k|k)＝[In-Ki(k)Ci(k)]Pi(k|k-1)

In是单位阵；

主滤波器更新：

Pm(k+1)＝Pm(k|k-1)

步骤S44、将各局部滤波定位结果从局部坐标系转换到机器人质心坐标系下，结合各可用局部位姿估计结果与主滤波器反馈结果，利用主滤波器进行组合估计得到全局最优估计

局部坐标系到机器人质心坐标系的转换，利用各传感器安装位置到机器人质心坐标系的坐标转换关系进行确定。

主滤波器进行自适应组合信息融合：

步骤S45、将主滤波器的融合定位结果转换回各传感器坐标系所在局部坐标系，利用坐标变换重置各局部滤波器和主滤波器的定位估计值和协方差矩阵，进行下一迭代周期的位姿估计。

实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。