CN109084777A - 基于自适应遗传算法的粒子滤波定位与地图构建方法 - Google Patents
基于自适应遗传算法的粒子滤波定位与地图构建方法 Download PDFInfo
- Publication number
- CN109084777A CN109084777A CN201811066110.2A CN201811066110A CN109084777A CN 109084777 A CN109084777 A CN 109084777A CN 201811066110 A CN201811066110 A CN 201811066110A CN 109084777 A CN109084777 A CN 109084777A
- Authority
- CN
- China
- Prior art keywords
- particle
- value
- map
- self
- moment
- 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
Links
- 239000002245 particle Substances 0.000 title claims abstract description 191
- 238000000034 method Methods 0.000 title claims abstract description 69
- 230000002068 genetic effect Effects 0.000 title claims abstract description 35
- 238000001914 filtration Methods 0.000 title claims abstract description 34
- 238000005457 optimization Methods 0.000 claims abstract description 17
- 238000004364 calculation method Methods 0.000 claims description 42
- 239000011159 matrix material Substances 0.000 claims description 18
- 238000005259 measurement Methods 0.000 claims description 11
- 230000035772 mutation Effects 0.000 claims description 8
- 230000003416 augmentation Effects 0.000 claims description 6
- 239000012141 concentrate Substances 0.000 claims description 6
- 230000033001 locomotion Effects 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 5
- 230000007613 environmental effect Effects 0.000 claims description 4
- HUTDUHSNJYTCAR-UHFFFAOYSA-N ancymidol Chemical compound C1=CC(OC)=CC=C1C(O)(C=1C=NC=NC=1)C1CC1 HUTDUHSNJYTCAR-UHFFFAOYSA-N 0.000 claims description 3
- 238000005315 distribution function Methods 0.000 claims description 3
- 238000005530 etching Methods 0.000 claims description 3
- 230000003716 rejuvenation Effects 0.000 claims description 3
- 230000009897 systematic effect Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims 1
- 238000002986 genetic algorithm method Methods 0.000 claims 1
- 238000012952 Resampling Methods 0.000 abstract description 17
- 230000004075 alteration Effects 0.000 description 5
- 239000000523 sample Substances 0.000 description 5
- 230000003044 adaptive effect Effects 0.000 description 3
- 230000006978 adaptation Effects 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000013456 study Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000003930 cognitive ability Effects 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
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)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
- Feedback Control In General (AREA)
Abstract
本发明公开了基于自适应遗传算法的粒子滤波定位与地图构建方法，包括初始化、判断机器人当前位置是否到达终止位置、状态估计、特征匹配、状态更新、路径估计、自适应遗传算法优化k时刻机器人路径分布。本发明用自适应遗传算法来优化表示路径后验分布的粒子集，去掉了一般粒子滤波SLAM方法中的重采样，解决了重采样带来的“粒子耗尽问题”，并使整个SLAM系统的运行效率得到提升。
Description
技术领域
本发明涉及一种基于粒子滤波的同时定位与地图构建方法，尤其是涉及基于自适应遗传算法的粒子滤波定位与地图构建方法。
背景技术
伴随着机器人技术的蓬勃发展，机器人对未知环境的自主认知能力已经成为机器人学中研究的一个热点。构建未知环境的地图，也就是移动机器人的定位与导航问题是机器人自主认知的其中一项重点研究内容和研究热点。其中，同时定位与地图构建(SLAM)则是移动机器人实现定位和导航的一种有效手段，就是机器人根据获取到的机器人相关数据，提取特征，经过特征匹配，最终自主构建机器人所处未知环境的地图并同时获取自身位置，对自身进行定位。进而，可以利用构建好的环境地图，继续进行其他的相关研究。
目前现有的常用于同时定位与地图构建的方法是用粒子滤波器估计机器人的运动路径，即用粒子集近似表示机器人路径的后验概率分布，粒子集的粒子权值方差会随时间是递增变化的。经过多次的迭代之后，少数的样本将会占去总体样本的绝大部分权重，除了这几个粒子外的其他粒子就具有很小的权值，这样一来样本就不能有效表达真是的后验概率密度。由此产生粒子滤波器中普遍存在的“粒子退化”问题，粒子的多样性受到影响。对路径的估计精度会下降。重采样是减弱“粒子退化”现象的一种常用方法，但是，重采样会引起“粒子耗尽”的问题。重采样的过程也会使整个SLAM方法的效率变低。
发明内容
本发明的目的在于基于自适应遗传算法的粒子滤波定位与地图构建方法，本发明用自适应遗传算法来优化表示路径后验分布的粒子集，去掉了一般粒子滤波SLAM方法中的重采样，解决了重采样带来的“粒子耗尽问题”，并使整个SLAM系统的运行效率得到提升。
本发明解决上述技术问题采用的技术方案是：基于自适应遗传算法的粒子滤波定位与地图构建方法，包括以下步骤：
步骤一：初始化：设置机器人运动的初始位置和终止位置；
步骤二：判断：机器人从初始位置向终止位置移动过程中，判断机器人当前位置是否到达终止位置，若到达终止位置，则机器人停止移动，移动过程中的位置判断包括机器人在初始位置和终止位置时进行判断；
步骤三：状态估计：若未到达终止位置，则采用无迹卡尔曼滤波算法计算k-1时刻各个粒子的位姿建议分布；
步骤三：特征匹配：获取k时刻机器人对环境特征的实际观测值，并通过数据关联方法将实际观测值与观测估计值进行特征匹配；
步骤四：状态更新：用无迹卡尔曼滤波算法计算k时刻各个粒子的位姿建议分布；
步骤五：路径估计：从粒子的k时刻位姿建议分布中进行采样得到粒子在k时刻的位姿向量，计算每个粒子的非正则权值，得到k时刻机器人路径分布；
步骤六：采用自适应遗传算法优化k时刻机器人路径分布，返回步骤二。
利用自适应遗传算法，使粒子集能够根据自身状态，自动调整变异率和交叉率，保护粒子的多样性，得到更好的优化粒子集，提高了SLAM问题中路径估计的精度，同时自适应遗传算法来优化表示路径后验分布的粒子集，去掉了一般粒子滤波SLAM方法中的重采样，解决了重采样带来的“粒子耗尽问题”，并使整个SLAM系统的运行效率得到提升。
作为优选，步骤六中还包括自适应遗传算法优化方法，自适应遗传算法优化方法包括以下步骤：
步骤A：对k时刻粒子集的各个粒子的适应度值和粒子的相似度值进行计算；
步骤B：依据步骤A中适应度值与相似度值计算粒子总体评分，并将各个粒子按照总体评分从大到小排名，计算公式为：
ci＝0.6ai+0.4bi，
ai为粒子适应度值，bi为粒子相似度值，ci为粒子总体评分；
步骤C：选取粒子总体评分高的N/2个粒子，加入到优化粒子集中；
步骤D：对k时刻粒子集进行交叉操作和变异操作分别获得粒子集xnew1和xnew2；
步骤E：将步骤D中获得的粒子集xnew1和xnew2按照步骤B进行总体评分，按照总体评分从大到小进行排序，选取N/2个总体评分高的粒子，加入到优化粒子集中，与步骤C中获得的N/2个粒子构成完整的优化粒子集。
自适应遗传算法来优化表示路径后验分布的粒子集，去掉了一般粒子滤波SLAM方法中的重采样，解决了重采样带来的“粒子耗尽问题”，并使整个SLAM系统的运行效率得到提升。
作为优选，步骤D中包括交叉操作方法，交叉操作方法包括以下步骤：
步骤1：从k时刻粒子集中随机选择n个粒子，将n个粒子的适应度值相加，取适应度平均值；
步骤2：再次重复步骤1，获得两个适应度平均值，对两个适应度平均值再取平均值，得到群体的适应度平均值，计算粒子集的交叉概率Pc，计算公式为：
其中，fmax为群体中个体最大的适应度值，favg为群体的适应度平均值，f'为要交叉的两个个体中最大的适应度值，f为变异个体的适应度值。设定k1取[0.5,1]中的值，Pc1取0.9，Pc2取[0.5,1]；
步骤3：从粒子集中随机生成粒子集再从优化粒子集中随机选取粒子集
步骤4：对随机生成粒子集和随机选取粒子集进行交叉操作，计算公式为：
其中λ∈[0,1]是均匀分布的随机数,和粒子集中的粒子共同构成的粒子集xnew1。
交叉操作方法能够自动调整交叉率，保护粒子的多样性，得到更好的优化粒子集，提高了SLAM问题中路径估计的精度。
作为优选，步骤D中包括变异操作方法，包括以下步骤：
步骤①：从k时刻粒子集中随机选择n个粒子，将n个粒子的适应度值相加，取适应度平均值；
步骤②：再次重复步骤1，获得两个适应度平均值，计算粒子集的交叉概率Pc，计算公式为：
其中，fmax为群体中个体最大的适应度值，favg为群体的适应度平均值，f'为要交叉的两个个体中最大的适应度值，f为变异个体的适应度值。设定k2取[0.5,1]中的值，Pm1＝0.1，Pm2取[0.045,1]；
步骤③：从粒子集中随机生成粒子集对随机生成的粒子集做变异操作，计算公式为：
其中，γ为服从高斯分布的随机数，σ2为协方差。
变异操作方法能够自动调整变异率，保护粒子的多样性，得到更好的优化粒子集，提高了SLAM问题中路径估计的精度。
作为优选，步骤五还包括地图更新方法，包括以下步骤：
步骤A1：用无迹卡尔曼滤波算法计算分布函数，第i个粒子的地图计算表示为：其中和分别表示第i个粒子地图里的第j个特征的均值和方差；
步骤A2：对于没有关联的地图特征，保持其均值和方差不变，对于新的地图特征，将新的地图特征加入到地图中，各个关联地图特征的均值和方差计算公式为：
Kk＝λj,k-1Pzz[(Pzz)Tλj,k-1Pzz+R]，
λj,k＝λj,k-1-Kk(Pzz)Tλj,k-1，
其中，Kk为观测卡尔曼增益，R为测量噪音。
步骤A3：地图更新后，返回步骤二。
地图更新方法解决了机器人在未知环境下，自动地图构建的问题。
作为优选，状态估计方法包括以下步骤：
步骤B1：k-1时刻状态向量的均值为方差为Pk-1，对状态向量进行增广得到向量和方差矩阵，
其中，Qk-1表示k-1时刻系统控制噪声的方差矩阵，Rk-1表示k-1时刻系统观测噪声的方差矩阵；
步骤B2：在步骤B1得到的向量中加入控制量和观测量，计算Sigma点，计算公式为：
其中，表示取均方根矩阵的第i列，λ的取值范围由公式λ＝β2(nx+κ)-n决定，nx和n分别为状态向量增广前后的维数，β为尺度参数，κ取0。
步骤B3：计算时间更新，计算公式为：
状态估计值：
状态估计均值：
状态估计方差：
观测量估计值：
观测量估计均值：
其中，和分别为均值权值和方差权值，取值公式为：
状态估计方法解决了机器人在k-1时刻的定位与观测。
作为优选，步骤三中的数据关联方法包括以下步骤：
步骤C1：k时刻机器人获得m个环境的实际观测值
步骤C2：计算实际观测值和步骤B3得到的观测量估计值的滤波残差，计算公式为：
其中，观测量估计值为n为地图特征总数目；
步骤C3：通过步骤C2得到的滤波残差计算马氏距离，计算公式为：
其中，为的协方差矩阵；
步骤C4：依据最近邻滤波选择最佳特征对，计算公式为：
dik＝min(dij)，j＝1，2，...，n；k∈(1，2，...，n)；
步骤C5：采用椭圆门限进行检验特征的一致性，对于满足检验公式的特征，认为落入椭圆门限内，实际观测值和观测量估计值具有一致性，未落入椭圆门限的特征，认为是系统测量的新特征，检验公式为：
其中，观测向量位数为2，自由度d＝2,1-α为置信度。
数据关联方法解决了实际观测与观测量估计的特征匹配问题，使得机器人能够实现状态更新。
作为优选，状态更新包括测量更新和观测更新。
测量更新和观测更新能够实现机器人对k时刻的状态更新。
作为优选，测量更新计算公式为：
作为优选，观测更新计算公式为：
卡尔曼增益：K＝Pxz(Pzz)-1；
滤波值：
滤波方差矩阵：Pk＝Pk,k-1-KPzzKT。
k时刻粒子的位姿建议分布表示为
本发明具有的有益效果是：
1、利用自适应遗传算法，使粒子集能够根据自身状态，自动调整变异率和交叉率，保护粒子的多样性，得到更好的优化粒子集，提高了SLAM问题中路径估计的精度。
2、用自适应遗传算法来优化表示路径后验分布的粒子集，去掉了一般粒子滤波SLAM方法中的重采样，解决了重采样带来的“粒子耗尽问题”，并使整个SLAM系统的运行效率得到提升。
附图说明
图1是本发明的流程图。
具体实施方式
以下结合附图和实施方式对本发明作进一步的说明。
如图1所示，本发明基于自适应遗传算法的粒子滤波定位与地图构建方法，包括以下步骤：
步骤一：初始化：设置机器人运动的初始位置和终止位置；
步骤二：判断：机器人从初始位置向终止位置移动过程中，判断机器人当前位置是否到达终止位置，若到达终止位置，则机器人停止移动；
步骤三：状态估计：若未到达终止位置，则用无迹卡尔曼滤波算法计算k-1时刻各个粒子的位姿建议分布；
步骤三：特征匹配：获取k时刻机器人对环境特征的实际观测值，并用数据关联方法将实际观测值与观测估计值进行特征匹配；
步骤四：状态更新：用无迹卡尔曼滤波算法计算k时刻各个粒子的位姿建议分布；
步骤五：路径估计：从粒子的k时刻位姿建议分布中进行采样得到粒子在k时刻的位姿向量，计算每个粒子的非正则权值，得到k时刻机器人路径分布；
步骤六：采用自适应遗传算法优化k时刻机器人路径分布，返回步骤二。
利用自适应遗传算法，使粒子集能够根据自身状态，自动调整变异率和交叉率，保护粒子的多样性，得到更好的优化粒子集，提高了SLAM问题中路径估计的精度，同时自适应遗传算法来优化表示路径后验分布的粒子集，去掉了一般粒子滤波SLAM方法中的重采样，解决了重采样带来的“粒子耗尽问题”，并使整个SLAM系统的运行效率得到提升。
步骤六中还包括自适应遗传算法优化方法，包括以下步骤：
步骤A：对k时刻粒子集的各个粒子的适应度值和粒子的相似度值进行计算；
步骤B：依据步骤A中计算的粒子适应度值与相似度值计算粒子总体评分，并将各个粒子按照分值从大到小排名，计算公式为：
ci＝0.6ai+0.4bi，
ai为粒子适应度值，bi为粒子相似度值，ci为粒子总体评分；
步骤C：选取粒子总体评分高的N/2个粒子，加入到优化粒子集中；
步骤D：对k时刻粒子集进行交叉操作和变异操作获得粒子集xnew1和xnew2；
步骤E：将步骤D中获得的粒子集xnew1和xnew2按照步骤B进行总体评分，按照总体评分从大到小进行排序，选取N/2个总体评分高的粒子，加入到优化粒子集中，与步骤C中获得的N/2个粒子构成完整的优化粒子集。
自适应遗传算法来优化表示路径后验分布的粒子集，去掉了一般粒子滤波SLAM方法中的重采样，解决了重采样带来的“粒子耗尽问题”，并使整个SLAM系统的运行效率得到提升。
步骤D中包括交叉操作方法，包括以下步骤：
步骤1：从k时刻粒子集中随机选择n个粒子，将n个粒子的适应度值相加，取适应度平均值；
步骤2：再次重复步骤1，获得两个适应度平均值，对两个适应度平均值再取平均值，得到群体的适应度平均值，计算粒子集的交叉概率Pc，计算公式为：
其中，fmax为群体中个体最大的适应度值，favg为群体的适应度平均值，f'为要交叉的两个个体中最大的适应度值，f为变异个体的适应度值。设定k1取[0.5,1]中的值，Pc1取0.9，Pc2取[0.5,1]；
步骤3：从粒子集中随机生成粒子集再从优化粒子集中随机选取粒子集
步骤4：对随机生成粒子集和随机选取粒子集进行交叉操作，计算公式为：
其中λ∈[0,1]是均匀分布的随机数,和粒子集中的粒子共同构成的粒子集xnew1。
交叉操作方法能够自动调整交叉率，保护粒子的多样性，得到更好的优化粒子集，提高了SLAM问题中路径估计的精度。
步骤D中包括变异操作方法，包括以下步骤：
步骤①：从k时刻粒子集中随机选择n个粒子，将n个粒子的适应度值相加，取适应度平均值；
步骤②：再次重复步骤①，获得两个适应度平均值，计算粒子集的交叉概率Pc，计算公式为：
其中，fmax为群体中个体最大的适应度值，favg为群体的适应度平均值，f'为要交叉的两个个体中最大的适应度值，f为变异个体的适应度值。设定k2取[0.5,1]中的值，Pm1＝0.1，Pm2取[0.045,1]；
步骤③：从粒子集中随机生成粒子集对随机生成的粒子集做变异操作，计算公式为：
其中，γ为服从高斯分布的随机数，σ2为协方差。
变异操作方法能够自动调整变异率，保护粒子的多样性，得到更好的优化粒子集，提高了SLAM问题中路径估计的精度。
步骤五还包括地图更新方法，包括以下步骤：
步骤A1：用无迹卡尔曼滤波算法计算分布函数，第i个粒子的地图计算表示为：其中和分别表示第i个粒子地图里的第j个特征的均值和方差；
步骤A2：对于没有关联的地图特征，保持其均值和方差不变，对于新的地图特征，将新的地图特征加入到地图中，各个关联地图特征的均值和方差计算公式为：
Kk＝λj,k-1Pzz[(Pzz)Tλj,k-1Pzz+R]，
λj,k＝λj,k-1-Kk(Pzz)Tλj,k-1，
其中，Kk为观测卡尔曼增益，R为测量噪音。
步骤A3：地图更新后，返回步骤二。
地图更新方法解决了机器人在未知环境下，自动地图构建的问题。
状态估计方法包括以下步骤：
步骤B1：k-1时刻状态向量的均值为方差为Pk-1，对状态向量进行增广得到向量和方差矩阵，
其中，Qk-1表示k-1时刻系统控制噪声的方差矩阵，Rk-1表示k-1时刻系统观测噪声的方差矩阵；
步骤B2：在步骤B1得到的向量中加入控制量和观测量，计算Sigma点，计算公式为：
其中，表示取均方根矩阵的第i列，λ的取值范围由公式λ＝β2(nx+κ)-n决定，nx和n分别为状态向量增广前后的维数，β为尺度参数，κ取0。
步骤B3：计算时间更新，计算公式为：
状态估计值：
状态估计均值：
状态估计方差：
观测量估计值：
观测量估计均值：
其中，和分别为均值权值和方差权值，取值公式为：
状态估计方法解决了机器人在k-1时刻的定位与观测。
步骤三中的数据关联方法包括以下步骤：
步骤C1：k时刻机器人获得m个环境的实际观测值
步骤C2：计算实际观测值和步骤B3得到的观测量估计值的滤波残差，计算公式为：
其中，观测量估计值为n为地图特征总数目；
步骤C3：使用步骤C2得到的滤波残差计算马氏距离，计算公式为：
其中，为的协方差矩阵；
步骤C4：依据最近邻滤波选择最佳特征对，计算公式为：
dik＝min(dij)，j＝1，2，...，n；k∈(1，2，...，n)；
步骤C5：采用椭圆门限进行检验特征的一致性，对于满足检验公式的特征，认为落入椭圆们限内，实际观测值和观测量估计值具有一致性；未落入椭圆门限的特征，认为是系统测量的新特征，检验公式为：
其中，观测向量位数为2，自由度d＝2,1-α为置信度。
数据关联方法解决了实际观测与观测量估计的特征匹配问题，使得机器人能够实现状态更新。
状态更新包括测量更新和观测更新。
测量更新和观测更新能够实现机器人对k时刻的状态更新。
测量更新计算公式为：
观测更新计算公式为：
卡尔曼增益：K＝Pxz(Pzz)-1；
滤波值：
滤波方差矩阵：Pk＝Pk,k-1-KPzzKT。
k时刻粒子的位姿建议分布表示为
步骤五路径估计中从粒子的后验位姿建议分布中进行采样得到粒子在k时刻的位姿向量计算每个粒子的非正则权值，计算公式为：
对权值进行归一化处理，得到k时刻粒子集表示机器人路径后验分布p(x1:k|z1:k-1,u0:k,x0)，计算公式为：
说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
Claims (10)
1.基于自适应遗传算法的粒子滤波定位与地图构建方法，其特征在于：包括以下步骤：
步骤一：初始化：设置机器人运动的初始位置和终止位置；
步骤二：位置判断：机器人从初始位置向终止位置移动过程中，判断机器人当前位置是否到达终止位置，若到达终止位置，则机器人停止移动；
步骤三：状态估计：若未到达终止位置，则采用无迹卡尔曼滤波算法计算k-1时刻各个粒子的位姿建议分布；
步骤三：特征匹配：获取k时刻机器人对环境特征的实际观测值，并通过数据关联方法将实际观测值与观测估计值进行特征匹配；
步骤四：状态更新：用无迹卡尔曼滤波算法计算k时刻各个粒子的位姿建议分布；
步骤五：路径估计：从粒子的k时刻位姿建议分布中进行采样得到粒子在k时刻的位姿向量，计算每个粒子的非正则权值，得到k时刻机器人路径分布；
步骤六：采用自适应遗传算法优化k时刻机器人路径分布，返回步骤二。
2.根据权利要求1所述的基于自适应遗传算法的粒子滤波定位与地图构建方法，其特征在于：步骤六中还包括自适应遗传算法优化方法，自适应遗传算法优化方法包括以下步骤：
步骤A：对k时刻粒子集的各个粒子的适应度值和粒子的相似度值进行计算；
步骤B：依据步骤A中的适应度值与相似度值计算粒子总体评分，并将各个粒子按照总体评分从大到小排名，计算公式为：
ci＝0.6ai+0.4bi，
ai为粒子适应度值，bi为粒子相似度值，ci为粒子总体评分；
步骤C：选取粒子总体评分高的N/2个粒子，加入到优化粒子集中；
步骤D：对k时刻粒子集进行交叉操作和变异操作分别获得粒子集xnew1和xnew2；
步骤E：将步骤D中获得的粒子集xnew1和xnew2按照步骤B进行总体评分，按照总体评分从大到小进行排序，选取N/2个总体评分高的粒子，加入到优化粒子集中，与步骤C中获得的N/2个粒子构成完整的优化粒子集。
3.根据权利要求2所述的基于自适应遗传算法的粒子滤波定位与地图构建方法，其特征在于：步骤D中包括交叉操作方法，交叉操作方法包括以下步骤：
步骤1：从k时刻粒子集中随机选择n个粒子，将n个粒子的适应度值相加，取适应度平均值；
步骤2：再次重复步骤1，获得两个适应度平均值，对两个适应度平均值再取平均值，得到群体的适应度平均值，计算粒子集的交叉概率Pc，计算公式为：
其中，fmax为群体中个体最大的适应度值，favg为群体的适应度平均值，f'为要交叉的两个个体中最大的适应度值，f为变异个体的适应度值，设定k1取[0.5,1]中的值，Pc1取0.9，Pc2取[0.5,1]；
步骤3：从粒子集中随机生成粒子集再从优化粒子集中随机选取粒子集
步骤4：对随机生成粒子集和随机选取粒子集进行交叉操作，计算公式为：
其中λ∈[0,1]是均匀分布的随机数,和粒子集中的粒子共同构成的粒子集xnew1。
4.根据根据权利要求2或3所述的基于自适应遗传算法的粒子滤波定位与地图构建方法，其特征在于：步骤D中包括变异操作方法，变异操作方法包括以下步骤：
步骤①：从k时刻粒子集中随机选择n个粒子，将n个粒子的适应度值相加，取适应度平均值；
步骤②：再次重复步骤①，获得两个适应度平均值，计算粒子集的交叉概率Pc，计算公式为：
其中，fmax为群体中个体最大的适应度值，favg为群体的适应度平均值，f'为要交叉的两个个体中最大的适应度值，f为变异个体的适应度值，设定k2取[0.5,1]中的值，Pm1＝0.1，Pm2取[0.045,1]；
步骤③：从粒子集中随机生成粒子集对随机生成的粒子集做变异操作，计算公式为：
其中，γ为服从高斯分布的随机数，σ2为协方差。
5.根据根据权利要求1或2或3所述的基于自适应遗传算法的粒子滤波定位与地图构建方法，其特征在于：步骤五还包括地图更新方法，地图更新方法包括以下步骤：
步骤A1：采用无迹卡尔曼滤波算法计算分布函数，第i个粒子的地图计算表示为：其中和分别表示第i个粒子地图里的第j个特征的均值和方差；
步骤A2：对于没有关联的地图特征，保持其均值和方差不变，对于新的地图特征，将新的地图特征加入到地图中，各个关联地图特征的均值和方差计算公式为：
Kk＝λj,k-1Pzz[(Pzz)Tλj,k-1Pzz+R]，
λj,k＝λj,k-1-Kk(Pzz)Tλj,k-1，
其中，Kk为观测卡尔曼增益，R为测量噪音；
步骤A3：地图更新后，返回步骤二。
6.根据根据权利要求5所述的基于自适应遗传算法的粒子滤波定位与地图构建方法，其特征在于：状态估计方法包括以下步骤：
步骤B1：k-1时刻状态向量的均值为方差为Pk-1，对状态向量进行增广得到向量和方差矩阵，
其中，Qk-1表示k-1时刻系统控制噪声的方差矩阵，Rk-1表示k-1时刻系统观测噪声的方差矩阵；
步骤B2：在步骤B1得到的向量中加入控制量和观测量，计算Sigma点，计算公式为：
其中，表示取均方根矩阵的第i列，λ的取值范围由公式λ＝β2(nx+κ)-n决定，nx和n分别为状态向量增广前后的维数，β为尺度参数，κ取0；
步骤B3：计算时间更新，计算公式为：
状态估计值：
状态估计均值：
状态估计方差：
观测量估计值：
观测量估计均值：
其中，和分别为均值权值和方差权值。
7.根据根据权利要求1或2或3或6所述的基于自适应遗传算法的粒子滤波定位与地图构建方法，其特征在于：步骤三中的数据关联方法包括以下步骤：
步骤C1：k时刻机器人获得m个环境的实际观测值
步骤C2：计算实际观测值和步骤B3得到的观测量估计值的滤波残差，计算公式为：
其中，观测量估计值为n为地图特征总数目；
步骤C3：通过步骤C2得到的滤波残差计算马氏距离，计算公式为：
其中，为的协方差矩阵；
步骤C4：依据最近邻滤波选择最佳特征对，计算公式为：
dik＝min(dij)，j＝1，2，...，n；k∈(1，2，...，n)；
步骤C5：采用椭圆门限进行检验特征的一致性，对于满足检验公式的特征，认为落入椭圆门限内，实际观测值和观测量估计值具有一致性；未落入椭圆门限的特征，认为是系统测量的新特征，检验公式为：
其中，观测向量位数为2，自由度d＝2,1-α为置信度。
8.根据根据权利要求7所述的基于自适应遗传算法的粒子滤波定位与地图构建方法，其特征在于：状态更新包括测量更新和观测更新。
9.根据根据权利要求8所述的基于自适应遗传算法的粒子滤波定位与地图构建方法，其特征在于：测量更新计算公式为：
10.根据根据权利要求9所述的基于自适应遗传算法的粒子滤波定位与地图构建方法，其特征在于：观测更新计算公式为：
卡尔曼增益：K＝Pxz(Pzz)-1；
滤波值：
滤波方差矩阵：Pk＝Pk,k-1-KPzzKT。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811066110.2A CN109084777A (zh) | 2018-09-13 | 2018-09-13 | 基于自适应遗传算法的粒子滤波定位与地图构建方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811066110.2A CN109084777A (zh) | 2018-09-13 | 2018-09-13 | 基于自适应遗传算法的粒子滤波定位与地图构建方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN109084777A true CN109084777A (zh) | 2018-12-25 |
Family
ID=64841535
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811066110.2A Pending CN109084777A (zh) | 2018-09-13 | 2018-09-13 | 基于自适应遗传算法的粒子滤波定位与地图构建方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109084777A (zh) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109798896A (zh) * | 2019-01-21 | 2019-05-24 | 东南大学 | 一种室内机器人定位与建图方法及装置 |
| CN110726415A (zh) * | 2019-10-21 | 2020-01-24 | 哈尔滨工程大学 | 一种自适应的水下多波束同步定位与建图方法 |
| CN110941004A (zh) * | 2019-12-04 | 2020-03-31 | 南京航空航天大学 | 一种基于gnss/uwb的移动机器人室内外联合定位方法及装置 |
| CN111103801A (zh) * | 2019-12-31 | 2020-05-05 | 芜湖哈特机器人产业技术研究院有限公司 | 基于遗传算法的移动机器人重定位方法及移动机器人 |
| CN112732854A (zh) * | 2021-01-11 | 2021-04-30 | 哈尔滨工程大学 | 一种粒子滤波bslam方法 |
| CN114543793A (zh) * | 2020-11-24 | 2022-05-27 | 上海汽车集团股份有限公司 | 车辆导航系统的多传感器融合定位方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101807900A (zh) * | 2010-03-10 | 2010-08-18 | 北京航空航天大学 | 基于并行遗传重采样的粒子滤波技术 |
| CN102103815A (zh) * | 2009-12-17 | 2011-06-22 | 上海电机学院 | 移动机器人的粒子定位方法及其装置 |
| US20130132454A1 (en) * | 2011-11-21 | 2013-05-23 | The Board Of Trustees Of The University Of Illinois | Feedback-Based Particle Filtering |
| CN107270889A (zh) * | 2017-06-08 | 2017-10-20 | 东南大学 | 一种基于地磁图谱的室内定位方法及定位系统 |
| CN107609594A (zh) * | 2017-09-18 | 2018-01-19 | 湖州师范学院 | 基于自适应遗传方法的显著性检测方法 |
-
2018
- 2018-09-13 CN CN201811066110.2A patent/CN109084777A/zh active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102103815A (zh) * | 2009-12-17 | 2011-06-22 | 上海电机学院 | 移动机器人的粒子定位方法及其装置 |
| CN101807900A (zh) * | 2010-03-10 | 2010-08-18 | 北京航空航天大学 | 基于并行遗传重采样的粒子滤波技术 |
| US20130132454A1 (en) * | 2011-11-21 | 2013-05-23 | The Board Of Trustees Of The University Of Illinois | Feedback-Based Particle Filtering |
| CN107270889A (zh) * | 2017-06-08 | 2017-10-20 | 东南大学 | 一种基于地磁图谱的室内定位方法及定位系统 |
| CN107609594A (zh) * | 2017-09-18 | 2018-01-19 | 湖州师范学院 | 基于自适应遗传方法的显著性检测方法 |
Non-Patent Citations (4)
| Title |
|---|
| 吕太之 等: ""一种基于SR-UKF 的FastSLAM 算法"", 《计算机应用研究》 * |
| 周武等: "一种基于遗传算法的FastSLAM 2．0算法", 《机器人》 * |
| 张路金: ""移动机器人同时定位与地图创建研究"", 《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》 * |
| 王浩等: "融合遗传优化的粒子滤波器算法", 《计算机科学与探索》 * |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109798896A (zh) * | 2019-01-21 | 2019-05-24 | 东南大学 | 一种室内机器人定位与建图方法及装置 |
| CN110726415A (zh) * | 2019-10-21 | 2020-01-24 | 哈尔滨工程大学 | 一种自适应的水下多波束同步定位与建图方法 |
| CN110726415B (zh) * | 2019-10-21 | 2023-09-29 | 哈尔滨工程大学 | 一种自适应的水下多波束同步定位与建图方法 |
| CN110941004A (zh) * | 2019-12-04 | 2020-03-31 | 南京航空航天大学 | 一种基于gnss/uwb的移动机器人室内外联合定位方法及装置 |
| CN111103801A (zh) * | 2019-12-31 | 2020-05-05 | 芜湖哈特机器人产业技术研究院有限公司 | 基于遗传算法的移动机器人重定位方法及移动机器人 |
| CN111103801B (zh) * | 2019-12-31 | 2022-05-17 | 芜湖哈特机器人产业技术研究院有限公司 | 基于遗传算法的移动机器人重定位方法及移动机器人 |
| CN114543793A (zh) * | 2020-11-24 | 2022-05-27 | 上海汽车集团股份有限公司 | 车辆导航系统的多传感器融合定位方法 |
| CN114543793B (zh) * | 2020-11-24 | 2024-02-09 | 上海汽车集团股份有限公司 | 车辆导航系统的多传感器融合定位方法 |
| CN112732854A (zh) * | 2021-01-11 | 2021-04-30 | 哈尔滨工程大学 | 一种粒子滤波bslam方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109084777A (zh) | 基于自适应遗传算法的粒子滤波定位与地图构建方法 | |
| CN107247961B (zh) | 一种应用模糊轨迹序列的轨迹预测方法 | |
| CN107087256A (zh) | 一种基于WiFi室内定位的指纹聚类方法及装置 | |
| Örkcü et al. | Estimating the parameters of 3-p Weibull distribution using particle swarm optimization: A comprehensive experimental comparison | |
| CN105554873B (zh) | 一种基于pso-ga-rbf-hop的无线传感器网络定位算法 | |
| CN112135248B (zh) | 一种基于K-means最优估计的WIFI指纹定位方法 | |
| CN107084714A (zh) | 一种基于RoboCup3D的多机器人协作目标定位方法 | |
| Lei et al. | Improved artificial bee colony algorithm and its application in data clustering | |
| CN105930688A (zh) | 基于改进pso算法的蛋白质功能模块检测方法 | |
| Özsoy et al. | Estimating the parameters of nonlinear regression models through particle swarm optimization | |
| CN109961129B (zh) | 一种基于改进粒子群的海上静止目标搜寻方案生成方法 | |
| Abdou et al. | An efficient indoor localization system based on Affinity Propagation and Support Vector Regression | |
| CN108955689A (zh) | 基于自适应细菌觅食优化算法的rbpf-slam方法 | |
| CN109121081B (zh) | 一种基于位置候选集与em算法的室内定位方法 | |
| Panda et al. | Reduction of linear time-invariant systems using Routh-approximation and PSO | |
| CN106028450A (zh) | 室内定位方法及设备 | |
| CN108280548A (zh) | 基于网络传输的智能处理方法 | |
| Moreno et al. | Differential Evolution Markov Chain filter for global localization | |
| Messaoudi et al. | Community detection using fireworks optimization algorithm | |
| CN115273237B (zh) | 基于集成随机配置神经网络的人体姿态与动作识别方法 | |
| CN109803234A (zh) | 基于权值重要度约束的无监督融合定位方法 | |
| CN108182273A (zh) | 基于云存储的网络数据处理方法 | |
| CN115226027A (zh) | 一种WiFi室内指纹定位方法及装置 | |
| CN115034159A (zh) | 一种海上风电场的功率预测方法、装置、存储介质及系统 | |
| Zhao et al. | Particle swarm algorithm for classification rules generation |
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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20181225 |