平面三点定位原理与实现

最新推荐文章于 2023-11-14 09:26:14 发布

Focus Y

最新推荐文章于 2023-11-14 09:26:14 发布

阅读量6k

点赞数 6

分类专栏：定位算法文章标签： matlab 线性代数定位

本文链接：https://blog.csdn.net/weixin_43479834/article/details/120572391

版权

算法同时被 2 个专栏收录

6 篇文章 2 订阅

订阅专栏

定位

4 篇文章 6 订阅

订阅专栏

平面三点定位原理与实现

文章目录

平面三点定位原理与实现
前言
一、平面三点定位原理
- 1.原理推导
二、Matlab代码实现
总结

前言

最近在看卫星定位相关知识，其中伪距定位通过测量星地距离，并结合多个已知位置的卫星建立方程组，可以解算出地面待定位点的坐标。三点定位是此原理在平面中的体现。所涉及代码放在了文章最后。

一、平面三点定位原理

1.原理推导

已知平面内三点和它们到任意一点的距离，则可以通过距离关系得到三个二次等式，在通过两组差分消去高次项后得到方程组，通过矩阵运算解出任意一点的坐标。
假设平面中已知三点A、B、C，坐标分别为 $x_{1},y_{1})$ 、 $x_{2},y_{2})$ 、 $x_{3},y_{3})$ ，一待定位点P坐标为 $(x, y)$ ，P到三点的距离分别为 $\rho_{1},\rho_{2},\rho_{3}$ ，根据距离公式，可以得到一个如下的方程组：
$\begin{cases} (x_{1}-x)^2+(y_{1}-y)^2=\rho_{1}^2 \\ (x_{2}-x)^2+(y_{2}-y)^2=\rho_{2}^2 \\ (x_{3}-x)^2+(y_{3}-y)^2=\rho_{3}^2 \end{cases}$
进行整理得到：
$\begin{cases} x^2-2x_{1}x+y^2-2y_{1}y=\rho_{1}^2-x_{1}^{2}- y_{1}^{2}\\ x^2-2x_{2}x+y^2-2y_{2}y=\rho_{2}^2-x_{2}^{2}- y_{2}^{2} \\ x^2-2x_{3}x+y^2-2y_{3}y=\rho_{3}^2-x_{3}^{2}- y_{3}^{2} \end{cases}$
可以看出上式中含有高次项，不利于方程的求解，而对方程进行两次差分可以消除掉高次项，得到以下的一次方程组：
$\begin{cases} 2x(x_{2}-x_{1})+2y(y_{2}-y_{1})=\rho_{1}^2-\rho_{2}^2+x_{2}^{2}-x_{1}^{2}+y_{2}^{2}-y_{1}^{2}\\ 2x(x_{3}-x_{2})+2y(y_{3}-y_{2})=\rho_{2}^2-\rho_{3}^2+x_{3}^{2}-x_{2}^{2}+y_{3}^{2}-y_{2}^{2}\end{cases}$
将方程式用矩阵形式表示，
$\mathrm{A}c=b$
其中
$\mathrm{A}=\begin{bmatrix} 2(x_{2}-x_{1})&2(y_{2}-y_{1}))\\ 2(x_{3}-x_{2})&2(y_{3}-y_{2})) \end{bmatrix}$
$c=\begin{pmatrix} x\\ y \end{pmatrix}$
$b=\begin{pmatrix} \rho_{1}^2-\rho_{2}^2+x_{2}^{2}-x_{1}^{2}+y_{2}^{2}-y_{1}^{2}\\ \rho_{2}^2-\rho_{3}^2+x_{3}^{2}-x_{2}^{2}+y_{3}^{2}-y_{2}^{2} \end{pmatrix}$
由矩阵的运算可见，要求得待定位点坐标 $(x, y)$ ，也就是求解向量 $c$ ，如果矩阵 $\mathrm{A}$ 的逆矩阵存在，由下式即可求解出向量 $c$ ：
$c=\mathrm{A}^{-1}b$
到此为止，待求解点的坐标已经可以解算出，这要求我们在选取已知点时保证矩阵 $\mathrm{A}$ 的可逆性。

