1. 项目背景与核心需求
差分驱动轮式机器人在工业自动化、仓储物流和服务机器人领域有着广泛应用。这类机器人通过独立控制两侧轮子的转速实现前进、后退和转向，其运动特性决定了位姿估计（位置和朝向）的复杂性。在实际应用中，单一传感器往往难以满足全天候、高精度的定位需求：
- 里程计（编码器）提供高频但存在累计误差的位移信息
- GPS模块在开阔区域能提供绝对位置但受信号遮挡影响
- 多机协作时车间测距传感器可提供相对位置约束
我在参与某自动化仓储项目时，曾遇到AGV小车运行2小时后定位偏差达1.5米的痛点问题。通过引入扩展卡尔曼滤波（EKF）融合多源传感器数据，最终将定位误差控制在0.2米以内。下面将详细解析该方案的实现细节。
2. 系统建模与EKF原理
2.1 差分驱动运动学模型
对于两轮差分驱动机器人，其连续时间运动模型可表示为：
ẋ = v·cosθ ẏ = v·sinθ θ̇ = ω其中v和ω分别表示机器人线速度和角速度，可通过编码器测量值计算得到：
v = (ω_r·r + ω_l·r)/2 ω = (ω_r·r - ω_l·r)/L这里ω_r和ω_l是左右轮角速度，r为轮半径，L为轮距。在实际离散化实现时，采用如下递推形式：
x_k = x_{k-1} + Δt·v·cosθ y_k = y_{k-1} + Δt·v·sinθ θ_k = θ_{k-1} + Δt·ω注意：当Δt较大或ω较高时，这种近似会引入明显误差。改进方法是采用精确的离散化模型或减小控制周期。
2.2 扩展卡尔曼滤波框架
EKF处理非线性系统的核心步骤：
预测阶段：
- 状态预测：x̂_k|k-1 = f(x̂_k-1|k-1, u_k)
- 协方差预测：P_k|k-1 = F_k P_k-1|k-1 F_k^T + Q_k
更新阶段：
- 卡尔曼增益：K_k = P_k|k-1 H_k^T (H_k P_k|k-1 H_k^T + R_k)^-1
- 状态更新：x̂_k|k = x̂_k|k-1 + K_k(z_k - h(x̂_k|k-1))
- 协方差更新：P_k|k = (I - K_k H_k) P_k|k-1
其中F_k和H_k分别是状态转移函数f和观测函数h的雅可比矩阵。对于我们的多传感器系统，观测函数需要处理不同类型的数据融合。
3. 多传感器融合实现
3.1 传感器数据处理
里程计数据预处理：
% 编码器脉冲转位移 left_dist = (left_encoder - last_left_encoder) * encoder_resolution; right_dist = (right_encoder - last_right_encoder) * encoder_resolution; % 计算线速度和角速度 v = (left_dist + right_dist) / (2 * dt); w = (right_dist - left_dist) / (wheel_base * dt);GPS数据校准：
function gps_data = process_gps(raw_gps) % 坐标转换（WGS84转局部坐标系） [x,y] = wgs2local(raw_gps.lat, raw_gps.lon); % 有效性检查 if raw_gps.hdop > HDOP_THRESHOLD gps_data.valid = false; else gps_data.valid = true; gps_data.x = x; gps_data.y = y; end end3.2 观测模型构建
对于多传感器系统，观测方程需要整合不同类型的数据：
z = [ x_gps (GPS位置) y_gps dist_ij (机器人i到j的距离) bearing_ij (相对方位角) odom_v (里程计线速度) odom_w ] (里程计角速度)对应的观测矩阵H需要根据可用传感器动态调整。例如当GPS失效时，应移除对应的行。
3.3 协方差矩阵调参
传感器噪声特性直接影响滤波效果：
里程计噪声：通常与速度成正比，可建模为：
Q_odom = diag([alpha1*v^2 + alpha2*w^2, alpha1*v^2 + alpha2*w^2, alpha3*v^2 + alpha4*w^2]);GPS噪声：与HDOP值相关：
R_gps = [(HDOP*GPS_POS_STD)^2 0; 0 (HDOP*GPS_POS_STD)^2];车间测距噪声：取决于传感器类型（UWB/激光等）：
R_range = [RANGE_STD^2 0; 0 BEARING_STD^2];
4. MATLAB实现关键代码
4.1 主滤波循环框架
function [state, cov] = ekf_localization(prev_state, prev_cov, sensors, dt) % 预测步骤 [state_pred, F] = motion_model(prev_state, sensors.odom, dt); Q = process_noise(sensors.odom); cov_pred = F * prev_cov * F' + Q; % 可用传感器检测 valid_sensors = check_sensor_availability(sensors); % 更新步骤 if any(valid_sensors) [z, H, R] = measurement_model(state_pred, sensors, valid_sensors); y = z - observation_function(state_pred, sensors, valid_sensors); S = H * cov_pred * H' + R; K = cov_pred * H' / S; state = state_pred + K * y; cov = (eye(3) - K * H) * cov_pred; else state = state_pred; cov = cov_pred; end end4.2 雅可比矩阵计算
function F = jacobian_motion(x, u, dt) theta = x(3); v = u(1); F = [1 0 -v*sin(theta)*dt; 0 1 v*cos(theta)*dt; 0 0 1]; end function H = jacobian_measurement(x, sensor_type) switch sensor_type case 'GPS' H = [1 0 0; 0 1 0]; case 'RANGE' dx = x(1) - anchor_x; dy = x(2) - anchor_y; dist = sqrt(dx^2 + dy^2); H = [dx/dist dy/dist 0; -dy/dist^2 dx/dist^2 -1]; end end5. 调试与优化经验
5.1 典型问题排查
问题1：GPS更新导致位置跳变
- 现象：每次GPS更新时估计位置出现明显跳跃
- 排查：检查GPS坐标转换是否正确，确认WGS84到局部坐标的转换参数
- 解决：增加GPS速度一致性检查，异常数据直接丢弃
问题2：长时间运行后航向角漂移
- 现象：运行1小时后机器人朝向出现明显偏差
- 排查：检查陀螺仪零偏校准过程
- 解决：增加磁力计辅助航向参考，设置EKF中角度更新的权重系数
5.2 性能优化技巧
- 异步传感器处理：
% 根据传感器时间戳进行插值处理 function sync_data = synchronize_sensors(t, sensor_data) [~, idx] = min(abs([sensor_data.timestamp] - t)); sync_data = sensor_data(idx); end- 自适应噪声调整：
function R = dynamic_noise(sensor) if sensor.type == "GPS" R = base_gps_noise * (1 + sensor.hdop/2); end end- 多机协同优化：
- 建立通信拓扑管理机制
- 采用分布式EKF架构降低计算负载
- 设置信息年龄阈值避免陈旧数据影响
6. 实际部署考量
在真实场景部署时，还需要考虑：
- 计算资源分配：
- 单机EKF处理时间应小于传感器最短更新周期
- 对于10+机器人集群，建议采用边缘计算节点分担负载
- 通信延迟补偿：
- 为车间测距数据添加时间戳
- 采用状态预测补偿网络延迟
- 故障恢复机制：
- 设计EKF重置条件（如持续大残差）
- 实现状态快照保存与恢复
我在某汽车工厂AGV系统中实施的方案性能指标：
- 定位精度：0.15m（室内），0.3m（室外）
- 更新频率：50Hz（里程计）+1Hz（GPS）+5Hz（UWB测距）
- 计算耗时：<2ms/次（i5-8250U处理器）
