本發(fā)明屬于移動(dòng)機(jī)器人視覺(jué)定位技術(shù)領(lǐng)域，具體涉及一種基于SURF算法的雙目全景視覺(jué)機(jī)器人自主定位方法。

背景技術(shù)：

移動(dòng)機(jī)器人自主定位導(dǎo)航能力是衡量其智能化水平的關(guān)鍵因素，環(huán)境感知能力是移動(dòng)機(jī)器人智能化的基礎(chǔ)，一直是機(jī)器人領(lǐng)域一個(gè)的研究熱點(diǎn)。室內(nèi)定位方式比較成熟的是磁軌,但適用范圍有限；RFID、藍(lán)牙、WLAN等信號(hào)容易衰減,精度差；超聲波容易受障礙物干擾、激光雷達(dá)成本太高；電機(jī)編碼脈沖運(yùn)動(dòng)學(xué)定位存在累計(jì)誤差，而全景視覺(jué)傳感器具有探測(cè)范圍廣，獲取信息量大、成像旋轉(zhuǎn)不變性、快速、高精度等優(yōu)點(diǎn)，恰好能夠?yàn)樽灾饕苿?dòng)機(jī)器人提供豐富的環(huán)境信息。

移動(dòng)機(jī)器人定位路標(biāo)有自然路標(biāo)和人工路標(biāo)兩種，利用自然路標(biāo)，例如采用雙閾值FAST特征檢測(cè)的SIFT描述算法,解決了在室內(nèi)非結(jié)構(gòu)化環(huán)境中的定位問(wèn)題，但視覺(jué)路標(biāo)庫(kù)的采樣規(guī)則難以滿足。而應(yīng)用路標(biāo)定位則可通過(guò)設(shè)置人工路標(biāo)，將任意環(huán)境結(jié)構(gòu)化。針對(duì)移動(dòng)機(jī)器人定位過(guò)程中視覺(jué)圖像特征點(diǎn)提取與匹配數(shù)量較多，邊緣特征點(diǎn)不穩(wěn)定的問(wèn)題，在Bay結(jié)合SIFT特征點(diǎn)提取過(guò)程,提出了一種加速的、具有特征魯棒性的SURF算法。通過(guò)設(shè)置多個(gè)路標(biāo)，利用改進(jìn)SURF算法進(jìn)行路標(biāo)定位。根據(jù)垂直基線雙目全景系統(tǒng)測(cè)量精度與基線距成正比、與測(cè)量距離平方成反比的特點(diǎn)，建立垂直基線的雙目全景系統(tǒng)三維逆光路成像分析模型，通過(guò)雙目全景視覺(jué)獲取路標(biāo)的相對(duì)距離和方位等深度信息，從而得到機(jī)器人本身的位置的移動(dòng)機(jī)器人視覺(jué)定位方法，很好地解決了視覺(jué)機(jī)器人自主定位的關(guān)鍵問(wèn)題。

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

本發(fā)明目的在于提供一種基于SURF算法的雙目全景視覺(jué)機(jī)器人自主定位方法。

本發(fā)明的目的是這樣實(shí)現(xiàn)的：

種基于SURF算法的雙目全景視覺(jué)機(jī)器人自主定位方法，包括如下步驟:

(1)設(shè)置人工路標(biāo)作為先驗(yàn)位置信息：

在未知非結(jié)構(gòu)化環(huán)境中，根據(jù)SURF特征點(diǎn)提取算法，采集環(huán)境全景圖像庫(kù)測(cè)試，設(shè)計(jì)弱干擾匹配路標(biāo)，在機(jī)器人工作環(huán)境內(nèi)遮擋度低的一側(cè)等間隔、等高設(shè)置兩個(gè)以上不同特征的人工路標(biāo)；

(2)改進(jìn)的MDGHM-SURF特征匹配：

采用改進(jìn)MDGHM-SURF算法進(jìn)行路標(biāo)特征匹配定位，對(duì)全景圖像掩膜進(jìn)行等間隔采樣,并進(jìn)行像素坐標(biāo)變換，定義圖像I(i,j)的(p，q)階改進(jìn)后離散Gaussian-Hermit矩為：

