一种视觉自主定位系统实现方法技术方案

本发明专利技术提出一种视觉自主定位系统实现方法，包括以下步骤：使用RGBD传感器采集场景的RGB图像和深度图像，从中计算点云图像；提取所述点云图像的特征点和特征平面信息；利用所述特征点和特征平面进行全局地图匹配，计算传感器位姿；维护关键帧序列和全局地图并进行全局优化。根据本发明专利技术实施例的视觉自主定位系统实现方法，算法鲁棒性强，实现自主定位的同时可以实时获得场景的点云模型，可广泛应用于无人机、机器人等多种移动平台在室内和室外环境下的自主导航等。

【技术实现步骤摘要】
一种视觉自主定位系统实现方法
本专利技术涉及视觉导航研究领域，特别涉及一种一种视觉自主定位系统实现方法。
技术介绍
无人机视觉导航技术可以分为单目视觉导航、双目视觉导航和深度视觉导航。这种区分体现在使用的摄像头上：前两者使用的是一个或多个普通摄像头，而后者使用的是Kinect、Xtion等能够直接获得图像深度信息的摄像头(即深度摄像头)。直接使用深度摄像头的优势在于能够直接获得较为准确的场景深度信息，劣势在于可视范围有限，仅能应用于室内场景。视觉SLAM的研究成果广泛，有PTAM、LSD-SLAM、point-planeSLAM等，但它们的效果均不足以完成无人机视觉导航的任务。同时，由于人造场景是无人机的主要应用场景，而人造场景的特征就是具有很多平面，因此平面特征的提取和运用也成为研究导航和环境重建的新思路。
技术实现思路
本专利技术旨在至少在一定程度上解决上述技术问题之一或至少提供一种有用的商业选择。为此，本专利技术的一个目的在于提出一种一种视觉自主定位系统实现方法，该方法算法鲁棒性强，实现自主定位的同时可以实时获得场景的点云模型，可广泛应用于无人机、机器人等多种移动平台在室内和室外环境下的自主导航。为了实现上述目的，本专利技术公开了一种视觉自主定位系统实现方法，包括以下步骤：使用RGBD传感器采集场景的RGB图像和深度图像，从中计算点云图像；提取所述点云图像的特征点和特征平面信息；利用所述特征点和特征平面进行全局地图匹配，计算传感器位姿；维护关键帧序列和全局地图并进行全局优化。根据本专利技术的视觉自主定位系统实现方法，算法鲁棒性强，实现自主定位的同时可以实时获得场景的点云模型，可广泛应用于无人机、机器人等多种移动平台在室内和室外环境下的自主导航。另外，根据本专利技术上述的视觉自主定位系统实现方法还可以具有如下附加的技术特征：进一步地，所述使用RGBD传感器采集场景的RGB图像和深度图像，从中计算点云图像的步骤具体包括：在获得RGB图像和深度图像之后，通过对齐所述RGB图像和深度图像计算出点云图像，其中，所述RGBD传感器包括可同时采集RGB图像和深度图像的传感器。进一步地，所述提取所述点云图像的特征点和特征平面信息的步骤具体包括：使用SIFT算法、SURF算法或ORB算法提取所述点云图像的特征点，其中，所述特征点包括RGB图像中的二维特征点，并在获得RGB图像中的二维特征点后，反投影到点云图像上获得该点在当前相机坐标系下的实际空间三维坐标。进一步地，所述提取所述点云图像的特征点和特征平面信息的具体步骤：采用Grid-BasedRANSAC方法提取所述点云图像的特征平面信息，其中：将点云图像分块，在每个点云块中使用RANSAC算法计算其平面方程，获得平面内点数Nk和平面方程的方程参数(a,b,c,d)，若Nk>Nth则判定该点云块具有平面特征，并对该点云块进行编号，将编号的所述点云块集合为集合G，其中，Nth为根据点云块中的点云数量设定阈值；选取集合G中编号最小的点云块标记为gc，其所代表的平面标记为pi，对与其空间相邻的具有平面特征的点云块gn进行条件判断，所述条件为：(1)(2)(3)MSE(gc∪gn)<ε，若点云块gn满足所述条件时，将所述点云块gn加入到gc所代表的平面中并将其从集合G中去除，历遍所有点云块gn直到没有新的邻域点云平面加入或当前邻域点云块均不具备平面特征为止，当平面pi包含的gc和所有满足所述条件而加入pi的点云块gn大于特定阈值时，提取一个平面，并将其记为Pi，其中，n＝1,2,3，…n，表示具有平面特征的点云块gc的法向量，表示具有平面特征的点云块gn的法向量，表示点云块gn的重心，为gn所有点的平均值，表示点云块gc的重心，为gc所有点的平均值，η表示判定平面gn和gc的法向量是否平行的阈值，η越接近1表示两个平面越接近平行，γ表示判断两个平行平面是否共面的阈值，如果平面gn的法向量与两个平面重心之差的内积大于阈值γ，则两个平面平行非共面，∈表示平面gn与gc融合在一起之后的新点云块的均方误差阈值，如果新点云块的均方误差大于阈值∈，则判定为非平面，表明gn不能融合进gc，这里的均方误差指的是新点云块中的点沿着gc法方向的均方误差；当所述集合G为空集时，则判定已将点云图像中所有平面都已提取完毕，所有Pi的平面集合记为平面集合P。进一步地，还包括：对平面集合P进行平面优化，其中，选择平面集合P中所含内点数量最多的平面记为PM，以所述PM的平面方程为参考方程，所述PM的法向量为参考法向量，将所述平面集合P除去所述PM的平面的法向量与参考法向量求点积，点积值记为Doti，i为平面序号，i＝1，2，3，…，i，若Dotmin-th<Doti<Dotmax-th，则认为该平面的方向不符合要求，将其从平面集合P中去掉，其中，Dotmin-th和Dotmax-th是根据具体传感器性能设置的阈值，所述法向量已归一化。进一步地，所述利用所述特征点和特征平面进行全局地图匹配，计算传感器位姿的步骤具体包括：使用ICP算法对所述特征点和特征平面进行全局地图匹配，其中，使用上一帧相对于所述全局地图的位姿变换关系作为ICP算法迭代的初始值。进一步地，还包括：取当前帧中最大的特征平面作为参考平面Pr，其在全局地图中的对应平面为Pr′；当前帧中任意特征点x和其在全局地图中的对应点x′，如果Dist(x,Pr)与Dist(x′,Pr′)相差过大，则认为该特征点的匹配是错误的，Dist(,Pr)表示在同一坐标系下点x到平面Pr的距离。进一步地，还包括：R为待求位姿旋转矩阵，t为待求位姿平移向量，qi与q′i是当前帧与全局地图中的一对对应特征点的三维坐标，nj和n′j是当前帧与全局地图中的一对对应特征平面的法向量，误差方程为：其中是第j个平面法向量在计算中所占的权重，其值与该平面大小有关。使用SVD算法求解MSE最小值点，得到位姿旋转矩阵R的估计匹配上的特征点对和特征平面对的数量分别为M和N，利用点和平面的混合求解平移向量，累加所有点估计出的平移误差和平面估计出的平移误差得到平移误差公式：其中，是当前帧中特征点的坐标算术平均值，是对应的全局地图中特征点的坐标算术平均值是第j对平面特征在平移误差计算中的权重，nj′是第j个全局地图平面的法向量，dj和dj′是在同一坐标系下，当前帧第j个平面和全局地图中对应的平面到坐标原点的距离；所述平移误差的求解对应于求解一个线性系统，A1＝MI3,其中I3为单位阵，所述线性系统的最小平方解按由如下思路求解：得到当前帧相对于全局地图的位姿估计进一步地，还包括：获得当前帧相对于全局地图的位姿后，将其特征点和特征平面整合到全局地图中，其中，对于有匹配的平面或点，与其匹配取平均作为新的地标，对于没有找到匹配的平面和点，加入全局地图成为新的地标。进一步地，所述维护关键帧序列和全局地图并进行全局优化的步骤具体包括：若获得当前本文档来自技高网...

