CN115388892B 一种基于改进rbpf-slam算法的多传感器融合slam方法 （安徽赛因斯先进技术有限公司）

开发区梅林大道59号未来科技城1#一种基于改进RBPF-SLAM算法的多传感器融本发明公开了一种基于改进RBPF_SLAM算法与建图技术领域，本发明首先通过Keepbot机器用扩展卡尔曼滤波(EKF)有效融合里程计与惯性导航(IMU)数据作为初始位姿信息，其次在计算提议分布时在里程计信息中添加高精度的激光21.一种基于改进RBPF_SLAM算法的多传感器融合SLAM方法，其特征在于，包括如下步传感器融合算法融合轮式里程计数据与IMU数据作为初始位姿信息，利用所述初始位姿信步骤二、计算提议分布时在里程计信息中添加激光雷达观步骤三、引入阈值判断有效粒子数目简化重采样步骤，2.根据权利要求1所述的一种基于改进RBPF_SLAkxTyky扩展卡尔曼滤波下对应的系统状态转移方程如公式中Hik表示IMU的观测矩阵，wik(d)为IMU数据的观测误差的协方3.根据权利要求2所述的一种基于改进RBPF_SL3(5)监听IMU信息作为观测量和观测协方差矩阵，对(4)中得到的系统状态量和系统协(6)将融合的系统状态量和协方差矩阵作为SLAM算法的初始位姿，时刻的传感器信息4.根据权利要求3所述的一种基于改进RBPF_SLAM算法的多首先利用移动机器人的运动模型推算出粒子点k时刻的新位姿基于第i个粒子的初始位姿生成在k_1时刻的地图:然后在附近区域搜索，计算观测zk与已有地图计算匹配度：将采样点集{xt}的分布近似为高斯分布，并将移动机器人的运动模型和激光雷达的观45.根据权利要求4所述的一种基于改进RBPF_SL6.根据权利要求1所述的一种基于改进RBPF_SLAM算征在于，所述移动机器人硬件平台包括树莓派4B处理平台单元、激光雷达传感器单元、4B处理平台相互连接，STM32底层控制单元与IMU传感器单元以及电机驱动单元相互连接；所述树莓派4B处理平台与STM32底层控制单元通过5[0001]本发明涉及移动机器人定位与建图技术领域，具体是一种基于改进RBPF_SLAM算个领域的应用越来越广泛，同时定位与建图(SimultaneousLocalizationandMapping,然被应用到了对SLAM的研究中。Thrun等人将粒子滤波算法引入到移动机器人的SLAM问题[0004](1)Murphy提出了把Rao_BlackwillisedParticleFilter加入SLAM解决方案中，RBPF_SLAM将机器人建图的过程分解成机器人位姿估计和基于位姿的地图估计，大幅度地[0006](3)陈建军、廖小飞等人提出了基于改进小生境遗传优化的RBPF_SLAM算法INGO_RBPF，分别采用改进的RBPF_SLAM方法和扩展卡尔曼滤波器算法(ExtendKalmanFilter,[0007](3)王伊人、邓国庆等人利用高进度激光雷达数据修正基于里程计读数的建议分[0008]发明目的：为了解决粒子滤波算法部分存在的粒子分布误差大及粒子退化问6[0009]技术方案：一种基于改进RBPF_SLAM算法的多传感器融合SLAM方法，包括如下步的多传感器融合算法融合轮式里程计数据与IMU数据作为初始位姿信息，利用所述初始位yy[0024]式中Hik表示IMU的观测矩阵，wik(d)为IMU数据的观测误差的i[0026](1)获取轮式里程计与IMU的传感器数据；(2)利用轮式里程计的非线性模型构建7[0029]首先利用移动机器人的运动模型推算出粒子点k时刻的新位姿",基于第i个粒子的初始位姿生成在k_1时刻的地图mi:然后在附近区域搜索，计算观测zk与已有[0031]将激光雷达观测模型与移动机器人的运动模型进行融合得到改善的建议分布函8[0054]步骤一、通过移动机器人(Keepbot机器人)搭载的激光雷达传感器获取周围环境y轴方向上机器人的移动速度，v表示y轴方向上机器人的移动速度，ω为机器人的旋转角y9[0063]式中Hik表示IMU的观测矩阵，wik(d)为IMU数据的观测误差的i将激光雷达通过扫描匹配得到的观测模型与移动机器人运动模型进行融合得到改善的建[0068]首先利用移动机器人的运动模型推算出粒子点k时刻的新位姿基于第i个粒[0071]此时将激光雷达通过扫描匹配得到的观测模型与机器人的运动模型进行融合得的观测模型(观测信息)都考虑进来，通过点集{xt}计算该高斯分布的参数和如下[0080](4)采用高斯分布x"-M(ut".2")采样生成新粒子点集t"y并更新粒子权重。[0083]更新完k时刻的所有粒子后，通过计算出全部有效粒子的数目判断是否需要重采动机器人硬件平台包括上层处理平台与底层控制单元两部分，两部分之间通过USB串口连述STM32底层控制单元与IMU传感器单元以及电机驱动单元通过动态/tf输入给/robot_pose_ekf传感器融合节点，同时/base_to_link节点与/base_to_gyro节点也把机器人的相关位姿信息输给/robot_pose_ekf节点，/robot_pose_ekf节点通过EKF方法不断更新机器人的运动状态并传输给SLAM的建图节点，激光雷达节点/为本发明基于改进RBPF_SLAM算法的多传感器融合SLAM方法构建地图，可明显看出传统[