其中利用改進(jìn)離散Gaussian-Hermit矩描述局部特征點(diǎn)，以描述特征點(diǎn)為中心，沿特征點(diǎn)主方向的正方形區(qū)域劃分為若干個(gè)子區(qū)域，各區(qū)域獲取多個(gè)采樣點(diǎn)，在X和Y方向上分別描述；

(3)匹配點(diǎn)重心迭代算法消除誤匹配：

由步驟(2)得匹配點(diǎn)坐標(biāo)為A_i(x_i,y_i),匹配度為m_i設(shè)計(jì)的路標(biāo)的特征均布，則匹配路標(biāo)點(diǎn)重心為通過(guò)計(jì)算A_i距離Z的歐式距離d_i，建立循環(huán)求取距離最大值d_imax,設(shè)定距離閾值d_H，如d_imax>d_H,則刪除該誤匹配點(diǎn)，重新迭代計(jì)算重心，直到d′_imax≤d_H，實(shí)現(xiàn)路標(biāo)中心精確定位；

(4)垂直基線式雙目全景系統(tǒng)三維逆光路成像測(cè)距：

由步驟(3)定位出上下全景中各路標(biāo)位置后，計(jì)算圖像中路標(biāo)距全景中心的像素距離，根據(jù)全景成像旋轉(zhuǎn)對(duì)稱性，取一個(gè)光軸截面為研究平面，雙曲面鏡的上支頂點(diǎn)為坐標(biāo)原點(diǎn)，水平、豎直方向分別為X、Y軸建立坐標(biāo)系,雙曲面鏡曲線方程為(y-c)²/a²-x²/b²＝1其中a、b、c分別為雙曲面鏡實(shí)軸、虛軸和焦距長(zhǎng)度，在全景圖像中測(cè)得路標(biāo)點(diǎn)距全景中心的距離為r₁，相機(jī)標(biāo)定焦距為f，根據(jù)折反射成像原理有r/f＝m/n，求解出鏡面曲線上反射點(diǎn)的坐標(biāo)為P(m,n)

根據(jù)垂直雙目全景系統(tǒng)結(jié)構(gòu)，入射光線的斜率為k＝(n-2c)/m，設(shè)上下全景系統(tǒng)的入射光線與雙曲面鏡的交點(diǎn)分別為Q₁(m₁,n₁)和Q₂(m₂,n₂)，其中L為基線距離，上下全景相機(jī)的入射光線方程為

聯(lián)立方程組求解與路標(biāo)水平距離：

在實(shí)際應(yīng)用中，當(dāng)透視相機(jī)和反射鏡面的焦點(diǎn)沒(méi)有嚴(yán)格重合時(shí)，將不滿足單視點(diǎn)約束；若仍按單視點(diǎn)測(cè)距算法求解，誤差較大,必須對(duì)非單視點(diǎn)進(jìn)行優(yōu)化；由于入射反射鏡面光線的延長(zhǎng)線不經(jīng)過(guò)雙曲線焦點(diǎn)，設(shè)鏡面直徑為R，相機(jī)和雙曲面鏡的焦點(diǎn)存在偏移量t，則折反射成像原理t＝Rf/r-H；由此可得變化后鏡面方程(y-c-t)²/a²-x²/b²＝1,得到雙曲面反射點(diǎn)Q(m′,n′)的坐標(biāo)；同理，可求解上下全景相機(jī)的入射光線方程y′₁＝k₁x+n₁-k₁m₁-L，距離由求解；

(5)全景三角定位：

由步驟(4)得到機(jī)器人與等高設(shè)置在同一墻壁A(x₁,0)，B(x₂,0)位置的2個(gè)環(huán)境路標(biāo)距離為d₁,d₂，在全景圖像中定位出兩路標(biāo)距中心的夾角β，所以機(jī)器人的坐標(biāo)計(jì)算公式為

