一种基于移动机器人定位系统的分布式集员滤波方法

1.本发明涉及一种滤波方法，具体涉及一种加密解密机制下具有状态饱和的传感器网络下移动机器人定位系统的分布式集员滤波方法。


背景技术：

2.移动机器人系统是一个能够感知周围环境同时具备规划、决策及控制等功能的一类综合系统，在军事、农业、医疗、民航、物流仓储业等诸多领域均具有广泛应用前景。移动机器人系统的定位问题是当前研究的一个热点问题，处理好该问题有助于提高移动机器人的自动化水平。
3.由于信息技术飞速发展，为保证数据的安全性，在信号传输过程中引入加密解密机制来对数据进行调度可以有效避免数据遭到破坏，从而确保数据信号可以安全传输。同时，考虑到在实际情况中，移动机器人通常在有限空间内进行运动，例如室内移动机器人，其位置信息以及速度信息等均会受到约束，因而在系统建模时考虑状态饱和现象具有实际意义。
4.由于现有的滤波方法很少将状态饱和以及加密解密机制同时进行考虑，特别是针对未知但有界的系统噪声，已有滤波方法无法处理此类问题，进而导致滤波算法性能受到损害，估计精度降低等问题的产生。


技术实现要素：

5.为了解决基于移动机器人定位系统的加密解密机制下具有状态饱和的分布式集员滤波问题，本发明提供了一种基于移动机器人定位系统的分布式集员滤波方法。该方法可以更加真实地反应实际情况，且采用递推方法，易于在线执行及应用。
6.本发明的目的是通过以下技术方案实现的：
7.一种基于移动机器人定位系统的分布式集员滤波方法，包括如下步骤：
8.步骤一、基于笛卡尔坐标系，选取移动机器人定位系统的位置及方向信息作为状态变量，建立加密解密机制下具有状态饱和的移动机器人定位系统动态模型；
9.步骤二、对步骤一建立的移动机器人定位系统动态模型，在最小化滤波误差椭球域意义下设计分布式集员滤波器；
10.步骤三、计算传感器网络中每个节点在k时刻的中间矩阵p
i,k+1|k
；
11.步骤四、依据步骤三中获得的中间矩阵p
i,k+1|k
，计算每个传感器节点的滤波器增益矩阵
12.步骤五、根据步骤四中得到的滤波器增益矩阵将其代入至步骤二设计的分布式集员滤波器中，设计出第i个传感器节点在k+1时刻的分布式集员滤波器判断k+1是否达到总时长m，如果k+1＜m，则执行步骤六，若k+1≥m，则结束运行；
13.步骤六、根据步骤四中得到的每个传感器节点的滤波器增益矩阵计算每个
传感器节点的滤波误差受限矩阵p
i,k+1|k+1
；令k＝k+1，执行步骤二，直至满足k+1≥m。
14.相比于现有技术，本发明具有如下优点：
15.1、本发明同时考虑到加密解密机制以及状态饱和对滤波算法性能产生的影响，利用分布式集员滤波方法全面考虑了滤波误差受限矩阵的有效信息，与已有分布式滤波方法相比，本发明对移动机器人定位系统设计分布式集员滤波器的同时考虑到状态饱和及加密解密机制，完成了基于移动机器人定位系统的分布式集员滤波算法设计过程，提高滤波抗干扰能力并且该算法具有易于在线应用的特点。
16.2、本发明采用随机分析技术以及矩阵理论的相关知识，通过考虑滤波误差受限矩阵的可用信息得到受限矩阵的具体表达式，接下来，通过设计适当的分布式滤波器增益矩阵确保滤波误差位于椭球域内。以上方法保证了滤波误差椭球域的最小化，并完成了在状态饱和及加密解密机制同时发生的情况下滤波算法仍可保持较好性能。从仿真实验结果中可以看出量化区间长度的减小使得节点1的平均滤波误差降低了0.51，节点2的平均滤波误差降低了0.9，节点3的平均滤波误差降低了0.82，节点4的平均滤波误差降低了0.96。
17.3、本发明设计的分布式递推集员滤波算法可以有效估计出移动机器人定位系统的状态信息。
18.4、本发明设计的分布式递推集员滤波算法易于在线求解，解决了现有分布式滤波方法不能处理加密解密机制下具有状态饱和的传感器网络的分布式滤波问题。
附图说明
19.图1是本发明基于移动机器人定位系统的分布式集员滤波方法流程图；
20.图2是移动机器人定位系统实际状态轨迹的第一个分量及第一个节点对其的滤波和第二个节点对其的滤波
21.图3是移动机器人定位系统实际状态轨迹的第一个分量及第三个节点对其的滤波和第四个节点对其的滤波
22.图4是移动机器人定位系统实际状态轨迹的第二个分量及第一个节点对其的滤波和第二个节点对其的滤波
23.图5是移动机器人定位系统实际状态轨迹的第二个分量及第三个节点对其的滤波和第四个节点对其的滤波
24.图6是移动机器人定位系统实际状态轨迹的第三个分量及第一个节点对其的滤波和第二个节点对其的滤波
25.图7是移动机器人定位系统实际状态轨迹的第三个分量及第三个节点对其的滤波和第四个节点对其的滤波
26.图8是第一个传感器节点及第二个传感器节点的滤波误差以及受限矩阵的迹的轨迹图；
27.图9是第三个传感器节点及第四个传感器节点的滤波误差以及受限矩阵的迹的轨
迹图；
28.图10是第一个传感器节点及第二个传感器节点在不同量化区间长度下的滤波误差；
29.图11是第三个传感器节点及第四个传感器节点在不同量化区间长度下的滤波误差；
30.图2～图7中：“——”表示移动机器人定位系统的实际状态轨迹，表示基于第一个传感器节点的测量值对移动机器人定位系统的状态的滤波轨迹，表示基于第二个传感器节点的测量值对移动机器人定位系统的状态的滤波轨迹，表示基于第三个传感器节点的测量值对移动机器人定位系统的状态的滤波轨迹，表示基于第四个传感器节点的测量值对移动机器人定位系统的状态的滤波轨迹；
31.图8～图9中：“——”表示移动机器人定位系统的滤波误差，表示滤波误差的上界；
32.图10～图11中：表示d＝0.01下各节点的滤波误差，表示d＝0.05下各节点的滤波误差。
具体实施方式
33.下面结合附图对本发明的技术方案作进一步的说明，但并不局限于此，凡是对本发明技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，均应涵盖在本发明的保护范围中。
34.本发明提供了一种基于移动机器人定位系统的分布式集员滤波方法，首先，构建具有状态饱和的移动机器人定位系统的状态空间模型以及加密解密机制下的测量模型。基于可测信息，设计新型分布式集员饱和滤波器对上述系统的状态进行估计。接下来，计算分布式滤波器增益矩阵。最后，将设计的分布式滤波器增益矩阵代入至分布式集员滤波器中，构建出加密解密机制下具有状态饱和的分布式集员滤波算法。如图1所示，所述方法包括如下步骤：
35.步骤一、基于笛卡尔坐标系，选取移动机器人定位系统的位置及方向信息作为状态变量，建立加密解密机制下具有状态饱和的移动机器人定位系统动态模型。
36.本步骤中，建立的具有状态饱和的移动机器人定位系统动态模型为：
[0037][0038][0039]
式中，xk＝[π
k τ
k θk]
t
，t表示转置，(πk,τk)表示移动机器人在第k时刻笛卡尔坐标系下的位置信息，θk表示移动机器人在第k时刻笛卡尔坐标系下的方向信息；x
k+1
＝[π
k+1 τ
k+1 θ
k+1
]
t
，(π
k+1
,τ
k+1
)表示移动机器人在第k+1时刻笛卡尔坐标系下的位置信息，θ
k+1
表示
移动机器人在第k+1时刻笛卡尔坐标系下的方向信息；表示移动机器人的速度信息，wk为移动机器人定位系统的过程噪声，y
i,k
为移动机器人定位系统的测量输出信息，v
i,k
为移动机器人定位系统的测量噪声；i为传感器节点标号，n表示本发明研究的传感器网络中节点的数量；σ(
·
)表示饱和函数，具体定义见下式：
[0040]
σ(s)＝[σ1(s1)σ2(s2)σ3(s3)]
t
[0041]
σi(si)＝sign(si)min{s
i,max
,|si}(i＝1,2,3)
[0042]
式中，s
i,max
表示饱和水平s
max
的第i个元素；sign(
·
)为符号函数，min(
·
)为最小值函数，|
·
|表示对“·”取绝对值，[
·
]
t
表示对矩阵“[
·
]”取转置。
[0043]
为提高数据的安全性以及通信效率，本发明采用加密解密机制来调度测量值的传输情况。具体地，量化函数q(υ)＝[q(υ1)q(υ2)]
t
的量化水平表示为下式：
[0044][0045]
式中，υ
l
(l＝1,2)表示待量化值，q(υ
l
)表示量化后的测量值，κ＝0,1,2,
…
,l为量化区间的个数，l＝max
i,k
||s
i,k
/d||
∞
表示量化区间的最大个数，d表示每个量化区间的长度。此外，注意到量化误差可以表示为：为提高数据的安全性，本发明引入加密解密机制，具体过程描述如下：
[0046]
加密策略：对于数据信息y
i,k
，加密后的测量数据为：式中，y
i,k
表示待加密信息，s
i,k
表示加密后信息，q(
·
)为加密准则，为加密密钥。
[0047]
解密策略：基于密钥及得到的加密信息s
i,k
，解密后的测量值可表示为：式中，s
i,k
表示加密后的数据信息，表示解密后的数据信息。
[0048]
基于以上分析，定义测量输出经由加密解密机制后产生的解密误差为量化误差为因此，不难得到
[0049]
步骤二、基于步骤一建立的加密解密机制下具有状态饱和的移动机器人定位系统动态模型，在最小化滤波误差椭球域意义下设计分布式集员滤波器。具体步骤如下：
[0050]
为便于后续理论推导，本发明引入以下符号：
[0051][0052]
基于可测信息，构造如下分布式集员滤波器：
[0053]
[0054][0055]
式中，φk表示k时刻基于移动机器人定位系统的状态转移矩阵，为第i个传感器节点在第k时刻基于移动机器人定位系统的非线性函数的滤波形式，表示第i个传感器节点在第k+1时刻经由加密解密机制后的实际测量输出信息，h
i,k+1
为第i个传感器节点在第k+1时刻基于移动机器人定位系统的测量矩阵，σ(
·
)为饱和函数，为第i个传感器节点在第k时刻的一步预测，为第j个传感器节点在第k时刻的一步预测，为第i个传感器节点在第k+1时刻的滤波，为第i个传感器节点在第k时刻的滤波，为第i个传感器节点在k+1时刻的滤波器增益矩阵，e
i,k+1
表示第i个传感器节点在k+1时刻的解密误差，εi为第i个传感器节点的一致性增益参数，为第i个传感器节点与第j个传感器节点之间的连接权重，表示与第i个传感器节点有关的邻接节点构成的集合，σ表示求和函数。
[0056]
步骤三、计算传感器网络中每个传感器节点在k时刻的中间矩阵p
i,k+1|k
。
[0057]
本步骤中，中间矩阵p
i,k+1|k
的计算公式如下：
[0058][0059]
式中，φk为k时刻的移动机器人定位系统的状态转移矩阵，p
i,k+1|k
为第i个节点在k时刻的中间矩阵，p
i,k|k
为第i个节点在k时刻的滤波误差受限矩阵，为k时刻过程噪声受限矩阵，fk为基于移动机器人定位系统的非线性函数参数矩阵，λk为基于移动机器人定位系统的非线性函数的上界放缩系数，表示饱和水平第一个分量的平方，表示饱和水平第二个分量的平方，表示饱和水平第三个分量的平方，为第一参数，为的逆，为第二参数，为的逆，为第三参数，为的逆，min{a,b}表示取“a,b”取最小值，tr{
·
}表示对“{
·
}”取迹，i是三维单位矩阵。
[0060]
步骤四、根据步骤三得到的中间矩阵p
i,k+1|k
，计算出k+1时刻的滤波器增益矩阵
[0061]
本步骤中，滤波器增益矩阵的计算公式如下：
[0062][0063]
式中，∈1为一号中间变量，∈2为二号中间变量，∈3为三号中间变量，∈4为四号中间变量，∈5为五号中间变量，∈6为六号中间变量，表示∈s的逆(s＝1,2,
…
,6)。i是二维单位矩阵，是第i个传感器节点在k+1时刻的滤波器增益矩阵，p
i,k+1|k
表示第i个传感器节点在k时刻的中间矩阵，表示在k+1时刻的密钥，d为量化区间长度，h
i,k+1
是第i个传感
器节点在k+1时刻的移动机器人定位系统的测量矩阵，为h
i,k+1
的转置，为移动机器人定位系统的测量噪声受限矩阵，[
·
]-1
表示矩阵“[
·
]”的逆。
[0064]
步骤五、根据步骤四得到的每个传感器节点的滤波器增益矩阵将其代入至步骤二设计的分布式集员滤波器中，获得第i个传感器节点在第k+1时刻的滤波判断k+1是否达到总时长m，若k+1＜m，则执行步骤六，若k+1≥m，则结束运行。
[0065]
步骤六、根据步骤四中计算得到的计算滤波误差受限矩阵p
i,k+1|k+1
；令k＝k+1，执行步骤二，直至满足k+1≥m。
[0066]
本步骤中，滤波误差受限矩阵p
i,k+1|k+1
的计算公式为：
[0067][0068]
式中，∈1为一号中间变量，∈2为二号中间变量，∈3为三号中间变量，∈4为四号中间变量，∈5为五号中间变量，∈6为六号中间变量，表示∈s的逆(s＝1,2,
…
,6)，p
i,k+1|k
为第i个节点在k时刻的中间矩阵，p
i,k+1|k+1
为第i个节点在k+1时刻的滤波误差受限矩阵，是第i个节点在k+1时刻的滤波器增益矩阵，是第i个节点在k时刻的一步预测，是第j个节点在k时刻的一步预测，为移动机器人定位系统的测量噪声受限矩阵，h
i,k+1
是第i个传感器节点在k+1时刻的移动机器人定位系统的测量矩阵，表示在k+1时刻的密钥，d为量化区间长度，为矩阵的转置，εi为第i个节点的一致性增益，为第i个传感器节点与第j个传感器节点之间的连接权重，表示第i个传感器节点的邻接节点集，σ表示求和函数，i是三维单位矩阵。
[0069]
根据本步骤计算出的p
i,k+1|k+1
，通过最小化tr{p
i,k+1|k+1
}设计出滤波器增益数
[0070]
实施例：
[0071]
本实施例以加密解密机制下具有状态饱和的移动机器人定位系统为例，采用本发明所述方法进行仿真：
[0072]
本发明选取具有四个传感器节点的传感器网络进行数值仿真，其边集可表示为：
[0073][0074]
式中，每一个有序数对表示节点之间信息的交互行为，如：(1,3)表示第3个节点可以传输信息至第1个节点.
[0075]
选取如下参数：
[0076][0077]
式中，为第一个传感器节点的滤波初值，其中向量的第一个分量、第二个分量及第三个分量分别表示在初始时刻第一个传感器节点对移动机器人水平及垂直方向的位置及方向信息，为第二个传感器节点的滤波初值，其中向量的第一个分量、第二个分量及第三个分量分别表示在初始时刻第二个传感器节点对移动机器人水平及垂直方向的位置及方向信息，为第三个传感器节点的滤波初值，其中向量的第一个分量、第二个分量及第三个分量分别表示在初始时刻第三个传感器节点对移动机器人水平及垂直方向的位置及方向信息，为第四个传感器节点的滤波初值，其中向量的第一个分量、第二个分量及第三个分量分别表示在初始时刻第四个传感器节点对移动机器人水平及垂直方向的位置及方向信息，加密及解密密钥设置为移动机器人定位系统的过程噪声为wk＝[0.3sin(0.1k)0.3cos(0.3k)0.4cos(0.2k)]
t
，移动机器人定位系统的测量噪声为ν
i,k
＝[1.2sin(0.1k)1.2sin(0.1k)]
t
，水平、垂直位置及方向的饱和水平分别为ψ
1,max
＝20、ψ
2,max
＝30及ψ
3,max
＝20，fk＝i3表示基于移动机器人定位系统的非线性函数参数矩阵，λk＝3表示基于移动机器人定位系统的非线性函数的上界放缩系数，d＝0.01表示量化区间长度，表示移动机器人定位系统的过程噪声的受限矩阵，表示移动机器人定位系统的测量噪声受限矩阵，i3表示三维单位矩阵，p
i,0|0
＝2i3表示滤波误差受限矩阵，其中i＝1,2,3,4表示传感器节点的个数。本发明采用滤波误差来评估算法性能，其计算方法为：特别地，引入平均滤波误差来进一步衡量不同量化区间长度下的滤波性能变化情况，其计算方法为：本发明中m＝40。
[0078]
分布式滤波器效果：
[0079]
由图2至图7可见，针对加密解密机制下具有状态饱和的移动机器人定位系统，所发明的分布式集员滤波方法可以有效地对目标状态进行估计，并且系统状态始终保持在已知饱和水平之下，符合本发明理论结果。
[0080]
由图8及图9可以看出，针对每个传感器节点，滤波误差的轨迹始终保持在滤波误差受限矩阵的迹之下，验证了本发明的正确性。
[0081]
由图10及图11可见，随着量化区间长度的增大，滤波误差逐渐增大。进一步地，由表1可得，节点1、节点2、节点3及节点4的平均滤波误差分别降低了0.51、0.9、0.82及0.96。
[0082]
表1
[0083]

