一、原理方法
四点定位(Four-Anchor Positioning)是一种基于距离测量的定位方法,通常采用TOA方法来计算目标物体到每个基站的距离。通过测量目标物体到至少四个基站的距离,并利用三角定位等算法计算出目标物体的位置。因此,四点定位属于TOA定位方法的一种。
在UWB精确定位中,四点定位(Four-Anchor Positioning)是一种常用的定位方法,它需要至少四个固定在空间中的基站来定位目标物体的位置。四点定位的基本原理是通过测量目标物体到每个基站的距离,计算出目标物体的位置。
四点定位的具体步骤如下: 基站部署:将至少四个基站固定在不同的位置,并记录下它们的坐标。 测距:基站发送UWB信号,目标物体接收信号后返回一个响应信号,基站接收到响应信号后测量出目标物体到基站的距离。 计算:根据测量得到的距离以及基站的坐标,使用三角定位等算法计算出目标物体的位置。 误差校正:对测量误差进行校正,提高定位精度。
在四维空间中分别在4个角落A0,A1,A2,A3放置UWB锚点(anchor),锚点向所有方向发送信号。Tag是UWB标签(靶点),即需要定位的目标(只在测试环境范围内)。在已知锚点坐标求靶点坐标。
UWB定位的3种算法:TWR、TOA和TDOA算法。
一、模型建立 给出的数据中,三维空间中的四个锚点位置坐标以及锚点到靶点的距离是已知的,可以通过距离公式得出四个二次等式。
上式中, (x1,y1,z1)、(x2,y2,z2)、(x3,y3,z3)、(x4,y4,z4)为四个锚点A0、A1、A2、A3的坐标,(x,y,z) 为需要预测的靶点位置坐标,d1、d2、d3、d4为靶点分别到四个锚点的欧氏距离。对上式进行整理可以得到公式。
对于整理后的式子,因为其靶点坐标未知数含有高次项,无法直接求解,故对方程进行三次差分从而消除高次项,经差分可得到公式。
通过上式可以得到A*c=b的矩阵形式,从而可以得到靶点坐标为(A^-1) *b,其中A为可逆矩阵。
二、matlab实现
clc
clear
A0=[0,0,1300];
A1=[5000,0,1700];
A2=[0,5000,1700];
A3=[5000,5000,1300];
A=[];
A=[A0
A1
A2
A3];
x=A(:,1);
y=A(:,2);
z=A(:,3);
L1=length(x(:,1));
r=1700;
x2 = x.'
y2 = y.'
z2 = z.'
figure('Name','空间四点定位图','NumberTitle','off');
plot3(x2,y2,z2,'mo');
xlabel('x坐标/mm');
ylabel('y坐标/mm');
zlabel('z坐标/mm');
axis equal;
hold on
%距离
distance=[760 4550 4550 6300]
AA1 = zeros(3);
b= zeros(3,1);
c = zeros(3,1);
AA1 = 2*([x2(2)-x2(1) y2(2)-y2(1) z2(2)-z2(1);
x2(3)-x2(1) y2(3)-y2(1) z2(3)-z2(1);
x2(4)-x2(1) y2(4)-y2(1) z2(4)-z2(1)])
AA2 = inv(AA1)
b = [distance(1)^2-distance(2)^2+x2(2)^2-x2(1)^2+y2(2)^2-y2(1)^2+z2(2)^2-z2(1)^2;
distance(1)^2-distance(3)^2+x2(3)^2-x2(1)^2+y2(3)^2-y2(1)^2+z2(3)^2-z2(1)^2;
distance(1)^2-distance(4)^2+x2(4)^2-x2(1)^2+y2(4)^2-y2(1)^2+z2(4)^2-z2(1)^2]
c=AA2*b
plot3(c(1),c(2),c(3),'k+');
plot3(50,50,88,'r*');
a2=[distance(1),x(1),y(1),z(1)];
b2=[distance(2),x(2),y(2),z(2)];
c2=[distance(3),x(3),y(3),z(3)];
d2=[distance(4),x(4),y(4),z(4)];
A2=[a2
b2
c2
d2];
r4=A2(:,1).'
x4=A2(:,2).'
y4=A2(:,3).'
z4=A2(:,4).'
for i=1:L1
r5=r4(i);
x5=x4(i);
y5=y4(i);
z5=z4(i);
[x6,y6,z6]=sphere();
%调整半径
x6=r5*x6;
y6=r5*y6;
z6=r5*z6;
%调整圆心
x6=x6+x5;
y6=y6+y5;
z6=z6+z5;
axis equal;
s = surf(x6,y6,z6,'FaceAlpha',0.3);
s.EdgeColor = 'none';
plot3([x(1),c(1)],[y(1),c(2)],[z(1),c(3)],'k');
plot3([x(2),c(1)],[y(2),c(2)],[z(2),c(3)],'k');
plot3([x(3),c(1)],[y(3),c(2)],[z(3),c(3)],'k');
plot3([x(4),c(1)],[y(4),c(2)],[z(4),c(3)],'k');
axis equal;
grid on;
end
legend('锚点','预测靶点','实际靶点');
title('四点定位图');
以上是基于最小二乘的原理求解节点坐标位置,若采用TDOA的方程形式,则可以用chan算法进行定位求解。
clear all;
clc;
%% 初始化数值
c = 3120.77; % 标准值
t1 = 0.22 * 0.0001; % 传感器1的时间延迟
t2 = 0.58 * 0.0001; % 传感器2的时间延迟
x1 = 0; y1 = 0.5; % 传感器1
x2 = 0; y2 = 0; % 传感器2
x3 = 0.5; y3 = 0; % 传感器3
s1 = c * t1; % 时延距离 s1 = r21
s2 = c * t2; % 试验距离 s2 = r31
%% 求解未知点
k1 = x1^2 + y1^2; % 中间值
k2 = x2^2 + y2^2;
k3 = x3^2 + y3^2;
p1_molecule = (y2 - y1)*s2^2 - (y3 - y1)*s1^2 + (y3 - y1)*(k2 - k1) - (y2 - y1)*(k3 - k1);
p1_denominator = (x2 - x1)*(y3 - y1) - (x3 - x1)*(y2 - y1);
p1 = p1_molecule / (p1_denominator*2);
q1 = ((y2 - y1)*s2 - (y3 - y1)*s1)/((x2 - x1)*(y3 - y1)-(x3 - x1)*(y2 - y1));
p2_molecule = (x2 - x1)*s2^2 - (x3 - x1)*s1^2 + (x3 - x1)*(k2 - k1) - (x2 - x1)*(k3 - k1);
p2_denominator = (x3 - x1)*(y2 - y1) - (x2 - x1)*(y3 - y1);
p2 = p2_molecule / (p2_denominator * 2);
q2 = ((x2 - x1)*s2 - (x3 - x1)*s1)/((x3 - x1)*(y2 - y1)-(x2 - x1)*(y3 - y1));
%% 求取方程
a = q1^2 + q2^2 -1;
b = -2*((x1 - p1)*q1 + (y1 - p2)*q2);
c = (x1 - p1)^2 + (y1 - p2)^2;
r1 = (-b + sqrt(b^2 - 4*a*c))/(2*a);
r2 = (-b - sqrt(b^2 - 4*a*c))/(2*a);
if r1 > 0
x = p1 + q1*r1;
y = p2 + q2*r1;
else
x = p1 + q1*r2;
y = p2 + q2*r2;
end
disp('未知点坐标: ')
disp(x), disp(y)