CN111583316B - 一种视觉自主定位系统实现方法 - Google Patents

一种视觉自主定位系统实现方法 Download PDF

Info

Publication number
CN111583316B
CN111583316B CN202010352385.3A CN202010352385A CN111583316B CN 111583316 B CN111583316 B CN 111583316B CN 202010352385 A CN202010352385 A CN 202010352385A CN 111583316 B CN111583316 B CN 111583316B
Authority
CN
China
Prior art keywords
plane
point cloud
feature
points
image
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010352385.3A
Other languages
English (en)
Other versions
CN111583316A (zh
Inventor
李一鹏
安亮
戴琼海
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tsinghua University
Original Assignee
Tsinghua University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tsinghua University filed Critical Tsinghua University
Priority to CN202010352385.3A priority Critical patent/CN111583316B/zh
Publication of CN111583316A publication Critical patent/CN111583316A/zh
Application granted granted Critical
Publication of CN111583316B publication Critical patent/CN111583316B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10004—Still image; Photographic image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Navigation (AREA)
  • Image Analysis (AREA)

Abstract

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

Description

一种视觉自主定位系统实现方法
技术领域
本发明涉及视觉导航研究领域，特别涉及一种一种视觉自主定位系统实现方法。
背景技术
无人机视觉导航技术可以分为单目视觉导航、双目视觉导航和深度视觉导航。这种区分体现在使用的摄像头上：前两者使用的是一个或多个普通摄像头，而后者使用的是Kinect、Xtion等能够直接获得图像深度信息的摄像头(即深度摄像头)。直接使用深度摄像头的优势在于能够直接获得较为准确的场景深度信息，劣势在于可视范围有限，仅能应用于室内场景。
视觉SLAM的研究成果广泛，有PTAM、LSD-SLAM、point-plane SLAM等，但它们的效果均不足以完成无人机视觉导航的任务。同时，由于人造场景是无人机的主要应用场景，而人造场景的特征就是具有很多平面，因此平面特征的提取和运用也成为研究导航和环境重建的新思路。
发明内容
本发明旨在至少在一定程度上解决上述技术问题之一或至少提供一种有用的商业选择。
为此，本发明的一个目的在于提出一种一种视觉自主定位系统实现方法，该方法算法鲁棒性强，实现自主定位的同时可以实时获得场景的点云模型，可广泛应用于无人机、机器人等多种移动平台在室内和室外环境下的自主导航。
为了实现上述目的，本发明公开了一种视觉自主定位系统实现方法，包括以下步骤：使用RGBD传感器采集场景的RGB图像和深度图像，从中计算点云图像；提取所述点云图像的特征点和特征平面信息；利用所述特征点和特征平面进行全局地图匹配，计算传感器位姿；维护关键帧序列和全局地图并进行全局优化。
根据本发明的视觉自主定位系统实现方法，算法鲁棒性强，实现自主定位的同时可以实时获得场景的点云模型，可广泛应用于无人机、机器人等多种移动平台在室内和室外环境下的自主导航。
另外，根据本发明上述的视觉自主定位系统实现方法还可以具有如下附加的技术特征：
进一步地，所述使用RGBD传感器采集场景的RGB图像和深度图像，从中计算点云图像的步骤具体包括：在获得RGB图像和深度图像之后，通过对齐所述RGB图像和深度图像计算出点云图像，其中，所述RGBD传感器包括可同时采集RGB图像和深度图像的传感器。
进一步地，所述提取所述点云图像的特征点和特征平面信息的步骤具体包括：使用SIFT算法、SURF算法或ORB算法提取所述点云图像的特征点，其中，所述特征点包括RGB图像中的二维特征点，并在获得RGB图像中的二维特征点后，反投影到点云图像上获得该点在当前相机坐标系下的实际空间三维坐标。
进一步地，所述提取所述点云图像的特征点和特征平面信息的具体步骤：采用Grid-Based RANSAC方法提取所述点云图像的特征平面信息，其中：将点云图像分块，在每个点云块中使用RANSAC算法计算其平面方程，获得平面内点数Nk和平面方程的方程参数(a,b,c,d)，若Nk>Nth则判定该点云块具有平面特征，并对该点云块进行编号，将编号的所述点云块集合为集合G，其中，Nth为根据点云块中的点云数量设定阈值；选取集合G中编号最小的点云块标记为gc，其所代表的平面标记为pi，对与其空间相邻的具有平面特征的点云块gn进行条件判断，所述条件为：(1)
Figure BDA0002472308730000021
(2)
Figure BDA0002472308730000022
(3)MSE(gc∪gn)<ε，若点云块gn满足所述条件时，将所述点云块gn加入到gc所代表的平面中并将其从集合G中去除，历遍所有点云块gn直到没有新的邻域点云平面加入或当前邻域点云块均不具备平面特征为止，当平面pi包含的gc和所有满足所述条件而加入pi的点云块gn大于特定阈值时，提取一个平面，并将其记为Pi，其中，n＝1,2,3，…n，
Figure BDA0002472308730000023
表示具有平面特征的点云块gc的法向量，
Figure BDA0002472308730000024
表示具有平面特征的点云块gn的法向量，
Figure BDA0002472308730000025
表示点云块gn的重心，为gn所有点的平均值，
Figure BDA0002472308730000026
表示点云块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是当前帧与全局地图中的一对对应特征平面的法向量，误差方程为：
Figure BDA0002472308730000031
其中
Figure BDA0002472308730000032
是第j个平面法向量在计算中所占的权重，其值与该平面大小有关。使用SVD算法求解MSE最小值点，得到位姿旋转矩阵R的估计
Figure BDA0002472308730000033
匹配上的特征点对和特征平面对的数量分别为M和N，利用点和平面的混合求解平移向量，累加所有点估计出的平移误差和平面估计出的平移误差得到平移误差公式：
Figure BDA0002472308730000034
其中，
Figure BDA0002472308730000035
是当前帧中特征点的坐标算术平均值，
Figure BDA0002472308730000036
是对应的全局地图中特征点的坐标算术平均值
Figure BDA00024723087300000313
是第j对平面特征在平移误差计算中的权重，nj′是第j个全局地图平面的法向量，dj和dj′是在同一坐标系下，当前帧第j个平面和全局地图中对应的平面到坐标原点的距离；所述平移误差的求解对应于求解一个线性系统，
Figure BDA0002472308730000038
A1＝MI3,
Figure BDA0002472308730000039
Figure BDA00024723087300000310
其中I3为单位阵，所述线性系统的最小平方解按由如下思路求解：
Figure BDA00024723087300000311
得到当前帧相对于全局地图的位姿估计
Figure BDA00024723087300000312
进一步地，还包括：获得当前帧相对于全局地图的位姿后，将其特征点和特征平面整合到全局地图中，其中，对于有匹配的平面或点，与其匹配取平均作为新的地标，对于没有找到匹配的平面和点，加入全局地图成为新的地标。
进一步地，所述维护关键帧序列和全局地图并进行全局优化的步骤具体包括：若获得当前帧的相对于全局地图的位姿与上一关键帧的位姿之间的差距足够大，则将该当前帧加入到所述关键帧序列中，其中，所述差距足够大是指旋转矩阵之间的角度大于一定值或者平移向量的差的模大于一定值。
进一步地，还包括：使用g2o算法利用关键帧的位姿和点云数据进行全局地图的优化。
本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
图1是根据本发明一个实施例的视觉自主定位系统实现方法的流程图。
具体实施方式
在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“顺时针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
在本发明中，除非另有明确的规定和限定，第一特征在第二特征之“上”或之“下”可以包括第一和第二特征直接接触，也可以包括第一和第二特征不是直接接触而是通过它们之间的另外的特征接触。而且，第一特征在第二特征“之上”、“上方”和“上面”包括第一特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”包括第一特征在第二特征正下方和斜下方，或仅仅表示第一特征水平高度小于第二特征。
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。
图1是根据本发明一个实施例的视觉自主定位系统实现方法的流程图。
如图1所示，本发明实施例的视觉自主定位系统实现方法，包括以下步骤：
S110：使用RGBD传感器采集场景的RGB图像和深度图像，从中计算点云图像。
具体来说，在获得RGB图像和深度图像之后，通过对齐RGB图像和深度图像计算出点云图像，其中，RGBD传感器包括可同时采集RGB图像和深度图像的传感器，进一步地，使用的是Kinect传感器。
S120：提取点云图像的特征点和特征平面信息。
具体来说，包括但不限于使用SIFT算法、SURF算法或ORB算法等提取点云图像的特征点，其中，特征点包括RGB图像中的二维特征点，并在获得RGB图像中的二维特征点后，反投影到点云图像上获得该点在当前相机坐标系下的实际空间三维坐标。
作为一个示例，采用Grid-Based RANSAC方法提取所述点云图像的特征平面信息，其中：将点云图像分块，在每个点云块中使用RANSAC算法计算其平面方程，获得平面内点数Nk和平面方程的方程参数(a,b,c,d)，若Nk>Nth则判定该点云块具有平面特征，并对该点云块进行编号，将编号的所述点云块集合为集合G，其中，Nth为根据点云块中的点云数量设定阈值。选取集合G中编号最小的点云块标记为gc，其所代表的平面标记为pi，对与其空间相邻的具有平面特征的点云块gn进行条件判断，条件为：(1)
Figure BDA0002472308730000051
(2)
Figure BDA0002472308730000052
(3)MSE(gc∪gn)<ε，若点云块gn满足条件时，将点云块gn加入到gc所代表的平面中并将其从集合G中去除，历遍所有点云块gn直到没有新的邻域点云平面加入或当前邻域点云块均不具备平面特征为止，当平面pi包含的gc和所有满足条件而加入pi的点云块gn大于特定阈值时，提取一个平面，并将其记为Pi，其中，n＝1,2,3，…n，
Figure BDA0002472308730000053
表示具有平面特征的点云块gc的法向量，
Figure BDA0002472308730000054
表示具有平面特征的点云块gn的法向量，
Figure BDA0002472308730000055
表示点云块gn的重心，为gn所有点的平均值，
Figure BDA0002472308730000056
表示点云块gc的重心，为gc所有点的平均值，η表示判定平面gn和gc的法向量是否平行的阈值，η越接近1表示两个平面越接近平行，γ表示判断两个平行平面是否共面的阈值，如果平面gn的法向量与两个平面重心之差的内积大于阈值γ，则两个平面平行非共面，∈表示平面gn与gc融合在一起之后的新点云块的均方误差阈值，如果新点云块的均方误差大于阈值∈，则判定为非平面，表明gn不能融合进gc，这里的均方误差指的是新点云块中的点沿着gc法方向的均方误差；当所述集合G为空集时，则判定已将点云图像中所有平面都已提取完毕，所有Pi的平面集合记为平面集合P。以使用的是Kinect传感器为例，η取0.95，γ取0.05，∈取0.05，特定阈值可以取4。
进一步地，还包括：对平面集合P进行平面优化，其中，选择平面集合P中所含内点数量最多的平面记为PM，以所述PM的平面方程为参考方程，所述PM的法向量为参考法向量，将所述平面集合P除去所述PM的平面的法向量与参考法向量求点积，点积值记为Doti，i为平面序号，i＝1，2，3，…，i，若Dotmin-th<Doti<Dotmax-th，则认为该平面的方向不符合要求，将其从平面集合P中去掉，其中，Dotmin-th和Dotmax-th是根据具体传感器性能设置的阈值，所述法向量已归一化。以Kinect为传感器为例，根据Kinect的测量误差，Dotmin-th取为0.25，Dotmax-th取为0.75。
S130：利用特征点和特征平面进行全局地图匹配，计算传感器位姿。
具体来说，使用ICP算法对特征点和特征平面进行全局地图匹配，其中，使用上一帧相对于全局地图的位姿变换关系作为ICP算法迭代的初始值。进一步地，还包括：取当前帧中最大的特征平面作为参考平面Pr，其在全局地图中的对应平面为Pr′，当前帧中任意特征点x和其在全局地图中的对应点x′，如果Dist(x,Pr)与Dist(x′,Pr′)相差过大，则认为该特征点的匹配是错误的，Dist(x,Pr)表示在同一坐标系下点x到平面Pr的距离。具体来说，判定Dist(x,Pr)与Dist(x′,Pr′)之间的差距使用比值判定，即：若
Figure BDA0002472308730000061
则认为两者相差不大，否则认为两者相差过大。
进一步地，还包括：R为待求位姿旋转矩阵，t为待求位姿平移向量，qi与q′i是当前帧与全局地图中的一对对应特征点的三维坐标，nj和n′j是当前帧与全局地图中的一对对应特征平面的法向量，误差方程为，
Figure BDA0002472308730000062
其中
Figure BDA0002472308730000063
是第j个平面法向量在计算中所占的权重，其值与该平面大小有关，使用SVD算法求解MSE最小值点，得到位姿旋转矩阵R的估计
Figure BDA0002472308730000064
匹配上的特征点对和特征平面对的数量分别为M和N，利用点和平面的混合求解平移向量，累加所有点估计出的平移误差和平面估计出的平移误差得到平移误差公式：
Figure BDA0002472308730000071
其中，
Figure BDA0002472308730000072
是当前帧中特征点的坐标算术平均值，
Figure BDA0002472308730000073
是对应的全局地图中特征点的坐标算术平均值，
Figure BDA00024723087300000710
是第j对平面特征在平移误差计算中的权重，nj′是第j个全局地图平面的法向量，dj和dj′是在同一坐标系下，当前帧第j个平面和全局地图中对应的平面到坐标原点的距离。这个公式的求解对应于求解一个线性系统：
Figure BDA0002472308730000075
A1＝MI3,
Figure BDA0002472308730000076
Figure BDA0002472308730000077
其中I3为单位阵，值得注意的是，此高维线性系统表现的形式取决于M和N的数值。如果匹配出的特征点的数量为0即M＝0时，方程退化为仅有平面的状态，则回到了平面与平面组合的配准方法。如果环境情况很复杂以至于没有平面被提取出来即N＝0时，问题就退化到仅由点对点的配准。在普通的情况下，该线性系统的最小平方解按由如下思路求解：
Figure BDA0002472308730000078
得到当前帧相对于全局地图的位姿估计
Figure BDA0002472308730000079
进一步地，还包括：获得当前帧相对于全局地图的位姿后，将其特征点和特征平面整合到全局地图中，其中，对于有匹配的平面或点，与其匹配取平均作为新的地标，对于没有找到匹配的平面和点，加入全局地图成为新的地标。
S140：维护关键帧序列和全局地图并进行全局优化。
具体来说，若获得当前帧的相对于全局地图的位姿与上一关键帧的位姿之间的差距足够大，则将该当前帧加入到关键帧序列中，其中，差距足够大是指旋转矩阵之间的角度大于一定值或者平移向量的差的模大于一定值。进一步地，还包括：使用g2o算法利用关键帧的位姿和点云数据进行全局地图的优化。
本实施案例只是该方法实现方法的一种，细节之处可能有所出入，但都不离本方法范围。
根据本发明实施例的视觉自主定位系统实现方法，算法鲁棒性强，实现自主定位的同时可以实时获得场景的点云模型，可广泛应用于无人机、机器人等多种移动平台在室内和室外环境下的自主导航等。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (10)

1.一种视觉自主定位系统实现方法，其特征在于，包括以下步骤：
使用RGBD传感器采集场景的RGB图像和深度图像，从中计算点云图像；
提取所述点云图像的特征点和特征平面信息；
利用所述特征点和特征平面进行全局地图匹配，计算传感器位姿；
维护关键帧序列和全局地图并进行全局优化；
其中，所述提取所述点云图像的特征点和特征平面信息，包括：采用Grid-BasedRANSAC方法提取所述点云图像的特征平面信息，其中：
将点云图像分块，在每个点云块中使用RANSAC算法计算其平面方程，获得平面内点数Nk和平面方程的方程参数(a，b，c，d)，若Nk＞Nth则判定该点云块具有平面特征，并对该点云块进行编号，将编号的所述点云块集合为集合G，其中，Nth为根据点云块中的点云数量设定阈值；
选取集合G中编号最小的点云块标记为gc，其所代表的平面标记为pi，对与其空间相邻的具有平面特征的点云块gn进行条件判断，所述条件为：(1)
Figure FDA0003689091250000011
(2)
Figure FDA0003689091250000012
Figure FDA0003689091250000013
(3)MSEgc∪gn＜∈，若点云块gn满足所述条件时，将所述点云块gn加入到gc所代表的平面中并将其从集合G中去除，历遍所有点云块gn直到没有新的邻域点云平面加入或当前邻域点云块均不具备平面特征为止，当平面pi包含的gc和所有满足所述条件而加入pi的点云块gn大于特定阈值时，提取一个平面，并将其记为Pi，其中，n＝1，2，3，…n，
Figure FDA0003689091250000014
表示具有平面特征的点云块gc的法向量，
Figure FDA0003689091250000015
表示具有平面特征的点云块gn的法向量，
Figure FDA0003689091250000016
表示点云块gn的重心，为gn所有点的平均值，
Figure FDA0003689091250000017
表示点云块gc的重心，为gc所有点的平均值，η表示判定平面gn和gc的法向量是否平行的阈值，η越接近1表示两个平面越接近平行，γ表示判断两个平行平面是否共面的阈值，如果平面gn的法向量与两个平面重心之差的内积大于阈值γ，则两个平面平行非共面，∈表示平面gn与gc融合在一起之后的新点云块的均方误差阈值，如果新点云块的均方误差大于阈值∈，则判定为非平面，表明gn不能融合进gc，这里的均方误差指的是新点云块中的点沿着gc法方向的均方误差；
当所述集合G为空集时，则判定已将点云图像中所有平面都已提取完毕，所有Pi的平面集合记为平面集合P。
2.根据权利要求1所述的视觉自主定位系统实现方法，其特征在于，所述使用RGBD传感器采集场景的RGB图像和深度图像，从中计算点云图像的步骤具体包括：在获得RGB图像和深度图像之后，通过对齐所述RGB图像和深度图像计算出点云图像，其中，所述RGBD传感器包括可同时采集RGB图像和深度图像的传感器。
3.根据权利要求1所述的视觉自主定位系统实现方法，其特征在于，所述提取所述点云图像的特征点和特征平面信息的步骤具体包括：使用SIFT算法、SURF算法或ORB算法提取所述点云图像的特征点，其中，所述特征点包括RGB图像中的二维特征点，并在获得RGB图像中的二维特征点后，反投影到点云图像上获得该点在当前相机坐标系下的实际空间三维坐标。
4.根据权利要求1所述的视觉自主定位系统实现方法，其特征在于，还包括：
对平面集合P进行平面优化，其中，选择平面集合P中所含内点数量最多的平面记为PM，以所述PM的平面方程为参考方程，所述PM的法向量为参考法向量，将所述平面集合P除去所述PM的平面的法向量与参考法向量求点积，点积值记为Doti，i为平面序号，i＝1，2，3，...，i，若Dotmin-th＜Doti＜Dotmax-th，则认为该平面的方向不符合要求，将其从平面集合P中去掉，其中，Dotmin-th和Dotmax-th是根据具体传感器性能设置的阈值，所述法向量已归一化。
5.根据权利要求1所述的视觉自主定位系统实现方法，其特征在于，所述利用所述特征点和特征平面进行全局地图匹配，计算传感器位姿的步骤具体包括：使用ICP算法对所述特征点和特征平面进行全局地图匹配，其中，使用上一帧相对于所述全局地图的位姿变换关系作为ICP算法迭代的初始值。
6.根据权利要求5所述的视觉自主定位系统实现方法，其特征在于，还包括：取当前帧中最大的特征平面作为参考平面Pr，其在全局地图中的对应平面为Pr′；当前帧中任意特征点x和其在全局地图中的对应点x′，如果Dist(x，Pr)与Dist(x′，Pr′)相差过大，则认为该特征点的匹配是错误的，Dist(x，Pr)表示在同一坐标系下点x到平面Pr的距离。
7.根据权利要求6所述的视觉自主定位系统实现方法，其特征在于，还包括：
R为待求位姿旋转矩阵，t为待求位姿平移向量，qi与q′i是当前帧与全局地图中的一对对应特征点的三维坐标，nj和n′j是当前帧与全局地图中的一对对应特征平面的法向量，误差方程为：
Figure FDA0003689091250000021
其中
Figure FDA0003689091250000022
是第j个平面法向量在计算中所占的权重，其值与该平面大小有关，使用SVD算法求解MSE最小值点，得到位姿旋转矩阵R的估计
Figure FDA0003689091250000023
匹配上的特征点对和特征平面对的数量分别为M和N，利用点和平面的混合求解平移向量，累加所有点估计出的平移误差和平面估计出的平移误差得到平移误差公式：
Figure FDA0003689091250000031
其中，
Figure FDA0003689091250000032
是当前帧中特征点的坐标算术平均值，
Figure FDA0003689091250000033
是对应的全局地图中特征点的坐标算术平均值，wj是第j对平面特征在平移误差计算中的权重，nj′是第j个全局地图平面的法向量，dj和dj′是在同一坐标系下，当前帧第j个平面和全局地图中对应的平面到坐标原点的距离；
所述平移误差公式的求解对应于求解一个线性系统：
Figure FDA0003689091250000034
A1＝MI3，
Figure FDA0003689091250000035
Figure FDA0003689091250000036
其中I3为单位阵，所述线性系统的最小平方解按由如下思路求解：
Figure FDA0003689091250000037
得到当前帧相对于全局地图的位姿估计
Figure FDA0003689091250000038
8.根据权利要求7所述的视觉自主定位系统实现方法，其特征在于，还包括：获得当前帧相对于全局地图的位姿后，将其特征点和特征平面整合到全局地图中，其中，对于有匹配的平面或点，与其匹配取平均作为新的地标，对于没有找到匹配的平面和点，加入全局地图成为新的地标。
9.根据权利要求1所述的视觉自主定位系统实现方法，其特征在于，所述维护关键帧序列和全局地图并进行全局优化的步骤具体包括：若获得当前帧的相对于全局地图的位姿与上一关键帧的位姿之间的差距足够大，则将该当前帧加入到所述关键帧序列中，其中，所述差距足够大是指旋转矩阵之间的角度大于一定值或者平移向量的差的模大于一定值。
10.根据权利要求9所述的视觉自主定位系统实现方法，其特征在于，还包括：使用g2o算法利用关键帧的位姿和点云数据进行全局地图的优化。
CN202010352385.3A 2020-04-28 2020-04-28 一种视觉自主定位系统实现方法 Active CN111583316B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010352385.3A CN111583316B (zh) 2020-04-28 2020-04-28 一种视觉自主定位系统实现方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010352385.3A CN111583316B (zh) 2020-04-28 2020-04-28 一种视觉自主定位系统实现方法

Publications (2)

Publication Number Publication Date
CN111583316A CN111583316A (zh) 2020-08-25
CN111583316B true CN111583316B (zh) 2022-08-23

Family

ID=72124567

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010352385.3A Active CN111583316B (zh) 2020-04-28 2020-04-28 一种视觉自主定位系统实现方法

Country Status (1)

Country Link
CN (1) CN111583316B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022073172A1 (zh) * 2020-10-09 2022-04-14 浙江大学 一种基于点线特征的机器人全局最优视觉定位方法及装置
CN112669358B (zh) * 2020-12-30 2023-09-26 中山大学 一种适用于多平台协同感知的地图融合方法
CN113658257B (zh) * 2021-08-17 2022-05-27 广州文远知行科技有限公司 一种无人设备定位方法、装置、设备及存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108303099A (zh) * 2018-06-14 2018-07-20 江苏中科院智能科学技术应用研究院 基于三维视觉slam的无人机室内自主导航方法
US20190206046A1 (en) * 2016-06-22 2019-07-04 Q-Bot Limited Autonomous surveying of underfloor voids
CN110260866A (zh) * 2019-07-19 2019-09-20 闪电(昆山)智能科技有限公司 一种基于视觉传感器的机器人定位与避障方法
US20200047340A1 (en) * 2018-08-13 2020-02-13 Beijing Jingdong Shangke Information Technology Co., Ltd. System and method for autonomous navigation using visual sparse map

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190206046A1 (en) * 2016-06-22 2019-07-04 Q-Bot Limited Autonomous surveying of underfloor voids
CN108303099A (zh) * 2018-06-14 2018-07-20 江苏中科院智能科学技术应用研究院 基于三维视觉slam的无人机室内自主导航方法
US20200047340A1 (en) * 2018-08-13 2020-02-13 Beijing Jingdong Shangke Information Technology Co., Ltd. System and method for autonomous navigation using visual sparse map
CN110260866A (zh) * 2019-07-19 2019-09-20 闪电(昆山)智能科技有限公司 一种基于视觉传感器的机器人定位与避障方法

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
"Biomimetic Design for Unmanned Aerial Vehicle Safe Landing in Hazardous Terrain";Cai Luo 等;《IEEE》;20150601;全文 *
"Dynamic visual localization and tracking method based on RGB-D information";Chunxia Yin 等;《IEEE》;20140929;全文 *
"Methods and Techniques in Data Visualization Model";Yipeng Li 等;《IEEE》;20190808;全文 *
"Point-cloud refinement via exact matching";Xiaoduan Feng 等;《IEEE》;20090818;全文 *
"基于机器视觉的移动机器人定位与三维地图重建方法研究";康凯;《中国优秀博硕士学位论文全文数据库(硕士)》;20180215;全文 *
"微型无人机视觉定位与环境建模研究";吕科 等;《电子科技大学学报》;20170531;第46卷(第3期);全文 *

Also Published As

Publication number Publication date
CN111583316A (zh) 2020-08-25

Similar Documents

Publication Publication Date Title
CN111583316B (zh) 一种视觉自主定位系统实现方法
Heng et al. Project autovision: Localization and 3d scene perception for an autonomous vehicle with a multi-camera system
CN108717712B (zh) 一种基于地平面假设的视觉惯导slam方法
CN111263960B (zh) 用于更新高清晰度地图的设备和方法
CN107843251B (zh) 移动机器人的位姿估计方法
CN110567469A (zh) 视觉定位方法、装置、电子设备及系统
CN106017463A (zh) 一种基于定位传感装置的飞行器定位方法
CN109579825B (zh) 基于双目视觉和卷积神经网络的机器人定位系统及方法
CN106204443A (zh) 一种基于多目复用的全景无人机系统
CN110411457B (zh) 基于行程感知与视觉融合的定位方法、系统、终端和存储介质
CN108519102B (zh) 一种基于二次投影的双目视觉里程计算方法
CN108489454A (zh) 深度距离测量方法、装置、计算机可读存储介质及电子设备
CN112258409A (zh) 一种用于无人驾驶的单目相机绝对尺度恢复方法及装置
CN110749308B (zh) 使用消费级gps和2.5d建筑物模型的面向slam的室外定位方法
CN105844692A (zh) 基于双目立体视觉的三维重建装置、方法、系统及无人机
CN115371673A (zh) 一种未知环境中基于Bundle Adjustment的双目相机目标定位方法
CN110598370B (zh) 基于sip和ekf融合的多旋翼无人机鲁棒姿态估计
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN114529585A (zh) 基于深度视觉和惯性测量的移动设备自主定位方法
CN112862818B (zh) 惯性传感器和多鱼眼相机联合的地下停车场车辆定位方法
CN113807435A (zh) 一种基于多传感器的遥感图像特征点高程获取方法
CN113723568A (zh) 基于多传感器和海平面的遥感图像特征点高程获取方法
CN113701750A (zh) 一种井下多传感器的融合定位系统
CN112945233A (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
CN110992291A (zh) 基于三目视觉的测距方法、系统及存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant