基于MATLAB的四旋翼無(wú)人機(jī)飛控PID仿真設(shè)計(jì)*

邵霖文，廖 芳，丁黎明，舒 薇，何志謙

(懷化學(xué)院電氣與信息工程學(xué)院，湖南 懷化 418000)

0 引言

四旋翼無(wú)人機(jī)在較好的環(huán)境中飛行具有較好的穩(wěn)定性，但是對(duì)于較為復(fù)雜的飛行環(huán)境，往往難以達(dá)到用戶的需求，特別是在農(nóng)業(yè)植保無(wú)人機(jī)方面。因此，對(duì)四旋翼無(wú)人機(jī)的控制研究具有重要的意義和應(yīng)用前景。

四旋翼無(wú)人機(jī)核心設(shè)計(jì)包括無(wú)人機(jī)的姿態(tài)控制器設(shè)計(jì)，姿態(tài)控制器將決定無(wú)人機(jī)的運(yùn)行穩(wěn)定性和靈敏性，本文將闡述一種通過(guò)四旋翼無(wú)人機(jī)的姿態(tài)數(shù)據(jù)解算與融合濾波算法而設(shè)計(jì)的雙閉環(huán)級(jí)聯(lián)PID控制器，并利用MATLAB開(kāi)展軟件的仿真實(shí)驗(yàn)，從而驗(yàn)證無(wú)人機(jī)控制器的穩(wěn)定性。

1 四旋翼飛控姿態(tài)解算

在無(wú)人機(jī)控制系統(tǒng)中，主要采用捷聯(lián)慣性導(dǎo)航的基本思想，結(jié)合其他定位方式對(duì)其在捷聯(lián)慣性導(dǎo)航中的誤差進(jìn)行修正[1]。處理器讀取每個(gè)傳感器的原始測(cè)量數(shù)據(jù)，使用四元數(shù)方法更新姿態(tài)，然后將四元數(shù)轉(zhuǎn)化為歐拉角，通過(guò)互補(bǔ)濾波器執(zhí)行姿態(tài)校正，最后將校正后的歐拉角轉(zhuǎn)換為四元數(shù)并對(duì)其進(jìn)行歸一化后執(zhí)行下一個(gè)姿勢(shì)更新，以便獲得實(shí)際角度值[2]。

四元數(shù)的定義式如下：

i2=j2=k2=ijk=-1.

(1)

式中i、j、k分別為空間直角坐標(biāo)系中x、y、z軸正方向上的單位向量。

變化公式如下：

Q=

T.

(2)

|Q|2=w2+x2+y2+z2=1.

(3)

使用旋轉(zhuǎn)軸和圍繞旋轉(zhuǎn)軸的旋轉(zhuǎn)角度來(lái)構(gòu)建四元數(shù)：

(4)

式中w為四元數(shù)的標(biāo)量部分的系數(shù)；xyz表示四元數(shù)的矢量部分的系數(shù)[3]；α是繞旋轉(zhuǎn)軸旋轉(zhuǎn)的角度；cos(βx)、cos(βy)、cos(βz)分別為旋轉(zhuǎn)軸在x、y、z方向上的分量。

根據(jù)四元數(shù)的定義，可以得到歐拉角對(duì)四元數(shù)的算法：

(5)

由此可以得出四元數(shù)和歐拉角的轉(zhuǎn)換方法：

(6)

由公式(5)和公式(6)，可以建立四元數(shù)與歐拉角的關(guān)系。陀螺儀的輸出可以轉(zhuǎn)換為四元數(shù)，并在前一時(shí)刻與四元數(shù)相乘，以執(zhí)行姿態(tài)遞歸，然后將四元數(shù)轉(zhuǎn)換為歐拉角以進(jìn)行輸出[4]，這就是姿態(tài)更新。

2 互補(bǔ)濾波

在四旋翼無(wú)人機(jī)上安裝陀螺儀可以用來(lái)測(cè)得四旋翼無(wú)人機(jī)的角速率，通過(guò)積分得到角度，如果角速率信號(hào)略有偏差，則在積分操作后，隨著時(shí)間的流逝，該誤差將逐漸增加，并最終導(dǎo)致電路飽和，無(wú)法形成正確的角度信號(hào)[5]。

