UWB多基站定位浅试
介绍
在现代的定位技术中，通过多个基站（或锚点）来确定目标位置是一种常见的方法。这种技术被广泛应用于许多领域，如无线通信、室内定位和物联网等。本文将介绍一种通过多个基站进行三边测量（trilateration）的定位算法，以估计目标在三维空间中的坐标。
算法原理
该算法基于三边测量原理，通过测量目标到多个基站的距离来估计目标的位置。下面是算法的基本步骤：
- 定义一个三维向量类
vec3d，用于表示三维空间中的坐标点。 - 实现一个距离计算函数
calculate_distance，用于计算两个坐标点之间的欧氏距离。 - 实现一个误差函数
error_function，用于评估估计位置与实际位置之间的误差。该函数计算估计位置到每个基站的距离，并将其与实际测量的距离进行比较。 - 主要的定位算法函数
trilateration使用最小二乘优化方法来最小化误差函数。它需要提供初始估计位置、基站的位置信息以及目标到每个基站的距离作为输入参数，并返回估计的目标位置。
最小二乘法是一种常用的数学优化方法，用于找到一组参数，使得观测数据和模型预测之间的残差平方和最小化。它被广泛应用于拟合数据、解决线性方程组和参数估计等问题。
最小二乘法的基本原理是通过调整参数的值，使得模型的预测值与观测数据之间的误差最小化。具体来说，对于给定的数据集，最小二乘法通过最小化残差的平方和来确定最佳的参数。残差是观测数据与模型预测值之间的差异，平方和是为了忽略正负差异并放大较大的误差。
通过最小化残差的平方和，最小二乘法可以得到参数的最优解，这个解可以使得模型与实际观测数据之间的差异最小化。最小二乘法可以用于线性问题和非线性问题，具体的求解方法和算法可能会有所不同。
在实际应用中，最小二乘法广泛应用于各种领域，例如回归分析、数据拟合、信号处理和机器学习等。它提供了一种数学上可行且较为稳定的方法，通过优化参数来拟合数据并找到最佳的解决方案。
代码实现
以上述算法原理为基础，我们编写了以下2份Python代码来实现该定位算法
Python源码
通过4个基站求出标签坐标
import numpy as np
from scipy.optimize import least_squares
class vec3d:
def __init__(self, x, y, z):
self.x = x
self.y = y
self.z = z
def calculate_distance(p1, p2):
return np.sqrt((p1.x - p2.x)**2 + (p1.y - p2.y)**2 + (p1.z - p2.z)**2)
def error_function(params, anchor_positions, distances):
num_anchors = len(anchor_positions)
x, y, z = params
# Compute distances from estimated position to anchors
estimated_position = vec3d(x, y, z)
estimated_distances = np.zeros(num_anchors)
for i in range(num_anchors):
estimated_distances[i] = calculate_distance(estimated_position,
anchor_positions[i])
# Calculate error as the difference between estimated distances and measured distances
error = estimated_distances - distances
return error
def trilateration(anchor_positions, distances):
# Initial estimate for the target position
initial_position = np.array([0.0, 0.0, 0.0])
# Use least squares optimization to minimize the error function
result = least_squares(error_function,
initial_position,
args=(anchor_positions, distances))
# Extract the estimated target position
estimated_position = result.x
return estimated_position
if __name__ == '__main__':
# Example usage
anchor_positions = [
vec3d(0, 0, 0.5),
vec3d(6, 0, 0.5),
vec3d(6, 6, 0.5),
vec3d(0, 6, 2.5),
]
distances = np.array([
1.000000,
5.000000,
7.810250,
6.403125,
])
estimated_position = trilateration(anchor_positions, distances)
# 结果精确到小数点后三位即可
estimated_position = np.round(estimated_position, 3)
print("Estimated Position:", estimated_position)
# 验证距离以检查答案是否正确
for i in range(len(anchor_positions)):
print(
calculate_distance(
vec3d(estimated_position[0], estimated_position[1],
estimated_position[2]), anchor_positions[i]))
输出信息
Estimated Position: [ 1. -0. 0.5]
1.0
5.0
7.810249675906654
6.4031242374328485
通过多个基站求出标签坐标
多基站定位其实跟上面的4基站定位是一样的代码，只不过anchor_positions，distances这两个变量中多新增n组值，具体源码见下
import numpy as np
from scipy.optimize import least_squares
class vec3d:
def __init__(self, x, y, z):
self.x = x
self.y = y
self.z = z
def calculate_distance(p1, p2):
return np.sqrt((p1.x - p2.x)**2 + (p1.y - p2.y)**2 + (p1.z - p2.z)**2)
def error_function(params, anchor_positions, distances):
num_anchors = len(anchor_positions)
x, y, z = params
# Compute distances from estimated position to anchors
estimated_position = vec3d(x, y, z)
estimated_distances = np.zeros(num_anchors)
for i in range(num_anchors):
estimated_distances[i] = calculate_distance(estimated_position,
anchor_positions[i])
# Calculate error as the difference between estimated distances and measured distances
error = estimated_distances - distances
return error
def trilateration(anchor_positions, distances):
# Initial estimate for the target position
initial_position = np.array([0.0, 0.0, 0.0])
# Use least squares optimization to minimize the error function
result = least_squares(error_function,
initial_position,
args=(anchor_positions, distances))
# Extract the estimated target position
estimated_position = result.x
return estimated_position
# 通过指定标签坐标，计算与各基站的距离
def get_distance(anchor_positions, tag_position):
num_anchors = len(anchor_positions)
distances = np.zeros(num_anchors)
for i in range(num_anchors):
distances[i] = calculate_distance(tag_position, anchor_positions[i])
return distances
if __name__ == '__main__':
# 基站坐标
anchor_positions = [
vec3d(0, 0, 0.5),
vec3d(6, 0, 0.5),
vec3d(6, 6, 0.5),
vec3d(0, 6, 2.5),
vec3d(12, 6, 1.5),
vec3d(18, 12, 0.5),
]
# 通过指定标签坐标，计算与各基站的距离
# tag_position = vec3d(3, 3, 1)
# distances = get_distance(anchor_positions, tag_position)
# print(distances)
# 通过指定距离，计算标签坐标
distances = np.array([4.27200187, 4.27200187, 4.27200187, 4.5, 9.5, 17.5])
estimated_position = trilateration(anchor_positions, distances)
estimated_position = np.round(estimated_position, 3)
print("Estimated Position:", estimated_position)
# 验证距离以检查答案是否正确
for i in range(len(anchor_positions)):
print(
calculate_distance(
vec3d(estimated_position[0], estimated_position[1],
estimated_position[2]), anchor_positions[i]))
输出信息
Estimated Position: [3. 3. 1.]
4.272001872658765
4.272001872658765
4.272001872658765
4.5
9.5
17.5
使用示例
为了演示该算法的使用，我们提供了一个简单的示例。假设有多个基站的位置和目标到这些基站的距离已知。通过调用trilateration函数，我们可以获得估计的目标位置。
结果分析
该算法通过利用多个基站的位置和距离信息，利用三边测量原理来估计目标在三维空间中的坐标。通过最小二乘优化，该算法能够提供较为准确的估计结果。然而，需要注意的是，该算法对基站位置的准确性要求较高，且受到信号传输的影响。
结论
通过多个基站求出标签坐标的定位算法是一种常见而有效的定位方法。它在许多领域中具有广泛的应用前景，特别是在室内定位和物联网领域。通过三边测量和最小二乘优化，该算法能够提供准确的目标定位结果，为实际应用提供了一种可行的解决方案。
其他
此外，这是根据某UWB店铺给出的QT源码写的ESP32单片机的三边定位算法demo
该demo通过输入3个或4个基站的坐标，以及标签分别与基站的距离，得到标签的空间坐标