基於北鬥導航系統的微小型無人飛行器定位與導航方法與流程
2023-09-10 07:32:10
本發明涉及無人飛行器和飛行控制領域；具體講，涉及使用北鬥衛星導航系統的多旋翼飛行器定位與導航。
背景技術：
隨著低成本慣性測量系統(Inertial Navigation System，INS)和全球定位系統的出現，無人飛行器系統的應用已經不僅局限於軍用領域，低成本的無人飛行器越來越多的被應用於民用領域中。目前無人機領域研究成果中的絕大多數均依賴於全球定位系統(Global Positioning System，GPS)系統進行定位。然而，GPS接收機是一種被動傳感器，極易受到幹擾或欺騙，在常用的低成本微機電系統(Micro-Electro-Mechanical System，MEMS)傳感器組成的慣性導航系統中，一旦失去GPS位置信息的更新，位置和速度的解算精度將會迅速的降低。在微型無人飛行器潛在應用場景中，GPS信號的可靠性無法得到保證，急需一種替代定位方式。
通過機載視覺傳感器獲得的圖像信息，通過相關圖像處理算法，獲得飛行器位置與姿態信息進而進行控制，是微型無人飛行器自主定位與導航方面可行的一條途徑。然而單獨視覺導航算法，無法提供目標的準確經緯度信息，在戰場偵查等方面的應用受到很大限制。
由美國軍方控制的GPS系統，於1994年完全建成，與之相比，中國新近開發的北鬥導航系統，由於擁有獨特的雙向通信能力，使其對於幹擾和欺騙信號具有很高的抵抗能力。目前，北鬥導航系統尚未完全建成，覆蓋區域僅限於中國和亞太地區，與成熟的GPS系統相比，定位精度也有差距，在中國大陸地區的定位精度約為10m，而GPS系統在經過幾十年的完善後，定位精度已提高到1m。北鬥系統相對較低的定位精度，對於需要懸停飛行的四旋翼飛行器的控制帶來了很大的挑戰，現有商品化、成熟的飛行控制系統所採用的GPS+慣導的融合方式，已不再適用於北鬥系統。
為了實現使用北鬥導航系統的高精度位置、速度估計，結合機載視覺導航系統是解決方案之一。在相關文獻中，已有將視覺導航應用於輔助GPS系統的應用案例。例如：使用光流傳感器獲得的速度信息提高GPS的位置和速度測量精度。光流法提供的速度信息還可與GPS的速度進行比較，進而獲得地面的相對高度。在空間交會對接過程中，視覺導航算法被用於與GPS信息融合，以獲得空間飛行器間的相對位置信息。
在過去的研究工作中，使用GPS衛星導航系統的無人飛行器導航算法已取得了相對多的研究成果，尤其是在輔助GPS的廣域增強系統(Wide Area Augmentation System，WAAS)投入應用以及美國軍方去除SA(Selective Availability)幹擾之後，民用GPS定位精度已經從初期的100米提高了1米。WAAS廣域增強系統提供的輔助信息，可提高GPS位置測量信息的可靠性和精度。而SA幹擾在2000年被美軍關閉後，更大大提高了民用GPS的定位精度。在WASS系統覆蓋的區域，GPS系統的定位精度可達1米，這就使低成本慣性導航系統也能在GPS的幫助下獲得相對精確的位置估計，進而大大推動了民用無人飛行器領域的發展。
然而作為我國尚處於建設階段的北鬥衛星導航系統，類似WAAS這樣的增強系統尚未搭建，因此無論是定位精度還是定位可靠性方面，均有較大的差距。在同一地點同一時刻採集北鬥導航系統與GPS導航系統的原始測量值進行對比。通過靜止狀態下連續10分鐘的測量，在東方向上所有GPS的位置測量值均在±1.5m的範圍內，而北鬥導航系統的位置測量值散布在±15m的範圍。
除了精度的不足之外，實際應用中北鬥導航系統由於在軌衛星數量有限，定位可靠性較低，時常會因遮擋造成定位丟失。而對於傳統的與低成本慣性導航器件組成的系統來說，可靠的位置測量是必不可少的，缺少位置信息的更新，低成本、低精度的慣性導航系統的位置估計在幾秒鐘內就會發散。即便是使用精密的導航級慣性器件，慣性導航系統的位置估計誤差在五分鐘內就達到了30m以上。
技術實現要素：
為了實現對四旋翼飛行器的精確控制，獲得位置的準確、平滑估計是非常重要的。但是由於北鬥衛星導航系統精度的不足，使用傳統的、應用於GPS系統的傳感器融合方法，並不能實現四旋翼飛行器的穩定自主懸停。另一方面，北鬥導航系統尚處於建設當中，在軌衛星數量的限制，使北鬥系統更易受建築、樹木等障礙物的遮擋，造成定位質量的惡化。為解決上述問題,本發明提出了一種適合北鬥系統的傳感器融合方法，通過引入視覺導航，位置估計的準確性和魯邦性均得到了提升。本發明採用的具體技術方案是，基於北鬥導航系統的微小型無人飛行器定位與導航方法，包括如下步驟：利用安裝在四旋翼無人機底部的光流傳感器獲取無人機的速度信息，利用機載慣性導航裝置獲取加速度信息,利用機載視覺系統獲取速度信息，結合北鬥系統位置的原始測量值，經融合濾波獲得對於位置和速度的估計；進而通過非線性的位置控制算法，實現飛行器位置控制。
所述融合濾波其濾波器的狀態向量X定義為：
其中，(x,y)為水平方向的位置，為水平方向的速度；
濾波器為卡爾曼濾波器，狀態轉移方程和觀測方程如下式所示：
X(k)＝AX(k-1)+Bu(k-1)+ω(k-1)
Z(k)＝CX(k)+ν(k)
其中，k代表時刻，u為輸入向量，Z為觀測向量，ω和v為具有正態分布特徵的相互獨立的輸入噪聲和觀測噪聲，輸入向量u，和觀測向量Z的定義如下：
u＝(ax,ay)T
其中(ax,ay)為機載慣性傳感器獲得的水平方向加速度測量值，狀態轉換矩陣A和輸入控制矩陣B的定義如下：
其中δt為傳感器的採樣周期，觀測矩陣C定義如下：
卡爾曼濾波器的目標是使用k時刻的觀測值Y(k)，前一時刻的狀態估計值以及前一時刻的輸入控制量u(k-1)，對k時刻的狀態進行最優估計，這一統計學上的最優濾波器如下式所示：
P(k|k-1)＝AP(k-1)AT+BQBT
H(k)＝P(k|k-1)CT(CP(k|k-1)CT+R)-1
P(k)＝(I-H(k)C)P(k|k-1)
其中P為狀態的協方差矩陣，協方差矩陣Q代表了加速度數據的噪聲，協方差矩陣R代表北鬥系統和視覺系統的觀測值的噪聲，這兩個矩陣均為對角陣，並可以由記錄的實際飛行數據確定，
其中矩陣Q的數值較小，而R中對應與北鬥系統觀測值的項選取了較大的數值，而對應視覺系統觀測值的項數值較小
通過非線性的位置控制算法，實現飛行器位置控制是指採用非線性的魯邦控制器，具體如下所述：
選擇飛行器的位置和偏航角作為系統的輸出，表示為η＝[x y z ψ]T，控制目標是使飛行器跟蹤某一給定的軌跡，這條軌跡可表示為ηd＝[xd yd zd ψd]T；橫向位置x和縱向位置y可通過機載視覺系統的反饋獲得，豎直方向的位置z可從板載氣壓計的讀數獲得，控制器由內環既姿態環與外環既位置環構成；內環採用了經典的比例、積分、微分控制(Proportion Integration Differentiation，PID)控制器，外環使用了非線性魯邦控制器，期望的橫滾、俯仰姿態角及橫滾、俯仰姿態角速度由外環控制器計算得到，簡化後的的四旋翼飛行器的平動方向的動力學模型，表示為：
當飛行器到達了給定的軌跡ηd＝[xd yd zd ψd]T時,
定義輔助向量μ＝[μx μy μz]T:
這裡μ代表了期望的加速度向量或虛擬位置控制向量。
外環位置控制器使用了基於魯棒誤差符號函數積分(Rotust Inetgral of the Signum of the Error，RISE)的新型魯棒控制器。定義位置跟蹤的誤差信號如下：
ex1＝xd-x ey1＝yd-y ez1＝zd-z
其中xd,yd,zd為時變的參考軌跡，引入如下的輔助誤差信號：
這裡αx,αy和αz是正的增益，設計位置控制器μ：
其中ksx,ksy,ksz,βx,βy,βz為正的增益，sgn(·)為標註的符號函數。
μ(t)中各項可表示為：
解出總升力u1(t)和期望的姿態角
設計內環即姿態環控制器的控制輸入u2,u3,u4如下：
式中kpθ,kdθ,kiθ,kpψ,kdψ,kiψ是正的增益，其中，為滾轉角度姿態控制器的比例、微分、積分係數，kpθ,kdθ,kiθ,為俯仰角度姿態控制器的比例、微分、積分係數，kpψ,kdψ,kiψ為偏航角度姿態控制器的比例、微分、積分係數，跟蹤誤差eθ,eψ定義為：
eθ＝θd-θ eψ＝ψd-ψ
θd由外環控制器得到，ψd為偏航角的時變軌跡。
與已有技術相比，本發明的技術特點與效果：
使用相對低精度的國產北鬥衛星導航系統時，無人飛行器高精度位置控制的方法進行了初步研究。通過將北鬥位置信息與機載視覺導航系統的速度信息進行融合，獲得了對飛行器位置高精度、無累積誤差的估計值，同時，在非線性位置控制器的控制下，實現了使用北鬥衛星導航系統的高精度位置控制。長距離的飛行驗證顯示，本發明所提出的傳感器融合方案，結合了光流法短期位置估計精度高和北鬥衛星導航系統無長期累積誤差的優點，初步實現了北鬥導航系統在具備懸停飛行能力的無人飛行器上的應用。
附圖說明
圖1為本發明所涉及的多傳感器融合濾波器的一種具體實施方式的示意圖。
圖2為本發明所涉及的機載北鬥飛行控制系統的一種具體實施方式。
圖3為本發明所涉及方法的實際效果。
具體實施方式
本發明所要解決的技術問題是，提供一種基於北鬥衛星導航系統與視覺傳感器數據融合的無人機自主定位方法，實現室外環境下無人機的精準無漂移定位。
本發明採用的技術方案是：採用北鬥衛星導航系統與光流傳感器數據融合的方法用於無人機的定位系統中，包括如下步驟：
本發明採用「傳感器融合(濾波)—控制」架構，將北鬥與光流、慣導通過濾波器進行融合，實現了單獨北鬥導航系統無法實現的飛行控制精度。此外，還將非線性的基於魯棒誤差符號函數積分(Rotust Inetgral of the Signum of the Error，RISE)的新型魯棒控制器用於飛行器的控制算法中，進一步提高了控制效果。
利用安裝在四旋翼無人機底部的光流傳感器獲取無人機的速度信息，並利用此速度信息來提高位置估計精度。通過使用如圖1示的濾波器結構，在慣導的加速度信息、機載視覺系統的速度信息的幫助下，結合北鬥系統位置的原始測量值，可以獲得對於位置和速度的高精度的的可靠估計。進而通過非線性的位置控制算法，實現高精度的飛行器位置控制。
本發明採取的技術方案是，將視覺傳感器的數據與北鬥導航系統的位置信息進行融合，進而實現無人機的定位，包括如下步驟：
利用安裝在四旋翼無人機底部的光流傳感器獲取無人機的速度信息，並利用此速度信息來提高位置估計精度。通過使用如圖1示的濾波器結構，在慣導的加速度信息、機載視覺系統的速度信息的幫助下，結合北鬥系統位置的原始測量值，可以獲得對於位置和速度的高精度的可靠估計。
所設計的濾波器的狀態向量定義為：
其中，(x,y)為水平方向的位置，為水平方向的速度。
卡爾曼濾波器的狀態轉移方程和觀測方程如下式所示：
X(k)＝AX(k-1)+Bu(k-1)+ω(k-1)
Z(k)＝CX(k)+ν(k)
其中，u為輸入向量，Z為觀測向量，w和v為具有正態分布特徵的相互獨立的輸入噪聲和觀測噪聲。輸入向量u，和觀測向量Z的定義如下：
u＝(ax,ay)T
其中(ax,ay)為機載慣性傳感器獲得的水平方向加速度測量值，狀態轉換矩陣A和輸入控制矩陣B的定義如下：
其中δt為傳感器的採樣周期，觀測矩陣C定義如下：
卡爾曼濾波器的目標是使用k時刻的觀測值Y(k)，前一時刻的狀態估計值以及前一時刻的輸入控制量u(k-1)，對k時刻的狀態進行最優估計，這一統計學上的最優濾波器如下式所示：
P(k|k-1)＝AP(k-1)AT+BQBT
H(k)＝P(k|k-1)CT(CP(k|k-1)CT+R)-1
P(k)＝(I-H(k)C)P(k|k-1)
其中協方差矩陣Q代表了加速度數據的噪聲，協方差矩陣R代表北鬥系統和視覺系統的觀測值的噪聲，這兩個矩陣均為對角陣，並可以由記錄的實際飛行數據確定。在本系統中，矩陣Q的數值較小，而R中對應與北鬥系統觀測值的項選取了較大的數值，而對應視覺系統觀測值的項數值較小
為了提高對外界擾動的抑制能力，在無人飛行器上使用了非線性的魯邦控制器。該控制器設計如下：
選擇飛行器的位置和偏航角作為系統的輸出，表示為η＝[x y z ψ]T，控制目標是使飛行器跟蹤某一給定的軌跡，這條軌跡可表示為ηd＝[xd yd zd ψd]T。平動方向的位置x和y可通過機載視覺系統的反饋獲得，豎直方向的位置z可從板載氣壓計的讀數獲得。控制器由內環(姿態環)與外環(位置環)構成。內環採用了經典的比例、積分、微分控制(Proportion Integration Differentiation，PID)控制器，外環使用了非線性魯邦控制器。期望的姿態角及姿態角速度由外環控制器計算得到，簡化後的的四旋翼飛行器的平動方向的動力學模型，可表示為：
當飛行器到達了給定的軌跡ηd＝[xd yd zd ψd]T時,
定義輔助向量μ＝[μx μy μz]T:
這裡μ(t)代表了期望的加速度向量或虛擬位置控制向量。
由於微小型無人飛行器的重量較小，易受到氣流等外界擾動的影響。為了提高控制器的魯棒性，外環位置控制器使用了基於魯棒誤差符號函數積分(Rotust Inetgral of the Signum of the Error，RISE)的新型魯棒控制器。定義位置跟蹤的誤差信號如下：
ex1＝xd-x ey1＝yd-y ez1＝zd-z
其中xd,yd,zd為時變的參考軌跡。為了方便後面的控制器設計，引入如下的輔助誤差信號：
這裡αx,αy和αz是正的增益。設計位置控制器μ(t)：
其中ksx,ksy,ksz,βx,βy,βz為正的增益，sgn(·)為標註的符號函數。
μ(t)中各項可表示為：
可解出總升力u1和期望的姿態角
設計內環(姿態環)控制器的控制輸入u2,u3,u4如下：
式中kpθ,kdθ,kiθ,kpψ,kdψ,kiψ是正的增益，其中，為滾轉角度姿態控制器的比例、微分、積分係數，kpθ,kdθ,kiθ,為俯仰角度姿態控制器的比例、微分、積分係數，kpψ,kdψ,kiψ為偏航角度姿態控制器的比例、微分、積分係數，跟蹤誤差eθ,eψ定義為：
eθ＝θd-θ eψ＝ψd-ψ
θd由外環控制器得到，ψd為偏航角的時變軌跡。
下面給出具體的實例：
一、系統硬體連接及配置
如圖2所示，本發明的基於視覺的四旋翼無人機自主飛行控制方法採用基於嵌入式架構的飛行控制結構，所搭建的實驗平臺包括四旋翼無人機本體、地面站、遙控器等。其中四旋翼無人機搭載了嵌入式計算機(該計算機內嵌Intel Core i3雙核處理器，主頻1.8GHz)、機載PX4FLOW光流傳感器、GPS和飛行控制器(含慣性導航單元和氣壓計模塊等)。地面站包括一臺裝有Linux作業系統的筆記本計算機，用於機載程序的啟動及遠程監控。該平臺可通過遙控器進行手動起飛和降落，並在發生意外時緊急切換為手動模式，以確保實驗安全。
二、飛行實驗結果
本實施例對上述實驗平臺進行了多組飛行控制實驗，飛行實驗環境為室外校園環境中。實驗目標是使用北鬥導航系統信息實現無人飛行器的高精度無漂移定位。
室外手持實驗過程中的飛行軌跡曲線如圖3所示。其中，標有●的曲線為用作參考的高精度GPS接收機的測量值，標有▲的為使用視覺傳感器的速度信息積分而得的視覺裡程計測量值，標有■的為本發明所設計的多傳感器融合方法的融合結果。從圖中可以看出，北鬥衛星導航系統的原始測量值的精度相對較低，而視覺裡程計法在長距離的工作後，產生了明顯的累積誤差，而使用本發明所設計的融合方法，可以獲得高精度且無累積誤差的位置估計，證明了本發明所設計的融合算法的有效性。
顯然，上述實例僅僅是為清楚的說明所作的舉例，而並非對實施方式的限定，對於所屬領域的普通技術人員來說，在上述說明的基礎上還可以做出其他不同形式的變化或變動。這裡無需也無法對所有實施方式予以窮舉。而由此引申出的顯而易見的變化或變動仍處於本發明創造的保護範圍之中。