基于VxWorks的小型四旋翼飛行器半實物仿真平臺設計

李 瑞， 史瑩晶， 李青松

(1. 電子科技大學 自動化工程學院， 成都 611731； 2. 中國船舶重工集團公司 第七二二研究所， 武漢 225001)

0 引 言

與固定翼飛行器相比，四旋翼飛行器(以下簡稱：四旋翼)結構簡單緊湊、機動性強、操控靈活、能源利用率高、安全性高，適合在近地面環境中執行監視、偵查等任務，具有廣闊的軍事和民用前景，因而，吸引了大批科研人員進行相關研究[1-6]。

由于四旋翼飛行器在動力學上具有靜不穩定、欠驅動、強耦合、非線性等特點，使得控制器設計與實現有較大難度。傳統的純數值仿真雖簡便、高效，但控制效果不夠直觀，且仿真環境過于理想化，難以真實地反映復雜的實際情況；而實物飛行測試雖直觀、真實，但實驗風險大、成本高、周期長，并且容易受到天氣、環境等制約。

半實物仿真則介于純數值仿真和實物飛行之間，其將部分控制對象及控制裝置實物嵌入仿真系統結構中，旨在保留數值仿真優良特性的同時，不失被控對象的真實性。飛行器模型可從實物控制器采集到真實的信息，以此檢驗飛控板的控制性能。同時，半實物仿真可靠、操作便捷，可謂兼備了純數值仿真和實物飛行測試的優點。

對四旋翼來說，半實物仿真既是便捷可靠的科研手段，又可作為從純數值仿真向實物飛行的過渡平臺，國內外已有多所高校及科研院所成功搭建了四旋翼半實物仿真平臺[7-11]。固高四旋翼半實物仿真平臺主要由運動控制卡、3自由度實驗本體組成，基于Matlab快速模型，方便修改調試，大大縮短控制器設計周期。dSPACE是由軟件和專用硬件組成的一整套開發平臺，可實現快速原型化和半實物仿真一體化。Chen等[11]采用dSPACE仿真平臺對一款商用四旋翼飛行器(Draganflyer3)進行了辨識和控制算法檢驗，Kis等[12]也基于自主研發的四旋翼平臺進行了實時仿真。宋子豪[13]利用dSPACE系統搭建了八旋翼飛行器仿真平臺，控制器采用實物飛控板，仿真對象是Simulink數學模型，對八旋翼無人機的多種飛行模式進行仿真并成功運用到實物飛行中。但是，目前多數半實物仿真平臺或采用商用的仿真系統，其軟、硬件造價昂貴，或仿真系統的實時性和穩定性不好，或真實信息舍棄過多，導致控制效果不夠明顯。

針對實際應用需求，設計并實現了一款可快速迭代、實時性高、界面友好、可信度高的半實物仿真平臺，并在該平臺上進行了四旋翼的仿真分析，所得半實物仿真結果能夠直接應用于四旋翼實物平臺。

因為仿真回路中有硬件實物，為保證對象模型解算能夠在規定的采樣周期內完成，仿真平臺需要滿足強實時性。實時操作系統VxWorks是專為實時嵌入式系統設計的操作系統，自推出以來，得到包括美國軍方在內的國內外用戶的認可，已被廣泛應用于包括美國火星探路者、F-16、B-2轟炸機等的大量嵌入式設備上[14]，是市場占有率最高的嵌入式系統之一。本文采用VxWorks操作系統，使仿真系統軟件在可靠性、穩定性和實時性方面有了保障；同時為VxWorks操作系統和仿真計算機在半實物仿真應用提供了經驗。

本文將從上位機、仿真計算機、四旋翼飛行控制器3方面介紹上述四旋翼半實物仿真平臺，并給出對四旋翼的姿態、位置控制的實驗結果。

1 總體框架設計及軟硬件平臺選擇