加速率傳感器和電子羅盤在測(cè)量姿態(tài)上沒(méi)有累積誤差，但是它們的動(dòng)態(tài)響應(yīng)很差。因此，這三個(gè)傳感器在頻域具有互補(bǔ)特性，并且它們的數(shù)據(jù)可以通過(guò)互補(bǔ)濾波器融合，可以對(duì)其數(shù)據(jù)進(jìn)行校正以消除此累積誤差，從而提高了測(cè)量精度和系統(tǒng)的動(dòng)態(tài)性能[2]。

《河北省天然氣發(fā)展“十三五”規(guī)劃》提出，到2020年力爭(zhēng)天然氣發(fā)電裝機(jī)比例達(dá)到5%以上，裝機(jī)折合150萬(wàn)千瓦。目前河北已有華電石家莊、國(guó)電投廊坊等在建項(xiàng)目。建議結(jié)合地方政府能源政策調(diào)整以及在建項(xiàng)目投產(chǎn)后相關(guān)配套政策和運(yùn)行情況，在有供熱需求的城市和工業(yè)園區(qū)加快前期布局并適時(shí)穩(wěn)妥推進(jìn)。

=R(s)+GL(s)uH(s)+GH(s)uL(s)

≈R(s).

(7)

所以，互補(bǔ)濾波算法能同時(shí)濾除低頻和高頻干擾，方便實(shí)現(xiàn)傳感器數(shù)據(jù)融合[2]。

圖1是互補(bǔ)濾波器的結(jié)構(gòu)圖，圖中分別是GH(s)高通濾波器和GL(s)低通濾波器，分別針對(duì)陀螺儀和加速率計(jì)進(jìn)行濾波處理。

圖1 互補(bǔ)濾波器的結(jié)構(gòu)圖

3 雙閉環(huán)級(jí)聯(lián)PID控制

3.1 雙閉環(huán)級(jí)聯(lián)PID控制原理

在本文中，四旋翼無(wú)人機(jī)控制分為兩個(gè)環(huán)路控制。一個(gè)環(huán)路控制是內(nèi)環(huán)控制，也稱為姿態(tài)控制；另一個(gè)環(huán)路是外環(huán)控制，也稱為位置控制，它是一個(gè)雙閉環(huán)控制結(jié)構(gòu)。因此，本文中將使用雙閉環(huán)串級(jí)PID控制器來(lái)增強(qiáng)系統(tǒng)的魯棒性。系統(tǒng)框圖如圖2。

圖2 系統(tǒng)框圖

從圖2中可以看出，內(nèi)環(huán)PD控制器對(duì)無(wú)人機(jī)的角速率輸出進(jìn)行積分，角速率由陀螺儀直接輸出；外環(huán)PID控制器對(duì)無(wú)人機(jī)的角度輸出進(jìn)行積分，該角度由加速率計(jì)和電磁羅盤確定，該數(shù)據(jù)用于姿態(tài)計(jì)算。外圈P的作用是將無(wú)人機(jī)校正到所需角度，I的作用是消除穩(wěn)態(tài)誤差，D的作用是減少過(guò)沖；內(nèi)圈P的作用是將角速率偏差校正為所需的角速率，D作用是抑制和調(diào)整系統(tǒng)的沖擊和波動(dòng)[7]。這樣，當(dāng)外環(huán)的輸出急劇變化時(shí)，內(nèi)環(huán)也可以用作調(diào)節(jié)緩沖器，不會(huì)以相同的方式引起較大的波動(dòng)，提高了調(diào)節(jié)效率。雙閉環(huán)串級(jí)PID算法同時(shí)控制角速率和角度兩個(gè)變量，提高了四旋翼無(wú)人機(jī)飛行控制系統(tǒng)的魯棒性[8]。

3.2 雙閉環(huán)級(jí)聯(lián)PID控制流程設(shè)計(jì)

控制系統(tǒng)軟件的主要功能為對(duì)傳感器中測(cè)得數(shù)據(jù)進(jìn)行讀取，然后對(duì)姿態(tài)和位置進(jìn)行解算并控制。軟件流程如圖3所示。

