CN103150728A - 一种动态环境中的视觉定位方法 - Google Patents
一种动态环境中的视觉定位方法 Download PDFInfo
- Publication number
- CN103150728A CN103150728A CN 201310067475 CN201310067475A CN103150728A CN 103150728 A CN103150728 A CN 103150728A CN 201310067475 CN201310067475 CN 201310067475 CN 201310067475 A CN201310067475 A CN 201310067475A CN 103150728 A CN103150728 A CN 103150728A
- Authority
- CN
- China
- Prior art keywords
- point
- image
- field flow
- vision
- unique
- 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.)
- Pending
Links
Images
Landscapes
- Length Measuring Devices By Optical Means (AREA)
- Image Analysis (AREA)
- Image Processing (AREA)
Abstract
本发明公开了一种可以在动态环境中实现精确定位的立体视觉定位方法。通过双目摄像机实时采集环境图像。通过特征提取算法提取图像中的特征点。对双目摄像机前后两个时刻采集的四幅图像进行立体匹配。利用双视几何原理恢复特征点三维信息。以回环匹配的方式提高匹配准确度。使用场流法剔除位于动态物体上的特征点。考虑可能引起误差的因素并通过计算协方差矩阵改善场流法效果。使用高斯牛顿迭代的方法由特征点位置信息求得机器人运动参数。使用RANSIC算法进一步提高视觉定位准确度。整个过程不断迭代,实现对机器人姿态和位置的实时计算。
Description
技术领域:
本发明是涉及图像处理及机器视觉的移动机器人自定位技术,该技术可以在复杂的动态场景中以视觉信息作为输入实现移动机器人自定位。
背景技术:
在智能机器人领域,移动机器人定位与导航技术一直是研究的关键问题。定位的精度及速度直接影响机器人完成既定任务的效率和准确度。之前的定位方法,如:编码器或轮式里程计等无法克服由摩擦、滑动等非线性因素引起的定位误差,近几年兴起的视觉定位方法可以很好地解决非线性误差,但现有的视觉定位方法存在一定缺陷:
1:当环境中存在纹理相似区域,及大量相似的重复性结构时,容易引起特征点的误匹配,导致定位错误。
2:环境中存在大量独立运动物体时,位于动态物体上的特征点会干扰里程计结果,导致定位精度降低。
为了使视觉定位技术可以在复杂的动态场景中得以应用,需要采用一定的方法来剔除位于动态物体上的特征点,然而目前的算法,如离群度测量算法,图像理解算法等均不能同时满足定位的实时性及有效性要求。如何让视觉里程计在动态环境中实时有效地工作成为本专利申请人关注和研究的新课题。
发明内容:
本发明的目的在于解决现有技术中存在的问题,提供一种可以在复杂动态环境中应用的视觉定位算法,使得移动机器人可以依靠视觉信息作为输入,完成在复杂环境中的自定位任务。本发明的技术方案包括以下步骤:
1:用已标定好的双目相机,进行左右图像连续采集。
2:采集当前时刻左右两幅图像,使用sobel算子提取图像中的特征点,并利用SIFT描述符进行描述,通过计算左右图像各点间特征描述符的欧式距离进行匹配,使用三维重建算法将特征点二维图像坐标转换为三维空间坐标,初始化位姿变换矩阵为单位矩阵,并设当前位置为原点。如果图像是第一帧,从新进入步骤2;如果图像不是第一帧进入步骤3。
3:将前时刻获取的左右两帧图像中的特征点,与前一时刻的两帧图像一起进行特征点的立体匹配,并通过回环匹配增加匹配准确度,得到各帧图像之间特征点的对应关系,利用双视几何原理求解出特征点在两帧图像中相对于摄像机坐标系的坐标。
4:使用场流法对匹配后的特征点集进行计算,求出该点在t-1时刻的位置经过位姿变换矩阵RT投影以后的三维坐标与该点t时刻三维坐标的差值,差值大于阈值的点认为是位于动态物体上的特征点,将其从特征点集中删除。其详细步骤如下:
4-1.由于机器人的运动过程是连续的,所以认为机器人在t时刻的位姿变化与t-1时刻位姿变化近似相等,以t-1时刻的RT作为t时刻RT的估计值。
4-2.利用4-1的结果,考虑相机成像时的像素位置误差,计算估计RT的不准确度,求出场流法的误差模型。
4-3.计算每个点对应于该模型的马氏距离值,值越大说明其离群度较高,将大于一定阈值的点从点集中删除。
5:使用高斯-牛顿迭代算法,以当前时刻和前一时刻的特征点集作为输入量求解机器人当前时刻相对前一时刻的位姿变化,同时使用RANSIC算法进一步提高定位准确度。
5-1.在特征点集中随机选择3对特征点,通过迭代算法,进行一次运动估计,得到一个RT。
5-2.利用当前的RT,对所有前一时刻特征点进行投影,对比投影结果与当前观测位置的差,将差值小于阈值的点记为局内点。
5-3.重复5-1、5-2,进行N次迭代,比较每次局内点数目,将局内点数目最大的一组局内点重新构成特征点集。
5-4.利用新的特征点集,使用高斯-牛顿迭代进行运动估计,得到更优的R和T。
6:使用步骤5的结果对全局的RT进行更新,得到机器人相对于初始坐标系的位置和姿态。
本发明的优点在于:
1)本发明通过回环匹配来增加匹配稳定性,减小了因误匹配引入的误差。
2)本发明使用场流法对特征点进行处理,可以将处于运动物体上的特征点剔除出去,使视觉定位可以在复杂的动态场景中实时有效的工作。
3)本发明在使用场流法时,考虑了可能产生误差的因素,求解其误差模型,计算各点对应该模型的马氏距离,使得对局外点的判断可以通过统一的阈值来决定。
4)在视觉定位过程中使用了RANSIC算法,该方法与场流法结合使用,提高了视觉定位方法的实时性与有效性
附图说明:
图1是本发明提出的动态环境中视觉定位方法总体流程图。
图2是本发明中定义的各个坐标系。
图3是本发明中使用的回环匹配算法示意图。
图4是本发明中场流法的计算流程图。
图5是本发明中RANSIC算法流程图。
具体实施方式:
下面结合说明书附图对本发明方法做详细说明:
步骤1:对双目摄像机进行标定,获取相机内外参数,包括:焦距f,基线长度b,图像中心像素位置u0,v0,整幅图像的校正矩阵等。
步骤2:开启双目相机,进行左右图像连续采集,利用步骤1获取的相机参数对图像进行校正。
步骤3:如果图像是第一帧,对采集的左右图像使用sobel算子提取特征,并利用SIFT描述符进行描述,在左右图像之间进行特正匹配,得到特征点对应关系。如果图像不是第一帧,则将前后两个时刻共四幅图像进行特征匹配,匹配通过回环检测的方式提高准确度,如说明书附图3:从当前时刻左图像开始,按照顺序进行立体匹配,如果最后通过匹配可以找到该特征点本身,则将特征点信息存储于点集中,否则予以舍弃。
步骤4:利用步骤3的结果,使用场流法对特征点进行筛选,使用立体几何原理求得像素点对应的三维坐标值:
步骤4-2:利用步骤4-1的结果,可以求得机器人运动参数估计的不准确性:Σq=(HT×Σx×H)-1。其中H为由三维点集推算机器人运动参数时的误差传递矩阵。
步骤4-3:计算场流的表达式为:
场流法的误差模型表达式式中SSF为场流表达式中所有输入变量的协方差矩阵,可以通过对Zsf=(ul',ul',v',ul,ul,v,tx,ty,tz,qx,qy,qz)求偏导数获得,JSF对应(2)式的雅克比矩阵。
步骤4-4:利用步骤4-3求得的ΣSF可以求得取每一点对应场流的马氏距离马氏距离反应的是符合一定概率分布总体中某一样本的离群度,值越大,对应的离群度越高,在场流法中越有可能是属于动态物体上的特征点。将大于一定阈值的特征点对从特征点集中删除从而达到排除动态物体对里程计影响的目的。
步骤5:对步骤4得到的三维特征点集结合RANSIC算法与牛顿高-斯迭代进行运动估计,得到R和T:
如图5,其中详细步骤如下:
5-1.步骤4已经获得的三维点集包括同一个点在当前时刻和前一时刻在机器人坐标系中的坐标,随机选择其中的三个特征点,进行一次运动估计,求解出一个R和T。
其中{x、y、z}为点集中一个点在t-1时刻的三维坐标,点对应的像素位置u、v在实际中均取整数,计算由(3)式得到的u、v值与该点在t时刻观测到的u、v值之差。差值小于阈值的点纪录为局内点。遍历当前时刻所有点,得到局内特征点集记录局内特征点数。
5-3.重复步骤5-1,5-2,进行多次迭代,记录每一次迭代中得到的局内特征点数,找出局内特征点数最多的一次,利用所有局内特征点作为输入,进行运动估计,得到更优的R和T。
其中运动估计算法如下:
(1)将公式(3)展开并对待求未知量tx,ty,tz与rx,ry,rz求偏导数。
(2)利用(1)的结果,若为第一次计算,则首先将(tx,ty,tz,rx,ry,rz)的初值全部设为0,否则以上一次运算的结果作为初值。以Δ表示待求变量估计值与真实值之间的差值,构建标准方程组
b11Δtx+b12Δty+b13Δtz+b14Δrx+b15Δry+b16Δrz=B1
b21Δtx+b22Δty+b23Δtz+b24Δrx+b25Δry+b26Δrz=B2
b31Δtx+b32Δty+b33Δtz+b34Δrx+b35Δry+b36Δrz=B3 (4)
b41Δtx+b42Δty+b43Δtz+b44Δrx+b45Δry+b46Δrz=B4
b51Δtx+b52Δty+b53Δtz+b54Δrx+b55Δry+b56Δrz=B5
b61Δtx+b62Δty+b63Δtz+b64Δrx+b65Δry+b66Δrz=B6
其中 其中表示对(3)式的第k行对第i个变量求偏导数,其中ai∈(tx,ty,tz,rx,ry,rz)。Ck为特征点在t时刻对应的像素坐标位置。
(3)使用消元法求解(2)中的方程组,可以解出Δi的值。将Δi与初值相加可以求得各个变量的当前估计值。
(4)使用步骤3得出的运动参数(tx,ty,tz,rx,ry,rz)将前一时刻空间点三维位置,投影到当前时刻的图像平面,当满足精度要求或达到最大迭代次数时,使用当前的(tx,ty,tz,rx,ry,rz)对运动参数进行更新。
步骤6.将每一次步骤5求出的结果转化为4*4的位姿变换矩阵,从初始时刻开始进行累乘,最终得到的结果就是机器人目前相对于初始时刻的位姿状态。
Claims (4)
1.一种可以在复杂动态场景中应用的视觉定位方法,其特征在于,包括以下步骤:
步骤一:对双目摄像机进行标定。
步骤二:开启相机,进行左右图像的连续采集。
步骤三:如果图像是第一帧,sobel算子提取图像中的特征点位置,并利用SIFT描述符进行描述,在左右图像之间进行特正匹配,得到特征点对应关系。如果图像不是第一帧,则在前后两个时刻共四幅图像间进行特征匹配,匹配通过回环检测的方式提高准确度。
步骤四:通过双视几何的原理计算特征像素点对应的三维空间位置,使用场流法对特征点进行筛选,考虑计算场流过程中可能引入的误差,通过计算每个点对应的马氏距离,将离群度大于一定阈值的点从特征点集中删除。
步骤五:使用RANSIC算法进行迭代,每次迭代时利用求得的姿态R和位置T计算满足该变换的特征点数目,求取包含局内点最多一组结果,并利用该组中所有局内点求解更准确的R和T。
步骤六:将步骤五求得的结果进行累乘,得到机器人在当前时刻相对于初始时刻的位姿变换矩阵。
2.权利要求1所述的一种动态场景中视觉定位方法,其特征在于,所述步骤三中采用回环匹配的方法减小误匹配概率。
3.权利要求1所述的一种动态场景中视觉定位方法,其特征在于,所述步骤四中考虑了场流法的误差模型,通过每一组特征点对应的马氏距离,将离群度大于阈值的特征点对从点集中删除,达到排除位于动态物体上的特征点的效果。
步骤4-1计算场流法误差模型,最初的像素点误差成高斯分布,计算定位空间点三维位置的误差。
步骤4-2利用4-1求得的误差模型以及视觉定位模型的雅克比矩阵,求得估计出的RT的误差模型。
步骤4-3利用4-2的结果以及和场流法对应的雅克比矩阵,求得场流法的误差模型。
步骤4-4利用4-3求得的场流误差模型计算每一特征点对应的马氏距离。
4.权利要求1所述的一种动态场景中视觉定位方法,其特征在于,使用RANSIC算法,每次选取三个特征点对计算RT,对所有特征点对按照计算出的RT进行投影,***内点数目,迭代N次后,以局内点数目最大的一组特征点对为输入,估计得到更精确的R和T。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN 201310067475 CN103150728A (zh) | 2013-03-04 | 2013-03-04 | 一种动态环境中的视觉定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN 201310067475 CN103150728A (zh) | 2013-03-04 | 2013-03-04 | 一种动态环境中的视觉定位方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN103150728A true CN103150728A (zh) | 2013-06-12 |
Family
ID=48548781
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN 201310067475 Pending CN103150728A (zh) | 2013-03-04 | 2013-03-04 | 一种动态环境中的视觉定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103150728A (zh) |
Cited By (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103345751A (zh) * | 2013-07-02 | 2013-10-09 | 北京邮电大学 | 一种基于鲁棒特征跟踪的视觉定位方法 |
CN104714964A (zh) * | 2013-12-13 | 2015-06-17 | ***通信集团公司 | 一种生理数据离群检测方法及装置 |
CN104915965A (zh) * | 2014-03-14 | 2015-09-16 | 华为技术有限公司 | 一种摄像机跟踪方法及装置 |
CN105072432A (zh) * | 2015-08-13 | 2015-11-18 | 杜宪利 | 基于光流场和动态规划搜索的视觉匹配方法 |
CN106444780A (zh) * | 2016-11-10 | 2017-02-22 | 速感科技(北京)有限公司 | 一种基于视觉定位算法的机器人的自主导航方法及*** |
CN107067436A (zh) * | 2015-11-17 | 2017-08-18 | 株式会社东芝 | 位姿估计装置及真空清洁器*** |
CN107967691A (zh) * | 2016-10-20 | 2018-04-27 | 株式会社理光 | 一种视觉里程计算方法和装置 |
CN108022265A (zh) * | 2016-11-01 | 2018-05-11 | 狒特科技(北京)有限公司 | 红外相机位姿确定方法、设备及*** |
CN108332758A (zh) * | 2018-01-26 | 2018-07-27 | 上海思岚科技有限公司 | 一种移动机器人的走廊识别方法及装置 |
CN109191502A (zh) * | 2018-08-14 | 2019-01-11 | 南京工业大学 | 一种自动识别弹壳痕迹的方法 |
CN109389004A (zh) * | 2017-08-02 | 2019-02-26 | 珊口(上海)智能科技有限公司 | 场景特征点获取方法、***、介质、计算机***及机器人 |
CN109472821A (zh) * | 2018-10-29 | 2019-03-15 | 南京怀萃智能科技有限公司 | 深度估计方法、装置、设备和存储介质 |
CN110189382A (zh) * | 2019-05-31 | 2019-08-30 | 东北大学 | 一种基于无共视区域的多双目相机运动标定方法 |
CN110520694A (zh) * | 2017-10-31 | 2019-11-29 | 深圳市大疆创新科技有限公司 | 一种视觉里程计及其实现方法 |
CN110542402A (zh) * | 2019-09-09 | 2019-12-06 | 上海中车瑞伯德智能***股份有限公司 | 复杂结构体的rgb-d视觉定位***及方法 |
CN110673607A (zh) * | 2019-09-25 | 2020-01-10 | 优地网络有限公司 | 动态场景下的特征点提取方法、装置、及终端设备 |
-
2013
- 2013-03-04 CN CN 201310067475 patent/CN103150728A/zh active Pending
Cited By (25)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103345751A (zh) * | 2013-07-02 | 2013-10-09 | 北京邮电大学 | 一种基于鲁棒特征跟踪的视觉定位方法 |
CN104714964B (zh) * | 2013-12-13 | 2018-03-23 | ***通信集团公司 | 一种生理数据离群检测方法及装置 |
CN104714964A (zh) * | 2013-12-13 | 2015-06-17 | ***通信集团公司 | 一种生理数据离群检测方法及装置 |
WO2015135323A1 (zh) * | 2014-03-14 | 2015-09-17 | 华为技术有限公司 | 一种摄像机跟踪方法及装置 |
CN104915965A (zh) * | 2014-03-14 | 2015-09-16 | 华为技术有限公司 | 一种摄像机跟踪方法及装置 |
CN105072432B (zh) * | 2015-08-13 | 2017-12-15 | 杜宪利 | 基于光流场和动态规划搜索的视觉匹配方法 |
CN105072432A (zh) * | 2015-08-13 | 2015-11-18 | 杜宪利 | 基于光流场和动态规划搜索的视觉匹配方法 |
CN107067436A (zh) * | 2015-11-17 | 2017-08-18 | 株式会社东芝 | 位姿估计装置及真空清洁器*** |
CN107067436B (zh) * | 2015-11-17 | 2021-01-05 | 株式会社东芝 | 位姿估计装置及真空清洁器*** |
CN107967691B (zh) * | 2016-10-20 | 2021-11-23 | 株式会社理光 | 一种视觉里程计算方法和装置 |
CN107967691A (zh) * | 2016-10-20 | 2018-04-27 | 株式会社理光 | 一种视觉里程计算方法和装置 |
CN108022265A (zh) * | 2016-11-01 | 2018-05-11 | 狒特科技(北京)有限公司 | 红外相机位姿确定方法、设备及*** |
CN108022265B (zh) * | 2016-11-01 | 2023-06-13 | 北京墨土科技有限公司 | 红外相机位姿确定方法、设备及*** |
CN106444780A (zh) * | 2016-11-10 | 2017-02-22 | 速感科技(北京)有限公司 | 一种基于视觉定位算法的机器人的自主导航方法及*** |
CN106444780B (zh) * | 2016-11-10 | 2019-06-28 | 速感科技(北京)有限公司 | 一种基于视觉定位算法的机器人的自主导航方法及*** |
CN109389004A (zh) * | 2017-08-02 | 2019-02-26 | 珊口(上海)智能科技有限公司 | 场景特征点获取方法、***、介质、计算机***及机器人 |
CN110520694A (zh) * | 2017-10-31 | 2019-11-29 | 深圳市大疆创新科技有限公司 | 一种视觉里程计及其实现方法 |
CN108332758B (zh) * | 2018-01-26 | 2021-07-09 | 上海思岚科技有限公司 | 一种移动机器人的走廊识别方法及装置 |
CN108332758A (zh) * | 2018-01-26 | 2018-07-27 | 上海思岚科技有限公司 | 一种移动机器人的走廊识别方法及装置 |
CN109191502A (zh) * | 2018-08-14 | 2019-01-11 | 南京工业大学 | 一种自动识别弹壳痕迹的方法 |
CN109191502B (zh) * | 2018-08-14 | 2021-08-24 | 南京工业大学 | 一种自动识别弹壳痕迹的方法 |
CN109472821A (zh) * | 2018-10-29 | 2019-03-15 | 南京怀萃智能科技有限公司 | 深度估计方法、装置、设备和存储介质 |
CN110189382A (zh) * | 2019-05-31 | 2019-08-30 | 东北大学 | 一种基于无共视区域的多双目相机运动标定方法 |
CN110542402A (zh) * | 2019-09-09 | 2019-12-06 | 上海中车瑞伯德智能***股份有限公司 | 复杂结构体的rgb-d视觉定位***及方法 |
CN110673607A (zh) * | 2019-09-25 | 2020-01-10 | 优地网络有限公司 | 动态场景下的特征点提取方法、装置、及终端设备 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103150728A (zh) | 一种动态环境中的视觉定位方法 | |
CN110349250B (zh) | 一种基于rgbd相机的室内动态场景的三维重建方法 | |
CN108520554B (zh) | 一种基于orb-slam2的双目三维稠密建图方法 | |
WO2019157925A1 (zh) | 视觉惯性里程计的实现方法及*** | |
CN102519481B (zh) | 一种双目视觉里程计实现方法 | |
CN103345751A (zh) | 一种基于鲁棒特征跟踪的视觉定位方法 | |
CN107193279A (zh) | 基于单目视觉和imu信息的机器人定位与地图构建*** | |
CN104180818B (zh) | 一种单目视觉里程计算装置 | |
CN105225269A (zh) | 基于运动机构的三维物体建模*** | |
CN105528785A (zh) | 一种双目视觉图像立体匹配方法 | |
CN112734841B (zh) | 一种用轮式里程计-imu和单目相机实现定位的方法 | |
CN113108771B (zh) | 一种基于闭环直接稀疏视觉里程计的移动位姿估计方法 | |
CN103869820A (zh) | 一种巡视器地面导航规划控制方法 | |
US20210183100A1 (en) | Data processing method and apparatus | |
CN116205947B (zh) | 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质 | |
CN110992487B (zh) | 手持式飞机油箱快速三维地图重建装置及重建方法 | |
CN114001733B (zh) | 一种基于地图的一致性高效视觉惯性定位算法 | |
CN110533720A (zh) | 基于联合约束的语义slam***及方法 | |
CN108519102A (zh) | 一种基于二次投影的双目视觉里程计算方法 | |
CN112258409A (zh) | 一种用于无人驾驶的单目相机绝对尺度恢复方法及装置 | |
CN112418288A (zh) | 一种基于gms和运动检测的动态视觉slam方法 | |
CN111882602A (zh) | 基于orb特征点和gms匹配过滤器的视觉里程计实现方法 | |
Fan et al. | Dynamicfilter: an online dynamic objects removal framework for highly dynamic environments | |
CN104864849A (zh) | 视觉导航方法和装置以及机器人 | |
He et al. | A rotation-translation-decoupled solution for robust and efficient visual-inertial initialization |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C02 | Deemed withdrawal of patent application after publication (patent law 2001) | ||
WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20130612 |