1.一种基于改进RBPF-SLAM算法的多传感器融合SLAM方法,其特征在于,包括如下步骤:步骤一、通过移动机器人搭载的激光雷达传感器获取周围环境信息,利用基于EKF的多传感器融合算法融合轮式里程计数据与IMU数据作为初始位姿信息,利用所述初始位姿信息构建移动机器人的运动模型;
步骤二、计算提议分布时在里程计信息中添加激光雷达观测模型,将激光雷达观测模型与移动机器人运动模型进行融合得到改善的建议分布函数,从改善的建议分布函数中进行粒子的采样;
步骤三、引入阈值判断有效粒子数目简化重采样步骤,不断更新地图完成不同场景下的栅格地图构建实验。
2.根据权利要求1所述的一种基于改进RBPF-SLAM算法的多传感器融合SLAM方法,其特征在于,所述步骤一中:移动机器人k时刻的位姿如公式(1)所示:
xk=(Xk,Yk,θ,vx,vy,ω)T (1)式中,Xk,Yk,θ表示机器人在世界坐标系下的坐标及其与初始方向的夹角,vx表示x轴方向上机器人的移动速度,vy表示y轴方向上移动机器人的移动速度,ω为机器人的旋转角度;
扩展卡尔曼滤波下对应的系统状态转移方程如公式(2)所示:轮式里程计的观测方程如公式(3)所示:
zok=Hokxk=I6(Xk,Yk,θ,vx,vy,ω)T+wok(d) (3)式中Hok表示轮式里程计的观测矩阵,I6为单位6阶矩阵,wok(d)为预测误差的协方差矩阵且服从N(0,Ro(d))分布;
惯性测量单元IMU的观测方程如公式(4)所示:
式中Hik表示IMU的观测矩阵,wik(d)为IMU数据的观测误差的协方差矩阵且服从N(0,Ri(d))分布。
3.根据权利要求2所述的一种基于改进RBPF-SLAM算法的多传感器融合SLAM方法,其特征在于,所述步骤一中,基于EKF的多传感器融合算法具体为:(1)获取轮式里程计与IMU的传感器数据;
(2)利用轮式里程计的非线性模型构建扩展卡尔曼滤波器;
(3)对系统开始状态更新并加入系统噪声;
(4)结合上一时刻状态量,监听轮式里程计信息作为观测量和观测协方差矩阵,对系统状态量和系统协方差矩阵进行更新;
(5)监听IMU信息作为观测量和观测协方差矩阵,对(4)中得到的系统状态量和系统协方差矩阵进行状态更新;
(6)将融合的系统状态量和协方差矩阵作为SLAM算法的初始位姿,时刻的传感器信息融合结束。
4.根据权利要求3所述的一种基于改进RBPF-SLAM算法的多传感器融合SLAM方法,其特征在于,所述步骤二的具体操作如下:(1)创建改善的建议分布函数;
首先利用移动机器人的运动模型推算出粒子点k时刻的新位姿基于第i个粒子的初始位姿生成在k-1时刻的地图然后在附近区域搜索,计算观测zk与已有地图的匹配度,当搜索区域存在使得匹配度很高时,就认为观测可靠性高,根据公式(5)计算匹配度:将激光雷达观测模型与移动机器人的运动模型进行融合得到改善的建议分布函数,则采样提议分布如公式(6)所示:当观测可靠性高时,观测分布的区间L(i)的范围可以定义成其中ε为一个常数,x为机器人位姿变量,搜索出的匹配度最高的位姿点就是区间L(i)概率峰值区域;
(2)采样;
以为中心,以Δ为半径的区域内随机采固定数量的K个点{xt};
(3)计算高斯分布的参数;
将采样点集{xt}的分布近似为高斯分布,并将移动机器人的运动模型和激光雷达的观测模型都考虑进来,根据公式(7)计算高斯分布参数:其中
(4)采用高斯分布采样生成新粒子点集并更新粒子权重。
5.根据权利要求4所述的一种基于改进RBPF-SLAM算法的多传感器融合SLAM方法,其特征在于,步骤三中引入阈值判断有效粒子数目简化重采样具体为:更新完k时刻的所有粒子后,通过计算出全部有效粒子的数目判断是否需要重采样,引入阈值来判断参数Neff的有效性;
其中参数Neff如公式(8)所示:
式中是粒子的归一化权重,将阈值设为N为粒子总数,当Neff小于时就执行重采样,否则跳过重采样。
6.根据权利要求1所述的一种基于改进RBPF-SLAM算法的多传感器融合SLAM方法,其特征在于,所述移动机器人硬件平台包括树莓派4B处理平台单元、激光雷达传感器单元、STM32底层控制单元、IMU传感器单元和电机驱动单元;所述激光雷达传感器单元与树莓派4B处理平台相互连接,STM32底层控制单元与IMU传感器单元以及电机驱动单元相互连接;所述树莓派4B处理平台与STM32底层控制单元通过串口交互通信。