[发明专利]一种石化工厂巡检机器人定位系统和方法

权利要求书

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

其特征在于，所述定位方法包括以下步骤：

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

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

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

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

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

2.根据权利要求1所述定位方法，其特征在于，环境模型先验信息模块(6)经过原始地图对齐得到GIS电子地图、视觉特征地图和激光点云地图三种类型叠加的地图模型。