基于Pixhawk 飛控的硬件在環仿真系統設計①

吳和龍 向政蓉 高宇航 楊劍鋒 蔡茗茜 吳其琦

(?工業和信息化部電子第五研究所 廣州511370)

(??廣西科技師范學院機械與電氣工程學院 來賓546199)

(???廣西科技大學自動化學院 柳州545006)

0 引言

目前無人機(unmanned aerial vehicle,UAV)的應用越來越廣泛,其工作環境中影響飛行的不確定因素越來越多,這些工況對飛行控制系統(以下簡稱飛控)的設計與開發提出了更高的要求[1]。Pixhawk 是一款開源、可二次開發的飛控,支持多旋翼、固定翼等多種類型的無人機,在工業、農業、民用等領域得到廣泛使用[2-4]。因此,針對Pixhawk 飛控設計飛行仿真系統,用于測試無人機的新功能與新航線,或者用于無人機出廠前的性能測試,都具有積極的現實意義。對于飛控的設計,驗證邏輯正確性和穩定性顯得尤為重要。如果使用全軟件仿真對飛控系統進行驗證[5-6],由于仿真環境理想化,無法反映無人機飛行時的真實情況;如果使用真實無人機測試,由于待測飛控系統的不確定性,可能會出現墜機危險,試驗風險大。為此,本文設計了基于Pixhawk飛控的硬件在環仿真系統。

目前,國內外已經有多所科研單位以及高校對飛控硬件在環仿真系統進行研究。呂永璽等人[7]結合無人機數學模型和xPC 實時系統設計了無人機飛控半實物仿真系統。李瑞等人[8]采用上位機、仿真計算機、飛控3 部分相結合的方式設計了基于VxWorks 的四旋翼半實物仿真平臺。Prabowo 等人[9]通過飛控硬件控制無人機模型,實現了硬件在環仿真系統,并使用FlightGear 顯示仿真飛行畫面。以上仿真系統在專用平臺上獲得了較好仿真效果,但由于使用了閉源的飛控,不能對廣泛應用的開源Pixhawk 飛控仿真,存在一定的局限性。為了在Pixhawk 飛控上實現硬件在環仿真,設計了新的仿真系統,能夠對多旋翼、固定翼或直升機等無人機進行仿真。本文以四旋翼為例,建立了四旋翼機體模型,在Pixhawk 飛控中嵌入傳感器數據重構算法、傳感器驅動程序、仿真通信協議,最終實現傳感器行為級別的仿真,獲得真實度較高的仿真效果。

1 硬件在環仿真設計方案

Pixhawk 飛控的工作原理如圖1 所示。飛控利用傳感器測量無人機機體的位置、姿態與角速度(為便于描述統稱為位姿數據),在內核中使用位姿數據與航線計算旋翼電機當前的轉速,控制無人機沿著航線飛行。根據上述原理,為了在飛控中實現仿真功能,需要完成如下2 點關鍵設計:(1)建立無人機機體的數學模型,即把圖1中的無人機機體進行數字化;(2)在飛控中完成位姿數據至傳感數據的變換,即把圖1 中的物理傳感器進行軟件化。

具體的硬件在環仿真系統設計方案如圖2 所示。仿真系統主要包括Pixhawk 飛控和仿真計算機2 部分。飛控作為硬件部分接入仿真系統中,仿真計算機顯示四旋翼機體模型在虛擬場景中飛行的三維畫面。仿真通信協議充當兩部分之間串口通信的橋梁[10],機體模型輸出位姿態數據送至飛控中運算得到轉速數據,轉速再送至計算機中控制機體模型,形成閉環控制。使用Mission Planner 地面站[11]并通過MavLink 通信協議連接飛控[12],用于控制與觀測無人機。仿真計算機運行Matlab 軟件,在Simulink環境中運行機體模型[13-14],同時嵌入運行FlightGear視景窗口軟件,使用位姿數據顯示仿真過程中無人機的飛行畫面;Pixhawk 運行的飛控軟件為ArduPilot,在原有軟件基礎上設計仿真通信協議、模擬傳感器數據重構程序、軟件傳感驅動程序與轉速轉換程序。

