CN115077529A - 一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统 - Google Patents

Abstract

本发明提出一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统，涉及多机器人协同定位技术领域，用以解决现有的基于粒子滤波的多机器人协同定位精度较低的问题。本发明的技术要点包括：根据多机器人协同定位系统中不同机器人性能不同造成的观测信息可靠性不同，使用最优加权融合理论改进粒子滤波算法，求出使总体测距方差最小的多个观测值的权值，使得定位机器人对融合了系统中其他机器人观测信息计算出的定位结果有最优权值，从而计算出协同定位结果，进一步提高多机器人定位系统的定位精度和鲁棒性。本发明可应用于多机器人协同定位之中。

Description

一种基于最优加权改进粒子滤波的多机器人协同定位方法及 系统

技术领域

本发明涉及多机器人协同定位技术领域，具体涉及一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统。

背景技术

多机器人系统中，通过多机器人之间的相互观测和信息传递，可以提高单个机器人定位能力。当前，针对机器人观测信息和自身定位信息的融合，主要有基于卡尔曼滤波的多机器人协同定位算法、基于粒子滤波的多机器人协同定位算法等。由于实际应用场景中，机器人运动方程和观测方程非线性，且噪声环境多变，不能保持高斯分布，基于卡尔曼滤波的多机器人协同定位算法对定位精度的改进有限，且容易引起滤波发散等问题；基于粒子滤波(Particle Filter,PF)的多机器人协同定位算法进行多机器人观测值融合时，存在对不同观测值使用平均加权导致观测置信度不可靠等问题。

发明内容

鉴于以上问题，本发明提出一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统，用以解决现有的基于粒子滤波的多机器人协同定位精度较低的问题。

根据本发明的一方面，提供一种基于最优加权改进粒子滤波的多机器人协同定位方法，该方法包括以下步骤：

步骤一、建立基于距离观测的多机器人协同定位系统中机器人的运动状态方程和观测方程；

步骤二、在所述机器人的运动状态方程和观测方程中，利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位。

进一步地，步骤二中利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位的具体步骤包括：

步骤二一、初始化粒子群和每个粒子群的权值；

步骤二二、对于每个采样时刻，按照下述过程进行迭代计算，以获得每个机器人的状态；具体包括：

步骤二二一、将上一个采样时刻获得的粒子群代入运动状态方程，以更新该采样时刻的粒子群；

步骤二二二、将上一个采样时刻获得的粒子群代入观测方程，计算观测值的预测值；

步骤二二三、根据该采样时刻的观测值，计算机器人间的测距方差；

步骤二二四、计算该采样时刻的预测值与观测值之间的偏差绝对值；

步骤二二五、根据偏差绝对值计算粒子群的重要性权值，并将其归一化；

步骤二二六、采用随机重采样算法进行粒子群重采样；

步骤二二七、计算单个粒子群的状态输出；

步骤二三、构造辅助函数，计算使得测距总方差最小的权值；

步骤二四、基于测距总方差最小的权值，对每个粒子群的状态进行最优加权融合，获得多个机器人的定位结果。

进一步地，步骤二二六中采用随机重采样算法进行粒子群重采样的具体步骤包括：产生[0,1]上均匀分布随机数组；计算粒子权重累积函数；计算归一化权值且对粒子群进行复制和淘汰，得到用于状态估计的粒子索引；重新设置粒子群权值。

进一步地，步骤二三中测距总方差为

则使得测距总方差最小的权值为：

其中，

表示机器人R_j的测距方差；N表示粒子数。

进一步地，步骤二四中多个机器人的定位结果中每个机器人R_i在k时刻的定位结果为：

其中，i≠j，

表示除了机器人R_i以外的其他机器人的状态；M-1表示粒子群总数。

进一步地，步骤一中所述机器人的运动状态方程为：

X_k＝ΦX_k-1+ΓW_k

