平面三点定位原理与实现

文章目录

平面三点定位原理与实现
前言
一、平面三点定位原理
- 1.原理推导
二、Matlab代码实现
- 1.画圆程序
- 2.主程序
- - 2.1 ginput(n)函数实现待定位点获取
  - 2.2 距离计算
  - 2.3 坐标解算
- 3.误差分析
- 4.动态轨迹跟踪
总结

前言

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

一、平面三点定位原理

1.原理推导

已知平面内三点和它们到任意一点的距离，则可以通过距离关系得到三个二次等式，在通过两组差分消去高次项后得到方程组，通过矩阵运算解出任意一点的坐标。
假设平面中已知三点A、B、C，坐标分别为(x1,y1)(x_{1},y_{1})(x1,y1)、(x2,y2)(x_{2},y_{2})(x2,y2)、(x3,y3)(x_{3},y_{3})(x3,y3)，一待定位点P坐标为(x,y)(x,y)(x,y)，P到三点的距离分别为ρ1,ρ2,ρ3\rho_{1},\rho_{2},\rho_{3}ρ1,ρ2,ρ3，根据距离公式，可以得到一个如下的方程组：
{(x1−x)2+(y1−y)2=ρ12(x2−x)2+(y2−y)2=ρ22(x3−x)2+(y3−y)2=ρ32\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} ⎩⎪⎨⎪⎧(x1−x)2+(y1−y)2=ρ12(x2−x)2+(y2−y)2=ρ22(x3−x)2+(y3−y)2=ρ32
进行整理得到：
{x2−2x1x+y2−2y1y=ρ12−x12−y12x2−2x2x+y2−2y2y=ρ22−x22−y22x2−2x3x+y2−2y3y=ρ32−x32−y32\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} ⎩⎪⎨⎪⎧x2−2x1x+y2−2y1y=ρ12−x12−y12x2−2x2x+y2−2y2y=ρ22−x22−y22x2−2x3x+y2−2y3y=ρ32−x32−y32
可以看出上式中含有高次项，不利于方程的求解，而对方程进行两次差分可以消除掉高次项，得到以下的一次方程组：
{2x(x2−x1)+2y(y2−y1)=ρ12−ρ22+x22−x12+y22−y122x(x3−x2)+2y(y3−y2)=ρ22−ρ32+x32−x22+y32−y22\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} {2x(x2−x1)+2y(y2−y1)=ρ12−ρ22+x22−x12+y22−y122x(x3−x2)+2y(y3−y2)=ρ22−ρ32+x32−x22+y32−y22
将方程式用矩阵形式表示，
Ac=b\mathrm{A}c=b Ac=b
其中
A=[2(x2−x1)2(y2−y1))2(x3−x2)2(y3−y2))]\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} A=[2(x2−x1)2(x3−x2)2(y2−y1))2(y3−y2))]
c=(xy)c=\begin{pmatrix} x\\ y \end{pmatrix} c=(xy)
b=(ρ12−ρ22+x22−x12+y22−y12ρ22−ρ32+x32−x22+y32−y22)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} b=(ρ12−ρ22+x22−x12+y22−y12ρ22−ρ32+x32−x22+y32−y22)
由矩阵的运算可见，要求得待定位点坐标(x,y)(x,y)(x,y)，也就是求解向量ccc，如果矩阵A\mathrm{A}A的逆矩阵存在，由下式即可求解出向量ccc：
c=A−1bc=\mathrm{A}^{-1}b c=A−1b
到此为止，待求解点的坐标已经可以解算出，这要求我们在选取已知点时保证矩阵A\mathrm{A}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]=ginput(n)[x,y]=ginput(n)[x,y]=ginput(n)，用鼠标在坐标系内任意确定nnn个点并保存它们的坐标。

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:3for j=1:3distance(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:3aa(:,:,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(δx+δy)CEP=0.589(\delta_{x}+\delta_{y})CEP=0.589(δx+δy)
δx,δy\delta_{x},\delta_{y}δx,δy指点集所有点的x,yx,yx,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:3for j=1:100distance(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:100aa(:,:,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:3for j=1:sample_numberdistance(i,j) = sqrt((x(i)-x0(j))^2 + (y(i)-y0(j))^2);end
endrandom_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_numberaa(:,:,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');
endhold 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

以下是一个例子：

文章涉及的代码都放在了以下链接中：
链接：https://pan.baidu.com/s/1Ct5eFdxDJ8-kjOD578FQRw
提取码：ek8j

总结

以上是三点定位的原理和一些拓展，既然平面内能够实现三点定位，那么空间中多点也能够实现定位，将在以后的博客中记录。