一種視覺信息輔助的無(wú)人機(jī)自主導(dǎo)航定位方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種視覺信息輔助的無(wú)人機(jī)自主導(dǎo)航定位方法，特別是一種基于視覺 測(cè)程結(jié)果輔助慣性導(dǎo)航系統(tǒng)的無(wú)人機(jī)自主導(dǎo)航定位方法，適用于無(wú)人機(jī)自主導(dǎo)航和定位系 統(tǒng)。
【背景技術(shù)】
[0002] 無(wú)人機(jī)是一類低風(fēng)險(xiǎn)低成本，可以自主運(yùn)作完成各項(xiàng)任務(wù)的自動(dòng)化飛行載具，被 廣泛用于自動(dòng)運(yùn)輸、區(qū)域監(jiān)視、災(zāi)后重建、復(fù)雜環(huán)境探測(cè)以及地面目標(biāo)跟蹤等軍民用領(lǐng)域 中。為了在無(wú)人監(jiān)測(cè)的情況下自主完成任務(wù)，一個(gè)穩(wěn)定、快速、精確的導(dǎo)航系統(tǒng)對(duì)于無(wú)人機(jī) 而言至關(guān)重要。由于低成本慣性導(dǎo)航系統(tǒng)誤差發(fā)散很快，依靠單一的低成本慣性導(dǎo)航系統(tǒng) 很無(wú)法滿足實(shí)際應(yīng)用要求。將慣性導(dǎo)航系統(tǒng)與GNSS信息進(jìn)行融合來獲得無(wú)人機(jī)精確導(dǎo)航 定位信息的方法已被廣泛應(yīng)用。但由于當(dāng)GNSS信號(hào)受到外界干擾或欺騙，甚至信號(hào)轉(zhuǎn)發(fā)源 被摧毀時(shí)，GNSS系統(tǒng)無(wú)法長(zhǎng)期提供穩(wěn)定可靠的定位信息。因此，在軍民用領(lǐng)域中，尤其是室 內(nèi)、叢林、多雨地區(qū)等應(yīng)用環(huán)境下，尚不能完全依賴于GNSS來輔助慣性導(dǎo)航實(shí)現(xiàn)無(wú)人機(jī)的 精確導(dǎo)航定位。為了解決這個(gè)問題，引入適當(dāng)?shù)膶?dǎo)航傳感器和新的導(dǎo)航算法來解決GNSS短 期和長(zhǎng)期故障時(shí)無(wú)人機(jī)自主導(dǎo)航定位問題，已成為軍民用領(lǐng)域無(wú)人機(jī)應(yīng)用研宄的重點(diǎn)。
[0003] 視覺傳感器作為一種兼具導(dǎo)航傳感器和任務(wù)傳感器雙重特性的設(shè)備，因其體積 小，安裝簡(jiǎn)單，信息豐富等特點(diǎn)，在無(wú)人機(jī)上已經(jīng)得到了廣泛應(yīng)用。而且基于視覺傳感器的 導(dǎo)航算法無(wú)需增添額外設(shè)備即可提供較準(zhǔn)確的導(dǎo)航信息，是目前研宄與應(yīng)用的重點(diǎn)。
[0004] 本發(fā)明中提到一種視覺信息輔助的無(wú)人機(jī)自主導(dǎo)航定位方法，該方法根據(jù)視覺測(cè) 程原理，通過組合式視覺測(cè)程算法來推算無(wú)人機(jī)的速度和位置信息，用于輔助慣性導(dǎo)航定 位。在GNSS信號(hào)可用時(shí)，利用慣性導(dǎo)航/GNSS組合導(dǎo)航來修正慣性導(dǎo)航系統(tǒng)誤差，并建立 視覺測(cè)程評(píng)估準(zhǔn)則；當(dāng)GNSS信號(hào)不可用時(shí)，利用視覺測(cè)程評(píng)估準(zhǔn)則對(duì)組合式視覺測(cè)程算法 獲得的視覺測(cè)程結(jié)果進(jìn)行評(píng)估，利用視覺測(cè)程結(jié)果和視覺測(cè)程評(píng)估結(jié)果估計(jì)并補(bǔ)償慣導(dǎo)系 統(tǒng)的誤差，為無(wú)人機(jī)提供連續(xù)、可靠的導(dǎo)航定位信息。
[0005] 本發(fā)明與其它無(wú)人機(jī)自主導(dǎo)航和定位方法不同在于：在當(dāng)前已公開發(fā)表的文獻(xiàn) 中，依靠視覺導(dǎo)航結(jié)果輔助慣性導(dǎo)航系統(tǒng)提供無(wú)人機(jī)的導(dǎo)航定位信息，但這些導(dǎo)航方法在 實(shí)際應(yīng)用中無(wú)法根據(jù)外部環(huán)境和導(dǎo)航結(jié)果有針對(duì)性地采取不同的導(dǎo)航策略，且無(wú)法評(píng)估視 覺導(dǎo)航信息的有效性，使視覺導(dǎo)航信息易受誤差影響，無(wú)法提供長(zhǎng)時(shí)間穩(wěn)定的導(dǎo)航信息而 導(dǎo)致無(wú)法完成預(yù)定任務(wù)，甚至導(dǎo)致無(wú)人機(jī)和機(jī)載設(shè)備有損毀的風(fēng)險(xiǎn)。