二、Matlab代码实现

1.画圆程序

为了体现原理，需要在已知圆心处按一定半径画圆，因此首先实现画圆程序

function h=circle(r,x0,y0,C,Nb) 

% CIRCLE adds circles to the current plot 

% 

% CIRCLE(r,x0,y0) adds a circle of radius r centered at point x0,y0. 

% If r is a vector of length L and x0,y0 scalars, L circles with radiu r 

% are added at point x0,y0. 

% If r is a scalar and x0,y0 vectors of length M, M circles are with the same 

% radius r are added at the points x0,y0. 

% If r, x0,y0 are vector of the same length L=M, M circles are added. (At each 

% point one circle). 

% if r is a vector of length L and x0,y0 vectors of length M~=L, L*M circles are 

% added, at each point x0,y0, L circles of radius r. 

% 

% CIRCLE(r,x0,y0,C) 

% adds circles of color C. C may be a string ('r','b',...) or the RGB value. 

% If no color is specified, it makes automatic use of the colors specified by 

% the axes ColorOrder property. For several circles C may be a vector. 

% 

% CIRCLE(r,x0,y0,C,Nb), Nb specifies the number of points used to draw the 

% circle. The default value is 300. Nb may be used for each circle individually. 

% 

% h=CIRCLE(...) returns the handles to the circles. 

% 

% Try out the following (nice) examples: 

% 

%% Example 1 

% 

% clf; 

% x=zeros(1,200); 

% y=cos(linspace(0,1,200)*4*pi); 

% rad=linspace(1,0,200); 

% cmap=hot(50); 

% circle(rad,x,y,[flipud(cmap);cmap]); 

% 

%% Example 2 

% 

% clf; 

% the=linspace(0,pi,200); 

% r=cos(5*the); 

% circle(0.1,r.*sin(the),r.*cos(the),hsv(40)); 

% 

% 

%% Example 3 

% 

% clf 

% [x,y]=meshdom(1:10,1:10); 

% circle([0.5,0.3,0.1],x,y,['r';'y']); 

% 

%% Example 4 

% 

% clf 

% circle(1:10,0,0,[],3:12); 

% 

%% Example 5 

% 

% clf; 

% circle((1:10),[0,0,20,20],[0,20,20,0]); 

% rewritten by Din-sue Fon. Dept. of Bio-Industrial Mechatronics Engineering, 

% National Taiwan University March 10,2001 

% dsfong@ccms.ntu.edu.tw 

% written by Peter Blattner, Institute of Microtechnology, University of 

% Neuchatel, Switzerland, blattner@imt.unine.ch 

  

% Check the number of input arguments 

switch nargin 

case 0 

   r=[];x0=[];y0=[];C=[];Nb=[]; 

case 1 

   x0=[];y0=[];C=[];Nb=[]; 

case 2 

   y0=zeros(1,length(x0));C=[];Nb=[]; 

case 3 

   C=[];Nb=[]; 

case 4 

   Nb=[]; 

end 

  

if length(x0)~=length(y0), %有一个坐标只有一个时，扩展到与另一个量相同处

   if length(y0)==1, 

      y0=ones(1,length(x0))*y0; 

   elseif length(x0)==1, 

      x0=ones(1,length(y0))*x0; 

   else 

      error('The lengths of x0 and y0 must be identical'); 

   end; 

end; 

  

% set up the default values 

  

if isempty(r),r=1;end; 

if isempty(x0),x0=0;end; 

if isempty(y0),y0=0;end; 

if isempty(Nb),Nb=300;end; 

if isempty(C),C=get(gca,'colororder');end; 

if isstr(C),C=C(:);end; 

  

% work on the variable sizes 

  

x0=x0(:); 

y0=y0(:); 

r=r(:); 

Nb=Nb(:); 

  

  

