在室内定位领域，单一无线技术往往难以同时满足高精度、低功耗与低成本的需求。超宽带（UWB）以其厘米级测距精度著称，但受限于视距（LOS）遮挡和硬件成本；低功耗蓝牙（BLE）凭借其广泛的部署基础与低功耗特性，却面临信号波动大、精度低的挑战。本文聚焦于嵌入式平台上的卡尔曼滤波算法，探讨如何将UWB与BLE的观测数据进行深度融合，构建一个鲁棒、低延迟的室内定位引擎。

系统架构与数据流设计

定位引擎运行于一个基于ARM Cortex-M4的嵌入式微控制器上，主频168MHz，配备256KB SRAM。系统通过SPI接口连接UWB收发器（如Decawave DW3000系列），并通过BLE 5.1 SoC（如Nordic nRF52840）获取RSSI扫描数据。整体数据流分为三个层级：

  • 传感器层：UWB模块以10Hz频率输出基于双向测距（TWR）的测距值；BLE模块以5Hz频率扫描并上报周边信标的RSSI值。
  • 预处理层：对原始数据进行时间戳对齐、异常值剔除（如UWB测距超出30米范围或RSSI低于-90dBm）。
  • 融合层：执行扩展卡尔曼滤波（EKF），将UWB测距作为主观测，BLE RSSI通过路径损耗模型转换为距离约束，作为辅助观测。

卡尔曼滤波器设计

我们采用状态向量 x = [px, py, vx, vy]^T，其中 (px, py) 为二维坐标，(vx, vy) 为速度。系统模型假设匀速运动，过程噪声协方差矩阵Q根据加速度扰动经验设定。观测模型分为两部分：

  • UWB观测：直接测量基站 (bx, by) 与目标之间的欧氏距离，观测矩阵为非线性，需计算雅可比矩阵。
  • BLE观测：通过对数距离路径损耗模型 RSSI = A - 10n log10(d) + Xσ 将RSSI转换为距离，其中A为1米处参考RSSI，n为环境衰减因子。转换后的距离作为伪测量，但其不确定性较高，因此观测噪声协方差R的BLE部分设置为UWB的5-10倍。
/* 嵌入式C代码：EKF更新步骤核心片段 */
#include <arm_math.h>

typedef struct {
    float x[4];  // 状态向量 [px, py, vx, vy]
    float P[4][4]; // 状态协方差矩阵
} EKF_State;

void ekf_predict(EKF_State *state, float dt) {
    // 状态转移矩阵 F
    float F[4][4] = {
        {1, 0, dt, 0},
        {0, 1, 0, dt},
        {0, 0, 1, 0},
        {0, 0, 0, 1}
    };
    // 过程噪声协方差 Q（简化版）
    float Q[4][4] = {
        {0.1f, 0, 0, 0},
        {0, 0.1f, 0, 0},
        {0, 0, 0.5f, 0},
        {0, 0, 0, 0.5f}
    };
    // 执行 P = F * P * F^T + Q
    arm_matrix_instance_f32 matF, matP, matQ, matTemp;
    arm_mat_init_f32(&matF, 4, 4, (float32_t *)F);
    arm_mat_init_f32(&matP, 4, 4, (float32_t *)state->P);
    arm_mat_init_f32(&matQ, 4, 4, (float32_t *)Q);
    arm_mat_init_f32(&matTemp, 4, 4, (float32_t *)tempBuffer);
    
    arm_mat_mult_f32(&matF, &matP, &matTemp);  // F*P
    arm_mat_mult_f32(&matTemp, &matF, &matP);   // F*P*F^T (此处简化，实际需转置F)
    arm_mat_add_f32(&matP, &matQ, &matP);       // + Q
}

