一种动态环境下融合边缘信息的稠密视觉里程计方法与流程

本发明属于视觉slam领域，特别是一种动态环境下融合边缘信息的稠密视觉里程计方法。

背景技术：

视觉里程计(visualodometry)是一种利用视觉传感器信息对机器人或其他载体进行准确的位姿估计的方法，相比于使用激光传感器的方法，视觉信息更加丰富，可用于三维重建、物体识别、语义分割等。同时，因其成本低、体积小、精度高等优点，而广泛的应用于机器人自主导航、自动驾驶、增强现实等领域。

为了简化定位与建图问题，现阶段大部分视觉里程计都是假设相机处于静态环境或者图像信息的变化仅依赖于相机自身的运动。然而实际的环境中不可避免的存在动态物体，当动态物体在图像中所占比重较小时，一般可采用随机采样一致性(ransac)或鲁棒huber函数等概率方法将动态部分当作噪声剔除掉。若动态物体在图像中占比较大时，则无法通过概率方法剔除，动态特征会影响到位姿估计，得到错误的位姿，最终导致定位与建图失败。

技术实现要素：

本发明旨在解决以上现有技术的问题。提出了一种能够在动态环境下实时且准确的进行机器人自主定位的稠密视觉里程计方法。

鉴于此，本发明采用的技术方案如下：一种动态环境下融合边缘信息的稠密视觉里程计方法，包括以下步骤：

s1，使用k-means聚类法，将图像分为n个集群。

s2，将聚类后的边缘图与灰度图分别坐标变换到关键帧及其dt图上，分别计算出光度及几何一致性误差与边缘对齐误差。

s3，将s2中的两种误差进行数据融合，构建融合边缘信息的聚类残差模型，并将其正则化。

s4，引入平均背景深度进行运动分割，基于s3构建引入平均背景深度的聚类残差模型。

s5，构建基于t分布的非参数统计模型，根据自适应阈值构建场景分割模型，并计算得到加权聚类残差。

s6，将步骤s5得到的加权聚类残差加入到优化函数中进行位姿估算，并将得到的位姿进行跟踪质量检查，更新下次迭代的变换矩阵或关键帧。

本发明的优点及有益效果如下：

本发明针对传统的视觉里程计方法在动态环境下鲁棒性和精度不高的问题，提出一种融合边缘信息的稠密视觉里程计方法。该算法将边缘信息加入到运动分割与位姿估计中，利用边缘信息对于光照变化以及图像模糊不敏感的优点，使得本发明在多种不同的环境下拥有更好的鲁棒性与更高的精度，同时对于几何纹理较少的环境，更多的采用光度结合深度信息来进行位姿估计，降低环境特征过少而导致的误差。另外，本发明通过引入平均背景深度，使距离背景较远的动态部分有更大的残差值，而距离背景较近的动态部分本身就有较大的残差，以此来扩大动、静部分残差差距而有利于正确的运动分割。通过边缘信息的融合使得算法在静态环境下以及富有挑战性的高动态环境下都能取得较好的结果，能够检测出距离相机较远的动态部分，在信息丢失、图像模糊等情况下也有更好的鲁棒性。

附图说明

图1是本发明方法的实现框架；

图2是非参数统计模型拟合实际实验残差图；

图3是对fr3/walking_xyz数据进行实验的相对平移误差图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

参见图1，本方法的主要步骤为：

s1，使用k-means聚类法，将图像分为n个集群。

s2，将聚类后的边缘图与灰度图分别坐标变换到关键帧及其dt图上，分别计算出光度及几何一致性误差与边缘对齐误差。

s3，将s2中的两种误差进行数据融合，构建融合边缘信息的聚类残差模型，并将其正则化以构建与动态物体有较高相关性的残差模型。

s4，引入平均背景深度来辅助算法进行运动分割，基于s3构建引入平均背景深度的聚类残差模型。

s5，构建基于t分布的非参数统计模型来拟合聚类残差模型，根据自适应阈值构建场景分割模型，并计算得到加权聚类残差。

s6，将步骤s5得到的加权聚类残差加入到优化函数中进行位姿估算，并将得到的位姿进行跟踪质量检查，更新下次迭代的变换矩阵或关键帧。

步骤s1的具体实施过程为：

采用k-means聚类法，通过计算每一个三维空间点距离集群中心点的距离，来确定该空间点属于哪一个集群。

步骤s2的具体实施过程为：

s21：将聚类后的当前帧fc的灰度图坐标变换到关键帧fk，根据光度及几何一致性构建残差模型，具体公式如下：

式中，和分别表示点i对应的光度误差与深度误差。为当前帧的第i个像素点。ik和zk表示关键帧的灰度图与深度图，ic和zc表示当前帧的灰度图与深度图。|·|z表示三维坐标点的z坐标值。tkc表示从当前帧到关键帧的坐标变换矩阵。ξkc为当前帧到关键帧坐标变换的李代数。π^-1为逆投影函数。ω为坐标变换函数。