【技术保护点】
1.一种视觉自主定位系统实现方法，其特征在于，包括以下步骤：/n使用RGBD传感器采集场景的RGB图像和深度图像，从中计算点云图像；/n提取所述点云图像的特征点和特征平面信息；/n利用所述特征点和特征平面进行全局地图匹配，计算传感器位姿；/n维护关键帧序列和全局地图并进行全局优化。/n

【技术特征摘要】
1.一种视觉自主定位系统实现方法，其特征在于，包括以下步骤：
使用RGBD传感器采集场景的RGB图像和深度图像，从中计算点云图像；
提取所述点云图像的特征点和特征平面信息；
利用所述特征点和特征平面进行全局地图匹配，计算传感器位姿；
维护关键帧序列和全局地图并进行全局优化。


2.根据权利要求1所述的视觉自主定位系统实现方法，其特征在于，所述使用RGBD传感器采集场景的RGB图像和深度图像，从中计算点云图像的步骤具体包括：在获得RGB图像和深度图像之后，通过对齐所述RGB图像和深度图像计算出点云图像，其中，所述RGBD传感器包括可同时采集RGB图像和深度图像的传感器。


3.根据权利要求1所述的视觉自主定位系统实现方法，其特征在于，所述提取所述点云图像的特征点和特征平面信息的步骤具体包括：使用SIFT算法、SURF算法或ORB算法提取所述点云图像的特征点，其中，所述特征点包括RGB图像中的二维特征点，并在获得RGB图像中的二维特征点后，反投影到点云图像上获得该点在当前相机坐标系下的实际空间三维坐标。