【發(fā)明內(nèi)容】

[0006] 本發(fā)明要解決的技術(shù)問題：為解決在GNSS信號(hào)短時(shí)間和長(zhǎng)時(shí)間不可用時(shí)，導(dǎo)航系 統(tǒng)能夠長(zhǎng)時(shí)間提供穩(wěn)定的姿態(tài)和定位信息來控制無(wú)人機(jī)并穩(wěn)定、可靠地完成既定任務(wù)，提 出一種視覺信息輔助的無(wú)人機(jī)自主導(dǎo)航定位方法。
[0007] 本發(fā)明采用的技術(shù)方案為：一種視覺信息輔助的無(wú)人機(jī)自主導(dǎo)航定位方法，其特 征在于通過組合式視覺測(cè)程算法提供較準(zhǔn)確的視覺測(cè)程結(jié)果；在GNSS信號(hào)可用時(shí)，利用慣 性導(dǎo)航/GNSS組合導(dǎo)航來修正慣性導(dǎo)航系統(tǒng)誤差，并建立視覺測(cè)程評(píng)估準(zhǔn)則；當(dāng)GNSS信號(hào) 不可用時(shí)，利用視覺測(cè)程評(píng)估準(zhǔn)則對(duì)組合式視覺測(cè)程算法獲得的視覺測(cè)程結(jié)果進(jìn)行評(píng)估， 利用視覺測(cè)程結(jié)果和視覺測(cè)程評(píng)估結(jié)果估計(jì)并補(bǔ)償慣導(dǎo)系統(tǒng)的誤差，為無(wú)人機(jī)提供連續(xù)、 可靠的導(dǎo)航定位信息。具體實(shí)現(xiàn)步驟為：
[0008] (1)將采集到的圖像分為普通幀與關(guān)鍵幀，利用光流估計(jì)方法獲得普通幀的幀間 匹配，利用特征匹配方法獲得關(guān)鍵幀的幀間匹配，進(jìn)而根據(jù)最小誤差準(zhǔn)則，選取基于單應(yīng)性 矩陣的視覺測(cè)程方式和基于基礎(chǔ)矩陣的視覺測(cè)程方式中的最優(yōu)者來分別計(jì)算普通幀與關(guān) 鍵幀的幀間運(yùn)動(dòng)參數(shù)，并將兩種幀間運(yùn)動(dòng)參數(shù)進(jìn)行組合來獲得視覺測(cè)程結(jié)果，其具體方法 為：
[0009] al、以視覺傳感器輸出的圖像序列作為普通幀序列，利用金字塔Lucas-Kanade光 流估計(jì)方法估計(jì)普通幀間的光流場(chǎng)，實(shí)現(xiàn)普通幀的幀間匹配；同時(shí)，取第一幀為關(guān)鍵幀，且 每隔八個(gè)普通幀取出一幀作為關(guān)鍵幀，利用基于SURF的特征匹配算法實(shí)現(xiàn)關(guān)鍵幀的幀間 匹配；利用隨機(jī)采樣一致性RANSAC算法分別剔除普通幀的幀間匹配和關(guān)鍵幀的幀間匹配 中的誤匹配點(diǎn)，當(dāng)關(guān)鍵幀的幀間匹配數(shù)小于設(shè)定的閾值，給出錯(cuò)誤標(biāo)識(shí)1 ;
[0010] a2、從普通幀的幀間匹配點(diǎn)和關(guān)鍵幀的幀間匹配點(diǎn)中分別隨機(jī)選取匹配點(diǎn)總數(shù)的 50 %來建立圖像幀間單應(yīng)性矩陣和基礎(chǔ)矩陣約束方程分別為：
[0011] X' = Hx (1)
[0012] X'tFx = O (2)
[0013] 通過求解式（1)和（2)獲得圖像幀間單應(yīng)性矩陣與基礎(chǔ)矩陣；重復(fù)上述計(jì)算過程 三次，分別獲得獨(dú)立的三組單應(yīng)性矩陣和基礎(chǔ)矩陣，如果單應(yīng)性矩陣的誤差小于基礎(chǔ)矩陣 的誤差，則無(wú)人機(jī)外部環(huán)境為平坦空曠的二維環(huán)境，通過對(duì)單應(yīng)性矩陣進(jìn)行分解來獲得無(wú) 人機(jī)相對(duì)運(yùn)動(dòng)參數(shù)；如果單應(yīng)性矩陣的誤差大于基礎(chǔ)矩陣的誤差，則無(wú)人機(jī)外部環(huán)境為復(fù) 雜的三維環(huán)境，通過對(duì)基礎(chǔ)矩陣進(jìn)行分解來獲得無(wú)人機(jī)相對(duì)運(yùn)動(dòng)參數(shù)；在針對(duì)關(guān)鍵幀的幀 間匹配展開計(jì)算時(shí)，如果得到的兩種矩陣誤差均大于設(shè)定的閾值，給出錯(cuò)誤標(biāo)識(shí)2 ;
[0014] a3、如果新采集的圖像未被選為關(guān)鍵幀，按照a2中計(jì)算過程來計(jì)算普通幀的幀間 運(yùn)動(dòng)參數(shù)，并根據(jù)前一個(gè)普通幀計(jì)算出的無(wú)人機(jī)位置和速度信息來計(jì)算當(dāng)前幀無(wú)人機(jī)的位 置和速度信息作為視覺測(cè)程結(jié)果；
[0015] a4、如果新采集的圖像被選取為關(guān)鍵幀，按照a2中計(jì)算過程來計(jì)算關(guān)鍵幀的幀間 運(yùn)動(dòng)參數(shù)，并對(duì)關(guān)鍵幀的幀間運(yùn)動(dòng)參數(shù)計(jì)算過程進(jìn)行評(píng)估檢測(cè)；如果未檢測(cè)到錯(cuò)誤標(biāo)識(shí)，則 當(dāng)前關(guān)鍵幀為有效關(guān)鍵幀，利用當(dāng)前關(guān)鍵幀的幀間運(yùn)動(dòng)參數(shù)和前一關(guān)鍵幀計(jì)算出的無(wú)人機(jī) 位置和速度計(jì)算當(dāng)前幀無(wú)人機(jī)的位置和速度信息作為視覺測(cè)程結(jié)果；如果出現(xiàn)在al中所 述的錯(cuò)誤標(biāo)識(shí)1或在a2中所述的錯(cuò)誤標(biāo)識(shí)2,則當(dāng)前關(guān)鍵幀為無(wú)效關(guān)鍵幀，并將當(dāng)前關(guān)鍵幀 按普通幀進(jìn)行處理，按a3中的計(jì)算過程來計(jì)算視覺測(cè)程結(jié)果。
[0016] (2)當(dāng)GNSS信號(hào)可用時(shí)，利用一個(gè)七狀態(tài)Kalman濾波器實(shí)現(xiàn)慣導(dǎo)系統(tǒng)/GNSS組 合，估計(jì)并補(bǔ)償慣導(dǎo)系統(tǒng)的誤差，獲得精確的無(wú)人機(jī)的位置、速度和姿態(tài)全狀態(tài)導(dǎo)航定位信 息，并以全狀態(tài)導(dǎo)航定位信息為基準(zhǔn)值，計(jì)算視覺測(cè)程結(jié)果的誤差，建立視覺測(cè)程評(píng)估準(zhǔn) 貝IJ，其具體方法為：
[0017] bl、根據(jù)慣性導(dǎo)航系統(tǒng)誤差方程建立慣性導(dǎo)航系統(tǒng)狀態(tài)方程為： (3)
【主權(quán)項(xiàng)】
1. 一種視覺信息輔助的無(wú)人機(jī)自主導(dǎo)航定位方法，其特征在于：利用組合式視覺測(cè)程 算法獲取無(wú)人機(jī)的視覺測(cè)程結(jié)果，當(dāng)GNSS信號(hào)可用時(shí)，利用慣性導(dǎo)航/GNSS組合導(dǎo)航來修 正慣性導(dǎo)航系統(tǒng)誤差，建立視覺測(cè)程評(píng)估準(zhǔn)則；當(dāng)GNSS信號(hào)不可用時(shí)，利用視覺測(cè)程評(píng)估 準(zhǔn)則對(duì)視覺測(cè)程結(jié)果進(jìn)行評(píng)估，將經(jīng)評(píng)估后的視覺測(cè)程信息與慣性導(dǎo)航系統(tǒng)信息進(jìn)行融 合，估計(jì)并補(bǔ)償慣導(dǎo)系統(tǒng)的誤差，為無(wú)人機(jī)提供連續(xù)、可靠的導(dǎo)航定位信息，其具體實(shí)現(xiàn)過 程： 步驟（1)、將采集到的圖像分為普通幀與關(guān)鍵幀，利用光流估計(jì)方法獲得普通幀的幀間 匹配，利用特征匹配方法獲得關(guān)鍵幀的幀間匹配，進(jìn)而根據(jù)最小誤差準(zhǔn)則，選取基于單應(yīng)性 矩陣的視覺測(cè)程方式和基于基礎(chǔ)矩陣的視覺測(cè)程方式中的最優(yōu)者來分別計(jì)算普通幀與關(guān) 鍵幀的幀間運(yùn)動(dòng)參數(shù)，并將兩種幀間運(yùn)動(dòng)參數(shù)進(jìn)行組合來獲得視覺測(cè)程結(jié)果； 步驟（2)、當(dāng)GNSS信號(hào)可用時(shí)，利用一個(gè)七狀態(tài)Kalman濾波器實(shí)現(xiàn)慣導(dǎo)系統(tǒng)/GNSS組 合，估計(jì)并補(bǔ)償慣導(dǎo)系統(tǒng)的誤差，獲得精確的無(wú)人機(jī)的位置、速度和姿態(tài)全狀態(tài)導(dǎo)航定位信 息，并以全狀態(tài)導(dǎo)航定位信息為基準(zhǔn)值，計(jì)算視覺測(cè)程結(jié)果的誤差，建立視覺測(cè)程評(píng)估準(zhǔn) 則； 步驟（3)、當(dāng)GNSS信號(hào)不可用時(shí)，利用運(yùn)動(dòng)模型預(yù)測(cè)無(wú)人機(jī)的運(yùn)動(dòng)信息，計(jì)算無(wú)人機(jī)運(yùn) 動(dòng)的預(yù)測(cè)值與視覺測(cè)程結(jié)果的誤差，根據(jù)這一誤差和視覺測(cè)程評(píng)估準(zhǔn)則得出視覺測(cè)程評(píng)估 結(jié)果； 步驟（4)、根據(jù)視覺測(cè)程評(píng)估結(jié)果，利用七狀態(tài)Kalman濾波融合視覺測(cè)程結(jié)果和慣性 導(dǎo)航系統(tǒng)信息，估計(jì)并補(bǔ)償慣性導(dǎo)航系統(tǒng)的誤差，獲得無(wú)人機(jī)精