四旋翼半實物仿真平臺總體設計框架如圖1所示。整個仿真平臺包括由個人電腦組成的上位機、四旋翼飛行控制板、工控機組成的仿真計算機3部分。

圖1 總體框架圖

個人電腦運行上位機程序，主要用于在線監控仿真過程和參數修改以及仿真結果展示。除此之外，在嵌入式交叉編譯中，個人電腦不但充當宿主機用于VxWorks目標機操作系統配置、驅動軟件和應用程序的編譯和調試，而且實現四旋翼模型、飛控系統仿真模型的建立及其全數字仿真的運行、驗證，并且完成模型及其控制律的下載等任務。四旋翼飛控板運行原始四旋翼飛控程序，在半實物仿真中作為實物接入。工控機運行VxWorks操作系統和仿真應用程序，主要負責解析上位機命令和參數、對象模型離散化、四旋翼動力學和運動學模型解算。個人電腦和飛控板、飛控板與工控機之間通過串口連接，個人電腦與工控機之間通過網絡連接，以太網的作用是下載編譯后的可運行模塊、在線調整模型參數、數據傳輸、仿真啟停控制。四旋翼飛控板和實時對象模型通過高速硬件接口形成一個閉環控制回路。

2 四旋翼飛行器動力學模型構建

對物體運動的描述首先要建立合適的坐標系。文章采用慣性坐標系和機體坐標系構建四旋翼飛行器動力學模型。慣性坐標系中OXe指向地理北向，OYe指向地理東向，OZe垂直于地面向下。機體坐標系中沿著機體向前的方向為機體系X軸，沿著機身向右，與X軸垂直為機體Y軸方向，沿著機體垂直向下為機體系Z軸，滿足右手定則。

將四旋翼飛行器看做一個剛體，可以將四旋翼動力學模型分為平移運動和旋轉運動兩個子系統。在懸停狀態下，忽略空氣阻力，由文獻[15]可知，四旋翼飛行器的非線性數學模型為：

式中：(x,y,z)為四旋翼的位置坐標；φ,θ,φ為機體坐標系下四旋翼沿3軸的旋轉角度，分別是滾轉角、俯仰角和偏航角；Ix,Iy,Iz分別為四旋翼飛行器3軸轉動慣量；l為旋翼在機體軸向與四旋翼質心的距離；系統的控制輸入U1和U2～U4分別為4個旋翼產生的力和力矩；m為四旋翼飛行器的質量。

3 四旋翼飛行器半實物仿真平臺設計

根據第1節提出的總體設計框架，下文將分別設計上位機程序、四旋翼飛行控制器和仿真計算機程序。

3.1 上位機程序設計

仿真平臺的上位機程序框架如圖2所示。

圖2 上位機程序框架

上位機程序設計主要在虛擬儀器開發環境Labwindows/CVI中完成。通過與數據采集卡連接，CVI可以實現個性化定制，針對專用界面和功能，在同一臺電腦上可以實現示波器、頻譜分析、信號發生等各種功能。CVI應用程序采用模塊化設計，基于消息響應機制，用戶只需要編寫模塊的響應回調函數即可實現相應的功能，方便快捷，不需要用戶對界面編程有很深入的了解。

如圖3所示，上位機程序主要分為仿真控制、參數配置、結果展示以及通信4個模塊。其中網絡通信采用傳輸控制協議/網間協議(Transmission Control Protocol/Internet Protocol，TCP/IP)，上位機程序作為客戶端，打開上位機的時候主動發出連接請求。由于四旋翼飛行控制分為姿態控制和位置控制，因此需要對不同的控制模式進行切換，仿真控制程序主要負責模式切換和仿真運行以及數據保存回顯等功能。

圖3 上位機界面

3.2 四旋翼飛行器控制器設計

為了實現半實物仿真平臺的設計，首先需要設計四旋翼飛行器的控制器。在第2節的基礎上，對四旋翼非線性數學模型進行線性化處理，四旋翼的控制可以解耦成高度、俯仰、滾轉和偏航等通道。采用PID控制算法對4個通道分別控制，系統框圖見圖4。

