本發(fā)明涉及導(dǎo)航領(lǐng)域，尤其涉及一種基于光流的導(dǎo)航方法。

背景技術(shù)：

通常多旋翼無(wú)人機(jī)采用GPS/MEMS慣性組合導(dǎo)航系統(tǒng)來(lái)提供飛行器的導(dǎo)航信息。但是GPS從本質(zhì)上屬于無(wú)線電導(dǎo)航，在叢林、峽谷及室內(nèi)等特殊環(huán)境會(huì)因信號(hào)丟失而出現(xiàn)失鎖情況。由于MEMS慣性器件的精度較低，誤差隨時(shí)間發(fā)散較快，因此無(wú)法單獨(dú)提供有效的導(dǎo)航信息。所以需要引入一種新的導(dǎo)航方式來(lái)抑制慣性器件的誤差發(fā)散。

科學(xué)研究表明，自然界中的蜜蜂通過(guò)使用其特殊的眼睛感知外界光線的變化來(lái)進(jìn)行導(dǎo)航和避障，進(jìn)而啟發(fā)了人們使用光流法來(lái)進(jìn)行導(dǎo)航。隨著計(jì)算機(jī)視覺(jué)的發(fā)展，光流導(dǎo)航因其具有成本低、體積小、重量輕等優(yōu)點(diǎn)被應(yīng)用于微小型無(wú)人機(jī)導(dǎo)航領(lǐng)域。

由于經(jīng)典的光流算法是建立在三個(gè)假設(shè)之上的，因此多旋翼飛行器實(shí)際飛行環(huán)境中可能出現(xiàn)的光照強(qiáng)度變化、圖像遮擋及大尺度運(yùn)動(dòng)等因素都會(huì)對(duì)光流導(dǎo)航信息的可靠性造成影響。

采用光流/慣性組合導(dǎo)航系統(tǒng)，可以利用光流導(dǎo)航算法的速度信息來(lái)抑制慣導(dǎo)信息的發(fā)散，同時(shí)也可以利用慣導(dǎo)輸出的信息來(lái)提升光流信息求解的精度，從而提升導(dǎo)航系統(tǒng)的可靠性，增強(qiáng)多旋翼無(wú)人機(jī)在無(wú)GPS環(huán)境下的生存能力。

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

鑒于上述問(wèn)題，本發(fā)明提供一種基于光流的導(dǎo)航方法。

一種基于光流的導(dǎo)航方法，其中，包括：

建立飛行器的前右下坐標(biāo)系，前向指向飛行器的機(jī)頭，光流傳感器采集若干圖像，并檢測(cè)出每張所述圖像中特征點(diǎn)的坐標(biāo)集，挑選任兩張所述圖像的特征點(diǎn)進(jìn)行對(duì)比，篩選出相同特征點(diǎn)在不同圖像上的坐標(biāo)集，計(jì)算相同特征點(diǎn)在不同圖像上坐標(biāo)點(diǎn)之間的幾何距離，以此計(jì)算出特征點(diǎn)在圖像上的飛行速度；

定義所述飛行器的導(dǎo)航系坐標(biāo)，并記錄所述飛行器的水平飛行速度；

將特征點(diǎn)在圖像上的飛行速度和所述水平飛行速度進(jìn)行融合處理，以得到所述飛行器的導(dǎo)航信息。

上述的方法，其中，所述方法還包括：

以慣性導(dǎo)航信息求解所述特征點(diǎn)位移作為光流的理想值，結(jié)合所述特征點(diǎn)求解的光流值計(jì)算每個(gè)特征點(diǎn)光流的角誤差，從而篩選出角誤差小的特征點(diǎn)。

上述的方法，其中，所述方法還包括：

選取若干誤差狀態(tài)量，建立狀態(tài)方程，以特征點(diǎn)在像素坐標(biāo)系下的速度誤差為量測(cè)量，建立量測(cè)方程，利用卡爾曼濾波遞推方程估計(jì)所述飛行器的飛行速度。

上述的方法，其中，采用LK金字塔光流算法計(jì)算出特征點(diǎn)在圖像上的飛行速度。

上述的方法，其中，所述方法還包括：

利用慣性導(dǎo)航系統(tǒng)推出的特征點(diǎn)速度來(lái)修正LK金字塔光流算法計(jì)算出特征點(diǎn)在圖像上的飛行速度。

上述的方法，其中，所述導(dǎo)航信息包括所述飛行器的方向角，陀螺誤差和位置坐標(biāo)。

