[发明专利]一种基于节点运动状态约束的协作定位系统和方法

权利要求书

1.一种基于节点运动状态约束的协作定位系统的定位方法，所述的协作定位系统包括预测模块(101)、先验信息融合模块(102)、相对距离信息融合模块(103)和校正模块(104)；预测模块(101)的输出端连接到先验信息融合模块(102)和校正模块(104)的输入端，先验信息融合模块(102)的输出端连接到相对距离信息融合模块(103)的输入端，相对距离信息融合模块(103)的输出端与校正模块(104)的输入端相连；

其特征在于，所述的定位方法包括如下步骤：

(1)预测模块根据前一时刻节点状态的后验估计，基于节点的运动状态约束，对当前时刻节点的状态信息进行预测，将预测信息送入先验信息融合模块和校正模块；

(2)在每一个时间片内，先验信息融合模块根据参考节点的位置估计信息和预测模块输出的每个节点的位置预测信息，计算得到每个节点位置的先验估计，该先验信息送入到相对距离信息融合模块；

(3)相对距离信息融合模块基于先验信息和节点通信范围内参考节点的位置信息，对相对距离信息进行融合，得到节点位置的观测信息，将这个观测信息送入到校正模块；

(4)校正模块根据节点的运动状态约束和相对距离信息融合模块得到的节点位置观测信息，对节点状态的预测值进行校正，得到节点状态的后验估计，这其中就包括了节点位置的后验估计；

所述的步骤(1)具体为：

基于前一时刻节点状态的后验估计，利用扩展卡尔曼滤波算法作为节点的运动状态约束，对节点在当前时刻的状态进行预测：

其中，表示前一时刻节点m状态的后验估计，这个状态信息包括了节点的位置信息和速度信息，为其协方差矩阵；F为状态转移矩阵，表示当前时刻节点状态的预测值，为其协方差矩阵，为系统建模噪声的协方差矩阵，该预测值送入到先验信息融合模块和校正模块。

2.如权利要求1所述的基于节点运动状态约束的协作定位系统的定位方法，其特征在于所述的步骤(2)具体为：

对所有的先验信息进行融合，得到融合的先验信息，在某一时刻，参考节点的先验信息包括参考节点的位置估计信息和预测模块输出的每个节点的位置预测信息，基于最小均方误差准则对这些先验信息进行融合；非参考节点的先验信息来自预测信息，将融合的先验信息送入相对距离信息融合模块。

3.如权利要求1所述的基于节点运动状态约束的协作定位系统的定位方法，其特征在于所述的步骤(3)具体为：

相对距离信息融合模块收集当前时刻节点通信范围内参考节点的位置信息及相对距离信息，基于融合的先验信息，利用参数化的置信度传播算法，具体来讲，就是利用蒙特卡洛随机采样的方法对相对距离信息进行融合，得到节点位置的观测信息，其均值为协方差矩阵为将观测信息送入校正模块。

4.如权利要求1所述的基于节点运动状态约束的协作定位系统的定位方法，其特征在于所述的步骤(4)具体为：

校正模块根据节点的运动状态约束，利用相对距离信息融合模块所得到的节点位置的观测信息对节点状态的预测信息进行修正，得到节点状态的后验估计；首先，计算测量余量及其协方差矩阵

其中，H为观测矩阵；然后，计算卡尔曼增益

最后，得到校正后的，也就是当前时刻节点状态的后验估计，其均值和协方差矩阵分别为