CN115388892A - 一种基于改进rbpf-slam算法的多传感器融合slam方法 - Google Patents

一种基于改进rbpf-slam算法的多传感器融合slam方法 Download PDF

Info

Publication number
CN115388892A
CN115388892A CN202211004853.3A CN202211004853A CN115388892A CN 115388892 A CN115388892 A CN 115388892A CN 202211004853 A CN202211004853 A CN 202211004853A CN 115388892 A CN115388892 A CN 115388892A
Authority
CN
China
Prior art keywords
slam
improved
observation
robot
formula
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211004853.3A
Other languages
English (en)
Inventor
周锋
李楠
姚凯文
吴瑞琦
刘海波
王如刚
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Yancheng Institute of Technology
Original Assignee
Yancheng Institute of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Yancheng Institute of Technology filed Critical Yancheng Institute of Technology
Priority to CN202211004853.3A priority Critical patent/CN115388892A/zh
Publication of CN115388892A publication Critical patent/CN115388892A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于改进RBPF‑SLAM算法的多传感器融合SLAM方法，涉及移动机器人定位与建图技术领域，本发明首先通过Keepbot机器人搭载的激光雷达传感器获取周围环境信息，利用扩展卡尔曼滤波(EKF)有效融合里程计与惯性导航(IMU)数据作为初始位姿信息，其次在计算提议分布时在里程计信息中添加高精度的激光雷达观测模型，并引入阈值判断有效粒子数目简化重采样步骤，最终不断更新地图完成不同场景下的栅格地图构建实验，有效提高了机器人位姿估计精度，能用更少的粒子数获取精确性可靠的2D栅格地图，显著减少了计算量，也对室内移动机器人SLAM领域研究提供了重要的参考价值。

Description

