CN110333720A - 一种基于粒子滤波的slam优化方法 - Google Patents

一种基于粒子滤波的slam优化方法 Download PDF

Info

Publication number
CN110333720A
CN110333720A CN201910620350.0A CN201910620350A CN110333720A CN 110333720 A CN110333720 A CN 110333720A CN 201910620350 A CN201910620350 A CN 201910620350A CN 110333720 A CN110333720 A CN 110333720A
Authority
CN
China
Prior art keywords
robot
map
particle
model
particle filter
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
CN201910620350.0A
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.)
SICHUAN ARTIGENT ROBOTICS EQUIPMENT Co Ltd
Electric Power Research Institute of State Grid Sichuan Electric Power Co Ltd
Original Assignee
SICHUAN ARTIGENT ROBOTICS EQUIPMENT Co Ltd
Electric Power Research Institute of State Grid Sichuan Electric Power Co Ltd
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 SICHUAN ARTIGENT ROBOTICS EQUIPMENT Co Ltd, Electric Power Research Institute of State Grid Sichuan Electric Power Co Ltd filed Critical SICHUAN ARTIGENT ROBOTICS EQUIPMENT Co Ltd
Priority to CN201910620350.0A priority Critical patent/CN110333720A/zh
Publication of CN110333720A publication Critical patent/CN110333720A/zh
Pending legal-status Critical Current

Links

Classifications

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257Control of position or course in two dimensions specially adapted to land vehicles using a radar

Landscapes

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

Abstract

本发明公开了一种基于粒子滤波的SLAM优化方法,涉及机器人即时定位与地图构建,机器人执行任务时收到噪声干扰导致机器人执行任务失败的问题。本发明包括以下步骤;步骤1,定义变量;步骤2,根据变量进行概率模型建立,所述概率模型建立包括运动概率学建模、传感器观测模型建立、栅格地图模型建立;步骤3,基于粒子滤波和概率模型对SLAM进行优化,包括每次更新地图后机器人自动对位置进行确认,机器人自动对位置进行确认至少包括初始化、采样、扫描匹配、权值计算和重采样。本发明具有得到目标的真实状态,避免机器人执行任务时收到噪声干扰导致机器人执行任务失败等优点。

Description

