CN104656058B - 分布式多移动节点协作定位系统 - Google Patents

Abstract

本发明公开了一种分布式多移动节点协作定位系统，其包括若干个移动节点，每个移动节点均包括以下模块：自身移动估计模块：负责监测本移动节点在两个时间点内的有关位置信息的变化；自身定点定位估计模块：负责通过外界定位系统直接测量本移动节点的当前位置；间测距模块：负责测量本移动节点和另一个移动节点之间的位置信息；通讯模块：负责进行本移动节点和相邻的移动节点进行通讯；定位计算模块：负责把所有获得的信息进行组合并且计算本移动节点位置。本发明具有系统架构普适化，各种传感器和定位仪器都可以整合到系统中、对场景的依赖性非常小，分布式，计算复杂度和内存利用率可控等优点。

Description

分布式多移动节点协作定位系统

技术领域

本发明属于信息技术领域，尤其涉及一种分布式多移动节点协作定位系统。

背景技术

定位系统一般应用于消防员定位(消防领域)，行人定位(商业娱乐领域)，车辆定位(交通领域)，洋面及水下移动工具定位(海事领域)，机器人定位(工业领域)，动物定位(科学研究领域)。

目前，针对消防员定位，现有三种技术：1)多消防车超宽带定位技术：多量消防车在着火的建筑至少三个角定点发射强超宽带电磁波，楼内每个消防员随身携带接收器，过滤接受到的信号来估算到信号源距离，该方法消防车的信号在地下室或者大楼中信号无法穿透，因而无法定位，并且楼内和楼外无法进行信号传递，且成本非常高，中心式节点如果失效，全员无法定位；2)足部惯性导航系统：在足部放置惯性传感器，在已知初始点的情况下，利用一次积分和二次积分的方法，来估算消防员每一时刻的位置，每个人只能根据自己脚上的导航系统进行独立定位，多人员之间的信息不能共享，因此不能进行多人协作来进一步减小误差；3)多定位模块整合定位系统：整合足部惯性导航系统以及多消防车超宽带定位技术，综合利用两方面的信息进行中心定位，该方法没有利用消防队员之间的距离测量来进行定位估算，因此精确度仍然有限，存在单节点失效的问题。

目前，针对行人定位，有三种技术：1)GPS，只能进行室外定位，定位精确度有限，实时更新频率有限；2)足部惯性导航系统；3)GPS加足部惯性导航系统：全程利用足部惯性导航系统，并且定时用GPS信号矫正，在室内，有高建筑物的市区内，以及森林里，定位系统退化为足部惯性导航系统。

目前，针对车辆定位，有三种技术：1)GPS；2)车辆协作定位：利用车的里程计来估计车辆本身的在两个时间点的移动距离，并且利用摄像头来测量两辆车之间的角度和距离，利用所有这些信息组成测距图，并且利用基尔霍夫电路原理来估算车辆的位置，但是没有不确定性估计(即无方差矩阵)，因此无法估计车辆的定位估算准确度；3)GPS加车辆协作定位：利用车辆协作定位，再加GPS定时纠正，但是没有不确定性估计(即无方差矩阵)，因此无法估计车辆的定位估算准确度。

目前，针对洋面及水下移动工具定位，有三种技术：1)GPS；2)惯性定位加定时GPS：当潜水艇在水下工作时，完全利用惯性导航传感器，进行独立定位，当误差累计到一定程度时，浮出水面，用GPS进行校正，但是当多潜艇协同工作时，没有利用其它潜艇的信息；3)协作定位加定时GPS，此算法需要保存的EKF随着潜艇数量进行指数型增长，因此没有可扩展性。

目前，针对机器人定位，有两种技术：1)即时定位与地图构建(SLAM—Simultaneous localization and mapping)：利用机器人的里程器对自身进行位移估算，利用传感器对周围的环境物体进行位置估算，但是只能对自己进行独立定位，机器人之间没有协作，因此没有利用全部可以利用的信息，计算量极大；2)多机器人协作定位，利用机器人自身的里程器对两个时间点的位移进行估算，又利用传感器来测两个机器人之间的距离，然后利用优化算法，估算机器人位置，但是中心式的算法存在单节点失效，依赖于可靠地中心式通讯的缺点，分布式算法存在内存消耗和运算量不可控的问题。

发明内容

为了解决上述问题，本发明提供一种精度高、计算量小的分布式多移动节点协作定位系统。

分布式多移动节点协作定位系统，包括若干个移动节点，每个所述移动节点均包括以下模块：

一移动节点自身移动估计模块：负责监测本移动节点在两个时间点内的有关位置信息的变化，所述位置信息指的是两个时间点内笛卡尔坐标系的坐标变化；

一移动节点自身定点定位估计模块：负责通过外界定位系统直接测量本移动节点的当前位置；

一移动节点间测距模块：负责测量本移动节点和另一个移动节点在同一时刻的相对位置信息，所述相对位置信息包括相对位置坐标信息，相对位置距离信息或相对位置角度信息；

一移动节点通讯模块：负责本移动节点和相邻的移动节点或者中心节点之间进行通讯；

一移动节点定位计算模块：负责把所有获得的信息进行整合并且计算本移动节点位置。

前述的定义统一模块测量模型：

z_k＝h（x_I,k）+∈_I,k (1)

其中，z_k为k时刻的测量值，h(.)为测量函数，x_I,k为测量牵涉到的所有移动节点的位置状态集合；∈_I,k为测量误差，

