具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1

如图1所示，基于点线特征融合的单目视觉贯导SLAM方法，所述方法包括以下步骤：

S1：将单目相机获取的图像和IMU获取的环境信息作为输入信息；相机和IMU是并行运行的，分别提取出图像信息和环境测量信息，然后通过两者紧密耦合的方式对两者的环境信息进行初始化。所谓紧密耦合就是指两个者采集的信息综合在一起作为输入信息，而不是单独作为输入信息。需要说明的是，所述步骤S1之前还包括IMU初始化，IMU初始化的过程为：

将IMU采集的参数放在一起构成状态向量y_k＝{s,R_wg,b,v_0:k}，其中，s表示尺度因子，R_wg表示重力向量，重力向量在世界坐标系下的表示为g＝R_wgg_I，其中g_I＝(0,0,G)^T，G是重力大小模值；表示加速度计以及陀螺仪的偏置，在初始化阶段该偏置为常数；v_0:k表示从首帧图像到末帧图像的无尺度机体速度；

IMU初始化中被最大化的后验分布为p(y_k|I_0:k)∝p(I_0:k|y_k)p(y_k)，其中，I_0:k表示首帧图像到末帧图像的测量信息，表示已经测量到的首帧图像到末帧图像的测量信息且p(y_k|I_0:k)表示最大化的后验分布，∝表示相当于，p(I_0:k|y_k)表示似然，p(y_k)表示先验分布；

IMU的最大后验估计表示为

其中，I_i-1表示第i-1帧的测量信息，g_dir表示重力向量，v_i-1表示第i-1帧图像的无尺度机体速度，v_i表示第i帧图像的无尺度机体速度。

对IMU的最大后验估计取负对数并且假设IMU预积分以及先验分布服从高斯分布，于是IMU最终的最大后验估计表示为

其中，r_p表示先验残差项，I_i-1,i表示相邻两帧的测量信息。

S2：基于输入信息进行点线特征提取、点线特征加权及点线特征匹配，点线特征提取的同时进行三角化测量实时获取相机的位姿，IMU安装在单目相机的重心处，IMU也实时获取相机的位姿，对两种方式得到的相机的位姿融合得到相机移动过程，相机移动过程中获得很多帧图像，将所有帧图像中点线特征匹配成功的图像帧作为关键帧；

本实施例中，通过FAST角点提取算法进行点特征提取，通过线段检测器LSD算法进行线特征提取，在低纹理场景中，对点线特征加权且线特征的权重高于点特征的权重。FAST角点提取算法属于现有技术，其大致过程如下：

(1)在图像中选取像素p，假设它的亮度为I_p。

(2)设置一个阈值T(比如，I_p的10％)。

(3)以像素p为中心，选取半径为3的圆上的16个像素点。

(4)假设选取的圆上有连续的N个点的亮度大于I_p+T或小于I_p-T，那么像素p可以被认为是点特征。

(5)循环以上四部，对每一个像素执行相同的操作。

在提取点、线特征时，考虑到本发明是在低纹理下进行的，点特征不易被提取这一根本前提。对点、线特征进行加权处理。本发明基于低纹理场景下进行的，线特征提取数量比点特征多，并且因此在加权处理时以线特征为主，点特征为副进行加权，此种加权方式是对环境提取到的有效的点线特征进行加权，用此种方式加权可以在点线特征提取时，连续几帧没有提取到点特征时，依旧可以依照线特征初始化地图，不影响系统的有序进行。因此本发明根据线特征的个数来划分点、线特征的比重，避免了复杂的权值分配计算，保证系统的实时性。设置线特征个数的阈值为N。

点线特征权重w_p和w_l分布如下：

N_l>N：w_p＝0.2，w_l＝0.8

N_l<N：

其中，N_l表示线匹配成功的个数。

所述点线特征匹配的过程为：

单目相机获得的图像的相邻帧中的点特征分别记为K_i和Q_j,i、j＝1,2,3…，把K₁与Q_j中距离最近的点特征特征匹配，把K₂与Q_j中距离最近的点特征特征匹配，依此类推完成点特征匹配；

在进行线特征匹配前应该对空间中的特征线进行数学化表示，便于计算机描述。设空间直线L两个端点的齐次坐标为P₁＝[x₁,y₁,z₁,w₁]^T和P₂＝[x₂,y₂,z₂,w₂]^T，其普吕克矩阵定义为则其非齐次坐标为和则直线的普吕克坐标为：

其中，L是个6×1的向量，包含n和v，n是直线的矩向量，v是直线的方向向量，n垂直于由原点和直线决定的平面。w₁和w₂表示齐次因子。

普吕克矩阵T和坐标L的转换关系如下：

其中，n^∧表示直线的矩向量n的反对称矩阵。

设直线变换矩阵H_cw，它由旋转矩阵和平移向量构成。L_c是空间中直线L_w由空间坐标系转换到相机坐标系的坐标。把L_c用普吕克坐标表示为：

其中，表示平移向量的反对称矩阵。

设直线L_c投影到图像平面上为l_c，其普吕克坐标表示为：

其中K_l表示直线的投影矩阵，f_x、f_y、c_x、c_y表示相机内参数里边的焦距和图像中心(每个相机都有固定的焦距和图像中心)，n_c表示普吕克表达中的法向量，l₁、l₂、l₃表示直线l_c的系数，l_c＝l₁x+l₂y+l₃z。

如图2及图3所示，单目相机获得的图像的相邻帧中的线特征分别记为l_c和l′_c，通过公式获取重投影误差，当图像当前帧的当前线特征与其相邻帧中的某个线特征的重投影误差最小时，线特征匹配成功，其中，l₁、l₂分别表示直线l_c的x方向系数以及y方向系数，表示l′_c的两个端点分别到l_c的距离。

所述步骤S2中对两种方式得到的相机的位姿融合得到相机移动过程包括：根据三角化测量获取相机的位姿形成相机的第一移动轨迹，根据IMU获取相机的位姿形成相机的第二移动轨迹，第一移动轨迹与第二移动轨迹按照时间轴重叠后，剔除同一时刻两者的位姿偏差超过第三预设值的位姿，然后对第一移动轨迹和第二移动轨迹求平均值，得到相机移动过程。

S3：对所有关键帧中提取特征点低于第一预设值的关键帧剔除，剩余的关键帧形成的相机运动轨迹构成多个局部地图，所有局部地图中点线特征个数低于第二预设值的剔除，对剩下的局部地图进行BA优化；BA优化的过程为现有技术，在CSDN博客上Jinterest2020年1月14日上传的文献《SLAM十四讲_后端1》公开了BA优化的具体过程如下：

把世界坐标转换到相机坐标，用到相机的旋转矩阵R和平移矩阵t。

P′＝Rp+t＝[X′,Y′,Z′]

将P′投至归一化平面，得到归一化坐标：

考虑归一化坐标的畸变情况，得到去畸变前的原始像素坐标

根据内参模型，计算像素坐标：

把相机在移动过程中的观测数据记为：

z＝h(x,y)

x指代相机的位姿，即对应的是变换矩阵T，y指代三维点p，则观测误差为：

e＝z-h(T,p)

把其他时刻的观测量考虑进来。设z_ij为在位姿T_i处观察路标p_j产生的数据，那么整体的代价函数为：

对这个最小二乘求解，相当于对位姿和路标同时做了调整，也就是所谓的BA。

S4：系统初始化，重复相机轨迹进行循环检测和循环修正，得到新的局部地图，循环检测和循环修正结束后，将同一个局部地图的经多次检测和修正后的结果进行平均值操作，得到最终局部地图；其中重复相机轨迹进行循环检测和循环修正包括：在重复相机轨迹过程中，循环检测主要是在关键帧数据库中查找与当前检测的关键帧是否为相似的关键帧，如果是，则当作闭环候选帧，使用闭环候选帧再次定位相机轨迹，得到新的局部地图，循环修正是在每次循环检测的过程中，将同一个局部地图的经多次检测和修正后的结果进行平均值操作，得到最终局部地图。

S5：所有的最终局部地图形成全局地图，对全局地图进行全局BA优化，再经过循环检测和循环修正更新全局地图，构建出环境中的完整地图。

通过以上技术方案，本发明将单目相机和IMU分别输入先验信息，来确定相机移动过程，剔除误差较大的数据后进行BA优化，然后重复相机轨迹经过循环检测和循环修正，轨迹平均得出最终轨迹也即最终局部地图，此过程具有更高的精度和更小的轨迹误差。

实施例2

基于本发明实施例1，本发明实施例2还提供基于点线特征融合的单目视觉贯导SLAM装置，所述装置包括：

信息获取模块，用于将单目相机获取的图像和IMU获取的环境信息作为输入信息；

