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

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

0 引言

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

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

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

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

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

i2=j2=k2=ijk=-1.

(1)

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

變化公式如下：

Q=

T.

(2)

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

(3)

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

(4)

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

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

(5)

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

(6)

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

2 互補濾波

在四旋翼無人機上安裝陀螺儀可以用來測得四旋翼無人機的角速率，通過積分得到角度，如果角速率信號略有偏差，則在積分操作后，隨著時間的流逝，該誤差將逐漸增加，并最終導致電路飽和，無法形成正確的角度信號[5]。

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

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

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

≈R(s).

(7)

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

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

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

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

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

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

圖2 系統(tǒng)框圖

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

3.2 雙閉環(huán)級聯(lián)PID控制流程設計

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

圖3 軟件流程

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)語

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