一種未知環境下無人機視覺/慣性組合導航方法

專利名稱：一種未知環境下無人機視覺/慣性組合導航方法
技術領域：
本發明涉及導航技術領域，具體來說是一種未知環境下無人機視覺/慣性組合導航方法。

背景技術：
無人機導航是指無人機依賴機載的導航設備和飛行控制系統來進行定位和控制并最終完成飛行任務的技術，它是無人機研制的關鍵技術之一。在無人機導航中常用的方法包括遙控指揮、慣性導航系統(Inertial Navigation System，INS)、衛星導航、組合導航等。
目前也逐漸開展視覺導航系統的研究，但基本上都是以GPS衛星導航為主，其他幾種導航方式為輔的方案。然而，由于無人機，特別是作戰用小型無人機的應用電磁環境惡劣，遙控通訊鏈路難以維持，GPS信號也容易受到干擾而無法使用。為了提高無人機導航的適應性，經常采用的導航方法包括慣性導航、景象匹配導航等。
然而這些方法仍然存在一些問題。慣性導航雖然具有自主性好、抗干擾能力強等優點，然而，相對于無人機體積和載荷來講，高精度的慣性導航系統體積龐大，重量和耗電量則遠遠超出了無人機的能力范圍，同時，慣性導航系統還存在著誤差累積等不足而難以單獨應用，無法實現無人機的全自主導航。
景象匹配導航的原理是利用航天、航空等遙感手段獲取飛行器規劃路徑下方的地物景象(基準圖)并存儲于機載計算機中；當攜帶相應傳感器的飛行器飛過預定位置范圍時，便即時測量出當地的地物景象(實時圖)；將實時圖和基準圖在機載計算機中進行相關匹配即可以確定出當前飛行器的準確位置，然后把該位置參數送到慣導系統以修正慣導系統積累的誤差，從而實現飛行器長時間的精確導航和制導。其特點是導航精度高，不存在INS中的誤差累積問題，但是其缺點在于必須依賴于機載的匹配基準圖才能對航空器進行導航定位，因此，其應用范圍受到了較大的限制而不能稱之為未知環境下的導航方法；同時，在景象匹配中如何對圖像進行矯正和在巨大的圖像數據庫中進行圖像匹配對無人機機載處理器也是一種考驗。
本發明中提到的未知環境下的視覺導航技術是利用視覺傳感器獲得圖像，并且通過圖像處理方法得到無人機導航參數的一種方式，該方法所采用的傳感器包括攝像頭、慣性測量單元(IMU)等。視覺傳感器具有輕便、低功耗、體積小等優點，十分適合在微小型無人機上使用，而本發明中所涉及到的IMU同樣適用于無人機場合。此外，視覺導航作為自主導航的一種，工作在被動模式，不需要接收外部信號，因此具有隱蔽性好，抗干擾能力強等優點，將其與IMU相結合，可以同時克服單個攝像頭無法得到景象深度信息、特征點匹配誤差大，慣性導航系統存在誤差累計問題等不足。目前基于視覺信息進行無人機導航是該領域研究的一個熱點。
本發明與其他無人機應用中的視覺方法不同在于在當前已公開發表的資料中，計算機視覺方法在無人機中的應用主要集中于自主著陸、目標識別以及作為載荷等。自主著陸中視覺方法僅僅應用于飛行器的著陸階段，無法應用于任務執行階段，而任務執行階段是飛行器應用的主要階段，也是體現無人機價值的階段，并且無人機視覺自主著陸方法要求已知著陸場信息，仍然屬于半自主的導航方式；目標識別則需要實現已知該目標信息，從而可以采用視覺方法得到目標相對于飛行器的位置信息，對于占視覺主要成分的自然環境卻無能為力；將視覺作為載荷應用于無人機則完全沒有考慮到視覺圖像中包含的豐富的導航信息。因此，雖然有將視覺方法應用于導航方面的研究，但是這些方法無一例外均要求已知環境信息或者需要設置人工參考信息等，無法實現真正意義上的“未知環境”下的導航應用。