其中，X_k-1表示k-1时刻的状态；Φ表示状态转移矩阵；Γ表示噪声驱动矩阵；W_k表示过程噪声；

基于距离观测的观测方程为：机器人R_j在k时刻以机器人R_i为参考坐标的观测方程为：

其中，

表示机器人R_i在k时刻的位置坐标；

表示机器人R_j在k时刻的位置坐标；vⁱ表示机器人R_i的测量噪声方差。

根据本发明的另一方面，提供一种基于最优加权改进粒子滤波的多机器人协同定位系统，该系统包括：

系统方程构建模块，其配置成建立基于距离观测的多机器人协同定位系统中机器人的运动状态方程和观测方程；

协同定位模块，其配置成在所述机器人的运动状态方程和观测方程中，利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位。

进一步地，所述协同定位模块中利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位的具体步骤包括：

步骤二一、初始化粒子群和每个粒子群的权值；

步骤二二、对于每个采样时刻，按照下述过程进行迭代计算，以获得每个机器人的状态；具体包括：

步骤二二一、将上一个采样时刻获得的粒子群代入运动状态方程，以更新该采样时刻的粒子群；

步骤二二二、将上一个采样时刻获得的粒子群代入观测方程，计算观测值的预测值；

步骤二二三、根据该采样时刻的观测值，计算机器人间的测距方差；

步骤二二四、计算该采样时刻的预测值与观测值之间的偏差绝对值；

步骤二二五、根据偏差绝对值计算粒子群的重要性权值，并将其归一化；

步骤二二六、采用随机重采样算法进行粒子群重采样，具体包括：产生[0,1]上均匀分布随机数组；计算粒子权重累积函数；计算归一化权值且对粒子群进行复制和淘汰，得到用于状态估计的粒子索引；重新设置粒子群权值；

步骤二二七、计算单个粒子群的状态输出；

步骤二三、构造辅助函数，计算使得测距总方差

最小的权值为：

其中，

表示机器人R_j的测距方差；N表示粒子数；

步骤二四、基于测距总方差最小的权值，对每个粒子群的状态进行最优加权融合，获得多个机器人的定位结果；其中每个机器人R_i在k时刻的定位结果为：

其中，i≠j，

表示除了机器人R_i以外的其他机器人的状态；M-1表示粒子群总数。

进一步地，所述系统方程构建模块中所述机器人的运动状态方程为：

X_k＝ΦX_k-1+ΓW_k

其中，X_k-1表示k-1时刻的状态；Φ表示状态转移矩阵；Γ表示噪声驱动矩阵；W_k表示过程噪声；

基于距离观测的观测方程为：机器人R_j在k时刻以机器人R_i为参考坐标的观测方程为：

其中，

表示机器人R_i在k时刻的位置坐标；

表示机器人R_j在k时刻的位置坐标；vⁱ表示机器人R_i的测量噪声方差。

本发明的有益技术效果是：

本发明提出一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统，根据多机器人协同定位系统中不同机器人性能不同造成的观测信息可靠性不同，使用最优加权融合理论改进PF，求出使总体估计方差最小的多个观测值的权值，使得定位机器人对融合了系统中其他机器人观测信息计算出的定位结果有最优权值，从而计算出协同定位结果，进一步提高多机器人定位系统的定位精度和鲁棒性。

附图说明

本发明可以通过参考下文中结合附图所给出的描述而得到更好的理解，所述附图连同下面的详细说明一起包含在本说明书中并且形成本说明书的一部分，而且用来进一步举例说明本发明的优选实施例和解释本发明的原理和优点。

图1是本发明实施例一种基于最优加权改进粒子滤波的多机器人协同定位方法的流程图；

图2是本发明实施例实验仿真过程中本发明方法和基于原始PF的多机器人协同定位方法对六机器人组成的多机器人系统的定位轨迹图；

