【《室内机器人SLAM传感器数据处理分析案例》8800字】

室内机器人SLAM传感器数据处理分析案例目录TOC\o"1-3"\h\u287711.1引言 1188261.2惯性测量单元数据处理 3242221.1.1IMU系统坐标系 3140061.1.2IMU数学模型 7157791.1.3IMU数据滤波处理 9152771.3双目相机数据处理 1120001.3.1双目相机工作原理 1152701.3.2双目相机视觉里程计 1538141.4激光雷达数据处理 181.1引言多传感器融合技术能够使机器人在构建地图时获取到更加丰富的环境信息，能够得到较精确的局部定位数据，提高机器人建图质量和导航精度。近年来，针对不同的应用场景和技术要求，多种类型的传感器组合形式也相继出现，如以相机为主体的融合方案有相机与激光雷达进行数据融合、相机与惯性测量单元（IMU）进行数据融合等。多种融合方式各有优劣，如下表2-1所示。表2-1传感器组合方式组合方式优点缺点激光雷达和惯性融合可靠性高，且解决了激光SLAM过程中激光雷达垂直分辨率低、更新速度低以及由运动引起的失真等问题探测距离有限，不能识别物体，在特征不明显的地方定位效果差，难以重定位毫米波和激光雷达探测距离远，不受环境光线影响，可靠性高，且同时提高了局部定位精度和全局定位精度成本高，计算量大，且无法识别物体，在结构单一环境中定位效果差视觉和惯性融合减轻运动对相机的影响，提高了相机在快速移动时的定位和跟踪能力易受环境光线的影响，计算量大，存在累计误差激光雷达和视觉融合同时提高了局部定位能力和全局定位能力，减小了累计误差，并且提高了鲁棒性计算量较大，定位依赖于准确的联合标定，在运动过快情况下定位效果差惯性导航和声呐融合探测距离远，可靠性强，定位精度高存在累计误差，对动态目标跟踪效果差。SLAM进行多传感器融合关键在于不同的数据如何进行融合，其融合示意图如下图2-1所示，融合的基本要求是多个数据必须基本实时、互补、不冗余。另外，根据数据融合的层次大致可以分为决策层融合、数据层融合、特征层融合；根据融合耦合复杂度的不同有松耦合、紧耦合以及超紧耦合；根据融合的方法不同，可以分为加权平均法、贝叶斯估计法、扩展卡尔曼滤波法、模糊逻辑、D-S证据理论法、神经网络法等等[45]。图2-1传感器数据融合示意图1.2惯性测量单元数据处理惯性测量单元（InertialMeasurementUnit）简称惯导（IMU），是一种机电一体化的综合系统，系统较为复杂，被广泛用于汽车自动驾驶、自主移动机器人领域，也被用于需要利用姿态进行精密位移推算的场合，比如潜艇、飞机等惯性导航装备中。根据IMU安装方式的不同，惯导系统可以分成平台式和捷联式两种类型，平台式系统中的导航系统姿态信息是通过稳定平台提供的，但是其存在不便于携带，耗电量大，制作工序复杂等缺点。而捷联式惯导系统是将惯性传感器直接安装在载体上，具有安装方便、重量轻、体积小、可靠性高等优点，因此在机器人领域主要是采用捷联式惯导系统，其原理图如2-2所示。图2-2惯性测量单元原理示意图IMU通过一个数学计算平台计算并维持陀螺仪的角速度和线速度。通过对角速度进行积分获取载体的姿态信息，根据得到的姿态信息将加速度变换到导航坐标系下，通过对加速度的输出进行一次积分得到载体的速度信息，对速度进行再次积分得到载体的位置信息。为保证惯导算法的误差可以忽略不计，在角速度、线加速度以及速度积分过程中采用高精度的数值积分算法来减小IMU在转动过程产生的划船效应、卷轴效应等负面影响。1.1.1IMU系统坐标系将IMU安装到机器人载体上，为保证IMU数据与机器人相关位置信息保持一致需要进行坐标系的转换，与IMU相关的坐标系有导航坐标系、载体坐标系、地心坐标系[46]。地心坐标系是惯性测量单元在地球上随着地球自转而转动的一个基准坐标系。导航坐标系根据机器人选取的坐标轴不同有多种表示方法，其常用的坐标是东北天坐标系，原点位于载体的中心，符合右手准则。载体坐标系是描述机器人载体方位的坐标系，机器人载体的重力中心作为其原点。不同坐标系之间的常用坐标转换方法有方向余弦法、四元数法以及欧拉角法。欧拉角法通过三个转角来描述旋转过程，下面是三个旋转的基本旋转矩阵。绕X轴的旋转变换矩阵：（2-1）绕Y轴的旋转变换矩阵：（2-2）绕Z轴的旋转变换矩阵：（2-3）使用这样的三维向量来表示欧拉角旋转矩阵的转换关系。欧拉角不适用于差值和迭代，其任意旋转欧拉角会出现万向节死锁的问题，致使系统会丢失某一个自由度，因此在SLAM系统中很少使用欧拉角对姿态进行表示。（2）方向余弦法在解析几何中，向量的三个方向的余弦是向量与三个坐标轴之间的夹角的余弦。假设V是三维空间中的向量，由一组标准正交基组成，如下式所示。（2-4）其中，，，为一组标准正交基的单位基底向量，，，分别为相对于x轴、y轴、z轴的分量，轴相对于x轴、y轴、z轴的方向余弦、、如下式所示。（2-5）（2-6）（2-7）其中，a、b、c分别为z轴相对于x轴、y轴、z轴的高度。（3）四元数法四元数q由实部和虚部组成，如下式所示。（2-8）上式中的虚部需要满足如下式所示关系：（2-9）在三维空间中，一般使用四元数描述旋转过程。假设单位向量产生了一个角度的旋转，那么此时四元数的形式可表示为下式所示形式：（2-10）从单位四元数反推出旋转轴和夹角，如式所示：（2-11）依据罗德里格斯公式，其旋转向量到旋转矩阵R的转换如下式所示：（2-12）上式中为反对称矩阵。旋转矩阵和四元数之前的等式关系为：（2-13）欧拉角转换为四元数形式如下式所示：（2-14）四元数转化为欧拉角为式：（2-15）四元数方法计算量较小且便于进行插值运算，因此本文采用四元数方法对姿态进行表示。1.1.2IMU数学模型通过建立惯导数学模型，推导其运动学模型和误差模型。数学模型中主要针对IMU随机游走误差以及高斯白噪声进行分析，通过对IMU标定来得到这两个噪声，也可以直接采用IMU的设备出厂参数，通过误差模型可以建立出IMU真实值和测量值之间的计算关系[47]。六轴的IMU主要有两部分组成，分别是三轴加速度计和三轴陀螺仪，加速度计用来获取机器人载体自身的运动加速度数据，陀螺仪用来获取机器人自身的运动角速度数据。测量值中存在噪声，为了降低测量值与实际测量值之间的误差，对于陀螺仪和加速度计，对其测量值与真实值之间的关系建立如下式所示模型。（2-16）上式中表示加速度计的测量值，表示陀螺仪测量值，表示陀螺仪的真实值，表示加速度计的随机游走值误差，表示陀螺仪的随机游走值误差，和分别表示二者的噪声。假设加速度计和陀螺仪的噪声均服从高斯分布，即均为白噪声，那么和的数学期望都为0，如下式所示。（2-17）上式中表示加速度计的噪声强度，表示陀螺仪的噪声强度。在计算过程中，IMU测量数据都是通过离散采样的方式获取的，所以需要在上式的基础上进行离散化，离散化后和白噪声之间的方差存在下式所示关系。（2-18）针对陀螺仪和加速度计产生的随机游走误差，可以建立一个如下式所示的维纳过程（WienerProcess）。（2-19）表示随机游走的强度，表示单位的高斯白噪声，上式建立的模型是高斯白噪声的积分，因为IMU内部硬件构造、环境温度等综合影响造成的噪声在离散情况下称之为随机游走，如下式所示。（2-20）其中是离散情况下的随机游走强度，从随机游动的分布可以推断，随机游动是在前一个噪声的基础上叠加了高斯噪声，其下一步也是随机游动。1.1.3IMU数据滤波处理IMU数据由于其自身内部结构以及环境等因素导致其测量的数据中掺杂了较多噪声，因此，考虑到对实测数据的滤波，常用的滤波算法有KF和EKF、互补滤波等来进行误差处理，但是都存在一些缺点[48]，如KF在滤波过程中，其噪声误差是采用简单统计的方法得到的，无法随着加速度计和陀螺仪的变化而变化；EKF用于非线性系统上，首先对非线性系统进行线性化，即通过泰勒级数展开对其进行线性化，然后采用KF的方法进行滤波，对处理简单的非线性系统效率很高，但是应用在较复杂的系统上时，其误差较大，效果不理想，存在线性化误差，影响计算结果；互补滤波可以对陀螺仪和加速度计的优缺点互补，它采用低通滤波器对加速度计的高频噪声进行抑制，采用高通滤波器对陀螺仪的低频噪声进行抑制，但是在滤波过程中要不断切换低通滤波器和高通滤波器的频率并且需要不断进行调试才能确定出其合适的参数。因此，由于上述的算法都存在一些较明显的缺点，本文采用处理非平稳信号的希尔伯特-黄变换的方式对IMU中产生的噪声进行滤波处理。首先采用经验模式对IMU的测量数据进行分解，首先根据样条函数对测量数据的局部极大值和极小值进行处理，然后根据测量数据和其均值做差，通过判断差值是否满足固有模式条件对k进行多次筛选，符合后即可得到本征模态函数的第一个分量，如下式：（2-21）第一个IMF分量与测量数据进行作差，得到第一个剩余量，将作为新的变量对上述过程进行重复操作，直到剩余量无法再进行分解，最后测量数据通过经验模式分解后如下式：（2-22）对上式进行希尔伯特变换，进行瞬时分析，可得下式。（2-23）其中Re表示函数的实部，以上就是希尔伯特变换的全部过程。接下来采用上述方法对IMU数据进行处理，图2-3和2-4为原始数据，图2-5到2-10为采用希尔伯特黄变换进行处理后的结果。图2-3三轴角速度原始数据图2-4三轴线加速度原始数据图2-5X轴加速度图2-6X轴线加速度图2-7Y轴角速度图2-8Y轴线加速度图2-9Z轴角速度图2-10Z轴线加速度由图2-5到2-10分析可知，对原始数据进行经验模式分解后得到IMF，在本文实验数据中将陀螺仪角速度三轴数据分解为2阶IMF，如图2-5，2-7，2-9，将线速度分解为4阶IMF，如图2-6，2-8，2-10，依次滤除各阶的噪声后，原始数据的分布曲线变的平稳，达到了较好的降噪处理。1.3双目相机数据处理1.3.1双目相机工作原理双目相机可以将三维空间中各点的坐标映射到二维图像平面上，并用几何模型来表示。有许多模型，其中之一是针孔模型。针孔模型描述了一束光线通过针孔后在针孔的背面形成投影的关系，在本文中首先对针孔模型进行简单的映射关系建模。图2-11针孔成像模型如上图所示，设为相机坐标系，一般相机前方为z轴，x轴向右，y轴向下。图中O为相机的光心，在针孔模型中表示针孔。现实三维空间中的点P经过针孔O投影后，会在物理成像平面出现成像点。设P的坐标为，坐标为，物理成像平面到小孔的距离为，则依据三角形相似原理有下式所示关系。（2-24）在公式中，负号表示图像是倒立的。为了使模型更符合实际，可以将成像平面对称地放置在相机前方，与三维空间点一起放置在相机坐标系的同一侧，如下图2-12所示。这样就可以去掉上面公式2-24中的负号进行简化，简化后如下公式所示。（2-25）a真实成像平面b对称的成像平面c归一化成像平面图2-12三角形成像原理示意图将，移到等式的左边，整理得：（2-26）（2-27）上式描述了三维空间中的点和其像之间的空间关系，所有点的距离度量单位都可以看成米，比如相机焦距是0.2米。在相机中，最终需要获取到的是像素点，因此需要在成像平面上对像进行采样和量化。像素坐标系的定义一般是先选取一个原点，为方便表示各个像素点的位置，我们将原点放置在图像的左上角，轴向右和轴平行，轴向下和轴平行，成像平面和像素坐标系之间有一个原点的平移和缩放。设像素坐标在轴上缩放了倍，在轴上缩放了倍。与此同时，像素原点坐标平移了，则的坐标和像素坐标的关系表示如下式所示：（2-28）将式2-26和2-27代入2-28式中，并把合并成，将合并成，得：（2-29）其中，的单位是米，和的单位是像素/米，则可知，以及，的单位均为像素。为了简化计算，上述公式可以写成矩阵形式，如下式所示，左侧需要使用齐次坐标，右侧是非齐次坐标。（2-30）便于理解和计算，将Z移到等式的左边。（2-31）机器的内部参数矩阵是由上述公式中的中间量组成的矩阵，称为K。一般来说，相机的内部参数出厂后是固定的，在使用过程中通常不会改变。一些相机制造商会告诉用户相机的内部参数，但是有时候就需要用户自己来确定相机的内参数，这个过程叫做相机标定，相机的标定会在后文中进行详细阐述。由于相机有内部参数，相应的外部参数也存在。在公式2-32中，使用在相机坐标系中的坐标，但是在相机托架的移动过程中，相机也会随之移动。因此，的相机坐标是世界坐标，是根据相机的当前姿态将其转换为相机坐标系中的坐标。通常，相机的姿态由旋转矩阵和来描述，如下所示。（2-32）上式中隐含一次齐次坐标到非齐次坐标之间的转换，其表示的世界坐标和像素坐标之间的投影关系。其中，相机的位姿R，t称为相机的外参数，和内参相比，外参会随着相机的运动而发生改变，在SLAM中，相机的外参也表示机器人的轨迹运动。投影过程从另一个角度来看，可以把世界坐标系中的点先转换到相机坐标系，然后删除最后一维的数值，即把最后一维进行归一化处理，从而得到点P在相机归一化平面上的投影归一化可以看做是相机前方处平面的一个点，此平面也称为归一化平面。像素坐标通过归一化坐标左乘内参即可得到，所以将像素坐标看做是对归一化平面上的点进行量化测量的结果。针孔相机只是描述了单个相机的成像模型，在SLAM机器人应用中，单个相机并不能为机器人提供丰富的环境信息，因为仅仅根据一个像素点无法确定三维空间中点的位置，从相机光心到归一化平面上的点都可以投影到某个像素上，深度信息无法得知，如图2-13所示。因此在机器人应用中需要用到能够得到三维空间中点的深度信息的相机，比如双目相机、RGBD深度相机等。2-13像素点可能存在的位置双目相机通过同时采集左右相机获取到的图像，然后计算两张图像之间的视差来估算每一个像素的深度信息，接下来介绍双目相机的成像原理，其示意图如下所示。2-14双目相机成像模型如上图所示，和分别为相机左右光圈的中心，方框为成像平面，为焦距，和为成像平面的坐标。双目摄像机主要由左眼摄像机和右眼摄像机组成。两个摄像头水平放置，也可以看作两个摄像头。但是，为了安装方便和美观，一般采用左右摄像头。在双目照相机中，每个照相机都可以看作一个针孔照相机。由于两个摄像机是水平放置的，其光圈中心在轴上，两个镜头之间的距离是双目摄像机的基线，这是双目摄像机的一个重要参数，假设三维空间中有一点，其在双目相机的左相机和右相机中都各成一像，记作，，因为其左右相机之间有一定的距离，因此这两个成像的位置肯定是不同的，在比较理想的情况下，因为左右两个相机只是在x轴上有位移，所以两个相机的成像也只会在x轴上有差异。设左相机的坐标为，右相机的坐标为，观察上图2-14并根据三角形相似原理，可得下式。（2-33）进行简化得：（2-34）上式中，左右图像横坐标的差值定义为d，称为视差，依据视差，可以推算出一个像素点距离相机的深度信息，像素和相机之间的距离与左右相机采集的图像的视差成反比。视差越大，距离越近。同时，因为最小视差是一个像素，所以双目相机可以测量的深度存在理论上的最大值，这是由参数决定的。根据双目相机的成像原理，基线越长，测量的最大距离越远。因此，小型双目摄像机只能用于小尺度测量。双目摄像机视差计算的深度公式非常简单，但视差d的计算比较复杂。首先，需要确定左眼图像的像素在右眼图像中出现的位置。在计算像素深度时，其精度和计算量是优先考虑的，视差只能在图像纹理变化较大的地方计算。由于视觉的应用，计算量比较大，因此双目摄像机的深度计算还需要使用GPU或FPGA进行实时计算。1.3.2双目相机视觉里程计一个完整的SLAM系统一般包含前端和后端，在视觉SLAM中，前端指视觉里程计。其思想是根据相机观测到的相邻图像信息，粗略估计相机的运动变化，为后端优化提供更好的初始值。目前，视觉里程计计算主要有两种方法，一种是直接法，另一种是特征点法。基于特征点法的视觉里程计的优点是对光和动态物体不敏感，稳定性好。目前应用广泛。直接法具有计算速度快的优点。本课题主要研究基于特征点法的视觉里程计。与传统里程计技术相比，视觉里程计具有更多的优点。与编码器等其它传感器相比，视觉里程计没有因读数不准确、精度降低或惯性漂移而引起的数据误差。因此，视觉里程计可以应用于非结构化场景，如GPS不能满足狭小空间或室内环境的要求，传统的里程计不能用于腿式机器人的导航和定位。此外，视觉里程计不仅可以提供机器人里程计数据，还可以获取丰富的环境特征信息，完成障碍物识别、目标检测和通行区域划分等任务，为机器人的功能扩展提供更多的支持。基于特征点法的视觉里程计的主要问题是如何根据图像估计相机的运动轨迹。首先，我们需要找到图像中的特种点，当相机的视角稍有变化时，这些特征点将保持不变，从而将相邻两幅图像中的特征点进行配对，并通过匹配的特征点来估计两幅图像之间的运动关系。特征点的选取原则是特征点具有可重复性、可分辨性、高效性和局部性。重复性意味着在不同的图像中可以找到相同的特征，可分辨性意味着不同的特征点应该有自己的特征而不能与其他特征点相同，高效率意味着同一图像中特征点的个数应该远远小于像素的个数，局部性意味着所发现的特征点只与一个小的图像区域相关。可视里程计的实现过程如下图所示。图2-15双目相机视觉里程计实现流程首先获取相邻时刻的图像序列It，It+1，为了找到正确图像与畸变图像像素点之间的映射关系需要对获取到的图像进行去畸变操作。图像之间的位置变换关系通过追踪两图像中相同的特征点进行计算，因此首先提取相邻图像间的特征点，对提取出的两组特征点进行特征匹配或特征跟踪，根据使用传感器类型选择相应的运动估计方法进行相机t时刻下的运动变化，从而得到相机的运动里程数据。特征点由关键点和描述子两部分组成，经过多年的发展，现在具有代表性的特征点类型是ORB（OrientedFASTandRotatedBRIEF）特征点，其关键点称为“OrientedFAST”，它是一种改进的FAST角点。FAST角点主要检测图像中像素灰度变化较明显的地方。首先在图像中选取一个像素点，得到其灰度值为，然后以该像素点为圆心选取半径为3的圆上的16个像素点，如果选取的圆上有连续的N个点的亮度大于加上某一个阈值或者小于减去某一个阈值，则该点可作为特征点，对图像上所有的像素点都进行这样的过程就完成了对一帧图像的特征点检测，特征点示意图如下图所示。图2-16ORB特征点FAST特征点的计算速度快，但是存在重复性强以及分布不均匀的缺点，从而导致特征点的尺度问题，其中尺度不变性由构建图像金字塔的方式解决。金字塔是计算机图像处理常用的一种方法，示意图如下图所示。图2-17图像金字塔如上图所示，金字塔的底部是原始图像，从下往上，每上一层就对图像进行某一固定倍率的缩放操作，从而得到不同分辨率的图像。在特征匹配算法中，通过匹配不同层上的图像实现尺度不变性。针对旋转，使用质心作为图像块灰度值权重的中心。首先在图像的某一个小的图像块中定义图像的矩为如下式所示。（2-35）通过距找到图像块的中心，如下式所示。（2-36）连接图像块的质心H与几何中心O得到方向向量从而确定特征点的方向向量如下式所示。（2-37）通过上述方法，提升了特征点在不同图像之间表示的鲁棒性，从而可以顺利完成特征点的匹配。从两张图像中得到一对匹配好的特征点后，就可以通过这些二维图像点的对应关系恢复出两帧图像之间的运动关系，如下图所示。图2-18特征点匹配假设第一帧图像到第二帧图像之间的运动关系为R，t，两个相机的中心分别为，.并且已知两帧图像已经通过特征点匹配得到匹配点和。首先和连线在三维空间中相交于点P。连线与像平面，相交于，。从代数的角度分析图中所示的几何关系，在第一帧的坐标系下，设P的空间位置如下式所示。（2-38）根据针孔相机模型，得到像素点，的像素位置如下式。（2-39）其中，K为相机的内参，R，t为两帧图像之间的运动关系，即相机的运动。1.4激光雷达数据处理在机器人领域，激光雷达是机器人获取环境信息最常使用的传感器，从刚开始的单一传感器SLAM到现在的多传感器融合SLAM。激光雷达始终处于核心位置。激光雷达通过获取激光信号的时间差和相位差来得知扫描到的障碍物距离激光雷达的距离和角度。激光雷达在运行时会返回一系列数据，假设将这一组激光数据采集到的距离个数设为K，则有观测集合：