根據(jù)全景圖像中路標(biāo)角度和機(jī)器人絕對(duì)的位置坐標(biāo)關(guān)系可以實(shí)現(xiàn)對(duì)于姿態(tài)的測(cè)量；

(6)預(yù)測(cè)檢測(cè)窗口加快定位速度

根據(jù)步驟(5)中機(jī)器人初始化定位后，機(jī)器人的絕對(duì)位置坐標(biāo)和各路標(biāo)在全景圖像中的位置已經(jīng)確定，由于機(jī)器人最大運(yùn)動(dòng)速度有限，對(duì)應(yīng)路標(biāo)在全景圖像中移動(dòng)范圍有限，利用機(jī)器人當(dāng)前的運(yùn)動(dòng)狀態(tài)，估測(cè)下一刻機(jī)器人全景圖像中路標(biāo)檢測(cè)區(qū)域，實(shí)現(xiàn)移動(dòng)中快速定位。

本發(fā)明的有益效果在于：

全景視覺(jué)傳感器具有探測(cè)范圍廣，獲取信息量大、成像旋轉(zhuǎn)不變性、快速、高精度等優(yōu)點(diǎn)，恰好能夠?yàn)樽灾饕苿?dòng)機(jī)器人提供豐富的環(huán)境信息。針對(duì)移動(dòng)機(jī)器人室內(nèi)自主定位問(wèn)題，研究了一種利用雙目全景視覺(jué)系統(tǒng)實(shí)現(xiàn)的機(jī)器人絕對(duì)定位方法。通過(guò)設(shè)置人工路標(biāo)作為先驗(yàn)位置信息，采用改進(jìn)的MDGHM-SURF算法進(jìn)行特征點(diǎn)快速檢測(cè)，并通過(guò)特征匹配進(jìn)行路標(biāo)識(shí)別定位，由匹配點(diǎn)重心迭代算法減小匹配誤差，提高路標(biāo)中心的定位精度，由機(jī)器人運(yùn)動(dòng)狀態(tài)預(yù)測(cè)路標(biāo)檢測(cè)區(qū)域，提高了運(yùn)動(dòng)中定位的快速性；通過(guò)垂直基線式雙目全景系統(tǒng)三維逆光路成像測(cè)距算法實(shí)現(xiàn)了對(duì)深度信息的測(cè)量，并采用三角定位算法計(jì)算出機(jī)器人的坐標(biāo)及姿態(tài)。該方法能夠有效避免環(huán)境背景變化干擾，具有較高的定位精度和實(shí)時(shí)性。

附圖說(shuō)明

圖1是基于SURF算法的雙目全景視覺(jué)機(jī)器人自主定位流程方框圖；

圖2是移動(dòng)機(jī)器人雙目全景視覺(jué)系統(tǒng)定位原理圖；

圖3是雙目全景視覺(jué)系統(tǒng)成像原理圖。

具體實(shí)施方式

下面結(jié)合附圖對(duì)本發(fā)明做進(jìn)一步描述：

基于SURF算法的人工路標(biāo)定位方法，包括如下步驟

(1)設(shè)置人工路標(biāo)作為先驗(yàn)位置信息

在未知非結(jié)構(gòu)化環(huán)境中，根據(jù)SURF特征點(diǎn)提取算法，采集環(huán)境全景圖像庫(kù)測(cè)試，設(shè)計(jì)弱干擾匹配路標(biāo)。在機(jī)器人工作環(huán)境內(nèi)遮擋度低的一側(cè)等間隔、等高設(shè)置兩個(gè)以上不同特征的人工路標(biāo)。

(2)改進(jìn)的MDGHM-SURF特征匹配算法

采用改進(jìn)MDGHM-SURF算法進(jìn)行路標(biāo)特征匹配定位，對(duì)全景圖像掩膜進(jìn)行等間隔采樣,并進(jìn)行像素坐標(biāo)變換，定義圖像I(i,j)的(p，q)階改進(jìn)后離散Gaussian-Hermit矩為：