技术特征：
1.一种基于移动机器人定位系统的分布式集员滤波方法，其特征在于所述方法包括如下步骤：步骤一、基于笛卡尔坐标系，选取移动机器人定位系统的位置及方向信息作为状态变量，建立加密解密机制下具有状态饱和的移动机器人定位系统动态模型；步骤二、对步骤一建立的移动机器人定位系统动态模型，在最小化滤波误差椭球域意义下设计分布式集员滤波器；步骤三、计算传感器网络中每个节点在k时刻的中间矩阵p
i,k+1|k
；步骤四、依据步骤三中获得的中间矩阵p
i,k+1|k
，计算每个传感器节点的滤波器增益矩阵步骤五、根据步骤四中得到的滤波器增益矩阵将其代入至步骤二设计的分布式集员滤波器中，设计出第i个传感器节点在k+1时刻的分布式集员滤波器判断k+1是否达到总时长m，如果k+1＜m，则执行步骤六，若k+1≥m，则结束运行；步骤六、根据步骤四中得到的每个传感器节点的滤波器增益矩阵计算每个传感器节点的滤波误差受限矩阵p
i,k+1|k+1
；令k＝k+1，执行步骤二，直至满足k+1≥m。2.根据权利要求1所述的基于移动机器人定位系统的分布式集员滤波方法，其特征在于所述步骤一中，加密解密机制下具有状态饱和的移动机器人定位系统动态模型如下：于所述步骤一中，加密解密机制下具有状态饱和的移动机器人定位系统动态模型如下：于所述步骤一中，加密解密机制下具有状态饱和的移动机器人定位系统动态模型如下：式中，x
k
＝[π
k τ
k θ
k
]
t
，t表示转置，(π
k
,τ
k
)表示移动机器人在第k时刻笛卡尔坐标系下的位置信息，θ
k
表示移动机器人在第k时刻笛卡尔坐标系下的方向信息，σ(
·
)表示饱和函数，x
k+1
＝[π
k+1 τ
k+1 θ
k+1
]
t
，(π
k+1
,τ
k+1
)表示移动机器人在第k+1时刻笛卡尔坐标系下的位置信息，θ
k+1
表示移动机器人在第k+1时刻笛卡尔坐标系下的方向信息；表示移动机器人的速度信息，w
k
为移动机器人定位系统的过程噪声，y
i,k
为移动机器人定位系统的测量输出信息，v
i,k
为移动机器人定位系统的测量噪声，i为传感器节点标号，i＝1,2,...,n，n表示传感器网络中节点的数量，表示解密后的数据信息，e
i,k
为解密误差。3.根据权利要求1所述的基于移动机器人定位系统的分布式集员滤波方法，其特征在于所述步骤二中，分布式集员滤波器为：于所述步骤二中，分布式集员滤波器为：式中，φ
k
表示k时刻基于移动机器人定位系统的状态转移矩阵，为第i个传感
器节点在第k时刻基于移动机器人定位系统的非线性函数的滤波形式，表示第i个传感器节点在第k+1时刻经由加密解密机制后的实际测量输出信息，h
i,k+1
为第i个传感器节点在第k+1时刻基于移动机器人定位系统的测量矩阵，σ(
·
)为饱和函数，为第i个传感器节点在第k时刻的一步预测，为第j个传感器节点在第k时刻的一步预测，为第i个传感器节点在第k+1时刻的滤波，为第i个传感器节点在第k时刻的滤波，为第i个传感器节点在k+1时刻的滤波器增益矩阵，e
i,k+1
表示第i个传感器节点在k+1时刻的解密误差，ε
i
为第i个传感器节点的一致性增益参数，为第i个传感器节点与第j个传感器节点之间的连接权重，表示与第i个传感器节点有关的邻接节点构成的集合，σ表示求和函数。4.根据权利要求1所述的基于移动机器人定位系统的分布式集员滤波方法，其特征在于所述步骤三中，中间矩阵p
i,k+1|k
的计算公式如下：式中，φ
k
表示k时刻基于移动机器人定位系统的状态转移矩阵，p
i,k+1|k
为第i个节点在k时刻的中间矩阵，p
i,k|k
为第i个节点在k时刻的滤波误差受限矩阵，为k时刻过程噪声受限矩阵，f
k
为基于移动机器人定位系统的非线性函数参数矩阵，λ
k
为基于移动机器人定位系统的非线性函数的上界放缩系数，表示饱和水平第一个分量的平方，表示饱和水平第二个分量的平方，表示饱和水平第三个分量的平方，为第一参数，为的逆，为第二参数，为的逆，为第三参数，为的逆，min{a,b}表示取“a,b”取最小值，tr{
·
}表示对“{
·
}”取迹，i是三维单位矩阵。5.根据权利要求1所述的基于移动机器人定位系统的分布式集员滤波方法，其特征在于所述步骤四中，滤波器增益矩阵的计算公式如下：式中，∈1为一号中间变量，∈2为二号中间变量，∈3为三号中间变量，∈4为四号中间变量，∈5为五号中间变量，∈6为六号中间变量，表示∈
s
的逆(s＝1,2,
…
,6)，i是二维单位矩阵，是第i个传感器节点在k+1时刻的滤波器增益矩阵，p
i,k+1|k
表示第i个传感器节点在k时刻的中间矩阵，表示在k+1时刻的密钥，d为量化区间长度，h
i,k+1
是第i个传感器节点在k+1时刻的移动机器人定位系统的测量矩阵，为h
i,k+1
的转置，为移动机器人定位系统的测量噪声受限矩阵，[
·
]-1
表示矩阵“[
·
]”的逆。6.根据权利要求1所述的基于移动机器人定位系统的分布式集员滤波方法，其特征在于所述步骤六中，滤波误差受限矩阵p
i,k+1|k+1
的计算公式为：
式中，∈1为一号中间变量，∈2为二号中间变量，∈3为三号中间变量，∈4为四号中间变量，∈5为五号中间变量，∈6为六号中间变量，表示∈
s
的逆(s＝1,2,
…
,6)，p
i,k+1|k
为第i个节点在k时刻的中间矩阵，p
i,k+1|k+1
为第i个节点在k+1时刻的滤波误差受限矩阵，是第i个节点在k+1时刻的滤波器增益矩阵，是第i个节点在k时刻的一步预测，是第j个节点在k时刻的一步预测，为移动机器人定位系统的测量噪声受限矩阵，h
i,k+1
是第i个传感器节点在k+1时刻的移动机器人定位系统的测量矩阵，表示在k+1时刻的密钥，d为量化区间长度，为矩阵的转置，ε
i
为第i个节点的一致性增益，为第i个传感器节点与第j个传感器节点之间的连接权重，表示第i个传感器节点的邻接节点集，σ表示求和函数，i是三维单位矩阵。

技术总结
本发明公开了一种基于移动机器人定位系统的分布式集员滤波方法，所述方法包括如下步骤：步骤一、建立加密解密机制下具有状态饱和的移动机器人定位系统动态模型；步骤二、在最小化滤波误差椭球域意义下设计分布式集员滤波器；步骤三、计算传感器网络中每个节点在k时刻的中间矩阵P


技术研发人员：胡军 李佳兴 张红旭 武志辉 徐龙 杜君花 高培夏 董睿杰 周奥展
受保护的技术使用者：哈尔滨理工大学
技术研发日：2022.12.07
技术公布日：2023/7/19