CN107246873A - 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法 - Google Patents

一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法 Download PDF

Info

Publication number
CN107246873A
CN107246873A CN201710533573.4A CN201710533573A CN107246873A CN 107246873 A CN107246873 A CN 107246873A CN 201710533573 A CN201710533573 A CN 201710533573A CN 107246873 A CN107246873 A CN 107246873A
Authority
CN
China
Prior art keywords
mrow
msub
msubsup
particle
density function
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
CN201710533573.4A
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.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201710533573.4A priority Critical patent/CN107246873A/zh
Publication of CN107246873A publication Critical patent/CN107246873A/zh
Pending legal-status Critical Current

Links

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

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

本发明公开了一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法，初始化机器人初始时刻位姿；根据t‑1时刻的位姿信息，得到t时刻先验概率密度函数，生成采样粒子集p；对粒子的权值进行初始化处理；选取重要性概率密度函数，生成新采样粒子集q，计算粒子权重，更新粒子权值，进行权值的归一化处理；计算当前时刻t随机样本粒子的加权和来表示后验概率密度，得到移动位姿和环境地图信息；判断是否有新观测值输入，如果有则返回，如果没有则结束循环，在返回之前，判断是否需要进行重采样。根据系统状态的不同，设定动态的阈值进行判断，并结合了遗传算法。本发明减小了粒子退化问题对SLAM的影响，减小了SLAM问题的计算量。

Description

