CN114061584A - 一种基于多机器人的势均衡多伯努利滤波slam方法 - Google Patents

一种基于多机器人的势均衡多伯努利滤波slam方法 Download PDF

Info

Publication number
CN114061584A
CN114061584A CN202111288777.9A CN202111288777A CN114061584A CN 114061584 A CN114061584 A CN 114061584A CN 202111288777 A CN202111288777 A CN 202111288777A CN 114061584 A CN114061584 A CN 114061584A
Authority
CN
China
Prior art keywords
robot
bernoulli
gaussian
moment
map
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.)
Granted
Application number
CN202111288777.9A
Other languages
English (en)
Other versions
CN114061584B (zh
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.)
Jiangsu University of Science and Technology
Original Assignee
Jiangsu University of Science and 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 Jiangsu University of Science and Technology filed Critical Jiangsu University of Science and Technology
Priority to CN202111288777.9A priority Critical patent/CN114061584B/zh
Publication of CN114061584A publication Critical patent/CN114061584A/zh
Application granted granted Critical
Publication of CN114061584B publication Critical patent/CN114061584B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/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方法的准确性下降。为了解决复杂的数据关联问题，一种方法是避免数据关联问题，另一种方法是使用更有效的算法，比如基于图的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模型：
Figure BDA0003333842720000021
其中，集合
Figure BDA0003333842720000022
表示k时刻第n个机器人的特征观测集合，
Figure BDA0003333842720000023
为机器人位姿，
Figure BDA0003333842720000024
表示机器人在m处对真实地图特征的观测，
Figure BDA0003333842720000025
表示第n个机器人在其位姿为
Figure BDA0003333842720000026
观测到的杂波RFS；
所述地图特征的多伯努利随机有限集RFS模型：
Figure BDA0003333842720000027
其中，
Figure BDA0003333842720000028
表示第n个机器人平台0到k-1时刻地图特征的RFS，M表示整个地图特征的RFS；
Figure BDA0003333842720000029
表示第n个机器人第i个粒子在k时刻的机器人的位姿，
Figure BDA00033338427200000210
表示k时刻第n个机器人第i个粒子新生地图特征；
新生地图的多伯努利随机有限集RFS模型为：
Figure BDA00033338427200000211
其中，
Figure BDA0003333842720000031
表示k时刻新生地图特征多伯努利RFS，
Figure BDA0003333842720000032
表示k时刻新生地图特征多伯努利RFS表现形式下的存在概率参数r，
Figure BDA0003333842720000033
表示k时刻新生地图特征多伯努利RFS表现形式下的概率密度参数
Figure BDA0003333842720000034
表示k时刻新生地图特征多伯努利项的项数，
Figure BDA0003333842720000035
表示所有机器人平台所滤波得到的高斯项信息中靠近各自自身位姿的高斯项信息。
步骤(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个粒子所观测到的新生地图特征的多伯努利概率密度参数集合为
Figure BDA0003333842720000036
则新生地图特征为：
Figure BDA0003333842720000037
其中,
Figure BDA0003333842720000038
表示所有机器人平台所滤波得到的高斯项信息中靠近各自自身位姿的高斯项信息；
(42)第n个机器人的位姿为
Figure BDA0003333842720000039
时，其高斯混合项表达形式：
Figure BDA00033338427200000310
其中，
Figure BDA00033338427200000311
表示k时刻新生地图特征的多伯努利项所对应的高斯项的个数，
Figure BDA00033338427200000312
表示k时刻新生地图特征的多伯努利项所对应其高斯项的权重，
Figure BDA00033338427200000313
表示k时刻新生地图特征的多伯努利项所对应其高斯项的均值，
Figure BDA00033338427200000314
表示k时刻新生地图特征的多伯努利项所对应其高斯项的协方差矩阵；
(43)在k-1时刻，机器人位姿为
Figure BDA0003333842720000041
先验地图的概率密度集合表示为：
Figure BDA0003333842720000042
(44)单个粒子在相应条件下的概率密度表示为
Figure BDA0003333842720000043
则k-1时刻先验地图的地图特征多伯努利RFS形式下的概率密度参数p的各高斯项表示为：
Figure BDA0003333842720000044
其中，
Figure BDA0003333842720000045
分别代表k-1时刻的地图特征权重、均值和协方差，
Figure BDA0003333842720000046
为高斯项个数；
(45)获得k时刻的地图特征状态预测估计为：
Figure BDA0003333842720000047
其中，
Figure BDA0003333842720000048
为多伯努利项的概率密度参数p的高斯混合形式下的表达式；
Figure BDA0003333842720000049
为多伯努利项的存在概率参数r的预测公式；
Figure BDA00033338427200000410
为高斯项的均值的预测公式；
Figure BDA00033338427200000411
为高斯项的协方差矩阵更新公式；
(46)获得势均衡多伯努利r参数和p参数的更新公式
Figure BDA00033338427200000412
Figure BDA00033338427200000413
Figure BDA00033338427200000414
Figure BDA0003333842720000051
(47)获得更新后的地图特征的多伯努利随机集：
Figure BDA0003333842720000052
其中，
Figure BDA0003333842720000053
表示k时刻的第n个机器人的第i个粒子的更新阶段对漏检目标的更新后的多伯努利项，多伯努利项数为Lk|k-1个；
Figure BDA0003333842720000054
表示k时刻第n个机器人的第i个粒子的第l个特征通过观测得到的更新部分的多伯努利项In表示粒子总数。
所述步骤(7)包括以下步骤：
(71)修剪和合并处理后的高斯信息表示为
Figure BDA0003333842720000055
(72)如果
Figure BDA0003333842720000056
为空，则将时刻k的第n个机器人的观测集合作为时刻k的新生地图信息；
(73)如果
Figure BDA0003333842720000057
不为空，则将k-1时刻所有机器人得到的高斯信息中，满足
Figure BDA0003333842720000058
的高斯信息加入到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为：
Figure BDA0003333842720000061
用集合
Figure BDA0003333842720000062
表示k时刻第n个机器人的特征观测集合，此时机器人位姿为
Figure BDA0003333842720000063
其观测的多伯努利随机有限集RFS模型
Figure BDA0003333842720000064
其中，
Figure BDA0003333842720000065
表示第n个机器人在时刻k对地图特征的观测。
Figure BDA0003333842720000066
表示机器人在m处对真实地图特征的观测，其模型是一个多伯努利RFS。
Figure BDA0003333842720000071
的概率密度是
Figure BDA0003333842720000072
并且
Figure BDA0003333842720000073
表示当机器人姿态为
Figure BDA0003333842720000074
时，机器人可以检测到位于m处的地图特征概率，也就是检测概率。
Figure BDA0003333842720000075
表示第n个机器人的观测模型。
Figure BDA0003333842720000076
表示第n个机器人在其位姿为
Figure BDA0003333842720000077
观测到的杂波随机有限集RFS。
k时刻第n个机器人平台的特征地图用一个随机集合
Figure BDA0003333842720000078
表示，其多伯努利随机有限集RFS模型为：
Figure BDA0003333842720000079
其中，
Figure BDA00033338427200000710
表示第n个机器人平台0到k-1时刻地图特征的RFS，M表示整个地图特征的RFS。
Figure BDA00033338427200000711
表示第n个机器人平台第i个粒子在k时刻的机器人的位姿，
Figure BDA00033338427200000712
表示k时刻第n个机器人平台第i个粒子新生地图特征。由等式(4)可看出，地图特征数目在机器人的运动过程中逐步增加。
新生地图的多伯努利随机有限集RFS模型为：
Figure BDA00033338427200000713
其中，
Figure BDA00033338427200000714
表示k时刻新生地图特征多伯努利RFS，
Figure BDA00033338427200000715
表示k时刻新生地图特征多伯努利RFS表现形式下的存在概率参数r，
Figure BDA00033338427200000716
表示k时刻新生地图特征多伯努利RFS表现形式下的概率密度参数
Figure BDA00033338427200000717
表示k时刻新生地图特征多伯努利项的项数，
Figure BDA00033338427200000718
表示所有机器人平台所滤波得到的高斯项信息中靠近各自自身位姿的高斯项信息。
新生地图特征不仅包括自身机器人观察到的新生地图特征b(m|Xk)，还包括前一时刻所有机器人过滤处理的高斯信息中与自身机器人姿态接近的地图特征。
(2)将SLAM问题转换为独立机器人位姿状态估计和地图特征状态估计；
在RFS-SLAM中，地图特征Mk和观测集合Zk用随机有限集RFS来描述，其中
Figure BDA00033338427200000719
集合表示不像向量表示，如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)来预测机器人的位姿
Figure BDA0003333842720000081
以初始时刻k＝0的位姿作为原点、以初始时刻的运动方向为y轴建立笛卡尔坐标系，其中v为机器人的速度，θ为机器人运行方向角度，t为运行时间。
(4)采用势均衡策略获得k时刻地图特征状态估计,即获得多伯努利参数存在概率参数r和概率密度参数p估计值；具体包括以下步骤：
(41)在k时刻，第n个机器人的第i个粒子所观测到的新生地图特征的多伯努利概率密度参数集合为
Figure BDA0003333842720000082
则新生地图特征为：
Figure BDA0003333842720000083
其中,
Figure BDA0003333842720000084
表示所有机器人平台所滤波得到的高斯项信息中靠近各自自身位姿的高斯项信息；此时的新生地图特征bn(m|Xk)，不仅包括k-1时刻单个机器人自身的观测到的目标，还包括被加进来的k-1时刻之前其他机器人滤波处理之后的高斯项信息。而这些信息取自k-1时刻之前所有机器人的处理后的高斯项信息中，靠近每个机器人自身位姿的高斯项信息，作为新生地图特征信息集合的一部分。
(42)第n个机器人的位姿为
Figure BDA0003333842720000091
时，其高斯混合项表达形式：
Figure BDA0003333842720000092
其中，
Figure BDA0003333842720000093
表示k时刻新生地图特征的多伯努利项所对应的高斯项的个数，
Figure BDA0003333842720000094
表示k时刻新生地图特征的多伯努利项所对应其高斯项的权重，
Figure BDA0003333842720000095
表示k时刻新生地图特征的多伯努利项所对应其高斯项的均值，
Figure BDA0003333842720000096
表示k时刻新生地图特征的多伯努利项所对应其高斯项的协方差矩阵；
(43)在k-1时刻，机器人位姿为
Figure BDA0003333842720000097
先验地图的概率密度集合表示为：
Figure BDA0003333842720000098
(44)单个粒子在相应条件下的概率密度表示为
Figure BDA0003333842720000099
则k-1时刻先验地图的地图特征多伯努利RFS形式下的概率密度参数p的各高斯项表示为：
Figure BDA00033338427200000910
其中，
Figure BDA00033338427200000911
分别代表k-1时刻的地图特征权重、均值和协方差，
Figure BDA00033338427200000912
为高斯项个数；
(45)因此获得k时刻的地图特征状态预测估计为：
Figure BDA00033338427200000913
其中，多伯努利项的概率密度参数p的高斯混合形式下的表达式：
Figure BDA00033338427200000914
多伯努利项的存在概率参数r的预测公式：
Figure BDA0003333842720000101
高斯项的均值的预测公式：
Figure BDA0003333842720000102
高斯项的协方差矩阵更新公式：
Figure BDA0003333842720000103
(46)根据多伯努利滤波器工作原理，结合SLAM问题，将势均衡策略用于提高SLAM的地图特征数目的估计精度。
通过扩展卡尔曼滤波求得混合高斯形式的p参数所对应的高斯项的权值、均值、协方差矩阵如下：
Figure BDA0003333842720000104
Figure BDA0003333842720000105
Figure BDA0003333842720000106
Figure BDA0003333842720000107
Figure BDA0003333842720000108
多机器人势均衡多伯努利参数在其更新过程的表达式为
Figure BDA0003333842720000109
Figure BDA00033338427200001010
Figure BDA00033338427200001011
Figure BDA0003333842720000111
按照上述公式对地图特征位姿进行更新，即对参数r和p进行更新。
(47)由于多伯努利RFS的并集仍然为一个多伯努利RFS，因此等式(12)可以记为
Figure BDA0003333842720000112
式中Lk|k-1＝Lk-1+Lb,k。此时单个粒子概率密度的高斯混合表达形式为：
Figure BDA0003333842720000113
获得更新后的地图特征密度可由如下的多伯努利随机集表示：
Figure BDA0003333842720000114
由上式可知，更新后的地图特征概率密度由两部分组成，
其中，
Figure BDA0003333842720000115
表示k时刻的第n个机器人的第i个粒子的更新阶段对漏检目标的更新后的多伯努利项，多伯努利项数为Lk|k-1个；
Figure BDA0003333842720000116
表示k时刻第n个机器人的第i个粒子的第l个特征通过观测得到的更新部分的多伯努利项，In表示粒子总数。
对于单个粒子的操作，只要区别于各自的机器人的观测信息和各自的位姿对应即可，其他与势均衡多伯努利滤波器的更新过程一致。
(5)修剪和合并更新后的高斯项。
首先，设置一个存在概率门限Tr＝10-3，每次更新后，剔除存在概率小于门限值的多伯努利项。然后，对剩余多伯努利项的高斯分量进行修剪、合并。修剪的标准为高斯分量的权重大小，设定一个权重阈值Tp＝10-5，将权重小于阈值的高斯分量修剪掉。接着设定一个高斯项距离门限Tm，将相互之间的位置距离小于门限Tm＝1米高斯项合并为一个高斯分量。
(6)根据高斯项修正地图特征的状态估计。
首先将幸存的多伯努利项的势求和取整数，得到的即为地图特征的数目，记为Mn。
接着，取每一时刻机器人所得的幸存的多伯努利项所对应的高斯项中权重最大的高斯项，该高斯项的均值，即地图特征点的状态值，作为幸存的多伯努利项所对应的地图特征的状态估计值。
(7)将步骤(5)中各个机器人处理完的高斯项信息融合到每一个机器人的观测集合中，带入k+1时刻的机器人的先验信息集合中进行k+1时刻的处理过程；
首先将修剪和合并处理后的高斯信息表示为
Figure BDA0003333842720000121
如果
Figure BDA0003333842720000122
为空，则将时刻k的第n个机器人的观测集合作为时刻k的新生地图信息。
如果
Figure BDA00033338427200001213
不为空，则将k-1时刻所有机器人得到的高斯信息中，满足
Figure BDA0003333842720000123
的高斯信息加入到k时刻的观测集合中作为当前时刻的先验信息集合，其中T为融合阈值，T＝R+v*dt，R为机器人的扫描半径，v为机器人k时刻的移动速度,dt是采样时间间隔。
(8)根据步骤(4)获得的势均衡多伯努利滤波器对每个机器人所对应的每个粒子地图估计结果调整粒子权重,然后通过采样粒子的加权平均，更新机器人k时刻位姿状态估计预测值；
首先，在k时刻，机器人位姿的后验估计采用所有粒子加权和。
其次，进行重要新采样，在k-1时刻，机器人位姿估计的样本集为
Figure BDA0003333842720000124
通过机器人运动模型得到k时刻机器人位姿先验估计样本集
Figure BDA0003333842720000125
结合PHD-SLAM地图估计，利用观测似然函数可得到机器人位姿后验分布
Figure BDA0003333842720000126
表示k时刻第i个粒子的权值，其递推等式如下：
Figure BDA0003333842720000127
其中，
Figure BDA0003333842720000128
是计算粒子权值的核心，与FastSLAM区别在于，FastSLAM定义在Euclidean空间，而基于随机有限集理论的滤波器方法则定义在随机有限集空间，根据等式(29)对
Figure BDA0003333842720000129
的求解。
Figure BDA00033338427200001210
其中，
Figure BDA00033338427200001211
ck(z|Xk)表示量测杂波的概率假设密度，
Figure BDA00033338427200001212
表示更新的地图特征的数目，
Figure BDA0003333842720000131
表示预测地图特征的数目。
然后，对粒子重采样，为了避免出现粒子退化的问题，需要对粒子进行重采样，得到新的粒子集
Figure BDA0003333842720000132
最后得到机器人更新后的位姿
Figure BDA0003333842720000133
(9)令k＝k+1，执行步骤(3)进行下一时刻的运算。

