[发明专利]一种基于GPS、5G以及视觉的石化巡检机器人导航定位方法

权利要求书

1.一种基于GPS、5G以及视觉的石化巡检机器人导航定位方法，其特征在于：

(1)GPS模块(1)采集量测信息Z₁(k)以特定频率发送到GPS判断模块(5)，5G模块(2)采集量测信息Z₂(k)以特定频率发送到5G判断模块(6)、视觉模块(3)采集量测信息Z₃(k)以特定频率发送到视觉判断模块(7)；

(2)里程计模块(4)的输出量以特定频率发送到滤波器A(8)、滤波器B(9)和滤波器C(10)，石化巡检机器人以里程计输出导航参数的误差量作为状态量，结合状态量构建里程计误差模型m是粒子序号，且m＝1,2,3,…,M,M是粒子样本总数，是k时刻粒子样本中第m个粒子的状态值，是k-1时刻粒子样本中第m个粒子的状态值，为先验估计，即当前时刻状态仅与上一时刻状态有关；

(3)GPS判断模块(5)结合环境特征地图(14)判定可用量测信息并发送到滤波器A(8)，5G判断模块(6)结合环境特征地图(14)判定可用量测信息并发送到滤波器B(9)，视觉判断模块(7)结合环境特征地图(14)判定可用量测信息并发送到滤波器C(10)，接收到信息的滤波器A(8)、滤波器B(9)和滤波器C(10)采用里程计输出量与GPS模块(1)、5G模块(2)和视觉模块(3)输出的量测信息差值作为量测方程，构建量测模型并计算重要性权值：进行归一化权值处理，得到：其中Z_j(k)中，当j＝1，2,3时分别表示GPS模块(1)、5G模块(2)、视觉模块(3)采集的量测信息，中，当j＝1，2,3时分别表示滤波器A(8)、滤波器B(9)和滤波器C(10)中第m个粒子的重要性权值，中，当j＝1，2,3时分别表示滤波器A(8)、滤波器B(9)和滤波器C(10)中第m个粒子的归一化重要性权值，为似然概率密度函数，即为石化巡检机器人在状态下观察Z_j(k)的概率，p(Z_j(k))为边缘概率，为重要性密度函数，中，当j＝1,2,3时分别表示滤波器A(8)、滤波器B(9)和滤波器C(10)在k时刻粒子样本中第m个样本值，且符合重要性密度函数

(4)滤波器A(8)、滤波器B(9)和滤波器C(10)利用并行隐式等权粒子滤波定位算法建立机器人误差模型和量测模型获得相应的局部状态估计和局部协方差其中中，当j＝1，2，3时分别表示滤波器A(8)、滤波器B(9)、滤波器C(10)输出的石化巡检机器人的局部状态估计；中，当j＝1，2，3时分别表示滤波器A(8)、滤波器B(9)、滤波器C(10)输出的石化巡检机器人的局部协方差；滤波器A(8)以特定频率将局部状态估计和局部协方差发送到分布式多传感器融合模块A(11)；滤波器B(9)以特定频率将局部状态估计和局部协方差发送到分布式多传感器融合模块A(11)和分布式多传感器融合模块B(12)；滤波器C(10)以特定频率将局部状态估计和局部协方差发送到分布式多传感器融合模块B(12)，

更新石化巡检机器人的局部状态估计：

更新石化巡检机器人的局部协方差：

(5)由分布式多传感器融合模块A(11)接收来自滤波器A(8)的局部状态估计局部协方差和来自滤波器B(9)的局部状态估计局部协方差分布式多传感器融合模块A(11)利用最优信息融合准则得到组合状态估计为：

组合协方差估计表达式为：

其中为最优权值矩阵,E＝(e₄，e₄)^T，e₄是4×4单位矩阵；将融合后的组合状态估计X₁(k)和组合协方差P₁(k)发送到融合信息定位模块(13)；

由分布式多传感器融合模块B(12)接收来自滤波器B(9)的局部状态估计和局部协方差以及来自滤波器C(10)的局部状态估计和局部协方差分布式多传感器融合模块B(12)利用最优信息融合准则得到组合状态估计为：

组合协方差估计表达式为：

其中为最优权值矩阵，将融合后的组合状态估计X₂(k)和组合协方差P₂(k)发送到融合信息定位模块(13)；

(6)融合信息定位模块(13)利用最优信息融合准则将接收到的组合状态估计和组合协方差进行融合，决策出全局最佳状态估计X以及全局协方差P，其中为最优权值矩阵，

2.基于权利要求1所述的导航定位方法，其特征在于，在步骤(3)中，采用调节α和β两个参数对每个粒子进行更新：由于是粒子的确定性移动，经坐标转换其中为雅可比矩阵行列式的绝对值，中，当j＝1，2，3时分别表示滤波器A(8)、滤波器B(9)和滤波器C(10)在k-1时刻输出的局部协方差，ξ^m是从标准多元高斯分布中得出，S^m是一个随机向量，ξ^m、S^m以及q(ξ)均为标准正态分布N(0,1)，N(0,1)是期望为0，标准差为1的概率分布。