一种基于无人机协同编队的无源多点融合定位方法

1.本发明涉及无人机编队多源融合定位领域，尤其涉及一种基于无人机协同编队的无源多点融合定位方法。


背景技术：

2.无人机无源多点融合定位在军用侦查、突袭、搜捕等协同指挥作战领域，民用飞行表演、应急救援、地形勘探等通用航空性质领域中应用十分广泛。由于单个无人机的载荷能力有限，携带的传感器种类较少，作用范围较小；多个无人机协同则能有效拓宽监视探测的时空覆盖范围，提高目标探测的实时性，发挥多传感器协同工作的优点，提高目标识别的准确性，增强系统的抗干扰性能，改进系统工作的可靠性、容错性，所以多无人机编队逐渐成为无人机领域的一个研究热点；无人机协同编队飞行、无源多点融合定位技术也逐渐开始兴起。然而，对于多无人机协同方面的研究我国起步较晚，目前开展的研究主要还处于设定某个场景，设定好无人机群需要完成的任务，在此基础上展开相关的工作。此外，现有的无人机无源多点定位技术仍存在着误差大、误差分析方法不完善等问题。


技术实现要素：

3.针对现有技术的问题，本发明提供了一种基于无人机协同编队的无源多点融合定位方法，能够减小定位误差，提升无人机的高精度协同定位能力。
4.为解决上述技术问题，本发明所采用的技术方案是：
5.一种基于无人机协同编队的无源多点融合定位方法，包括以下步骤：
6.步骤s1、通过广义互相关算法以及双曲线方程解析式构建非线性方程组；然后基于矩阵分块以及初等行变换进行无人机无源多点定位公式推导获得目标位置解析解；最后基于误差多维正态分布函数以及解析解矩阵最大特征值进行解析解误差分析；
7.步骤s2、构建无人机编队空间直角坐标参考系，采用doa和tdoa三站无源多点定位方法结合最小二乘拟合方法获得定位期望坐标与误差分布公式，然后设计模拟退火算法搜索无人机最优融合定位编队构型。
8.进一步的，所述步骤s1中目标位置解析解的最终表达式如下：
[0009][0010]
其中：
[0011]
[0012][0013][0014]
i＝1,2,3,x
i1
＝x
i-x1,y
i1
＝y
i-y1,i＝2,3；(xi,yi),i＝1,2,3表示为无
[0015]
人机位置坐标；p1为参考基准无人机位置，有p1＝[x1,y1]；p2，p3为其他两个无人机位置，有p2＝[x2,y2]，p3＝[x3,y3]；x
ij
表示x
i-xj,y
ij
表示y
i-yj；即无人机i在无人机j坐标系中的坐标表示；r1表示从基准无人机p1测得到目标位置的欧式距离；r
ij
表示目标到第i个无人机和第j个无人机之间的距离差；||pi,pj||2表示从位置pi到位置pj的2-范数。
[0016]
进一步的，所述步骤s2具体包括：
[0017]
步骤s21、基于无人机位置向量构建无人机编队空间直角坐标系；并采用施密特正交化获得标准正交基；
[0018]
步骤s22、基于doa和tdoa三站协同探测技术，通过无人机和目标的空间几何关系来确定目标位置，首先以无人机1为基准无人机，将无人机1,2,3分为1,2与1,3两组，然后分别对任一组进行双站无源定位分析，获得其期望定位坐标解析解与误差概率分布特征；
[0019]
步骤s23、采用线性最小二乘拟合方法对解析解的解空间进行拟合，得到目标点的估计位置与误差分布公式；
[0020]
步骤s24、设计模拟退火算法搜索不同的无人机空间位置构型，然后求得优化后定位欧式距离均方误差最小的无人机最优融合定位编队构型。
[0021]
进一步的，最终获得的无人机最优融合定位编队构型具有以下特征：
[0022]
(1)三架无人机的空间构型形成一个90-120度夹角的空间钝角三角形；
[0023]
(2)其中两架无人机初始位置在相同或近似飞行高度，另一架无人机与其他两架无人机具有一定飞行高度差异；
[0024]
(3)三架无人机构成空间三角形边长之比接近最短边:次短边:最长边＝1:1.33:1.76。
[0025]
与现有技术相比，本发明具有以下有益效果：
[0026]
1.相比于传统方法，本发明中的无人机融合定位编队构型下的目标横向定位误差95％置信区间不超过25m，测向定位误差95％置信区间不超过27m，具有定位误差小，协同定位精度高的优点；
[0027]
2.本发明提出的基于无人机协同编队的无源多点融合定位方法具有较好的定位实时性，能够解决单站无源定位探测精度较低、有源定位探测易被截获的问题。
附图说明
[0028]
图1为本发明一种基于无人机编队的无源多点融合定位方法的整体技术路线图；
[0029]
图2为本发明一种基于无人机协同编队的无源多点定位方法的示意图
[0030]
图3为基于doa和tdoa的双站无源定位分析模型；
[0031]
图4为基于模拟退火的无人机最优无源定位构型搜索算法流程图；
[0032]
图5为可实现高精度目标定位的无人机最优无源多点定位构型1；
[0033]
图6为可实现高精度目标定位的无人机最优无源多点定位构型2；
[0034]
图7为无人机最优构型下的定位误差分布。
具体实施方式
[0035]
下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
[0036]
如图1和图2所示，本发明公开了一种基于无人机协同编队的无源多点融合定位方法。首先采用最小二乘法计算得到在多种高斯白噪声影响下的目标定位解析解，得出目标三维位置估计值；然后构建基于模拟退火算法的无人机空间构型优化模型，求解得到目标定位均方差最小的无人机空间构型，随后通过大量仿真实验对比得出了可实现高精度目标定位的无人机空间构型的基本特征。具体步骤如下：
[0037]
步骤一：无人机无源多点定位公式推导
[0038]
步骤1.1构建解析解方程
[0039]
假设二维平面中三个无人机接收同一目标信号源发射的信号，如图3所示，无人机位置坐标分别为(xi,yi),i＝1,2,3，目标位置坐标为t:[x,y]。
[0040]
假设第i个无人机的接收信号为：
[0041]
ui(t)＝s(t-di)+vi(t),i＝1,2,3
[0042]
其中，s(t)为目标发射的源信号，di为源信号传播到达第i个无人机的时延，vi(t)为时延性高斯白噪声，且噪声与信号相互独立。
[0043]
首先，以第1个无人机的时延为参考基准，利用广义互相关算法(generalized cross-correlation,gcc)估计源信号到达第i个无人机与第1个基准无人机的时差为：
[0044]di1
＝d
i-d1,i＝2,3
[0045]
然后，利用时差d
i1
和光速c，根据无人机与目标的几何关系，列方程组求解目标位置，可得目标到第i个无人机和第1个无人机之间的距离差r
i1
为：
[0046]ri1
＝cd
i1
＝r
i-r1,i＝2,3
[0047]
其中，ri为目标到第i个无人机的距离，满足
[0048]
根据电磁波到达第i个无人机与第1个无人机的程差构建双曲线方程为：
[0049][0050]
再将ri两边同时平方展开可得：
[0051]ri2
＝(x
i-x)2+(y
i-y)2＝k
i-2xix-2yiy+x2+y2,i＝1,2,3
[0052]
其中，
[0053]
利用双曲线方程构建非线性方程组可求解目标位置，但是求非线性方程组的解非常困难。因此，考虑将上式构成的方程组转换成一个伪线性方程组，代入得：
[0054]
(r
i1
+r1)2＝k
i-2xix-2yiy+x2+y2[0055][0056]
为消去x2和y2，再上式两边同时减去r
12
＝k
1-2x1x-2y1y+x2+y2可得：
[0057]
[0058][0059][0060]
即：
[0061][0062]
其中，x
i1
＝x
i-x1,y
i1
＝y
i-y1,i＝2,3。
[0063]
现在得到了一个非线性方程组，为了求解这个方程组，首先将r1看作是一个已知量，因此我们可以得到如下矩阵表达式：
[0064]
ax＝f
[0065]
其中，
[0066]
有rank(a)＝2，方程组未知数个数等于方程个数，有唯一解。用伪逆法解方程可以得到：
[0067][0068]
由此，方程组转化为关于未知数x,y和r1的伪线性方程组。下面的问题即为求解该伪线性方程组。
[0069]
步骤1.2解析解推导
[0070]
通过联立两个双曲线方程，推导公式求解伪线性方程，可得x,y的最小二乘估计：
[0071][0072]
其中，
[0073]
令
[0074]
则
[0075]
再令
[0076]
则
[0077]
这样，就能将x,y转化成一个有关于q1,q2,p1,p2的表达式：
[0078][0079]
此时方程组中只有一个未知变量，即为r1。可得如下方程：
[0080][0081]
此时方程即等价于是一个关于r1的一元二次方程，其中：
[0082][0083]
通过对一元二次方程求解，即可得到r1的两个解，根据先验信息舍去一个无效解，将有效解带入上述方程组，即可求得目标t的解析解为：
[0084][0085]
其中：
[0086][0087]
i＝1,2,3,x
i1
＝x
i-x1,y
i1
＝y
i-y1,i＝2,3。
[0088]
步骤1.3解析解误差分析
[0089]
基于目标位置t的解析解公式可进一步简化为：
[0090][0091]
其中，p1为参考基准无人机位置，有p1＝[x1,y1]；p2，p3为其他两个无人机位置，有p2＝[x2,y2]，p3＝[x3,y3]；x
ij
表示x
i-xj,即无人机i在无人机j坐标系中的坐标表示；r1表示从基准无人机p1测得到目标位置的欧式距离；pi,p
j2
表示从位置pi到位置pj的2-范数；
[0092]
根据模型假设，有：
[0093]
x
k1
'～n(x
k1
,σ
12
),x
k1
'＝x
k1
+n(0,σ
12
)
[0094]yk1
'～n(y
k1
,σ
22
),y
k1
'＝y
k1
+n(0,σ
22
)
[0095]
r'～n(r,δ2),r'＝r+n(0,δ2)
[0096]
其中，x
k1
'表示带白噪声的第k个无人机相对于基准无人机的x轴位置，y
k1
'表示带白噪声的第k个无人机相对于基准无人机的y轴位置；x
k1
和y
k1
分别表示不带白噪声的第k个无人机相对于基准无人机的x轴位置和y轴位置，n(u,σ2)表示服从均值u和均方差σ的正态分布。
[0097]
根据相互独立概率分布求和方差公式：
[0098]
d(x+y)＝d(x)+d(y)
[0099]
其中x,y表示服从一定概率分布的随机变量。
[0100]
两个独立正态分布随机变量乘积方差公式为：
[0101][0102]
两个相同正态分布随机变量乘积特征：
[0103][0104]
其中，χ2(1)表示自由度为1的卡方分布，有d(χ2(n))＝2n，n为自由度。因此，有：
[0105][0106]
推导得：
[0107][0108]
此时的α变化为：
[0109][0110][0111][0112]
[0113]
其中，t
x’表示目标在白噪声干扰下求解得到的水平x轴坐标，t
y’表示目标在白噪声干扰下求解得到的垂直y轴坐标；σ1表示无人机相对定位的水平方向标准差，σ2表示无人机相对定位的垂直方向标准差；π
tx
表示目标位置在水平方向上的概率分布，π
ty
表示目标位置在垂直方向上的概率分布。
[0114]
步骤二：基于模拟退火算法的最优无人机空间构型搜索算法设计
[0115]
步骤2.1构建无人机编队空间直角坐标系
[0116]
任意选择一架无人机作为基准无人机，基准无人机坐标始终为(0,0,0)，由无人机2到基准无人机的向量p2:(x2,y2,z2)，由无人机3到基准无人机的向量p3:(x3,y3,z3),当三架无人机不共线时，这三架无人机构成的向量空间s
p
＝span(p2,p3),采用施密特(gram-schmidt)正交化构建一个标准正交基：
[0117][0118]
其中,《p2,γ1》表示求向量p2和向量γ1的夹角余弦。
[0119]
有向量空间span(p2,p3)＝span(γ1,γ2)。
[0120]
假设舰船与基准无人机构成向量p4:(x4,y4,z4)，p4与p2，p3不共面，因此，有正交于γ1和γ2的基向量γ3：
[0121]
γ3＝p
3-λ
[0122]
λ＝《p3,γ1》γ1+《p3,γ2》γ2[0123]
至此，三架无人机与舰船之间构成空间直角坐标系有标准正交基：[γ1，γ2，γ3]。且作为原点的基准无人机坐标为(0,0,0)。在该空间中，无人机与船舶运动具有线性变化下的相对位置不变性，即：
[0124][0125]
其中，ψ为空间s
p
＝span(γ1,γ2)中的任意线性变换。
[0126]
步骤2.2基于doa和tdoa的三站无源定位
[0127]
针对单站无源定位探测精度较低、有源定位探测易被截获的问题，本节将重点研究基于doa和tdoa三站协同探测技术，通过无人机和目标的空间几何关系来确定目标位置，首先以无人机1为基准无人机，将无人机1,2,3分为1,2与1,3两组，然后分别对其中一组进行双站无源定位分析，具体定位模型如附图1所示。
[0128]
对于三维空间的无源定位，以无人机1为坐标原点构建三维坐标系，使三架无人机1,2,3处于同一个平面内，令无人机1为基准无人机，目标t为辐射源，则无人机1,2,3的坐标分别为:s1:(x1,y1,z1),s2:(x2,y2,z2),s3:(x3,y3,z3)，目标t的坐标为t:(x,y,z)，辐射源对于无人机1的目标方位角与俯仰角分别为α,β，辐射源发射的电磁波到达无人机i所需的时间为di，则到达无人机i与到达无人机1的时间差为d
i1
，则根据几何关系可得:
[0129][0130]
其中c为光速，方程组中第一式为基于tdoa方程，通过利用电磁波到达无人机i与无人机1的程差得到一个双曲面，第二式为通过无人机1与信号到达方向确定的空间直线方
程。由空间几何关系可知，tdoa的双曲面与doa的空间直线会有两个交点，其中一个即是所求辐射源即目标t的位置。
[0131]
定义n＝[cosβcosα,cosβsinα,sinβ]
t
，得到：
[0132]
cd
i1
+||s
1-ng||＝||s
i-ng||
[0133]
将两边平方可得：
[0134]
2cd
i1
||s
1-ng||＝2(s
1-si)
t
ng+||si||
2-||s1||
2-c2(d
i1
)2[0135]
令h＝||si||
2-||s1||
2-c2(d
i1
)2，再将上式两端平方，可以得到一个关于g的一元二次方程：a0g2+b0g+c0＝0，其中：
[0136][0137]
此时可求得将求得的g带入上述方程组可得目标t的坐标位置：
[0138]
x＝ng
[0139]
此时，该坐标为通过无人机1与无人机2的程差测量值与在无人机1所接受电磁波的侧向角度信息求得的有关于无人机1的两个定位结果，需要再根据无人机2的测向角度信息，用doa空间直线与tdoa双曲面再次交于两点，两组结果联合，最终可以确定其中一个点为最终的辐射源即目标t位置。
[0140]
随后，利用同样的方法，对于无人机1与无人机3两点，通过辐射源到无人机3与辐射源到无人机1之间的程差，以及辐射源到达无人机1与无人机3的侧向角度信息，求得辐射源的最终位置，即t点，由于程差误差与角度误差的存在，两组模型求得的t点并不一定交于一点，而是一定误差空间范围。
[0141]
步骤2.3最小二乘拟合
[0142]
由两组基于doa-tdoa结合的无源定位方法求得两组目标位置的参考值x1x2。
[0143]
理想的doa参考值为一条直线，但在实际测量中，由于测向基站(即无人机)不可避免地存在测量误差，得到的结果是在一定空间误差范围(方位、俯仰角)分布的射线，由此产生的定位误差。根据模型假设，有：
[0144][0145][0146]
其中，αi'表示带有白噪声的第i个无人机对目标的偏向角，βi'表示带有白噪声干扰下的第i个无人机对目标的俯仰角。
[0147]
理想的tdoa参考值为双曲面的一部分，而由于电磁波传输信号的误差不可避免，得到的结果是在一定空间误差范围分布的曲面。
[0148]
[0149]
其中，si'表示带有白噪声第i个无人机和目标位置方向的空间直线方程。
[0150]
根据doa和tdoa结合，由基准无人机分别与两架辅助无人机进行双站定位，分别得到目标位置的观测值：
[0151]
x
1k
＝n
1k
g,k＝2,3
[0152]
其中，x
1k
表示由基准无人机1和无人机k(k＝2,3)双站定位的可行解向量空间，n
1k
表示角度测量值系数矩阵，n
1k
＝[n
1 n
2 n3…
]
t
，g为解析解的中间变量。
[0153]
因为g中的未知参数si与目标位置相关，因此得到的结果是关于si的函数，并不是关于目标位置的线性方程，而是伪线性形式。由于这些线性方程不一定存在精确解，因此利用最小二乘估计法对解空间进行拟合得到目标点t的估计位置。
[0154]
步骤2.4最优无人机构型搜索法设计
[0155]
根据各个定位误差独立的假设以及步骤一中解析解形式，可以得到，当无人机与目标位置之间满足如下关系时，无人机对于目标的平面内定位误差最小：
[0156][0157][0158]
如图4所示为基于模拟退火的无人机最优无源定位构型搜索算法流程图。假设水平方向定位误差与测向角度误差之间相互独立，即测向角度误差不受无人机空间构型影响，则需优化求得使上述向量模最小时的无人机间相对位置坐标即可。然而，直接对于双目标非线性函数优化求解十分困难，因此，采用模拟退火算法搜索不同的无人机的空间位置构型，然后求得优化后定位欧式距离均方误差最小的无人机空间位置构型，是一种较为直观且简单易行的方法。
[0159]
采用模拟退火算法进行定位均方差优化算法伪代码如下表：
[0160]
基于模拟退火算法的无人机定位模型均方误差优化算法
[0161]
[0162][0163]
最终搜索得到比较适合无人机融合定位的无人机协同编队队形，如附图5、6所示，无人机编队队形具有以下特征：
[0164]
(1)三架无人机的空间构型形成一个90-120度夹角的空间钝角三角形。
[0165]
(2)两架无人机初于相同或近似飞行高度，另一架无人机与其他两架无人机具有一定飞行高度差异。
[0166]
(3)三架无人机构成空间三角形边长之比接近最短边:次短边:最长边＝1:1.33:1.76。
[0167]
最终，可以得到如图7所示的无人机最优构型下的定位误差分布，实现高精度目标定位。
[0168]
最后应说明的是：以上实施例仅用以说明本发明的技术方案而非对其进行限制，尽管参照较佳实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对本发明的技术方案进行修改或者等同替换，而这些修改或者等同替换亦不能使修改后的技术方案脱离本发明技术方案的精神和范围。