图3是本发明实施例实验仿真过程中本发明方法与基于原始PF的多机器人协同定位方法对多机器人定位跟踪误差对比图；其中，(a)对应机器人1-3；(b)对应机器人4-6；

图4是本发明实施例实验仿真过程中本发明方法与基于原始PF的多机器人协同定位方法的迭代时间比较图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，在下文中将结合附图对本发明的示范性实施方式或实施例进行描述。显然，所描述的实施方式或实施例仅仅是本发明一部分的实施方式或实施例，而不是全部的。基于本发明中的实施方式或实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式或实施例，都应当属于本发明保护的范围。

本发明实施例提出一种基于最优加权改进粒子滤波的多机器人协同定位方法，如图1所示，该方法包括以下步骤：

步骤一：建立基于距离观测的多机器人协同定位系统中机器人的运动状态方程和观测方程为：

其中，在本发明中设置状态方程为：

X_k＝ΦX_k-1+ΓW_k (2)

则k时刻机器人R_i,i∈[1,M](M为系统中机器人个数)的状态为

其中

分别表示k时刻机器人R_i在定位系统坐标系(即多机器人协同定位系统中的全局定位坐标系)中的x轴坐标、y轴坐标、沿x轴运动速度，沿y轴运动速度；状态转移矩阵Φ、噪声驱动矩阵Γ为：

过程噪声W_k的均值为0，方差为：

其中，ω是一个可调参数，且ω＜＜1。

系统观测方程为：

Z_k＝HX_k+V_k

其中，H为观测矩阵，V_k为均值为0、噪声方差为R的观测噪声。

本发明实施例中使用测距信息作为观测，机器人R_j,j∈[1,M]在k时刻以R_i,{1≤i≤M|i≠j}为参考坐标时，其观测方程为：

其中，

为机器人R_i在k时刻的位置坐标；

为机器人R_j在k时刻的位置坐标；vⁱ为机器人R_i的测量噪声方差，且由于多机器人系统中不同机器人的性能存在差异，不同机器人的测距噪声方差不同。

步骤二：粒子群初始化；

根据本发明实施例，初始化M-1个粒子群，其中，每个粒子群都是在k＝0时刻从机器人初始状态中随机生成N个粒子

所得，也即，将每个粒子的权值初始化为

其中，

为第i个粒子集的初始状态。

步骤三：粒子集合采样；

根据本发明实施例，在每个采样时刻，将粒子集

带入状态方程X_k＝f(X_k-1,W_k)，更新粒子集

步骤四：更新粒子重要性权值；

根据本发明实施例，具体包括以下步骤：

a)在每个采样时刻，将粒子集

带入观测方程Z_k＝h(X_k,V_k)，计算观测值的预测值

b)假设测量系统采集到观测值

使用方差计算函数var(·)统计计算每个采样时刻的测距方差

(机器人j对机器人i的测距方差)，以便在步骤七最优加权融合所用：

c)计算粒子集合的观测预测与测量值之间的偏差绝对值：

d)将观测预测与测量值之间的偏差绝对值带入标准高斯分布计算粒子的重要性权值：

e)粒子权重归一化的结果为：

步骤五：粒子集合重采样；

根据本发明实施例，采用随机重采样算法进行粒子集重采样，主要步骤如下：

a)产生[0,1]上均匀分布随机数组

其中N为粒子数；

b)计算粒子权重累积函数cdf，且满足

c)计算归一化权值

且对粒子集合

进行复制和淘汰，得到用于状态估计的粒子索引；

d)重新设置粒子权值

步骤六：计算单个粒子集的状态输出；

根据本发明实施例，根据重采样得到的索引挑选对应的粒子，重构的集合

即是滤波后的状态集合，其中

为在k时刻粒子群i中选取索引为outindex的粒子状态。使用均值计算函数mean(·)计算此粒子集的输出，

步骤七：多粒子集最优加权融合；

根据本发明实施例，最优加权融合算法需要求得使得测距总方差

