技術(shù)領(lǐng)域：

本發(fā)明涉及機(jī)器人視覺定位與導(dǎo)航技術(shù)領(lǐng)域，具體是一種基于gpu-sift實(shí)時(shí)雙目視覺里程計(jì)系統(tǒng)。

背景技術(shù)：
：

隨著機(jī)器人和計(jì)算機(jī)視覺的不斷發(fā)展，相機(jī)被越來越多的用于機(jī)器人的視覺定位與導(dǎo)航之中。機(jī)器人定位主要是碼盤、聲吶、imu、gps、北斗、激光掃描儀、rgbd相機(jī)以及雙目相機(jī)進(jìn)行視覺定位等方法，其中碼盤根據(jù)電機(jī)轉(zhuǎn)動(dòng)的圈數(shù)換算成輪子轉(zhuǎn)動(dòng)的圈數(shù)進(jìn)而對(duì)機(jī)器人的行程進(jìn)行推導(dǎo)實(shí)現(xiàn)定位，但是這種定位方式在沙地、草地或者輪子打滑等情況下誤差較大，定位不準(zhǔn)。聲吶定位主要靠超聲傳感器發(fā)射與返回信號(hào)分析判斷障礙物進(jìn)行定位與導(dǎo)航，但是聲吶的分辨率較低，信號(hào)中存在較多的噪聲，容易對(duì)定位造成干擾。機(jī)器人采用imu進(jìn)行定位往往會(huì)存在累積誤差，機(jī)器人進(jìn)行長(zhǎng)時(shí)間長(zhǎng)距離的定位與導(dǎo)航過程中往往需要矯正才能實(shí)現(xiàn)準(zhǔn)確定位。采用gps或者北斗等衛(wèi)星進(jìn)行定位方式，精度往往很差，要獲得高精度定位往往成本較高難以實(shí)現(xiàn)，并且gps或者北斗定位往往僅適用于室外衛(wèi)星信號(hào)較好的環(huán)境之中，對(duì)于室內(nèi)定位或者衛(wèi)星信號(hào)較差的環(huán)境往往無能為力。激光掃描儀雖然具備高精度的在任何環(huán)境下的定位能力，但是其成本高昂，數(shù)據(jù)量大，處理復(fù)雜，并且功耗較大。目前較為常用的是采用單線的激光進(jìn)行定位，但是應(yīng)用環(huán)境比較受限，僅適用于平面環(huán)境，對(duì)于起伏地形環(huán)境無法使用。采用rgbd相機(jī)進(jìn)行定位雖然能獲得障礙物與圖像信息，但是由于紅外激光發(fā)射強(qiáng)度受環(huán)境限制，基本只適用于室內(nèi)環(huán)境，并且有效距離有限。采用普通單相機(jī)進(jìn)行定位只能實(shí)現(xiàn)相對(duì)定位，并且定位精度受到極大限制，然而采用平行雙目相機(jī)可進(jìn)行絕對(duì)定位，并且定位精度在一定的情況下可達(dá)到激光定位的精度，并且在光照允許的情況下在通常環(huán)境中均可使用，但是基于雙目相機(jī)的視覺定位數(shù)據(jù)計(jì)算復(fù)雜度高，計(jì)算量大，往往難以達(dá)到實(shí)時(shí)的定位要求。而為了達(dá)到實(shí)時(shí)的視覺定位效果，往往采用較為簡(jiǎn)單的圖像處理算法，尤其是在視覺里程計(jì)中。

視覺里程計(jì)是僅采用移動(dòng)車輛或者機(jī)器人上的相機(jī)所獲得的視覺信息來實(shí)現(xiàn)車輛或者機(jī)器人移動(dòng)的定位，即通過移動(dòng)車體或者機(jī)器人車載相機(jī)拍攝運(yùn)行過程中的周圍場(chǎng)景的圖像或者視頻中提取出車體或者機(jī)器人運(yùn)行的情況和運(yùn)行環(huán)境信息對(duì)移動(dòng)車體或者機(jī)器人進(jìn)行實(shí)時(shí)定位。為了實(shí)現(xiàn)實(shí)時(shí)的視覺里程計(jì)，基本上時(shí)間消耗都出現(xiàn)在圖像匹配部分，而而圖像匹配之中80％的時(shí)間消耗出現(xiàn)在特征提取及特征描述上，所以為了減少視覺里程計(jì)的時(shí)間消耗，基本上都是采用簡(jiǎn)單的局部特征和特征描述方法，來實(shí)現(xiàn)實(shí)時(shí)的視覺里程計(jì)定位功能。較為常用的有harris、fast、censure以及簡(jiǎn)單的邊緣特征點(diǎn)，但是這些簡(jiǎn)單的特征描述是難以實(shí)現(xiàn)尺度和旋轉(zhuǎn)不變性的，而這些情況往往在相機(jī)運(yùn)行過程中普遍存在的，所以這些簡(jiǎn)單的特征難以實(shí)現(xiàn)準(zhǔn)確的圖像匹配，繼而達(dá)到較高精度的視覺定位效果。而sift特征專為解決尺度和旋轉(zhuǎn)的不變性而設(shè)計(jì)，能夠很好的克服圖像的尺度和旋轉(zhuǎn)變化，實(shí)現(xiàn)準(zhǔn)確的圖像匹配，獲得較高精度的視覺定位效果。但是sift特征提取和描述時(shí)間消耗較大難以實(shí)現(xiàn)實(shí)時(shí)的圖像匹配，采用gpu對(duì)sift特征提取、描述和匹配過程進(jìn)行加速處理的gpu-sift能極大地加速sift特征匹配過程，實(shí)現(xiàn)實(shí)時(shí)的sift特征匹配。本發(fā)明采用gpu-sift配合雙目視覺定位，實(shí)現(xiàn)實(shí)時(shí)的視覺里程計(jì)系統(tǒng)，用于機(jī)器人的實(shí)時(shí)定位與導(dǎo)航。

技術(shù)實(shí)現(xiàn)要素：
：

本發(fā)明為了克服現(xiàn)有技術(shù)中存在的上述缺陷，提供了一種基于gpu-sift的實(shí)時(shí)雙目視覺定位方法，對(duì)sift特征匹配過程進(jìn)行加速，使之達(dá)到實(shí)時(shí)的匹配速度，配合雙目視覺定位，實(shí)現(xiàn)實(shí)時(shí)的視覺里程計(jì)系統(tǒng)，用于機(jī)器人或者移動(dòng)平臺(tái)的實(shí)時(shí)視覺定位與導(dǎo)航。

為解決上述問題，本發(fā)明提出的基于gpu-sift的實(shí)時(shí)雙目視覺定位方法，包括以下步驟：

步驟一、采用平行雙目相機(jī)獲取機(jī)器人或者移動(dòng)平臺(tái)移動(dòng)過程中的左右眼圖像的立體圖像視頻；

步驟二、采用特征點(diǎn)匹配的方法獲得運(yùn)動(dòng)過程中拍攝視頻的前后兩幀中對(duì)應(yīng)的匹配點(diǎn)；

步驟三、通過匹配點(diǎn)在成像空間坐標(biāo)變化或者建立三維坐標(biāo)來求解運(yùn)動(dòng)方程從而估計(jì)出相機(jī)的位移；

步驟四、獲得相機(jī)行進(jìn)的各個(gè)時(shí)刻的位置、旋轉(zhuǎn)角后，結(jié)合kalman濾波即可獲得整個(gè)過程中相機(jī)的行進(jìn)路線，即可實(shí)現(xiàn)對(duì)機(jī)器人或者移動(dòng)平臺(tái)的實(shí)時(shí)雙目視覺定位。

上述技術(shù)方案中，所述步驟二中特征點(diǎn)匹配采用gpu-sift特征匹配算法，gpu-sift是指采用圖像處理器進(jìn)行加速的尺度不變特征轉(zhuǎn)換。

上述技術(shù)方案中，所述步驟二中特征點(diǎn)匹配具體包括以下子步驟：

子步驟s21、提取兩幀雙目圖像左右四幅圖像的sift特征點(diǎn)，并對(duì)sift特征點(diǎn)生成sift特征描述；

