目标定位算法（二）之基于测距的定位算法

基于测距的定位算法

非测距的定位算法往往误差较大，提高精度往往依赖于密集的观测站部署。基于测距的定位算法中，借助了最小二乘原理估算目标的位置。

1.最小二乘原理

最小二乘法通过最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知数据，并使得这些求得的数据与实际数据之间误差的平方和为最小。

给定一组数据点$(x_i,y_i),i=1,2,3...,$同时寻求一个近似函数f(x),那么两者的误差大小$r_i=f(x_i)-y_i,i=1,2,3,...。$在曲线拟合中常采用误差平方和${\sum }_{i=0}^m{r}_i^2$来度量误差的整体大小。
对给定数据$(x_i,y_i),i=1,2,3...,$在取定的函数类$\Phi$中，求$f(x)\in \Phi$，使误差的平方和最小，即：

就是寻找与给定点的距离平方和为最小的曲线y=f(x)。函数f(x)称为拟合函数或最小二乘解，求拟合函数f(x)的方法称为曲线拟合的最小二乘法。

函数$\Phi$的选取采用最常用的多项拟合法：

2.最小二乘定位算法

例：在二维平面中，有随机部署的5个观测站，观测站能通过某种传感器手段测得观测站与目标之间的距离。每个观测站将自己的位置信息和观测结果交给监控中心，监控中心则能根据这些信息完成对目标的定位。用MATLAB仿真如下：

function Erchengfa
%定位初始化
Length=100;                          %场地空间，单位：米
Width=100;                           %场地空间，单位：米
Node_number=5;                       %观测站的个数
for i= 1:Node_number                 %观测站的位置初始化，这里位置是随机给定的
    Node(i).x= Width * rand;
    Node(i).y= Length * rand;
    Node(i).D= Node(i).x^2+Node(i).y^2; % 固定参数便于位置估计
end
%目标的真实位置，这里也随机给定
Target.x=Width * rand;
Target.y=Length * rand;
%观测站探测目标
X=[];
Z=[];                                 %测量距离
for i=1:Node_number
    [d1,d2]=DIST(Node(i),Target);     %观测站离目标的真实距离
    d1=d1+sqrt(5)*randn;              %假设测量距离受到均值为5的高斯白噪声的污染
    X=[X;Node(i).x,Node(i).y];
    Z=[Z,d1];
end
H=[];b=[];
for i=2:Node_number
    H=[H;2*(X(i,1)-X(1,1)),2*(X(i,2)-X(1,2))];
    b=[b;Z(1)^2-Z(i)^2+Node(i).D-Node(1).D];