發明內容
本發明提出一種未知環境下無人機視覺/慣性組合導航方法，其目的是為了解決在未知環境下無人機的組合導航問題，它采用了計算機視覺方法，實現了在無人機執行任務過程中的抗干擾、全自主導航。該方法可在不依賴外部信息的情況下，只利用機載慣性導航系統、高度表、機載視覺系統等被動傳感器和地面圖像特征，為飛行器在任務執行過程中提供導航信息，該方法可以作為衛星導航等半自主導航方式的備份導航手段，并且還可以在干擾、遮擋情況下短時單獨使用，從而提高飛行器的生存能力；也可以裝載于有人飛機或者地面、水下機器人等載體上作為導航信息來源。
本發明一種未知環境下無人機視覺/慣性組合導航方法，該方法涉及到圖像處理、慣性導航計算和濾波估計，因此，在進行主要導航方法之前首先需要進行坐標系定義、坐標系對準和濾波方程建立等。坐標系定義是進行此類導航方法的前提，在理論分析中的導航方法是建立在一定的坐標系關系基礎上的，但是在實際中由于安裝誤差和安裝條件的限制，難以做到與理論情況完全相符。因此，IMU測量到的載體加速度和角速度值與從視覺系統中提取到的此參數并非完全相同；同時，二者坐標原點也并非完全重合而是存在一定的偏差，這些因素在實際實現過程中都需要予以考慮。
在本發明中所述的導航過程主要通過5步來實現 步驟一圖像獲取并對圖像進行預處理； 圖像獲取是由攝像頭在機載計算機控制下完成的，由于載體環境和所用設備條件限制，所采集到的圖像質量難以保證，如果對攝像頭輸出圖像直接進行處理難免會影響該方法的效果甚至無法進行導航，因此，需要對圖像進行預處理。
本發明中采用的圖像為灰度圖像，為了滿足視覺導航系統對處理速度和信息量的要求采用了中值濾波方法對圖像進行預處理。
步驟二通過SIFT方法進行特征點提取； 視覺對慣性系統的約束主要體現在前后幾幀圖像中特征點坐標的變化上，因此，如何得到穩定的特征點成為該導航方法的首要問題，SIFT方法具有良好的穩定性、很高的匹配準確度，并且其特征點具有很好的分布特性，甚至在紋理不很明顯的情況下也能得到充足的特征點。因此，SIFT方法在視覺導航中得到了廣泛的應用。
SIFT方法進行特征點提取分為四步驟來實現 1)檢測尺度空間極值點； 2)精確確定極值點位置；將位于邊緣的極值點刪掉，余下的則為SIFT特征點； 3)為每個特征點指定方向參數 4)特征點描述向量的生成。
步驟三特征點匹配； 為了得到特征點坐標變化關系，在進行特征點提取之后還需要對前后時刻的特征點對應關系進行匹配，這是決定該導航方法是否準確可靠最關鍵的一步。由于在第二步中已經得到了每個特征點對應的描述向量，因此，在此部分可以直接對前后兩幅圖像中的特征點進行匹配。
步驟四濾波方程更新 根據步驟三中特征點匹配結果可以將特征點分為三類，這三類特征點可以代表實際過程中所有可能出現的情況，分別采用不同的策略對濾波方程進行更新。
步驟五對更新后的濾波方程進行濾波估計； 經過第四步更新后的濾波方程已經包含了圖像中特征點的所有運動信息，并且包含了慣性導航系統輸出的慣性測量信息進行圖像/慣性組合導航，即是對該方程進行濾波處理從而得到對無人機飛行狀態的估計(位置、速度、姿態等)。然而，由以上方法建立的系統和觀測方程為非線性方程，為了對其進行濾波處理必須采用非線性濾波方法，在本方案中采用了無跡卡爾曼濾波(UKF)方法。
本發明的優點在于 1)適應性廣，該方法只要求當前的地面圖像而不需要特定的外部信息配合，因此，理論上可以在任何環境(包括水下、遮擋、峽谷、地下等)下使用； 2)隱蔽性好，該方法不需要外部信息，也不需要向外部發出信息，因此，可以具有很好的保密性； 3)精度高，當應用于偵察、通訊中繼等小范圍場合時由于機載攝像頭可以得到活動范圍的全景圖像，因此導航精度可以達到很高的程度。



