[发明专利]分布式多移动节点协作定位系统

摘要

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

主权项

分布式多移动节点协作定位系统，包括若干个移动节点，其特征在于，每个所述移动节点均包括以下模块：一移动节点自身移动估计模块：负责监测本移动节点在两个时间点内的有关位置信息的变化，所述位置信息指的是两个时间点内笛卡尔坐标系的坐标变化；一移动节点自身定点定位估计模块：负责通过外界定位系统直接测量本移动节点的当前位置；一移动节点间测距模块：负责测量本移动节点和另一个移动节点在同一时刻的相对位置信息，所述相对位置信息包括相对位置坐标信息，相对位置距离信息或相对位置角度信息；一移动节点通讯模块：负责本移动节点和相邻的移动节点或者中心节点之间进行通讯；一移动节点定位计算模块：负责把所有获得的信息进行整合并且计算本移动节点位置；定义统一模块测量模型：zk＝h(xI,k)+∈I,k                                               (1)其中，zk为k时刻的测量值，h(.)为测量函数，xI,k为测量牵涉到的所有移动节点的位置状态集合；∈I,k为测量误差，对于所述移动节点自身移动估计模块，令：zk＝ui,k，h(.)＝f(.)，xI,k＝(xi，k‑1，xi,k)，∈I,k＝wi,k，则移动节点自身移动计算模型为：ui,k＝f(xi，k‑1,xi,k)+wi,k＝xi,k‑xi,k‑1+wi,k                           (2)其中，ui,k是移动节点i在时间k‑1到k的位移测量值，f(xi,k‑1,xi,k)＝xi,k‑xi,k‑1为测量函数，xi,k表示节点i在时间k的笛卡尔坐标值，xi,k‑1表示节点i在时间k‑1的笛卡尔坐标值，wi，k是移动节点自身移动的测量误差；对于所述移动节点间测距模块，令：zk＝vi，j，k，h(.)＝g(.)，xI，k＝(xi，k，xj，k)，∈I，k＝ei，j，k，则移动节点间测距计算模型为：vi,j,k＝g(xi,k,xj,k)+ei,j,k                                               (3)其中，vi,j,k是移动节点i在时刻k测量得到和移动节点i和j坐标位置有关的任何位置信息，包括相对坐标、相对距离或者相对角度，如果是相对坐标，则测量函数为：g(xi,k,xj,k)＝xi,k‑xj,k，如果是相对距离，则测量函数为：如果是相对角度，则测量函数为：xi,k表示节点i在时间k的笛卡尔坐标值，xj,k表示节点j在时间k的笛卡尔坐标值，上标T表示向量或者矩阵的转置，xi,k(1)表示向量xi,k的第1个数，即X坐标，xi,k(2)表示向量xi,k的第2个数，即Y坐标，ei,j，k是移动节点间测距的测量误差；对于所述移动节点定位计算模块，令：zk＝ci,k，h(.)＝gps(.)，xI,k＝(xi，k)，∈I，k＝βi,k，则移动节点自身定点定位计算模型为：ci,k＝gps(xi,k)+βi,k                                             (4)其中，ci,k是移动节点i在时刻k对自身绝对位置的测量值，gps(xi,k)＝xi，k是测量函数，βi，k是移动节点自身定点定位测量误差；所述移动节点定位计算模块计算本移动节点位置的步骤为：1)移动节点i保存五个数据值(id,Gi(k),Li(k),Qi,bel(xi,k))，其中，id为移动节点的Id、Gi(k)为k时刻的二元信息关系图、Li(k)为k时刻的二元信息关系图中的所有状态点Id、Qi为边缘化阈值，为k时刻本移动节点对于自身位置估算和准确度的信念，表示本移动节点k时间对自身位置的位置估计，表示本移动节点对自身位置的位置估算方差；2)当本移动节点向前移动并且得到从移动节点自身移动估计模块的测量值时，执行以下步骤：2‑1)把测量值转化为二元信息关系元组，并且加入到二元信息关系图Gi(k)中，2‑2)加入一个新的状态点Id：i.k到Li(k)，i.k表示k时刻的移动节点i；2‑3)用公式(5)和(6)更新自身位置估计信息x^i,ki+=x^i,ki-+zk---(5)]]>Pi,i,ki+=Pi,i,ki-+RI,k---(6)]]>其中，和分别表示更新后的位置估计和位置估算方差，和分别表示更新前的位置估计和位置估算方差，RI，k为k时间i节点自身移动的测量误差方差矩阵，zk为k时间的测量值；3)当本移动节点得到自身定点定位测量值时，执行以下步骤：3‑1)把测量值转化为二元信息关系元组，并且加入到二元信息关系图Gi(k)中；3‑2)用公式(7)和(8)更新自身位置估计信息bel(xi,k)x^i,ki+=Pi,i,ki-(Pi,i,ki-+RI,k)-1zk+RI,k(Pi,i,ki-+RI,k)-1x^i,ki----(7)]]>Pi,i,ki+=Pi,i,ki-RI,k(Pi,i,ki-+RI,k)-1---(8)]]>其中，和分别表示更新后的位置估计和位置估算方差，和分别表示更新前的位置估计和位置估算方差，RI,k为k时间i节点自身定点定位的测量误差方差矩阵，zk为k时间的测量值；4)当移动节点i遇到另外一个移动节点j的时候，执行以下步骤：4‑1)两个移动节点互相测距并且把测量值转换成二元信息关系元组；4‑2)把当前的二元信息关系元组信息通过移动节点通讯模块和对方共享；4‑3)两个节点把各自所缺失的新的二元信息关系元组加入到二元信息关系图中，并更新Li(k)和Lj(k)。