本發(fā)明的實(shí)施例總體上涉及用于運(yùn)動(dòng)估計(jì)的方法和系統(tǒng)，具體地，涉及用于對(duì)自身運(yùn)動(dòng)(ego-motion)進(jìn)行基于視覺的計(jì)算的方法和系統(tǒng)。

背景技術(shù)：

對(duì)車輛相對(duì)于車輛行駛的表面路徑的自身運(yùn)動(dòng)進(jìn)行精確估計(jì)可以是用于自動(dòng)駕駛和基于計(jì)算機(jī)視覺的駕駛輔助的關(guān)鍵要素。與其他傳感器相比，使用一個(gè)或更多個(gè)攝像機(jī)來計(jì)算自身運(yùn)動(dòng)可以允許將自身運(yùn)動(dòng)數(shù)據(jù)集成到其他基于視覺的算法(例如，障礙物檢測(cè)和/或回避、行人檢測(cè)和/或回避、物體檢測(cè)和/或回避以及其他基于視覺的算法)中，而不需要在傳感器之間進(jìn)行校準(zhǔn)。這可以減少維護(hù)要求和成本。僅使用來自被附接到代理(agent)(例如，車輛、人、機(jī)器人和其他移動(dòng)代理)的一個(gè)或更多個(gè)攝像機(jī)的輸入來估計(jì)該代理的自身運(yùn)動(dòng)的過程被稱為視覺里程計(jì)(vo)。

在vo中，通過檢查由于移動(dòng)代理的運(yùn)動(dòng)引起的在由一個(gè)或更多個(gè)機(jī)載攝像機(jī)獲得的圖像上的變化，來增量式地估計(jì)移動(dòng)代理的姿態(tài)。為了使vo有效地工作，可能需要充分的環(huán)境照明和具有足夠紋理的靜態(tài)場(chǎng)景，以允許提取明顯的運(yùn)動(dòng)。此外，應(yīng)當(dāng)捕獲時(shí)間連續(xù)的幀以確保充分的場(chǎng)景交疊。

用于提供運(yùn)動(dòng)估計(jì)的vo的一個(gè)優(yōu)點(diǎn)在于：vo不受不平坦地形和其他不利條件下的車輪滑移的影響。此外，vo可以向其他運(yùn)動(dòng)估計(jì)過程和系統(tǒng)(例如，全球定位系統(tǒng)(gps)、慣性測(cè)量單元(imu)、激光里程計(jì)和提供運(yùn)動(dòng)估計(jì)的其他系統(tǒng))提供重要的補(bǔ)充信息。此外，在拒絕gps的環(huán)境(例如，水下、空中和可能拒絕gps的其他環(huán)境)以及gps信息由于例如多徑、衛(wèi)星覆蓋差和其他可靠性因素而不可靠的環(huán)境中，可能增加vo的重要性。

使用專門的視頻輸入來估計(jì)運(yùn)動(dòng)的許多運(yùn)動(dòng)估計(jì)算法假定了靜態(tài)場(chǎng)景。此外，使用專門的視頻輸入來估計(jì)運(yùn)動(dòng)的許多運(yùn)動(dòng)估計(jì)算法不能處理動(dòng)態(tài)和/或混亂的環(huán)境或由過往車輛產(chǎn)生的較大遮擋。此外，運(yùn)動(dòng)估計(jì)中的特征匹配和異常值去除可能不是魯棒的，且可能隨后失敗。許多運(yùn)動(dòng)估計(jì)方案需要大量的關(guān)鍵點(diǎn)，且在缺少結(jié)構(gòu)的場(chǎng)景中可用的關(guān)鍵點(diǎn)數(shù)量有限的情況下，這些運(yùn)動(dòng)估計(jì)方案可能失敗。

不依賴于上述假設(shè)并克服上述限制的實(shí)時(shí)vo方法和系統(tǒng)可能是期望的。

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

本發(fā)明的一些實(shí)施例包括用于使用攝像機(jī)來估計(jì)自身運(yùn)動(dòng)的方法和系統(tǒng)。

根據(jù)本發(fā)明的第一方面，精確校準(zhǔn)的攝像機(jī)可以相對(duì)于移動(dòng)代理和地平面被剛性安裝在移動(dòng)代理上。通過假定移動(dòng)代理行駛在主要為平面狀的地平面上，從而在移動(dòng)代理行駛期間的每個(gè)時(shí)刻保持固定的攝像機(jī)與地面關(guān)系，可以將移動(dòng)代理在任何時(shí)刻的運(yùn)動(dòng)的參數(shù)表示從六個(gè)自由度簡(jiǎn)化到三個(gè)自由度。

根據(jù)本發(fā)明的第二方面，可以從攝像機(jī)接收輸入圖像。根據(jù)本發(fā)明的第三方面，可以對(duì)接收的輸入圖像執(zhí)行特征檢測(cè)。根據(jù)本發(fā)明的第四方面，可以計(jì)算針對(duì)每個(gè)檢測(cè)到的特征的特征描述符。根據(jù)本發(fā)明的第五方面，可以在針對(duì)接收的輸入圖像計(jì)算出的特征描述符和針對(duì)先前接收的圖像計(jì)算出的特征描述符之間執(zhí)行特征匹配。根據(jù)本發(fā)明的第六方面，可以將與特征對(duì)應(yīng)關(guān)系相關(guān)聯(lián)的關(guān)鍵點(diǎn)從圖像坐標(biāo)投影到世界坐標(biāo)。根據(jù)本發(fā)明的第七方面，可以根據(jù)與特征對(duì)應(yīng)關(guān)系相關(guān)聯(lián)的世界坐標(biāo)對(duì)來計(jì)算運(yùn)動(dòng)估計(jì)。根據(jù)本發(fā)明的第八方面，當(dāng)檢測(cè)到關(guān)鍵姿態(tài)(keypose)時(shí)，可以產(chǎn)生全局坐標(biāo)系下的當(dāng)前攝像機(jī)姿態(tài)。根據(jù)本發(fā)明的第九方面，可以根據(jù)當(dāng)前攝像機(jī)姿態(tài)確定運(yùn)動(dòng)軌跡。根據(jù)本發(fā)明的第十方面，可以將針對(duì)先前接收的圖像計(jì)算出的特征描述符更新為針對(duì)接收的輸入圖像計(jì)算出的特征描述符。

在結(jié)合附圖考慮本發(fā)明的以下詳細(xì)描述時(shí)，本發(fā)明的以上和其它目的、特征和優(yōu)點(diǎn)將更易于理解。

附圖說明

圖1是示出了本發(fā)明的示例性實(shí)施例的圖，其包括：對(duì)接收的輸入圖像執(zhí)行特征檢測(cè)；計(jì)算每個(gè)檢測(cè)到的特征的特征描述符；在針對(duì)接收的輸入圖像計(jì)算出的特征描述和針對(duì)先前接收的圖像計(jì)算出的特征描述符之間執(zhí)行特征匹配；將與特征對(duì)應(yīng)關(guān)系相關(guān)聯(lián)的關(guān)鍵點(diǎn)從圖像坐標(biāo)投影到世界坐標(biāo)；根據(jù)與所述特征對(duì)應(yīng)關(guān)系相關(guān)聯(lián)的世界坐標(biāo)對(duì)計(jì)算運(yùn)動(dòng)估計(jì)；當(dāng)檢測(cè)到關(guān)鍵姿態(tài)時(shí)，在全局坐標(biāo)系下產(chǎn)生當(dāng)前攝像機(jī)姿態(tài)；根據(jù)當(dāng)前攝像機(jī)姿態(tài)確定運(yùn)動(dòng)軌跡；以及將針對(duì)先前接收的圖像計(jì)算出的特征描述符更新為針對(duì)接收的輸入圖像計(jì)算出的特征描述符；

圖2是示出了與本發(fā)明的實(shí)施例相關(guān)的多個(gè)坐標(biāo)系的圖片；

圖3是示出了根據(jù)本發(fā)明示例性實(shí)施例的移動(dòng)代理平臺(tái)上的示例性攝像機(jī)配置的圖片；

圖4是示出了包括基于絕對(duì)取向(基于ao)的運(yùn)動(dòng)估計(jì)的本發(fā)明的示例性實(shí)施例的圖表；以及

圖5是示出了包括基于正交procrustes分析的運(yùn)動(dòng)估計(jì)的本發(fā)明的示例性實(shí)施例的圖表。

具體實(shí)施方式

通過參考附圖將最佳理解本發(fā)明的實(shí)施例，其中貫穿附圖，類似的部件由類似的附圖標(biāo)記表示。以上所列的附圖被明確地并入作為該詳細(xì)描述的一部分。

將容易理解的是可以以各種不同配置來布置和設(shè)計(jì)在本文附圖中概括描述和示出的本發(fā)明的部件。因此，以下對(duì)本發(fā)明的方法、系統(tǒng)和裝置的實(shí)施例的更詳細(xì)描述并不旨在限制本發(fā)明的范圍，而是僅代表本發(fā)明的當(dāng)前優(yōu)選實(shí)施例。

本發(fā)明的實(shí)施例的元件可以以硬件、固件和/或包括計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)的非暫時(shí)性計(jì)算機(jī)程序產(chǎn)品來實(shí)現(xiàn)，其中所述計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)將可以用于對(duì)計(jì)算系統(tǒng)進(jìn)行編程的指令存儲(chǔ)在其上/其中。雖然本文公開的示例性實(shí)施例僅可以描述這些形式中的一種，但是應(yīng)當(dāng)理解，本領(lǐng)域技術(shù)人員將能夠以這些形式中的任何形式實(shí)現(xiàn)這些元件，同時(shí)保持在本發(fā)明的范圍內(nèi)。

盡管附圖中的圖表和圖可以示出特定執(zhí)行順序，但是應(yīng)當(dāng)理解，所述執(zhí)行順序可以不同于所描繪的順序。例如，可以相對(duì)于所示順序改變所述塊的執(zhí)行順序。此外，作為另一示例，可以同時(shí)或部分同時(shí)地執(zhí)行圖中連續(xù)示出的兩個(gè)或更多個(gè)塊。本領(lǐng)域普通技術(shù)人員應(yīng)當(dāng)理解，本領(lǐng)域的普通技術(shù)人員可以創(chuàng)建包括計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)的非暫時(shí)性計(jì)算機(jī)程序產(chǎn)品以執(zhí)行本文所描述的各種邏輯功能，其中所述計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)將可以用于對(duì)計(jì)算系統(tǒng)、硬件和/或固件進(jìn)行編程的指令存儲(chǔ)在其上/其中。