对于所述移动节点自身移动估计模块，令：z_k＝u_i,k，h（.）＝f(.)，x_I,k＝(x_i,k-1,x_i,k)，∈_I,k＝w_i,k，则移动节点自身移动计算模型为：

u_i,k＝f（x_i,k-1,x_i,k）+w_i,k＝x_i,k-x_i,k-1+w_i,k (2)

其中，u_i,k是移动节点i在时间k-1到k的位移测量值，f（x_i,k-1,x_i,k）＝x_i,k-x_i,k-1为测量函数，x_i,k表示节点i在时间k的笛卡尔坐标值，x_i,k-1表示节点i在时间k-1的笛卡尔坐标值，w_i,k是移动节点自身移动的测量误差；

对于所述移动节点间测距模块，令：z_k＝v_i,j,k，h（.）＝g(.)，x_I,k＝(x_i,k,x_j,k)，∈_I,k＝e_i,j,k，则移动节点间测距计算模型为：

v_i,j,k＝g(x_i,k,x_j,k)+e_i,j,k (3)

其中，v_i,j,k是移动节点i在时刻k测量得到和移动节点i和j坐标位置有关的任何位置信息，包括相对坐标、相对距离或者相对角度，如果是相对坐标，则测量函数为：

g(x_i,k,x_j,k)＝x_i,k-x_j,k，如果是相对距离，则测量函数为：

如果是相对角度，则测量函数为： x_i,k表示节点i在时间k的笛卡尔坐标值，x_j,k表示节点j在时间k的笛卡尔坐标值，上标T表示向量或者矩阵的转置，x_i,k(1)表示向量x_i,k的第1个数，即X坐标，x_i,k(2)表示向量x_i,k的第2个数，即Y坐标，e_i,j,k是移动节点间测距的测量误差；

对于所述移动节点定位计算模块，令：z_k＝c_i,k，h（.）＝gps(.)，x_I,k＝(x_i,k)，∈_I,k＝β_i,k，则移动节点自身定点定位计算模型为：

c_i,k＝gps(x_i,k)+β_i,k (4)

其中，c_i,k是移动节点i在时刻k对自身绝对位置的测量值，gps(x_i,k)＝x_i,k是测量函数，β_i,k是移动节点自身定点定位测量误差。

前述的移动节点定位计算模块计算本移动节点位置的步骤为：

1)移动节点i保存五个数据值(id,G_i(k),L_i(k),Q_i,bel(x_i,k))，其中，id为移动节点的Id、G_i(k)为k时刻的二元信息关系图、L_i(k)为k时刻的二元信息关系图中的所有状态点Id、Q_i为边缘化阈值，为k时刻本移动节点对于自身位置估算和准确度的信念，表示本移动节点k时间对自身位置的位置估计，表示本移动节点对自身位置的位置估算方差；

2)当本移动节点向前移动并且得到从移动节点自身移动估计模块的测量值时，执行以下步骤：

2-1)把测量值转化为二元信息关系元组，并且加入到二元信息关系图G_i(k)中，

2-2)加入一个新的状态点Id：i.k到L_i(k)，i.k表示k时刻的移动节点i；

2-3)用公式(5)和(6)更新自身位置估计信息

其中，和分别表示更新后的位置估计和位置估算方差，和分别表示更新前的位置估计和位置估算方差，R_I,k为k时间i节点自身移动的测量误差方差矩阵，z_k为k时间的测量值；

3)当本移动节点得到自身定点定位测量值时，执行以下步骤：

3-1)把测量值转化为二元信息关系元组，并且加入到二元信息关系图G_i(k)中；

3-2)用公式(7)和(8)更新自身位置估计信息bel(x_i,k)

其中，和分别表示更新后的位置估计和位置估算方差，和分别表示更新前的位置估计和位置估算方差，R_I,k为k时间i节点自身定点定位的测量误差方差矩阵，z_k为k时间的测量值；

4)当移动节点i遇到另外一个移动节点j的时候，执行以下步骤：

4-1)两个移动节点互相测距并且把测量值转换成二元信息关系元组；

4-2)把当前的二元信息关系元组信息通过移动节点通讯模块和对方共享；

4-3)两个节点把各自所缺失的新的二元信息关系元组加入到二元信息关系图中，并更新L_i(k)和L_j(k)。

前述的步骤1)中二元信息关系图是指，将移动节点在一个时刻的位置定义为一个状态点，各个状态点之间都由二元信息关系元组连接；移动节点自身移动估计模块得到的二元信息关系元组连接本移动节点在前后时刻的状态点，而移动节点间测距模块得到的二元信息关系元组连接同一时刻两个移动节点的状态点；移动节点自身定点定位估计模块得到的二元信息关系元组连接某一时刻自身状态点和全球坐标系原点；当上述所有二元信息关系元组连接而得到的关系图，即为二元信息关系图。

前述的步骤2)，步骤3)，步骤4)中，所述把测量值转换为二元信息关系元组，所述二元信息关系元组ξ_ij(k)为：

其中，i和j是所测量的两个状态点的Id，代表在时间k关于状态点i和j位置关系测量的信息关系值和信息关系矩阵；

所述信息关系值为：

所述信息关系矩阵为：

其中，R_I,k为k时间测量误差方差矩阵；z_k为k时间测量值；是在k时间测量之前的对于牵涉到测量之中的所有移动节点的位置估计；是对于此测量的估计值；代表函数h(.)对牵涉到测量之中的各个移动节点的位置变量在其估计值上的偏导。