一种基于粒子滤波的SLAM优化方法
技术领域
本发明涉及机器人即时定位与地图构建,具体涉及一种基于粒子滤波的SLAM优化方法。
背景技术
现有技术中机器人执行的各种任务,无论是定位、建图,或是识别等,其需要的环境与 自身状态信息均来自于传感器。理想情况下,传感器的各种信息集合就可以代表机器人与所 处环境的具体状态。但在实际使用中,由于传感器自身特性或环境限制,噪声时刻存在,机 器人在执行任务时受到噪声干扰,则机器人所处环境的具体状态与实际所处环境的具体状态 存在差异,造成机器人执行任务失败。
发明内容
本发明目的在于提供一种基于粒子滤波的SLAM优化方法,解决机器人执行任务时收到 噪声干扰导致机器人执行任务失败的问题。
名称解析;
SLAM(simultaneous localization and mapping),也称为CML(ConcurrentMapping and Localization),机器人即时定位与地图构建,或并发建图与定位。问题可以描述为:将一个机 器人放入未知环境中的未知位置,是否有办法让机器人一边移动一边逐步描绘出此环境完全 的地图,所谓完全的地图是指不受障碍行进到房间可进入的每个角落。
一种基于粒子滤波的SLAM优化方法,至少包括以下步骤;
步骤1,对机器人建立概率模型,所述建立概率模型包括运动概率学建模、传感器观测 模型建立和栅格地图模型建立;
步骤2,概率模型结合粒子滤波对机器人即时定位与地图构建进行优化,优化过程包括 初始化、采样、扫描匹配、权值计算和重采样;其中,优化过程引入多个变量;所述多个变 量包括:
xt:描述机器人在全局坐标下的位置以及朝向的状态变量,具体为
ut:机器人控制向量,t时刻的控制向量表示经此控制驱动机器人在t-1时刻的位置状态 由xt-1转换到了状态xt,且将两时刻内的位置变换看作控制输入;
m:地图状态变量,对环境信息的直观描述;
表示传感器在t时刻观测得到的第k个环境信息,由于本课题采用的是激光数据,k 表示第k个激光光束。
经过长期的实践与研究,本申请的发明人发现,现有技术中机器人执行的各种任务中, 由于传感器自身特性或环境限制,噪声时刻存在,机器人在执行任务时受到噪声干扰,则机 器人所处环境的具体状态与实际所处环境的具体状态存在差异,造成机器人执行任务失败。 本申请提供一种基于粒子滤波的SLAM优化方法,具体的,定义变量是为了更好的使用数学 语言描述SLAM过程,进行概率模型建立是为了将机器人的实际运动与观测转换为概率的形式 根据变量,基于粒子滤波和概率模型对SLAM进行优化则为了将机器人位置与地图分开进行 估计,且机器人自动对位置进行确认至少包括初始化、采样、扫描匹配、权值计算和重采样, 得到一个较好的估计结果即目标的真实状态。
优选的,所述运动概率学建模采用里程计运动模型,二维环境下,机器人在全局坐标下 的位姿向量表示为(px,py,pθ)T,则p(xt|xt-1,ut)表示在控制输入ut的作用下,位姿状态向量xt可能出现的概率;当机器人从A状态移动到B状态,颜色越深表示位置精度越高。
优选的,所述传感器观测模型建立基于激光雷达的可能性区域模型对观测信息进行建模。
优选的,所述传感器观测模型建立引入至少三种类型的噪声,至少三种类型的噪声包括: 测量噪声、错误测量值噪声和随机测量噪声。
优选的,所述随机测量噪声包括当激光传感器击中半透明玻璃,或者声纳传感器接收到 同波段声波时都会产生不固定的随机噪声,此随机噪声用均匀分布表示,如下式3-9所示:
根据传感器观测模型的误差即可计算出观测概率p(zt|xt,m),计算公式3-10如下:
其中q表示每个激光数据的观测概率,zhit表示测量噪声出现的概率阈值,zrand表示随机 噪声出现的概率阈值,zhit远大于zrand且两者和为1。
优选的,所述栅格地图模型建立采用占据栅格地图,具体的,将整个待估计二维空间表 示为具有大小值的网格集合,网格的尺寸大小表示地图的分辨率,每个网格维护一个二进制 值,0代表该网格处无障碍,1代表该网格坐标处有障碍。
优选的,所述初始化包括在SLAM算法起始即t=0时刻开始执行,默认初始机器人位姿 向量为(0,0,0)T,初始化得到的地图表示为m0,初始化完成后的粒子内容为公式3-17,其中 每个粒子的权值相同,均为1/M;
由式3-16可知,已知机器人位置时,仅根据传感器数据即可对地图进行更新,初始状态 时对于一帧中的所有K组激光数据根据式3-10可计算得到激光光束 端点在全局坐标系下的位置并初始化地图。
优选的,所述采样包括使用里程计运动模型进行采样,粒子中位置的更新如下式3-18所 示,其中δ′trans,δ′rot1,δ′rot2分别代表了带误差的位置变换值:
优选的,所述扫描匹配采用基于可能性区域模型的扫描匹配算法。
优选的,所述权值计算与重采样包括第m个粒子的权值计算方式如下式3-22所示:
所述重采样采用自适应重采样技术决定重采样步骤何时发生,如下式3-23所示:
其中Neff又被称为有效粒子数,当Neff值小于某一给定阈值时进行重采样步骤,否则 在权值计算完成后直接进行下一次算法迭代。
本发明具有如下的优点和有益效果:
本发明一种基于粒子滤波的SLAM优化方法,通过定义变量、概率模型建立以及基于粒子 滤波和概率模型对SLAM进行优化得到目标的真实状态,避免机器人执行任务时收到噪声干扰 导致机器人执行任务失败。
附图说明
此处所说明的附图用来提供对本发明实施例的进一步理解,构成本申请的一部分,并不 构成对本发明实施例的限定。在附图中:
图1为本发明中SLAM马尔可夫过程示意图。
图2(a)为本发明中机器人全局位姿图。
图2(b)为本发明中机器人采样位姿图。
图3为本发明中里程计模型分解示意图。
图4为本发明中机器人所在地图更新示意图。
图5为本发明中提议分布采样示意图。
图6为本发明中权值计算示意图。
图7为本发明中重采样示意图。
图8(a)为本发明中仿真坐标示意图。
图8(b)为本发明中雷达观测示意图。
图9为本发明中机器人运动轨迹示意图。
图10为本发明中初始化地图示意图。
图11为本发明中粒子数为30的采样示意图。
图12为本发明中待匹配激光数据示意图。
图13(a)为本发明中粒子采样分布图。
图13(b)为本发明中扫描匹配粒子分布图。
图14(a)为本发明中扫描匹配距离误差对比示意图。
图14(b)为本发明中扫描匹配角度误差对比示意图。
图15为本发明中粒子滤波SLAM系统流程图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白,下面结合实施例和附图,对本发明 作进一步的详细说明,本发明的示意性实施方式及其说明仅用于解释本发明,并不作为对本 发明的限定。
在以下描述中,为了提供对本发明的透彻理解阐述了大量特定细节。然而,对于本领域 普通技术人员显而易见的是:不必采用这些特定细节来实行本发明。在其他实例中,为了避 免混淆本发明,未具体描述公知的结构、电路、材料或方法。
在整个说明书中,对“一个实施例”、“实施例”、“一个示例”或“示例”的提及意味着: 结合该实施例或示例描述的特定特征、结构或特性被包含在本发明至少一个实施例中。因此, 在整个说明书的各个地方出现的短语“一个实施例”、“实施例”、“一个示例”或“示例”不 一定都指同一实施例或示例。此外,可以以任何适当的组合和、或子组合将特定的特征、结 构或特性组合在一个或多个实施例或示例中。此外,本领域普通技术人员应当理解,在此提 供的示图都是为了说明的目的,并且示图不一定是按比例绘制的。这里使用的术语“和/或” 包括一个或多个相关列出的项目的任何和所有组合。
在本发明的描述中,需要理解的是,术语“前”、“后”、“左”、“右”、“上”、“下”、 “竖直”、“水平”、“高”、“低”“内”、“外”等指示的方位或位置关系为基于附图 所示的方位或位置关系,仅是为了便于描述本发明和简化描述,而不是指示或暗示所指的装 置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明保护范 围的限制。
实施例
本发明目的在于提供一种基于粒子滤波的SLAM优化方法,解决机器人执行任务时收到噪 声干扰导致机器人执行任务失败的问题。
名称解析;
SLAM(simultaneous localization and mapping),也称为CML(ConcurrentMapping and Localization),机器人即时定位与地图构建,或并发建图与定位。问题可以描述为:将一个机 器人放入未知环境中的未知位置,是否有办法让机器人一边移动一边逐步描绘出此环境完全 的地图,所谓完全的地图是指不受障碍行进到房间可进入的每个角落。
一种基于粒子滤波的SLAM优化方法,至少包括以下步骤;
步骤1,对机器人建立概率模型,所述建立概率模型包括运动概率学建模、传感器观测 模型建立和栅格地图模型建立;
步骤2,概率模型结合粒子滤波对机器人即时定位与地图构建进行优化,优化过程包括 初始化、采样、扫描匹配、权值计算和重采样;其中,优化过程引入多个变量;所述多个变 量包括:
xt:描述机器人在全局坐标下的位置以及朝向的状态变量,具体为
ut:机器人控制向量,t时刻的控制向量表示经此控制驱动机器人在t-1时刻的位置状态 由xt-1转换到了状态xt,且将两时刻内的位置变换看作控制输入;
m:地图状态变量,对环境信息的直观描述;
表示传感器在t时刻观测得到的第k个环境信息,由于本课题采用的是激光数据,k 表示第k个激光光束。
经过长期的实践与研究,本申请的发明人发现,现有技术中机器人执行的各种任务中, 由于传感器自身特性或环境限制,噪声时刻存在,机器人在执行任务时受到噪声干扰,则机 器人所处环境的具体状态与实际所处环境的具体状态存在差异,造成机器人执行任务失败。 本申请提供一种基于粒子滤波的SLAM优化方法,具体的,定义变量是为了更好的使用数学语 言描述SLAM过程,进行概率模型建立是为了将机器人的实际运动与观测转换为概率的形式根 据变量,基于粒子滤波和概率模型对SLAM进行优化则为了将机器人位置与地图分开进行估 计,且机器人自动对位置进行确认至少包括初始化、采样、扫描匹配、权值计算和重采样, 得到一个较好的估计结果即目标的真实状态。
优选的,所述运动概率学建模采用里程计运动模型,二维环境下,机器人在全局坐标下 的位姿向量表示为(px,py,pθ)T,则p(xt|xt-1,ut)表示在控制输入ut的作用下,位姿状态向量xt可能出现的概率;当机器人从A状态移动到B状态,颜色越深表示位置精度越高。
优选的,所述传感器观测模型建立基于激光雷达的可能性区域模型对观测信息进行建模。
优选的,所述传感器观测模型建立引入至少三种类型的噪声,至少三种类型的噪声包括: 测量噪声、错误测量值噪声和随机测量噪声。
优选的,所述随机测量噪声包括当激光传感器击中半透明玻璃,或者声纳传感器接收到 同波段声波时都会产生不固定的随机噪声,此随机噪声用均匀分布表示,如下式3-9所示:
根据传感器观测模型的误差即可计算出观测概率p(zt|xt,m),计算公式3-10如下:
其中q表示每个激光数据的观测概率,zhit表示测量噪声出现的概率阈值,zrand表示随机 噪声出现的概率阈值,zhit远大于zrand且两者和为1。
优选的,所述栅格地图模型建立采用占据栅格地图,具体的,将整个待估计二维空间表 示为具有大小值的网格集合,网格的尺寸大小表示地图的分辨率,每个网格维护一个二进制 值,0代表该网格处无障碍,1代表该网格坐标处有障碍。
优选的,所述初始化包括在SLAM算法起始即t=0时刻开始执行,默认初始机器人位姿向 量为(0,0,0)T,初始化得到的地图表示为m0,初始化完成后的粒子内容为公式1-17,其中每 个粒子的权值相同,均为1/M;
由式3-16可知,已知机器人位置时,仅根据传感器数据即可对地图进行更新,初始状态 时对于一帧中的所有K组激光数据根据式3-10可计算得到激光光束 端点在全局坐标系下的位置并初始化地图。
优选的,所述采样包括使用里程计运动模型进行采样,粒子中位置的更新如下式3-18所 示,其中δ′trans,δ′rot1,δ′rot2分别代表了带误差的位置变换值:
优选的,所述扫描匹配采用基于可能性区域模型的扫描匹配算法。
优选的,所述权值计算与重采样包括第m个粒子的权值计算方式如下式3-22所示:
所述重采样采用自适应重采样技术决定重采样步骤何时发生,如下式3-23所示:
其中Neff又被称为有效粒子数,当Neff值小于某一给定阈值时进行重采样步骤,否则 在权值计算完成后直接进行下一次算法迭代。
具体的,为了更好的使用数学语言描述SLAM过程,我们定义如下变量(t时刻):
xt:描述机器人在全局坐标下的位置以及朝向的状态变量,具体为
ut:机器人控制向量,t时刻的控制向量表示经此控制驱动机器人在t-1时刻的位置状态 由xt-1转换到了状态xt。特别的,可以将两时刻内的位置变换看作控制输入;
m:地图状态变量,对环境信息的直观描述;
表示传感器在t时刻观测得到的第k个环境信息,由于本课题采用的是激光数据,k 表示第k个激光光束;
从概率学的角度来看,SLAM问题主要估计的是在已知控制输入与环境观测信息下机器人 位置以及地图环境的条件概率,如下式3-1所示:
p(xt,m|z1:t,u1:t) (3-1);
其中z1:t与u1:t表示从机器人开始运动时刻起所有的观测数据与控制向量。实际上,SLAM 算法是一个迭代的估计问题,3-1式的结果是t-1时刻的估计值p(xt-1,m|z1:t-1,u1:t-1)根据t时刻 的控制输入ut和观测值zt计算得到,同时SLAM也是一个马尔可夫过程,如图1所示,在已知 机器人位置的情况下,地图由某一位置状态下的观测数据构成。
概率模型建立,为了将机器人的实际运动与观测转换为概率的形式,需要对机器人运动、 传感器观测、地图表示进行概率学建模。
运动概率学建模,运动概率学建模的过程就是将实际的控制输入转化为算法计算需要的 运动学概率值的过程。目前使用最为广泛的是速度控制模型与里程计控制模型,本课题使用 里程计运动模型。二维环境下,机器人在全局坐标下的位姿向量如图2(a)所示,表示为 (px,py,pθ)T,因此p(xt|xt-1,ut)表示在控制输入ut的作用下,位姿状态向量xt可能出现的概率。 如图2(b)所示,机器人从A状态移动到B状态,由于误差的原因无法知道B状态的确切位 置,颜色越深表示B状态位置的可能性越高。
对于里程计运动模型,在t-1到t的时间间隔内里程计反馈的为机器人位姿状态的相对变换估计值,如下式3-2所示:
其中为了能够准确的计算状态转移概率,将ut分 解为旋转-平移-旋转的步骤,如图3所示:
初始旋转为δrot1,平移为δtrans,第二个旋转为δrot2。每一对位置都有一个独特的参数向 量(δrot1transrot2)T。概率学运动模型假设这些参数由独立的噪声所扰动。针对这三个参数我 们可以得到状态转移概率的计算过程:
计算控制输入变换参数:
计算由控制输入导致的状态估计变换参数:
计算状态转移概率:
其中参数αrrtrrttt分别表示由角度变换引起的角度误差参数、平移变换引起的角度 误差参数、角度变换引起的平移误差参数、平移变换引起的平移误差参数,getP(a,b)函数表 示a在均值为0,方差为b的高斯分布下的概率值。最终的状态转移概率值即为3-5式中各个 结果的乘积:
p(xt|xt-1,ut)=p1·p2·p3 (3-6);
传感器观测模型建立。传感器观测概率模型种类较多,例如基于激光雷达的光束模型、 可能性区域模型、相关性模型,基于特征识别传感器的特征测量模型等等。本申请将使用基 于激光雷达的可能性区域模型对观测信息进行建模。
未经处理的原始激光雷达数据为K组距离值与角度值,距离值表示环境障碍物点与激光 雷达之间的距离,角度值表示该障碍点与激光雷达正方向的夹角(通常激光雷达正方向与机器 人正方向一致)。对于t时刻任意一组激光数据其中r代表距离,表示角度, 为了计算激光终点在全局坐标系下的位置,我们需要知道雷达本体相对于机器人中心的安装 位置(xsens,ysens)T与机器人本身在世界坐标系下的估计位置通过坐标系间的变换 关系我们可以得到式3-7:
其中就代表了第个k激光光束终点在全局坐标系下的位置。值得注意的是仅当 传感器观测到障碍物时式上式的结果才有意义,因此实际使用中一般都会设置r的范围,将 大于某一阈值范围外的传感器数据舍弃掉。
为了计算观测概率,同运动模型类似,观测模型也引入了三种类型的噪声:
测量噪声。测量噪声使用高斯分布表示,对任意一组激光数据的端点坐标计 算其与现有地图中最近障碍物坐标的欧式距离dist,则由测量噪声导致的传感器测量概率值 如下式:
错误测量值噪声。对于激光雷达此类的距离传感器,较为典型的错误测量值就是返回激 光数据的最大范围,但由于实际应用中对激光数据的预处理会将最大观测距离的数据过滤掉, 因此计算时不做考虑。
随机测量噪声。当激光传感器击中半透明玻璃,或者声纳传感器接收到同波段声波时都 会产生不固定的随机噪声,此随机噪声一般用均匀分布表示,如下:
根据上述观测模型的误差即可计算出观测概率p(zt|xt,m),计算公式如下:
其中q表示每个激光数据的观测概率,zhit表示测量噪声出现的概率阈值,zrand表示随机 噪声出现的概率阈值,一般情况下zhit远大于zrand且两者和为1。
相对于其他几种观测模型,可能性区域模型的优点在于其平滑性,当机器人位姿移动较 小时,观测概率p(zt|xt,m)并不会发生大幅度的跳变,因此该模型也被应用在扫描匹配算法 当中。
栅格地图模型建立。目前2D环境下的地图表示方式主要有拓扑地图,特征地图,栅格地 图等。本课题使用的地图描述方式为占据栅格地图,将整个待估计二维空间表示为一定大小 的网格(cell)集合,网格的尺寸大小表示地图的分辨率,每个网格维护一个二进制值,0代 表该网格处无障碍(free),1代表该网格坐标处有障碍(occupied)。使用概率学估计占据栅格 地图可抽象为对概率p(m|z1:t,x1:t)的计算。
值得注意的是,假定一副地图由1000个网格构成,那么由这些网格构成的地图数量有 21000个,单独估计每幅地图的概率显然是不现实的,因此需单独的对每一个网格的占据概 率进行估计:
由于占据网格概率的独特性质,可以使用下式3-12的形式简化概率计算,同时避免占据 概率过于接近0或者1而导致错误估计情况的发生:
实际上,每进行一次估计时并不会更新地图内的所有网格,而是仅针对K组激光数据扫 描过的网格展开计算,因此我们可以得到地图模型的更新过程:
获取每组激光数据的端点以及机器人与端点之间的地图网格坐标。
t时刻对于激光光束端点的网格占据值lt,i=lt-1,i+locc,locc表示占据更新值:
t时刻对于机器人与激光雷达端点之间的网格占据值lt,i=lt-1,i+lfree,lfree表示空闲更新 值。
图4就表示了一部分激光数据的更新方式,三角形代表机器人,假定两台边线之间激光 光束稠密,黑色区域网格为占据网格,白色区域网格为空闲网格,灰色区域网格为未知环境 网格,特别的,未知区域网格p(mi)=0.5,lt,i=0。
粒子滤波基本概念
粒子滤波采用状态空间内的有限个样本来对结果后验概率进行估计,减少了高斯滤波器 中对非线性系统的线性化步骤带来的误差。标准的粒子滤波是一个迭代的估计过程,t时刻系 统的更新需要上一t-1时刻的粒子集Xt-1,控制输入ut,观测值zt作为算法输入。具体的,更 新过程主要分为采样,加权,重采样三个步骤,粒子集由待估计状态样本与粒子权值构成。
采样,采样过程表示t-1时刻的待估计状态在控制输入ut的作用下发生的改变,假设粒 子总数为M,则对于单个粒子有:
其中m表示粒子集内粒子序号,p(xt|ut,xt-1)称为提议分布。对于t-1时刻的粒子集,经 过t时刻的采样过程后,粒子集Xt-1变为如下式3-14:
其中w表示每个粒子的权值,采样过程不会使权值发生任何变化。特别的,图5为根据 提议分布进行的采样计算示意图:
其中黑色矩形表示粒子,粒子的个数为10个,从图5中可以看到粒子的分布遵循提议分 布在状态空间中的分布形式,粒子越密集表示待估计状态落在该区域的可能性越高。
加权。经过采样步骤,已经得到了t时刻状态的M个初始估计粒子,加权步骤会引入传 感器观测信息,权值的计算如下式3-15:
对应的,粒子权值计算的示意图如图6所示,粒子的权值越高,其对应的黑色矩形高度 越高。
重采样。重采样也叫做重要性采样过程。t-1时刻的粒子集Xt-1经过对提议分布采样,计 算权值两步骤已经得到了加权粒子集重采样步骤将中权值较小的粒子舍弃,用较大 权值粒子代替其位置,意味着重采样过程后,粒子集中粒子总数目并不会发生改变,某些权 值较大的粒子会在粒子集中多次出现。重采样的示意图如图7所示,重采样完成后,对每个 粒子的权值进行归一化处理,粒子的分布逐渐向目标分布接近。此时的粒子集已经可以代表 目标分布。
对于标准粒子滤波算法来说,粒子数目越大,系统精度越高,但相应的计算时间会大大 增加,同时频繁的重采样会使粒子的多样性减少,有限的粒子数目,极小的粒子多样性会使 粒子滤波的估计结果出现较大的偏差,因为当t时刻的控制输入到来时,采样步骤得到的粒子 无法代表提议分布的空间状态分布。但随着滤波算法不断迭代,不进行重采样或极少数的运 行重采样步骤会使少数粒子维护较大的权值,这意味着大多数粒子的权值较小,滤波系统需 要耗费大量的计算资源来处理更新对整个状态估计过程无用的粒子。综上所示,粒子数目的 选择与重采样步骤的运行时机对整个估计系统的精度来说十分重要。
基于粒子滤波的SLAM算法优化
基于粒子滤波的激光SLAM算法核心在于将机器人位置与地图分开进行估计,每个粒子内 存在一组机器人位置数据与地图数据,这意味着每次地图的更新都假设机器人位置已知且准 确,如下3-16所示。
具体的,基于粒子滤波的SLAM算法主要步骤主要有初始化,粒子采样,扫描匹配,权值 计算,重采样五个步骤,下文将依次对其进行详细描述。
为了直观描述算法效果使用了ROS+Gazebo+RVIZ的三维仿真环境,使用Gazebo中的激光 雷达模拟插件获取虚拟激光数据,使用双轮差分驱动插件获取里程计数据,并假定此数据为 机器人真实位置数据,并通过对此数据添加噪声来验证估计结果。生成的栅格地图在RVIZ中 显示,同时仿真使用键盘控制环境中的机器人进行移动,验证本申请所描述的算法效果与可 行性。特别的,虚拟激光雷达的使用参数如表1所示,其中采样精度1弧度表示在-π~π的 区间范围内共有360组激光数据,角度增益为1弧度。
仿真环境与激光雷达数据如图8(a)和图8(b)所示,机器人起始位置即为全局坐标系 下的原点,且此时机器人朝向为x轴正方向,可以看到-π~π范围内的激光数据反应了机器 人周围360度区域内的所有障碍物信息,如图8(a)和图8(b)所示。
表1仿真激光参数
为了方便说明基于粒子滤波的SLAM算法实现过程,机器人在图8(a)中以如图9所示 的轨迹进行移动,算法步骤将结合此仿真过程进行说明。
初始化,SLAM算法起始即t=0时刻开始执行初始化步骤,由于此时未接收到任何传感器 数据,因此本申请默认初始机器人位姿向量为(0,0,0)T,初始化步骤的目的有两个,一是根 据粒子总数初始化粒子集,二是根据第一帧激光数据初始化环境地图,此地图将作为后续迭 代估计算法的输入,初始化步骤得到的地图表示为m0。初始化完成后的粒子内容为式1-17, 其中每个粒子的权值相同,均为1/M。
由式3-16可知,已知机器人位置时,仅根据传感器数据即可对地图进行更新,初始状态 时对于一帧中的所有K组激光数据根据式3-10可计算得到激光光束 端点在全局坐标系下的位置并初始化地图。具体的,设置地图分辨率为5cm,使用RVIZ观察 初始化步骤得到的地图如图10所示,其中黑色区域表示机器人周围障碍物,对应图4中机器 人四周墙壁,白色区域表示空闲无障碍,黑色区域表示环境未知,传感器无法获得此处数据。
采样。采样过程与标准粒子滤波算法类似,t时刻粒子代表的机器人位置根据控制输入ut与t-1时刻的机器人位置得到,对于本课题使用的里程计运动模型,采样过程相当于状态转 移过程的逆,对位姿状态的采样实际为对误差分布进行采样,粒子中位置的更新如下式3-18 所示,其中δ′trans,δ′rot1,δ′rot2分别代表了带误差的位置变换值:
如图11所示对于图9所示的仿真过程,取t=10时刻进行分析。不考虑机器人角度数据, t=9时的机器人真实位置坐标为(2.12,-1.10),估计位置结果为(2.16,-1.16)。t=10时刻 的机器人真实位置变为(2.17,-1.58),相比于上一时刻其真实位置变换为(0.05,-0.48), 但由于噪声的影响,t时刻里程计获取到的机器人位置变换为(-0.03,-0.51)。t时刻的粒子 集将由此变换值与t-1时刻的粒子集采样得到,其中,粒子数总数为30,黑色点表示粒子位 置,每个粒子代表了一个真实状态可能出现的结果,t时刻的样本集转换为下式3-19的形式。
扫描匹配。粒子滤波系统的整体估计精度与粒子数目的选取密切相关,但对于SLAM 算法,每个粒子内不仅包括机器人位置数据,还维护一份包含有多个个网格的栅格地图。粒 子数目过大会使整个系统运算时间增加,算法可用性降低。为了减少粒子数目,优化粒子采 样结果,本申请使用了基于可能性区域模型的扫描匹配算法。其核心思想在于将激光数据与 已有地图进行匹配,进一步改善机器人位置,特别的,图12即表示了一组待匹配地图与激光 数据。
粒子滤波算法中,在权值计算步骤前根据激光雷达数据与当前栅格地图对每个粒子内的 机器人位置数据进行优化。如下式3-20所示:
对于图11中所示的粒子分布,扫描匹配后的结果如图13(a)和图13(b)所示,可以看到,扫描匹配步骤后粒子总体开始朝机器人真实位置处变化。
更进一步,由于t时刻的粒子是由t-1时刻的粒子通过采样计算得到,粒子误差会随时间 累积。为了更好的描述扫描匹配的作用,此处假设不进行重采样,即粒子集内的样本不会发 生删除,取代等操作。定义误差和E,表示t时刻每个粒子内机器人位置与真实位置的偏差值。 如下式:
其中分别代表机器人的真实位置值。
采用图9中的仿真方式,记录有无扫描匹配过程的总误差和可以得到图14(a)和图14 (b),其中B线条表示无扫描匹配过程发生,A线条表示扫描匹配过程后的误差总和,可以看到扫描匹配算法极大的改善了粒子集中机器人位置分布情况,使其更接近机器人真实位置。 但随着时间戳的增加,机器人不断移动,无论是否进行扫描匹配,粒子集代表的总体机器人 位置误差都在不断增加,为了解决此问题,需要重采样步骤改善粒子内容,将误差较大的粒 子舍弃,保留精准粒子。
权值计算与重采样。与标准粒子滤波算法中权值的计算略有不同,第m个粒子的权值计 算方式如下式3-22所示:
上式中的概率值正是计算得到的观测概率值。同时本申请使用了自适应重采 样技术决定重采样步骤何时发生,如下式:
其中Neff又被称为有效粒子数,当Neff值小于某一给定阈值时进行重采样步骤,否则 在权值计算完成后直接进行下一次算法迭代。与标准粒子滤波中的重采样步骤类似,重采样 发生时,每个粒子根据其权值的大小确定此粒子被保留的概率,小权值粒子会在重采样过程 中由权值较大的粒子代替,保证粒子总数不发生变化。
综上所述,基于粒子滤波的SLAM算法流程图如图15所示。此SLAM算法可以得到一个较 好的估计结果。
以上所述的具体实施方式,对本发明的目的、技术方案和有益效果进行了进一步详细说 明,所应理解的是,以上所述仅为本发明的具体实施方式而已,并不用于限定本发明的保护 范围,凡在本发明的精神和原则之内,所做的任何修改、等同替换、改进等,均应包含在本 发明的保护范围之内。