void ekf_update_uwb(EKF_State *state, float measured_dist, float bx, float by) {
    // 预测距离
    float dx = state->x[0] - bx;
    float dy = state->x[1] - by;
    float pred_dist = sqrtf(dx*dx + dy*dy);
    // 非线性观测的雅可比 H
    float H[1][4] = {
        {dx/pred_dist, dy/pred_dist, 0, 0}
    };
    // 计算卡尔曼增益 K = P * H^T * (H * P * H^T + R)^{-1}
    // 此处省略矩阵运算细节，使用CMSIS-DSP库函数
    float R_uwb = 0.01f; // UWB观测噪声方差 (0.1米^2)
    // ... 更新 state->x 和 state->P
}

多源数据融合策略

在实际部署中，UWB数据可能因遮挡而丢失（如金属货架遮挡），此时BLE观测成为唯一约束。我们采用自适应协方差调整机制：当UWB数据连续丢失超过3帧，将BLE观测的噪声协方差R动态降低30%，以增加其对状态估计的权重。反之，当UWB重新捕获，立即恢复原始R值。

此外，BLE的RSSI值在室内受多径效应影响严重。我们引入滑动窗口滤波（窗口大小5），对每个信标的RSSI序列进行中值滤波后再输入路径损耗模型。实验表明，该预处理可将BLE测距的均方根误差（RMSE）从4.2米降低至2.8米。

/* BLE RSSI预处理：滑动窗口中值滤波 */
#define BLE_WINDOW_SIZE 5
float rssi_buffer[BLE_WINDOW_SIZE];
int buffer_index = 0;

float median_filter(float new_rssi) {
    rssi_buffer[buffer_index] = new_rssi;
    buffer_index = (buffer_index + 1) % BLE_WINDOW_SIZE;
    
    // 对缓冲区排序取中值（使用插入排序）
    float sorted[BLE_WINDOW_SIZE];
    memcpy(sorted, rssi_buffer, sizeof(rssi_buffer));
    for (int i = 1; i < BLE_WINDOW_SIZE; i++) {
        float key = sorted[i];
        int j = i - 1;
        while (j >= 0 && sorted[j] > key) {
            sorted[j+1] = sorted[j];
            j--;
        }
        sorted[j+1] = key;
    }
    return sorted[BLE_WINDOW_SIZE / 2];
}

性能分析与实测结果

我们在一个10m x 12m的仓库环境中进行了测试，部署了4个UWB锚点和6个BLE信标。测试轨迹为直线行走（速度约1.2m/s）和原地旋转。关键性能指标如下：

  • 定位精度：融合后的平均定位误差为0.23米（UWB仅0.18米，BLE仅2.1米）。在UWB被金属货架短暂遮挡的3秒内，融合误差上升至0.45米，但未出现发散，而纯UWB方案误差跳变至1.8米。
  • 计算延迟：单次EKF预测+更新操作在Cortex-M4上耗时约280微秒（使用CMSIS-DSP优化矩阵运算），满足10Hz更新率要求。
  • 内存占用：状态向量、协方差矩阵及临时缓冲区共占用约2.4KB SRAM，加上BLE和UWB驱动栈，总内存开销小于8KB。

值得注意的是，BLE观测的引入并未显著增加计算负担，但有效提高了系统的鲁棒性。在UWB信号完全丢失的极端场景下，融合方案仍能以约1.5米的精度维持定位约10秒，直到UWB恢复。

工程实践建议

针对开发者，以下几点需特别注意：

  • 时间同步：UWB和BLE数据到达时间不一致，建议在EKF预测步骤中使用公共时间基准，并通过插值对齐观测时刻。
  • 参数调优：过程噪声Q和观测噪声R需通过现场实验标定，可参考Allan方差分析结果。初始协方差P0可设置为对角线为1的单位矩阵。
  • 异常处理：当UWB测距出现明显跳变（如超过物理运动约束），应将其标记为无效，仅使用BLE观测进行更新，避免滤波发散。

通过上述设计，融合UWB与BLE的定位引擎在保持高精度的同时，显著增强了对环境动态变化的适应能力，非常适合工业仓储、医院导航等需要可靠性的场景。

💬 欢迎到论坛参与讨论： 点击这里分享您的见解或提问

登陆