工作過程如下:飛控通過仿真通信協議獲得位姿數據,首先經過傳感器數據重構程序處理后轉換為模擬傳感器數據;再通過傳感驅動程序把傳感器數據傳至飛控內核,內核輸出代表轉速的脈沖寬度調制(pulse width modulation,PWM)信號,經過轉速轉換程序轉換為轉速數據;最后通過仿真通信協議把轉速數據傳回計算機,整個系統形成控制閉環回路。在完成硬件在環飛行仿真之后,使用參數切換真實傳感系統的數據至內核,再對真實無人機中進行實飛測試。

2 四旋翼無人機機體模型

四旋翼的機體模型采用北航可靠飛行控制研究組發布的開源模型,如圖3 所示,以電機的PWM 控制量為輸入,以多旋翼的狀態和傳感器信息為輸出,四旋翼無人機機體模型主要由動力單元模型、控制效率模型和剛體控制模型組成。動力單元模型是由無刷電機、電子調速器和螺旋槳組成的動力機構,輸入量是PWM 信號,輸出量是螺旋槳轉速;控制效率模型的作用是將螺旋槳旋轉所產生的拉力和反扭力矩計算出來。剛體控制模型包括動力學模型和運動學模型,動力學模型計算出的拉力方向始終與機體Z軸的復方向一致,運動學模型可計算出機體位置、速度、姿態和角速度等參量。

圖3 四旋翼無人機機體模型

四旋翼無人機模型的輸入量是4 個電機的轉速信號,輸出量是三軸位置PosE、姿態角AngEuler、三軸旋轉速率(陀螺儀數據)AngRateB、4 個旋翼的轉速和旋轉矩陣(dimensional constraint manager,DCM),該模型在Matlab/Simulink 中經過軟件在環仿真驗證,模型輸出值正確,可將該模型作為飛控硬件在環仿真系統中的無人機機體模型,模型設計過程可參考文獻[15]。

3 飛控仿真程序設計

飛控仿真程序的設計基于開源飛控軟件ArduPilot。這是一款應用廣泛與功能強大的自動駕駛軟件,支持多旋翼、固定翼與直升機等無人機,具備手動飛行、自動飛行、故障返航等各功能[16]。ArduPilot 軟件兼容Pixhawk 飛控,是基于Pixhawk 原生代碼開發的上層應用[17-18]。系統在ArduPilot 原生代碼上嵌入了模擬傳感器數據重構程序、軟件傳感器驅動程序、轉速轉換程序和仿真通信程序。

3.1 模擬傳感器數據重構程序設計

飛控工作所需的傳感器數據來源于陀螺儀、加速度計、磁力計、氣壓計和全球定位系統(global positioning system,GPS)。由第2 節可知,四旋翼機體模型的輸出值只能提供陀螺儀數據AngRateB(xyz軸角速度)與GPS 數據PosE(經緯度與高度),而加速度計、磁力計與氣壓計數據需要根據模型的輸出值重構,重構算法如下。

加速計測量加速度值。在北-東-地球坐標系中,當加速計水平放置且Z軸垂直向上時,可以測量Z軸上重力加速度值-g,X軸和Y軸的值為0,記錄為(0,0,-g)。當無人機旋轉一定的姿態時,重力加速度將在加速度計的3 個軸上產生相應的分量,這本質上是從大地坐標系中的(0,0,-g)到無人機坐標系的轉換。無人機坐標系和大地坐標系之間相差一個旋轉矩陣DCM,無人機坐標系下的三軸加速度計值為

其中,θ是俯仰角,?是橫滾角,φ是偏航角。

磁力計測量機體所處位置的磁場強度。當無人機靜止且X軸朝北向時,無人機的X軸與北方重合。此時,只有X軸上能測量到磁場強度,Y軸和Z軸的磁場強度為0。假設磁場強度為B,則三軸磁場強度為(B,0,0)。當無人機旋轉一定姿態時,磁場強度會在磁力計的3 個軸上產生相應的分量,這本質上是從大地坐標系中的(B,0,0)到無人機坐標系的轉換。無人機坐標系和大地坐標系之間相差一個旋轉矩陣DCM,無人機坐標系下的三軸磁力計值為

氣壓計用于測量無人機的高度,在Ardupilot 中主要參考氣壓計高度而不是GPS 高度。氣壓測高方法是在起飛前測量起飛點的氣壓P0和地面溫度T,在飛行過程中再測量所處高度的氣壓P,把P0、P與T代入氣壓測高公式計算相對于起飛點的高度h。在Ardupilot 官方代碼中使用式(3)計算高度:

當重構氣壓數據時,對式(3)反變換得到高度至氣壓的變換關系:

傳感器數據重構程序的邏輯結構如圖4 所示,從仿真通信協議中獲得位姿數據。圖中,ax、ay、az分別表示x軸、y軸和z軸的加速度,mx、my、mz分別表示x軸、y軸和z軸的磁場值。利用姿態角橫滾、俯仰與偏航重構加速度計與磁力計數據;利用高度重構氣壓計數據。陀螺儀數據直接使用xyz軸角速度;GPS 數據直接使用位置數據經度、緯度與高度。以上傳感器數據通過軟件傳感器驅動程序傳輸到飛控內核之中。

圖4 傳感器數據重構程序邏輯結構圖

3.2 軟件傳感器驅動程序設計

ArduPilot中驅動程序的結構如圖5所示。驅動程序包括驅動前臺接口、驅動后臺接口和傳感器驅動程序。其中驅動前臺接口負責將傳感器數據傳至飛控內核;驅動后臺接口負責管理傳感器,一類傳感器對應一種驅動程序,另一類傳感器中包括各型號傳感器,每個驅動接口對應一種型號的傳感器。

圖5 Ardupilot 驅動程序結構

為了把數據重構程序輸出的傳感器數據傳輸至飛控內核,需要為各類傳感器設計相應的驅動程序,在此稱為軟件傳感器驅動,而傳遞真實傳感器數據的驅動稱為硬件傳感器驅動。軟件傳感器驅動包括加速度計、陀螺儀、磁力計、氣壓計和GPS 等5 部分,如圖6 所示。每一類傳感器對應一個后臺接口,每個接口包括硬件驅動和軟件驅動。硬件驅動是系統中原有的驅動,用來接收真實傳感器信號;軟件驅動用于接收傳感器數據重構程序輸出的模擬傳感器數據。當使用真實無人機飛行時,后臺接口讀取硬件驅動的數據;當使用機體模型飛行時,后臺接口讀取軟件驅動的數據,實現了把機體模型位姿數據傳輸至飛控內核的功能。

圖6 傳感器驅動設計結構

3.3 仿真通信協議

為了保證仿真計算機與飛控之間數據傳輸的正確性與傳輸效率,需要根據傳輸數據的特性設計專用的通信協議。協議以數據包為基本單位,包含包頭、類型、參數長度、參數、循環冗余校驗碼(cyclic redundancy check,CRC)5 個要素,各個要素如表1所示。

表1 數據包要素

數據包各個要素的描述如下。(1)包頭:用于標識數據包的開始。(2)類型:用于表明通信載荷的屬性。其中姿態類型是1;位置類型是2;角速度類型是3;電機轉速數據類型是4。(3)參數長度:類型附帶參數的長度,數值范圍0～255。(4)參數:類型附帶的參數,長度0～255。(5)CRC 校驗碼:用于校驗數據幀是否存在錯誤。校驗的數據是類型、參數長度與參數3 個要素,采用的CRC 多項式為

各個數據包的內容如表2 所示。

表2 傳輸數據參數

姿態數據包傳輸無人機橫滾、俯仰與偏航3 個長度為4 字節的角度數據;位置數據包傳輸經度、緯度與高度長度為4 字節的位置數據;角速度數據包傳輸xyz軸的角速度數據,轉速數據包傳輸4 個長度為2 字節的電機轉速數據。仿真通信協議有2 個版本,分別運行在Ardupilot 與Simulink 中。

4 仿真測試與分析

無人機硬件在環仿真系統實物如圖7 所示,包括四旋翼無人機、仿真計算機與地面站3 部分。無人機使用的飛控是重新設計的Pixhawk 飛控,飛控通過串口與仿真計算機相連,計算機中的機體模型依據圖中四旋翼無人機建構。在進行硬件在環仿真時,首先設置飛控參數以指定使用軟件傳感系統的傳感器數據,并輸入起飛點的位置、氣壓氣溫、磁場強度等數據。然后運行Simulink 中的機體模型,與飛控進行位姿數據與轉速數據的交互通信,并使用Mission Planner 地面站控制與觀測無人機;使用FlightGear 視景窗口顯示三維飛行畫面,飛行界面如圖8 所示。完成硬件在環仿真飛行之后,通過修改飛控參數使用真實傳感器數據,即可進行真實無人機的飛行測試。

