基于UWB定位的蟻群無人機的設計

張耀金 黃沁穎 李子韓 劉宇健 孔文迪 王月

1研究背景與意義

縱觀國際層面，四旋翼無人機已經成為世界范圍內研究的標準平臺，研究進度十分超前。在軍用領域，無人飛行器由于可以代替飛行員執行戰場緊急危險任務以及執行超越人類生理極限的任務。在民用領域，無人機在農業植保、環境保護、國土監測、科學考察、氣象探測、應急搶險、公共安全等領域有著豐富的需求。

2四軸飛行器控制原理

2.1 PID控制算法

四軸飛行器姿態的控制用到 PID 控制算法，本文為實現飛行器姿態控制算法采用串級 PID 控制方法，其本質是根據期望值和實際值的偏差計算出電機的控制量，PID控制器主要由比例單元、積分單元和微分單元組成。

（1）比例控制算法

比例控制算法利用實際與期望的差值乘增益來影響輸出值，其公式為：

（2）積分控制算法

積分控制算法利用實際與期望的差值的累加和乘增益來影響輸出值，其公式為：

（3）微分控制算法

微分控制算法利用實際與期望的差值的變化快慢乘增益來影響輸出值，其公式為：

根據以上三種算法，可以得到PID控制的表達式為：

2.2四軸飛行器的算法

2.2.1歐拉角

歐拉角是用來唯一地確定定點轉動剛體位置的三個一組獨立角參量，由章動角θ、進動角ψ和自轉角φ組成。本文將yz軸作為參考系的參考軸，并且將xy平面和XY平面的交線稱作為N。

2.2.2四元數

與三維旋轉矩陣相比，四元數可以更好的給出旋轉角度和旋轉軸。因此，三維旋轉用四元數來表示。若要將一個復數旋轉任意角度，只用乘以另一個復數，乘數與實軸的夾角角度為被乘數要旋轉的角度。四元數，就是有四個變量的數，其基本公式為：

3四軸飛行器硬件組成

3.1總體設計方案

四軸飛行器由兩個MCU共同控制，它的任務是控制四旋翼無人機完成的基礎飛行中的各個任務，在飛行控制板上有很多擴展接口，它可以讓飛行器對多個環境適用。

3.2主控MCU

主控器是一個系統的核心單元，本文采用的飛行器主控MCU為STM32F411CEU6。

姿態傳感器主要由加速度計、陀螺儀、磁力計和氣壓傳感器組成，其中3軸加速度傳感器、3軸陀螺儀傳感器和3軸磁力計集成于九軸傳感器芯片MPU9250。

氣壓傳感器BMP280是BOSCH公司生產的一個高精度低功耗數字氣壓傳感器，四軸飛行器的通信芯片選擇NRF51822，并且選擇一顆為RFX2401C的功率放大芯片。功率放大芯片RFX2401C是集成電路RF單片機。

4四軸飛行器軟件原理

4.1 NRF5822程序框架

NRF51822用來控制四軸飛行器無線通信和管理電源。

esb.c是四軸飛行器的無線驅動代碼，system.c是四軸飛行器系統滴答定時器驅動代碼，uart.c是飛行器的串口驅動代碼最下方的button.c是飛行器的按鍵驅動代碼，pm.c是飛行器的電源管理驅動代碼。

無線通信的過程是，遙控器發送的數據包通過radiolink.c發送給飛行器的main.c，之后main.c接受數據包并對其進行解析，若發送對象不是NRF51822就將數據包經過uartlink.c發送給STM32F411。當STM32F411接收到一條來自uartlink.c的數據包時。則返回一條數據包通過uartlink.c給main.c，最后main.c再經過radiolink.c轉發給遙控器，通過以上步驟可以發現NRF51822起到了重要的通信橋梁作用。

4.2 STM32F411程序框架

radiolinkTask無線通信任務，將接收NRF51822發送的串口數據并打包為ATKP格式發給atkpRxAnlTask的接收隊列中，與此同時回發一幀數據給NRF51822。

usblinkRxTask USB通信接收任務，此任務接收從上位機通過USB虛擬串口的方式發送的數據，將數據包發送給atkpRxAnlTask的接收隊列中。

atkpRxAnlTask是接收ATKP數據包并處理，這項任務重要地負責來自上位機和遙控器發送的數據包的處理工作。

stabilizerTask是所有任務最終的實現單元，代表著控制四軸飛行器的平衡飛行。

wifilinkTask代表手機控制任務，此任務通過接收Wifi攝像頭模塊的數據，對于Wifi攝像頭模塊相應的通訊協議來將數據解析為相對應的控制指令發送給stabilizerTask。

atkpTxTask代表的是ATKP數據包的發送任務，這個任務是將stabilizerTask中獲取到的傳感器、PWM輸出數據和姿態數據等進行收取發給usblinkTxTask和radiolinkTask，即發送給上位機和遙控器。

usblinkTxTask代表的是usb通信的發送任務，將atkpTxTask要發送的姿態數據和傳感器數據等數據包發送給上位機。

4.3姿態解算和PID算法流程圖

四軸飛行器有兩個相異的控制方法，分別是常用的手動模式和定高模式。手動模式中，四軸飛行器的油門值與設定油門值相等。定高模式中，四軸飛行器的控制油門會被Z軸速度替換，由于本文所設計的四軸飛行器配備了一顆高精度氣壓計BMP280，通過該模式可以使飛行器在空中定高巡航，對氣壓傳感器數據和垂直地面的速度數據進行疊加獲得高度的實際測量值。高度的設定值是對Z軸設定值進行積分得到的，同樣將其與實際值做差運算，得到的目標量作為輸入進入Z軸高度PID控制器，輸出值即為油門變化量，變化量與油門基礎值的和為油門實際值。

4.4 Z軸高度環PID

真實油門值由兩部分合成，油門的基本參考值確定了四軸飛行器在固定高度飛行時脫離控制的油門大小，它可以為四軸飛行器的空中懸停起很大的幫助，并且對四軸飛行器固定高度的狀態有影響。

4.54D空翻算法與基礎原理

實現4D空翻的主要秘訣為內環PID。將姿態角度的設定值當做角速度換的設定值，這是控制翻滾的基礎。

5實驗結果分析與展望

實驗所需的飛行器可采取串口方式與匿名科創地面站通信，進而調試參數和PID系數。本文使用兩條白色線將四軸飛行器拴在室內黑凳子的扶手處，扶手高度足夠高已保證四軸飛行器任何情況下不與凳子接觸。另一端栓在固定電池的排針處。

本文提起的四軸飛行器可以隨意使用傳感器模塊并轉換不同工作模式，從而讓飛行器對多種工作環境都有極好的適應性。本文可以通過多個環境監測的傳感器，實時對飛行器周圍環境做出監測，并根據不同監測的環境自動轉換最適合的工作模式以加強飛行器的實用性。當前市面上現有的小型四軸飛行器機體較輕，導致抗風性很差，容易造成事故，所以，在不損失飛行器飛行穩定性的情況下將四軸飛行器機體重量變小和降低它對環境的危險系數是未來研究的重點和難點。

本文系中國礦業大學（北京）“大學生創新訓練項目”《基于UWB定位的蟻群無人機設計》（項目編號： C201902444）的成果。