前述的把二元信息关系元组加入到二元信息关系图G_i(k)中是指：假设二元信息关系图中所有状态点的估算位置总向量是方差矩阵是P，则定义：

信息矩阵Y：Y＝P^-1 (12)

信息向量

通过公式(14)更新信息二元信息关系图的信息向量和信息矩阵：

其中和表示更新后的信息向量和信息矩阵，为测量对应的两个状态点的位置估算信息向量的总向量，为不参与此次测量的状态点的位置估算信息向量的总向量，Y₁₁表示不参与此次测量的状态点的信息矩阵，Y₂₂为参加此次测量的状态点的信息矩阵，Y₁₂和Y₂₁分别为参加此次测量和不参加此次测量的状态点之间的协信息矩阵，i表示新的二元信息关系元组的信息关系值，I表示新的二元信息关系元组的信息关系矩阵；

如果二元信息关系图中多了一个状态点，此状态点与某一个老的状态点通过一个二元信息关系发生联系，则通过如下方式更新信息矩阵和信息向量：

首先用0来扩充新加入的状态点的对应位置；

令

则，

其中，为不参与此次测量的状态点的位置估算信息向量的总向量，为测量对应的包含的老的状态点的两个状态点的位置估算信息向量的总向量，Y₁₁表示不参与此次测量的状态点的信息矩阵，Y₂₂为参加此次测量的老的状态点的信息矩阵，Y₁₂和Y₂₁分别为不参加此次测量的所有状态点和参加此次测量的老的状态点之间的协信息矩阵，为此次牵涉到测量值中新老状态点的位置估算信息向量的总向量，Y₂₃为此次牵涉到测量值中新老状态点的位置估算信息矩阵，i_2,3为此次测量的信息关系值，I_2,3为此次测量的信息关系矩阵值。

前述的移动节点定位计算模块计算本移动节点位置的过程中，移动节点随着不断前进或者遇到别的移动节点，G_i(k)会不断增大，当G_i(k)增大到超过边缘化阈值Q_i时，节点i需要进行缩减二元信息关系图操作：

i)找出需要进行缩减操作的状态点：缩减操作以时间和移动节点为顺序，具有最老的时间的状态节点先进行缩减，具有相同时间戳的状态节点则移动节点小的先进行缩减，但相同时间戳的所有状态节点中，自身节点最后进行缩减；

ii)状态点的缩减操作：通过以下公式对状态点2进行缩减操作，下标1代表除了2的所有状态点，

其中，代表缩减后的信息向量，代表缩减后的信息矩阵，和分别为需要保留的状态点的综合信息向量以及需要缩减的状态点的综合信息向量，Y₁₁、Y₁₂和Y₂₂分别表示需要保留的状态点的综合信息矩阵，需要保留的状态点和需要缩减的状态点的综合相关信息矩阵，以及需要缩减的状态点的综合信息矩阵。

iii)缩减完毕后，被缩减的状态点的上一个节点成为新的基状态点，需要对新的基状态点的位置进行估算，更新基状态点的位置信念集合bel(X_B)；

iv)每缩减一个状态点，重新判断当前G_i(k)大小是否在边缘化阈值Q_i之内，如果是则停止缩减，不是则重复进行上述步骤i)-iii)的操作。

前述的边缘化阈值Q_i的定义是G_i(k)占用的内存大小，或者总的二元信息关系元组的个数|γ_i(k)|，或者是总的二元信息关系元组的个数加上基状态点数|γ_i(k)|+|X_B|。

前述的还包括一个或多个中心节点，所述移动节点均能通过所述移动节点通讯模块与所述中心节点通讯。

前述的外界定位系统为卫星定位系统，包括GPS定位系统、北斗定位系统、格洛纳斯定位系统或伽利略定位系统。

前述的移动节点自身移动估计模块通过惯性元件监测本移动节点在两个时间点内的有关位置信息的变化。

前述的移动节点间测距模块采用电磁波强度距离估算法，超声波与光速度差距离估算法或激光测距法测量本移动节点和另一个移动节点之间的位置信息。

本发明的多移动节点协作定位系统具有以下优点：

多种距离测量融合，精确度大大提高。

兼容但不依赖于GPS，有GPS信号的时候累计误差可以被实时校正，没有GPS的时候，误差累计速率很慢。

系统架构普适化，各种传感器和定位仪器都可以整合到系统中。

对场景的依赖性非常小(比如消防员在金属楼，地下室或者高楼都可以通过本系统进行定位)。

完全的分布式异步定位算法，不依赖于固定中心点。

内存和运算量可控(取决于Q_i的设置)。

附图说明

图1是本发明实施例中的二元信息关系图；

图2是本发明实施例中相遇之前移动节点1和移动节点3的二元信息关系图；

图3是本发明实施例中相遇后的移动节点3的二元信息关系图。

具体实施方式

下面结合附图和具体实施方式对本发明作进一步详细的描述。

本发明的分布式多移动节点协作定位系统，包括若干个移动节点，每个移动节点均包括以下模块：

移动节点自身移动估计模块(DR)：通过惯性元件监测本移动节点自身在两个时间点内的有关位置信息的变化，如位移变化，距离变化，角度变化等，位置信息指的是两个时间点内笛卡尔坐标系的坐标变化。

