CN114459470B 基于多传感器融合的巡检机器人定位方法 （国网江西省电力有限公司超高压分公司）

WO2020087845A1,2020.05.07US11002859B1,2021.05.11本发明公开了一种基于多传感器融合的巡与惯性测量单元的测量量进行一次EKF数据融位系统的绝对定位信息进行二次EKF数据融合，3D先验地图进行蒙特卡洛定位。本发明基于EKF2重wk-1,m，根据所有粒子的定位信息和权重来加权计算得到机器人的最终定位信息(Xk-1，新后的粒子位姿和当前先验地图测量量来更新每个粒子的权重wk,m，最终得到机器人当前k-1时刻序号m的粒子的权重，Xk为k时刻机器人x轴定位数据，Yk为k时刻机器人y轴定位数k为k-1时刻机器人角度定位数据；S3-1.根据所有样本粒子与融合位姿的欧式距融合位姿（xkal,ykal,ρkal）的欧氏距离来更新当前时刻样本中所有粒子的位姿信息；(Δ3xkal,k,Δykal,k,Δρkalk,mρk-1,m)+(Δxkal,k,Δykal,k,Δρkal,k）+Zk融合位姿为测量量在全局map坐标系下进行二次EKF融合，由此获当前时刻粒子权重，并根据最新权重和步骤1),S3-3中，使用最终计算得到的k时刻的机定位位姿的增量信息ΔXkal,k来进行航迹推算，直到粒子滤波估计位姿重新获取较高匹配,Δykal,k,Δρkal,k)为两次EKF融合定位位姿增量。4有正态分布变换算法(NDT)以及基于特征匹配算法，除此之外还有很多基于深度学习来改曼滤波(EKF)算法进行多传感器融合策略来进行局部和全局的位姿估计。结合融合后验位的粒子匹配度来切换MCL算法的定位与融合位姿航迹推算。由此来保证巡检导航定位的鲁[0008]本发明通过下述技术方案来实现：一种基于多传感器融合的巡检机器人定位方5子根据二次融合位姿(xkal,ykal,ρkal)和方差信息在先验地图中以正态分布采样，二次融合[0015]具体地，步骤S3中，蒙特卡洛定位中的每个粒子都有对应的定位信息(xk-1,m，然后根据更新后的粒子位姿和当前先验地图测量量来更新每个粒子的权重wk,m，最终得到-1,m为k-1时刻序号m的粒子的x轴定位数k-1,m为k-1时刻序号m的粒子的y轴定位数据，ρk-1,m为k-1时刻序号m的粒子的角度定位6xkal,k,Δykal,k,Δρkal,k)ρk-1,m)+(Δxkal,k,Δykal,k,Δρkal,k)+Zk,m；[0024]Δxkal,k为k时刻融合定位位姿x轴的增量，Δykal,k为k时刻融合定位位姿y轴的增重和步骤1)中更新后的粒子k时刻的位姿信息来加权计算机器算法的重要性采样和重采样过程，并且基于先验地图的粒子匹配度来切换MCL算法的定位些复杂空旷环境和遮蔽环境下都能保证导航的安全7人中编码器(Odom)的数据和惯性测量单元(I[0035]在北斗定位系统(BDS)中，数据参考坐标系为世界大地测量系统面笛卡尔参考坐标系下，本发明使用通用横墨卡托格网系统(UTM)来实现角度到距离的度还提供了航向信息，因此需要分别读取定位报文与航向报文。其解算的绝对定位信息为：[0038]惯性测量单元(IMU)能够提供精确的瞬态信息，分为三轴加速度信息和三轴角速8[0043]其中σk,odom为编码器实时方差,h(·)为关于速度的线性方程；σx为编码器x轴方绝对定位信息(xbds,ybds,ρbds)为北斗定位数据，是北斗模块从差分服务端获取到的机器人非线性非高斯系统来说扩展卡尔曼能够给出较好9定位中的粒子可以根据二次融合位姿(xkal,ykal,ρkal)和方差信息在先验地图中以正态分布[0077]蒙特卡洛定位中的每个粒子都有对应的定位信息(xk-1,m，yk-1,m，ρk-1,m)和权重粒子位姿和当前先验地图测量量来更新每个粒子的权重wk,m，最终得到机器人当前时刻的丢失导致的危险发生，本发明在有效粒子数过少的情况下，使用二次k-1,m为k-1时刻序号m的粒子的角度定位数据，wk-1,m为k-1时刻序号m的粒子[0087]为序号m的粒子在k时刻的先验预测状态量,这些预测粒子共同近ρk-1,m)+(Δxkal,k,Δykal,k,Δρkal,k)+Zk,m；[0099]Δxkal,k为k时刻融合定位位姿x轴的增量，Δykal,k为k时刻融合定位位姿y轴的增重并根据最新权重和步骤1)中更新后的粒子k时刻的位姿[0103]重采样过程中，使用最终计算得到的k时刻的机器人后验估计状态量来更新当[0106]即其中xi表示k-1时刻的机器人位姿，ΔXkal,k＝(Δxkal,k,Δykal,k,Δρkal,k)为两次EKF融合定位位姿增