其中利用改進(jìn)離散Gaussian-Hermit矩描述局部特征點(diǎn)，以描述特征點(diǎn)為中心，沿特征點(diǎn)主方向的正方形區(qū)域劃分為若干個(gè)子區(qū)域，各區(qū)域獲取多個(gè)采樣點(diǎn)，在X和Y方向上分別描述。為提高算法匹配的正確率，以最近鄰距離與次近鄰歐式距離的比值作為匹配特征向量之間相似度的準(zhǔn)則，比值越小相似度越大。

(3)匹配點(diǎn)重心迭代算法消除誤匹配

由步驟(2)得匹配點(diǎn)坐標(biāo)為A_i(x_i,y_i),匹配度為m_i設(shè)計(jì)的路標(biāo)的特征均布，則匹配路標(biāo)點(diǎn)重心為通過(guò)計(jì)算A_i距離Z的歐式距離d_i，建立循環(huán)求取距離最大值d_imax,設(shè)定距離閾值d_H，假如d_imax>d_H,則刪除該誤匹配點(diǎn)，重新迭代計(jì)算重心，直到d′_imax≤d_H，實(shí)現(xiàn)路標(biāo)中心精確定位。

(4)垂直基線式雙目全景系統(tǒng)三維逆光路成像測(cè)距算法。

由步驟(3)定位出上下全景中各路標(biāo)位置后，計(jì)算圖像中路標(biāo)距全景中心的像素距離，根據(jù)全景成像旋轉(zhuǎn)對(duì)稱性，取一個(gè)光軸截面為研究平面，雙曲面鏡的上支頂點(diǎn)為坐標(biāo)原點(diǎn)，水平、豎直方向分別為X、Y軸建立坐標(biāo)系,雙曲面鏡曲線方程為(y-c)²/a²-x²/b²＝1其中a、b、c分別為雙曲面鏡實(shí)軸、虛軸和焦距長(zhǎng)度，在全景圖像中測(cè)得路標(biāo)點(diǎn)距全景中心的距離為r₁，相機(jī)標(biāo)定焦距為f，根據(jù)折反射成像原理有r/f＝m/n，可求解出鏡面曲線上反射點(diǎn)的坐標(biāo)為P(m,n)

根據(jù)垂直雙目全景系統(tǒng)結(jié)構(gòu)，入射光線的斜率為k＝(n-2c)/m。假設(shè)上下全景系統(tǒng)的入射光線與雙曲面鏡的交點(diǎn)分別為Q₁(m₁,n₁)和Q₂(m₂,n₂)，其中L為基線距離，上下全景相機(jī)的入射光線方程為

聯(lián)立方程組求解與路標(biāo)水平距離：

在實(shí)際應(yīng)用中，當(dāng)透視相機(jī)和反射鏡面的焦點(diǎn)沒(méi)有嚴(yán)格重合時(shí)，將不滿足單視點(diǎn)約束。若仍按單視點(diǎn)測(cè)距算法求解，誤差較大,必須對(duì)非單視點(diǎn)進(jìn)行優(yōu)化。由于入射反射鏡面光線的延長(zhǎng)線不經(jīng)過(guò)雙曲線焦點(diǎn)，設(shè)鏡面直徑為R，相機(jī)和雙曲面鏡的焦點(diǎn)存在偏移量t，則折反射成像原理t＝Rf/r-H。由此可得變化后鏡面方程(y-c-t)²/a²-x²/b²＝1,得到雙曲面反射點(diǎn)Q(m′,n′)的坐標(biāo)。同理，可求解上下全景相機(jī)的入射光線方程y′₁＝k₁x+n₁-k₁m₁-L，距離由求解。

(5)全景三角定位算法

由步驟(4)得到機(jī)器人與等高設(shè)置在同一墻壁A(x₁,0)，B(x₂,0)位置的2個(gè)環(huán)境路標(biāo)距離為d₁,d₂，在全景圖像中定位出兩路標(biāo)距中心的夾角β，所以機(jī)器人的坐標(biāo)計(jì)算公式為