對(duì)車輛相對(duì)于車輛行駛的表面路徑的自身運(yùn)動(dòng)進(jìn)行的精確估計(jì)可以是用于自動(dòng)駕駛和基于計(jì)算機(jī)視覺的駕駛輔助的關(guān)鍵要素。與其他傳感器相比，使用一個(gè)或更多個(gè)攝像機(jī)來計(jì)算自身運(yùn)動(dòng)可以允許將自身運(yùn)動(dòng)數(shù)據(jù)集成到其他基于視覺的算法(例如，障礙物檢測(cè)和/或回避、行人檢測(cè)和/或回避、物體檢測(cè)和/或回避以及其他基于視覺的算法)中，而不需要在傳感器之間進(jìn)行校準(zhǔn)。這可以減少維護(hù)要求和成本。僅使用來自被附接到代理(例如，車輛、人、機(jī)器人和其他移動(dòng)代理)的一個(gè)或更多個(gè)攝像機(jī)的輸入來估計(jì)該代理的自身運(yùn)動(dòng)的過程被稱為視覺里程計(jì)(vo)。

在vo中，通過檢查由于移動(dòng)代理的運(yùn)動(dòng)引起的在由一個(gè)或更多個(gè)機(jī)載攝像機(jī)獲得的圖像上的變化，來增量式地估計(jì)移動(dòng)代理的姿態(tài)。為了使vo有效地工作，可能需要充分的環(huán)境照明和具有足夠紋理的靜態(tài)場(chǎng)景，以允許提取明顯的運(yùn)動(dòng)。此外，應(yīng)當(dāng)捕獲時(shí)間連續(xù)的幀以確保充分的場(chǎng)景交疊。

用于提供運(yùn)動(dòng)估計(jì)的vo的一個(gè)優(yōu)點(diǎn)在于：vo不受不平坦地形和其他不利條件下的車輪滑移的影響。此外，vo可以向其他運(yùn)動(dòng)估計(jì)過程和系統(tǒng)(例如，全球定位系統(tǒng)(gps)、慣性測(cè)量單元(imu)、激光里程計(jì)和提供運(yùn)動(dòng)估計(jì)的其他系統(tǒng))提供重要的補(bǔ)充信息。此外，在拒絕gps的環(huán)境(例如，水下、空中和可能拒絕gps的其他環(huán)境)以及gps信息由于例如多徑、衛(wèi)星覆蓋差和其他可靠性因素而不可靠的環(huán)境中，可能增加vo的重要性。

使用專門的視頻輸入來估計(jì)運(yùn)動(dòng)的許多運(yùn)動(dòng)估計(jì)算法假定了靜態(tài)場(chǎng)景。此外，使用專門的視頻輸入來估計(jì)運(yùn)動(dòng)的許多運(yùn)動(dòng)估計(jì)算法不能處理動(dòng)態(tài)和/或混亂的環(huán)境或由過往車輛產(chǎn)生的較大遮擋。此外，運(yùn)動(dòng)估計(jì)中的特征匹配和異常值去除可能不是魯棒的，且可能隨后失敗。許多運(yùn)動(dòng)估計(jì)方案需要大量的關(guān)鍵點(diǎn)，且在缺少結(jié)構(gòu)的場(chǎng)景中可用的關(guān)鍵點(diǎn)數(shù)量有限的情況下，這些運(yùn)動(dòng)估計(jì)方案可能失敗。

不依賴于上述假設(shè)并克服上述限制的實(shí)時(shí)vo方法和系統(tǒng)可能是期望的。

結(jié)合圖1描述的本發(fā)明的一些實(shí)施例可以包括用于視覺里程計(jì)的方法100。可以在處理器組件中接收102輸入圖像(也被稱為輸入圖像幀和輸入幀)。在本發(fā)明的一些實(shí)施例中，可以直接從圖像獲取組件接收輸入圖像。在備選實(shí)施例中，可以間接從圖像獲取組件接收輸入圖像。在本發(fā)明的一些實(shí)施例中，圖像獲取組件和處理器組件可以都布置在剛性附接到移動(dòng)代理的攝像機(jī)中。在本發(fā)明的備選實(shí)施例中，圖像獲取組件可以布置在剛性附接到移動(dòng)代理的攝像機(jī)中，而處理器組件可以布置在移動(dòng)代理上，但不與被剛性附接的攝像機(jī)集成在一起。在本發(fā)明的一些實(shí)施例中，輸入圖像可以是彩色圖像。在本發(fā)明的備選實(shí)施例中，輸入圖像可以是灰度圖像。在本發(fā)明的一些實(shí)施例中，輸入圖像可以是根據(jù)獲取的彩色圖像產(chǎn)生的灰度圖像。在這些實(shí)施例的一些實(shí)施例中，可以在圖像獲取組件內(nèi)產(chǎn)生輸入灰度圖像。在這些實(shí)施例的備選實(shí)施例中，可以在圖像獲取組件的外部根據(jù)所獲取的彩色圖像產(chǎn)生輸入灰度圖像。