圖4 四旋翼飛行器控制器結構

針對四旋翼的實物平臺，將上文設計的控制器轉為嵌入式C語言程序，并將程序移植到飛行控制板上，根據主控芯片的處理速度，設計合適的控制流程，如圖5所示。

圖5 四旋翼飛行控制器

在實物平臺上，通過遙控器或者個人電腦PC端給出控制指令，控制器首先接收控制指令并對指令進行解析，然后判斷四旋翼的控制模式，最后根據期望的控制量對四旋翼進行控制。其中，模式判斷每3 ms進行一次，角速度控制周期為2 ms，角度控制周期為5 ms，速度控制周期為20 ms，位置控制周期為50 ms。在姿態控制模式中，外環為角度控制，內環為角速度控制，外環的輸出作為內環的輸入；在位置控制模式中，外環為位置控制，內環為速度控制，速度控制器的輸出量為角度控制器的輸入。

3.3 仿真計算機程序設計

仿真計算機主要負責網絡通信任務、串口通信任務、通信狀態檢測、飛行控制器解算、對象模型動力學解算、仿真控制指令的解析和執行、仿真周期定時任務。根據任務的功能和特性，進行模塊化設計。

圖6 仿真計算機程序結構

本文針對第2節構建的四旋翼模型和3.2節中設計的雙回路PID控制器，首先在Matlab環境中搭建了Simulink模型。實時工作間(Real-Time Workshop，RTW)是Matlab/Simulink一個工具箱，在代碼自動生成技術方面較為成熟。因而，使用RTW將仿真模型轉化為實時性高、性能可靠的目標平臺代碼。

四旋翼仿真模型導入過程為：① 設置仿真模型參數，設置目標文件格式。針對工控機平臺選擇ert.tlc文件和C語言。② 設置仿真周期。針對實時仿真平臺采用固定仿真周期，選擇ode4步長模式解算器。③ 在Simulink界面點擊Build Model(生成模型)等待生成嵌入式代碼，存放在當前目錄文件夾中。④ 在協同仿真環境Workbench中新建仿真項目，復制嵌入式代碼到項目文件夾中，編寫仿真任務應用程序，設置控制器和對象模型的輸入輸出接口。

網絡通信是一種常用的通信方式，其傳輸速度快、實現方便并且技術已經成熟，可實現跨平臺之間的通信，適合Windows程序和VxWorks程序之間的通信。因此，本文將工控機平臺作為服務器端，上位機作為客戶端，使用網絡通信的方式來實現工控機平臺與PC端上位機之間的長時間的可靠通信。

四旋翼飛行控制板與外界數據的交互通過串口方式實現。在對四旋翼進行控制時，控制任務采用固定周期解算控制輸出，角度環控制采用5 ms的更新周期，角速度環更新周期是2 ms。為了保證串口通信的可靠、實時性，采用有限狀態機編寫串口接收模塊。對數據幀解析過程如圖7所示。

圖7 串口解析有限狀態機

仿真任務模塊是半實物仿真平臺的核心，必須保證一定的實時性和準確性。本文設計的仿真任務模塊主要完成仿真初始化、四旋翼動力學解算。仿真任務阻塞在接收信號量階段，一旦串口數據解析正確將發送信號量，實現仿真任務的同步。仿真任務解算完畢將分別通過串口和網口將四旋翼反饋狀態發送至上位機用于實時顯示和保存，并通過網口將反饋狀態發送至飛行控制器進行下一步控制解算。半實物仿真平臺如圖8所示。

4 四旋翼飛行器半實物仿真實驗

