基于改進(jìn)PID算法橫列式雙旋翼無(wú)人機(jī)系統(tǒng)

陳文娟,潘乾曜,蘇世達(dá),宋瑛麒,曹佳音,徐志杰,王玉斗

(中國(guó)石油大學(xué)(華東) 理學(xué)院,山東 青島 266580)

旋翼無(wú)人機(jī)是目前人工智能領(lǐng)域的一個(gè)重要分支。目前市面上常見(jiàn)的多旋翼無(wú)人機(jī)旋翼固定,無(wú)法實(shí)現(xiàn)推力矢量的控制。隨著無(wú)人機(jī)飛行任務(wù)的復(fù)雜化,對(duì)于無(wú)人機(jī)機(jī)動(dòng)性以及續(xù)航能力的要求日益提升。橫列式雙旋翼無(wú)人機(jī)作為一種新型無(wú)人機(jī)機(jī)型,其兩組旋翼左右對(duì)稱(chēng),彼此螺旋槳轉(zhuǎn)向相反,旋翼扭矩力可以相互抵消,具有較強(qiáng)的控制靈活性與續(xù)航能力[1],能夠彌補(bǔ)固定旋翼無(wú)人機(jī)機(jī)動(dòng)性能上的不足。

國(guó)內(nèi)外近些年針對(duì)橫列式雙旋翼無(wú)人機(jī)的研究也取得了許多進(jìn)展,2012年美國(guó)麻省理工學(xué)院Markus Ryll團(tuán)隊(duì)最早提出傾轉(zhuǎn)旋翼這一設(shè)計(jì)結(jié)構(gòu)[2],其主要研究的方向?yàn)樵撔滦酮?dú)立可傾轉(zhuǎn)四旋翼無(wú)人機(jī)在螺旋槳傾轉(zhuǎn)方向可控之后對(duì)無(wú)人機(jī)六個(gè)自由度的完全驅(qū)動(dòng)[3];Fernandes團(tuán)隊(duì)提出一種在同軸的兩個(gè)旋翼上增加2自由度傾轉(zhuǎn)機(jī)構(gòu),另兩個(gè)電機(jī)保持固定的傾轉(zhuǎn)旋翼無(wú)人機(jī)方案[4];國(guó)內(nèi)杜玉虎團(tuán)隊(duì)研制了一種傾轉(zhuǎn)旋翼與固定翼結(jié)合的小型無(wú)人機(jī)[5];楊立本研究團(tuán)隊(duì)針對(duì)橫列式雙旋翼無(wú)人機(jī)開(kāi)發(fā)了ADRC姿態(tài)控制算法,增強(qiáng)了其抗干擾性[6];馬逸君、戰(zhàn)強(qiáng)團(tuán)隊(duì)完成了一種新型橫列雙旋翼無(wú)人機(jī)的設(shè)計(jì)并實(shí)現(xiàn)了仿真運(yùn)行[7]。

根據(jù)上述調(diào)研,相關(guān)研究均沒(méi)有考慮傳感器測(cè)量誤差對(duì)于橫列式雙旋翼無(wú)人機(jī)控制精度的影響,且目前所使用的傳統(tǒng)PID算法無(wú)法消除三軸加速度計(jì)測(cè)量誤差[8]。同時(shí)現(xiàn)階段的研究缺乏一套完整的試驗(yàn)樣機(jī)+測(cè)控地面站系統(tǒng)。針對(duì)上述問(wèn)題,本文創(chuàng)新性開(kāi)發(fā)了一種基于三軸加速度計(jì)誤差補(bǔ)償算法與閉環(huán)PID控制算法相結(jié)合的改進(jìn)串級(jí)雙閉環(huán)PID算法,相比于傳統(tǒng)閉環(huán)PID算法可以有效抑制三軸加速度計(jì)測(cè)量值誤差以提高無(wú)人機(jī)的控制精度,基于此在Simulink環(huán)境下進(jìn)行了飛行控制仿真;隨后搭建了試驗(yàn)樣機(jī)并開(kāi)發(fā)了測(cè)控地面站系統(tǒng);最終完成了試驗(yàn)樣機(jī)空中懸停實(shí)驗(yàn)。仿真與實(shí)際實(shí)驗(yàn)結(jié)果驗(yàn)證了改進(jìn)算法控制系統(tǒng)在理論與實(shí)際環(huán)境下的優(yōu)勢(shì)與可執(zhí)行性。

