视觉SLAM十四讲：运动方程

SLAM：同时定位和建图（Simultaneous Localization and Mapping）。

  希望机器人从未知环境的未知地点出发，在运动过程中通过重复观测到的地图特征（比如，墙角，柱子等）定位自身位置和姿态，再根据自身位置增量式的构建地图，从而达到同时定位和地图构建的目的。   所用传感器：激光雷达，相机（单目，双目，RGBD，事件）   SLAM框架： 传感器信息提取—前端视觉里程计—后端优化—建图 传感器信息提取—回环检测—后端优化（这两条线路之间形成一个闭环）   传感器信息提取：SLAM中传感器信息的提取和预处理，往往是多种传感器信息的融合。   前端视觉里程计（VO）：通过估算相邻图像间的关系来推导相机的运动，以及局部地图的样子。   后端优化：接受不同时刻视觉里程计测量的相机位姿以及回环检测的信息，并对他们进行优化（往往是非线性优化），进而得到全局地图和轨迹。   回环检测：用于判断机器人是否到达先前预定的位置，用于解决位置估计随时间漂移的问题。   建图：根据估计的轨迹，建立相应要求的地图（栅格地图，拓扑地图，点云地图，网格地图）。 综上，定位与建图便完成了。  
  SLAM数学表达 此处分为4个部分进行阐述：运动方程及李代数优化，观测方程，非线性优化。   **1. 运动方程：**从K-1时刻运动到K时刻，位姿X变化的数学表示。 可以表述为一个方程：X_k = F(X_(k-1), U_k, W_k) 其中U_k表示传感器输入，W_k表示过程中的噪声，F往往表示非线性函数。   为了进一步从数学角度来理解空间变换，需要了解一下旋转矩阵、变换矩阵。   1.1 旋转矩阵 我们用一个3X1的矩阵来表示物体的位置，3X3的矩阵来表示物体的姿态。 物体在空间里的运动可以分解为平移加旋转，平移操作只需要坐标的简单相加，旋转则涉及到坐标系的变换，下面对旋转矩阵进行分析： 设某个单位正交基(e1，e2，e3) 经过一次旋转，变成了(e1, e2, e3)。那么，对于同一个向量a（该向量并没有随着坐标系的旋转而发生运动），它在两个坐标系下的坐标为[a1, a2, a3]T 和[a1, a2, a3]。根据坐标的定义，有：  
  上面等式左右同乘以[a1, a2, a3`]T，变成：  
  我们把R称为旋转矩阵，它描述了旋转前后同一个向量的坐标变换关系。通过旋转矩阵，我们便可以求解物体的旋转变换了。   1.2 变换矩阵 有了旋转矩阵和平移矩阵，便可以描述一个物体的空间变换了，不过还有一个遗憾，便是操作的复杂性，因为目前变换前后的坐标不是一个线性关系，而是两次变换（平移和旋转）。为了使其变为一次线性操作，我们提出了变换矩阵的概念： 将平移矩阵和旋转矩阵按照如下规律合在一起，便称之为变换矩阵T：  
  式中[a, i]T为齐次坐标（四维向量）。   如此，变换前后的坐标便成了线性关系。   但是，这样的表示还不够，因为旋转矩阵一共有9个量，但是它只描述了3个自由度的旋转，这便产生了冗余。同理，变换矩阵有16个量，但只描述了6个自由度。于是，我们就寻找更加紧凑的表达。   书中给出了很多种表达，如旋转向量、欧拉角和四元数等，但是由于前两种三维向量描述方式都带有奇异性（所谓奇异性，即在某些情况下这种描述方式不管用），所以常用的还是四元数，此处仅介绍一下四元数：   四元数是一种类似于复数的代数，由四个部分组成，即是紧凑的，也没有奇异性。 一个四元数q 拥有一个实部和三个虚部。q = q0 + q1i + q2j + q3k; 其中i; j; k 为四元数的三个虚部。这三个虚部满足关系式：  
  假设某个旋转是绕单位向量n = [nx; ny; nz]T 进行了角度为 theta 的旋转，那么这个旋转的四元数形式为：  
  具体表示：假设一个空间三维点p = [x; y; z]，以及一个由轴角n; theta 指定的旋转。三维点p 经过旋转之后变成为p′。如果使用矩阵描述，那么有p′ = Rp。如果用四元数描述旋转: 首先，把三维空间点用一个虚四元数来描述: p = [0; x; y; z] 用四元数q 表示这个旋转：  
  旋转后的点p′ 即可表示为这样的乘积：  
  至此，我们已经可以用四元数和一个平移矩阵来表示物体的空间运动了。 2. 运动方程的李代数优化 目前，我们已经知道了三维世界中刚体运动的表示方式，但是在SLAM 中，除了表示之 外，我们还要对它们进行估计和优化。因为在 SLAM 中位姿是未知的，而我们需要解决什 么样的相机位姿最符合当前观测数据这样的问题。   一种典型的方式是把它构建成一个优化问题，求解最优的R; t，使得误差最小化。   但是，由于旋转矩阵自身是带有约束的（正交且行列式为1）。所以它们作为优化变量时，会引入额外的约束，使优化问题变得困难。于是我们提出了通过李群——李代数间的转换关系，把位姿估计变成无约束的优化问题，简化求解方式。   李群：下节再讲，好累啊，感觉在做无用功···   3. 观测方程：在X_k位置上观测到路标点Y_i时，产生的观测数据Z_(k,j). Z_(k,j) = H(Y_i, X_k, V_(k,j)) 其中V_(k,j)表示此时的噪声