移动节点间测距模块(IM)：用来测量本移动节点和另一个移动节点在同一时刻的相对位置信息，比如相对位置坐标信息，相对位置距离信息，相对位置角度信息等。该模块采用电磁波强度距离估算法，超声波与光速度差距离估算法或激光测距法测量本移动节点和另一个移动节点之间的位置信息。

移动节点自身定点定位估计模块(SP)：通过外界定位系统直接测量移动节点当前位置，比如GPS定位系统、北斗定位系统、格洛纳斯定位系统或伽利略定位系统。

移动节点通讯模块(RF)：用来进行和周围移动节点，或者中心节点进行通讯。

移动节点定位计算模块(CU)：用来把所有获得的信息进行融合并且估算本移动节点位置。

注意：上述所有模块在分布式系统中一般都位于移动节点上。

以消防员系统为例，系统结构可以采用：

移动节点自身移动估计模块(DR)：足部惯性导航系统。

移动节点间测距模块(IM)：电磁波强度距离估算法，超声波与光速度差距离估算法，激光测距法等。

移动节点自身定点定位估计模块(SP)：GPS、北斗等卫星导航系统。

移动节点通讯模块(RF)：任何合法频段的RF模块。

移动节点定位计算模块(CU)：任何具有计算能力的便携式处理器。

对于移动节点的定位计算，现阶段主要存在如下几种分布式算法：

平均值估计：把所有关于对当前自身节点的位置信息进行平均。缺点是没有考虑各种位置信息的准确度，对所有信息同等对待；另外，没有考虑各种信息之间存在的相关性，因此无法得出比较准确的方差矩阵。

加权平均估计：把所有关于对当前自身节点的位置信息进行加权平均，权值和误差的方差成反比例关系(对于方差矩阵同理)。缺点是没有考虑各种信息之间存在的相关性，因此无法得出比较准确的方差矩阵。

集合所有的测量信息，用卡尔曼滤波器，信息滤波器或者传统的优化算法进行估计位置，并且利用边缘化(marginalization)把保证可以安全过滤的历史测量值融合掉。缺点是此方法不能保证内存消耗和计算量是有限的。

在移动节点的定位计算之前，本发明首先定义了统一模块测量模型：

z_k＝h（x_I,k）+∈_I,k (1)

其中，z_k为k时刻的测量值，h(.)为测量函数，x_I,k为测量牵涉到的所有移动节点的位置状态集合，∈_I,k为测量误差，其测量误差方差矩阵为R_I,k。

对于移动节点自身移动估计模块，令：z_k＝u_i,k，h（.）＝f(.)，x_I,k＝(x_i,k-1,x_i,k)，∈_I,k＝w_i,k，R_I,k＝W_i,k，则移动节点自身移动计算模型为：

u_i,k＝f（x_i,k-1,x_i,k）+w_i,k＝x_i,k-x_i,k-1+w_i,k (2)

其中，u_i,k是移动节点i在时间k-1到k的位移测量值，f（x_i,k-1,x_i,k）＝x_i,k-x_i,k-1为测量函数，x_i,k表示节点i在时间k的笛卡尔坐标值，x_i,k-1表示节点i在时间k-1的笛卡尔坐标值，w_i,k是移动节点自身移动的测量误差，其测量误差方差矩阵为W_i,k。

对于移动节点间测距模块，令：z_k＝v_i,j,k，h（.）＝g(.)，x_I,k＝(x_i,k,x_j,k)，∈_I,k＝e_i,j,k，R_I,k＝E_i,j,k，则移动节点间测距计算模型为：

v_i,j,k＝g(x_i,k,x_j,k)+e_i,j,k (3)

其中，v_i,j,k是移动节点i在时刻k测量得到和移动节点i和j坐标位置有关的任何位置信息，比如相对坐标、相对距离或者相对角度，如果是相对坐标，则测量函数为：

g(x_i,k,x_j,k)＝x_i,k-x_j,k，如果是相对距离，则测量函数为：

如果是相对角度，则测量函数为： x_i,k表示节点i在时间k的笛卡尔坐标值，x_j,k表示节点j在时间k的笛卡尔坐标值，上标T表示向量或者矩阵的转置，x_i,k(1)表示向量x_i,k的第1个数，即X坐标，x_i,k(2)表示向量x_i,k的第2个数，即Y坐标，e_i,j,k是移动节点间测距的测量误差，其测量误差方差矩阵是E_i,j,k。

对于所述移动节点定位计算模块，令：z_k＝c_i,k，h(.)＝gps(.)，x_I,k＝(x_i,k)，∈_I,k＝β_i,k，R_I,k＝B_i,k，则移动节点自身定点定位计算模型为：

c_i,k＝gps(x_i,k)+β_i,k (4)

其中，c_i,k是移动节点i在时刻k对自身绝对位置的测量值，gps(x_i,k)＝x_i,k是测量函数，β_i,k是移动节点自身定点定位测量误差，其测量误差方差矩阵是B_i,k。

对于测量误差方差矩阵，在二维空间中，其中c_xx代表测量误差在x坐标的方差，c_xy和c_yx代表测量误差在x坐标和y坐标的相关，c_yy代表测量误差在y坐标的方差。其余各方差矩阵同理。

本发明中移动节点定位计算模块计算本移动节点位置的步骤为：