1 系統(tǒng)動(dòng)力學(xué)模型建立

首先建立如圖1所示的坐標(biāo)系,(x0,y0,z0)(x,y,z)(x1,y1,z1)(x2,y2,z2)分別為慣性坐標(biāo)系、機(jī)體坐標(biāo)系、左旋翼坐標(biāo)系與右旋翼坐標(biāo)系。

圖1 無(wú)人機(jī)相關(guān)坐標(biāo)系定義

(1)

為方便表述,式1中sin與cos均使用s、c表示。其中φ為橫滾角,θ為俯仰角,ψ為偏航角。

根據(jù)圖1分析可知,橫列式雙旋翼無(wú)人機(jī)通過(guò)同時(shí)改變同列電機(jī)轉(zhuǎn)速來(lái)產(chǎn)生垂直上下運(yùn)動(dòng);當(dāng)電機(jī)同速旋轉(zhuǎn)時(shí),通過(guò)旋翼傾轉(zhuǎn)舵機(jī)的同向傾轉(zhuǎn)來(lái)產(chǎn)生俯仰運(yùn)動(dòng);當(dāng)旋翼傾轉(zhuǎn)舵機(jī)傾轉(zhuǎn)角度同時(shí)為0,通過(guò)調(diào)節(jié)無(wú)人機(jī)左右電機(jī)的轉(zhuǎn)速差來(lái)實(shí)現(xiàn)滾轉(zhuǎn)運(yùn)動(dòng);最后通過(guò)旋翼傾轉(zhuǎn)舵機(jī)的反向同角度傾轉(zhuǎn)以實(shí)現(xiàn)偏航運(yùn)動(dòng)。

根據(jù)牛頓-歐拉定理,可以推導(dǎo)出橫列式雙旋翼無(wú)人機(jī)的六自由度非線性動(dòng)力學(xué)方程:式中輸入?yún)?shù)U1-U4分別滿(mǎn)足式2和式3。

(2)

(3)

其中,U1-U4是旋翼沿著不同軸線方向的升力;KT是旋翼螺旋槳的推力系數(shù);ω1、ω2與δ1、δ2分別為同列2個(gè)螺旋槳的轉(zhuǎn)速與2個(gè)傾轉(zhuǎn)舵機(jī)產(chǎn)生的傾轉(zhuǎn)角度。

橫列式雙旋翼無(wú)人機(jī)實(shí)際物理參數(shù)經(jīng)整定如表1所示。

表1 實(shí)際物理參數(shù)

2 系統(tǒng)控制算法設(shè)計(jì)與仿真

2.1 控制系統(tǒng)設(shè)計(jì)流程

控制系統(tǒng)工作流程如圖2所示,控制系統(tǒng)設(shè)計(jì)包括MCU初始化、姿態(tài)解算、控制信號(hào)讀取與飛行姿態(tài)控制四部分,其中飛行姿態(tài)控制是控制系統(tǒng)設(shè)計(jì)的重點(diǎn)與難點(diǎn)。

圖2 橫列式雙旋翼控制軟件工作流程

(1)MCU初始化:初始化寄存器,包括對(duì)三軸加速度計(jì)、氣壓高度傳感器與陀螺儀芯片進(jìn)行程序覆寫(xiě)初始化操作。

(2)姿態(tài)解算:根據(jù)IMU數(shù)據(jù)利用四元數(shù)法表示出三軸坐標(biāo)旋轉(zhuǎn)矩陣,將得到的三軸旋轉(zhuǎn)矩陣相乘以實(shí)時(shí)更新無(wú)人機(jī)姿態(tài)矩陣,最后將姿態(tài)矩陣轉(zhuǎn)化為三軸歐拉角,求解出飛行器的實(shí)際姿態(tài)角。