根據(jù)全景圖像中路標(biāo)角度和機(jī)器人絕對(duì)的位置坐標(biāo)關(guān)系可以實(shí)現(xiàn)對(duì)于姿態(tài)的測(cè)量。

(6)預(yù)測(cè)檢測(cè)窗口加快定位速度

根據(jù)步驟(5)中機(jī)器人初始化定位后，機(jī)器人的絕對(duì)位置坐標(biāo)和各路標(biāo)在全景圖像中的位置已經(jīng)確定，由于機(jī)器人最大運(yùn)動(dòng)速度有限，對(duì)應(yīng)路標(biāo)在全景圖像中移動(dòng)范圍較小，為減小圖像SURF特征檢測(cè)區(qū)域，降低運(yùn)算量，利用機(jī)器人當(dāng)前的運(yùn)動(dòng)狀態(tài)，估測(cè)下一刻機(jī)器人全景圖像中路標(biāo)檢測(cè)區(qū)域，實(shí)現(xiàn)移動(dòng)中快速定位。

在機(jī)器人自主定位導(dǎo)航方面，使用雙目立體視覺(jué)系統(tǒng)獲取空間景物的度量信息是最常用的技術(shù)手段，雙目全景在機(jī)器人定位導(dǎo)航中的應(yīng)用擁有巨大潛力，該技術(shù)滿足了智能駕駛、安防監(jiān)控、運(yùn)動(dòng)目標(biāo)的跟蹤、三維測(cè)量及重構(gòu)、視頻會(huì)議等用途，更廣泛用于軍事偵察、地形勘測(cè)、安全鑒定、空中導(dǎo)航、機(jī)器人視覺(jué)、虛擬現(xiàn)實(shí)等領(lǐng)域，已成為機(jī)器視覺(jué)研究的熱點(diǎn)方向。針對(duì)復(fù)雜環(huán)境下機(jī)器人的定位問(wèn)題，提出了基于SURF算法實(shí)現(xiàn)的雙目全景機(jī)器人自主定位方法。

實(shí)現(xiàn)步驟如下：

步驟1設(shè)置人工路標(biāo)作為先驗(yàn)位置信息。根據(jù)SURF特征點(diǎn)提取算法，采集未知非結(jié)構(gòu)化環(huán)境全景圖像庫(kù)測(cè)試，設(shè)計(jì)弱干擾匹配路標(biāo)。在機(jī)器人工作環(huán)境內(nèi)遮擋度低的一側(cè)設(shè)置多個(gè)不同特征的路標(biāo)。

步驟2改進(jìn)的MDGHM-SURF特征匹配算法。采用改進(jìn)MDGHM-SURF算法進(jìn)行路標(biāo)特征匹配定位，對(duì)全景圖像掩膜進(jìn)行等間隔采樣,像素坐標(biāo)變換，利用改進(jìn)離散Gaussian-Hermit矩描述局部特征點(diǎn)并匹配。

步驟3匹配點(diǎn)重心迭代算法消除誤匹配。迭代計(jì)算匹配重心，比較匹配距離閾值，刪除誤匹配點(diǎn)，實(shí)現(xiàn)路標(biāo)中心精確定位。

步驟4垂直基線式雙目全景系統(tǒng)三維逆光路成像測(cè)距算法。由步驟(3)定位出上下全景中各路標(biāo)位置，計(jì)算圖像中路標(biāo)距全景中心的像素距離，根據(jù)成像原理求解機(jī)器人與路標(biāo)水平距離。

步驟5全景三角定位算法

由步驟4得到機(jī)器人與路標(biāo)距離關(guān)系，在全景圖像中定位出兩路標(biāo)距中心的夾角，解算機(jī)器人坐標(biāo)，由全景圖像中路標(biāo)角度和機(jī)器人絕對(duì)的位置坐標(biāo)關(guān)系可以實(shí)現(xiàn)對(duì)于姿態(tài)的測(cè)量。

步驟6預(yù)測(cè)檢測(cè)窗口加快定位速度

