基于PID算法的巡邏無人機(jī)

劉新宇，桑晨，李媛媛，游璧銘，劉暢

（東南大學(xué)成賢學(xué)院，江蘇 南京 210088）

0 引 言

隨著科技的飛速發(fā)展，幾乎各項(xiàng)技術(shù)的發(fā)展都涉及到了人工智能技術(shù)領(lǐng)域。無人機(jī)作為新時(shí)代人工智能技術(shù)的產(chǎn)物應(yīng)運(yùn)而生，在國防、農(nóng)業(yè)、植保、交通運(yùn)輸?shù)阮I(lǐng)域得到了廣泛的應(yīng)用。四軸無人機(jī)，又稱四旋翼飛行器。四軸無人機(jī)穩(wěn)定性高、便于控制、使用難度不大。隨著微機(jī)電控制技術(shù)的發(fā)展，常用于監(jiān)控、航空攝影、測量地形等，四軸無人機(jī)在民用無人機(jī)領(lǐng)域受到了廣泛的關(guān)注與認(rèn)可。

當(dāng)前信息時(shí)代，人工智能技術(shù)讓我們的生活更加便利。巡邏對于治安人員與巡警而言并不是一件易事，需要大量的人力資源和時(shí)間資源，傳統(tǒng)人力巡邏方式無法及時(shí)發(fā)現(xiàn)危機(jī)并進(jìn)行處理，監(jiān)控所能夠監(jiān)視的范圍也有一定的限制性。為減輕巡邏人員負(fù)擔(dān)，提高巡邏效率，增強(qiáng)環(huán)境安全性，我們想出結(jié)合單片機(jī)，設(shè)計(jì)一個(gè)可尋跡的大范圍實(shí)時(shí)監(jiān)控的基于PID 算法的巡邏無人機(jī)。我們對無人機(jī)的硬件模塊和軟件模塊分別進(jìn)行設(shè)計(jì)，所設(shè)計(jì)的巡邏無人機(jī)成本低，精確度高，飛行費(fèi)用低，機(jī)動靈活，功能多樣化，具有極大的優(yōu)越性。

1 硬件模塊

1.1 系統(tǒng)總體設(shè)計(jì)

巡邏無人機(jī)主要使用STM32 主控進(jìn)行總體設(shè)計(jì)，分別對MPU6050 傳感器模塊、nRF24L01 通信模塊、無刷電極驅(qū)動和ESP32-cam 攝像模塊進(jìn)行設(shè)計(jì)控制，再使用自制遙控手柄實(shí)現(xiàn)與主控之間的通信。

首先使用姿態(tài)傳感器采集相關(guān)數(shù)據(jù)，并將數(shù)據(jù)通過通信模塊傳輸?shù)街骺厣?。主控對?shù)據(jù)進(jìn)行處理后產(chǎn)生四路PWM值，從而完成對無刷電機(jī)的驅(qū)動控制，實(shí)現(xiàn)巡邏無人機(jī)的飛控；然后操縱自制遙控手柄，對手柄的數(shù)值進(jìn)行數(shù)據(jù)輸出，再通過通信模塊發(fā)送數(shù)據(jù)回電腦端，電腦端進(jìn)行數(shù)據(jù)處理后發(fā)送給無人機(jī)主控，使巡邏無人機(jī)進(jìn)行飛行調(diào)整；最后通過主控給攝像模塊供電，實(shí)現(xiàn)圖像傳輸功能。

圖1 硬件流程圖

1.2 主控模塊（飛控+遙控）

1.2.1 飛控端

本設(shè)計(jì)選用了STM32F103C8T6 作為主控芯片，設(shè)計(jì)原理圖如圖2所示。STM32 無人機(jī)主控主要負(fù)責(zé)接收、處理數(shù)據(jù)和進(jìn)行供電。

圖2 飛控端模塊設(shè)計(jì)原理圖

主控端接收通信模塊傳輸?shù)淖藨B(tài)解算值后，對數(shù)據(jù)進(jìn)行PID 算法處理，再通過主控定時(shí)器發(fā)送四路PWM 值驅(qū)動控制電機(jī)，完成對巡邏無人機(jī)的飛行控制；主控端可以通過通信模塊接收自制遙控手柄的ADC 值與按鍵值，再將數(shù)據(jù)傳輸?shù)诫娔X端，由電腦處理數(shù)據(jù)后再傳輸?shù)綗o人機(jī)端，實(shí)現(xiàn)巡邏無人機(jī)上下左右移動的調(diào)整，完成遙控功能；主控端還可以給攝像模塊提供5 V/2 A 穩(wěn)壓源。

1.2.2 遙控端

遙控模塊采用靈動MM32SPIN27PS 為主芯片進(jìn)行自制PCB 板設(shè)計(jì)，原理圖如圖3所示。

圖3 搖控端模塊設(shè)計(jì)原理圖

