無人機飛控仿真教學實驗平臺

一  概  述

   無人機飛控仿真教學實驗平臺主要由主控上位機、實時仿真機、三軸電動轉臺、電機舵機教學實驗展板、飛行系統組成。平臺主要應用于飛行器相關學科專業的日常教學實驗中，同時兼顧對無人機飛控系統進行功能性驗證。利用搭建的仿真模型對主控制器發出的控制指令進行解析和飛行姿態反饋，所具備的功能包括：

（1）無人機運動姿態仿真：能夠完整地展現出無人機滾轉、俯仰、偏轉運動姿態，復現無人機飛行時的姿態角變化；

（2）姿態監視和調參：飛行仿真模型運行過程中，可以利用外部調試接口實時顯示姿態信息和調整參數；

（3）故障注入：在進行完整半物理仿真飛行時，可以按測試需求模擬飛行過程的各種突發故障類型，對飛控系統控制算法的魯棒性進行評估。

二、系統總體設計

為了最大限度模擬真實的動態飛行環境，無人機開源飛控教學實驗平臺采用數學模型來為半物理仿真系統提供機體運動狀態和動態大氣環境等動態飛行參數。采用Simulink進行無人機本體模型的搭建，并利用RTW自動代碼工具完成目標仿真平臺的嵌入式代碼自動生成。該平臺的總體結構2.1所示。

圖2.1 平臺系統結構圖

圖中主控上位機主要作用是在仿真前進行無人機動力學模型的構建、數字仿真、目標機代碼生成，通過集成開發環境Tornado，完成VxWorks操作系統內核的生成、主機和目標機的搭接、下載目標機代碼等；在仿真過程中通過RTW外部模式在線修改無人機模型中各種參數、獲取各種仿真數據；還可以選擇運行自行開發的仿真控制軟件。

實時仿真機中運行無人機動力學模型代碼，接收由舵機發送的舵偏角信號，根據無人機動力學模型完成無人機當前姿態信息的解算并將這些信息通過光纖網絡發送到MPC08運動控制卡，根據收到的無人機當前姿態信息驅動三軸轉臺。轉臺上的慣性測量元件測量實際的姿態和速率信息，通過DA和串口發送到主控制器，根據獲取的傳感器信息解算控制律，通過DA向舵機發送控制指令，形成控制閉環。

三、平臺硬件設計

3.1 系統組成

無人機開源飛控教學實驗平臺硬件部分主要包括三軸轉臺、運動控制箱、實時仿真機、MPC08控制卡、主控上位機和無人機機體結構組成。

無人機機體結構為平臺姿態角實際展示部分，為一個真實的無人機航模，其中主控制器、慣性測量單元、舵機等均已安裝在航模上。通過與實時仿真機通信，由舵機等執行機構能夠實際展現出當前根據不同姿態角無人機航模的反應。

三軸轉臺是平臺的姿態運動模擬器，能夠模擬無人機飛行當中的姿態運動，展現無人機飛行時姿態角度的變化。MPC08控制卡安裝在PC機的PCI插槽內，運動控制箱則是將電源、轉臺驅動器和各種接口控制電路集成與一體，與控制卡連接完成對三軸轉臺三個軸步進電機的控制。實時仿真機負責姿態傳感器數據的獲取、姿態解算算法和舵機控制算法實現等功能。上位機則是完成數據的顯示和對三軸轉臺轉動指令的發送。

3.2 主要器件的介紹

3.2.1 主控制器（STM32F103ZET6）

STM32F103ZET6控制器是一款大容量增強型芯片，內部包含豐富的外設配置，可以滿足平臺所需的計算速度和數據處理、存儲容量，同時具備足夠的擴展性，以便后續工作的補充。

3.2.1.1 技術指標

?                      內存：512k；

?                      SRAM：64k；

?                      靜態存儲控制器：有；

?                      定時器：通用定時器（4）、高級定時器（2）、基本定時器（2）；

?                      通信接口：SPI（3）、I2C（2）、UART（5）、USB（1）、CAN（1）、SDIO（1）；

?                      GPIO端口：112；

?                      12位ADC模塊（通道數）：3（21）；

?                      CPU頻率：72MHz；

?                      工作電壓：2.0-3.6V；

?                      封裝形式：LQFP114、BGA144