一种基于改进RBPF-SLAM算法的多传感器融合SLAM方法
技术领域
本发明涉及移动机器人定位与建图技术领域，具体是一种基于改进RBPF-SLAM算法的多传感器融合SLAM方法。
背景技术
近年来，随着机器人技术的迅猛发展，移动机器人在制造业、工业以及服务业等各个领域的应用越来越广泛，同时定位与建图(Simultaneous Localization and Mapping,SLAM)技术是移动机器人实现自主路径规划的核心。粒子滤波(ParticleFilter,PF)是一种基于蒙特卡洛原理的非线性非高斯系统的递归贝叶斯滤波算法，用一系列通过后验概率分布随机采样的状态粒子近似表示后验概率分布，其在目标定位方面有着很强的优越性，自然被应用到了对SLAM的研究中。Thrun等人将粒子滤波算法引入到移动机器人的SLAM问题中，通过赋予粒子权值得到机器人系统状态的后验概率分布，使定位更加精确，但随着粒子数量的增加，构建地图复杂度增加。
为了解决粒子滤波算法中存在的构建地图效率低下和计算复杂度大的问题：
(1)Murphy提出了把Rao-Blackwillised ParticleFilter加入SLAM解决方案中，RBPF-SLAM将机器人建图的过程分解成机器人位姿估计和基于位姿的地图估计，大幅度地减少了SLAM方法的计算量，但由于依赖于建图时所需的粒子数，该方法仍存在粒子耗尽问题；
(2)伍永健、陈跃东等人提出一种量子粒子群(QPSO)优化下的RBPF-SLAM算法，通过在重采样过程中引入量子粒子群优化算法更新粒子位姿，根据权值划分粒子种类，并对所得粒子集进行优化、调整，有效地防止粒子退化以及保持粒子的多样性。
(3)陈建军、廖小飞等人提出了基于改进小生境遗传优化的RBPF-SLAM算法INGO-RBPF，分别采用改进的RBPF-SLAM方法和扩展卡尔曼滤波器算法(Extend Kalman Filter,EKF)解决SLAM中的路径估计和地图估计问题。但由于单一传感器提供的信息具有一定局限性，特别是在移动机器人运动速度过快或转向角过大时表现漂移等，无法确保任意时刻信息的准确可靠性，因此利用多传感器融合技术在SLAM建图时提供更丰富的环境信息，提高移动机器人对周围环境的感知能力。
(3)王伊人、邓国庆等人利用高进度激光雷达数据修正基于里程计读数的建议分布函数，减少了滤波过程中所需的粒子数目；王馨阁、田宗强等人将运动里程计与激光雷达测量数据相结合，调整粒子权重，并通过自适应重采样的方法重新估计粒子的权重得到精准的栅格地图。
发明内容
发明目的：为了解决粒子滤波算法部分存在的粒子分布误差大及粒子退化问题。本发明在现有研究的基础上，研究改进传统RBPF-SLAM算法，设计一种基于改进RBPF-SLAM算法的多传感器融合SLAM方法，采用EKF方法将轮式里程计数据与IMU数据进行融合，将融合后的位姿信息作为移动机器人初始的运动模型，添加激光雷达的观测扫描信息优化采样粒子的提议分布，并引入阈值判断有效粒子数目简化重采样步骤。
技术方案：一种基于改进RBPF-SLAM算法的多传感器融合SLAM方法，包括如下步骤：
步骤一、通过移动机器人搭载的激光雷达传感器获取周围环境信息，利用基于EKF的多传感器融合算法融合轮式里程计数据与IMU数据作为初始位姿信息，利用所述初始位姿信息构建移动机器人的运动模型；
步骤二、计算提议分布时在里程计信息中添加激光雷达观测模型，将激光雷达观测模型与移动机器人运动模型进行融合得到改善的建议分布函数，从改善的建议分布函数中进行粒子的采样；
步骤三、引入阈值判断有效粒子数目简化重采样步骤，不断更新地图完成不同场景下的栅格地图构建实验。
进一步的，所述步骤一中：
移动机器人k时刻的位姿如公式(1)所示：
xk＝(Xk,Yk,θ,vx,vy,ω)T (1)
式中，Xk,Yk,θ表示机器人在世界坐标系下的坐标及其与初始方向的夹角，vx表示x轴方向上机器人的移动速度，vy表示y轴方向上移动机器人的移动速度，ω为机器人的旋转角度；
扩展卡尔曼滤波下对应的系统状态转移方程如公式(2)所示：
Figure BDA0003808248260000031
轮式里程计的观测方程如公式(3)所示：
zok＝Hokxk＝I6(Xk,Yk,θ,vx,vy,ω)T+wok(d) (3)
式中Hok表示轮式里程计的观测矩阵，I6为单位6阶矩阵，wok(d)为预测误差的协方差矩阵且服从N(0,Ro(d))分布；
惯性测量单元IMU的观测方程如公式(4)所示：
Figure BDA0003808248260000032
式中Hik表示IMU的观测矩阵，wik(d)为IMU数据的观测误差的协方差矩阵且服从N(0,Ri(d))分布。
进一步的，所述步骤一中，基于EKF的多传感器融合算法具体为：
(1)获取轮式里程计与IMU的传感器数据；(2)利用轮式里程计的非线性模型构建扩展卡尔曼滤波器；(3)对系统开始状态更新并加入系统噪声；(4)结合上一时刻状态量，监听轮式里程计信息作为观测量和观测协方差矩阵，对系统状态量和系统协方差矩阵进行更新；(5)监听IMU信息作为观测量和观测协方差矩阵，对(4)中得到的系统状态量和系统协方差矩阵进行状态更新；(6)将融合的系统状态量和协方差矩阵作为SLAM算法的初始位姿，时刻的传感器信息融合结束。
进一步的，所述步骤二的具体操作如下：
(1)创建改善的建议分布函数；
首先利用移动机器人的运动模型推算出粒子点k时刻的新位姿
Figure BDA0003808248260000033
基于第i个粒子的初始位姿
Figure BDA0003808248260000041
生成在k-1时刻的地图
Figure BDA0003808248260000042
然后在
Figure BDA0003808248260000043
附近区域搜索，计算观测zk与已有地图
Figure BDA0003808248260000044
的匹配度，当搜索区域存在
Figure BDA0003808248260000045
使得匹配度很高时，就认为观测可靠性高，根据公式(5)计算匹配度：
Figure BDA0003808248260000046
将激光雷达观测模型与移动机器人的运动模型进行融合得到改善的建议分布函数，则采样提议分布如公式(6)所示：
Figure BDA0003808248260000047
当观测可靠性高时，观测分布的区间L(i)的范围可以定义成
Figure BDA0003808248260000048
其中ε为一个常数，x为机器人位姿变量，搜索出的匹配度最高的位姿点
Figure BDA0003808248260000049
就是区间L(i)概率峰值区域；
(2)采样；
以
Figure BDA00038082482600000410
为中心，以Δ为半径的区域内随机采固定数量的K个点{xt}；
(3)计算高斯分布的参数；
将采样点集{xt}的分布近似为高斯分布，并将移动机器人的运动模型和激光雷达的观测模型都考虑进来，根据公式(7)计算高斯分布参数：
Figure BDA00038082482600000411
其中
Figure BDA00038082482600000412
(4)采用高斯分布
Figure BDA00038082482600000413
采样生成新粒子点集
Figure BDA00038082482600000414
并更新粒子权重。
进一步的，步骤三中引入阈值判断有效粒子数目简化重采样具体为：更新完k时刻的所有粒子后，通过计算出全部有效粒子的数目判断是否需要重采样，引入阈值来判断参数Neff的有效性；
其中参数Neff如公式(8)所示：
Figure BDA0003808248260000051
式中
Figure BDA0003808248260000052
是粒子的归一化权重，将阈值设为
Figure BDA0003808248260000053
N为粒子总数，当Neff小于
Figure BDA0003808248260000054
时就执行重采样，否则跳过重采样。
进一步的，所述移动机器人硬件平台包括树莓派4B处理平台单元、激光雷达传感器单元、STM32底层控制单元、IMU传感器单元和电机驱动单元；所述激光雷达传感器单元与树莓派4B处理平台相互连接，STM32底层控制单元与IMU传感器单元以及电机驱动单元相互连接；所述树莓派4B处理平台与STM32底层控制单元通过串口交互通信。
有益效果：本发明采用EKF方法将里程计数据与IMU数据进行融合，将融合后的位姿信息作为算法的初始运动模型，并引入激光雷达的观测扫描信息优化采样粒子的提议分布，另外通过引入阈值判断有效粒子数改进粒子的重采样过程从而缓解粒子耗散问题，提高了算法效率。该方法相比于传统算法能用更少计算时间的同时建图精度和建图准确度更高。
附图说明
图1为本发明算法的流程图；
图2为本发明的硬件平台实施方案示意图；
图3为本发明的ROS节点示意图；
图4为传统RBPF-SLAM算法和本发明算法下的室内场景建图效果示意图；
图5为传统RBPF-SLAM算法和本发明算法下的长走廊场景建图效果示意图。
具体实施方式
下面通过附图对本发明技术方案进行详细说明，但是本发明的保护范围不局限于实施例。
如图1所示，一种基于改进RBPF-SLAM算法的多传感器融合SLAM方法，包括如下步骤：
步骤一、通过移动机器人(Keepbot机器人)搭载的激光雷达传感器获取周围环境信息，利用基于扩展卡尔曼滤波(EKF)的多传感器融合算法，融合轮式里程计数据与IMU数据作为初始位姿信息；利用融合后得到的初始位姿信息构建移动机器人的运动模型；
所述步骤一中，为了准确地描述机器人在k时刻的状态，将移动机器人k时刻的位姿如公式(1)所示：
xk＝(Xk,Yk,θ,vx,vy,ω)T (1)
式中，Xk,Yk,θ表示机器人在世界坐标系下的坐标及其与初始方向的夹角，vx表示x轴方向上机器人的移动速度，vy表示y轴方向上机器人的移动速度，ω为机器人的旋转角度；可得出扩展卡尔曼滤波(EKF)下对应的系统状态转移方程如公式(2)所示：
Figure BDA0003808248260000061
此时移动机器人轮式里程计的观测方程表示为：
zok＝Hokxk＝I6(Xk,Yk,θ,vx,vy,ω)T+wok(d) (3)
式中Hok表示轮式里程计的观测矩阵，I6为单位6阶矩阵，wok(d)为预测误差的协方差矩阵且服从N(0,Ro(d))分布；由于仅在二维空间内建图，因此无需考虑IMU中Z轴的数据，惯性测量单元IMU的观测方程如公式(4)所示：
Figure BDA0003808248260000062
式中Hik表示IMU的观测矩阵，wik(d)为IMU数据的观测误差的协方差矩阵且服从N(0,Ri(d))分布。
所述步骤一中，基于EKF的多传感器融合算法具体为：(1)获取轮式里程计与IMU的传感器数据；(2)利用轮式里程计的非线性模型构建扩展卡尔曼滤波器；(3)对系统开始状态更新并加入系统噪声；(4)结合上一时刻状态量，监听轮式里程计信息作为观测量和观测协方差矩阵，对系统状态量和系统协方差矩阵进行更新；(5)监听IMU信息作为观测量和观测协方差矩阵，对(4)中得到的系统状态量和系统协方差矩阵进行状态更新；(6)将融合的系统状态量和协方差矩阵作为SLAM算法的初始位姿，时刻的传感器信息融合结束。
步骤二、在计算提议分布时在里程计信息中添加高精度的激光雷达观测模型，在将激光雷达通过扫描匹配得到的观测模型与移动机器人运动模型进行融合得到改善的建议分布函数，从改善的建议分布函数中进行粒子的采样；
所述步骤二的具体操作如下：
(1)创建改善的建议分布函数；
首先利用移动机器人的运动模型推算出粒子点k时刻的新位姿
Figure BDA0003808248260000071
基于第i个粒子的初始位姿
Figure BDA0003808248260000072
生成在k-1时刻的地图
Figure BDA0003808248260000073
然后在
Figure BDA0003808248260000074
附近区域搜索，计算观测zk与已有地图
Figure BDA0003808248260000075
的匹配度，当搜索区域存在
Figure BDA0003808248260000076
使得匹配度很高时，就认为观测可靠性高，根据公式(5)计算匹配度：
Figure BDA0003808248260000077
此时将激光雷达通过扫描匹配得到的观测模型与机器人的运动模型进行融合得到改善的建议分布函数，则采样提议分布为：
Figure BDA0003808248260000078
当观测可靠性高时，观测分布的区间L(i)的范围可以定义成
Figure BDA0003808248260000079
其中，ε为一个常数，x为机器人位姿变量，搜索出的匹配度最高的位姿点
Figure BDA00038082482600000710
就是区间L(i)概率峰值区域；
(2)采样；
从改善的建议分布中进行粒子的采样，以
Figure BDA00038082482600000711
为中心，以Δ为半径的区域内随机采固定数量的K个点{xt}；
(3)计算高斯分布的参数；
将采样点集{xt}的分布近似为高斯分布，并将移动机器人的运动模型和激光雷达的观测模型(观测信息)都考虑进来，通过点集{xt}计算该高斯分布的参数
Figure BDA0003808248260000081
和
Figure BDA0003808248260000082
如下所示：
Figure BDA0003808248260000083
其中
Figure BDA0003808248260000084
(4)采用高斯分布
Figure BDA0003808248260000085
采样生成新粒子点集
Figure BDA0003808248260000086
并更新粒子权重。
步骤三、引入阈值判断有效粒子数目简化重采样步骤，最终不断更新地图完成不同场景下的栅格地图构建实验。
所述步骤三中引入阈值判断有效粒子数目简化重采样具体为：
更新完k时刻的所有粒子后，通过计算出全部有效粒子的数目判断是否需要重采样，引入阈值来判断参数Neff的有效性，其中参数Neff为：
Figure BDA0003808248260000087
式中
Figure BDA0003808248260000088
是粒子的归一化权重，将阈值设为
Figure BDA0003808248260000089
N为粒子总数，当Neff小于
Figure BDA00038082482600000810
时就执行重采样，否则跳过重采样。
而当扫描匹配度较低时，即不符合公式(5)时,不进行步骤二，直接根据运动模型采样，计算更新粒子权重，进行步骤三，通过计算出全部有效粒子的数目判断是否需要重采样。
如图2所示，本实施例的硬件平台的框架，实验平台采用差分轮式驱动模型，该移动机器人硬件平台包括上层处理平台与底层控制单元两部分，两部分之间通过USB串口连接进行通信。所述上层处理平台选择树莓派4B(4G)处理平台单元，所述树莓派4B(4G)处理平台单元与激光雷达传感器单元相互连接；所述底层控制单元选择STM32底层控制单元，所述STM32底层控制单元与IMU传感器单元以及电机驱动单元相互连接。
树莓派4B(4G)处理平台单元作为移动机器人的上层处理平台，负责收集激光雷达传感器单元的数据并进行实时处理，同时根据指令发布控制信号；树莓派4B(4G)处理平台单元的运行环境为Ubuntu18.04操作系统，并构建ROS-Melodic系统框架，直接读取并处理激光雷达数据进行建图。
STM32底层控制单元采用STM32F767IGT6芯片，主要负责电机的驱动控制，光电编码器和惯性测量单元IMU的数据处理。所STM32底层控制单元根据发布的实时控制指令信号驱动电机控制移动机器人移动。电源模块中的25.9V锂电池提供直流工作电压，直流电机驱动选用6.5寸轮毂伺服电机，其控制精度高、运行稳定，能同时驱动两路电机。以便于后续程序开发及远端控制。IMU选用了九轴运动处理传感器MPU6050，能为移动机器人提供更为精确的位姿信息。
如图3所示，本发明的ROS节点示意图，其中，机器人控制节点将编码里程计解析后通过动态/tf输入给/robot_pose_ekf传感器融合节点，同时/base_to_link节点与/base_to_gyro节点也把机器人的相关位姿信息输给/robot_pose_ekf节点，/robot_pose_ekf节点通过EKF方法不断更新机器人的运动状态并传输给SLAM的建图节点，激光雷达节点/rplldarNode经话题/scan将数据输入给SLAM建图节点，urdf解析节点将机器人的各个传感器坐标关系通过静态/tf_static输入给SLAM建图节点。而SLAM建图节点利用这些输入数据进行建图，并将地图发布到对应的话题，同时输出map至odom之间的动态tf关系到/tf。
图4为室内场景建图效果示意图，图4(a)为传统RBPF-SLAM算法构建地图，图4(b)为本发明基于改进RBPF-SLAM算法的多传感器融合SLAM方法构建地图，可明显看出传统RBPF-SLAM算法构建的地图中由于仅由里程计提供位姿信息，地图上存在位置漂移，另外当运行时间过长时出现粒子退化现象，引起粒子多样性不足，建立的地图多处与真实环境存在明显偏差，建图效果较差。而本发明算法构建地图后，建立的地图与真实环境接近，建图效果良好，有效避免了由于粒子退化带来的建图精确度下降问题。
图5为长走廊场景建图效果示意图，图5(a)为传统RBPF-SLAM算法构建长走廊地图，图5(b)为本发明构建长走廊地图，传统RBPF-SLAM算法只使用里程计的运动模型作为提议分布进行采样，当里程计误差随着时间推移不断累积，机器人构建的地图中红圈部分存在假墙及漂移现象，而使用本发明构建的地图精度更高，与真实环境匹配度高。
如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims (6)

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)所示：
Figure FDA0003808248250000011
轮式里程计的观测方程如公式(3)所示：
zok＝Hokxk＝I6(Xk,Yk,θ,vx,vy,ω)T+wok(d) (3)
式中Hok表示轮式里程计的观测矩阵，I6为单位6阶矩阵，wok(d)为预测误差的协方差矩阵且服从N(0,Ro(d))分布；
惯性测量单元IMU的观测方程如公式(4)所示：
Figure FDA0003808248250000021
式中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时刻的新位姿
Figure FDA0003808248250000022
基于第i个粒子的初始位姿
Figure FDA0003808248250000023
生成在k-1时刻的地图
Figure FDA0003808248250000024
然后在
Figure FDA0003808248250000025
附近区域搜索，计算观测zk与已有地图
Figure FDA0003808248250000026
的匹配度，当搜索区域存在
Figure FDA0003808248250000027
使得匹配度很高时，就认为观测可靠性高，根据公式(5)计算匹配度：
Figure FDA0003808248250000031
将激光雷达观测模型与移动机器人的运动模型进行融合得到改善的建议分布函数，则采样提议分布如公式(6)所示：
Figure FDA0003808248250000032
当观测可靠性高时，观测分布的区间L(i)的范围可以定义成
Figure FDA0003808248250000033
其中ε为一个常数，x为机器人位姿变量，搜索出的匹配度最高的位姿点
Figure FDA0003808248250000034
就是区间L(i)概率峰值区域；
(2)采样；
以
Figure FDA0003808248250000035
为中心，以Δ为半径的区域内随机采固定数量的K个点{xt}；
(3)计算高斯分布的参数；
将采样点集{xt}的分布近似为高斯分布，并将移动机器人的运动模型和激光雷达的观测模型都考虑进来，根据公式(7)计算高斯分布参数：
Figure FDA0003808248250000036
其中
Figure FDA0003808248250000037
(4)采用高斯分布
Figure FDA0003808248250000038
采样生成新粒子点集
Figure FDA0003808248250000039
并更新粒子权重。
5.根据权利要求4所述的一种基于改进RBPF-SLAM算法的多传感器融合SLAM方法，其特征在于，步骤三中引入阈值判断有效粒子数目简化重采样具体为：更新完k时刻的所有粒子后，通过计算出全部有效粒子的数目判断是否需要重采样，引入阈值来判断参数Neff的有效性；
其中参数Neff如公式(8)所示：
Figure FDA0003808248250000041
式中
Figure FDA0003808248250000042
是粒子的归一化权重，将阈值设为
Figure FDA0003808248250000043
N为粒子总数，当Neff小于
Figure FDA0003808248250000044
时就执行重采样，否则跳过重采样。
6.根据权利要求1所述的一种基于改进RBPF-SLAM算法的多传感器融合SLAM方法，其特征在于，所述移动机器人硬件平台包括树莓派4B处理平台单元、激光雷达传感器单元、STM32底层控制单元、IMU传感器单元和电机驱动单元；所述激光雷达传感器单元与树莓派4B处理平台相互连接，STM32底层控制单元与IMU传感器单元以及电机驱动单元相互连接；所述树莓派4B处理平台与STM32底层控制单元通过串口交互通信。
CN202211004853.3A 2022-08-22 2022-08-22 一种基于改进rbpf-slam算法的多传感器融合slam方法 Pending CN115388892A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211004853.3A CN115388892A (zh) 2022-08-22 2022-08-22 一种基于改进rbpf-slam算法的多传感器融合slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211004853.3A CN115388892A (zh) 2022-08-22 2022-08-22 一种基于改进rbpf-slam算法的多传感器融合slam方法

