基于APM的高級飛控系統設計

龔玉鋒，趙文杰，何通能

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

0 引言

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

1 基于高級飛控系統硬件系統設計

1.1 高級飛控系統硬件系統設計

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

圖1 系統連接方案整體架構圖

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

圖2 高級飛控系統安裝示意圖

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

1.2 電源供電系統設計

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

圖3 高級飛控系統電源模塊設計圖

2 高級飛控系統通信協議與操作系統

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

2.1 無人機MAVLink通信協議

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

2.2 飛控UCOSⅢ操作系統

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

3 高級飛控系統基本功能控制方式

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

3.1 一鍵起飛APM無人機算法設計

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

圖4 一鍵起飛控制流程圖

3.1.1 解鎖無人機

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

3.1.2 請求無人機數據流

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

3.1.3 設定無人機飛行模式

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

3.1.4 一鍵起飛

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

3.2 高級飛控系統的遙控器功能

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

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

圖5 遙控功能控制流程圖

3.3 高級飛控系統控制APM的任務功能

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

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

表1 航點任務的參數說明

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

圖6 航點上傳協議

4 高級飛控系統自定義功能設計

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

4.1 高級飛控系統組網架構

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

圖7 無人機組網架構

4.2 高級飛控系統通信協議配置與精簡

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

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

4.3 高級飛控系統的運算能力半實物仿真

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

表2 無人機粒子群算法參數設置

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

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

5 結束語

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