(3)控制信號(hào)讀取:利用中斷處理算法輸入捕捉中斷以實(shí)現(xiàn)遙控器控制信號(hào)的讀取,利用串口中斷算法實(shí)現(xiàn)與地面站之間數(shù)據(jù)的收發(fā),以完成遙控器、地面站以及導(dǎo)航解算所給定控制信號(hào)的讀取。

(4)飛行姿態(tài)控制:包括改進(jìn)串級(jí)雙閉環(huán)PID控制算法與PWM信號(hào)輸出2部分,改進(jìn)串級(jí)雙閉環(huán)PID控制算法設(shè)計(jì)將在下一小節(jié)具體介紹;通過(guò)PID控制器輸出的PWM信號(hào)會(huì)根據(jù)其占空比實(shí)現(xiàn)對(duì)電機(jī)轉(zhuǎn)速與舵機(jī)傾轉(zhuǎn)角度的控制。

2.2 改進(jìn)串級(jí)雙閉環(huán)PID控制算法設(shè)計(jì)

2.2.1 算法整體結(jié)構(gòu)框架設(shè)計(jì)

本文設(shè)計(jì)的改進(jìn)串級(jí)雙閉環(huán)PID算法創(chuàng)新性將自主開(kāi)發(fā)的三軸加速度誤差補(bǔ)償算法與閉環(huán)PID控制算法相結(jié)合,解決了傳統(tǒng)閉環(huán)PID算法無(wú)法實(shí)現(xiàn)三軸加速度計(jì)觀測(cè)誤差補(bǔ)償?shù)膯?wèn)題,該算法的整體結(jié)構(gòu)框圖如圖3所示。

圖3 改進(jìn)串級(jí)雙閉環(huán)PID算法結(jié)構(gòu)框圖

2.2.2 三軸加速度誤差補(bǔ)償算法設(shè)計(jì)

三軸加速度計(jì)是無(wú)人機(jī)慣導(dǎo)的關(guān)鍵部件,其輸出精度直接影響無(wú)人機(jī)飛行控制的精度。因?yàn)榧庸すに嚨南拗?本文所使用的MPU6000三軸加速度計(jì)難免會(huì)產(chǎn)生三軸不正交誤差。三軸不正交誤差指的是加速度計(jì)三個(gè)敏感軸會(huì)偏離理想正交系微小的角度,從而產(chǎn)生加速度測(cè)量值的非線性誤差。

針對(duì)這一問(wèn)題,首先對(duì)三軸加速度計(jì)的誤差進(jìn)行數(shù)學(xué)建模[9],如式(4)所示。由于該款三軸加速度計(jì)在生產(chǎn)過(guò)程中已經(jīng)校準(zhǔn)了零位偏移誤差,所以在模型中可以不用考慮零位偏移誤差的影響。

(4)

式(4)中ax、ay、az為三軸加速度計(jì)實(shí)際測(cè)量值;axT、ayT、azT為三軸加速度計(jì)真實(shí)值;矩陣Eij為靈敏度與非正交誤差系數(shù)矩陣;矩陣ej為隨機(jī)誤差;α、β、γ分別為3個(gè)敏感軸偏離理想正交系的角度。

根據(jù)上述模型,本文采用基于最大似然法的橢球擬合算法[10]來(lái)搭建誤差補(bǔ)償器。對(duì)式10進(jìn)行變形,可以得到三軸加速度計(jì)誤差補(bǔ)償模型[10]:

(5)

式(5)中esum_x、esum_y、esum_z為修正后的隨機(jī)誤差系數(shù),圖4為誤差補(bǔ)償器的系統(tǒng)流程框圖。

圖4 三軸加速度計(jì)誤差補(bǔ)償系統(tǒng)框圖

以無(wú)人機(jī)靜止于水平面為例,取保持靜止姿態(tài)1s內(nèi)三軸加速度計(jì)的數(shù)據(jù)作為1組樣本,取100組數(shù)據(jù)樣本,在誤差補(bǔ)償器中進(jìn)行迭代,得到如表2所示無(wú)人機(jī)靜止條件下M與esum的參數(shù)值。