1)移动节点i保存五个数据值(id,G_i(k),L_i(k),Q_i,bel(x_i,k))，其中，id为移动节点的Id，由节点编号和时间戳组成、G_i(k)为k时刻的二元信息关系图、L_i(k)为k时刻的二元信息关系图中的所有状态点Id、Q_i为边缘化阈值，为k时刻本移动节点对于自身位置估算和准确度的信念，表示本移动节点k时间对自身位置的位置估计，表示本移动节点对自身位置的位置估算方差；G_i（k）由两个部分组成，分别是二元信息关系组集合Y_i(k)，以及基状态点的位置信念集合表示所有基状态点的位置估算，P_B表示所有基状态点的位置估算方差；假设本移动节点的初始位置bel(x_i(0))是已知；

中和定义如下：

在k时刻，移动节点i所估算的移动节点1到m的位置；即表示移动节点k时间对自身位置的位置估计；

在k时刻，移动节点i的位置估算方差矩阵，m代表k时刻移动节点i只保留了m个移动节点的信息；Pⁱ _j,j,k代表移动节点i认为移动节点j的位置估算方差子矩阵；Pⁱ _j,t,k代表移动节点i认为移动节点j和t之间的位置估算相关矩阵；即表示移动节点对自身位置的位置估算方差。

2)当本移动节点向前移动并且得到从移动节点自身移动估计模块的测量值时，执行以下步骤：

2-1)把测量值转化为二元信息关系元组，并且加入到二元信息关系图G_i(k)中，

2-2)加入一个新的状态点Id：i.k到L_i(k)，i.k表示k时刻的移动节点i；

2-3)用公式(5)和(6)更新自身位置估计信息

其中，和分别表示更新后的位置估计和位置估算方差，和分别表示更新前的位置估计和位置估算方差，R_I,k为k时间i节点自身移动的测量误差方差矩阵，z_k为k时间的测量值；

3)当本移动节点得到自身定点定位测量值时，执行以下步骤：

3-1)把测量值转化为二元信息关系元组，并且加入到二元信息关系图G_i(k)中；

3-2)用公式(7)和(8)更新自身位置估计信息

其中，和分别表示更新后的位置估计和位置估算方差，和分别表示更新前的位置估计和位置估算方差，R_I,k为k时间i节点自身定点定位的测量误差方差矩阵，z_k为k时间的测量值；

4)当移动节点i遇到另外一个移动节点j的时候，执行以下步骤：

4-1)两个移动节点互相测距并将测量值转换成二元信息关系元组；

4-2)把当前的二元信息关系元组信息通过移动节点通讯模块和对方共享，通过结构对比，分析出对方差什么二元信息关系元组，把对方缺的二元信息关系元组发给对方；

4-3)两个节点把各自所缺失的新的二元信息关系元组加入到二元信息关系图中，并更新L_i(k)和L_j(k)。

上述二元信息关系图是指，将移动节点在一个时刻的位置定义为一个状态点，各个状态点之间都由二元信息关系元组连接；移动节点自身移动估计模块得到的二元信息关系元组连接本移动节点在前后时刻的状态点，而移动节点间测距模块得到的二元信息关系元组连接同一时刻两个移动节点的状态点；移动节点自身定点定位估计模块得到的二元信息关系元组连接某一时刻自身状态点和全球坐标系原点；当上述所有二元信息关系元组连接而得到的关系图，即为二元信息关系图。如图1所示的信息关系图，图中每一个状态点的Id的形式为{Id.时间戳}，比如状态点3.2代表移动节点3在时刻2的位置，状态点0代表全球坐标系原点；图中的箭头代表二元信息关系元组，横向箭头是移动节点间测距模块得到的二元信息关系元组，比如7，垂直方向的箭头是移动节点自身移动模块得到的二元信息关系元组，比如2，曲线连接到状态点0的二元信息关系元组代表移动节点自身定点定位移动模块得到的测量。虚线连接的状态点表示此刻我们知道的状态点的位置信念，称为基状态点，比如状态点2.1。

上述二元信息关系元组指的是，任何关于移动节点i和j的信息位置的测量都可以转化为一个二元信息关系元组；i和j分别是两个移动节点的ID，对于移动节点自身定点定位估计模块得到的一元测量值，j＝0，

将测量值转换成二元信息关系元组，二元信息关系元组ξ_ij(k)为：

其中，i和j是此次二元测量值所测量的两个状态点的编号，代表在时间k关于状态点i和j位置关系测量的信息关系值和信息关系矩阵；

其中，信息关系值为：

信息关系矩阵为：

式中，R_I,k为k时间测量误差方差矩阵；z_k为k时间测量值；是在k时间测量之前的对于牵涉到测量之中的所有移动节点的位置估计；是对于此测量的估计值；代表函数h(.)对牵涉到测量之中的各个移动节点的位置变量在其估计值上的偏导。

上述把二元信息关系元组加入到二元信息关系图G_i(k)中是指：假设二元信息关系图中所有状态点的估算位置总向量是方差矩阵是P，则定义：

信息矩阵Y：Y＝P^-1 (12)

信息向量

通过公式(14)更新信息二元信息关系图的信息向量和信息矩阵：

其中和表示更新后的信息向量和信息矩阵，为测量对应的两个状态点的位置估算信息向量的总向量，为不参与此次测量的状态点的位置估算信息向量的总向量，Y₁₁表示不参与此次测量的状态点的信息矩阵，Y₂₂为参加此次测量的状态点的信息矩阵，Y₁₂和Y₂₁分别为参加此次测量和不参加此次测量的状态点之间的协信息矩阵，i表示新的二元信息关系元组的信息关系值，I表示新的二元信息关系元组的信息关系矩阵；