关键帧获取模块，用于基于输入信息进行点线特征提取、点线特征加权及点线特征匹配，点线特征提取的同时进行三角化测量实时获取相机的位姿，IMU安装在单目相机的重心处，IMU也实时获取相机的位姿，对两种方式得到的相机的位姿融合得到相机移动过程，相机移动过程中获得很多帧图像，将所有帧图像中点线特征匹配成功的图像帧作为关键帧；

BA优化模块，用于对所有关键帧中提取特征点低于第一预设值的关键帧剔除，剩余的关键帧形成的相机运动轨迹构成多个局部地图，所有局部地图中点线特征个数低于第二预设值的剔除，对剩下的局部地图进行BA优化；

检测修正模块，用于系统初始化，重复相机轨迹进行循环检测和循环修正，得到新的局部地图，循环检测和循环修正结束后，将同一个局部地图的经多次检测和修正后的结果进行平均值操作，得到最终局部地图；

全局优化模块，用于所有的最终局部地图形成全局地图，对全局地图进行全局BA优化，再经过循环检测和循环修正更新全局地图，构建出环境中的完整地图。

具体的，所述信息获取模块之前还包括IMU初始化，IMU初始化的过程为：

将IMU采集的参数放在一起构成状态向量y_k＝{s,R_wg,b,v_0:k}，其中，s表示尺度因子，R_wg表示重力向量，重力向量在世界坐标系下的表示为g＝R_wgg_I，其中g_I＝(0,0,G)^T，G是重力大小模值；表示加速度计以及陀螺仪的偏置，在初始化阶段该偏置为常数；v_0:k表示从首帧图像到末帧图像的无尺度机体速度；

IMU初始化中被最大化的后验分布为p(y_k|I_0:k)∝p(I_0:k|y_k)p(y_k)，其中，I_0:k表示首帧图像到末帧图像的测量信息，表示已经测量到的首帧图像到末帧图像的测量信息且p(y_k|I_0:k)表示最大化的后验分布，∝表示相当于，p(I_0:k|y_k)表示似然，p(y_k)表示先验分布；

IMU的最大后验估计表示为

其中，I_i-1表示第i-1帧的测量信息，g_dir表示重力向量，v_i-1表示第i-1帧图像的无尺度机体速度，v_i表示第i帧图像的无尺度机体速度；

对IMU的最大后验估计取负对数并且假设IMU预积分以及先验分布服从高斯分布，于是IMU最终的最大后验估计表示为

其中，r_p表示先验残差项，I_i-1,i表示相邻两帧的测量信息。

具体的，所述关键帧获取模块还用于：通过FAST角点提取算法进行点特征提取，通过线段检测器LSD算法进行线特征提取，在低纹理场景中，对点线特征加权且线特征的权重高于点特征的权重。

具体的，所述关键帧获取模块中所述点线特征匹配的过程为：

单目相机获得的图像的相邻帧中的点特征分别记为K_i和Q_j,i、j＝1,2,3…，把K₁与Q_j中距离最近的点特征特征匹配，把K₂与Q_j中距离最近的点特征特征匹配，依此类推完成点特征匹配；

单目相机获得的图像的相邻帧中的线特征分别记为l_c和l′_c，通过公式获取重投影误差，当图像当前帧的当前线特征与其相邻帧中的某个线特征的重投影误差最小时，线特征匹配成功，其中，l₁、l₂分别表示直线l_c的x方向系数以及y方向系数，表示l′_c的两个端点分别到l_c的距离。

具体的，所述关键帧获取模块中对两种方式得到的相机的位姿融合得到相机移动过程包括：根据三角化测量获取相机的位姿形成相机的第一移动轨迹，根据IMU获取相机的位姿形成相机的第二移动轨迹，第一移动轨迹与第二移动轨迹按照时间轴重叠后，剔除同一时刻两者的位姿偏差超过第三预设值的位姿，然后对第一移动轨迹和第二移动轨迹求平均值，得到相机移动过程。

具体的，所述检测修正模块中重复相机轨迹进行循环检测和循环修正包括：在重复相机轨迹过程中，循环检测主要是在关键帧数据库中查找与当前检测的关键帧是否为相似的关键帧，如果是，则当作闭环候选帧，使用闭环候选帧再次定位相机轨迹，得到新的局部地图，循环修正是在每次循环检测的过程中，将同一个局部地图的经多次检测和修正后的结果进行平均值操作，得到最终局部地图。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。