一种基于GPS、5G以及视觉的石化巡检机器人导航定位方法与流程

本发明涉及了一种基于gps、5g以及视觉的石化巡检机器人导航定位方法，属于机器人导航定位领域。

背景技术：

随着当前社会的发展和技术的进步，机器人自动巡检技术得到空前发展和广泛应用。很多场合需要用到自动巡检技术，诸如石化工厂、油田、天然气站场、输油场站等。这些场所兼具工作场地面积大、工作环境复杂、各种安全隐患不易排查等特点，给人工排查带来了极大的工作难度和工作强度。不仅如此，传统的人工巡检，不仅效率低，工作量大，而且由于不确定性因素的存在，甚至在某些特定场合具有一定的危险性。一方面，人工巡检会导致一些比较隐蔽的故障没有及时排查，很可能给公司和企业造成重大财产损失甚至人员伤亡。另一方面，人工排查效率较低。而对于这些工作场所的巡检，排查的内容可能存在于室内室外，工作环境复杂多样，会遇到阴雨雷雪等恶劣天气。对于上述问题，如果能用巡检机器人代替人工作业，不仅可以降低员工的工作难度和工作强度，而且可以在一定程度上避免因危险环境作业而导致的人员伤亡。由此可见，研究一种巡检功能强大，适用范围广，对环境有一定的适应能力，检测结果准确的智能巡检机器人势在必行。

自主导航定位问题是智能巡检机器人技术研究的关键所在，是在机器人实践作业中解决“我在哪儿”的问题，这不仅关系到巡检机器人能否按照预定方案完成作业，而且定位的精准和迅速已经成为移动机器人能否运用到实践中的决定性因素。

目前有很多与巡检机器人相关的理论和研究，并且已经取得了一定的应用和发展。下面介绍几种导航定位方法及其优缺点：

（1）基于激光雷达+slam技术导航定位方法：此类方法应用于室内移动机器人和变电站机器人，其主要缺点是巡检机器人观测范围有限，且对环境的适应能力不够，对特征稀疏的某些场合无法做到精准定位，且激光技术受天气影响较大，对于天气变化的适应性差。

（2）基于灯光追踪与红外图像的导航定位方法：该方法和磁条引导加射频识别rfid，尽管此方法导航定位精度较高，在一定范围内对环境适应性较好。但是需要预先布置导航灯，加大了工作难度，并且后期维护成本较高。再者只能在置有导航灯的环境中工作，在此之外，基本丧失导航定位能力或者导航精度将受到影响。

（3）基于北斗卫星的gps轨迹定位方法：当前gps轨迹定位方法均采用单频定位，且数据来源单一，在数据异常情况下容易出现定位精度的问题。

此外也有部分研究多传感器融合定位的方法，但大多只针对于特定场合，在大多数情况下，难以保证其信号稳定，对于一般场合的适应性较差。

采用多传感器数据融合，不仅可以加大导航定位的精度，而且对于普遍的工作环境有很好的适应性。目前我国5g技术飞速发展，相对于4g而言，5g具有高速、低功耗的、泛在网和低时延等优点，给通讯和导航定位等领域带来了极大的便利。与传统非线性滤波方法相比，粒子滤波直接采用非线性模型，使得粒子滤波具有更高灵活性和更优估计精度。因此粒子滤波技术逐渐在非线性研究和非高斯系统最优估计问题中取得独特地位，但由于粒子滤波仍然存在一些问题，诸如需要大量的样本数量，且大量的样本数量对应的算法复杂度高，而且重采样阶段会造成样本有效性和多样性的损失，导致样本贫化现象。为解决这类问题，我们提出一种基于并行隐式等权粒子滤波和5g等多传感器融合的导航定位方法。

技术实现要素：
：

为了解决上述背景技术提出的技术问题，本发明旨在提供一种基于gps、5g以及视觉的石化巡检机器人导航定位方法，实现石化巡检机器人在各种复杂环境运行时的无缝精准定位，保证巡检过程中的快速定位与精准定位。

为了实现上述技术目的，一种基于gps、5g以及视觉的石化巡检机器人导航定位方法，由gps模块、5g模块、视觉模块、里程计模块、gps判断模块、5g判断模块、视觉判断模块、滤波器a、滤波器b、滤波器c、分布式多传感器融合模块a、分布式多传感器融合模块b、融合信息定位模块和环境特征地图组成，具体方法为：

