基于UWB定位的蟻群無(wú)人機(jī)的設(shè)計(jì)

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

1研究背景與意義

縱觀國(guó)際層面，四旋翼無(wú)人機(jī)已經(jīng)成為世界范圍內(nèi)研究的標(biāo)準(zhǔn)平臺(tái)，研究進(jìn)度十分超前。在軍用領(lǐng)域，無(wú)人飛行器由于可以代替飛行員執(zhí)行戰(zhàn)場(chǎng)緊急危險(xiǎn)任務(wù)以及執(zhí)行超越人類生理極限的任務(wù)。在民用領(lǐng)域，無(wú)人機(jī)在農(nóng)業(yè)植保、環(huán)境保護(hù)、國(guó)土監(jiān)測(cè)、科學(xué)考察、氣象探測(cè)、應(yīng)急搶險(xiǎn)、公共安全等領(lǐng)域有著豐富的需求。

2四軸飛行器控制原理

2.1 PID控制算法

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

（1）比例控制算法

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

（2）積分控制算法

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

（3）微分控制算法

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

根據(jù)以上三種算法，可以得到PID控制的表達(dá)式為：

2.2四軸飛行器的算法

2.2.1歐拉角

歐拉角是用來(lái)唯一地確定定點(diǎn)轉(zhuǎn)動(dòng)剛體位置的三個(gè)一組獨(dú)立角參量，由章動(dòng)角θ、進(jìn)動(dòng)角ψ和自轉(zhuǎn)角φ組成。本文將yz軸作為參考系的參考軸，并且將xy平面和XY平面的交線稱作為N。

2.2.2四元數(shù)

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

3四軸飛行器硬件組成

3.1總體設(shè)計(jì)方案

四軸飛行器由兩個(gè)MCU共同控制，它的任務(wù)是控制四旋翼無(wú)人機(jī)完成的基礎(chǔ)飛行中的各個(gè)任務(wù)，在飛行控制板上有很多擴(kuò)展接口，它可以讓飛行器對(duì)多個(gè)環(huán)境適用。

3.2主控MCU

主控器是一個(gè)系統(tǒng)的核心單元，本文采用的飛行器主控MCU為STM32F411CEU6。

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

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

4四軸飛行器軟件原理

4.1 NRF5822程序框架

NRF51822用來(lái)控制四軸飛行器無(wú)線通信和管理電源。

esb.c是四軸飛行器的無(wú)線驅(qū)動(dòng)代碼，system.c是四軸飛行器系統(tǒng)滴答定時(shí)器驅(qū)動(dòng)代碼，uart.c是飛行器的串口驅(qū)動(dòng)代碼最下方的button.c是飛行器的按鍵驅(qū)動(dòng)代碼，pm.c是飛行器的電源管理驅(qū)動(dòng)代碼。

無(wú)線通信的過(guò)程是，遙控器發(fā)送的數(shù)據(jù)包通過(guò)radiolink.c發(fā)送給飛行器的main.c，之后main.c接受數(shù)據(jù)包并對(duì)其進(jìn)行解析，若發(fā)送對(duì)象不是NRF51822就將數(shù)據(jù)包經(jīng)過(guò)uartlink.c發(fā)送給STM32F411。當(dāng)STM32F411接收到一條來(lái)自u(píng)artlink.c的數(shù)據(jù)包時(shí)。則返回一條數(shù)據(jù)包通過(guò)uartlink.c給main.c，最后main.c再經(jīng)過(guò)radiolink.c轉(zhuǎn)發(fā)給遙控器，通過(guò)以上步驟可以發(fā)現(xiàn)NRF51822起到了重要的通信橋梁作用。

4.2 STM32F411程序框架

radiolinkTask無(wú)線通信任務(wù)，將接收NRF51822發(fā)送的串口數(shù)據(jù)并打包為ATKP格式發(fā)給atkpRxAnlTask的接收隊(duì)列中，與此同時(shí)回發(fā)一幀數(shù)據(jù)給NRF51822。

usblinkRxTask USB通信接收任務(wù)，此任務(wù)接收從上位機(jī)通過(guò)USB虛擬串口的方式發(fā)送的數(shù)據(jù)，將數(shù)據(jù)包發(fā)送給atkpRxAnlTask的接收隊(duì)列中。

atkpRxAnlTask是接收ATKP數(shù)據(jù)包并處理，這項(xiàng)任務(wù)重要地負(fù)責(zé)來(lái)自上位機(jī)和遙控器發(fā)送的數(shù)據(jù)包的處理工作。

stabilizerTask是所有任務(wù)最終的實(shí)現(xiàn)單元，代表著控制四軸飛行器的平衡飛行。

wifilinkTask代表手機(jī)控制任務(wù)，此任務(wù)通過(guò)接收Wifi攝像頭模塊的數(shù)據(jù)，對(duì)于Wifi攝像頭模塊相應(yīng)的通訊協(xié)議來(lái)將數(shù)據(jù)解析為相對(duì)應(yīng)的控制指令發(fā)送給stabilizerTask。

atkpTxTask代表的是ATKP數(shù)據(jù)包的發(fā)送任務(wù)，這個(gè)任務(wù)是將stabilizerTask中獲取到的傳感器、PWM輸出數(shù)據(jù)和姿態(tài)數(shù)據(jù)等進(jìn)行收取發(fā)給usblinkTxTask和radiolinkTask，即發(fā)送給上位機(jī)和遙控器。

usblinkTxTask代表的是usb通信的發(fā)送任務(wù)，將atkpTxTask要發(fā)送的姿態(tài)數(shù)據(jù)和傳感器數(shù)據(jù)等數(shù)據(jù)包發(fā)送給上位機(jī)。

4.3姿態(tài)解算和PID算法流程圖

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

4.4 Z軸高度環(huán)PID

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

4.54D空翻算法與基礎(chǔ)原理

實(shí)現(xiàn)4D空翻的主要秘訣為內(nèi)環(huán)PID。將姿態(tài)角度的設(shè)定值當(dāng)做角速度換的設(shè)定值，這是控制翻滾的基礎(chǔ)。

5實(shí)驗(yàn)結(jié)果分析與展望

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

本文提起的四軸飛行器可以隨意使用傳感器模塊并轉(zhuǎn)換不同工作模式，從而讓飛行器對(duì)多種工作環(huán)境都有極好的適應(yīng)性。本文可以通過(guò)多個(gè)環(huán)境監(jiān)測(cè)的傳感器，實(shí)時(shí)對(duì)飛行器周圍環(huán)境做出監(jiān)測(cè)，并根據(jù)不同監(jiān)測(cè)的環(huán)境自動(dòng)轉(zhuǎn)換最適合的工作模式以加強(qiáng)飛行器的實(shí)用性。當(dāng)前市面上現(xiàn)有的小型四軸飛行器機(jī)體較輕，導(dǎo)致抗風(fēng)性很差，容易造成事故，所以，在不損失飛行器飛行穩(wěn)定性的情況下將四軸飛行器機(jī)體重量變小和降低它對(duì)環(huán)境的危險(xiǎn)系數(shù)是未來(lái)研究的重點(diǎn)和難點(diǎn)。

本文系中國(guó)礦業(yè)大學(xué)（北京）“大學(xué)生創(chuàng)新訓(xùn)練項(xiàng)目”《基于UWB定位的蟻群無(wú)人機(jī)設(shè)計(jì)》（項(xiàng)目編號(hào)： C201902444）的成果。