子步驟s22、匹配第一幀左相機(jī)圖像和右相機(jī)圖像的sift特征點(diǎn)，得到立體匹配點(diǎn)(pl1，pr1)；

子步驟s23、匹配第二幀左相機(jī)圖像和右相機(jī)圖像的sift特征點(diǎn)，得到立體匹配點(diǎn)(pl2，pr2)；

子步驟s24、匹配第一幀左相機(jī)圖像和第二幀左相機(jī)圖像的sift特征點(diǎn)(ll1，ll2)；

子步驟s25、找出步驟s24中得到的第一幀左相機(jī)圖像匹配點(diǎn)ll1與子步驟s22中得到的第一幀左相機(jī)圖像匹配點(diǎn)pl1相同的特征點(diǎn)作為第一幀左相機(jī)圖像最終的匹配點(diǎn)；同理得到第二幀左相機(jī)圖像的匹配點(diǎn)；

子步驟s26、根據(jù)子步驟s25中得到的左相機(jī)圖像匹配點(diǎn)，由子步驟s22中的匹配點(diǎn)對(duì)找到對(duì)應(yīng)的右相機(jī)圖像匹配點(diǎn)；同理找到第二幀右相機(jī)圖像匹配點(diǎn)，即完成兩幀四幅圖像的匹配過程。

上述技術(shù)方案中，所述步驟三具體包括以下子步驟：

子步驟s31、建立一像空間輔助坐標(biāo)系，由得到的前后兩幀四幅圖像中的對(duì)應(yīng)匹配點(diǎn)，根據(jù)三角測(cè)量的方法通過公式計(jì)算同一時(shí)刻對(duì)應(yīng)匹配點(diǎn)在像空間輔助坐標(biāo)系下的三維坐標(biāo)點(diǎn)pi；

子步驟s32、將得到三維坐標(biāo)pi代入到運(yùn)動(dòng)方程pi＝rpi'+t中求解，得出左相機(jī)和右相機(jī)的自由度參數(shù)分別為t(tx,ty,tz)和r(rx,ry,rz)；

子步驟s33、采用ransac方法每次隨機(jī)選擇三個(gè)坐標(biāo)點(diǎn)pi，將所有點(diǎn)代入誤差公式中計(jì)算；

子步驟s34、統(tǒng)計(jì)e(r,t)值小于某一閾值的點(diǎn)的個(gè)數(shù)，經(jīng)過若干次選擇后取內(nèi)點(diǎn)數(shù)最多的那一組結(jié)果為最終的計(jì)算結(jié)果；

子步驟s35、將最終的計(jì)算結(jié)果代入到運(yùn)動(dòng)方程pi＝rpi'+t即得到相機(jī)的運(yùn)動(dòng)方程從而估計(jì)出相機(jī)的位移。

上述技術(shù)方案中，所述步驟四具體包括：將繪制點(diǎn)作為矢量點(diǎn)，矢量方向?yàn)榍耙粠男D(zhuǎn)角的累加和，得出下一點(diǎn)時(shí)在當(dāng)前點(diǎn)方向上平移t，確定其坐標(biāo)，旋轉(zhuǎn)角是前一幀的方向乘以旋轉(zhuǎn)矩陣r，根據(jù)公式確定具體的路徑反演中點(diǎn)，其中po為初始時(shí)刻相機(jī)在xoz平面的位置坐標(biāo)，設(shè)為(0,0)；pi為第i時(shí)刻相機(jī)在xoz平面的位置坐標(biāo)，ti是第i時(shí)刻在當(dāng)前點(diǎn)方向上的平移距離。

本發(fā)明與現(xiàn)有技術(shù)方案相比具有以下有益效果和優(yōu)點(diǎn)：

本發(fā)明提出的基于gpu-sift的實(shí)時(shí)雙目視覺定位方法采用gpu-sift對(duì)sift特征匹配過程進(jìn)行加速，使之達(dá)到實(shí)時(shí)的匹配速度，配合雙目視覺定位，能夠?qū)崿F(xiàn)機(jī)器人或者移動(dòng)平臺(tái)實(shí)時(shí)視覺定位，獲得較高的定位精度，可擴(kuò)展性和實(shí)用性強(qiáng)，環(huán)境適用性強(qiáng)。

附圖說明

圖1是本發(fā)明中像空間輔助坐標(biāo)系的示意圖。

圖2是本發(fā)明中三角測(cè)量原理圖。

圖中編號(hào)說明：1、左相機(jī)；2、右相機(jī)。

具體實(shí)施方式

以下結(jié)合附圖和具體實(shí)施例對(duì)本發(fā)明作進(jìn)一步的詳細(xì)描述：

本實(shí)施例中，本發(fā)明提出的基于gpu-sift的實(shí)時(shí)雙目視覺定位方法，包括以下步驟：

步驟一、采用平行雙目相機(jī)獲取機(jī)器人或者移動(dòng)平臺(tái)移動(dòng)過程中的左右眼圖像的立體圖像視頻；

步驟二、采用gpu-sift特征匹配算法獲得運(yùn)動(dòng)過程中拍攝視頻的前后兩幀中對(duì)應(yīng)的匹配點(diǎn)；

步驟三、通過匹配點(diǎn)在成像空間坐標(biāo)變化或者建立三維坐標(biāo)來求解運(yùn)動(dòng)方程從而估計(jì)出相機(jī)的位移；

步驟四、獲得相機(jī)行進(jìn)的各個(gè)時(shí)刻的位置、旋轉(zhuǎn)角后，結(jié)合kalman濾波即可獲得整個(gè)過程中相機(jī)的行進(jìn)路線，即可實(shí)現(xiàn)對(duì)機(jī)器人或者移動(dòng)平臺(tái)的實(shí)時(shí)雙目視覺定位。

步驟二中特征點(diǎn)匹配具體包括以下子步驟：

子步驟s21、提取兩幀雙目圖像左右四幅圖像的sift特征點(diǎn)，并對(duì)sift特征點(diǎn)生成sift特征描述；

子步驟s22、匹配第一幀左相機(jī)圖像和右相機(jī)圖像的sift特征點(diǎn)，得到立體匹配點(diǎn)(pl1，pr1)；

子步驟s23、匹配第二幀左相機(jī)圖像和右相機(jī)圖像的sift特征點(diǎn)，得到立體匹配點(diǎn)(pl2，pr2)；

子步驟s24、匹配第一幀左相機(jī)圖像和第二幀左相機(jī)圖像的sift特征點(diǎn)(ll1，ll2)；

子步驟s25、找出步驟s24中得到的第一幀左相機(jī)圖像匹配點(diǎn)ll1與子步驟s22中得到的第一幀左相機(jī)圖像匹配點(diǎn)pl1相同的特征點(diǎn)作為第一幀左相機(jī)圖像最終的匹配點(diǎn)；同理得到第二幀左相機(jī)圖像的匹配點(diǎn)；

子步驟s26、根據(jù)子步驟s25中得到的左相機(jī)圖像匹配點(diǎn)，由子步驟s22中的匹配點(diǎn)對(duì)找到對(duì)應(yīng)的右相機(jī)圖像匹配點(diǎn)；同理找到第二幀右相機(jī)圖像匹配點(diǎn)，即完成兩幀四幅圖像的匹配過程。

步驟三具體包括以下子步驟：

子步驟s31、采用一像空間輔助坐標(biāo)系，由得到的前后兩幀四幅圖像中的對(duì)應(yīng)匹配點(diǎn)，根據(jù)三角測(cè)量的方法計(jì)算同一時(shí)刻對(duì)應(yīng)匹配點(diǎn)在像空間輔助坐標(biāo)系下的三維坐標(biāo)點(diǎn)pi，其中采用的像空間輔助坐標(biāo)系s-xyz如圖1所示，以左相機(jī)的后側(cè)端面中心點(diǎn)為坐標(biāo)原點(diǎn)，x軸位于左相機(jī)和右相機(jī)的后側(cè)端面的兩個(gè)中心點(diǎn)連線上，z軸位于左相機(jī)的中心軸線上，三角測(cè)量的原理如圖2所示，由圖2中的三角形s1與s2的相似以及s1’與s2’的相似得出以下計(jì)算公式：

