無人機(jī)飛控仿真教學(xué)實驗平臺

一  概  述

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

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

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

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

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

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

圖2.1 平臺系統(tǒng)結(jié)構(gòu)圖

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

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

三、平臺硬件設(shè)計

3.1 系統(tǒng)組成

無人機(jī)開源飛控教學(xué)實驗平臺硬件部分主要包括三軸轉(zhuǎn)臺、運動控制箱、實時仿真機(jī)、MPC08控制卡、主控上位機(jī)和無人機(jī)機(jī)體結(jié)構(gòu)組成。

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

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

3.2 主要器件的介紹

3.2.1 主控制器（STM32F103ZET6）

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

3.2.1.1 技術(shù)指標(biāo)

?                      內(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 三軸電動轉(zhuǎn)臺（TT-3DM-3E-10）

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

圖3.1  三軸電動轉(zhuǎn)臺TT-3DM-3E-10

3.2.2.1 特點

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

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

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

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

3.2.2.2 技術(shù)指標(biāo)

?                      負(fù)載尺寸重量： 5 kg；

?                      負(fù)載及夾具安裝空間：120 mm×120mm×120mm；

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

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

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

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

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

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

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

?                      臺體重量：30 Kg（不含負(fù)載）

3.2.3 運動控制卡（MPC08）

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

3.2.3.1 技術(shù)指標(biāo)

?                      主接口：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點；

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

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

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

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

?                      變速：運動中變速度；

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

3.2.4 運動控制箱

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

3.2.5 仿真及測控計算機(jī)

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

圖3.2  仿真及測控計算機(jī)

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

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

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

3.2.6.1 特 性 

?                      實時三軸慣性輸出；

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

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

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

3.2.6.2 技術(shù)參數(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 電機(jī)、舵機(jī)控制實驗板

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

圖3.4  電機(jī)、舵機(jī)控制實驗板

四、平臺軟件設(shè)計

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

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

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

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

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

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

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

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

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

（1）傳感器故障

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

（2）舵機(jī)故障

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

4.3 下位機(jī)軟件設(shè)計

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

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

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

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

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

4.4 上位機(jī)軟件設(shè)計

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

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

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

圖4.4 高校飛行控制實驗室設(shè)備圖

4.4.2 上位機(jī)控制界面設(shè)計

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

圖4.5 上位機(jī)界面

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

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

五、實驗內(nèi)容

可支持開設(shè)實驗

5.1 控制率離散化實驗

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

5.2 仿真模型代碼生成實驗

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

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

5.3 半實物仿真實驗

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

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

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

圖5.2 無人機(jī)GPS和IMU組合導(dǎo)航標(biāo)定實驗

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

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

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