CN113658337B - 一种基于车辙线的多模态里程计方法 - Google Patents

Abstract

一种基于车辙线的多模态里程计方法，本发明涉及基于车辙线的多模态里程计方法。本发明的目的是为了解决现有点特征对于环境依赖较大，在纹理缺失等场景中表现不佳，导致无法得到准确的机器人里程信息问题。过程为：一、对非重叠视角下多相机和激光雷达进行联合标定；二、设定标定误差约束；三、对激光点云进行去噪预处理；四、得到边缘、平面特征误差模型；五、获得图像帧与帧之间点特征和图像帧与帧之间线特征；六、获得点线特征误差模型；七、获得深度恢复误差模型；八、构建视觉激光的数据关联模型；九、对智能体的位姿进行增量式更新；十、重复三至九，估计连续帧位姿变换，完成多模态里程计构建。本发明用于机器人SLAM领域。

Description

一种基于车辙线的多模态里程计方法

技术领域

本发明属于机器人SLAM领域(Simultaneous Localization and Mapping)，具体涉及基于车辙信息的多模态里程计方法。

背景技术

近年来，随着人工智能的发展，移动机器人也开始在社会各个领域迅速发展，在特殊场景去替代人们完成一些工作。但是，在沙地或土质较为松软的环境中，由于路面的变形，车轮会产生滑转下陷等问题导致轮式里程计失效，从而无法得到准确的机器人里程信息。而SLAM作为一种定位与地图重建技术，通过各类传感器的应用以及相互融合，可以使得移动机器人具有感知自身位置和周围环境的能力。目前没有一种普适的SLAM方案，单一传感器SLAM算法有其不可避免的传感器自身问题，尤其是面对复杂恶劣的环境，所以需要多传感器之间的融合提高SLAM算法的精度、冗余性以及鲁棒性。目前SLAM中最广泛使用的特征是点特征，在特征提取、匹配和表示上都比较成熟。然而点特征对于环境依赖较大，在纹理缺失等场景中表现不佳，导致无法得到准确的机器人里程信息；

发明内容

本发明的目的是为了解决现有点特征对于环境依赖较大，在纹理缺失等场景中表现不佳，导致无法得到准确的机器人里程信息问题，而提出一种基于车辙线的多模态里程计方法。

一种基于车辙线的多模态里程计方法具体过程为：

步骤一、对非重叠视角下多相机和激光雷达进行联合标定；

步骤二、对非重叠视角下多相机和激光雷达的联合标定设定标定误差约束；

步骤三、激光雷达获取激光点云，利用点云分割算法对激光点云进行去噪预处理；

步骤四、利用帧-图的匹配方式对步骤三去噪预处理后的激光点云提取点云特征，对提取的点云特征进行点云匹配，得到边缘、平面特征误差模型；

步骤五、多视角相机拍摄彩色图像，提取点线特征，获得匹配追踪得到的图像帧与帧之间点特征和图像帧与帧之间线特征；

步骤六、将步骤五匹配追踪得到的点特征、线特征投影到图像坐标系，投影到图像坐标系中的点与相机成像平面中对应的2D点线建立约束关系，获得点线特征误差模型；

步骤七、对步骤三去噪预处理后的激光点云进行处理，获得深度恢复误差模型；

步骤八、基于步骤二、步骤四、步骤六、步骤七，构建视觉激光的数据关联模型；

步骤九、基于步骤八使用因子图算法对智能体的位姿进行增量式更新；

步骤十、重复步骤三至步骤九，估计连续帧位姿变换，完成多模态里程计构建。

本发明的有益效果为：

在沙土或土质松软环境中，在经过机器人行驶之后，存在着“车辙”这一明显的特征，车辙线是一个良好的线特征，可以与图像中的点特征起到良好的信息互补作用。

本发明针对单个传感器的里程计在复杂未知环境下定位失败，以及传统轮式里程计在沙地等松软环境中因打滑失效等情况，提出了一种基于车辙线的多模态里程计方法，利用激光雷达和多相机视觉系统观测车辙线轨迹和智能体周围环境，如图1。利用多相机视觉系统提取图像中的视觉信息，通过特征匹配算法建立当前帧与上一帧的约束关系，完成点线特征追踪；利用激光雷达匹配相邻帧激光点云，建立平面点，边缘点误差模型，并且通过带有深度信息的激光点云与视觉特征建立关联模型，将视觉里程计，激光里程计紧耦合，并作为因子加入因子图中，实现SLAM系统高效增量式更新；本发明提高了单一视觉传感器里程计特征提取的稳定性，融合了特征点、线数据信息，并且建立紧耦合的激光视觉SLAM系统，解决了单个传感器里程计在复杂多样定位失败以及轮式里程计在沙地等松软环境中因打滑失效的问题，利用紧耦合的激光视觉里程计解决了复杂条件下里程计精度不高且不稳定的问题，可以有效可靠的估计移动机器人里程信息，进一步提高了定位精度及稳定性；解决了现有点特征对于环境依赖较大，在纹理缺失等场景中表现不佳，导致无法得到准确的机器人里程信息问题；

本发明利用移动机器人车辙线这一特征，采用点线特征融合，解决了一般的视觉里程计在低纹理条件下，会出现提取特征不足，无法进行特征追踪，从而定位丢失的问题，实现了更为稳定，更高精度的定位工作。

本发明在观测误差模型部分，采用了因子图优化的方式，标定约束因子，激光里程计因子，视觉里程计因子，增量式的更新位姿轨迹，加强了帧与帧之间的约束关系，增加了重投影误差的可靠性，提高了位姿估计的准确性。

附图说明

图1为移动机器人走过沙地等松软环境产生车辙示意图；

图2为算法流程框图；

