CN105635963A - 多智能体分布式协同定位方法 - Google Patents

Abstract

本发明涉及一种多智能体分布式协同定位方法，属于信号处理技术领域，本方法包括：每个智能体分别建立用于实时估计的状态变量；对该状态变量进行预测、对邻近智能体进行相对距离的测量、对状态变量预测值进行本地优化、对状态变量中他体位置矢量估计值进行邻域索求与响应、对状态变量估计值进行邻域优化、对状态变量中本体位置矢量估计值进行邻域交换、对状态变量估计值进行重建，每当新时刻来临，重复进行新时刻的实时定位估计。本发明可面向连通的任意规模多智能体网络，具有规模可扩展性，可充分利用多智能体网络中的点对点信息，实现高精度的智能体定位能力。

Description

多智能体分布式协同定位方法

技术领域

本发明属于信号处理技术领域，特别涉及一种多智能体分布式协同定位方法。

背景技术

智能体泛指具有智能的实体，通常具有计算能力、通信能力、响应能力等功能，常见的智能体有人造卫星、无人机、无人车、智能家居、智能硬件、机电一体化传感器等等。自身位置待定的智能体称为普通智能体，已知自身位置的实体或者具有绝对定位能力的实体均称为锚点智能体，简称锚点。

锚点指已知自身位置的一类实体，可以是一类具有绝对定位能力的特殊智能体，也可以是具有绝对定位能力的其他实体；普通智能体指自身位置待定的智能体；r_i代表智能体i的真实位置矢量，代表智能体i对智能体j位置的估计量；代表智能体i邻近的锚点集合，代表智能体i邻近的普通智能体集合，和均已排序，其集合大小分别用和表示，集合中的第k个锚点或邻近智能体分别表示为和

随着社会需求的快速增长和科学技术的不断进步，可以利用多个相对简单的智能体协同配合工作组合成多智能体系统，完成原本复杂大型化的单一智能体才能胜任的任务。多智能体系统得到了人们日益广泛的重视与应用，例如常见的多智能体系统有无线传感器网络、合成孔径相机等，通过多个简单智能体配合工作，可以实现复杂的环境监测、目标跟踪、高分辨率成像等功能。智能体定位是多智能体系统的关键技术之一，大量协同任务都高度依赖于定位能力和定位精度。为所有智能体配备定位装置(例如GPS模块、北斗模块等)是昂贵且高功耗的，尤其不适合拥有成百上千数量智能体的大规模多智能体系统。

多智能体系统的定位问题可以表述为，如何利用系统中较少的锚点去定位系统中众多普通智能体。

多智能体系统的定位方法可以分为集中式方法和分布式方法两大类，分布式方法又可细分为增量式和并行式两小类。

中国专利200910000855.3公布了一种传感器网络的无线定位方法，各无线传感器分别将自身获得的信号通过树形网络传达到总台服务器，总台服务器对全部信号进行集中处理，进而计算出每个传感器的位置，再通过树形网络分发回每个智能体，从而实现多智能体的定位，该方法属于集中式定位方法。该方法通过总台服务器进行集中的信息汇总、计算和分发，因此总台服务器的计算代价、通信代价和成本代价巨大，受网络拓扑结构的限制严重，而且一旦总台服务器失效，则其定位功能完全失效，因此不适合大规模多智能体系统。

中国专利201310072598.0公布了一种无线传感器网络中智能体的定位方法，待定位智能体查询与自身邻近的锚点信息表，当其邻近的锚点数目大于或者等于3个时，该待定位智能体与锚点进行测距，获得至少3个测距样本，进而采用最大似然估计算法获得待定位智能体的位置坐标，当某个智能体完成定位后，这个智能体又充当起新的锚点，辅助其他智能体进行定位，因此该方法的定位是一层一层扩增的，属于增量式的分布式定位方法。增量式方法相比集中式方法而言，具有一定的分布计算的特征，因此降低了对任意节点计算通信能力的要求。然而这种方法也具有较大的缺点，主要有两个方面，其一，增量式方法中每个智能体只能利用智能体与锚点之间的测距信息进行定位，而不能利用智能体与智能体之间测距信息，定位精度较低，其二，增量式方法的定位误差会随着定位传播而累积扩大，不适合大规模多智能体系统。

