CN115077529B - 一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统 - Google Patents
一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统 Download PDFInfo
- Publication number
- CN115077529B CN115077529B CN202210677851.4A CN202210677851A CN115077529B CN 115077529 B CN115077529 B CN 115077529B CN 202210677851 A CN202210677851 A CN 202210677851A CN 115077529 B CN115077529 B CN 115077529B
- Authority
- CN
- China
- Prior art keywords
- robot
- particle
- particle swarm
- calculating
- weight
- 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
- 239000002245 particle Substances 0.000 title claims abstract description 145
- 238000000034 method Methods 0.000 title claims abstract description 54
- 238000001914 filtration Methods 0.000 title claims abstract description 36
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 34
- 230000004927 fusion Effects 0.000 claims abstract description 14
- 238000005070 sampling Methods 0.000 claims description 36
- 238000012952 Resampling Methods 0.000 claims description 13
- 239000011159 matrix material Substances 0.000 claims description 13
- 238000010276 construction Methods 0.000 claims description 6
- 230000001186 cumulative effect Effects 0.000 claims description 6
- 238000005259 measurement Methods 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 6
- 230000008045 co-localization Effects 0.000 claims 1
- 238000004364 calculation method Methods 0.000 description 7
- 238000004088 simulation Methods 0.000 description 6
- 230000008901 benefit Effects 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000010606 normalization 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/004—Artificial life, i.e. computing arrangements simulating life
- G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- General Health & Medical Sciences (AREA)
- General Engineering & Computer Science (AREA)
- Data Mining & Analysis (AREA)
- Evolutionary Computation (AREA)
- Biophysics (AREA)
- Molecular Biology (AREA)
- Computing Systems (AREA)
- Computational Linguistics (AREA)
- Biomedical Technology (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Artificial Intelligence (AREA)
- Life Sciences & Earth Sciences (AREA)
- Health & Medical Sciences (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
本发明提出一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统,涉及多机器人协同定位技术领域,用以解决现有的基于粒子滤波的多机器人协同定位精度较低的问题。本发明的技术要点包括:根据多机器人协同定位系统中不同机器人性能不同造成的观测信息可靠性不同,使用最优加权融合理论改进粒子滤波算法,求出使总体测距方差最小的多个观测值的权值,使得定位机器人对融合了系统中其他机器人观测信息计算出的定位结果有最优权值,从而计算出协同定位结果,进一步提高多机器人定位系统的定位精度和鲁棒性。本发明可应用于多机器人协同定位之中。
Description
技术领域
本发明涉及多机器人协同定位技术领域,具体涉及一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统。
背景技术
多机器人系统中,通过多机器人之间的相互观测和信息传递,可以提高单个机器人定位能力。当前,针对机器人观测信息和自身定位信息的融合,主要有基于卡尔曼滤波的多机器人协同定位算法、基于粒子滤波的多机器人协同定位算法等。由于实际应用场景中,机器人运动方程和观测方程非线性,且噪声环境多变,不能保持高斯分布,基于卡尔曼滤波的多机器人协同定位算法对定位精度的改进有限,且容易引起滤波发散等问题;基于粒子滤波(Particle Filter,PF)的多机器人协同定位算法进行多机器人观测值融合时,存在对不同观测值使用平均加权导致观测置信度不可靠等问题。
发明内容
鉴于以上问题,本发明提出一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统,用以解决现有的基于粒子滤波的多机器人协同定位精度较低的问题。
根据本发明的一方面,提供一种基于最优加权改进粒子滤波的多机器人协同定位方法,该方法包括以下步骤:
步骤一、建立基于距离观测的多机器人协同定位系统中机器人的运动状态方程和观测方程;
步骤二、在所述机器人的运动状态方程和观测方程中,利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位。
进一步地,步骤二中利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位的具体步骤包括:
步骤二一、初始化粒子群和每个粒子群的权值;
步骤二二、对于每个采样时刻,按照下述过程进行迭代计算,以获得每个机器人的状态;具体包括:
步骤二二一、将上一个采样时刻获得的粒子群代入运动状态方程,以更新该采样时刻的粒子群;
步骤二二二、将上一个采样时刻获得的粒子群代入观测方程,计算观测值的预测值;
步骤二二三、根据该采样时刻的观测值,计算机器人间的测距方差;
步骤二二四、计算该采样时刻的预测值与观测值之间的偏差绝对值;
步骤二二五、根据偏差绝对值计算粒子群的重要性权值,并将其归一化;
步骤二二六、采用随机重采样算法进行粒子群重采样;
步骤二二七、计算单个粒子群的状态输出;
步骤二三、构造辅助函数,计算使得测距总方差最小的权值;
步骤二四、基于测距总方差最小的权值,对每个粒子群的状态进行最优加权融合,获得多个机器人的定位结果。
进一步地,步骤二二六中采用随机重采样算法进行粒子群重采样的具体步骤包括:产生[0,1]上均匀分布随机数组;计算粒子权重累积函数;计算归一化权值且对粒子群进行复制和淘汰,得到用于状态估计的粒子索引;重新设置粒子群权值。
进一步地,步骤二三中测距总方差为则使得测距总方差最小的权值为:
其中,表示机器人Rj的测距方差;N表示粒子数。
进一步地,步骤二四中多个机器人的定位结果中每个机器人Ri在k时刻的定位结果为:
其中,i≠j,表示除了机器人Ri以外的其他机器人的状态;M-1表示粒子群总数。
进一步地,步骤一中所述机器人的运动状态方程为:
Xk=ΦXk-1+ΓWk
其中,Xk-1表示k-1时刻的状态;Φ表示状态转移矩阵;Γ表示噪声驱动矩阵;Wk表示过程噪声;
基于距离观测的观测方程为:机器人Rj在k时刻以机器人Ri为参考坐标的观测方程为:
其中,表示机器人Ri在k时刻的位置坐标;/>表示机器人Rj在k时刻的位置坐标;vi表示机器人Ri的测量噪声方差。
根据本发明的另一方面,提供一种基于最优加权改进粒子滤波的多机器人协同定位系统,该系统包括:
系统方程构建模块,其配置成建立基于距离观测的多机器人协同定位系统中机器人的运动状态方程和观测方程;
协同定位模块,其配置成在所述机器人的运动状态方程和观测方程中,利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位。
进一步地,所述协同定位模块中利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位的具体步骤包括:
步骤二一、初始化粒子群和每个粒子群的权值;
步骤二二、对于每个采样时刻,按照下述过程进行迭代计算,以获得每个机器人的状态;具体包括:
步骤二二一、将上一个采样时刻获得的粒子群代入运动状态方程,以更新该采样时刻的粒子群;
步骤二二二、将上一个采样时刻获得的粒子群代入观测方程,计算观测值的预测值;
步骤二二三、根据该采样时刻的观测值,计算机器人间的测距方差;
步骤二二四、计算该采样时刻的预测值与观测值之间的偏差绝对值;
步骤二二五、根据偏差绝对值计算粒子群的重要性权值,并将其归一化;
步骤二二六、采用随机重采样算法进行粒子群重采样,具体包括:产生[0,1]上均匀分布随机数组;计算粒子权重累积函数;计算归一化权值且对粒子群进行复制和淘汰,得到用于状态估计的粒子索引;重新设置粒子群权值;
步骤二二七、计算单个粒子群的状态输出;
步骤二三、构造辅助函数,计算使得测距总方差最小的权值为:
其中,表示机器人Rj的测距方差;N表示粒子数;
步骤二四、基于测距总方差最小的权值,对每个粒子群的状态进行最优加权融合,获得多个机器人的定位结果;其中每个机器人Ri在k时刻的定位结果为:
其中,i≠j,表示除了机器人Ri以外的其他机器人的状态;M-1表示粒子群总数。
进一步地,所述系统方程构建模块中所述机器人的运动状态方程为:
Xk=ΦXk-1+ΓWk
其中,Xk-1表示k-1时刻的状态;Φ表示状态转移矩阵;Γ表示噪声驱动矩阵;Wk表示过程噪声;
基于距离观测的观测方程为:机器人Rj在k时刻以机器人Ri为参考坐标的观测方程为:
其中,表示机器人Ri在k时刻的位置坐标;/>表示机器人Rj在k时刻的位置坐标;vi表示机器人Ri的测量噪声方差。
本发明的有益技术效果是:
本发明提出一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统,根据多机器人协同定位系统中不同机器人性能不同造成的观测信息可靠性不同,使用最优加权融合理论改进PF,求出使总体估计方差最小的多个观测值的权值,使得定位机器人对融合了系统中其他机器人观测信息计算出的定位结果有最优权值,从而计算出协同定位结果,进一步提高多机器人定位系统的定位精度和鲁棒性。
附图说明
本发明可以通过参考下文中结合附图所给出的描述而得到更好的理解,所述附图连同下面的详细说明一起包含在本说明书中并且形成本说明书的一部分,而且用来进一步举例说明本发明的优选实施例和解释本发明的原理和优点。
图1是本发明实施例一种基于最优加权改进粒子滤波的多机器人协同定位方法的流程图;
图2是本发明实施例实验仿真过程中本发明方法和基于原始PF的多机器人协同定位方法对六机器人组成的多机器人系统的定位轨迹图;
图3是本发明实施例实验仿真过程中本发明方法与基于原始PF的多机器人协同定位方法对多机器人定位跟踪误差对比图;其中,(a)对应机器人1-3;(b)对应机器人4-6;
图4是本发明实施例实验仿真过程中本发明方法与基于原始PF的多机器人协同定位方法的迭代时间比较图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案,在下文中将结合附图对本发明的示范性实施方式或实施例进行描述。显然,所描述的实施方式或实施例仅仅是本发明一部分的实施方式或实施例,而不是全部的。基于本发明中的实施方式或实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式或实施例,都应当属于本发明保护的范围。
本发明实施例提出一种基于最优加权改进粒子滤波的多机器人协同定位方法,如图1所示,该方法包括以下步骤:
步骤一:建立基于距离观测的多机器人协同定位系统中机器人的运动状态方程和观测方程为:
其中,在本发明中设置状态方程为:
Xk=ΦXk-1+ΓWk (2)
则k时刻机器人Ri,i∈[1,M](M为系统中机器人个数)的状态为其中/>分别表示k时刻机器人Ri在定位系统坐标系(即多机器人协同定位系统中的全局定位坐标系)中的x轴坐标、y轴坐标、沿x轴运动速度,沿y轴运动速度;状态转移矩阵Φ、噪声驱动矩阵Γ为:
过程噪声Wk的均值为0,方差为:
其中,ω是一个可调参数,且ω<<1。
系统观测方程为:
Zk=HXk+Vk
其中,H为观测矩阵,Vk为均值为0、噪声方差为R的观测噪声。
本发明实施例中使用测距信息作为观测,机器人Rj,j∈[1,M]在k时刻以Ri,{1≤i≤M|i≠j}为参考坐标时,其观测方程为:
其中,为机器人Ri在k时刻的位置坐标;/>为机器人Rj在k时刻的位置坐标;vi为机器人Ri的测量噪声方差,且由于多机器人系统中不同机器人的性能存在差异,不同机器人的测距噪声方差不同。
步骤二:粒子群初始化;
根据本发明实施例,初始化M-1个粒子群,其中,每个粒子群都是在k=0时刻从机器人初始状态中随机生成N个粒子所得,也即,将每个粒子的权值初始化为/>其中,/>为第i个粒子集的初始状态。
步骤三:粒子集合采样;
根据本发明实施例,在每个采样时刻,将粒子集带入状态方程Xk=f(Xk-1,Wk),更新粒子集/>
步骤四:更新粒子重要性权值;
根据本发明实施例,具体包括以下步骤:
a)在每个采样时刻,将粒子集带入观测方程Zk=h(Xk,Vk),计算观测值的预测值/>
b)假设测量系统采集到观测值使用方差计算函数var(·)统计计算每个采样时刻的测距方差/>(机器人j对机器人i的测距方差),以便在步骤七最优加权融合所用:
c)计算粒子集合的观测预测与测量值之间的偏差绝对值:
d)将观测预测与测量值之间的偏差绝对值带入标准高斯分布计算粒子的重要性权值:
e)粒子权重归一化的结果为:
步骤五:粒子集合重采样;
根据本发明实施例,采用随机重采样算法进行粒子集重采样,主要步骤如下:
a)产生[0,1]上均匀分布随机数组其中N为粒子数;
b)计算粒子权重累积函数cdf,且满足
c)计算归一化权值且对粒子集合/>进行复制和淘汰,得到用于状态估计的粒子索引;
d)重新设置粒子权值
步骤六:计算单个粒子集的状态输出;
根据本发明实施例,根据重采样得到的索引挑选对应的粒子,重构的集合即是滤波后的状态集合,其中/>为在k时刻粒子群i中选取索引为outindex的粒子状态。使用均值计算函数mean(·)计算此粒子集的输出,
步骤七:多粒子集最优加权融合;
根据本发明实施例,最优加权融合算法需要求得使得测距总方差最小的权重Wj。根据拉格朗日乘数法,构造辅助函数:
其中λ为待定常数,计算得到:
从而得到观测值的权重为:
其中为步骤四中通过重复的距离观测,计算统计方差得到。
在本发明中,由于每个机器人的定位性能不同,且不同机器人间的观测方差不同,因此测距统计方差不同,机器人协同定位的精度受不同机器人观测影响。因此,通过最优加权融合理论改进粒子滤波,将机器人i维护的M-1个粒子集的输出进行融合,则机器人i在k时刻经该算法定位计算后的定位结果为:
进一步通过实验验证本发明的技术效果。
如图2所示,图2所示为多机器人系统中6个机器人的轨迹跟踪,有如下初始设置:
设置R1的初始状态为:X1(:,1)=[0,T,0,0]';
设置R2的初始状态为:X2(:,1)=[0,T,0,sqrt(3)/T+0.1*randn]';
设置R3的初始状态为:X3(:,1)=[0,-T,0,sqrt(3)/T+0.1*randn]';
设置R4的初始状态为:X4(:,1)=[0,-T,0,0]';
设置R5的初始状态为:X5(:,1)=[0,-T,0,-sqrt(3)/T+0.1*randn]';
设置R6的初始状态为:X6(:,1)=[0,T,0,-sqrt(3)/T+0.1*randn]';
设置高精度定位机器人R1的观测噪声标准差为0.1m,低精度定位机器人R2、R3、R4、R5、R6的观测噪声标准差均为4m;采样间隔1s,运行时间为30s;粒子滤波算法中粒子数为500。分别使用原始PF和本发明方法进行观测数据融合。
依据图2的仿真场景,针对系统中6个机器人使用两种算法的轨迹跟踪效果进行对比,如图3所示。进一步地,为了定量分析本发明方法的优势,使用两种算法的平均定位均方根误差计算如表1所示:
表1两种算法的均方根位置误差对比
根据表1计算定位误差降低的百分比,则机器人R1、R2、R3、R4、R5、R6的定位误差分别降低了定位误差:8.139%,36.870%,0.818%,39.774%,6.003%,66.778%,机器人系统中所有机器人的定位误差平均降低了26.38%。由此可见,本发明方法高于原始基于PF的协同定位算法,此外,低精度机器人R4,R5的定位精度优于高精度机器人R1,机器人R6定位精度与机器人R1相当。这得益于使用最优加权理论改进多机器人PF协同定位算法,使得PF算法在后验均值估计时对维护的M-1个粒子集均值赋予了具有反映测距可靠性的权值,对多个粒子集的定位结果进行了精确的融合。
在算法效率方面,从理论上分析,本发明方法相比于使用平均加权的基于PF的多机器人协同定位算法增加了统计计算机器人观测噪声方差从而计算最优权值的步骤。
依据图2的仿真场景,对每个机器人统计计算50次测距结果计算方差,则得到图4为对6个机器人分别使用普通PF与本发明提出的改进PF的每一步迭代时间进行统计,计算每一步的运行时间的均值并做图,且两种算法在机器人运动过程中每次定位耗时如表2所示:
表2仿真场景中算法运行时间
由表2可以计算得出:相对于原始PF,本发明提出的基于最优加权改进PF的多机器人协同定位方法由于在每个采样节点进行了机器人间的测距统计计算,统计测距方差来计算不同粒子群的权值,因此不可避免的消耗了一部分时间,在每次迭代时平均多消耗了0.02s,机器人系统采用分布式结构以及计算能力提升时,这部分消耗在接受范围内。
本发明另一实施例还提供一种基于最优加权改进粒子滤波的多机器人协同定位系统,该系统包括:
系统方程构建模块,其配置成建立基于距离观测的多机器人协同定位系统中机器人的运动状态方程和观测方程;
协同定位模块,其配置成在机器人的运动状态方程和观测方程中,利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位。
本实施例中,优选地,协同定位模块中利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位的具体步骤包括:
步骤二一、初始化粒子群和每个粒子群的权值;
步骤二二、对于每个采样时刻,按照下述过程进行迭代计算,以获得每个机器人的状态;具体包括:
步骤二二一、将上一个采样时刻获得的粒子群代入运动状态方程,以更新该采样时刻的粒子群;
步骤二二二、将上一个采样时刻获得的粒子群代入观测方程,计算观测值的预测值;
步骤二二三、根据该采样时刻的观测值,计算机器人间的测距方差;
步骤二二四、计算该采样时刻的预测值与观测值之间的偏差绝对值;
步骤二二五、根据偏差绝对值计算粒子群的重要性权值,并将其归一化;
步骤二二六、采用随机重采样算法进行粒子群重采样,具体包括:产生[0,1]上均匀分布随机数组;计算粒子权重累积函数;计算归一化权值且对粒子群进行复制和淘汰,得到用于状态估计的粒子索引;重新设置粒子群权值;
步骤二二七、计算单个粒子群的状态输出;
步骤二三、构造辅助函数,计算使得测距总方差最小的权值为:
其中,表示机器人Rj的测距方差;N表示粒子数;
步骤二四、基于测距总方差最小的权值,对每个粒子群的状态进行最优加权融合,获得多个机器人的定位结果;其中每个机器人Ri在k时刻的定位结果为:
其中,i≠j,表示除了机器人Ri以外的其他机器人的状态;M-1表示粒子群总数。
本实施例中,优选地,系统方程构建模块中机器人的运动状态方程为:
Xk=ΦXk-1+ΓWk
其中,Xk-1表示k-1时刻的状态;Φ表示状态转移矩阵;Γ表示噪声驱动矩阵;Wk表示过程噪声;
基于距离观测的观测方程为:机器人Rj在k时刻以机器人Ri为参考坐标的观测方程为:
其中,表示机器人Ri在k时刻的位置坐标;/>表示机器人Rj在k时刻的位置坐标;vi表示机器人Ri的测量噪声方差。
本发明实施例一种基于最优加权改进粒子滤波的多机器人协同定位系统的功能可以由前述一种基于最优加权改进粒子滤波的多机器人协同定位方法说明,因此系统实施例未详述部分,可参见以上方法实施例,在此不再赘述。
尽管根据有限数量的实施例描述了本发明,但是受益于上面的描述,本技术领域内的技术人员明白,在由此描述的本发明的范围内,可以设想其它实施例。对于本发明的范围,对本发明所做的公开是说明性的,而非限制性的,本发明的范围由所附权利要求书限定。
Claims (6)
1.一种基于最优加权改进粒子滤波的多机器人协同定位方法,其特征在于,包括以下步骤:
步骤一、建立基于距离观测的多机器人协同定位系统中机器人的运动状态方程和观测方程;
步骤二、在所述机器人的运动状态方程和观测方程中,利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位;具体步骤包括:
步骤二一、初始化粒子群和每个粒子群的权值;
步骤二二、对于每个采样时刻,按照下述过程进行迭代计算,以获得每个机器人的状态;具体包括:
步骤二二一、将上一个采样时刻获得的粒子群代入运动状态方程,以更新该采样时刻的粒子群;
步骤二二二、将上一个采样时刻获得的粒子群代入观测方程,计算观测值的预测值;
步骤二二三、根据该采样时刻的观测值,计算机器人间的测距方差;
步骤二二四、计算该采样时刻的预测值与观测值之间的偏差绝对值;
步骤二二五、根据偏差绝对值计算粒子群的重要性权值,并将其归一化;
步骤二二六、采用随机重采样算法进行粒子群重采样;
步骤二二七、计算单个粒子群的状态输出;
步骤二三、构造辅助函数,计算获得每个粒子群的使得测距总方差最小的权值;所述测距总方差为则使得测距总方差最小的权值为:
其中,表示机器人Rj的测距方差;N表示粒子数;
步骤二四、基于测距总方差最小的权值,对每个粒子群的状态进行最优加权融合,获得多个机器人的定位结果。
2.根据权利要求1所述的一种基于最优加权改进粒子滤波的多机器人协同定位方法,其特征在于,步骤二二六中采用随机重采样算法进行粒子群重采样的具体步骤包括:产生[0,1]上均匀分布随机数组;计算粒子权重累积函数;计算归一化权值且对粒子群进行复制和淘汰,得到用于状态估计的粒子索引;重新设置粒子群权值。
3.根据权利要求1所述的一种基于最优加权改进粒子滤波的多机器人协同定位方法,其特征在于,步骤二四中多个机器人的定位结果中每个机器人Ri在k时刻的定位结果为:
其中,i≠j,表示除了机器人Ri以外的其他机器人的状态;M-1表示粒子群总数。
4.根据权利要求1所述的一种基于最优加权改进粒子滤波的多机器人协同定位方法,其特征在于,步骤一中所述机器人的运动状态方程为:
Xk=ΦXk-1+ΓWk
其中,Xk-1表示k-1时刻的状态;Φ表示状态转移矩阵;Γ表示噪声驱动矩阵;Wk表示过程噪声;
基于距离观测的观测方程为:机器人Rj在k时刻以机器人Ri为参考坐标的观测方程为:
其中,表示机器人Ri在k时刻的位置坐标;/>表示机器人Rj在k时刻的位置坐标;vi表示机器人Ri的测量噪声方差。
5.一种基于最优加权改进粒子滤波的多机器人协同定位系统,其特征在于,包括:
系统方程构建模块,其配置成建立基于距离观测的多机器人协同定位系统中机器人的运动状态方程和观测方程;
协同定位模块,其配置成在所述机器人的运动状态方程和观测方程中,利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位;定位的具体步骤包括:
步骤二一、初始化粒子群和每个粒子群的权值;
步骤二二、对于每个采样时刻,按照下述过程进行迭代计算,以获得每个机器人的状态;具体包括:
步骤二二一、将上一个采样时刻获得的粒子群代入运动状态方程,以更新该采样时刻的粒子群;
步骤二二二、将上一个采样时刻获得的粒子群代入观测方程,计算观测值的预测值;
步骤二二三、根据该采样时刻的观测值,计算机器人间的测距方差;
步骤二二四、计算该采样时刻的预测值与观测值之间的偏差绝对值;
步骤二二五、根据偏差绝对值计算粒子群的重要性权值,并将其归一化;
步骤二二六、采用随机重采样算法进行粒子群重采样,具体包括:产生[0,1]上均匀分布随机数组;计算粒子权重累积函数;计算归一化权值且对粒子群进行复制和淘汰,得到用于状态估计的粒子索引;重新设置粒子群权值;
步骤二二七、计算单个粒子群的状态输出;
步骤二三、构造辅助函数,计算使得测距总方差最小的权值为:
其中,表示机器人Rj的测距方差;N表示粒子数;
步骤二四、基于测距总方差最小的权值,对每个粒子群的状态进行最优加权融合,获得多个机器人的定位结果;其中每个机器人Ri在k时刻的定位结果为:
其中,i≠j,表示除了机器人Ri以外的其他机器人的状态;M-1表示粒子群总数。
6.根据权利要求5所述的一种基于最优加权改进粒子滤波的多机器人协同定位系统,其特征在于,所述系统方程构建模块中所述机器人的运动状态方程为:
Xk=ΦXk-1+ΓWk
其中,Xk-1表示k-1时刻的状态;Φ表示状态转移矩阵;Γ表示噪声驱动矩阵;Wk表示过程噪声;
基于距离观测的观测方程为:机器人Rj在k时刻以机器人Ri为参考坐标的观测方程为:
其中,表示机器人Ri在k时刻的位置坐标;/>表示机器人Rj在k时刻的位置坐标;vi表示机器人Ri的测量噪声方差。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210677851.4A CN115077529B (zh) | 2022-06-16 | 2022-06-16 | 一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210677851.4A CN115077529B (zh) | 2022-06-16 | 2022-06-16 | 一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115077529A CN115077529A (zh) | 2022-09-20 |
CN115077529B true CN115077529B (zh) | 2024-04-26 |
Family
ID=83253835
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210677851.4A Active CN115077529B (zh) | 2022-06-16 | 2022-06-16 | 一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115077529B (zh) |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107084714A (zh) * | 2017-04-29 | 2017-08-22 | 天津大学 | 一种基于RoboCup3D的多机器人协作目标定位方法 |
CN108489498A (zh) * | 2018-06-15 | 2018-09-04 | 哈尔滨工程大学 | 一种基于最大互相关熵无迹粒子滤波的auv协同导航方法 |
CN109084767A (zh) * | 2018-06-15 | 2018-12-25 | 哈尔滨工程大学 | 一种最大互相关熵自适应容积粒子滤波的auv协同导航方法 |
CN110178046A (zh) * | 2017-01-24 | 2019-08-27 | 深圳市大疆创新科技有限公司 | 用于无人机的基于雷达的避障系统和方法 |
CN110455287A (zh) * | 2019-07-24 | 2019-11-15 | 南京理工大学 | 自适应无迹卡尔曼粒子滤波方法 |
CN110933599A (zh) * | 2019-12-17 | 2020-03-27 | 北京理工大学 | 一种融合uwb与wifi指纹的自适应定位方法 |
CN112039496A (zh) * | 2020-09-10 | 2020-12-04 | 南京航空航天大学 | 一种基于人工鱼群优化的快速高斯粒子滤波数据融合方法 |
CN112584306A (zh) * | 2020-11-27 | 2021-03-30 | 巢湖学院 | 一种基于卡尔曼滤波的室内机器人定位算法 |
-
2022
- 2022-06-16 CN CN202210677851.4A patent/CN115077529B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110178046A (zh) * | 2017-01-24 | 2019-08-27 | 深圳市大疆创新科技有限公司 | 用于无人机的基于雷达的避障系统和方法 |
CN107084714A (zh) * | 2017-04-29 | 2017-08-22 | 天津大学 | 一种基于RoboCup3D的多机器人协作目标定位方法 |
CN108489498A (zh) * | 2018-06-15 | 2018-09-04 | 哈尔滨工程大学 | 一种基于最大互相关熵无迹粒子滤波的auv协同导航方法 |
CN109084767A (zh) * | 2018-06-15 | 2018-12-25 | 哈尔滨工程大学 | 一种最大互相关熵自适应容积粒子滤波的auv协同导航方法 |
CN110455287A (zh) * | 2019-07-24 | 2019-11-15 | 南京理工大学 | 自适应无迹卡尔曼粒子滤波方法 |
CN110933599A (zh) * | 2019-12-17 | 2020-03-27 | 北京理工大学 | 一种融合uwb与wifi指纹的自适应定位方法 |
CN112039496A (zh) * | 2020-09-10 | 2020-12-04 | 南京航空航天大学 | 一种基于人工鱼群优化的快速高斯粒子滤波数据融合方法 |
CN112584306A (zh) * | 2020-11-27 | 2021-03-30 | 巢湖学院 | 一种基于卡尔曼滤波的室内机器人定位算法 |
Non-Patent Citations (3)
Title |
---|
Yang Shao,et al.A multi-AUV cooperative navigation method.《PHM 2021》.2021,全文. * |
基于粒子滤波的智能机器人定位算法;章弘凯;陈年生;范光宇;;计算机应用与软件;20200212(第02期);全文 * |
平方根容积卡尔曼滤波在移动机器人SLAM中的应用;康轶非;宋永端;宋宇;闫德立;李丹勇;;机器人;20130315(第02期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN115077529A (zh) | 2022-09-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108896047B (zh) | 分布式传感器网络协同融合与传感器位置修正方法 | |
CN106599368B (zh) | 基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法 | |
CN111161179A (zh) | 一种基于法向量的点云平滑光顺滤波方法 | |
US20220120815A1 (en) | Method, apparatus, device and medium for estimating performance parameters of a battery | |
CN110969649A (zh) | 一种激光点云与地图的匹配评价方法、介质、终端和装置 | |
Pan et al. | Voxfield: Non-projective signed distance fields for online planning and 3d reconstruction | |
CN111707279B (zh) | 一种激光点云与地图的匹配评价方法、介质、终端和装置 | |
CN113701742B (zh) | 一种基于云端与边端融合计算的移动机器人slam方法 | |
CN109460539B (zh) | 一种基于简化容积粒子滤波的目标定位方法 | |
CN110763245A (zh) | 一种基于流式计算的地图创建方法及其系统 | |
CN112954594A (zh) | 一种基于人工蜂群的无线传感器网络节点定位算法 | |
CN115077529B (zh) | 一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统 | |
WO2022036981A1 (zh) | 机器人及其地图构建方法和装置 | |
CN109253727A (zh) | 一种基于改进迭代容积粒子滤波算法的定位方法 | |
CN110146084B (zh) | 面向卫星故障的多星编队系统分布式协同导航滤波方法 | |
CN114993341B (zh) | 一种基于天基测量的运载火箭弹道估计方法及装置 | |
CN116819975A (zh) | 一种基于纯角度观测的多目标几何中心估计方法 | |
CN111397607A (zh) | 一种采用并行融合机制的信息滤波方法 | |
US7013244B2 (en) | Method and system for estimation of quantities corrupted by noise and use of estimates in decision making | |
CN115906717A (zh) | 一种水土保持的侵蚀计算方法及系统 | |
CN116224070A (zh) | 一种基于ocv-soc模型的全温度锂离子电池ocv评估方法及系统 | |
JP7513391B2 (ja) | 二次電池管理システム及び二次電池管理方法 | |
Wang et al. | A fast, accurate, and scalable probabilistic sample-based approach for counting swarm size | |
CN105825267A (zh) | 一种基于pso-pgsa的室内定位方法 | |
CN113432608A (zh) | 适于ins/cns组合导航系统的基于最大相关熵的广义高阶ckf算法 |
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 |