3.2.2 三軸電動轉臺（TT-3DM-3E-10）

如圖3.1所示，三軸電動轉臺TT-3DM-3E-10采用UUO形鋁合金框架結構，由內環橫滾軸框架、外環俯仰軸框架、以及方位軸底座組成相互垂直的三維旋轉坐標系，采集控制器串行接口連接上位計算機實現測量控制。

圖3.1  三軸電動轉臺TT-3DM-3E-10

3.2.2.1 特點

?                      操作使用方便，便于搬動和攜帶；

?                      旋轉角位置動態跟蹤測量與控制；

?                      串行輸出實時角位置數據 ，串行輸入控制指令；

?                      位置、速率、搖擺功能。

3.2.2.2 技術指標

?                      負載尺寸重量： 5 kg；

?                      負載及夾具安裝空間：120 mm×120mm×120mm；

?                      三軸轉角范圍：連續無限；

?                      角位置綜合測量精度：±0.05o；

?                      控制到位精度：±0.01o；

?                      速率范圍：0.1o/s～25 o/s；

?                      速率精度與平穩度：1%；

?                      測角數據采集頻率：20Hz；

?                      用戶導電滑環：8環/每環2A；

?                      臺體重量：30 Kg（不含負載）

3.2.3 運動控制卡（MPC08）

MPC08控制卡是一個開放運動控制的平臺，與PC機配合使用，直接插在PC機的PCI槽中，安裝相應的驅動即可使用。

3.2.3.1 技術指標

?                      主接口：PCI 3.3V；

?                      控制軸數：4軸；

?                      編碼器輸入：4路；

?                      編碼器輸入計數器：四軸32bit，符號數2147483647，A/B/Z相（2M pps）；

?                      通用數字輸入：DCV 24/DCV 5，光耦 16點；

?                      通用數字輸出：DCV 24/DCV 5，光耦 16點；

?                      專用輸入：每軸四點（正限位、負限位、原點、減速），報警；

?                      脈沖輸出最大頻率：4M；

?                      脈沖輸出規格：每軸梯形加減速；

?                      脈沖輸出計數器：每軸32bit，符號數2147483647；

?                      變速：運動中變速度；

?                      操作系統：Window 98，Window 2000，Window XP

3.2.4 運動控制箱

運動控制箱用于控制步進電機驅動的旋轉平臺實現多種運動。通過DB62線與MPC08控制卡相連，接收來自PC上位機的各種控制信號。同時通過RS232接口與三軸轉臺相連，由其內部包含的步進電機驅動進行驅動控制三軸轉臺的步進電機。該控制箱最多可獨立或聯合驅動控制三臺步進電機，實現三維空間的任意方式運動。

3.2.5 仿真及測控計算機

如圖3.2，測控計算機是系統的主控計算機，用于系統測試和控制，編制的專用測控軟件都安裝在該計算機系統中。

圖3.2  仿真及測控計算機

3.2.6 慣性測量單元（3DM-E10A）

慣性測量單元3DM-E10A是一款微型的全姿態測量傳感裝置，它由三軸MEMS陀螺、三軸MEMS加速度計、三軸磁阻型磁強計等三種類型的傳感器構成。三軸陀螺用于測量載體三個方向的絕對角速率，三軸加速度計用于測量載體三個方向的加速度，在系統工作中，主要作用是感知系統的水平方向的傾斜，并用于修正陀螺在俯仰和滾動方向的漂移，三軸磁阻型磁強計測量三維地磁強度，用于提供方向角的初始對準以及修正航向角漂移。慣性測量單元3DM-E10A可提供的輸出數據有：原始數據、四元數、姿態數據等（產品外形如圖3.3所示）。更詳細的資料參見慣性測量單元3DM-E10A相關章節。

圖3.3  慣性測量單元3DM-E10A

3.2.6.1 特 性 

?                      實時三軸慣性輸出；

?                      大于100Hz的內部更新率；

?                      尺寸小、重量輕、低功耗；

?                      RS-232/RS-485A串行接口，方便連接。

3.2.6.2 技術參數

?                      輸出數據格式：原始數據、姿態角、四元數；

?                      內部更新率        100        Hz；

?                      啟動時間        < 1         sec；

?                      靜態角度誤差（俯仰、滾動） ± 0.1        degree；