图3为FLD算法线特征的近线合并示意图；

图4为FLD算法线特征的断线拼接示意图；

图5为点特征的重投影误差示意图，e为重投影误差；

图6为线特征的重投影误差示意图；

图7为多模态里程计因子图模型，F₀为视觉关键帧，m为是二分之一视觉滑动窗口的尺寸，n为激光雷达滑动窗口的尺寸，x_i为观测值。

具体实施方式

具体实施方式一：本实施方式一种基于车辙线的多模态里程计方法具体过程为：

步骤一、对非重叠视角下多相机和激光雷达进行联合标定；

步骤二、对非重叠视角下多相机和激光雷达的联合标定设定标定误差约束；

步骤三、激光雷达获取激光点云，利用点云分割算法对激光点云进行去噪预处理；

步骤四、利用帧-图的匹配方式对步骤三去噪预处理后的激光点云提取点云特征，对提取的点云特征进行点云匹配，得到边缘、平面特征误差模型；

步骤五、多视角相机拍摄彩色图像，提取点线特征，获得匹配追踪得到的图像帧与帧之间点特征和图像帧与帧之间线特征；

步骤六、将步骤五匹配追踪得到的点特征、线特征投影到图像坐标系，投影到图像坐标系中的点与相机成像平面中对应的2D点线建立约束关系(点特征误差方程公式9，线特征误差方程公式12)，获得点线特征误差模型；

步骤七、对步骤三去噪预处理后的激光点云进行处理，获得深度恢复误差模型；

步骤八、基于步骤二、步骤四、步骤六、步骤七，构建视觉激光的数据关联模型；

步骤九、基于步骤八使用因子图算法对智能体的位姿进行增量式更新；

步骤十、重复步骤三至步骤九，估计连续帧位姿变换，完成多模态里程计构建。

具体实施方式二：本实施方式与具体实施方式一不同的是，所述步骤一中对非重叠视角下多相机和激光雷达进行联合标定；具体过程为；

联合标定分为两部分：空间标定和时间标定；

空间标定为：利用张正友的相机标定法分别标定多相机系统的各个单目相机的内参，在单目相机标定完成后，构建多视角相机的约束关系；

多视角相机的约束关系为：使用OpenCV生成点云标定板，在标定过程中读取点云标定板，相机个数，相机内参，在标定过程中仅需要相邻的两个相机能够同时看到标定板的同一部分，即可完成两个相机之间的标定，以此类推，即可完成整个多相机系统的标定工作；

利用开源的Autoware无人驾驶中的标定工具对激光雷达进行标定；

多相机系统中共有5台单目相机，镜头分别向外，构成全景视觉系统，相邻两个相机之间的重叠视野为14度，激光雷达位于五个单目相机的几何中心；根据多相机系统内部单目相机之间的相对位置和几何角度的关系，以及多视角相机的约束关系，完成单目相机之间的坐标转换，再根据单个相机与激光雷达的标定结果，即可完成激光雷达与多相机系统的联合标定(相机系统内部转换关系就是各个相机的相对位置，角度的关系，可以建立一个转换关系)；

时间标定为：利用软硬触发协同统一相机和激光雷达的时间戳；

软触发是在程序中设置相机和激光雷达的同步触发器，硬触发是当激光基线扫描到相机所在角度时，通过FPGA给出触发信号，完成同步触发；

从空间和时间维度完成非重叠视角下多相机和激光雷达的联合标定。

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是，所述单个相机与激光雷达的标定结果表达为：

式中，u、v是空间3D点投影到相机像素坐标系下二维像素点的横纵坐标，f_x、f_y为常数，c_x、c_y为相机内参中的平移参数，表示相机像素坐标系与成像平面相差一个缩放倍数和原点的平移，上式表示为像素坐标在u轴缩放了f_x倍，在v缩放了f_y倍，与此同时像素坐标系原点平移了c_x，c_y；

R为3D点在相机坐标系和雷达坐标系下的旋转矩阵，t为3D点在相机坐标系和雷达坐标系下的平移向量，x、y、z为空间3D点的三维坐标，M为相机内参和相关坐标系的旋转平移的矩阵乘积，m₁₁为M中第一行第一列元素，m₁₂为M中第一行第二列元素，m₁₃为M中第一行第三列元素，m₁₄为M中第一行第四列元素，m₂₁为M中第二行第一列元素，m₂₂为M中第二行第二列元素，m₂₃为M中第二行第三列元素，m₂₄为M中第二行第四列元素，m₃₁为M中第三行第一列元素，m₃₂为M中第三行第二列元素，m₃₃为M中第三行第三列元素，m₃₄为M中第三行第四列元素。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是，所述步骤二中对非重叠视角下多相机和激光雷达的联合标定设定标定误差约束；具体过程为：

为了增加约束条件，本发明采取大于25帧的数据点对相机和激光雷达进行外参标定，但是标定必然存在误差，在此考虑构建误差优化函数，利用所得到的匹配点构建优化框架：

F(R,t,P_i′,p_i′)＝sK(R·P_i′+t)-p_i′

式中，F为空间点到相机坐标系的投影函数，P_i′为空间3D点，p_i′为对应的相机成像平面的2D像素点，s为尺度因子，K为相机的内参矩阵；

对于多个对应的匹配点，可以最小化其重投影误差，通过该约束关系来调整激光雷达和相机的外参(R,t)，即可得到较为理想的优化结果，假设待优化的误差标定函数为ε，表达式为：

其中，ε为待优化的标定误差约束函数；n为图像中特征点的个数，i′为图像中第i′个特征点。

