AOA(Angle of Arrival，到达角)定位算法及其误差分析的原理和MATLAB仿真

二站测向定位算法

1.测向原理

如图所示，有基站S1(x1,y1,z1),S2(x2,y2,z2)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−y1x−x1相对于站S1的方位角tan⁡β2=y−y2x−x2相对于站S2的方位角tan⁡ξ1=z−z1(x−x1)2+(y−y1)2相对于站S1的俯仰角tan⁡ξ2=z−z2(x−x2)2+(y−y2)2相对于站S2的俯仰角\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的俯仰角
由这四个角中的三个，结合基站坐标即可反推出目标位置，推导如下：
step1:两基站间的距离L=(x1−x2)2+(y1−y2)2+(z1−z2)2step1:两基站间的距离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
step2:在△S1T′S2中，由正弦定理得:sin⁡(β2−β1)L=sin⁡(π−β2)R=sin⁡(β2)R⟹R=Lsin⁡β2sin⁡(β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
step3:在△TT′S1中，得T距S1的距离R′=Rcos⁡ξ1step3:在\triangle TT'S_1中，得T距S_1的距离R'=\frac{R}{\cos \xi_1}step3:在△TT′S1中，得T距S1的距离R′=cosξ1R
stpe4:可得:x=R′cos⁡ξ1cos⁡β1+x1;y=R′cos⁡ξ1sin⁡β1+y1;z=R′sin⁡ξ1+z1stpe4:可得: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_1stpe4:可得:x=R′cosξ1cosβ1+x1;y=R′cosξ1sinβ1+y1;z=R′sinξ1+z1

2.误差推导

条件:
S1(x1,y1,z1)S2(x2,y2,z2)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−y1)(x−x1)2+(y−y1)2dx+−(x−x1)(x−x1)2+(y−y1)2dy+k1dβ2=−(y−y2)(x−x2)2+(y−y2)2dx+−(x−x2)(x−x2)2+(y−y2)2dy+k2dξ1=−(x−x1)(z−z1)L2(x−x1)2+(y−y1)2dx+−(y−y1)(z−z1)L2(x−x1)2+(y−y1)2dy+(x−x1)2+(y−y1)2L2dz+k3dξ2=−(x−x2)(z−z2)L2(x−x2)2+(y−y2)2dx+−(y−y2)(z−z2)L2(x−x2)2+(y−y2)2dy+(x−x2)2+(y−y2)2L2dz+k4其中的L为基站S1或S2到目标的距离k1=(y−y1)(x−x1)2+(y−y1)2dx1+(x−x1)(x−x1)2+(y−y1)2dy1k2=(y−y2)(x−x2)2+(y−y2)2dx2+(x−x2)(x−x2)2+(y−y2)2dy2k3=(x−x1)(z−z1)L2(x−x1)2+(y−y1)2dx1+(y−y1)(z−z1)L2(x−x1)2+(y−y1)2dy1−(x−x1)2+(y−y1)2L2dz1k4=(x−x2)(z−z2)L2(x−x2)2+(y−y2)2dx2+(y−y2)(z−z2)L2(x−x2)2+(y−y2)2dy2−(x−x2)2+(y−y2)2L2dz2{\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
将上述式子中的前三个写成矩阵的形式有
dA=Fdr+dXdA=[dβ1dβ2dξ1]dr=[dxdydz]dX=[k1k2k3]{\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−y1)(x−x1)2+(y−y1)2−(x−x1)(x−x1)2+(y−y1)20−(y−y2)(x−x2)2+(y−y2)2−(x−x2)(x−x2)2+(y−y2)20−(x−x1)(z−z1)L2(x−x1)2+(y−y1)2−(y−y1)(z−z1)L2(x−x1)2+(y−y1)2(x−x1)2+(y−y1)2L2]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⎦⎥⎥⎤
因此，dr{\rm d}rdr的协方差矩阵可写为：
Pdr=(FTF)−1FT(PdA+PX)F(FTF)−1P_{{\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
其中：
PdA=[σβ12000σβ22000σε12]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⎦⎤

PX=σs2[1(x−x1)2+(y−y1)202(x−x1)(y−y1)(z−z1)L2((x−x1)2+(y−y1)2)3201(x−x2)2+(y−y2)202(x−x1)(y−y1)(z−z1)L2((x−x1)2+(y−y1)2)320((x−x1)2+(y−y1)2)(z−z1)2L4((x−x1)2+(y−y1)2)+L4((x−x1)2+(y−y1)2)L8]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)⎦⎥⎥⎤
则定位几何精度为：
GDOP=tr(Pdr)=Pdr(1,1)+Pdr(2,2)+Pdr(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:Nangs(i,:) = Angs(stations*1e3,[X(i),Y(i),Z(i)]*1e3);
endfor i = 1:NLocations(i,:) = AOA1(stations*1e3,angs(i,:))/1e3;
endfigure(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