圖3 軟件流程

當(dāng)控制系統(tǒng)啟動(dòng)時(shí)，它首先初始化控制系統(tǒng)的所有設(shè)備，然后依次讀取每個(gè)傳感器的數(shù)據(jù)以計(jì)算位置和姿態(tài)，融合計(jì)算出四元數(shù)的姿態(tài)和位置，并根據(jù)誤差使用雙閉環(huán)串級(jí)PID算法，求出控制量和輸出，從而調(diào)節(jié)四旋翼電機(jī)的轉(zhuǎn)速，以達(dá)到快速調(diào)節(jié)的目的。

3.3 雙閉環(huán)級(jí)聯(lián)PID控制MATLAB仿真

在MATLAB軟件的SIMULINK工具模塊中，結(jié)合線性模型的傳遞函數(shù)，以構(gòu)建雙閉環(huán)串級(jí)PID控制器的仿真模型。在仿真模型中，選擇0-1的階躍信號(hào)作為信號(hào)發(fā)生器。被控制對(duì)象的外環(huán)是角度變換的傳遞函數(shù)，內(nèi)環(huán)是角速率變換的傳遞函數(shù)。

姿態(tài)角度的雙閉環(huán)串級(jí)PID控制器的內(nèi)外環(huán)P、I、D參數(shù)如表1、表2所示。

表1 雙閉環(huán)串級(jí)PID控制器外環(huán)控制參數(shù)

表2 雙閉環(huán)串級(jí)PID控制器內(nèi)環(huán)控制參數(shù)

在驗(yàn)證雙閉環(huán)串級(jí)PID控制器對(duì)內(nèi)環(huán)姿態(tài)角速率和外環(huán)位置跟隨的控制效果時(shí)設(shè)置四旋翼無(wú)人機(jī)初始狀態(tài)為：俯仰、橫滾和偏航，姿態(tài)角速率全部為0 rad/s。初始位置設(shè)置為：x、y和z全部為0 m。運(yùn)行仿真模型，分別得到三個(gè)姿態(tài)角速率的仿真結(jié)果圖和位置的仿真結(jié)果圖，如圖4和圖5所示。

圖4 四旋翼無(wú)人機(jī)雙閉環(huán)串級(jí)PID控制器三個(gè)姿態(tài)角速率仿真結(jié)果圖

圖5 雙閉環(huán)串級(jí)PID控制器位置控制仿真結(jié)果圖

從姿態(tài)角速率仿真結(jié)果可以看出，經(jīng)過(guò)雙閉環(huán)串級(jí)PID控制器的調(diào)整，俯仰角速率、橫滾角速率和偏航角速率均達(dá)到了預(yù)期的穩(wěn)態(tài)值，證明雙閉環(huán)串級(jí)PID控制器對(duì)四旋翼無(wú)人機(jī)的姿態(tài)角速率具有良好的控制能力。

從位置控制仿真結(jié)果可以看出，在雙閉環(huán)串級(jí)PID控制器對(duì)位置跟隨控制的仿真過(guò)程中，x、y和z軸方向均達(dá)到了預(yù)期的穩(wěn)態(tài)值。但z軸方向達(dá)到穩(wěn)定狀態(tài)需要的時(shí)間較長(zhǎng)，通過(guò)對(duì)參數(shù)的調(diào)節(jié)圖像可以更加快速的趨于穩(wěn)定。該雙閉環(huán)串級(jí)PID控制器基本實(shí)現(xiàn)了位置跟隨的目標(biāo)。

4 結(jié)語(yǔ)

本文設(shè)計(jì)了一種基于四旋翼無(wú)人機(jī)的姿態(tài)數(shù)據(jù)解算與融合濾波算法的雙閉環(huán)級(jí)聯(lián)PID控制器。通過(guò)對(duì)控制器模型的理論分析，設(shè)計(jì)出了基于雙閉環(huán)的PID控制器，并通過(guò)MATLAB仿真平臺(tái)進(jìn)行了仿真，試驗(yàn)結(jié)果表明，雙閉環(huán)串級(jí)PID控制器使四旋翼無(wú)人機(jī)飛行控制系統(tǒng)具有良好的魯棒性。