表2 無(wú)人機(jī)靜止條件下誤差補(bǔ)償器參數(shù)

2.2.3 閉環(huán)PID控制算法設(shè)計(jì)

閉環(huán)PID控制器采用增量式PID控制:

(6)

(7)

其中ev與ew分別等于vdes-v與ωdes-ω,通過(guò)上述公式可以分別得出橫列式雙旋翼矢量無(wú)人機(jī)的期望升力與期望旋轉(zhuǎn)力矩,再通過(guò)控制分配得出2個(gè)旋翼電機(jī)的期望轉(zhuǎn)速與2個(gè)傾轉(zhuǎn)舵機(jī)的期望傾轉(zhuǎn)角度,控制分配公式如下:

(8)

(9)

(10)

(11)

2.3 控制系統(tǒng)Simulink仿真

基于上述理論分析,在所建立的橫列式雙旋翼無(wú)人機(jī)動(dòng)力學(xué)模型中加入改進(jìn)串級(jí)雙閉環(huán)PID控制算法對(duì)控制器進(jìn)行階躍輸入,并根據(jù)對(duì)應(yīng)的階躍響應(yīng)曲線進(jìn)行PID參數(shù)的整定,如圖5所示。

圖5 仿真系統(tǒng)Simulink框圖

該仿真系統(tǒng)包括:位置控制器、姿態(tài)控制器、橫列式雙旋翼無(wú)人機(jī)數(shù)學(xué)模型與誤差補(bǔ)償器四部分。輸入為4個(gè)階躍控制信號(hào),自上而下分別為期望三軸xyz位置信號(hào)與期望偏航角信號(hào)。四個(gè)模塊功能如下:

(1)位置控制模塊:將輸入的三軸位置信號(hào)與偏航角信號(hào)利用四元數(shù)轉(zhuǎn)換法轉(zhuǎn)化成期望升力、期望俯仰角與期望滾轉(zhuǎn)角信號(hào)并輸入姿態(tài)控制模塊進(jìn)行下一步運(yùn)算。

(2)姿態(tài)控制模塊:利用牛頓-歐拉定理將從位置控制器輸出的期望升力、俯仰角與滾轉(zhuǎn)角信號(hào)進(jìn)行姿態(tài)解算,輸出同軸2個(gè)旋翼的升力與2個(gè)旋翼螺旋槳的旋轉(zhuǎn)力矩。

(3)無(wú)人機(jī)數(shù)學(xué)模型:將從角度控制器輸出的升力與旋轉(zhuǎn)力矩導(dǎo)入動(dòng)力學(xué)模型中,解算出無(wú)人機(jī)的實(shí)際位置與姿態(tài)角信號(hào)并反饋給上述2個(gè)模塊進(jìn)行調(diào)整。

(4)誤差補(bǔ)償模塊:對(duì)模擬觀測(cè)三軸位移參數(shù)的累積誤差進(jìn)行補(bǔ)償與消除。

圖6為改進(jìn)PID算法與傳統(tǒng)PID算法仿真結(jié)果對(duì)比圖。初始輸入階躍模擬信號(hào)為:x0=25,y0=12,z0=30,yaw_des=0.5;初始設(shè)置觀測(cè)誤差為:y軸方向0.4°。紅色實(shí)線為改進(jìn)串級(jí)雙閉環(huán)PID算法響應(yīng)曲線,藍(lán)色虛線為傳統(tǒng)PID算法響應(yīng)曲線。

時(shí)間/s

據(jù)圖分析,經(jīng)過(guò)30次仿真實(shí)驗(yàn)可得出相關(guān)結(jié)論:改進(jìn)串級(jí)雙閉環(huán)PID算法對(duì)于y軸位移以及3軸姿態(tài)角控制的超調(diào)量同比減少了約78%、26%、19%與44%。仿真結(jié)果說(shuō)明相較于傳統(tǒng)串級(jí)雙閉環(huán)PID算法,改進(jìn)串級(jí)雙閉環(huán)PID算法有效地提升了系統(tǒng)的控制性能,具有更強(qiáng)的誤差抑制能力。