% how many rings are plottet 

  

if length(r)~=length(x0) %半径数应当等于原点数

  maxk=length(r)*length(x0); 

else 

  maxk=length(r); 

end; 

route=0; 

if length(x0)==1, route=1; end 

if length(r)==1, route=2; end 

if length(x0)==length(r), route=3; end 

  

% drawing loop 

  

for k=1:maxk 

  switch route 

  case 1 

    xpos=x0; 

    ypos=y0; 

    rad=r(k); 

  case 2 

    xpos=x0(k); 

    ypos=y0(k); 

    rad=r; 

  case 3 

    xpos=x0(k); 

    ypos=y0(k); 

    rad=r(k); 

  otherwise 

    rad=r(fix(y(k-1)/size(x0,1))+1); 

    xpos=x0(rem(k-1,size(x0,1))+1); 

    ypos=y0(rem(k-1,size(y0,1))+1); 

  end; % for switch 

  

  theta=linspace(0,2*pi,Nb(rem(k-1,size(Nb,1))+1,:)+1); 

  h(k)=line(rad*cos(theta)+xpos,rad*sin(theta)+ypos); 

  set(h(k),'color',C(rem(k-1,size(C,1))+1,:)); 

end;

2.主程序

2.1 ginput(n)函数实现待定位点获取

首先需要将三个已知坐标点的坐标保存并在坐标系中绘制出，之后获取几个待定位点的坐标，用于计算距离和最终与解算点作对比，通过 $[x, y] = g i n p u t (n)$ ，用鼠标在坐标系内任意确定 $n$ 个点并保存它们的坐标。

clear all
disp('Trilateral Measurement')
x=[5 35 53];
y=[41 10 30];	%三个已知点坐标
plot(x,y,'o');
axis([-5 65 -5 65]);
hold on
[x0,y0]=ginput(3);%用鼠标获得三点坐标
plot(x0,y0,'g:o')
axis([-5 65 -5 65])

2.2 距离计算

通过距离公式可以获得待定位点到三个已知位置点的距离，这在伪距定位中对应卫星和地面待测点之间的伪距，通过对距离加入误差，进而观察最终的定位精度可以模拟伪距定位过程中各因素对最终定位结果产生的影响。

distance= zeros(3,3);
for i=1:3
    for j=1:3
    distance(i,j) = sqrt((x(i)-x0(j))^2 + (y(i)-y0(j))^2);
    end
end
% error=0; 
random_error=rand(3,100)-0.3;
bias_error = 0.05*distance;
distance=distance-distance.*(random_error/7) + bias_error; %加入干扰

2.3 坐标解算

通过一中原理推导，只需要简单的矩阵运算就可以解算出待定位点坐标

aa = zeros(2,2,3);
bb = zeros(2,1,3);
cc = zeros(1,2,3);
for h=1:3
    aa(:,:,h)=inv(2*([x(1)-x(3) y(1)-y(3);x(2)-x(3) y(2)-y(3)]));
    bb(:,:,h)=[x(1)^2-x(3)^2+y(1)^2-y(3)^2+distance(3,h)^2-distance(1,h)^2;x(2)^2-x(3)^2+y(2)^2-y(3)^2+distance(3,h)^2-distance(2,h)^2];
    cc(:,:,h)=(aa(:,:,h)*bb(:,:,h))'; %计算并存放估算位置
    plot(cc(:,1,h),cc(:,2,h),'+')%画交点
    axis([-50 100 -50 100])
    circle(distance(1,h),x(1),y(1),'r');
    circle(distance(2,h),x(2),y(2),'b');
    circle(distance(3,h),x(3),y(3),'cyan');
    plot([x(1),cc(1,1,h)],[y(1),cc(1,2,h)],'k');
    plot([x(2),cc(1,1,h)],[y(2),cc(1,2,h)],'k');
    plot([x(3),cc(1,1,h)],[y(3),cc(1,2,h)],'k');
   axis equal;
end