中国专利200910310553.6公布了一种分布式无线传感器网络节点定位方法，包括三个定位阶段，第一阶段每个智能体随机定位，互相比较定时的时间长度，将比较结果作为一种依据，自主选择起始定位的智能体，第二阶段进行增量式定位，第三阶段将这种增量式定位扩大到网络全局，最终达到全网智能体的定位。该方法属于分布式定位方法，兼具增量式和并行式的部分特征。该方法的主要缺点与增量式方法相同。

国际期刊文献中，目前广为使用的分布式定位方法为虚拟弹簧振子松弛法(很多文献叫做AFL方法)，该方法的定位原理基于虚拟的弹簧振子模型，将位置估计值作为虚拟的质量振子，将邻近距离测量值作为虚拟的弹簧力，通过模拟该虚拟系统的运动直至达到平衡稳态，求解达到平衡稳态时虚拟质量振子的位置，作为最终的位置估计值，作为每个智能体的定位结果。该方法是完全的分布式方法，具有并发特征，适合大规模多智能体系统的定位，它不仅利用了智能体与锚点的相对测距信息，也利用了智能体与智能体之间的相对测距信息，因此定位精度较高。然而该方法只考虑位置的估计值，并未考虑估计值的可信度及误差分布概率，没有充分利用除位置以外的其他大量有益信息，定位精度受到限制。

发明内容

本发明的目的是为解决现有技术存在的问题，提供一种多智能体分布式协同定位方法，本方法可以适用于连通的任意规模多智能体网络，具有规模可扩展性，可以充分利用多智能体网络中的点对点信息，实现高精度的智能体定位能力，本发明方法不需要所有智能体具有绝对定位能力，不需要智能体具有全局通信能力，可以帮助降低智能体的成本和功耗。

本发明提出的一种多智能体分布式协同定位方法。流程如图1所示，包括以下步骤：

1)每个智能体建立用于位置估计的状态变量，状态变量包含该智能体的本体位置矢量以及与它邻近的普通智能体的他体位置矢量，每个智能体初始化状态变量的估计值及其对应的误差协方差矩阵，每个智能体初始化预测过程中的系统噪声矩阵和测量噪声矩阵，包括以下具体步骤：

1-1)构造状态变量：设智能体i建立状态变量X_i，为其中，r_i代表智能体i的本体位置矢量，上标^T代表转置，代表第n个与智能体i邻近的普通智能体的他体位置矢量，代表所有与智能体i邻近的普通智能体的数量，i、n、均为正整数，表示邻近智能体编号；

1-2)初始化定位变量：设智能体i状态变量X_i的估计值(也即是初始化智能体i对其本体位置矢量及与其邻近的普通智能体的他体位置矢量的估计值)；为其中，代表智能体i对本智能体i位置矢量的估计值，代表智能体i对与其邻近的第n个普通智能体的他体位置矢量的估计值；初始化状态变量在0时刻的估计值其中符号|₀用于指示竖线左侧变量在t＝0时刻的值；智能体i根据状态变量估计值的初始误差，初始化其误差协方差矩阵P_i，得到P_i|₀；智能体i根据状态变量X_i的维度，初始化系统噪声矩阵Q_i；

2)在第t＝k时刻，对每个智能体的状态变量采用实时迭代方法进行实时估计，每一次迭代过程依次包括：对该状态变量进行预测、对邻近智能体进行相对距离的测量、对状态变量预测值进行本地优化、对状态变量中他体位置矢量估计值进行邻域索求与响应、对状态变量估计值进行邻域优化、对状态变量中本体位置矢量估计值进行邻域交换、对状态变量估计值进行重建，具体步骤为：

2-1)对状态变量进行预测：每个智能体根据上一时刻即k-1时刻状态变量估计值，对k时刻的状态变量进行预测；设智能体i在k时刻状态变量的预测值误差协方差矩阵预测值P_i|_k/k-1，分别为：

${{\hat{X}}_i|}_{k/k-1}={{\hat{X}}_i|}_{k-1}---\left(1\right)$

P_i|_k/k-1＝P_i|_k-1+Q_i(2)

其中，为k-1时刻的状态变量估计值，P_i|_k-1为对应的误差协方差矩阵；