如果二元信息关系图中多了一个新状态点，此状态点与某一个老的状态点通过一个二元信息关系发生联系，则通过如下方式更新信息矩阵和信息向量：

首先用0来扩充新加入的状态点的对应位置；

令

则，

其中，为不参与此次测量的状态点的位置估算信息向量的总向量，为测量对应的包含的老的状态点的两个状态点的位置估算信息向量的总向量，Y₁₁表示不参与此次测量的状态点的信息矩阵，Y₂₂为参加此次测量的老的状态点的信息矩阵，Y₁₂和Y₂₁分别为不参加此次测量的所有状态点和参加此次测量的老的状态点之间的协信息矩阵，为此次牵涉到测量值中新老状态点的位置估算信息向量的总向量，Y₂₃为此次牵涉到测量值中新老状态点的位置估算信息矩阵，i_2,3为此次测量的信息关系值，I_2,3为此次测量的信息关系矩阵值。

在移动节点定位计算模块计算本移动节点位置的过程中，移动节点随着不断前进或者遇到别的移动节点，G_i(k)会不断增大，当G_i(k)增大到超过边缘化阈值Q_i时，节点i需要进行缩减二元信息关系图操作：

i)找出需要进行缩减操作的状态点：缩减操作以时间和移动节点为顺序，具有最老的时间的状态节点先进行缩减，具有相同时间戳的状态节点则移动节点小的先进行缩减，但相同时间戳的所有状态节点中，自身节点最后进行缩减。如图2的相遇之前移动节点1和移动节点3的二元信息关系图中，左图的移动节点1G_i(k)中的状态点缩减顺序是3.1,2.2,3.2,1.3和1.4。图中纵轴的1，2，3，4代表时刻，横轴代表移动节点。

ii)状态点的缩减操作：通过以下公式对状态点2进行缩减操作，下标1代表除了2的所有状态点，

其中，代表缩减后的信息向量，代表缩减后的信息矩阵，和分别为需要保留的状态点的综合信息向量以及需要缩减的状态点的综合信息向量，Y₁₁、Y₁₂和Y₂₂分别表示需要保留的状态点的综合信息矩阵，需要保留的状态点和需要缩减的状态点的综合相关信息矩阵，以及需要缩减的状态点的综合信息矩阵；

iii)缩减完毕后，被缩减的状态点的上一个节点成为新的基状态点，需要对新的基状态点的位置进行估算，更新基状态点的位置信念集合bel(X_B)；

iv)每缩减一个状态点，重新判断当前G_i(k)大小是否在边缘化阈值Q_i之内，如果是则停止缩减，不是则重复进行上述步骤i)-iii)的操作。边缘化阈值Q_i的定义可以是G_i(k)占用的内存大小，或者总的二元信息关系元组的个数|γ_i(k)|，或者是总的二元信息关系元组的个数加上基状态点数|γ_i(k)|+|X_B|。

图3为相遇后的移动节点3的二元信息关系图。其中二元信息关系7是节点间测距对应的二元信息关系。图3中，移动节点3新增加的状态点(1.3，1.4和2.3)和二元信息关系(4,5,6和7)，以及更新的基状态点位置信念(1.3)。

以上实施例仅为本发明其中的一种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (10)

1.分布式多移动节点协作定位系统，包括若干个移动节点，其特征在于，每个所述移动节点均包括以下模块：

一移动节点自身移动估计模块：负责监测本移动节点在两个时间点内的有关位置信息的变化，所述位置信息指的是两个时间点内笛卡尔坐标系的坐标变化；

一移动节点自身定点定位估计模块：负责通过外界定位系统直接测量本移动节点的当前位置；

一移动节点间测距模块：负责测量本移动节点和另一个移动节点在同一时刻的相对位置信息，所述相对位置信息包括相对位置坐标信息，相对位置距离信息或相对位置角度信息；

一移动节点通讯模块：负责本移动节点和相邻的移动节点或者中心节点之间进行通讯；

一移动节点定位计算模块：负责把所有获得的信息进行整合并且计算本移动节点位置；

定义统一模块测量模型：

z_k＝h(x_I,k)+∈_I,k (1)

其中，z_k为k时刻的测量值，h(.)为测量函数，x_I,k为测量牵涉到的所有移动节点的位置状态集合；∈_I,k为测量误差，

对于所述移动节点自身移动估计模块，令：z_k＝u_i,k，h(.)＝f(.)，x_I,k＝(x_i，k-1，x_i,k)，∈_I,k＝w_i,k，则移动节点自身移动计算模型为：

u_i,k＝f(x_i，k-1,x_i,k)+w_i,k＝x_i,k-x_i,k-1+w_i,k (2)

其中，u_i,k是移动节点i在时间k-1到k的位移测量值，f(x_i,k-1,x_i,k)＝x_i,k-x_i,k-1为测量函数，x_i,k表示节点i在时间k的笛卡尔坐标值，x_i,k-1表示节点i在时间k-1的笛卡尔坐标值，w_i，k是移动节点自身移动的测量误差；

对于所述移动节点间测距模块，令：z_k＝v_i，j，k，h(.)＝g(.)，x_I，k＝(x_i，k，x_j，k)，∈_I，k＝e_i，j，k，则移动节点间测距计算模型为：

v_i,j,k＝g(x_i,k,x_j,k)+e_i,j,k (3)

