一种基于地平面的单目视觉辅助激光雷达里程计方法与流程

本发明涉及机器人识别技术领域，尤其是涉及一种基于地平面的单目视觉辅助激光雷达里程计方法。

背景技术：

同时定位与建图(slam，simultaneouslocalizationandmapping)是机器人领域的一项关键技术，是机器人能够自主运行的一项基本要求。激光雷达和视觉传感器是slam中的两种主流传感器，近年来基于这两种传感器的slam算法得到广泛的研究和应用。视觉slam方面，产生了orb-slam2、dso、vins等为代表的优秀方案。激光slam中则形成了loam、imls-slam、segmatch等框架。然而，单一的传感器总是存在某种不足，例如视觉传感器对环境纹理及光照条件敏感，而激光雷达存在由本体运动引起的点云畸变问题以及回环检测能力不足等。因此，将视觉传感器与激光雷达融合逐渐成为当前的主流技术，例如：

demo是一种通过将激光雷达的深度测量用于恢复图像中特征点深度从而增强视觉里程计(在slam中，通过连续地定位估计机器人的运动轨迹的过程又称为里程计)的方法；v-loam进一步利用demo的视觉里程计实现高频运动估计来矫正激光雷达点云畸变并为激光里程计提供运动估计初值，从而实现更加精确的激光里程计；limo中提出了一种新的将激光雷达测量用于图像特征深度恢复方法从而实现无尺度漂移的视觉里程计，并使用深度学习算法剔除错误特征点。但是，上述这些传统的通过激光雷达距离测量恢复图像特征点深度的方法容易由于错误的深度匹配而引入额外的几何残差，从而降低系统精度和稳定性。而且demo和limo这类方法丢弃了大量有用的激光雷达测量信息。

为了最大化利用相机和激光雷达的测量信息，同时避免不佳的深度匹配的影响，tvlo提出分别对两种传感器独立地运行里程计和建图流程，最后对它们的残差进行联合优化，以此实现视觉和激光的紧耦合。然而，基于两种传感器同时进行里程计和建图需要很大的系统开销，不适用于嵌入式平台等低算力场合。

因此，研究如何分配少量系统资源将视觉传感器用于辅助激光雷达进行同时定位与建图，弥补激光雷达在自运动畸变和回环检测等方面的不足，避免现有方法计算复杂度高、系统稳定性差等问题，具有十分重要的意义。

技术实现要素：

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于地平面的单目视觉辅助激光雷达里程计方法，利用激光里程计中的地面点云高效提取图像中的地面特征点，基于单应性变换实现高效的绝对尺度相机运动估计，进而将运动估计用于矫正激光里程计中的自运动点云畸变和位姿优化，实现高效精准的里程计。

本发明的目的可以通过以下技术方案来实现：

一种基于地平面的单目视觉辅助激光雷达里程计方法，包括以下步骤：

s1、从激光点云中分割出地面点云用于筛选图像中的地面orb特征点；

s2、对相邻两帧图像中的地面orb特征点进行匹配，进行共线特征点搜索并通过交比不变性验证特征匹配的正确性，通过验证后的特征匹配集合用于计算图像之间的单应性变换矩阵；

s3、对单应性变换矩阵进行分解，并利用已知的相机高度，得到绝对尺度的估计相机运动；

s4、利用估计的相机运动对激光雷达自运动造成的点云畸变进行矫正；

s5、对矫正后的激光点云进行分割并提取直线和平面特征；

s6、将相邻两帧激光点云中的直线和平面特征进行匹配，通过levenberg-marquardt算法迭代求解位姿，并利用相机运动先验加速迭代过程。

进一步地，所述步骤s1中，利用激光点云地面点提取图像中的地面orb特征需要将激光点云投影到图像中，其计算表示式为：

其中，qi为激光点云中的地面点，为相机与激光雷达之间的标定矩阵，p0为相机的投影矩阵，qi为投影点的齐次像素坐标；