end
Estimate=inv(H'*H) *H'*b;             %目标的估计位置
Est_Target.x = Estimate(1);Est_Target.y = Estimate(2);

%画图
figure
hold on;box on;axis([0 100 0 100]);     % 输出图形的框架
for i=1: Node_number
    h1= plot(Node(i).x, Node(i).y,'ko','MarkerFace','g','MarkerSize',10);
    text(Node(i).x+2,Node(i).y,[ 'Node ', num2str(i)]);
end
h2= plot(Target.x,Target.y,'k^','MarkerFace','b','MarkerSize', 10) ;
h3= plot(Est_Target.x,Est_Target.y,'ks','MarkerFace','r','MarkerSize',10);
line([Target.x,Est_Target.x],[Target.y,Est_Target.y] ,'Color' ,'k');
legend([h1,h2,h3 ],'Observation Station', 'Target Postion','Estimate Postion');
[Error_Dist,d2]= DIST( Est_Target,Target);
xlabel(['error=',num2str( Error_Dist),'m']);

%计算两点间距离
function [dist,dist2]=DIST(A,B)
dist2=(A.x-B.x)^2+(A.y-B.y)^2;
dist=sqrt(dist2);
end

运行结果：

例：假设在一个100mX100mX100m的立体空间内，在墙壁、杆柱或地面上随机部署5个具有测距功能的传感器探测节点，对该空间内的移动机器人进行目标定位，建立坐标系仿真定位算法过程，用最小二乘法估计目标位置，MATLAB 仿真如下：

function ErChengFa3d
%定位初始化
Length=100;                    %场地空间,单位:米
Width=100;                     %场地空间,单位:米
Hight=100;
Node_number=5;               %观测站的个数,也可以设置成6个或7个等,但是最少必须有4个
R=5;                            %观测站测量噪声方差
for i=1: Node_number            %观测站的位置初始化，这里位置是随机给定的
    %此处不考虑观测站在同一平面的情况,若在同-一个平面,可先估计出x,y,再算出z
    Node(i).x= Width*rand;
    Node(i).y= Length*rand;
    Node(i).z= Hight*rand;
    Node(i).D= Node(i).x^2+Node(i).y^2+Node(i).z^2; % 固定参数便于位置估计
end
%目标的真实位置,这里也随机给定
Target.x= Width*rand;
Target.y= Length*rand;
Target.z= Hight*rand; 
%观测站探测目标
X=[];                            %基站(观测站)的位置集合
Z=[];                            %测量距离
for i=1:Node_number
    [d1,d2]= DIST(Node(i),Target);%观测站离目标的真实距离
    d1=d1+sqrt(R)*randn;        %假设测量距离受到均值为5的高斯白噪声的污染
    X=[X;Node(i).x,Node(i).y,Node(i).z];
    Z=[Z,d1];
end
H=[];b=[];
for i=2:Node_number
    H=[H;2*(X(i,1)-X(1,1)),2*(X(i,2)-X(1,2)),2*(X(i,3)-X(1,3))];
    b=[b;Z(1)^2-Z(i)^2+Node(i).D-Node(1).D];
end
Estimate=inv(H'*H) *H'*b;           %目标的估计位置
Est_Target.x= Estimate( );Est_Target.y= Estimate(2);Est_Target.z= Estimate(3); 
%开始画图
figure
hold on;box on;axis([0 Width 0 Length 0 Hight]); % 输出图形的框架
for i= 1:Node_number
    h1=plot3(Node(i).x,Node(i).y,Node(i).z,'ko','MarkerFace','g');
    text(Node(i).x+2,Node(i).y,Node(i).z,[ 'Node',num2str(i)]);
end
h2=plot3(Target.x,Target.y ,Target.z,'k^','MarkerFace','b');
h3=plot3(Est_Target.x,Est_Target.y,Est_Target.z,'ks','MarkerFace','r');
legend([h1,h2,h3],'Observation Station','Target Postion','Estimate Postion');
[Error_Dist,d2]=DIST(Est_Target,Target);
title(['error=',num2str( Error_Dist),'m']);
axis square;view(3);grid on;

%子函数,计算两点间的距离
function [dist,dist2]= DIST(A,B)
dist2=(A.x-B.x)*2+(A.y-B.y)^2+(A.z-B.z)^2;
dist=sqrt(dist2);
end

运行结果：

3.基于RSSI测距的定位算法

实际中一般采用简化的渐变模型：

通常取$d_0$为1m，p(d)改写成RRSI，可得

算法流程：

例：假定一个监测场地( 100mx 100m)中随机部署3个观测站，观测站能探测到目标发送的无线信号强度，且能知道某一类目标的发射功率，观测站能根据发射功率和自身接收到的信号强度估计出目标与观测站之间的距离，最后用最小二乘法算得目标的位置，MATLAB仿真如下：

function RssiEstimate
%第一步:定位初始化
Length=100;                          %场地空间，单位：米
Width=100;                           %场地空间，单位：米
Node_number=3;                       %观测站的个数,最少必须有3个
for i=1:Node_number                  %观测站的位置初始化,这里位置是随机给定的
    Node(i).x= Width * rand;
    Node(i).y= Length * rand;
    Node(i).D= Node(i).x^2+Node(i).y^2; % 固定参数便于位置估计
end
%目标的真实位置,这里也随机给定
Target.x= Width * rand;
Target.y= Length * rand;
%第二步:各观测站对目标探测10次,将平均值作为最终RSSI值
Z=[ ];                                  %各观测站采集10次RSSI
for i=1: Node_number
    for t=1:10                          %10次采样
        [d]=DIST(Node(i),Target);        %观测站与目标的真实距离
        %距离为d时,得到的RSSI 测量值
        Z(i,t)= GetRssiValue(d)
    end
end
%求10次观测的平均值
ZZ=[];
for i=1:Node_number
    ZZ(i)=sum(Z(i,:))/10;
end
%第三步:根据采样的RSSI值，求观测距离
Zd=[];                                  %根据RSSI计算得到的距离
for i=1:Node_number
    Zd(i)=GetDistByRssi(ZZ(i));
end
%第四步:根据观测距离，用最小二乘法计算目标估计位置
H=[];
b=[];
for i=2:Node_number
    %参照三边测距法公式
    H=[H;2*(Node(i).x-Node(1).x),2*(Node(i).y-Node(1).y)];
    b=[b;Zd(1)^2-Zd(i)^2+Node(i).D-Node(1).D];
end
Estimate=inv(H'*H)*H'*b;                 %目标的估计位置
Est_Target.x=Estimate(1);Est_Target.y=Estimate(2);

%画图
figure
hold on;box on;axis([0 120 0 120]);      %输出图形的框架
for i= 1: Node_number
    h1= plot(Node(i).x, Node(i).y,'ko','MarkerFace','g','MarkerSize',10);
    text(Node(i).x+2,Node(i).y,[ 'Node ', num2str(i)]);
end
h2= plot(Target.x,Target.y,'k^','MarkerFace','b','MarkerSize', 10) ;
h3= plot(Est_Target.x,Est_Target.y,'ks','MarkerFace','r','MarkerSize',10);
line([Target.x,Est_Target.x],[Target.y,Est_Target.y] ,'Color' ,'k');
legend([h1,h2,h3 ],'Observation Station','Target Postion','Estimate Postion');
[Error_Dist]=DIST(Est_Target,Target);
xlabel(['error=',num2str( Error_Dist),'m']);

%子函数,计算两点间的距离
function [ dist]=DIST(A,B)
dist=sqrt((A.x-B.x)^2+(A.y-B.y)^2);
end

%子函数,观测距离为d时,采样得到RSSI
function value = GetRssiValue(d)
%当距离为d时,仿真系统给出一个与实际距离相对应的仿真值
A=-42; %注意A和n在不同硬件系统中取值是不一样的
n=2;
value=A-10*n*log10(d);
%实际测量值是受到噪声的污染的,这里假定噪声方差Q非常大
%因为RSSI的干扰非常大
Q=5;
value=value+sqrt(Q) * randn; %实际观测量是带有噪声的,仿真就是在模仿真实情况
end

%子函数,根据RSSI计算得到距离d
function value = GetDistByRssi( rssi)
A=-42; %注意A和n在不同硬件系统中取值是不一样的
n=2;
value= 10^(( A-rssi)/10/n);
end

运行结果：

4.基于TOA/TDOA的目标定位算法

1）基于TOA测距

基于TOA测距，是通过测量信号传输时间来估算两节点之间的距离精度。

该方法的缺点是无线信号的传输速度快，时间测量上很小的误差可导致很大的距离误差值，另外要求传感器节点具有较强的计算能力。由于无线传感器网络节点的硬件尺寸、价格和功耗限制，实际应用TOA技术定位的方案较少。

2）基于TDOA测距

在基于到达时间差(TDOA)的定位原理中，发射节点同时发射无线电磁波信号和超声波信号两种不同传播速度的信号,接收节点根据两种信号到达的时间差，利用这两种信号的传输速度，计算出收发节点之间的距离，再根据现有的基本定位算法算出未知节点的位置。

如上图所示，发射节点同时发射无线电磁波信号和超声波信号，已知无线电磁波和超声波的传播速度为$c_1$与$c_2$，接收节点记录无线电磁波信号的到达时间$T_1$，超声波的到达时间$T_2$，计算出两点之间的距离为:

$c_1$>>$c_2$，上式可简化为 $d=(T_2-T_1)\ast c_2$。

超声波定位比较实用与小范围内精度要求较高的室内定位环境。

例：在一个100mx100m的室内监控环境中，随机部署了能与目标进行无线和超声通信的3个观测基站，基站位置随机部署，超声在室内传播速度假定为340m/s，观测站分别记录每次接收到目标发送的无线信号和超声信号的时间，根据两者之间的时间差，计算出目标与观测站之间的距离，最终利用3个观测站的距离数据，用最小二乘法估计出目标的位置，MATLAB仿真如下：

function TDOAEstimate
%第一步:定位初始化
Length=100;                          %场地空间，单位：米
Width=100;                           %场地空间，单位：米
Node_number=3;                       %观测站的个数,最少必须有3个
for i=1:Node_number                  %观测站的位置初始化,这里位置是随机给定的
    Node(i).x= Width * rand;
    Node(i).y= Length * rand;
    Node(i).D= Node(i).x^2+Node(i).y^2; % 固定参数便于位置估计
end
%目标的真实位置,这里也随机给定
Target.x= Width * rand;
Target.y= Length * rand;
BroadcastPacket=0;                    %数据包，用于目标节点周期性广播数据包和超声
ultrasonicV=340;                      %超声在空气中传输速度为340m/s
%第二步:各观测站对目标探测，记录时间。收到无线信号,启动计时器
%目标节点发送数据包
sendData(BroadcastPacket);
%延时一段时间后发送超声
delaytime=10; %延时10ms
delay(delaytime);
%目标节点发送超声脉冲
sendUltraPlus();
%第三步:各个观测站接收无线数据包和超声
%假设所有观测站之前都成功接收到BroadeastPacket数据包，之后启动定时器计时
uT=[];                              %各观测站采集时间差
for i=1:Node_number
    recvUltraPlus();               %第i个观测站成功接收到超声脉冲
    %每个节点成功接收到超声脉冲后，记录时间
    [d]= DIST( Node(i),Target);     %观测站离目标的真实距离
    %第i个观测站记录的时间
    uT(i)= GetTimeLength(d);
end
%第四步:根据记录的时间,计算观测站与目标之间的距离
Zd=[];
for i=1:Node_number
    Zd(i)= uT(i) * ultrasonicV;     %距离=时间*速度
end
%第五步:根据距离,用最小二乘法计算目标节点的估计位置
H=[];b=[];
for i=2:Node_number
    H=[ H;2* ( Node(i).x-Node(1).x) ,2* (Node(i).y-Node(1).y)]; %参照三边测距法公式
    b=[b;Zd(1)^2-Zd(i)^2+Node(i).D-Node(1).D];                   %参照三边测距法公式
end
Estimate=inv(H'*H)*H'*b;             %目标的估计位置
Est_Target.x= Estimate(1);Est_Target.y= Estimate(2);

%画图
figure
hold on;box on;axis([0 120 0 120]);
%输出图形的框架
for i= 1: Node_number
    h1= plot(Node(i).x, Node(i).y,'ko','MarkerFace','g','MarkerSize',10);
    text(Node(i).x+2,Node(i).y,[ 'Node ', num2str(i)]);
end
h2= plot(Target.x,Target.y,'k^','MarkerFace','b','MarkerSize', 10) ;
h3= plot(Est_Target.x,Est_Target.y,'ks','MarkerFace','r','MarkerSize',10);
line([Target.x,Est_Target.x],[Target.y,Est_Target.y] ,'Color' ,'k');
legend([h1,h2,h3 ],'Observation Station','Target Postion','Estimate Postion');
[Error_Dist]=DIST(Est_Target,Target);
xlabel(['error=',num2str( Error_Dist),'m']);

%子函数,计算两点间的距离
function [dist]= DIST(A,B)
dist=sqrt((A.x-B.x)^2+(A.y-B.y)^2);
end

%子函数,观测站距离目标点为d时，超声脉冲传输的时间
function time = GetTimeLength(d)
%当距离为d时,仿真系统给出一个与实际距离相对应的仿真值
ultrasonicV=340; %超声在空气中传输速度为340m/s
time = d/ultrasonicV;
%实际测量值是受到噪声污染的，需要考虑实际的温度、湿度、气压等因素
Q=5e-6;
time = time+sqrt(Q) * randn; %实际观测量是带有噪声的,仿真就是在模仿真实情况
end

%子函数，仿真通信机制，目标节点发送数据包和超声
function sendData( BroadeastData)
%由于MATLAB不便于仿真通信机制，这里假定发送的数据都被成功接收
disp( 'The TargetNode send wireless data success !\n')
end

%子函数,仿真目标节点发送超声
function sendUltraPlus( )
disp( 'The TargetNode send ultrasonie plus success !\n')
end

%子函数,观测站成功接收超声
function recvUltraPlus()
disp( 'The ObserNode receive ultrasonie plus success !\n')
end

运行结果：

通过时间校准可以使TOA定位误差更小