2-2)对邻近智能体进行相对距离的测量：根据每个智能体i采集包含有它与邻近智能体之间相对位置信息的测量距离按式(3)得到k时刻的邻近智能体相对距离测量变量

其中，为智能体i与邻近的第n个普通智能体间相对距离测量值，为智能体i与邻近的第n个锚点间相对距离测量值，锚点指的是具有绝对定位能力的位置参考实体；

2-3)对状态变量预测值进行本地优化：具体包括以下步骤：

2-3-1)预测邻近相对距离，每个智能体i根据状态变量的预测值进一步预测邻近相对距离组成邻近测量变量的预测值

其中，为智能体i与邻近的第n个普通智能体间相对距离预测值，为智能体i与邻近的第n个锚点间相对距离预测值；

2-3-2)根据邻近测量变量预测值的维度、大小及测量条件，适应性设定测量噪声矩阵R_i；

2-3-3)以扩展卡尔曼滤波的最优化策略，对状态变量及其协方差矩阵进行优化，得到本地优化后的状态变量及其误差协方差矩阵P_i|_k-0.5，它们分别为

${{\hat{X}}_i|}_{k-0.5}={{\hat{X}}_i|}_{k/k-1}+{K_i|}_k({{\tilde{Z}}_i|}_k-{{\hat{Z}}_i|}_{k/k-1})---\left(5\right)$

${P_i|}_{k-0.5}=(I-{K_i|}_k{H_i|}_k){P_i|}_{k/k-1}{(I-{K_i|}_k{H_i|}_k)}^T+{K_i|}_k{R}_i{K_i|}_k^T---\left(6\right)$

式(5)中增益修正矩阵K_i|_k、式(6)中线性化观测矩阵H_i|_k分别为

${K_i|}_k={P_i|}_{k|k-1}{H_i|}_k{({H_i|}_k{P_i|}_{k|k-1}{H_i|}_k^T+R_i)}^{-1}---\left(7\right)$

2-4)对状态变量中他体位置矢量估计值进行邻域索求与响应：每个智能体索求邻近智能体对该主动索求智能体位置矢量的最优估计，并响应其邻近智能体的同类索求；具体而言，每个智能体i本地优化后的状态变量中均包含有对其邻近智能体的他体位置矢量的估计；每个智能体i，向其邻近智能体索求中包含的位置估计量及对应的包含于P_j|_k-0.5中的误差协方差矩阵块P_j<i>|_k-0.5；同时，每个智能体i也向其邻近智能体提供中包含的位置估计量及对应的包含于P_i|_k-0.5中的误差协方差矩阵块P_i<j>|_k-0.5；

2-5)对状态变量估计值进行邻域优化：首先计算智能体i的邻近智能体个数并计算系数ω_i，随后按式(9)式(10)对本体位置矢量的估计值及其误差协方差矩阵进行邻域优化，优化后的位置矢量估计值及其误差协方差矩阵P_i<i>|_k,block分别为

${\left({P_i<i>|}_{k,block}\right)}^{-1}=\underset{j\&Element;\left\{i\right\}\&cup;N_i}{\&Sigma;}\&lsqb;{\mathrm{\&omega;}}_i{\left({P_j<i>|}_{k-0.5}\right)}^{-1}\&rsqb;---\left(10\right)$

2-6)对状态变量中本体位置矢量估计值进行邻域交换：智能体i邻域优化后的本体位置估计值及P_i<i>|_k,block，与其邻近普通智能体的邻域优化后的本体位置估计值及P_j<j>|_k,block，进行彼此交换；

2-7)重建状态变量估计值及其协方差：根据智能体i邻域交换的本体位置估计值，按式(11)式(12)重建智能体i的状态变量的估计值及误差协方差，用于下一时刻的迭代过程；

3)随着时间的进行，重复步骤2)的迭代过程：即每当新时刻来临，即t＝k+1时刻，依次重复步骤2-1)至2-7)进行新时刻的实时定位估计。

本发明的特点及有益效果是：

本发明不仅可以充分利用智能体与锚点、智能体与智能体之间的全部有益信息，同时还考虑了误差分布概率，采用并行算法对全部有益信息进行分布式的数据融合，能够大大提高智能体定位的精度，实现高精度的智能体定位。