通过主程序代码可以实现平面中三点定位原理的示意。
在这里插入图片描述
在平面中用鼠标确定三个点，以上图片为两个输出例子，图中红色和蓝色，紫色分别为以三个已知点为圆心所生成的圆，绿色点为圆之间的交点，+号的点为开始用鼠标在坐标系中确定的点，可以发现两者是完全重合的，证明三点定位算法原理的正确性。

3.误差分析

主程序中的例子是为了说明三点定位的原理，实际意义有限，毕竟某种程度上来说是已知答案推过程。为了实际性，对平面三点定位的误差进行分析，在主程序的基础上加入干扰，多次定位同一点，观察误差情况。
误差评定指标：
1.平均偏差：定位点与待定位点距离的平均值，反映定位精确度
2.均方根误差：反映定位精确度
在这里插入图片描述

3.标准差:反映定位精密度
在这里插入图片描述

4.CEP圆概率
指以平均坐标为圆心，使50%点落在圆内的半径，CEP越小代表定位精度越高。
$CEP=0.589(\delta_{x}+\delta_{y})$
$\delta_{x},\delta_{y}$ 指点集所有点的 $x, y$ 坐标标准差。

clear all
disp('Trilateral Measurement')
x=[5 35 53];
y=[41 10 30];
figure('Name','定位图','NumberTitle','off');
plot(x,y,'o');
axis([-5 65 -5 65]);
hold on
x0 = 20*ones(100,1);
y0 = 20*ones(100,1);
plot(x0,y0,'g:o')
xlabel('x坐标/m');
ylabel('y坐标/m');
axis([-5 65 -5 65])

distance= zeros(3,3);
for i=1:3
    for j=1:100
    distance(i,j) = sqrt((x(i)-x0(j))^2 + (y(i)-y0(j))^2);
    end
end
% error=0; %rand(1,3)-0.3
random_error=rand(3,100)-0.3;
bias_error = 0.05*distance;
distance=distance-distance.*(random_error/7) + bias_error;      %误差控制？
% distance=distance + bias_error;
aa = zeros(2,2,100);
bb = zeros(2,1,100);
cc = zeros(1,2,100);
for h=1:100
    aa(:,:,h)=inv(2*([x(1)-x(3) y(1)-y(3);x(2)-x(3) y(2)-y(3)]));
    bb(:,:,h)=[x(1)^2-x(3)^2+y(1)^2-y(3)^2+distance(3,h)^2-distance(1,h)^2;x(2)^2-x(3)^2+y(2)^2-y(3)^2+distance(3,h)^2-distance(2,h)^2];
    cc(:,:,h)=permute(aa(:,:,h)*bb(:,:,h),[2,1,3]); %计算并存放估算位置
    plot(cc(:,1,h),cc(:,2,h),'+')%画交点
    axis([-50 100 -50 100])
    %e(j)=sqrt((cc(j,1)-x0)^2+(cc(j,2)-y0)^2) %求出误差
%    circle(distance(1,h),x(1),y(1),'r');
%    circle(distance(2,h),x(2),y(2),'b');
%    circle(distance(3,h),x(3),y(3),'cyan');
%    plot([x(1),cc(1,1,h)],[y(1),cc(1,2,h)],'k');
%    plot([x(2),cc(1,1,h)],[y(2),cc(1,2,h)],'k');
%    plot([x(3),cc(1,1,h)],[y(3),cc(1,2,h)],'k');
   axis equal;
end
%计算误差
%1.平均偏差
loop_index = 1:100;
distance_error = sqrt((cc(:,1,:)-20).^2+(cc(:,2,:)-20).^2);
mean_error = mean(distance_error);

%2.均方根误差
RMSE = sqrt(mean((cc(:,1,:)-20).^2+(cc(:,2,:)-20).^2));
%3.标准差
mean_position = mean(cc,3);
std_error = sqrt(mean((cc(:,1,:)-mean_position(1)).^2+(cc(:,2,:)-mean_position(2)).^2));
%4.CEP圆概率
std_x = sqrt(mean((cc(:,1,:)-mean_position(1)).^2));
std_y = sqrt(mean((cc(:,2,:)-mean_position(2)).^2));
CEP = 0.589*(std_x+std_y);
figure(1);
circle(CEP,mean_position(1),mean_position(2));

