基于APM的高級飛控系統(tǒng)設(shè)計

龔玉鋒，趙文杰，何通能

(1.浙江工業(yè)大學(xué) 信息工程學(xué)院，杭州 310023；2.浙江大學(xué) 航空航天學(xué)院，杭州 310027)

0 引言

無人機(UAV，unmanned aerial vehicle)是現(xiàn)代科技中集成了多方面高科技技術(shù)的一種無人駕駛飛行器的名稱，并且由于現(xiàn)代微電子技術(shù)的發(fā)展，無人機的成本可控制到較低，可以集群部署用于大規(guī)模任務(wù)。無人機的基本硬件組成包括傳感器系統(tǒng)、處理器以及控制機構(gòu)。無人機系統(tǒng)(UAS,unmanned aerial system)包括無人機以及相關(guān)配套設(shè)施，由地面站模塊、數(shù)據(jù)通信模塊、命令指揮控制以及維護人員等部分組成，采用算法和軟件流程達到高度智能化閉環(huán)反饋控制[1]。APM(Ardupilot)飛控是現(xiàn)有開源飛控中最為成熟穩(wěn)定的開源飛控系統(tǒng)，且和PX4飛控使用同一套底層PIXHawk硬件,國際也有使用PIXHawk研究控制算法的研究者[2]，APM擁有廣大使用者和成熟的開發(fā)社區(qū)，適用于多種無人駕駛系統(tǒng)，如固定翼、無人車、無人潛航器等[2]，其功能比較完善，APM飛控基本具備現(xiàn)代無人機飛控技術(shù)的大多數(shù)控制功能，飛控如果從頭開發(fā)周期會非常漫長，因為無人控制系統(tǒng)非常復(fù)雜，因此在開源飛控上進行二次開發(fā)是一種優(yōu)秀的選擇，但開源飛控軟硬件偏于無人機愛好者和研究者[3]，且操作復(fù)雜、人工參與量大，需要有效發(fā)揮無人機的優(yōu)勢，無法滿足無人機在日益增長的多方面民用、工業(yè)、商業(yè)需求，需要進一步提升飛控系統(tǒng)自動化、智能化、平臺化、系統(tǒng)化。開源飛控的源代碼已經(jīng)非常完善，對于修改其源代碼復(fù)雜度很高且具有危險性，APM飛控的代碼1.7 M已經(jīng)接近滿負荷存儲2 M，且其RAM空間必須預(yù)留30%，額外的修改代碼會影響源飛控的代碼執(zhí)行效率，將系統(tǒng)分層是一種工程上非常重要的做法，通過使用MAVLink協(xié)議完成不同層次模塊的通信，且開發(fā)出的系統(tǒng)適用于同協(xié)議的其他飛控。

1 基于高級飛控系統(tǒng)硬件系統(tǒng)設(shè)計

1.1 高級飛控系統(tǒng)硬件系統(tǒng)設(shè)計

本文所設(shè)計的高級飛控系統(tǒng)是一種獨立的硬件模塊，通過通信接口與其他模塊進行通信。將設(shè)計的高級飛控系統(tǒng)搭載在無人機上，通過高級飛控系統(tǒng)上的通信接口完成對無人機的通信和無線數(shù)傳的通信。本文采用的底層硬件為PIXHawk2.4.8.版本[4]，經(jīng)過運行相應(yīng)的算法程序測試，證明了硬件設(shè)計方案的可行性，且運行帶操作系統(tǒng)的程序經(jīng)歷長時間運行不會出現(xiàn)死機等情況，這套硬件系統(tǒng)在飛控領(lǐng)域具備相當(dāng)?shù)姆€(wěn)定性與可靠性[5]。高級飛控系統(tǒng)與APM飛控、無線數(shù)傳連接方案如圖1所示。

圖1 系統(tǒng)連接方案整體架構(gòu)圖

APM飛控的底層硬件PIXHawk是基于Cortex-M4內(nèi)核，工作頻率為168 MHz，本文設(shè)計的核心處理器選擇了與之匹配的同系列STM32H743芯片，STM32處理器在飛控領(lǐng)域擁有大量的使用者[6]，改核心頻率高達400 MHz，可以高效處理飛控相關(guān)的復(fù)雜運算，依賴于STM32H7的硬件FPU浮點運算，以及DSP算法庫，主流的算法函數(shù)都能以專有的加速模式運行，對于飛控領(lǐng)域的常用函數(shù)具有非常好的軟硬件優(yōu)化效果?？刂葡到y(tǒng)在嵌入式平臺高速完成各種程序和函數(shù)運算，再與APM通信時采用串口直連的方式通信，小型四旋翼的安裝位置有限，高級飛控系統(tǒng)內(nèi)部無慣導(dǎo)傳感器可以捆綁固定在電池下方，在六軸及大四軸可以很方便地固定在底層飛控APM的旁邊，無人機中間底盤有許多螺絲孔可以擴展安裝位置，安裝方式如圖2所示。

