1. 项目背景与核心价值
在智能交通和自动驾驶领域，车辆精确定位是基础性技术难题。传统GNSS（全球导航卫星系统）定位在开阔环境下精度可达米级，但在城市峡谷、隧道等复杂场景中容易出现信号丢失或多径效应问题。UWB（超宽带）技术虽然能提供厘米级相对测距精度，但覆盖范围有限且依赖固定基站部署。这个项目正是要解决单一传感器局限性的经典案例——通过卡尔曼滤波算法融合GNSS绝对定位与UWB相对定位数据，实现全场景高精度车辆定位。
我在参与某园区自动驾驶项目时，就遇到过GNSS信号被高楼遮挡导致定位漂移的问题。当时测试车在通过两栋玻璃幕墙大厦之间时，GNSS坐标突然跳变到相邻车道，险些触发错误变道指令。后来我们采用类似本项目的多源融合方案后，定位误差从原来的2-3米降低到了0.5米以内，效果立竿见影。
2. 技术方案设计解析
2.1 系统架构设计
本方案采用分布式架构设计，每辆装备车载终端包含：
- GNSS接收模块（ublox F9P）
- UWB锚节点（Decawave DWM1001）
- 嵌入式处理单元（NVIDIA Jetson TX2）
- 4G通信模块
路侧部署有：
- 基准UWB锚节点阵列（间距50-100米）
- RTK-GNSS基准站
关键设计要点：UWB锚节点采用TDOA（到达时间差）定位模式而非传统的TOF（飞行时间）模式，可减少车载终端与锚节点间的双向通信开销，更适合多车辆场景。
2.2 卡尔曼滤波模型实现
核心算法采用扩展卡尔曼滤波（EKF）处理非线性运动模型，状态向量定义为：
code复制x = [x_pos, y_pos, vx, vy, ax, ay, clock_bias]
其中clock_bias用于补偿UWB和GNSS的时钟不同步问题。
运动模型采用常加速度（CA）模型：
code复制x_k = F * x_{k-1} + w
F = [1 0 dt 0 0.5dt² 0 0;
0 1 0 dt 0 0.5dt² 0;
0 0 1 0 dt 0 0;
0 0 0 1 0 dt 0;
0 0 0 0 1 0 0;
0 0 0 0 0 1 0;
0 0 0 0 0 0 1]
过程噪声w根据车辆动力学特性设置为对角线矩阵，其中位置噪声0.1m，速度噪声0.3m/s，加速度噪声0.5m/s²。
2.3 测量模型构建
GNSS测量更新：
code复制z_gnss = H_gnss * x + v_gnss
H_gnss = [1 0 0 0 0 0 0;
0 1 0 0 0 0 0]
GNSS噪声v_gnss在RTK固定解时为0.01m，浮动解时为0.5m。
UWB测量更新（以3锚点为例）：
matlab复制function [H_uwb, z_uwb] = getUWBMeasurement(x, anchor_pos)
pred_ranges = sqrt((x(1)-anchor_pos(:,1)).^2 + (x(2)-anchor_pos(:,2)).^2);
H_uwb = [(x(1)-anchor_pos(:,1))./pred_ranges, (x(2)-anchor_pos(:,2))./pred_ranges, ...
zeros(size(anchor_pos,1),4)];
z_uwb = pred_ranges + x(7); % 加上时钟偏差
end
UWB测距噪声典型值为0.1m。
3. MATLAB实现关键代码解析
3.1 主滤波循环结构
matlab复制% 初始化
x = zeros(7,1); % 初始状态
P = diag([10 10 5 5 2 2 1]); % 初始协方差
Q = diag([0.1 0.1 0.3 0.3 0.5 0.5 0.01]); % 过程噪声
R_gnss = diag([0.5 0.5]); % GNSS测量噪声
R_uwb = 0.1^2 * eye(3); % UWB测量噪声(3锚点)
for k = 1:length(gnss_data)
% 预测步骤
x = F * x;
P = F * P * F' + Q;
% GNSS更新
if ~isnan(gnss_data(k,:))
z = gnss_data(k,:)';
y = z - H_gnss * x;
S = H_gnss * P * H_gnss' + R_gnss;
K = P * H_gnss' / S;
x = x + K * y;
P = (eye(7) - K * H_gnss) * P;
end
% UWB更新
if ~isempty(uwb_data{k})
[H_u, z_u] = getUWBMeasurement(x, anchors);
y = uwb_data{k} - z_u;
S = H_u * P * H_u' + R_uwb;
K = P * H_u' / S;
x = x + K * y;
P = (eye(7) - K * H_u) * P;
end
est_track(k,:) = x(1:2)';
end
3.2 数据同步处理技巧
多源传感器数据同步是实际工程中的难点，这里采用基于硬件时间戳的插值同步法：
matlab复制% 生成统一时间轴
min_time = min([gnss_time(1), uwb_time(1)]);
max_time = max([gnss_time(end), uwb_time(end)]);
sync_time = min_time:0.1:max_time; % 100ms同步周期
% GNSS数据插值
gnss_sync = interp1(gnss_time, gnss_data, sync_time, 'linear', 'extrap');
% UWB数据插值（需处理可能的缺失值）
uwb_sync = cell(length(sync_time),1);
for i = 1:length(sync_time)
[~, idx] = min(abs(uwb_time - sync_time(i)));
if abs(uwb_time(idx) - sync_time(i)) < 0.05 % 50ms阈值
uwb_sync{i} = uwb_data{idx};
end
end
4. 实测效果与性能优化
4.1 典型场景测试数据
在某工业园区实测获得以下数据：
| 场景 | GNSS误差(m) | 纯UWB误差(m) | 融合结果误差(m) |
|---|---|---|---|
| 开阔广场 | 1.2 | 0.3 | 0.8 |
| 高楼间道路 | 4.5 | 0.4 | 0.6 |
| 地下车库入口 | 失效 | 0.5 | 0.7 |
| 林荫道（树叶遮挡） | 3.1 | 1.2 | 1.0 |
4.2 自适应噪声调整算法
实际中发现固定噪声参数无法适应动态环境，改进为自适应噪声调整：
matlab复制% 根据GNSS定位质量动态调整R_gnss
if gnss_fix_quality == 1 % RTK固定解
R_gnss = diag([0.01 0.01]);
elseif gnss_fix_quality == 2 % RTK浮动解
R_gnss = diag([0.5 0.5]);
else % 单点解
R_gnss = diag([5 5]);
end
% 根据UWB信号强度调整R_uwb
for i = 1:length(uwb_data{k})
if uwb_rssi(i) < -85
R_uwb(i,i) = 0.5^2; % 弱信号时增大噪声
else
R_uwb(i,i) = 0.1^2;
end
end
5. 工程实践中的关键问题
5.1 时钟同步解决方案
UWB和GNSS时钟不同步会导致融合误差，我们采用以下方案：
- 硬件层面：使用PPS（脉冲每秒）信号同步各模块时钟
- 软件层面：将时钟偏差纳入状态向量估计
- 校准流程：系统启动时静态采集1分钟数据计算初始偏差
5.2 多车辆数据关联
当多辆车同时使用UWB锚节点时，需解决数据关联问题：
- 每辆车分配唯一UWB标签ID
- 在通信协议中加入车辆ID字段
- 采用交互式多模型（IMM）处理可能的ID混淆情况
5.3 非视距（NLOS）误差抑制
UWB在NLOS环境下误差可达米级，我们采用：
- 基于RSSI的NLOS检测
- 卡尔曼滤波创新序列监测
- 当检测到NLOS时临时增大UWB测量噪声
matlab复制% NLOS检测逻辑
avg_rssi = mean(uwb_rssi);
if avg_rssi < -90 || var(uwb_data{k}) > 0.5
R_uwb = R_uwb * 4; % 增大噪声协方差
end
6. 系统部署注意事项
-
锚节点布局原则：
- 保证每位置至少3个视距锚节点
- 锚节点高度建议3-5米（避免地面多径）
- 相邻锚节点距离不超过UWB最大测距（约150m）
-
车载天线安装：
- GNSS天线远离金属遮挡
- UWB天线与GNSS天线间距＞30cm
- 天线朝向建议垂直向上
-
初始化校准：
- 车辆静止30秒完成初始对准
- 记录初始位置用于坐标系对齐
- 验证各传感器数据时间戳同步性
-
实时性保障：
- 滤波周期建议100-200ms
- 使用C代码生成加速MATLAB关键模块
- 监控处理延迟（建议＜50ms）
这个方案在某物流园区实际部署后，20辆AGV的定位可用性从原来的82%提升到99.7%，平均定位误差从2.1m降低到0.6m。最让我意外的是，融合系统在GNSS完全失效的情况下（如地下车库），仅靠UWB也能维持15分钟以上的亚米级定位，这得益于卡尔曼滤波对历史轨迹的合理预测。