每一个激光投影点可以确定一个以其为中心，半径为r的窗口，所有投影点窗口外的特征点首先被剔除；其次，若窗口中存在多个特征点，则比较它们的描述子差异，选择描述子差异最大的特征点作为该窗口的地面特征点。

进一步地，所述步骤s2中，特征校验采用的交比不变性计算表示式为：

cross(x1,x2,x3,x4)＝cross(x′1,x′2,x′3,x′4)

其中，x1,x2,x3,x4为图像中共线的4个特征点，x′1,x′2,x′3,x′4为另一图像中匹配的4个特征点；交比定义为：

其中，在几何上表示xi到xj的欧式距离。

进一步地，所述步骤s2中，单应性矩阵的计算表示式为：

ei(h)＝ρ(d(x′i,hxi))+ρ(d(xi,h^-1x′i))

其中，xi和x′i为两帧图像上经过校验的匹配特征点，d(·)表示两像素点的欧式距离，ρ(·)是为避免较大误差主导优化方向而引入的鲁棒核函数。

进一步地，所述步骤s4中，点云畸变矫正后齐次坐标的计算表示式为：

其中，pi为矫正前的激光点坐标，为通过相机图像估计出的相机运动，为相机和激光雷达之间的标定矩阵，τi表示第i个时刻的时间。

与现有技术相比，本发明具有以下有益效果：

1、本发明提出的单目视觉辅助激光雷达里程计方法，能够有效利用单目相机估计相对运动，对自运动畸变的激光点云进行矫正，从而提高激光里程计精度。

2、本发明利用视觉前端的相对运动可以加速激光里程计的优化求解过程，保证系统的实时性。相较目前的视觉和激光雷达融合的里程计方法，本发明高效利用了两种传感器中的地平面信息，最大化发挥激光雷达性能的同时避免了系统精度和稳定性受错误匹配影响的问题。

3、本发明提出的方法可以在低算力平台上实时地运行。

4、步骤s6中利用所述步骤s3中得到的绝对尺度相机运动，可以加速激光特征点云匹配的迭代优化过程，实现高效精确的激光里程计。

附图说明

图1为本发明的流程示意图。

图2为实施例提供的特征点筛选效果图。

图3为实施例提供的特征点匹配校验效果图。

图4为实施例提供的交比不变性示意图。

图5为实施例提供的室内场景实验验证结果图。

图6为实施例提供的室外场景实验验证结果图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

本实施例提供了一种基于地平面的单目视觉辅助激光雷达里程计方法，是一种单目视觉和激光雷达紧耦合的里程计方案，利用激光里程计中的地面点云高效提取图像中的地面特征点，基于单应性变换实现高效的绝对尺度相机运动估计，进而将运动估计用于矫正激光里程计中的自运动点云畸变和位姿优化。针对地面的弱纹理特性导致的错误匹配问题，提出通过共线特征点搜索和交比不变性校验来挑选正确的匹配特征，保证单应性矩阵计算的正确性。

如图1所示，本实施例的具体步骤如下：

步骤s1、从激光点云中分割出地面点云用于提取图像中的地面orb特征点。

步骤s2、对相邻两帧图像中的地面orb特征点进行匹配，并通过交比不变性验证特征匹配的正确性，保留下来的特征匹配集合用于计算图像之间的单应性变换矩阵。

步骤s3、对单应性变换矩阵进行分解，并利用已知的相机高度，得到绝对尺度的相机运动估计。

步骤s4、利用估计的相机运动矫正激光雷达自运动造成的点云畸变。

步骤s5、对矫正后的激光点云进行分割并提取直线和平面特征。

步骤s6、将相邻两帧激光点云中的直线和平面特征进行匹配，最后通过现有成熟的levenberg-marquardt算法迭代求解更加精确的位姿，并利用相机运动先验加速迭代过程。

步骤的进一步展开如下：

一、图像地平面特征点提取

