CN114061584B - 一种基于多机器人的势均衡多伯努利滤波slam方法 - Google Patents
一种基于多机器人的势均衡多伯努利滤波slam方法 Download PDFInfo
- Publication number
- CN114061584B CN114061584B CN202111288777.9A CN202111288777A CN114061584B CN 114061584 B CN114061584 B CN 114061584B CN 202111288777 A CN202111288777 A CN 202111288777A CN 114061584 B CN114061584 B CN 114061584B
- Authority
- CN
- China
- Prior art keywords
- robot
- moment
- bernoulli
- gaussian
- pose
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 238000001914 filtration Methods 0.000 title claims abstract description 21
- 239000002245 particle Substances 0.000 claims abstract description 46
- 238000013138 pruning Methods 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 13
- 239000000203 mixture Substances 0.000 claims description 10
- 238000005070 sampling Methods 0.000 claims description 7
- 238000001514 detection method Methods 0.000 claims description 6
- 230000004927 fusion Effects 0.000 claims description 4
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 230000008569 process Effects 0.000 description 8
- 230000033001 locomotion Effects 0.000 description 5
- 238000005259 measurement Methods 0.000 description 3
- 239000013598 vector Substances 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000007500 overflow downdraw method Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000004083 survival effect Effects 0.000 description 2
- 238000012935 Averaging Methods 0.000 description 1
- 238000012952 Resampling Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000007704 transition Effects 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
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
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
本发明公开基于多机器人的势均衡多伯努利滤波SLAM方法,包括(1)建立观测和地图特征多伯努利RFS模型;(2)SLAM问题转换为独立机器人位姿状态估计和地图特征状态估计;(3)获得k时刻机器人位姿状态估计预测值;(4)用势均衡策略获得k时刻地图特征状态估计;(5)修剪合并更新后高斯项;(6)用高斯项修正地图特征状态估计;(7)将高斯项融合到每一个机器人观测集合中带入k+1时刻机器人先验信息集合;(8)用(4)中滤波器对机器人每个粒子地图估计调整权重,用粒子加权平均更新k时刻位姿状态估计预测值;(9)k=k+1执行步骤(3)。本发明改善SLAM中特征数量过估计问题,提高多机器人SLAM精度。
Description
技术领域
本发明涉及一种SLAM方法,尤其涉及一种基于多机器人的势均衡多伯努利滤波SLAM方法。
背景技术
导航技术对于移动机器人在未知环境中工作必不可少,而同步定位与地图构建(SLAM)是近年来的主要导航技术。导航算法精度、速度、实时性和稳定性的不断提高一直是研究的重点。在一些复杂的环境中,如水下勘探环境、室内消防救援环境等,由于杂波密集,传统SLAM算法的数据关联精度会降低,计算量会大大增加,导致传统SLAM方法的准确性下降。为了解决复杂的数据关联问题,一种方法是避免数据关联问题,另一种方法是使用更有效的算法,比如基于图的SLAM。
基于随机有限集(RFS)的多目标贝叶斯滤波器通过对测量更新中所有可能的关联进行平均来避免数据关联的问题。RFS是一个设置值的随机变量,即状态向量的个数和状态向量本身都是随机的,所以RFS自然获得了地图的不确定性。在随后的研究中,将RFS应用到SLAM领域,提出了假设概率密度滤波器-同时定位与建图(PHD-SLAM)算法。PHD-SLAM算法在线性条件下通过高斯混合实现,在非线性条件下通过顺序蒙特卡罗实现,但这两种方法都使用近似策略计算粒子权重。虽然避免了数据关联,但其算法估计误差较大。单机器人多伯努利滤波器SLAM方法存在对地图特征数目过估计的问题。
多机器人联合估计策略,目前只有基于PHD-SLAM的多机器人联合估计方法,即MR-PHD-SLAM,但是由于PHD-SLAM信噪比较低以及在强非线性情况下会存在精度不高的问题。并且MR-PHD-SLAM采用特征级的信息融合,虽然可以提升单机器人SLAM精度,但是对于复杂环境来说,精度仍然不够,且实时性不好。
发明内容
发明目的:本发明旨在提供一种基于多机器人的势均衡多伯努利滤波SLAM方法,解决多机器人的SLAM精度低,实时性不好的问题。
技术方案:本发明的基于多机器人的势均衡多伯努利滤波SLAM方法,包括以下步骤:
(1)建立观测的多伯努利随机有限集RFS模型和地图特征的多伯努利随机有限集RFS模型;
(2)将SLAM问题转换为独立机器人位姿状态估计和地图特征状态估计;
(3)采用滤波器获得k时刻机器人位姿状态估计预测值;
(4)采用势均衡策略获得k时刻地图特征状态估计,即获得多伯努利参数存在概率参数r和概率密度参数p估计值;
(5)修剪和合并更新后的高斯项;
(6)根据高斯项修正地图特征的状态估计;
(7)将步骤(5)中各个机器人处理完的高斯项信息融合到每一个机器人的观测集合中,带入k+1时刻的机器人的先验信息集合中进行k+1时刻的处理过程;
(8)根据步骤(4)获得的势均衡多伯努利滤波器对每个机器人所对应的每个粒子地图估计结果调整粒子权重,然后通过采样粒子的加权平均,更新机器人k时刻位姿状态估计预测值;
(9)令k=k+1,执行步骤(3)。
所述观测的多伯努利随机有限集RFS模型:
其中,集合表示k时刻第n个机器人的特征观测集合,/>为机器人位姿,/>表示机器人在m处对真实地图特征的观测,/>表示第n个机器人在其位姿为/>观测到的杂波RFS;
所述地图特征的多伯努利随机有限集RFS模型:
其中,表示第n个机器人平台0到k-1时刻地图特征的RFS,M表示整个地图特征的RFS;/>表示第n个机器人第i个粒子在k时刻的机器人的位姿,/>表示k时刻第n个机器人第i个粒子新生地图特征;
新生地图的多伯努利随机有限集RFS模型为:
其中,表示k时刻新生地图特征多伯努利RFS,/>表示k时刻新生地图特征多伯努利RFS表现形式下的存在概率参数r,/>表示k时刻新生地图特征多伯努利RFS表现形式下的概率密度参数/>表示k时刻新生地图特征多伯努利项的项数,/>表示所有机器人平台所滤波得到的高斯项信息中靠近各自自身位姿的高斯项信息。
步骤(2)中分为机器人位姿的估计和在机器人位姿已知情况下对地图特征的估计,分解后联合后验估计表示为
πk(Mk,X1:k|Z0:k,u0:k-1,X0:k)=πk(X1:k|Z0:k,u0:k-1,X0)πk(Mk|Z0:k,X0:k) (7)
其中,πk(X1:k|Z0:k,u0:k-1,X0)表示机器人在k时刻观测量、k-1时刻的控制量和初始位姿量下的联合后验估计;πk(Mk|Z0:k,X0:k)表示机器人在位姿为X0:k时进行观测量Z0:k和位姿为X0:k条件下的地图特征联合后验估计。
步骤(3)中所述滤波器为Rao-Blackwellised粒子滤波器。
步骤(4)包括以下步骤:
(41)在k时刻,第n个机器人的第i个粒子所观测到的新生地图特征的多伯努利概率密度参数集合为则新生地图特征为:
其中,表示所有机器人平台所滤波得到的高斯项信息中靠近各自自身位姿的高斯项信息;
(42)第n个机器人的位姿为时,其高斯混合项表达形式:
其中,表示k时刻新生地图特征的多伯努利项所对应的高斯项的个数,/>表示k时刻新生地图特征的多伯努利项所对应其高斯项的权重,/>表示k时刻新生地图特征的多伯努利项所对应其高斯项的均值,/>表示k时刻新生地图特征的多伯努利项所对应其高斯项的协方差矩阵;
(43)在k-1时刻,机器人位姿为先验地图的概率密度集合表示为:
(44)单个粒子在相应条件下的概率密度表示为则k-1时刻先验地图的地图特征多伯努利RFS形式下的概率密度参数p的各高斯项表示为:
其中,分别代表k-1时刻的地图特征权重、均值和协方差,/>为高斯项个数;
(45)获得k时刻的地图特征状态预测估计为:
其中,为多伯努利项的概率密度参数p的高斯混合形式下的表达式;/>为多伯努利项的存在概率参数r的预测公式;为高斯项的均值的预测公式;/>为高斯项的协方差矩阵更新公式;
(46)获得势均衡多伯努利r参数和p参数的更新公式
(47)获得更新后的地图特征的多伯努利随机集:
其中,表示k时刻的第n个机器人的第i个粒子的更新阶段对漏检目标的更新后的多伯努利项,多伯努利项数为Lk|k-1个;/>表示k时刻第n个机器人的第i个粒子的第l个特征通过观测得到的更新部分的多伯努利项In表示粒子总数。
所述步骤(7)包括以下步骤:
(71)修剪和合并处理后的高斯信息表示为
(72)如果为空,则将时刻k的第n个机器人的观测集合作为时刻k的新生地图信息;
(73)如果不为空,则将k-1时刻所有机器人得到的高斯信息中,满足/>的高斯信息加入到k时刻的观测集合中作为当前时刻的先验信息集合,其中T为融合阈值,T=R+v*dt,R为机器人的扫描半径,v为机器人k时刻的移动速度,dt是采样时间间隔。
有益效果:与现有技术相比,本发明具有如下显著优点:
(1)提出势均衡多伯努利滤波器(CBMber-SLAM)方法改善多伯努利滤波器对SLAM中特征数量过估计问题。
(2)提出新的多机器人高斯信息融合方法(MR-GIF),提高了多机器人SLAM的精度。
附图说明
图1是本发明基于单机器人的势均衡多伯努利滤波SLAM方法结构图;
图2是本发明基于多机器人的势均衡多伯努利滤波SLAM方法结构图;
图3是本发明多机器人高斯项信息融合法的流程图。
具体实施方式
下面结合附图对本发明的技术方案作进一步说明。
由图1-图3可知,本发明所述基于多机器人的势均衡多伯努利滤波SLAM方法,包括以下步骤:
(1)建立观测的多伯努利随机有限集RFS模型和地图特征的多伯努利随机有限集RFS模型。
使用高斯混合形式实现CBMber-SLAM地图估计,并求得其参数表现形式。建模之前,先说明如下:
1)每个粒子都服从线性高斯的运动模型和观测模型,非线性时采用EKF的方式将其线性化。
fk|k-1(x|ξ)=N(x;Fk-1ξ,Qk-1) (1)
gk(z|x)=N(z;Hkx,Rk) (2)
fk|k-1(x|ξ)表示k时刻的状态转移函数,gk(z|x)表示时刻k地图特征的似然函数;N(;m,P)代表高斯分布,其中m为高斯均值,P则是高斯协方差矩阵。Fk-1代表目标状态转移矩阵,Qk-1代表的是过程噪声带来的协方差矩阵。Hk,Rk分别表示机器人观测形成的矩阵和传感器测量噪声带来的协方差矩阵。
2)目标的状态无关于检测概率以及目标的存活概率,且这两个概率是相互独立的。目标的存活概率用pS,k来表示,检测概率用pD,k来表示。
3)新生地图特征b(m|Xk)也为一个多伯努利随机有限集RFS,新生地图的多伯努利随机有限集RFS为:
用集合表示k时刻第n个机器人的特征观测集合,此时机器人位姿为/>
其观测的多伯努利随机有限集RFS模型
其中,表示第n个机器人在时刻k对地图特征的观测。/>表示机器人在m处对真实地图特征的观测,其模型是一个多伯努利RFS。/>的概率密度是/>并且/>表示当机器人姿态为/>时,机器人可以检测到位于m处的地图特征概率,也就是检测概率。/>表示第n个机器人的观测模型。/>表示第n个机器人在其位姿为/>观测到的杂波随机有限集RFS。
k时刻第n个机器人平台的特征地图用一个随机集合表示,其多伯努利随机有限集RFS模型为:
其中,表示第n个机器人平台0到k-1时刻地图特征的RFS,M表示整个地图特征的RFS。/>表示第n个机器人平台第i个粒子在k时刻的机器人的位姿,/>表示k时刻第n个机器人平台第i个粒子新生地图特征。由等式(4)可看出,地图特征数目在机器人的运动过程中逐步增加。
新生地图的多伯努利随机有限集RFS模型为:
其中,表示k时刻新生地图特征多伯努利RFS,/>表示k时刻新生地图特征多伯努利RFS表现形式下的存在概率参数r,/>表示k时刻新生地图特征多伯努利RFS表现形式下的概率密度参数/>表示k时刻新生地图特征多伯努利项的项数,/>表示所有机器人平台所滤波得到的高斯项信息中靠近各自自身位姿的高斯项信息。
新生地图特征不仅包括自身机器人观察到的新生地图特征b(m|Xk),还包括前一时刻所有机器人过滤处理的高斯信息中与自身机器人姿态接近的地图特征。
(2)将SLAM问题转换为独立机器人位姿状态估计和地图特征状态估计;
在RFS-SLAM中,地图特征Mk和观测集合Zk用随机有限集RFS来描述,其中集合表示不像向量表示,如FastSLAM,假设特征或测量是连续的,因此可以避免地图特征关联导致算法复杂的问题。
在RFS框架下,SLAM问题可以转化为在机器人观测集合Zk条件下估计地图特征Mk和机器人位姿X1:k的联合后验概率密度的过程。这种转换实现了机器人位姿和地图的同时估计。转换后联合后验概率密度表示为:
πk|k(Mk,X1:k|Z1:k,u1:k,X0) (6)
由于不熟悉环境的限制,在SLAM过程中无法知道机器人本身的真实位姿。因此,不能直接估计Mk和X1:k的联合后验概率密度,只能用条件概率等式将πk|k分解成两部分。分别为机器人位姿状态估计和在机器人位姿已知情况下对地图特征状态估计。分解后联合后验估计的表示为
πk(Mk,X1:k|Z0:k,u0:k-1,X0:k)=πk(X1:k|Z0:k,u0:k-1,X0)πk(Mk|Z0:k,X0:k) (7)
其中,πk(X1:k|Z0:k,u0:k-1,X0)表示机器人在k时刻观测量、k-1时刻的控制量和初始位姿量下的联合后验估计;πk(Mk|Z0:k,X0:k)表示机器人在位姿为X0:k时进行观测量Z0:k和位姿为X0:k条件下的地图特征联合后验估计。
(3)采用滤波器获得k时刻机器人位姿状态估计预测值。使用Rao-Blackwellised粒子滤波器进行机器人位姿估计得到机器人位姿先验估计的粒子集合,获得机器人位姿的预测估计值。
通过机器人的运动方程f(v,θ,t)来预测机器人的位姿以初始时刻k=0的位姿作为原点、以初始时刻的运动方向为y轴建立笛卡尔坐标系,其中v为机器人的速度,θ为机器人运行方向角度,t为运行时间。
(4)采用势均衡策略获得k时刻地图特征状态估计,即获得多伯努利参数存在概率参数r和概率密度参数p估计值;具体包括以下步骤:
(41)在k时刻,第n个机器人的第i个粒子所观测到的新生地图特征的多伯努利概率密度参数集合为则新生地图特征为:
其中,表示所有机器人平台所滤波得到的高斯项信息中靠近各自自身位姿的高斯项信息;此时的新生地图特征bn(m|Xk),不仅包括k-1时刻单个机器人自身的观测到的目标,还包括被加进来的k-1时刻之前其他机器人滤波处理之后的高斯项信息。而这些信息取自k-1时刻之前所有机器人的处理后的高斯项信息中,靠近每个机器人自身位姿的高斯项信息,作为新生地图特征信息集合的一部分。
(42)第n个机器人的位姿为时,其高斯混合项表达形式:
其中,表示k时刻新生地图特征的多伯努利项所对应的高斯项的个数,/>表示k时刻新生地图特征的多伯努利项所对应其高斯项的权重,/>表示k时刻新生地图特征的多伯努利项所对应其高斯项的均值,/>表示k时刻新生地图特征的多伯努利项所对应其高斯项的协方差矩阵;
(43)在k-1时刻,机器人位姿为先验地图的概率密度集合表示为:
(44)单个粒子在相应条件下的概率密度表示为则k-1时刻先验地图的地图特征多伯努利RFS形式下的概率密度参数p的各高斯项表示为:
其中,分别代表k-1时刻的地图特征权重、均值和协方差,/>为高斯项个数;
(45)因此获得k时刻的地图特征状态预测估计为:
其中,多伯努利项的概率密度参数p的高斯混合形式下的表达式:
多伯努利项的存在概率参数r的预测公式:
高斯项的均值的预测公式:
高斯项的协方差矩阵更新公式:
(46)根据多伯努利滤波器工作原理,结合SLAM问题,将势均衡策略用于提高SLAM的地图特征数目的估计精度。
通过扩展卡尔曼滤波求得混合高斯形式的p参数所对应的高斯项的权值、均值、协方差矩阵如下:
多机器人势均衡多伯努利参数在其更新过程的表达式为
按照上述公式对地图特征位姿进行更新,即对参数r和p进行更新。
(47)由于多伯努利RFS的并集仍然为一个多伯努利RFS,因此等式(12)可以记为式中Lk|k-1=Lk-1+Lb,k。此时单个粒子概率密度的高斯混合表达形式为:
获得更新后的地图特征密度可由如下的多伯努利随机集表示:
由上式可知,更新后的地图特征概率密度由两部分组成,
其中,表示k时刻的第n个机器人的第i个粒子的更新阶段对漏检目标的更新后的多伯努利项,多伯努利项数为Lk|k-1个;/>表示k时刻第n个机器人的第i个粒子的第l个特征通过观测得到的更新部分的多伯努利项,In表示粒子总数。
对于单个粒子的操作,只要区别于各自的机器人的观测信息和各自的位姿对应即可,其他与势均衡多伯努利滤波器的更新过程一致。
(5)修剪和合并更新后的高斯项。
首先,设置一个存在概率门限Tr=10-3,每次更新后,剔除存在概率小于门限值的多伯努利项。然后,对剩余多伯努利项的高斯分量进行修剪、合并。修剪的标准为高斯分量的权重大小,设定一个权重阈值Tp=10-5,将权重小于阈值的高斯分量修剪掉。接着设定一个高斯项距离门限Tm,将相互之间的位置距离小于门限Tm=1米高斯项合并为一个高斯分量。
(6)根据高斯项修正地图特征的状态估计。
首先将幸存的多伯努利项的势求和取整数,得到的即为地图特征的数目,记为Mn。
接着,取每一时刻机器人所得的幸存的多伯努利项所对应的高斯项中权重最大的高斯项,该高斯项的均值,即地图特征点的状态值,作为幸存的多伯努利项所对应的地图特征的状态估计值。
(7)将步骤(5)中各个机器人处理完的高斯项信息融合到每一个机器人的观测集合中,带入k+1时刻的机器人的先验信息集合中进行k+1时刻的处理过程;
首先将修剪和合并处理后的高斯信息表示为如果/>为空,则将时刻k的第n个机器人的观测集合作为时刻k的新生地图信息。
如果不为空,则将k-1时刻所有机器人得到的高斯信息中,满足/>的高斯信息加入到k时刻的观测集合中作为当前时刻的先验信息集合,其中T为融合阈值,T=R+v*dt,R为机器人的扫描半径,v为机器人k时刻的移动速度,dt是采样时间间隔。
(8)根据步骤(4)获得的势均衡多伯努利滤波器对每个机器人所对应的每个粒子地图估计结果调整粒子权重,然后通过采样粒子的加权平均,更新机器人k时刻位姿状态估计预测值;
首先,在k时刻,机器人位姿的后验估计采用所有粒子加权和。
其次,进行重要新采样,在k-1时刻,机器人位姿估计的样本集为通过机器人运动模型得到k时刻机器人位姿先验估计样本集/>结合PHD-SLAM地图估计,利用观测似然函数可得到机器人位姿后验分布/>表示k时刻第i个粒子的权值,其递推等式如下:
其中,是计算粒子权值的核心,与FastSLAM区别在于,FastSLAM定义在Euclidean空间,而基于随机有限集理论的滤波器方法则定义在随机有限集空间,根据等式(29)对/>的求解。
其中,ck(z|Xk)表示量测杂波的概率假设密度,/>表示更新的地图特征的数目,/>表示预测地图特征的数目。
然后,对粒子重采样,为了避免出现粒子退化的问题,需要对粒子进行重采样,得到新的粒子集最后得到机器人更新后的位姿/>
(9)令k=k+1,执行步骤(3)进行下一时刻的运算。
Claims (4)
1.一种基于多机器人的势均衡多伯努利滤波SLAM方法,其特征在于:包括以下步骤:
(1)建立观测的多伯努利随机有限集RFS模型和地图特征的多伯努利随机有限集RFS模型;
所述观测的多伯努利随机有限集RFS模型
其中,集合表示k时刻第n个机器人的特征观测集合,/>为机器人位姿,表示机器人在m处对真实地图特征的观测,/>表示第n个机器人在其位姿为/>观测到的杂波RFS;
所述地图特征的多伯努利随机有限集RFS模型
其中,表示第n个机器人平台0到k-1时刻地图特征的RFS,M表示整个地图特征的RFS;/>表示第n个机器人第i个粒子在k时刻的机器人的位姿,/>表示k时刻第n个机器人第i个粒子新生地图特征;
新生地图的多伯努利随机有限集RFS模型为
其中,表示k时刻新生地图特征多伯努利RFS,/>表示k时刻新生地图特征多伯努利RFS表现形式下的存在概率参数r,/>表示k时刻新生地图特征多伯努利RFS表现形式下的概率密度参数p,Lb,k表示k时刻新生地图特征多伯努利项的项数,/>表示所有机器人平台所滤波得到的高斯项信息中靠近各自自身位姿的高斯项信息;
(2)将SLAM问题转换为独立机器人位姿状态估计和地图特征状态估计;
(3)采用滤波器获得k时刻机器人位姿状态估计预测值;
(4)采用势均衡策略获得k时刻地图特征状态估计,即获得多伯努利参数存在概率参数r和概率密度参数p估计值;
(5)修剪和合并更新后的高斯项;
(6)根据高斯项修正地图特征的状态估计;
(7)将步骤(5)中各个机器人处理完的高斯项信息融合到每一个机器人的观测集合中,带入k+1时刻的机器人的先验信息集合中;具体包括:
(71)修剪和合并处理后的高斯信息表示为其中n表示第n个机器人,l表示第l个特征,/>表示高斯项个数;
(72)如果为空,则将k时刻的第n个机器人的观测集合作为k时刻的新生地图信息;
(73)如果不为空,则将k-1时刻所有机器人得到的高斯信息中,满足/>的高斯信息加入到k时刻的观测集合中作为当前时刻的先验信息集合,其中T为融合阈值,为机器人位姿,T=R+v*dt,R为机器人的扫描半径,v为机器人k时刻的移动速度,dt是采样时间间隔;
(8)根据步骤(4)获得的势均衡多伯努利滤波器对每个机器人所对应的每个粒子地图估计结果调整粒子权重,然后通过采样粒子的加权平均,更新k时刻机器人位姿状态估计预测值;
(9)令k=k+1,执行步骤(3)。
2.根据权利要求1所述的基于多机器人的势均衡多伯努利滤波SLAM方法,其特征在于:步骤(2)中分为机器人位姿状态估计和在机器人位姿已知情况下对地图特征的状态估计,分解后联合后验估计表示为
πk(Mk,X1:k|Z0:k,u0:k-1,X0:k)=πk(X1:k|Z0:k,u0:k-1,X0)πk(Mk|Z0:k,X0:k) (7)
其中,πk(X1:k|Z0:k,u0:k-1,X0)表示机器人在k时刻观测量、k-1时刻的控制量和初始位姿量下的联合后验估计;πk(Mk|Z0:k,X0:k)表示机器人在位姿为X0:k时进行观测量Z0:k和位姿为X0:k条件下的地图特征联合后验估计。
3.根据权利要求1所述的基于多机器人的势均衡多伯努利滤波SLAM方法,其特征在于:步骤(3)中所述滤波器为Rao-Blackwellised粒子滤波器。
4.根据权利要求1所述的基于多机器人的势均衡多伯努利滤波SLAM方法,其特征在于:步骤(4)包括以下步骤:
(41)在k时刻,第n个机器人的第i个粒子所观测到的新生地图特征的多伯努利概率密度参数集合为则新生地图特征为
其中,表示所有机器人平台所滤波得到的高斯项信息中靠近各自自身位姿的高斯项信息;
(42)第n个机器人的位姿为时,其高斯混合项表达形式
其中,表示k时刻新生地图特征的多伯努利项所对应的高斯项的个数,/>表示k时刻新生地图特征的多伯努利项所对应其高斯项的权重,/>表示k时刻新生地图特征的多伯努利项所对应其高斯项的均值,/>表示k时刻新生地图特征的多伯努利项所对应其高斯项的协方差矩阵;
(43)在k-1时刻,机器人位姿为先验地图的概率密度集合表示为
(44)单个粒子在相应条件下的概率密度表示为则k-1时刻先验地图的地图特征多伯努利RFS形式下的概率密度参数p的各高斯项表示为
其中,分别代表k-1时刻的地图特征权重、均值和协方差,/>为高斯项个数;
(45)获得k时刻的地图特征状态预测估计为
其中,为多伯努利项的概率密度参数p的高斯混合形式下的表达式;/>为多伯努利项的存在概率参数r的预测公式;为高斯项的均值的预测公式;/>为高斯项的协方差矩阵更新公式;
(46)获得势均衡多伯努利r参数和p参数的更新公式
(47)获得更新后的地图特征的多伯努利随机集
其中, 表示k时刻的第n个机器人的第i个粒子的更新阶段对漏检目标的更新后的多伯努利项,多伯努利项数为Lk|k-1个;/>表示k时刻第n个机器人的第i个粒子的第l个特征通过观测得到的更新部分的多伯努利项,In表示粒子总数。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111288777.9A CN114061584B (zh) | 2021-11-02 | 2021-11-02 | 一种基于多机器人的势均衡多伯努利滤波slam方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111288777.9A CN114061584B (zh) | 2021-11-02 | 2021-11-02 | 一种基于多机器人的势均衡多伯努利滤波slam方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114061584A CN114061584A (zh) | 2022-02-18 |
CN114061584B true CN114061584B (zh) | 2024-05-24 |
Family
ID=80236336
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111288777.9A Active CN114061584B (zh) | 2021-11-02 | 2021-11-02 | 一种基于多机器人的势均衡多伯努利滤波slam方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114061584B (zh) |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105467838A (zh) * | 2015-11-10 | 2016-04-06 | 山西大学 | 一种随机有限集框架下的同步定位与地图构建方法 |
CN106408594A (zh) * | 2016-09-28 | 2017-02-15 | 江南大学 | 基于多伯努利特征协方差的视频多目标跟踪方法 |
CN106403953A (zh) * | 2016-09-05 | 2017-02-15 | 江苏科技大学 | 一种用于水下自主导航与定位的方法 |
CN110503071A (zh) * | 2019-08-29 | 2019-11-26 | 电子科技大学 | 基于变分贝叶斯标签多伯努利叠加模型的多目标跟踪方法 |
KR20200036071A (ko) * | 2018-09-17 | 2020-04-07 | 한빛 세마텍(주) | 자율주행통합제어기를 이용한 지능형 자동주차 및 호출 시스템 과 플랫폼 |
CN111580084A (zh) * | 2020-05-13 | 2020-08-25 | 中国人民解放军国防科技大学 | 一种面向多距离扩展目标的多伯努利检测前跟踪方法 |
WO2021008077A1 (zh) * | 2019-07-16 | 2021-01-21 | 深圳大学 | 一种闪烁噪声下的多目标跟踪方法及*** |
CN112526886A (zh) * | 2020-12-08 | 2021-03-19 | 北京航空航天大学 | 随机试验长度下离散多智能体***迭代学习编队控制方法 |
CN113344970A (zh) * | 2021-05-19 | 2021-09-03 | 电子科技大学 | 基于多伯努利的非规则多扩展目标联合跟踪与分类方法 |
WO2021188566A1 (en) * | 2020-03-18 | 2021-09-23 | Realtime Robotics, Inc. | Digital representations of robot operational environment, useful in motion planning for robots |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US11449061B2 (en) * | 2016-02-29 | 2022-09-20 | AI Incorporated | Obstacle recognition method for autonomous robots |
-
2021
- 2021-11-02 CN CN202111288777.9A patent/CN114061584B/zh active Active
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105467838A (zh) * | 2015-11-10 | 2016-04-06 | 山西大学 | 一种随机有限集框架下的同步定位与地图构建方法 |
CN106403953A (zh) * | 2016-09-05 | 2017-02-15 | 江苏科技大学 | 一种用于水下自主导航与定位的方法 |
CN106408594A (zh) * | 2016-09-28 | 2017-02-15 | 江南大学 | 基于多伯努利特征协方差的视频多目标跟踪方法 |
KR20200036071A (ko) * | 2018-09-17 | 2020-04-07 | 한빛 세마텍(주) | 자율주행통합제어기를 이용한 지능형 자동주차 및 호출 시스템 과 플랫폼 |
WO2021008077A1 (zh) * | 2019-07-16 | 2021-01-21 | 深圳大学 | 一种闪烁噪声下的多目标跟踪方法及*** |
CN110503071A (zh) * | 2019-08-29 | 2019-11-26 | 电子科技大学 | 基于变分贝叶斯标签多伯努利叠加模型的多目标跟踪方法 |
WO2021188566A1 (en) * | 2020-03-18 | 2021-09-23 | Realtime Robotics, Inc. | Digital representations of robot operational environment, useful in motion planning for robots |
CN111580084A (zh) * | 2020-05-13 | 2020-08-25 | 中国人民解放军国防科技大学 | 一种面向多距离扩展目标的多伯努利检测前跟踪方法 |
CN112526886A (zh) * | 2020-12-08 | 2021-03-19 | 北京航空航天大学 | 随机试验长度下离散多智能体***迭代学习编队控制方法 |
CN113344970A (zh) * | 2021-05-19 | 2021-09-03 | 电子科技大学 | 基于多伯努利的非规则多扩展目标联合跟踪与分类方法 |
Non-Patent Citations (6)
Title |
---|
Diluka Moratuwage , Martin Adams ,and Felipe Inostroza .δ-Generalized Labeled Multi-Bernoulli Simultaneous Localization and Mapping with an Optimal Kernel-Based Particle Filtering Approach.SENSORS.2019,1-21. * |
Rao-Blackwellized粒子势均衡多目标多伯努利滤波器;陈辉;韩崇昭;;控制理论与应用(第02期);全文 * |
一种改进的多伯努利多目标跟踪算法;王海环;王俊;;西安电子科技大学学报;20161220(第06期);全文 * |
低检测率条件下改进的势均衡多目标多伯努利算法+;马丽丽;王战;陈金广;;计算机工程;20160127(第07期);全文 * |
基于多平台联合的势均衡多伯努利滤波SLAM方法研究;李宁;中国优秀硕士学位论文全文数据库 信息科技辑;19-36 * |
改进的GM-CBMeMBer机载多普勒雷达多目标跟踪算法;魏立兴;孙合敏;吴卫华;黄志良;;空军预警学院学报;20170615(第03期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN114061584A (zh) | 2022-02-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111985093B (zh) | 一种带噪声估计器的自适应无迹卡尔曼滤波状态估计方法 | |
CN109798896B (zh) | 一种室内机器人定位与建图方法及装置 | |
CN109597864B (zh) | 椭球边界卡尔曼滤波的即时定位与地图构建方法及*** | |
CN108255791B (zh) | 基于分布式传感器一致性的机动目标跟踪方法 | |
CN106772524B (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
KR100816269B1 (ko) | 언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도작성 방법 | |
CN111127523B (zh) | 基于量测迭代更新的多传感器gmphd自适应融合方法 | |
CN108267731B (zh) | 无人机目标跟踪***的构建方法及应用 | |
CN107084714A (zh) | 一种基于RoboCup3D的多机器人协作目标定位方法 | |
CN104090262B (zh) | 一种基于多采样率多模型融合估计的移动目标跟踪方法 | |
CN110209180B (zh) | 一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法 | |
CN112527017B (zh) | 一种基于多auv的海洋观测方法 | |
CN111948601B (zh) | 一种非高斯噪声条件下的单站纯角度目标定位与跟踪方法 | |
CN108226887B (zh) | 一种观测量短暂丢失情况下的水面目标救援状态估计方法 | |
CN110232662B (zh) | 一种人脸位姿协同***下的多新息抗扰滤波方法 | |
CN110989341B (zh) | 一种约束辅助粒子滤波方法及目标跟踪方法 | |
CN109579860B (zh) | 一种基于场在线估计的水下机器人场源搜索方法 | |
CN108680162B (zh) | 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法 | |
Qian et al. | An INS/DVL integrated navigation filtering method against complex underwater environment | |
CN114061584B (zh) | 一种基于多机器人的势均衡多伯努利滤波slam方法 | |
CN111340853B (zh) | 基于ospa迭代的多传感器gmphd自适应融合方法 | |
CN110426689B (zh) | 一种基于em-cks的机载多平台多传感器***误差配准算法 | |
CN111931368A (zh) | 一种基于gru粒子滤波的uuv目标状态估计方法 | |
CN107966697B (zh) | 一种基于渐进无迹卡尔曼的移动目标跟踪方法 | |
CN111262556A (zh) | 一种同时估计未知高斯测量噪声统计量的多目标跟踪方法 |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |