一種基于sift算法的視覺導航方法
【專利摘要】本發明提供了一種基于SIFT算法的視覺導航方法，包括：第一步驟：利用機器人的導航相機獲取圖像；第二步驟：對于導航相機獲取的圖像基于SIFT方法提取自然路標的特征點；第三步驟：根據所提取的自然路標的特征點，利用動態擴展卡爾曼濾波器融合視覺信息以修正機器人的慣性導航系統的測量誤差，由此對機器人運行狀態和視覺特征的位置進行估計，從而獲得機器人的導航參數。
【專利說明】一種基于SIFT算法的視覺導航方法
【技術領域】
[0001]本發明涉及機器人視覺導航領域，更具體地說，本發明涉及一種基于SIFT算法的視覺導航方法。
【背景技術】
[0002]目前移動機器人通常采用的定位傳感器包括里程計、激光雷達、超聲波、紅外線、微波雷達、陀螺儀、指南針、速度或加速度計、觸覺或接近覺傳感器等。
[0003]另一方面，機器人通常采用的定位技術主要包括絕對定位和相對定位技術兩類。絕對定位采用導航信標、主動或被動標識、地圖匹配或全球定位(GPS)進行定位，定位精度較高，但成本較高，不適用于小型機器人。
[0004]相對定位是通過測量機器人相對于初始位置的距離和方向來確定機器人的當前位置，通常也稱為航位推算法，所用的傳感器主要為里程計、陀螺儀、加速度計等；其中，當采用航位推算法時，機器人的位置是自我推算出來的，因而不需要對外界環境的感知信息，但是，這種方法的漂移誤差會隨時間積累，不適于精確定位。
[0005]對于這些現有技術的定位方法，存在以下缺點:
[0006]⑴在視覺導航定位中，圖像處理計算量大，計算機實時處理的速度無法滿足現實中的需求。
[0007]⑵高精度定位與導航技術成本高，難以應用在中小型機器人中，而低成本的技術，精度低，誤差大，嚴重影響了導航效果。
[0008]由此，希望能夠提供一種用于機器人的定位精確高、微型化且低成本的定位技術。
【發明內容】
[0009]本發明所要解決的技術問題是針對現有技術中存在上述缺陷，提供一種基于SIFT的機器人視覺導航方法，從而實現機器人高定位精確、微型化及低成本的特點，具有理論與實用價值。
[0010]為了實現上述技術目的，根據本發明，提供了一種基于SIFT算法的視覺導航方法，其包括:
[0011]第一步驟:利用機器人的導航相機獲取圖像；
[0012]第二步驟:對于導航相機獲取的圖像基于SIFT方法提取自然路標的特征點；
[0013]第三步驟:根據所提取的自然路標的特征點，利用動態擴展卡爾曼濾波器融合視覺信息以修正機器人的慣性導航系統的測量誤差，由此對機器人運行狀態和視覺特征的位置進行估計，從而獲得機器人的導航參數。
[0014]優選地，所述第二步驟包括:
[0015]第一步，進行尺度空間極點檢測，其中通過不同尺度的高斯核函數對導航相機獲取的圖像進行連續濾波和下采樣，形成高斯金字塔圖像，然后再對相鄰尺度的兩個高斯圖像相減得到高斯偏差，以得到關鍵點；其中，關鍵點不僅是當前高斯偏差圖像局部相鄰的預定個數的像素的極大值或極小值，而且是前后相鄰尺度的高斯偏差領域的極大或極小值；
[0016]第二步，進行特征點定位，其中在極值點的局部領域用三維二次函數擬合方法來差值確定極值點的坐標；
[0017]第三步，進行主方位賦值，其中在構造SIFT描述算子之前，首先為每個關鍵點賦予一個主方向，其中主方向是指關鍵點領域內各點梯度方向的直方圖中最大值所對應的方向；
[0018]第四步，進行SIFT描述算子的建立，其中將坐標軸旋轉為關鍵點的方向，以確保任意一個關鍵點的旋轉不變性，而且在關鍵點所在的尺度空間，取以關鍵點為中心的16像素X 16像素大小的領域，再將此領域均勻地分為4X4的子區域，對每個子區域計算梯度方向直方圖，以構成一個128維的特征向量，再將特征向量的長度歸一化，從而獲得作為SIFT描述算子的向量 ；
[0019]第五步，在導航相機獲取的多幅圖像的SIFT特征點集中找到相應匹配特征點之間的對應關系。
[0020]優選地，所述第五步包括:
[0021]針對導航相機獲取的圖像與模板圖像的SIFT特征點，執行歐式距離匹配；
[0022]剔除歐式距離匹配中的誤匹配；
[0023]根據剔除了誤匹配的歐式距離匹配執行當前匹配圖像與模板圖像的仿射變換。
[0024]優選地，在第三步驟中，定義機器人方向角為(a，(6, 9 )，定義機器人中的陀螺的偏差為(bp, bq, br),定義視覺信息為(a v, P v, 0 v),則在將慣性導航系統狀態變量選擇為x=[a, P，0，bp, bq, br]T,并將觀測變量選擇為[a v, P v, 0 v]的情況下,建立慣性導航系統的狀態方程及觀測方程，并利用動態擴展卡爾曼濾波器對狀態方程及觀測方程進行遞推以估計機器人的位置信息。
【專利附圖】
【附圖說明】
[0025]結合附圖，并通過參考下面的詳細描述，將會更容易地對本發明有更完整的理解并且更容易地理解其伴隨的優點和特征，其中:
[0026]圖1示意性地示出了根據本發明優選實施例的基于SIFT算法的視覺導航方法的流程圖。
[0027]圖2示意性地示出了根據本發明優選實施例的基于SIFT算法的視覺導航方法。
[0028]圖3示意性地示出了根據本發明優選實施例的基于動態擴展卡爾曼濾波融合視覺信息與慣性導航參數的一種原理。
[0029]需要說明的是，附圖用于說明本發明，而非限制本發明。注意，表示結構的附圖可能并非按比例繪制。并且，附圖中，相同或者類似的元件標有相同或者類似的標號。
【具體實施方式】
[0030]為了使本發明的內容更加清楚和易懂，下面結合具體實施例和附圖對本發明的內容進行詳細描述。
[0031]本發明克服了上述現有技術的方法的不足，利用SIFT算法實現自然路標的特征提取與跟蹤，根據圖像序列中的自然路標位置信息確定機器人位置，并采用動態擴展卡爾曼濾波與慣性導航系統參數的數據融合，從而實現視覺導航信息對慣性導航系統誤差的修正，獲得精確的導航坐標，同時，達到微型化和低成本的特點，以提高在中小型機器人中的頭用性。
[0032]下面將更具體的描述本發明的具體實施例。
[0033]圖1示意性地示出了根據本發明優選實施例的基于SIFT (Scale-1nvariantfeature transform,尺度不變特征轉換)算法的視覺導航方法的流程圖。
[0034]具體地說，根據本發明優選實施例的基于SIFT算法的視覺導航方法包括:
[0035]第一步驟S1:利用機器人的導航攝像頭獲取圖像；例如，獲取的圖像是機器人經過的局部環境的圖像；而且，優選地，可以將帶有云臺的攝像頭安裝在機器人的正前方，由此可以隨時拍攝周圍的圖像。
[0036]第二步驟S2:對于導航相機獲取的圖像基于SIFT方法提取自然路標的特征點；
[0037]其中，對于第二步驟S2，可通過以下幾步完成:
[0038]第一步S10，進行尺度空間極點檢測，其中通過不同尺度的高斯核函數對導航相機獲取的圖像進行連續濾波和下采樣，形成高斯金字塔圖像，然后再對相鄰尺度的兩個高斯圖像相減得到DOG (高斯偏差)，以得到關鍵點；其中，關鍵點不僅是當前DOG圖像局部相鄰的預定個數的像素(相鄰8像素)的極大值或極小值，而且是前后相鄰尺度的DOG領域的極大或極小值。其中，“D0G領域”指的是經過高斯圖像相減后得到的一系列有高斯偏差的多個圖像。
[0039]第二步S20，進行特征點定位:在極值點的局部領域用三維二次函數擬合方法來差值確定極值點的坐標。
[0040]第三步S30，進行主方位賦值:在構造SIFT描述算子之前，首先為每個關鍵點賦予一個主方向，其中主方向是指關鍵點領域內各點梯度方向的直方圖中最大值所對應的方向。但是，在某些情況下，這個主導方位可能不唯一，除了直方圖中最大值(最高峰值)，其它所有大于最高峰值80%的方位值(是大于最高峰值的80%的其它值)都要記錄下來，后續的描述算子構造均以該方向為參考，這樣所構造的描述算子具有旋轉不變性。
[0041]第四步S40，進行SIFT描述算子的建立，其中將坐標軸旋轉為關鍵點的方向，以確保任意一個關鍵點的旋轉不變性，而且在關鍵點所在的尺度空間(即高斯金字塔結構的某一層)，取以關鍵點為中心的16像素X 16像素大小的領域，再將此領域均勻地分為4X4的子區域，對每個子區域計算梯度方向直方圖(直方圖均勻分為8個方向)，這樣就構成了一個4X4X8 = 128維的特征向量，再繼續將特征向量的長度歸一化，則可以進一步去除光照變化的影響。此向量就是SIFT描述算子。其中，SIFT描述算子是特征點的數學向量表示法。
[0042]第五步S50，在導航相機獲取的多幅圖像的SIFT特征點集中找到相應匹配特征點之間的對應關系。
[0043]具體地說，由于一幅圖像中SIFT特征點很多，不同的特征點對應于場景的不同的物體。當選中自然路標時，將所有屬于該路標的特征(自然路標的特征，換言之，這個特征就是其SIFT描述算子)作為模板特征點集，其它特征點將被拋棄。當獲取了某時刻采集圖像的SIFT特征點(自然路標的特征點就是用SIFT向量表示的，那些特征點也可以稱為SIFT特征點)后，需要找到其與上一時刻自然路標模板特征點集(數據庫)之間的匹配關系，也就是需要在導航相機獲取的多幅圖像的SIFT特征點集中找到相應匹配特征點之間的對應關系。具體過程如下:
[0044]①針對導航相機獲取的圖像與模板圖像(模板圖像是利用獲取的一幅圖像，從模板圖像數據庫里查詢出的與之可匹配的圖像)的SIFT特征點，執行歐式距離匹配:通常可以用某種距離來度量兩個描述算子的相似性，以此作為匹配依據，通常使用的是歐式距離。對于模板圖像中的某個關鍵點，找出其與待匹配圖像中歐式距離最近的前兩個關鍵點，在這兩個關鍵點中，如果最近的距離除以次近的距離少于某個比例閾值，則接受這一對匹配點。舍棄所有大于0.8的匹配，則可以剔除大約90%的錯誤匹配，而只拋棄了大約不到5%的正確匹配，當匹配點的數量達到一定的閾值，認為在當前圖像中存在數據庫中選定的自然路標，否則，繼續在數據庫中搜索。
[0045]②剔除歐式距離匹配中的誤匹配:歐式距離較小只能表征兩個描述算子高度相似，而不能保證匹配的正確性。可以設想待匹配圖像中的特征點區域是模板圖像中的某個區域通過一個圖像空間的相似變化轉化來的，即所有模板圖像中特征點是經過同一個平移運動、同一個旋轉、同一個尺度縮放而變為待匹配圖像中相匹配的特征點。對于不滿足大部分對應點構成的相似變換的點可以認為是誤匹配。
[0046]③根據剔除了誤匹配的歐式距離匹配執行當前匹配圖像與模板圖像的仿射變換:當攝像機移動時，對于圖像中屬于同一自然路標的特征點應當滿足同一個運動模型。因此，設想當前匹配圖像中的目標區域是模板圖像中的目標區域通過一個更細膩的仿射變換運動得到的，通過求解這個仿射變換，既可以求得一個比較精確的圖像運動變換，以后還可以通過這個圖像運動變換求得模板圖像中目標體上的其它元素在當前匹配圖像中的位置。
[0047]第三步驟S3:根據所提取的自然路標的特征點，利用動態擴展卡爾曼濾波器融合自然路標特征匹配的結果 信息以修正機器人的慣性導航系統的測量誤差，由此對機器人運行狀態和自然路標匹配的結果情況進行估計，從而獲得機器人的導航參數。
[0048]具體地說，在第三步驟S3中，可利用動態擴展卡爾曼濾波器融合視覺信息修正機器人的慣性導航系統測量誤差；更具體地說，例如，可定義機器人方向角為(a，(6, 0)，定義機器人中的陀螺的偏差為(bp, bq, br),定義視覺信息為(a V，P V，0 v)(該參數與前面“提取的自然路標的特征點”有關)，則在將慣性導航系統狀態變量選擇為x=[ a , (6, 0，bp, bq, br]T,并將觀測變量選擇為[a v, 0 v, 0 v]的情況下，建立慣性導航系統的狀態方程及觀測方程，并利用動態擴展卡爾曼濾波器對狀態方程及觀測方程進行遞推以估計機器人的位置信息。
[0049]作為示例，圖3示出了基于動態擴展卡爾曼濾波融合視覺信息與慣性導航參數的一種原理。如圖3所示，狀態更新模塊更新狀態后，與視覺測量的結果進行比較，產生一個修正量，經過誤差修正后，再作用于狀態更新模塊，同時可以估計機器人的大概位姿。
[0050]本方法針對現有技術的不足，基于SIFT算法實現自然路標的特征提取，設計動態擴展卡爾曼濾波器對機器人的運動狀態和視覺特征的位置進行估計，從而獲得機器人的導航參數，解決視覺信息與慣性導航系統參數的數據融合問題，實現視覺信息對慣性導航系統誤差的修正，獲得精確的導航坐標。
[0051]本發明的優點在于，該導航系統成本低廉、體積小、操作方便、精度高，滿足中小型機器人的載荷、尺寸、功率、成本等方面的需求，具有良好的應用前景。
[0052]此外，需要說明的是，除非特別說明或者指出，否則說明書中的術語“第一”、“第二”、“第三”等描述僅僅用于區分說明書中的各個組件、元素、步驟等，而不是用于表示各個組件、元素、步驟之間的邏輯關系或者順序關系等。
[0053]可以理解的是，雖然本發明已以較佳實施例披露如上，然而上述實施例并非用以限定本發明。對于任何熟悉本領域的技術人員而言，在不脫離本發明技術方案范圍情況下，都可利用上述揭示的技術內容對本發明技術方案作出許多可能的變動和修飾，或修改為等同變化的等效實施例。因此，凡是未脫離本發明技術方案的內容，依據本發明的技術實質對以上實施例所做的任何簡單修改、等同變化及修飾，均仍屬于本發明技術方案保護的范圍內。
【權利要求】
1.一種基于SIFT算法的視覺導航方法，其特征在于包括: 第一步驟:利用機器人的導航相機獲取圖像； 第二步驟:對于導航相機獲取的圖像基于SIFT方法提取自然路標的特征點； 第三步驟:根據所提取的自然路標的特征點，利用動態擴展卡爾曼濾波器融合視覺信息以修正機器人的慣性導航系統的測量誤差，由此對機器人運行狀態和視覺特征的位置進行估計，從而獲得機器人的導航參數。
2.根據權利要求1所述的基于SIFT算法的視覺導航方法，其特征在于，所述第二步驟包括: 第一步，進行尺度空間極點檢測，其中通過不同尺度的高斯核函數對導航相機獲取的圖像進行連續濾波和下采樣，形成高斯金字塔圖像，然后再對相鄰尺度的兩個高斯圖像相減得到高斯偏差，以得到關鍵點；其中，關鍵點不僅是當前高斯偏差圖像局部相鄰的預定個數的像素的極大值或極小值，而且是前后相鄰尺度的高斯偏差領域的極大或極小值； 第二步，進行特征點定位，其中在極值點的局部領域用三維二次函數擬合方法來差值確定極值點的坐標； 第三步，進行主方位賦值，其中在構造SIFT描述算子之前，首先為每個關鍵點賦予一個主方向，其中主方向是指關鍵點領域內各點梯度方向的直方圖中最大值所對應的方向； 第四步，進行SIFT描述算子的建立，其中將坐標軸旋轉為關鍵點的方向，以確保任意一個關鍵點的旋轉不變性，而且在關鍵點所在的尺度空間，取以關鍵點為中心的16像素X 16像素大小的領域，再將此領域均勻地分為4X4的子區域，對每個子區域計算梯度方向直方圖，以構成一個128維的特征向量，再將特征向量的長度歸一化，從而獲得作為SIFT描述算子的向量； 第五步，在導航相機獲取的多幅圖像的SIFT特征點集中找到相應匹配特征點之間的對應關系。
3.根據權利要求2所述的基于SIFT算法的視覺導航方法，其特征在于，所述第五步包括: 針對導航相機獲取的圖像與模板圖像的SIFT特征點，執行歐式距離匹配； 剔除歐式距離匹配中的誤匹配； 根據剔除了誤匹配的歐式距離匹配執行當前匹配圖像與模板圖像的仿射變換。
4.根據權利要求1或2所述的基于SIFT算法的視覺導航方法，其特征在于，在第三步驟中，定義機器人方向角為(a，(6, 0)，定義機器人中的陀螺的偏差為6?，1^，1^)，定義視覺信息為(a V，P V，0 V),則在將慣性導航系統狀態變量選擇為x=[ a，P，0，bp, bq, br]T，并將觀測變量選擇為[av，Pv，0v]的情況下，建立慣性導航系統的狀態方程及觀測方程，并利用動態擴展卡爾曼濾波器對狀態方程及觀測方程進行遞推以估計機器人的位置信肩、O
【文檔編號】G01C21/00GK103644904SQ201310698173
【公開日】2014年3月19日 申請日期:2013年12月17日 優先權日:2013年12月17日
【發明者】崔顯龍, 朱旭紅, 張哲棟, 王海軍 申請人:上海電機學院