根據(jù)步驟5中機(jī)器人初始化定位后，根據(jù)機(jī)器人的運(yùn)動(dòng)狀態(tài)，估測(cè)機(jī)器人全景圖像中路標(biāo)檢測(cè)區(qū)域，加快運(yùn)動(dòng)中定位速度。

實(shí)施例：

1.設(shè)置人工路標(biāo)作為先驗(yàn)位置信息

由于移動(dòng)機(jī)器人在執(zhí)行任務(wù)過(guò)程中，常常工作在未知非結(jié)構(gòu)化環(huán)境中，為將陌生環(huán)境轉(zhuǎn)化為熟悉環(huán)境，使用路標(biāo)作為先驗(yàn)位置信息，根據(jù)SURF特征點(diǎn)提取算法，采集環(huán)境圖像庫(kù)測(cè)試，設(shè)計(jì)弱干擾匹配路標(biāo)。在機(jī)器人工作環(huán)境內(nèi)遮擋度低的一側(cè)等間隔、等高設(shè)置兩個(gè)以上不同特征的人工路標(biāo)。

2.改進(jìn)的MDGHM-SURF特征匹配算法

使用改進(jìn)的MDGHM-SURF算法進(jìn)行路標(biāo)特征匹配定位，對(duì)全景圖像掩膜進(jìn)行等間隔采樣,并進(jìn)行像素坐標(biāo)變換，全景圖像I(i,j)的(p，q)階改進(jìn)后離散Gaussian-Hermit矩定義為：

其中利用改進(jìn)離散Gaussian-Hermit矩描述局部特征點(diǎn)，以描述特征點(diǎn)為中心，沿特征點(diǎn)主方向的正方形區(qū)域劃分為若干個(gè)子區(qū)域，各區(qū)域獲取多個(gè)采樣點(diǎn)，在X和Y方向上分別描述。為提高算法匹配的正確率，以最近鄰距離與次近鄰歐式距離的比值作為匹配特征向量之間相似度的準(zhǔn)則，比值越小相似度越大。

3.匹配點(diǎn)重心迭代算法消除誤匹配

由步驟(2)在全景圖像中匹配點(diǎn)坐標(biāo)為A_i(x_i,y_i),匹配度為m_i設(shè)計(jì)的路標(biāo)的特征均布，則匹配路標(biāo)點(diǎn)重心為通過(guò)計(jì)算A_i距離Z的歐式距離d_i，建立循環(huán)求取距離最大值d_imax,設(shè)定距離閾值d_H，假如d_imax>d_H,則刪除該誤匹配點(diǎn)，重新迭代計(jì)算重心，直到d′_imax≤d_H，實(shí)現(xiàn)路標(biāo)中心精確定位。

4.垂直基線式雙目全景系統(tǒng)三維逆光路成像測(cè)距算法。

由步驟(3)定位出上下全景中各路標(biāo)位置后，計(jì)算圖像中路標(biāo)距全景中心的像素距離，根據(jù)全景成像旋轉(zhuǎn)對(duì)稱性，取一個(gè)光軸截面為研究平面，雙曲面鏡的上支頂點(diǎn)為坐標(biāo)原點(diǎn)，水平、豎直方向分別為X、Y軸建立坐標(biāo)系(如圖3所示)，雙曲面鏡曲線方程為(y-c)²/a²-x²/b²＝1其中a、b、c分別為雙曲面鏡實(shí)軸、虛軸和焦距長(zhǎng)度，在全景圖像中測(cè)得路標(biāo)點(diǎn)距全景中心的距離為r₁，相機(jī)標(biāo)定焦距為f，根據(jù)折反射成像原理有r/f＝m/n，可求解出鏡面曲線上反射點(diǎn)的坐標(biāo)為P(m,n)

根據(jù)垂直雙目全景系統(tǒng)結(jié)構(gòu)，入射光線的斜率為k＝(n-2c)/m。假設(shè)上下全景系統(tǒng)的入射光線與雙曲面鏡的交點(diǎn)分別為Q₁(m₁,n₁)和Q₂(m₂,n₂)，其中L為基線距離，上下全景相機(jī)的入射光線方程。