圖7 硬件在環無人機仿真系統實物

圖8 無人機仿真飛行界面

仿真效果測試的目的是驗證硬件在環仿真的真實度,測試方法是使用相同的飛行航線分別進行硬件在環仿真飛行與真實無人機飛行,然后計算飛行數據的相似度。飛行航線如圖9 所示,航線中總共設有7 個航點,無人機從初始位置航點H起飛,高度設定為20 m,飛到2 號航點(2 號航點與6 號航點重合),按照航點順序繞航線飛行,飛到6 號航點后自動返回初始航點,至此完成一次飛行。

圖9 無人機飛行航線

無人機按照上述航線飛行后,分別得到仿真飛行與真實飛行數據(下文簡稱仿真數據和實飛數據)曲線。飛控的飛行日志單元記錄飛行數據,并以bin 文件存儲在TF 卡(trans-flash card)中。利用Matlab 軟件并遵照日志格式把bin 文件導入到計算機中進行繪圖與分析。由于仿真飛行與真實飛行在起飛時刻上的不同步,為了提高分析準確度,對兩組數據進行了時間上的對齊處理。選取有代表性的姿態角、加速度、陀螺儀、磁力計、高度氣壓與經緯度數據作為分析對象,如圖10～15 所示,其中仿真數據冠以仿真-前綴以區別實飛數據。

圖10 無人機姿態角曲線

通過比較仿真和實飛的數據,從直觀上可知2種數據的吻合度較高,進一步使用式(5)計算2 種數據之間的線性相關系數r[19]。

式中x是仿真數據,y是實飛數據,每組數據中有n個數據量,ˉ、ˉ分別是x與y的平均值。每組數據對應的線性相關系數如表3 所示。

表3 傳感器數據線性相關系數

線性相關系數越接近于1 表明相似程度越高。表3 中數據都大于0.8,大部分大于0.85,表明仿真數據接近實飛數據,仿真效果較好。在圖10 的姿態角曲線中,融合加速度、陀螺儀與磁力計數據后解算得到姿態角,相關系數約為0.85。仿真曲線與實飛曲線交織變化,表明變化趨勢一致,差別主要表現在瞬時數據變化上,原因是真實飛行中受到環境風力、機械振動與電磁干擾等因素的影響,而這些影響在仿真環境中無法模擬,故造成了數據差異。在圖11的加速度曲線與圖12 的陀螺儀曲線中,仿真曲線較平滑,而實飛曲線中包含了電機轉動引起的噪音,故相關系數較低。在圖13 的磁力計曲線中,電流引起的磁干擾是造成數據差異的主要因素。飛行過程中磁力計xy軸曲線大幅變化,磁干擾占比小,故相關系數較大;z軸曲線幾乎不變,磁干擾占比大,故相關系數較小。在圖14 的高度與氣壓曲線中,仿真曲線較平滑,而環境風力與氣壓變化造成實飛曲線的噪音較大。在圖15 的經緯度曲線中,因為航線的飛行范圍小,飛行過程中經緯度微量變化,故相關系數很高。

圖11 無人機加速度計曲線

圖12 無人機陀螺儀曲線

圖13 無人機磁力計曲線

圖14 無人機氣壓計曲線

圖15 無人機經緯度曲線

5 結論

本文基于Pixhawk 飛控設計無人機仿真系統,實現了硬件在環仿真功能,只需要在無人機中安裝仿真固件就能進行硬件在環仿真,在Pixhawk 飛控得到廣泛應用的民用無人機領域,具有較好的應用價值。仿真飛行與真實飛行的數據比較表明,仿真系統的仿真相似程度較高;由于仿真飛行在計算機上的虛擬環境中進行,不存在墜機風險,安全性得到極大保證;仿真測試全程在室內進行,高效的測試效率、優越的便利性與低廉的測試成本都得到了充分體現。通過以上分析,該硬件在環仿真系統具有較好的綜合性能。

從本文設計的仿真系統結構可知,在飛控軟件中額外運行了軟件傳感器驅動、數據傳感器重構與仿真通信協議等程序,盡管這些程序運行在獨立進程中,已經最小限度改動原有軟件結構,但仍然占用了一定的飛控運行時間和資源,造成仿真效果與真實飛行仍然有一定差距,如果通過其他方式解決該問題,仿真真實度還可以進一步提升。