Claims (10)

1.一种基于粒子滤波的SLAM优化方法,其特征在于,至少包括以下步骤;
步骤1,对机器人建立概率模型,所述建立概率模型包括运动概率学建模、传感器观测模型建立和栅格地图模型建立;
步骤2,概率模型结合粒子滤波对机器人即时定位与地图构建进行优化,优化过程包括初始化、采样、扫描匹配、权值计算和重采样;其中,优化过程引入多个变量;所述多个变量包括:
xt:描述机器人在全局坐标下的位置以及朝向的状态变量,具体为
ut:机器人控制向量,t时刻的控制向量表示经此控制驱动机器人在t-1时刻的位置状态由xt-1转换到了状态xt,且将两时刻内的位置变换看作控制输入;
m:地图状态变量,对环境信息的直观描述;
表示传感器在t时刻观测得到的第k个环境信息,由于本课题采用的是激光数据,k表示第k个激光光束。
2.根据权利要求1所述的一种基于粒子滤波的SLAM优化方法,其特征在于,所述运动概率学建模采用里程计运动模型,二维环境下,机器人在全局坐标下的位姿向量表示为(px,py,pθ)T,则p(xt|xt-1,ut)表示在控制输入ut的作用下,位姿状态向量xt可能出现的概率;当机器人从A状态移动到B状态,颜色越深表示位置精度越高。
3.根据权利要求1所述的一种基于粒子滤波的SLAM优化方法,其特征在于,所述传感器观测模型建立基于激光雷达的可能性区域模型对观测信息进行建模。
4.根据权利要求3所述的一种基于粒子滤波的SLAM优化方法,其特征在于,所述传感器观测模型建立引入至少三种类型的噪声,至少三种类型的噪声包括:测量噪声、错误测量值噪声和随机测量噪声。
5.根据权利要求4所述的一种基于粒子滤波的SLAM优化方法,其特征在于,所述随机测量噪声包括当激光传感器击中半透明玻璃,或者声纳传感器接收到同波段声波时都会产生不固定的随机噪声,此随机噪声用均匀分布表示,如下式3-9所示:
根据传感器观测模型的误差即可计算出观测概率p(zt|xt,m),计算公式3-10如下:
其中q表示每个激光数据的观测概率,zhit表示测量噪声出现的概率阈值,zrand表示随机噪声出现的概率阈值,zhit远大于zrand且两者和为1。
6.根据权利要求1所述的一种基于粒子滤波的SLAM优化方法,其特征在于,所述栅格地图模型建立采用占据栅格地图,具体的,将整个待估计二维空间表示为具有大小值的网格集合,网格的尺寸大小表示地图的分辨率,每个网格维护一个二进制值,0代表该网格处无障碍,1代表该网格坐标处有障碍。
7.根据权利要求5所述的一种基于粒子滤波的SLAM优化方法,其特征在于,所述初始化包括在SLAM算法起始即t=0时刻开始执行,默认初始机器人位姿向量为(0,0,0)T,初始化得到的地图表示为m0,初始化完成后的粒子内容如公式3-17,其中每个粒子的权值相同,均为1/M,
当已知机器人位置时,仅根据传感器数据即可对地图进行更新,初始状态时为对于一帧中的所有K组激光数据根据式3-10可计算得到激光光束端点在全局坐标系下的位置并初始化地图。
8.根据权利要求1所述的一种基于粒子滤波的SLAM优化方法,其特征在于,所述采样包括使用里程计运动模型进行采样,粒子中位置的更新如下式3-18所示,其中δ′trans,δ′rot1,δ′rot2分别代表了带误差的位置变换值:
9.根据权利要求1所述的一种基于粒子滤波的SLAM优化方法,其特征在于,所述扫描匹配采用基于可能性区域模型的扫描匹配算法。
10.根据权利要求1所述的一种基于粒子滤波的SLAM优化方法,其特征在于,所述权值计算与重采样包括第m个粒子的权值计算方式如下式3-22所示:
所述重采样采用自适应重采样技术决定重采样步骤何时发生,如下式3-23所示:
其中Neff又被称为有效粒子数,当Neff值小于某一给定阈值时进行重采样步骤,否则在权值计算完成后直接进行下一次算法迭代。
CN201910620350.0A 2019-07-10 2019-07-10 一种基于粒子滤波的slam优化方法 Pending CN110333720A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910620350.0A CN110333720A (zh) 2019-07-10 2019-07-10 一种基于粒子滤波的slam优化方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910620350.0A CN110333720A (zh) 2019-07-10 2019-07-10 一种基于粒子滤波的slam优化方法