%画误差图
figure('Name','定位误差图','NumberTitle','off');
plot([1:100],squeeze(distance_error),[1:100],mean_error*ones(1,100),'r',[1:100],RMSE*ones(1,100),[1:100],std_error*ones(1,100)),legend('偏差曲线','平均偏差曲线','均方根误差','标准差');
xlabel('定位点数');
ylabel('平均误差/m');

figure(1);
s='Trilateral Measurement';           %书写图名
title(s)
% figure
% rate=1:1:7;
% plot(rate,e,'g:*')
% xlabel('x/Error Rate') %是xlabel,x-l-a-b-e-l
% ylabel('y/Error Rate')
% axis([0 7 -20 50])
% hold on

以下是程序运行的结果：
在这里插入图片描述

4.动态轨迹跟踪

前文实现了单点和多点定位，而一点的运动轨迹可以分解为多个点的集合，因此既然能实现单点定位，也能实现轨迹跟踪。在单点和多点定位的基础上，分别加入误差和不加入误差，对平面内一运动点的轨迹进行跟踪。

clear all
disp('Trilateral Measurement')
sample_number = 100
x=[5 35 53];
y=[41 10 30];
plot(x,y,'o');
s='Trilateral Measurement'           %书写图名
title(s)
xlabel('x坐标/m');
ylabel('y坐标/m');
axis([-10 65 -10 65]);
hold on
theta = linspace(0,50,sample_number);
x0 = theta;
y0 = 10*sin(theta/10) + 10;
%plot(x0,y0,'g:o')
mycomet2(x0,y0)
axis([-5 65 -5 65])

distance= zeros(3,sample_number);

for i=1:3
    for j=1:sample_number
    distance(i,j) = sqrt((x(i)-x0(j))^2 + (y(i)-y0(j))^2);
    end
end

random_error=rand(3,sample_number)-0.3;
bias_error = 0.05*distance;
distance=distance-distance.*(random_error/7) + bias_error;      %误差控制？


aa = zeros(2,2,sample_number);
bb = zeros(2,1,sample_number);
cc = zeros(1,2,sample_number);
for h=1:sample_number
    aa(:,:,h)=inv(2*([x(1)-x(3) y(1)-y(3);x(2)-x(3) y(2)-y(3)]));
    bb(:,:,h)=[x(1)^2-x(3)^2+y(1)^2-y(3)^2+distance(3,h)^2-distance(1,h)^2;x(2)^2-x(3)^2+y(2)^2-y(3)^2+distance(3,h)^2-distance(2,h)^2];
    cc(:,:,h)=(aa(:,:,h)*bb(:,:,h))'; %计算并存放估算位置
    mycomet(cc(:,1,h),cc(:,2,h))
%     plot(cc(:,1,h),cc(:,2,h),'+')%画交点
    axis([-10 65 -10 65]);
    %e(j)=sqrt((cc(j,1)-x0)^2+(cc(j,2)-y0)^2) %求出误差
%    circle(distance(1,h),x(1),y(1),'r');
%    circle(distance(2,h),x(2),y(2),'b');
%    circle(distance(3,h),x(3),y(3),'cyan');
%    plot([x(1),cc(1,1,h)],[y(1),cc(1,2,h)],'k');
%    plot([x(2),cc(1,1,h)],[y(2),cc(1,2,h)],'k');
%    plot([x(3),cc(1,1,h)],[y(3),cc(1,2,h)],'k');
end


hold on
% figure
% rate=1:1:7;
% plot(rate,e,'g:*')
% xlabel('x/Error Rate') %是xlabel,x-l-a-b-e-l
% ylabel('y/Error Rate')
% axis([0 7 -20 50])
% hold on