Trilateration Algorithm

基于测距的定位技术仿真——三边测量法
仿真软件——MATLAB

1、定位算法的实现
1.1基本原理

1) 已知三点位置$(x_1,y_1 ),(x_2,y_2 ),(x_3,y_3)$
2) 已知未知点 $(x_0, y_0)$ 到三点距离$d_1,d_2,d_3$
3) 以$d_1,d_2,d_3$ 为半径作三个圆，根据毕达哥拉斯定理，得出交点即未知点的位置计算公式：
$(x_1-x_0)^2+(y_1-y_0)^2=d_1^2$
$(x_2-x_0)^2+(y_2-y_0)^2=d_2^2$
$(x_3-x_0)^2+(y_3-y_0)^2=d_3^2$

1.2解法推导

设未知点位置为 $(x, y)$， 令其中的第一个球形 $P_1$ 的球心坐标为 (0, 0)，$P_2$ 处于相同纵坐标，球心坐标为$(d, 0)，P_3$球心坐标为$(i, j)$，三个球形半径分别为$r_1, r_2, r_3，z$为三球形相交点与水平面高度。则有：
$r_{12} = x^2+y^2+z^2$
$r_{22} = (x-d)^2+y^2+z^2$
$r_{32} = (x-i)^2+(x-j)^2+z^2$
当 z = 0 时， 即为三个圆在水平面上相交为一点，首先解出 x:
$x =\frac {((r_1^2-r_2^2+d^2))}{2d}$
将公式二变形，将公式一的$z^2$ 代入公式二，再代入公式三得到 y 的计算公式：
$y = \frac{((r_1^2-r_3^2-x^2+(x-i)^2+j^2))}{2j}$

1.3代码实现
1.3.1 主文件locac3.m

%*********locac3.m****************
%---采用三边定位法对未知节点定位---------
clear;
len=1000; %参考节点数
maxx=1000;%参考节点分布的最大横坐标
maxy=1000;%参考节点分布的最大纵坐标
%------参考点[cx,cy]-----------------
cx=maxx*rand(1,len);
cy=maxy*rand(1,len);
plot(cx,cy,'k^');%参考节点图
%--------未知节点（mx,my）-----------
mlen=200;  %未知点数
mx=maxx*rand(1,mlen);
my=maxy*rand(1,mlen);
hold on;
plot(mx,my,'go'); %盲节点图
%---------定位估计----------------------
wucha_p=0.00001;   %定位误差
for p=1:mlen %对每个未知盲节点
     [xa,ya,xb,yb,xc,yc]=find_three_p(cx,cy); %从参考点中任意选出三点
     da=sqrt((mx(p)-xa)^2+(my(p)-ya)^2);
     db=sqrt((mx(p)-xb)^2+(my(p)-yb)^2);
     dc=sqrt((mx(p)-xc)^2+(my(p)-yc)^2);
     [locx(p),locy(p)]=triposition(xa,ya,da,xb,yb,db,xc,yc,dc); %计算定位坐标        
     plot(locx(p),locy(p),'r*');
end
legend('参考节点','盲节点','定位节点','Location','SouthEast');  
title('三边测量法的定位');
wdot=0;%统计定位正确的点数
for p=1:mlen
    dr(p)=sqrt((locx(p)-mx(p))^2+(locy(p)-my(p))^2);   
    if dr(p)<wucha_p
        wdot=wdot+1;    
    end
end
disp('未知节点定位准确率：');
wdot/mlen

1.3.2 find_three_p.m

function [xa,ya,xb,yb,xc,yc]=find_three_p(x,y) 
%从N点中随机选出不同的三点
% 输入：x,y是N点的坐标
%输出：xa,ya,xb,yb,xc,yc
%     为选出的任意三点的坐标
len=length(x);
% 取一点
t=fix(len*rand(1,1))+1;
ma=t;
xa=x(t);
ya=y(t);
%取第二点
while(1)
    t=fix(len*rand(1,1))+1;
    if(t~=ma)       
      mb=t;
      xb=x(t);
      yb=y(t);      
      break;
    end   
end
%取第三点
while(1)
    t=fix(len*rand(1,1))+1;
    if (t~=ma)&&(t~=mb)       
      mc=t;
      xc=x(t);
      yc=y(t);     
      break;
    end   
end

1.3.3 triposition.m

%一、m函数triposition.m文件内容
%一、m函数*****************triposition.m**************************%
function [locx,locy]=triposition(xa,ya,da,xb,yb,db,xc,yc,dc)
%              三点定位法                          %
%输入：
%   1.参考节点A（xa,ya）,B(xb,yb),C(xc,yc)
%   2.定位节点D(locx,locy)到这三点的距离分别为da,db,dc
%返回：
%   （locx，locy)为计算的定位节点D点的位置坐标
%
syms x y   %f符号变量
%--------------求解方程组------------------------------------
f1='2*x*(xa-xc)+xc^2-xa^2+2*y*(ya-yc)+yc^2-ya^2=dc^2-da^2';
f2='2*x*(xb-xc)+xc^2-xb^2+2*y*(yb-yc)+yc^2-yb^2=dc^2-db^2';
[xx,yy]=solve(f1,f2,x,y); %解关于x,y的符号方程组，得到解的符号表示，并存入xx,yy
px=eval(xx);  %解的数值px(1),px(2)
py=eval(yy);  %解的数值py(1),py(2)
locx=px;
locy=py;

2、仿真结果和分析
2.1 仿真结果

2.2 结果分析
三边定位算法是最容易实现的定位算法之一，只需要较低的成本就可以实现。但是，也存在着定位精度不高，算法鲁棒性不好，节点不规则布置引起的定位误差等。为了降低定位误差，提高算法的鲁棒性，可以使用基于加权处理的三边测量定位算法。第一步，依据位置节点与锚节点的通信质量选取符合条件的锚节点参与定位计算；第二步，依据改进的三边测量定位算法计算出未知节点的多个估算坐标；第三步，利用权重函数计算未知节点坐标