基于視覺/慣導的無人機自主著陸方法與流程
本發明涉及一種無人機自主著陸方法，屬于組合導航領域。
背景技術：
視覺導航技術是利用數字圖像處理技術，對機載圖像傳感器所獲取的航拍圖像進行處理和分析，最終得到無人機導航控制所需要的位姿參數的技術。圖像處理的大信息量等特點使得無人機在視覺導航定位時實時性不能得到滿足，因此需要一種快速實時圖像匹配算法來解決這一問題。
慣導技術是一種傳統的不接收外來的無線電信號，也不向外部輻射能量的自主式導航技術。慣性導航設備具有全天候、全時空工作能力和很好的隱蔽性，能夠提供短期高精度、高穩定性的導航參數。但是，慣性導航設備是一個時間積分系統，其誤差隨著時間迅速累積且垂直定位功能不好，不可以單獨使用，因此需要通過引入另一種導航源對慣導長時間累積的誤差進行修正。
技術實現要素：
為了克服現有技術的不足，本發明提供一種基于視覺/慣導的無人機自主著陸方法，通過視覺和慣性導航組合的方法來設計無人機自主著陸系統，利用慣性導航來快速剔除視覺導航圖像匹配的誤匹配點提高視覺導航的實時性，通過無跡卡爾曼濾波(UKF)的方式將視覺導航輸出參數與慣導進行組合，用視覺來修正慣性導航誤差，解決慣導隨時間誤差累積較大的問題。
本發明解決其技術問題所采用的技術方案包括以下步驟：
1)利用視覺導航算法求解無人機位置及位姿，步驟如下：
1.1)利用無人機機載攝像頭獲取實時圖像；
1.2)判斷獲取的實時圖像是否為第一幀圖像，若是，轉到步驟1.3)，否則轉到步驟1.4)；
1.3)提取實時圖像的SURF特征點，與參考圖像的SURF特征點進行SURF匹配，利用RANSAC算法剔除SURF誤匹配點對，獲取正確的SURF匹配點對，轉到步驟1.7)；
1.4)判斷實時圖像幀數是否為設定的固定周期T的倍數，若是，轉到步驟1.5)，否則轉到步驟1.6)；
1.5)提取實時圖像的SURF特征點，與參考圖像的SURF特征點進行SURF匹配，轉到步驟3)；
1.6)利用LK光流法跟蹤上一幀實時圖像提取的SURF特征點；
1.7)利用獲取的SURF匹配點對進行無人機位姿解算；
2)利用無人機位姿作為慣導的初始值，開始求解慣性導航參數，步驟如下：
2.1)利用陀螺輸出的角速度觀測值，采用四元數法進行姿態更新，然后將更新后的四元數轉換成姿態矩陣k表示上一時刻，k+1表示當前時刻，L為導航坐標系，b為載體坐標系；
2.2)根據姿態矩陣中元素的值，計算載體的姿態，包括航向角、俯仰角、翻滾角；
2.3)利用姿態矩陣，將加速度計輸出的比力觀測值fb由載體坐標系轉換到導航坐標系，得到導航坐標系下的比例觀測值
2.4)有害加速度的補償：對fL進行哥氏加速度修正和正常重力補償，得到載體在地球表面的加速度a＝fL-(2ΩLie+ΩLel)vL+gL，其中，ΩLie為地球自轉角速度三維分量構成的反對稱陣，ΩLel為L系相對于地固系旋轉角速度三維分量構成的反對稱陣，vL為速度矢量，gL為地球重力矢量；
2.5)利用加速度進行積分得到速度增量，得到當前時刻的速度vLk+1＝vLk+a.Δt；
2.6)利用速度進行積分得到位置增量，進而得到當前時刻的位置
3)利用慣導相鄰時間獲取的參數剔除實時圖像與參考圖像SURF匹配的誤匹配點對，步驟如下：
3.1)獲取時刻t與時刻t+Δt的慣導參數；
3.2)求取時刻t與時刻t+Δt之間的慣導參數變換；
3.3)獲取t時刻特征點在攝像頭成像平面上的投影點坐標為Wt＝[u,v]T，其中，u、v表示投影點在圖像坐標系中的坐標值；
3.4)根據特征點匹配方法得到t+Δt時刻特征點在攝像頭成像平面上的投影點坐標為Wt+Δt；
3.3)獲取視覺導航t時刻到t+Δt時刻投影點坐標變化D＝wt+Δt-wt＝[xi，yi]T；
3.4)預測t+Δt時刻投影點坐標為時刻t到t+Δt之間飛行器航向變化，T為時刻t到t+Δt之間投影點平移向量；
3.5)得到特征點坐標變化向量
3.6)定義匹配誤差
3.7)誤匹配點Pem＝{P(x,y)|ex＞dx,ey＞dy}，其中dx，dy分別為預先設定的匹配誤差閾值；
3.8)將誤匹配點剔除；返回步驟1.7)；
4)利用無跡卡爾曼濾波組合導航參數；
5)利用獲取的無人機參數實時調整無人機位姿進行著陸引導。
本發明的有益效果是：
(1)本發明利用慣性導航計算得到的位置和姿態來對視覺導航匹配算法中的誤匹配點實現快速剔除，不僅可以提高視覺導航算法的實時性，而且可以使視覺系統保持長時間的高精度；
(2)本發明采用視覺導航修正慣導，解決了單一慣導在導航著陸階段，垂直定位功能較差，誤差發散不可以單獨使用的問題，且在視覺導航解算失敗的情況下，組合系統則完全依靠慣性導航來提供載體的導航參數。
附圖說明
圖1是本發明步驟流程示意圖；
圖2是慣導系統修正視覺導航系統原理圖；
圖3是視覺導航修正慣導系統的原理圖；
圖4是仿真驗證系統程序流程圖；
圖5是仿真結果圖。
具體實施方式
下面結合附圖和實施例對本發明進一步說明，本發明包括但不僅限于下述實施例。
本發明的組合導航方法是按以下步驟1)至步驟4)的基礎上循環運行：
步驟1)利用視覺導航算法求解無人機位置及位姿包括1.1)～1.8)；
1.1)利用無人機機載攝像頭獲取實時圖像，轉到步驟1.2)；
1.2)判斷獲取的實時圖像是否為第一幀圖像，若是轉到步驟1.3)，否則轉到步驟1.4)；
1.3)第一幀圖像處理過程如下：
1.3.1)對實時獲取的圖像進行預處理，提取實時圖像的SURF特征點，轉到步驟1.3.2)；
1.3.2)將實時圖像提取的SURF特征點與參考圖像的SURF特征點進行SURF匹配，轉到步驟1.3.3)；
1.3.3)利用RANSAC算法剔除SURF誤匹配點對，獲取正確的SURF匹配點對，轉到步驟1.8)，執行完后轉到步驟2)；
1.4)判斷當前獲取的實時圖像幀數是否為T的倍數(T為預先設定的固定周期，本算法取T＝5)，若是轉到步驟1.5)，否則轉到步驟1.7)；
1.5)對實時獲取的圖像進行預處理，提取實時圖像的SURF特征點，轉到步驟1.6)；
1.6)將實時圖像提取的SURF特征點與參考圖像的SURF特征點進行SURF匹配，轉到步驟3)，執行完后轉到步驟1.8)；
1.7)利用LK光流法跟蹤上一幀實時圖像提取的SURF特征點，轉到步驟1.8)；
1.8)利用獲取的SURF匹配點對進行無人機位姿解算；
步驟2)利用視覺導航求解的無人機位姿作為慣導的初始值開始求解慣性導航參數包括2.1)～2.6)順序執行；
2.1)姿態更新：利用陀螺輸出的角速度觀測值，采用四元數法進行姿態更新，然后將更新后的四元數轉換成姿態矩陣，記為k表示上一時刻，k+1表示當前時刻，L為導航坐標系，b為載體坐標系；
2.2)姿態提取：根據姿態矩陣中元素的值，計算載體的姿態，包括航向角、俯仰角、翻滾角；
2.3)比力分解：利用姿態矩陣，將加速度計輸出的比力觀測值fb由載體坐標系轉換到導航坐標系，
2..4)有害加速度的補償：對fL進行哥氏加速度修正和正常重力補償，得到載體在地球表面的加速度，如下所示：
a＝fL-(2ΩLie+ΩLel)vL+gL其中ΩLie為地球自轉角速度三維分量構成的反對稱陣，ΩLel為L系相對于地固系旋轉角速度三維分量構成的反對稱陣，vL為速度矢量，gL為地球重力矢量；
2.5)積分求取速度；利用加速度進行積分得到速度增量，進而得到當前時刻的速度vLk+1如下所示：vLk+1＝vLk+a.Δt；
2.6)積分求取位置：利用速度進行積分得到位置增量，進而得到當前時刻的位置rLk+1，如下所示：
步驟3)利用慣導相鄰時間獲取的參數剔除實時圖像與參考圖像SURF匹配的誤匹配點對，包括3.1)～3.6)順序執行；
3.1)獲取時刻t與時刻t+Δt的慣導參數(t與t+Δt為視覺導航mT幀數對應時刻與(m+1)T幀數對應的時刻，m為整數)；
3.2)求取時刻T與時刻t+Δt之間的慣導參數變換；
3.3)獲取t時刻特征點在攝像頭成像平面上的投影點坐標為Wt＝[u,v]T，其中，u、v表示投影點在圖像坐標系中的坐標值；
3.4)根據特征點匹配方法得到t+Δt時刻特征點在攝像頭成像平面上的投影點坐標為Wt+Δt；
3.3)獲取視覺導航t時刻到t+Δt時刻投影點坐標變化D＝wt+Δt-wt＝[xi,yi]T；
3.4)根據慣導參數預測t+Δt時刻投影點坐標為:為時刻t到t+Δt之間飛行器航向變化，T為時刻t到t+Δt之間投影點平移向量；
3.5)根據慣導系統得到的特征點坐標變化向量
3.6)定義匹配誤差:
3.7)誤匹配點可以表示為:Pem＝{P(x,y)|ex＞dx,ey＞dy}其中dx，dy分別為預先設定的匹配誤差閾值；
3.8)將滿足Pem＝{P(x，y)|ex＞dx,ey＞dy}的誤匹配點剔除。
步驟4)利用無跡卡爾曼濾波組合導航參數
4.1)求解狀態方程；
4.2)求解量測方程；
4.3)無跡卡爾曼濾波器設計；
步驟5)利用獲取的無人機參數實時調整無人機位姿進行著陸引導。
本發明的實施例如圖1所示，在以下步驟1)至步驟4)的基礎上循環運行：
步驟1)：利用視覺導航算法求解無人機位置及位姿包括1.1)～1.8)；
1.1)利用無人機機載攝像頭獲取實時圖像，轉到步驟1.2)；
1.2)判斷獲取的實時圖像是否為第一幀圖像，若是轉到步驟1.3)，否則轉到步驟1.4)；
1.3)第一幀圖像處理過程如下：
1.3.1)對實時獲取的圖像進行預處理，提取實時圖像的SURF特征點，轉到步驟1.3.2)；
1.3.2)將實時圖像提取的SURF特征點與參考圖像的SURF特征點進行SURF匹配，轉到步驟1.3.3)；
1.3.3)利用RANSAC算法剔除SURF誤匹配點對，獲取正確的SURF匹配點對，轉到步驟1.8)，執行完后轉到步驟2)；
1.4)判斷當前獲取的實時圖像幀數是否為T的倍數(T為預先設定的固定周期，本算法取T＝5)，若是轉到步驟1.5)，否則轉到步驟1.7)；
1.5)對實時獲取的圖像進行預處理，提取實時圖像的SURF特征點，轉到步驟1.6)；
1.6)將實時圖像提取的SURF特征點與參考圖像的SURF特征點進行SURF匹配，轉到步驟3)，執行完后轉到步驟1.8)；
1.7)利用LK光流法跟蹤上一幀實時圖像提取的SURF特征點，轉到步驟1.8)；
1.8)利用獲取的SURF匹配點對進行無人機位姿解算；
步驟2)；利用視覺導航求解的無人機位姿作為慣導的初始值開始求解慣性導航參數包括2.1)～2.6)；
2.1)姿態更新：利用陀螺輸出的角速度觀測值，采用四元數法進行姿態更新，然后將更新后的四元數轉換成姿態矩陣，記為k表示上一時刻，k+1表示當前時刻，L為導航坐標系，b為載體坐標系；
2.2)姿態提取：根據姿態矩陣中元素的值，計算載體的姿態，包括航向角、俯仰角、翻滾角；
2.3)比力分解：利用姿態矩陣，將加速度計輸出的比力觀測值fb由載體坐標系轉換到導航坐標系，
2..4)有害加速度的補償：對fL進行哥氏加速度修正和正常重力補償，得到載體在地球表面的加速度，如下所示：
a＝fL-(2ΩLie+ΩLel)vL+gL其中ΩLie為地球自轉角速度三維分量構成的反對稱陣，ΩLel為L系相對于地固系旋轉角速度三維分量構成的反對稱陣，vL為速度矢量，gL為地球重力矢量；
2.5)積分求取速度；利用加速度進行積分得到速度增量，進而得到當前時刻的速度vLk+1如下所示：vLk+1＝vLk+a.Δt；
2.6)積分求取位置：利用速度進行積分得到位置增量，進而得到當前時刻的位置rLk+1，如下所示：
步驟3)；利用慣導相鄰時間獲取的參數剔除實時圖像與參考圖像SURF匹配的誤匹配點對，包括3.1)～3.6)；
3.1)獲取時刻t與時刻t+Δt的慣導參數(t與t+Δt為視覺導航mT幀數對應時刻與(m+1)T幀數對應的時刻，m為整數)；
3.2)求取時刻T與時刻t+Δt之間的慣導參數變換；
3.3)獲取t時刻特征點在攝像頭成像平面上的投影點坐標為Wt＝[u,v]T；
3.4)根據特征點匹配方法得到t+Δt時刻特征點在攝像頭成像平面上的投影點坐標為Wt+Δt；
3.5)獲取視覺導航t時刻到t+Δt時刻投影點坐標變化D＝wt+Δt-wt＝[xi,yi]T；
3.6)根據慣導參數預測t+Δt時刻投影點坐標為:為時刻t到t+Δt之間飛行器航向變化，T為時刻t到t+Δt之間投影點平移向量；
3.7)根據慣導系統得到的特征點坐標變化向量
3.8)定義匹配誤差:
3.9)誤匹配點可以表示為:Pem＝{P(x,y)|ex＞dx,ey＞dy}其中dx，dy分別為預先設定的匹配誤差閾值；
3.10)將滿足Pem＝{P(x,y)|ex＞dx,ey＞dy}的誤匹配點剔除。
步驟4)利用無跡卡爾曼濾波組合導航參數具體實施過程見圖3所示：
4.1)系統狀態方程求解：
式中，狀態向量X為：
L-無人機所在位置的緯度；λ-無人機所在位置的經度；h-無人機的高度；δVe-無人機在地理系中的東向速度；δVn-無人機在地理系中的北向速度；δVu-無人機在地理系中的天向速度；-無人機俯仰角；-無人機橫滾角；-無人機航向角；f(.)為9維向量函數，Wk為9維系統過程噪聲。
4.2)系統量測方程求解：
由算機視覺定位幾何關系圖得量測方程如下所示：
其中Zk為量測矩陣，X為狀態向量，Vk為系統觀測噪聲。
4.3)組合濾波器設計：
4.3.1)在濾波開始時設定初始估計值和初始誤差方程方差：
4.3.2)計算設狀態變量為n維，計算2n+1個采樣點及其權值,這里n取9；
4.3.3)計算時間更新方程
4.3.5)濾波更新
式中：Wi(m)和Wi(c)分別為計算y的均值和方差權值：W0(m)＝κ/(n+κ)；W0(c)＝W0(m)+(1-α2+β)；Wi(m)＝Wi(c)＝κ/[2(n+κ)],i＝1,...,2n；α,β,λ為3個參數，其中1e-4≤α≤1,λ＝0或3-n,β＝0
步驟5)利用獲取的無人機參數實時調整無人機位姿進行著陸引導。
將獲取的無人機姿態參數送入飛行控制系統，控制無人機進行著陸。
以上為無人機基于視覺/慣導組合的自主著陸方法步驟。
本發明的仿真驗證系統利用虛擬現實技術和可視化技術，具體實施圖4所示：模擬了真實的無人機飛行環境，具有沉浸式的真實感，解決了無人機自主著陸視景仿真模擬的問題，將視覺、慣導組合著陸算法與三維視景仿真軟件結合，通過多通道展示整個著陸階段并實時的計算和顯示組合著陸導航所需數據。
給定實驗體初始時刻的位置(L0,λ0),姿態以及速度即實驗體初始時刻的導航信息。仿真結果如下:經過100s的仿真，從圖5中可以看出，實驗體的位置誤差、速度誤差以及姿態誤差的誤差基本上都是在0附近，誤差曲線是收斂的。從這些結果可以看出，實驗體導航信息的誤差得到了較好的控制，同時也說明了本文中所提出的組合導航方案是可行的。
- 還沒有人留言評論。精彩留言會獲得點贊！