可以對(duì)接收的輸入圖像執(zhí)行104特征檢測(cè)，以識(shí)別在接收的輸入圖像中的表現(xiàn)出明顯強(qiáng)度變化的一個(gè)或更多個(gè)區(qū)域(例如，邊緣、拐角、斑點(diǎn)、尖峰和表現(xiàn)出明顯強(qiáng)度變化的其它空間特征)。一個(gè)或更多個(gè)識(shí)別出的區(qū)域中的每一個(gè)的中心(或基本上的中心)可以被指定為關(guān)鍵點(diǎn)，從而產(chǎn)生與接收的輸入圖像相關(guān)聯(lián)的一個(gè)或更多個(gè)關(guān)鍵點(diǎn)。執(zhí)行特征檢測(cè)可以包括：向接收的輸入圖像應(yīng)用本領(lǐng)域已知的一個(gè)或更多個(gè)關(guān)鍵點(diǎn)特征檢測(cè)器，例如，拐角檢測(cè)器、斑點(diǎn)檢測(cè)器、尖峰檢測(cè)器、尺度不變特征變換(sift)關(guān)鍵點(diǎn)檢測(cè)器、加速魯棒特征(surf)關(guān)鍵點(diǎn)檢測(cè)器、加速分割檢測(cè)特征(fast)關(guān)鍵點(diǎn)檢測(cè)器、取向fast和旋轉(zhuǎn)brief(orb)關(guān)鍵點(diǎn)檢測(cè)器、最大穩(wěn)定極值區(qū)域(mser)關(guān)鍵點(diǎn)檢測(cè)器、二進(jìn)制魯棒獨(dú)立基本特征(brief)關(guān)鍵點(diǎn)檢測(cè)器、二進(jìn)制魯棒不變可調(diào)關(guān)鍵點(diǎn)(brisk)關(guān)鍵點(diǎn)檢測(cè)器、快速retina關(guān)鍵點(diǎn)(freak)關(guān)鍵點(diǎn)檢測(cè)器、star關(guān)鍵點(diǎn)檢測(cè)器和其他關(guān)鍵點(diǎn)特征檢測(cè)器。在本發(fā)明的一些實(shí)施例中，由于fast關(guān)鍵點(diǎn)檢測(cè)器具有較快的檢測(cè)速度使其適合于實(shí)時(shí)自主導(dǎo)航應(yīng)用，因此可以使用fast關(guān)鍵點(diǎn)檢測(cè)器。

在本發(fā)明的一些實(shí)施例中，可以使用基于網(wǎng)格的fast特征檢測(cè)器來執(zhí)行特征檢測(cè)104。在這些實(shí)施例中，接收的輸入圖像可以被劃分為“m×n”網(wǎng)格。在本發(fā)明的一些實(shí)施例中，“m”和“n”可以是不同的值。在備選實(shí)施例中，“m”的值和“n”的值可以是相同的?？梢詫ast關(guān)鍵點(diǎn)檢測(cè)器應(yīng)用于每個(gè)網(wǎng)格單元。在本發(fā)明的一些實(shí)施例中，可以針對(duì)圖像中檢測(cè)到的關(guān)鍵點(diǎn)的最大計(jì)數(shù)(maximumcount)指定最大關(guān)鍵點(diǎn)計(jì)數(shù)門限值。最大關(guān)鍵點(diǎn)計(jì)數(shù)門限值的值可以用于調(diào)整針對(duì)每個(gè)輸入圖像幀的處理速度。在本發(fā)明的一些實(shí)施例中，可以基于輸入圖像的期望處理速度來選擇網(wǎng)格單元的數(shù)量。

可以計(jì)算106用于表征每個(gè)指定關(guān)鍵點(diǎn)周圍的區(qū)域的特征描述符。在本發(fā)明的一些實(shí)施例中，與區(qū)域相關(guān)聯(lián)的特征描述符可以是區(qū)域的外觀，例如，像素強(qiáng)度或顏色值。在本發(fā)明的一些實(shí)施例中，與區(qū)域相關(guān)聯(lián)的特征描述符可以基于區(qū)域的外觀，例如，原始像素值可以通過像素值、像素值的能量或根據(jù)原始像素值計(jì)算的一些其他統(tǒng)計(jì)量的平均值來歸一化。在備選實(shí)施例中，可以通過像素值的平均值來調(diào)整原始像素值，例如，可以從原始像素值中減去平均像素值。在本發(fā)明的一些實(shí)施例中，特征描述符可以是區(qū)域上的空間梯度。在本發(fā)明的一些實(shí)施例中，與區(qū)域相關(guān)聯(lián)的特征描述符可以對(duì)于視點(diǎn)變化、旋轉(zhuǎn)和尺度變化中的一個(gè)或更多個(gè)是魯棒的。示例性描述符包括sift特征描述符、surf特征描述符、orb特征描述符、brief特征描述符、brisk特征描述符、freak特征描述符以及其它視點(diǎn)不變、旋轉(zhuǎn)不變或尺度不變的特征描述符。在本發(fā)明的一些實(shí)施例中，特征描述符可以是緊湊型特征描述符。以上所列的特征描述符旨在用于說明而不是限制的目的。