技术特征：
1.一种基于无人机协同编队的无源多点融合定位方法，其特征在于，所述方法包括以下步骤：步骤s1、通过广义互相关算法以及双曲线方程解析式构建非线性方程组；然后基于矩阵分块以及初等行变换进行无人机无源多点定位公式推导获得目标位置解析解；最后基于误差多维正态分布函数以及解析解矩阵最大特征值进行解析解误差分析；步骤s2、构建无人机编队空间直角坐标参考系，采用doa和tdoa三站无源多点定位方法，并结合最小二乘拟合方法获得定位期望坐标与误差分布公式，然后设计模拟退火算法搜索无人机最优融合定位编队构型。2.根据权利要求1所述的一种基于无人机协同编队的无源多点融合定位方法，其特征在于，所述步骤s1中目标位置解析解的最终表达式如下：其中：其中：其中：其中：x
i1
＝x
i-x1,y
i1
＝y
i-y1,i＝2,3；(x
i
,y
i
),i＝1,2,3表示为无人机位置坐标；p1为参考基准无人机位置，有p1＝[x1,y1]；p2，p3为其他两个无人机位置，有p2＝[x2,y2]，p3＝[x3,y3]；x
ij
表示x
i-x
j
,y
ij
表示y
i-y
j
；即无人机i在无人机j坐标系中的坐标表示；r1表示从基准无人机p1测得到目标位置的欧式距离；r
ij
表示目标到第i个无人机和第j个无人机之间的距离差；||p
i
,p
j
||2表示从位置p
i
到位置p
j
的2-范数。3.根据权利要求1所述的一种基于无人机协同编队的无源多点融合定位方法，其特征在于，所述步骤s2具体包括：步骤s21、基于无人机位置向量构建无人机编队空间直角坐标系；并采用施密特正交化获得标准正交基；步骤s22、基于doa和tdoa三站协同探测技术，通过无人机和目标的空间几何关系来确定目标位置，首先以无人机1为基准无人机，将无人机1,2,3分为1,2与1,3两组，然后分别对任一组进行双站无源定位分析，获得其期望定位坐标解析解与误差概率分布特征；步骤s23、采用线性最小二乘拟合方法对解析解的解空间进行拟合得到目标点的估计位置和误差分布公式；步骤s24、设计模拟退火算法搜索不同的无人机空间位置构型，求得优化后定位欧式距离均方误差最小的无人机最优融合定位编队构型。
4.根据权利要求1所述的一种基于无人机协同编队的无源多点融合定位方法，其特征在于，最终获得的无人机最优融合定位编队构型具有以下特征：(1)三架无人机的空间构型形成一个90-120度夹角的空间钝角三角形；(2)其中两架无人机的初始位置在相同或近似飞行高度，另一架无人机与其他两架无人机具有一定飞行高度差异；(3)三架无人机构成空间三角形边长之比接近最短边:次短边:最长边＝1:1.33:1.76。

技术总结
本发明公开了一种基于无人机协同编队的无源多点融合定位方法。通过广义互相关算法以及双曲线方程解析式构建非线性方程组，然后基于矩阵分块以及初等行变换进行无人机无源多点定位公式推导获得目标位置解析解，基于误差多维正态分布函数以及解析解矩阵最大特征值进行解析解误差分析；接着构建无人机编队空间直角坐标参考系，采用DOA和TDOA三站无源多点定位方法结合最小二乘拟合方法获得定位期望坐标与误差分布公式，最后设计模拟退火算法搜索无人机最优融合定位编队构型。通过大量仿真实验对比得出本发明可实现高精度目标定位的无人机空间构型的基本特征，能实现三维动态条件下多无人机实时协同高精度定位。件下多无人机实时协同高精度定位。件下多无人机实时协同高精度定位。


技术研发人员：周锦伦 张洪海 李金波 何纯韵 王雨菲
受保护的技术使用者：南京航空航天大学
技术研发日：2023.05.26
技术公布日：2023/8/21