一个状态估计问题——机器人学中的状态估计(二)

type

status

date

slug

summary

从SLAM问题开始说起

在SLAM十四讲中，在第一部分(数学部分)中将SLAM中需要的知识分为三维空间刚体运动、李群李代数、相机模型和非线性优化四大部分。在第二部分中对又把一个SLAM系统划分为视觉里程计(几种里程计方法)、后端(优化方法)、回环三个部分。如果我们用一个比较工程的角度看这个SLAM系统，其实就是书中的第二部分的分类，也就是视觉里程计、后端优化和回环三个部分，其实回环是一个单独的可以剥离的部分。

第一部分的所有知识都是为了这三个部分来服务的，三维空间的刚体运动自然不用多说，用来描述一个空间运动信息，离不开这个数学工具；而相机模型是为了视觉里程计前端来服务的；剩下的李群李代数和非线性优化都是后端优化这个部分的。这篇博客就先给后端优化部分起一个头。

为什么这里要从后端优化开始呢？

对于分析一个问题，我们可以将问题分为我们目前有什么和要得到什么两个部分。从SLAM问题的结果来看，我们有的是环境的信息，这里有很多类型：视觉的、激光的、深度的等等；要得到的就是机器人的位姿。

这里简化一点，就拿一个激光SLAM来分析，我们有两帧激光信息，要得到机器人的位姿。那么不就是分析两帧激光点之间对应点的运动关系，反向计算出机器人的位姿。

但是实际上这个计算是有噪声的，位姿计算也不是准确的。所以我们就需要一个方法来排除掉这些噪声，得到一个真实的轨迹，这个过程就是优化的过程。也就是SLAM工程里面最核心、代码量最多的一个部分。

所以在一个SLAM工程中，他的两个部分其实是非常清晰，一个就是利用环境信息计算机器人当前的位姿，这也就是里程计部分。根据传感器的种类能够有很多的里程计：视觉、激光、轮速、惯性(IMU)等等。第二部分就是优化部分，就是把第一部分包含噪声的信息进行优化，得到一个相对最好的轨迹出来。

其实里程计部分是相对比较好理解的，解决的方法也比较多，任何一个传感器其实都能有现成的比较可靠的方法来获得短暂的一段时间的运动变化。那么现在这个SLAM问题就来到了一个新的困境，就是我现在有的不再是简单的环境的信息，而是一个有噪声的运动信息，那么我该怎么得到一个机器人的运动轨迹呢？这个部分就是优化部分，也就是状态估计问题。

一个优化问题

观测方程和运动方程的建立

既然我们之前将SLAM问题的核心看做一个优化问题，那么求解一个优化问题的核心就是要构建一个优化函数，然后求这个函数的最值。那么在SLAM问题中是怎么构建这个优化函数的呢？一般将一个SLAM问题看做两个方程，即运动方程和观测方程。

对于运动方程，其中的就是k时刻的位姿，是运动方程，是运动的输入，是运动方程的噪声。

对于观测方程，其中在观测的目标对象，是当前的位姿，是在当前位置对目标进行观测得到的观测量。那么观测方程的意义就是：在位置对一个对象进行一次观测，得到一个观测量。

其中和都是运动方程和观测方程的噪声。

贝叶斯形式的优化函数的构建

我们需要做的事情就是在已知当前的输入和当前的观测的情况下估计出当前的位置，同时我们假设运动方程的噪声和观测方程的噪声都符合高斯分布，那么这个优化函数以概率的形式来展示就非常的方便了。就是已知输入和观测情况下，求状态x的最大的概率分布。这个过程可以用贝叶斯推断的形式来表达：

因为和无关，所以我们在求解的时候可以忽略。整个表达形式可以写成：

那么求解最大的概率分布就是就是求解最大的：

但是这个形式还是比较抽象的，在实际中应该如何来应用这个理论呢？这里假设观测和运动信息的输入是独立的，也就是对于环境中一个物体的观测和我对机器人的运动输入是无关的(独立和无关在高斯分布中是相等的)，那么可以将之前的联合概率分布的形式写开成为:

PS:这里得出的结论和14讲的结论是一致的，只不过14讲的更加简单。

那么这个概率分布可以分为三个部分，对于第二部分。对于一个机器人的运动控制，我们给这个机器人发送一个的运动指令，这个运动方程中的噪声通常可能是控制器的不准确性、末端电机的精度或者轮胎的打滑现象等等。这里为了简化问题，把这个噪声看做是一个高斯分布的形式：,那么方程。而对于P(u)来说，虽然我们可以随意的给机器人一个运动信息，但是在SLAM问题中，一般会习惯性的将这个P(u)假设为等速运动模型，也就是输入的u一个不变的速度。

对于第一部分，也就是观测模型部分而言，一般会假设这样一个贝叶斯推断形式：

其中表示传感器的测量值，可以是一个图像，也可以是激光雷达的距离值。x表示当前的位姿，y表示观测的对象。那么这个观测模型的贝叶斯推断的物理意义就是：

表示当前观测情况下对于x和y的估计的概率分布。

表示对于一个位置x和一个观测对象y，观测出来的值是z。举一个实际的例子，对于一个真实距离是1m的障碍物，使用激光雷达对这个障碍物进行观测，激光雷达返回的值是一个符合高斯分布的模型，这个实际就是我们的传感器模型。

就是状态x和观测对象y的分布概率，一般而言，当前时刻的状态x和上一时刻的x-1的位置不会相差太大；而对于一个观测对象，如果整个环境是静态的，y也就是不变的，如果是动态的，就可以假设一个关于y的运动方程来进行估计。一般而言这个x和y如果都是动态的，都可以使用等速运动模型来求解。

最后是的含义，这个就是传感器的具体读数，一般这个概率比较难以估计，但是所幸的是，我们在求解最优化的过程中可以抛去这部分。