圖1為本發明準備工作中坐標系定義示意圖； 圖2為本發明準備工作中攝像機坐標系和IMU坐標系對準示意圖； 圖3為本發明一種未知環境下無人機視覺/慣性組合導航方法流程圖。

具體實施例方式 進行本方法前需要首先進行坐標系定義及對準工作 1，坐標系的定義 如圖1所示，坐標軸定義包括機體坐標系(obxbybzb)、導航坐標系(onxnynzn)、攝像機坐標系(osxsyszs)和IMU坐標系(oixiyizi)的定義； 攝像機坐標系(osxsyszs)在飛行器上捷聯固定一下視覺傳感器以獲得一系列的地面圖像，其坐標原點位于攝像機中心，oszs沿攝像機光軸方向，osxs和osys分別沿攝像機成像平面的長、寬方向，三者互相垂直成右手坐標系； IMU坐標系(oixiyizi)IMU出廠時定義的坐標系，三個坐標軸互相垂直，沿三個坐標軸分別各安裝一個加速度計和一個陀螺儀敏感載體的加速度和角速度信息。
機體坐標系(obxbybzb)原點在飛行器重心，沿飛行器縱軸為x正方向，水平向右為y正方向，z方向與二者垂直并向下，同時，IMU的坐標系(oixiyizi)與該坐標系固連； 導航坐標系(onxnynzn)為當地地理坐標系，原點位于導航起始位置，坐標軸分別沿北-東-地方向； 如圖2所示，攝像機坐標系和IMU坐標系是機體坐標系經過簡單的平移，而后依次水平轉動ψ和俯仰轉動θ得到的，ψ和θ分別是攝像機相對于機體坐標系的轉動角。
2，坐標軸對準 坐標軸對準包括了IMU坐標系與機體坐標系、IMU坐標系與攝像機坐標系對準。IMU中加速度計和陀螺儀之間的安裝誤差可以忽略不計，在此情況下，可以采用加速度計輸出對二者坐標系進行對準。為了得到IMU坐標系到攝像機坐標系的旋轉關系，可以使所有傳感器均敏感垂直方向，利用重力加速度信息對二者進行對準。
其對準方法如下 首先，在攝像機視場中放置一垂直于水平面的攝像頭標定靶；其次，采集攝像頭中的標定靶圖像，計算其中對應垂線的消失點得到攝像頭測量到的垂直方向向量

并同時采集加速度計輸出以得到當前加速度計相對于當地垂線方向向量