?                      動態角度誤差（俯仰、滾動） ± 2.0        degree；

?                      靜態角度誤差（航向） ± 0.5        degree；

?                      動態角度誤差（航向） ± 2        degree；

?                      航向角分辨率 <0.1        degree；

?                      加速度計測量范圍        ± 2g；

?                      加速度計非線性        0.2 % ；

?                      速率陀螺測量范圍        ± 300°/sec ；

?                      速率陀螺非線性        0.2 %         ；

?                      磁力計測量范圍        ±1.3 Gauss；

?                      磁力計非線性        0.4 %         ；

?                      短時沖擊        500 (Not reinforced) g 。

3.2.6 電機、舵機控制實驗板

如圖3.4所示，實驗板包含固定翼無人機1個、 電機舵機實驗展板，含ST32ARM開發系統、實驗軟件一套。使用主控制器的定時器功能輸出相應的PWM波即可控制舵機的轉動角度。

圖3.4  電機、舵機控制實驗板

四、平臺軟件設計

基于總體方案的設計需求，從四個部分進行軟件實現，包括無人機動力學模型實現、控制算法實現、下位機軟件設計和上位機軟件設計。最終將模型編譯后加載到實時仿真機中進行仿真控制，完成整個無人機半實物仿真教學實驗平臺的閉環測試。

4.1 無人機動力學模型實現

參考牛頓-歐拉方程，得出表征飛機運動特性的12個微分方程，利用Simulink創建無人機六自由度仿真模型。加上由經驗公式得出的氣動力矩計算模塊和大氣模型實現對本體模型的搭建。無人機本體模型的六自由度飛行姿態解算模塊如圖4.1所示。

圖4.1 無人機6自由度數學模型

4.2 控制算法實現

飛控算法采用雙閉環PID控制結合速度前饋，其外環為角度（angle）控制，角度值是由濾波與姿態解算后得到的歐拉角，有延遲且存在誤差，單閉環無法實現姿態控制過程。在此基礎上引入內環，內環選擇角速度（rate）控制，角速度由陀螺儀直接測量得到，誤差小，響應快，延遲短。

4.3 接口模型實現與故障注入

根據仿真系統的設計需求，在MATLAB中利用S函數模板搭建仿真所需要的各設備通信接口和基本數據流，然后在Simulink中定義設備板卡的接口模型，并映射到驅動函數。然后將定義完整的設備模型添加到用戶自定義的模型系統中，在系統模型需要的時候調用。最后，利用RTW模型代碼自動生成工具對設備接口模型進行編譯，完成各仿真系統子模塊的通信交互。

接口模型的功能包括，靜態模型參數調試（包括對特定的傳感器模型賦值或改變某一控制位來測試飛控程序），動態模型參數調試（主要包括在動態飛行過程中改變飛行狀態等）。利用此功能，可對設備接口模型注入靜態故障和動態故障，將故障信息注入仿真平臺。

（1）傳感器故障

平臺傳感器組模塊主要包括慣性測量單元、大氣模型、無線電高度計模型和導航系統模型等。在飛行仿真過程中，對傳感器組接口模型建模時預留故障注入接口，實現傳感器故障狀態實時注入（包括靜態故障注入和動態故障注入）

（2）舵機故障

舵機接口模型的主要作用是通過串口模型將舵機輸出的信號發送到實時仿真機進行相應的姿態解算。因此在對接口建模時預留故障注入接口，可實現實時模擬執行器系統故障注入。

4.3 下位機軟件設計

無人機半實物仿真教學實驗平臺下位機部分主要完成三軸轉臺實時姿態解算，并將相關傳感器測得的數據和姿態發送給主控制器。

姿態解算的主要傳感器3DM-E10A負責獲取轉臺的姿態數據信息，采用I2C總線驅動，與控制器STM32的管腳相連，通過STM32獲取傳感器的數據，繼而進行后面的數據處理。其核心部分為內置加速度計、陀螺儀和磁強計數據的獲取，相應的程序流程圖如圖4.2所示。

圖4.2 數據采集流程圖

對3DM-E10A的數據完成獲取后，控制器STM32需要根據數據完成轉臺實時的姿態的估計。根據四元數法和四階龍格庫塔法解算姿態角，加上磁強計修正偏航角，便可準確實時更新姿態角。其數據融合姿態角解算流程如圖4.3所示。

