本发明属于导航领域，是一种基于节点运动状态约束的协作定位系统和方法。
背景技术：
无人机在近年来得到了越来越广泛的关注，它可以应用于监测、测绘、农作物保护等各种应用中。通常情况下，可以通过多无人机协作的方式来提高该无人系统的性能和鲁棒性。对于这个协作系统来说，节点位置信息是至关重要的，目前，GPS被广泛应用于节点定位问题当中。但是，在恶劣的环境条件下，如城市峡谷、森林及强信号干扰环境，GPS信号容易受到干扰，此时，GPS提供的位置信息与真实位置存在很大的偏差，这使得GPS定位系统的性能大大降低。协作定位方法能够有效利用网络中节点间的相对距离信息或者角度信息，来提高节点定位的性能和稳定性。置信度传播算法和扩展卡尔曼滤波算法是比较常用的协作定位方法，其他方法还包括最小二乘算法和无迹卡尔曼滤波算法等。
扩展卡尔曼滤波算法由于具有较低的计算复杂度而得到了广泛的应用，它利用线性方程对非线性的系统方程或者状态方程进行逼近。线性方程与非线性方程之间的失配使得系统的性能大大降低。另外，基于因子图的置信度传播算法也被广泛运用，它没有考虑到节点的运动状态约束，在节点运动速度比较小的时候，该算法能够提供较高的定位精度，但是当运用于无人机网络这样的节点高速运动的场景时，其先验信息误差较大，因而其最终的位置估计精度也较差。
在无人机网络中，由于遮挡等原因，只有部分节点能够通过传统方法，如GPS，对自身进行定位，将这些节点称为参考节点，其余节点无法进行自定位，称之为非参考节点。另外，节点间通信距离较长，节点速度比较快，利用传统的协作定位方法对无人机网络中各个节点进行定位时其定位精度较差，并且定位结果容易发散。
本发明提出了一种基于节点运动状态约束的协作定位系统和方法。该系统是针对高速运动的无人机网络中部分节点无法完成自定位的问题，利用参考节点的位置估计信息和节点间的相对距离信息完成网络中所有节点的定位。所提出的协作定位方法在节点的状态模型中加入了节点的运动速度，可以有效提高节点位置的预测精度；对于每一个节点，基于预测信息和该节点通信范围内的参考节点的位置估计信息，对相对距离信息进行融合，得到节点位置的观测信息；利用观测信息和节点的运动状态约束对预测信息进行修正，可以得到节点状态的后验估计，这其中就包括了节点位置的后验估计。
技术实现要素：
本发明的目的是针对恶劣环境条件下，高速运动的无人机网络中部分节点无法通过传统方法，如GPS,进行定位，提出一种基于节点运动状态约束的协作定位系统和方法。
基于节点运动状态约束的协作定位系统可以分为四个模块：预测模块、先验信息融合模块、相对距离信息融合模块、校正模块。前一时刻节点状态的后验估计输入到预测模块，预测模块的输出端连接到先验信息融合模块和校正模块的输入端，先验信息融合模块的输出端与相对距离信息融合模块的输入端相连，相对距离信息融合模块的输出连接到校正模块，校正模块的输出就是当前时刻节点状态的后验估计。
针对该协作定位系统所提出的基于节点运动状态约束的协作定位方法包括如下步骤：
(1)对节点状态信息进行预测。基于前一时刻节点状态的后验估计，根据节点的运动状态约束对当前时刻节点的状态信息进行预测。该预测值送入到先验信息融合模块和校正模块；
(2)对所有的先验信息进行融合，得到融合的先验信息。在某一时刻，参考节点的先验信息包括预测信息和通过传统定位方法，如GPS,所获得的位置估计信息；非参考节点的先验信息来自预测信息。将融合的先验信息送入到相对距离信息融合模块；
(3)相对距离信息融合模块收集当前时刻节点通信范围内参考节点的位置估计信息及相对距离信息，基于融合的先验信息，对相对距离信息进行融合，得到节点位置的观测信息，将该观测信息送入校正模块；
(4)校正模块根据节点的运动状态约束，利用相对距离信息融合模块所得到的节点位置的观测信息对节点状态的预测值进行修正，得到节点状态的后验估计；
(5)将校正模块的结果输出，即可得到当前时刻节点状态的后验估计，其中就包括了节点位置的后验估计。
本发明在节点的状态模型中加入了节点的运动速度，然后利用节点的运动状态约束有效提高了节点位置的预测精度，这不仅提高了节点的定位精度，也提高了在网络连通性较差或者节点速度较大时方法的稳定性。
附图说明
图1是基于节点运动状态约束的协作定位方法的框图；
图2通信范围内的参考节点数不足时非参考节点的定位性能；
图3是非参考节点的定位误差累积密度图；
图4是参考节点的定位误差累积密度图。
具体实施方式
本发明设计了一种协作定位系统和算法。该系统是针对高速运动的无人机网络中部分节点无法完成自定位的问题，利用参考节点的位置估计信息和节点间的相对距离信息完成网络中所有节点的定位。所提出的协作定位方法在节点的状态模型中加入了节点的运动速度，并且在计算节点的状态信息时利用了节点的运动状态约束。该方法能够有效提高节点状态的预测精度，进而提高节点的定位精度，并且提高了在网络连通性较差或者节点速度较大时方法的稳定性。
基于节点运动状态约束的协作定位系统可以分为四个模块：预测模块、先验信息融合模块、相对距离信息融合模块、校正模块。针对该系统所提出的协作定位方法包括如下步骤：
(1)建立系统的状态模型，确定系统的状态方程和观测方程：
其中，为节点m在t时刻的状态变量，包括节点的位置信息和速度信息，F为状态转移矩阵，为系统建模噪声，其均值为零，协方差矩阵为表示观测向量，表示相对距离信息融合所得到的节点位置的观测信息，表示的是t时刻的观测误差，其均值为零，协方差矩阵为
(2)对节点状态进行预测。基于前一时刻节点状态的后验估计，利用扩展卡尔曼滤波算法作为节点的运动状态约束，对节点在当前时刻的状态进行预测，该预测值的均值和协方差矩阵分别为：
其中，分别表示前一时刻节点状态后验估计的均值和协方差矩阵。该预测值送入到先验信息融合模块和校正模块；
(3)对所有的先验信息进行融合，得到融合的先验信息。在某一时刻，参考节点的先验信息包括预测信息和通过传统定位方法，如GPS，所获得的位置估计信息，可基于最小均方误差准则对这些先验信息进行融合；非参考节点的先验信息来自预测信息。将融合的先验信息送入相对距离信息融合模块；
(4)相对距离信息融合模块收集当前时刻节点通信范围内参考节点的位置估计信息及相对距离信息，基于融合的先验信息，利用参数化的置信度传播算法对相对距离信息进行融合，得到节点位置的观测信息，其均值为协方差矩阵为将观测信息送入校正模块；
(5)校正模块根据节点的运动状态约束，利用相对距离信息融合模块所得到的节点位置的观测信息对节点状态的预测值进行修正，得到节点状态的后验估计。首先，计算测量余量及其协方差矩阵
然后，计算卡尔曼增益
最后，得到校正后的节点状态的均值和协方差矩阵
(6)将校正模块的结果输出，即可得到当前时刻节点状态的后验估计。该后验估计信息输入到下一个时间片的预测模块的输入端，用于对下一时刻节点状态信息的预测。
协作定位系统的系统框图如图1所示，包括了预测模块101、先验信息融合模块102、相对距离信息融合模块103、校正模块104；前一时刻节点状态的后验估计输入到预测模块101，预测模块101的输出端连接到先验信息融合模块102和校正模块104的输入端，先验信息融合模块102的输出端连接到相对距离信息融合模块103的输入端，相对距离信息融合模块103的输出连接到校正模块104的输入端。
实施例
将扩展卡尔曼滤波算法作为节点的运动状态约束，用参数化的置信度传播算法对节点的相对距离信息进行融合为例。设定在一个无人机网络中，有18个无人机，其中8个无人机可通过GPS对自身进行定位，通过GPS所获得位置估计信息是存在一定误差的，其余无人机不能完成自定位，所有无人机节点随机分布在5000m×5000m的范围内。由于距离与功耗的限制以及障碍物的遮挡，每一个节点只能与网络中的部分节点进行通信，通过通信链路可以得到节点间的相对距离信息，每个节点的运动速度在40m/s左右，加速度在1～2m/s2之间，节点的通信半径为1400m。参考节点通过GPS所得位置的误差标准差为5m，节点间相对距离信息的误差标准差为3m。
基于节点运动状态约束的协作定位系统可以分为四个模块：预测模块、先验信息融合模块、相对距离信息融合模块、校正模块。针对该系统所提出的协作定位方法包括如下步骤：
(1)建立系统的状态模型，确定系统的状态方程和观测方程：
其中，为节点m在t时刻的状态变量，包括节点的位置信息和速度信息，F为状态转移矩阵：
T表示两个时间片间的时间间隔，为系统建模噪声，其均值为零，协方差矩阵为表示观测向量，是相对距离信息融合所得到的节点位置的观测信息，观测矩阵H为
表示的是t时刻的观测误差，其均值为零，协方差矩阵为
(2)根据t-1时刻节点状态的后验估计信息对t时刻的节点状态及其协方差矩阵进行预测，计算公式分别为：
其中，为节点m在t-1时刻的状态估计结果，包括了节点的位置信息和速度信息；为t-1时刻节点状态估计的协方差矩阵，为系统噪声的协方差矩阵；
(3)对节点位置估计的先验信息进行融合。在每个时间片内，对于参考节点，其先验信息包括预测信息和通过GPS所获得的节点位置的估计信息，可基于最小均方误差准则进行融合；对于非参考节点，其先验信息就是预测信息。
其中，
这里，假设节点的位置估计满足高斯分布。另外，表示节点位置的预测信息，是状态预测值的一部分，表示位置预测信息的协方差矩阵。表示参考节点通过GPS获得的位置估计信息，表示该估计结果的协方差矩阵；
(4)基于融合的先验信息，利用参数化的置信度传播算法，也就是蒙特卡洛随机采样的方法，利用大量带有权值的随机采用点对相对距离信息进行融合，得到节点位置的观测信息。首先，在融合的先验信息中随机采样得到K个采样点在该分布中，采样点的概率密度为
其中，Zq是一个归一化的参数。然后，根据节点m与参考节点n之间的相对距离信息计算出该测距信息约束下各个采样点的概率密度，计算公式为：
其中，Zr是一个归一化的参数，是相对距离信息的测量方差，与表示的是节点m通信范围内的一个参考节点n的位置估计的均值和协方差矩阵。接下来，赋予每一个采样点一个权重，其计算公式为
并对所有采样点的权重进行归一化，使得
最后，基于这些带有权值的随机采样点，可以得到由相对距离信息融合得到的节点位置的观测信息，其均值和协方差矩阵分别为
(5)建立校正方程，利用相对距离信息融合模块的节点位置观测结果对节点状态的预测信息进行校正。首先，计算测量余量及其协方差矩阵
然后，计算卡尔曼增益
最后，得到校正后的节点状态的均值和协方差矩阵
(6)校正方程的输出输出即为t时刻的节点状态的后验估计，该结果将输入到预测模块，用于对t+1时刻节点状态进行预测。
图2表示在参考节点数不足时，非参考节点的定位性能，SPA-EKF是本发明所提出的一种协作定位方法。STOC-EKF和SPAWN是两个对比算法。在参考节点足够时，图3表示非参考节点的定位误差累积密度，图4表示参考节点的定位误差累积密度。其中，图4中“GPS localization”表示参考节点通过传统的GPS定位方法所得到的节点定位误差累积密度。
图中可以看出，当运用于高速运动的无人机网络时，基于节点运动状态约束的协作定位系统和方法在参考节点数不足时定位误差发散较缓慢，因而方法的稳定性更好。在参考节点足够时，该协作定位系统和方法的定位精度更高。