专利名称：基于rgb-d相机的视觉里程计方法
技术领域：
本发明属于车辆自主导航领域，具体地涉及ー种基于RGB-D相机的视觉里程计方法。
背景技术：
里程计在车辆导航定位过程中至关重要。视觉里程计是ー种依靠视觉信息测算车辆运动距离和方向的方法，解决了轮式里程计因车轮打滑造成的測量错误，不存在传感器精度降低或惯导漂移等因素造成的误差，是传统方法的有效补充。目前，已提出的视觉里程计方法都是基于单目或双目相机。视觉里程计依靠单目 或双目相机得到的图像序列，通过特征提取、特征匹配与跟踪和运动估计得出车体六个自由度更新(位置和姿态)。单目视觉里程计假定路面平坦，通过标定得到路面上点在世界坐标系下的坐标与相应的图像坐标之间的对应关系，再将车辆运动前后得到的两帧图像中相同的点匹配起来，从而利用运动估计算法求得车体运动參数。单目视觉里程计最大的局限在于只能处理位于ー个平面上的场景点，无法得到场景的三維信息，因此当路面存在起伏或凸出的部分时算法会失效。双目视觉里程计是建立在立体视觉基础上的，它通过双目立体摄像机获取立体图像序列，通过特征提取、特征点立体匹配、特征点跟踪匹配、坐标变换和运动估计等步骤求得车辆的运动数据。一般认为，双目立体系统能取得比单目系统更可靠，精确和方便的结果(Nister, Visual Odometry for Ground Venicle Applications, Journal of FieldRobotics, 2006)，然而双目视觉里程计往往设备较为复杂，安装不便，图像处理工作量大，现有的商业化双目立体相机也售价昂贵。
发明内容
本发明的目的是针对现有技术的不足，提供一种基于RGB-D相机的视觉里程计方法。本发明包括RGB-D相机、电脑主机和自主车辆；电脑主机安装在自主车辆内部，RGB-D相机固定在自主车辆外围顶端；RGB-D相机通过USB接ロ(或1394接ロ)与电脑主机相连。本发明解决其技术问题所采用的技术方案包括如下步骤
步骤(I).将RGB-D相机固定在自主车辆外围顶端，代替传统的单目或双目相机来感知
环境，并输出彩色图像和深度图像4。步骤(2).由于RGB-D相机中彩色相机和深度相机处于不同位置，因此，将给出的深度图像4与彩色图像z对齐。
深度图像ら)与彩色图像4对齐步骤如下
2-1.针孔相机成像模型的齐次坐标形式如下 m =(I)
其中A =[足Hlf表示ー个世界坐标系下的三维点，雨=是点设投影在ニ
维图像平面的点， Γ为针孔相机的内參数矩阵，为针孔相机的外參数矩阵ャ·カ针孔相机坐标系相对于世界坐标系的旋转矩阵，i为针孔相机坐标系相对于世界坐标系的平移向量，Ζ； Γ, 2是三维点i在世界坐标系下的坐标，& V是投影占在ニ维图像平面的坐标。2-2.对式(I)展开得:
免== KBlX -C1J -C公Z - Cj = KR(M1-C) (2)
其中Mt = IX,Y,I]7 ^t=-RC , C是平移向量￡的另外ー种表示方式，表示针孔相机
坐标系与世界坐标系的原点之间的位移“为针孔相机平移矩阵，R为针孔相机旋转矩阵。2-3.根据式(2)，以深度相机的坐标系作为世界坐标系，对彩色相机建立投影模型得
みc =D、(3)
以深度相机的坐标系作为世界坐标系，对深度相机建立投影模型得 mB = K3Mt3 (4)
由式(3)和(4)得出
= KcRd(K^md-Cd)或ち=- ) (5)
其中邊c·是以深度相机坐标系作为世界坐标系下的三维点投影在彩色相机ニ维图像
平面的点，·*1 是以深度相机坐标系作为世界坐标系下的三维点投影在深度相机ニ维图像
平面的点，Kc为彩色相机内參数矩阵，13为深度相机内參数矩阵，[ち: ]为彩色相机相
对于深度相机的外參数矩阵。Rn为彩色相机相坐标系对于深度相机坐标系的旋转矩阵
为彩色相机坐标系相对于深度相机坐标系的平移向量，Cs是ら的另外ー种表不方式，表不
彩色相机坐标系与深度相机坐标系的原点间的位移，表示ー个以深度相机坐标系作为世界坐标系下的三维点。2-4.根据式(4)算出深度图像4上每ー个像素点在以深度相机的坐标系作为世界坐标系下的三维坐标，同时根据式(5)，算出深度图像/s上每ー个像素点对应的彩色图像4的投影平面坐标，得到与彩色图像4对齐的深度图像，并将对齐后且包含三维坐标信息的深度图像记为4。步骤(3).剔除深度图像/i中无效和不稳定的区域，同时对彩色图像Zc进行图像平滑操作，得到准确可靠的ニ维RGB-D图像4_。步骤(4).以时间T为周期，均匀采集彩色图像/c和深度图像/^根据步骤⑵、步骤⑶将采集到的彩色图像4和深度图像4转换为ニ维RGB-D图像，从而得到在时间轴上连续的ニ维RGB-D图像序列{4_)。步骤(5).根据时间轴的先后顺序，依次选取ニ维RGB-D图像序列中连续的前后两帧图像/和/^，分别对这两帧 图像进行特征点的提取与描述，得到特征点集び”和ぴ8+1},所有特征点集里的特征点有相同的维数为正整数。步骤(6).对于特征点集び”中的每ー个特征点F，从特征点集び8中找到与特征点P的特征向量最邻近的特征点i7S+1 ;
若特征点广与特征点/^+1之间特征向量的距离小于阈值7 ，则记录特征点f与特征点产1为相匹配的一对特征点对，表示为m ；
若特征点Fs与特征点i K+1之间特征向量的距离大于等于阈值7 ,则特征点集びs+1)中没有与特征点i s相匹配的特征点；
特征点对表示了三维场景中同一个点在连续的前后两帧图像上的不同投影，通过步骤(6)可得到ー个特征点对集合i(F，Fs+1))。步骤(7).对得到特征点对集合((f，F+1)}进行筛选，获得较优的用于确定运动參数的三维点对。特征点对集合筛选步骤如下
7-1.对特征点对集合(Ρκ，グ+1)}进行多次随机取样，毎次从中随机抽出a对特征点对，根据这a对特征点对及其对应的估计刚体的运动參数的方法算出两帧图像间的运动參数，即旋转矩阵i 和平移向量 ，其中a为正整数。7-2.对于特征点对集合{(F'f+1)}中的每ー个特征点对(F1Jw1)，算出前帧图像1\獅中特征点f经旋转矩阵Λ和平移向量i后的三维坐标(ダ"1/ ;
若,ヤ与的三维距离小于阈值，则特征点F被划分为内点；
若(ァx+1y与F科1的三维距离大于等于阈值 ，则特征点r被划分为外点。7-3.经过b次随机取样试验后，找出内点数目最多的那次随机取样试验，其中b为正整数；将该次取样试验所取的特征点对，作为最终筛选的用于确定运动參数的三维点对；将该次取样试验所估计的运动參数，作为最终确定的运动參数。本发明与现有技术相比，具有的有益效果是
1)本发明设备简单，安装便利，成本较低；
2)本发明无需进行左右图像的立体匹配，图像处理工作量较小；3)本发明可获取精确的场景三维信息，运动估计结果精确可靠；
4)本发明拥有很强的灵活性，可根据不同需求选用不同的前后帧匹配算法和刚体运动估计方法。
图I是本发明的基本结构 图2是本发明的的工作流程图。
具体实施例方式下面将结合附图对本发明方法作进ー步说明。 如图I所示，本发明包括RGB-D相机、电脑主机和自主车辆三个部分；电脑主机安装在自主车辆内部，RGB-D相机固定在自主车辆顶端；RGB-D相机通过USB接ロ(或1394接ロ)与电脑主机相连。如图2所示，本发明方法具体步骤如下
步骤(I).将RGB-D相机固定在自主车辆外围顶端，代替传统的单目或双目相机来感知
环境，并输出彩色图像ん和深度图像。步骤(2).由于RGB-D相机中彩色相机和深度相机处于不同位置，它们各自所成的像也是从两个不同的视角对三维世界进行了投影，这样的数据不利于后期的处理，因此对
给出的深度图像ら与彩色图像4对齐。对齐的效果相当于在彩色相机的位置同时放置一
台深度相机，两者同时对三维场景进行成像，得到的ニ维图像每ー个象素既包含顔色信息，也包含深度信息。深度图像4与彩色图像/c对齐步骤如下
2-1.针孔相机成像模型的齐次坐标形式如下 m =(I)
其中愈=[U,2,If表示ー个世界坐标系下的三维点，尤为针孔相机的内參数矩阵，[5 为针孔相机的外參数矩阵，兩=[iんv，lf是点M投影在ニ维图像平面的坐标，& V是
投影点 在ニ维图像平面的坐标，ぶ；Γ, Z是三维点M在ー个世界坐标系下的坐标。2-2.对式(I)展开得
m = KR[lI-CjM= KR[X-C1J-C2,Z-C3f = KR(M1-C) (2)
其中jT = [iT,r，Zf ,,c是平移向量I的另外ー种表示方式，表示针孔相机
坐标系与世界坐标系的原点之间的位移， 为针孔相机平移矩阵力针孔相机旋转矩阵。2-3.根据式(2)，以深度相机的坐标系作为世界坐标系，对彩色相机建立投影模型得
も=[cも(め-ら)(3)
以深度相机的坐标系作为世界坐标系，对深度相机建立投影模型得 = KsM1s (4)
由式(3)和(4)得出
も=( -も)或も=Kc(RDKtmD - ) (5)
其中是以深度相机坐标系作为世界坐标系下的三维点投影在彩色相机ニ维图像平面的点，みD是以深度相机坐标系作为世界坐标系下的三维点投影在深度相机ニ维图像平面的点，C为彩色相机内參数矩阵，13为深度相机内參数矩阵，为彩色相机相对于深度相机的外參数矩阵。Rn为彩色相机相坐标系对于深度相机坐标系的旋转矩阵为彩色相机坐标系相对于深度相机坐标系的平移向量，ら是ら的另外ー种表不方式,表不彩色相机坐标系与深度相机坐标系的原点间的位移，表示ー个以深度相机坐标系作为世界坐标系下的三维点。
2-4.根据式(5)，将深度图像ら对齐到彩色图像4的投影平面上，获得对齐后的深度图像，同时根据式(4)算出对齐后的深度图像上每ー个像素点在世界坐标系下的三维坐标，将对齐后且包含三维坐标信息的深度图像记为4。步骤(3).剔除深度图像/:η中无效和不稳定的区域，同时对彩色图像4进行图像平滑操作，得到准确可靠的ニ维RGB-D图像4_。步骤(4).以时间T为周期，均匀采集彩色图像/c和深度图像ら，根据步骤⑵、步骤⑶将采集到的彩色图像4和深度图像4转换为ニ维RGB-D图像从而得到在时间轴上连续的ニ维RGB-D图像序列。步骤(5).选取ニ维RGB-D图像序列{4卿}中连续的前后两帧图像和!ニ
，分别对这两帧图像进行特征点的提取与描述，得到特征点集{ "}和，所有特征点集里的特征点有相同的维数力正整数。步骤(6).对于特征点集び"}中的每ー个特征点i へ从特征点集び"+1}找到与特征点i H的特征向量最邻近的特征点，记为FK+1 ；
若特征点尸与特征点FB+1之间特征向量的距离小于阈值1 ,则记录特征点Fm与特征点Fx+1为相匹配的ー对特征点对，表示为；
若特征点Fn与特征点i s+1之间特征向量的距离大于等于阈值”贝IJ特征点集ぴ8+1)中没有与特征点Fs相匹配的特征点；
特征点对(タ'/^+1)表示了三维场景中同一个点在连续的前后两帧图像上的不同投影，通过该步骤可以得到ー个特征点对集合{(F'f+1)}。步骤(7).刚体运动參数的估计最少需要两对连续的前后帧匹配的三维点对(常用估计方法有奇异值分解法、正交分解法、単位四元数法和基于ニ维特征点的“8点算法”)，通过步骤(6)得到的连续前后帧匹配的特征点对集合ダ1+1)}包含大量的三维点对，其
中也包含一些误匹配的三维点对，本步骤对得到特征点对集合((,，尸+1)}进行筛选，基于
随机抽样一致性(RANSAC)思想，获取较优的用于确定运动參数的三维点对，剔除掉存在的误匹配点。特征点对集合{(P,尸+1)}筛选步骤如下
7-1.对特征点对集合((,，尸+1)}进行多次随机取样，毎次从中随机抽出a对匹配点对，根据这a对匹配点对及其对应的估计刚体的运动參数的方法算出两帧图像间的运动參数，即旋转矩阵P和平移矩阵，其中a为正整数。
7-2.对于特征点对集合《F，f+1)}中的每ー个特征点对GWk+1)，算出前帧图中特征点Fs经旋转矩阵i 和平移矩阵 后的三维坐标(ァ"+1ア;
若与i +1的三维距离小于阈值TM，则特征点i 被划分为内点；
若与ド的三维距离大于等于阈值TM，则特征点F及被划分为外点。7-3.因为正确匹配点对所确定的模型逼近于真实的模型，大多数数据会成为内点，而错误的匹配点由于数据杂乱无章所确定的模型大部分数据会成为外点。经过b次随机取样试验后，找出内点数目最多的那次随机取样试验，其中b为正整数；将该次取样试所取的特征点对，作为最终筛选的用于确定运动參数的三维点对；将该次取样试验所估计的运动參数，作为最终确定的运动參数。
权利要求
1.基于RGB-D相机的视觉里程计方法，其特征在于包括如下步骤 步骤(I).将RGB-D相机固定在自主车辆外围顶端，代替传统的单目或双目相机来感知环境，并输出彩色图像4和深度图像^); 步骤(2).由于RGB-D相机中彩色相机和深度相机处于不同位置，因此，将给出的深度图像ら与彩色图像ら对齐； 深度图像4与彩色图像4对齐步骤如下 2-1.针孔相机成像模型的齐次坐标形式如下 m = K[Rt]M (I) 其中A =[足表示ー个世界坐标系下的三维点，麻=[av,if是点砬投影在ニ维图像平面的点，i力针孔相机的内參数矩阵，为针孔相机的外參数矩阵，P、为针孔相机坐标系相对于世界坐标系的旋转矩阵，i为针孔相机坐标系相对于世界坐标系的平移向量，I 7, Z是三维点it 在世界坐标系下的坐标，& V是投影占，在ニ维图像平面的坐标； 2-2.对式(I)展开得
全文摘要
本发明公开了一种基于RGB-D相机的视觉里程计方法。现有视觉里程计方法都是基于单目或双目相机，或无法得到场景的三维信息，或设备较为复杂安装不便。本发明包括RGB-D相机、电脑主机和自主车辆；电脑主机安装在自主车辆内部，RGB-D相机固定在自主车辆外围顶端；RGB-D相机通过USB接口(或1394接口)与电脑主机相连，本发明依靠RGB-D相机得到的对齐RGB-D图像序列，通过前后帧图像的特征提取、特征匹配与跟踪和运动估计得出车体的运动距离和方向；本发明设备简单，安装便利，成本较低，图像处理工作量较小，可获取精确的场景三维信息，运动估计结果精确可靠。
文档编号G01C22/00GK102692236SQ20121015142
公开日2012年9月26日 申请日期2012年5月16日 优先权日2012年5月16日
发明者刘济林, 曹腾, 龚小谨 申请人:浙江大学