地平面特征点提取的关键在于如何确定图像中的地平面。本实施例将激光雷达后端流程中提取的地平面激光点云投影到图像上，实现高效且鲁棒地提取图像中的地面特征点。地平面在图像中主要位于下半部分，因此首先在图像的下半部分提取orb特征点。然后，利用相机与激光雷达之间的变换矩阵和相机的投影模型，将激光点云中的地平面点投影到图像上。假设标定好的相机与激光雷达间的变换矩阵为相机的投影矩阵为p0，则地面激光点云中给定一点qi的投影点齐次像素坐标为：

每一个激光投影点可以确定一个以其为中心，半径为r的窗口，所有投影点窗口外的特征点首先被剔除。进一步地，窗口中如果存在多个特征点，则比较它们的描述子差异，选择描述子差异最大的特征点作为该窗口的地面特征点。地平面特征提取效果如图2中所示，从图中可以看出上述步骤能排除大量非地面特征点同时保留大量正确地面特征点，筛选地面特征点的效果较好。

二、基于交比不变性的特征匹配验证

orb特征具有方向和尺度不变性，是一种快速鲁棒的图像特征。然而，由于地平面通常缺乏丰富的纹理，各特征点之间的描述子差异并不十分明显，导致误匹配的发生，即所谓的匹配外点。基于ransac的特征匹配外点剔除算法是目前普遍采用的方法。然而，当错误匹配的数量较多时，基于ransac的方法无法保证正确地剔除外点(从大量错误匹配中找出少量正确匹配)。单应性矩阵的计算至少只需要4对匹配特征，因此本实施例中利用交比不变性进行特征匹配的校验，并且只采用满足交比不变性的特征点对进行单应性矩阵的计算。

交比是图像投影几何中的一种不变量。给定图像上共线四点x1,x2,x3,x4，它们的交比定义为：

其中，在几何上表示xi到xj的欧式距离。若另一图像上与它们匹配的特征点为x′1,x′2,x′3,x′4，则交比不变性可表达为：

cross(x1,x2,x3,x4)＝cross(x′1,x′2,x′3,x′4)

交比不变性的几何表达如图3所示，该性质本质上反应的是投影变换下距离比率之比的不变性。

本实施例通过交比不变性来验证特征匹配的正确性。首先搜索所有共线点数量在4点以上的特征点。然后对每四个共线特征点及它们的匹配特征点计算交比，一旦两个交比相等就接受所有的4个匹配点。需要注意的是对于不满足交比不变性的特征点并不会立即剔除，因为它们之中可能存在正确的匹配点。本实施例在所有大于4点的共线特征验证结束之后剔除未通过交比不变性校验的特征点对。经过共线特征点搜索和交比不变性验证后的特征匹配如图4所示，经校验后的特征匹配绝大多数为正确匹配。

三、绝对尺度相机运动估计

单目相机图像之间的帧间运动估计存在尺度歧义，该尺度由两帧图像对应的相机平移决定。因此，不同图像之间估计的运动将在不同的尺度之下。若直接将不同尺度下估计出的相机运动用于激光点云的矫正，将导致两组点云被矫正到不同尺度而发生误匹配，使整个系统失效。为了解决此问题，本实施例中利用图像中的地平面信息和已知的相机安装高度来估计尺度因子，从而将不同时刻的相机运动估计统一在公制尺度下。

通常，相机是固定安装在平台上，其相对于地面的高度是已知的，因此可以利用相机高度这一先验信息来估计尺度。假设已知的相机高度为h^*，通过图像估计出的相机高度信息为h，则尺度因子s＝h/h^*。

单应性矩阵描述了相机运动时平面上点的变换关系，其定义为：

其中，k相机内参，r和t分别为相机的旋转和平移。单应性矩阵至少可通过四个共面的匹配点计算得到。从弱纹理地面提取的图像特征含有较多噪声，因此容易产生错误匹配。为了保证鲁棒地通过地面特征点计算h矩阵，本实施例采用优化的方法。通过最小化由对称转移误差定义的代价函数得到最优的单应性变换：