3 試驗(yàn)樣機(jī)設(shè)計(jì)與實(shí)驗(yàn)

3.1 試驗(yàn)樣機(jī)系統(tǒng)總體設(shè)計(jì)

橫列式雙旋翼無(wú)人機(jī)由5個(gè)分系統(tǒng)組合而成,即飛行主控系統(tǒng)、傳感器系統(tǒng)、控制執(zhí)行系統(tǒng)、信號(hào)收發(fā)系統(tǒng)與電源穩(wěn)壓系統(tǒng)。試驗(yàn)樣機(jī)系統(tǒng)硬件總體結(jié)構(gòu)如圖7所示。

圖7 試驗(yàn)樣機(jī)系統(tǒng)總體硬件結(jié)構(gòu)

(1)飛行主控系統(tǒng)采用基于PX4的32位開(kāi)源飛控Pixhawk。該飛控以STM32F427VIT6(168MHz,256KB,ROM/2MB閃存,100Pin)為中央處理器,主要負(fù)責(zé)傳感器誤差補(bǔ)償與無(wú)人機(jī)姿態(tài)角與飛行高度的解算;結(jié)合遙控器接收機(jī)(FS-IA10B)的控制信號(hào)實(shí)時(shí)控制同列2個(gè)旋翼電機(jī)轉(zhuǎn)速與2個(gè)傾轉(zhuǎn)舵機(jī)傾轉(zhuǎn)角度;結(jié)合無(wú)線數(shù)傳完成與無(wú)人機(jī)地面站數(shù)據(jù)的雙向交互[11-13]。

(2)傳感器系統(tǒng)包括MPU6000三軸加速度計(jì)、L3GD20H陀螺儀芯片與MS5611氣壓計(jì),主要負(fù)責(zé)測(cè)量角加速度、線加速度與海拔高度等姿態(tài)信息的測(cè)量與反饋[14]。

(3)控制執(zhí)行系統(tǒng)包括動(dòng)力系統(tǒng)與旋翼傾轉(zhuǎn)系統(tǒng)。其中動(dòng)力系統(tǒng)選用1對(duì)大疆2312A無(wú)刷電機(jī)與好盈40A電子調(diào)速器,分別安裝1對(duì)9 450自鎖正反槳提供動(dòng)力。傾轉(zhuǎn)舵機(jī)采用JX1109數(shù)碼舵機(jī),該舵最大扭力為1.7kg/cm[15-16]。

(4)信號(hào)收發(fā)系統(tǒng)包括FS-IA10B接收機(jī)與ESP8266Wi-Fi無(wú)線數(shù)傳,主要負(fù)責(zé)主控系統(tǒng)與遙控器和地面站數(shù)據(jù)信號(hào)的雙向交互。

(5)電源穩(wěn)壓系統(tǒng)采用LM2596S可調(diào)降壓模塊,主要負(fù)責(zé)將14.8V直流電源電壓降低到5V以供給主控系統(tǒng)使用,如圖8所示。

圖8 橫列式雙旋翼無(wú)人機(jī)實(shí)物圖

3.2 測(cè)控地面站軟件開(kāi)發(fā)

測(cè)控地面站軟件采用UE4引擎作為開(kāi)發(fā)平臺(tái),主要作用在于實(shí)現(xiàn)空中懸停實(shí)驗(yàn)數(shù)據(jù)的采集與可視化。圖9為測(cè)控地面站系統(tǒng)操作界面。該系統(tǒng)可以實(shí)時(shí)動(dòng)態(tài)顯示不同型號(hào)無(wú)人機(jī)的三維空間姿態(tài)角與高度變化、旋翼螺旋槳運(yùn)行狀態(tài)與虛擬第一人稱(chēng)視角。

圖9 地面站操作主界面

3.3 試驗(yàn)樣機(jī)空中懸停實(shí)驗(yàn)

圖10為進(jìn)行中的試驗(yàn)樣機(jī)空中懸停實(shí)驗(yàn)。完成了改進(jìn)PID算法與傳統(tǒng)PID算法試驗(yàn)樣機(jī)懸停對(duì)比實(shí)驗(yàn)。對(duì)比實(shí)驗(yàn)指定懸停高度均為4m,一共進(jìn)行了10組對(duì)比實(shí)驗(yàn)。