圖4.3 數據融合姿態角解算流程

4.4 上位機軟件設計

4.4.1 MPC08運動控制系統

實現對三軸轉臺的實時控制是上位機的一大主要功能，MPC08控制卡對三軸轉臺的運動控制功能取決于其內部的運動函數庫。庫中包含了對單軸到多軸轉臺的多種運動函數，包括單軸運動、多軸獨立運動、運動指令執行等方式。利用VS編譯軟件新建工程時，加入相應的動態鏈接庫，然后根據上位機界面的功能所需調用動態函數庫中的函數體來實現轉臺的運動。

同時，考慮到實驗室安裝，調試的準確性，如圖4.4所示我們還增加了“置水平位"和“歸位"操作，分別能夠使轉臺旋轉至初始0度位置，和垂直于水平面的位置。這一功能能夠使在學生完成實驗之后，統一調整轉臺至同一姿態，保證實驗室整齊整潔。

圖4.4 高校飛行控制實驗室設備圖

4.4.2 上位機控制界面設計

上位機主要包括與下位機通信方式的選擇、三軸運動形式的選擇、姿態角指令的輸入、三軸轉臺反饋位置的讀取、姿態角指令文件和樣本采樣文件的數據對比、加速度角速度數據波形顯示及姿態角數據儀表顯示以及姿態3D顯示等相關功能。其上位機界面如圖4.5所示。

圖4.5 上位機界面

3D姿態界面圖如圖4.6所示，其三維圖形用長方體代替無人機。

圖4.6 3D姿態顯示界面

五、實驗內容

可支持開設實驗

5.1 控制率離散化實驗

在控制系統的純數字設計與仿真中，一般應用連續域控制器，但是在半物理仿真中，由于需要將控制算法與硬件相結合，所以必須將連續域控制器離散化。在數學仿真實驗環節中，學生通過對比離散化前后的系統響應，并以主控制器的基本運行方式和涉及的相關原理為背景，有助于開啟從書本中的控制系統到真實控制系統設計觀念的轉變

5.2 仿真模型代碼生成實驗

對模型代碼生成規則進行配置優化。主要涉及參數包括仿真Solver參數，目標平臺參Target Selection和Interface等的配置。其中Solver參數主要完成模塊仿真周期，仿真時間和仿真算法的配置。Interface參數主要是配置代碼生成后的函數調用接口。完成參數配置后，通過Simulink生成mdl模型代碼，在利用編譯器編譯成仿真控制系統能識別的動態庫dll文件，編譯流程如圖5.1所示。利用RTW工具對mdl模型文件進行處理，分析模型文件描述和信息結構以及各模塊間的邏輯關系。利用解析器完成對文件的轉換后生成rtw中間文件。通過目標語言編譯器，RTW配置參數進行控制，生成用戶定義的目標平臺二進制代碼。在生成C代碼過程中，可通過修改聯編文件對代碼生成過程進行編譯和鏈接控制，并最終綜合生成可執行文件。

圖5.1 模型仿真代碼生成過程

5.3 半實物仿真實驗

包括舵機系統硬件仿真、無人機組合導航實驗、硬件閉環實驗以及故障診斷與飛控算法魯棒性評估實驗。

舵機和控制板，可連接實驗終端，做舵機原理及控制實驗，方向舵角度開環控制實驗，升降舵角度開環控制實驗，副翼角度開環控制實驗。

無人機組合導航實驗包括GPS和IMU組合導航標定實驗和無人機導航實驗路徑規劃和軌跡數據采集。如圖5.2和5.3所示，學生可以觀察無人機的實際飛行軌跡。

圖5.2 無人機GPS和IMU組合導航標定實驗

圖5.3 無人機導航實驗路徑規劃和軌跡數據采集

硬件在環實驗是在開環實驗的基礎上，加入控制器將所有硬件形成閉環，將實驗結果與數字離散仿真結果進行對比，可以更直觀地向學生解釋半物理仿真中接入了串口通信、轉臺、傳感器等實物，帶來了附加的延遲、死區等非線性動力學特征的影響。

飛控算法魯棒性評估實驗是在飛行仿真中，對傳感器或執行器注入一定大小的故障，觀察在故障情況下，飛控算法跟蹤控制指令的響應速度和誤差效果，用以評估算法的魯棒性能。