4.根据权利要求1或3所述的视觉自主定位系统实现方法，其特征在于，所述提取所述点云图像的特征点和特征平面信息的具体步骤：采用Grid-BasedRANSAC方法提取所述点云图像的特征平面信息，其中：
将点云图像分块，在每个点云块中使用RANSAC算法计算其平面方程，获得平面内点数Nk和平面方程的方程参数(a，b，c，d)，若Nk＞Nth则判定该点云块具有平面特征，并对该点云块进行编号，将编号的所述点云块集合为集合G，其中，Nth为根据点云块中的点云数量设定阈值；
选取集合G中编号最小的点云块标记为gc，其所代表的平面标记为pi，对与其空间相邻的具有平面特征的点云块gn进行条件判断，所述条件为：(1)(2)(3)MSE(gc∪gn)＜∈，若点云块gn满足所述条件时，将所述点云块gn加入到gc所代表的平面中并将其从集合G中去除，历遍所有点云块gn直到没有新的邻域点云平面加入或当前邻域点云块均不具备平面特征为止，当平面pi包含的gc和所有满足所述条件而加入pi的点云块gn大于特定阈值时，提取一个平面，并将其记为Pi，其中，n＝1，2，3，…n，表示具有平面特征的点云块gc的法向量，表示具有平面特征的点云块gn的法向量，表示点云块gn的重心，为gn所有点的平均值，表示点云块gc的重心，为gc所有点的平均值，η表示判定平面gn和gc的法向量是否平行的阈值，η越接近1表示两个平面越接近平行，γ表示判断两个平行平面是否共面的阈值，如果平面gn的法向量与两个平面重心之差的内积大于阈值γ，则两个平面平行非共面，∈表示平面gn与gc融合在一起之后的新点云块的均方误差阈值，如果新点云块的均方误差大于阈值∈，则判定为非平面，表明gn不能融合进gc，这里的均方误差指的是新点云块中的点沿着gc法方向的均方误差；
当所述集合G为空集时，则判定已将点云图像中所有平面都已提取完毕，所有Pi的平面集合记为平面集合P。


5.根据权利要求4所述的视觉自主定位系统实现方法，其特征在于，还包括：
对平面集合P进行平面优化，其中，选择平面集合P中所含内点数量最多的平面记为PM，以所述PM的平面方程为参考方程，所述PM的法向量为参考法向量，将所...

【专利技术属性】
技术研发人员：李一鹏，安亮，戴琼海，
申请(专利权)人：清华大学，
类型：发明
国别省市：北京;11