Claims (6)

1.一种基于多机器人的势均衡多伯努利滤波SLAM方法，其特征在于：包括以下步骤：
(1)建立观测的多伯努利随机有限集RFS模型和地图特征的多伯努利随机有限集RFS模型；
(2)将SLAM问题转换为独立机器人位姿状态估计和地图特征状态估计；
(3)采用滤波器获得k时刻机器人位姿状态估计预测值；
(4)采用势均衡策略获得k时刻地图特征状态估计,即获得多伯努利参数存在概率参数r和概率密度参数p估计值；
(5)修剪和合并更新后的高斯项；
(6)根据高斯项修正地图特征的状态估计；
(7)将步骤(5)中各个机器人处理完的高斯项信息融合到每一个机器人的观测集合中，带入k+1时刻的机器人的先验信息集合中；
(8)根据步骤(4)获得的势均衡多伯努利滤波器对每个机器人所对应的每个粒子地图估计结果调整粒子权重,然后通过采样粒子的加权平均，更新k时刻机器人位姿状态估计预测值；
(9)令k＝k+1，执行步骤(3)。
2.根据权利要求1所述的基于多机器人的势均衡多伯努利滤波SLAM方法，其特征在于：
所述观测的多伯努利随机有限集RFS模型
Figure FDA0003333842710000011
其中，集合
Figure FDA0003333842710000012
表示k时刻第n个机器人的特征观测集合，
Figure FDA0003333842710000013
为机器人位姿，
Figure FDA0003333842710000014
表示机器人在m处对真实地图特征的观测，
Figure FDA0003333842710000015
表示第n个机器人在其位姿为
Figure FDA0003333842710000016
观测到的杂波RFS；
所述地图特征的多伯努利随机有限集RFS模型
Figure FDA0003333842710000017
其中，
Figure FDA0003333842710000018
表示第n个机器人平台0到k-1时刻地图特征的RFS，M表示整个地图特征的RFS；
Figure FDA0003333842710000021
表示第n个机器人第i个粒子在k时刻的机器人的位姿，
Figure FDA0003333842710000022
表示k时刻第n个机器人第i个粒子新生地图特征；
新生地图的多伯努利随机有限集RFS模型为
Figure FDA0003333842710000023
其中，
Figure FDA0003333842710000024
表示k时刻新生地图特征多伯努利RFS，
Figure FDA0003333842710000025
表示k时刻新生地图特征多伯努利RFS表现形式下的存在概率参数r，
Figure FDA0003333842710000026
表示k时刻新生地图特征多伯努利RFS表现形式下的概率密度参数p，Lb,k表示k时刻新生地图特征多伯努利项的项数，
Figure FDA0003333842710000027
表示所有机器人平台所滤波得到的高斯项信息中靠近各自自身位姿的高斯项信息。
3.根据权利要求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条件下的地图特征联合后验估计。
4.根据权利要求1所述的基于多机器人的势均衡多伯努利滤波SLAM方法，其特征在于：步骤(3)中所述滤波器为Rao-Blackwellised粒子滤波器。
5.根据权利要求1所述的基于多机器人的势均衡多伯努利滤波SLAM方法，其特征在于：步骤(4)包括以下步骤：
(41)在k时刻，第n个机器人的第i个粒子所观测到的新生地图特征的多伯努利概率密度参数集合为
Figure FDA0003333842710000028
则新生地图特征为
Figure FDA0003333842710000029
其中,
Figure FDA00033338427100000210
表示所有机器人平台所滤波得到的高斯项信息中靠近各自自身位姿的高斯项信息；
(42)第n个机器人的位姿为
Figure FDA0003333842710000031
时，其高斯混合项表达形式
Figure FDA0003333842710000032
其中，
Figure FDA0003333842710000033
表示k时刻新生地图特征的多伯努利项所对应的高斯项的个数，
Figure FDA0003333842710000034
表示k时刻新生地图特征的多伯努利项所对应其高斯项的权重，
Figure FDA0003333842710000035
表示k时刻新生地图特征的多伯努利项所对应其高斯项的均值，
Figure FDA0003333842710000036
表示k时刻新生地图特征的多伯努利项所对应其高斯项的协方差矩阵；
(43)在k-1时刻，机器人位姿为
Figure FDA0003333842710000037
先验地图的概率密度集合表示为
Figure FDA0003333842710000038
(44)单个粒子在相应条件下的概率密度表示为
Figure FDA0003333842710000039
则k-1时刻先验地图的地图特征多伯努利RFS形式下的概率密度参数p的各高斯项表示为
Figure FDA00033338427100000310
其中，
Figure FDA00033338427100000311
分别代表k-1时刻的地图特征权重、均值和协方差，
Figure FDA00033338427100000312
为高斯项个数；
(45)获得k时刻的地图特征状态预测估计为
Figure FDA00033338427100000313
其中，
Figure FDA00033338427100000314
为多伯努利项的概率密度参数p的高斯混合形式下的表达式；
Figure FDA00033338427100000315
为多伯努利项的存在概率参数r的预测公式；
Figure FDA00033338427100000316
为高斯项的均值的预测公式；
Figure FDA00033338427100000317
为高斯项的协方差矩阵更新公式；
(46)获得势均衡多伯努利r参数和p参数的更新公式
Figure FDA0003333842710000041
Figure FDA0003333842710000042
Figure FDA0003333842710000043
Figure FDA0003333842710000044
(47)获得更新后的地图特征的多伯努利随机集
Figure FDA0003333842710000045
其中，
Figure FDA0003333842710000046
表示k时刻的第n个机器人的第i个粒子的更新阶段对漏检目标的更新后的多伯努利项，多伯努利项数为Lk|k-1个；
Figure FDA0003333842710000047
表示k时刻第n个机器人的第i个粒子的第l个特征通过观测得到的更新部分的多伯努利项，In表示粒子总数。
6.根据权利要求1所述的基于多机器人的势均衡多伯努利滤波SLAM方法，其特征在于：所述步骤(7)包括以下步骤：
(71)修剪和合并处理后的高斯信息表示为
Figure FDA0003333842710000048
(72)如果
Figure FDA0003333842710000049
为空，则将k时刻的第n个机器人的观测集合作为k时刻的新生地图信息；
(73)如果
Figure FDA00033338427100000410
不为空，则将k-1时刻所有机器人得到的高斯信息中，满足
Figure FDA00033338427100000411
的高斯信息加入到k时刻的观测集合中作为当前时刻的先验信息集合，其中T为融合阈值，T＝R+v*dt，R为机器人的扫描半径，v为机器人k时刻的移动速度,dt是采样时间间隔。
CN202111288777.9A 2021-11-02 2021-11-02 一种基于多机器人的势均衡多伯努利滤波slam方法 Active CN114061584B (zh)

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 true CN114061584A (zh) 2022-02-18
CN114061584B 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 (11)