圖2 高級飛控系統(tǒng)安裝示意圖

由于無人機內(nèi)部空間限制，需要將PCB設(shè)計盡量地緊湊，還需考慮航空環(huán)境比一般的普通環(huán)境要惡劣，電路板最終需要裝在帶屏蔽的金屬盒內(nèi)，做密封防水處理以保護內(nèi)部模塊，以及提供良好的電磁屏蔽，采用航空航天級別J30J接插接件引出通信接口以及電源線。

1.2 電源供電系統(tǒng)設(shè)計

穩(wěn)定的硬件系統(tǒng)設(shè)計依賴于穩(wěn)定的電源系統(tǒng)，選擇URB2405YMD-6WR3型號DC-DC隔離電源模塊作為外部寬電源9～36 V輸入電壓轉(zhuǎn)換器，輸出為6 W，將隔離電源的DC-DC模塊輸出分為兩路供電，一路給主處理器供電，一路給外圍的芯片和接口供電，提高系統(tǒng)的電源安全性，主電源相關(guān)電路電源運行需要設(shè)計指示燈，放電源反接MOS管，壓敏電阻14D560 K保護，以及防浪涌TVS保護二極管，以及電阻、電容、電感構(gòu)成濾波與穩(wěn)壓電路，電源供電設(shè)計如圖3所示。

圖3 高級飛控系統(tǒng)電源模塊設(shè)計圖

2 高級飛控系統(tǒng)通信協(xié)議與操作系統(tǒng)

APM飛控的程序為上層開發(fā)開放了基于MAVLink協(xié)議的控制API，可以通過協(xié)議消息實現(xiàn)控制無人機，本文的APM軟件版本為ArduCopter V3.3，飛控硬件版本為Pixhawk2.4.8，同時控制無人機需要在高級飛控系統(tǒng)上使用合適的嵌入式操作系統(tǒng)。

2.1 無人機MAVLink通信協(xié)議

在通信協(xié)議方面需要一種簡單高效的協(xié)議，因為無人機的通信通常距離較遠時通信數(shù)據(jù)量減少，而MAVLink提供了一種非常輕量級的消息傳遞協(xié)議，用于與無人機(以及機載無人機組件之間)進行通信。MAVLink非常高效，由于它不需要任何其他框架，因此非常適合通信帶寬非常有限的應(yīng)用。除了常規(guī)的控制命令和消息，可以使用高級飛控系統(tǒng)控制復(fù)雜任務(wù)，如任務(wù)計劃、航跡規(guī)劃、特殊姿態(tài)控制等，該協(xié)議的完備性和科學(xué)性使得它協(xié)議在自駕儀領(lǐng)域獲得了廣泛的采用，APM和PX4等飛控都使用此協(xié)議，本文的通信主要使用固有協(xié)議和自定義的擴展消息協(xié)議[7]。

2.2 飛控UCOSⅢ操作系統(tǒng)

APM飛控由于出現(xiàn)較早，沒有使用操作系統(tǒng)，現(xiàn)在已經(jīng)出現(xiàn)很多開發(fā)不便的弊端，高級飛控系統(tǒng)的算法軟件比較復(fù)雜，使用裸機開發(fā)已經(jīng)不適合，需要選擇一種合適的嵌入式實時操作系統(tǒng)，UCOSⅢ非常適合需要安全認證的航空航天系統(tǒng)。它由 Jean Labrosse1998年更新到UCOS-II,并于2000年得到美國航天管理局(FAA)認證,可以用于飛行器中，使用操作系統(tǒng)的開發(fā)方式可以方便的完成裸機所不具備的系統(tǒng)調(diào)度、優(yōu)先級分配、多線程編程等方式，無人機控制需要高的實時性且多線程任務(wù)同時進行，本文選擇UCOSIII為無人機的算法和程序提供基礎(chǔ)的操作系統(tǒng)，本文在高級飛控系統(tǒng)的MCU上移植了UCOSIII，并與MAVLink協(xié)議一起運行，可以實現(xiàn)協(xié)議的高速收發(fā)解析，保障通信功能的穩(wěn)定高效。

3 高級飛控系統(tǒng)基本功能控制方式