本发明可以适用于连通的任意规模多智能体网络，所述方法不因智能体网络规模的增大而变化，具有可扩展性；本发明中所述方法通过利用多智能体网络中充分的点对点信息，可以实现非常高精度的智能体定位能力；本发明中所述方法是完全分布式方法，不需要智能体具有网络全局通信能力，也不需要智能体具有感知网络全局信息能力，不需要所有智能体都具有GPS等定位装置，降低了智能体的成本和功耗，也提高了方法的适用范围。

附图说明

图1是本发明的多智能体分布式协同定位方法的流程示意框图。

图2是本发明的实施例多智能体网络中智能体间的通信关系图。

图3是本实施例中每个普通智能体的定位误差随迭代次数变化曲线图，其中(a)表示x轴的定位误差，(b)表示y轴的定位误差。

图4是本实施例中本发明方法与已有技术的定位平均误差对比图。

图5是实施例中本发明方法与已有技术的定位结构误差对比图。

具体实施方式

本发明提出的一种多智能体分布式协同定位方法，下面结合附图和具体实施例进一步说明如下。

本发明提出的一种多智能体分布式协同定位方法，其特征在于：该定位方法包括以下步骤：

1)每个智能体建立用于位置估计的状态变量，状态变量包含该智能体的本体位置矢量以及与它邻近的普通智能体的他体位置矢量，每个智能体初始化状态变量的估计值及其对应的误差协方差矩阵，每个智能体初始化预测过程中的系统噪声矩阵和测量噪声矩阵，包括以下具体步骤：

1-1)构造状态变量：设智能体i建立状态变量X_i，为其中，r_i代表智能体i的本体位置矢量，上标^T代表转置，代表第n个与智能体i邻近的普通智能体的他体位置矢量，代表所有与智能体i邻近的普通智能体的数量，i、n、均为正整数，表示邻近智能体编号，该编号的排列顺序是严格单调的，即为或举例而言，假设智能体1(编号为1的智能体)有3个与其邻近的普通智能体，分别为智能体5、智能体6和智能体7，则集合集合中普通智能体的数量智能体1所构造的状态变量X₁为 $X_3={\&lsqb;r_1^T,r_5^T,r_6^T,r_7^T\&rsqb;}^T;$

1-2)初始化定位变量：设智能体i状态变量X_i的估计值(也即是初始化智能体i对其本体位置矢量及与其邻近的普通智能体的他体位置矢量的估计值)；为其中，代表智能体i对本智能体i位置矢量的估计值，代表智能体i对与其邻近的第n个普通智能体的他体位置矢量的估计值；初始化0时刻的状态变量估计值，得到其中符号|₀用于指示竖线左侧变量在t＝0时刻的值；；智能体i根据状态变量估计值的初始误差，初始化其误差协方差矩阵P_i，得到P_i|₀；智能体i根据状态变量X_i的维度，初始化系统误差矩阵Q_i为具有阶数等于状态变量X_i维度的对角线矩阵，对角线元素均为正数且取值范围根据实际需求取于(0,1]之间；举例而言，假设初始化后的与真实值X_i存在不超过0.5米的误差(假设状态变量X_i是12维矢量)，那么可以将P_i初始化为P_i|₀＝0.5I_12×12，将Q_i初始化为Q_i＝0.0001I_12×12，其中I_12×12为12维单位矩阵，事实上由于这个初始化初始化误差通常是难以衡量的，因此P_i初始化的较为宽松，Q_i的大小取决于定位问题的尺度，例如多个智能体分布于一个小房间内部，则Q_i可以取的更小，而假如多个智能体分布于几十平方公里的范围，则Q_i没有必要取的很小，可以取值略大；

2)在第t＝k时刻，对每个智能体的状态变量采用实时迭代方法进行实时估计，每一次迭代过程依次包括：对该状态变量进行预测、对邻近智能体进行相对距离的测量、对状态变量预测值进行本地优化、对状态变量中他体位置矢量估计值进行邻域索求与响应、对状态变量估计值进行邻域优化、对状态变量中本体位置矢量估计值进行邻域交换、对状态变量估计值进行重建，具体为：

