[发明专利]一种机载多主动雷达测距的地面物体高精度定位方法

权利要求书

1.一种机载多主动雷达测距的地面物体高精度定位方法，其特征在于：包括如下步骤：

步骤(一)、采用三架无人机对同一地面物体进行搜索，将首先搜索到所述地面物体的无人机定义为主机，主机根据在WGS-84坐标系中自身的位置坐标、对物体的测距值和对物体的测角值解算出所述地面物体的位置坐标S₀，并将位置坐标S₀发送给其余两架无人机，其余两架无人机根据位置坐标S₀对所述地面物体进行搜素，当搜索到地面物体后，其余两架无人机将对地面物体的测距值和无人机在WGS-84坐标系中自身的位置坐标发送给主机；

步骤(二)、设主机在WGS-84坐标系中自身的位置坐标为P₁(x₁，y₁，z₁)、其余两架无人机自身的位置坐标分别为P₂(x₂，y₂，z₂)，P₃(x₃，y₃，z₃)，主机对地面物体的测距值为r₁，其余两架无人机对地面物体的测距值分别为r₂，r₃，求解地面物体的位置坐标S(x，y，z)，具体方法如下：

(x-x1)2+(y-y1)2+(z-z1)2=r12(x-x2)2+(y-y2)2+(z-z2)2=r22(x-x3)2+(y-y3)2+(z-z3)2=r32---(1)]]>

令：

d1=r12,d2=r22,d3=r32;X=x-x1,Y=y-y1,Z=z-z1;a1=x2-x1,b1=y2-y1,c1=z2-z1;a2=x3-x1,b2=y3-y1,c2=z3-z1;---(2)]]>

其中d₁，d₂，d₃均为中间变量，无含义；X，Y，Z均为中间变量，无含义；a₁b₁，c₁，a₂，b₂，c₂均为中间变量，无含义；

结合式(1)和式(2)可以得到：

d₁＝X²+Y²+Z²             (3)

d₂＝(X-a₁)²+(Y-b₁)²+(Z-c₁)²                (4)

d₃＝(X-a₂)²+(Y-b₂)²+(Z-c₂)²            (5)

令式(3)减去式(4)得到式(7)，式(3)减去式(5)得到式(8)：

d₁＝X²+Y²+Z²            (3)

d₁-d₂+a₁²+b₁²+c₁²＝2a₁X+2b₁Y+2c₁Z         (7)

d₁-d₃+a₂²+b₂²+c₂²＝2a₂X+2b₂Y+2c₂Z     (8)

令D₂＝(d₁-d₂+a₁²+b₁²+c₁²)/2，D₃＝(d₁-d₃+a₂²+b₂²+c₂²)/2，则上式变为：

d₁＝X²+Y²+Z²        (3)

D₂＝a₁X+b₁Y+c₁Z         (10)

D₃＝a₂X+b₂Y+c₂Z        (11)

其中D₂，D₃均为中间变量，无含义；

联立式(10)和式(11)可以得出如下公式：

X＝(b₂D₂-b₁D₃)/(b₂a₁-a₂b₁)+(b₁c₂-c₁b₂)/(b₂a₁-a₂b₁)Z   (12)

Y＝(a₂D₂-a₁D₃)/(a₂b₁-b₂a₁)+(a₁c₂-c₁a₂)/(a₂b₁-b_2a1)Z   (13)

令m₁，m₂，n₁，n₂满足：

X＝(b₂D₂-b₁D₃)/(b₂a₁-a₂b₁)+(b₁c₂-c₁b₂)/(b₂a₁-a₂b₁)Z＝m₁+n₁Z   (14)

Y＝(a₂D₂-a₁D₃)/(a₂b₁-b₂a₁)+(a₁c₂-c₁a₂)/(a₂b₁-b₂a₁)Z＝m₂+n₂Z   (15)

其中m₁，m₂，n₁，n₂均为中间变量，无含义；

将式(14)和式(15)代入式(3)可以得到：

d₁＝(m₁+n₁Z)²+(m₂+n₂Z)²+Z²          (16)

将上式整理得：

(1+n₁²+n₂²)Z²+(2m₁n₁+2m₂n₂)Z+(m₁²+m₂²-d₁)＝0   (17)

令β＝1+n₁²+n₂²，δ＝2m₁n₁+2m₂n₂，u＝m₁²+m₂²-d₁解得：

Z=-δ±δ2-4βu2β---(18)]]>

其中β，δ，u均为中间变量，无含义；

舍去Z为负数的值，将Z的值分别代入式(12)和式(13)即可解得X，Y的值，根据式(2)进一步解算出地面物体的坐标S(x，y，z)。