采用科學(xué)的系統(tǒng)分層，使每一層的功能更加明確。兩個模塊各自處理自身數(shù)據(jù)，飛控模塊專注于無人機的控制，高實時性和穩(wěn)定性，高級飛控模塊則專注于高級控制算法、通信組網(wǎng)、路徑規(guī)劃算法等功能。這種無人機控制方式充分發(fā)揮了開源飛控的強大功能和高級飛控的自定義高級功能特性使之能適夠應(yīng)各種需求場景。

3.1 一鍵起飛APM無人機算法設(shè)計

APM等開源飛控的控制系統(tǒng)比較成熟，其操作步驟也比較嚴謹和復(fù)雜，無人機的一鍵起飛包含多步驟檢查和判斷，是無人機自動化起飛基本操作，本文設(shè)計控制APM飛控的一鍵起飛的流程，通過算法編程使無人機系統(tǒng)能夠自動完成無人機的起飛的過程，可以有效減少操作者檢查無人機、切換模式、解鎖飛機、發(fā)送起飛命令的一些列操作流程，以一套自動化流程程序替代[8]，高級飛控系統(tǒng)一鍵起飛流程控制如圖4所示。

圖4 一鍵起飛控制流程圖

3.1.1 解鎖無人機

APM無人機在GPS信號良好的情況下才能解鎖，例如在室內(nèi)信號不好的情況時無法解鎖，無人機GPS信號良好的情況下，正面的主指示燈會由藍色變色綠色，綠色燈閃爍代表GPS信號良好，當(dāng)無人機解鎖時，綠燈會變?yōu)槌Ａ?，解鎖指令屬于CMD命令指令，將mavlink_command_long_t的command參數(shù)填充為400(解鎖/上鎖)，將parama1參數(shù)設(shè)定為0解鎖，1為上鎖，配置好解鎖指令參數(shù)時，通過MAVLink協(xié)議參數(shù)指令消發(fā)出。

3.1.2 請求無人機數(shù)據(jù)流

通常需要發(fā)送請求數(shù)據(jù)流來接收無人機的全部數(shù)據(jù)流，否則某些設(shè)定的無人機只會廣播心跳包，心跳包包含了無人機基本模式、用戶模式、無人機系統(tǒng)類型、無人機種類、系統(tǒng)狀態(tài)、MAVLink軟件版本，全部數(shù)據(jù)流包含無人機的空速、高度、GPS位置、傳感器數(shù)據(jù)等。請求數(shù)據(jù)流的消息為協(xié)議 66號mavlink_request_data_stream_t，通過填充相應(yīng)的參數(shù)即可完成數(shù)據(jù)流請求，設(shè)置參數(shù)start_stop為1即可開始請求數(shù)據(jù)。請求數(shù)據(jù)流為 66號MSG消息包。

3.1.3 設(shè)定無人機飛行模式

APM具有多種飛行模式有很多，常用模式有自穩(wěn)模式、自動模式、引導(dǎo)模式、定高模式、留待模式、返航模式、定點模式、特技模式等，在不同應(yīng)用場景下可發(fā)送對應(yīng)指令進行切換，一鍵起飛功能需要將模式設(shè)置為GUIDE引導(dǎo)模式，發(fā)送mavlink_set_mode_t消息包將對應(yīng)的模式參數(shù)custom_mode設(shè)置為4(GUIDE引導(dǎo)模式)。

3.1.4 一鍵起飛

一鍵起飛命令需要無人機完成以上步驟之后，發(fā)送MAVLink的CMD命令TAKEOFF完成起飛動作，建議測試時將起飛參數(shù)高度設(shè)置為10 m，其他默認即可，測試高級飛控系統(tǒng)程序按照軟件流程發(fā)送一鍵起飛的算法命令，成功解鎖并切換模式，然后無人機螺旋槳開始旋轉(zhuǎn)。采用以上命令算法流程寫入高級飛控系統(tǒng)后，在GPS良好的場地上，經(jīng)測試完成一鍵起飛功能。

3.2 高級飛控系統(tǒng)的遙控器功能

無人機的遙控器需要手動操控，主要操作量為油門、偏航、俯仰和橫滾，這些操作量的控制以人的判斷為根據(jù)，結(jié)合現(xiàn)在技術(shù)進展飛快的機器視覺和激光雷達等技術(shù)，可以設(shè)計相關(guān)算法來代替人的判斷，而執(zhí)行這些操作，則需要使用到MAVLink協(xié)議的控制無人機姿態(tài)的方法。