最小的权重W_j。根据拉格朗日乘数法，构造辅助函数：

其中λ为待定常数，计算得到：

从而得到观测值的权重为：

其中

为步骤四中通过重复的距离观测，计算统计方差得到。

在本发明中，由于每个机器人的定位性能不同，且不同机器人间的观测方差不同，因此测距统计方差不同，机器人协同定位的精度受不同机器人观测影响。因此，通过最优加权融合理论改进粒子滤波，将机器人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]'；

设置高精度定位机器人R₁的观测噪声标准差为0.1m，低精度定位机器人R₂、R₃、R₄、R₅、R₆的观测噪声标准差均为4m；采样间隔1s，运行时间为30s；粒子滤波算法中粒子数为500。分别使用原始PF和本发明方法进行观测数据融合。

依据图2的仿真场景，针对系统中6个机器人使用两种算法的轨迹跟踪效果进行对比，如图3所示。进一步地，为了定量分析本发明方法的优势，使用两种算法的平均定位均方根误差计算如表1所示：

表1两种算法的均方根位置误差对比

根据表1计算定位误差降低的百分比，则机器人R₁、R₂、R₃、R₄、R₅、R₆的定位误差分别降低了定位误差：8.139％，36.870％，0.818％，39.774％，6.003％，66.778％，机器人系统中所有机器人的定位误差平均降低了26.38％。由此可见，本发明方法高于原始基于PF的协同定位算法，此外，低精度机器人R₄，R₅的定位精度优于高精度机器人R₁，机器人R₆定位精度与机器人R₁相当。这得益于使用最优加权理论改进多机器人PF协同定位算法，使得PF算法在后验均值估计时对维护的M-1个粒子集均值赋予了具有反映测距可靠性的权值，对多个粒子集的定位结果进行了精确的融合。

在算法效率方面，从理论上分析，本发明方法相比于使用平均加权的基于PF的多机器人协同定位算法增加了统计计算机器人观测噪声方差从而计算最优权值的步骤。

依据图2的仿真场景，对每个机器人统计计算50次测距结果计算方差，则得到图4为对6个机器人分别使用普通PF与本发明提出的改进PF的每一步迭代时间进行统计，计算每一步的运行时间的均值并做图，且两种算法在机器人运动过程中每次定位耗时如表2所示：

表2仿真场景中算法运行时间

由表2可以计算得出：相对于原始PF，本发明提出的基于最优加权改进PF的多机器人协同定位方法由于在每个采样节点进行了机器人间的测距统计计算，统计测距方差来计算不同粒子群的权值，因此不可避免的消耗了一部分时间，在每次迭代时平均多消耗了0.02s，机器人系统采用分布式结构以及计算能力提升时，这部分消耗在接受范围内。

本发明另一实施例还提供一种基于最优加权改进粒子滤波的多机器人协同定位系统，该系统包括：

系统方程构建模块，其配置成建立基于距离观测的多机器人协同定位系统中机器人的运动状态方程和观测方程；

协同定位模块，其配置成在机器人的运动状态方程和观测方程中，利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位。

本实施例中，优选地，协同定位模块中利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位的具体步骤包括：

步骤二一、初始化粒子群和每个粒子群的权值；

步骤二二、对于每个采样时刻，按照下述过程进行迭代计算，以获得每个机器人的状态；具体包括：

步骤二二一、将上一个采样时刻获得的粒子群代入运动状态方程，以更新该采样时刻的粒子群；

步骤二二二、将上一个采样时刻获得的粒子群代入观测方程，计算观测值的预测值；

步骤二二三、根据该采样时刻的观测值，计算机器人间的测距方差；

步骤二二四、计算该采样时刻的预测值与观测值之间的偏差绝对值；

步骤二二五、根据偏差绝对值计算粒子群的重要性权值，并将其归一化；

