告别WiFi和GPS：用UWB给MiniFly无人机做室内‘厘米级’定位的实战笔记
在无人机技术快速发展的今天，室内精准定位一直是开发者面临的重大挑战。传统WiFi定位精度通常在米级徘徊，GPS信号在室内几乎完全失效，而视觉定位又受限于光照条件和计算资源。当我们需要在复杂室内环境中实现无人机编队飞行或精准作业时，这些方案都显得力不从心。超宽带（UWB）技术的出现，为这一难题提供了突破性的解决方案——它能在室内环境中实现厘米级定位精度，且不受多径效应和电磁干扰的显著影响。本文将分享如何基于UWB技术，特别是DS-TWR算法，为MiniFly无人机打造一套高精度室内定位系统，从技术选型到实战部署的全过程。
1. 为什么选择UWB：室内定位技术深度对比
在开始技术实现之前，我们需要明确为什么UWB是室内无人机定位的最佳选择。让我们先对比几种主流室内定位技术的核心指标：
| 技术指标 | WiFi定位 | 蓝牙Beacon | 视觉SLAM | UWB定位 |
|---|---|---|---|---|
| 理论精度 | 2-5米 | 1-3米 | 厘米级 | 10-30厘米 |
| 抗多径干扰能力 | 弱 | 中等 | 强 | 极强 |
| 系统延迟 | 高 | 中等 | 高 | 低 |
| 硬件成本 | 低 | 低 | 高 | 中等 |
| 部署复杂度 | 低 | 低 | 高 | 中等 |
UWB的核心优势在于其时间分辨率极高的脉冲信号。传统无线技术使用连续无线电波，而UWB发射的是持续时间极短（纳秒级）的脉冲，这使得它能精确测量信号飞行时间（Time of Flight）。实际测试表明，在典型的室内环境中，UWB可以实现：
- 静态定位精度：±10厘米
- 动态定位精度：±30厘米（移动速度<2m/s）
- 刷新率：可达100Hz（取决于配置）
对于MiniFly这类小型无人机，UWB的另一大优势是低功耗。以DW1000芯片为例，在定位模式下功耗仅35mA，远低于持续传输视频流的视觉方案。这使得在保持10分钟以上续航的同时实现精确定位成为可能。
2. UWB定位原理与算法选型：从SS-TWR到DS-TWR
UWB定位的核心是测距，而测距的准确性直接决定了整个系统的性能。目前主流的测距算法有两种：
2.1 单边双向测距（SS-TWR）
SS-TWR是最基础的测距方式，其原理如下：
- 设备A发送测距请求，并记录发送时间T1
- 设备B收到请求后，在时间T2发送响应
- 设备A记录收到响应的时间T3
- 计算飞行时间：Tprop = [(T3-T1)-(T2'-T2)]/2
这种方法的误差主要来自设备B的处理延迟(T2'-T2)和时钟不同步。实测数据显示，当时钟精度为20ppm时，10米距离的测距误差可达20厘米以上。
2.2 双边双向测距（DS-TWR）
DS-TWR通过增加一次往返测量，显著降低了时钟偏差的影响。其流程为：
# 伪代码展示DS-TWR流程 def DS_TWR(): # 第一次往返 T1 = deviceA.send_poll() T2 = deviceB.receive_poll() T3 = deviceB.send_response() T4 = deviceA.receive_response() # 第二次往返 T5 = deviceA.send_final() T6 = deviceB.receive_final() T7 = deviceB.send_report() T8 = deviceA.receive_report() # 计算飞行时间 Tprop1 = (T4-T1)-(T3-T2) Tprop2 = (T8-T5)-(T7-T6) Tprop = (Tprop1 * Tprop2) / (Tprop1 + Tprop2) return Tprop * speed_of_light / 2实测数据对比：
| 场景 | SS-TWR误差 | DS-TWR误差 |
|---|---|---|
| 5米空旷环境 | ±18cm | ±3cm |
| 10米多径环境 | ±35cm | ±8cm |
| 动态测距 | ±50cm | ±15cm |
注意：DS-TWR虽然精度更高，但需要额外的通信开销。在实际部署中，需要在精度和刷新率之间找到平衡点。
3. 系统架构设计：三基站一标签方案实战
针对MiniFly无人机的特点，我们选择了三基站一标签的架构设计。这种方案的优势在于：
- 标签端计算量小，适合搭载在资源有限的无人机上
- 基站部署灵活，最少只需3个即可实现3D定位
- 扩展性强，后续可轻松增加无人机数量
3.1 硬件选型与改造
我们选用了广州联网科技的UWB套件，核心组件包括：
基站模块：
- 主控：STM32F105
- UWB芯片：DW1000
- 通信接口：USB/串口
- 供电：POE或5V DC
标签模块：
- 尺寸：25mm×25mm（适合MiniFly）
- 重量：3.2g（含天线）
- 功耗：30mA@3.3V
对MiniFly的改造包括：
- 将716电机升级为720，提高升力
- 桨叶改为3英寸正反桨，增强稳定性
- 电池更换为850mAh，平衡续航与重量
- 在机身底部安装UWB标签，确保全向覆盖
3.2 基站部署要点
基站的部署位置直接影响定位精度。经过多次测试，我们总结出以下最佳实践：
- 高度布置：基站应高于飞行区域0.5-1米，避免信号被遮挡
- 几何分布：三个基站应呈等边三角形布置，边长建议3-5米
- 避免反射面：远离金属表面和大面积玻璃，减少多径干扰
- 同步校准：使用有线同步或无线同步确保时间基准一致
典型的部署坐标示例：
| 基站 | X坐标(m) | Y坐标(m) | Z坐标(m) |
|---|---|---|---|
| A | 0 | 0 | 1.5 |
| B | 4 | 0 | 1.5 |
| C | 2 | 3.46 | 1.5 |
4. 软件实现：从定位解算到飞行控制
4.1 定位解算算法实现
基于三基站的测距数据，我们可以通过以下步骤解算标签位置：
建立距离方程： $$(x-x_i)^2 + (y-y_i)^2 + (z-z_i)^2 = d_i^2 \quad (i=1,2,3)$$
通过差分消去二次项，得到线性方程组：
A = [2*(x2-x1), 2*(y2-y1), 2*(z2-z1); 2*(x3-x1), 2*(y3-y1), 2*(z3-z1)]; b = [d1^2 - d2^2 + x2^2 + y2^2 + z2^2 - x1^2 - y1^2 - z1^2; d1^2 - d3^2 + x3^2 + y3^2 + z3^2 - x1^2 - y1^2 - z1^2];使用最小二乘法求解：
# Python示例代码 def trilaterate(anchors, distances): A = [] b = [] for i in range(1, len(anchors)): A.append([2*(anchors[i][0]-anchors[0][0]), 2*(anchors[i][1]-anchors[0][1]), 2*(anchors[i][2]-anchors[0][2])]) b.append(distances[0]**2 - distances[i]**2 + anchors[i][0]**2 + anchors[i][1]**2 + anchors[i][2]**2 - anchors[0][0]**2 - anchors[0][1]**2 - anchors[0][2]**2) A = np.array(A) b = np.array(b) return np.linalg.lstsq(A, b, rcond=None)[0]
4.2 无人机控制逻辑优化
为了实现精准的定点飞行，我们开发了多级控制策略：
- 粗调阶段：当无人机距离目标点>1米时，采用固定速度飞行
- 精调阶段：进入1米范围内后，启用动态速度调整
- 保持阶段：到达目标点后，启用位置保持算法
关键代码逻辑：
// 动态速度调整示例 void adjustSpeed(float distance) { if (distance > 1.0f) { speed = HIGH_SPEED; } else if (distance > 0.3f) { speed = map(distance, 0.3f, 1.0f, LOW_SPEED, HIGH_SPEED); } else { speed = 0; // 到达目标 } }实测数据显示，这种分级控制策略可以将定位误差控制在±5厘米内，同时避免过度振荡。
5. 性能优化与实战技巧
经过多次实地测试，我们总结出以下提升系统性能的关键技巧：
天线优化：
- 使用全向天线而非定向天线
- 天线应垂直于地面安装
- 避免金属物体靠近天线
时钟校准：
- 每天首次使用前执行基站时钟同步
- 温度变化超过10℃时重新校准
多径抑制：
- 在基站固件中启用NLOS检测
- 设置合理的信号质量阈值
定位平滑：
# 卡尔曼滤波示例 class KalmanFilter: def __init__(self): self.Q = 0.01 # 过程噪声 self.R = 0.1 # 观测噪声 self.P = 1.0 self.x = 0.0 def update(self, z): # 预测 x_pred = self.x P_pred = self.P + self.Q # 更新 K = P_pred / (P_pred + self.R) self.x = x_pred + K * (z - x_pred) self.P = (1 - K) * P_pred return self.x
在实际项目中，我们遇到的最棘手问题是多无人机间的信号干扰。解决方案包括：
- 时分复用（TDMA）调度不同无人机的通信时段
- 为每架无人机分配唯一ID
- 动态调整发射功率
6. 进阶应用：从单机定位到集群编队
基于稳定的单机定位，我们可以进一步实现无人机编队控制。核心思路包括：
相对位置控制：
- 指定领航无人机
- 其他无人机保持与领航机的相对位置
队形变换算法：
def formation_control(drones, formation): leader_pos = drones[0].position for i in range(1, len(drones)): target_pos = leader_pos + formation.offsets[i] drones[i].fly_to(target_pos)碰撞避免：
- 设置安全半径
- 启用紧急避障策略
实测中，三架MiniFly无人机可以稳定保持三角形编队，间距误差小于15厘米，完全满足室内表演的需求。