可以在根據(jù)接收的輸入幀計(jì)算出的特征描述符以及根據(jù)前一幀計(jì)算出的特征描述符之間執(zhí)行108特征匹配。接收的輸入幀中的每個(gè)關(guān)鍵點(diǎn)可以與來自前一幀的關(guān)鍵點(diǎn)相關(guān)聯(lián)，從而產(chǎn)生多個(gè)特征對(duì)應(yīng)關(guān)系，其中每個(gè)特征對(duì)應(yīng)關(guān)系將接收的輸入幀中的關(guān)鍵點(diǎn)與前一幀中的關(guān)鍵點(diǎn)相關(guān)聯(lián)。

在本發(fā)明的一些實(shí)施例中，針對(duì)接收的輸入幀計(jì)算出的每個(gè)特征描述符可以與針對(duì)前一幀計(jì)算出的每個(gè)特征描述符進(jìn)行窮舉比較。輸入幀特征描述符與前一幀特征描述符的比較可以包括計(jì)算相似性度量。在特征描述符是特征外觀的本發(fā)明的一些實(shí)施例中，相似性度量可以是平方差的和。在特征描述符是特征外觀的本發(fā)明的備選實(shí)施例中，相似性度量可以是歸一化互相關(guān)。

在本發(fā)明的一些實(shí)施例中，執(zhí)行特征匹配108可以包括利用“k”維樹的快速最近鄰搜索。在特征描述符是諸如brief描述符的二進(jìn)制描述符的本發(fā)明的一些實(shí)施例中，可以使用漢明距離來測(cè)量相似性。在可以使用其他非二進(jìn)制特征描述符的備選實(shí)施例中，可以將數(shù)學(xué)范數(shù)(例如，l2范數(shù)、l1范數(shù)和由范數(shù)限定的其他度量)用于相似性度量。

在本發(fā)明的一些實(shí)施例中，可以修剪特征匹配，以僅保留足夠好的匹配用于計(jì)算運(yùn)動(dòng)變換。在本發(fā)明的一些實(shí)施例中，可以在最好的兩個(gè)匹配之間執(zhí)行距離比率測(cè)試?？梢杂?jì)算最接近的匹配和第二接近的匹配之間的比率。如果比率小于用戶指定的門限值，則可以接受最接近的匹配。在本發(fā)明的一些實(shí)施例中，用戶指定的門限值可以等于0.65。在用距離比率測(cè)試執(zhí)行匹配之后，可以執(zhí)行相互一致性檢查以消除在接收的輸入圖像中的與前一圖像中的多于一個(gè)特征相匹配的特征。相互一致性檢查消除了不相互優(yōu)選的所有匹配。

可以將與每個(gè)特征對(duì)應(yīng)關(guān)系對(duì)相關(guān)聯(lián)的關(guān)鍵點(diǎn)的坐標(biāo)從圖像坐標(biāo)系投影110到世界坐標(biāo)系(也被稱為移動(dòng)代理坐標(biāo)系)。

圖2示出了與本發(fā)明的實(shí)施例相關(guān)的多個(gè)坐標(biāo)系。圖2分別描繪了在兩個(gè)時(shí)刻(t＝k和t＝k+1)下的具有單個(gè)機(jī)載剛性安裝的攝像機(jī)204、206的移動(dòng)代理200、202。可以將具有表示為xw、yw和zw的坐標(biāo)軸的全局世界坐標(biāo)系208表示為w。當(dāng)移動(dòng)代理200、202沿著平面狀地平面210路徑行駛時(shí)，全局世界坐標(biāo)系208(w)是靜止的。可以假定平面狀地平面210位于z＝0的平面中。全局世界坐標(biāo)系208(w)的原點(diǎn)是路徑的起始位置?？梢詫z像機(jī)坐標(biāo)系212表示為c，其中坐標(biāo)軸被表示為xc、yc和zc；可以將圖像坐標(biāo)系214表示為i，其中坐標(biāo)軸被表示為xi、yi和zi；且可以將移動(dòng)代理坐標(biāo)系216表示為m，其中坐標(biāo)軸被表示為xm、ym和zm。移動(dòng)代理坐標(biāo)系216也可以被稱為世界坐標(biāo)系。攝像機(jī)坐標(biāo)系212(c)和移動(dòng)代理坐標(biāo)系216(m)二者可以隨著移動(dòng)代理200、202的移動(dòng)而移動(dòng)。

由于攝像機(jī)204、206被剛性安裝在移動(dòng)代理200、202上，攝像機(jī)坐標(biāo)系212(c)與移動(dòng)代理坐標(biāo)系216(m)具有固定關(guān)系。在移動(dòng)代理坐標(biāo)系216(m)下與可以表示為p的點(diǎn)相對(duì)應(yīng)的坐標(biāo)向量xxm可以根據(jù)下式與p點(diǎn)在攝像機(jī)坐標(biāo)系212(c)下的坐標(biāo)向量xxc相關(guān)：

xxc＝rc*xxm+tc，

其中rc可以表示從移動(dòng)代理坐標(biāo)系216(m)到攝像機(jī)坐標(biāo)系212(c)的旋轉(zhuǎn)矩陣，且tc可以表示平移向量，所述平移向量是在移動(dòng)代理坐標(biāo)系216(m)的原點(diǎn)在攝像機(jī)坐標(biāo)系212(c)下的坐標(biāo)向量。rc和tc是外部攝像機(jī)參數(shù)，且可以通過攝像機(jī)校準(zhǔn)獲得。點(diǎn)p可以由攝像機(jī)投影到圖像平面上。將投影圖像點(diǎn)表示為具有圖像坐標(biāo)系坐標(biāo)向量xxi的p，攝像機(jī)坐標(biāo)向量xxc可以根據(jù)下式與xxi相關(guān)：