文中設計了一系列仿真實驗對半實物仿真平臺進行性能測試。首先在Matlab/Simulink中完成四旋翼控制器和對象模型的搭建，對姿態控制和位置控制進行仿真驗證。然后將四旋翼對象模型生成相應的嵌入式C代碼，移植到仿真計算機，將控制器移植到實際的飛控板中，在半實物仿真平臺中驗證控制器的控制效果。最后將飛控板移植到實際四旋翼實物平臺進行實際飛行驗證。

在將控制器移植到飛控板之前，必要的Matlab仿真可以減小后續飛行器調試的工作量。姿態控制是四旋翼穩定飛行的關鍵，在Simulink中分別設置四旋翼期望的滾轉角、俯仰角、偏航角均為12°，四旋翼飛行器的階躍響應如圖9所示。由圖可見，在控制器作用下，四旋翼姿態角的角度響應能在2s以內達到期望姿態角，超調量小于3%，且幾乎沒有穩態誤差。

(a) 滾轉角的姿態控制

在姿態角控制的基礎上加入位置環控制器，同樣的，設置四旋翼為零初始狀態，分別給定3個軸向坐標點為10 m，四旋翼可以在4 s內從靜止狀態飛到期望目標位置。

為了保證VxWorks中實時仿真的可靠性，分別在Matlab中和實時操作系統中運行相同的控制器和對象模型，對比兩者的運行結果。采用RTW生成四旋翼PID控制器和對象模型的VxWorks應用程序。在全數字仿真中，仿真步長采用固定步長，為了得到準確的計時，在仿真中采用輔助時鐘計時方法。Matlab中仿真周期是10 ms，在VxWorks應用中采用同樣的仿真周期。

從圖10可以看出，Matlab解算和工控機解算高度一致，證明了基于VxWorks的實時仿真可靠性較高。VxWorks為了滿足高實時性，犧牲了部分數據精度，在拐點附近表現明顯，但不影響最終的仿真結論。

(a) 基于滾轉角的仿真對比(b) 基于俯仰角的仿真對比

在前期工作的基礎上，將四旋翼飛行控制器直接應用于實物平臺，將飛控板固定在實際四旋翼上，連接傳感器、電調、數傳等接口，對基本功能調試后進行室外實際飛行。在室外飛行中，通過3位開關切換到指定模式，給四旋翼一個期望的坐標位置，在5 s左右都能接近目標點附近并逐漸收斂，實際飛行比半實物仿真稍慢，但總體趨勢吻合較好，說明半實物仿真置信度較高，可用于指導四旋翼飛行控制器的設計。

圖11 X軸向位置仿真

進一步，采用導航向量場算法設計了四旋翼包圍飛行實驗。讓四旋翼從任意點起飛，在導航向量場控制和雙回路PID控制器作用下，四旋翼將逐漸沿著期望的圓形軌跡飛行。這里，以參考點作為坐標原點，期望半徑為4 m，起飛位置坐標(-3,-3)，起飛點初始狀態設為零。首先在半實物仿真平臺中驗證，圖12(a)表示半實物仿真結果，圖12(b)表示實際飛行效果。對比分析半實物仿真和實際飛行結果可以發現，半實物仿真結果更加平滑，穩態誤差更小。實際飛行效果與半實物仿真效果吻合，進一步驗證了半實物仿真的可靠性，因而，四旋翼飛行控制器半實物仿真結果可以直接應用于實際飛行。

(a) 半實物仿真結果(b) 實際飛行效果

5 結 語

根據四旋翼飛行器半實物仿真的功能要求，設計了半實物仿真平臺的總體框架和硬件選型。為保證半實物仿真平臺的實時性，采用嵌入式實時操作系統VxWorks。在此基礎上，對半實物仿真平臺的3部分：上位機、飛行控制器、仿真計算機進行設計和實現，分別完成了四旋翼飛行器的全數字仿真、半實物仿真，并通過半實物仿真與實際飛行數據對比驗證了半實物仿真平臺的可靠性。

本文設計的四旋翼半實物仿真平臺界面友好，實時性高，能指導四旋翼飛行控制器設計，具有很好的實用價值。