其它步骤及参数与具体实施方式一至三之一相同。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是，所述步骤四中利用帧-图的匹配方式对步骤三去噪预处理后的激光点云提取点云特征，对提取的点云特征进行点云匹配，得到边缘、平面特征误差模型；具体过程为；

利用帧-图的匹配方式对步骤三去噪预处理后的激光点云提取点云特征，利用ICP算法对提取的点云特征进行点云匹配，得到与步骤三去噪预处理后的激光点云相关的位姿变换矩阵

通过

将步骤三去噪预处理后的激光点云数据

转换到世界坐标系下，构成局部地图M；

式中，i为激光雷达第i帧关键帧，

为激光雷达关键帧的总数；

局部地图分为子地图边缘特征地图和平面特征地图，M＝{M^e,M^p}，

其中，M^e为子地图边缘特征地图，M^p为平面特征地图；

式中，

为激光雷达第i帧关键帧的边缘特征点，

为激光雷达第i帧关键帧的平面特征点；

为激光雷达关键帧的总数；

计算第i″个边缘特征点到M^e中对应边缘线(l,j)的距离，距离公式如下，

其中，

分别是i″，j，l三点在前一关键帧的同一激光雷达坐标系下的三维坐标；k′为激光雷达关键帧的序号；j,l是边缘线两个端点(关键点)；L为雷达；

计算第i″′个平面特征点到M^p中对应特征平面(l′,j′,m′)的距离，距离公式如下，

其中，

分别是i″′,j′,l′,m′四点在前一关键帧的同一激光雷达坐标系下的三维坐标；

根据距离公式(5)建立边缘特征点到M^e的边缘线的距离函数

根据距离公式(6)建立平面特征点到M^p的特征平面的距离函数

式中，

为当前帧第i″个边缘特征点，

为当前帧第i″′个平面特征点；

为当前帧与前一帧的位姿转换矩阵，L_k′为第k′个激光雷达关键帧，f_ε为当前帧与前一帧边缘特征点的投影约束方程，f_H为当前帧与前一帧平面特征点的投影约束方程；

基于边缘特征点到M^e的边缘线的距离函数和平面特征点到M^p的特征平面的距离函数建立边缘、平面特征误差模型；

所述基于边缘特征点到M^e的边缘线的距离函数和平面特征点到M^p的特征平面的距离函数建立边缘、平面特征误差模型；表达式为：

其它步骤及参数与具体实施方式一至四之一相同。

具体实施方式六：本实施方式与具体实施方式一至五之一不同的是，所述步骤五中多视角相机拍摄彩色图像，提取点线特征，获得匹配追踪得到的图像帧与帧之间点特征和图像帧与帧之间线特征；具体过程为：

步骤五一、多视角相机拍摄彩色图像；

步骤五二、利用ORB算法提取彩色图像中的点特征；

步骤五三、利用FLD算法提取彩色图像中的线特征(主要为车辙线)，针对FLD算法线特征提取质量不高的问题，对线特征加入近线合并(图3)，断线拼接(图4)，长度抑制等策略优化特征提取结果；

步骤五四、基于步骤五二的点特征，利用描述子算法，匹配追踪步骤五二中图像帧与帧之间点特征；

基于步骤五三的线特征，利用描述子算法，匹配追踪步骤五三中图像帧与帧之间线特征。

其它步骤及参数与具体实施方式一至五之一相同。

具体实施方式七：本实施方式与具体实施方式一至六之一不同的是，所述步骤六中将步骤五匹配追踪得到的点特征、线特征投影到图像坐标系，投影到图像坐标系中的点与相机成像平面中对应的2D点线建立约束关系(点特征误差方程公式9，线特征误差方程公式12)，获得点线特征误差模型；具体过程为：

建立视觉里程计点特征投影模型和点特征误差模型：

式中，R_v、t_v为视觉(相机拍摄的)相邻关键帧的旋转矩阵和平移向量，

为视觉观测的空间3D点，

为对应的相机成像平面的2D像素点，ε_v为点特征误差模型函数；F_V为点特征投影模型的数学表达；

与点特征的空间投影模型不同，线特征观测模型采用精度较高的3D-2D方式，将空间直线投影到图像中，计算投影直线与图中匹配直线间的误差。类比于空间中的三维点世界坐标系到图像坐标系的转换方式，将直线(相机拍摄的)转换到图像坐标系下，如图6，并记做L_c，使用普吕克坐标表示为：

式中，L_c为相机坐标系下3D特征线的普吕克坐标表示，n_c、v_c为普吕克坐标，H_cw为直线变换矩阵，由旋转矩阵和平移向量构成；(t_cw)^{^}为平移向量的反对称矩阵；L_w为世界坐标系下特征线的普吕克坐标，R_cw为普吕克坐标系下的旋转矩阵，t_cw为普吕克坐标系下的平移向量；

将直线投影到平面上得到线特征，特征线的普吕克坐标为：

式中，l_c为相机成像平面的2D特征线，K为相机内参；

空间3D直线(相机拍摄的)投影到相机成像平面得到投影线段；

投影线段与相邻帧图像进行线特征匹配得到对应的匹配线段；

通过匹配线段两端点到投影线段的代数距离的非线性误差，将线特征的误差模型表示为：

式中，ε_l为线特征误差模型函数，

为匹配线段两端点到投影线段的距离，

为匹配线段两端点到投影线段的法向量，l_c为相机成像平面的2D特征线，h₁、h₂为匹配线段与投影线段的横纵坐标差值，T为转置。

其它步骤及参数与具体实施方式一至六之一相同。

具体实施方式八：本实施方式与具体实施方式一至七之一不同的是，所述步骤七中对步骤三去噪预处理后的激光点云进行处理，获得深度恢复误差模型；具体过程为：

将步骤三去噪预处理后的激光点云三维坐标转换到相机坐标系下，之后将步骤五获得的点特征和线特征(步骤五匹配后的)的两个端点从相机坐标系投影到相机归一化平面，经过归一化处理获取齐次坐标坐标

按照线特征的端点和点特征的坐标确定每个特征点的窗口范围box，在box中的点云簇中选取出最靠近相机的激光点云，构成的平面为n^TX+d＝0；

计算点特征的坐标与平面n^TX+d＝0交点，得到视觉点特征在相机坐标系下的3D坐标

根据视觉里程计点特征投影模型公式(8)，设前一帧的视觉点特征集为

当前帧视觉点特征集为

当前帧位姿变换

从当前帧坐标系L_k′+1投影到前一帧坐标系L_k′的视觉点特征集为

设

则视觉里程计点特征的投影点到其相邻帧匹配点的欧几里得距离记为深度恢复误差模型：

式中，ε_d为深度恢复误差模型函数，

为第α帧特征点的像素坐标，

为第β帧特征点对应的3D空间点匹配到前一帧像素特征点再反投影到第j帧得到的像素坐标(因为当前帧与前一帧图像的几何关系，存在一个位姿转换关系，但是根据相机运动估计的位姿转换关系并不是绝对正确的，所以反投影之后会存在像素位置的偏差，根据这个误差，即(14)，构建误差模型，就可以调整位姿转换关系，使得像素位置的偏差达到最小)。

其它步骤及参数与具体实施方式一至七之一相同。

具体实施方式九：本实施方式与具体实施方式一至八之一不同的是，所述步骤八中基于步骤二、步骤四、步骤六、步骤七，构建视觉激光的数据关联模型；具体过程为：

数据关联模型包括标定误差约束(联合标定，公式2)；点线特征误差模型(视觉特征追踪)；边缘、平面特征误差模型(激光点云匹配，公式7)；深度恢复误差模型(激光点深度值匹配视觉特征，公式14)；

将标定误差约束、点线特征误差模型、边缘、平面特征误差模型、深度恢复误差模型数据关联作为因子加入因子图。

构建视觉激光的数据关联模型，本发明的多模态里程计为紧耦合的激光视觉SLAM系统，主要分为两部分，视觉里程计和激光里程计，通过数据关联模型将这两部分联系起来，对系统整体进行优化。

其它步骤及参数与具体实施方式一至八之一相同。

具体实施方式十：本实施方式与具体实施方式一至九之一不同的是，所述步骤九中基于步骤八使用因子图算法对智能体的位姿进行增量式更新；

将上述数据关联模型的误差函数加入因子图中，在SLAM增量式更新过程中实现对位姿估计残差的整体优化，因子图的优势在于只在矩阵最后一行或多行添加变量，且在因子图优化过程中增加了Givens旋转消元，从而可以单独处理矩阵的最后几行，与之前的数据解耦，减少了计算资源的消耗。采用贝叶斯树的结构进行因子图优化，本专利的因子图模型如图7所示，其中包括标定约束因子，激光里程计因子，视觉里程计因子；

其中标定约束因子由标定误差约束构成，激光里程计因子由边缘、平面特征误差模型构成，视觉里程计因子由点线特征误差模型和深度恢复误差模型构成；

多模态的里程计系统在全局优化的目标函数为：

式中，e_δξ为标定约束因子，e′_δξ为视觉里程计因子，e″_δξ为激光里程计因子；C_pre为激光雷达和多视角相机的联合标定，VO为视觉里程计，LO为激光里程计；

其中e_δξ、e′_δξ、e″_δξ统一为e_p,q：

上式中

和

为第p个关键帧和第q个关键帧在世界坐标系下的位姿估计，ΔT_pq为视觉里程计因子或者激光里程计因子或者标定约束因子，即相对的位姿变换。

其它步骤及参数与具体实施方式一至九之一相同。

采用以下实施例验证本发明的有益效果：

实施例一：

下面结合附图与具体实施方式对本发明作进一步详细描述。

本发明改善了多模态里程计位姿优化的误差模型，基于激光雷达和多视角相机系统建立了紧耦合的激光视觉里程计，为提高SLAM位姿优化的精确度做出显著贡献。所涉及的方法总体系统框图如图2所示，据图实施过程分为以下步骤：

步骤一、非重叠视角下多相机和激光雷达的联合标定。标定多视角相机和全向机械激光雷达，联合标定工作分为两部分：空间标定和时间标定。空间标定则需要利用张正友的相机标定法分别标定多相机系统的各个单目相机。在单目相机标定完成后，构建多视角相机的约束关系，与传统方案中利用共视区域的视觉特征进行对极几何约束构建多相机之间的联系不同，本专利拟开发的是重叠率较低的多相机系统，相邻相机共同视野较小，无法利用传统方案进行多相机的联合标定。针对这种情况，本发明采用一种随机模型的标定对象——布满SURF特征点的点云标定板，使用OpenCV生成点云标定板，在标定过程中读取点云标定板，相机个数，相机模型，在标定过程中仅需要相邻的两个相机能够同时看到标定板的一部分，即可完成两个相机之间的标定，以此类推，即可完成整个多相机系统的标定工作。本发明利用利用开源的Autoware无人驾驶方案中的标定工具对激光雷达进行标定，为了减少标定工作量，对于激光雷达与多相机系统的数据融合，对单个激光雷达与单个相机进行联合标定，根据多相机系统内部的转换关系，完成激光雷达与多相机系统的联合标定。本发明利用软硬触发协同统一相机和激光雷达的时间戳，从空间和时间维度完成多视角相机系统和激光雷达的联合标定。

