基于测距的定位算法

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

1.最小二乘原理

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

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

就是寻找与给定点的距离平方和为最小的曲线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_numberH=[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_numberh1= 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,再算出zNode(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_numberH=[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_numberh1=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 d_0 d0为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_numberfor t=1:10                          %10次采样[d]=DIST(Node(i),Target);        %观测站与目标的真实距离%距离为d时,得到的RSSI 测量值Z(i,t)= GetRssiValue(d)end
end
%求10次观测的平均值
ZZ=[];
for i=1:Node_numberZZ(i)=sum(Z(i,:))/10;
end
%第三步:根据采样的RSSI值，求观测距离
Zd=[];                                  %根据RSSI计算得到的距离
for i=1:Node_numberZd(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_numberh1= 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_1 c1与 c 2 c_2 c2，接收节点记录无线电磁波信号的到达时间 T 1 T_1 T1，超声波的到达时间 T 2 T_2 T2，计算出两点之间的距离为:

c 1 c_1 c1>> c 2 c_2 c2，上式可简化为 d = ( T 2 − T 1 ) ∗ c 2 d=(T_2-T_1)*c_2 d=(T2−T1)∗c2。

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

例：在一个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_numberrecvUltraPlus();               %第i个观测站成功接收到超声脉冲%每个节点成功接收到超声脉冲后，记录时间[d]= DIST( Node(i),Target);     %观测站离目标的真实距离%第i个观测站记录的时间uT(i)= GetTimeLength(d);
end
%第四步:根据记录的时间,计算观测站与目标之间的距离
Zd=[];
for i=1:Node_numberZd(i)= uT(i) * ultrasonicV;     %距离=时间*速度
end
%第五步:根据距离,用最小二乘法计算目标节点的估计位置
H=[];b=[];
for i=2:Node_numberH=[ 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_numberh1= 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定位误差更小

目标定位算法（二）之基于测距的定位算法相关推荐

多传感器融合定位（二）——基于地图的定位
目录一.回环检测 1.1 基于Scan Context 1.2 基于直方图一.回环检测回环检测只能消除一部分误差,不能消除全部误差.运用视觉用特征点描述子比较简单. 1.1 基于Scan Con ...
一文详解基于测距的空间定位算法
一文详解基于测距的空间定位算法文章目录一文详解基于测距的空间定位算法 0 定位算法分类 0.1 基于测距与非基于测距的定位算法 0.2 集中式与分布式定位算法 0.3 绝对与相对定位算法 0.4 ...
95% 的算法都是基于这 6 种算法思想
95% 的算法都是基于这 6 种算法思想算法思想是解决问题的核心,万丈高楼起于平地,在算法中也是如此,95% 的算法都是基于这 6 种算法思想,结下了介绍一下这 6 种算法思想,帮助你理解及解决各种 ...
基于测距的定位（RSSI定位算法原理）
基于RSSI测距的定位算法包括三个阶段 (1)测距阶段:锚节点和未知节点发送RSSI信号,利用信号衰减模型和RSSI值估计未知节点和锚节点之间的距离 (2)定位阶段:利用第一步得到的距离信息,通过三边 ...
机器学习-无监督学习-聚类：聚类方法（二）--- 基于密度的聚类算法【DBSCAN文本聚类算法，密度最大值文本聚类算法】
密度聚类方法的指导思想是,只要样本点的密度大于某阀值,则将该样本添加到最近的簇中. 基于密度的聚类算法假设聚类结构能够通过样本分布的紧密程度确定,以数据集在空间分布上的稠密程度为依据进行聚类,即只要一 ...
kcf算法流程_基于运动检测与KCF算法的目标跟踪方法
基于运动检测与 KCF 算法的目标跟踪方法易诗,唐文 [摘要] 摘要:近年来机器视觉技术广泛采用于各个领域,本文设计实现了一种基于运动检测与 KCF 算法的目标跟踪方法,该方法相对于传统单一 ...
95% 的算法都是基于这 6 种算法思想，大厂Java面试必考点
// 当前结点的 id 符合查找条件,返回当前结点 if(node.id === id) return node // 前结点的 id 不符合查找条件,继续查找它的每一个子结点 for(var i = ...
【玩转嵌入式屏幕显示】（三）TFT-LCD屏幕打点 + 画线 + 画矩形 + 画圆Bresenham算法实现（基于打点函数，算法可移植到任何屏幕的驱动程序之上）
0. 引言 TFT-LCD屏幕的画直线.画斜线.画矩形.画圆等算法都是基于打点函数的,所以此程序可以移植到任何屏幕的基本驱动程序之上. 1. 打点函数 -- 底层函数(移植需修改) 打点函数其实就是屏 ...
android定位打卡实现,基于Android移动定位课堂考勤系统设计与实现.doc
基于Android移动定位课堂考勤系统设计与实现基于Android移动定位课堂考勤系统设计与实现摘要: 为促进高职院校课堂考勤管理工作的规范化,提高课堂考勤管理水平,使用PHP语言开发了Web服 ...

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

文章目录