4. UWB定位算法：三边定位法、最小二乘法、卡尔曼滤波融合定位
定位算法，说白了就是UWB系统的“大脑”。天线再好，标签再准，算法不行，定位结果照样飘。我见过不少项目，硬件选型花了大价钱，结果算法随便写个三边定位就上线了，最后客户投诉定位精度还不如蓝牙。嗯，这里面的坑，我今天一次性给你讲透。
4.1 三边定位法：最直观的几何解法
三边定位法的原理其实很简单——初中几何就学过。已知三个基站（Anchor）的坐标，再知道标签（Tag）到每个基站的距离，那标签的位置就是三个圆的交点。
核心公式：
(x - x₁)² + (y - y₁)² = d₁²
(x - x₂)² + (y - y₂)² = d₂²
(x - x₃)² + (y - y₃)² = d₃²
其中 (xᵢ, yᵢ) 是基站坐标，dᵢ 是测距结果，(x, y) 是待求的标签坐标。
理想情况下，三个圆交于一点。但现实很骨感——测距误差、多径效应、NLOS（非视距）都会让三个圆变成三个“糊饼”，要么交出一个区域，要么根本不交。我在一个仓储项目中就遇到过，货架金属反射严重，三个圆画出来完全不沾边，定位结果直接飞到仓库外面去了。
我的经验：三边定位适合做“粗定位”或“区域判定”。如果你只需要知道标签在哪个房间、哪个货架区，三边定位够用了。但要做厘米级精确定位，别指望它。
4.2 最小二乘法：让误差“平均”掉
既然三个圆交不到一个点，那我们就找一个“最可能”的点。最小二乘法的思路是：找一个点，让它到三个基站的距离误差平方和最小。
数学上，我们把三边定位的方程改写成线性形式：
// 将非线性方程线性化
// 用第1个基站减去第2、3个基站，消去二次项
2(x₂ - x₁)x + 2(y₂ - y₁)y = d₁² - d₂² + x₂² - x₁² + y₂² - y₁²
2(x₃ - x₁)x + 2(y₃ - y₁)y = d₁² - d₃² + x₃² - x₁² + y₃² - y₁²
// 写成矩阵形式：A * X = B
// 其中 X = [x, y]ᵀ
// 最小二乘解：X = (AᵀA)⁻¹AᵀB为什么用最小二乘法？因为它能“容忍”误差。你想想看，三个基站的数据都有噪声，最小二乘法相当于让所有基站“投票”，误差大的基站权重自然就低。我在做工厂AGV定位时，4个基站里有一个被叉车挡住了，最小二乘法硬是把定位误差控制在15厘米以内——换成三边定位，早就飞了。
注意：最小二乘法对“粗大误差”很敏感。如果某个基站的测距值突然跳变（比如被金属柜子遮挡），它会把这个错误值也“平均”进去。我建议你在输入最小二乘法之前，先做一步“野值剔除”——把明显偏离均值的测距值扔掉。
4.3 卡尔曼滤波：让定位“平滑”起来
三边定位和最小二乘法都是“单帧定位”——每一帧独立算位置。但实际场景中，标签是在移动的。你想想看，一个移动的标签，上一帧在A点，下一帧在B点，中间肯定有个连续轨迹。卡尔曼滤波就是利用这个“运动连续性”来优化定位结果。
卡尔曼滤波的核心思想就两句话：
- 预测：根据上一帧的位置和速度，猜这一帧大概在哪
- 更新：用这一帧的测距数据，修正预测结果
我画个流程图，你看一眼就明白了：
卡尔曼滤波的数学实现，我直接给你代码：
// 卡尔曼滤波核心结构体
typedef struct {
float x; // 位置 (m)
float y; // 位置 (m)
float vx; // 速度 (m/s)
float vy; // 速度 (m/s)
float P[4][4]; // 协方差矩阵
float Q[4][4]; // 过程噪声
float R[3][3]; // 观测噪声
} KalmanFilter;
// 预测步骤
void kalman_predict(KalmanFilter *kf, float dt) {
// 状态转移矩阵 F
float F[4][4] = {
{1, 0, dt, 0},
{0, 1, 0, dt},
{0, 0, 1, 0},
{0, 0, 0, 1}
};
// x = F * x
// P = F * P * F' + Q
// 具体矩阵运算略...
}
// 更新步骤
void kalman_update(KalmanFilter *kf, float *z, int n_anchors) {
// z: 当前帧测距值数组
// 计算卡尔曼增益 K
// x = x + K * (z - H*x)
// P = (I - K*H) * P
// 具体矩阵运算略...
}调参经验：卡尔曼滤波有两个关键参数——Q（过程噪声）和R（观测噪声）。Q越大，滤波器越“相信”观测值，响应快但噪声大；R越大，滤波器越“相信”预测值，平滑但滞后。我一般先设R为测距模块标称误差的平方（比如±10cm，R=0.01），Q从0.001开始调，直到轨迹既平滑又不滞后。
4.4 三种方法的融合策略
在实际项目中，我不会只用一种方法。我的融合策略是这样的：
| 场景 | 主算法 | 辅助算法 | 典型精度 |
|---|---|---|---|
| 静态定位（标签不动） | 最小二乘法 | 卡尔曼滤波（小Q） | ±5cm |
| 低速移动（< 1m/s） | 卡尔曼滤波 | 最小二乘法（初始化） | ±10cm |
| 高速移动（1-5m/s） | 卡尔曼滤波（大Q） | 三边定位（野值检测） | ±20cm |
| 基站遮挡/信号差 | 三边定位（降级） | 卡尔曼滤波（预测模式） | ±50cm |
为什么要这么分层？我举个例子。在AGV调度项目中，AGV静止充电时，我用最小二乘法+小Q卡尔曼，定位精度能到3厘米，AGV能精准停到充电桩上。AGV全速运行时（2m/s），我切换到卡尔曼滤波大Q模式，虽然精度降到15厘米，但轨迹不滞后，调度系统不会误判。如果某个基站突然被货架挡住，我立刻降级到三边定位+卡尔曼预测模式——精度虽然差一些，但至少不会丢定位。
避坑指南：我曾经在融合策略里犯过一个错——把所有场景都用同一套卡尔曼参数。结果静态时抖动大，动态时滞后严重。后来我加了场景识别逻辑：根据标签的加速度计数据和测距值的方差，自动切换参数集。效果立竿见影。
4.5 实战中的“最后一公里”
算法讲完了，但我想多说一句：算法再好，也架不住数据烂。我见过太多人把精力花在调卡尔曼参数上，结果发现是天线没校准、基站高度不一致、或者测距模块的晶振漂移。我的建议是：
- 先保证数据质量：测距值的标准差超过15cm，先别急着调算法，回去查硬件
- 做一次静态测试：标签放固定位置，看测距值的分布，确认是高斯噪声还是系统偏差
- 记录原始数据：把每次测距的原始值、RSSI、FP索引都存下来，方便事后分析