ei(h)＝ρ(d(x′i,hxi))+ρ(d(xi,h^-1x′i))

其中，xi和x′i为两帧图像上经过校验的匹配特征点，d(·)表示两像素点的欧式距离，ρ(·)是为避免较大误差主导优化方向而引入的鲁棒核函数。本实施例采用huber核：

其中，k是预先给定的阈值。首先采用归一化的直接线性变换算法对满足交比不变性的四对特征匹配计算得到h矩阵的初始估计，然后基于该初值通过levenberg-marquardt算法优化代价函数得到最优的单应性矩阵。

对计算出的h矩阵进行分解，就能得到相机运动的旋转矩阵r和平移向量t、地平面法向量n和相机高度h。由于旋转矩阵满足正交矩阵约束，因此只需要矫正平移t的尺度：

最后得到绝对尺度的估计相机运动即为：

四、矫正激光雷达自运动造成的点云畸变

点云畸变矫正后齐次坐标的计算表示式为：

其中，pi为矫正前的激光点坐标，为通过相机图像估计出的相机运动，为相机和激光雷达之间的标定矩阵，τi表示第i个时刻的时间。

五、激光点云特征提取

矫正后的激光点云首先根据距离投影得到一幅深度图，然后利用基于图像的方法进行分割，得到分割后的点云。点云分割后得到的点云簇利用基于曲率的方法提取直线特征和平面特征。

六、相机运动先验加速特征匹配

激光里程计估计两帧连续激光点云之间的运动，通过执行直线特征点到直线和平面特征点到平面的扫描匹配，并最小化所有匹配之间的几何距离求取两帧点云之间的变换。特征点之间的匹配采用最近点搜索，即认为两帧点云中空间上距离最近的点之间是对应的。这种匹配搜索方式需要一个良好的位姿初值，否则计算求解的激光里程计位姿可能是次优的。由于相机与激光雷达之间的相对位姿是已知的，利用这一位姿变换将相机的运动估计变换到激光雷达坐标系下，可以提供一个良好的初值。利用这一位姿初值对点云进行变换后，两帧点云之间的初始重合度更高，最近点搜索匹配算法可以更加快速地收敛。

本实施例的实际效果通过在自行搭建的机器人平台上实施进行验证。移动机器人上搭载了研华ark-3500工控机、velodyne激光雷达vlp-16、fotric红外可见光双视云台相机、wi-fi模块、电池等。vlp-16为本实施例所述方法提供周围环境的激光点云数据，云台中的可见光相机提供环境的图像数据。定位和建图过程中，云台是固定不动的。由于实验平台没有imu设备，实际场景实验只与无imu传感器数据支持的lego-loam(以下称lego-loam-noimu)算法进行对比。

实验分别测试了室内场景和室外场景。室内场景环境为实验室环境，包含弱纹理的地板、桌椅、白墙、玻璃墙等物体。实验时控制机器人绕办公桌行驶一圈，通过尺规工具测量，实际运行轨迹近似为长11.4米、宽4.2米的长方形轨迹，本实施例所述方法和lego-loam-noimu算法估计的轨迹如图5所示。从图5中可以看出两种算法估计的轨迹与尺规工具测量的轨迹基本符合，lego-loam-noimu估计的轨迹有更明显的误差漂移，本实施例提出的方法估计的轨迹明显更接近真实轨迹(长方形轨迹)。

室外场景为某大学校园内的道路环境。

本实施例的方法和lego-loam-noimu估计轨迹实验结果如图6所示。从图6中可以看出lego-loam-noimu估计的轨迹(lego-noimu)逐渐偏离，估计轨迹的终点未与起点(坐标(0,0)处)重合，整条轨迹与实际运行轨迹相差较大。而实施例所述方法估计的轨迹(ours)始终与实际运行轨迹基本吻合，轨迹终点与起点重合，表明本实施例所述方法优于lego-loam-noimu算法，具有足够的精度和场景鲁棒性。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。