上述的方法，其中，選取的誤差狀態(tài)量包括捷聯(lián)慣性導(dǎo)航系統(tǒng)的數(shù)學(xué)平臺(tái)誤差角、所述飛行器的速度誤差、所述飛行的位置誤差、陀螺誤差和加速度計(jì)的誤差。

上述的方法，其中，采用LK金字塔光流算法估算出一張圖像中特征點(diǎn)在另一張圖像中的坐標(biāo)集。

綜上所述，本發(fā)明設(shè)計(jì)的一種基于光流的導(dǎo)航方法，通過(guò)將慣導(dǎo)的速度信息轉(zhuǎn)換到圖像坐標(biāo)系下，采用圖像特征點(diǎn)的像素速度作為量測(cè)量對(duì)光流和慣性的信息進(jìn)行深度融合，從而利用光流的信息對(duì)慣導(dǎo)的誤差進(jìn)行修正，通過(guò)計(jì)算特征點(diǎn)光流的角誤差，利用慣導(dǎo)的信息提升光流的可靠性，最終組合導(dǎo)航系統(tǒng)輸出更為可靠的導(dǎo)航信息供多旋翼無(wú)人機(jī)使用。

附圖說(shuō)明

參考所附附圖，以更加充分的描述本發(fā)明的實(shí)施例。然而，所附附圖僅用于說(shuō)明和闡述，并不構(gòu)成對(duì)本發(fā)明范圍的限制。

圖1為本發(fā)明的多旋翼飛行器運(yùn)動(dòng)場(chǎng)與機(jī)載相機(jī)測(cè)量的光流場(chǎng)之間關(guān)系的示意圖；

圖2為本發(fā)明所采用的光流算法結(jié)構(gòu)示意圖；

圖3是基于特征點(diǎn)像素速度組合的光流/慣性組合導(dǎo)航算法結(jié)構(gòu)示意圖。

具體實(shí)施方式

下面結(jié)合附圖和具體的實(shí)施例對(duì)本發(fā)明作進(jìn)一步的說(shuō)明，但是不作為本發(fā)明的限定。

實(shí)施例

根據(jù)圖1所示，設(shè)定多旋翼無(wú)人機(jī)機(jī)體系為“前右下”坐標(biāo)系(即機(jī)頭的方向)，將相機(jī)與機(jī)體固連作為一個(gè)剛體。相機(jī)坐標(biāo)系的x，y，z分別與機(jī)體系的三個(gè)方向相重合。p為t時(shí)刻世界坐標(biāo)系下特征點(diǎn)P在圖像中的坐標(biāo)，p′為t+k時(shí)刻特征點(diǎn)P在圖像中的坐標(biāo)。由此，可以推導(dǎo)出圖像特征點(diǎn)的光流為

其中[u v]^T為T(mén)時(shí)刻到T+k時(shí)刻特征點(diǎn)P在圖像坐標(biāo)系下兩個(gè)方向的光流，對(duì)其求導(dǎo)可以得到特征點(diǎn)在兩個(gè)方向的像素速度：

在得到像素速度之后結(jié)合相機(jī)的成像模型，即可推出圖像光流場(chǎng)與載體運(yùn)動(dòng)場(chǎng)之間的關(guān)系：

式中f為相機(jī)的焦距，Z為載體的飛行高度，ω_x,ω_y為載體在x，y兩個(gè)方向的角速度，[T_x T_y]^T載體在水平方向上的速度；

依據(jù)圖2為ORB特征點(diǎn)光流算法的流程圖，其主要的步驟為：

1、相機(jī)在t時(shí)刻圖像I_t，并將其進(jìn)行灰度變換得到灰度圖G_t；

2、利用ORB特征點(diǎn)檢測(cè)算法，獲取G_t的局部特征點(diǎn)坐標(biāo)p_t；

3、采集t+k時(shí)刻圖像I_t+k，進(jìn)行灰度轉(zhuǎn)換后得到灰度圖G_t+k；

4、將G_t，G_t+k，p_t作為輸入，采用金字塔LK光流算法估算G_t+k圖像上特征點(diǎn)坐標(biāo)p_t+k；

5、將G_t+k,G_t,p_t+k作為輸入，利用4中步驟，計(jì)算出G_t圖像特征點(diǎn)坐標(biāo)

6、計(jì)算與p_t之間的距離d并對(duì)其求均值，將大于均值的點(diǎn)認(rèn)為是跟蹤誤差較大的點(diǎn)，進(jìn)行剔除；

7、利用p_t+k與p_t求解準(zhǔn)確光流；

圖3為基于特征點(diǎn)像素速度的光流/慣性組合導(dǎo)航算法結(jié)構(gòu)示意圖，其主要的步驟為：