步骤二二六、采用随机重采样算法进行粒子群重采样，具体包括：产生[0,1]上均匀分布随机数组；计算粒子权重累积函数；计算归一化权值且对粒子群进行复制和淘汰，得到用于状态估计的粒子索引；重新设置粒子群权值；

步骤二二七、计算单个粒子群的状态输出；

步骤二三、构造辅助函数，计算使得测距总方差

最小的权值为：

其中，

表示机器人R_j的测距方差；N表示粒子数；

步骤二四、基于测距总方差最小的权值，对每个粒子群的状态进行最优加权融合，获得多个机器人的定位结果；其中每个机器人R_i在k时刻的定位结果为：

其中，i≠j，

表示除了机器人R_i以外的其他机器人的状态；M-1表示粒子群总数。

本实施例中，优选地，系统方程构建模块中机器人的运动状态方程为：

X_k＝ΦX_k-1+ΓW_k

其中，X_k-1表示k-1时刻的状态；Φ表示状态转移矩阵；Γ表示噪声驱动矩阵；W_k表示过程噪声；

基于距离观测的观测方程为：机器人R_j在k时刻以机器人R_i为参考坐标的观测方程为：

其中，

表示机器人R_i在k时刻的位置坐标；

表示机器人R_j在k时刻的位置坐标；vⁱ表示机器人R_i的测量噪声方差。

本发明实施例一种基于最优加权改进粒子滤波的多机器人协同定位系统的功能可以由前述一种基于最优加权改进粒子滤波的多机器人协同定位方法说明，因此系统实施例未详述部分，可参见以上方法实施例，在此不再赘述。

尽管根据有限数量的实施例描述了本发明，但是受益于上面的描述，本技术领域内的技术人员明白，在由此描述的本发明的范围内，可以设想其它实施例。对于本发明的范围，对本发明所做的公开是说明性的，而非限制性的，本发明的范围由所附权利要求书限定。

Claims (9)

1.一种基于最优加权改进粒子滤波的多机器人协同定位方法，其特征在于，包括以下步骤：

步骤一、建立基于距离观测的多机器人协同定位系统中机器人的运动状态方程和观测方程；

步骤二、在所述机器人的运动状态方程和观测方程中，利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位。

2.根据权利要求1所述的一种基于最优加权改进粒子滤波的多机器人协同定位方法，其特征在于，步骤二中利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位的具体步骤包括：

步骤二一、初始化粒子群和每个粒子群的权值；

步骤二二、对于每个采样时刻，按照下述过程进行迭代计算，以获得每个机器人的状态；具体包括：

步骤二二一、将上一个采样时刻获得的粒子群代入运动状态方程，以更新该采样时刻的粒子群；

步骤二二二、将上一个采样时刻获得的粒子群代入观测方程，计算观测值的预测值；

步骤二二三、根据该采样时刻的观测值，计算机器人间的测距方差；

步骤二二四、计算该采样时刻的预测值与观测值之间的偏差绝对值；

步骤二二五、根据偏差绝对值计算粒子群的重要性权值，并将其归一化；

步骤二二六、采用随机重采样算法进行粒子群重采样；

步骤二二七、计算单个粒子群的状态输出；

步骤二三、构造辅助函数，计算获得每个粒子群的使得测距总方差最小的权值；

步骤二四、基于测距总方差最小的权值，对每个粒子群的状态进行最优加权融合，获得多个机器人的定位结果。

3.根据权利要求2所述的一种基于最优加权改进粒子滤波的多机器人协同定位方法，其特征在于，步骤二二六中采用随机重采样算法进行粒子群重采样的具体步骤包括：产生[0,1]上均匀分布随机数组；计算粒子权重累积函数；计算归一化权值且对粒子群进行复制和淘汰，得到用于状态估计的粒子索引；重新设置粒子群权值。

4.根据权利要求2或3所述的一种基于最优加权改进粒子滤波的多机器人协同定位方法，其特征在于，步骤二三中测距总方差为

