基于距离测量的无人机群分布式相对定位方法

1.本发明属于无线定位技术领域，涉及一种无人机群相对定位方法，具体涉及一种基于距离测量的无人机群分布式相对定位方法，可用于无人机群编队控制、编队飞行、部署空中网络等场景下获取无人机之间的相对位置，为无人机群路径规划、安全飞行及协作完成任务提供保障。


背景技术：

2.随着5g通信和无人机自动驾驶技术的发展，无人机的市场需求急速上升。因其具有成本低、体积小、机动性强等优点被广泛应用于快递运输、灾害救援、农业生产等。由于单个无人机的生存能力差，无法完成复杂的任务，无人机集群协同作业则是当前科技现代化的任务要求，能有效提高整体工作效能。作为无人机群技术中的关键技术，定位技术引起了广泛关注，是无人机群路径规划、安全飞行以及任务分配的重要保障。全球定位系统广泛用于获取无人机绝对位置，而卫星信号在恶劣环境中会受到路径衰减和建筑遮挡的影响，定位误差甚至会超过十米。现有的无人机的相对定位研究较少，而获取无人机之间的相对位置更适合于无人机群执行协作任务，如编队控制、编队飞行、目标跟踪等。得益于无线定位技术的发展，使无人机之间的精准相对定位成为了可能，尤其是基于超宽带测距的定位技术，其测距精度可达厘米级。在无人机群中，无人机之间通过无线通信进行相互测距和协同定位，提升定位精度。因此对无人机群的相对定位方法研究具有重要意义。
3.无人机群的相对定位算法分为集中式和分布式。分布式相对定位是集群中的无人机利用自身测量信息以及相邻无人机的交互信息，去获取自己的位置。首先，分布式相对定位方法利用集群内部相互测量实现相对定位，无需依赖于卫星导航信息，可以确保集群编队构型和内部拓扑空间关系的稳定性；而且，实际中无人机群数量的大规模和拓扑结构的不规则性，处于边缘的无人机之间的长距离而无人机的无线通信范围有限会导致数据包丢失，使测量信息不完整，导致定位定位精度下降，而分布式相对定位系统基于通信感知与相邻无人机动态组网，并在局部完成相对定位从而在很大程度上克服了这一问题；另外，分布式相对定位方法不需要一个全局的中心无人机收集全局信息并完成相应解算，而是将整个集群进行了划分，相比于集中式方法，分摊了中心单元的存储和运算，即使某些无人机出现异常，整个集群的定位系统仍能运行。
4.基于距离测量的无人机群分布式相对定位方法的基本思路是：首先，对集群进行分簇，分簇依据有基于连通度、基于聚类分析和基于节点的剩余能量等；然后，各个簇内的无人机通过其与相邻无人机之间的测距信息，基于凸规划算法、自定位算法或多维尺度分析算法等完成簇内局部相对定位；最后，基于不同簇之间的公共节点，通过最小化空间构型误差求得最优坐标旋转变换参数，将不同簇的局部相对定位结果融合到全局坐标系下，完成对整个无人机群相对定位。
5.定位精度是衡量定位算法的一个非常重要的指标之一，提升基于距离测量的无人机群分布式相对定位精度的难点在于：首先，无人机群分布式定位方法需要利用无人机之
间的相互测距信息完成局部相对定位，基于无线通信技术测量无人机之间的距离信息的过程中，无线信号容易受到多径衰落、非视距传播和设备间时钟不同步等影响从而产生测距误差，因此如何对存在误差的测距值进行识别或修正，是减小测距误差对定位结果的干扰从而提升定位精度的难点；然后，在分布式全局坐标融合过程中，由于每个簇的局部相对定位结果误差不同，串行融合方式会出现误差累计问题，而且，全局融合过程中对不同的簇的局部相对定位结果平等看待会出现误差传递问题，因此，如何设计并行的融合方式且考虑不同簇的局部相对定位误差并赋予不同的权重，是进一步提高定位精度的难点。
6.在y.liu,y.wang,j.wang,and y.shen,“distributed 3d relative localization of uavs,”ieee trans.veh.technol.,vol.69,no.10,pp.11756
–
11770,oct.2020.中，提出了一种基于测距的无人机群分布式相对定位方法：该方法首先基于连通度从大到小的对无人机群进行分簇，然后在簇内采用基于加权半定规划的优化算法完成局部相对位置估计，最后基于统计信息对每个簇的局部相对位置估计结果进行全局坐标融合，得到整个无人机群的相对定位结果。由于测距误差与无线信号的信噪比和实际距离有关，该方法基于无线信号的测距误差模型，将信噪比和测距值引入权重表达式，基于加权半定规划算法对测距值进行优化，相比于直接使用原始测距值的相对定位方法在精度方面有显著提升；并且在全局融合过程中基于统计信息对不同簇赋予权重，进一步提升了精度。缺点是：由于每个簇的局部相对定位结果的误差不同，在全局坐标融合过程中，对不同的簇的局部相对定位结果平等看待以及采取串行融合方式，会导致误差累计问题，从而影响定位精度。