一种基于改进的粒子滤波的移动机器人同时定位与地图构建 的方法
技术领域
本发明涉及移动机器人自主定位与环境感知技术领域，特别涉及一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法。
背景技术
伴随着机器人技术的蓬勃发展，机器人对未知环境的自主认知能力已经成为机器人学中研究的一个热点。构建未知环境的地图，也就是移动机器人的定位与导航问题是机器人自主认知的其中一项重点研究内容和研究热点。其中，同时定位与地图构建(SLAM)则是移动机器人实现定位和导航的一种有效手段，就是机器人根据获取到的传感器相关数据，提取特征，经过特征匹配，最终自主构建机器人所处未知环境的地图并同时获取自身位置，对自身进行定位。进而，可以利用构建好的环境地图，继续进行其他的相关研究，如：路径规划，导航避障，完成任务等。
早期的未知环境的地图构建常用的方式是航迹推算法，一般都是假设获得到的自身传感器数据是精确的，并且假设机器人的位姿是可以完全通过传感器数据得到。根据这种方法，提出了多种构建环境地图的方式，如：基于卡尔曼滤波的构建特征地图的方法。实际上，通过航迹推算，即只通过自身传感器获得机器人的位姿是不准确的，它会存在一定的测量误差。误差的来源主要是轮子打滑、地面的摩擦等因素，而且这些误差会随着时间的增长而累积，最终导致机器人完全偏离实际的航线，所建的未知环境地图完全错误。
目前现有的常用于同时定位与地图构建的方法有：基于卡尔曼的滤波方法(Kalman Filter，缩写为KF)、基于扩展卡尔曼滤波算法(Extended Kalman Filter，缩写为EKF)、基于粒子滤波算法(Particle Filter，缩写为PF)、基于Markov原理的蒙特卡洛方法、基于最大似然估计的SLAM方法等。
卡尔曼滤波和扩展卡尔曼滤波对于系统的要求比较高，需要对运动模型和观测模型进行近似线性化处理，而且要求系统噪声为白噪声。在计算时，运用卡尔曼滤波算法对系统的状态递归的进行估计，需要反复的计算协防矩阵，计算量极大。扩展卡尔曼滤波在SLAM问题中应用很广泛，也是很多其他方法的理论基础。但是，该方法存在对系统的要求高以及计算量大的问题，算法复杂、用时长，而且近似线性化的过程会引入线性化误差。
粒子滤波算法的基本思想是用随机样本来描述概率分布。在测量的基础上，通过调节各个粒子的权值大小和样本位置来近似实际的概率分布，并以样本均值作为系统的估计值。但是粒子滤波存在计算量较大，还有经过多次迭代，粒子会存在退化的问题。
发明内容
本发明的目的在于提供一种实现未知环境下的移动机器人建图与定位功能的基于改进的粒子滤波的移动机器人同时定位与地图构建的方法。
一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法，包括以下步骤：
S1：初始化机器人初始时刻位姿，默认起始位置为零位；
S2：根据移动机器人t-1时刻的位姿信息，即t-1时刻的后验概率密度函数，通过已有的先验知识对当前时刻的状态进行预测，得到t时刻先验概率密度函数，根据先验概率密度函数生成采样粒子集p；采集N个粒子，对粒子的权值进行初始化处理，均为1/N；
S3：选取重要性概率密度函数，即运动模型和观测值相结合的建议分布，生成新的采样粒子集q，并计算粒子权重，更新粒子权值，并进行权值的归一化处理；
S4：根据外部传感器获取的观测信息，根据采样N个粒子以及得到的粒子权值，计算当前时刻t随机样本粒子的加权和来表示后验概率密度函数估计，得到当前时刻t移动机器人的位姿和环境的地图信息，作为输出，用于同时定位与地图构建时更新地图信息；
S5：判断是否有新的观测值输入，如果有新的观测值输入，判断是否有足够的有效粒子，设置有效粒子动态阈值T，将有效粒子数与T进行比较，如果小于T，则结合遗传算法进行重采样，然后返回S3，如果大于等于T，则直接返回S3；如果没有新的观测值输入，则结束循环，得到环境地图。
S2所述的获取t时刻的先验概率密度函数的方法为：
根据t-1时刻的后验概率密度函数p(xt-1|y1：t-1)，通过已有的先验知识对t时刻的状态进行预测，得到t时刻的先验概率密度函数p(xt|y1：t-1)，服从一阶马尔科夫模型：
p(xt|y1：t-1)＝∫p(xt|xt-1)p(xt-1|y1：t-1)dxt-1
其中，p(xt|xt-1)为状态变量的转移概率密度函数，x为机器人的位姿估计，y为外部传感器获取到的环境观测值。
S2所述的由t时刻先验概率密度函数生成采样粒子集p，采集N个粒子，为：
S3具体为：
引入一个已知的、容易采样的重要性概率密度函数q(x0：t|y1：t)，选择接近系统状态后验概率，而且能使重要性权重方差最小，选择的重要性概率密度函数为：
根据重要性概率密度函数获得新的采样粒子集q，从粒子集q中随机抽样得到N个独立同分布的粒子：
此时f(xt)的期望表示为：
其中，f(xt)为系统的状态方程函数；w(xt)为重要性权重：
得到权重w(xt)，即表示为第i个粒子在t时刻的权值，并进行权值的归一化处理：
S4所述的获得环境地图的机器人位姿、更新地图的方法具体为：
根据分布获得的随机样本粒子的加权和来表示后验概率密度函数估计，得到t时刻移动机器人的位姿估计：
其中：表示为第i个粒子在t时刻的状态，粒子滤波器中的每个粒子都代表对机器人路径的估计，当粒子的后验概率密度函数，即t时刻机器人的位姿更新的时候，结合外部传感器获取到的观测信息，同时更新地图，即对环境信息进行特征提取和匹配，机器人的当前位置根据机器人起始位置和当前时刻的观测共同确定。
S5中有效例子为：
S5中有效粒子动态阈值T的范围设定在
与传统的机器人基于扩展卡尔曼滤波、粒子滤波的同时定位与地图构建不同，本发明采用改进的粒子滤波算法，针对不同的环境，设定不同的动态阈值，判断系统是否需要进行重采样，重采样过程结合了遗传算法，可以更好地保留好的粒子，减小了粒子退化问题对SLAM的影响，这样一来减小了SLAM问题的计算量，效果更好，实现起来更加方便。
粒子滤波是基于蒙特卡洛方法，将复杂的积分问题转化为求均值的问题，简化求解过程；粒子滤波对于基于控制量和观测量的后验概率分布可以准确的表达。改进的粒子滤波根据地图的复杂度，设定动态阈值，减小计算量；再加上机器人操作系统ROS开发全面，代码开源，接口统一等优势，因此基于改进的粒子滤波算法的同时定位与地图构建方法可以作为移动机器人SLAM领域中一个非常有效而且高效的手段。
本发明涉及的方法解决了移动机器人在未知环境中的同时定位与地图构建问题，利用改进的粒子滤波算法，在机器人操作系统ROS上实现地图的构建和机器人位姿的估计，为移动机器人接下来的导航问题奠定了基础，也可以直接应用到诸如工业机器人、服务型机器人或者探测机器人的领域中。
附图说明
图1是本发明方法流程图；
具体实施方式
基于改进的粒子滤波的移动机器人同时定位与地图构建方法，通过利用改进的粒子滤波算法，在选择重要性概率密度函数时，选用运动模型和观测值相结合的建议分布。在粒子滤波的重采样阶段，将用于判断有效粒子数的阈值设定为动态阈值，根据环境的复杂度选择不同的临界值；选择引入遗传算法，将该算法中的选择，交叉，变异，遗传也应用在粒子的选择中，即保证了粒子数充足，又防止粒子单一性。
下面结合附图对本发明进行详细说明。
首先初始化机器人初始时刻的位姿，一般定义相对位置，即设定初始时刻的位姿为(0，0，0)。
机器人的系统一般可以表示为它的状态方程和观测方程：
状态方程：xt＝f(xt-1，vt-1)
观测方程：yt＝h(xt，nt)
其中：x为系统状态，y为观测到的数据，f和h分别表示状态转移函数和测量函数，v和n分别表示过程噪声和测量噪声。
根据t-1时刻的后验概率密度函数p(xt-1|y1：t-1)，通过已有的先验知识对t时刻的状态进行猜测，得到t时刻的先验概率密度函数p(xt|y1：t-1)，服从一阶马尔科夫模型：
其中，p(xt|xt-1)为状态变量的转移概率密度函数，x为机器人的位姿估计，y为外部传感器获取到的环境观测值。
由当前时刻的先验概率生成采样粒子集p：
对粒子的权值进行初始化处理均为则粒子的分布p的一个估计可以表示为如下形式：
其中：δ(x-xt)为单位冲激函数(狄拉克函数)，即δ(x-xt)＝0，x≠xt，且∫δ(x)dx＝1
根据蒙特卡洛方法将复杂的积分过程近似为求和过程进行求解，当前状态的期望可以表示为：
选取重要性概率密度函数：
引入一个已知的、容易采样的重要性概率密度函数q(x0：t|y1：t)，可以分解为：
q(x0：t|y0：t)＝q(x0：t-1|y0：t-1)q(xt|x0：t-1，y0：t)
重要性概率密度函数结合了外部传感器获得的观测值y1：t和运动模型，尽可能的选择接近系统状态后验概率，选择重要性概率密度函数的一个标准是使得粒子权值的方差最小，提高抽样的效率。一般选择的重要性概率密度函数为：
根据重要性概率密度函数获得新的采样粒子集q，从粒子集q中随机抽样得到N个独立同分布的粒子：
引入重要性采样函数后，期望E[f(xt)]的可以表示为：
根据上述相应的关系，得到粒子的权重w(xt)：
更新当前的粒子的权值w(xt)，得到粒子权重的递归形式，即w(xt)和w(xt-1)的关系为：
粒子的权值归一化处理得：
根据分布获得的随机样本粒子的加权和来表示后验概率密度函数估计，得到t时刻移动机器人的位姿估计：
其中：表示为第i个粒子在t时刻的状态，粒子滤波器中的每个粒子都代表对机器人路径的估计。
当粒子的后验概率密度函数，即t时刻机器人的位姿更新的时候，作为系统的输出结合外部传感器获取到的观测信息，同时更新地图，即对环境信息进行特征提取和匹配，得到所处未知环境的地图。机器人的当前位置可以根据机器人起始位置和当前时刻的观测共同确定。
同时判断是否有新的观测值的输入，如果有，则重新进行上述步骤的循环，不断的更新地图，直至建图结束，不再有新的观测值输入。
判断如果有新的观测值的输入，循环重新计算之前，需要判断系统是否需要重采样。由于经过多次迭代后，受到环境和噪声的影响，粒子的权值方差会随着循环不断增大，权值退化严重，有效粒子数减少。利用有效粒子来衡量粒子权值的退化程度。有效粒子越小，权重方差越大，粒子权值退化严重。计算t时刻系统有效粒子为：
根据系统的复杂程度设定动态阈值T，与有效粒子进行比较。如果系统状态复杂，粒子数多，可设定动态阈值为如果系统状态简单，粒子数少，可设定动态阈值为动态阈值T的大致范围可以设定在如果有效粒子数小于阈值T，则结合遗传算法进行重采样后，返回执行选取重要性概率密度函数；反之，则跳过重采样，直接执行选取重要性概率密度函数。
若有效粒子小于设定的阈值，就需要进行重采样，以丰富粒子的多样性和有效性。
重采样的方法是：
保持粒子数目不变，在粒子归一化的基础上，通过服从U[0，1)的均匀分布中产生的随机数来确定粒子是否被复制。将权值大的粒子依照权值所占比例进行复制。在实现重采样时，可以结合遗传算法。对于权重大粒子，利用遗传算法中的选择、交叉、遗传、变异等方式，留下优良的粒子，并继续进行迭代。既保证了有效粒子个数的充足，还能保证粒子的多样性。即：对粒子集进行重采样，重采样后的粒子集为将重采样后的粒子集的权值赋值为
重采样后：
其中：为t时刻的粒子，为t时刻重采样后的粒子
ni是粒子产生粒子时被复制的次数