（1）gps模块采集量测信息以特定频率发送到gps判断模块，5g模块采集量测信息以特定频率发送到5g判断模块、视觉模块采集量测信息以特定频率发送到视觉判断模块；

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

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

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

状态更新：

方差更新：

（5）由分布式多传感器融合模块a接收来自滤波器a的局部状态估计、局部协方差和来自滤波器b的局部状态估计、局部协方差，

分布式多传感器融合模块a利用最优信息融合准则得到组合状态估计为：

组合协方差表达式为：

其中为最优权值矩阵,,

，，是4×4单位矩阵；将融合后的组合状态估计和组合协方差发送到融合信息定位模块；

由分布式多传感器融合模块b接收来自滤波器b的局部状态估计、局部协方差和滤波器c的局部状态估计、局部协方差，分布式多传感器融合模块b利用最优信息融合准则得到组合状态估计为：

组合协方差表达式为：

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

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

进一步地，基于上述所述的导航定位方法，在步骤（3）中，采用调节α和β两个参数对每个粒子进行更新：，由于是粒子的确定性移动，经坐标转换,

，其中为雅可比矩阵行列式的绝对值，中，当j=1,2,3时分别表示滤波器a、滤波器b和滤波器c在k-1时刻输出的局部协方差，是从标准多元高斯分布中得出，是一个随机向量，、以及均为标准正态分布n(0,1)，n(0,1)是期望为0，标准差为1的概率分布。

进一步地，基于上述所述的定位方法，在步骤（4）中，所述的并行隐式等权粒子滤波定位算法方法如下：

①初始化k=0,从先验分布p（x（0））中采样m个样本，得到,m=1,2,3,…,m,对应的权值均为1/m；

②采样得到k时刻的样本，该样本符合重要性密度函数；

③根据量测模型计算粒子权重：

进行归一化权值：

④输出局部状态估计和局部协方差；

⑤当下一时刻量测值到来时，返回步骤②；

进一步地，基于上述所述的导航定位方法，环境特征地图经过原始地图对应的映射矩阵生成3d点云地图模型。

本发明使用并行隐式等权粒子滤波定位算法，具有更高灵活性和更优估计精度，解决了一般粒子滤波算法需要大量样本、算法复杂度高、容易出现样本贫化等缺点；融合5g技术进行定位，延时短，精度高，能很好地支持室内定位，扩展了巡检机器人的应用场景；采用多传感器数据融合，提升导航定位的精度，而且速度高、消耗低、时延低，对于石化巡检机器人工作环境有很好的适应性，保证了机器人的全场景全工况下的无缝连接。

附图说明

图1是本发明的方法的组成部分。

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

图中：1、gps模块，2、5g模块，3、视觉模块，4、里程计模块，5、gps判断模块，6、5g判断模块，7、视觉判断模块，8、滤波器a，9、滤波器b，10、滤波器c，11、分布式多传感器融合模块a，12、分布式多传感器融合模块b，13、融合信息定位模块，14环境特征地图。

具体实施方式

下面结合附图1、2对本发明所述定位方法做进一步说明。

基于上述导航定位系统的定位方法，其具体实施步骤如下：

步骤s1，当巡检机器人开机开始正常作业时，初始化机器人状态和gps模块1、5g模块2和视觉模块3的量测信息，其中k代表离散时间，中，当i=1，2，3时分别表示gps模块1、5g模块2、视觉模块3量测信息；

步骤s2，在机器人正常的运行过程中，gps模块1在石化巡检机器人正常运行过程中，以特定频率自动采集gps量测信息并传送给gps判断模块5,gps判断模块5判定量测信息是否有效；5g模块2在石化巡检机器人正常运行的过程中，以特定频率自动采集5g量测信息并传送给5g判断模块6，5g判断模块6判定量测信息是否有效；视觉模块3在石化巡检机器人正常运行过程中，以特定频率自动采集图像信息数据并传送给视觉判断模块7，视觉判断模块7判定量测信息是否有效，其具体的判定方法在步骤s4中详细介绍；

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

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

采用调节α和β两个参数对每个粒子进行更新：，由于是粒子的确定性移动，经坐标转换,则重要性权值为