技术实现要素：

7.本发明的目的在于克服上述现有技术存在的缺陷，提出了一种基于距离测量的无人机群分布式相对定位方法，旨在提高无人机群相对定位的精度。
8.为实现上述目的，本发明采取的技术方案包括如下步骤：
9.(1)对无人机群进行分簇：
10.构建包括分布在三维空间中配备有无线通信设备的n架无人机的无人机群分布式相对定位系统，并根据每架无人机的编号idn、连通度dcn、邻居表ntn和通信范围为r，将无人机群分为k个簇g＝{gk|1≤k≤k}，其中，n≥4，1≤n≤n，gk表示包含m架无人机的第k个簇；
11.(2)构建每个簇的距离信息矩阵并对其进行修正：
12.(2a)第k个簇gk中的第m架无人机通过无线通信设备采集其与第j架无人机之间的距离信息以及采集时无线信号的信噪比并基于所有距离信息构建gk的距离信息矩阵基于所有信噪比构建gk的距离信息可信度矩阵wk：
[0013][0014][0015]
其中，j∈m且j≠m，(
·
).
^
2表示生成对矩阵(
·
)中每一个元素求平方后的矩阵；
[0016]
(2b)采用基于加权的半定规划算法，并通过gk的距离信息可信度矩阵wk对gk的距离信息矩阵进行修正，得到gk的修正后的距离信息矩阵则k个簇g对应的修正后的距离信息矩阵的集合为
[0017]
(3)基于多维标度分析算法对每个簇中的无人机进行局部相对定位：
[0018]
(3a)计算去中心化矩阵j，并通过j和gk的修正后的距离信息矩阵计算内积矩阵bk，然后对bk进行特征值分解，得到包括m个特征值的集合和m个特征向量的集合
[0019][0020][0021]
其中，im表示维度为m
×
m的单位矩阵，1m表示元素全为1的m
×
1的列向量；
[0022]
(3b)通过特征值集合λk中前三个最大特征值和作为对角元素构成的对角矩阵和对应的三个特征向量构成的矩阵计算每个簇gk中的m架无人机的局部相对定位坐标矩阵并取的第m列向量作为簇gk中第m架无人机的局部相对定位结果，则k个簇g对应的局部相对定位坐标矩阵构成的的集合为
[0023]
[0024][0025][0026][0027]
其中，diag(
·
)表示生成由(
·
)作为主对角元素且其余元素为零的对角矩阵，()
:,m
表示取矩阵()的第m列向量，(
·
)
t
表示求转置操作；
[0028]
(4)对局部相对定位结果进行全局坐标融合以获取无人机群相对定位结果：
[0029]
(4a)将n架无人机中同时被划分在k个簇g中的h架无人机作为公共无人机，则每个簇gk中的m架无人机可被划分为包括h架公共无人机的集合和包括l架非公共无人机的集合其中，l＝m-h，分别表示第k个簇gk中的第h架公共无人机、第l架非公共无人机；
[0030]
(4b)通过每个簇gk中公共无人机集合每架公共无人机对应的局部相对定位结果构成公共无人机局部相对坐标矩阵并通过和修正后测距信息矩阵计算权重ψk，再通过k个簇g对应的公共无人机的局部相对坐标矩阵构成的集合和权重集合ψ＝{ψk|1≤k≤k}计算初始最优坐标矩阵z
opt
：
[0031][0032][0033][0034][0035]
其中，v∈h，v≠h，||
·
||2表示求向量2范数，∑(
·
)表示求和操作，(
·
)
h,v
表示取矩阵第h行第v列对应的元素，argmax{
·
}表示取集合{
·
}中元素值最大时对应的元素序号；
[0036]
(4c)采用迭代的加权最小二乘坐标融合方法，并通过p
common
、ψ和z
opt
计算最优坐标矩阵z
i+1
、旋转矩阵集合平移向量集合并将z
i+1
作为k个簇g中对应的h个公共无人机的相对定位结果，其中，分别表示第k个簇gk中对应的公共无人机局部相对定位坐标矩阵变换到z
i+1
的旋转矩阵、平移向量；
[0037]
(4d)通过旋转矩阵集合r
i+1
和平移向量集合t
i+1
，对每个簇gk中非公共无人机集合中每架非公共无人机对应的局部相对定位结果进行全局坐标融合，得到第l架非公共无人机的相对定位结果并将l架非公共无人机的相对定位结果构成非公共无人机相对坐标矩阵则k个簇对应的非公共无人机的相对定位结果构成的坐标矩阵为最优坐标矩阵z
i+1
和的联合矩阵即n架人机构成的无人机群的相对定
位结果：
[0038][0039][0040][0041]
本发明与现有技术相比，具有如下优点：
[0042]
本发明在对局部相对定位结果进行全局坐标融合的过程中，根据不同簇的局部相对定位结果计算坐标融合时的全局误差函数的权重，并采用迭代的加权最小二乘坐标融合方法，以获取所有簇进行坐标融合时最优的旋转矩阵、平移向量，避免了串行的坐标融合方式带来的误差传递，减小了由于不同簇的局部相对定位结果存在误差导致的坐标融合时的误差累计对定位结果准确性的影响，与现有技术相比，有效提高了集群的相对定位精度。
附图说明
[0043]
图1是本发明的实现流程图；
[0044]
图2是本发明实施例中无人机群分簇的结构示意图；
[0045]
图3是本发明实施例中采用迭代的加权最小二乘坐标融合方法求解最优旋转矩阵、平移向量的原理图；
[0046]
图4是本发明对局部相对定位结果进行全局坐标融合的实现流程图；
[0047]
图5是本发明与现有技术定位精度的仿真结果对比图。
具体实施方式
[0048]
以下结合附图和具体实施例，对本发明作进一步详细描述。
[0049]
参照图1，本发明包括如下步骤：
[0050]
(1)对无人机群进行分簇：
[0051]
(1a)构建包括分布在三维空间中配备有无线通信设备的n架无人机的无人机群分布式相对定位系统和定位场景，每架无人机的通信范围为r，其中，n≥4；在本实例中，n＝14，r＝50米，第n架无人机在三维空间中的坐标为pn＝(xn,yn,zn)，其中：p1＝(70,220,80)米，p2＝(60,200,30)米，p3＝(60,180,20)米，p4＝(80,200,40)米,p5＝(90,220,40)米，p6＝(110,200,35)米，p7＝(100,200,20)米，p8＝(120,200,30)米，p9＝(95,180,30)米，p
10
＝(120,160,60)米，p
11
＝(80,190,40)米，p
12
＝(75,160,35)米，p
13
＝(90,160,20)米，p
14
＝(100,170,50)米。
[0052]
(1b)每架无人机通过无线通信模块周期性地广播包含自己编号idn的消息，并接收通信范围r内其他无人机的广播消息，每架无人机通过接收到的来自第n'架无人机的广播消息中的无人机编号id
n'
，计算得到其自身的邻居表即ntn＝{id
n'
|d
nn'
≤r}，则n架无人机的邻居表构成集合nt＝{ntn|1≤n≤n}，其中，n'∈n，n'≠n，d
nn'
表示第n架无人机到第n'架无人机之间的距离；
[0053]
(1c)令k＝1，通过集合nt，计算每架无人机的连通度则n架无人机的连通度构成集合并选择dck中连通度最大的无人机作为中心
无人机gk，将gk与其邻居表中的无人机构成第k个簇gk，并计算已分簇无人机构成的集合wk＝gk，其中，card(
·
)表示取元素数量；参照图2，图2(a)表示第一个广播周期结束后，每架无人机根据其接收到的来自其他无人机的消息，计算自己的连通度，图2(b)表示选择连通度最大的无人机即编号为4的无人机作为第一架中心无人机，并与邻居表中的无人机构成第一个簇g1＝{1,2,3,4,5,6,9,11}；
[0054]
(1d)判断card(wk)＝n是否成立，若是，得到包括k个簇的集合g＝{gk|1≤k≤k},参照图2(c)、图2(d),在本实例中，步骤(1e)执行了两次，依次得到簇g2＝{4,6,9,10,11,12,13,14}、簇g3＝{4,5,6,7,8,9,10,11}}，则分簇结果为包括3个簇的集合g＝{g1,g2,g3}；否则，令k＝k+1，执行步骤(1e)。
[0055]
(1e)将集合w
k-1
中除中心无人机以外的s架无人机构成待选无人机集合q，并将q中每架待选无人机与其邻居表构成集合cus，并计算cus和w
k-1
的相交数目cs，则s架待选无人机对应的相交数目cs构成的集合为c＝{cs|1≤s≤s}；通过cus和w
k-1
，计算集合q中每架待选无人机的连通度则s架待选无人机的连通度构成的集合为通过ds，在集合c中选择满足条件cs≥4的无人机中对应的连通度最大的无人机作为第k架中心无人机gk，将gk及其邻居表中的无人机构成第k个簇gk，通过w
k-1
和gk计算已分簇无人机构成的集合wk，执行步骤(1d)；
[0056][0057][0058]
wk＝w
k-1
∪gk[0059]
其中，∩表示求交集，∪表示求并集。
[0060]
(2)构建每个簇的距离信息矩阵并对其进行修正：
[0061]
(2a)第k个簇gk中的第m架无人机通过无线通信设备采集其与第j架无人机之间的距离信息以及采集时无线信号的信噪比并基于所有距离信息构建gk的距离信息矩阵基于所有信噪比构建gk的距离信息可信度矩阵wk：
[0062]
[0063][0064]
其中，j∈m且j≠m，(
·
).
^
2表示生成对矩阵(
·
)中每一个元素求平方后的矩阵；
[0065]
(2b)采用基于加权的半定规划算法对gk的距离信息矩阵进行修正，构建映射函数fd(
·
)，通过第k个簇gk中的无人机数量m计算正定矩阵f，通过wk、fd(
·
)、f、构建加权误差最小化的凸松弛半定规划模型(ι)，通过matlab软件提供的cvx工具包，求得其最优结果为通过f和fd(
·
)，计算修正后的距离信息矩阵则k个簇g对应的修正后的距离信息矩阵的集合为
[0066][0067][0068][0069][0070]
其中，argminf()表示取使得f()函数值最大时自变量的值，||
·
||f表示取矩阵的f范数，diag(
·
)表示生成一个由矩阵(
·
)的主对角线元素组成的列向量，
⊙
表示哈达玛乘积。
[0071]
(3)基于多维标度分析算法对每个簇中的无人机进行局部相对定位：
[0072]
(3a)计算去中心化矩阵j，并通过j和gk的修正后的距离信息矩阵计算内积矩阵bk，然后对bk进行特征值分解，得到包括m个特征值的集合和m个特征向量的集合
[0073]
[0074][0075]
其中，im表示维度为m
×
m的单位矩阵，1m表示元素全为1的m
×
1的列向量。
[0076]
(3b)通过特征值集合λk中前三个最大特征值和作为对角元素构成的对角矩阵和对应的三个特征向量构成的矩阵计算每个簇gk中的m架无人机的局部相对定位坐标矩阵并取的第m列向量作为簇gk中第m架无人机的局部相对定位结果，则k个簇g对应的局部相对定位坐标矩阵构成的的集合为
[0077][0078][0079][0080][0081]
其中，diag(
·
)表示生成由(
·
)作为主对角元素且其余元素为零的对角矩阵，(
·
)
:,m
表示取矩阵(
·
)的第m列向量，(
·
)
t
表示求转置操作。
[0082]
(4)对局部相对定位结果进行全局坐标融合以获取无人机群相对定位结果，其中全局坐标融合的实现步骤如图4所示：
[0083]
(4a)将n架无人机中同时被划分在k个簇g中的h架无人机作为公共无人机，则每个簇gk中的m架无人机可被划分为包括h架公共无人机的集合和包括l架非公共无人机的集合其中，l＝m-h，分别表示第k个簇gk中的第h架公共无人机、第l架非公共无人机；参照图3，在本实例中，h＝4，图3(a)、3(b)、3(c)分别为4架公共无人机在簇g1、簇g2、簇g3的局部相对定位结果示意图。
[0084]
(4b)通过每个簇gk中公共无人机集合每架公共无人机对应的局部相对定位结果构成公共无人机局部相对坐标矩阵并通过和修正后测距信息矩阵计算权重ψk，再通过k个簇g对应的公共无人机的局部相对坐标矩阵构成的集合和权重集合ψ＝{ψk|1≤k≤k}计算初始最优坐标矩阵z
opt
：
[0085][0086][0087][0088][0089]
其中，v∈h，v≠h，||
·
||2表示求向量2范数，∑(
·
)表示求和操作，(
·
)
h,v
表示取矩阵第h行第v列对应的元素，argmax{
·
}表示取集合{
·
}中元素值最大时对应的元素序
号。
[0090]
(4c)初始化第i次循环时的最优坐标矩阵为zi、旋转矩阵集合为ri、平移向量集合为ti、变换误差集合αi、加权变换误差集合ξi，并令i＝0：
[0091]
zi＝z
opt
[0092][0093][0094][0095]
其中，分别表示第i次循环时坐标矩阵变换到最优坐标矩阵zi的旋转矩阵、平移向量、变换误差，03×3表示元素值全为零的维度为3
×
3的矩阵，03×1表示三个元素值全为零的列向量。
[0096]
(4d)计算每个簇gk对应的公共无人机局部相对定位坐标矩阵的均值向量μ
p
和最优坐标矩阵zi的均值向量μz，以及和zi的协方差矩阵c
p,z
，对协方差矩阵c
p,z
进行奇异值分解得到特征向量和并计算旋转矩阵通过μ
p
、μz和计算平移向量通过zi和计算变换误差则k个旋转矩阵构成的集合、k个平移向量构成的集合、k个变换误差构成的集合分别为量构成的集合、k个变换误差构成的集合分别为
[0097][0098][0099][0100][0101][0102][0103]
其中，(
·
)
:,h
表示取矩阵(
·
)的第h列向量。
[0104]
(4e)通过ψ、r
i+1
、t
i+1
计算每一个坐标向量加权变换的最优坐标向量并构成最优坐标矩阵z
i+1
，通过权重集合ψ、旋转矩阵集合r
i+1
、平移向量集合t
i+1
，计算每一个坐标向量变换到最优坐标向量的加权变换误差则h个加权变换误差构成的加权变换误差集合为
[0105]
[0106][0107][0108]
(4f)通过ψ、α
i+1
和ξ
i+1
，判断是否成立，若是，则输出最优坐标矩阵z
i+1
、集合r
i+1
和集合t
i+1
，否则，令i＝i+1，并执行步骤(4d)；参照图3(d)，在本实例中，4个公共无人机在每个簇中的局部相对定位结果误差不同，为避免在对3个簇中的局部相对定位结果进行全局融合的过程中产生误差累计，使得融合后的对应坐标之间的空间构型误差最小，采用步骤(4c)～步骤(4f)的迭代的加权最小二乘坐标融合算法，其本质是一种并行的融合方式，相比于串行的融合方式，降低了误差累计。
[0109]
(4g)通过旋转矩阵集合r
i+1
和平移向量集合t
i+1
，对每个簇gk中非公共无人机集合中每架非公共无人机对应的局部相对定位结果进行坐标变换，得到第l架非公共无人机的相对定位结果并将l架非公共无人机的相对定位结果构成非公共无人机相对坐标矩阵则k个簇对应的非公共无人机的相对定位结果构成的坐标矩阵为最优坐标矩阵z
i+1
和的联合矩阵即n架人机构成的无人机群的相对定位结果：
[0110][0111][0112][0113]
通过每个簇的局部相对定位结果进行全局融合的过程中，产生的误差累积是由于不同簇的局部相对定位结果的定位误差不同导致的，本发明采用迭代的加权最小二乘坐标融合方法：依据簇内相对位置估计误差计算坐标融合的全局误差函数的权重，并基于迭代优化方法获取所有簇进行坐标融合时最优的旋转矩阵、平移向量，且以并行的方式进行坐标融合，降低了误差噪声的累计，提升了集群的定位精度。
[0114]
结合以下仿真实验，对本发明的技术效果作以说明。
[0115]
1.仿真条件和内容：
[0116]
仿真采用cpu为inter酷睿i76500u，主频为2.50ghz，内存为8.0gb，64位操作系统和microsoft windows 10专业版，matlab 2018a仿真软件。
[0117]
无人机数量为n＝14，第n架无人机的坐标pn＝(xn,yn,zn)分布在oxyz坐标系下的三维空间中，具体为：p1＝(70,220,80)米，p2＝(60,200,30)米，p3＝(60,180,20)米，p4＝(80,200,40)米,p5＝(90,220,40)米，p6＝(110,200,35)米，p7＝(100,200,20)米，p8＝(120,200,30)米，p9＝(95,180,30)米，p
10
＝(120,160,60)米，p
11
＝(80,190,40)米，p
12
＝(75,160,35)米，p
13
＝(90,160,20)米，p
14
＝(100,170,50)米，每架无人机的通信范围为r＝50米。设无人机之间的距离测量误差均服从零均值高斯分布，距离测量误差的标准差起始
值为0.01米，然后从0.1米增加到0.9米且每次改变的间隔为0.1米。每次距离测量误差改变后，进行10000次蒙特卡洛仿真。定义无人机群相对定位估计结果的均方根误差为rmse，其表达式为：
[0118][0119]
其中，表示定位算法对于第n架无人机的相对定位坐标pn的估计，||
·
||f表示取矩阵f范数操作，t表示同一个测距误差下的仿真次数。
[0120]
对本发明与现有的基于距离测量的分布式相对定位方法的相对位置估计的均方根误差rmse进行对比仿真，其结果如图5所示。
[0121]
2.仿真结果分析：
[0122]
参照图5，横轴表示距离测量误差，单位为米，纵轴表示相对定位估计结果的均方根误差为rmse。从图5可以看出，在距离测量误差在0.01米到0.3米的范围内，本发明的相对定位估计结果的均方误差与现有技术的相对定位估计结果的均方误差非常接近，但随着距离测量误差的增加，本发明的相对定位估计结果的均方根误差显著降低，并且，测距测量误差越大，相对定位估计结果的均方根误差降低越明显；当测距测量误差为0.9米时，现有技术的相对定位估计结果的均方根误差为6.068米，本发明的相对定位估计结果的均方根误差仅为4.809米，下降了约20.74％，提高了无人机集群相对定位的精度。
[0123]
综上所述，通过仿真证明了本发明方法能够完成对无人机群的分布式相对定位，同时相较于现有技术提高了无人机群的相对定位精度。

