同步定位与地图构建(simultaneous localizationand mapping，SLAM)在机器人实时定位方面得到广泛应用，视觉里程计(visual odometry，VO)是SLAM算法的重要组成部分，其通过分析图像序列确定机器人的位姿信息，并根据位姿信息进行后续优化处理．根据优化方法不同VO可分为特征点法、直接法和半直接法[1-4]．特征点法视觉里程计[5-7]，通过特征匹配，建立重投影函数进行估计．直接法视觉里程计[8-10]，根据灰度不变假设，通过光度和几何误差函数估计帧间运动．半直接法视觉里程计[11]具有特征点法和直接法的优点，将多个特征点当做整体与相邻图像进行匹配．在SLAM算法的应用场景中，动态环境是无法避免的，而大多数算法都假设相机处于静态环境，忽略动态物体在视觉里程计中的影响[12]．视觉里程计针对动态物体可通过多传感器信息改进的随机一致性(RANSAC)策略、前后景模型、语义信息等多种方法进行处理．如ORB-SLAM2[7]通过RANSAC算法将动态物体当做噪声剔除，但无法处理高动态场景．文献[12-14]则是先区分前、后景模型，再通过聚类处理并联合光度误差构建优化函数分割动态、静态物体，但在高动态场景中精确度较差．文献[15-17]将语义信息添加到SLAM系统中，通过先验信息判断和分割环境中运动的物体，从而进行位姿估计．该类方法精确度高，但须要进行提前训练且需要GPU加速，无法达到实时性的要求．Zhou等[18-19]提出近似最近邻域(approximate nearest neighbor fields，ANNFs)和定向最近邻域(oriented nearest neighbor fields，ONNFs)代替距离变换，提高边缘对齐的效率，提高算法实时性，但未对动态物体进行处理，无法有效剔除动态物体，导致位姿估计出现偏差．本研究提出一种融合边缘信息的动态视觉里程计，通过边缘信息优化直接法易受光照变化影响的缺点．并在几何纹理较少的场景中，增加光度和几何误差的权重，弥补边缘信息缺失导致算法不稳定的缺点．1 算法框架本文算法过程如图1所示．首先，通过聚类算法将图像分为24个集群，并从图像中提取边缘信息，将处理后的边缘图和灰度图进行坐标变换到关键帧，构建残差模型并分配静态概率因子．然后，通过面元重构算法分离动态和静态物体，动态物体被分到低权值，根据权重不同剔除动态物体．最后，将加权残差模型加入到联合优化函数进行处理，更新下次迭代所用的关键帧．通过引入边缘信息，并对边缘信息提取、聚类处理并分配静态概率因子，将边缘对齐误差和光度误差并进行融合，再将高残差值、空间正则化权值误差作为补偿动态集群的约束条件构建联合优化函数，提高算法估算的准确性和鲁棒性．10.13245/j.hust.238606.F001图1算法框架图2 融合边缘信息的视觉里程计2.1　相机模型RGB-D相机在每个时间帧中会获取一个RGB图像Cc和一个深度图Zc，其中RGB图像通过处理可以得到灰度图Ic．深度图可用于场景聚类，构建残差模型，灰度图可进行边缘信息的提取．采用针孔相机模型，将3D点通过投影函数π映射到图像像素中，可以得到图像的像素坐标为p=π(P)=(Xfx/Z+cx,Yfy/Z+cy)，(1)式中：fx和fy为焦距；cx和cy为针孔模型定义的图像坐标；3D点坐标为P=(X,Y,Z)，若知道深度坐标为Z=Zc(p)，则可以定义逆投影函数π-1，具体为P=π-1(p,Z)=x-cxfxZ,y-cyfyZ,Z． (2)两帧之间的相对运动通过变换矩阵T∈SE(3)(李代数)进行表示，SE(3)是变换矩阵构成的特殊欧式群，当前帧3D点坐标为Pc=Tcc-1Pc-1=Rcc-1tcc-101Pc-1，(3)式中：R3×3∈SO(3)为旋转矩阵，SO(3)为三维旋转矩阵构成的特殊正交群；t3×1∈R3为平移向量；Pc-1为上一帧的3D点坐标．刚体运动有6个自由度，通过李代数SE(3)中的ξ∈R3表示SE(3)的变换矩阵T∈R4．坐标变换函数ω为pc=ω(ξcc-1,pc-1)=π(Tcc-1π-1(pc-1,Zc-1(pc-1))), (4)式中：ξcc-1为Tcc-1的李代数；pc-1和pc为相邻两帧对应的像素点．2.2　场景聚类采用K-means算法，并利用深度信息和逆投影函数得到对应点的三维坐标．计算该点与集群中心点的距离，确定其所属集群．聚类算法可避免产生像素级噪声，简化运动分割中的计算量．2.3　残差模型2.3.1　光度及几何误差为了标识帧间的动态、静态部分，要分别计算每个集群的残差大小，再进行运动分割．首先，将聚类处理后的当前帧Fc通过式(4)变换到关键帧Fk，再根据直接法中光度和几何一致性原则构建残差模型，即rZp(ξ)=Zk(ω(ξkc,xcp))-Tkcπ-1(xcp,Zc(xcp))Z；(5)rIp(ξ)=Ik(ω(ξkc,Zc(xcp))-Ic(xcp)，(6)式中：rIp和rZp为像素点p对应的光度和深度误差；∙Z表示Z点对应的三维坐标；xp∈Ω为当前图像的像素坐标；ξkc为当前帧到关键帧进行坐标变换的李代数．2.3.2　边缘对齐误差文献[18-20]对比了多种边缘检测算法，文献[19]的方法对当前帧Fc进行边缘提取，再将边缘点按集群分类，最后将边缘坐标通过近似最近邻域(ANNFs)变换到关键帧计算边缘对齐误差．边缘对齐误差模型为rEp(ξ)=∑pk∈SkwpkgT(xkc-n(xkc))γ，(7)式中：rEp为边缘点p到切线的边缘对齐误差；Sk为边缘像素投影到当前帧中的集合；wpk为关键帧为选中像素分配的权重；∙γ表示Huber正则化；n(∙)使用欧氏距离表示当前帧k中的最近邻域边缘像素；g为g(n(xkc))的缩写，表示p的时间匹配的梯度向量．通过边缘对齐可实现ANNFs[21]与金字塔粗到精细的方案，式(7)可进行基于ICP的优化，并基于边缘距离信息对关键帧的选择进行优化，该策略已在DSO算法[9]中实现．通过关键帧提取边缘数据，可以算出对应的ANNFs．边缘信息如图2所示．10.13245/j.hust.238606.F002图2边缘信息示意图2.3.3　数据融合误差由于直接法里程计基于灰度一致性假设，因此处理运动模糊图像时的误差较大．边缘信息对运动模糊的敏感度较低，可通过边缘误差提高算法的精确度和鲁棒性．在几何纹理较少的情况下，通过提高光度和几何误差的权重避免边缘信息过少导致的问题．为了同时估算相机运动和场景分割，提出一种新的最小化公式，分别对静态集群和高残差值对象进行最小化，具体为minξ,b{D(ξ,b)+S(b)};   s.t.  bi∈[0,1] ∀i，(8)式中：b表示进行场景分割时每个集群被赋予的分数bi∈[0,1]，b≃1对应静态集群，b≃0对应动态集群，0b1则对应不确定状态的中间集群；S(b)为对D(ξ,b)的补充，当集群的残差值很高时，集群被分割为动态集群；D(ξ,b)表示使用静态概率加权对应的深度残差、光度残差和边缘对齐残差，且D(ξ,b)=bi(p)F(wZprZp(ξ))+F(αIwIprIp(ξ))+F(αEwEprEp(ξ)), (9)其中，bi(p)为像素p所属集群i的静态概率，wZp，wIp和wEp分别表示点p在深度、光度和边缘图中的权重，F(r)=(δ2/2)log(1+(r/2)2)为Cauchy鲁棒函数，δ为Cauchy鲁棒函数的残差最小化，即F的转折点；αI和αE分别为光度和边缘误差在残差模型中的权重．构建边缘误差的权重公式为αE=ηνc∑n=1NEn/∑n=1NSnαI，(11)式中：νc为相机运动速度；η为αI到αE的尺度系数．在边缘信息丰富或相机运动较快的场景中，αE的值较大边缘信息易受到影响，相反，当αE值较小时边缘信息受到的影响较小．参考文献[13-14]中关于高残差值的定义划分，将平均残差值高的聚类分为动态集群，将残差值低的集群分为静态集群．其中高残差和低残差都是相对于相机运动，所以要将高残差值进行量化处理．当残差值高于Cauchy鲁棒函数中参数c的残差，即F(r)函数的转折点时，定义其为高残差值．对该阈值的最小化进行定义为SD(b)=2∑i=1N1-bi)KiF(c^)，(12)式中：Ki表示集群i中的像素个数；c^为阈值，限定了低残差和高残值之间的边界．当集群i的平均残差大于c^时，式(9)和(12)会使bi尽可能得低(最小为0)；当集群i的平均残差小于c^时，bi值会尽可能得高(最大为1)．参考Scona等[14]中正则化的方法，对连续的集群进行相似运动分割，使得相邻的集群拥有相似的残差值，并惩罚平均深度差较大的值，通过建立最小化模型来正则化残差公式，即SR(b)=λR∑i=1N∑j=i+1NGij(bi-bj)2+λP∑i=1N(bi-biP)2; (13)biP=1-kpNi-1∑j=1NiZc(xj)-Zk(xj)，(14)式中：Gij为聚类联通函数，Gij=1表示集群i与j在空间中连续，否则这两个集群在空间上不连续；biP为动态点的评分；kp为当深度差异高于阈值时，才执行动态评分；λR和λP分别为空间正则化与几何正则化的权重．式(12)和(13)构成S(b)作为D(ξ,b)的高动态残差的补充．而式(14)对于ξ是非线性的，在已知ξ的前提下是可以求得b的值．因此使用迭代重加权最小二乘法求解ξ，然后再求解b的值，重复多次，可以得到最好的收敛结果．2.4　面元重构算法相机位姿评估面元是可以表示静态场景的3D模型，一个面元是一个3D圆盘，具有位置和法向量p,n∈R3，颜色c∈N3，半径r∈R，生存能力w∈[0,1]，其中w→1表示能够生存等信息．在每个时间簇中，系统将最后的RGB-D图像对(C,Z)以及属于背景的每个点的像素分割成图像BD作为输入，采用文献[21-22]中预处理和数据关联的方法，对面元的可行性进行判断，删除动态输入点，并进行融合．2.4.1　面元可行性若一个面元被反复观察并与静态输入点相匹配，则可以认为它是可行的．这些条件确保可行性面元既是静态的也是真实的，这种情况下令w→1．当模型中引入新的面元可行性较低时，令w→0．在随后的观测中，w通过匹配输入点的对数概率进行更新．在面元重构算法中，只有与静态点重复匹配，其可行性才会增加(BD0.5)，随着动态特性的增加，其可行性会自动下降(BD0.5)，即signlnw1-w=-1     (w0.5);0       (w=0.5);1        (其他). (15)2.4.2　加权融合在融合过程中，采用加权平均的方式更新面元的位置、颜色、法线．通过BD，γDi和vD这3个权重来代表每个输入点的质量．对于每个像素点p对应的坐标xp∈Ω，有对应的高斯权重γDp∈[0,1]表示其距离图像中心xc[21]，高斯权重定义为γDp=exp1-xp-xc/2σγ2xc．(16)速度权重vD∈[0,1]表示在不同运动期间中观察到的点，缓慢运动期间权重值高，当快速运动时会惩罚其记录的点将权重降低，速度权重定义为vD=max1-1-maxξ,0.150.15,0.5∈[0,1]．(17)最后，累加当前帧生存能力的对数概率到模型中对应的面元中，若累加值为正数，则说明该面元属于静态的概率较大；为负数则说明该面元可能是动态的．同时，可以将连续10帧对数概率累加值为负数的面元进行剔除，参考文献[21]中方法对面元空间进行检查，剔除违反空间条件的面元．3 实验与分析3.1　实验数据为验证本文算法有效性，在TUM数据集上进行实验，该数据集是慕尼黑工业大学提供RGB-D开源数据集[23]．将数据集分为静态、低动态、高动态三类环境，在这三类环境中评估算法的综合性能．本实验在神舟Z7-KP5D1笔记本电脑(Intel Core i5 7300HQ，16 GiB内存，不使用GPU加速)Ubuntu 16.04环境下进行实验．本文算法须要进行静态概率点设置，在最开始的5帧不进行比对测试，只针对后续的序列进行测试，并设置其他参数如下：残差模型部分，如图3所示，图3(a)为RGB-D图像．图3(b)仅使用几何信息进行运动分割，因信息缺失无法将动态物体完全包含，导致分割错误．图3(c)使用光度信息进行分割，设置光度信息权重αE=0，导致过度分割将静态物体也分割为动态．图3(d)融合光度和几何误差，并设置合适的权重，分割结果较为准确，但仍有部分分割错误．图3(e)仅使用边缘误差并设置高权值，导致图像过度分割效果．图3(f)中平衡几何、光度、边缘信息误差关系，通过实验设置合适权重αI=0.15,η=0.35分割效果较准确．式(9)可动态调节αE，改变边缘信息在残差模型中的权重，提高算法的鲁棒性．残差正则化部分，将式(13)中的λR和λP分别设置为0.5和1.5，分别表示空间正则化与几何正则化的在高残差值场景中所占权重[13]．10.13245/j.hust.238606.F003图3信息权重示意图为了评估本算法的精确度，与三种算法进行对比：结合光流估计和场景流的VO-SF算法[13]，结合场景流的SF算法[14]，基于边缘对齐的REVO算法[20]．REVO算法使用GPU加速DT对齐误差的计算，但未对动态物体进行处理．对比目的在于评估不同边缘信息提取方案对于算法精确度和实时性的影响[12]．VO-SF和SF算法均采用场景流的方法对图像进行运动分割，但VO-SF算法无法处理动态物体占比较大的情况．SF算法针对动态物体进行优化，但未将边缘信息融入到残差模型中．3.2　运动分割运动分割是为了区分动态、静态物体后，正确剔除动态物体，仅保留静态物体进行后续的位姿估计．若分割错误，则动态物体会被纳入后续的位姿估算中，导致估算结果错误，发生分割不足或过度分割，因此动态环境下正确的运动分割很重要．本文算法与SF算法[14]在动态环境下对比结果如图4所示．图4(a)为数据集原始RGB图像，图4(b)为提取边缘信息并进行聚类处理后的图像，图4(c)为SF算法分割结果，图4(d)为本算法分割结果，分割图中蓝色部分为静态部分，红色部分为动态部分．通过比对图4(d)图中的(1)、(2)、(3)表明当动态物体占比较大时，通过引入边缘信息可以获得更加精准的运动分割．图(4)表明当移动的人远离相机且边缘信息不足时，无法识别动态物体．图(5)中右侧桌椅因靠近移动的人而被分割成动态物体，与SF算法相比，本文算法均准确将左右两侧移动的人识别出来．图(6)、(7)因深度信息过少且相机移动过快等原因，导致分割效果较差．与SF算法相比分割效果仍好于SF算法．10.13245/j.hust.238606.F004图4运动分割示意图3.3　评估指标为了评价位姿估计的准确性，根据Sturm等[23]提出指标通过平移和旋转相对姿势误差(RPE)测量每秒的平均局部漂移；平移绝对轨迹误差(ATE)来测量全局轨迹质量．比较静态、低动态和高动态3种不同场景，实验结果见表1和表2．10.13245/j.hust.238606.T001表1相对位姿误差序列平移RMSE/(cm∙s-1)旋转RMSE/((°)∙s-1)REVOVO-SFSF本文算法REVOVO-SFSF本文算法静态fr1/rpy3.5684.5542.3012.4552.9982.3711.4231.507fr2/desk1.8762.3893.2472.8470.6711.4822.1762.085低动态fr3/sit_static1.3205.7211.1030.9570.7190.7150.4310.478fr3/sit_xyz2.0392.4382.8472.9460.6741.4400.9270.871高动态fr3/walk_static31.63610.5471.3241.10812.5611.6850.3840.426fr3/walk_xyz22.14927.74612.12611.3873.8475.1142.6682.375fr3/walk_halfsphere46.30533.44220.74819.8538.9426.6955.0454.87310.13245/j.hust.238606.T002表2绝对轨迹误差序列平移RMSEREVOVO-SFSF本文算法静态fr1/rpy3.5745.1521.4181.395fr2/desk1.8765.6072.3412.357低动态fr3/sit_static1.3202.91.3851.415fr3/sit_xyz2.03911.4174.0744.138高动态fr3/walk_static31.63632.7101.4251.451fr3/walk_xyz22.14990.612.79511.842fr3/walk_halfsphere46.30563.839.13437.587cm在静态和低动态场景中，本文算法与SF算法具有相似结果，优于仅使用边缘信息的REVO算法．在高动态环境中，本文算法具有更高的精确度，因VO-SF算法无法处理动态物体在图像中占比超过50%的序列，故本文算法相比VO-SF算法精确度提高了49.8%．本文算法引入边缘信息，相对于SF算法精确度提高了6.2%．相对于仅使用边缘信息的REVO算法精确度提升了53%．通过表2的高动态序列fr3/walking表明本文算法通过引入边缘信息并通过帧-模型对齐策略势，明显好于 VO-SF算法和REVO算法，相对于SF算法有一定提升，减少了轨迹漂移误差．本文算法采用QVGA图像(320×240)进行实验，REVO算法仅使用边缘信息进行处理，每帧运行时间速度为55 ms；VO-SF算法每帧运行速度为80 ms；SF算法因采用非线性优化方法，每帧运行速度为30 ms；本文算法因引入边缘信息，须要进行边缘信息的提取及边缘误差对齐计算，因此运行时间高于SF，但仍满足实时性要求，其运行时间为70 ms/帧．4 结语提出一种融合边缘信息的视觉里程计，将边缘信息引入图像的运动分割和位姿估计，并改进边缘信息提取方法，减少提取所需时间．边缘信息的引入可以获得更好的鲁棒性，并在光照变化场景中获得正确的运动分割．在几何纹理较少的场景，通过提高光度和深度信息的所占权重，避免因几何特征过少导致的误差．实验结果表明：本算法在动态环境下可以取得较准确结果，在模糊场景中的分割效果较好，但对于相机较远的动态物体，仍无法正确识别．因此，下一步工作将对算法改进，引入深度学习等方法，针对距离相机较远部分的动态物体的识别问题进行优化提高算法精确度，并针对算法运行时间进行优化提高算法效率．