2-1)首先对状态变量进行预测：每个智能体根据上一时刻即t＝k-1时刻状态变量估计值，对k时刻的状态变量进行预测；设智能体i在k时刻状态变量的预测值误差协方差矩阵预测值P_i|_k/k-1，分别为：

${{\hat{X}}_i|}_{k/k-1}={{\hat{X}}_i|}_{k-1}---\left(1\right)$

P_i|_k/k-1＝P_i|_k-1+Q_i(2)

其中，为k-1时刻的状态变量估计值，P_i|_k-1为对应的误差协方差矩阵；

2-2)其次对邻近智能体进行相对距离的测量：根据每个智能体i采集包含有它与邻近智能体之间相对位置信息的测量距离按式(3)得到k时刻的邻近智能体相对距离测量变量

其中，为智能体i与邻近的第n个普通智能体间相对距离测量值，为智能体i与邻近的第n个锚点间相对距离测量值，锚点指的是具有绝对定位能力的位置参考实体；举例而言，假如智能体1邻近的普通智能体个数有3个、邻近的锚点有1个，则可测量得到3组智能体与智能体之间的相对距离以及1组智能体与锚点之间的相对距离，构成的相对距离测量变量是3+1＝4维矢量；

2-3)然后，对状态变量预测值进行本地优化：具体包括以下步骤：

2-3-1)预测邻近相对距离，每个智能体i根据状态变量的预测值进一步预测邻近相对距离组成邻近测量变量的预测值

其中，为智能体i与邻近的第n个普通智能体间相对距离预测值，为智能体i与邻近的第n个锚点间相对距离预测值；

2-3-2)根据邻近测量变量预测值的维度、大小及测量条件，适应性设定测量误差矩阵R_i；为矩阵阶数等于邻近测量变量预测值维数的对角矩阵，每个对角线元素与邻近测量变量预测值中对应维度元素值有关，具体关系为：

式中，对于不同的正整数n而言，其中和为测量变量预测值中对应维度的元素，系数Y为正数，取值范围为Y∈(0,100]。举例而言，假设邻近测量变量预测值是4维的，则所设定的R_i也是4维的矩阵，例如可以设定为4阶对角线矩阵，对角线元素的大小根据根据测量噪声水平的大小决定；

2-3-3)以扩展卡尔曼滤波的最优化策略，对状态变量及其协方差矩阵进行优化，得到本地优化后的状态变量及其误差协方差矩阵P_i|_k-0.5

${{\hat{X}}_i|}_{k-0.5}={{\hat{X}}_i|}_{k/k-1}+{K_i|}_k({{\tilde{Z}}_i|}_k-{{\hat{Z}}_i|}_{k/k-1})---\left(5\right)$

${P_i|}_{k-0.5}=(I-{K_i|}_k{H_i|}_k){P_i|}_{k/k-1}{(I-{K_i|}_k{H_i|}_k)}^T+{K_i|}_k{R}_i{K_i|}_k^T---\left(6\right)$

式(5)中增益修正矩阵K_i|_k、式(6)中线性化观测矩阵H_i|_k分别为

${K_i|}_k={P_i|}_{k/k-1}{H_i|}_k{({H_i|}_k{P_i|}_{k/k-1}{H_i|}_k^T+R_i)}^{-1}---\left(7\right)$

2-4)接着，对状态变量中他体位置矢量估计值进行邻域索求与响应：每个智能体索求邻近智能体对该主动索求智能体位置矢量的最优估计，并响应其邻近智能体的同类索求；具体而言，每个智能体i本地优化后的状态变量中均包含有对其邻近智能体的他体位置矢量的估计；每个智能体i，向其邻近智能体索求中包含的位置估计量及对应的包含于P_j|_k-0.5中的误差协方差矩阵块P_j<i>|_k-0.5；同时，每个智能体i也向其邻近智能体提供中包含的位置估计量及对应的包含于P_i|_k-0.5中的误差协方差矩阵块P_i<j>|_k-0.5；这一步的进行主要为状态变量构造方式，每个智能体不仅估计自身的本体位置矢量，同时也估计其邻近智能体的他体位置矢量(即允许智能体i充分利用智能体与智能体之间的信息，有助于2-5)中通过获知其他智能体对自己的位置矢量估计值，而进一步修正对自身本体位置的估计)；