其中，v_i,j,k是移动节点i在时刻k测量得到和移动节点i和j坐标位置有关的任何位置信息，包括相对坐标、相对距离或者相对角度，如果是相对坐标，则测量函数为：g(x_i,k,x_j,k)＝x_i,k-x_j,k，如果是相对距离，则测量函数为：如果是相对角度，则测量函数为： x_i,k表示节点i在时间k的笛卡尔坐标值，x_j,k表示节点j在时间k的笛卡尔坐标值，上标T表示向量或者矩阵的转置，x_i,k(1)表示向量x_i,k的第1个数，即X坐标，x_i,k(2)表示向量x_i,k的第2个数，即Y坐标，e_i,j，k是移动节点间测距的测量误差；

对于所述移动节点定位计算模块，令：z_k＝c_i,k，h(.)＝gps(.)，x_I,k＝(x_i，k)，∈_I，k＝β_i,k，则移动节点自身定点定位计算模型为：

c_i,k＝gps(x_i,k)+β_i,k (4)

其中，c_i,k是移动节点i在时刻k对自身绝对位置的测量值，gps(x_i,k)＝x_i，k是测量函数，β_i，k是移动节点自身定点定位测量误差；

所述移动节点定位计算模块计算本移动节点位置的步骤为：

1)移动节点i保存五个数据值(id,G_i(k),L_i(k),Q_i,bel(x_i,k))，其中，id为移动节点的Id、G_i(k)为k时刻的二元信息关系图、L_i(k)为k时刻的二元信息关系图中的所有状态点Id、Q_i为边缘化阈值，为k时刻本移动节点对于自身位置估算和准确度的信念，表示本移动节点k时间对自身位置的位置估计，表示本移动节点对自身位置的位置估算方差；

2)当本移动节点向前移动并且得到从移动节点自身移动估计模块的测量值时，执行以下步骤：

2-1)把测量值转化为二元信息关系元组，并且加入到二元信息关系图G_i(k)中，

2-2)加入一个新的状态点Id：i.k到L_i(k)，i.k表示k时刻的移动节点i；

2-3)用公式(5)和(6)更新自身位置估计信息

${{\hat{x}}_{i,k}^i}^{+}={{\hat{x}}_{i,k}^i}^{-}+z_k---\left(5\right)$

${P_{i,i,k}^i}^{+}={P_{i,i,k}^i}^{-}+R_{I,k}---\left(6\right)$

其中，和分别表示更新后的位置估计和位置估算方差，和分别表示更新前的位置估计和位置估算方差，R_I，k为k时间i节点自身移动的测量误差方差矩阵，z_k为k时间的测量值；

3)当本移动节点得到自身定点定位测量值时，执行以下步骤：

3-1)把测量值转化为二元信息关系元组，并且加入到二元信息关系图G_i(k)中；

3-2)用公式(7)和(8)更新自身位置估计信息bel(x_i,k)

${{\hat{x}}_{i,k}^i}^{+}={P_{i,i,k}^i}^{-}{({P_{i,i,k}^i}^{-}+R_{I,k})}^{-1}{z}_k+R_{I,k}{({P_{i,i,k}^i}^{-}+R_{I,k})}^{-1}{{\hat{x}}_{i,k}^i}^{-}---\left(7\right)$

${P_{i,i,k}^i}^{+}={P_{i,i,k}^i}^{-}{R}_{I,k}{({P_{i,i,k}^i}^{-}+R_{I,k})}^{-1}---\left(8\right)$

其中，和分别表示更新后的位置估计和位置估算方差，和分别表示更新前的位置估计和位置估算方差，R_I,k为k时间i节点自身定点定位的测量误差方差矩阵，z_k为k时间的测量值；

4)当移动节点i遇到另外一个移动节点j的时候，执行以下步骤：

4-1)两个移动节点互相测距并且把测量值转换成二元信息关系元组；

4-2)把当前的二元信息关系元组信息通过移动节点通讯模块和对方共享；

4-3)两个节点把各自所缺失的新的二元信息关系元组加入到二元信息关系图中，并更新L_i(k)和L_j(k)。

2.根据权利要求1所述的分布式多移动节点协作定位系统，其特征在于：所述步骤1)中二元信息关系图是指，将移动节点在一个时刻的位置定义为一个状态点，各个状态点之间都由二元信息关系元组连接；移动节点自身移动估计模块得到的二元信息关系元组连接本移动节点在前后时刻的状态点，而移动节点间测距模块得到的二元信息关系元组连接同一时刻两个移动节点的状态点；移动节点自身定点定位估计模块得到的二元信息关系元组连接某一时刻自身状态点和全球坐标系原点；当上述所有二元信息关系元组连接而得到的关系图，即为二元信息关系图。

3.根据权利要求1所述的分布式多移动节点协作定位系统，其特征在于：所述步骤2)，步骤3)，步骤4)中，所述把测量值转换为二元信息关系元组，所述二元信息关系元组ξ_ij(k)为：

${\mathrm{\ξ}}_{ij}\left(k\right)=(i_k^{ij},I_k^{ij})---\left(9\right)$

其中，i和j是所测量的两个状态点的Id，代表在时间k关于状态点i和j位置关系测量的信息关系值和信息关系矩阵；

所述信息关系值为：

$i_k^{ij}=H_k^T{R}_{I,k}^{-1}(z_k-h({\hat{x}}_{I,k}^{-})+H_k{\hat{x}}_{I,k}^{-})---\left(10\right)$

所述信息关系矩阵为：

$I_k^{ij}=H_k^T{R}_{I,k}^{-1}{H}_k---\left(11\right)$

