3. 状态估计与滤波：卡尔曼滤波原理，扩展卡尔曼滤波（EKF），粒子滤波，状态估计在机器人定位中的应用
各位同学，今天我们来聊聊机器人定位里最核心的一块——状态估计与滤波。
说实话，我刚入行那会儿，觉得定位嘛，GPS+IMU不就够了？直到我在一个仓库AGV项目里，车子过了一个金属货架，里程计瞬间漂了半米，GPS又被遮挡……嗯，从那以后，我老老实实把卡尔曼滤波啃透了。
你想想看，机器人身上全是传感器——激光雷达、轮式里程计、IMU、视觉相机。每个传感器都有噪声，每个测量都不完美。状态估计要做的，就是把这些“不靠谱”的信息揉在一起，猜出机器人最可能的位置。
核心思想：用数学模型预测状态，用传感器测量修正预测。说白了，就是“猜一个，测一个，再修正一个”。
3.1 卡尔曼滤波原理
卡尔曼滤波，我愿称之为“线性系统的最优滤波器”。它假设系统是线性的，噪声是高斯白噪声。这两个假设很关键——现实世界不完美，但卡尔曼滤波在满足条件时，效果惊人。
它的工作流程分两步：
- 预测（Predict）：根据上一时刻的状态和控制输入，预测当前状态和协方差。
- 更新（Update）：用当前传感器测量值，修正预测结果。
公式其实不复杂，我习惯这样记：
# 预测步骤
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这里有个关键点——卡尔曼增益 K。它决定了你更相信预测还是测量。K 大，就多信传感器；K 小，就多信模型。我在调参时，经常盯着 K 的变化曲线，看它是否收敛。
我的小技巧：刚开始调卡尔曼滤波时，把 Q（过程噪声协方差）和 R（测量噪声协方差）设成对角矩阵，先调对角线元素。Q 越大，系统越“灵活”；R 越大，测量越“不可信”。
3.2 扩展卡尔曼滤波（EKF）
现实世界哪有那么多线性系统？机器人运动是非线性的，传感器观测也是非线性的。这时候，EKF 就派上用场了。
EKF 的思路很简单：把非线性函数在估计点附近做一阶泰勒展开，也就是线性化。然后套用标准卡尔曼滤波的框架。
我曾经在一个两轮差速机器人项目里用过 EKF。运动模型是：
x_k = x_{k-1} + v * dt * cos(theta)
y_k = y_{k-1} + v * dt * sin(theta)
theta_k = theta_{k-1} + omega * dt你看，这明显是非线性的（有 cos、sin）。EKF 的做法是：对状态向量求雅可比矩阵，得到线性近似。
但这里有个坑——线性化误差。如果系统非线性太强，或者初始误差太大，EKF 很容易发散。我记得有一次调试，机器人转了个急弯，EKF 直接飞到了地图外面……
避坑指南：我曾经在 EKF 初始化时，把协方差矩阵 P 设得太小，导致滤波器“过于自信”，后续测量根本拉不回来。建议 P 初始值设大一些，比如单位矩阵的 10 倍，让滤波器先“探索”一下。
3.3 粒子滤波
如果系统是非线性的，而且噪声不是高斯分布呢？EKF 就力不从心了。这时候，粒子滤波登场。
粒子滤波的思想很直观：用一堆随机粒子（样本）来近似状态的后验分布。每个粒子代表一个可能的状态，权重代表这个状态的可信度。
流程是这样的：
- 初始化：在状态空间撒一堆粒子，权重均匀。
- 预测：每个粒子根据运动模型移动，加上随机噪声。
- 更新：根据传感器测量，计算每个粒子的权重（似然）。
- 重采样：根据权重重新采样，权重高的粒子多复制，权重低的被淘汰。
我个人觉得，粒子滤波最大的优点是能处理多模态分布。比如机器人可能在大厅的左边或右边，粒子滤波可以同时保持这两种假设。而卡尔曼滤波只能假设一个高斯峰。
但代价是计算量大。粒子数太少，精度不够；粒子数太多，实时性堪忧。我在一个室内定位项目里，用了 500 个粒子，在树莓派上勉强跑到 20Hz。
经验之谈：粒子数不是越多越好。我建议从 100 个粒子开始，逐步增加，直到定位精度不再明显提升为止。另外，重采样策略也很关键——我习惯用“系统重采样”，比简单随机重采样更稳定。
3.4 状态估计在机器人定位中的应用
好了，理论讲完了，咱们看看实际怎么用。
一个典型的机器人定位系统，会融合多种传感器：
| 传感器 | 测量内容 | 特点 |
|---|---|---|
| 轮式里程计 | 位移、速度 | 短期准，长期漂 |
| IMU | 加速度、角速度 | 高频，但有零偏 |
| 激光雷达 | 距离、点云 | 精度高，但计算量大 |
| GPS | 经纬度 | 室外可用，室内失效 |
我常用的方案是：EKF 做核心融合，里程计+IMU 做预测，激光雷达或 GPS 做观测更新。如果环境特征不明显（比如长走廊），我会切换到粒子滤波，因为它能保持多个假设，等机器人看到新特征时再收敛。
下面是我画的一个状态估计流程图，帮你理清思路：
你看这个流程，其实就是个闭环：预测→更新→输出→反馈。每次迭代，状态的不确定性都在减小——前提是你的模型和噪声参数没设错。
总结一下我的经验：
- 系统简单、线性好 → 用卡尔曼滤波，又快又稳
- 系统轻度非线性 → 用 EKF，注意雅可比矩阵别算错
- 系统高度非线性、多模态 → 用粒子滤波，但注意计算资源
- 永远记得调参！Q、R、P 这三个矩阵，调好了是神器，调不好是灾难
最后说一句，状态估计不是一锤子买卖。你在实际项目中，很可能需要根据传感器特性、环境变化、算力限制，动态切换滤波算法。我那个仓库 AGV 项目，最后用了两层滤波——上层 EKF 做全局定位，下层粒子滤波做局部跟踪。效果嘛，反正再也没撞过货架。
一个小练习：找一台带里程计和激光雷达的机器人，先跑纯里程计，看看漂移有多快。然后加上卡尔曼滤波融合激光雷达，对比定位精度。你会发现，滤波前后的差距，就像近视眼戴上了眼镜。
公众号:蓝海资料掘金营，微信deep3321