Publications (1)

Publication Number Publication Date
CN115388892A true CN115388892A (zh) 2022-11-25

Family

ID=84120023

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211004853.3A Pending CN115388892A (zh) 2022-08-22 2022-08-22 一种基于改进rbpf-slam算法的多传感器融合slam方法

Country Status (1)

Country Link
CN (1) CN115388892A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117346768A (zh) * 2023-11-03 2024-01-05 昆明理工大学 同时适用于室内外的多传感器融合感知定位方法
CN117405118A (zh) * 2023-10-16 2024-01-16 西安工业大学 一种多传感器融合建图方法、系统、设备及存储介质

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117405118A (zh) * 2023-10-16 2024-01-16 西安工业大学 一种多传感器融合建图方法、系统、设备及存储介质
CN117346768A (zh) * 2023-11-03 2024-01-05 昆明理工大学 同时适用于室内外的多传感器融合感知定位方法
CN117346768B (zh) * 2023-11-03 2024-04-19 昆明理工大学 同时适用于室内外的多传感器融合感知定位方法

Similar Documents

Publication Publication Date Title
Xuexi et al. SLAM algorithm analysis of mobile robot based on lidar
CN115388892A (zh) 一种基于改进rbpf-slam算法的多传感器融合slam方法
CN111308490B (zh) 基于单线激光雷达的平衡车室内定位与导航系统
CN112629522B (zh) 一种反光板与激光slam融合的agv定位方法及系统
CN111427370A (zh) 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法
CN112284376A (zh) 一种基于多传感器融合的移动机器人室内定位建图方法
CN112762928B (zh) 含有激光slam的odom与dm地标组合移动机器人及导航方法
CN115993825A (zh) 一种基于空地协同的无人车集群控制系统
CN111578926A (zh) 一种基于自动驾驶平台的地图生成与导航避障的方法
Yang et al. Mobile robot motion control and autonomous navigation in GPS-denied outdoor environments using 3D laser scanning
CN112857370A (zh) 一种基于时序信息建模的机器人无地图导航方法
CN118020038A (zh) 两轮自平衡机器人
Zhang et al. Design of dual-LiDAR high precision natural navigation system
CN113296519A (zh) 一种基于麦克纳姆轮的移动机器人运动规划方法及系统
CN115655311A (zh) 一种基于扫描匹配的阿克曼型机器人里程计标定方法
Dong et al. Path Planning Research for Outdoor Mobile Robot
Li et al. Object-Aware View Planning for Autonomous 3D Model Reconstruction of Buildings Using a Mobile Robot
He et al. Research on mobile robot positioning and navigation system based on multi-sensor fusion
Yang et al. AGV robot for laser-SLAM based method testing in automated container terminal
Lin et al. Realization of Ackermann robot obstacle avoidance navigation based on Multi-sensor fusion SLAM
Li et al. Multisensor Fusion SLAM Research Based on Improved RBPF‐SLAM Algorithm
Zhu et al. Visible RRT*: Asymptotically Optimal Random Search Tree for Visual Servo Tasks with the FOV Constraint
CN115014357A (zh) 一种基于自定义地图区域的导航机器人系统
Jiang et al. Research on the Positioning and Navigation Accuracy Based on Multi-sensor Smart Car
Chen et al. Navigation method of fire-fighting robot based on a commercial electronic map

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination