3. 状态估计与定位：卡尔曼滤波原理、扩展卡尔曼滤波、粒子滤波在避障中的应用
做避障导航，有个核心问题绕不开：机器人在哪儿？
你想想看，传感器给的数据，比如激光雷达、里程计、IMU，哪个没噪声？里程计跑久了会漂，激光雷达偶尔跳点，IMU零偏慢慢变。如果直接拿这些原始数据去规划路径，那结果就是——机器人要么撞墙，要么原地打转。
我当年做AGV（自动导引车）项目时，就吃过这个亏。里程计积分半小时，定位误差直接飙到半米。后来才明白，状态估计才是避障的基石。今天咱们就聊聊三种最常用的滤波方法：卡尔曼滤波、扩展卡尔曼滤波、粒子滤波。
3.1 卡尔曼滤波：线性系统的最优估计
卡尔曼滤波，说白了就是用预测+观测，算出最靠谱的状态。它假设系统是线性的，噪声是高斯分布。
核心就两个步骤：
- 预测（Predict）：根据上一时刻的状态和控制量，猜一下当前状态。
- 更新（Update）：用传感器观测值，修正这个猜测。
数学上，它维护两个东西：状态向量 x 和协方差矩阵 P。协方差代表不确定性——嗯，你可以理解为“我有多信这个估计”。
// 预测
x_pred = A * x_prev + B * u
P_pred = A * P_prev * A^T + Q
// 更新
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_est = x_pred + K * (z - H * x_pred)
P_est = (I - K * H) * P_pred这里 Q 是过程噪声协方差，R 是观测噪声协方差。我习惯把 Q 调得稍微大一点，让滤波器更相信观测——尤其是在避障场景下，传感器数据往往比模型更可靠。
R（用传感器手册的标称值），再调 Q。如果滤波结果太“抖”，说明 Q 偏大；如果太“滞后”，说明 Q 偏小。
3.2 扩展卡尔曼滤波：搞定非线性
现实中的避障系统，哪有那么多线性关系？
比如，机器人用激光雷达测距，距离和位置的关系是 r = sqrt(x² + y²)——这是非线性。再比如，航向角的变化用 cos、sin 算，也是非线性。
标准卡尔曼滤波搞不定。怎么办？扩展卡尔曼滤波（EKF） 登场。
EKF 的思路很简单：把非线性函数在估计点附近做一阶泰勒展开，也就是求雅可比矩阵。然后，用这个线性近似后的模型，套用标准卡尔曼滤波的框架。
举个例子，避障中常见的运动模型（恒定转弯率）：
// 状态向量: [x, y, theta, v, omega]
// 预测函数 f:
x' = x + v * cos(theta) * dt
y' = y + v * sin(theta) * dt
theta' = theta + omega * dt
v' = v
omega' = omega
// 雅可比矩阵 A（对状态求偏导）：
A = [[1, 0, -v*sin(theta)*dt, cos(theta)*dt, 0],
[0, 1, v*cos(theta)*dt, sin(theta)*dt, 0],
[0, 0, 1, 0, dt],
[0, 0, 0, 1, 0],
[0, 0, 0, 0, 1]]EKF 在避障中很实用。比如，用激光雷达观测到障碍物的距离和角度，EKF 可以同时估计机器人的位姿和障碍物的位置——这就是联合状态估计。不过要注意，EKF 假设噪声是高斯分布，如果遇到非高斯噪声（比如激光雷达的离群点），效果会打折扣。
3.3 粒子滤波：不惧非高斯、非线性
如果系统是非线性的，噪声也不是高斯分布——比如，机器人被突然推了一下，或者传感器出现多峰分布——EKF 就有点力不从心了。
这时候，粒子滤波（Particle Filter） 是个好选择。
粒子滤波的核心思想：用一堆带权重的随机样本（粒子）来近似状态的后验概率分布。粒子越多，近似越精确。
流程大致是：
- 初始化：在状态空间撒 N 个粒子，权重均匀。
- 预测：每个粒子根据运动模型加噪声，往前推一步。
- 更新：用观测值计算每个粒子的似然，更新权重。
- 重采样：根据权重重新抽取粒子，权重高的多复制，权重低的丢弃。
- 回到步骤2，循环。
在避障场景中，粒子滤波特别适合全局定位。比如，机器人被搬到陌生环境，不知道自己在哪——撒几千个粒子，覆盖整个地图。随着传感器数据进来，粒子会逐渐收敛到真实位置。
| 场景 | 推荐粒子数 | 说明 |
|---|---|---|
| 全局定位（2D） | 1000 ~ 5000 | 环境越大，粒子越多 |
| 局部跟踪（2D） | 100 ~ 500 | 已知大致位置，只需微调 |
| 3D定位 | 5000 ~ 20000 | 维度灾难，粒子数指数增长 |
3.4 三种滤波器的对比与选型
做项目时，到底用哪个？我列个表，你一看就明白：
| 滤波器 | 适用场景 | 优点 | 缺点 |
|---|---|---|---|
| KF | 线性系统，高斯噪声 | 计算快，最优解 | 太理想，现实很少用 |
| EKF | 弱非线性，高斯噪声 | 成熟，计算量适中 | 雅可比难求，非高斯易发散 |
| PF | 强非线性，非高斯噪声 | 通用性强，多峰分布 | 计算量大，维度灾难 |
我的建议是：能用 EKF 就别上 PF。EKF 计算量小，实时性好。只有在 EKF 明显失效时（比如定位丢失、多峰分布），才考虑粒子滤波。
3.5 避障中的实战案例
说个我实际做过的案例吧。
一台差速轮式机器人，在仓库里做避障。传感器有：轮式里程计（低频、有滑移）、IMU（高频、有零偏）、激光雷达（精确但偶尔丢帧）。
我用了扩展卡尔曼滤波做传感器融合：
- 预测步：用里程计+IMU的角速度做运动模型。
- 更新步：用激光雷达的匹配结果（比如ICP）做观测。
效果怎么样？定位误差从原来的30cm降到了5cm以内。避障路径平滑多了，不再出现急转弯。
但有一次，机器人经过一面大玻璃墙，激光雷达直接穿透了——观测值全错。EKF 的定位瞬间漂了。怎么办？
我加了一个异常检测：计算观测残差的马氏距离，如果超过阈值，就跳过这次更新。说白了，就是“我不信你”。这个技巧在避障中非常实用。
3.6 知识体系总览
下面这张图，帮你理清三种滤波器的关系和适用边界：
这张图的核心逻辑：从简单到复杂。先问系统是否线性、噪声是否高斯。如果是，用 KF 最省事。如果不是，再看非线性程度——弱非线性用 EKF，强非线性或非高斯噪声，才上 PF。
好了，状态估计这块就聊到这儿。记住一句话：没有最好的滤波器，只有最合适的。选型时，多想想你的传感器特性、计算资源、实时性要求。
可可精品资料库