本文設(shè)計的控制流程可以通過算法程序在高級飛控系統(tǒng)以及地面站實現(xiàn)遙控器的手柄搖桿量的操作功能，通過MAVLink的#70號消息可以實現(xiàn)模擬遙控器控制，本文所測試的APM版本遙控器可控制通道為chan1～chan8共8個通道，四旋翼需要用到4個chan通道，每個通道的控制數(shù)值范圍為1 000～2 000，此流程需要解鎖無人機解鎖命令、修改模式為自穩(wěn)、發(fā)送遙控指令，實現(xiàn)對無人機模擬遙控器的直接解鎖與啟動，軟件程序?qū)PM本文測試將PWM通道的電機chan值設(shè)定為1 600，無人機正確解鎖并啟動，遙控功能控制流程如圖5所示。

圖5 遙控功能控制流程圖

3.3 高級飛控系統(tǒng)控制APM的任務(wù)功能

APM飛控模式中有自動模式，自動模式下可以執(zhí)行APM飛控所存儲的任務(wù)，任務(wù)可以是航點或者其他命令，CMD命令類型選擇如TAKE_OFF起飛、CMD_NAV_WAYPOINT導(dǎo)航至航點、LAND返航、LOITER懸停、RTL返航等操作，都可以使用任務(wù)規(guī)劃的航點命令來執(zhí)行，將上面的操作寫成一系列任務(wù)，當(dāng)APM無人機切換到自動模式時將開始執(zhí)行APM所存儲的任務(wù)。

任務(wù)計劃可以實時控制無人機，也可以提前規(guī)劃寫入控制量，使用#39號消息mission_item和任務(wù)協(xié)議將一系列的任務(wù)寫入APM飛控，由無人機自動執(zhí)行寫入的飛行計劃，航點的任務(wù)參數(shù)說明如表1所示。

表1 航點任務(wù)的參數(shù)說明

由多個航點任務(wù)所組成的典型任務(wù)結(jié)構(gòu)可為第一個點TAKEOFF起飛，第二航點為目的航點WAYPOINT，第三個航點為RETURN_TO_LANUCH返回出發(fā)點，通過高級飛控系統(tǒng)將APM無人機模式切換為AUTO自動模式，將航點任務(wù)參數(shù)填充至MISSION_ITEM，寫入航點的任務(wù)協(xié)議流程圖6所示。

圖6 航點上傳協(xié)議

4 高級飛控系統(tǒng)自定義功能設(shè)計

高級飛控系統(tǒng)的一個重要功能便是將控制分層，在高級飛控系統(tǒng)層上構(gòu)建組網(wǎng)功能[9]。在使用高級飛控系統(tǒng)控制多架無人機時無須一對一進行復(fù)雜繁瑣的連接通信，而由一架主機執(zhí)行算法程序以控制其他高級飛控功系統(tǒng)完成對底層APM無人機的控制，提高了控制效率和系統(tǒng)的智能化，以及可以利用這套無人機組網(wǎng)協(xié)議構(gòu)建智能控制集群算法[10]、運行航跡規(guī)劃等功能。

4.1 高級飛控系統(tǒng)組網(wǎng)架構(gòu)

開源飛控?zé)o人機只具備地面站和無人機，或者遙控器和無人機之間的一對一通信，具備了組網(wǎng)條件下可以使用多無人機路徑規(guī)劃、任務(wù)分配等智能算法來集群規(guī)劃無人機。本文所設(shè)計的高級飛控系統(tǒng)硬件接口連接無線電模塊(測試采用了LORA無線通信模塊)，將所有通信模塊調(diào)節(jié)到一個信道，采用主機輪詢算法程序控制互相通信，將無人機間的通信鏈路打通，實現(xiàn)無人機間的互相通信。本文的組網(wǎng)功能是基于MAVLink-V1.0協(xié)議地址序號標(biāo)識進行組網(wǎng)，組網(wǎng)無人機數(shù)量不超過256，默認APM的系統(tǒng)(SYSID)地址為01，在所有MAVLink指令都會攜帶目標(biāo)地址(TARGETID)，這樣就構(gòu)成基于地址身份識別的功能。高級飛控系統(tǒng)上通過算法程序可實現(xiàn)編隊，無人機數(shù)量統(tǒng)計，任務(wù)分配等組網(wǎng)功能，地面站作為監(jiān)控設(shè)備或者命名下達設(shè)備，配置01地址的高級飛控系統(tǒng)為主機，組網(wǎng)架構(gòu)如圖7所示。

圖7 無人機組網(wǎng)架構(gòu)

4.2 高級飛控系統(tǒng)通信協(xié)議配置與精簡