* Cited by examiner, † Cited by third party
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 北京航空航天大学 随机试验长度下离散多智能体***迭代学习编队控制方法
US20210089040A1 (en) * 2016-02-29 2021-03-25 AI Incorporated Obstacle recognition method for autonomous robots
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

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105467838A (zh) * 2015-11-10 2016-04-06 山西大学 一种随机有限集框架下的同步定位与地图构建方法
US20210089040A1 (en) * 2016-02-29 2021-03-25 AI Incorporated Obstacle recognition method for autonomous robots
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)

* Cited by examiner, † Cited by third party
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, 17 May 2019 (2019-05-17), pages 1 - 21 *
李宁: "基于多平台联合的势均衡多伯努利滤波SLAM方法研究", 中国优秀硕士学位论文全文数据库 信息科技辑, pages 19 - 36 *
王海环;王俊;: "一种改进的多伯努利多目标跟踪算法", 西安电子科技大学学报, no. 06, 20 December 2016 (2016-12-20) *
陈辉;韩崇昭;: "Rao-Blackwellized粒子势均衡多目标多伯努利滤波器", 控制理论与应用, no. 02 *
马丽丽;王战;陈金广;: "低检测率条件下改进的势均衡多目标多伯努利算法+", 计算机工程, no. 07, 27 January 2016 (2016-01-27) *
魏立兴;孙合敏;吴卫华;黄志良;: "改进的GM-CBMeMBer机载多普勒雷达多目标跟踪算法", 空军预警学院学报, no. 03, 15 June 2017 (2017-06-15) *