最后，將以上標定圖像和加速度計輸出數據代入標定方程中可以得到兩個坐標系之間的對應關系。該方法不需要任何其他設備，而標定中所需要的標定靶是進行攝像頭標定必須設備之一。因此，該標定方法具有使用方便的優點。其詳細計算過程如下 記qic為從慣性坐標系到攝像機坐標系的旋轉四元數，于是，以上標定過程即選擇qic使方程 取得最大。在得到當前的旋轉四元數qic之后可以根據 得到慣性坐標系到攝像機坐標系的旋轉矩陣，其中q1 q2 q3 q4為四元數qic的四個分量。從而，對于任意IMU坐標系下的坐標點Xi而言都有其在攝像機坐標系下Xc為 通過上式即可實現兩坐標系下坐標的互相轉換。
3，建立濾波方程 此處濾波方程包括了狀態方程和觀測方程，其建立方法分別如下 (1)狀態方程的建立 狀態方程包括了飛行器和特征點的動態模型，它們可以寫成如下的一階差分模型 此處Xa(k)為第k步狀態向量，xv(k)為飛行器運動狀態，包括位置、速度和姿態，可以通過慣性導航方程解算得到； Cbn為從載體坐標系到導航坐標系的轉移矩陣，Ebn為載體角速率到歐拉角速率的轉移矩陣，wv(k)為系統過程噪聲，T為濾波周期。
xm(k)為視覺獲取的特征點三維位置向量，對于第i個特征點而言，在假設場景不變前提下，其狀態方程為 xmi(k)＝xmi(k-1)(6) (2)觀測方程的建立 觀測量由三部分組成相對高度zh、航向zψ和攝像機坐標系下的特征點像素坐標zmi。其中高度和航向觀測量直接從其對應的狀態向量中輸出，即 特征點像素坐標部分觀測方程構建如下 設視覺傳感器內參數為 其中，fx，fy分別為視覺傳感器在x，y方向的焦距，u0，v0為光學中心，γ為不垂直因子。則對于第i個特征點有攝像機坐標系下系統觀測值u，v為 此處，x，y，z為特征點在攝像機坐標系中坐標Pms(k)s的分量，它可以從導航坐標系下的飛行器和特征點坐標中得到 Cbs(k)為從機體坐標系到攝像機坐標系的轉移矩陣，xmin(k)為狀態向量中當前特征點對應的分量，Cnb(k)為從導航坐標系到機體坐標系的轉移矩陣，Psbb為視覺傳感器中心與飛行器重心的偏差量，Pn(k)為當前的載體(無人機)位置坐標。
將以上高度zh、航向zψ和特征點像素坐標zmi觀測量合成得到系統觀測方程為 h(.，.)為當前時刻非線性觀測模型，v(k)為觀測噪聲。
(3)無人機視覺/慣性組合導航具體實現方法 本發明的具體步驟如圖3所示 步驟一圖像獲取并對圖像進行預處理； 當前對灰度圖像的處理較為成熟，同時運算速度較快，因此，本發明中采用了灰度圖像進行處理，通過公式 Y＝0.299R+0.596G+0.211B(12) 將獲取到的彩色圖像轉化為灰度圖像，可得到圖像的灰度值Y，其中R G B分別表示圖像中紅、綠、藍三種顏色分量； 然后對圖像信息進行濾波處理，一般情況下認為圖像中包含的噪聲近似為高斯白噪聲，可以采用中值濾波對圖像的噪聲進行抑制，即通過鄰域中亮度的中間值代替圖像的當前點。中值濾波具有很好的低通特性，同時還可以保留圖像的邊緣細節信息，而鄰域中亮度的中值不受個別噪聲毛刺的影響，因此，中值濾波十分適合于此類圖像處理； 步驟二通過SIFT方法進行特征點提取； 視覺對慣性系統的約束主要體現在前后幾幀圖像中特征點坐標的變化上，因此，需要對預處理后的圖像提取特征點； SIFT方法進行特征點提取分為四步驟來實現 1)檢測尺度空間極值點； 首次，定義圖像尺度空間函數為L(x，y，σ)，輸入圖像用I(x，y)表示，對輸入圖像進行卷積操作，則有其中是尺度可變高斯函數，(x，y)是空間坐標，σ是尺度坐標。采用不同的σ對圖像進行高斯卷積，得到高斯圖像金字塔。
其次，對高斯圖像金字塔中相鄰尺度的兩個高斯圖像相減得到高斯差分圖像 最后，將高斯差分圖像中每一個像素都與其周圍點相比較，得到極值點； 2)精確確定極值點位置；將位于邊緣的極值點刪掉，余下的則為SIFT特征點； 3)為每個特征點指定方向參數 在以特征點為中心的鄰域窗口內采樣，并用直方圖統計鄰域像素的梯度方向。直方圖的峰值代表了該特征點處鄰域梯度的主方向，即作為該特征點的方向。當存在另一個相當于主峰值80％能量的峰值時，則將這個方向認為是該特征點的輔方向。
4)特征點描述向量的生成；首先將坐標軸旋轉為特征點的方向，計算每個特征點鄰域中梯度方向直方圖并根據位置排序，然后將特征向量長度歸一化，該向量就是SIFT特征點描述向量。
步驟三特征點匹配； 每一個SIFT特征點描述向量均代表了該特征點唯一的特征，對特征點進行匹配即對其特征點描述向量進行匹配。由于SIFT特征點提取方法提取出的特征點具有很高的魯棒性，對圖像的旋轉、縮放、平移、光線以及遮擋等具有不變性。用128維的高維度來對特征點進行描述，使得特征點描述符之間具有很大的差異性，因此可以直接利用特征點之間的幾何特性進行特征點的匹配；當前應用較多的為基于歐幾里德距離的最近鄰法作為衡量標準，相似性的進行圖像匹配，該方法具體實現過程如下 設第一幅圖像中第i個特征點，第二幅圖像中的第j個特征點ai，bj的特征描述符為 ai＝[αi0，αi1，……，αi127](13) bj＝[βj0，βj1，……，βj127](14) 則這兩個特征點描述符的歐幾里德距離為 將ai與第二幅圖像中所有的特征點計算距離，存入數組Di·，并將這些距離從小到大排序，如果最小的距離Di0與次最小距離di1的比值在某一個閾值內(τ)，即di0＜＝τ×Di1，則認為匹配成功。把當前幀中所有的特征點都與前一幀進行匹配便得到特征點的匹配關系。
步驟四濾波方程更新 根據步驟三中特征點匹配結果可以將特征點分為三類 A、在前后兩幀圖像中都出現并且可以成功匹配的特征點認為是匹配成功的特征點，對其進行處理即對該特征點對應的觀測量進行更新。首先在觀測方程(11)中找到與該匹配特征點對應的濾波觀測值分量zi，而后用當前時刻該特征點在攝像機坐標系中的像素坐標代替該分量。
B、僅在第一幀圖像中出現，而在當前幀圖像中無法找到與之對應的匹配特征點認為是跟蹤丟失的特征點； 從攝像機中采集到的視覺圖像中提取的特征點數量十分龐大，隨著導航時間的延長將會導致濾波方程中狀態維數不斷膨脹，這對機載處理器的處理能力提出了很高的要求。因此，需要對特征點進行管理，隨著飛行器的運動將不再可見的特征點從濾波方程中刪除。由于濾波方程中各特征點之間互相獨立，因此，刪除特征點只需要將其對應的狀態向量及其在濾波方差陣中對應的行和列刪除即可。即分別從狀態方程(4)中將該丟失的特征點在系統狀態向量Xa(k)和系統方程f(Xa(k-1))中所對應的狀態分量刪除，從觀測方程(11)中將該丟失的特征點在觀測向量z和觀測方程h(χi，·)中所對應的分量刪除即可。
C、在當前幀圖像中新出現的特征點認為是新的特征點，對其進行初始化。
所述初始化包括狀態向量初始化和觀測向量的初始化，通過以下三步完成 a、得到新特征點在導航坐標系下坐標 此處Pn(k)為k時刻無人機位置坐標，Cbn為從載體坐標系到導航坐標系的轉移矩陣，Psbb為視覺傳感器中心與飛行器重心的偏差量，Csb為從攝像機坐標系到機體坐標系的轉移矩陣，Pmss(k)為在攝像機坐標系下的特征點位置與攝像機位置之差，它可以通過觀測得到 h為當前時刻飛行器的飛行高度，u0，v0為攝像機成像平面的光學中心，fx，fy分別為視覺傳感器在x，y方向的焦距，u，v為該特征點在攝像機坐標系下的像素坐標。
b、對狀態向量及新特征點對應的濾波估計誤差方差子矩陣進行初始化； 將新特征點坐標xmin(k)作為狀態變量加入到原系統的狀態方程(4)式中的狀態向量Xa中得到新的狀態向量Xa+與新的估計誤差方差陣Pa+ 其中， P為加入該新特征點之前的估計誤差協方差陣，R為新特征點對應的視覺傳感器的觀測誤差協方差陣。
c、觀測向量的初始化 觀測向量的初始化即將新特征點的觀測值加入到觀測向量中， 其中，z(k)為原來觀測向量，zmis為攝像機坐標系下新特征點的觀測值。
步驟五對更新后的濾波方程進行濾波估計； 此處更新后的濾波方程指的是狀態方程(4)和觀測方程(11)，對其進行濾波計算即可得到對無人機飛行狀態的估計(位置、速度、姿態等)。
UKF濾波方法結合了UT(Unscented Transformation)變換的思想。它首先通過選擇一組權值不同、且能夠代表隨機狀態變量統計特性的采樣點；其次，將這些采樣點帶入非線性函數處理后重構出新的統計特性(如均值和方差)；最后將得到的均值、方差和量測方程代入到Kalman濾波器的逐步遞推過程中就構成了UKF濾波。因此，UKF本質上是非線性的，不需要近似線性化，避免了EKF(擴展Kalman濾波)中線性化所帶來的誤差，而且其計算復雜度與EKF相當，適合應用于視覺導航等領域。
采用UKF方法對系統狀態方程(4)和觀測方程(11)構成的濾波模型進行濾波，濾波輸出的結果即為視覺/慣性組合導航系統對飛行器狀態的估計，該估計結果可以返回到控制系統對無人機進行導航定位、控制等。
權利要求
1、一種未知環境下無人機視覺/慣性組合導航方法，在進行導航過程之前首先進行坐標系定義及坐標軸對準工作，并建立系統運動狀態方程及觀測方程，其特征在于該導航方法通過下列步驟來實現
步驟一圖像獲取并對圖像進行預處理；
圖像預處理指的是對原始圖像進行灰度矯正、去噪聲處理，根據無人機機載視覺系統特點，采用灰度圖像進行處理，通過公式
Y＝0.299R+0.596G+0.211B(1)
將獲取到的彩色圖像轉化為灰度圖像Y，式中RGB分別表示圖像中紅、綠、藍三種顏色分量；
然后對圖像信息進行濾波處理，一般情況下，圖像中包含的噪聲近似為高斯自噪聲，采用中值濾波進行濾波處理，對圖像的噪聲進行抑制，通過鄰域中亮度的中值代替圖像的當前點；
步驟二通過SIFT方法進行特征點提取；
視覺對慣性系統的約束主要體現在圖像特征點坐標上，通過SIFT方法對預處理的圖像提取特征點；
步驟三特征點匹配；
每一個SIFT特征點描述向量均代表了該特征點唯一的特征，對特征點進行匹配即對其特征點描述向量進行匹配；用128維的高維度來對特征點進行描述，使得特征點描述符之間具有很大的差異性；用基于歐幾里德距離的最近鄰法作為衡量圖像相似性的度量進行圖像匹配；
步驟四濾波方程更新
根據步驟三中特征點匹配結果將特征點分為三類
A、在前后兩幀圖像中均出現并且成功匹配的特征點認為是匹配成功的特征點，對匹配成功的特征點進行處理即對該特征點對應的觀測值進行更新；
首先在濾波觀測向量z中找到與該匹配特征點對應的分量，而后用當前時刻該特征點在攝像機坐標系中的像素坐標代替該分量，并且根據當前慣性傳感器輸出重新建立濾波方程，濾波方程包括狀態方程和觀測方程；
B、僅在第一幀圖像中出現，而在當前幀圖像中無法找到與之對應的匹配特征點認為是跟蹤丟失的特征點，對其進行刪除；
由于濾波方程中各地標點之間互相獨立，因此，刪除地標點只需要刪除其對應的狀態向量及在濾波方差陣中對應的行和列；
C、在當前幀圖像中新出現的特征點認為是新的特征點，對其進行初始化；
步驟五對更新后的濾波方程進行UKF濾波估計；
首先通過選擇一組權值不同、且能夠代表隨機狀態變量統計特性的sigma點；其次，將這些采樣點代入非線性函數處理后，重構出新的統計特性；最后將得到的均值、方差和測量方程代入到Kalman濾波器的逐步遞推過程中就構成了UKF濾波。
2、根據權利要求1所述的一種未知環境下無人機視覺/慣性組合導航方法，其特征在于所述步驟二中通過SIFT方法進行特征點提取，其具體方法如下
1)檢測尺度空間極值點；將輸入圖像通過不同尺度的高斯核函數連續卷積和降采樣，形成高斯金字塔圖像；然后再對相鄰尺度的兩個高斯圖像相減得到高斯差分圖像；將高斯差分圖像中每一個像素都與其周圍點相比較，得到極值點；
2)精確確定極值點位置；將位于邊緣的極值點刪掉，余下的則為SIFT特征點；
3)為每個特征點指定方向參數；通過梯度直方圖的統計計算，為特征點賦予方向參數；
4)特征點描述向量的生成；首先將坐標軸旋轉為特征點的方向，計算每個特征點鄰域中梯度方向直方圖并以根據位置排序，然后將特征向量長度歸一化，該向量就是SIFT特征點描述向量。
3、根據權利要求1所述的一種未知環境下無人機視覺/慣性組合導航方法，其特征在于所述步驟四的狀態方程包括飛行器和地標點的動態模型，它們寫成如下的一階差分模型
此處Xa為狀態向量，xv(k)為飛行器運動狀態，包括位置、速度和姿態，通過慣性導航方程解算得到；
Cbn為方向余弦矩陣，Ebn為載體角速率到歐拉角速率的轉移矩陣，wv(k)為系統過程噪聲，T為濾波周期；
xm(k)為視覺獲取的地標點三維位置向量，對于第i個地標點而言，在假設場景不變前提下，其狀態方程為
xmi(k)＝xmi(k-1)。(4)
4、根據權利要求1所述的一種未知環境下無人機視覺/慣性組合導航方法，其特征在于所述步驟四的觀測方程中觀測量由高度及攝像機坐標系下的地標點坐標兩部分組成，其構建方法如下
設視覺傳感器內參數為
其中，fx，fy分別為視覺傳感器在x，y方向的焦距，u0，v0為光學中心，γ為不垂直因子；則系統觀測值u，v為
此處，[x y z]T為地標點在攝像機坐標系中坐標Pmss(k)的分量，從導航坐標系下的飛行器和地標點位置中得到
Cbs(k)為從機體坐標系到攝像機坐標系的轉移矩陣，xmin(k)為狀態向量中當前特征點對應的分量，Cnb(k)為從導航坐標系到機體坐標系的轉移矩陣，Psbb為視覺傳感器中心與飛行器重心的偏差量，Pn(k)為當前的載體(無人機)位置坐標；
由上得到觀測方程為
z(k)＝h(x(k)，v(k))(8)
h(.，.)為當前時刻非線性觀測模型，v(k)為觀測噪聲。
5、根據權利要求1所述的一種未知環境下無人機視覺/慣性組合導航方法，其特征在于
步驟四C中所述初始化通過兩步完成
a、得到新地標點在導航坐標系下坐標
此處Pn(k)為當前時刻無人機位置坐標，Pmss(k)為在攝像機坐標系下的地標點與攝像機坐標之差，它通過觀測得到
h為當前時刻飛行器的飛行高度；
b、對狀態向量及新地標點對應的濾波估計誤差協方差子矩陣進行初始化；
將新地標點坐標xmin(k)作為狀態變量加入到原系統的狀態方程(2)式中的狀態向量Xa中得到新的狀態向量Xa+與新的估計誤差方差陣Pa+
其中，
P為加入該新地標點之前的估計誤差方差陣，R為新地標點對應的視覺傳感器的觀測方差陣。
6、根據權利要求1所述的一種未知環境下無人機視覺/慣性組合導航方法，其特征在于
所述坐標系的定義，包括機體坐標系(obxbybzb)、導航坐標系(onxnynzn)、攝像機坐標系(osxsyszs)的定義；
機體坐標系(obsbybzb)原點在飛行器重心，沿飛行器縱軸向前為x正方向，水平向右為y正方向，z方向與二者垂直并向下，同時，認為IMU(慣性測量單元)的坐標系(oixiyizi)與該坐標系固連；
導航坐標系(onxnynzn)為當地地理坐標系，原點位于導航起始位置，坐標軸分別沿北-東-地方向；
攝像機坐標系(osxsyszs)在飛行器上捷聯固定一下視覺傳感器以獲得一系列的地面圖像，其坐標原點位于攝像機中心，oszs沿攝像機光軸方向，osxs和osys分別沿攝像機成像平面的長、寬方向，三者互相垂直成右手坐標系；
攝像機坐標系是機體坐標系經過簡單的平移，而后依次水平轉動ψ和俯仰轉動θ得到的，ψ和θ分別是攝像機相對于機體坐標系的轉動角。
7、根據權利要求6所述的一種未知環境下無人機視覺/慣性組合導航方法，其特征在于所述的導航坐標系和攝像機坐標系之間的旋轉關系標定方法如下
首先，令所有傳感器均敏感垂直方向，在攝像機視場中放置一垂直于水平面的攝像頭標定靶；其次，采集攝像頭中的標定靶圖像，并計算其中對應垂線的消失點得到攝像頭對應的垂直方向，對于加速度計而言，當加速度計輸出等于重力加速度時認為該傳感器方向即為垂直方向；最后，將以上標定圖像和加速度計輸出數據代入標定方程中得到兩個坐標系之間的對應關系；
假設
為慣性傳感器敏感到的垂直方向向量，而
為與慣性傳感器固連的攝像頭測量到的垂直方向向量，記qic為從慣性坐標系到攝像機坐標系的旋轉四元數，于是，以上標定過程即選擇qic使方程
取得最大，
從而，當得到當前的qic旋轉四元數之后根據
得到慣性坐標系到攝像機坐標系的旋轉矩陣，其中q1…q4為四元數qic的四個分量，從而，對于任意慣性坐標系下的坐標點Xi而言都有其在攝像機坐標系下Xc為
即通過上式將兩坐標系下的坐標進行互相轉換，同時也對二者進行對準。
全文摘要
本發明一種未知環境下無人機視覺/慣性組合導航方法，該方法涉及到圖像處理、慣性導航計算和濾波估計，因此，在進行主要導航方法之前首先需要進行坐標系定義、坐標系對準和濾波方程建立等。在本發明中所述的導航過程主要包括圖像獲取并對圖像進行預處理；通過SIFT方法進行特征點提取；特征點匹配；濾波方程更新；對更新后的濾波方程進行濾波估計五步驟來實現。本發明只要求當前的地面圖像而不需要特定的外部信息配合，因此，理論上可以在任何環境(包括水下、遮擋、峽谷、地下等)下使用；具有隱蔽性好，精度高的優點。
文檔編號G01C21/24GK101598556SQ20091008931
公開日2009年12月9日 申請日期2009年7月15日 優先權日2009年7月15日
發明者范耀祖, 超 徐, 沈曉蓉, 海 張 申請人:北京航空航天大學