自定義通信組網(wǎng)擴展消息仍然使用MAVLink協(xié)議，該協(xié)議具有自定義消息的功能，通過自定義擴展消息包，可完成大量參數(shù)的通信和設(shè)置。以源碼庫的commmon.xml為模板生成，從預(yù)留的自定義配置150開始的id 消息來生成，名稱(name)和id不能重復(fù)，按照標(biāo)準(zhǔn)格式參數(shù)填充，即可配置為自定義MAVLink消息包。

在組網(wǎng)系統(tǒng)中信息量會大增，更加精簡的消息流，可以更快速地完成網(wǎng)絡(luò)信息通信和降低通信負載。本文實際測試了APM無人機輸出數(shù)據(jù)流的有效消息包，經(jīng)過對數(shù)據(jù)流的分析驗證，將消息包進行裁剪，裁剪過后的MAVLink消息包，消息核心數(shù)據(jù)保留#0心跳包、#1系統(tǒng)狀態(tài)、#2系統(tǒng)時間、#24定位GPS數(shù)據(jù)、#33姿態(tài)數(shù)據(jù)、#36伺服電機控制值輸出、#62無人機導(dǎo)航控制消息、#74無人機空速計等速度參數(shù)、#141無人機海拔和高度信息，如果無人機添加了如光流等組件可以將對應(yīng)的消息包再添加至數(shù)據(jù)結(jié)構(gòu)體即可。

4.3 高級飛控系統(tǒng)的運算能力半實物仿真

無人機通信組網(wǎng)下的自動航跡規(guī)劃與任務(wù)分配是是無人機發(fā)展和研究的重點，在控制無人機群的集群控制算法中，常用有蟻群算法、魚群算法、鳥群算法(粒子群算法)等[11]，粒子群算法對于無人機群的目標(biāo)搜索與路徑規(guī)劃有很好的效果。典型的無人機飛行航線與目標(biāo)分配都需要人為手動操作，將智能無人機算法在本文所設(shè)計的高級飛控系統(tǒng)上測試仿真，使用智能算法的無人機規(guī)劃將使得無人機任務(wù)性能得到提升。本文實驗所采用的粒子群算法在嵌入式C語言平臺設(shè)計，運行在高級飛控系統(tǒng)中，對無人機特性進行數(shù)學(xué)建模模擬，無人機路徑規(guī)劃與任務(wù)分配仿真群算法參數(shù)如表2所示。

表2 無人機粒子群算法參數(shù)設(shè)置

運行占用RAM空間為500 KB字節(jié)的粒子群無人機搜索、路徑規(guī)劃、任務(wù)分配算法仿真時，程序不斷將無人機自身的位置坐標(biāo)、目標(biāo)坐標(biāo)、狀態(tài)信息等消息發(fā)送，程序更新無人機位置與期望參數(shù)的迭代計算周期為200 ms，這個迭代更新速度滿足無人機的實際更新速率要求1 s以內(nèi)。高級飛控系統(tǒng)的運算輸出作為無人機路線飛行航跡圖，將STM32H7處理器的無人機仿真算法位置通過串口傳輸給PC端，模擬9架無人機的搜索路線，X軸與Y軸代表空間的二維圖，航跡顯示很好的規(guī)避免無人機搜索空間時路徑重疊問題和搜索時自動規(guī)劃無人機路線，PC端可視化顯示無人機航跡路線如圖8所示。

圖8 仿真多架無人機搜索航跡路徑圖

5 結(jié)束語

本文所設(shè)計的高級飛控系統(tǒng)為開源飛控和高級自定義功能設(shè)計了可行方案，采用本文所設(shè)計的控制方法可以對無人機進行高度的智能化和自動化控制，測試了使用MAVLink協(xié)議控制飛控的多種基礎(chǔ)功能，高級飛控稍作針對性優(yōu)化就可以應(yīng)用與其他同協(xié)議飛控如PX4，高級飛控系統(tǒng)可以作為編隊組網(wǎng)算法、目標(biāo)搜索算法、路徑規(guī)劃算法、任務(wù)分配法等高級控制功能的實現(xiàn)平臺，本文對這些二次開發(fā)功能進行了實際的測試與半實物仿真，結(jié)果顯示計算能力優(yōu)異和運行穩(wěn)定，基于MAVLink協(xié)議可以將算法給出的控制量傳遞給無人機飛控實現(xiàn)對無人機的飛行任務(wù)操控，這種控制方式和控制流程對于無人機二次開發(fā)具有相當(dāng)架構(gòu)和方法參考價值和實際應(yīng)用價值。