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

一  概  述

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

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

（2）姿態(tài)監(jiān)視和調(diào)參：飛行仿真模型運行過程中，可以利用外部調(diào)試接口實時顯示姿態(tài)信息和調(diào)整參數(shù)；

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

二、系統(tǒng)總體設計

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

圖2.1 平臺系統(tǒng)結構圖

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

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

三、平臺硬件設計

3.1 系統(tǒng)組成

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

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

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

3.2 主要器件的介紹

3.2.1 主控制器（STM32F103ZET6）

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

3.2.1.1 技術指標

?                      內(nèi)存：512k；

?                      SRAM：64k；

?                      靜態(tài)存儲控制器：有；

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

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

?                      GPIO端口：112；

?                      12位ADC模塊（通道數(shù)）：3（21）；

?                      CPU頻率：72MHz；

?                      工作電壓：2.0-3.6V；

?                      封裝形式：LQFP114、BGA144

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

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

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

3.2.2.1 特點

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

?                      旋轉角位置動態(tài)跟蹤測量與控制；

?                      串行輸出實時角位置數(shù)據(jù) ，串行輸入控制指令；

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

3.2.2.2 技術指標

?                      負載尺寸重量： 5 kg；

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

?                      三軸轉角范圍：連續(xù)無限；

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

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

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

?                      速率精度與平穩(wěn)度：1%；

?                      測角數(shù)據(jù)采集頻率：20Hz；

?                      用戶導電滑環(huán)：8環(huán)/每環(huán)2A；

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

3.2.3 運動控制卡（MPC08）

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

3.2.3.1 技術指標

?                      主接口：PCI 3.3V；

?                      控制軸數(shù)：4軸；

?                      編碼器輸入：4路；

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

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

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

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

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

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

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

?                      變速：運動中變速度；

?                      操作系統(tǒng)：Window 98，Window 2000，Window XP

3.2.4 運動控制箱

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

3.2.5 仿真及測控計算機

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

圖3.2  仿真及測控計算機

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

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

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

3.2.6.1 特 性 

?                      實時三軸慣性輸出；

?                      大于100Hz的內(nèi)部更新率；

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

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

3.2.6.2 技術參數(shù)

?                      輸出數(shù)據(jù)格式：原始數(shù)據(jù)、姿態(tài)角、四元數(shù)；

?                      內(nèi)部更新率        100        Hz；

?                      啟動時間        < 1         sec；

?                      靜態(tài)角度誤差（俯仰、滾動） ± 0.1        degree；

?                      動態(tài)角度誤差（俯仰、滾動） ± 2.0        degree；

?                      靜態(tài)角度誤差（航向） ± 0.5        degree；

?                      動態(tài)角度誤差（航向） ± 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開發(fā)系統(tǒng)、實驗軟件一套。使用主控制器的定時器功能輸出相應的PWM波即可控制舵機的轉動角度。

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

四、平臺軟件設計

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

4.1 無人機動力學模型實現(xiàn)

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

圖4.1 無人機6自由度數(shù)學模型

4.2 控制算法實現(xiàn)

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

4.3 接口模型實現(xiàn)與故障注入

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

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

（1）傳感器故障

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

（2）舵機故障

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

4.3 下位機軟件設計

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

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

圖4.2 數(shù)據(jù)采集流程圖

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

圖4.3 數(shù)據(jù)融合姿態(tài)角解算流程

4.4 上位機軟件設計

4.4.1 MPC08運動控制系統(tǒng)

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

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

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

4.4.2 上位機控制界面設計

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

圖4.5 上位機界面

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

圖4.6 3D姿態(tài)顯示界面

五、實驗內(nèi)容

可支持開設實驗

5.1 控制率離散化實驗

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

5.2 仿真模型代碼生成實驗

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

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

5.3 半實物仿真實驗

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

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

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

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

圖5.3 無人機導航實驗路徑規(guī)劃和軌跡數(shù)據(jù)采集

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

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