2-5)随后，对状态变量估计值进行邻域优化：首先计算每个智能体i其邻近智能体的个数计算系数ω_i，随后按式(9)式(10)对本体位置矢量的估计值及其误差协方差矩阵的进行邻域优化，优化后的位置矢量估计值及其误差协方差矩阵P_i<i>|_k,block为

${\left({P_i<i>|}_{k,block}\right)}^{-1}=\underset{j\&Element;\left\{i\right\}\&cup;N_i}{\&Sigma;}\&lsqb;{\mathrm{\&omega;}}_i{\left({P_j<i>|}_{k-0.5}\right)}^{-1}\&rsqb;---\left(10\right)$

2-6)然后，对状态变量中本体位置矢量估计值进行邻域交换：智能体i将邻域优化后的本体位置估计值及P_i<i>|_k,block，与其邻近普通智能体的邻域优化后的本体位置估计值及P_j<j>|_k,block，进行彼此交换(这一步将邻域优化后的本体位置估计进行邻域交换的做法，是重建状态变量估计值的关键前提)；

2-7)最后，重建状态变量估计值及其协方差：每个智能体根据邻域交换的本体位置估计值，按式(11)式(12)重建自身的状态变量的估计值及误差协方差，用于下一时刻的迭代过程，状态变量估计值及误差协方差的重建方法为

(重建方法(11)与(12)主要在于，既能快速完成当前迭代过程中的状态变量估计值的最终优化，又在求解快速性和矩阵正定性的约束下给出了稳定的协方差矩阵构造方式，确保了迭代过程的不断进行)；

3)每个智能体随着时间的进行，重复步骤2)的迭代过程：即每当新一时刻来临，即t＝k+1时刻，依次重复步骤2-1)至2-7)进行新一时刻的实时定位估计；

本发明方法是实时迭代方法，每一次迭代过程包括以下步骤：每个智能体分别建立用于实时估计的状态变量，状态变量包含智能体的本体位置状态和其邻近智能体的他体位置状态；每个智能体通过传感器，采集该智能体与邻近智能体之间的相对距离测量信息，每个智能体利用各自采集到的测量信息，分别对各自建立的状态变量进行本地优化；本地优化之后，每个智能体向邻近智能体索求他体位置的最优估计，同时响应邻近智能体的同类请求、发送相应信息，根据邻近智能体的响应信息，再次优化状态变量中的本体位置的估计量，完成邻域优化；随后，每个智能体与邻近智能体交换本体位置的估计量，重新构造建立新的状态变量。系统，是具有数据计算能力、无线通信能力、和相对感知能力且嵌入有所述定位方法的定位系统。

下面结合一个具体实施例对本发明进一步详细解释如下。本实施例以分布于100m×100m平面区域内的多智能体系统为例，通过本发明方法实现智能体定位，并与目前常用的经典已有技术进行效果对比。在本实施例中，如图2所示，分布于100m×100m区域的多智能体网络，实心圆圈代表普通智能体，空心圆圈代表锚点智能体，智能体间连线代表智能体存在邻近通信关系，多智能体系统拥有锚点智能体4个、普通智能体100个；多智能体系统中每个智能体的通信距离均为25m。相邻智能体间距离测量值由式(13)仿真生成

${\tilde{d}}_{ij}=d_{ij}+{\mathrm{\&epsiv;}}_{ij}---\left(13\right)$

其中，表示测量值，d_ij表示真实值；ε_ij表示测量误差，服从高斯分布其标准差与真实距离d_ij及噪声比Y有关，本实施例中Y＝3，标准差按式(14)进行

${\mathrm{\&sigma;}}_{{\mathrm{\&epsiv;}}_{ij}}=d_{ij}\&times;\frac{Y}{100}---\left(14\right)$

初始化中，每个普通智能体初始位置估计具有1m以内的误差，按照本发明所述方法，实施定位。仿真结果的定位误差如图3所示，(a)和(b)分别是两个x轴和y轴方向上的定位误差，可以看到每个智能体的定位误差在各方向上是收敛的，误差随迭代次数的增多而有效减小。