Publications (1)

Publication Number Publication Date
CN110333720A true CN110333720A (zh) 2019-10-15

Family

ID=68146157

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910620350.0A Pending CN110333720A (zh) 2019-07-10 2019-07-10 一种基于粒子滤波的slam优化方法

Country Status (1)

Country Link
CN (1) CN110333720A (zh)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110887489A (zh) * 2019-11-22 2020-03-17 深圳晨芯时代科技有限公司 一种基于ar机器人的slam算法的实验方法
CN110956697A (zh) * 2019-12-20 2020-04-03 上海有个机器人有限公司 基于激光slam的内存优化方法、介质、终端和装置
CN111024100A (zh) * 2019-12-20 2020-04-17 深圳市优必选科技股份有限公司 一种导航地图更新方法、装置、可读存储介质及机器人
CN111105495A (zh) * 2019-11-26 2020-05-05 四川阿泰因机器人智能装备有限公司 一种融合视觉语义信息的激光雷达建图方法及系统
CN111207754A (zh) * 2020-02-28 2020-05-29 上海交通大学 基于粒子滤波器的多机器人编队定位方法及机器人设备
CN111210870A (zh) * 2019-12-27 2020-05-29 青岛海洋科学与技术国家实验室发展中心 蛋白质分类方法
CN111324116A (zh) * 2020-02-14 2020-06-23 南京航空航天大学 基于粒子滤波的机器人定位方法
CN111895990A (zh) * 2020-07-14 2020-11-06 武汉科技大学 一种基于多指标绑架检测及移动机器人重定位的方法
CN112732854A (zh) * 2021-01-11 2021-04-30 哈尔滨工程大学 一种粒子滤波bslam方法
CN112923933A (zh) * 2019-12-06 2021-06-08 北理慧动(常熟)车辆科技有限公司 一种激光雷达slam算法与惯导融合定位的方法
CN113111081A (zh) * 2021-04-16 2021-07-13 四川阿泰因机器人智能装备有限公司 一种弱特征环境下的移动机器人建图方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104236551A (zh) * 2014-09-28 2014-12-24 北京信息科技大学 一种蛇形机器人基于激光测距仪的地图创建方法
CN107063264A (zh) * 2017-04-13 2017-08-18 杭州申昊科技股份有限公司 一种适用于大规模变电站环境的机器人地图创建方法
CN107246873A (zh) * 2017-07-03 2017-10-13 哈尔滨工程大学 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法
WO2018160267A2 (en) * 2017-02-16 2018-09-07 Indiana University Research And Technology Corporation Cloud based robotic control systems and methods
CN109781092A (zh) * 2019-01-19 2019-05-21 北京化工大学 一种危险化工事故中移动机器人定位与建图方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104236551A (zh) * 2014-09-28 2014-12-24 北京信息科技大学 一种蛇形机器人基于激光测距仪的地图创建方法
WO2018160267A2 (en) * 2017-02-16 2018-09-07 Indiana University Research And Technology Corporation Cloud based robotic control systems and methods
CN107063264A (zh) * 2017-04-13 2017-08-18 杭州申昊科技股份有限公司 一种适用于大规模变电站环境的机器人地图创建方法
CN107246873A (zh) * 2017-07-03 2017-10-13 哈尔滨工程大学 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法
CN109781092A (zh) * 2019-01-19 2019-05-21 北京化工大学 一种危险化工事故中移动机器人定位与建图方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
孙海波 等: "基于卡尔曼滤波与粒子滤波的SLAM研究综述", 《软件导刊》 *
徐新: "移动机器人在线环境感知与自定位方法研究", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 *

