一种基于深度点线特征的无人机视觉里程计方法与流程
本发明涉及机器人视觉技术领域，更具体地，涉及一种基于深度点线特征的无人机视觉里程计方法。
背景技术：
机器人在无gps的未知环境下，通过传感器(如激光、摄像头等)搜集到的距离或者图像等信息来估计机器人的运动并把地图建立起来，这样机器人就能在未知的领域完成自主运动，即slam(simultaneouslocalizationandmapping)。它是近几十年来机器人领域最活跃的研究方向，应用于军事和生活的各个方面，如扫地机器人，电网巡视监管无人机等。根据传感器的不同分为激光slam和视觉slam。在视觉slam中，当前开源了许多优秀算法，如orbslam(特征点法的代表)，vins-mono(用的光流法跟踪和融合imu)，rgbdslam等等。
激光slam目前已经非常成熟，已经有大量实地应用。相对于激光slam，视觉slam由于相机价格更低廉、能收集到更多的信息，越来越受到研究者们的关注，当然视觉图像里面也提供了许多无用的信息，导致了处理起来困难，需要占用大量的计算资源。基于特征点的slam由于需要大量的时间计算特征点，还未能很好的应用于实践。在现有的基于特征点的slam算法中，由于相机提取的特征点受到外部环境的影响(如动态环境，光照变化明显的室外，特征点较少的平面墙壁，天花板等等)，系统很容易导致跟踪特征点失败、出现误匹配和特征跟丢的情况，从而整个slam系统精度下降甚至无法工作。
当前，无人机在特征点较少的场景和快速移动下会出现的特征误匹配、特征点跟丢的缺点，从而导致估计位姿不准确。
技术实现要素：
本发明为克服上述现有技术中无人机在特征点较少的场景和快速移动下出现的特征误匹配、特征点跟丢导致位姿估计不准确的缺陷，提供一种基于深度点线特征的无人机视觉里程计方法。
本发明的首要目的是为解决上述技术问题，本发明的技术方案如下：
一种基于深度点线特征的无人机视觉里程计方法，包括以下步骤：
s1：进行深度相机的标定，获取深度相机的内参矩阵和畸变系数；
s2：利用深度相机获取视觉图像，提取视觉图像的关键点特征，并将关键点的像素坐标转化为三维坐标；
s3：提取视觉图像的线特征；
s4：利用平均视差法和跟踪质量法在视觉图像中提取当前的关键帧；
s5：利用已提取的关键点特征和线特征对视觉图像相邻帧进行特征匹配，利用icp算法估计设备的运动位姿；
s6：对估计的运动位姿进行局部优化，输出位姿。
进一步地，步骤s2的具体步骤为：
s201：将视觉图像进行灰度化处理，得到灰度图像并对灰度图像构建高斯金字塔，按照高斯金字塔每层图像的比例因子确定每层图像的特征点个数，根据特征点的个数提取关键点；
s202：利用灰度质心法计算关键点的质心像素坐标，计算公式如下：
其中，f是灰度值，当像素灰度值小于阈值t时为0，大于阈值t时为f；
s203：计算关键点的brief描述子，利用brief描述子对特征点进行排序，brief描述子计算过程为：设定p为关键点，在关键点p的周围选取n个点对，把n个点对组合起来作为描述子，p1(a,b),p2(a,b),.pn(a,b),其中，a和b代表一个点对中的两个像素点，当a的灰度大于b时，值为1，反之为0；
s204：利用深度相机测得的视觉图像的深度将关键点的像素坐标转化为三维坐标。
进一步地，步骤s3提取线特征的具体步骤为：
s301：利用线段检测器lsd对视觉图像灰度图进行线性切割，提取出视觉图像上的线段s＝{si,i＝1,2,3,…,}，其中si＝[ai,bi]，ai，bi分别是线段两个端点的像素坐标；
s302：设置采样点个数，对线段s进行采样，其中采样点的个数为n,n＝min(100,
s303：利用随机采样一致性的方法剔除采样点中的误差点，得到误差点剔除后的点集合记为n'，将点集合n'中点进行直线拟合，计算选取点集合n'中点到拟合直线的马氏距离，得到最优3d点，利用最优3d点拟合直线。
进一步地，所述利用马氏距离公式得到最优3d点，具体过程为：
若误差剔除后的采样点的个数与原采样点的个数的比值大于设定阈值，则计算采样点的马氏距离，计算公式如下：
其中p是三维空间点，∑p是协方差，l是由若干个点构成的线，q是l上任意一点；
将q＝a+ω(b-a)代入马氏距离计算公式：
将ω的最优解代入即求得马氏距离的最优解。
进一步地，步骤s4所述的平均视差法为：若在当前视觉图像帧和上一关键帧之前跟踪的特征点平均视差超出阈值t，则将当前视觉图像帧设为新的关键帧；
所述跟踪质量法为：如果当前视觉图像帧跟踪的特征点数量小于预设数量阈值，则将当前视觉图像帧设为关键帧。
进一步地，步骤s5利用已提取的关键点特征和线特征对视觉图像相邻帧进行特征匹配，利用icp算法估计设备的运动位姿；具体步骤为：
若匹配中只检测到点特征没有检测到线特征，则将检测到的点转化为3d点，并利用icp算法估计相机运动；
若匹配中只检测到线特征没有检测到特征点，则在检测到的线上随机选择点，并将所选的点利用icp算法估计相机运动；
若匹配中同时检测到点特征和线特征，则对检测到的点利用icp算法估计相机运动。
进一步地，步骤s6所述对估计的运动位姿进行局部优化是采用滑动窗口的位姿图进行优化，具体过程为：设置滑动窗口的大小为m帧，m为正整数，每一次对m帧的位姿进行局部的非线性优化，构建最小二乘模型，利用深度相机的观测像素值，内参矩阵，并采用列文伯格-马夸尔特方法求解最优位姿。
与现有技术相比，本发明技术方案的有益效果是：
本发明通过分别提取视觉图像的点特征和线特征，根据点特征和线特征的不同匹配进行关键帧的选取，克服了无人机在特征点较少的场景和快速移动下出现的特征误匹配、特征点跟丢导致位姿估计不准确的缺陷，提高了位姿估计的准确度。
附图说明
图1为本发明方法流程图。
具体实施方式
为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。
在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。
实施例1
如图1所示一种基于深度点线特征的无人机视觉里程计方法，包括以下步骤：
s1：进行深度相机的标定，获取深度相机的内参矩阵和畸变系数；
在一个具体的实施例中，用ros(机器人操作系统)的camera_calibration功能包进行深度相机的标定，获得相机的内参矩阵(fx、fy、cx、cy)和外参(畸变系数)。
s2：利用深度相机获取视觉图像，提取视觉图像的关键点特征，并将关键点的像素坐标转化为三维坐标；
更具体地，步骤s2的具体步骤为：
s201：将视觉图像进行灰度化处理，得到灰度图像并对灰度图像构建高斯金字塔，按照高斯金字塔每层图像的比例因子确定每层图像的特征点个数，根据特征点的个数提取关键点；
需要说明的是，在一个具体的实施例中，所述高斯金字塔可以为7层，所述关键点为fast关键点。
s202：利用灰度质心法计算关键点的质心像素坐标，计算公式如下：
其中，f是灰度值，当像素灰度值小于阈值t时为0，大于阈值t是为f；
s203：计算关键点的brief描述子，利用brief描述子对特征点进行排序去掉密集的点，使特征点分布均匀，进行匹配。具体描述子计算过程为：设定p为关键点，在关键点p的周围选取n个点对，把n个点对组合起来作为描述子，p1(a,b),p2(a,b),.pn(a,b),其中，a和b代表一个点对中的两个像素点，当a的灰度大于b时，值为1，反之为0。
s204：利用深度相机测得的视觉图像的深度将关键点的像素坐标转化为三维坐标。计算公式如下：
其中，u表示像素的横坐标，v表示像素的纵坐标，d表示视觉图像的深度，p表示关键点，(fx、fy、cx、cy)为深度相机的内参矩阵。
s3：提取视觉图像的线特征；
步骤s3提取线特征的具体步骤为：
s301：利用线段检测器lsd对视觉图像灰度图进行线性切割，提取出视觉图像上的线段s＝{si,i＝1,2,3,…,}，其中si＝[ai,bi]，ai，bi分别是线段两个端点的像素坐标；
s302：设置采样点个数，对线段s进行采样，其中采样点的个数为n,n＝min(100,
s303：利用随机采样一致性的方法剔除采样点中的误差点，得到误差点剔除后的点集合记为n'，将点集合n'中点进行直线拟合，计算选取点集合n'中点到拟合直线的马氏距离，得到最优3d点，利用最优3d点拟合直线。
进一步地，所述利用马氏距离公式得到最优3d点，具体过程为：
若误差剔除后的采样点的个数与原采样点的个数的比值大于设定阈值，则计算采样点的马氏距离，计算公式如下：
其中，p是三维空间点，∑p是协方差，l是由若干个点构成的线，q是l上任意一点；
将q＝a+ω(b-a)代入马氏距离计算公式：
将ω的最优解代入即求得马氏距离的最优解。
s4：利用平均视差法和跟踪质量法在视觉图像中提取当前的关键帧；
步骤s4所述的平均视差法为：若在当前视觉图像帧和上一关键帧之前跟踪的特征点平均视差超出阈值t，则将当前视觉图像帧设为新的关键帧；
所述跟踪质量法为：如果当前视觉图像帧跟踪的特征点数量小于预设数量阈值，则将当前视觉图像帧设为关键帧。
s5：利用已提取的关键点特征和线特征对视觉图像相邻帧进行特征匹配，利用icp算法估计设备的运动位姿；
步骤s5利用已提取的关键点特征和线特征对视觉图像相邻帧进行特征匹配，利用icp算法估计设备的运动位姿；具体步骤为：
若匹配中只检测到点特征没有检测到线特征，则将检测到的点转化为3d点，并利用icp算法估计相机运动；
若匹配中只检测到线特征没有检测到特征点，则在检测到的线上随机选择点，并将所选的点利用icp算法估计相机运动；
若匹配中同时检测到点特征和线特征，则对检测到的点利用icp算法估计相机运动。
s6：对估计的运动位姿进行局部优化，输出位姿。
更具体地，步骤s6所述对估计的运动位姿进行局部优化是采用滑动窗口的位姿图进行优化，具体过程为：设置滑动窗口的大小为m帧，m为正整数，每一次对m帧的位姿进行局部的非线性优化，构建最小二乘模型，利用深度相机的观测像素值，内参矩阵，并采用列文伯格-马夸尔特方法求解最优位姿。
例如，当前关键帧序列为{k1,k2,…,k10}，当新加入一个关键帧k11时，就舍去最前面一帧k1，此时的滑动窗口帧序列为{k2,k3,…,k11}，每次对这10帧的位姿进行局部的非线性优化，构建最小二乘模型为
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。
- 还没有人留言评论。精彩留言会获得点赞！