遙控器端使用控制手柄和按鍵給無人機(jī)發(fā)送飛行指令，再通過通信模塊將ADC 值和按鍵值傳輸?shù)诫娔X端和無人機(jī)端。本設(shè)計(jì)中巡邏無人機(jī)的遙控部分功能作用有兩個(gè)：在巡邏無人機(jī)循跡前，使用遙控手柄讓無人機(jī)按照所要求的路線飛行，通過MM32 主芯片的ADC 口PA0，PA2，PA3，PA5獲取ADC 值和IO 口PA15，PA1，PD6，PD7 獲取按鍵值，并將數(shù)據(jù)傳輸給STM32 主控，主控端將數(shù)據(jù)處理成無人機(jī)端可以識別的信號，給巡邏無人機(jī)制定一條飛行路線，并通過UART 口將姿態(tài)數(shù)據(jù)傳輸給電腦端，讓無人機(jī)實(shí)現(xiàn)初步自動循跡功能；遙控部分的另外一個(gè)功能是調(diào)整巡邏無人機(jī)的飛行狀態(tài)。操縱遙控手柄，將得到的ADC 和按鍵值通過通信模塊直接傳輸?shù)诫娔X端，進(jìn)行數(shù)據(jù)處理后傳輸回STM32主控，完成對巡邏無人機(jī)飛行的狀態(tài)進(jìn)行及時(shí)調(diào)整。

1.3 傳感器模塊（MPU6050+攝像頭）

1.3.1 MPU6050

本設(shè)計(jì)選用MPU6050 芯片作為傳感器模塊原理圖如圖4所示，主要負(fù)責(zé)進(jìn)行巡邏無人機(jī)飛行時(shí)的加速度和角速度數(shù)據(jù)采集。

圖4 傳感器模塊設(shè)計(jì)原理圖

由于地球重力垂直向下，所以我們將芯片正面朝下嵌入無人機(jī)框架來獲取以重力加速度作為參考的無人機(jī)當(dāng)前飛行狀態(tài)的三個(gè)軸向上的加速度值；而無人機(jī)飛行狀態(tài)的角速度值采集，則是應(yīng)用到了傳感器模塊的陀螺儀。將采集到的加速度和角速度值通過I2C 通信協(xié)議傳輸?shù)街骺啬K，再對加速度和角速度進(jìn)行PID 算法及姿態(tài)解算，使得定時(shí)器輸出四路PWM 值完成對無刷電機(jī)的驅(qū)動控制，實(shí)現(xiàn)巡邏無人機(jī)的飛行控制。其中，由于大量數(shù)據(jù)的接收會夾雜大量的噪音影響輸出結(jié)果，所以需要濾波來消除噪聲。本設(shè)計(jì)所使用的是卡爾曼濾波來實(shí)現(xiàn)此功能。

1.3.2 攝像模塊

本設(shè)計(jì)使用ESP32-CAM 實(shí)現(xiàn)無人機(jī)與電腦間的圖像通信。將圖像實(shí)時(shí)傳輸?shù)诫娔X端，實(shí)現(xiàn)巡邏無人機(jī)的實(shí)時(shí)大范圍監(jiān)控功能。給ESP32-CAM 芯片提供5V/2A 的穩(wěn)壓電流源供電，再使用串口助手接收攝像模塊，當(dāng)模塊連接Wi-Fi之后，串口上會收到視頻的IP 地址，在瀏覽器中輸入對應(yīng)的IP 地址就可以獲得攝像頭的實(shí)時(shí)傳輸圖像，實(shí)現(xiàn)無人機(jī)與電腦間的圖像通信。

2 軟件模塊設(shè)計(jì)

2.1 總體設(shè)計(jì)思路

2.2 卡爾曼濾波與姿態(tài)解算

卡爾曼濾波的基本思想是綜合利用上一次的狀態(tài)和測量值來對物理量的狀態(tài)進(jìn)行預(yù)測估計(jì)。對于狀態(tài)估計(jì)算法而言，我們可以獲取狀態(tài)量的三個(gè)值：狀態(tài)預(yù)測值、最優(yōu)估計(jì)值以及真實(shí)值，卡爾曼濾波的原理就是利用卡爾曼增益來修正狀態(tài)預(yù)測值，使其逼近真實(shí)值。對卡爾曼濾波的推導(dǎo)過程，第一個(gè)過程分為狀態(tài)估計(jì)協(xié)方差Pk 的推導(dǎo)，即代價(jià)函數(shù)的求??；第二步則是卡爾曼增益矩陣及其他規(guī)則的推導(dǎo)，最后則是針對這些參數(shù)進(jìn)行整合。

圖5 軟件流程圖

飛控需要進(jìn)行姿態(tài)估計(jì)，通過傳感器進(jìn)行慣性測量單元（IMU）進(jìn)行測量姿態(tài)值。MCU 通過讀取姿態(tài)傳感器數(shù)據(jù)計(jì)算出姿態(tài)角的過程稱為姿態(tài)計(jì)算，這是無人機(jī)飛控的關(guān)鍵之處。本設(shè)計(jì)中姿態(tài)計(jì)算程序主要是把MOU6050 讀取出來的數(shù)據(jù)值進(jìn)行互補(bǔ)濾波處理最終得到歐拉角度值。無人機(jī)在空間中此刻至下一秒的運(yùn)動姿態(tài)是由陀螺儀的數(shù)學(xué)積分方程通過計(jì)算來確定的。