l*xxi＝k*xxc，

其中k可以表示攝像機(jī)的內(nèi)在矩陣，且l可以表示對(duì)由于從三維到二維的投影而導(dǎo)致的深度丟失加以表示的比例因子。

可以通過本領(lǐng)域中公知技術(shù)來估計(jì)內(nèi)在(intrinsic)的和外部(extrinsic)的攝像機(jī)參數(shù)。

在本發(fā)明的一些實(shí)施例中，可以獲得內(nèi)在參數(shù)并將內(nèi)在參數(shù)永久地存儲(chǔ)在移動(dòng)代理中。在本發(fā)明的一些實(shí)施例中，可以在移動(dòng)代理運(yùn)行時(shí)執(zhí)行對(duì)外部攝像機(jī)參數(shù)的重新校準(zhǔn)。可以捕獲地面上的校準(zhǔn)圖案。可以從校準(zhǔn)圖案的圖像提取拐角點(diǎn)，并可以計(jì)算外部攝像機(jī)參數(shù)。當(dāng)需要重新校準(zhǔn)時(shí)，這些新計(jì)算的外部攝像機(jī)參數(shù)和所存儲(chǔ)的內(nèi)部攝像機(jī)參數(shù)可以用于更新攝像機(jī)參數(shù)。

對(duì)于在圖像坐標(biāo)表示為[x，y]^t的圖像上的關(guān)鍵點(diǎn)，可以根據(jù)下式來計(jì)算移動(dòng)代理坐標(biāo)系(也被稱為世界坐標(biāo)系)下的對(duì)應(yīng)點(diǎn)：

【式1】

其中k可以表示攝像機(jī)內(nèi)在參數(shù)，rc和tc可以表示攝像機(jī)外部參數(shù)，[x，y，z]^t可以表示移動(dòng)代理坐標(biāo)系下的坐標(biāo)，且1可以表示該坐標(biāo)在移動(dòng)代理坐標(biāo)系下的未知比例因子。因此，可以根據(jù)下式來計(jì)算移動(dòng)代理坐標(biāo)：

【式2】

假設(shè)移動(dòng)代理主要在地平面上行駛，且攝像機(jī)只看地平面，則世界坐標(biāo)系下的關(guān)鍵點(diǎn)的坐標(biāo)必須滿足地平面方程。任何平面的一般形式可以表示為：

ax+by+cz+d＝0

并可以寫成：

【式3】

因此：

【式4】

以及

【式5】

假設(shè)地面位于z＝0的平面上，則d＝0，地平面的法向向量[abc]^t＝[001]^t，且上述等式中的所有變量是已知的。因此，可以針對(duì)關(guān)鍵點(diǎn)的每個(gè)坐標(biāo)計(jì)算1值，且可以使用計(jì)算出的1值來計(jì)算移動(dòng)代理坐標(biāo)。

再次參考圖1，可以使用針對(duì)特征對(duì)應(yīng)關(guān)系對(duì)的關(guān)鍵點(diǎn)的世界坐標(biāo)來計(jì)算112運(yùn)動(dòng)估計(jì)。運(yùn)動(dòng)估計(jì)可以包括最佳的剛性變換(可以被表示為tk，k-1)，其可以將前一幀中的與特征對(duì)應(yīng)關(guān)系點(diǎn)相關(guān)聯(lián)的每個(gè)關(guān)鍵點(diǎn)的三維坐標(biāo)和當(dāng)前幀中的對(duì)應(yīng)關(guān)鍵點(diǎn)的三維坐標(biāo)相對(duì)準(zhǔn)。

將前一幀中的與特征對(duì)應(yīng)關(guān)系點(diǎn)相關(guān)聯(lián)的關(guān)鍵點(diǎn)的三維坐標(biāo)集合表示為a，并將當(dāng)前幀中的對(duì)應(yīng)關(guān)鍵點(diǎn)的三維坐標(biāo)集合表示為b，a和b之間的關(guān)系可以寫成：

b＝tk，k-1a，

其中tk，k-1可以表示當(dāng)前幀和前一幀之間的變換。由于攝像機(jī)運(yùn)動(dòng)是剛性運(yùn)動(dòng)，因此變換tk，k-1可以包括前一時(shí)刻和當(dāng)前時(shí)刻之間的剛性旋轉(zhuǎn)以及前一時(shí)刻和當(dāng)前時(shí)刻之間的平移，其中可以將所述剛性旋轉(zhuǎn)表示為rk，k-1，且可以將所述平移表示為tk，k-1。因此，

【式6】

在本發(fā)明的一些實(shí)施例中，可以結(jié)合圖3描述在移動(dòng)代理平臺(tái)上的攝像機(jī)配置。精確校準(zhǔn)的攝像機(jī)300可以相對(duì)于移動(dòng)代理302和地平面304剛性安裝。在本發(fā)明的一些實(shí)施例中，攝像機(jī)300可以安裝在移動(dòng)代理302的前端，在主體底盤上方。攝像機(jī)300可以向地面傾斜以提供在移動(dòng)代理302正前方的地面的良好視圖。在本發(fā)明的備選實(shí)施例(未示出)中，攝像機(jī)可以安裝在移動(dòng)代理的后端，位于主體底盤上方。攝像機(jī)可以向地面傾斜以提供在移動(dòng)代理正后方的地面的良好視圖。

