CN117237406A - 一种机器人视觉跟踪方法 - Google Patents
一种机器人视觉跟踪方法 Download PDFInfo
- Publication number
- CN117237406A CN117237406A CN202210638498.9A CN202210638498A CN117237406A CN 117237406 A CN117237406 A CN 117237406A CN 202210638498 A CN202210638498 A CN 202210638498A CN 117237406 A CN117237406 A CN 117237406A
- Authority
- CN
- China
- Prior art keywords
- frame image
- vector
- robot
- current frame
- reference frame
- 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
- 238000000034 method Methods 0.000 title claims abstract description 128
- 239000013598 vector Substances 0.000 claims description 498
- 239000011159 matrix material Substances 0.000 claims description 148
- 238000013519 translation Methods 0.000 claims description 128
- 238000006243 chemical reaction Methods 0.000 claims description 67
- 230000003287 optical effect Effects 0.000 claims description 46
- 238000003384 imaging method Methods 0.000 claims description 34
- 238000004364 calculation method Methods 0.000 claims description 33
- 238000012216 screening Methods 0.000 claims description 27
- 230000014509 gene expression Effects 0.000 claims description 18
- 230000008569 process Effects 0.000 claims description 18
- 238000009795 derivation Methods 0.000 claims description 13
- 230000008859 change Effects 0.000 claims description 9
- 238000001514 detection method Methods 0.000 claims description 9
- 238000013507 mapping Methods 0.000 claims description 7
- 230000007613 environmental effect Effects 0.000 claims description 5
- 230000014616 translation Effects 0.000 description 94
- 238000006073 displacement reaction Methods 0.000 description 11
- 230000033001 locomotion Effects 0.000 description 9
- 230000009466 transformation Effects 0.000 description 9
- 230000000007 visual effect Effects 0.000 description 8
- 238000010276 construction Methods 0.000 description 7
- 238000012545 processing Methods 0.000 description 7
- 238000012937 correction Methods 0.000 description 6
- 238000001914 filtration Methods 0.000 description 5
- 238000005259 measurement Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 4
- 230000003044 adaptive effect Effects 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 238000011524 similarity measure Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000006978 adaptation Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 239000011295 pitch Substances 0.000 description 1
- 238000000844 transformation Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/223—Analysis of motion using block-matching
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
- G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
Landscapes
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- General Physics & Mathematics (AREA)
- Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Multimedia (AREA)
- Computing Systems (AREA)
- Software Systems (AREA)
- Medical Informatics (AREA)
- General Health & Medical Sciences (AREA)
- Evolutionary Computation (AREA)
- Databases & Information Systems (AREA)
- Artificial Intelligence (AREA)
- Health & Medical Sciences (AREA)
- Automation & Control Theory (AREA)
- Image Analysis (AREA)
- Manipulator (AREA)
Abstract
本发明公开一种机器人视觉跟踪方法,所述机器人视觉跟踪方法的执行主体是装配摄像头和惯性传感器的机器人;所述机器人视觉跟踪方法包括机器人使用窗口匹配方式进行图像跟踪,当机器人使用窗口匹配方式跟踪成功时,机器人停止使用窗口匹配方式进行图像跟踪,然后机器人使用投影匹配方式进行图像跟踪;然后,当机器人使用投影匹配方式跟踪失败时,机器人停止使用投影匹配方式进行图像跟踪,然后机器人使用窗口匹配方式进行图像跟踪。
Description
技术领域
本发明涉及计算机视觉的技术领域,尤其涉及一种机器人视觉跟踪方法。
背景技术
视觉惯性里程计(VIO,visual-inertial odometry),有时也叫视觉惯性系统(VINS,visual-inertial system),是融合相机和惯性测量单元(IMU,InertialMeasurement Unit)传感器数据实现SLAM的算法。传统经典的VIO方案,其初始化阶段从利用特征点的纯视觉SFM(Structure from Motion ,从运动中恢复结构)开始,然后通过将该结构与IMU预积分测量值松耦合对齐来恢复度量尺度、速度、重力加速度方向和IMU零偏。SLAM(simultaneous localization and mapping ,即时定位与地图构建)指的是机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位,同时在自身定位的基础上建造增量式地图,实现机器人的自主定位和导航。
目前,基于点特征的SLAM算法利用具有投影关系的特征点为基础,能够实时的进行特征跟踪、构图、闭环检测,完成同时定位与制图的全过程。但是,特征跟踪(特征的提取和匹配)会消耗较多的计算量,降低机器人定位和导航的实时性。
发明内容
为了解决上述技术缺陷,本发明公开一种机器人视觉跟踪方法,具体的技术方案如下:
一种机器人视觉跟踪方法,所述机器人视觉跟踪方法的执行主体是固定装配摄像头和惯性传感器的机器人;所述机器人视觉跟踪方法包括:机器人使用窗口匹配方式进行图像跟踪,当机器人使用窗口匹配方式跟踪成功时,机器人停止使用窗口匹配方式进行图像跟踪,然后机器人使用投影匹配方式进行图像跟踪;然后,当机器人使用投影匹配方式跟踪失败时,机器人停止使用投影匹配方式进行图像跟踪,然后机器人使用窗口匹配方式进行图像跟踪。
进一步地,所述机器人视觉跟踪方法还包括:当机器人使用窗口匹配方式跟踪失败时,机器人停止使用窗口匹配方式进行图像跟踪,机器人清空滑动窗口,再使用窗口匹配方式进行图像跟踪;其中,图像跟踪用于表示在先采集的图像的特征点与当前帧图像的特征点之间的匹配;其中,机器人使用窗口匹配方式跟踪成功后,将当前帧图像填入所述滑动窗口,以便于跟踪机器人实时采集的图像;机器人使用投影匹配方式进行图像跟踪的过程中,若检测到当前帧图像与上一帧图像的时间间隔是超出预设时间阈值,则机器人停止使用投影匹配方式进行图像跟踪,转而使用窗口匹配方式进行图像跟踪;其中,特征点是属于图像的像素点,特征点是在所述摄像头所处的环境中,以点的形式存在的环境元素。
进一步地,机器人使用窗口匹配方式进行图像跟踪的方法包括:步骤S11,机器人通过摄像头采集当前帧图像,并通过惯性传感器获取惯性数据;步骤S12,在惯性数据的基础上,利用对极约束误差值,从当前帧图像的特征点和滑动窗口内所有的参考帧图像的特征点中筛选出第一特征点对;其中,滑动窗口被设置为填入预先采集的至少一帧图像;特征点是图像的像素点,特征点是在所述摄像头所处的环境中,以点的形式存在的环境元素;步骤S13,在所述惯性数据的基础上,利用特征点的深度值,从第一特征点对中筛选出第二特征点对;步骤S14,根据第二特征点对所对应的描述子的相似度,从第二特征点对中筛选出第三特征点对;步骤S15,在第三特征点对之间引入残差,再结合残差及其对惯性数据的求导结果,计算出惯性补偿值,再使用惯性补偿值对惯性数据进行修正;然后将修正后的惯性数据更新为步骤S12所述惯性数据,并将步骤S14所述的第三特征点对在当前帧图像的特征点更新为步骤S12所述的当前帧图像的特征点,并将步骤S14所述的第三特征点对在参考帧图像的特征点更新为步骤S12所述的滑动窗口内所有的参考帧图像的特征点;步骤S16,重复执行步骤S12和步骤S13,直至重复执行次数达到预设迭代匹配次数,再基于第二特征点对在每帧参考帧图像内的特征点的数量,从滑动窗口内的参考帧图像中筛选出匹配帧图像;其中,每当重复执行步骤S12和步骤S13后,机器人在最新执行的步骤S13所筛选出的第二特征点对之间引入残差,再结合该残差及其对最新获得的惯性数据求导的结果,计算出惯性补偿值,再使用惯性补偿值对最新获得的惯性数据进行修正,然后将修正后的惯性数据更新为步骤S12所述的惯性数据,并将最新执行的步骤S13所筛选出的第二特征点对所包括的特征点对应更新为步骤S12所述的当前帧图像的特征点和滑动窗口内所有的参考帧图像的特征点;步骤S17,基于对极约束误差值与第二特征点对在每帧匹配帧图像内的特征点的数量,在所有的匹配帧图像中选择出最优匹配帧图像,并确定机器人使用窗口匹配方式跟踪成功;其中,步骤S12、步骤S13、步骤S14、步骤S15、步骤S16和步骤S17的组合是所述窗口匹配方式。
进一步地,在所述步骤S12中,在惯性数据的基础上,利用对极约束误差值,从当前帧图像的特征点和滑动窗口内所有的参考帧图像的特征点中筛选出第一特征点对的方法包括:机器人计算特征点对的对极约束误差值;当特征点对的对极约束误差值大于或等于预设像素距离阈值时,将该特征点对标记为错误匹配点对;当特征点对的对极约束误差值小于预设像素距离阈值时,将该特征点对标记为第一特征点对并确定筛选出第一特征点对;其中,每个特征点对是配置为由当前帧图像的一个特征点和参考帧图像的一个特征点组成,而且,所述当前帧图像的每个特征点都与滑动窗口内的每个参考帧图像中的每个特征点都组成特征点对。
进一步地,在所述步骤S12中,当惯性数据包括当前帧图像相对于参考帧图像的平移向量、以及当前帧图像相对于参考帧图像的旋转矩阵时,机器人将当前帧图像相对于参考帧图像的平移向量标记为第一平移向量,并将当前帧图像相对于参考帧图像的旋转矩阵标记为第一旋转矩阵;然后,机器人控制第一旋转矩阵将当前帧图像的特征点的归一化向量转换到参考帧图像的坐标系下,得到第一一向量;再控制第一平移向量叉乘第一一向量,得到第一二向量;然后控制滑动窗口内的参考帧图像中的特征点的归一化向量与第一二向量点乘,再将点乘的结果设置为对应特征点对的对极约束误差值;或者,在所述步骤S12中,当惯性数据包括参考帧图像相对于当前帧图像的平移向量、以及参考帧图像相对于当前帧图像的旋转矩阵时,机器人将参考帧图像相对于当前帧图像的平移向量标记为第二平移向量,并将参考帧图像相对于当前帧图像的旋转矩阵标记为第二旋转矩阵;然后,机器人控制第二旋转矩阵将滑动窗口内的参考帧图像的特征点的归一化平面向量转换到当前帧图像的坐标系下,得到第二一向量;再控制第二平移向量叉乘第二一向量,得到第二二向量;然后控制当前帧图像中的特征点的归一化平面向量与第二二向量点乘,再将点乘的结果设置为对应特征点对的对极约束误差值;其中,当前帧图像的特征点的归一化向量是当前帧图像的特征点的归一化平面坐标相对于当前帧图像的坐标系的原点形成的向量;参考帧图像的特征点的归一化向量是参考帧图像的特征点的归一化平面坐标相对于参考帧图像的坐标系的原点形成的向量。
进一步地,在所述步骤S13中,在所述惯性数据的基础上,利用特征点的深度值,从第一特征点对中筛选出第二特征点对的方法包括:机器人计算所述步骤S12筛选出的第一特征点对在当前帧图像中的特征点的深度值与该第一特征点对在参考帧图像中的特征点的深度值的比值;当第一特征点对在当前帧图像中的特征点的深度值与该第一特征点对在参考帧图像中的特征点的深度值的比值处于预设比值阈值范围内时,将该第一特征点对标记为第二特征点对并确定筛选出第二特征点对;当第一特征点对在当前帧图像中的特征点的深度值与该第一特征点对在参考帧图像中的特征点的深度值的比值没有处于预设比值阈值范围内时,将该第一特征点对标记为错误匹配点对。
进一步地,在所述步骤S13中,当惯性数据包括当前帧图像相对于参考帧图像的平移向量、以及当前帧图像相对于参考帧图像的旋转矩阵时,机器人将当前帧图像相对于参考帧图像的平移向量标记为第一平移向量,将当前帧图像相对于参考帧图像的旋转矩阵标记为第一旋转矩阵,机器人控制第一旋转矩阵将第一特征点对在当前帧图像中的特征点的归一化向量转换到参考帧图像的坐标系下,得到第一一向量;再控制该第一特征点对在参考帧图像中的特征点的归一化向量叉乘第一一向量,得到第一二向量;同时控制该第一特征点对在参考帧图像中的特征点的归一化向量叉乘第一平移向量,再对叉乘结果取反,得到第一三向量;然后将第一三向量与第一二向量的逆向量的乘积设置为该第一特征点对在当前帧图像中的特征点的深度值,并标记为第一深度值,表示摄像头探测的三维点与摄像头采集到当前帧图像时的光心之间的距离;然后将第一一向量与第一深度值的乘积与第一平移向量的和值标记为第一四向量,然后将第一四向量与第一特征点对在参考帧图像中的特征点的归一化向量的逆向量的乘积设置为该第一特征点对在参考帧图像中的特征点的深度值,并标记为第二深度值,表示同一三维点与摄像头采集到参考帧图像时的光心之间的距离;或者,在所述步骤S13中,当惯性数据包括参考帧图像相对于当前帧图像的平移向量、以及参考帧图像相对于当前帧图像的旋转矩阵时,机器人将参考帧图像相对于当前帧图像的平移向量记为第二平移向量,将参考帧图像相对于当前帧图像的旋转矩阵记为第二旋转矩阵,机器人控制第二旋转矩阵将第一特征点对在参考帧图像中的特征点的归一化向量转换到当前帧图像的坐标系下,得到第二一向量;再控制该第一特征点对在当前帧图像中的特征点的归一化向量叉乘第二一向量,得到第二二向量;同时控制该第一特征点对在当前帧图像中的特征点的归一化向量叉乘第二平移向量,再对叉乘结果取反,得到第二三向量;然后将第二三向量与第二二向量的逆向量的乘积设置为该第一特征点对在参考帧图像中的特征点的深度值,并标记为第二深度值,表示摄像头探测的三维点与摄像头采集到参考帧图像时的光心之间的距离;然后将第二一向量与第二深度值的乘积与第二平移向量的和值标记为第二四向量,然后将第二四向量与第一特征点对在当前帧图像中的特征点的归一化向量的逆向量的乘积设置为该第一特征点对在当前帧图像中的特征点的深度值,并标记为第一深度值,表示同一三维点与摄像头采集到当前帧图像时的光心之间的距离;其中,第一特征点对在当前帧图像中的特征点的归一化向量是第一特征点对在当前帧图像中的特征点的归一化平面坐标相对于当前帧图像的坐标系的原点形成的向量;第一特征点对在参考帧图像中的特征点的归一化向量是第一特征点对在参考帧图像的特征点的归一化平面坐标相对于参考帧图像的坐标系的原点形成的向量。
进一步地,在所述步骤S14中,所述根据第二特征点对所对应的描述子的相似度,从第二特征点对中筛选出第三特征点对的方法包括:对于当前帧图像与所述滑动窗口内的每帧参考帧图像,机器人计算第二特征点对在参考帧图像中的特征点的描述子与该第二特征点对在当前帧图像中的特征点的描述子之间的相似度;当第二特征点对在参考帧图像中的特征点的描述子与该第二特征点对在当前帧图像中的特征点的描述子之间的相似度是当前帧图像的描述子与该第二特征点对的特征点所在的参考帧图像的描述子之间的相似度当中的最小值时,将该第二特征点对标记为第三特征点对并确定筛选出第三特征点对;其中,该第二特征点对的特征点所在的参考帧图像的描述子是该第二特征点对的特征点所在的参考帧图像内所有组成第二特征点对的特征点的描述子;当前帧图像的描述子是当前帧图像内,与该第二特征点对的特征点所在的参考帧图像的特征点组成第二特征点对的特征点的描述子;其中,第二特征点对所对应的描述子的相似度,使用当前帧图像中特征点的描述子和滑动窗口内对应的参考帧图像中特征点的描述子之间的欧式距离或汉明距离表示。
进一步地,所述步骤S14还包括:每当机器人搜索完当前帧图像和所述滑动窗口内的一帧参考帧图像之间组成第二特征点对的所有特征点后,若机器人在该帧参考帧图像内统计到第三特征点对的数量小于或等于第一预设点数阈值,则确定当前帧图像和该帧参考帧图像匹配失败,并将该帧参考帧图像设置为误匹配参考帧图像;若机器人在该帧参考帧图像内统计到第三特征点对的数量大于第一预设点数阈值,则确定当前帧图像和该帧参考帧图像匹配成功;其中,当机器人确定当前帧图像和所述滑动窗口内所有帧参考帧图像都匹配失败时,确定机器人使用窗口匹配方式跟踪失败,然后机器人将所述滑动窗口内的图像清空。
进一步地,机器人将摄像头采集到当前帧图像时的光心与预设特征点对在当前帧图像内的特征点的连线标记为第一观测线,并将摄像头采集到参考帧图像时的光心与同一个预设特征点对在参考帧图像内的特征点的连线标记为第二观测线,然后将第一观测线与第二观测线的交点标记为目标探测点;其中,该预设特征点对、摄像头采集到当前帧图像时的光心、以及摄像头采集到参考帧图像时的光心都处于同一平面;或者,摄像头采集到当前帧图像时的光心、摄像头采集到参考帧图像时的光心、以及目标探测点都在同一平面;所述同一平面是极平面;机器人将极平面与当前帧图像的交线记为当前帧图像的成像平面中的极线,并将极平面与参考帧图像的交线记为参考帧图像的成像平面的极线;在同一个预设特征点对中,由当前帧图像的特征点转换到参考帧图像后,变为第一投影点,其坐标为第一一坐标;将第一投影点到参考帧图像的成像平面中的极线的距离表示为第一残差值;在同一个预设特征点对中,由参考帧图像的特征点转换到当前帧图像后,变为第二投影点,其坐标为第二一坐标;将第二投影点到当前帧图像的成像平面中的极线的距离表示为第二残差值;在步骤S15中,预设特征点对是第三特征点对;在步骤S16中,每当重复执行步骤S12和步骤S13后,预设特征点对是最新执行的步骤S13所筛选出的第二特征点对。
进一步地,在所述步骤S15或所述步骤S16中,引入残差的方法包括:当惯性数据包括当前帧图像相对于参考帧图像的平移向量、以及当前帧图像相对于参考帧图像的旋转矩阵时,机器人将当前帧图像相对于参考帧图像的平移向量记为第一平移向量,将当前帧图像相对于参考帧图像的旋转矩阵记为第一旋转矩阵,机器人控制第一旋转矩阵将预设特征点对在当前帧图像中的特征点的归一化向量转换到参考帧图像的坐标系下,得到第一一向量;再控制第一平移向量叉乘第一一向量,得到第一二向量,并形成参考帧图像的成像平面中的极线;然后对第一二向量中的横轴坐标和该第一二向量中的纵轴坐标的平方和求平方根,得到极线的模长;同时,控制该预设特征点对在参考帧图像中的特征点的归一化向量与第一二向量点乘,再将点乘的结果设置为该预设特征点对的对极约束误差值;然后将预设特征点对的对极约束误差值与极线的模长的比值设置为第一残差值;或者,当惯性数据包括参考帧图像相对于当前帧图像的平移向量、以及参考帧图像相对于当前帧图像的旋转矩阵时,机器人将参考帧图像相对于当前帧图像的平移向量记为第二平移向量,将参考帧图像相对于当前帧图像的旋转矩阵记为第二旋转矩阵,机器人控制第二旋转矩阵将预设特征点对在参考帧图像中的特征点的归一化向量转换到当前帧图像的坐标系下,得到第二一向量;再控制第二平移向量叉乘第二一向量,得到第二二向量,并形成当前帧图像的成像平面中的极线;然后对第二二向量中的横轴坐标和该第二二向量中的纵轴坐标的平方和求平方根,得到极线的模长;同时,控制该预设特征点对在当前帧图像中的特征点的归一化向量与第二二向量点乘,再将点乘的结果设置为该预设特征点对的对极约束误差值;然后将该预设特征点对的对极约束误差值与极线的模长的比值设置为第二残差值;其中,预设特征点对在当前帧图像中的特征点的归一化向量是预设特征点对在当前帧图像中的特征点的归一化平面坐标相对于当前帧图像的坐标系的原点形成的向量;预设特征点对在参考帧图像中的特征点的归一化向量是预设特征点对在参考帧图像的特征点的归一化平面坐标相对于参考帧图像的坐标系的原点形成的向量。
进一步地,在所述步骤S15或所述步骤S16中,在预设特征点对之间引入残差,再结合该残差及其对最新获得的惯性数据求导的结果,计算出惯性补偿值,再使用惯性补偿值对最新获得的惯性数据进行修正的方法包括:当惯性数据包括当前帧图像相对于参考帧图像的平移向量、以及当前帧图像相对于参考帧图像的旋转矩阵时,机器人将第一旋转矩阵与预设特征点对在当前帧图像中的特征点的归一化向量相乘的算式标记为第一一转换式;再将第一平移向量与第一一转换式相叉乘的算式标记为第一二转换式;再将预设特征点对在参考帧图像中的特征点的归一化向量与第一二转换式相点乘的算式标记为第一三转换式;再将第一二转换式的计算结果置为数值0,构成直线方程,再对该直线方程在横轴坐标维度的系数和纵轴坐标维度的系数求取平方和,再对求取得到的平方和计算平方根,得到第一平方根,再将第一平方根的倒数与第一三转换式相乘的算式设置为第一四转换式;然后将第一四转换式的计算结果设置为第一残差值,形成第一残差推导式,并确定在该预设特征点对之间引入残差;然后控制第一残差推导式分别对第一平移向量和第一旋转矩阵求偏导,得到雅克比矩阵;然后将雅克比矩阵的逆矩阵与第一残差值乘积设置为惯性补偿值;然后机器人使用惯性补偿值对惯性数据进行修正;或者,当惯性数据包括参考帧图像相对于当前帧图像的平移向量、以及参考帧图像相对于当前帧图像的旋转矩阵时,机器人将第二旋转矩阵与预设特征点对在参考帧图像中的特征点的归一化向量相乘的算式标记为第二一转换式;再将第二平移向量与第二一转换式相叉乘的算式标记为第二二转换式;再将预设特征点对在当前帧图像中的特征点的归一化向量与第二二转换式相点乘的算式标记为第二三转换式;再将第二二转换式的计算结果置为数值0,构成直线方程,再对该直线方程在横轴坐标维度的系数和纵轴坐标维度的系数求取平方和,再将求取得到的平方和计算平方根,得到第二平方根,再将第二平方根的倒数与第二三转换式相乘的算式设置为第二四转换式;然后将第二四转换式的计算结果设置为第二残差值,形成第二残差推导式,并确定在该预设特征点对之间引入残差;然后控制第二残差推导式分别对第二平移向量和第二旋转矩阵求偏导,获得雅克比矩阵;然后将雅克比矩阵的逆矩阵与第二残差值乘积设置为惯性补偿值;然后机器人使用惯性补偿值对惯性数据进行修正。
进一步地,对于所述步骤S16,在机器人执行完所述步骤S15后,第一次重复执行步骤S12时,机器人计算除了误匹配参考帧图像之外的每个第三特征点对的对极约束误差值,其中,每个第三特征点对的对极约束误差值是由步骤S15修正过的惯性数据决定;当第三特征点对的对极约束误差值小于预设像素距离阈值时,将该第三特征点对更新为第一特征点对,并确定从第三特征点对当中筛选出新的第一特征点对;第N次重复执行步骤S12时,机器人计算最新执行的步骤S13中筛选出的每个第二特征点对的对极约束误差值;当第二特征点对的对极约束误差值小于预设像素距离阈值时,将该第二特征点对更新为第一特征点对,并确定从所述步骤S13筛选出的所有第二特征点对当中筛选出新的第一特征点对;其中,N是设置为大于1且小于或等于所述预设迭代匹配次数。
进一步地,在所述步骤S16中,所述基于第二特征点对在每帧参考帧图像内的特征点的数量,从滑动窗口内的参考帧图像中筛选出匹配帧图像的方法包括:机器人分别在所述滑动窗口内的每帧参考帧图像中,统计第二特征点对在该帧参考帧图像中的特征点的数量;若机器人在其中一帧参考帧图像内匹配出的第二特征点对的数量小于或等于第二预设点数阈值,则确定所述其中一帧参考帧图像与所述当前帧图像匹配失败;若机器人在其中一帧参考帧图像内匹配出的第二特征点对的数量大于第二预设点数阈值,则确定所述其中一帧参考帧图像与所述当前帧图像匹配成功,并将所述其中一帧参考帧图像设置为匹配帧图像;若机器人在每帧参考帧图像内匹配出的第二特征点对的数量都小于或等于第二预设点数阈值时,确定滑动窗口内的每帧参考帧图像都与所述当前帧图像匹配失败,则确定机器人使用窗口匹配方式跟踪失败。
进一步地,在所述步骤S17中,所述基于对极约束误差值与第二特征点对在每帧匹配帧图像内的特征点的数量,在所有的匹配帧图像中选择出最优匹配帧图像的方法包括:在每帧匹配帧图像内,计算该帧匹配帧图像内的特征点所属的第二特征点对的对极约束误差值的和值,作为该帧匹配帧图像的极约束误差值累加值;在每帧匹配帧图像内,统计该帧匹配帧图像内组成第二特征点对的特征点的数量,作为该帧匹配帧图像的特征点匹配数量;然后将极约束误差值累加值最小且特征点匹配数量最大的匹配帧图像设置为最优匹配帧图像。
进一步地,机器人使用投影匹配方式进行图像跟踪的方法包括:步骤S21,机器人通过摄像头采集图像,并通过惯性传感器获取惯性数据;其中,摄像头采集的图像包括上一帧图像和当前帧图像;步骤S22,机器人利用惯性数据将上一帧图像的特征点投影到当前帧图像内,得到投影点,其中,惯性数据包括上一帧图像相对于当前帧图像的旋转矩阵、以及上一帧图像相对于当前帧图像的平移向量;步骤S23,机器人根据描述子之间的标准距离,分别在每个投影点的预设搜索邻域内搜索待匹配点;然后机器人计算投影点与搜索出的每个待匹配点之间的向量,并将该投影点指向已搜索出的待匹配点的向量标记为待匹配向量;其中,待匹配点是来源于当前帧图像内的特征点,待匹配点不属于投影点;每个待匹配向量都与一个投影点和一个待匹配点相对应;步骤S24,机器人统计相互平行的待匹配向量的数量,当统计到相互平行的待匹配向量的数量大于或等于预设匹配数量时,确定机器人使用投影匹配方式跟踪成功,并确定机器人跟踪当前帧图像成功。
进一步地,机器人将相互平行的每个待匹配向量都设置为目标匹配向量,并在所有投影点的预设搜索邻域内将不与所述目标匹配向量平行的待匹配向量标记为误匹配向量,再将误匹配向量对应的一个投影点和该误匹配向量对应的一个待匹配点设置为一对误匹配点,将目标匹配向量对应的一个投影点和该目标匹配向量对应的一个待匹配点设置为一对目标匹配点;其中,相互平行的每个待匹配向量的方向是相同或相反,且目标匹配向量保持与预设映射方向平行,预设映射方向与惯性数据关联。
进一步地,步骤S24还包括:当统计到相互平行的待匹配向量的数量小于预设匹配数量时,机器人按照预设扩展步长扩大每个投影点的预设搜索邻域的覆盖范围,得到扩大后的预设搜索邻域,并将扩大后的预设搜索邻域更新为步骤S23所述的预设搜索邻域,然后执行步骤S23,直至机器人重复执行步骤S23的次数达到预设扩展次数,然后停止扩大每个投影点的预设搜索邻域的覆盖范围,并确定机器人使用投影匹配方式跟踪失败;其中,步骤S22、步骤S23与步骤S24的组合是所述投影匹配方式。
进一步地,所述机器人根据描述子之间的标准距离,在每个投影点的预设搜索邻域内搜索待匹配点的方法包括:机器人以每个投影点为圆心点设置圆形邻域,并将该圆形邻域设置为该投影点的预设搜索邻域,其中,惯性数据包括上一帧图像与当前帧图像之间的摄像头的位姿变化量;上一帧图像与当前帧图像之间的摄像头的位姿变化量越大,则将所述预设搜索邻域的半径设置得越大;上一帧图像与当前帧图像之间的摄像头的位姿变化量越小,则将所述预设搜索邻域的半径设置得越小;机器人在每个投影点的预设搜索邻域内,从该投影点的预设搜索邻域的圆心点开始搜索除了该投影点之外的特征点;当搜索到的当前帧图像内的特征点的描述子与该预设搜索邻域的圆心点的描述子的标准距离最近时,将搜索到的当前帧图像内的特征点设置为该预设搜索邻域内的待匹配点;其中,所述标准距离使用欧式距离或汉明距离表示。
综上,本发明结合窗口匹配方式和投影匹配方式进行图像跟踪,具体在不同的跟踪结果下,采用相适应的匹配方式进行图像跟踪,实现分阶段适应性地对机器人实时采集的当前帧图像进行跟踪,在不同的匹配方式下预先确定的图像以不同的特征点搜索范围和转换方式跟踪实时采集到当前帧图像,完成相邻两帧图像之间的高效合理匹配,解决了在单纯特征法视觉里程计在计算能力受限的机器人平台中运行帧率较低、导航定位的实时性较差的问题,大幅降低当前帧图像的平均跟踪时间,提升了摄像头和惯性传感器组成的视觉里程计的运行帧率,很好地实现了机器人的实时定位。
在窗口匹配方式进行图像跟踪时,机器人从当前帧图像的特征点和滑动窗口内所有的参考帧图像的特征点中筛选出所有的第一特征点对,完成当前帧每个特征点与参考帧的所有特征点的匹配,获得初步过滤出来的特征点对,并去除部分不满足相应对极约束误差值的特征点对的干扰。在此基础上,再结合深度信息获得两帧图像的特征点在另一种尺度信息维度的匹配情况,从而提高特征点对匹配和图像跟踪的鲁棒性和准确性,使得机器人定位更可靠。然后在预设特征点对之间引入残差,再结合残差及其对惯性数据的求导结果,计算出惯性补偿值使得原始的惯性数据经受视觉特征点对的关于对极约束误差值以及深度值的比值的迭代匹配的考验后,得到能够进行修正的偏导信息,让惯性数据得到优化处理,提高机器人的定位精度。节约了大量的计算量,提高了计算效率,提高了定位与地图构建速度。
在投影匹配方式进行图像跟踪时,结合相邻两帧图像之间的机器人的位姿变化量、特征点的投影变换关系,在待跟踪的当前帧图像内识别方向一致的待匹配向量及统计其数量,确定机器人机器人使用投影匹配方式是否成功地完成图像跟踪,降低了特征的误匹配率,降低计算难度,还能在跟踪失败后切换入窗口匹配方式,进一步地缩小特征点匹配的搜索范围,提高机器人视觉定位的准确性和效率;另外本发明仅使用单个摄像头进行定位,设备简单成本低。
附图说明
图1是本发明一种实施例公开一种机器人视觉跟踪方法的流程图。
图2是本发明另一种实施例公开的机器人使用窗口匹配方式进行图像跟踪的方法的流程图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行详细描述。为进一步说明各实施例,本发明提供有附图。这些附图为本发明揭露内容的一部分,其主要用以说明实施例,并可配合说明书的相关描述来解释实施例的运作原理。配合参考这些内容,本领域普通技术人员应能理解其他可能的实施方式以及本发明的优点。作为流程图描绘的处理或方法。虽然流程图将各步骤描述成顺序的处理,但是其中的许多步骤可以被并行地、并发地或者同时实施。此外,各步骤的顺序可以被重新安排。当其操作完成时所述处理可以被终止,但是还可以具有未包括在附图中的附加步骤。所述处理可以对应于方法、函数、规程、子例程、子程序等等。
需要说明的是,本申请的说明书和权利要求书及上述附图中的术语“包括”和“具有”以及他们的任何变形,意图在于覆盖不排他的包含,例如,包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元,而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
应注意到:相似的标号和字母在下面的附图中表示类似项,因此,一旦某一项在一个附图中被定义,则在随后的附图中不需要对其进行进一步定义和解释。
本文中术语“和/或”,仅仅是描述一种关联关系,表示可以存在三种关系,例如,A和/或B,可以表示:单独存在A,同时存在A和B,单独存在B这三种情况。另外,本文中术语“至少一种”表示多种中的任意一种或多种中的至少两种的任意组合,例如,包括A、B、C中的至少一种,可以表示包括从A、B和C构成的集合中选择的任意一个或多个元素。
作为一种实施例,公开一种机器人视觉跟踪方法,所述机器人视觉跟踪方法的执行主体是固定装配摄像头和惯性传感器的机器人,其中,该机器人是自主移动机器人。如图1所示,所述机器人视觉跟踪方法包括:机器人使用窗口匹配方式进行图像跟踪,当机器人使用窗口匹配方式跟踪成功时,机器人停止使用窗口匹配方式进行图像跟踪,然后机器人使用投影匹配方式进行图像跟踪。在本实施例中,机器人使用窗口匹配方式进行图像跟踪的过程中,机器人使用窗口匹配方式对滑动窗口内的所有参考帧图像与同一当前帧图像之间进行匹配(具体涉及到特征点的匹配),从而通过参考帧图像帧对当前帧图像跟踪;当机器人使用窗口匹配方式跟踪当前帧图像成功时,机器人已经完成滑动窗口内的所有参考帧图像与同一当前帧图像的匹配,并从所有参考帧图像中获得匹配度最好的参考帧图像,则机器人可以停止使用窗口匹配方式进行图像跟踪,然后使用投影匹配方式进行上一帧图像与当前帧图像之间的匹配,开始通过上一帧图像跟踪当前帧图像。
然后,当机器人使用投影匹配方式跟踪失败时,机器人停止使用投影匹配方式进行图像跟踪,然后机器人使用窗口匹配方式进行图像跟踪。当机器人使用投影匹配方式跟踪图像失败时,确定上一帧图像与当前帧图像之间的匹配失败,则不继续使用投影匹配方式进行图像跟踪,然后重新使用窗口匹配方式进行新采集的图像跟踪。一般地,机器人使用窗口匹配方式跟踪失败或使用投影匹配方式跟踪失败时,都是在先采集的多帧图像都无法与当前帧图像匹配成功,则无法跟踪当前帧图像,即理解为跟丢当前帧图像。
综上,本实施例轮番使用窗口匹配方式和投影匹配方式进行图像跟踪,具体在不同的跟踪结果下,采用相适应的匹配方式进行图像跟踪,实现分阶段适应性地对机器人实时采集的当前帧图像进行跟踪,在不同的匹配方式下预先确定的图像以不同的特征点搜索范围和转换方式跟踪实时采集到当前帧图像,完成相邻两帧图像之间的高效合理匹配,解决了在单纯特征法视觉里程计在计算能力受限的机器人平台中运行帧率较低、导航定位的实时性较差的问题,大幅降低当前帧图像的平均跟踪时间,提升了摄像头和惯性传感器组成的视觉里程计的运行帧率,很好地实现了机器人的实时定位。
作为一种实施例,所述机器人视觉跟踪方法还包括:当机器人使用窗口匹配方式跟踪失败时,机器人停止使用窗口匹配方式进行图像跟踪,机器人清空滑动窗口,包括清空滑动窗口内所有帧图像,以便于填入新采集的图像,再使用窗口匹配方式进行图像跟踪,在更新参考帧图像基础上进行图像跟踪,实现将新的参考帧与当前帧图像上建立匹配关系;当机器人使用窗口匹配方式跟踪成功后,将当前帧图像填入所述滑动窗口,以便于跟踪机器人实时采集的图像,然后摄像头采集的下一帧图像会更新为当前帧图像,则机器人可以将所述滑动窗口内新填入的当前帧图像与所述下一帧图像进行匹配,以跟踪所述下一帧图像。从而排除错误匹配的图像帧的干扰,通过引入新采集的图像帧来保证跟踪的实时性和准确性。
另外,机器人使用投影匹配方式进行图像跟踪的过程中,若检测到当前帧图像与上一帧图像的时间间隔是超出预设时间阈值,则机器人停止使用投影匹配方式进行图像跟踪,转而使用窗口匹配方式进行图像跟踪;优选地,预设时间阈值是设置为1秒,这里的检测方式是计时检测;当机器人连续采集到的相邻两帧图像的时间间隔超出1秒,则机器人停止使用投影匹配方式进行图像跟踪,转而使用窗口匹配方式进行图像跟踪,因为过大的采集时间间隔会给相邻两帧图像之间的投影转换积累较大的位姿误差。
需要说明的是,图像跟踪用于表示在先采集的图像的特征点与当前帧图像的特征点之间的匹配;机器人使用窗口匹配方式进行图像跟踪的过程中,机器人已经将在先采集的至少一帧图像填入所述滑动窗口,以便于所述滑动窗口内的图像的特征点能够与当前帧图像的特征点进行匹配。所述窗口匹配方式所需的滑动窗口内填入的所有图像的帧数是为一个固定数值,即所述滑动窗口的大小;填入所述滑动窗口内图像都标记为参考帧图像,作为一组候选匹配的关键帧;本实施例提及的参考帧图像可以简称为参考帧,当前帧图像可以简称为当前帧,一帧图像可以简称为一个图像帧,相邻两帧图像可以简称为相邻两帧、或者相邻两个图像帧。所述特征点是属于图像的像素点,特征点是在所述摄像头所处的环境中,以点的形式存在的环境元素。
作为一种实施例,如图2所示,机器人使用窗口匹配方式进行图像跟踪的方法包括以下步骤:
步骤S101,机器人通过摄像头采集当前帧图像,并通过惯性传感器获取惯性数据;然后机器人执行步骤S102。摄像头装配在机器人的外侧,摄像头的镜头朝向指向机器人的前进方向,用于采集机器人前方的图像信息,按照帧计数的话,摄像头采集到当前帧图像,机器人从当前帧图像中获取特征点,其中,特征点指的是在所述摄像设备所处的环境中,以点的形式存在的环境元素,以便于与在先采集的图像进行匹配,可以实现跟踪当前帧图像;惯性传感器一般安装在机器人的机体内部,比如码盘安装在机器人的驱动轮中,用于获取机器人运动过程中产生的位移信息;惯性测量单元(比如陀螺仪)用于获取机器人运动过程中产生的角度信息,其中,码盘和惯性测量单元组成惯性系统,用于获取惯性数据,从而确定任意两帧图像之间的摄像头位姿变换关系,机器人可以使用这一位姿变换关系进行特征点之间的匹配转换,其中,两帧图像之间的摄像头的位姿变换关系中,涉及的旋转矩阵的初始状态量和平移向量的初始状态量是预先设定;在这些初始状态量的基础上,机器人依靠码盘在摄像头先后采集的两帧图像之间感测的位移变化量、以及陀螺仪在摄像头先后采集的两帧图像之间感测的角度变化量,进行积分处理,可以是使用欧拉积分分别对位移变化量和角度变化量进行积分处理,得到机器人在任意两帧图像(包括来源于预先采集的多帧图像中)之间的位姿变化量,进而可以获得最新的旋转矩阵和最新的平移向量。因此,本实施例公开的惯性数据能够表示当前帧图像的坐标系与参考帧图像的坐标系之间的转换关系,包括平移转换关系、旋转转换关系、位移差值、角度增量等,其中,参考帧图像是前述实施例公开的具有固定大小的滑动窗口内的图像。
步骤S102,在惯性数据的基础上,机器人利用对极约束误差值,从当前帧图像的特征点和滑动窗口内所有的参考帧图像的特征点中筛选出第一特征点对,从而过滤掉对极约束误差值过大的特征点对,实现在当前帧图像和每帧参考帧图像之间过滤不匹配的特征点;然后执行步骤S103。在本实施例中,机器人在惯性数据涉及的当前帧图像的坐标系与参考帧图像的坐标系之间的转换关系的基础上,对当前帧图像的特征点和滑动窗口内所有的参考帧图像的特征点进行对极约束,获得每个特征点对的对极约束误差值,其中,对极约束误差值是当前帧图像的特征点和滑动窗口内的参考帧图像的特征点按照对极约束下的几何成像关系计算出来的误差值,对极约束用于表示空间中的一个三维点在不同成像平面上对应的像素点的几何关系,也表示跟随机器人运动的摄像头先后采集的两帧图像中各像素的射影关系(或者说是各匹配点的几何关系)。第一特征点对的特征点可以是分别位于当前帧图像和每帧参考帧图像,也可以是分别位于当前帧图像和部分帧参考帧图像;滑动窗口被设置为填入预先采集的至少一帧图像,以便于后续实施当前帧与滑窗内的每一帧进行匹配;特征点是图像的像素点,特征点是在所述摄像头所处的环境中,以点的形式存在的环境元素,描述机器人所需跟踪的环境特征。
步骤S103,在所述惯性数据的基础上,机器人利用特征点的深度值,从第一特征点对中筛选出第二特征点对;然后执行步骤S104。在步骤S103中,机器人在步骤S102过滤出的第一特征点对的基础上,在像素点的噪声影响的范围内,计算第一特征点对在当前帧图像中的特征点的深度信息、以及第一特征点对在参考帧图像中的特征点的深度信息,具体是利用所述惯性数据中涉及机器人(或摄像头)在当前帧图像与参考帧图像之间的位移信息、以及第一特征点对的各个特征点的归一化平面坐标计算出相应特征点的深度值;在一些实施例中,第一特征点对在当前帧图像中的特征点标记为P1,摄像头采集到当前帧图像时的光心被标记为O1,第一特征点对在参考帧图像中的特征点标记为P2,摄像头采集到参考帧图像时的光心被标记为O2,则在不考虑像素点的噪声影响时,直线O1P1与直线O2P2相交于点P3,则线段O1P3的长度是特征点P1的深度值,线段O2P3的长度是特征点P2的深度值。然后,判断到特征点P1的深度值与特征点P2的深度值的比值满足一定的比值范围时,将特征点P1和特征点P2组成第二特征点对,否则特征点P1和特征点P2组成错误匹配点对,从而从第一特征点对中筛选出第二特征点对。
步骤S104,根据第二特征点对所对应的描述子的相似度,从第二特征点对中筛选出第三特征点对;然后执行步骤S105。步骤S104具体包括:对于当前帧图像与所述滑动窗口内的每帧参考帧图像,机器人计算每个第二特征点对在参考帧图像中的特征点的描述子与该第二特征点对在当前帧图像中的特征点的描述子之间的相似度;当机器人计算出的一个第二特征点对在参考帧图像中的特征点的描述子与该第二特征点对在当前帧图像中的特征点的描述子之间的相似度是当前帧图像的描述子与该第二特征点对的特征点所在的参考帧图像的描述子之间的相似度当中的最小值时,将该第二特征点对标记为第三特征点对并确定筛选出第三特征点对;从而缩小特征点的搜索范围。其中,该第二特征点对的特征点所在的参考帧图像的描述子是该第二特征点对的特征点所在的参考帧图像内所有组成第二特征点对的特征点的描述子,可以使用帧描述子表示。当前帧图像的描述子是当前帧图像内,与该第二特征点对的特征点所在的参考帧图像的特征点组成第二特征点对的特征点的描述子;第二特征点对所对应的描述子的相似度,使用当前帧图像中特征点的描述子和滑动窗口内对应的参考帧图像中特征点的描述子之间的欧式距离或汉明距离表示。从而实现使用两帧之间的像素点相似程度来跟踪当前帧图像,以跟踪机器人的运动。
具体地,在步骤S104中,机器人将每个第二特征点对的特征点所在的参考帧图像记为待匹配的参考帧图像,一般地,待匹配的参考帧图像的数量等于参考帧图像的数量;机器人还将当前帧图像与待匹配的参考帧图像之间存在的第二特征点对标记为第二待匹配特征点对,其中,该第二待匹配特征点对在当前帧图像中的特征点记为第二一特征点,该第二待匹配特征点对在参考帧图像中的特征点记为第二二特征点,则第二二特征点位于待匹配的参考帧图像中;机器人需要计算该参考帧图像中所有第二二特征点的描述子与其对应的第二一特征点的描述子之间的相似度。然后,当机器人计算出的一个第二待匹配特征点对在参考帧图像中的特征点的描述子与该第二待匹配特征点对在当前帧图像中的特征点的描述子之间的相似度是该参考帧图像中所有第二二特征点的描述子与其对应的第二一特征点的描述子之间的相似度当中的最小值时,将该第二待匹配特征点对标记为第三特征点对并确定筛选出第三特征点对,其中,每帧参考帧图像与当前帧图像之间可以被筛选出多对第三特征点对;一个第二待匹配特征点对在参考帧图像中的特征点的描述子与该第二待匹配特征点对在当前帧图像中的特征点的描述子之间的相似度是,第二二特征点的描述子与第二一特征点的描述子之间的相似度,作为两种描述子的相似性度量,具体地表示为第二二特征点与第二一特征点在多种维度下的欧式距离或汉明距离的平方和的平方根,其中,每一种维度可以表示特征点的一种二进制编码形式。
在上述实施例的基础上,每当机器人搜索完当前帧图像和所述滑动窗口内的一帧参考帧图像之间组成第二特征点对的所有特征点后,即机器人在一帧参考帧图像内计算完所有第二一特征点的描述子与对应的第二一特征点的描述子之间的相似度后,若机器人在该帧参考帧图像内统计到第三特征点对的数量大于第一预设点数阈值,则确定当前帧图像和该帧参考帧图像匹配成功,然后继续搜索当前帧图像和所述滑动窗口内的下一帧参考帧图像之间组成第二特征点对的所有特征点;若机器人在该帧参考帧图像内统计到第三特征点对的数量小于或等于第一预设点数阈值,则确定当前帧图像和该帧参考帧图像匹配失败,并将该帧参考帧图像设置为误匹配参考帧图像,然后继续搜索当前帧图像和所述滑动窗口内的下一帧参考帧图像之间组成第二特征点对的所有特征点;在一些实施例中,后续不使用该帧参考帧图像的特征点与当前帧图像的特征点进行匹配,优选地,第一预设点数阈值设置为20。在一些实施例中会将该帧参考帧图像的特征点标记为错误匹配特征点,不再与当前帧图像内的特征点组成所述第一特征点对、所述第二特征点对、或所述第三特征点对;若机器人在该帧参考帧图像内统计到第三特征点对的数量大于第一预设点数阈值,则确定当前帧图像和该帧参考帧图像匹配成功;其中,当机器人确定当前帧图像和所述滑动窗口内所有帧参考帧图像都匹配失败时,确定机器人使用窗口匹配方式跟踪失败,然后机器人将所述滑动窗口内的图像清空。
步骤S105,机器人在第三特征点对之间引入残差,再结合残差及其对惯性数据的求导结果,计算出惯性补偿值,再使用惯性补偿值对惯性数据进行修正;在一些实施例中,第三特征点对在当前帧图像e1中的特征点标记为点P4,摄像头采集到当前帧图像e1时的光心被标记为点O1,第三特征点对在参考帧图像e2中的特征点标记为点P5,摄像头采集到参考帧图像e2时的光心被标记为点O2,则直线O1P4与直线O2P5相交于点P6,点O1、点O2和点P6组成一个极平面,直线O1P4转换到参考帧图像e2后变成极线L;在不考虑误差的前提下,极平面与参考帧图像e2的交线与极线L重合,该交线是经过点P5,但实际上由于误差的存在而没有重合在一起,本实施例将点P5设置为极线L上的一个观测点,然后使用点P5到极线L的距离来表示这个误差,进而将这个误差设置为所述残差。
进而,在本实施例中,获取所述残差需要构建一个等效于求取点到线距离的推导式,先将对应的旋转矩阵和平移向量设置为已知状态量时,可以由这个推导式计算出残差值,作为所述残差的数值结果;然后将对应的旋转矩阵和平移向量设置为未知状态量,再控制该推导式(等效于一个方程式)分别对平移向量和旋转矩阵求偏导,获得雅克比矩阵,实现残差对位姿的求导,从而以矩阵的形式保存关于惯性数据的求导结果;然后结合导数的性质可知,机器人将雅克比矩阵的逆矩阵与残差值的乘积设置为惯性补偿值,获取到对位移积分量的补偿量和对角度积分量的补偿量,作为所述惯性补偿值,从而实现使用最小二乘法来所述残差对应的推导式以求解获得所述惯性补偿值;再使用该最优补偿值对惯性数据进行修正,具体的修正方式包括但不限于与原来的惯性数据相加、相减、相乘或相除。然后机器人将修正后的惯性数据更新为所述惯性数据,并将步骤S104所述的第三特征点对在当前帧图像的特征点更新为所述的当前帧图像的特征点,并将步骤S104所述的第三特征点对在参考帧图像的特征点更新为所述的滑动窗口内所有的参考帧图像的特征点,缩小后续特征点的搜索范围,完成一次特征点的过滤,也完成对当前帧图像的特征点和所述滑动窗口内的每帧参考帧图像的一次初始化;然后执行步骤S106。
步骤S106,判断步骤S107的执行次数和步骤S108的执行次数是否都达到预设迭代匹配次数,是则执行步骤S110,否则执行步骤S107。在一些实施例中,步骤S106也可以理解为判断步骤S107的执行次数、步骤S108的执行次数、以及步骤S109的执行次数是否都达到预设迭代匹配次数,是则执行步骤S110,否则执行步骤S107,使得惯性数据的修正次数达到所述预设迭代匹配次数与数值1的和值。其中,预设迭代匹配次数优选为2或3。机器人通过重复执行步骤S107和步骤S108来筛选出第二特征点对,并排除更多的错误匹配点对,减少搜索范围,减小惯性补偿值的计算量。
步骤S107,在惯性数据的基础上,机器人利用对极约束误差值,从当前帧图像的特征点和滑动窗口内所有的参考帧图像的特征点中筛选出第一特征点对,等效于重复执行步骤S102,从而过滤掉对极约束误差值过大的特征点对,实现在当前帧图像和每帧参考帧图像之间过滤不匹配的特征点;然后执行步骤S108。
步骤S108,在所述惯性数据的基础上,机器人利用特征点的深度值,从第一特征点对(最新执行的步骤S107中筛选出的)中筛选出第二特征点对;等效于重复执行步骤S103,利用所述惯性数据中涉及机器人(或摄像头)在当前帧图像与参考帧图像之间的位移信息、角度信息、以及第一特征点对的各个特征点的归一化平面坐标计算出相应特征点的深度值匹配的三角几何关系,计算出第一特征点对的各个特征点的深度值,再通过对比第一特征点对在当前帧图像中的特征点的深度值与该第一特征点对在参考帧图像中的特征点的深度值,来筛选出第二特征点对。然后执行步骤S109。
步骤S109,机器人在最新执行的步骤S108中筛选出的第二特征点对之间引入残差,再结合残差及其对最新获得的惯性数据(最新执行过的步骤S105或上一次执行过的步骤S109修正过的惯性数据)的求导结果,计算出惯性补偿值,再使用惯性补偿值对最新获得的惯性数据进行修正,然后将修正后的惯性数据更新为步骤S107所述的惯性数据,并将最新执行的步骤S108中筛选出的第二特征点对所包括的特征点对应更新为步骤S107所述的当前帧图像的特征点和滑动窗口内所有的参考帧图像的特征点;等效于重复执行步骤S105,而是由步骤S103跳过步骤S104中对第三特征点对的筛选,直接执行步骤S105。然后执行步骤S106,去判断步骤S107的执行次数和步骤S108的执行次数是否都达到预设迭代匹配次数,进而实现重复在步骤S109中修正惯性数据,降低残差,优化后续填入滑动窗口内的参考帧图像,提高机器人定位的准确性。
可以理解的是,执行步骤S102和步骤S103后、或每当重复执行步骤S107和步骤S108后(开始执行步骤S109),机器人在最新筛选出的第二特征点对之间引入残差,再结合该残差对最新获得的惯性数据求导的结果,计算出惯性补偿值,再使用惯性补偿值对最新获得的惯性数据进行修正,然后将修正后的惯性数据更新为所述的惯性数据,并将最新筛选出的第二特征点对所包括的特征点对应更新为所述的当前帧图像的特征点和滑动窗口内所有的参考帧图像的特征点,以缩小特征点的搜索范围,进一步节约了大量的匹配计算量,提高机器人定位与地图构建速度。
需要说明的是,对于所述步骤S107,在机器人执行完所述步骤S105后,第一次执行步骤S107时,机器人计算每个第三特征点对的对极约束误差值,其中,每个第三特征点对的对极约束误差值是由步骤S105修正过的惯性数据决定,对于第三特征点对的具体对极约束方式与步骤S102的相同,第三特征点对的的对极约束误差值的计算方式与与步骤S102的相同,但进行对极约束的特征点对的标记类型和数量都不相同。当机器人计算出的一个第三特征点对的对极约束误差值小于预设像素距离阈值时,将该第三特征点对更新为所述第一特征点对,并确定除了误匹配参考帧图像之外,从所述步骤S104筛选出的第三特征点对当中,筛选出新的第一特征点对。
需要说明的是,对于所述步骤S107,第N次重复执行步骤S107时,机器人计算最新执行的步骤S108中筛选出的每个第二特征点对的对极约束误差值,其中,每个第二特征点对的对极约束误差值是由上一次执行的步骤S109修正过的惯性数据确定;当机器人计算出的一个第二特征点对的对极约束误差值小于预设像素距离阈值时,将该第二特征点对标记为所述第一特征点对以更新第一特征点对,并确定从所述步骤S13筛选出的所有第二特征点对当中筛选出新的第一特征点对;其中,N是设置为大于1且小于或等于所述预设迭代匹配次数。
步骤S110,基于第二特征点对在每帧参考帧图像内的特征点的数量,从滑动窗口内的参考帧图像中筛选出匹配帧图像;然后执行步骤S111。因此,机器人选择在完成当前帧图像内的每个特征点与所述滑动窗口内每帧参考帧图像的所有特征点的迭代匹配后,会统计第二特征点对在每帧参考帧图像内的特征点的数量,再基于每帧参考帧内统计到的第二二特征点的数量是否满足阈值条件,在所述滑动窗口内筛选出与当前帧图像匹配的参考帧图像,从而将当前帧图像与对应的一帧参考帧图像组成一个匹配帧图像对,其中,第二特征点对在当前帧图像中的特征点记为第二一特征点,第二特征点对在参考帧图像中的特征点记为第二二特征点。
具体地,所述基于第二特征点对在每帧参考帧图像内的特征点的数量,从滑动窗口内的参考帧图像中筛选出匹配帧图像的方法包括:在所述惯性数据被重复修正的次数达到预设迭代匹配次数后,机器人分别在所述滑动窗口内的每帧参考帧图像中,统计第二特征点对在该帧参考帧图像中的特征点的数量,作为对应参考帧图像内匹配出的第二特征点对的数量,若将第二特征点对在参考帧图像中的特征点标记为第二二特征点,则该帧参考帧图像内匹配出的第二特征点对的数量等于该帧参考帧图像内的第二二特征点的数量;若机器人在其中一帧参考帧图像内匹配出的第二特征点对的数量小于或等于第二预设点数阈值,则确定所述其中一帧参考帧图像与所述当前帧图像匹配失败,可以将所述其中一帧参考帧图像设置为误匹配参考帧图像;若机器人在其中一帧参考帧图像内匹配出的第二特征点对的数量大于第二预设点数阈值,则确定所述其中一帧参考帧图像与所述当前帧图像匹配成功,并将所述其中一帧参考帧图像设置为匹配帧图像;进一步地,若机器人在每帧参考帧图像内匹配出的第二特征点对的数量都小于或等于第二预设点数阈值时,确定滑动窗口内的每帧参考帧图像都与所述当前帧图像匹配失败,则确定机器人使用窗口匹配方式跟踪失败。优选地,第二预设点数阈值设置为15,小于所述第一预设点数阈值。当预设迭代匹配次数被设置为增大时,所述滑动窗口内被排除的错误匹配点对变得更多或保持不变,因此,所有参考帧图像内匹配出的第二特征点对的数量会变小或保持不变,则每帧参考帧图像内的第二二特征点的数量或所有帧参考帧图像内的第二二特征点的总数量会变小或保持不变。
步骤S111,基于对极约束误差值与第二特征点对在每帧匹配帧图像内的特征点的数量,在所有的匹配帧图像中选择出最优匹配帧图像,并确定机器人使用窗口匹配方式跟踪成功,然后机器人会移除所述滑动窗口内部最早填入的参考帧图像,腾出内存空间,再将当前帧图像填入所述滑动窗口内以更新为新的一帧参考帧图像。在步骤S111中,在所有筛选出的匹配帧图像中,单帧匹配帧图像内的第二二特征点对应的对极约束误差的和值越小,则表示该帧匹配帧图像与当前帧图像的匹配程度越好,匹配误差越低;在所有筛选出的匹配帧图像中,单帧匹配帧图像内的第二二特征点的数量越多,则表示该帧匹配帧图像与当前帧图像的匹配程度越好,匹配点越多。
因此,在所述步骤S111中,所述基于对极约束误差值与第二特征点对在每帧匹配帧图像内的特征点的数量,在所有的匹配帧图像中选择出最优匹配帧图像的方法具体包括:在每帧匹配帧图像内,计算该帧匹配帧图像内的特征点所属的第二特征点对的对极约束误差值的和值,作为该帧匹配帧图像的极约束误差值累加值,使得每帧匹配帧图像配置一个极约束误差值累加值;其中,第二特征点对在匹配帧图像的特征点是所述第二二特征点,机器人将一帧匹配帧图像内最新标记出的每个第二二特征点所在的第二特征点对的对极约束误差值进行累加,得到该帧匹配帧图像内的特征点所属的第二特征点对的对极约束误差值的和值。在每帧匹配帧图像内,统计该帧匹配帧图像内组成第二特征点对的特征点的数量,作为该帧匹配帧图像的特征点匹配数量,使得每帧匹配帧图像配置一个特征点匹配数量;第二特征点对在匹配帧图像的特征点是所述第二二特征点时,该帧匹配帧图像内组成第二特征点对的特征点的数量是该帧匹配帧图像内存在的第二二特征点的数量。然后机器人将极约束误差值累加值(针对所有帧匹配帧图像而言)最大、且特征点匹配数量(针对所有帧匹配帧图像而言)最大的匹配帧图像设置为最优匹配帧图像。综上,步骤S102至步骤S111的组合是所述窗口匹配方式,机器人执行步骤S102至步骤S111的过程是机器人使用窗口匹配方式进行图像跟踪的过程。
作为一种实施例,对于步骤S102或步骤S107,在惯性数据的基础上,利用对极约束误差值,从当前帧图像的特征点和滑动窗口内所有的参考帧图像的特征点中筛选出第一特征点对的方法包括:机器人计算每个特征点对的对极约束误差值;当机器人计算出的一个特征点对的对极约束误差值大于或等于预设像素距离阈值时,将该特征点对标记为错误匹配点对,则相应一对特征点不能作为后续步骤的匹配对象;其中,机器人将预设像素距离阈值设置为3个像素点跨度的距离,比如位置相邻的3个像素点(一个像素点为中心,该中心的左右邻域中的像素点以及该中心组成位置相邻的3个像素点)所形成的距离,可以等效于同一行或同一列的3个像素点形成的2个像素间距;当机器人计算出的一个特征点对的对极约束误差值小于预设像素距离阈值时,将该特征点对标记为第一特征点对并确定筛选出第一特征点对,则机器人从当前帧图像的特征点和滑动窗口内所有的参考帧图像的特征点中筛选出第一特征点对;需要说明的是,一个特征点对的对极约束误差值越小,即当前帧图像的一个特征点和滑动窗口内的参考帧图像的一个特征点在对极约束下产生的对极约束误差值越小,这一对特征点之间匹配误差越小。
在本实施例中,每个特征点对是配置为由当前帧图像的一个特征点(任一个特征点)和参考帧图像的一个特征点(任一个特征点)组成,不能是同一帧参考帧图像中的一对特征点、不同帧参考帧图像之间的特征点、或者当前帧图像中的一对特征点组成;所述当前帧图像的每个特征点都与滑动窗口内的每个参考帧图像中的每个特征点都组成特征点对,从而在当前帧图像和滑动窗口内的所有参考帧图像之间实现暴力匹配。机器人会控制所述当前帧图像的每个特征点的归一化平面坐标都与滑动窗口内的每个参考帧图像中的每个特征点的归一化平面坐标依次计算出对应特征点对的对极约束误差值,然后每当计算出的一个特征点对的对极约束误差值大于或等于预设像素距离阈值,则过滤该特征点对,否则将该特征点对标记为第一特征点对;在遍历完所有所述特征点对后,机器人从当前帧图像的特征点和滑动窗口内所有的参考帧图像的特征点中筛选出所有的第一特征点对,完成当前帧每个特征点与参考帧的所有特征点的匹配,获得初步过滤出来的特征点对,并去除部分不满足误差值的特征点对的干扰。
需要说明的是,摄像头的刚体运动和机器人的运动一致,先后采集到的两帧图像会存在两种坐标系下的表达形式,包括当前帧图像相对于参考帧图像、参考帧图像相对于当前帧图像;摄像头先后采集的两帧图像中各点存在着某种几何关系,这种关系可以用对极几何来描述。对极几何描述了两帧图像中各像素的射影关系(或者说是各匹配点的几何关系),在一些实施例中,其与外部的场景本身无关,只与相机的内参以及两图像的拍摄位置有关。在理想情况下,所述对极约束误差值等于数值0,但是由于噪声的存在所述对极约束误差值必然不为数值0,这个非零数可以用于衡量参考帧图像的特征点与当前帧图像的特征点匹配误差的大小。
在一些实施例中,记R为从C1坐标系到C0坐标系的旋转矩阵,可以表示第k帧图像到第k+1帧图像的旋转;向量C0-C1就是光心C1相对于光心C0的平移,记为T,R和T可以表示两帧图像间机器人的运动,由惯性传感器给出,可以包含在惯性数据中,会存在两种坐标系下的表达形式,包括当前帧相对于参考帧、以及参考帧相对于当前帧。C0、C1分别是机器人的两个运动位置中摄像头的光心,也就是针孔相机模型中的针孔;Q是空间中的一个三维点,Q0和Q1分别是Q点在不同成像平面上对应的像素点;Q0和Q1都是图像上的二维点,本实施例把它们都设置为三维的方向向量来处理;假设一个归一化的图像平面,该平面上焦距f=1,因此可以定义在以光心C0为原点的坐标系下,Q0在以光心C0为原点的参考坐标系,Q1在以光心C1为原点的参考坐标系,所以还需要转换坐标系。这里把所有点的坐标都转换到以C0为原点的坐标系。由于方向向量和向量起始位置无关,所以这里对Q0和Q1的坐标系变换只考虑旋转就可以。这里的归一化的图像平面是C0-C1-Q0-Q1组成的极平面。
作为一种实施例,在步骤S102或步骤S107中,当惯性数据包括当前帧图像相对于参考帧图像的平移向量、以及当前帧图像相对于参考帧图像的旋转矩阵时,机器人将当前帧图像相对于参考帧图像的平移向量记为第一平移向量,并将当前帧图像相对于参考帧图像的旋转矩阵记为第一旋转矩阵,其中,第一平移向量表示从当前帧图像的坐标系到参考帧图像的坐标系的平移向量,第一旋转矩阵表示从当前帧图像的坐标系到参考帧图像的坐标系的旋转矩阵,使得惯性数据选择在参考帧图像的坐标系下表示位移信息和角度信息;在此基础上,机器人使用第一旋转矩阵将当前帧图像的一个特征点(可以拓展为任意一个特征点)的归一化平面坐标转换到参考帧图像的坐标系下,得到第一一坐标,其中,该特征点的归一化平面坐标在当前帧图像的坐标系下使用方向向量表示,只考虑方向向量的方向,而不考虑方向向量的起点或终点的向量,且是形成列向量,存在逆向量;则当前帧图像的所有特征点的坐标系转换只需要旋转即可,第一一坐标在参考帧图像的坐标系可以使用方向向量表示;因此,本实施例将当前帧图像的特征点的归一化向量设置为当前帧图像的特征点的归一化平面坐标相对于当前帧图像的坐标系的原点形成的向量;并将参考帧图像的特征点的归一化向量设置为参考帧图像的特征点的归一化平面坐标相对于参考帧图像的坐标系的原点形成的向量。则机器人控制第一旋转矩阵将当前帧图像的特征点的归一化向量转换到参考帧图像的坐标系下,得到第一一向量;再控制第一平移向量叉乘第一一向量,得到第一二向量,其中,第一二向量同时垂直于第一平移向量、以及第一一向量;然后控制滑动窗口内的参考帧图像中的特征点的归一化向量与第一二向量点乘,再将点乘的结果(表示参考帧图像中的特征点的归一化向量与第一二向量的夹角的余弦值)设置为对应特征点对的对极约束误差值;具体地,机器人会控制滑动窗口内的每帧参考帧图像中的每个特征点的归一化向量依次与第一二向量进行点乘,再将每次点乘的结果设置为对应特征点对的对极约束误差值。
需要说明的是,当前帧图像的特征点的归一化向量是当前帧图像的特征点的归一化平面坐标(向量的终点)相对于当前帧图像的坐标系的原点(向量的起点)形成的向量;参考帧图像的特征点的归一化向量是参考帧图像的特征点的归一化平面坐标(向量的终点)相对于参考帧图像的坐标系的原点形成的向量(向量的起点)。
作为一种实施例,在步骤S102或步骤S107中,当惯性数据包括参考帧图像相对于当前帧图像的平移向量、以及参考帧图像相对于当前帧图像的旋转矩阵时,机器人将参考帧图像相对于当前帧图像的平移向量标记为第二平移向量,并将参考帧图像相对于当前帧图像的旋转矩阵标记为第二旋转矩阵,其中,第二平移向量表示从参考帧图像的坐标系到当前帧图像的坐标系的平移向量,第二旋转矩阵表示从参考帧图像的坐标系到当前帧图像的坐标系的旋转矩阵,使得惯性数据选择在当前帧图像的坐标系下表示位移信息和角度信息;然后,机器人控制第二旋转矩阵将滑动窗口内的参考帧图像的特征点的归一化向量转换到当前帧图像的坐标系下,得到第二一向量;再控制第二平移向量叉乘第二一向量,得到第二二向量,其中,第二二向量同时垂直于第二平移向量以及第二一向量;然后控制当前帧图像中的特征点的归一化向量与第二二向量点乘,再将点乘的结果(表示当前帧图像中的特征点的归一化向量与第二二向量的夹角的余弦值)设置为对应特征点对的对极约束误差值;具体地,机器人会控制当前帧图像中的每个特征点的归一化向量依次与第一二向量进行点乘,再将每次点乘的结果设置为对应特征点对的对极约束误差值。使得对极约束误差值能够从几何维度去描述摄像头在不同视角下采集的图像帧之间的特征点匹配误差信息。
作为一种实施例,在步骤S103或步骤S108中,在所述惯性数据的基础上,利用特征点的深度值,从第一特征点对中筛选出第二特征点对的方法包括:机器人计算已筛选出的第一特征点对(可以是所述步骤S102或所述步骤S107筛选出的)在当前帧图像中的特征点的深度值与该第一特征点对在参考帧图像中的特征点的深度值的比值;若每个第一特征点对都是由当前帧图像中的第一特征点和参考帧图像中的第一特征点组成,则当前帧图像中的第一特征点的深度值与参考帧图像中的对应第一特征点的深度值的比值被记录下来,用于进行阈值比较以过滤部分比值不匹配的第一特征点所在的第一特征点对。当机器人计算出的第一特征点对在当前帧图像中的特征点的深度值与该第一特征点对在参考帧图像中的特征点的深度值的比值处于预设比值阈值范围内时,将该第一特征点对标记为第二特征点对并确定筛选出第二特征点对;优选地,预设比值阈值范围是设置为大于0.5且小于1.5。当机器人计算出的第一特征点对在当前帧图像中的特征点的深度值与该第一特征点对在参考帧图像中的特征点的深度值的比值没有处于预设比值阈值范围内时,将该第一特征点对标记为错误匹配点对,从而从所述步骤S102和所述步骤S107筛选出的第一特征点对中排除掉所述错误匹配点对,执行对第一特征点对的过滤,可以在后续特征点匹配时缩小特征点对的搜索范围。
作为一种实施例,在步骤S103或步骤S108中,机器人计算特征点的深度值的方法包括:当惯性数据包括当前帧图像相对于参考帧图像的平移向量、以及当前帧图像相对于参考帧图像的旋转矩阵时,机器人将当前帧图像相对于参考帧图像的平移向量记为第一平移向量,并将当前帧图像相对于参考帧图像的旋转矩阵记为第一旋转矩阵,其中,第一平移向量表示从当前帧图像的坐标系到参考帧图像的坐标系的平移向量,第一旋转矩阵表示从当前帧图像的坐标系到参考帧图像的坐标系的旋转矩阵;然后,机器人控制第一旋转矩阵将第一特征点对在当前帧图像中的特征点的归一化向量转换到参考帧图像的坐标系下,得到第一一向量;再控制该第一特征点对在参考帧图像中的特征点的归一化向量叉乘第一一向量,得到第一二向量;同时控制该第一特征点对在参考帧图像中的特征点的归一化向量叉乘第一平移向量,再对叉乘结果取反,得到第一三向量,其中,该第一特征点对在参考帧图像中的特征点的归一化向量与第一平移向量的叉乘结果是同时垂直于归一化向量与第一平移向量的向量,则该向量的相反向量是第一三向量;然后将第一三向量与第一二向量的逆向量的乘积设置为该第一特征点对在当前帧图像中的特征点的深度值,并标记为第一深度值,表示摄像头探测的三维点与摄像头采集到当前帧图像时的光心(当前帧图像的坐标系的原点)之间的距离;然后将第一一向量与第一深度值的乘积与第一平移向量的和值标记为第一四向量,然后将第一四向量与第一特征点对在参考帧图像中的特征点的归一化向量的逆向量的乘积设置为该第一特征点对在参考帧图像中的特征点的深度值,相当于将第一四向量与归一化向量的逆向量的乘积设置为参考帧图像中的特征点的深度值,并标记为第二深度值,表示同一三维点与摄像头采集到参考帧图像时的光心(参考帧图像的坐标系的原点)之间的距离。从而基于摄像头在两帧图像之间的位姿变换信息,三角化计算出一对特征点的深度信息。
作为一种实施例,在步骤S103或步骤S108中,机器人计算特征点的深度值的方法包括:当惯性数据包括参考帧图像相对于当前帧图像的平移向量、以及参考帧图像相对于当前帧图像的旋转矩阵时,机器人将参考帧图像相对于当前帧图像的平移向量记为第二平移向量,将参考帧图像相对于当前帧图像的旋转矩阵记为第二旋转矩阵,其中,第二平移向量表示从参考帧图像的坐标系到当前帧图像的坐标系的平移向量,第二旋转矩阵表示从参考帧图像的坐标系到当前帧图像的坐标系的旋转矩阵;然后,机器人控制第二旋转矩阵将第一特征点对在参考帧图像中的特征点的归一化向量转换到当前帧图像的坐标系下,得到第二一向量;再控制该第一特征点对在当前帧图像中的特征点的归一化向量叉乘第二一向量,得到第二二向量;同时控制该第一特征点对在当前帧图像中的特征点的归一化向量叉乘第二平移向量,再对叉乘结果取反,得到第二三向量;然后将第二三向量与第二二向量的逆向量的乘积设置为该第一特征点对在参考帧图像中的特征点的深度值,并标记为第二深度值,表示摄像头探测的三维点与摄像头采集到参考帧图像时的光心之间的距离;然后将第二一向量与第二深度值的乘积与第二平移向量的和值标记为第二四向量,然后将第二四向量与第一特征点对在当前帧图像中的特征点的归一化向量的逆向量的乘积设置为该第一特征点对在当前帧图像中的特征点的深度值,并标记为第一深度值,表示同一三维点与摄像头采集到当前帧图像时的光心之间的距离。
综合步骤S103或步骤S108,前述计算深度值的实施例基于同一个点投影到两帧不同视角的图像上时,该点在各帧图像中的投影点以及对应的光心形成的几何关系,并结合深度信息获得两帧图像的特征点在另一种尺度信息维度的匹配情况,从而提高特征点对匹配和图像跟踪的鲁棒性和准确性,使得机器人定位更可靠。
需要说明的是,第一特征点对在当前帧图像中的特征点的归一化向量是第一特征点对在当前帧图像中的特征点的归一化平面坐标相对于当前帧图像的坐标系的原点形成的向量;第一特征点对在参考帧图像中的特征点的归一化向量是第一特征点对在参考帧图像的特征点的归一化平面坐标相对于参考帧图像的坐标系的原点形成的向量。在一些实施例中,所述步骤S102或所述步骤S107筛选出的第一特征点对的数量较多,则需要通过最小二乘法获取一批匹配程度较高的第一特征点对,再进行特征点的深度值的求解;由于步骤S103或步骤S108是初步筛选,对于精确度要求不高,所以最小二乘法的使用不是必要的。
在一些实施例中,第一特征点对在当前帧图像中的特征点标记为P1,摄像头采集到当前帧图像时的光心被标记为O1,第一特征点对在参考帧图像中的特征点标记为P2,摄像头采集到参考帧图像时的光心被标记为O2,O1-O2-P1-P2组成极平面,极平面与当前帧图像的交线成为当前帧图像的成像平面的极线且经过P1,极平面与参考帧图像的交线成为参考帧图像的成像平面的极线且经过P2。则在不考虑像素点的噪声影响时,直线O1P1与直线O2P2相交于点P3,线段O1P3的长度是特征点P1的深度值,线段O2P3的长度是特征点P2的深度值;在考虑像素点的噪声影响范围时,直线O1P1与直线O2P2的交点是点P0,点P0不是点P3,则点P0与点P3的位置偏差可以用于衡量匹配误差,因此,需要设置所述预设比值阈值范围对特征点对之间的深度值的比值进行比较。
作为一种实施例,在步骤S104中,所述根据第二特征点对所对应的描述子的相似度,从第二特征点对中筛选出第三特征点对的方法具体包括:对于当前帧图像与所述滑动窗口内对应参考帧图像,可以认为是针对当前帧图像与所述滑动窗口内每帧参考帧图像之间被标记出的第二特征点对,机器人计算第二特征点对在参考帧图像中的特征点的描述子与该第二特征点对在当前帧图像中的特征点的描述子之间的相似度,可以理解为计算每个第二特征点对在参考帧图像中的特征点的描述子与该第二特征点对在当前帧图像中的特征点的描述子之间的相似度,也等同于所述滑动窗口内每帧参考帧图像的帧描述子与当前帧图像的帧描述子之间的相似度。然后,当机器人计算出的一个第二特征点对在参考帧图像中的特征点的描述子与该第二特征点对在当前帧图像中的特征点的描述子之间的相似度是当前帧图像的描述子与该第二特征点对的特征点所在的参考帧图像的描述子之间的相似度当中的最小值时,将该第二特征点对标记为第三特征点对并确定筛选出第三特征点对;其中,该第二特征点对的特征点所在的参考帧图像的描述子是该第二特征点对的特征点所在的参考帧图像内所有组成第二特征点对的特征点的描述子,即同一帧参考帧图像内存在多个关于第二特征点对的描述子;当前帧图像的描述子是当前帧图像内,与该第二特征点对的特征点所在的参考帧图像的特征点组成第二特征点对的特征点的描述子,即同一当前帧图像内存在多个关于该第二特征点对的描述子;优选地,第二特征点对所对应的描述子的相似度,使用当前帧图像中特征点的描述子和滑动窗口内对应的参考帧图像中特征点的描述子之间的欧式距离或汉明距离表示,从而可以利用多个维度的欧式距离或的平方和计算匹配点的描述子之间的相似度,再将距离最小的作为较为准确的待匹配点。
具体地,在步骤S104中,机器人将第二特征点对在当前帧图像中的特征点记为第二一特征点,该第二特征点对在参考帧图像中的特征点记为第二二特征点;机器人需计算该参考帧图像中所有第二二特征点的描述子与其对应的第二一特征点的描述子之间的相似度。然后,当存在一个第二待匹配特征点对在参考帧图像中的特征点的描述子与该第二待匹配特征点对在当前帧图像中的特征点的描述子之间的相似度是该参考帧图像中所有第二二特征点的描述子与其对应的第二一特征点的描述子之间的相似度当中的最小值时,将该第二特征点对标记为第三特征点对并确定筛选出第三特征点对,其中,每帧参考帧图像与当前帧图像之间可以被筛选出多对第三特征点对;一个第二特征点对在参考帧图像中的特征点的描述子与该第二特征点对在当前帧图像中的特征点的描述子之间的相似度是,第二二特征点的描述子与第二一特征点的描述子之间的相似度,作为两种描述子的相似性度量,具体计算方式可以表示为:第二二特征点与第二一特征点在多种维度下的欧式距离或汉明距离的平方和的平方根,其中,每一种维度可以表示特征点的一种二进制编码形式。
作为一种实施例,针对所述步骤S105或所述步骤S109,机器人将摄像头采集到当前帧图像时的光心与预设特征点对在当前帧图像内的特征点的连线标记为第一观测线,并将摄像头采集到参考帧图像时的光心与同一个预设特征点对在参考帧图像内的特征点的连线标记为第二观测线,然后在不考虑误差的环境条件下,将第一观测线与第二观测线的交点标记为目标探测点;其中,摄像头采集到当前帧图像时的光心、摄像头采集到参考帧图像时的光心、以及目标探测点都在同一平面,即形成三点共面的状态,然后将所述同一平面设置为所述极平面;或者,该预设特征点对、摄像头采集到当前帧图像时的光心、以及摄像头采集到参考帧图像时的光心都在同一平面,即形成四点共面的状态。机器人将极平面与当前帧图像的交线记为当前帧图像的成像平面中(在一些实施例中可视为当前帧图像的坐标系下)的极线,将极平面与参考帧图像的交线记为参考帧图像的成像平面中(在一些实施例中可视为参考帧图像的坐标系下)的极线。具体地,在同一个预设特征点对中,由当前帧图像的特征点转换到参考帧图像后,变为第一投影点,其坐标为第一一坐标;将第一投影点到参考帧图像的成像平面(参考帧图像的坐标系下)中的极线的距离表示为第一残差值,需要说明的是,在不考虑像素噪声的前提下,第一投影点是位于参考帧图像的成像平面(当前帧图像的坐标系下)中的极线上,即从参考帧图像的视角实际观测到的来自当前帧图像中的线段,在经过坐标系转换后能够与参考帧图像的成像平面中的极线重合;在同一个预设特征点对中,由参考帧图像的特征点转换到当前帧图像后,变为第二投影点,其坐标为第二一坐标;将第二投影点到当前帧图像的成像平面中的极线的距离表示为第二残差值。当第一残差值或第二残差值越小,则相应投影点相对于其所转换到的成像平面中的极线偏差越小,则对应的预设特征点对匹配程度越高。
需要说明的是,在步骤S105中,预设特征点对是所述第三特征点对,来自步骤S104筛选出来的特征点对;在步骤S109中,每当重复执行步骤S107和步骤S108后,预设特征点对是最新执行的步骤S108所筛选出的第二特征点对。第一特征点对、第二特征点对以及第三特征点对,都是由一个位于当前帧图像中的特征点和一个位于参考帧图像中的特征点组成的一对特征点。预设特征点对在当前帧图像中的特征点的归一化向量是预设特征点对在当前帧图像中的特征点的归一化平面坐标相对于当前帧图像的坐标系的原点形成的向量;预设特征点对在参考帧图像中的特征点的归一化向量是预设特征点对在参考帧图像的特征点的归一化平面坐标相对于参考帧图像的坐标系的原点形成的向量。其中,归一化平面坐标可以属于所述极平面中的坐标,使得预设特征点对在当前帧图像中的特征点的坐标和预设特征点对在参考帧图像中的特征点的坐标都归一化到所述极平面中表示。当然对于其余类型的特征点对也可以作相应的坐标归一化处理。
作为一种实施例,所述步骤S105或所述步骤S109中,在预先筛选出的特征点对中引入残差的方式,具体包括:当惯性数据包括当前帧图像相对于参考帧图像的平移向量、以及当前帧图像相对于参考帧图像的旋转矩阵时,机器人将当前帧图像相对于参考帧图像的平移向量记为第一平移向量,将当前帧图像相对于参考帧图像的旋转矩阵记为第一旋转矩阵;在步骤S105中,预设特征点对是所述第三特征点对,来自步骤S104筛选出来的特征点对;在步骤S109中,每当重复执行步骤S107和步骤S108后,预设特征点对是最新执行的步骤S108所筛选出的第二特征点对。机器人控制第一旋转矩阵将预设特征点对在当前帧图像中的特征点的归一化向量转换到参考帧图像的坐标系下,得到第一一向量;在本实施例中,预设特征点对在当前帧图像中的特征点的归一化向量使用方向向量表示,只考虑方向向量的方向,而不考虑方向向量的起点或终点的向量,且是形成列向量,存在逆向量;则当前帧图像的所有特征点的坐标系转换只需要旋转即可,第一一向量在参考帧图像的坐标系可以使用方向向量表示。机器人再控制第一平移向量叉乘第一一向量,得到第一二向量,并形成参考帧图像的成像平面中的极线,第一二向量作为三维方向向量,第一二向量的指向与极线平行,该极线是一个预设特征点对、当前帧图像对应的光心、以及参考帧图像对应的光心所构成的极平面与参考帧图像的成像平面的交线。然后对第一二向量中的横轴坐标和该第一二向量中的纵轴坐标的平方和求平方根,得到极线的模长,在一些实施例中可以视为极线的长度;同时,机器人控制该预设特征点对在参考帧图像中的特征点的归一化向量与第一二向量点乘,再将点乘的结果设置为该预设特征点对的对极约束误差值;然后将该预设特征点对的对极约束误差值与极线的模长的比值设置为第一残差值,并确定在该预设特征点对之间引入残差的结果数值。
在上述实施例的基础上,所述在预设特征点对之间引入残差,再结合残差及其对惯性数据的求导结果,计算出惯性补偿值,再使用惯性补偿值对惯性数据进行修正的方法具体包括:当惯性数据包括当前帧图像相对于参考帧图像的平移向量、以及当前帧图像相对于参考帧图像的旋转矩阵时,机器人将第一旋转矩阵与预设特征点对在当前帧图像中的特征点的归一化平面坐标相乘的算式标记为第一一转换式;再将第一平移向量与第一一转换式相叉乘的算式标记为第一二转换式;再将预设特征点对在参考帧图像中的特征点的归一化平面坐标与第一二转换式相点乘的算式标记为第一三转换式;再将第一二转换式的计算结果置为数值0,构成直线方程,再对该直线方程在横轴坐标维度的系数和纵轴坐标维度的系数求取平方和,再对求取得到的平方和计算平方根,得到第一平方根,在一些实施例中等效于该直线方程表示的直线在参考帧图像下的成像平面中的投影长度;再将第一平方根的倒数与第一三转换式相乘的算式设置为第一四转换式;然后将第一四转换式的计算结果设置为第一残差值,形成第一残差推导式,并确定在该预设特征点对之间引入残差;然后控制第一残差推导式分别对第一平移向量和第一旋转矩阵求偏导,获得雅克比矩阵,这里的雅克比矩阵是残差对第一平移向量的偏导结果和误差值对第一旋转矩阵的偏导结果的组合,达到纠正惯性数据变化影响的效果。然后将雅克比矩阵的逆矩阵与第一残差值乘积设置为惯性补偿值,从而实现构建最小二乘问题来寻找最优的惯性补偿值,在本实施例中,第一残差推导式是相当于:为了拟合惯性数据的补偿值,而设置的点到线的偏离误差值的拟合函数模型,其中的残差属于误差信息,比如最小二乘法条件下的最小误差平方和,使得第一残差推导式或直线方程成为求解最小误差平方和的表达式;然后对第一残差推导式分别对第一平移向量和第一旋转矩阵两种参数求偏导,得到的公式中可以整理为:将雅克比矩阵与拟合出的惯性数据的补偿值(惯性补偿值)相乘的结果设置为等于第一残差值,则机器人将雅克比矩阵的逆矩阵与第一残差值乘积设置为惯性补偿值,从而完成构建最小二乘问题来寻找最优的惯性补偿值。然后机器人使用惯性补偿值对惯性数据进行修正,具体的修正包括对原始惯性数据进行加、减、乘以及除运算,可以是简单的系数乘除、也可以是矩阵向量的相乘。
作为一种实施例,所述步骤S105或所述步骤S109中,在预先筛选出的特征点对中引入残差的方式,具体包括:当惯性数据包括参考帧图像相对于当前帧图像的平移向量、以及参考帧图像相对于当前帧图像的旋转矩阵时,机器人将参考帧图像相对于当前帧图像的平移向量记为第二平移向量,将参考帧图像相对于当前帧图像的旋转矩阵记为第二旋转矩阵;在步骤S105中,预设特征点对是所述第三特征点对,来自步骤S104筛选出来的特征点对;在步骤S109中,每当重复执行步骤S107和步骤S108后,预设特征点对是最新执行的步骤S108所筛选出的第二特征点对。机器人控制第二旋转矩阵将预设特征点对在参考帧图像中的特征点的归一化向量转换到当前帧图像的坐标系下,得到第二一向量,在本实施例中,预设特征点对在参考帧图像中的特征点的归一化向量使用方向向量表示,只考虑方向向量的方向,而不考虑方向向量的起点或终点的向量,且是形成列向量,存在逆向量;则当前帧图像的所有特征点的坐标系转换只需要旋转即可,第一一向量在当前帧图像的坐标系可以使用方向向量表示;而且所述预设特征点对是针对所述步骤S105,也可以更新为所述步骤S109中的第二特征点对。然后机器人控制第二平移向量叉乘第二一向量,得到第二二向量,并形成当前帧图像的成像平面中的极线,第二二向量作为三维方向向量,第二二向量的指向与极线平行,该极线是一个预设特征点对、当前帧图像对应的光心、以及参考帧图像对应的光心所构成的极平面与当前帧图像的成像平面的交线。然后对第二二向量中的横轴坐标和该第二二向量中的纵轴坐标的平方和求平方根,得到极线的模长,在一些实施例中可以视为第二二向量指向的直线在当前帧图像的成像平面中的投影长度;同时,机器人控制该预设特征点对在当前帧图像中的特征点的归一化向量与第二二向量点乘,再将点乘的结果设置为该预设特征点对的对极约束误差值;然后将该预设特征点对的对极约束误差值与极线的模长的比值设置为第二残差值,并确定在该预设特征点对之间引入残差的结果数值。
在上述实施例的基础上,所述在预设特征点对之间引入残差,再结合残差及其对惯性数据的求导结果,计算出惯性补偿值,再使用惯性补偿值对惯性数据进行修正的方法具体包括:当惯性数据包括参考帧图像相对于当前帧图像的平移向量、以及参考帧图像相对于当前帧图像的旋转矩阵时,机器人将第二旋转矩阵与预设特征点对在参考帧图像中的特征点的归一化平面坐标相乘的算式标记为第二一转换式;再将第二平移向量与第二一转换式相叉乘的算式标记为第二二转换式;再将预设特征点对在当前帧图像中的特征点的归一化平面坐标与第二二转换式相点乘的算式标记为第二三转换式;再将第二二转换式的计算结果置为数值0,构成直线方程,再对该直线方程在横轴坐标维度的系数和纵轴坐标维度的系数求取平方和,再将求取得到的平方和计算平方根,得到第二平方根,在一些实施例中等效于该直线方程表示的直线在当前帧图像下的成像平面中的投影长度;再将第二平方根的倒数与第二三转换式相乘的算式设置为第二四转换式;然后将第二四转换式的计算结果设置为第二残差值,形成第二残差推导式,并确定在该预设特征点对之间引入残差;然后控制第二残差推导式分别对第二平移向量和第二旋转矩阵求偏导,获得雅克比矩阵,这里的雅克比矩阵是残差对第二平移向量的偏导结果和误差值对第二旋转矩阵的偏导结果的组合,达到纠正惯性数据变化影响的效果。然后将雅克比矩阵的逆矩阵与第二残差值乘积设置为惯性补偿值,从而实现构建最小二乘问题来寻找最优的惯性补偿值,在本实施例中,第二残差推导式是相当于:为了拟合惯性数据的补偿值,而设置的拟合函数模型,其中的残差属于误差信息,比如最小二乘法条件下的最小误差平方和,使得直线方程或第二残差推导式成为求解最小误差平方和的表达式;然后对第一残差推导式分别对第一平移向量和第一旋转矩阵两种参数求偏导,得到的公式中可以整理为:将雅克比矩阵与拟合出的惯性数据的补偿值(惯性补偿值)相乘的结果设置为等于第一残差值,则机器人将雅克比矩阵的逆矩阵与第一残差值乘积设置为惯性补偿值,从而完成构建最小二乘问题来寻找最优的惯性补偿值。然后机器人使用惯性补偿值对惯性数据进行修正,具体的修正包括对原始惯性数据进行加、减、乘以及除运算,可以是简单的系数乘除、也可以是矩阵向量的相乘。使得原始的惯性数据经受视觉特征点对的迭代匹配的考验后,得到能够进行修正的偏导信息,让惯性数据得到优化处理,提高机器人的定位精度。
作为一种实施例,机器人使用投影匹配方式进行图像跟踪的方法包括:步骤21,机器人通过摄像头采集图像,并通过惯性传感器获取惯性数据;其中,摄像头先后采集的图像依次上一帧图像和当前帧图像,标记为相邻两帧图像。然后执行步骤22。摄像头采集到每帧图像后,会从每帧图像中获取特征点,其中,特征点指的是在所述机器人所处的环境中,以点的形式存在的环境元素,以便于与在先采集的图像的特征点进行匹配,可以实现上一帧图像跟踪当前帧图像。需要说明的是,相邻两帧(连续两帧图像)之间的摄像头的位姿变换关系中,涉及的旋转矩阵的初始状态量和平移向量的初始状态量是预先设定;在这些初始状态量的基础上,机器人依靠码盘在摄像头先后采集的两帧图像之间感测的位移变化量、以及陀螺仪在摄像头先后采集的两帧图像之间感测的角度变化量,进行积分处理,可以是使用欧拉积分分别对位移变化量和角度变化量进行积分处理,得到机器人在规定的采集时间间隔内的位姿变换关系以及位姿变化量,进而可以获得最新的旋转矩阵和最新的平移向量。
步骤22,机器人根据惯性数据将上一帧图像的特征点投影到当前帧图像内,得到投影点;然后执行步骤23。其中,所述惯性数据还包括上一帧图像相对于当前帧图像的旋转矩阵、以及上一帧图像相对于当前帧图像的平移向量;机器人控制所述旋转矩阵和所述平移向量将上一帧图像中的特征点转换到当前帧图像的坐标系中,然后通过摄像头的内参投影到当前帧图像的成像平面,得到所述投影点。其中,控制所述旋转矩阵和所述平移向量将上一帧图像中的特征点转换到当前帧图像的坐标系中的过程中,可以是直接对上一帧图像中的特征点的坐标进行旋转和平移操作,使该特征点的坐标转换到当前帧图像的坐标系中;也可以是:先构建上一帧图像中的特征点相对于上一帧图像的坐标系原点的向量,记为待转换向量;然后控制所述旋转矩阵乘以待转换向量,得到已旋转向量;然后使用的平移向量与已旋转向量作点乘运算,得到已转换向量;该已转换向量的起点是当前帧图像的坐标系的原点时,则该已转换向量的终点是所述投影点;其中,上一帧图像中的特征点可以是归一化平面坐标,但所述待转换向量、已旋转向量、或已转换向量可以是三维向量。从而形成两帧图像之间的姿态变换约束关系(极几何约束)。
步骤23,机器人根据描述子之间的标准距离,分别在每个投影点的预设搜索邻域内搜索待匹配点,其中,待匹配点是来源于当前帧图像内的特征点,待匹配点不属于投影点;然后机器人计算投影点与搜索出的每个待匹配点之间的向量,以确定投影点与所属的预设搜索邻域内搜索出的每个待匹配点之间的向量的方向,且机器人将预设搜索邻域内参与计算的一个投影点与一个待匹配点之间的向量标记为待匹配向量,是由投影点指向待匹配点的向量,由投影点指向待匹配点的指向是待匹配向量的方向;待匹配向量的计算方式是用当前帧图像内的待匹配点的归一化平面坐标,与上一帧图像投影到当前帧图像而得到的投影点的归一化平面坐标相减,产生待匹配向量;在一些实施例中,投影点是待匹配向量的起点,待匹配点是待匹配向量的终点;在一些实施例中,在当前帧图像中可以使用投影点与待匹配点的连线段表示该待匹配向量的模,并将投影点与待匹配点的连线段标记为待匹配线段;该向量的模长等于投影点与待匹配点的直线距离;然后机器人执行步骤24。
具体地,所述机器人根据描述子之间的标准距离,在每个投影点的预设搜索邻域内搜索待匹配点的方法包括:机器人以每个投影点为圆心点设置圆形邻域,并将该圆形邻域设置为该投影点的预设搜索邻域,其中,惯性数据包括上一帧图像与当前帧图像之间的摄像头的位姿变化量,可以等同于规定的采集时间间隔内的机器人的位姿变化量,摄像头固定装配在机器人上;上一帧图像与当前帧图像之间的摄像头的位姿变化量越大,则将所述预设搜索邻域的半径设置得越大;上一帧图像与当前帧图像之间的摄像头的位姿变化量越小,则将所述预设搜索邻域的半径设置得越小,使得特征点的匹配范围适应摄像头实际采集范围。其中,每个投影点为圆心点对应设置一个所述预设搜索邻域。然后,机器人在每个投影点的预设搜索邻域内,从该投影点的预设搜索邻域的圆心点开始搜索,具体是在该预设搜索邻域内搜索除了该投影点之外的特征点(原属于当前帧图像内的特征点);每当机器人搜索到的当前帧图像内的特征点的描述子与该预设搜索邻域的圆心点的描述子的标准距离最近时,将搜索到的当前帧图像内的特征点设置为该预设搜索邻域内的待匹配点,作为当前帧图像中与所述投影点的匹配度较高的候选匹配点,其中,该预设搜索邻域内的待匹配点的数量可以是至少一个,也可以是0个,则在没有搜索到一个待匹配点时,需扩大预设搜索邻域的半径,以在更大的圆域范围内继续搜索。
需要说明的是,所述标准距离是用描述子之间的标准匹配条件下使用的欧式距离或汉明距离。一个特征点的描述子是二进制描述向量,由许多个0和1组成,这里的0和1编码特征点附近的两个像素亮度(比如m和n)的大小关系,如果m比n小,取1;反之取0。
步骤231,描述子的来源具体包括:选定以一个特征点为中心的正方形邻域,将该正方形邻域设置为描述子的区域;
步骤232,然后可以对该正方形邻域进行去噪出来,可以使用高斯核卷积来消除像素点噪声,因为描述子的随机性较强,对噪声较为敏感。
步骤233,以一定的随机化算法生成点对<m,n>,若像素点m的亮度小于像素点n的亮度,则编码为值1,否则编码为0
步骤234,重复步骤233若干次(如128次),得到一个128位的二进制编码,即该特征点的描述子。
优选地,特征点的选择方法包括:在图像中选取像素点r,假设它的亮度为Ir;然后设置一个阈值T0(比如,Ir的20%);然后以像素点r为中心,选取半径为3个像素点距离的圆上的16个像素点。假如选取的圆上有连续的9个点的亮度大于(Ir+T0)或小于(Ir-T0),那么像素点r可以被认为是特征点。
步骤24,机器人统计相互平行的待匹配向量的数量,具体是所有投影点的预设搜索邻域,也可以是当前帧图像或其成像平面内,毕竟待匹配向量一开始只是在投影点的预设搜索邻域内标记出来的,预设搜索邻域之外的区域不存在计数干扰因素;相互平行的每个待匹配向量中,任两个待匹配向量的指向(方向)相同或相反,需要说明的是,在上一帧图像与当前帧图像之间,若一个投影点和一个待匹配点正常匹配,则该投影点指向该待匹配点的待匹配向量的方向是平行于固定的一个预设映射方向,则所有正常匹配的特征点对所对应的待匹配向量是相互平行;其中,预设映射方向与所述惯性数据关联,尤其是由所述旋转矩阵决定其指向的角度特征。然后执行步骤25。
步骤25,判断相互平行的待匹配向量的数量大于或等于预设匹配数量,是则执行步骤26,否则执行步骤27。其中,相互平行的待匹配向量的数量是机器人在所有投影点的预设搜索邻域内或所述当前帧图像内的统计结果。
步骤26,确定机器人使用投影匹配方式跟踪成功,并确定机器人跟踪当前帧图像成功。具体地,当相互平行的待匹配向量的数量大于或等于预设匹配数量时,将相互平行的每个待匹配向量设置为目标匹配向量,等同于将方向相同的至少两个待匹配向量、以及方向相反的至少两个待匹配向量都设置为目标匹配向量,相应地,将相互平行或重合的待匹配线段都设置为目标匹配线段,并将目标匹配向量的起点及其终点设置为一对目标匹配点,相应地将目标匹配线段的两个端点设置为一对目标匹配点,都属于特征点。再将不与目标匹配向量平行的待匹配向量的起点和该待匹配向量的终点设置为一对误匹配点,相应地,将不与目标匹配线段平行且不与目标匹配线段重合的待匹配线段设置为误匹配线段,则将误匹配线段的两个端点设置为一对误匹配点。从而在所述步骤S23设置的每个投影点的预设搜索邻域内完成一次特征点的匹配,获得相对于投影点的描述子的标准距离最近的待匹配点,在所述预设搜索邻域内过滤掉每对误匹配点。
步骤27,机器人在所有投影点的预设搜索邻域内(或在当前帧图像内)统计到相互平行的待匹配向量的数量小于预设匹配数量,然后判断步骤23的重复执行次数是否达到所述预设扩展次数,是则停止扩大每个投影点的预设搜索邻域的覆盖范围,并确定机器人使用投影匹配方式跟踪失败;否则扩大每个投影点的预设搜索邻域的覆盖范围,得到扩大后的预设搜索邻域,并将扩大后的预设搜索邻域更新为步骤23所述的预设搜索邻域,然后执行步骤23。优选地,预设匹配数量设置为15,所述预设扩展次数设置为2,在一些实施例中,该预设搜索邻域内的待匹配点的数量可以是至少一个,也可以是0个,则在没有搜索到一个待匹配点时,需扩大预设搜索邻域的半径,再返回步骤23以在更大的圆域范围内继续搜索。其中,预设扩展次数与当前帧图像的尺寸大小以及预设扩展步长关联,在当前帧图像内,若统计到相互平行的待匹配向量的数量小于预设匹配数量,则需要设置预设扩展步长去扩大每个投影点的预设搜索邻域的覆盖范围,但受到当前帧图像的尺寸大小的约束,使得每个投影点的预设搜索邻域只能在有限次数内进行合理的覆盖范围扩展以让同一个投影点匹配上更多合理的(描述子的标准距离最近)待匹配点。
由于每次执行步骤23后,会执行至步骤24,且在步骤24中会利用向量方向一致性来去除误匹配点,即过滤掉所述预设搜索邻域内的每个误匹配向量,然后在重复执行步骤23时就不需计算所述误匹配向量,大幅度减少计算量。直至机器人重复执行步骤23的次数达到预设扩展次数后,若仍统计到所述当前帧图像内的相互平行的待匹配向量的数量小于预设匹配数量,则停止扩大每个投影点的预设搜索邻域的覆盖范围,并确定机器人使用投影匹配方式跟踪失败。
综上,步骤22至步骤27的组合是所述投影匹配方式,结合相邻两帧图像之间的机器人的位姿变化量、特征点的投影变换关系,在待跟踪的当前帧图像内识别方向一致的待匹配向量及统计其数量,确定机器人机器人使用投影匹配方式是否成功地完成图像跟踪,降低了特征的误匹配率,降低计算难度,还能在跟踪失败后切换入窗口匹配方式,进一步地缩小特征点匹配的搜索范围,提高机器人视觉定位的准确性和效率;另外本发明仅使用单个摄像头进行定位,设备简单成本低。
需要说明的是,在本申请所提供的几个实施例中,应该理解到,所揭露的技术内容,可通过其它的方式实现。其中,以上所描述的装置实施例仅仅是示意性的,例如所述单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个单元或组件可以结合或者可以集成到另一个系统,或一些特征可以忽略,或不执行。另一点,所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口,单元或模块的间接耦合或通信连接,可以是电性或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。另外,在本申请各个实施例中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、只读存储器(ROM,Read-Only Memory)、随机存取存储器(RAM,Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述仅是本申请的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本申请原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视为本申请的保护范围。
Claims (19)
1.一种机器人视觉跟踪方法,其特征在于,所述机器人视觉跟踪方法的执行主体是固定装配摄像头和惯性传感器的机器人;
所述机器人视觉跟踪方法包括:
机器人使用窗口匹配方式进行图像跟踪,当机器人使用窗口匹配方式跟踪成功时,机器人停止使用窗口匹配方式进行图像跟踪,然后机器人使用投影匹配方式进行图像跟踪;
然后,当机器人使用投影匹配方式跟踪失败时,机器人停止使用投影匹配方式进行图像跟踪,然后机器人使用窗口匹配方式进行图像跟踪。
2.根据权利要求1所述机器人视觉跟踪方法,其特征在于,所述机器人视觉跟踪方法还包括:当机器人使用窗口匹配方式跟踪失败时,机器人停止使用窗口匹配方式进行图像跟踪,机器人清空滑动窗口,再使用窗口匹配方式进行图像跟踪;
其中,图像跟踪用于表示在先采集的图像的特征点与当前帧图像的特征点之间的匹配;
其中,机器人使用窗口匹配方式跟踪成功后,将当前帧图像填入所述滑动窗口,以便于跟踪机器人实时采集的图像;
机器人使用投影匹配方式进行图像跟踪的过程中,若检测到当前帧图像与上一帧图像的时间间隔是超出预设时间阈值,则机器人停止使用投影匹配方式进行图像跟踪,转而使用窗口匹配方式进行图像跟踪;
其中,特征点是属于图像的像素点,特征点是在所述摄像头所处的环境中,以点的形式存在的环境元素。
3.根据权利要求2所述机器人视觉跟踪方法,其特征在于,机器人使用窗口匹配方式进行图像跟踪的方法包括:
步骤S11,机器人通过摄像头采集当前帧图像,并通过惯性传感器获取惯性数据;
步骤S12,在惯性数据的基础上,利用对极约束误差值,从当前帧图像的特征点和滑动窗口内所有的参考帧图像的特征点中筛选出第一特征点对;其中,滑动窗口被设置为填入预先采集的至少一帧图像;特征点是图像的像素点,特征点是在所述摄像头所处的环境中,以点的形式存在的环境元素;
步骤S13,在所述惯性数据的基础上,利用特征点的深度值,从第一特征点对中筛选出第二特征点对;
步骤S14,根据第二特征点对所对应的描述子的相似度,从第二特征点对中筛选出第三特征点对;
步骤S15,在第三特征点对之间引入残差,再结合残差及其对惯性数据的求导结果,计算出惯性补偿值,再使用惯性补偿值对惯性数据进行修正;然后将修正后的惯性数据更新为步骤S12所述惯性数据,并将步骤S14所述的第三特征点对在当前帧图像的特征点更新为步骤S12所述的当前帧图像的特征点,并将步骤S14所述的第三特征点对在参考帧图像的特征点更新为步骤S12所述的滑动窗口内所有的参考帧图像的特征点;
步骤S16,重复执行步骤S12和步骤S13,直至重复执行次数达到预设迭代匹配次数,再基于第二特征点对在每帧参考帧图像内的特征点的数量,从滑动窗口内的参考帧图像中筛选出匹配帧图像;其中,每当重复执行步骤S12和步骤S13后,机器人在最新执行的步骤S13所筛选出的第二特征点对之间引入残差,再结合该残差及其对最新获得的惯性数据求导的结果,计算出惯性补偿值,再使用惯性补偿值对最新获得的惯性数据进行修正,然后将修正后的惯性数据更新为步骤S12所述的惯性数据,并将最新执行的步骤S13所筛选出的第二特征点对所包括的特征点对应更新为步骤S12所述的当前帧图像的特征点和滑动窗口内所有的参考帧图像的特征点;
步骤S17,基于对极约束误差值与第二特征点对在每帧匹配帧图像内的特征点的数量,在所有的匹配帧图像中选择出最优匹配帧图像,并确定机器人使用窗口匹配方式跟踪成功;
其中,步骤S12、步骤S13、步骤S14、步骤S15、步骤S16和步骤S17的组合是所述窗口匹配方式。
4.根据权利要求3所述机器人视觉跟踪方法,其特征在于,在所述步骤S12中,在惯性数据的基础上,利用对极约束误差值,从当前帧图像的特征点和滑动窗口内所有的参考帧图像的特征点中筛选出第一特征点对的方法包括:
机器人计算特征点对的对极约束误差值;当特征点对的对极约束误差值大于或等于预设像素距离阈值时,将该特征点对标记为错误匹配点对;当特征点对的对极约束误差值小于预设像素距离阈值时,将该特征点对标记为第一特征点对并确定筛选出第一特征点对;
其中,每个特征点对是配置为由当前帧图像的一个特征点和参考帧图像的一个特征点组成,而且,所述当前帧图像的每个特征点都与滑动窗口内的每个参考帧图像中的每个特征点都组成特征点对。
5.根据权利要求4所述机器人视觉跟踪方法,其特征在于,在所述步骤S12中,当惯性数据包括当前帧图像相对于参考帧图像的平移向量、以及当前帧图像相对于参考帧图像的旋转矩阵时,机器人将当前帧图像相对于参考帧图像的平移向量标记为第一平移向量,并将当前帧图像相对于参考帧图像的旋转矩阵标记为第一旋转矩阵;然后,机器人控制第一旋转矩阵将当前帧图像的特征点的归一化向量转换到参考帧图像的坐标系下,得到第一一向量;再控制第一平移向量叉乘第一一向量,得到第一二向量;然后控制滑动窗口内的参考帧图像中的特征点的归一化向量与第一二向量点乘,再将点乘的结果设置为对应特征点对的对极约束误差值;
或者,在所述步骤S12中,当惯性数据包括参考帧图像相对于当前帧图像的平移向量、以及参考帧图像相对于当前帧图像的旋转矩阵时,机器人将参考帧图像相对于当前帧图像的平移向量标记为第二平移向量,并将参考帧图像相对于当前帧图像的旋转矩阵标记为第二旋转矩阵;然后,机器人控制第二旋转矩阵将滑动窗口内的参考帧图像的特征点的归一化平面向量转换到当前帧图像的坐标系下,得到第二一向量;再控制第二平移向量叉乘第二一向量,得到第二二向量;然后控制当前帧图像中的特征点的归一化平面向量与第二二向量点乘,再将点乘的结果设置为对应特征点对的对极约束误差值;
其中,当前帧图像的特征点的归一化向量是当前帧图像的特征点的归一化平面坐标相对于当前帧图像的坐标系的原点形成的向量;参考帧图像的特征点的归一化向量是参考帧图像的特征点的归一化平面坐标相对于参考帧图像的坐标系的原点形成的向量。
6.根据权利要求3所述机器人视觉跟踪方法,其特征在于,在所述步骤S13中,在所述惯性数据的基础上,利用特征点的深度值,从第一特征点对中筛选出第二特征点对的方法包括:
机器人计算所述步骤S12筛选出的第一特征点对在当前帧图像中的特征点的深度值与该第一特征点对在参考帧图像中的特征点的深度值的比值;
当第一特征点对在当前帧图像中的特征点的深度值与该第一特征点对在参考帧图像中的特征点的深度值的比值处于预设比值阈值范围内时,将该第一特征点对标记为第二特征点对并确定筛选出第二特征点对;
当第一特征点对在当前帧图像中的特征点的深度值与该第一特征点对在参考帧图像中的特征点的深度值的比值没有处于预设比值阈值范围内时,将该第一特征点对标记为错误匹配点对。
7.根据权利要求6所述机器人视觉跟踪方法,其特征在于,在所述步骤S13中,当惯性数据包括当前帧图像相对于参考帧图像的平移向量、以及当前帧图像相对于参考帧图像的旋转矩阵时,机器人将当前帧图像相对于参考帧图像的平移向量标记为第一平移向量,将当前帧图像相对于参考帧图像的旋转矩阵标记为第一旋转矩阵,机器人控制第一旋转矩阵将第一特征点对在当前帧图像中的特征点的归一化向量转换到参考帧图像的坐标系下,得到第一一向量;再控制该第一特征点对在参考帧图像中的特征点的归一化向量叉乘第一一向量,得到第一二向量;同时控制该第一特征点对在参考帧图像中的特征点的归一化向量叉乘第一平移向量,再对叉乘结果取反,得到第一三向量;然后将第一三向量与第一二向量的逆向量的乘积设置为该第一特征点对在当前帧图像中的特征点的深度值,并标记为第一深度值,表示摄像头探测的三维点与摄像头采集到当前帧图像时的光心之间的距离;然后将第一一向量与第一深度值的乘积与第一平移向量的和值标记为第一四向量,然后将第一四向量与第一特征点对在参考帧图像中的特征点的归一化向量的逆向量的乘积设置为该第一特征点对在参考帧图像中的特征点的深度值,并标记为第二深度值,表示同一三维点与摄像头采集到参考帧图像时的光心之间的距离;
或者,在所述步骤S13中,当惯性数据包括参考帧图像相对于当前帧图像的平移向量、以及参考帧图像相对于当前帧图像的旋转矩阵时,机器人将参考帧图像相对于当前帧图像的平移向量记为第二平移向量,将参考帧图像相对于当前帧图像的旋转矩阵记为第二旋转矩阵,机器人控制第二旋转矩阵将第一特征点对在参考帧图像中的特征点的归一化向量转换到当前帧图像的坐标系下,得到第二一向量;再控制该第一特征点对在当前帧图像中的特征点的归一化向量叉乘第二一向量,得到第二二向量;同时控制该第一特征点对在当前帧图像中的特征点的归一化向量叉乘第二平移向量,再对叉乘结果取反,得到第二三向量;然后将第二三向量与第二二向量的逆向量的乘积设置为该第一特征点对在参考帧图像中的特征点的深度值,并标记为第二深度值,表示摄像头探测的三维点与摄像头采集到参考帧图像时的光心之间的距离;然后将第二一向量与第二深度值的乘积与第二平移向量的和值标记为第二四向量,然后将第二四向量与第一特征点对在当前帧图像中的特征点的归一化向量的逆向量的乘积设置为该第一特征点对在当前帧图像中的特征点的深度值,并标记为第一深度值,表示同一三维点与摄像头采集到当前帧图像时的光心之间的距离;
其中,第一特征点对在当前帧图像中的特征点的归一化向量是第一特征点对在当前帧图像中的特征点的归一化平面坐标相对于当前帧图像的坐标系的原点形成的向量;第一特征点对在参考帧图像中的特征点的归一化向量是第一特征点对在参考帧图像的特征点的归一化平面坐标相对于参考帧图像的坐标系的原点形成的向量。
8.根据权利要求3所述机器人视觉跟踪方法,其特征在于,在所述步骤S14中,所述根据第二特征点对所对应的描述子的相似度,从第二特征点对中筛选出第三特征点对的方法包括:
对于当前帧图像与所述滑动窗口内的每帧参考帧图像,机器人计算第二特征点对在参考帧图像中的特征点的描述子与该第二特征点对在当前帧图像中的特征点的描述子之间的相似度;
当第二特征点对在参考帧图像中的特征点的描述子与该第二特征点对在当前帧图像中的特征点的描述子之间的相似度是当前帧图像的描述子与该第二特征点对的特征点所在的参考帧图像的描述子之间的相似度当中的最小值时,将该第二特征点对标记为第三特征点对并确定筛选出第三特征点对;
其中,该第二特征点对的特征点所在的参考帧图像的描述子是该第二特征点对的特征点所在的参考帧图像内所有组成第二特征点对的特征点的描述子;当前帧图像的描述子是当前帧图像内,与该第二特征点对的特征点所在的参考帧图像的特征点组成第二特征点对的特征点的描述子;
其中,第二特征点对所对应的描述子的相似度,使用当前帧图像中特征点的描述子和滑动窗口内对应的参考帧图像中特征点的描述子之间的欧式距离或汉明距离表示。
9.根据权利要求8所述机器人视觉跟踪方法,其特征在于,所述步骤S14还包括:每当机器人搜索完当前帧图像和所述滑动窗口内的一帧参考帧图像之间组成第二特征点对的所有特征点后,若机器人在该帧参考帧图像内统计到第三特征点对的数量小于或等于第一预设点数阈值,则确定当前帧图像和该帧参考帧图像匹配失败,并将该帧参考帧图像设置为误匹配参考帧图像;若机器人在该帧参考帧图像内统计到第三特征点对的数量大于第一预设点数阈值,则确定当前帧图像和该帧参考帧图像匹配成功;
其中,当机器人确定当前帧图像和所述滑动窗口内所有帧参考帧图像都匹配失败时,确定机器人使用窗口匹配方式跟踪失败,然后机器人将所述滑动窗口内的图像清空。
10.根据权利要求3所述机器人视觉跟踪方法,其特征在于,机器人将摄像头采集到当前帧图像时的光心与预设特征点对在当前帧图像内的特征点的连线标记为第一观测线,并将摄像头采集到参考帧图像时的光心与同一个预设特征点对在参考帧图像内的特征点的连线标记为第二观测线,然后将第一观测线与第二观测线的交点标记为目标探测点;
其中,该预设特征点对、摄像头采集到当前帧图像时的光心、以及摄像头采集到参考帧图像时的光心都处于同一平面;或者,摄像头采集到当前帧图像时的光心、摄像头采集到参考帧图像时的光心、以及目标探测点都在同一平面;所述同一平面是极平面;
机器人将极平面与当前帧图像的交线记为当前帧图像的成像平面中的极线,并将极平面与参考帧图像的交线记为参考帧图像的成像平面的极线;
在同一个预设特征点对中,由当前帧图像的特征点转换到参考帧图像后,变为第一投影点,其坐标为第一一坐标;将第一投影点到参考帧图像的成像平面中的极线的距离表示为第一残差值;在同一个预设特征点对中,由参考帧图像的特征点转换到当前帧图像后,变为第二投影点,其坐标为第二一坐标;将第二投影点到当前帧图像的成像平面中的极线的距离表示为第二残差值;
在步骤S15中,预设特征点对是第三特征点对;
在步骤S16中,每当重复执行步骤S12和步骤S13后,预设特征点对是最新执行的步骤S13所筛选出的第二特征点对。
11.根据权利要求10所述机器人视觉跟踪方法,其特征在于,在所述步骤S15或所述步骤S16中,引入残差的方法包括:
当惯性数据包括当前帧图像相对于参考帧图像的平移向量、以及当前帧图像相对于参考帧图像的旋转矩阵时,机器人将当前帧图像相对于参考帧图像的平移向量记为第一平移向量,将当前帧图像相对于参考帧图像的旋转矩阵记为第一旋转矩阵,机器人控制第一旋转矩阵将预设特征点对在当前帧图像中的特征点的归一化向量转换到参考帧图像的坐标系下,得到第一一向量;再控制第一平移向量叉乘第一一向量,得到第一二向量,并形成参考帧图像的成像平面中的极线;然后对第一二向量中的横轴坐标和该第一二向量中的纵轴坐标的平方和求平方根,得到极线的模长;同时,控制该预设特征点对在参考帧图像中的特征点的归一化向量与第一二向量点乘,再将点乘的结果设置为该预设特征点对的对极约束误差值;然后将预设特征点对的对极约束误差值与极线的模长的比值设置为第一残差值;
或者,当惯性数据包括参考帧图像相对于当前帧图像的平移向量、以及参考帧图像相对于当前帧图像的旋转矩阵时,机器人将参考帧图像相对于当前帧图像的平移向量记为第二平移向量,将参考帧图像相对于当前帧图像的旋转矩阵记为第二旋转矩阵,机器人控制第二旋转矩阵将预设特征点对在参考帧图像中的特征点的归一化向量转换到当前帧图像的坐标系下,得到第二一向量;再控制第二平移向量叉乘第二一向量,得到第二二向量,并形成当前帧图像的成像平面中的极线;然后对第二二向量中的横轴坐标和该第二二向量中的纵轴坐标的平方和求平方根,得到极线的模长;同时,控制该预设特征点对在当前帧图像中的特征点的归一化向量与第二二向量点乘,再将点乘的结果设置为该预设特征点对的对极约束误差值;然后将该预设特征点对的对极约束误差值与极线的模长的比值设置为第二残差值;
其中,预设特征点对在当前帧图像中的特征点的归一化向量是预设特征点对在当前帧图像中的特征点的归一化平面坐标相对于当前帧图像的坐标系的原点形成的向量;预设特征点对在参考帧图像中的特征点的归一化向量是预设特征点对在参考帧图像的特征点的归一化平面坐标相对于参考帧图像的坐标系的原点形成的向量。
12.根据权利要求11所述机器人视觉跟踪方法,其特征在于,在所述步骤S15或所述步骤S16中,在预设特征点对之间引入残差,再结合该残差及其对最新获得的惯性数据求导的结果,计算出惯性补偿值,再使用惯性补偿值对最新获得的惯性数据进行修正的方法包括:
当惯性数据包括当前帧图像相对于参考帧图像的平移向量、以及当前帧图像相对于参考帧图像的旋转矩阵时,机器人将第一旋转矩阵与预设特征点对在当前帧图像中的特征点的归一化向量相乘的算式标记为第一一转换式;再将第一平移向量与第一一转换式相叉乘的算式标记为第一二转换式;再将预设特征点对在参考帧图像中的特征点的归一化向量与第一二转换式相点乘的算式标记为第一三转换式;再将第一二转换式的计算结果置为数值0,构成直线方程,再对该直线方程在横轴坐标维度的系数和纵轴坐标维度的系数求取平方和,再对求取得到的平方和计算平方根,得到第一平方根,再将第一平方根的倒数与第一三转换式相乘的算式设置为第一四转换式;然后将第一四转换式的计算结果设置为第一残差值,形成第一残差推导式,并确定在该预设特征点对之间引入残差;然后控制第一残差推导式分别对第一平移向量和第一旋转矩阵求偏导,得到雅克比矩阵;然后将雅克比矩阵的逆矩阵与第一残差值乘积设置为惯性补偿值;然后机器人使用惯性补偿值对惯性数据进行修正;
或者,当惯性数据包括参考帧图像相对于当前帧图像的平移向量、以及参考帧图像相对于当前帧图像的旋转矩阵时,机器人将第二旋转矩阵与预设特征点对在参考帧图像中的特征点的归一化向量相乘的算式标记为第二一转换式;再将第二平移向量与第二一转换式相叉乘的算式标记为第二二转换式;再将预设特征点对在当前帧图像中的特征点的归一化向量与第二二转换式相点乘的算式标记为第二三转换式;再将第二二转换式的计算结果置为数值0,构成直线方程,再对该直线方程在横轴坐标维度的系数和纵轴坐标维度的系数求取平方和,再将求取得到的平方和计算平方根,得到第二平方根,再将第二平方根的倒数与第二三转换式相乘的算式设置为第二四转换式;然后将第二四转换式的计算结果设置为第二残差值,形成第二残差推导式,并确定在该预设特征点对之间引入残差;然后控制第二残差推导式分别对第二平移向量和第二旋转矩阵求偏导,获得雅克比矩阵;然后将雅克比矩阵的逆矩阵与第二残差值乘积设置为惯性补偿值;然后机器人使用惯性补偿值对惯性数据进行修正。
13.根据权利要求9所述机器人视觉跟踪方法,其特征在于,对于所述步骤S16,在机器人执行完所述步骤S15后,第一次重复执行步骤S12时,机器人计算除了误匹配参考帧图像之外的每个第三特征点对的对极约束误差值,其中,每个第三特征点对的对极约束误差值是由步骤S15修正过的惯性数据决定;当第三特征点对的对极约束误差值小于预设像素距离阈值时,将该第三特征点对更新为第一特征点对,并确定从第三特征点对当中筛选出新的第一特征点对;
第N次重复执行步骤S12时,机器人计算最新执行的步骤S13中筛选出的每个第二特征点对的对极约束误差值;当第二特征点对的对极约束误差值小于预设像素距离阈值时,将该第二特征点对更新为第一特征点对,并确定从所述步骤S13筛选出的所有第二特征点对当中筛选出新的第一特征点对;其中,N是设置为大于1且小于或等于所述预设迭代匹配次数。
14.根据权利要求6所述机器人视觉跟踪方法,其特征在于,在所述步骤S16中,所述基于第二特征点对在每帧参考帧图像内的特征点的数量,从滑动窗口内的参考帧图像中筛选出匹配帧图像的方法包括:
机器人分别在所述滑动窗口内的每帧参考帧图像中,统计第二特征点对在该帧参考帧图像中的特征点的数量;
若机器人在其中一帧参考帧图像内匹配出的第二特征点对的数量小于或等于第二预设点数阈值,则确定所述其中一帧参考帧图像与所述当前帧图像匹配失败;若机器人在其中一帧参考帧图像内匹配出的第二特征点对的数量大于第二预设点数阈值,则确定所述其中一帧参考帧图像与所述当前帧图像匹配成功,并将所述其中一帧参考帧图像设置为匹配帧图像;若机器人在每帧参考帧图像内匹配出的第二特征点对的数量都小于或等于第二预设点数阈值时,确定滑动窗口内的每帧参考帧图像都与所述当前帧图像匹配失败,则确定机器人使用窗口匹配方式跟踪失败。
15.根据权利要求5所述机器人视觉跟踪方法,其特征在于,在所述步骤S17中,所述基于对极约束误差值与第二特征点对在每帧匹配帧图像内的特征点的数量,在所有的匹配帧图像中选择出最优匹配帧图像的方法包括:
在每帧匹配帧图像内,计算该帧匹配帧图像内的特征点所属的第二特征点对的对极约束误差值的和值,作为该帧匹配帧图像的极约束误差值累加值;
在每帧匹配帧图像内,统计该帧匹配帧图像内组成第二特征点对的特征点的数量,作为该帧匹配帧图像的特征点匹配数量;
然后将极约束误差值累加值最小且特征点匹配数量最大的匹配帧图像设置为最优匹配帧图像。
16.根据权利要求2所述机器人视觉跟踪方法,其特征在于,机器人使用投影匹配方式进行图像跟踪的方法包括:
步骤S21,机器人通过摄像头采集图像,并通过惯性传感器获取惯性数据;其中,摄像头采集的图像包括上一帧图像和当前帧图像;
步骤S22,机器人利用惯性数据将上一帧图像的特征点投影到当前帧图像内,得到投影点,其中,惯性数据包括上一帧图像相对于当前帧图像的旋转矩阵、以及上一帧图像相对于当前帧图像的平移向量;
步骤S23,机器人根据描述子之间的标准距离,分别在每个投影点的预设搜索邻域内搜索待匹配点;然后机器人计算投影点与搜索出的每个待匹配点之间的向量,并将该投影点指向已搜索出的待匹配点的向量标记为待匹配向量;其中,待匹配点是来源于当前帧图像内的特征点,待匹配点不属于投影点;每个待匹配向量都与一个投影点和一个待匹配点相对应;
步骤S24,机器人统计相互平行的待匹配向量的数量,当统计到相互平行的待匹配向量的数量大于或等于预设匹配数量时,确定机器人使用投影匹配方式跟踪成功,并确定机器人跟踪当前帧图像成功。
17.根据权利要求16所述机器人视觉跟踪方法,其特征在于,机器人将相互平行的每个待匹配向量都设置为目标匹配向量,并在所有投影点的预设搜索邻域内将不与所述目标匹配向量平行的待匹配向量标记为误匹配向量,再将误匹配向量对应的一个投影点和该误匹配向量对应的一个待匹配点设置为一对误匹配点,将目标匹配向量对应的一个投影点和该目标匹配向量对应的一个待匹配点设置为一对目标匹配点;
其中,相互平行的每个待匹配向量的方向是相同或相反,且目标匹配向量保持与预设映射方向平行,预设映射方向与惯性数据关联。
18.根据权利要求16所述机器人视觉跟踪方法,其特征在于,步骤S24还包括:当统计到相互平行的待匹配向量的数量小于预设匹配数量时,机器人按照预设扩展步长扩大每个投影点的预设搜索邻域的覆盖范围,得到扩大后的预设搜索邻域,并将扩大后的预设搜索邻域更新为步骤S23所述的预设搜索邻域,然后执行步骤S23,直至机器人重复执行步骤S23的次数达到预设扩展次数,然后停止扩大每个投影点的预设搜索邻域的覆盖范围,并确定机器人使用投影匹配方式跟踪失败;
其中,步骤S22、步骤S23与步骤S24的组合是所述投影匹配方式。
19.根据权利要求16所述机器人视觉跟踪方法,其特征在于,所述机器人根据描述子之间的标准距离,在每个投影点的预设搜索邻域内搜索待匹配点的方法包括:
机器人以每个投影点为圆心点设置圆形邻域,并将该圆形邻域设置为该投影点的预设搜索邻域,其中,惯性数据包括上一帧图像与当前帧图像之间的摄像头的位姿变化量;上一帧图像与当前帧图像之间的摄像头的位姿变化量越大,则将所述预设搜索邻域的半径设置得越大;上一帧图像与当前帧图像之间的摄像头的位姿变化量越小,则将所述预设搜索邻域的半径设置得越小;
机器人在每个投影点的预设搜索邻域内,从该投影点的预设搜索邻域的圆心点开始搜索除了该投影点之外的特征点;当搜索到的当前帧图像内的特征点的描述子与该预设搜索邻域的圆心点的描述子的标准距离最近时,将搜索到的当前帧图像内的特征点设置为该预设搜索邻域内的待匹配点;
其中,所述标准距离使用欧式距离或汉明距离表示。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210638498.9A CN117237406A (zh) | 2022-06-08 | 2022-06-08 | 一种机器人视觉跟踪方法 |
PCT/CN2023/094403 WO2023236733A1 (zh) | 2022-06-08 | 2023-05-16 | 一种机器人视觉跟踪方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210638498.9A CN117237406A (zh) | 2022-06-08 | 2022-06-08 | 一种机器人视觉跟踪方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN117237406A true CN117237406A (zh) | 2023-12-15 |
Family
ID=89091714
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210638498.9A Pending CN117237406A (zh) | 2022-06-08 | 2022-06-08 | 一种机器人视觉跟踪方法 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN117237406A (zh) |
WO (1) | WO2023236733A1 (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117934571B (zh) * | 2024-03-21 | 2024-06-07 | 广州市艾索技术有限公司 | 一种4k高清的kvm坐席管理系统 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9373036B1 (en) * | 2015-01-16 | 2016-06-21 | Toyota Motor Engineering & Manufacturing North America, Inc. | Collaborative distance metric learning for method and apparatus visual tracking |
CN105957109A (zh) * | 2016-04-29 | 2016-09-21 | 北京博瑞爱飞科技发展有限公司 | 目标跟踪方法和装置 |
CN111127519B (zh) * | 2019-12-25 | 2024-03-12 | 中国电子科技集团公司信息科学研究院 | 一种双模型融合的目标跟踪控制系统及其方法 |
CN113674310B (zh) * | 2021-05-11 | 2024-04-26 | 华南理工大学 | 一种基于主动视觉感知的四旋翼无人机目标跟踪方法 |
-
2022
- 2022-06-08 CN CN202210638498.9A patent/CN117237406A/zh active Pending
-
2023
- 2023-05-16 WO PCT/CN2023/094403 patent/WO2023236733A1/zh unknown
Also Published As
Publication number | Publication date |
---|---|
WO2023236733A1 (zh) | 2023-12-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111561923B (zh) | 基于多传感器融合的slam制图方法、系统 | |
CN112197770B (zh) | 一种机器人的定位方法及其定位装置 | |
KR102427921B1 (ko) | 실시간 맵핑 및 로컬리제이션을 위한 장치 및 방법 | |
US9946264B2 (en) | Autonomous navigation using visual odometry | |
CN110555901B (zh) | 动静态场景的定位和建图方法、装置、设备和存储介质 | |
JP5832341B2 (ja) | 動画処理装置、動画処理方法および動画処理用のプログラム | |
CN111445526B (zh) | 一种图像帧之间位姿的估计方法、估计装置和存储介质 | |
Clipp et al. | Parallel, real-time visual SLAM | |
CN112219087A (zh) | 位姿预测方法、地图构建方法、可移动平台及存储介质 | |
CN110411476B (zh) | 视觉惯性里程计标定适配及评价方法和系统 | |
CN105469405A (zh) | 基于视觉测程的同时定位与地图构建方法 | |
CN114623817B (zh) | 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法 | |
CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
CN112734852A (zh) | 一种机器人建图方法、装置及计算设备 | |
US9651388B1 (en) | System and method for improved simultaneous localization and mapping | |
CN108519102B (zh) | 一种基于二次投影的双目视觉里程计算方法 | |
CN102156970A (zh) | 基于畸变直线斜率计算的鱼眼图像校正方法 | |
CN111210477A (zh) | 一种运动目标的定位方法及系统 | |
CN112183171A (zh) | 一种基于视觉信标建立信标地图方法、装置 | |
CN116222543B (zh) | 用于机器人环境感知的多传感器融合地图构建方法及系统 | |
CN116184430B (zh) | 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法 | |
CN116295412A (zh) | 一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法 | |
CN112541423A (zh) | 一种同步定位与地图构建方法和系统 | |
WO2023236733A1 (zh) | 一种机器人视觉跟踪方法 | |
CN113744308B (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 |