具体实施方式

需要特别说明的是，在不冲突的情况下，本申请公开的各个实施方式之间可以相互组合。

具体实施方式一：参照图1具体说明本实施方式，本实施方式所述的一种基于激光雷达及双目相机的矿井机器人定位建图方法，包括以下步骤：

步骤一：利用激光雷达与双目摄像机进行联合标定，得到转换矩阵；

步骤二：利用激光雷达与双目摄像机对周围环境进行采集，得到点云深度信息及图像信息；

步骤三：对图像信息进行点特征提取和线特征提取，得到特征点及特征线；

步骤四：将点云深度信息通过转换矩阵得到深度图像，利用深度图像对特征点的深度进行校正估计，利用校正估计后的特征点及特征线建立约束关系，并利用建立的约束关系对双目摄像机进行初始位姿估计；

步骤五：对校正估计后的特征点及特征线进行特征融合，并利用特征融合后的结果构建点线误差模型，并利用点线误差模型优化双目摄像机位姿(双目摄像机完成图1中相邻帧匹配)；

步骤六：重复步骤二至步骤五，估计连续帧位姿变换，并进行回环检测与重定位，最后完成定位建图。

激光雷达几乎不受光照及纹理等因素的影响，但是激光雷达则较难完成回环检测及重定位，因此将两种传感器融合进行定位建图。另外，观察到矿井中存在的大量的液压支柱等支撑体，其具有明显的线特征，可以与较少的点特征形成良好的互补作用，增加定位及建图精度。

具体实施方式二：本实施方式是对具体实施方式一的进一步说明，本实施方式与具体实施方式一的区别是所述利用激光雷达与双目摄像机进行联合标定的具体步骤为：

首先进行双目摄像机内参标定，然后采用棋盘格通过Autoware标定工具进行双目摄像机雷达标定，得到激光雷达与双目摄像机的转换关系。

具体实施方式三：本实施方式是对具体实施方式一的进一步说明，本实施方式与具体实施方式一的区别是所述利用深度图像对特征点的深度进行校正估计的具体步骤为：

首先选择特征点周围10*10区域内的所有激光雷达点云，然后对区域内的所有激光雷达点云拟合出多个平面，计算特征点到所有平面的距离，选择最小距离的平面对光心方向进行投影，得到的点即为深度估计。

具体实施方式四：本实施方式是对具体实施方式一的进一步说明，本实施方式与具体实施方式一的区别是所述点特征提取采用ORB算法。

具体实施方式五：本实施方式是对具体实施方式一的进一步说明，本实施方式与具体实施方式一的区别是所述线特征提取采用改进的LSD算法，所述改进的LSD算法采用长度抑制、断线拼接和近线合并策略优化特征提取结果。

具体实施方式六：本实施方式是对具体实施方式一的进一步说明，本实施方式与具体实施方式一的区别是所述构建点线误差模型表示为：

其中，ρ_p，ρ_l表示Huber鲁棒核函数，ep_k,i表示点的误差，el_k,i表示线的误差，i表示第i个特征点，k表示第k个关键帧，j表示雅可比矩阵

具体实施方式七：本实施方式是对具体实施方式六的进一步说明，本实施方式与具体实施方式六的区别是所述ep_k,i表示为：

其中，p_ki为第k帧图像的第i个点，R_k为三维点到平面图像的旋转矩阵，t_k为平移向量，X_i表示为地图中的第i个点，π为投影到图像上的符合表示。

具体实施方式八：本实施方式是对具体实施方式六的进一步说明，本实施方式与具体实施方式六的区别是所述el_k,j表示为：

具体实施方式九：本实施方式是对具体实施方式一的进一步说明，本实施方式与具体实施方式一的区别是所述线特征提取之前还包括剔除短线段特征的步骤，短线段特征基于短线段剔除准则，所述短线段剔除准则为：

len_i≥(len_min＝ω×min(W_I,H_I))i＝1....n

W_I，H_I表示图像的尺寸，ω表示比例系数,len_i表示第i条线段。

具体实施方式十：本实施方式是对具体实施方式一的进一步说明，本实施方式与具体实施方式一的区别是所述回环检测的具体步骤为：

首先构建点线综合的视觉词典，然后对视觉词典进行离线训练，之后利用离线训练得到的视觉词典把每一个插入的关键帧转换为词包向量，并根据词包向量构造在线数据库，所述在线数据库根据倒排索引存储，之后通过倒排索引搜索到包含视觉词汇的所有关键帧，然后当地图中的关键帧与双目摄像机的当前帧具有相同词汇时，计算两者中点线特征的相似性得分：

s＝αs_p+βs_l