则使得测距总方差最小的权值为：

其中，

表示机器人R_j的测距方差；N表示粒子数。

5.根据权利要求4所述的一种基于最优加权改进粒子滤波的多机器人协同定位方法，其特征在于，步骤二四中多个机器人的定位结果中每个机器人R_i在k时刻的定位结果为：

其中，i≠j，

表示除了机器人R_i以外的其他机器人的状态；M-1表示粒子群总数。

6.根据权利要求4所述的一种基于最优加权改进粒子滤波的多机器人协同定位方法，其特征在于，步骤一中所述机器人的运动状态方程为：

X_k＝ΦX_k-1+ΓW_k

其中，X_k-1表示k-1时刻的状态；Φ表示状态转移矩阵；Γ表示噪声驱动矩阵；W_k表示过程噪声；

基于距离观测的观测方程为：机器人R_j在k时刻以机器人R_i为参考坐标的观测方程为：

其中，

表示机器人R_i在k时刻的位置坐标；

表示机器人R_j在k时刻的位置坐标；vⁱ表示机器人R_i的测量噪声方差。

7.一种基于最优加权改进粒子滤波的多机器人协同定位系统，其特征在于，包括：

系统方程构建模块，其配置成建立基于距离观测的多机器人协同定位系统中机器人的运动状态方程和观测方程；

协同定位模块，其配置成在所述机器人的运动状态方程和观测方程中，利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位。

8.根据权利要求7所述的一种基于最优加权改进粒子滤波的多机器人协同定位系统，其特征在于，所述协同定位模块中利用改进的粒子滤波算法对多机器人协同定位系统中多个机器人进行定位的具体步骤包括：

步骤二一、初始化粒子群和每个粒子群的权值；

步骤二二、对于每个采样时刻，按照下述过程进行迭代计算，以获得每个机器人的状态；具体包括：

步骤二二一、将上一个采样时刻获得的粒子群代入运动状态方程，以更新该采样时刻的粒子群；

步骤二二二、将上一个采样时刻获得的粒子群代入观测方程，计算观测值的预测值；

步骤二二三、根据该采样时刻的观测值，计算机器人间的测距方差；

步骤二二四、计算该采样时刻的预测值与观测值之间的偏差绝对值；

步骤二二五、根据偏差绝对值计算粒子群的重要性权值，并将其归一化；

步骤二二六、采用随机重采样算法进行粒子群重采样，具体包括：产生[0,1]上均匀分布随机数组；计算粒子权重累积函数；计算归一化权值且对粒子群进行复制和淘汰，得到用于状态估计的粒子索引；重新设置粒子群权值；

步骤二二七、计算单个粒子群的状态输出；

步骤二三、构造辅助函数，计算使得测距总方差

最小的权值为：

其中，

表示机器人R_j的测距方差；N表示粒子数；

步骤二四、基于测距总方差最小的权值，对每个粒子群的状态进行最优加权融合，获得多个机器人的定位结果；其中每个机器人R_i在k时刻的定位结果为：

其中，i≠j，

表示除了机器人R_i以外的其他机器人的状态；M-1表示粒子群总数。

9.根据权利要求7或8所述的一种基于最优加权改进粒子滤波的多机器人协同定位系统，其特征在于，所述系统方程构建模块中所述机器人的运动状态方程为：

X_k＝ΦX_k-1+ΓW_k

其中，X_k-1表示k-1时刻的状态；Φ表示状态转移矩阵；Γ表示噪声驱动矩阵；W_k表示过程噪声；

基于距离观测的观测方程为：机器人R_j在k时刻以机器人R_i为参考坐标的观测方程为：

其中，

表示机器人R_i在k时刻的位置坐标；

表示机器人R_j在k时刻的位置坐标；vⁱ表示机器人R_i的测量噪声方差。

Images