可以假設(shè)地平面304之上的高度306(可以表示為h)以及在地平面304的法線310和攝像機(jī)300視線312之間的角度308(可以表示為θ)是恒定的。此外，可以假設(shè)當(dāng)移動(dòng)代理302在地面上移動(dòng)時(shí)，角度308(θ)可以不受較大振動(dòng)的影響。相對(duì)于地面的外部攝像機(jī)參數(shù)表征h和θ值，以及攝像機(jī)坐標(biāo)系相對(duì)于移動(dòng)代理坐標(biāo)系的旋轉(zhuǎn)和平移。

此外，假設(shè)移動(dòng)代理302在主要為平面狀的地平面304上行駛，于是在移動(dòng)代理行駛期間的每個(gè)時(shí)刻具有固定的攝像機(jī)對(duì)地面的關(guān)系的假設(shè)將保持有效。

上述假設(shè)允許將對(duì)移動(dòng)代理在任意時(shí)刻的運(yùn)動(dòng)的參數(shù)表示從六個(gè)自由度簡(jiǎn)化到三個(gè)自由度：x和y方向上的平移，其可以表示為[xk，k-1，yk，k-1]；以及相對(duì)于全局世界坐標(biāo)系的旋轉(zhuǎn)，其可以表示為θk，k-1。

因此，利用幀間變換，可以將tk，k-1寫為：

【式7】

可以結(jié)合圖4描述包括用于估計(jì)剛性旋轉(zhuǎn)rk，k-1和平移tk，k-1的基于絕對(duì)取向方法400的一些實(shí)施例。在這些示例性實(shí)施例中，可以通過使中心點(diǎn)之間的均方誤差(mse)(可以表示為e)最小化來估計(jì)剛性旋轉(zhuǎn)rk，k-1和平移tk，k-1，其中所述中心點(diǎn)對(duì)應(yīng)于a和b中的配對(duì)點(diǎn)，根據(jù)下式將e表示為：

【式8】

其中n可以表示特征對(duì)應(yīng)關(guān)系對(duì)的數(shù)量，且ai’和bi’可以表示根據(jù)下式計(jì)算的中心點(diǎn)：

【式9】

在這些示例性實(shí)施例中，可以分別根據(jù)下式來計(jì)算402每個(gè)點(diǎn)集(a和b)的質(zhì)心：

【式10】

可以根據(jù)下式將每個(gè)點(diǎn)集帶到404其原點(diǎn)：

【式11】

可以根據(jù)下式來計(jì)算406旋轉(zhuǎn)角θk，k-1：

【式12】

以及

【式13】

其中

【式14】

以及

【式15】

其中·可以表示點(diǎn)積，×可以表示叉積，且

【式16】

可以表示旋轉(zhuǎn)所在的平面的法線。

可以通過使用旋轉(zhuǎn)角θk，k-1來根據(jù)下式構(gòu)造408旋轉(zhuǎn)矩陣rk，k-1：

【式17】

可以根據(jù)下式來計(jì)算410平移tk，k-1：

【式18】

可以結(jié)合圖5描述包括用于估計(jì)剛性旋轉(zhuǎn)rk，k-1和平移tk，k-1的基于正交procrustes分析的方法500的備選實(shí)施例。

在這些示例性實(shí)施例中，可以分別根據(jù)下式來計(jì)算502每個(gè)點(diǎn)集(a和b)的質(zhì)心：

【式19】

可以根據(jù)下式將每個(gè)點(diǎn)集帶到504其原點(diǎn)：

【式20】

通過使用奇異值分解(svd)來計(jì)算506旋轉(zhuǎn)矩陣。首先，可以根據(jù)下式計(jì)算協(xié)方差矩陣，其中協(xié)方差矩陣可以表示為h：

【式21】

可以計(jì)算協(xié)方差矩陣(h)的奇異值分解，其中：

【式22】

svd(h)＝u∑v^t

可以根據(jù)下式來計(jì)算旋轉(zhuǎn)矩陣rk，k-1：

【式23】

其中

d＝sign(det(vu^t))

且確保右手坐標(biāo)系統(tǒng)。

可以根據(jù)下式來計(jì)算508平移tk，k-1：

【式24】

在本發(fā)明的一些實(shí)施例中，運(yùn)動(dòng)估計(jì)112可以包括異常值剔除，以去除由于例如圖像噪聲、遮擋、模糊、視點(diǎn)變化、照明變化和特征檢測(cè)器的數(shù)學(xué)模型無法解釋的其他原因引起的異常值。在本發(fā)明的一些實(shí)施例中，可以執(zhí)行隨機(jī)抽樣一致性(ransac)迭代運(yùn)動(dòng)估計(jì)。ransac根據(jù)隨機(jī)抽樣的數(shù)據(jù)點(diǎn)集計(jì)算模型假設(shè)，然后在其他數(shù)據(jù)點(diǎn)上驗(yàn)證這些假設(shè)。將示出了與其他數(shù)據(jù)有最高一致性的假設(shè)選擇作為解。在這些實(shí)施例中，估計(jì)模型是兩個(gè)攝像機(jī)位置之間的相對(duì)運(yùn)動(dòng)rk，k-1和tk，k-1，且數(shù)據(jù)點(diǎn)是候選特征對(duì)應(yīng)關(guān)系。在本發(fā)明的一些實(shí)施例中，可以根據(jù)以下偽碼來執(zhí)行運(yùn)動(dòng)估計(jì)：