，其中为雅可比矩阵行列式的绝对值，中，当j=1,2,3时分别表示滤波器a8、滤波器b9和滤波器c10在k-1时刻输出的局部协方差，是从标准多元高斯分布中得出，是一个随机向量，、以及均为标准正态分布n(0,1)，n(0,1)是期望为0，标准差为1的概率分布。

gps判断模块5用于实时判定gps模块1是否可以至少接收四个及以上卫星发送的时间戳，若可以接收四个及以上卫星发送的时间戳，则此次量测信息可用，gps判断模块5传送量测信息给滤波器a8；反之此次量测信息为不可用，gps判断模块5取消此次数据传送；

5g判断模块6判定5g模块2的量测信息是否可用，具体方法如下：如果量测信息是接收两个以上基站信息而获得的，则此次量测信息为可用，5g判断模块6将此次量测信息传送给滤波器b9；反之为不可用，5g判断模块6将取消此次数据传送；

视觉判断模块7用于判定视觉模块3的采集量测信息是否有效。具体实施过程为：实时提取从相机采集到的图像帧中的特征点，再对提取到的特征点与3d点云地图的特征点相似度进行匹配，若相似度匹配度没有超过设定阈值t时，则此次量测信息为可用信息，视觉判断模块）将此次量测信息传送给滤波器c10；否则，此次量测信息不可用，视觉判断模块7将取消信息传送。

步骤s5，滤波器a8、滤波器b9和滤波器c10利用并行隐式等权粒子滤波定位算法建立机器人误差模型和量测模型获得相应的局部状态估计和局部协方差，其中中，当j=1，2，3时分别表示滤波器a8、滤波器b9、滤波器c10输出的石化巡检机器人的局部状态估计；中，当j=1，2，3时分别表示滤波器a8、滤波器b9、滤波器c10输出的石化巡检机器人的局部协方差；滤波器a8以特定频率将局部状态估计和局部协方差发送到分布式多传感器融合模块a11；滤波器b9以特定频率将局部状态估计和局部协方差发送到分布式多传感器融合模块a11和分布式多传感器融合模块b12；滤波器c10以特定频率将局部状态估计和局部协方差发送到分布式多传感器融合模块b12，

状态更新：

方差更新：

gps判断模块5、5g判断模块6和视觉判断模块7判定对应的量测信息为不可用信息时，假定至少有两个模块采集到的信息为可用数据，分布式多传感器融合模块a11、分布式多传感器融合模块b12的情况如下：

当gps判断模块5判定gps模块1量测信息不可用时，gps判断模块5取消将量测信息传送给滤波器a8。此时，分布式多传感器融合模块a11只接收来自滤波器b9的局部状态估计和局部协方差；

当5g判断模块6判定5g模块7量测信息不可用时，5g判断模块6取消将量测信息传送给滤波器b9。此时，分布式多传感器融合模块a11只接收来自滤波器a8的局部状态估计和局部协方差；分布式多传感器融合模块b12只接收来自滤波器c10的局部状态估计和局部协方差；

当视觉判断模块7判定视觉模块3量测信息不可用时，视觉判断模块7取消将量测信息传送给滤波器c10。此时，分布式多传感器融合模块b12只接收来自滤波器b9的局部状态估计和局部协方差。

步骤s6，由分布式多传感器融合模块a11接收来自滤波器a8的局部状态估计、局部协方差和来自滤波器b9的局部状态估计和局部协方差

分布式多传感器融合模块a11利用最优信息融合准则得到组合状态估计为：

组合协方差表达式为：

其中为最优权值矩阵,,

，，是4×4单位矩阵；

将融合后的组合状态估计和组合协方差发送到融合信息定位模块13；

由分布式多传感器融合模块b12接收来自滤波器b9的局部状态估计、局部协方差和滤波器c10的局部状态估计、局部协方差；

分布式多传感器融合模块b12利用最优信息融合准则得到组合状态估计为：

组合协方差表达式为：

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

当分布式多传感器融合模块a11或者分布式多传感器融合模块b12只接收一个滤波器的处理信息时，其本身不做任何数据处理，只作为数据处理信息的一个传送通道，将滤波器得到的局部状态估计和局部协方差传送给融合信息定位模块13。

步骤s7，融合信息定位模块13利用最优信息融合准则将接收到的组合状态估计和组合协方差进行融合，决策出全局最佳状态估计以及全局协方差。

其中

，，为最优权值矩阵，,；

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