STM32 + UWB + MPU9250 DS-TWR 定位方案技术手册
1. 文档说明
本文档用于说明本项目的方案设计、设备角色、固件运行流程、UWB 测距流程、MPU9250 运动检测流程，以及上位机接收与定位处理流程。阅读对象为客户、项目集成方、现场调试人员和技术支持人员。
本方案由标签、基站和上位机三部分组成：
- 标签：主动发起 UWB 测距，采集运动检测状态，并周期性汇总上报。
- 基站：响应标签测距请求，计算 UWB 距离，并将标签汇总数据转发给上位机。
- 上位机：接收基站串口数据，解析多基站距离和运动状态，完成简单融合与定位显示。
2. 系统组成
2.1 硬件组成
| 设备 | 硬件平台 | 主要功能 |
|---|---|---|
| UWB 标签 | STM32F401 + DW1000/UWB + MPU9250 | 发起 DS-TWR 测距，检测运动状态，上报多基站距离 |
| UWB 基站 | STM32F103 或 STM32F401 + DW1000/UWB | 响应测距请求，计算距离，汇聚并转发数据 |
| 上位机 | Windows PC + Qt 上位机软件 | 串口/TCP 接收数据，定位计算，界面显示 |
2.2 软件组成
| 软件部分 | 说明 |
|---|---|
| STM32F1 固件 | 用于 STM32F103 + UWB 设备，可作为标签或基站固件编译 |
| STM32F4 固件 | 用于 STM32F401 + UWB + MPU9250 设备，支持运动检测 |
| BP-UWB 公共库 | 封装 DW1000 初始化、UWB 帧结构、DS-TWR 测距应用逻辑 |
| MPU9250 驱动 | 配置 MPU9250 Wake-on-Motion 运动检测 |
| 上位机软件 | 接收串口/TCP 数据，解析距离，显示标签位置 |
3. 方案总体设计
本方案采用 DS-TWR 双边双向测距方式。标签依次与多个基站进行 UWB 测距，基站根据 UWB 收发时间戳计算标签到基站的距离。标签完成一轮测距后，将多基站距离和运动检测状态打包发送到汇聚基站，汇聚基站再通过串口发送给上位机。
整体数据链路如下：
- 标签按固定周期轮询基站。
- 标签向某个基站发送
P测距请求。 - 基站返回
A应答。 - 标签发送
F结束帧，帧中携带标签侧时间戳。 - 基站根据
P/A/F三帧计算本轮距离。 - 标签从基站应答中获得上一轮距离并缓存。
- 标签完成多个基站轮询后，发送
M汇总包给0x0001汇聚基站。 - 汇聚基站通过 UART 将
M包数据透传给上位机。 - 上位机解析距离、运动状态，并进行定位显示。
4. 设备角色
4.1 标签 Tag
标签是测距主动方，主要职责包括：
- 按定时器周期启动测距。
- 依次访问
0x0001、0x0002、0x0003等基站。 - 向当前目标基站发送
P帧。 - 接收基站
A帧，并发送F帧。 - 缓存各基站返回的距离。
- 读取 MPU9250 运动检测标志。
- 将距离和运动状态打成
M包发送给汇聚基站。
当前默认配置为 3 基站 2D 定位模式。标签的基站轮询顺序为：
Anchor 0x0001 -> Anchor 0x0002 -> Anchor 0x0003 -> Report to Anchor 0x0001 -> repeat
4.2 基站 Anchor
基站是测距响应方，主要职责包括：
- 长期开启 DW1000 接收模式。
- 接收标签
P帧并立即返回A帧。 - 接收标签
F帧，计算 DS-TWR 距离。 - 对距离进行 bias 修正和 Kalman 滤波。
- 作为汇聚基站时，接收标签
M包并通过 UART 转发给上位机。
4.3 汇聚基站
地址为 0x0001 的基站同时承担汇聚节点作用：
- 参与普通 UWB 测距。
- 接收标签发来的
M汇总数据。 - 将
M包中的上位机数据部分通过串口发出。
4.4 上位机
上位机主要职责包括：
- 打开串口或 TCP 通道。
- 接收汇聚基站转发的数据。
- 查找
mri数据头。 - 解析标签 ID、帧号、多基站距离和运动标志。
- 根据基站坐标和距离计算标签位置。
- 显示标签位置、距离、统计信息和轨迹。
5. 固件启动流程
5.1 STM32F1 固件启动流程
STM32F1 工程主要面向 STM32F103 + UWB 硬件。启动流程如下：
- 系统上电复位。
- 执行
HAL_Init()初始化 HAL。 - 执行
SystemClock_Config()配置系统时钟。 - 初始化 GPIO、SPI、TIM、UART 外设。
- 初始化 UWB 发送帧模板。
- 初始化 DW1000：
- 复位 DW1000。
- 配置 SPI 通信。
- 加载 DW1000 微码。
- 配置信道、速率、前导码、PANID、短地址。
- 配置天线延时和中断源。
- 根据编译角色进入标签流程或基站流程。
5.2 STM32F4 固件启动流程
STM32F4 工程主要面向 STM32F401 + UWB + MPU9250 硬件。启动流程如下：
- 系统上电复位。
- 初始化 HAL、系统时钟和基础外设。
- 初始化 UWB 发送帧模板。
- 初始化 DW1000。
- 如果启用 MPU9250：
- 初始化软件 I2C。
- 初始化 MPU9250。
- 配置 Wake-on-Motion 运动检测。
- 根据编译角色进入标签流程或基站流程。
5.3 DW1000 工作参数
当前 UWB 工作参数如下：
| 参数 | 当前配置 |
|---|---|
| Channel | 2 |
| PRF | 64 MHz |
| Preamble Length | 1024 |
| PAC | 32 |
| TX/RX Preamble Code | 9 |
| SFD | Non-standard SFD |
| Data Rate | 110 Kbps |
| PANID | 0xF0F2 |
| 短地址 | 按标签/基站角色配置 |
6. UWB 空中协议
6.1 帧类型
本方案使用 IEEE 802.15.4 短地址数据帧，应用层通过 messageData[0] 区分业务类型。
| 帧类型 | 方向 | 功能 |
|---|---|---|
P |
标签 -> 基站 | Poll，请求本轮测距 |
A |
基站 -> 标签 | Ack，响应 Poll，并携带上一轮距离 |
F |
标签 -> 基站 | Final，携带标签侧时间戳 |
M |
标签 -> 汇聚基站 | 汇总多基站距离和运动状态 |
C |
标签 -> 汇聚基站 | 控制信息，预留给小车控制等场景 |
6.2 DS-TWR 三帧测距
一次完整测距由 P/A/F 三个空中帧组成：
Tag Anchor
| |
| ---------- P Poll --------> |
| | 记录 poll_rx_ts
| <--------- A Ack ---------- |
| 记录 resp_rx_ts |
| ---------- F Final -------> |
| | 记录 final_rx_ts
| | 计算距离
标签侧记录：
poll_tx_ts：标签发送P的时间戳。resp_rx_ts：标签收到A的时间戳。final_tx_ts：标签发送F的时间戳。
基站侧记录：
poll_rx_ts：基站收到P的时间戳。resp_tx_ts：基站发送A的时间戳。final_rx_ts：基站收到F的时间戳。
基站收到 F 后，根据六个时间戳计算飞行时间，再换算为距离：
Ra = resp_rx_ts - poll_tx_ts
Rb = final_rx_ts - resp_tx_ts
Da = final_tx_ts - resp_rx_ts
Db = resp_tx_ts - poll_rx_ts
tof_dtu = (Ra * Rb - Da * Db) / (Ra + Rb + Da + Db)
distance = tof_dtu * DWT_TIME_UNITS * SPEED_OF_LIGHT
距离计算完成后，基站会进行 DW1000 range bias 修正，并对同一标签的距离结果进行 Kalman 滤波。
6.3 A 帧距离回传方式
基站在收到标签 P 帧后，会立即返回 A 帧。A 帧除了承担测距应答作用，还会携带该标签上一轮已计算完成的距离。
这种设计减少了额外的数据帧，使标签可以在轮询过程中同步缓存每个基站的距离。
6.4 M 汇总帧
标签完成一轮基站测距后，会向 0x0001 汇聚基站发送 M 帧。M 帧中包含上位机需要的定位输入数据：
- 标签 ID。
- 帧号。
- 到多个基站的距离。
- MPU9250 运动检测状态。
汇聚基站收到 M 帧后，不在本地计算定位结果，而是将数据通过 UART 发送给上位机。
7. 标签端运行流程
标签固件进入 tx_main() 后启动 TIM1 定时器中断，之后主循环保持空转。标签的实际业务由定时器中断和 DW1000 外部中断驱动。
7.1 定时器驱动测距
标签通过 TIM1 周期性启动一次动作。每次定时器到期后，标签根据当前目标地址决定执行哪种业务：
- 当前目标地址为基站地址：发送
P帧启动一次测距。 - 当前目标地址为
0x0000：说明基站轮询完成，发送M汇总包。
当前 3 基站配置下，标签一轮完整周期包括：
- 向
0x0001发送P。 - 向
0x0002发送P。 - 向
0x0003发送P。 - 向
0x0001发送M汇总包。
7.2 发送 Poll
标签发送 P 帧时会执行以下步骤：
- 设置目标基站短地址。
- 设置帧序号。
- 设置
messageData[0] = 'P'。 - 将帧写入 DW1000 TX buffer。
- 立即发送。
- 记录
poll_tx_ts。 - 打开接收窗口，等待基站
A帧。
7.3 接收 Ack 并发送 Final
标签收到基站 A 帧后：
- 记录
resp_rx_ts。 - 设置延迟发送时间
final_tx_ts。 - 构造
F帧。 - 将
poll_tx_ts、resp_rx_ts、final_tx_ts写入F帧。 - 通过 DW1000 delayed TX 发送
F。 - 从
A帧中读取该基站上一轮距离。 - 将距离缓存到
Final_Distance[]。
7.4 发送汇总数据
标签轮询完所有基站后，调用汇总发送流程：
- 读取缓存的
Final_Distance[]。 - 读取并打包
isMpu9250_moved运动状态。 - 构造
M帧。 - 将
M帧发送到0x0001汇聚基站。 - 帧号自增。
- 清除本轮运动标志，等待下一轮检测。
8. 基站端运行流程
基站固件进入 rx_main() 后，会将 DW1000 配置为常驻接收状态。后续全部由 DW1000 接收中断驱动。
8.1 常驻接收
基站初始化完成后执行：
- 使能 DW1000 数据帧过滤。
- 设置接收 timeout 为 0，即一直等待接收。
- 启动 DW1000 接收机。
- 初始化 Kalman 滤波状态。
8.2 响应 Poll
基站收到 P 帧后：
- 将回复目标地址设置为标签地址。
- 记录帧序号。
- 设置
messageData[0] = 'A'。 - 将上一轮距离写入
messageData[1..2]。 - 立即发送
A帧。 - 记录
poll_rx_ts。
8.3 计算距离
基站收到 F 帧后：
- 记录
resp_tx_ts。 - 记录
final_rx_ts。 - 从
F帧中取出标签侧 3 个时间戳。 - 使用 DS-TWR 公式计算 TOF。
- 将 TOF 换算为距离。
- 执行 range bias 修正。
- 使用标签 ID 对应的 Kalman 滤波器更新距离。
- 保存结果，供下一次
A帧回传给标签。
8.4 转发汇总数据
汇聚基站收到 M 帧后：
- 取出
messageData[1]起始的数据载荷。 - 通过
UWB_USART发送给上位机。 - 重新开启 DW1000 接收。
9. MPU9250 运动检测流程
MPU9250 在本方案中用于判断标签是否发生运动。固件使用 Wake-on-Motion 模式，不进行惯导积分。
9.1 初始化流程
STM32F4 标签启动时执行：
- 初始化软件 I2C。
- 读取并初始化 MPU9250。
- 配置低功耗运动检测模式。
- 使能 MPU9250 运动中断输出。
9.2 运动检测配置
运动检测相关配置包括：
- 选择稳定时钟源。
- 配置加速度计工作模式。
- 配置运动检测控制寄存器。
- 设置 WOM 阈值。
- 开启运动检测中断。
- 设置低功耗加速度采样频率。
9.3 运动状态上报
运动状态按如下方式进入定位链路：
- 标签运动时，MPU9250 产生中断。
- STM32F4 外部中断置位
isMpu9250_moved。 - 标签发送
M汇总包时读取该标志。 - 如果本周期未检测到运动，写入字符
s。 - 如果本周期检测到运动，写入字符
m。 - 上位机根据该字符更新标签运动状态。
10. 串口数据格式
标签发送给上位机的数据不是直接由标签串口发出，而是通过 UWB 发送到汇聚基站，再由汇聚基站串口转发。
当前上位机数据帧格式如下：
| 字段 | 长度 | 含义 |
|---|---|---|
m |
1 字节 | 帧头第 1 字节 |
r |
1 字节 | 帧头第 2 字节 |
i |
1 字节 | 表示包含 IMU 状态 |
frame_type |
1 字节 | 0x02 表示标签距离报告 |
tag_id |
1 字节 | 标签 ID |
frame_num |
2 字节 | 帧号，低字节在前 |
range0 |
2 字节 | 标签到基站 0 的距离，单位 cm |
range1 |
2 字节 | 标签到基站 1 的距离，单位 cm |
range2 |
2 字节 | 标签到基站 2 的距离，单位 cm |
range3 |
2 字节 | 第四路距离字段 |
motion |
1 字节 | s 表示未运动，m 表示检测到运动 |
\n\r |
2 字节 | 结束符 |
距离字段采用小端格式：
range = low_byte | (high_byte << 8)
固件侧距离单位为 cm，上位机内部处理时会换算为 mm 或 m。
11. 上位机处理流程
11.1 数据接收
上位机可以通过串口或 TCP 接收数据。串口模式下：
- 打开 COM 口。
- 绑定
readyRead()信号。 - 收到数据后调用
newData()。 newData()读取串口缓存。- 调用
raw_data_process()进入协议解析。
11.2 数据解析
raw_data_process() 使用状态机从字节流中查找帧头：
- 等待字符
m。 - 等待字符
r。 - 如果第三个字符是
i，进入 UWB + IMU 数据模式。 - 收满数据后调用
ProcessData()。
ProcessData() 解析字段：
frame_type。- 标签 ID。
- 帧号。
- 4 路距离。
- 运动状态。
11.3 距离处理
上位机解析标签距离后，会更新标签对象中的距离缓存：
latestRange[0]：标签到基站 0 的距离。latestRange[1]：标签到基站 1 的距离。latestRange[2]：标签到基站 2 的距离。latestRange[3]：标签到基站 3 的距离字段。
之后上位机会分别触发距离更新信号，用于界面显示每个基站到标签的距离。
11.4 定位计算
上位机定位流程：
- 读取配置中的基站坐标。
- 读取当前标签的多基站距离。
- 根据有效距离数量选择定位模式。
- 调用
GetLocation()计算标签坐标。 - 更新标签位置。
- 刷新界面显示。
11.5 运动状态使用
上位机会根据 motion 字段更新标签运动状态：
s：当前周期未检测到运动。m：当前周期检测到运动。
运动状态可用于辅助上位机融合逻辑，例如静止保持、运动更新、轨迹平滑等。
12. 总体流程图
flowchart TB
subgraph TAG["标签 Tag"]
T0["上电启动"]
T1["初始化 MCU 外设"]
T2["初始化 DW1000"]
T3["初始化 MPU9250 运动检测"]
T4["启动 TIM1 周期调度"]
T5{"当前目标地址"}
T6["发送 P Poll 给基站"]
T7["等待 A Ack"]
T8["发送 F Final"]
T9["缓存该基站距离"]
T10["发送 M 汇总包到 0x0001"]
T11["读取并清除运动标志"]
end
subgraph ANCHOR["基站 Anchor"]
A0["上电启动"]
A1["初始化 MCU 外设"]
A2["初始化 DW1000"]
A3["进入常驻接收"]
A4{"收到帧类型"}
A5["P: 返回 A Ack"]
A6["F: 计算距离并滤波"]
A7["M: UART 透传给上位机"]
A8["重新开启接收"]
end
subgraph PC["上位机"]
P0["串口/TCP 接收"]
P1["查找 mri 帧头"]
P2["解析标签ID/帧号/距离/运动状态"]
P3["更新距离缓存"]
P4["计算标签坐标"]
P5["界面显示位置和距离"]
end
T0 --> T1 --> T2 --> T3 --> T4 --> T5
T5 -- "基站地址" --> T6 --> T7 --> T8 --> T9 --> T4
T5 -- "汇总地址 0x0000" --> T11 --> T10 --> T4
A0 --> A1 --> A2 --> A3 --> A4
A4 -- "P" --> A5 --> A8 --> A3
A4 -- "F" --> A6 --> A8 --> A3
A4 -- "M" --> A7 --> A8 --> A3
T6 -- "P" --> A4
A5 -- "A + 上一轮距离" --> T7
T8 -- "F + 时间戳" --> A4
T10 -- "M + 距离 + 运动状态" --> A4
A7 --> P0 --> P1 --> P2 --> P3 --> P4 --> P5
13. DS-TWR 测距时序图
sequenceDiagram
participant Tag as 标签
participant Anchor as 基站
participant PC as 上位机
Tag->>Anchor: P Poll
Anchor->>Anchor: 记录 poll_rx_ts
Anchor->>Tag: A Ack + 上一轮距离
Tag->>Tag: 记录 resp_rx_ts
Tag->>Anchor: F Final + 标签侧时间戳
Anchor->>Anchor: 记录 final_rx_ts
Anchor->>Anchor: 计算 TOF 和距离
Anchor->>Anchor: 距离修正与滤波
loop 一轮基站轮询完成
Tag->>Anchor: M 汇总包
Anchor->>PC: UART 转发 mri 数据
PC->>PC: 解析距离和运动状态
PC->>PC: 定位计算与显示
end
14. 标签状态机
stateDiagram-v2
[*] --> BOOT
BOOT --> INIT_UWB
INIT_UWB --> INIT_IMU
INIT_IMU --> IDLE
IDLE --> SEND_POLL: 定时器到期且目标为基站
SEND_POLL --> WAIT_ACK: P 发送完成
WAIT_ACK --> SEND_FINAL: 收到 A
SEND_FINAL --> IDLE: F 发送完成
WAIT_ACK --> IDLE: 接收超时
IDLE --> REPORT: 定时器到期且一轮测距完成
REPORT --> IDLE: M 汇总包发送完成
15. 基站状态机
stateDiagram-v2
[*] --> BOOT
BOOT --> INIT_UWB
INIT_UWB --> RX_WAIT
RX_WAIT --> SEND_ACK: 收到 P
SEND_ACK --> RX_WAIT: A 发送完成
RX_WAIT --> CALC_RANGE: 收到 F
CALC_RANGE --> RX_WAIT: 距离计算完成
RX_WAIT --> FORWARD_PC: 收到 M
FORWARD_PC --> RX_WAIT: UART 转发完成
16. 上位机解析状态机
stateDiagram-v2
[*] --> WAIT_M
WAIT_M --> WAIT_R: 收到 m
WAIT_M --> WAIT_M: 其他字节
WAIT_R --> WAIT_I: 收到 r
WAIT_R --> WAIT_M: 其他字节
WAIT_I --> RECV_FRAME: 收到 i
WAIT_I --> RECV_UWB_ONLY: 非 i 数据
RECV_FRAME --> PROCESS: 收满 UWB + IMU 数据
RECV_UWB_ONLY --> PROCESS: 收满 UWB 数据
PROCESS --> WAIT_M: 处理完成
17. 典型运行周期说明
以 3 基站、1 标签为例，一个完整定位周期如下：
- 标签向基站
0x0001发起测距。 - 基站
0x0001完成本轮距离计算，并在下一次A帧中回传距离。 - 标签向基站
0x0002发起测距。 - 基站
0x0002完成本轮距离计算。 - 标签向基站
0x0003发起测距。 - 基站
0x0003完成本轮距离计算。 - 标签发送
M汇总包给0x0001。 - 汇聚基站将
M包内容通过 UART 发送给上位机。 - 上位机解析本轮多基站距离和运动状态。
- 上位机计算标签位置并更新界面。
18. 客户集成关注点
18.1 基站地址
基站使用短地址区分，当前 3 基站方案通常使用：
- Anchor 0：
0x0001 - Anchor 1：
0x0002 - Anchor 2：
0x0003
其中 0x0001 作为汇聚基站连接上位机。
18.2 标签地址
标签使用短地址作为标签 ID。上位机解析数据时使用该 ID 区分不同标签。
18.3 基站坐标
上位机定位计算需要预先配置基站坐标。实际部署时，应保证上位机中的基站坐标与现场安装位置一致。
18.4 串口连接
上位机连接汇聚基站串口。汇聚基站将标签汇总数据透传到 PC，因此普通基站无需连接上位机。
18.5 运动检测
MPU9250 输出的是运动状态标志，不输出完整 IMU 姿态或惯导位移。上位机可将该标志用于辅助定位显示或过滤逻辑。
19. 数据流总结
标签 TIM1 周期调度
-> 依次向基站发送 P
-> 收到 A 后发送 F
-> 基站计算距离
-> 标签缓存上一轮距离
-> 标签发送 M 汇总包
-> 汇聚基站 UART 转发
-> 上位机解析 mri 数据
-> 上位机计算并显示标签位置
该流程体现了本方案的职责划分：固件侧负责 UWB 测距和运动状态采集，基站侧负责距离计算与数据汇聚，上位机侧负责定位融合与显示。