s22：将聚类后的边缘图坐标变换到关键帧的dt图，得到基于边缘对齐的残差模型，具体公式如下：

式中，表示边缘点i对应的dt误差。dtk表示关键帧的dt误差函数。

步骤s3的具体实施过程为：

s31：将边缘对齐误差引入到光度及几何误差中构建融合边缘信息的残差模型，并在计算中通过去除每个集群的遮挡点从而降低遮挡的影响，具体公式如下：

式中，rn表示第n个集群的残差值。sn表示第n个集群中所有深度有效的像素个数。on表示第n个集群中遮挡的像素个数。en表示第n个集群中所有深度有效的边缘点。为第n个群的平均深度。αi与αe分别表示光度误差与dt误差在残差模型中所占权重；表示边缘点i对应的dt误差；

s32：为了加强算法应对各种复杂环境的鲁棒性，构建权重αi与αe的公式如下：

式中，vc为相机运动速度。n为集群总数量。μ为由αi到αe的尺度系数。在边缘信息丰富(边缘点占比高)或相机运动速度较快时(容易产生模糊图像)，αi有较大值。相反，αe较小时边缘信息影响较小。

s33：为了使得残差模型与动态物体有较高的相关性，将残差进行正则化，具体公式如下：

式中，e(rn)为非线性优化求解的最小误差。gmn为聚类连通函数，为1表示m与n集群连通。λr与λt表示对非线性优化函数求解的权重。rn与rm分别表示第n个集群和第m个集群的残差值。rn,c-1表示上一帧中第n个集群的残差值，因为在时间上，物体都是趋于保持移动或静止的，通过求解最小问题来得到最佳rn，正则化后的残差与动态物体有较高的相关性。

步骤s4的具体实施过程为：

s41：动态物体一般都为独立的刚体且远离背景，通过扩大远离背景部分的集群残差，使得靠近背景的集群偏向于分割为静态部分，而远离背景的集群有更大几率被判定为动态部分。平均背景深度的引入有助于正确的运动分割，定义平均背景深度公式为：

式中，表示当前帧平均背景深度。表示上一帧平均背景深度。dc-1为上一帧所分割出的动态集群数量。表示第n个集群的平均深度。λd为上一帧平均背景深度对计算当前帧平均背景深度影响的权重。

s42：根据s41的公式构建引入平均背景深度后的残差模型为：

式中，δn为引入平均背景深度后的第n个集群的残差值。表示第n个集群与背景的距离。距离背景较近的部分残差值几乎不变，而距离较远的部分会产生更大的残差值，有利于正确的运动分割。

步骤s5的具体实施过程为：

s51：根据聚类残差分布的普遍特征，建立一个基于t分布的非参数统计模型，如图2所示，非参数统计模型能够较好地拟合实际实验残差的直方图：

σ＝1.4826median{|δn-μ|}

式中，wn为t分布的概率值。v0为t分布的自由度，它决定了分布曲线的陡峭程度。平均值μ决定分布中心所处位置。σ为统计方差，通过中位数绝对偏差估算得到；

s52：在各种复杂的场景下，采用固定阈值会使得分割效果不佳。因此，使用自适应阈值构建场景分割模型，残差高于自适应阈值的集群可判定为动态部分，而低于阈值的集群可作为静态部分或赋权部分加入到位姿估算中，具体公式如下：

式中，代表第n个集群的权重，为0时表示该集群为动态集群，为1时表示为静态集群。vc为相机速度。σ为统计方差。β用于调整因变量到残差尺度的系数，通过公式计算得到。

步骤s6的具体实施过程为：

s61：采用cauchym-estimator模型，通过加权聚类残差来构建非线性优化函数，用以更准确的估计帧间运动，具体公式如下：

式中，m表示所有深度有效的点的数量。表示属于第n个集群的点i的权重。ξ为所求最优位姿的李代数。c定义了f(r)的拐点，调整c可增加对高残差的鲁棒性。通过最小化非线性优化函数求ξ以得到最佳的帧间运动；

s62：采用边缘信息来评估跟踪质量，通过将先前跟踪过的n帧的边缘投影到当前帧，得到像素点重叠计数图，通过对相同重叠数构建直方图，来获取图像重叠情况，具体公式如下：

式中h(j)表示重叠数为j的点的数量，h(0)表示未重叠的点的数量。γj与γ0分别表示重叠数为j和0的点在跟踪质量评估中所占的权重。当边缘重叠的加权和小于非重叠的数量时，说明跟踪质量较差，此时将上一跟踪较好的帧更新为关键帧。

完成上述所有步骤之后，便可以得到当前帧最优位姿，并为下一次更新迭代做好了准备。

对tum数据集的高动态环境fr3/walking_xyz数据进行实验，得到相对位姿误差(rpe)如图3所示，红色为本发明算法(为图中最下面的一条曲线)，其他为现有算法，横坐标为数据帧，纵坐标为平移误差。实验表明本发明算法在动态环境下比现有算法有更低的相对位姿误差，本发明算法有更高的精度与鲁棒性。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。