2.3 PID 控制算法

PID 算法是指“比例”“積分”“微分”三個(gè)元素。P指當(dāng)無人機(jī)收到外界的干擾后，機(jī)身對產(chǎn)生姿態(tài)偏移回到穩(wěn)定的時(shí)間，即用來控制從不穩(wěn)定到穩(wěn)定的響應(yīng)時(shí)間，P 越大響應(yīng)越快；I 指當(dāng)無人機(jī)從不穩(wěn)定回到穩(wěn)定狀態(tài)一段時(shí)間內(nèi)積累的誤差，只修正P 是無法達(dá)到的期望姿態(tài)值，也叫靜態(tài)差值；D 是為了加強(qiáng)對機(jī)體變化的快速響應(yīng)，對 P 有抑制作用，二者具有互斥性。

串聯(lián)級PID 由兩個(gè)閉環(huán)控制器串聯(lián)在一起工作，主控制器是主要部分，最終目的是確保主變量

的穩(wěn)定性。如果出現(xiàn)二次干擾，主控制器保持不動，副控制器先進(jìn)行調(diào)整，然后主控制器再根據(jù)副控制器的調(diào)整做進(jìn)一步的調(diào)整。所以，采用兩個(gè)閉環(huán)的控制器串聯(lián)工作效果更佳。環(huán)內(nèi)環(huán)副控制器的輸入，計(jì)算出一個(gè)較小的PWM 輸出信號。使無人機(jī)由快到慢恢復(fù)平衡。主要代碼實(shí)現(xiàn)：

（1）內(nèi)環(huán)PID

void pidUpdate (Pid0bject* pid,const float dt)

float error;

float deriv;

error - pid->desired - pid->measured;//當(dāng)前角度與實(shí)際角度的誤差

pid->integ += error * dt;//誤差積分累加值

//pid->integLIMIT(pid->integ,pid->InteqLimitLow,pid->IntegLimitHigh);//進(jìn)行積分限幅

deriv - (error - pid->prevError)/dt;//前后兩次誤差做微分

pid->out - pid->kp ‘ error + pid->ki * pid->integ + pid->kd * deriv;//PID 輸出

//pid->out*LIMIT(pid->out,pid->OutLimitLow,pid->OutLimitHigh);//輸出限幅

pid->prevError = error;//更新上次的誤差

（2）串級PID

void CascadePID(PidObject* pidRate,PidObject* pidAngE,const float dt)//串級PID

{pidUpdate (pidAngE,dt);//先計(jì)算外環(huán)

pidRate->desired = pid2ngE->out;

pidUpdate(pidRate,dt);//再計(jì)算內(nèi)環(huán)

}

3 關(guān)鍵性能指標(biāo)

為了對無人機(jī)巡邏各個(gè)關(guān)鍵功能進(jìn)行評估，本項(xiàng)目對關(guān)鍵功能模塊進(jìn)行了測試。

3.1 姿態(tài)角的融合

圖6為卡爾曼濾波進(jìn)行姿態(tài)角的融合效果圖，圖中可以看出，濾波后的三軸角度波形平滑，噪點(diǎn)較小。

圖6 卡爾曼濾波進(jìn)行姿態(tài)角的融合效果圖

3.2 PID 調(diào)節(jié)參數(shù)

PID 調(diào)節(jié)參數(shù)效果如圖7所示，期望角速度、期望姿態(tài)角度設(shè)為0。

圖7 PID 調(diào)節(jié)參數(shù)效果圖

3.3 飛行路線測試

在完成既定任務(wù)的前提下，我們對其實(shí)際飛行路線準(zhǔn)確度進(jìn)行了測試，實(shí)際飛行與設(shè)定路線吻合度如表1所示。

表1 實(shí)際飛行與設(shè)定路線吻合度

分析：實(shí)際飛行時(shí)，由于環(huán)境因素的不同會導(dǎo)致飛行路線的偏差，從而也使飛機(jī)最終的降落點(diǎn)偏離。

從幾次的測試結(jié)果中可以發(fā)現(xiàn)，設(shè)定距離越遠(yuǎn)，偏離次數(shù)越多且誤差會累計(jì)，但由于無人機(jī)本身的抗干擾能力，使最終誤差都在可控范圍之內(nèi)，最終與設(shè)定路線吻合度較高，飛行成功率也較高。

4 結(jié) 論

本設(shè)計(jì)以具有巡檢任務(wù)的群體為研究對象，采用STM32F103C8T6 開發(fā)板為主要控制核心，MPU6050 為姿態(tài)分析傳感芯片，使用nRF24L01 與ESP32-CAM 實(shí)現(xiàn)無線通信，采用 KeliuVision5 及Arduino 的綜合開發(fā)環(huán)境，完成了PID 算法的編制。從而達(dá)到自動跟蹤、實(shí)時(shí)監(jiān)測等功能，進(jìn)而達(dá)到完成巡邏的目的。