文章目录
- 三边测距定位算法简介
- 多边测量法公式推导
- 三边测距定位算法MATLAB程序
三边测距定位算法简介
三边测量法是多边测量法的低级应用，即已知三个点的横纵坐标和与未知点的距离 d d d，如下图所示：
如图所示已知 ( x 1 , y 1 ) (x_1,y_1) (x1,y1)， ( x 2 , y 2 ) (x_2,y_2) (x2,y2)， ( x 3 , y 3 ) (x_3,y_3) (x3,y3)和 d 1 d_1 d1， d 2 d_2 d2， d 3 d_3 d3就可以求出 ( x i , y i ) (x_i,y_i) (xi,yi)。
多边测量法公式推导
1.建立已知节点与未知节点的距离方程组
{ ( x 1 − x ) 2 + ( y 1 − y ) 2 = d 1 2 ⋮ ( x n − x ) 2 + ( y n − y ) 2 = d n 2 \begin{cases} (x_1-x)^2+(y_1-y)^2=d_1^2 \\ \qquad \quad \qquad \vdots \\ (x_n-x)^2+(y_n-y)^2=d_n^2\\ \end{cases} ⎩⎪⎪⎨⎪⎪⎧(x1−x)2+(y1−y)2=d12⋮(xn−x)2+(yn−y)2=dn2
2.上式方程组为非线性方程组，用方程组前 n − 1 n-1 n−1个方程减去第 n n n个方程后，得到线性化的方程：
A X = B AX=B AX=B
其中：
A = [ 2 ( x 1 − x n ) 2 ( y 1 − y n ) ⋮ ⋮ 2 ( x n − 1 − x n ) 2 ( y n − 1 − y n ) ] A=\begin{bmatrix} 2(x_1-x_n) & 2(y_1-y_n) \\ \vdots&\vdots\\ 2(x_{n-1}-x_n) & 2(y_{n-1}-y_n)\\ \end{bmatrix} A=⎣⎢⎡2(x1−xn)⋮2(xn−1−xn)2(y1−yn)⋮2(yn−1−yn)⎦⎥⎤
B = [ x 1 2 − x n 2 + y 1 2 − y n 2 + d n 2 − d 1 2 ⋮ x n − 1 2 − x n 2 + y n − 1 2 − y n 2 + d n 2 − d n − 1 2 ] B=\begin{bmatrix} x_1^2-x_n^2+ y_1^2-y_n^2+d_n^2-d_1^2 \\ \vdots\\ x_{n-1}^2-x_n^2+ y_{n-1}^2-y_n^2+d_n^2-d_{n-1}^2\\ \end{bmatrix} B=⎣⎢⎡x12−xn2+y12−yn2+dn2−d12⋮xn−12−xn2+yn−12−yn2+dn2−dn−12⎦⎥⎤
3.用最小二乘法求解上边方程得：
X = ( A T A ) − 1 A T b X=(A^TA)^{-1}A^Tb X=(ATA)−1ATb
三边测距定位算法MATLAB程序
%% x输入为横坐标矩阵 yx输入为纵坐标矩阵 distance为距离矩阵，距离矩阵里可能包含0值，即未给出距离的值
%% xx，yy 分别为求得的未知点横纵坐标
function [xx,yy]=Trilat(x,y,distance)
[sz1,sz2]=size(distance);%获取距离矩阵大小
%%处理数据
for i=1:sz1n=1;for m=1:sz2if distance(i,m)~=0%判断距离矩阵是否为0x1(i,n)=x(m);%不等于0时取对应的x,y值y1(i,n)=y(m);d(i,n)=distance(i,m);n=n+1; endend
end
[sz3,sz4]=size(x1);%获取处理后距离矩阵大小
%%根据三边测量法求解位置节点
for i=1:sz1
n=1;
for o=1:sz4-2for p=o+1:sz4-1for q=p+1:sz4A=[2*(x1(i,o)-x1(i,q)),2*(y1(i,o)-y1(i,q));2*(x1(i,p)-x1(i,q)),2*(y1(i,p)-y1(i,q))];B=[x1(i,o)^2-x1(i,q)^2+y1(i,o)^2-y1(i,q)^2+d(i,q)^2-d(i,o)^2;x1(i,p)^2-x1(i,q)^2+y1(i,p)^2-y1(i,q)^2+d(i,q)^2-d(i,p)^2];X=(A'*A)^(-1)*A'*B; xx(i,n)=X(1);yy(i,n)=X(2);n=n+1;endend
end
endend
多边测量法代码可私信我。创作不易，请大家多多支持！