其中α+β＝1，s_p、s_l分别为两者点、线特征的相似性，最后进行时间一致性检验、几何校验以及连续性检测校验得到最后的闭环结果。

实施例：

步骤一、进行激光雷达与双目视觉的联合标定，以得到两者坐标系的变换关系。联合标定使用的是Autoware的CalibrationTookit模块，需要准备一个9行7列的棋盘格标定板，联合标定分为两步：首先标定获得相机的内参，然后标定获得相机-Lidar的外参。

步骤二、系统开启，激光雷达与双目摄像机运行采集数据，激光雷达采集的是点云深度信息，摄像机采集的为图像信息；对图像进行图像预处理，采用ORB算法进行点特征提取，采用改进LSD算法提取线特征。改进的LSD算法包括长度抑制，断线拼接，近线合并等策略优化线特征提取结果。

过多的短线段特征不仅会加重线段检测，匹配的计算成本，也会增加线段的误匹配概率，长线段相对更稳定且更容易被检测到，对位姿估计的贡献也更大，所以设置短线段剔除准则：

len_i≥(len_min＝ω×min(W_I,H_I))i＝1....n

W_I，H_I表示图像的尺寸，ω表示比例系数，选取与图像尺寸适合的长线段。此外，由于LSD算法长线段经常被分为几个较短的部分，导致一些细长边缘经常被重复检测。这些相似线段往往质量不高，且增加特征检测的不确定性，所以采用近线合并和断线连接的方法改进LSD算法，利用线段之间的角度差，设置阈值，并使用最小二乘拟合方法将线段拟合，获取新的特征线段。

步骤三、通过标定得到的激光雷达到相机的转换矩阵，将点云投影到深度图像，然后对特征点对应深度进行估计。如图2，估计包括以下步骤：首先选择此深度周围特定区域的所有激光雷达点云，然后对所有点云拟合出多个平面，计算此点到所有平面的距离，选择最小距离的平面对光心方向进行投影，得到的点即为相对准确的深度估计。

根据点线三角化原理，将空间点与空间直线与图像坐标系的投影和平面中的2D点线建立约束关系。从而精确匹配追踪帧与帧之间点线特征，进行相机初始位姿估计。

步骤四、进行点线特征融合，构建点线误差模型，然后进行位姿路标点优化估计相机位姿。步骤如下：

(1)建立空间点的误差模型

设X作为地图中的第i个点，对于第k个关键帧，其在图像上的坐标为：

p_k＝KT_kiX_i

其中T_ki为旋转平移矩阵。

其误差可表示为：

ep_ki＝p_ki-π(R_kX_i+t_k)

π为投影到图像上的符合表示。

(2)建立空间直线的误差模型

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

上式中，H_cw为直线变换矩阵，由旋转矩阵R_cw和平移向量t_cw构成，其中(t_cw)^{^}为平移向量的反对称矩阵。将直线L_c投影到平面上得到直线l_c，其普吕克坐标为：

空间中直线L_w投影到当前帧图像坐标系l_c与当前帧图像中匹配的线特征l_c ^*，通过匹配线段两端点m_j,k、n_j,k到投影线段的代数距离d_mj,k、d_nj,k的非线性误差，将线特征的误差模型表示为：

(2)建立点线综合的误差模型

点的误差表示为：

线的误差表示为：

采用BA优化进行求解，方程可写为：

其中，ρ_p，ρ_l为Huber鲁棒核函数。由于误匹配的存在，可能会导致导致系统向错误的值进行优化，因此引入Huber函数来降低误差函数中的异常项。然后对误差函数E进行线性展开，求解其关于状态变量的雅可比矩阵J。

步骤五、构建点线综合的视觉词典，利用离线训练得到的视觉词典把每一个插入的关键帧转换为词包向量，并把这些词包向量构造一个在线数据库，其根据倒排索引存储，通过倒排索引可以快速地搜索到包含某个视觉词汇的所有关键帧。然后在地图中的关键帧与相机的当前帧具有相同词汇时，计算两者中点线特征的相似性得分。点线特征需要以一定的权重进行求和。

s＝αs_p+βs_l

其中α+β＝1，s_p、s_l分别为两者点、线特征的相似性。另外，还有进行时间一致性检验、几何校验以及连续性检测校验才能比较出最后的闭环结果。

重复步骤二至步骤五，估计连续帧位姿变换，通过闭环检测及重定位，最后完成定位及建图。

需要注意的是，具体实施方式仅仅是对本发明技术方案的解释和说明，不能以此限定权利保护范围。凡根据本发明权利要求书和说明书所做的仅仅是局部改变的，仍应落入本发明的保护范围内。