Also Published As

Publication number Publication date
CN114061584B (zh) 2024-05-24

Similar Documents

Publication Publication Date Title
CN110689576B (zh) 一种基于Autoware的动态3D点云正态分布AGV定位方法
CN109597864B (zh) 椭球边界卡尔曼滤波的即时定位与地图构建方法及***
CN105180935B (zh) 一种适用于gnss微弱信号的组合导航数据融合方法
CN106772524B (zh) 一种基于秩滤波的农业机器人组合导航信息融合方法
KR100816269B1 (ko) 언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도작성 방법
CN108255791B (zh) 基于分布式传感器一致性的机动目标跟踪方法
CN105467838B (zh) 一种随机有限集框架下的同步定位与地图构建方法
CN110209180B (zh) 一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法
CN106403953B (zh) 一种用于水下自主导航与定位的方法
CN110989341B (zh) 一种约束辅助粒子滤波方法及目标跟踪方法
CN110232662B (zh) 一种人脸位姿协同***下的多新息抗扰滤波方法
CN108680162B (zh) 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法
Qian et al. An INS/DVL integrated navigation filtering method against complex underwater environment
CN114608568A (zh) 一种基于多传感器信息即时融合定位方法
CN111262556B (zh) 一种同时估计未知高斯测量噪声统计量的多目标跟踪方法
CN114061584A (zh) 一种基于多机器人的势均衡多伯努利滤波slam方法
CN107562837B (zh) 一种基于道路网的机动目标跟踪算法
CN106556818B (zh) 一种用于单目标跟踪的低计算复杂度贝努利滤波器
CN107966697B (zh) 一种基于渐进无迹卡尔曼的移动目标跟踪方法
Zhang et al. Mobile robot localization based on gradient propagation particle filter network
CN114111772B (zh) 一种基于数据手套的水下机器人软体作业手位置跟踪方法
CN115963503A (zh) 激光雷达对空间非合作慢旋目标的相对位姿快速测量方法
CN114995403A (zh) 相关噪声及非高斯干扰下轮式移动机器人轨迹跟踪方法
CN115307645A (zh) 一种基于多伯努利滤波器的同时定位与建图方法
Aditya Implementation of a 4D fast SLAM including volumetric sum of the UAV

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