【视觉SLAM基本原理概述3400字】

视觉SLAM基本原理概述视觉SLAM的特征与激光SLAM相比，视觉SLAM起步较晚，目前还在逐渐落地产品；视觉SLAM的传感器结构简单、体积和功耗较小、重量较轻且成本较低；激光SLAM主要应用于室内，视觉SLAM在室内外都可以开展工作，但对环境要求较高；视觉SLAM构建的地图精度比激光SLAM低，存在累计误差。按照工作方式的不同，视觉SLAM所使用的相机可以分为单目、双目和深度相机。单目相机只有一个摄像头，结构简单，成本很低，也因此单目SLAM受到了很多关注，但单目SLAM无法确定真实尺度。有两个摄像头的双目相机消除了尺度不确定性，但它的配置与标定较为复杂，且计算量很大。深度相机的工作原理相对复杂，它不仅能够采集到彩色图片，还能读出相机与每个像素之间的距离REF_Ref19728\r\h[4]。SLAM问题的数学表述记离散时刻t=1,…,K，传感器在各时刻的位置为x1,…,xK，构成地图路标点为y1,…,yN。虽然传感器的读数不一定直接就是位置之差，但无论何种情况，我们都能用一个通用的、抽象的运动方程来说明：xk=f(x(2.1)其中：uk是运动传感器的读数或输入，wk为该运动过程加入的随机噪声，f函数可以指代任意运动的运动传感器/输入。除此之外还有一个观测方程，描述的是传感器在xk位置看到路标点yj时，产生了观测数据zk,jzk,j=h(yj,(2.2)其中vk,j是这次观测里的噪声。运动方程和观测方程将视觉SLAM问题转换成了一个状态估计问题，它的求解与这两个方程的具体形式及噪声服从的分布有关REF_Ref19728\r\h[4]。视觉SLAM基本流程整个视觉SLAM流程包含传感器信息读取、视觉里程计、后端优化、回环检测以及建图五个部分，经典的视觉SLAM框架的流程图如下所示，除了传感器信息读取部分，本文将对每个模块所涉及到的基本原理进行分析。图2.1经典视觉SLAM框架视觉里程计视觉里程计主要是估计相机的位姿与运动轨迹，为后端优化提供较好的初值，因此视觉里程计又可以叫做前端。它的算法分为特征点法、直接法和光流法，本节将介绍这三种算法的基本原理。特征点法特征点法需要先提取稳定的图像特征，然后对描述子进行匹配，由于图像特征具有局部性，特征匹配存在误匹配的情况，因此还要在运动估计是去除误匹配。之后利用匹配的点估计相机的运动，根据相机的不同，分别需要通过对极几何、ICP和PnP求解。（1）2D-2D:对极几何当视觉SLAM的传感器为单目相机时，匹配到的特征点是2D点，需要用对极几何来解决。以下图为例，我们要计算的是相片I1和相片I2之间的运动，C1和C2分别是两个相机的中心，p1和p2是特征匹配得到的一对特征点。图2.2对极几何示意图设I1到I2的运动为R、t，相机内参矩阵为K。则可得：p1=KP，p2=K(RP+t)(2.3)为了简化运算，取：a1=K-1p1，a2=K-1p2(2.4)可以得到:a2Tt^Ra(2.5)重新代入a1和a2，有：p2TK-Tt^RK-1p1(2.6)以上两个式子都可以被称为对极约束，进一步简化对极约束：E=t^R，F=K-Tt^RK-1，a2TEa1=p2TF(2.7)其中F为基础矩阵，E为本质矩阵，根据特征匹配得到的特征点对可以计算出E或F，由于E与F只差了K，而K是已知的，所以E更为常用，求出E后可以根据奇异值分解恢复出相机的运动R和t。（2）3D-2D：PnP当匹配到的特征点对一组为2D，另一组为3D时，即已知多个3D点和它们的投影位置时，需要用PnP估计相机位姿。PnP问题的解法有很多，比如：P3P、EPnP、DLT、UPnP等，除此之外还可以用光束法平差求解，这里介绍比较常用的P3P算法。P3P输入的数据为3对3D-2D匹配点，还有一对验证点。记3D点为A、B、C，它们在相机成像平面上的投影点为a、b、c，D为验证3D点，d为它对应的投影点，相机光心为P。示意图如下：图2.3P3P示意图根据三角形相似和余弦定理可得：PA2+PB2−2∙PA∙PB∙cos〈a,b〉=AB2PA2+PC2−2∙PA∙PC∙cos〈a,b〉=AC2(2.8)PB2+PC2−2∙PB∙PC∙cos〈b,c〉=BC2xyvw(2.9)令：替换并代入化简可得：(1−w)x2−w∙y2−2∙x∙cos〈a,c〉+2∙w∙x(2.10)(1−v)x2−v∙x2−2∙y∙cos〈b,c〉+2∙v∙x该方程组可以通过吴消元法来求解，再用验证点来计算最可能的解REF_Ref31569\r\h[16]，从而求出a、b、c在当前相机坐标系下3D坐标，此时可转化为ICP问题求解。（3）3D-3D：ICP当匹配到的特征点对均为3D点时，需要用ICP求解。求解ICP有两种方法，一种是利用线性代数的求解（典型的方法是SVD），它首先计算两组点的质心位置用来求解每个点的去质心坐标，然后构建最小二乘计算旋转矩阵，最后根据旋转矩阵求解tREF_Ref32751\r\h[17]。另一种是非线性优化方法，用迭代的方式去找最优值。尽管视觉里程计的主要使用特征点法，但它也有提取特征点和计算描述子非常耗时、忽略了除特征点以外的可用图像信息、计算结果受噪声影响较大、不易在纹理单一的地方提取特征点等缺点REF_Ref19728\r\h[4]。光流法光流是用来描述像素随时间在图像上运动的属性，也就是图像上每个像素随画面移动过程中的x,y位移量，其中稀疏光流法跟踪图像中的部分像素点，以Lucas-Kanade光流为代表REF_Ref787\r\h[18]；稠密光流法跟踪图像中的所有像素点，以Hom-Schunck光流为代表REF_Ref832\r\h[19]。本节主要介绍Lucas-Kanade光流，简称LK光流，示意图如下：图2.4LK光流法示意图设一个灰度为I(x,y,t)像素在t时刻位于(x,y)处，基于灰度不变假设，则有：I(x+dx,y+dy,t+dt)=I(x,y,t)(2.11)泰勒展开可得：∂(2.12)其中：dx/dt是x方向的速度，dy/dt是y方向的速度，分别记为u和v；∂I/∂x为图像在该点处x方向的梯度，∂I/∂y为图像在该点处y方向的梯度，分别记为Ix和Iy，∂I/∂t为图像灰度对时间的变化量，记为It，可以得到：I(2.13)假设某一个窗口内w2个像素具有相同的运动，A为所有像素Ix和Iyu(2.14)光流法避免了提取特征点和计算描述子的过程，能够直接得到特征点之间的对应关系，通过跟踪取代匹配，减少误匹配的情况，增加了算法的鲁棒性。但它所要求的灰度不变假设在实际中很可能不成立，因此通过光流法得到的结果并不一定是可靠的。直接法直接法与光流法具有一定的相似性，也要基于灰度不变假设，同时避免了提取特征点和计算描述子。设某个空间点Pi在两张相片上的投影分别为p1,i和p2,i，投影点对应的灰度值分别为I1(p1,i)和I2(p2,i)，相机位姿为K，为了减少p2,i实际值与估计值之间的差别，可以优化相机的位姿，此时最小化的是光度误差ei。则相机位姿估计问题转化为：minei=I1(p1,i)−I2(p2,i(2.15)然后推导出优化问题的雅可比矩阵，利用Gauss-Newton法或者Levenberg-Marquardt法计算增量，迭代求解。直接法通常比传统的特征点法和光流法更快，且可以在缺失特征的环境中使用，只要求有像素梯度，但它的鲁棒性比特征点法差，受相机运动和光照变化的影响REF_Ref1342\r\h[20]。后端优化误差累积在前端中是不可避免的，为了使相机的运动轨迹在更长的一段时间内保持最优状态，我们引入后端优化。记k时刻所有的未知量为xk，所有的观测为zk，过去0至k时刻中的数据来估计现在的状态分布:P(xk|x0,u1:k,z1:k)(2.16)按照贝叶斯法则，将xk与zk的位置交换，可得：P(xk|x0,u1:k,z1:k)∝P(zk|xk)P(xk|x0,u1:k,z1:k-1)(2.17)上式第一项称作似然，第二项称作先验。后续的处理大体上分为两种：一种是假设马尔可夫性，认为k时刻的状态只与k−1时刻的状态有关，这是以EKF为代表的滤波器方法；另一种考虑与k时刻之前所有时刻状态的关系，它们构成一个最小二乘问题，可以用非线性优化方法求解。回环检测视觉里程计只考虑相邻时间上的关键帧，其中的误差会一帧一帧的传递下去，也就是累积误差，使长期估计得到的结果变得不可靠。虽然后端优化可以进行全局一致的优化，但也不能消除累积误差，这时引入回环检测，它提供除了相邻关键帧以外的时隔更久远的约束，为后端优化提供更多有效数据。目前进行回环检测的主流方法是特征点结合词袋，即先预加载一个词袋字典树，将图像中特征点的描述子转换为单词，把整张图像转换为一个词袋向量，图像检索时先找出与当前帧拥有相同单词的候选关键帧，剔除相似度不够高的关键帧，按词袋向量的距离由近到远排序。如果当前帧与候选关键帧的相似度超过当前帧与上一个关键帧相似