步骤二、对原始激光点云进行预处理，主要进行激光点云的畸变补偿，激光点云的有序化处理，激光点云的地面分割以及激光点云的物体分割。对地面点云降采样得到五分之一的地面点和有效分割点计算曲率提取特征，并依据曲率剔除不可靠点，选取scan-to-map的匹配方式对相邻关键帧进行点云特征匹配，改良以往激光里程计使用的帧-帧匹配精度不高以及效率较低的缺陷，激光点云选取置信区域，进行基于前景分割的深度估计，为视觉特征点提供绝对深度和环境数据关联。

步骤三、利用ORB算法提取彩色图像中的点特征，同时利用FLD算法提取彩色图像中的线特征(主要为车辙线)，针对FLD算法线特征提取质量不高的问题，加入近线合并(图3)，断线拼接(图4)，长度抑制等策略优化特征提取结果。传统的FLD算法检测真实场景会检测到大量短线段特征，过多的短线段特征不仅会加重线段检测，匹配的计算成本，也会增加线段的误匹配概率，长线段相对更稳定且更容易被检测到，对位姿估计的贡献也更大，所以设置短线段剔除准则：

len≥(len_min＝w×min(W_I,H_I))

W_I,H_I表示图像的尺寸，w表示比例系数，选取与图像尺寸适合的长线段。此外，由于FLD算法长线段经常被分为几个较短的部分，导致一些细长边缘经常被重复检测。这些相似线段往往质量不高，且增加特征检测的不确定性，所以采用近线合并和断线连接的方法改进FLD算法，利用线段之间的角度差，设置阈值，并使用最小二乘拟合方法将线段拟合，获取新的特征线段。根据点线特征提取结果，分别利用BRIEF描述子算法和LBD特征线描述子算法，匹配追踪帧帧之间点线特征。对于特征点采用BRIEF描述子，采用汉明距离作为特征度量空间中特征间的相似程度，对于特征线采用的是浮点数向量形式的LBD描述子算法，对LBD描述子进行处理，将其转换为二进制形式，同样使用汉明距离来表示匹配特征间的相似程度，以特征间汉明距离小于最小距离的两倍作为筛选标准，随后选用快速近似最近邻(FLANN)算法，核心思想是利用导出索引结构将搜索范围定位在特征邻近区域，在邻近区域内完成特征匹配，可以有效地加快匹配速度。利用BRIEF描述子和LBD由0和1组成的特点，使用局部敏感哈希(LSH)作为导出索引结构。将特征按照同样的方式投影到哈希空间，在哈希空间邻域内进行匹配，有效的缩小了匹配范围。匹配结束后，根据点线三角化原理，将空间点与空间直线与图像坐标系的投影和平面中的2D点线建立约束关系，根据匹配结果建立点线特征的重投影误差模型，从而约束相邻帧相机位姿转换关系和视觉特征的3D点、2D点。

步骤四、构建视觉激光的数据关联模型，本专利的多模态里程计为紧耦合的激光视觉SLAM系统，主要分为两部分，视觉里程计和激光里程计，通过数据关联模型将这两部分联系起来，对系统整体进行优化，SLAM总体过程中的数据关联可以分为以下几个部分：标定，激光点云配准，视觉特征追踪，视觉特征深度值恢复。

(1)首先是标定多视角相机和全向机械激光雷达，按照步骤一的标定工作流程，外参标定的数学关系可以表达为：

式中，u、v是空间3D点投影到相机像素坐标系下二维像素点的横纵坐标，f_x、f_y为常数，c_x、c_y为相机内参中的平移参数，表示相机像素坐标系与成像平面相差一个缩放倍数和原点的平移，上式表示为像素坐标在u轴缩放了f_x倍，在v缩放了f_y倍，与此同时像素坐标系原点平移了c_x，c_y；

R为3D点在相机坐标系和雷达坐标系下的旋转矩阵，t为3D点在相机坐标系和雷达坐标系下的平移向量，x、y、z为空间3D点的三维坐标，M为相机内参和相关坐标系的旋转平移的矩阵乘积，m₁₁为M中第一行第一列元素，m₁₂为M中第一行第二列元素，m₁₃为M中第一行第三列元素，m₁₄为M中第一行第四列元素，m₂₁为M中第二行第一列元素，m₂₂为M中第二行第二列元素，m₂₃为M中第二行第三列元素，m₂₄为M中第二行第四列元素，m₃₁为M中第三行第一列元素，m₃₂为M中第三行第二列元素，m₃₃为M中第三行第三列元素，m₃₄为M中第三行第四列元素；

为了增加约束条件，本发明采取25帧以上的数据点进行外参标定，但是标定必然存在误差，在此考虑构建误差优化函数，利用所得到的匹配点构建优化框架：

F(R,t,P_i′,p_i′)＝sK(R·P_i′+t)-p_i′

对于多个对应的匹配点，可以最小化其重投影误差，通过该约束关系来调整激光雷达和相机的外参(R,t)，即可得到较为理想的优化结果，假设待优化的误标定差函数为ε，则其表达式为：

(2)相邻帧激光点云配准，在步骤二的点云预处理之后，与之相关的位姿变换矩阵为{T_i-n，…T_i}，通过{T_i-n，…T_i}将{F_i-n,…F_i}转换到世界坐标系下，构成局部地图M，局部地图分为两个子地图边缘特征地图和平面特征地图M＝{M^e,M^p}，其中

式中，

为第i帧图像中的边缘特征点，

为第i帧图像中的平面特征点；

对于当前关键帧的每一个边缘特征点

计算边缘特征点i″到M^e中对应边缘线(l,j)的距离，距离公式如下：

其中，

分别是i″，j，l三点在前一关键帧的同一激光雷达坐标系下的三维坐标；k′为激光雷达关键帧的序号；j,l是边缘线两个端点；L为雷达；

