UWB precise positioning problem (TOA positioning (four-point positioning in three-dimensional space) matlab implementation)
Article directory
foreword
Four-Anchor Positioning (Four-Anchor Positioning) is a positioning method based on distance measurement, usually using the TOA method to calculate the distance from the target object to each base station. By measuring the distance from the target object to at least four base stations, and using algorithms such as triangulation to calculate the position of the target object. Therefore, four-point positioning is a kind of TOA positioning method.
In UWB precise positioning, four-point positioning (Four-Anchor Positioning) is a commonly used positioning method, which requires at least four base stations fixed in space to locate the position of the target object. The basic principle of four-point positioning is to calculate the position of the target object by measuring the distance from the target object to each base station.
The specific steps of four-point positioning are as follows:
Base station deployment: fix at least four base stations at different positions, and record their coordinates.
Ranging: The base station sends a UWB signal, the target object returns a response signal after receiving the signal, and the base station measures the distance from the target object to the base station after receiving the response signal.
Calculation: According to the measured distance and the coordinates of the base station, use algorithms such as triangulation to calculate the position of the target object.
Error correction: Correct the measurement error to improve the positioning accuracy.
In the four-dimensional space, place UWB anchor points (anchor) at the four corners A0, A1, A2, and A3, and the anchor points send signals to all directions. Tag is a UWB tag (target), that is, the target that needs to be located (only within the scope of the test environment). Find the coordinates of the target point at the known anchor point coordinates.
Three algorithms for UWB positioning: TWR, TOA and TDOA algorithms, link: https://blog.csdn.net/qq_40276082/article/details/127422978?spm=1001.2014.3001.5502
1. Model establishment
In the given data, the position coordinates of the four anchor points in three-dimensional space and the distance from the anchor point to the target point are known, and four quadratic equations can be obtained through the distance formula.
In the above formula, (x1, y1, z1), (x2, y2, z2), (x3, y3, z3), (x4, y4, z4) are the coordinates of the four anchor points A0, A1, A2, A3, (x, y, z) are the position coordinates of the target point to be predicted, and d1, d2, d3, and d4 are the Euclidean distances from the target point to the four anchor points respectively. Arrange the above formula to get the formula.
For the sorted formula, because the target point coordinate unknown contains high-order terms, it cannot be solved directly, so the equation is subtracted three times to eliminate the high-order terms, and the formula can be obtained through the difference.
The matrix form of A*c=b can be obtained through the above formula, so that the coordinates of the target point can be obtained as (A^-1) *b, where A is an invertible matrix.
Two, matlab implementation
1. Matlab code
The code is as follows (example):
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('四点定位图');
2. Running results
3. Common methods
TWR, TOA and TDOA are three commonly used algorithms in UWB positioning. They measure the time or time difference of signal propagation in different ways to calculate the position of the target object.
Two-Way Ranging (TWR): The TWR algorithm calculates the propagation time of the signal through two-way ranging, so as to obtain the distance from the target object to each base station. In the TWR algorithm, the base station sends a pulse signal to the target object, the target object returns a response signal after receiving the signal, and the base station receives the response signal again. By calculating the time difference between two round trips, the signal propagation time can be calculated, so as to obtain the distance from the target object to the base station.
Time of Arrival (TOA): The TOA algorithm calculates the distance traveled by the signal by measuring the time when the signal arrives at the receiving point, thereby obtaining the position of the target object. In the TOA algorithm, the base station sends a pulse signal to the target object, and the target object records the receiving time after receiving the signal. By calculating the time difference of signal propagation, the distance from the target object to the base station can be obtained.
Time Difference of Arrival (TDOA): The TDOA algorithm calculates the position of the target object by measuring the time difference between the arrival of the signal at different base stations. In the TDOA algorithm, multiple base stations send signals to the target object at the same time, and the target object records the receiving time after receiving the signal. By calculating the time difference of the signal arriving at different base stations, the distance difference between the target object and each base station can be obtained, and the position of the target object can be calculated.
These three algorithms have their own advantages and disadvantages, and the appropriate algorithm needs to be selected according to the specific application scenario. For example, the TWR algorithm has high precision, but requires two-way communication; the TOA algorithm is simple and easy to use, but has strict requirements on signal transmission time; the TDOA algorithm is suitable for multi-target positioning, but requires time synchronization of signals.