为了验证本发明定位方法的高精度特性，采用对比仿真实验做直观说明。作为对比的已有技术是背景技术中提到的经典而有效的虚拟弹簧振子定位方法，其定位原理基于虚拟的弹簧振子模型，将位置估计值作为虚拟的质量振子，将邻近距离测量值作为虚拟的弹簧力，通过模拟该虚拟系统的运动直至达到平衡稳态，求解达到平衡稳态时虚拟质量振子的位置，作为最终的位置估计值。

为了衡量定位方法的定位精度，采取两种衡量指标，分别是平均误差和结构误差，其定义为：

平均误差

结构误差

与已有技术方法的定位平均误差对比结果如图4所示，图中小方框表示已有方法的曲线，小圆圈表示本发明方法的曲线，可以看出，本发明方法的定位平均误差显著小于已有技术方法，并且收敛时间较短。与已有技术的定位结构误差对比结果如图5所示，图中小方框表示已有方法的曲线，小圆圈表示本发明方法的曲线，可以看出，本发明方法的定位结构误差同样显著小于已有技术方法，并且收敛时间较短。本发明方法相比于已有技术方法，定位的快速性和精度均得到了较大幅度提高，换用多种实验参数进行重复实验，平均误差可以降低60％以上，结构误差指标降低70％以上。需要额外说明的是，尽管实施例中的直观说明以二维定位为示例，但本发明方法不仅局限于二维定位问题，同样适用于一维空间和三维空间的定位问题。

Claims (4)

1.一种多智能体分布式协同定位方法，其特征在于：该定位方法包括以下步骤：

1)每个智能体建立用于位置估计的状态变量，状态变量包含该智能体的本体位置矢量以及与它邻近的普通智能体的他体位置矢量，每个智能体初始化状态变量的估计值及其对应的误差协方差矩阵，每个智能体初始化预测过程中的系统噪声矩阵和测量噪声矩阵，包括以下具体步骤：

1-1)构造状态变量：设智能体i建立状态变量X_i，为其中，r_i代表智能体i的本体位置矢量，上标^T代表转置，代表第n个与智能体i邻近的普通智能体的他体位置矢量，代表所有与智能体i邻近的普通智能体的数量，i、n、均为正整数，表示邻近智能体编号；

1-2)初始化定位变量：设智能体i状态变量X_i的估计值为其中，代表智能体i对本智能体i位置矢量的估计值，代表智能体i对与其邻近的第n个普通智能体的他体位置矢量的估计值；初始化状态变量在0时刻的估计值其中符号|₀用于指示竖线左侧变量在t＝0时刻的值；智能体i根据状态变量估计值的初始误差，初始化其误差协方差矩阵P_i，得到P_i|₀；智能体i根据状态变量X_i的维度，初始化系统噪声矩阵Q_i；

2)在第t＝k时刻，对每个智能体的状态变量采用实时迭代方法进行实时估计，每一次迭代过程依次包括：对该状态变量进行预测、对邻近智能体进行相对距离的测量、对状态变量预测值进行本地优化、对状态变量中他体位置矢量估计值进行邻域索求与响应、对状态变量估计值进行邻域优化、对状态变量中本体位置矢量估计值进行邻域交换、对状态变量估计值进行重建，具体步骤为：

2-1)对状态变量进行预测：每个智能体根据上一时刻即k-1时刻状态变量估计值，对k时刻的状态变量进行预测；设智能体i在k时刻状态变量的预测值误差协方差矩阵预测值P_i|_k/k-1，分别为：

${\hat{X}}_i{|}_{k/k-1}={\hat{X}}_i{|}_{k-1}---\left(1\right)$

P_i|_k/k-1＝P_i|_k-1+Q_i(2)

其中，为k-1时刻的状态变量估计值，P_i|_k-1为对应的误差协方差矩阵；

2-2)对邻近智能体进行相对距离的测量：根据每个智能体i采集包含有它与邻近智能体之间相对位置信息的测量距离按式(3)得到k时刻的邻近智能体相对距离测量变量

其中，为智能体i与邻近的第n个普通智能体间相对距离测量值，为智能体i与邻近的第n个锚点间相对距离测量值，锚点指的是具有绝对定位能力的位置参考实体；

2-3)对状态变量预测值进行本地优化：具体包括以下步骤：

2-3-1)预测邻近相对距离，每个智能体i根据状态变量的预测值进一步预测邻近相对距离组成邻近测量变量的预测值