对于当前关键帧的每一个平面特征点

计算平面特征点i″′到M^p中对应特征平面(l′,j′,m′)的距离，距离公式如下，

其中，

分别是i″′,j′,l′,m′四点在前一关键帧的同一激光雷达坐标系下的三维坐标；

根据边缘线，平面域的距离公式，边缘点到

的边缘线的距离函数和构建平面点到

的特征平面的距离函数：

式中，

为当前帧第i″个边缘特征点，

为当前帧第i″′个平面特征点；

为当前帧与前一帧的位姿转换矩阵，L_k′为第k′个激光雷达关键帧，f_ε为当前帧与前一帧边缘特征点的投影约束方程，f_H为当前帧与前一帧平面特征点的投影约束方程；

所以由当前帧边缘特征点、平面特征点构成的总的损失函数为：

(3)视觉里程计在特征提取之后进行特征匹配与追踪，将空间点、空间直线投影到图像坐标系与平面中对应的2D点线建立约束关系。建立视觉里程计点特征的投影方程与误差方程：

式中，R_v、t_v为视觉相邻关键帧的旋转矩阵和平移向量，

为视觉观测的空间3D点，

为对应的相机成像平面的2D像素点，ε_v为点特征误差模型函数；F_V为点特征投影模型的数学表达；

与点特征的空间投影模型不同，线特征观测模型采用精度较高的3D-2D方式，将空间直线投影到图像中，计算投影直线与图中匹配直线间的误差。类比于空间中的三维点世界坐标系到图像坐标系的转换方式，将直线转换到图像坐标系下，如图6，并记做L_c，使用普吕克坐标表示为：

式中，L_c为相机坐标系下3D特征线的普吕克坐标表示，n_c、v_c为普吕克坐标，H_cw为直线变换矩阵，由旋转矩阵和平移向量构成；(t_cw)^{^}为平移向量的反对称矩阵；L_w为世界坐标系下特征线的普吕克坐标，R_cw为普吕克坐标系下的旋转矩阵，t_cw为普吕克坐标系下的平移向量；

上式中，为直线变换矩阵，由旋转矩阵和平移向量构成，其中为平移向量的反对称矩阵。将直线投影到相机成像平面上得到的线特征，其普吕克坐标为：

空间3D直线(相机拍摄的)投影到相机成像平面得到投影线段；

与相邻帧图像进行线特征匹配得到对应的匹配线段；

通过匹配线段两端点到投影线段的代数距离的非线性误差，将线特征的误差模型表示为：

式中，ε_l为线特征误差模型函数，

为匹配线段两端点到投影线段的距离，

为匹配线段两端点到投影线段的法向量，l_c为相机成像平面的2D特征线，h₁、h₂为匹配线段与投影线段的横纵坐标差值，T为转置；

(4)激光点云辅助视觉特征的深度值恢复。将激光点云三维坐标转换到相机坐标系下，之后将跟踪和匹配的点特征和线特征的两个端点从相机坐标系投影到相机归一化平面，获取相机归一化坐标

按照视觉线特征的端点和点特征坐标确定每个特征点的窗口范围box，在box中的点云簇中选取出最靠近相机的激光点云，构成的平面为n^TX+d＝0。视觉特征点与该拟合平面交点，则可以得到视觉特征在相机坐标系下的3D坐标

根据视觉里程计点特征投影模型公式，设前一帧的视觉点特征集为

当前帧视觉点特征集为

当前帧位姿变换

从当前帧坐标系L_k′+1投影到前一帧坐标系L_k′的视觉点特征集为

设

则视觉里程计点特征的投影点到其相邻帧匹配点的欧几里得距离记为深度恢复误差模型：

式中，ε_d为深度恢复误差模型函数，

为第α帧特征点的像素坐标，

为第β帧特征点对应的3D空间点匹配到前一帧像素特征点再反投影到第j帧得到的像素坐标；

步骤五、将上述数据关联模型的误差函数加入因子图中，在SLAM增量式更新过程中实现对位姿估计残差的整体优化，因子图的优势在于只在矩阵最后一行或多行添加变量，且在因子图优化过程中增加了Givens旋转消元，从而可以单独处理矩阵的最后几行，与之前的数据解耦，减少了计算资源的消耗。采用贝叶斯树的结构进行因子图优化，本专利的因子图模型如图7所示，其中包括标定约束因子，激光里程计因子，视觉里程计因子，其中标定约束因子由标定误差构成，激光里程计因子由边缘点和平面点的误差函数构成，视觉里程计因子由视觉投影模型的特征点误差，特征线误差，深度恢复误差构成。系统在全局优化的目标函数为：

多模态的里程计系统在全局优化的目标函数为：

式中，e_δξ为标定约束因子，e′_δξ为视觉里程计因子，e″_δξ为激光里程计因子；C_pre为激光雷达和多视角相机的联合标定，VO为视觉里程计，LO为激光里程计；

其中e_δξ、e′_δξ、e″_δξ统一为e_p,q：

上式中

和

为第p个关键帧和第q个关键帧在世界坐标系下的位姿估计，ΔT_pq为视觉里程计因子或者激光里程计因子或者标定约束因子，即相对的位姿变换；

步骤六、重复步骤二至步骤五，估计连续帧位姿变换，完成多模态里程计构建。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (9)

1.一种基于车辙线的多模态里程计方法，其特征在于：所述方法具体过程为：

步骤一、对非重叠视角下多相机和激光雷达进行联合标定；

步骤二、对非重叠视角下多相机和激光雷达的联合标定设定标定误差约束；

步骤三、激光雷达获取激光点云，利用点云分割算法对激光点云进行去噪预处理；