技术特征：
1.一种基于距离测量的无人机群分布式相对定位方法，其特征在于，包括如下步骤：(1)对无人机群进行分簇：构建包括分布在三维空间中配备有无线通信设备的n架无人机的无人机群分布式相对定位系统，并根据每架无人机的编号id
n
、连通度dc
n
、邻居表nt
n
和通信范围为r，将无人机群分为k个簇g＝{g
k
|1≤k≤k}，其中，n≥4，1≤n≤n，g
k
表示包含m架无人机的第k个簇；(2)构建每个簇的距离信息矩阵并对其进行修正：(2a)第k个簇g
k
中的第m架无人机通过无线通信设备采集其与第j架无人机之间的距离信息以及采集时无线信号的信噪比并基于所有距离信息构建g
k
的距离信息矩阵基于所有信噪比构建g
k
的距离信息可信度矩阵w
k
：：其中，j∈m且j≠m，(
·
).^2表示生成对矩阵(
·
)中每一个元素求平方后的矩阵；(2b)采用基于加权的半定规划算法，并通过g
k
的距离信息可信度矩阵w
k
对g
k
的距离信息矩阵进行修正，得到g
k
的修正后的距离信息矩阵则k个簇g对应的修正后的距离信息矩阵的集合为(3)基于多维标度分析算法对每个簇中的无人机进行局部相对定位：(3a)计算去中心化矩阵j，并通过j和g
k
的修正后的距离信息矩阵计算内积矩阵b
k
，然后对b
k
进行特征值分解，得到包括m个特征值的集合和m个特征向量的集合集合
其中，i
m
表示维度为m
×
m的单位矩阵，1
m
表示元素全为1的m
×
1的列向量；(3b)通过特征值集合λ
k
中前三个最大特征值和作为对角元素构成的对角矩阵和对应的三个特征向量构成的矩阵计算每个簇g
k
中的m架无人机的局部相对定位坐标矩阵并取的第m列向量作为簇g
k
中第m架无人机的局部相对定位结果，则k个簇g对应的局部相对定位坐标矩阵构成的的集合为个簇g对应的局部相对定位坐标矩阵构成的的集合为个簇g对应的局部相对定位坐标矩阵构成的的集合为个簇g对应的局部相对定位坐标矩阵构成的的集合为个簇g对应的局部相对定位坐标矩阵构成的的集合为其中，diag(
·
)表示生成由(
·
)作为主对角元素且其余元素为零的对角矩阵，()
:,m
表示取矩阵()的第m列向量，(
·
)
t
表示求转置操作；(4)对局部相对定位结果进行全局坐标融合以获取无人机群相对定位结果：(4a)将n架无人机中同时被划分在k个簇g中的h架无人机作为公共无人机，则每个簇g
k
中的m架无人机可被划分为包括h架公共无人机的集合和包括l架非公共无人机的集合其中，l＝m-h，分别表示第k个簇g
k
中的第h架公共无人机、第l架非公共无人机；(4b)通过每个簇g
k
中公共无人机集合每架公共无人机对应的局部相对定位结果构成公共无人机局部相对坐标矩阵并通过和修正后测距信息矩阵计算权重ψ
k
，再通过k个簇g对应的公共无人机的局部相对坐标矩阵构成的集合和权重集合ψ＝{ψ
k
|1≤k≤k}计算初始最优坐标矩阵z
opt
：：：：其中，v∈h，v≠h，||
·
||2表示求向量2范数，∑(
·
)表示求和操作，(
·
)
h,v
表示取矩阵第h行第v列对应的元素，arg max{}表示取集合{}中元素值最大时对应的元素序号；(4c)采用迭代的加权最小二乘坐标融合方法，并通过p
common
、ψ和z
opt
计算最优坐标矩
阵z
i+1
、旋转矩阵集合平移向量集合并将z
i+1
作为k个簇g中对应的h个公共无人机的相对定位结果，其中，分别表示第k个簇g
k
中对应的公共无人机局部相对定位坐标矩阵变换到z
i+1
的旋转矩阵、平移向量；(4d)通过旋转矩阵集合r
i+1
和平移向量集合t
i+1
，对每个簇g
k
中非公共无人机集合中每架非公共无人机对应的局部相对定位结果进行全局坐标融合，得到第l架非公共无人机的相对定位结果并将l架非公共无人机的相对定位结果构成非公共无人机相对坐标矩阵则k个簇对应的非公共无人机的相对定位结果构成的坐标矩阵为最优坐标矩阵z
i+1
和的联合矩阵即n架人机构成的无人机群的相对定位结果：果：果：2.根据权利要求1所述的基于距离测量的无人机群分布式相对定位方法，其特征在于，步骤(1)中所述的将无人机群分为k个簇，实现步骤为：(1a)每架无人机通过无线通信模块周期性地广播包含自己编号id
n
的消息，并接收通信范围r内其他无人机的广播消息，每架无人机通过接收到的来自第n'架无人机的广播消息中的无人机编号id
n'
，计算得到其自身的邻居表即nt
n
＝{id
n'
|d
nn'
≤r}，则n架无人机的邻居表构成集合nt＝{nt
n
|1≤n≤n}，其中，n'∈n，n'≠n，d
nn'
表示第n架无人机到第n'架无人机之间的距离；(1b)令k＝1，通过集合nt，计算每架无人机的连通度则n架无人机的连通度构成集合并选择dc
k
中连通度最大的无人机作为中心无人机g
k
，将g
k
与其邻居表中的无人机构成第k个簇g
k
，并计算已分簇无人机构成的集合w
k
＝g
k
，其中，card(
·
)表示取元素数量；(1c)判断card(w
k
)＝n是否成立，若是，得到包括k个簇的集合g＝{g
k
|1≤k≤k}；否则，令k＝k+1，执行步骤(1d)；(1d)将集合w
k-1
中除中心无人机以外的s架无人机构成待选无人机集合q，并将q中的每架待选无人机与其邻居表构成集合cu
s
，并计算cu
s
和w
k-1
的相交数目c
s
，则s架待选无人机对应的相交数目c
s
构成的集合为c＝{c
s
|1≤s≤s}；通过cu
s
和w
k-1
，计算集合q中每架待选无人机的连通度则s架待选无人机的连通度构成的集合为通过ds，在集合c中选择满足条件c
s
≥4的无人机中对应的连通度最大的无人机作为第k架中心无人机g
k
，将g
k
及其邻居表中的无人机构成第k个簇g
k
，通过w
k-1
和g
k
计算已分簇无人机构成的集合w
k
，执行步骤(1c)；
w
k
＝w
k-1
∪g
k
其中，∩表示求交集，∪表示求并集。3.根据权利要求1所述的基于距离测量的无人机群分布式协同自主定位方法，其特征在于，步骤(2b)中所述的采用基于加权的半定规划算法，并通过g
k
的距离信息可信度矩阵w
k
对g
k
的距离信息矩阵进行修正，得到g
k
的修正后的距离信息矩阵实现步骤为：构建映射函数f
d
(
·
)，通过第k个簇g
k
中的无人机数量m计算正定矩阵f，通过w
k
、f
d
(
·
)、f、构建加权误差最小化的半定规划模型(ι)并求得其最优结果通过f和f
d
(
·
)，计算修正后的距离信息矩阵计算修正后的距离信息矩阵计算修正后的距离信息矩阵(ι).(ι).其中，arg min f()表示取使得f()函数值最大时自变量的值，||
·
||
f
表示取矩阵的f范数，diag(
·
)表示生成一个由矩阵(
·
)的主对角线元素组成的列向量，
⊙
表示哈达玛乘积。4.根据权利要求1所述的基于距离测量的无人机群分布式相对定位方法，其特征在于，步骤(4c)中所述的采用迭代的加权最小二乘坐标融合方法，并通过p
common
、ψ和z
opt
计算最优坐标矩阵z
i+1
、旋转矩阵集合平移向量集合实现步骤为：(4c1)初始化第i次循环时的最优坐标矩阵为z
i
、旋转矩阵集合为r
i
、平移向量集合为t
i
、变换误差集合为α
i
，并令i＝0：z
i
＝z
optopt
其中，分别表示第i次循环时坐标矩阵变换到最优坐标矩阵z
i
的旋转矩阵、平移向量、变换误差，03×3表示元素值全为零的维度为3
×
3的矩阵，03×1表示三个元素值全为零的列向量；(4c2)计算每个簇g
k
对应的公共无人机局部相对定位坐标矩阵的均值向量μ
p
和最优坐标矩阵z
i
的均值向量μ
z
，以及和z
i
的协方差矩阵c
p,z
，对协方差矩阵c
p,z
进行奇异值分解得到特征向量和并计算旋转矩阵通过μ
p
、μ
z
和计算平移向量通过z
i
和计算变换误差则k个旋转矩阵构成的集合、k个平移向量构成的集合、k个变换误差构成的集合分别为构成的集合、k个变换误差构成的集合分别为构成的集合、k个变换误差构成的集合分别为构成的集合、k个变换误差构成的集合分别为构成的集合、k个变换误差构成的集合分别为构成的集合、k个变换误差构成的集合分别为构成的集合、k个变换误差构成的集合分别为构成的集合、k个变换误差构成的集合分别为其中，(
·
)
:,h
表示取矩阵(
·
)的第h列向量；(4c3)通过ψ、r
i+1
、t
i+1
计算每一个坐标向量加权变换的最优坐标向量并构成最优坐标矩阵z
i+1
，通过权重集合ψ、旋转矩阵集合r
i+1
、平移向量集合t
i+1
，计算每一个坐标向量变换到最优坐标向量的加权变换误差则h个加权变换误差构成的加权变换误差集合为误差集合为误差集合为误差集合为
(4c4)通过ψ、α
i+1
和ξ
i+1
，判断是否成立，若是，则输出最优坐标矩阵z
i+1
、旋转矩阵集合平移向量集合否则，令i＝i+1，并执行步骤(4c2)。

技术总结
本发明提出了一种基于距离测量的无人机群分布式相对定位方法，实现步骤为：对无人机群进行分簇；构建每个簇的距离信息矩阵并对其进行修正；基于多维标度分析算法对每个簇中的无人机进行局部相对定位；对局部相对定位结果进行全局坐标融合以获取无人机群相对定位结果。本发明在对局部相对定位结果进行全局坐标融合的过程中，根据不同簇的局部相对定位结果计算坐标融合的误差函数的权重，并采用迭代的加权最小二乘坐标融合方法，迭代计算所有簇进行坐标融合时的最优旋转矩阵、平移向量，避免了串行的坐标融合方式带来的误差传递，减小了由于不同簇的局部相对定位误差导致的坐标融合时的误差累计对定位结果准确性的影响，提升了集群的相对定位精度。了集群的相对定位精度。了集群的相对定位精度。


技术研发人员：王勇 刘运松
受保护的技术使用者：西安电子科技大学
技术研发日：2023.03.16
技术公布日：2023/7/19