其中，(xl,yl)、(xr,yr)為同一幀左右圖像匹配點(diǎn)相對(duì)于圖像中心的坐標(biāo)，d為雙目相機(jī)的基線，f為相機(jī)焦距；

子步驟s32、將得到三維坐標(biāo)pi代入到運(yùn)動(dòng)方程pi＝rpi'+t中求解，得出左相機(jī)和右相機(jī)的自由度參數(shù)分別為t(tx,ty,tz)和r(rx,ry,rz)；

子步驟s33、采用ransac方法每次隨機(jī)選擇三個(gè)坐標(biāo)點(diǎn)pi，將所有點(diǎn)代入誤差公式中計(jì)算誤差值e(r,t)；

子步驟s34、統(tǒng)計(jì)e(r,t)值小于某一閾值的點(diǎn)的個(gè)數(shù)，經(jīng)過若干次選擇后取小于某一閾值的點(diǎn)的個(gè)數(shù)最多的那一組結(jié)果為最終的計(jì)算結(jié)果，這樣就在很大程度上避免了匹配誤差較大的點(diǎn)的干擾，提高了計(jì)算結(jié)果精度；

子步驟s35、將最終的計(jì)算結(jié)果代入到運(yùn)動(dòng)方程pi＝rpi'+t即得到相機(jī)的運(yùn)動(dòng)方程從而估計(jì)出相機(jī)的位移。

步驟四具體包括：將繪制點(diǎn)作為矢量點(diǎn)，矢量方向?yàn)榍耙粠男D(zhuǎn)角的累加和，得出下一點(diǎn)時(shí)在當(dāng)前點(diǎn)方向上平移t，確定其坐標(biāo)，旋轉(zhuǎn)角是前一幀的方向乘以旋轉(zhuǎn)矩陣r，根據(jù)公式確定具體的路徑反演中點(diǎn)，其中po為初始時(shí)刻相機(jī)在xoz平面的位置坐標(biāo)，設(shè)為(0,0)；pi為第i時(shí)刻相機(jī)在xoz平面的位置坐標(biāo)，ti是第i時(shí)刻在當(dāng)前點(diǎn)方向上的平移距離。

由于提取特征點(diǎn)精度的限制、車體行進(jìn)過程中的抖動(dòng)、場(chǎng)景光線的變化等因素造成運(yùn)動(dòng)估計(jì)結(jié)果存在誤差，或者計(jì)算得出的r、t存在偶然性的跳變誤差，造成最終繪制路徑的誤差較大、路徑不連續(xù)，所以一般運(yùn)動(dòng)估計(jì)得到每相鄰兩幀之間的運(yùn)動(dòng)估計(jì)后一般需要對(duì)運(yùn)動(dòng)估計(jì)結(jié)果進(jìn)行平滑處理，通常使用到的限制條件是車體運(yùn)動(dòng)過程中旋轉(zhuǎn)和平移速度或者加速度的連續(xù)性、限制性，將誤差較大的估計(jì)結(jié)果使用鄰域均值或者中值代替，較為復(fù)雜的可以使用卡爾曼濾波或者擴(kuò)展卡爾曼濾波進(jìn)行平滑，使得到的路徑連續(xù)平滑。本文中為了盡量減少整個(gè)過程的時(shí)間消耗，使用的是前者較為簡(jiǎn)單的濾波處理，即剔除誤差較大的r、t并用鄰域中值代替，平滑效果較為理想。

最后說明的是，以上實(shí)施例僅用以說明本發(fā)明的技術(shù)方案而非限制，盡管參照較佳實(shí)施例對(duì)本發(fā)明進(jìn)行了詳細(xì)說明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解，可以對(duì)本發(fā)明的技術(shù)方案進(jìn)行修改或者等同替換，而不脫離本發(fā)明技術(shù)方案的宗旨和范圍，其均應(yīng)涵蓋在本發(fā)明的權(quán)利要求范圍中。