步骤四、利用帧-图的匹配方式对步骤三去噪预处理后的激光点云提取点云特征，对提取的点云特征进行点云匹配，得到边缘、平面特征误差模型；

步骤五、多视角相机拍摄彩色图像，提取点线特征，获得匹配追踪得到的图像帧与帧之间点特征和图像帧与帧之间线特征；所述线特征为车辙线特征；

步骤六、将步骤五匹配追踪得到的点特征、线特征投影到图像坐标系，投影到图像坐标系中的点与相机成像平面中对应的2D点线建立约束关系，获得点线特征误差模型；

步骤七、对步骤三去噪预处理后的激光点云进行处理，获得深度恢复误差模型；

步骤八、基于步骤二、步骤四、步骤六、步骤七，构建视觉激光的数据关联模型；

步骤九、基于步骤八使用因子图算法对智能体的位姿进行增量式更新；

步骤十、重复步骤三至步骤九，估计连续帧位姿变换，完成多模态里程计构建；

所述步骤一中对非重叠视角下多相机和激光雷达进行联合标定；具体过程为；

联合标定分为两部分：空间标定和时间标定；

空间标定为：利用张正友的相机标定法分别标定多相机系统的各个单目相机的内参，在单目相机标定完成后，构建多视角相机的约束关系；

多视角相机的约束关系为：使用OpenCV生成点云标定板，在标定过程中读取点云标定板，相机个数，相机内参，在标定过程中仅需要相邻的两个相机能够同时看到标定板的同一部分，即可完成两个相机之间的标定，以此类推，即可完成整个多相机系统的标定工作；

利用开源的Autoware无人驾驶中的标定工具对激光雷达进行标定；

多相机系统中共有5台单目相机，镜头分别向外，构成全景视觉系统，相邻两个相机之间的重叠视野为14度，激光雷达位于五个单目相机的几何中心；根据多相机系统内部单目相机之间的相对位置和几何角度的关系，以及多视角相机的约束关系，完成单目相机之间的坐标转换，再根据单个相机与激光雷达的标定结果，即可完成激光雷达与多相机系统的联合标定；

时间标定为：利用软硬触发协同统一相机和激光雷达的时间戳；

从空间和时间维度完成非重叠视角下多相机和激光雷达的联合标定。

2.根据权利要求1所述一种基于车辙线的多模态里程计方法，其特征在于：所述单个相机与激光雷达的标定结果表达为：

式中，u、v是空间3D点投影到相机像素坐标系下二维像素点的横纵坐标，f_x、f_y为常数，c_x、c_y为相机内参中的平移参数；

R为3D点在相机坐标系和雷达坐标系下的旋转矩阵，t为3D点在相机坐标系和雷达坐标系下的平移向量，x、y、z为空间3D点的三维坐标，M为相机内参和相关坐标系的旋转平移的矩阵乘积，m₁₁为M中第一行第一列元素，m₁₂为M中第一行第二列元素，m₁₃为M中第一行第三列元素，m₁₄为M中第一行第四列元素，m₂₁为M中第二行第一列元素，m₂₂为M中第二行第二列元素，m₂₃为M中第二行第三列元素，m₂₄为M中第二行第四列元素，m₃₁为M中第三行第一列元素，m₃₂为M中第三行第二列元素，m₃₃为M中第三行第三列元素，m₃₄为M中第三行第四列元素。

3.根据权利要求2所述一种基于车辙线的多模态里程计方法，其特征在于：所述步骤二中对非重叠视角下多相机和激光雷达的联合标定设定标定误差约束；具体过程为：

F(R,t,P_i′,p_i′)＝sK(R·P_i′+t)-p_i′

式中，F为空间点到相机坐标系的投影函数，P_i′为空间3D点，p_i′为对应的相机成像平面的2D像素点，s为尺度因子，K为相机的内参矩阵；

其中，ε为标定误差约束函数；n为图像中特征点的个数，i′为图像中第i′个特征点。

4.根据权利要求3所述一种基于车辙线的多模态里程计方法，其特征在于：所述步骤四中利用帧-图的匹配方式对步骤三去噪预处理后的激光点云提取点云特征，对提取的点云特征进行点云匹配，得到边缘、平面特征误差模型；具体过程为；

利用帧-图的匹配方式对步骤三去噪预处理后的激光点云提取点云特征，利用ICP算法对提取的点云特征进行点云匹配，得到与步骤三去噪预处理后的激光点云相关的位姿变换矩阵

通过

将步骤三去噪预处理后的激光点云数据

转换到世界坐标系下，构成局部地图M；

式中，i为激光雷达第i帧关键帧，

为激光雷达关键帧的总数；

局部地图分为子地图边缘特征地图和平面特征地图，M＝{M^e,M^p}，

其中，M^e为子地图边缘特征地图，M^p为平面特征地图；

式中，

为激光雷达第i帧关键帧的边缘特征点，

为激光雷达第i帧关键帧的平面特征点；

为激光雷达关键帧的总数；对于当前关键帧的第i″个边缘特征点

计算第i″个边缘特征点到M^e中对应边缘线(l,j)的距离，距离公式如下，

其中，

分别是i″，j，l三点在前一关键帧的同一激光雷达坐标系下的三维坐标；k′为激光雷达关键帧的序号；j,l是边缘线两个端点；L为雷达；

对于当前关键帧的第i″′个平面特征点

计算第i″′个平面特征点到M^p中对应特征平面(l′,j′,m′)的距离，距离公式如下，

其中，

分别是i″′,j′,l′,m′四点在前一关键帧的同一激光雷达坐标系下的三维坐标；

根据距离公式(5)建立边缘特征点到M^e的边缘线的距离函数

