基于扩展卡尔曼滤波(EKF)的机器人SLAM问题.pdf

《基于扩展卡尔曼滤波(EKF)的机器人SLAM问题.pdf》由会员上传分享，免费在线阅读，更多相关内容在学术论文-天天文库。

1、基于扩展卡尔曼滤波（EKF）的机器人SLAM问题AlanLee机器人研究中的SLAM问题•SLAM是一个先有鸡、还是先有蛋的问题:→先得到地图，才能确定当前的精确位置。→为了建立地图，必须得知现在的精确位置。2机器人SLAM问题的应用室内机器人水下机器人导航定位导航定位太空机器人地下机器人导航定位导航定位3SLAM的难点1SLAM:机器人的路径和地图都未知机器人路径错误与地图中的错误相关4SLAM的难点2机器人位置未知•在现实世界中，观测和地标之间的映射是未知的•测量的观测存在误差•位姿误差与数据误差关联5机器人的SLAM描

2、述已知:•机器人的控制量：•传感器的测量值：z如：里程计的距离求解:•地图的信息•机器人的路径SLAM描述•全局SLAM:p(x1:t,m

3、z1:t,u1:t)估计整个路径和地图•在线SLAM:p(xt,m

4、z1:t,u1:t)=òò…òp(x1:t,m

5、z1:t,u1:t)dx1dx2...dxt-1只估计最新的路径和地图7SLAM问题数学抽象不论采用任何传感器，激光也好，视觉也好，整个SLAM系统也好，要解决的问题只有一个：如何通过数据来估计自身状态。每种传感器的测量模型不一样，它们的精度也不一样。换句话说，状态估计问题

6、，也就是“如何最好地使用传感器数据”。SLAM是状态估计的一个特例。8离散时间系统的状态估计记机器人在各时刻的状态为，其中k是离散时间下标在SLAM中，我们通常要估计机器人的位置，那么系统的状态就指的是机器人的位姿。用两个方程来描述状态估计问题：运动方程描述了状态是怎么变到，而观测方程描述的是从是怎么得到观察数据9状态估计方程的具体化当有实际的机器人，实际的传感器时，方程的形式会更加具体，也就是所谓的参数化。当我们关心机器人空间位置时，可以取当机器人携带了里程计，能够得到两个时间间隔中的相对运动，即那么运动方程就

7、变为观测方程也随传感器的具体信息而变。例如激光传感器可以得到空间点离机器人的距离和角度，记为那么观测方程为：10线性高斯系统（1）根据是否线性，把它们分为线性/非线性系统。同时，对于噪声根据它们是否为高斯分布，分为高斯/非高斯噪声系统。最一般的，也是最困难的问题，是非线性-非高斯(NLNG,Nonlinear-NonGaussian)的状态估计。下面分析线性高斯系统。在线性高斯系统中，运动方程、观测方程是线性的，且两个噪声项服从零均值的高斯分布。这是最简单的情况。高斯分布经过线性变换之后仍为高斯分布。而对于一个高斯分布，只要

8、计算出它的一阶和二阶矩，就可以描述它（高斯分布只有两个参数）线性系统形式如下：其中是两个噪声项的协方差矩阵11A,C为转移矩阵和观测矩阵线性高斯系统（2）用表示的后验概率，用表示它的先验概率。因为系统是线性的，噪声是高斯的，所以状态也服从高斯分布，需要计算它的均值和协方差矩阵。记时刻的状态服从：希望得到状态变量的后验估计（MAP，MaximizeaPosterior），于是计算：12线性高斯系统（3）现在的问题是如何求解这个最大化问题。对于高斯分布，最大化问题可以变成最小化它的负对数。当我对一个高斯分布取负对数时，它的指

9、数项变成了一个二次项，而前面的因子则变为一个无关的常数项，可以略掉（这部分我不敲了，有疑问的同学可以问）。于是，定义以下形式的最小化函数：那么最大后验估计就等价于：13线性高斯系统（4）写成矩阵形式就得到矩阵形式的，类似最小二乘的问题：14线性高斯系统（5）于是令它的导数为零，得到：卡尔曼滤波就是递推地求解上式的过程前两个是预测，第三个是卡尔曼增益，四五是校正。15扩展卡尔曼滤波(1)线性高斯系统是理想的，许多现实世界中的系统都不是线性的，状态和噪声也不是高斯分布的。例如上面举的激光观测方程就不是线性的。当系统为非线性的时候

10、，高斯分布经过非线性变换后，不再是高斯分布。而且，分布状况未知。如果没有高斯分布，上面说的那些都不再成立了。于是扩展卡尔曼滤波(EKF:ExtendedKalmanFilter)就是用高斯分布去近似它，并且，在工作点附件对系统进行线性化。16扩展卡尔曼滤波(2)EKF的做法主要有两点。其一，在工作点附近对系统进行线性化这里的几个偏导数，都在工作点处取值。至此，系统被简化为一个线性系统17扩展卡尔曼滤波(3)第二，在线性系统近似下，把噪声项和状态都当成了高斯分布。这样，只要估计它们的均值和协方差矩阵，就可以描述状态了。经

11、过这样的近似之后呢，后续工作都和卡尔曼滤波是一样的了。所以EKF是卡尔曼滤波在NLNG系统下的直接扩展。EKF给出的公式和卡尔曼是一致的，用线性化之后的矩阵去代替卡尔曼滤波器里的转移矩阵和观测矩阵即可。18其中EKF线性化图例19EKF一阶泰勒级数展开实现预测:¶g(u,m)tt-1g(u