机器人学在Python中的实现（9）：Ekf-Slam

Hello，大家好！我是MPIG2018级研究生刁金辉。今天我给大家讲解Ekf-Slam。前面介绍了利用EKF去估计机器人的位姿，但此刻估计的位姿可能与实际差距比较大，因而，我们不能单纯的依靠机器人运动估计机器人位置信息。在使用机器人运动方程得到机器人位置估计后，我们可以使用测距单元得到的周围环境信息更正机器人的位置。上述更正过程一般通过提取环境特征，然后在机器人运动后重新观测特征的位置实现。

我们先预览一下本章讲解的主要内容：

（1）初识Ekf-Slam

（2）Ekf-Slam算法大致过程

（3）代码示例演示

一.初识Ekf-Slam

该算法与前面介绍的EKF算法是并行的，但有一个关键的不同：除了估计机器人的位姿，EKF-slam算法还得估计路径中所遇到所有地标的坐标，也就是对环境的估计。

EKF-slam用的还是EKF的大体框架，预测与更新，首先根据运动模型预测下一时刻的位姿，因为机器人该时刻的位姿误差较大，所以通过对附近的地标进行观测以及预测，来更新机器人当前的位姿。

当机器人运动时，其位置将会发生变化。此时，根据机器人位置传感器的观测，提取得到观测信息中的特征点，然后机器人通过EKF将目前观测到特征点的位置、机器人运动距离、机器人运动前观测到特征点的位置相互结合，对机器人当前位置和当前环境信息进行估计。

系统状态是SLAM过程中最为重要的向量，包含了机器人的位置(x,y,\theta)以及环境中每个路标的信息，系统状态为一个(3+2*n)*1的矩阵，即列向量，其中n为路标的个数，包括了路标的x与y。

二.Ekf-Slam算法大致过程

在t-1时刻，机器人先使用测距单元测量附近的环境地标距离机器人的位置与角度。

然后机器人开始运动，当机器人到达t时刻的位置时，机器人根据运动方程预测在t时刻的位姿。

在t时刻的位置，机器人通过测距单元重新测量各个地标相对于机器人的距离和角度，测量得到的距离和角度与上述预测结果可能并不一致，因而，上述预测值可能并不是机器人准确位置。

在机器人看来，通过传感器获得的信息相对于通过运动方程得到的信息更为准确，因而，机器人将通过传感器的数据更新对机器人位置的预测值，如上图中实线三角形所示(虚线为第一步中通过运动信息预测的机器人位置)。

经过上述结合直轴，我们重新估计得到的新的机器人位置如上图实线三角形所示，但由于测距单元精度有限，因而，此时，机器人可能实际处于上图点状三角形位置，但此时估计结果相对于初始预测结果已经有明显的改善。

三. 代码示例演示

Ekf-slam算法的预测与更新

机器人首先利用运动模型预测t时刻机器人所处的位姿，同时在t时刻机器人会利用测距单元对附近的环境路标进行观测，同时利用运动模型的位姿对附近的环境路标进行预测观测值，算出信息差，从而求出卡尔曼增益，更新机器人的位置。

代码运行结果：

红色轨迹为机器人的最佳运动轨迹，从图中可以看出，机器人一直在纠正位姿，同时误差也是越来越小。黑色为运动模型轨迹，绿色原点为环境地标，绿色×为对环境路标的预测观测值。

想要更加详细了解本讲更多细节的内容吗？那就一起来观看下面的Presentation的具体讲解吧：