SLAM系列(三)：扩展卡尔曼滤波SLAM理论部分b

2019-11-09 本文已影响0人陈瓜瓜_ARPG

又过了好久了...Em...上一讲我们不甚详细地提及了EKF扩展卡尔曼滤波，这一讲我们就来说扩展卡尔曼滤波在SLAM里的应用。需要再次提及的是EKF-SLAM是比较早期的应用，但是因为简单好懂，所以用来帮助我们来熟悉一下SLAM的过程。
一旦要使用扩展卡尔曼滤波，我们就需要两个重要模型，一个是过程模型(Process Model)一个是测量模型(Measurement Model)。过程模型在SLAM里用来描述机器人从当前时刻到下一时刻的运动状态是怎么关联起来的，而测量模型是某个时刻传感器获取的机器人的状态。
我们首先讲过程模型，再<Probabilistic Robotics>里主要提及了两种2维的机器人过程模型。一种是里程计模型(Odometry Model)另一种是(Velocity Model)。考虑到篇幅原因以及网上有关于Velocity Model的数据集可以使用
http://asrl.utias.utoronto.ca/datasets/mrclam/index.html
我们就只在讲解Velocity Model。
现在假设可以做2D SLAM的机器人像下面这个样子(简书的大毛病，先显示了图片之后，图片后面的公式就显示不了，无奈只有给出图片链接，大家点开链接看一下就行)。
robot.png
是常见的扫地机器人的样子。
我们假设机器人上有下面的这两个传感器，速度传感器可以测得当前的速度，类似于IMU原件(不知道IMU的可以去百度一下)的传感器可以测得当前的角速度。大家应该都看过扫地机器人是怎么运动的，如果没看过网上也有很多视频，它可以沿某个方向直线运动，或者在转动一定角度运动。它的某一时刻的运动状态可以表示为 $(x,y,\theta)$ ，即当前的位置和方向。其运动图为下面所示
coordinate_vm.png
这来源于<Probabilistic Robotics>老板的图5.5，个人添加了右边的坐标系原点 $X_0, Y_0$ 便于推导。
图中粗黑圆圈就代表机器人了，圆心的坐标是 $x,y$ ,从圆心到圆边有一条粗黑线连接，这个粗黑线代表当前瞬时机器人的运动方向。带箭头的曲线代表机器人转弯的朝向。垂直于机器人的运动方向从圆心连接了一条线到 $x_c,y_c$ ，这是机器人的专向圆心， $r$ 就是其转向半径。我想我们高中应该学过转向半径等于当前的切线速度除以当前的角速度(诶，学过吗？)
$r = |\frac{v}{\omega}| \tag{1}$
大家自己稍加推导，应该不难得到转向圆心和机器人圆心之间的关系
$x_c = x - \frac{v}{\omega}sin(\theta) \\ y_c = y + \frac{v}{\omega}cos(\theta) \tag{2}$
基于这个公式，假设机器人在 $\Delta t$ 时间内速率不变，角速度不变，其下一时刻的位置和角度就应该是
$\begin{bmatrix} x_{k} \\ y_{k} \\ \theta_{k} \\ \end{bmatrix} = \begin{bmatrix} x_{k-1} + \frac{v}{\omega} sin(\theta + \omega \Delta t)\\ y_{k-1} - \frac{v}{\omega} cos(\theta + \omega \Delta t)\\ \theta_{k-1} + \omega \Delta t\\ \end{bmatrix}$
把式子2带入到上面的式子变化一下就可以得到
$\begin{bmatrix} x_{k} \\ y_{k} \\ \theta_{k} \\ \end{bmatrix} = \begin{bmatrix} x_{k-1} \\ y_{k-1} \\ \theta_{k-1} \\ \end{bmatrix} + \begin{bmatrix} -\frac{v}{\omega}sin(\theta_{k-1}) + \frac{v}{\omega} sin(\theta_{k-1} + \omega \Delta t)\\ \frac{v}{\omega}cos(\theta_{k-1}) - \frac{v}{\omega} cos(\theta_{k-1} + \omega \Delta t)\\ \omega \Delta t\\ \end{bmatrix} \tag{3}$
这就是无噪音版本的运动模型了。也就是我们在卡尔曼滤波代码时要用到的模型，用于上一时刻到下一时刻的状态估计。加上噪音的运动模型比较复杂，我们就不写在这里了。通过上一讲我们应该了解到在实施卡尔曼滤波的时候我们写入代码的运动模型就是没有噪音的版本。
$\begin{bmatrix} \widehat{x}_{k|k-1} \\ \widehat{y}_{k|k-1} \\ \widehat{\theta}_{k|k-1} \\ \end{bmatrix} = \begin{bmatrix} \widehat{x}_{k-1|k-1} \\ \widehat{y}_{k-1|k-1} \\ \widehat{\theta}_{k-1|k-1} \\ \end{bmatrix} + \begin{bmatrix} -\frac{v}{\omega}sin(\widehat{\theta}_{k-1|k-1}) + \frac{v}{\omega} sin(\widehat{\theta}_{k-1|k-1} + \omega \Delta t)\\ \frac{v}{\omega}cos(\widehat{\theta}_{k-1|k-1}) - \frac{v}{\omega} cos(\widehat{\theta}_{k-1|k-1} + \omega \Delta t)\\ \omega \Delta t\\ \end{bmatrix} \tag{4}$
有了运动模型，即有了扩展卡尔曼滤波五步中的第一步，接下来就需要进行卡尔曼滤波的第二步，获得运动模型的雅各比矩阵，通过下面的式子(第二篇的式(6))获得。
$\mathbf{F}_k = \frac{\partial \mathbf{f}}{\partial \mathbf{x}} = \begin{align} \begin{bmatrix} \frac{f_1}{x_1} & \frac{f_1}{x_2} & \cdots & \frac{f_1}{x_n} \\ \frac{f_2}{x_1} & \frac{f_2}{x_2} & \cdots & \frac{f_2}{x_n} \\ \vdots & \vdots & \vdots & \vdots \\ \frac{f_n}{x_1} & \frac{f_n}{x_2} & \cdots & \frac{f_n}{x_n} \end{bmatrix} \end{align}$
对应于此处 $(x_1, x_2, x_3) = (x,y,\theta) \\ f_1 = x -\frac{v}{\omega}sin(\theta) + \frac{v}{\omega} sin(\theta + \omega \Delta t)\\ f_2 = y + \frac{v}{\omega}cos(\theta) - \frac{v}{\omega} cos(\theta + \omega \Delta t) \\ f_3 = \theta + \omega \Delta t$
那么你带入求导就可以得到
$\mathbf{F}_k = \mathbf{I} + \begin{align} \begin{bmatrix} 0 & 0 & -\frac{v}{\omega}cos(\widehat{\theta}_{k-1|k-1}) + \frac{v}{\omega} cos(\widehat{\theta}_{k-1|k-1} + \omega \Delta t) \\ 0 & 0 & -\frac{v}{\omega}sin(\widehat{\theta}_{k-1|k-1}) + \frac{v}{\omega} sin(\widehat{\theta}_{k-1|k-1} + \omega \Delta t)\\ 0 & 0 & 0 \end{bmatrix} \end{align} \tag{5}$
其中 $\mathbf{I}$ 是3x3的单位矩阵。在扩展卡尔曼滤波的第二步我们还有另外两个量需要确定，一个是误差协方差矩阵 $\mathbf{P}_k$ ，另一个是过程噪音矩阵 $\mathbf{Q}_k$ 。在设置了 $\mathbf{P}_0$ 之后， $\mathbf{P}_k$ 就可以根据算法迭代一直更新下去， $\mathbf{Q}_k$ 我们则根据卡尔曼滤波的表现来设置为一个值固定的矩阵。正如前一讲所说，这两个值都靠猜（根据滤波器的表现，比如根据真实值和滤波器的估计值之间的差别来调整这两个矩阵，这是很麻烦的事情)。我们后面写代码时直接给出这两个值。
接下来是卡尔曼滤波的第三步更新卡尔曼增益，这一步里涉及到三个变量 $\mathbf{P}$ 来自于第二步，测量噪音协方差矩阵 $\mathbf{R}_k$ 根据传感器提供的测量误差等信息进行更合理一些的猜测。而观察(测量)模型的雅各比矩阵则需要我们先对观察模型有一个了解。我们测量的是路标相对于机器人的角度和距离。
路标为固定在环境中的点,维度为2，即它的横纵坐标 $x,y$ ，用 $\mathbf{m}_{j} = (m_{j,x}, m_{j,y})$ 表示， $j$ 为坐标点的序号。根据机器人自己的位置和角度，我们可以很容易获得它相对于某一个路标的角度和距离
$\mathbf{z}_{k,j} = \begin{bmatrix} \sqrt{(m_{j,x} - x_{k})^2 + (m_{j,y} - y_k)^2} \\ atan2((m_{j,y} - y_k), (m_{j,x} - x_k) ) - \theta_k \end{bmatrix}$
矩阵的第一行明显就是距离了，第二行 $atan2((m_{j,y} - y_k), (m_{j,x} - x_k) )$ 其实就是直角三角形两个非斜边的比值再求个逆三角函数得到角度，最后减去机器人原来的角度得到相对角度。至于为什么用 $atan2$ (arctan2)而不是 $atan$ 自行百度。
无噪音版本的测量模型对应到卡尔曼滤波中，就应该是
$\widehat{\mathbf{z}}_{k,j} = \begin{bmatrix} \sqrt{(\hat{m}_{j,x} - \hat{x}_{k|k-1})^2 + (\hat{m}_{j,y} - \hat{y}_{k|k-1})^2} \\ atan2((\hat{m}_{j,x} - \hat{y}_{k|k-1})), (\hat{m}_{j,x} - \hat{x}_{k|k-1}) ) -\hat{\theta}_{k|k-1} \end{bmatrix} \tag{6}$
式子中的 $\hat{y}_{k+1|k}, \hat{x}_{k+1|k}, \hat{\theta}_{k+1|k}$ 来自于式4。注意这些符号都加了帽子符号' $\hat{}$ '是因为他们都是估计值。
为了书写简便，令 $\delta_x = \hat{m}_{j,x} - \hat{x}_{k+1|k}, \delta_y = \hat{m}_{j,y} - \hat{y}_{k+1|k},$
$\delta = \begin{bmatrix} \delta_x \\ \delta_y \end{bmatrix} \\ q = \delta^T\delta$
代入6式则有
$\widehat{\mathbf{z}}_k = \begin{bmatrix} \sqrt{q} \\ atan2(\delta_y, \delta_x ) -\hat{\theta}_{k+1|k} \end{bmatrix}$
接下来就是要求测量的雅各比矩阵了。我们需要把它同时对机器人的状态 $\hat{x}_{k+1|k}, \hat{y}_{k+1|k}, \hat{\theta}_{k+1|k}$ 和路标(landmark)的位置 $\hat{m}_{j,x}, \hat{m}_{j,y}$ 求导。这几个是我们需要知道的变量。即 $\sqrt{q}$ 和 $atan2(\delta_y, \delta_x ) -\hat{\theta}_{k+1|k}$ 分别对这5个量求导，求得一个2X5的矩阵。过程就不写了，用链式法则很容易求
$\mathbf{H}_{k,j} = \frac{1}{q_j} \begin{bmatrix} -\sqrt{q_j}\delta_{x,j} & -\sqrt{q_j}\delta_{y,j} & 0 & \sqrt{q_j}\delta_{x,j} & \sqrt{q_j}\delta_{y,j} \\ \delta_{y,j} & -\delta_{x,j} & -q_j & -\delta_{y_j} &\delta_{x,j} \end{bmatrix} \tag{7}$
前三列为对机器人状态量求导，后两列为对路标求导。
至此，有运动模型，测量模型以及雅各比矩阵，回顾第二讲最后的完整EKF的式子，只要我们设置了误差协方差矩阵的初始值 $\mathbf{P}_{0|0}$ ，卡尔曼滤波便可以根据那5个公式一步一步迭代下去了。
但是呢，其实还有一些事情没做。在SLAM里，我们想要测量的量不仅仅是机器人的位置和角度，我们也想要测量路标的位置。所以说真正的需要更新的状态量是
$\mathbf{\widehat{x}_{k|k}} = \begin{bmatrix} \widehat{x}_{k|k} \\ \widehat{y}_{k|k} \\ \widehat{\theta}_{k|k} \\ \hat{m}_{0,x,k|k} \\ \hat{m}_{0,y,k|k} \\ \vdots \\ \hat{m}_{n,x,k|k} \\ \hat{m}_{n,y,k|k} \\ \end{bmatrix} \tag{8}$
其中 $n$ 代表目前观察到的路标的总数量。所以其实状态量不是3个而是 $2n+3$ 。注意probabilistic的书里(2005版本)状态量维度是 $3n+3$ ，这是因为它给每个路标都考虑了一个额外的变量(177页,6.62以及313页10.2.2)，比如路标的颜色不同，形状不同等等，这里为了简便我们不考虑那个量。这里符号的标注有些复杂了，望大家弄懂每个角标的意思。这里冗余地重复一下。以 $\hat{m}_{0,x,k|k}$ 为例， $0$ 表示当前路标的标号， $x$ 是当前路标的横坐标， $k|k$ 表示当前路标在卡尔曼滤波中第 $k$ 步最终得到的更新值。
这样EKFSLAM中的过程模型就该写为
$\begin{bmatrix} \widehat{x}_{k|k-1} \\ \widehat{y}_{k|k-1} \\ \widehat{\theta}_{k|k-1} \\ \hat{m}_{0,x,k|k-1} \\ \hat{m}_{0,y,k|k-1} \\ \vdots \\ \hat{m}_{n,x,k|k-1} \\ \hat{m}_{n,y,k|k-1} \\ \end{bmatrix} = \begin{bmatrix} \widehat{x}_{k-1|k-1} \\ \widehat{y}_{k-1|k-1} \\ \widehat{\theta}_{k-1|k-1} \\ \hat{m}_{0,x,k-1|k-1} \\ \hat{m}_{0,y,k-1|k-1} \\ \vdots \\ \hat{m}_{n,x,k-1|k-1} \\ \hat{m}_{n,y,k-1|k-1} \\ \end{bmatrix} + \begin{bmatrix} -\frac{v}{\omega}sin(\widehat{\theta}_{k-1|k-1}) + \frac{v}{\omega} sin(\widehat{\theta}_{k-1|k-1} + \omega \Delta t)\\ \frac{v}{\omega}cos(\widehat{\theta}_{k-1|k-1}) - \frac{v}{\omega} cos(\widehat{\theta}_{k-1|k-1} + \omega \Delta t)\\ \omega \Delta t\\ 0 \\ 0 \\ \vdots \\ 0 \\ 0 \\ \end{bmatrix} \tag{9}$
简写为
$\mathbf{\widehat{x}}_{k|k-1} = g(\mathbf{\widehat{x}}_{k-1|k-1}, \mathbf{u}_k) \tag{10}$
过程模型的前三个量也就是机器人的状态模型的更新就是式子4。其余的路标的位置在这一步不更新，因为我们的假设是路标不会动的。与此对应的雅各比矩阵为
$\mathbf{G}_k = \begin{bmatrix} \mathbf{F}_k & \mathbf{0}^{3*2n}\\ \mathbf{0}^{2n*3} & \mathbf{I}^{2n*2n} \\ \tag{11} \end{bmatrix}$
其中 $\mathbf{F}_k$ 为式子5, $\mathbf{I}^{2n*2n}$ 表示 $2n$ 行， $2n$ 列的单位矩阵。求导数的过程仍然按照第二篇文章的式子6。读者可以自行求导，不会的可以私信。其实就是在原来的基础之上多了路标的求导，路标位置本身和机器人位置没有关系，对 $x,y,\theta$ 求导为0，某个路标位置和其他路标没有关系， $\hat{m}_{0,x,k-1|k-1}$ 只有对自身求导才得到1，对其他路标求导都是0.
接下来同样，由于我们当前观察到的路标有 $n$ 个，所以类似于式子7的雅各比矩阵应该有 $n$ 个。式6的测量模型更完整的写法就应该是
$\widehat{\mathbf{z}}_k = \begin{bmatrix} \sqrt{(\hat{m}_{0,x} - \hat{x}_{k|k-1})^2 + (\hat{m}_{0,y} - \hat{y}_{k|k-1})^2} \\ atan2((\hat{m}_{0,x} - \hat{y}_{k|k-1})), (\hat{m}_{0,x} - \hat{x}_{k|k-1}) ) -\hat{\theta}_{k|k-1}\\ \vdots \\ \sqrt{(\hat{m}_{n,x} - \hat{x}_{k|k-1})^2 + (\hat{m}_{n,y} - \hat{y}_{k|k-1})^2} \\ atan2((\hat{m}_{n,x} - \hat{y}_{k|k-1})), (\hat{m}_{n,x} - \hat{x}_{k|k-1}) ) -\hat{\theta}_{k|k-1}\\ \end{bmatrix} = \begin{bmatrix} \widehat{\mathbf{z}}_{k,0}\\ \vdots \\ \widehat{\mathbf{z}}_{k,n}\\ \end{bmatrix} \tag{12}$
该式子有2n行。把上式对状态量式8求导，可以得到完整的测量雅各比矩阵，维数为 $2n*(2n+3)$ 。
$\mathbf{H}_k = \frac{1}{q_j} \begin{bmatrix} -\sqrt{q_0} \delta_{x,0} & -\sqrt{q_0}\delta_{y,0} & 0 & \mathbf{0}^{1*2*0} & \sqrt{q_0}\delta_{x,0} & \sqrt{q_0}\delta_{y,0} & \mathbf{0}^{1*(2n-2)}\\ \delta_{y,0} & -\delta_{x,0} & -q_0 & \mathbf{0}^{1*2*0} & -\delta_{y_0} &\delta_{x,0} & \mathbf{0}^{1*(2n-2)}\\ \vdots & \vdots &\vdots & \vdots &\vdots& \vdots & \vdots\\ -\sqrt{q_0} \delta_{x,0} & -\sqrt{q_0}\delta_{y,0} & 0 & \mathbf{0}^{1*2j} & \sqrt{q_j}\delta_{x,j} & \sqrt{q_j}\delta_{y,j} & \mathbf{0}^{1*(2n-2j)}\\ \delta_{y,j} & -\delta_{x,j} & -q_j & \mathbf{0}^{1*2j} & -\delta_{y_j} &\delta_{x,j} & \mathbf{0}^{1*(2n-2j)}\\ \vdots & \vdots &\vdots & \vdots &\vdots& \vdots & \vdots\\ -\sqrt{q_n} \delta_{x,n} & -\sqrt{q_n}\delta_{y,n} & 0 & \mathbf{0}^{1*(2n-2)} & \sqrt{q_n}\delta_{x,n} & \sqrt{q_n}\delta_{y,n} & \mathbf{0}^{1*2*0}\\ \delta_{y,n} & -\delta_{x,n} & -q_n &\mathbf{0}^{1*(2n-2)} &-\delta_{y_n} &\delta_{x,n} & \mathbf{0}^{1*2*0}\\ \end{bmatrix} \tag{13}$
这个式子看起来贼复杂。其实就是公式12对式8求导。 $\mathbf{0}^{1*2j}$ 表示1行 $2j$ 列的0向量。每一个路标的测量 $\widehat{\mathbf{z}}_{k,j}$ 对机器人状态都是有导数的，所以式13的前三列都有数值，而路标只有对自身才有导数，对其他路标导数为0(他们没有任何关系)。
至此，我们完成了EKF-SLAM所需要的测量模型，过程模型以及相关的雅各比矩阵。
(未完待续....这公式写地怀疑人生)

SLAM系列(三)：扩展卡尔曼滤波SLAM理论部分b

猜你喜欢

热点阅读