【句法1】

其中在一些實(shí)施例中，迭代次數(shù)“n”被設(shè)置為50，要采樣的對(duì)的數(shù)量“k”被設(shè)置為3，誤差門限值t1在3-5mm的范圍內(nèi)，內(nèi)點(diǎn)的數(shù)量門限值為t2＝5，且內(nèi)點(diǎn)比率的門限值為t3＝0.55。

漂移可能隨著誤差累積變得嚴(yán)重。由于誤差累積隨著增加的運(yùn)動(dòng)估計(jì)的級(jí)聯(lián)而增加，為了減小漂移，本發(fā)明的一些實(shí)施例可以不確定與每個(gè)運(yùn)動(dòng)估計(jì)相對(duì)應(yīng)的攝像機(jī)姿態(tài)。因此，再次參考圖1，可以確定118當(dāng)前計(jì)算的運(yùn)動(dòng)估計(jì)是否應(yīng)當(dāng)用于估計(jì)攝像機(jī)姿態(tài)，從而識(shí)別關(guān)鍵姿態(tài)。在本發(fā)明的一些實(shí)施例中，如果當(dāng)前運(yùn)動(dòng)估計(jì)的旋轉(zhuǎn)角θk，k-1滿足針對(duì)角度門限值的第一條件，或如果行駛距離滿足針對(duì)距離門限值的第二條件，則可以認(rèn)為當(dāng)前攝像機(jī)姿勢(shì)是關(guān)鍵姿態(tài)。在一些實(shí)施例中，當(dāng)旋轉(zhuǎn)角θk，k-1大于角度門限值或行駛距離大于距離門限值時(shí)，可以認(rèn)為當(dāng)前攝像機(jī)姿勢(shì)是關(guān)鍵姿勢(shì)?？梢愿鶕?jù)下式來計(jì)算行駛距離：

【式25】

||tk，k-1||²

如果當(dāng)前攝像機(jī)姿態(tài)沒被確定120為關(guān)鍵姿態(tài)，則可以通過等待接收102下一輸入圖像來繼續(xù)所述方法100。

如果當(dāng)前攝像機(jī)姿態(tài)122被確定為關(guān)鍵姿態(tài)，則可以根據(jù)下式生成當(dāng)前攝像機(jī)姿態(tài)：

ck＝ck-1tk，k-1，

其中，ck-1表示在k-1時(shí)刻的攝像機(jī)姿態(tài)。在本發(fā)明的一些實(shí)施例中，可以將初始攝像機(jī)姿態(tài)設(shè)置為單位矩陣。當(dāng)前攝像機(jī)姿態(tài)包括所有變換的級(jí)聯(lián)tk，k-1(k＝1，...，n)，其中n是當(dāng)前時(shí)刻。當(dāng)前攝像機(jī)姿態(tài)ck具有以下形式：

【式26】

且可以根據(jù)關(guān)鍵姿態(tài)確定128運(yùn)動(dòng)參數(shù)[xk，yk，θk]，并存儲(chǔ)所述運(yùn)動(dòng)參數(shù)。當(dāng)前幀的描述符集合可以更新130先前幀的描述符集合?？梢酝ㄟ^等待接收102下一輸入幀來繼續(xù)所述方法100。

本發(fā)明的一些實(shí)施例可以包括含有計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)的計(jì)算機(jī)程序產(chǎn)品，其中所述計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)將可以用于對(duì)計(jì)算系統(tǒng)進(jìn)行編程以執(zhí)行本文所述的任何特征和方法的指令存儲(chǔ)在其上/其中。示例性計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)可以包括但不限于閃存設(shè)備、盤存儲(chǔ)介質(zhì)(例如，軟盤、光盤、磁光盤、數(shù)字通用盤(dvd)、壓縮盤(cd)、微驅(qū)動(dòng)器和其它盤存儲(chǔ)介質(zhì))、只讀存儲(chǔ)器(rom)、可編程只讀存儲(chǔ)器(prom)、可擦除可編程只讀存儲(chǔ)器(eproms)、電可擦除可編程只讀存儲(chǔ)器(eeprom)、隨機(jī)訪問存儲(chǔ)器(rams)、視頻隨機(jī)訪問存儲(chǔ)器(vram)、動(dòng)態(tài)隨機(jī)訪問存儲(chǔ)器(dram)以及適于存儲(chǔ)指令和/或數(shù)據(jù)的任何類型的介質(zhì)或設(shè)備。

在上述說明書中采用的術(shù)語和表述在本文中用作描述性而不是限制性的術(shù)語，且在使用這些術(shù)語和表述時(shí)不排除所示和所描述的特征或其部分的等同物，應(yīng)當(dāng)認(rèn)識(shí)到，本發(fā)明的范圍僅由所附權(quán)利要求限定和限制。