其中，为智能体i与邻近的第n个普通智能体间相对距离预测值，为智能体i与邻近的第n个锚点间相对距离预测值；

2-3-2)根据邻近测量变量预测值的维度、大小及测量条件，适应性设定测量噪声矩阵R_i；

2-3-3)以扩展卡尔曼滤波的最优化策略，对状态变量及其协方差矩阵进行优化，得到本地优化后的状态变量及其误差协方差矩阵P_i|_k-0.5，它们分别为

${\hat{X}}_i{|}_{k-0.5}={\hat{X}}_i{|}_{k/k-1}+K_i{|}_k({\tilde{Z}}_i{|}_k-{\hat{Z}}_i{|}_{k/k-1})---\left(5\right)$

$P_i{|}_{k-0.5}=(I-K_i{|}_k{H}_i{|}_k)P_i{|}_{k/k-1}{(I-K_i{|}_k{H}_i{|}_k)}^T+K_i{|}_k{R}_i{K}_i{|}_k^T---\left(6\right)$

式(5)中增益修正矩阵K_i|_k、式(6)中线性化观测矩阵H_i|_k分别为

$K_i{|}_k=P_i{|}_{k/k-1}{H}_i{|}_k{(H_i{|}_k{P}_i{|}_{k/k-1}{H}_i{|}_k^T+R_i)}^{-1}---\left(7\right)$

2-4)对状态变量中他体位置矢量估计值进行邻域索求与响应：每个智能体索求邻近智能体对该主动索求智能体位置矢量的最优估计，并响应其邻近智能体的同类索求；具体为：每个智能体i本地优化后的状态变量中均包含有对其邻近智能体的他体位置矢量的估计；每个智能体i，向其邻近智能体索求中包含的位置估计量及对应的包含于P_j|_k-0.5中的误差协方差矩阵块P_j<i>|_k-0.5；同时，每个智能体i也向其邻近智能体提供中包含的位置估计量及对应的包含于P_i|_k-0.5中的误差协方差矩阵块P_i<j>|_k-0.5；

2-5)对状态变量估计值进行邻域优化：首先计算智能体i的邻近智能体个数并计算系数ω_i，随后按式(9)式(10)对本体位置矢量的估计值及其误差协方差矩阵进行邻域优化，优化后的位置矢量估计值及其误差协方差矩阵P_i<i>|_k,block分别为

${(P_i<i>{|}_{k,block})}^{-1}=\underset{j\&Element;\left\{i\right\}\&cup;N_i}{\&Sigma;}\&lsqb;{\mathrm{\&omega;}}_i{(P_j<i>{|}_{k-0.5})}^{-1}\&rsqb;---\left(10\right)$

2-6)对状态变量中本体位置矢量估计值进行邻域交换：智能体i邻域优化后的本体位置估计值及P_i<i>|_k,block，与其邻近普通智能体的邻域优化后的本体位置估计值及P_j<j>|_k,block，进行彼此交换；

2-7)重建状态变量估计值及其协方差：根据智能体i邻域交换的本体位置估计值，按式(11)式(12)重建智能体i的状态变量的估计值及误差协方差，用于下一时刻的迭代过程；

3)每当新时刻来临，即t＝k+1时刻，依次重复步骤2-1)至2-7)进行新时刻的实时定位估计。

2.如权利要求1所述的一种多智能体分布式协同定位方法，其特征在于：所述智能体i建立状态变量中表示邻近智能体编号的排列顺序是严格单调的，即为或

3.如权利要求1所述的一种多智能体分布式协同定位方法，其特征在于：所述步骤1-2)中系统噪声矩阵Q_i为阶数等于状态变量X_i维数的对角线矩阵，其对角线元素均为正数且取值范围取于(0,1]之间。

4.如权利要求1所述的一种多智能体分布式协同定位方法，其特征在于：所述步骤2-3-2)根据邻近测量变量预测值的维度、大小及测量条件，适应性设定测量噪声矩阵R_i为矩阵阶数等于邻近测量变量预测值维数的对角矩阵，每个对角线元素与邻近测量变量预测值中对应维度元素值有关，具体关系为

式中，对于不同的正整数n而言，其中和为测量变量预测值中对应维度的元素，系数Y为正数，取值范围为Y∈(0,100]。