Cited By (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110887489A (zh) * 2019-11-22 2020-03-17 深圳晨芯时代科技有限公司 一种基于ar机器人的slam算法的实验方法
CN111105495A (zh) * 2019-11-26 2020-05-05 四川阿泰因机器人智能装备有限公司 一种融合视觉语义信息的激光雷达建图方法及系统
CN112923933A (zh) * 2019-12-06 2021-06-08 北理慧动(常熟)车辆科技有限公司 一种激光雷达slam算法与惯导融合定位的方法
CN110956697A (zh) * 2019-12-20 2020-04-03 上海有个机器人有限公司 基于激光slam的内存优化方法、介质、终端和装置
CN111024100A (zh) * 2019-12-20 2020-04-17 深圳市优必选科技股份有限公司 一种导航地图更新方法、装置、可读存储介质及机器人
CN110956697B (zh) * 2019-12-20 2023-05-05 上海有个机器人有限公司 基于激光slam的内存优化方法、介质、终端和装置
US11629964B2 (en) 2019-12-20 2023-04-18 Ubtech Robotics Corp Ltd Navigation map updating method and apparatus and robot using the same
CN111210870A (zh) * 2019-12-27 2020-05-29 青岛海洋科学与技术国家实验室发展中心 蛋白质分类方法
CN111210870B (zh) * 2019-12-27 2024-03-08 青岛海洋科技中心 蛋白质分类方法
CN111324116A (zh) * 2020-02-14 2020-06-23 南京航空航天大学 基于粒子滤波的机器人定位方法
CN111324116B (zh) * 2020-02-14 2021-09-21 南京航空航天大学 基于粒子滤波的机器人定位方法
CN111207754A (zh) * 2020-02-28 2020-05-29 上海交通大学 基于粒子滤波器的多机器人编队定位方法及机器人设备
CN111895990B (zh) * 2020-07-14 2022-04-15 武汉科技大学 一种基于多指标绑架检测及移动机器人重定位的方法
CN111895990A (zh) * 2020-07-14 2020-11-06 武汉科技大学 一种基于多指标绑架检测及移动机器人重定位的方法
CN112732854B (zh) * 2021-01-11 2023-03-31 哈尔滨工程大学 一种粒子滤波bslam方法
CN112732854A (zh) * 2021-01-11 2021-04-30 哈尔滨工程大学 一种粒子滤波bslam方法
CN113111081A (zh) * 2021-04-16 2021-07-13 四川阿泰因机器人智能装备有限公司 一种弱特征环境下的移动机器人建图方法

Similar Documents

Publication Publication Date Title
CN110333720A (zh) 一种基于粒子滤波的slam优化方法
Oleynikova et al. Voxblox: Incremental 3d euclidean signed distance fields for on-board mav planning
Wu et al. Plant phenotyping by deep-learning-based planner for multi-robots
Mallios et al. EKF-SLAM for AUV navigation under probabilistic sonar scan-matching
US9361590B2 (en) Information processing apparatus, information processing method, and program
O'Callaghan et al. Contextual occupancy maps using Gaussian processes
CN109798896A (zh) 一种室内机器人定位与建图方法及装置
CN113108773A (zh) 一种融合激光与视觉传感器的栅格地图构建方法
US20130243331A1 (en) Information processing device, information processing method, and program
Senanayake et al. Spatio-temporal hilbert maps for continuous occupancy representation in dynamic environments
CN110514567B (zh) 基于信息熵的气体源搜索方法
Hanten et al. Vector-AMCL: Vector based adaptive monte carlo localization for indoor maps
CN108564600A (zh) 运动物体姿态跟踪方法及装置
Wang et al. Virtual maps for autonomous exploration with pose SLAM
CN109033181B (zh) 一种复杂地形地区风场地理数值模拟方法
Huang et al. Incremental non-Gaussian inference for SLAM using normalizing flows
Pfaff et al. Gaussian mixture models for probabilistic localization
CN114329854A (zh) 一种基于多目标约束的二维空间视觉传感器布局优化方法
CN110333513A (zh) 一种融合最小二乘法的粒子滤波slam方法
CN114186112B (zh) 一种基于贝叶斯优化多重信息增益探索策略的机器人导航方法
CN115016499A (zh) 一种基于sca-ql的路径规划方法
Wang et al. Particle Filter with Hybrid Proposal Distribution for Nonlinear State Estimation.
Pfaff et al. Improved likelihood models for probabilistic localization based on range scans
Wang et al. Reinforcement learning for SAR view angle inversion with differentiable SAR renderer
Ding et al. Multi-strategy based exploration for 3D mapping in unknown environments using a mobile robot

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20191015

RJ01 Rejection of invention patent application after publication