其中，R_I,k为k时间测量误差方差矩阵；z_k为k时间测量值；是在k时间测量之前的对于牵涉到测量之中的所有移动节点的位置估计；是对于此测量的估计值； 代表函数h(.)对牵涉到测量之中的各个移动节点的位置变量在其估计值上的偏导。

4.根据权利要求1所述的分布式多移动节点协作定位系统，其特征在于：所述把二元信息关系元组加入到二元信息关系图G_i(k)中是指：假设二元信息关系图中所有状态点的估算位置总向量是方差矩阵是P，则定义：

信息矩阵Y：Y＝P^-1 (12)

信息向量

通过公式(14)更新信息二元信息关系图的信息向量和信息矩阵：

${\hat{y}}^{+}=\left[\begin{array}{c}{\hat{y}}_1\\ {\hat{y}}_2+i\end{array}\right],$

$Y^{+}=\left[\begin{array}{cc}{Y}_{11}& Y_{12}\\ Y_{21}& Y_{22}+I\end{array}\right]---\left(14\right)$

其中和表示更新后的信息向量和信息矩阵，为测量对应的两个状态点的位置估算信息向量的总向量，为不参与此次测量的状态点的位置估算信息向量的总向量，Y₁₁表示不参与此次测量的状态点的信息矩阵，Y₂₂为参加此次测量的状态点的信息矩阵，Y₁₂和Y₂₁分别为参加此次测量和不参加此次测量的状态点之间的协信息矩阵，i表示新的二元信息关系元组的信息关系值，I表示新的二元信息关系元组的信息关系矩阵；

如果二元信息关系图中多了一个状态点，此状态点与某一个老的状态点通过一个二元信息关系发生联系，则通过如下方式更新信息矩阵和信息向量：

首先用0来扩充新加入的状态点的对应位置；

令

则，

其中，为不参与此次测量的状态点的位置估算信息向量的总向量，为测量对应的包含的老的状态点的两个状态点的位置估算信息向量的总向量，Y₁₁表示不参与此次测量的状态点的信息矩阵，Y₂₂为参加此次测量的老的状态点的信息矩阵，Y₁₂和Y₂₁分别为不参加此次测量的所有状态点和参加此次测量的老的状态点之间的协信息矩阵，为此次牵涉到测量值中新老状态点的位置估算信息向量的总向量，Y₂₃为此次牵涉到测量值中新老状态点的位置估算信息矩阵，i_2,3为此次测量的信息关系值，I_2,3为此次测量的信息关系矩阵值。

5.根据权利要求1所述的分布式多移动节点协作定位系统，其特征在于：所述移动节点定位计算模块计算本移动节点位置的过程中，移动节点随着不断前进或者遇到别的移动节点，G_i(k)会不断增大，当G_i(k)增大到超过边缘化阈值Q_i时，节点i需要进行缩减二元信息关系图操作：

i)找出需要进行缩减操作的状态点：缩减操作以时间和移动节点为顺序，具有最老的时间的状态节点先进行缩减，具有相同时间戳的状态节点则移动节点小的先进行缩减，但相同时间戳的所有状态节点中，自身节点最后进行缩减；

ii)状态点的缩减操作：通过以下公式对状态点2进行缩减操作，下标1代表除了2的所有状态点，

${\hat{y}}_1^m={\hat{y}}_1-Y_{12}{Y}_{22}^{-1}{\hat{y}}_2$

$Y_{11}^m=Y_{11}-Y_{12}{Y}_{22}^{-1}{Y}_{12}^T---\left(17\right)$

其中，代表缩减后的信息向量，代表缩减后的信息矩阵，和分别为需要保留的状态点的综合信息向量以及需要缩减的状态点的综合信息向量，Y₁₁、Y₁₂和Y₂₂分别表示需要保留的状态点的综合信息矩阵，需要保留的状态点和需要缩减的状态点的综合相关信息矩阵，以及需要缩减的状态点的综合信息矩阵；

iii)缩减完毕后，被缩减的状态点的上一个节点成为新的基状态点，需要对新的基状态点的位置进行估算，更新基状态点的位置信念集合bel(X_B)；

iv)每缩减一个状态点，重新判断当前G_i(k)大小是否在边缘化阈值Q_i之内，如果是则停止缩减，不是则重复进行上述步骤i)-iii)的操作。

6.根据权利要求5所述的分布式多移动节点协作定位系统，其特征在于：所述边缘化阈值Q_i的定义是G_i(k)占用的内存大小，或者总的二元信息关系元组的个数|γ_i(k)|，或者是总的二元信息关系元组的个数加上基状态点数|γ_i(k)|+|X_B|。

7.根据权利要求1所述的分布式多移动节点协作定位系统，其特征在于：还包括一个或多个中心节点，所述移动节点均能通过所述移动节点通讯模块与所述中心节点通讯。

8.根据权利要求1所述的分布式多移动节点协作定位系统，其特征在于：所述外界定位系统为卫星定位系统，包括GPS定位系统、北斗定位系统、格洛纳斯定位系统或伽利略定位系统。

9.根据权利要求1所述的分布式多移动节点协作定位系统，其特征在于：所述移动节点自身移动估计模块通过惯性元件监测本移动节点在两个时间点内的有关位置信息的变化。

10.根据权利要求1所述的分布式多移动节点协作定位系统，其特征在于：所述移动节点间测距模块采用电磁波强度距离估算法，超声波与光速度差距离估算法或激光测距法测量本移动节点和另一个移动节点之间的位置信息。