圖10 試驗(yàn)樣機(jī)空中懸停實(shí)驗(yàn)

表3、表4分別為10組空中懸停對(duì)比實(shí)驗(yàn)測(cè)量數(shù)據(jù)平均值。表5為姿態(tài)角數(shù)據(jù)分析結(jié)果,表6為實(shí)驗(yàn)誤差分析。圖11為對(duì)比實(shí)驗(yàn)姿態(tài)數(shù)據(jù)時(shí)域曲線,其中實(shí)線為改進(jìn)串級(jí)雙閉環(huán)PID算法,虛線為傳統(tǒng)PID算法。

表3 基于改進(jìn)PID算法試驗(yàn)樣機(jī)實(shí)驗(yàn)數(shù)據(jù)記錄

表4 基于傳統(tǒng)PID算法試驗(yàn)樣機(jī)實(shí)驗(yàn)數(shù)據(jù)記錄

表5 對(duì)比實(shí)驗(yàn)姿態(tài)角數(shù)據(jù)

表6 對(duì)比實(shí)驗(yàn)誤差

時(shí)間/s

對(duì)無(wú)人機(jī)高度數(shù)據(jù)分析可知,前20 s無(wú)人機(jī)處于上升狀態(tài),20 s后穩(wěn)定在設(shè)定高度。據(jù)圖11所示,改進(jìn)串級(jí)雙閉環(huán)PID算法相較于傳統(tǒng)PID算法能更好地抑制高度位移的振蕩。

據(jù)表5分析可知,基于改進(jìn)PID算法試驗(yàn)樣機(jī)穩(wěn)定懸停時(shí)滾轉(zhuǎn)角與俯仰角的平均值相比于傳統(tǒng)PID算法更接近于0,且標(biāo)準(zhǔn)差同比減小了7.9%與23.8%。說(shuō)明改進(jìn)PID算法具有更強(qiáng)的姿態(tài)角控制穩(wěn)定性,在一定程度上提升了橫列式雙旋翼無(wú)人機(jī)的控制精度。

據(jù)表6分析可知,基于改進(jìn)PID算法試驗(yàn)樣機(jī)懸停高度、滾轉(zhuǎn)角、俯仰角與偏航角的穩(wěn)態(tài)誤差較傳統(tǒng)PID算法同比減少了35.68%、49.04%、2.33%與55.96%,說(shuō)明改進(jìn)PID算法具有更高的控制精度。

4 結(jié) 論

本文創(chuàng)新性將自主開(kāi)發(fā)的三軸加速度計(jì)誤差補(bǔ)償算法與閉環(huán)PID控制算法相結(jié)合,設(shè)計(jì)出一種基于改進(jìn)串級(jí)雙閉環(huán)PID算法的橫列式雙旋翼無(wú)人機(jī)。本文首先介紹了動(dòng)力學(xué)模型的搭建與物理參數(shù)的整定;其次闡述了控制系統(tǒng)與改進(jìn)串級(jí)雙閉環(huán)PID算法的設(shè)計(jì)并利用Simulink軟件仿真驗(yàn)證了該系統(tǒng)的理論穩(wěn)定性;再者完成了試驗(yàn)樣機(jī)+測(cè)控地面站系統(tǒng)的搭建并完成了空中懸停實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明,相較于傳統(tǒng)閉環(huán)PID算法,改進(jìn)串級(jí)雙閉環(huán)PID控制算法對(duì)于橫列式雙旋翼無(wú)人機(jī)具有更好的控制穩(wěn)定性,基本實(shí)現(xiàn)了對(duì)機(jī)體飛行姿態(tài)的自穩(wěn)定控制與誤差補(bǔ)償。后續(xù)還可以引入相關(guān)機(jī)器學(xué)習(xí)算法實(shí)現(xiàn)對(duì)超調(diào)量與調(diào)節(jié)時(shí)間的減少與縮短。