1、將導(dǎo)航系定義為北東地坐標(biāo)系，k時(shí)刻之間機(jī)載慣性導(dǎo)航系統(tǒng)測(cè)量得到的載體水平方向的速度為[v′_bx,v′_by]^T：

2、計(jì)算由慣性導(dǎo)航系統(tǒng)推導(dǎo)出的圖像坐標(biāo)系下的特征點(diǎn)速度為：

其中，f為相機(jī)的焦距，Z為相機(jī)離地面的高度，即載體的飛行高度,[ω′_x,ω′_y]^T為x，y兩個(gè)方向陀螺儀的測(cè)量值。因此，由慣性導(dǎo)航系統(tǒng)推導(dǎo)出來(lái)的圖像坐標(biāo)系下的特征點(diǎn)位移為：

3、計(jì)算各個(gè)特征點(diǎn)光流的角誤差

其中的表示第i個(gè)特征點(diǎn)光流的理想值，表示第i個(gè)特征點(diǎn)使用LK金字塔光流算法所估計(jì)的光流值，k為兩幅圖像之間相隔的幀數(shù)。由于相機(jī)兩幀之間的間隔時(shí)間較短，慣導(dǎo)信息的誤差較小，以慣導(dǎo)信息求解的特征點(diǎn)位移作為光流的理想值，將角誤差較大的點(diǎn)剔除，可以有效的降低圖像出現(xiàn)局部干擾造成的光流估算誤差。

4、選取捷聯(lián)慣性導(dǎo)航系統(tǒng)的數(shù)學(xué)平臺(tái)誤差角三個(gè)方向速度誤差[δV_n,δV_e,δV_d]^T，經(jīng)緯高三個(gè)方向的位置誤差[δL,δλ,δH]^T及陀螺和加速度計(jì)的誤差構(gòu)成15維狀量：

系統(tǒng)的噪聲矩陣W設(shè)置為：

其中，為陀螺儀的測(cè)量的白噪聲，為加速度測(cè)量的白噪聲；

5、以特征點(diǎn)在像素坐標(biāo)系下的速度誤差為量測(cè)量

由式(2)知，由慣導(dǎo)信息計(jì)算出的圖像特征點(diǎn)速度信息中包含了機(jī)體系下的速度誤差，由于狀態(tài)量中所含的速度誤差為導(dǎo)航系下誤差，二者之間的轉(zhuǎn)換關(guān)系可近似為：

6、聯(lián)合(9)，(10)兩式,可構(gòu)建量測(cè)方程如下：

其中[n_x,n_y]^T為量測(cè)噪聲，量測(cè)矩陣的設(shè)置如下：

通過(guò)上述的計(jì)算得出無(wú)人飛行器的組合導(dǎo)航系統(tǒng)輸出位置和速度，以此達(dá)到控制無(wú)人飛行器的飛行狀態(tài)的效果。

綜上所述，本申請(qǐng)?jiān)O(shè)計(jì)的基于光流的導(dǎo)航方法，采用計(jì)算特征點(diǎn)作為與p_I之間距離d的方式來(lái)剔除光流算法跟蹤出錯(cuò)的點(diǎn)，可以有效的提升光流算法的準(zhǔn)確性；利用慣性導(dǎo)航系統(tǒng)推出的特征點(diǎn)像素速度來(lái)修正光流速度信息，可以有效的增強(qiáng)光流算法抵抗圖像局部干擾的能力；采用基于特征點(diǎn)像素速度的光流/慣性組合導(dǎo)航算法相比于單純依靠慣性導(dǎo)航的方案準(zhǔn)確性與可靠性均有提升，可作為多旋翼無(wú)人機(jī)在無(wú)GPS環(huán)境自主導(dǎo)航的備選方案。

通過(guò)說(shuō)明和附圖，給出了具體實(shí)施方式的特定結(jié)構(gòu)的典型實(shí)施例，基于本發(fā)明精神，還可作其他的轉(zhuǎn)換。盡管上述發(fā)明提出了現(xiàn)有的較佳實(shí)施例，然而，這些內(nèi)容并不作為局限。

對(duì)于本領(lǐng)域的技術(shù)人員而言，閱讀上述說(shuō)明后，各種變化和修正無(wú)疑將顯而易見(jiàn)。因此，所附的權(quán)利要求書(shū)應(yīng)看作是涵蓋本發(fā)明的真實(shí)意圖和范圍的全部變化和修正。在權(quán)利要求書(shū)范圍內(nèi)任何和所有等價(jià)的范圍與內(nèi)容，都應(yīng)認(rèn)為仍屬本發(fā)明的意圖和范圍內(nèi)。