【技术实现步骤摘要】
基于GPS引导式粒子滤波的机器人多传感器融合定位方法
[0001]本专利技术属于移动机器人室外定位领域，涉及一种基于GPS引导式粒子滤波的机器人多传感器融合定位方法。
技术介绍
[0002]移动机器人在室外复杂环境下的高精度定位是实现移动机器人包括巡检、安防以及更多高级功能的先验基础之一。定位的准确性和稳定性直接决定了机器人是否可以安全、稳定地完成自主导航任务，而自主导航功能又是机器人实现其预设高级功能的基础。
[0003]在室外环境中，当前主流的定位方法依据是否使用预先建立的环境地图可分为基于地图的定位方式和不基于地图的定位方式。不基于地图的定位方式依靠传感器自身的精度，难以利用环境的纹理特征，不利于机器人后续的路径规划；基于地图的定位方式又可细分为单传感器定位、多传感器融合定位两大类，其中依靠单一传感器的常用定位方法包括：GPS定位、激光雷达定位、视觉定位、惯性导航定位技术。
[0004]GPS定位是一种以人造地球卫星为基础的高精度无线电导航的定位方式，具备在室外无遮蔽场所下进行全方位实时定位的能力，但是...
【技术保护点】
【技术特征摘要】
1.一种基于GPS引导式粒子滤波的机器人多传感器融合定位方法，其特征在于步骤如下：步骤1：对传感器数据进行预处理，即将IMU数据进行平滑滤波，将差分GPS获取的经纬度坐标以及偏航角转化为UTM坐标系下；步骤2：遥控机器人运动，记录起点处GPS的定位信息构造GPS里程计，并在起点处建立全局点云地图坐标系Map系与GPS东北天坐标系gps系；将预处理后的IMU数据、激光雷达数据和GPS里程计数据作为3D SLAM算法的输入，输出为全局点云地图，并保存全局点云地图为pcd格式，此地图是3D点云地图，无需进行2D投影；步骤3：计算机器人在GPS东北天坐标系gps系下的定位数据:式中，为k时刻GPS在UTM坐标系下的定位数据；将机器人在GPS东北天坐标系gps系中的位置信息变换到全局点云地图坐标系Map系下：计算GPS数据变换到全局点云地图坐标系下的定位数据，并发布该定位数据，后续步骤用到的GPS定位数据均为该数据：步骤4：加载构建好的全局点云地图，利用激光雷达、IMU和GPS的定位数据基于粒子滤波框架实现融合定位，具体步骤如下：步骤(1)初始化粒子步骤(2)对每个粒子的状态进行预测步骤(3)通过获取的激光雷达数据与IMU数据计算每个粒子的权值对于激光雷达数据，利用体素降采样过滤点云数据，将过滤后的点云数据利用正态分布变换法NDT与3D点云地图中每个粒子位姿处的点云匹配，并利用得分函数h1(
·
)计算一次迭代配准的得分，计算粒子权值：其中，为上一时刻粒子权值，为测量概率，z
laser
指当前时刻经过体素降采样后的激光雷达数据，h1(
·
)指激光雷达数据通过NDT一次迭代配准的得分函数，指粒子状态的预测值；对于IMU数据，计算粒子权值：
其中，为上一时刻粒子权值，为测量概率，指粒子状态的预测值，z
imu
是当前时刻的IMU的量测数据，h2(
·
)是IMU的观测方程，Q
k+1
是IMU观测协方差矩阵；所述IMU的观测方程h2(
·
)：其中，δ
k+1
是IMU的观测噪声，δ
k+1
的协方差为Q
k+1
，Q
k+1
用于衡量当前时刻IMU测量误差的大小；步骤(4)对每个粒子的权值进行归一化，获得每个粒子归一化后的权值步骤(4)对每个粒子的权值进行归一化，获得每个粒子归一化后的权值步骤(5)对粒子进行重采样，复制权值较高的粒子，去除权值较低的粒子，获取机器人位置的真实概率分布；步骤(6)发布当前时刻机器人的定位结果Φ
k+1
：步骤(7)添加GPS粒子，执行完转步骤(2)进行下一次预测，直至结束；经过重采样后的粒子在GPS量测下的权值经过重采样后的粒子在GPS量测下的权值其中g(
·
)为GPS的观测方程：z
k+1
指当前时刻GPS的量测数据，Σ
k+1
指GPS测量噪声的协方差矩阵，指当前时刻第i个粒子的状态；
定义在GPS量测数据下，权值的粒子称之有效GPS粒子，计算有效GPS粒子数占总粒子数的百分比N
g
：判断：当有效GPS粒子数占比N
g
小于阈值N
th
时，向粒子群中添加m个粒子；当有效GPS粒子数占比N
g
大于阈值N
th
时，目前粒子的位姿与GPS的定位数据接近，不需要添加新粒子，返...
【专利技术属性】
技术研发人员：唐炜，刘恩博，王增辉，吴佳乐，张仁远，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。