基于PID算法的巡邏無人機

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

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

0 引 言

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

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

1 硬件模塊

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

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

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

圖1 硬件流程圖

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

1.2.1 飛控端

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

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

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

1.2.2 遙控端

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

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

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

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

1.3.1 MPU6050

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

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

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

1.3.2 攝像模塊

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

2 軟件模塊設(shè)計

2.1 總體設(shè)計思路

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

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

圖5 軟件流程圖

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

2.3 PID 控制算法

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

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

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

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

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

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);//先計算外環(huán)

pidRate->desired = pid2ngE->out;

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

}

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

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

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

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

圖6 卡爾曼濾波進行姿態(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è)定路線吻合度如表1所示。

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

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

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

4 結(jié) 論

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