聯(lián)立方程組求解與路標(biāo)水平距離：

在實(shí)際應(yīng)用中，透視相機(jī)和反射鏡面的焦點(diǎn)沒(méi)有嚴(yán)格重合，不滿足單視點(diǎn)約束。若仍按單視點(diǎn)測(cè)距算法求解，誤差較大,必須對(duì)非單視點(diǎn)進(jìn)行優(yōu)化。由于入射反射鏡面光線的延長(zhǎng)線不經(jīng)過(guò)雙曲線焦點(diǎn)，設(shè)鏡面直徑為R，相機(jī)和雙曲面鏡的焦點(diǎn)存在偏移量t，則折反射成像原理t＝Rf/r-H。由此可得變化后鏡面方程(y-c-t)²/a²-x²/b²＝1,得到雙曲面反射點(diǎn)Q(m′,n′)的坐標(biāo)

上下全景相機(jī)的入射光線方程為y′₁＝k₁x+n₁-k₁m₁-L，距離通過(guò)公式求解。

5.全景三角定位算法

由步驟(4)得到機(jī)器人與等高設(shè)置在同一墻壁A(x₁,0)，B(x₂,0)位置的2個(gè)環(huán)境路標(biāo)距離為d₁,d₂，在全景圖像中定位出兩路標(biāo)距中心的夾角β(如圖2所示)，所以機(jī)器人的坐標(biāo)計(jì)算公式為

根據(jù)全景圖像中路標(biāo)角度和機(jī)器人絕對(duì)的位置坐標(biāo)關(guān)系可以實(shí)現(xiàn)對(duì)于姿態(tài)的測(cè)量。

6.預(yù)測(cè)檢測(cè)窗口加快定位速度

根據(jù)步驟(5)中機(jī)器人初始化定位后，機(jī)器人的絕對(duì)位置坐標(biāo)和各路標(biāo)在全景圖像中的位置已經(jīng)確定，由于機(jī)器人最大運(yùn)動(dòng)速度有限，對(duì)應(yīng)路標(biāo)在全景圖像中移動(dòng)范圍較小，為減小圖像SURF特征檢測(cè)區(qū)域，降低運(yùn)算量，可根據(jù)機(jī)器人的運(yùn)動(dòng)狀態(tài)，估測(cè)機(jī)器人全景圖像中路標(biāo)檢測(cè)區(qū)域。

具體實(shí)施例如下：

以路標(biāo)A為研究對(duì)象，全景中心坐標(biāo)為G(x₀,y₀),，假設(shè)t時(shí)刻路標(biāo)A在全景圖像中像素位置為A(ρ,θ,t)，以A為圓心，設(shè)置半徑為r的圓形檢測(cè)窗口，進(jìn)行SURF特征檢測(cè)與路標(biāo)匹配定位，假設(shè)機(jī)器人以角速度ω(t)在做原地旋轉(zhuǎn)運(yùn)動(dòng)，由于機(jī)器人距離路標(biāo)的距離不變，則路標(biāo)在全景圖像中的運(yùn)動(dòng)軌跡為以全景中心G為圓心，中心距離為半徑ρ的圓弧。所以t′時(shí)刻的檢測(cè)區(qū)域旋轉(zhuǎn)為以A′(ρ,θ+ω(t)Δt,t′)為中心的圓域。考慮機(jī)器人有平移速度時(shí)，路標(biāo)在圖像中像素變化的最大梯度方向?yàn)樗俣确较?，設(shè)機(jī)器人以最大速度運(yùn)行時(shí)，對(duì)應(yīng)的全景圖像中像素改變最快速度為V_max，所以t′時(shí)刻的檢測(cè)區(qū)域旋轉(zhuǎn)為以A′(ρ,θ+ω(t)Δt,t′)為中心，半徑為V_maxΔt+r的圓域，則預(yù)測(cè)在此圓域進(jìn)行路標(biāo)檢測(cè)，實(shí)現(xiàn)移動(dòng)中快速定位。