[发明专利]一种结合单机积分式里程计与多机互相测距的分布式无人机集群定位方法

权利要求书

1.一种结合单机积分式里程计与多机互相测距的分布式无人机集群定位方法，其特征在于，包括以下步骤：

S1，集群中各个个体之间互相测距和通信，集群各个体通过广播各自修正后的实时位置，从而对于每一个个体，它总是知道周围个体实时的理论绝对位置(可能存在偏差或漂移)和自身到周边个体的测距距离，从而基于这一距离和收到的其他个体的定位数据通过互相测距设备进行测距定位解算，解算出一个位置p1；

S2，个体进行累积式定位，得到一个无修正的定位输出p0；

S3，p1减去p0得到一个实时的定位误差；

S4，通过一个低通滤波器过滤掉定位误差的定位噪声，使得定位误差估计更为平滑；

S5，将滤波后的定位误差与累积式定位的无修正定位输出p0叠加，得到修正后的定位数据；

S6，然后将修正后的定位数据广播至其它个体，供其他个体校准定位。

2.根据权利要求1所述的一种结合单机积分式里程计与多机互相测距的分布式无人机集群定位方法，其特征在于，在S2步骤中加入EKF模块对累积式位进行修正得到所述定位输出p0。

3.根据权利要求1所述的一种结合单机积分式里程计与多机互相测距的分布式无人机集群定位方法，其特征在于，在S1步骤中的测距定位解算的方式为单步式，公式如下：

令本个体编号为i，其它个体编号为j，其中j∈[1,N],j≠i,N为集群总数；对于每一有效帧，即包含了测距值数据和其它个体定位信息的的数据帧，构造优化问题

其中，是本个体的定位值，也是优化问题的初值，右上角上标表示该数字变量当前处在流程的步骤，表示当前帧原始的未修正定位数据，表示未修正定位数据加上上一次定位校正所得的初步修正结果，表示考虑当前测距数据后得到的修正位置；优化项是指令优化的结果尽可能靠近上一轮定位数据得到的定位结果，使得减少优化问题的鞍点或局部极小值点，提高收敛率；后面的项与实际测距不一致的程度，各个平方项是为了尽可能保证优化问题的二阶导连续或跳变尽可能小，从而有利于优化算法对最优解的搜索。

4.根据权利要求3所述的一种结合单机积分式里程计与多机互相测距的分布式无人机集群定位方法，其特征在于，

经过以上优化问题得到基于当前次测距数据所得的定位位置后，减去原始未修正定位输出数据得到基于当前次测距数据的定位漂移量一般情况下，e^now应当是较为接近e^last的，因为原始的、基于累积式原理的定位数据的漂移应当是持续且变化缓慢的；于是，这里采用加权滑动窗口的方式进行滤波，窗口内记录最新的K个漂移量原始数据，按照

的方式获得当前帧的最佳漂移量估计，其中e^k|_k＝K即为e^now，但e^k|_k＝K-1≠e^last，因为e^k记录的是原始数据，而e^last是上一帧的滤波后数据；右上角的*代表最佳估计；

随后将当前帧的原始未校正定位数据加上基于当前帧测距的最佳位置估计e^*即可得到当前帧的修正后定位

该定位将被用作后续的导航、控制等，同时它将会通过网络广播给集群内的其它个体，用作它们的定位修正。