卡尔曼滤波原理_【机器人位置估计】卡尔曼滤波的原理与实现

项目地址

robotics_tutorial_for_zhihu​github.com

1.前言

前面三节我们分别讨论并以最基础的形式实现了PRM、RRT以及人工势场法三种机器人运动规划方法，这三种方法属于机器人运动规划（motion planning）中的路径规划（path planning）方法。

而这一章讲到的卡尔曼滤波方法有什么用呢？举个例子，前面的路径规划方法中，我们总是从当前位置（起始点）开始向目标点进行规划。但在实际应用中，我们要如何确定机器人的当前位置呢？

物理知识告诉我们，物体的位置

由速度、加速度和时间决定。对于轮式机器人，如果速度指令是已知的，那么原则上可以通过积分得到机器人的当前位置信息。但这样做有一个问题，由于存在外部干扰（风阻、地面打滑），我们发出的速度指令（或者测得轮子的转速）并不一定是机器人的真实运动速度，它们之间存在着一定的误差。如果通过积分得到位置信息，那么误差也会相应累积，如图 1 所示：

图 1 通过速度积分得到位置信息，随着误差累积，机器人的估计位置与实际位置逐渐偏离【1】

为了校正这种误差，我们通常会引入额外的传感器信息。例如，对上图来说，引入GPS定位信息就是一个不错的选择。这时又出现了问题：速度积分和GPS传感器包含有位置信息，那我们究竟该采信哪边的结果呢？如果两边的信息都不想浪费，该怎么把它们进行融合呢？实际上，无论是速度积分得到的位置信息，还是GPS得到的位置信息，都存在一定的误差（GPS也存在定位精度的问题）。而在机器人的实际应用场景中，可能还存在红外、激光、视觉等更多类型的传感器，都可以以一定精度返回机器人的位置信息。那么我们该如何综合利用这些信息（传感器信息融合）来估计机器人当前的位置信息呢，这里就要用到卡尔曼滤波了。

2.卡尔曼滤波的基本公式

关于卡尔曼滤波的公式及理解，可参考我的另一篇文章:

小明工坊：【传感器融合】扩展卡尔曼滤波的逐步理解与实现（上）​zhuanlan.zhihu.com

另外，B站有一则简短的讲解视频：

卡尔曼滤波器的原理​www.bilibili.com

还有一篇国外的博文图文并茂、十分详尽易懂，当然网上也有许多翻译版本：

How a Kalman filter works, in pictures​www.bzarg.com

国外博文的翻译版​blog.csdn.net

强烈建议大家先看完以上资料，这里简要总结其中的结论：

我们用状态向量

来表示机器人的当前状态。例如对轮式机器人，其状态取位置

和速度

：