根据距离公式(6)建立平面特征点到M^p的特征平面的距离函数

式中，

为当前帧第i″个边缘特征点，

为当前帧第i″′个平面特征点；

为当前帧与前一帧的位姿转换矩阵，L_k′为第k′个激光雷达关键帧，f_ε为当前帧与前一帧边缘特征点的投影约束方程，f_H为当前帧与前一帧平面特征点的投影约束方程；

基于边缘特征点到M^e的边缘线的距离函数和平面特征点到M^p的特征平面的距离函数建立边缘、平面特征误差模型；表达式为：

5.根据权利要求4所述一种基于车辙线的多模态里程计方法，其特征在于：所述步骤五中多视角相机拍摄彩色图像，提取点线特征，获得匹配追踪得到的图像帧与帧之间点特征和图像帧与帧之间线特征；具体过程为：

步骤五一、多视角相机拍摄彩色图像；

步骤五二、利用ORB算法提取彩色图像中的点特征；

步骤五三、利用FLD算法提取彩色图像中的线特征，对线特征加入近线合并，断线拼接，长度抑制策略；

步骤五四、基于步骤五二的点特征，利用描述子算法，匹配追踪步骤五二中图像帧与帧之间点特征；

基于步骤五三的线特征，利用描述子算法，匹配追踪步骤五三中图像帧与帧之间线特征。

6.根据权利要求5所述一种基于车辙线的多模态里程计方法，其特征在于：所述步骤六中将步骤五匹配追踪得到的点特征、线特征投影到图像坐标系，投影到图像坐标系中的点与相机成像平面中对应的2D点线建立约束关系，获得点线特征误差模型；具体过程为：

建立视觉里程计点特征投影模型和点特征误差模型：

式中，R_v、t_v为视觉相邻关键帧的旋转矩阵和平移向量，

为视觉观测的空间3D点，

为对应的相机成像平面的2D像素点，ε_v为点特征误差模型函数；F_V为点特征投影模型的数学表达；

将直线转换到图像坐标系下，使用普吕克坐标表示为：

式中，L_c为相机坐标系下3D特征线的普吕克坐标表示，n_c、v_c为普吕克坐标，H_cw为直线变换矩阵，(t_cw)^为平移向量的反对称矩阵；L_w为世界坐标系下特征线的普吕克坐标，R_cw为普吕克坐标系下的旋转矩阵，t_cw为普吕克坐标系下的平移向量；

将直线投影到平面上得到线特征，特征线的普吕克坐标为：

式中，l_c为相机成像平面的2D特征线，K为相机内参；

直线投影到相机成像平面得到投影线段；

投影线段与相邻帧图像进行线特征匹配得到对应的匹配线段；

将线特征的误差模型表示为：

式中，ε_l为线特征误差模型函数，

为匹配线段两端点到投影线段的距离，

为匹配线段两端点到投影线段的法向量，l_c为相机成像平面的2D特征线，h₁、h₂为匹配线段与投影线段的横纵坐标差值，T为转置。

7.根据权利要求6所述一种基于车辙线的多模态里程计方法，其特征在于：所述步骤七中对步骤三去噪预处理后的激光点云进行处理，获得深度恢复误差模型；具体过程为：

将步骤三去噪预处理后的激光点云三维坐标转换到相机坐标系下，之后将步骤五获得的点特征和线特征的两个端点从相机坐标系投影到相机归一化平面，经过归一化处理获取齐次坐标

按照线特征的端点和点特征的坐标确定每个特征点的窗口范围box，在box中的点云簇中选取出最靠近相机的激光点云，构成的平面为nTX+d＝0；

计算点特征的坐标与平面n^TX+d＝0交点，得到点特征在相机坐标系下的3D坐标

根据点特征投影模型公式(8)，设前一帧的点特征集为

当前帧点特征集为

当前帧位姿变换

从当前帧坐标系L_k′+1投影到前一帧坐标系L_k′的点特征集为

设

则点特征的投影点到其相邻帧匹配点的欧几里得距离记为深度恢复误差模型：

式中，ε_d为深度恢复误差模型函数，

为第α帧特征点的像素坐标，

为第β帧特征点对应的3D空间点匹配到前一帧像素特征点再反投影到第j帧得到的像素坐标。

8.根据权利要求7所述一种基于车辙线的多模态里程计方法，其特征在于：所述步骤八中基于步骤二、步骤四、步骤六、步骤七，构建视觉激光的数据关联模型；具体过程为：

数据关联模型包括标定误差约束，点线特征误差模型，边缘、平面特征误差模型和深度恢复误差模型；

将标定误差约束、点线特征误差模型、边缘、平面特征误差模型和深度恢复误差模型数据关联作为因子加入因子图。

9.根据权利要求8所述一种基于车辙线的多模态里程计方法，其特征在于：所述步骤九中基于步骤八使用因子图算法对智能体的位姿进行增量式更新；具体过程为：

因子图包括标定约束因子，激光里程计因子，视觉里程计因子；

其中标定约束因子由标定误差约束构成，激光里程计因子由边缘、平面特征误差模型构成，视觉里程计因子由点线特征误差模型和深度恢复误差模型构成；

多模态的里程计系统在全局优化的目标函数为：

式中，e_δξ为标定约束因子，e′_δξ为视觉里程计因子，e″_δξ为激光里程计因子；C_pre为激光雷达和多视角相机的联合标定，VO为视觉里程计，LO为激光里程计；

其中e_δξ、e′_δξ、e″_δξ统一为e_p,q：

上式中

和

为第p个关键帧和第q个关键帧在世界坐标系下的位姿估计，ΔT_pq为视觉里程计因子或者激光里程计因子或者标定约束因子，即相对的位姿变换。

Images