二站测向定位算法
1.测向原理
如图所示，有基站 S 1 ( x 1 , y 1 , z 1 ) , S 2 ( x 2 , y 2 , z 2 ) S_1(x_1,y_1,z_1),S_2(x_2,y_2,z_2) S1(x1,y1,z1),S2(x2,y2,z2)，目标 T ( x , y , z ) T(x,y,z) T(x,y,z)。图中关键角度可由基站坐标和目标坐标表示如下：
{ tan β 1 = y − y 1 x − x 1 相 对 于 站 S 1 的 方 位 角 tan β 2 = y − y 2 x − x 2 相 对 于 站 S 2 的 方 位 角 tan ξ 1 = z − z 1 ( x − x 1 ) 2 + ( y − y 1 ) 2 相 对 于 站 S 1 的 俯 仰 角 tan ξ 2 = z − z 2 ( x − x 2 ) 2 + ( y − y 2 ) 2 相 对 于 站 S 2 的 俯 仰 角 \begin{cases} \tan\beta_1= \frac{y-y_1}{x-x_1} \quad相对于站S_1的方位角\\ \tan\beta_2= \frac{y-y_2}{x-x_2} \quad相对于站S_2的方位角\\ \tan\xi_1=\frac{z-z1}{\sqrt{(x-x_1)^2+(y-y_1)^2}} \quad相对于站S_1的俯仰角\\ \tan\xi_2=\frac{z-z2}{\sqrt{(x-x_2)^2+(y-y_2)^2}} \quad相对于站S_2的俯仰角 \end{cases} ⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧tanβ1=x−x1y−y1相对于站S1的方位角tanβ2=x−x2y−y2相对于站S2的方位角tanξ1=(x−x1)2+(y−y1)2z−z1相对于站S1的俯仰角tanξ2=(x−x2)2+(y−y2)2z−z2相对于站S2的俯仰角
由这四个角中的三个，结合基站坐标即可反推出目标位置，推导如下：
s t e p 1 : 两 基 站 间 的 距 离 L = ( x 1 − x 2 ) 2 + ( y 1 − y 2 ) 2 + ( z 1 − z 2 ) 2 step1:两基站间的距离L=\sqrt{(x1-x2)^2+(y_1-y_2)^2+(z_1-z_2)^2} step1:两基站间的距离L=(x1−x2)2+(y1−y2)2+(z1−z2)2
s t e p 2 : 在 △ S 1 T ′ S 2 中 ， 由 正 弦 定 理 得 : sin ( β 2 − β 1 ) L = sin ( π − β 2 ) R = sin ( β 2 ) R ⟹ R = L sin β 2 sin ( β 2 − β 1 ) step2:在\triangle S1 T'S2中，由正弦定理得:\frac{\sin(\beta2-\beta1)}{L}=\frac{\sin(\pi-\beta2)}{R}=\frac{\sin(\beta2)}{R}\implies R=\frac{L\sin \beta2}{\sin(\beta2-\beta1)} step2:在△S1T′S2中，由正弦定理得:Lsin(β2−β1)=Rsin(π−β2)=Rsin(β2)⟹R=sin(β2−β1)Lsinβ2
s t e p 3 : 在 △ T T ′ S 1 中 ， 得 T 距 S 1 的 距 离 R ′ = R cos ξ 1 step3:在\triangle TT'S_1中，得T距S_1的距离R'=\frac{R}{\cos \xi_1} step3:在△TT′S1中，得T距S1的距离R′=cosξ1R
s t p e 4 : 可 得 : x = R ′ cos ξ 1 cos β 1 + x 1 ; y = R ′ cos ξ 1 sin β 1 + y 1 ; z = R ′ sin ξ 1 + z 1 stpe4:可得:x=R'\cos\xi_1\cos\beta_1+x_1;y=R'\cos\xi_1\sin\beta_1+y_1;z=R'\sin\xi_1+z_1 stpe4:可得:x=R′cosξ1cosβ1+x1;y=R′cosξ1sinβ1+y1;z=R′sinξ1+z1
2.误差推导
条件:
S 1 ( x 1 , y 1 , z 1 ) S 2 ( x 2 , y 2 , z 2 ) T ( x , y , z ) S_1(x_1,y_1,z_1) \qquad S_2(x_2,y_2,z_2) \qquad T(x,y,z) S1(x1,y1,z1)S2(x2,y2,z2)T(x,y,z)
对 β 1 , β 2 , ξ 1 , ξ 2 \beta1,\beta2,\xi_1,\xi_2 β1,β2,ξ1,ξ2求全微分，可得：
d β 1 = − ( y − y 1 ) ( x − x 1 ) 2 + ( y − y 1 ) 2 d x + − ( x − x 1 ) ( x − x 1 ) 2 + ( y − y 1 ) 2 d y + k 1 d β 2 = − ( y − y 2 ) ( x − x 2 ) 2 + ( y − y 2 ) 2 d x + − ( x − x 2 ) ( x − x 2 ) 2 + ( y − y 2 ) 2 d y + k 2 d ξ 1 = − ( x − x 1 ) ( z − z 1 ) L 2 ( x − x 1 ) 2 + ( y − y 1 ) 2 d x + − ( y − y 1 ) ( z − z 1 ) L 2 ( x − x 1 ) 2 + ( y − y 1 ) 2 d y + ( x − x 1 ) 2 + ( y − y 1 ) 2 L 2 d z + k 3 d ξ 2 = − ( x − x 2 ) ( z − z 2 ) L 2 ( x − x 2 ) 2 + ( y − y 2 ) 2 d x + − ( y − y 2 ) ( z − z 2 ) L 2 ( x − x 2 ) 2 + ( y − y 2 ) 2 d y + ( x − x 2 ) 2 + ( y − y 2 ) 2 L 2 d z + k 4 其 中 的 L 为 基 站 S 1 或 S 2 到 目 标 的 距 离 k 1 = ( y − y 1 ) ( x − x 1 ) 2 + ( y − y 1 ) 2 d x 1 + ( x − x 1 ) ( x − x 1 ) 2 + ( y − y 1 ) 2 d y 1 k 2 = ( y − y 2 ) ( x − x 2 ) 2 + ( y − y 2 ) 2 d x 2 + ( x − x 2 ) ( x − x 2 ) 2 + ( y − y 2 ) 2 d y 2 k 3 = ( x − x 1 ) ( z − z 1 ) L 2 ( x − x 1 ) 2 + ( y − y 1 ) 2 d x 1 + ( y − y 1 ) ( z − z 1 ) L 2 ( x − x 1 ) 2 + ( y − y 1 ) 2 d y 1 − ( x − x 1 ) 2 + ( y − y 1 ) 2 L 2 d z 1 k 4 = ( x − x 2 ) ( z − z 2 ) L 2 ( x − x 2 ) 2 + ( y − y 2 ) 2 d x 2 + ( y − y 2 ) ( z − z 2 ) L 2 ( x − x 2 ) 2 + ( y − y 2 ) 2 d y 2 − ( x − x 2 ) 2 + ( y − y 2 ) 2 L 2 d z 2 {\rm d}\beta_1=\frac{-(y-y_1)}{(x-x_1)^2+(y-y_1)^2}{\rm d}x+\frac{-(x-x_1)}{(x-x_1)^2+(y-y_1)^2}{\rm d}y+k_1\\ {\rm d}\beta_2=\frac{-(y-y_2)}{(x-x_2)^2+(y-y_2)^2}{\rm d}x+\frac{-(x-x_2)}{(x-x_2)^2+(y-y_2)^2}{\rm d}y+k_2\\ {\rm d}\xi_1=\frac{-(x-x_1)(z-z_1)}{L^2\sqrt{(x-x_1)^2+(y-y_1)^2}}{\rm d}x+\frac{-(y-y_1)(z-z_1)}{L^2\sqrt{(x-x_1)^2+(y-y_1)^2}}{\rm d}y+\frac{\sqrt{(x-x_1)^2+(y-y_1)^2}}{L^2}{\rm d}z+k_3\\ {\rm d}\xi_2=\frac{-(x-x_2)(z-z_2)}{L^2\sqrt{(x-x_2)^2+(y-y_2)^2}}{\rm d}x+\frac{-(y-y_2)(z-z_2)}{L^2\sqrt{(x-x_2)^2+(y-y_2)^2}}{\rm d}y+\frac{\sqrt{(x-x_2)^2+(y-y_2)^2}}{L^2}{\rm d}z+k_4\\ 其中的L为基站S_1或S_2到目标的距离\\ k_1=\frac{(y-y_1)}{(x-x_1)^2+(y-y_1)^2}{\rm d}x_1+\frac{(x-x_1)}{(x-x_1)^2+(y-y_1)^2}{\rm d}y_1\\ k_2=\frac{(y-y_2)}{(x-x_2)^2+(y-y_2)^2}{\rm d}x_2+\frac{(x-x_2)}{(x-x_2)^2+(y-y_2)^2}{\rm d}y_2\\ k_3=\frac{(x-x_1)(z-z_1)}{L^2\sqrt{(x-x_1)^2+(y-y_1)^2}}{\rm d}x_1+\frac{(y-y_1)(z-z_1)}{L^2\sqrt{(x-x_1)^2+(y-y_1)^2}}{\rm d}y_1-\frac{\sqrt{(x-x_1)^2+(y-y_1)^2}}{L^2}{\rm d}z_1\\ k_4=\frac{(x-x_2)(z-z_2)}{L^2\sqrt{(x-x_2)^2+(y-y_2)^2}}{\rm d}x_2+\frac{(y-y_2)(z-z_2)}{L^2\sqrt{(x-x_2)^2+(y-y_2)^2}}{\rm d}y_2-\frac{\sqrt{(x-x_2)^2+(y-y_2)^2}}{L^2}{\rm d}z_2 dβ1=(x−x1)2+(y−y1)2−(y−y1)dx+(x−x1)2+(y−y1)2−(x−x1)dy+k1dβ2=(x−x2)2+(y−y2)2−(y−y2)dx+(x−x2)2+(y−y2)2−(x−x2)dy+k2dξ1=L2(x−x1)2+(y−y1)2−(x−x1)(z−z1)dx+L2(x−x1)2+(y−y1)2−(y−y1)(z−z1)dy+L2(x−x1)2+(y−y1)2dz+k3dξ2=L2(x−x2)2+(y−y2)2−(x−x2)(z−z2)dx+L2(x−x2)2+(y−y2)2−(y−y2)(z−z2)dy+L2(x−x2)2+(y−y2)2dz+k4其中的L为基站S1或S2到目标的距离k1=(x−x1)2+(y−y1)2(y−y1)dx1+(x−x1)2+(y−y1)2(x−x1)dy1k2=(x−x2)2+(y−y2)2(y−y2)dx2+(x−x2)2+(y−y2)2(x−x2)dy2k3=L2(x−x1)2+(y−y1)2(x−x1)(z−z1)dx1+L2(x−x1)2+(y−y1)2(y−y1)(z−z1)dy1−L2(x−x1)2+(y−y1)2dz1k4=L2(x−x2)2+(y−y2)2(x−x2)(z−z2)dx2+L2(x−x2)2+(y−y2)2(y−y2)(z−z2)dy2−L2(x−x2)2+(y−y2)2dz2
将上述式子中的前三个写成矩阵的形式有
d A = F d r + d X d A = [ d β 1 d β 2 d ξ 1 ] d r = [ d x d y d z ] d X = [ k 1 k 2 k 3 ] {\rm d}A = F{\rm d}r+{\rm d}X\\ {\rm d}A = \begin{bmatrix} {\rm d}\beta_1\\ {\rm d}\beta_2\\ {\rm d}\xi_1 \end{bmatrix} {\rm d}r= \begin{bmatrix} {\rm d}x\\ {\rm d}y\\ {\rm d}z \end{bmatrix} {\rm d}X= \begin{bmatrix} k_1\\ k_2\\ k_3 \end{bmatrix} dA=Fdr+dXdA=⎣⎡dβ1dβ2dξ1⎦⎤dr=⎣⎡dxdydz⎦⎤dX=⎣⎡k1k2k3⎦⎤
F = [ − ( y − y 1 ) ( x − x 1 ) 2 + ( y − y 1 ) 2 − ( x − x 1 ) ( x − x 1 ) 2 + ( y − y 1 ) 2 0 − ( y − y 2 ) ( x − x 2 ) 2 + ( y − y 2 ) 2 − ( x − x 2 ) ( x − x 2 ) 2 + ( y − y 2 ) 2 0 − ( x − x 1 ) ( z − z 1 ) L 2 ( x − x 1 ) 2 + ( y − y 1 ) 2 − ( y − y 1 ) ( z − z 1 ) L 2 ( x − x 1 ) 2 + ( y − y 1 ) 2 ( x − x 1 ) 2 + ( y − y 1 ) 2 L 2 ] F=\begin{bmatrix} \frac{-(y-y_1)}{(x-x_1)^2+(y-y_1)^2}&&\frac{-(x-x_1)}{(x-x_1)^2+(y-y_1)^2}&&0\\ \frac{-(y-y_2)}{(x-x_2)^2+(y-y_2)^2}&&\frac{-(x-x_2)}{(x-x_2)^2+(y-y_2)^2}&&0\\ \frac{-(x-x_1)(z-z_1)}{L^2\sqrt{(x-x_1)^2+(y-y_1)^2}}&&\frac{-(y-y_1)(z-z_1)}{L^2\sqrt{(x-x_1)^2+(y-y_1)^2}}&&\frac{\sqrt{(x-x_1)^2+(y-y_1)^2}}{L^2} \end{bmatrix} F=⎣⎢⎢⎡(x−x1)2+(y−y1)2−(y−y1)(x−x2)2+(y−y2)2−(y−y2)L2(x−x1)2+(y−y1)2−(x−x1)(z−z1)(x−x1)2+(y−y1)2−(x−x1)(x−x2)2+(y−y2)2−(x−x2)L2(x−x1)2+(y−y1)2−(y−y1)(z−z1)00L2(x−x1)2+(y−y1)2⎦⎥⎥⎤
因此， d r {\rm d}r dr的协方差矩阵可写为：
P d r = ( F T F ) − 1 F T ( P d A + P X ) F ( F T F ) − 1 P_{ {\rm d}r}=(F^TF)^{-1}F^T(P_{ {\rm d}A}+P_X)F(F^TF)^{-1} Pdr=(FTF)−1FT(PdA+PX)F(FTF)−1
其中：
P d A = [ σ β 1 2 0 0 0 σ β 2 2 0 0 0 σ ε 1 2 ] P_{ {\rm d}A} = \begin{bmatrix} \sigma _{\beta1}^2&&0&&0\\ 0&&\sigma _{\beta2}^2&&0\\ 0&&0&&\sigma _{\varepsilon 1}^2 \end{bmatrix} PdA=⎣⎡σβ12000σβ22000σε12⎦⎤
P X = σ s 2 [ 1 ( x − x 1 ) 2 + ( y − y 1 ) 2 0 2 ( x − x 1 ) ( y − y 1 ) ( z − z 1 ) L 2 ( ( x − x 1 ) 2 + ( y − y 1 ) 2 ) 3 2 0 1 ( x − x 2 ) 2 + ( y − y 2 ) 2 0 2 ( x − x 1 ) ( y − y 1 ) ( z − z 1 ) L 2 ( ( x − x 1 ) 2 + ( y − y 1 ) 2 ) 3 2 0 ( ( x − x 1 ) 2 + ( y − y 1 ) 2 ) ( z − z 1 ) 2 L 4 ( ( x − x 1 ) 2 + ( y − y 1 ) 2 ) + L 4 ( ( x − x 1 ) 2 + ( y − y 1 ) 2 ) L 8 ] P_X=\sigma _s^2 \begin{bmatrix} \frac{1}{(x-x_1)^2+(y-y_1)^2}&&0&&\frac{2(x-x_1)(y-y_1)(z-z_1)}{L^2((x-x_1)^2+(y-y_1)^2)^{\frac{3}{2}}}\\ 0&&\frac{1}{(x-x_2)^2+(y-y_2)^2}&&0\\ \frac{2(x-x_1)(y-y_1)(z-z_1)}{L^2((x-x_1)^2+(y-y_1)^2)^{\frac{3}{2}}}&&0&&\frac{((x-x1)^2+(y-y_1)^2)(z-z_1)^2}{L^4((x-x_1)^2+(y-y_1)^2)}+\frac{L^4((x-x_1)^2+(y-y_1)^2)}{L^8} \end{bmatrix} PX=σs2⎣⎢⎢⎡(x−x1)2+(y−y1)210L2((x−x1)2+(y−y1)2)232(x−x1)(y−y1)(z−z1)0(x−x2)2+(y−y2)210L2((x−x1)2+(y−y1)2)232(x−x1)(y−y1)(z−z1)0L4((x−x1)2+(y−y1)2)((x−x1)2+(y−y1)2)(z−z1)2+L8L4((x−x1)2+(y−y1)2)⎦⎥⎥⎤
则定位几何精度为：
G D O P = t r ( P d r ) = P d r ( 1 , 1 ) + P d r ( 2 , 2 ) + P d r ( 3 , 3 ) GDOP=\sqrt{tr(P_{ {\rm d}r})}=\sqrt{P_{ {\rm d}r}(1,1)+P_{ {\rm d}r}(2,2)+P_{ {\rm d}r}(3,3)} GDOP=tr(Pdr)=Pdr(1,1)+Pdr(2,2)+Pdr(3,3)
定位算法的MATLAB实现及其效果图：
clear all; close all; clc;
stations = [0 0 0;20 0 0]; %基站位置
X = -100:100; %生成目标位置
Y = X.*sin(X);
Z = X+Y;
N = length(X);
Locations = zeros(N,3);
angs = zeros(N,4);
for i = 1:N
angs(i,:) = Angs(stations*1e3,[X(i),Y(i),Z(i)]*1e3);
end
for i = 1:N
Locations(i,:) = AOA1(stations*1e3,angs(i,:))/1e3;
end
figure(1);
plot3(X,Y,Z,'r*');
hold on;
plot3(Locations(:,1),Locations(:,2),Locations(:,3),'b--o');
legend('真实位置','定位位置');
xlabel('X Km');
ylabel('Y Km');
zlabel('Z Km');
%% 根据目标位置，生成观测样本（角度信息）
function [angs] = Angs(stations,T)
B1 = atan2((T(2)-stations(1,2)),(T(1)-stations(1,1)));
B2 = atan2((T(2)-stations(2,2)),(T(1)-stations(2,1)));
E1 = atan2((T(3)-stations(1,3)),(sqrt((T(1)-stations(1,1))^2+(T(2)-stations(1,2))^2)));
E2 = atan2((T(3)-stations(2,3)),(sqrt((T(1)-stations(2,1))^2+(T(2)-stations(2,2))^2)));
angs = [B1 B2 E1 E2];
end
%% 根据角度信息，基站位置反推目标位置
function [location] = AOA1(stations,angs)
L = sqrt((stations(1,1)-stations(2,1))^2+(stations(1,2)-stations(2,2))^2+(stations(1,3)-stations(2,3))^2); %两基站间距离，基线长度
R = (L*sin(angs(2)))/(sin(angs(2)-angs(1))*cos(angs(3)));
x = R*cos(angs(3))*cos(angs(1));
y = R*cos(angs(3))*sin(angs(1));
z = R*sin(angs(3));
location = [x y z];
end
定位精度可视化的MATLAB代码及其效果图：
%% 条件
% $s1=(-10,0,0)km, s2=(10,0,0)km T=(x,y,10)km, \sigma_{\beta}=1mrad,\sigma_{\varepsilon}=1mrad,\sigma_s = 0.001km$
clear all; close all; clc;
ang_1 = 0.001; %方位角标准差
ang_2 = 0.001; %俯仰角标准差
dx = 0.001; %站址误差
stations = [-10,0,0;10,0,0]; %基站位置
%% 在([-100,100][-100,100])遍历目标点（Z = 10Km），绘制不同目标点的GPOD
X = [-100:1:100]; % km
Y = [-100:1:-20,20:1:100];
G=zeros(length(X),length(Y),'double');
for i = 1:length(X)
for j = 1:length(Y)
G(i,j) = gdop_1(stations,[X(i),Y(j),10],ang_1,ang_2,dx);
end
end
G = real(G);
figure;
mesh(X,Y,G');hold on;grid on;
plot3(stations(:,1),stations(:,2),[0,0],'o','Linewidth',1.5);
xlabel('x(km)');ylabel('y(km)');zlabel('z(km)');
figure;
[C,h] = contour(X,Y,G',15);hold on; grid on; %contour绘制等高线
clabel(C,h); %clabel为等高线添加标签
plot(stations(:,1),stations(:,2),'o');
xlabel('x(km)');ylabel('y(km)');
title('{$$ S_1(-10,0,0),S_2(10,0,0),\sigma_{\beta}=1mrad,sigma_{\varepsilon}=1mrad,\sigma_s = 0.001km $$}','Interpreter','latex');
axis equal;
axis tight;
%% 取出Y方向的几个特殊列，观察定位误差与X方向距离的关系
figure(3);
plot(X,G(:,[82,102,142]));
legend('Y=20km','Y=40km','Y=80km');
%% 计算GPOD的过程
function [gdop] = gdop_1 (stations,t,ang_1,ang_2,dx)
x1 = stations(1,1);
y1 = stations(1,2);
z1 = stations(1,3);
x2 = stations(2,1);
y2= stations(2,2);
z2 = stations(2,3);
x = t(1);
y = t(2);
z = t(3);
F = zeros(3);
Px = zeros(3);
Pdr = zeros(3);
Pdq = [ang_1^2,0,0;0,ang_1^2,0;0,0,ang_2^2];
L = sqrt((x-x1)^2+(y-y1)^2+(z-z1)^2);
d1 = (x-x1)^2+(y-y1)^2;
d2 = (x-x2)^2+(y-y2)^2;
m1 = (x-x1)*(y-y1)*(z-z1);
N = L^2*sqrt(d1);
F(1,1) = -(y-y1)/d1;
F(1,2) = -(x-x1)/d1;
F(1,3) = 0;
F(2,1) = -(y-y2)/d2;
F(2,2) = -(x-x2)/d2;
F(2,3) = 0;
F(3,1) = -((x-x1)*(z-z1))/(L^2*sqrt(d1));
F(3,2) = -((y-y1)*(z-z1))/(L^2*sqrt(d1));
F(3,3) = sqrt(d1)/(L^2);
Px(1,1) = 1/d1;
Px(1,2) = 0;
Px(1,3) = 2*m1/(d1*N);
Px(2,1) = 0;
Px(2,2) = 1/d2;
Px(2,3) =0;
Px(3,1) = Px(1,3);
Px(3,2) = 0;
Px(3,3) = d1*(z-z1)^2/(N^2)+N^2/L^8;
Px = dx.*Px;
Pdr = (F'*F)^(-1)*(F')*(Pdq+Px)*F*((F'*F)^(-1));
gdop =sqrt(trace(Pdr));
end
京公网安备 11010502036488号