Claims (7)

1.一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法，其特征是，包括以下步骤：
S1：初始化机器人初始时刻位姿，默认起始位置为零位；
S2：根据移动机器人t-1时刻的位姿信息，即t-1时刻的后验概率密度函数，通过已有的先验知识对当前时刻的状态进行预测，得到t时刻先验概率密度函数，根据先验概率密度函数生成采样粒子集p；采集N个粒子，对粒子的权值进行初始化处理，均为1/N；
S3：选取重要性概率密度函数，即运动模型和观测值相结合的建议分布，生成新的采样粒子集q，并计算粒子权重，更新粒子权值，并进行权值的归一化处理；
S4：根据外部传感器获取的观测信息，根据采样N个粒子以及得到的粒子权值，计算当前时刻t随机样本粒子的加权和来表示后验概率密度函数估计，得到当前时刻t移动机器人的位姿和环境的地图信息，作为输出，用于同时定位与地图构建时更新地图信息；
S5：判断是否有新的观测值输入，如果有新的观测值输入，判断是否有足够的有效粒子，设置有效粒子动态阈值T，将有效粒子数Neff与T进行比较，如果Neff小于T，则结合遗传算法进行重采样，然后返回S3，如果Neff大于等于T，则直接返回S3；如果没有新的观测值输入，则结束循环，得到环境地图。
2.根据权利要求1所述的一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法，其特征在于：
S2所述的获取t时刻的先验概率密度函数的方法为：
根据t-1时刻的后验概率密度函数p(xt-1|y1:t-1)，通过已有的先验知识对t时刻的状态进行预测，得到t时刻的先验概率密度函数p(xt|y1:t-1)，服从一阶马尔科夫模型：
p(xt|y1:t-1)＝∫p(xt|xt-1)p(xt-1|y1:t-1)dxt-1
其中，p(xt|xt-1)为状态变量的转移概率密度函数，x为机器人的位姿估计，y为外部传感器获取到的环境观测值。
3.根据权利要求1所述的一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法，其特征在于：
S2所述的由t时刻先验概率密度函数生成采样粒子集p，采集N个粒子，为：
<mrow> <mo>{</mo> <msubsup> <mi>x</mi> <mrow> <mn>0</mn> <mo>:</mo> <mi>t</mi> </mrow> <mi>i</mi> </msubsup> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>~</mo> <mi>N</mi> <mo>}</mo> <mo>.</mo> </mrow>
4.根据权利要求1所述的一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法，其特征在于：
S3具体为：
引入一个已知的、容易采样的重要性概率密度函数q(x0:t|y1:t)，选择接近系统状态后验概率，而且能使重要性权重方差最小，选择的重要性概率密度函数为：
<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>q</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mn>0</mn> <mo>:</mo> <mi>t</mi> </mrow> </msub> <mo>|</mo> <msub> <mi>y</mi> <mrow> <mn>1</mn> <mo>:</mo> <mi>t</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mi>p</mi> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mi>t</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>|</mo> <msubsup> <mi>x</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>,</mo> <msub> <mi>y</mi> <mi>t</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mfrac> <mrow> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>t</mi> </msub> <mo>|</mo> <msubsup> <mi>x</mi> <mi>t</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>,</mo> <msubsup> <mi>x</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>)</mo> </mrow> <mi>p</mi> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mi>t</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>|</mo> <msubsup> <mi>x</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>)</mo> </mrow> </mrow> <mrow> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>t</mi> </msub> <mo>|</mo> <msubsup> <mi>x</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>)</mo> </mrow> </mrow> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mfrac> <mrow> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>t</mi> </msub> <mo>|</mo> <msubsup> <mi>x</mi> <mi>t</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>)</mo> </mrow> <mi>p</mi> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mi>t</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>|</mo> <msubsup> <mi>x</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>)</mo> </mrow> </mrow> <mrow> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>t</mi> </msub> <mo>|</mo> <msubsup> <mi>x</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>)</mo> </mrow> </mrow> </mfrac> </mrow> </mtd> </mtr> </mtable> </mfenced> 1
根据重要性概率密度函数获得新的采样粒子集q，从粒子集q中随机抽样得到N个独立同分布的粒子：
<mrow> <mo>{</mo> <msubsup> <mi>x</mi> <mrow> <mn>0</mn> <mo>:</mo> <mi>t</mi> </mrow> <mi>i</mi> </msubsup> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>~</mo> <mi>N</mi> <mo>}</mo> </mrow>
此时f(xt)的期望表示为：
<mrow> <mi>E</mi> <mrow> <mo>(</mo> <mi>f</mi> <mo>(</mo> <msub> <mi>x</mi> <mi>t</mi> </msub> <mo>)</mo> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mrow> <mo>&amp;Integral;</mo> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>t</mi> </msub> <mo>)</mo> </mrow> <mi>w</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>t</mi> </msub> <mo>)</mo> </mrow> <mi>q</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>t</mi> </msub> <mo>|</mo> <msub> <mi>y</mi> <mrow> <mn>1</mn> <mo>:</mo> <mi>t</mi> </mrow> </msub> <mo>)</mo> </mrow> <msub> <mi>dx</mi> <mi>t</mi> </msub> </mrow> <mrow> <mo>&amp;Integral;</mo> <mi>w</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>t</mi> </msub> <mo>)</mo> </mrow> <mi>q</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>t</mi> </msub> <mo>|</mo> <msub> <mi>y</mi> <mrow> <mn>1</mn> <mo>:</mo> <mi>t</mi> </mrow> </msub> <mo>)</mo> </mrow> <msub> <mi>dx</mi> <mi>t</mi> </msub> </mrow> </mfrac> </mrow>
其中，f(xt)为系统的状态方程函数；w(xt)为重要性权重：
<mrow> <mi>w</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>t</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mrow> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>t</mi> </msub> <mo>|</mo> <msub> <mi>y</mi> <mrow> <mn>1</mn> <mo>:</mo> <mi>t</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <mi>q</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>t</mi> </msub> <mo>|</mo> <msub> <mi>y</mi> <mrow> <mn>1</mn> <mo>:</mo> <mi>t</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mfrac> </mrow>
得到权重w(xt)，即表示为第i个粒子在t时刻的权值，并进行权值的归一化处理：
<mrow> <msubsup> <mover> <mi>w</mi> <mo>~</mo> </mover> <mi>t</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <mfrac> <msubsup> <mi>w</mi> <mi>t</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mrow> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <msubsup> <mi>w</mi> <mi>t</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> </mrow> </mfrac> <mo>.</mo> </mrow>
5.根据权利要求1所述的一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法，其特征在于：
S4所述的获得环境地图的机器人位姿、更新地图的方法具体为：
根据分布获得的随机样本粒子的加权和来表示后验概率密度函数估计，得到t时刻移动机器人的位姿估计：
<mrow> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>t</mi> </msub> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <msubsup> <mover> <mi>x</mi> <mo>~</mo> </mover> <mi>t</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <msubsup> <mover> <mi>w</mi> <mo>~</mo> </mover> <mi>t</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> </mrow>
其中：表示为第i个粒子在t时刻的状态，粒子滤波器中的每个粒子都代表对机器人路径的估计，当粒子的后验概率密度函数，即t时刻机器人的位姿更新的时候，结合外部传感器获取到的观测信息，同时更新地图，即对环境信息进行特征提取和匹配，机器人的当前位置根据机器人起始位置和当前时刻的观测共同确定。
6.根据权利要求1所述的一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法，其特征在于：S5中有效例子Neff为：
<mrow> <msub> <mi>N</mi> <mrow> <mi>e</mi> <mi>f</mi> <mi>f</mi> </mrow> </msub> <mo>&amp;ap;</mo> <mfrac> <mn>1</mn> <mrow> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <msup> <mrow> <mo>(</mo> <msubsup> <mi>w</mi> <mi>k</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mfrac> <mo>.</mo> </mrow>
7.根据权利要求1所述的一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法，其特征在于：S5中有效粒子动态阈值T的范围设定在
CN201710533573.4A 2017-07-03 2017-07-03 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法 Pending CN107246873A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710533573.4A CN107246873A (zh) 2017-07-03 2017-07-03 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710533573.4A CN107246873A (zh) 2017-07-03 2017-07-03 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法

Publications (1)

Publication Number Publication Date
CN107246873A true CN107246873A (zh) 2017-10-13

Family

ID=60014873

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710533573.4A Pending CN107246873A (zh) 2017-07-03 2017-07-03 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法

Country Status (1)

Country Link
CN (1) CN107246873A (zh)

Cited By (34)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108319572A (zh) * 2017-11-09 2018-07-24 电子科技大学中山学院 一种移动机器人故障诊断的混合自适应粒子滤波方法
CN109556611A (zh) * 2018-11-30 2019-04-02 广州高新兴机器人有限公司 一种基于图优化和粒子滤波的融合定位方法
CN109765569A (zh) * 2017-11-09 2019-05-17 电子科技大学中山学院 一种基于激光雷达实现虚拟航迹推算传感器的方法
CN109798896A (zh) * 2019-01-21 2019-05-24 东南大学 一种室内机器人定位与建图方法及装置
CN109917332A (zh) * 2019-02-01 2019-06-21 广东工业大学 一种基于改进粒子滤波的室内机器人定位方法
CN110045733A (zh) * 2019-04-04 2019-07-23 肖卫国 一种实时定位方法及其系统、计算机可读介质
CN110333720A (zh) * 2019-07-10 2019-10-15 国网四川省电力公司电力科学研究院 一种基于粒子滤波的slam优化方法
CN110598804A (zh) * 2019-10-14 2019-12-20 安徽理工大学 一种基于聚类和膜计算的改进FastSLAM算法
CN110702093A (zh) * 2019-09-27 2020-01-17 五邑大学 基于粒子滤波的定位方法、装置、存储介质及机器人
CN110763239A (zh) * 2019-11-14 2020-02-07 华南智能机器人创新研究院 滤波组合激光slam建图方法及装置
CN110779528A (zh) * 2019-11-07 2020-02-11 四川长虹电器股份有限公司 基于粒子滤波器的定位恢复方法及机器人设备
CN110909105A (zh) * 2019-11-25 2020-03-24 上海有个机器人有限公司 机器人地图的构建方法及系统
CN110941004A (zh) * 2019-12-04 2020-03-31 南京航空航天大学 一种基于gnss/uwb的移动机器人室内外联合定位方法及装置
CN110986956A (zh) * 2019-12-23 2020-04-10 苏州寻迹智行机器人技术有限公司 一种基于改进的蒙特卡洛算法的自主学习全局定位方法
CN111061287A (zh) * 2019-12-31 2020-04-24 芜湖哈特机器人产业技术研究院有限公司 基于粒子自收敛的移动机器人重定位方法
CN111090281A (zh) * 2019-11-27 2020-05-01 温州大学 一种基于改进粒子滤波算法估算移动机器人精确方位的方法和装置
CN111103801A (zh) * 2019-12-31 2020-05-05 芜湖哈特机器人产业技术研究院有限公司 基于遗传算法的移动机器人重定位方法及移动机器人
CN111189454A (zh) * 2020-01-09 2020-05-22 郑州轻工业大学 基于秩卡尔曼滤波的无人车slam导航方法
CN111198365A (zh) * 2020-01-16 2020-05-26 东方红卫星移动通信有限公司 一种基于射频信号的室内定位方法
CN111256699A (zh) * 2020-03-05 2020-06-09 安徽意欧斯物流机器人有限公司 一种基于粒子滤波器的agv激光定位方法
CN111578958A (zh) * 2020-05-19 2020-08-25 山东金惠新达智能制造科技有限公司 移动机器人导航实时定位方法、系统、介质及电子设备
CN111859773A (zh) * 2020-08-05 2020-10-30 哈尔滨工程大学 基于正则化粒子滤波的电动闸阀故障确定方法及系统
CN111954158A (zh) * 2020-07-01 2020-11-17 珠海高凌信息科技股份有限公司 基于rss地图的联合滤波室内单目标跟踪方法、装置及介质
CN112732854A (zh) * 2021-01-11 2021-04-30 哈尔滨工程大学 一种粒子滤波bslam方法
CN112771623A (zh) * 2018-08-31 2021-05-07 B·布莱恩·阿维图姆股份公司 用于医疗装置的自学习输入滤波器
CN112762928A (zh) * 2020-12-23 2021-05-07 重庆邮电大学 含有激光slam的odom与dm地标组合移动机器人及导航方法
CN113051529A (zh) * 2021-03-17 2021-06-29 哈尔滨工程大学 一种基于统计观测局地化均权重粒子滤波数据同化方法
CN113077055A (zh) * 2021-03-26 2021-07-06 北京邮电大学 一种基于FBPF-EIKF-FastSLAM的分布式多源融合定位方法
CN113375658A (zh) * 2021-06-15 2021-09-10 电子科技大学中山学院 移动机器人故障下同时fdd和slam的方法及系统
CN113434821A (zh) * 2021-07-07 2021-09-24 华北科技学院(中国煤矿安全技术培训中心) 一种分布式粒子m-h滤波方法及系统
CN115388893A (zh) * 2022-08-25 2022-11-25 西安电子科技大学芜湖研究院 基于遗传的滤波slam算法的移动机器人建图方法
CN116203586A (zh) * 2023-05-06 2023-06-02 南京农业大学 一种基于改进Gmapping的果园二维环境地图精准构建方法及系统
WO2023131048A1 (zh) * 2022-01-06 2023-07-13 上海安亭地平线智能交通技术有限公司 位姿信息的确定方法、装置、电子设备和存储介质
CN118168539A (zh) * 2024-04-29 2024-06-11 江西理工大学南昌校区 一种基于Gmapping算法的建图方法及系统

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103389094A (zh) * 2013-07-15 2013-11-13 哈尔滨工程大学 一种改进的粒子滤波方法
CN103487047A (zh) * 2013-08-06 2014-01-01 重庆邮电大学 一种基于改进粒子滤波的移动机器人定位方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103389094A (zh) * 2013-07-15 2013-11-13 哈尔滨工程大学 一种改进的粒子滤波方法
CN103487047A (zh) * 2013-08-06 2014-01-01 重庆邮电大学 一种基于改进粒子滤波的移动机器人定位方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
刘洞波: "移动机器人粒子滤波定位与地图创建方法研究", 《中国博士学位论文全文数据库•信息科技辑》 *
周旭: "基于改进粒子滤波的SLAM算法研究", 《中国优秀硕士学位论文全文数据库•信息科技辑》 *
宋越明: "基于粒子滤波的跟踪方法研究", 《中国博士学位论文全文数据库•信息科技辑》 *
杨宁等: "基于遗传算法的改进粒子滤波算法", 《上海交通大学学报》 *

Cited By (48)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109765569A (zh) * 2017-11-09 2019-05-17 电子科技大学中山学院 一种基于激光雷达实现虚拟航迹推算传感器的方法
CN108319572A (zh) * 2017-11-09 2018-07-24 电子科技大学中山学院 一种移动机器人故障诊断的混合自适应粒子滤波方法
CN112771623A (zh) * 2018-08-31 2021-05-07 B·布莱恩·阿维图姆股份公司 用于医疗装置的自学习输入滤波器
CN109556611A (zh) * 2018-11-30 2019-04-02 广州高新兴机器人有限公司 一种基于图优化和粒子滤波的融合定位方法
CN109798896A (zh) * 2019-01-21 2019-05-24 东南大学 一种室内机器人定位与建图方法及装置
CN109917332B (zh) * 2019-02-01 2022-12-16 广东工业大学 一种基于改进粒子滤波的室内机器人定位方法
CN109917332A (zh) * 2019-02-01 2019-06-21 广东工业大学 一种基于改进粒子滤波的室内机器人定位方法
CN110045733A (zh) * 2019-04-04 2019-07-23 肖卫国 一种实时定位方法及其系统、计算机可读介质
CN110045733B (zh) * 2019-04-04 2022-11-01 肖卫国 一种实时定位方法及其系统、计算机可读介质
CN110333720A (zh) * 2019-07-10 2019-10-15 国网四川省电力公司电力科学研究院 一种基于粒子滤波的slam优化方法
CN110702093A (zh) * 2019-09-27 2020-01-17 五邑大学 基于粒子滤波的定位方法、装置、存储介质及机器人
CN110598804A (zh) * 2019-10-14 2019-12-20 安徽理工大学 一种基于聚类和膜计算的改进FastSLAM算法
CN110598804B (zh) * 2019-10-14 2023-05-09 安徽理工大学 一种基于聚类和膜计算的改进FastSLAM方法
CN110779528A (zh) * 2019-11-07 2020-02-11 四川长虹电器股份有限公司 基于粒子滤波器的定位恢复方法及机器人设备
CN110763239A (zh) * 2019-11-14 2020-02-07 华南智能机器人创新研究院 滤波组合激光slam建图方法及装置
CN110909105B (zh) * 2019-11-25 2022-08-19 上海有个机器人有限公司 机器人地图的构建方法及系统
CN110909105A (zh) * 2019-11-25 2020-03-24 上海有个机器人有限公司 机器人地图的构建方法及系统
CN111090281A (zh) * 2019-11-27 2020-05-01 温州大学 一种基于改进粒子滤波算法估算移动机器人精确方位的方法和装置
CN110941004A (zh) * 2019-12-04 2020-03-31 南京航空航天大学 一种基于gnss/uwb的移动机器人室内外联合定位方法及装置
CN110986956A (zh) * 2019-12-23 2020-04-10 苏州寻迹智行机器人技术有限公司 一种基于改进的蒙特卡洛算法的自主学习全局定位方法
CN110986956B (zh) * 2019-12-23 2023-06-30 苏州寻迹智行机器人技术有限公司 一种基于改进的蒙特卡洛算法的自主学习全局定位方法
CN111103801A (zh) * 2019-12-31 2020-05-05 芜湖哈特机器人产业技术研究院有限公司 基于遗传算法的移动机器人重定位方法及移动机器人
CN111061287A (zh) * 2019-12-31 2020-04-24 芜湖哈特机器人产业技术研究院有限公司 基于粒子自收敛的移动机器人重定位方法
CN111103801B (zh) * 2019-12-31 2022-05-17 芜湖哈特机器人产业技术研究院有限公司 基于遗传算法的移动机器人重定位方法及移动机器人
CN111061287B (zh) * 2019-12-31 2022-05-27 芜湖哈特机器人产业技术研究院有限公司 基于粒子自收敛的移动机器人重定位方法
CN111189454A (zh) * 2020-01-09 2020-05-22 郑州轻工业大学 基于秩卡尔曼滤波的无人车slam导航方法
CN111198365A (zh) * 2020-01-16 2020-05-26 东方红卫星移动通信有限公司 一种基于射频信号的室内定位方法
CN111256699A (zh) * 2020-03-05 2020-06-09 安徽意欧斯物流机器人有限公司 一种基于粒子滤波器的agv激光定位方法
CN111578958A (zh) * 2020-05-19 2020-08-25 山东金惠新达智能制造科技有限公司 移动机器人导航实时定位方法、系统、介质及电子设备
CN111954158A (zh) * 2020-07-01 2020-11-17 珠海高凌信息科技股份有限公司 基于rss地图的联合滤波室内单目标跟踪方法、装置及介质
CN111859773A (zh) * 2020-08-05 2020-10-30 哈尔滨工程大学 基于正则化粒子滤波的电动闸阀故障确定方法及系统
CN111859773B (zh) * 2020-08-05 2023-07-18 哈尔滨工程大学 基于正则化粒子滤波的电动闸阀故障确定方法及系统
CN112762928A (zh) * 2020-12-23 2021-05-07 重庆邮电大学 含有激光slam的odom与dm地标组合移动机器人及导航方法
CN112762928B (zh) * 2020-12-23 2022-07-15 重庆邮电大学 含有激光slam的odom与dm地标组合移动机器人及导航方法
CN112732854A (zh) * 2021-01-11 2021-04-30 哈尔滨工程大学 一种粒子滤波bslam方法
CN112732854B (zh) * 2021-01-11 2023-03-31 哈尔滨工程大学 一种粒子滤波bslam方法
CN113051529A (zh) * 2021-03-17 2021-06-29 哈尔滨工程大学 一种基于统计观测局地化均权重粒子滤波数据同化方法
CN113051529B (zh) * 2021-03-17 2023-05-30 哈尔滨工程大学 一种基于统计观测局地化均权重粒子滤波数据同化方法
CN113077055A (zh) * 2021-03-26 2021-07-06 北京邮电大学 一种基于FBPF-EIKF-FastSLAM的分布式多源融合定位方法
CN113375658B (zh) * 2021-06-15 2023-05-09 电子科技大学中山学院 移动机器人故障下同时fdd和slam的方法及系统
CN113375658A (zh) * 2021-06-15 2021-09-10 电子科技大学中山学院 移动机器人故障下同时fdd和slam的方法及系统
CN113434821B (zh) * 2021-07-07 2024-05-14 华北科技学院(中国煤矿安全技术培训中心) 一种分布式粒子m-h滤波方法及系统
CN113434821A (zh) * 2021-07-07 2021-09-24 华北科技学院(中国煤矿安全技术培训中心) 一种分布式粒子m-h滤波方法及系统
WO2023131048A1 (zh) * 2022-01-06 2023-07-13 上海安亭地平线智能交通技术有限公司 位姿信息的确定方法、装置、电子设备和存储介质
CN115388893A (zh) * 2022-08-25 2022-11-25 西安电子科技大学芜湖研究院 基于遗传的滤波slam算法的移动机器人建图方法
CN115388893B (zh) * 2022-08-25 2024-05-14 西安电子科技大学芜湖研究院 基于遗传的滤波slam算法的移动机器人建图方法
CN116203586A (zh) * 2023-05-06 2023-06-02 南京农业大学 一种基于改进Gmapping的果园二维环境地图精准构建方法及系统
CN118168539A (zh) * 2024-04-29 2024-06-11 江西理工大学南昌校区 一种基于Gmapping算法的建图方法及系统

Similar Documents

Publication Publication Date Title
CN107246873A (zh) 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法
CN105957342B (zh) 基于众包时空大数据的车道级道路测图方法及系统
US8626443B2 (en) Method for creating a map relating to location-related data on the probability of future movement of a person
Ruchti et al. Localization on openstreetmap data using a 3d laser scanner
Zhang et al. Increasing GPS localization accuracy with reinforcement learning
CN106599368B (zh) 基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法
CN106683122A (zh) 一种基于高斯混合模型和变分贝叶斯的粒子滤波方法
EP2439492A1 (en) System and method for wavelet-based gait classification
JP2017520841A (ja) 目標の実時間測位と地図構築の方法及び装置
CN105467838B (zh) 一种随机有限集框架下的同步定位与地图构建方法
CN111060099B (zh) 一种无人驾驶汽车实时定位方法
CN106471338A (zh) 确定移动设备在地理区域中的位置
JP4984659B2 (ja) 自車両位置推定装置
Saulnier et al. Information theoretic active exploration in signed distance fields
CN112486197B (zh) 基于多源图像自适应选权的融合定位跟踪控制方法
CN108426582B (zh) 行人室内三维地图匹配方法
CN107490378A (zh) 一种基于mpu6050与智能手机的室内定位与导航的方法
CN112904388A (zh) 基于领航者策略的融合定位跟踪控制方法
Krauße et al. Robust multi-objective calibration strategies–possibilities for improving flood forecasting
Yan et al. A novel fastslam algorithm based on iterated unscented kalman filter
Godoy et al. Development of an particle swarm algorithm for vehicle localization
CN116170874A (zh) 一种鲁棒性WiFi指纹室内定位方法及系统
CN110245326B (zh) 基于神经网络的数据估算方法、设备、存储介质及装置
Huang et al. A directional particle filter-based multi-floor indoor positioning system
Luo Matlab-realized visual A* path planning teaching platform

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: 20171013

RJ01 Rejection of invention patent application after publication