本發(fā)明涉及無(wú)人機(jī)技術(shù)領(lǐng)域，尤其涉及一種基于四旋翼無(wú)人機(jī)的姿態(tài)控制的方法及其系統(tǒng)。

背景技術(shù)：

四旋翼無(wú)人機(jī)飛行器具有可以垂直起降、結(jié)構(gòu)簡(jiǎn)單、機(jī)動(dòng)性強(qiáng)等特點(diǎn)，近年來(lái)成為研究熱點(diǎn)，在消費(fèi)類(lèi)領(lǐng)域得到巨大發(fā)展。四旋翼無(wú)人機(jī)飛行器一般采用對(duì)稱(chēng)結(jié)構(gòu)布局，機(jī)械結(jié)構(gòu)相對(duì)簡(jiǎn)單，四個(gè)旋翼均勻分布，具有欠驅(qū)動(dòng)、強(qiáng)耦合和高階非線性，對(duì)無(wú)人機(jī)飛行控制系統(tǒng)的設(shè)計(jì)提出了高要求。飛行控制系統(tǒng)是由控制器硬件及軟件算法兩部分組成，在現(xiàn)在消費(fèi)類(lèi)無(wú)人機(jī)硬件平臺(tái)基本趨于一致的前提下，高性能、運(yùn)算效率的軟件算法是飛行器系統(tǒng)穩(wěn)定性、可靠性、飛行性能的重要保證，而由其動(dòng)力學(xué)模型的復(fù)雜性、模型參數(shù)的不確定性和建模不精確性，對(duì)控制算法的魯棒性、抗干擾性提出了更高要求。

現(xiàn)在的消費(fèi)類(lèi)四旋翼無(wú)人機(jī)基本采用的是傳統(tǒng)PID控制算法及基于該算法的改進(jìn)PID算法，根據(jù)無(wú)人機(jī)飛行狀態(tài)及目標(biāo)狀態(tài)的誤差反饋構(gòu)建控制律，最終得到控制輸出量。傳統(tǒng)PID控制以其簡(jiǎn)單易懂的控制結(jié)構(gòu)、良好的實(shí)用性而受到廣泛應(yīng)用。

傳統(tǒng)PID算法的控制缺陷在于：(1)直接以e＝v-y的方式產(chǎn)生原始誤差?？刂颇繕?biāo)v是有可能產(chǎn)生突變的，而對(duì)象輸出y一定是連續(xù)的，用連續(xù)的緩變的變量追蹤可能跳變的變量本身就是不合理的。(2)產(chǎn)生e的微分信號(hào)沒(méi)有太好的辦法。(3)線性組合不一定是最好的組合方式。(4)誤差信號(hào)e的積分反饋的引入有很多負(fù)作用。

在無(wú)人機(jī)控制上主要有兩點(diǎn)不足：

1)基于無(wú)人機(jī)系統(tǒng)處于平衡點(diǎn)附近的假設(shè)，其控制性能在飛行器狀態(tài)大范圍偏離平衡點(diǎn)后會(huì)大打折扣。

2)算法對(duì)系統(tǒng)模型的不確定、外界干擾的魯棒性較差。在無(wú)人機(jī)系統(tǒng)模型參數(shù)出現(xiàn)變動(dòng)和干擾時(shí)，其控制器性能會(huì)變差。需要重新調(diào)節(jié)參數(shù)、尋優(yōu)最佳PID參數(shù)。

3)PID控制中，積分作用項(xiàng)I參數(shù)難調(diào)，微分作用項(xiàng)D參數(shù)難調(diào)。

基于自抗擾技術(shù)的四旋翼無(wú)人機(jī)控制技術(shù)對(duì)飛行過(guò)程中的干擾和飛行器自身狀態(tài)的不確定性具有更好的魯棒性，在整個(gè)飛行狀態(tài)內(nèi)控制都具有極高的控制性能。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于克服現(xiàn)有技術(shù)的不足，本發(fā)明提供了一種基于四旋翼無(wú)人機(jī)的姿態(tài)控制的方法及其系統(tǒng)，在繼承了傳統(tǒng)PID控制優(yōu)點(diǎn)，不依賴(lài)模型，只利用飛行器的誤差反饋進(jìn)行控制律設(shè)計(jì)，同時(shí)采用擴(kuò)張狀態(tài)觀測(cè)器對(duì)擾動(dòng)(外部干擾和模型不確定性)進(jìn)行實(shí)時(shí)估計(jì)和補(bǔ)償，對(duì)無(wú)人機(jī)飛行控制中的干擾和不確定性具有較好的魯棒性，參數(shù)具有更好的適用性。

為了解決上述問(wèn)題，本發(fā)明提出了一種基于四旋翼無(wú)人機(jī)的姿態(tài)控制的方法，包括如下步驟：

接收控制參考狀態(tài)輸入值；

采用跟蹤微分器提取控制參考狀態(tài)輸入值的微分信號(hào)；

基于擴(kuò)展?fàn)顟B(tài)觀測(cè)器獲取當(dāng)前姿態(tài)調(diào)整下的擴(kuò)展?fàn)顟B(tài)輸出量；

根據(jù)擴(kuò)展?fàn)顟B(tài)觀測(cè)器獲取的擴(kuò)展?fàn)顟B(tài)輸出量與微分信號(hào)構(gòu)建誤差，基于誤差設(shè)計(jì)得到非線性反饋控制器的初始控制輸出量；

采用擴(kuò)展?fàn)顟B(tài)跟蹤器中未知干擾的觀測(cè)值對(duì)初始控制輸出量進(jìn)行擾動(dòng)補(bǔ)償，得到內(nèi)環(huán)姿態(tài)控制器的控制輸出值。

所述參考狀態(tài)輸入值包括：橫滾狀態(tài)、俯仰狀態(tài)、偏航狀態(tài)。

所述采用跟蹤微分器提取控制參考狀態(tài)輸入值的微分信號(hào)包括：

采用如下約束值進(jìn)行微分信號(hào)的提取：

其中：

v₁：跟蹤微分器輸出量之一，用來(lái)跟蹤控制參考狀態(tài)輸入值θ_c

v₂：跟蹤微分器輸出量之一，用來(lái)表示v₁的變化量；

e：表示v₁與跟蹤量參考輸入θ_c之間的誤差；

r：跟蹤微分器參數(shù)之一，表示速度因子，表征過(guò)渡過(guò)程跟蹤輸入信號(hào)的快慢程度，與跟蹤速度成正比，由過(guò)渡過(guò)程快慢及無(wú)人機(jī)系統(tǒng)承受能力決定；

h：跟蹤微分器參數(shù)之一，表示采樣步長(zhǎng)，具有濾波效果，由控制系統(tǒng)的采樣頻率決定；

fh：離散系統(tǒng)最速控制函數(shù)fhan的簡(jiǎn)寫(xiě)。

所述基于擴(kuò)展?fàn)顟B(tài)觀測(cè)器獲取當(dāng)前姿態(tài)調(diào)整下的擴(kuò)展?fàn)顟B(tài)輸出量包括：

采用如下約束值進(jìn)行擴(kuò)展?fàn)顟B(tài)輸出量的提?。?/p>

其中：

z₁：擴(kuò)展?fàn)顟B(tài)觀測(cè)器輸出量之一，用來(lái)跟蹤過(guò)渡過(guò)程輸出v₁；

z₂：擴(kuò)展?fàn)顟B(tài)觀測(cè)器輸出量之一，用來(lái)跟蹤過(guò)渡過(guò)程輸出v₂；

z₃：擴(kuò)展?fàn)顟B(tài)觀測(cè)器輸出量之一，用來(lái)跟蹤系統(tǒng)中的未知擾動(dòng)；

u：擴(kuò)展?fàn)顟B(tài)觀測(cè)器輸入量之一，控制系統(tǒng)輸出值；

fe：跟觀測(cè)誤差e相關(guān)函數(shù)，影響觀測(cè)器輸出z₂的收斂性；

fe₁：跟觀測(cè)誤差e相關(guān)函數(shù)，影響觀測(cè)器輸出z₃的收斂性；

β₀₁：狀態(tài)觀測(cè)器參數(shù)之一，影響觀測(cè)器輸出z₁的收斂速度；

β₀₂：狀態(tài)觀測(cè)器參數(shù)之一，影響觀測(cè)器輸出z₂的收斂速度；

β₀₃：狀態(tài)觀測(cè)器參數(shù)之一，影響觀測(cè)器輸出z₃的收斂速度；

h：狀態(tài)觀測(cè)器參數(shù)之一，采樣步長(zhǎng)，由控制系統(tǒng)的采樣頻率決定；

b₀：補(bǔ)償因子，跟無(wú)人機(jī)系統(tǒng)的時(shí)間尺度相關(guān)，為常數(shù)；

β₀₁,β₀₂,β₀₃：輸出誤差校正增益，由無(wú)人機(jī)控制頻率來(lái)決定。

所述根據(jù)擴(kuò)展?fàn)顟B(tài)觀測(cè)器獲取的擴(kuò)展?fàn)顟B(tài)輸出量與微分信號(hào)構(gòu)建誤差，基于誤差設(shè)計(jì)得到非線性反饋控制器的初始控制輸出量包括：

采用如下約束值進(jìn)行初始控制輸出量的提?。?/p>

其中：

e₁：非線性反饋控制器輸入量之一，過(guò)渡過(guò)程輸出v₁與擴(kuò)展?fàn)顟B(tài)觀測(cè)器z₁之間誤差，表征的是系統(tǒng)控制目標(biāo)角度值與實(shí)際值偏差；

e₂：非線性反饋控制器輸入量之一，過(guò)渡過(guò)程輸出v₂與擴(kuò)展?fàn)顟B(tài)觀測(cè)器z₂之間誤差，表征的是系統(tǒng)角度誤差值的微分量；

b₁：fal函數(shù)參數(shù)之一，跟擴(kuò)展?fàn)顟B(tài)觀測(cè)器參數(shù)一致；

b₂：fal函數(shù)參數(shù)之一，跟擴(kuò)展?fàn)顟B(tài)觀測(cè)器參數(shù)一致；

δ：fal函數(shù)參數(shù)之一，跟擴(kuò)展?fàn)顟B(tài)觀測(cè)器參數(shù)一致；

k₁：比例因子，誤差量的偏差比例系數(shù)；

k₂：比例因子，微分量的反饋系數(shù)。

所述采用擴(kuò)展?fàn)顟B(tài)跟蹤器中未知干擾的觀測(cè)值對(duì)初始控制輸出量進(jìn)行擾動(dòng)補(bǔ)償，得到內(nèi)環(huán)姿態(tài)控制器的控制輸出值包括：

采用如下約束值進(jìn)行內(nèi)環(huán)姿態(tài)控制器的控制輸出值的提?。?/p>

其中b₀是補(bǔ)償因子，跟無(wú)人機(jī)系統(tǒng)的時(shí)間尺度相關(guān)，為常數(shù)。

相應(yīng)的，本發(fā)明還提供一種基于四旋翼無(wú)人機(jī)的姿態(tài)控制的系統(tǒng)，其特征在于，所述系統(tǒng)包括：

輸入模塊，用于接收控制參考狀態(tài)輸入值；

跟蹤微分器模塊，用于采用跟蹤微分器提取控制參考狀態(tài)輸入值的微分信號(hào)；

擴(kuò)展?fàn)顟B(tài)觀測(cè)器模塊，用于基于擴(kuò)展?fàn)顟B(tài)觀測(cè)器獲取當(dāng)前姿態(tài)調(diào)整下的擴(kuò)展?fàn)顟B(tài)輸出量；

非線性組合模塊，用于根據(jù)擴(kuò)展?fàn)顟B(tài)觀測(cè)器獲取的擴(kuò)展?fàn)顟B(tài)輸出量與微分信號(hào)構(gòu)建誤差，基于誤差設(shè)計(jì)得到非線性反饋控制器的初始控制輸出量；

控制輸出值模塊，用于采用擴(kuò)展?fàn)顟B(tài)跟蹤器中未知干擾的觀測(cè)值對(duì)初始控制輸出量進(jìn)行擾動(dòng)補(bǔ)償，得到內(nèi)環(huán)姿態(tài)控制器的控制輸出值。

所述參考狀態(tài)輸入值包括：橫滾狀態(tài)、俯仰狀態(tài)、偏航狀態(tài)。

在本發(fā)明實(shí)施例中，在整個(gè)具體實(shí)施過(guò)程中，安排過(guò)渡過(guò)程，采用跟蹤微分器對(duì)被控對(duì)象提取微分信號(hào)；通過(guò)跟蹤參考輸入信號(hào)，柔化參考輸入信號(hào)的變化，減少系統(tǒng)輸出的超調(diào)，增強(qiáng)魯棒性。由非線性擴(kuò)張觀測(cè)器實(shí)現(xiàn)對(duì)未知擾動(dòng)的估計(jì)；擴(kuò)展?fàn)顟B(tài)觀測(cè)器除了觀測(cè)過(guò)渡過(guò)程輸出值v₁,v₂外，進(jìn)一步的估計(jì)了系統(tǒng)未知擾動(dòng)，為后面對(duì)擾動(dòng)的補(bǔ)償提供了基礎(chǔ)；引入擾動(dòng)補(bǔ)償過(guò)程。最終實(shí)現(xiàn)了更好的抗干擾能力及適用更大范圍的參數(shù)設(shè)計(jì)。

附圖說(shuō)明

為了更清楚地說(shuō)明本發(fā)明實(shí)施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對(duì)實(shí)施例或現(xiàn)有技術(shù)描述中所需要使用的附圖作簡(jiǎn)單地介紹，顯而易見(jiàn)地，下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例，對(duì)于本領(lǐng)域普通技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)的前提下，還可以根據(jù)這些附圖獲得其它的附圖。

圖1是本發(fā)明實(shí)施例的無(wú)人機(jī)控制系統(tǒng)原理圖；

圖2是本發(fā)明實(shí)施例的基于無(wú)人機(jī)系統(tǒng)的控制方法原理圖；

圖3是本發(fā)明實(shí)施例的基于四旋翼無(wú)人機(jī)的姿態(tài)控制的方法流程圖；

圖4是本發(fā)明實(shí)施例的擴(kuò)展?fàn)顟B(tài)觀測(cè)器原理圖；

圖5是本發(fā)明實(shí)施例的基于四旋翼無(wú)人機(jī)的姿態(tài)控制的系統(tǒng)結(jié)構(gòu)圖。

具體實(shí)施方式

下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例?；诒景l(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒(méi)有作出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。

本發(fā)明實(shí)施例中的基于四旋翼無(wú)人機(jī)的姿態(tài)控制的方法，其采用：接收控制參考狀態(tài)輸入值；采用跟蹤微分器提取控制參考狀態(tài)輸入值的微分信號(hào)；基于擴(kuò)展?fàn)顟B(tài)觀測(cè)器獲取當(dāng)前姿態(tài)調(diào)整下的擴(kuò)展?fàn)顟B(tài)輸出量；根據(jù)擴(kuò)展?fàn)顟B(tài)觀測(cè)器獲取的擴(kuò)展?fàn)顟B(tài)輸出量與微分信號(hào)構(gòu)建誤差，基于誤差設(shè)計(jì)得到非線性反饋控制器的初始控制輸出量；采用擴(kuò)展?fàn)顟B(tài)跟蹤器中未知干擾的觀測(cè)值對(duì)初始控制輸出量進(jìn)行擾動(dòng)補(bǔ)償，得到內(nèi)環(huán)姿態(tài)控制器的控制輸出值。

首先，先介紹一些無(wú)人機(jī)控制原理，在無(wú)人機(jī)的控制中，有一類(lèi)是設(shè)定飛行軌跡由無(wú)人機(jī)自主飛行，另一類(lèi)是通過(guò)人為操作控制無(wú)人機(jī)的飛行軌跡，其中人為操控時(shí)輸入的是無(wú)人機(jī)的姿態(tài)指令，無(wú)人機(jī)在姿態(tài)變化后達(dá)到位置相應(yīng)變化的目的。簡(jiǎn)而言之，無(wú)人機(jī)先有姿態(tài)變化再有位置變化，先改變姿態(tài)再改變位置，基于以上特點(diǎn)我們優(yōu)先考慮內(nèi)環(huán)姿態(tài)控制環(huán)節(jié)的高效、高性能控制算法，所設(shè)計(jì)的控制算法既能夠服務(wù)于無(wú)人機(jī)的自主飛行控制，也可以服務(wù)于人工操作時(shí)的姿態(tài)控制。

本方案適用于無(wú)人機(jī)的控制架構(gòu)是采用內(nèi)外兩個(gè)獨(dú)立閉環(huán)控制結(jié)構(gòu)的情況，其中內(nèi)環(huán)控制無(wú)人機(jī)的飛行姿態(tài)，外環(huán)控制無(wú)人機(jī)的飛行位置，外環(huán)的控制算法只需計(jì)算出要改變無(wú)人機(jī)飛行位置狀態(tài)所需的姿態(tài)角和總矩，輸入到內(nèi)環(huán)，由內(nèi)環(huán)的姿態(tài)控制算法實(shí)現(xiàn)飛行姿態(tài)的控制，進(jìn)而實(shí)現(xiàn)無(wú)人機(jī)位置控制的目的。

以小型無(wú)人機(jī)為例，其控制原理圖如圖1中所示，在進(jìn)行自主飛行控制時(shí)，輸入位置控制輸入量p_r(x,y,z)即無(wú)人機(jī)的期望位置(不同坐標(biāo)體系下，含義可能不同，如經(jīng)緯度x、y，高度z)，結(jié)合位置數(shù)據(jù)信息反饋x_m,y_m,z_m(無(wú)人機(jī)的真實(shí)位置)一起輸入外環(huán)控制器，通過(guò)外環(huán)控制(外環(huán)控制一般也設(shè)計(jì)為兩環(huán)的控制框架，但不是此處重點(diǎn)，可以簡(jiǎn)化修改)得到下一級(jí)的控制量δ_col(油門(mén)控制量)，φ_r(橫滾角度Roll期望值)，θ_r(俯仰角Pitch期望值)，(偏航角Yaw期望值)。其中，φ_r，θ_r，作為內(nèi)環(huán)控制器的控制量輸入。在由人工操控時(shí)，直接由操控手輸入控制量δ_col，φ_r，θ_r，

控制輸入φ_r，θ_r，結(jié)合姿態(tài)數(shù)據(jù)信息反饋φ，θ，(無(wú)人機(jī)的真實(shí)姿態(tài))經(jīng)過(guò)內(nèi)環(huán)控制器后，得到下一級(jí)控制量δ_lat，δ_lon，δ_yaw，這部分是本方案研究創(chuàng)新的重點(diǎn)部分。提出了具有高效、魯棒性的內(nèi)環(huán)姿態(tài)控制算法。

將內(nèi)環(huán)控制器輸出δ_lat,δ_col,δ_ped，結(jié)合δ_col一起輸入到下一級(jí)，通過(guò)距分配及限幅(或者是解耦混控并限幅)后得到了每個(gè)電機(jī)的控制量，在四旋翼無(wú)人機(jī)中控制四個(gè)電機(jī)轉(zhuǎn)速的快慢，從而引起四個(gè)電機(jī)升力發(fā)生變化，使得飛機(jī)姿態(tài)發(fā)生相應(yīng)變化，進(jìn)一步的使得飛機(jī)的位置發(fā)生相應(yīng)變化。

本發(fā)明實(shí)施過(guò)程中，基于自抗擾技術(shù)的內(nèi)環(huán)姿態(tài)控制器的設(shè)計(jì)方法，該方法設(shè)計(jì)控制框架相同、控制參數(shù)差異的內(nèi)環(huán)姿態(tài)控制器，對(duì)無(wú)人機(jī)姿態(tài)的Roll(橫滾φ)、Pitch(俯仰θ)、Yaw(偏航))控制，變成了單輸入單輸出系統(tǒng)。期望角度及真實(shí)測(cè)量角度作為輸入，經(jīng)過(guò)該控制器后得到相應(yīng)的控制量，互相之間獨(dú)立，分別計(jì)算得到相應(yīng)的控制量，以pitch俯仰角度的控制為例，θ_c為參考輸入，控制流程框圖如圖2中所示，roll及yaw角控制流程相同，只是參數(shù)表示不同，具體基于四旋翼無(wú)人機(jī)的姿態(tài)控制的方法流程圖如圖3，結(jié)合圖2詳細(xì)說(shuō)明如下：

S31、接收控制參考狀態(tài)輸入值；

控制算法的輸入v為參考輸入θ_c(此處，內(nèi)環(huán)姿態(tài)pitch角目標(biāo)值)，控制輸出為u。本發(fā)明實(shí)施例子中以pitch俯仰角度的控制為例，θ_c為控制參考輸入，roll及yaw角控制流程相同，只是參數(shù)表示不同。

S32、采用跟蹤微分器提取控制參考狀態(tài)輸入值的微分信號(hào)；

利用跟蹤微分器(TD，Tracking Differentiator)，對(duì)參考輸入θ_c(內(nèi)環(huán)姿態(tài)pitch角目標(biāo)值)安排過(guò)渡過(guò)程并提取其微分信號(hào)，過(guò)渡過(guò)程保障對(duì)θ_c的跟蹤輸出結(jié)果沒(méi)有超調(diào)。

跟蹤微分器設(shè)計(jì)過(guò)程如下，以v₁跟蹤θ_c，v₂表示v₁的變化量，跟蹤微分器約束條件如下：

其中：

v₁：跟蹤微分器輸出量之一，用來(lái)跟蹤參考輸入θ_c；

v₂：跟蹤微分器輸出量之一，用來(lái)表示v₁的變化量；

e：表示v₁與跟蹤量參考輸入θ_c之間的誤差；

r：跟蹤微分器參數(shù)之一。速度因子，表征過(guò)渡過(guò)程跟蹤輸入信號(hào)的快慢程度，與跟蹤速度成正比，由過(guò)渡過(guò)程快慢及無(wú)人機(jī)系統(tǒng)承受能力決定。

h：跟蹤微分器參數(shù)之一。采樣步長(zhǎng)，具有濾波效果。由控制系統(tǒng)的采樣頻率決定；

fh：離散系統(tǒng)最速控制函數(shù)fhan的簡(jiǎn)寫(xiě)，詳細(xì)說(shuō)明如下。

無(wú)人機(jī)系統(tǒng)在實(shí)際控制處理時(shí)是一個(gè)高頻率的離散控制系統(tǒng)，按一定的采樣頻率，每隔相應(yīng)的采樣周期，更新整個(gè)系統(tǒng)的狀態(tài)量?；陔x散系統(tǒng)的特性，采用遞推的方法，跟蹤微分器的實(shí)現(xiàn)原理簡(jiǎn)單說(shuō)明如下：

v1＝v1+hv2，v2是v1的變化量，h是采樣步長(zhǎng)，v1下一采樣時(shí)間的值就是當(dāng)前采用值加上采樣步長(zhǎng)乘以變化量。類(lèi)似原理V2＝v2+hfh得到v2的值。

函數(shù)fhan是離散系統(tǒng)最速控制函數(shù)，根據(jù)離散系統(tǒng)控制理論，其構(gòu)造如下：

fhan＝-r(a/d)fsg(a,d)-r·sign(a)[1-fsg(a,d)]

其中，fhan里面的部分參數(shù)由下列式子構(gòu)造得出：

fsg函數(shù)是跟符號(hào)相關(guān)函數(shù)，表達(dá)如下：

fsg(x,d)＝[sign(x+d)-sign(x-d)]/2

本步驟中，得到了跟蹤微分器(TD)的輸出v₁，v₂，選擇合適的參數(shù)可以使得出v₁，v₂，快速跟蹤上參考輸入θ_c，達(dá)到穩(wěn)定值。

S33、基于擴(kuò)展?fàn)顟B(tài)觀測(cè)器獲取當(dāng)前姿態(tài)調(diào)整下的擴(kuò)展?fàn)顟B(tài)輸出量；

系統(tǒng)在運(yùn)行過(guò)程中與外界環(huán)境進(jìn)行信息交流，其中系統(tǒng)的外部變量就是系統(tǒng)傳給外部的輸出變量，包括部分狀態(tài)變量信息和外部給系統(tǒng)的輸入量。可以根據(jù)外部變量來(lái)觀測(cè)確定系統(tǒng)內(nèi)部狀態(tài)變量的裝置就是狀態(tài)觀測(cè)器。

無(wú)人機(jī)控制系統(tǒng)中的擴(kuò)展?fàn)顟B(tài)觀測(cè)器(ESO，Extended State Observer)，在這里以四旋翼無(wú)人機(jī)的姿態(tài)輸出(pitch角度值)θ值和控制輸入u作為觀測(cè)器的輸入來(lái)設(shè)計(jì)用來(lái)估計(jì)四旋翼無(wú)人機(jī)的系統(tǒng)狀態(tài)的觀測(cè)器，擴(kuò)展觀測(cè)器的輸出量為z1,z₂,z₃，實(shí)現(xiàn)了用z₁用來(lái)追蹤跟蹤微分器輸出結(jié)果v₁，z₂用來(lái)追蹤跟蹤微分器輸出結(jié)果v₂，z₃用來(lái)跟蹤四旋翼無(wú)人機(jī)系統(tǒng)的未知擾動(dòng)。整個(gè)擴(kuò)展?fàn)顟B(tài)觀測(cè)器原理圖如圖4中所示，根據(jù)離散系統(tǒng)性質(zhì)，結(jié)合其狀態(tài)觀測(cè)器設(shè)計(jì)框架及無(wú)人機(jī)控制系統(tǒng)實(shí)際情況，約束擴(kuò)展?fàn)顟B(tài)觀測(cè)器條件如下：

其中：

z₁：狀態(tài)觀測(cè)器輸出量之一，用來(lái)跟蹤過(guò)渡過(guò)程輸出v₁；

z₂：狀態(tài)觀測(cè)器輸出量之一，用來(lái)跟蹤過(guò)渡過(guò)程輸出v₂；

z₃：狀態(tài)觀測(cè)器輸出量之一，用來(lái)跟蹤系統(tǒng)中的未知擾動(dòng)；

u：狀態(tài)觀測(cè)器輸入量之一，控制系統(tǒng)輸出；

fe：跟觀測(cè)誤差e相關(guān)函數(shù)，影響觀測(cè)器輸出z₂的收斂性；

fe₁：跟觀測(cè)誤差e相關(guān)函數(shù)，影響觀測(cè)器輸出z₃的收斂性；

β₀₁：狀態(tài)觀測(cè)器參數(shù)之一，影響觀測(cè)器輸出z₁的收斂速度；

β₀₂：狀態(tài)觀測(cè)器參數(shù)之一，影響觀測(cè)器輸出z₂的收斂速度；

β₀₃：狀態(tài)觀測(cè)器參數(shù)之一，影響觀測(cè)器輸出z₃的收斂速度；

h：狀態(tài)觀測(cè)器參數(shù)之一。采樣步長(zhǎng)，由控制系統(tǒng)的采樣頻率決定；

b₀：補(bǔ)償因子，跟無(wú)人機(jī)系統(tǒng)的時(shí)間尺度相關(guān)，為常數(shù)；

β₀₁,β₀₂,β₀₃：輸出誤差校正增益，由無(wú)人機(jī)控制頻率來(lái)決定。

fe、fe₁是跟觀測(cè)誤差量e相關(guān)的函數(shù)，表達(dá)式如下：

其中，fal函數(shù)表達(dá)式如下：

fal：fal函數(shù)是構(gòu)造設(shè)計(jì)出來(lái)具有一定特性的函數(shù)，后面說(shuō)明

e：fal函數(shù)的輸入量；

b：fal函數(shù)參數(shù)之一，影響濾波速度；

δ：fal函數(shù)參數(shù)之一，影響濾波效果。

fal函數(shù)具有變?cè)鲆娴臄?shù)學(xué)擬合作用及快速收斂特性，根據(jù)不同的誤差e分別輸出不同的結(jié)果，最終能使誤差e在有限時(shí)間內(nèi)快速收斂到0，具有較好的濾波效果。

具體實(shí)施過(guò)程中，調(diào)節(jié)fal函數(shù)中的參數(shù)a,δ可以得到良好的濾波效果。其中，δ影響濾波效果，增大δ濾波效果加強(qiáng)，但會(huì)延長(zhǎng)跟蹤輸入信號(hào)的響應(yīng)時(shí)間，一般取5T<δ<10T,其中T是采樣時(shí)間。b是0～1之間常數(shù)，b越小跟蹤越快，但會(huì)導(dǎo)致濾波效果變差。

具體實(shí)施過(guò)程匯總，誤差校正增益β₀₁,β₀₂,β₀₃與積分步長(zhǎng)近似滿(mǎn)足以下關(guān)系時(shí)：

擴(kuò)展?fàn)顟B(tài)觀測(cè)器具有很高的估計(jì)效率，跟蹤性能良好。

擴(kuò)展?fàn)顟B(tài)觀測(cè)器(ESO)是自抗擾設(shè)計(jì)ADRC的核心，采用雙通道補(bǔ)償?shù)姆椒ǜ脑鞂?duì)象模型，將非線性、不確定的系統(tǒng)近似線性化和確定性化。利用擴(kuò)張狀態(tài)觀測(cè)器(ESO)對(duì)系統(tǒng)對(duì)象進(jìn)行估計(jì)，不僅能得到各個(gè)狀態(tài)變量的估計(jì)，而且能得到對(duì)象方程右端估計(jì)，即擾動(dòng)估計(jì)。在通過(guò)跟蹤微分器完成過(guò)渡過(guò)程，并且用擴(kuò)展?fàn)顟B(tài)觀測(cè)器對(duì)系統(tǒng)狀態(tài)量和不確定性完成估計(jì)之后，可以結(jié)合跟蹤微分器的輸出和擴(kuò)展?fàn)顟B(tài)觀測(cè)器的輸出，設(shè)計(jì)非線性反饋控制器并進(jìn)行擾動(dòng)補(bǔ)償。

S34、根據(jù)擴(kuò)展?fàn)顟B(tài)觀測(cè)器獲取的擴(kuò)展?fàn)顟B(tài)輸出量與微分信號(hào)構(gòu)建誤差；

S35、基于誤差設(shè)計(jì)得到非線性反饋控制器的初始控制輸出量；

步驟S34至S35接下來(lái)將過(guò)渡過(guò)程輸出v₁,v₂與擴(kuò)展?fàn)顟B(tài)觀測(cè)器輸出z₁,z₂構(gòu)建誤差，基于誤差反饋設(shè)計(jì)原理，設(shè)計(jì)得到非線性反饋控制器(NF，Nonlinear Feedback)。

其中：

e₁：非線性反饋控制器輸入量之一，過(guò)渡過(guò)程輸出v₁與擴(kuò)展?fàn)顟B(tài)觀測(cè)器z₁之間誤差，表征的是系統(tǒng)控制目標(biāo)角度值與實(shí)際值偏差；

e₂：非線性反饋控制器輸入量之一，過(guò)渡過(guò)程輸出v₂與擴(kuò)展?fàn)顟B(tài)觀測(cè)器z₂之間誤差，表征的是系統(tǒng)角度誤差值的微分量(變化)；

b₁：fal函數(shù)參數(shù)之一，跟擴(kuò)展?fàn)顟B(tài)觀測(cè)器參數(shù)一致；

b₂：fal函數(shù)參數(shù)之一，跟擴(kuò)展?fàn)顟B(tài)觀測(cè)器參數(shù)一致；

δ：fal函數(shù)參數(shù)之一，跟擴(kuò)展?fàn)顟B(tài)觀測(cè)器參數(shù)一致；

k₁：比例因子，誤差量的偏差比例系數(shù)；

k₂：比例因子，微分量的反饋系數(shù)；

u₀：初始控制輸出量。

類(lèi)似于傳統(tǒng)PID參數(shù)，需整定到合適的系數(shù)得到良好的控制效果。非線性反饋控制器通過(guò)調(diào)節(jié)這兩個(gè)參數(shù)可以兼顧無(wú)人機(jī)系統(tǒng)反應(yīng)的快速性和超調(diào)性?xún)身?xiàng)指標(biāo)，另外對(duì)消除靜差不在依賴(lài)于積分器(I)，避免了積分反饋給系統(tǒng)帶來(lái)的負(fù)面影響(對(duì)比PID控制)。

S306、采用擴(kuò)展?fàn)顟B(tài)跟蹤器中未知干擾的觀測(cè)值對(duì)初始控制輸出量進(jìn)行擾動(dòng)補(bǔ)償，得到內(nèi)環(huán)姿態(tài)控制器的控制輸出值。

最后對(duì)擴(kuò)展?fàn)顟B(tài)跟蹤器(ESO)中未知干擾的觀測(cè)值z(mì)₃進(jìn)行補(bǔ)償，得到最終內(nèi)環(huán)姿態(tài)控制器的輸出u的表達(dá)式。

其中b₀是補(bǔ)償因子，跟無(wú)人機(jī)系統(tǒng)的時(shí)間尺度相關(guān)，為常數(shù)。

綜上，進(jìn)過(guò)以上步驟實(shí)現(xiàn)了基于自抗擾的角度控制器設(shè)計(jì)(單軸)。綜上，控制算法的輸入v(就是參考輸入θ_c，此處，內(nèi)環(huán)姿態(tài)pitch角目標(biāo)值)，經(jīng)過(guò)ADRC自抗擾算法，最終得到控制輸出為u。以俯仰pitch角為例，即由期望的俯仰角度v＝θ_r輸入內(nèi)環(huán)自抗擾控制器，最終得到了控制量u＝δ_lon。其他角度可以采用類(lèi)似過(guò)程推導(dǎo)得到。然后按第一個(gè)框圖的控制流程繼續(xù)，通過(guò)解耦混控，最終得到每個(gè)電機(jī)(執(zhí)行機(jī)構(gòu))的輸出。

圖5示出了本發(fā)明實(shí)施例中的基于四旋翼無(wú)人機(jī)的姿態(tài)控制的系統(tǒng)結(jié)構(gòu)示意圖，包括如下：

輸入模塊，用于接收控制參考狀態(tài)輸入值；

跟蹤微分器模塊，用于采用跟蹤微分器提取控制參考狀態(tài)輸入值的微分信號(hào)；

擴(kuò)展?fàn)顟B(tài)觀測(cè)器模塊，用于基于擴(kuò)展?fàn)顟B(tài)觀測(cè)器獲取當(dāng)前姿態(tài)調(diào)整下的擴(kuò)展?fàn)顟B(tài)輸出量；

非線性組合模塊，用于根據(jù)擴(kuò)展?fàn)顟B(tài)觀測(cè)器獲取的擴(kuò)展?fàn)顟B(tài)輸出量與微分信號(hào)構(gòu)建誤差，基于誤差設(shè)計(jì)得到非線性反饋控制器的初始控制輸出量；

控制輸出值模塊，用于采用擴(kuò)展?fàn)顟B(tài)跟蹤器中未知干擾的觀測(cè)值對(duì)初始控制輸出量進(jìn)行擾動(dòng)補(bǔ)償，得到內(nèi)環(huán)姿態(tài)控制器的控制輸出值。

在具體實(shí)施過(guò)程中，該參考狀態(tài)輸入值包括：橫滾狀態(tài)、俯仰狀態(tài)、偏航狀態(tài)。在整個(gè)具體實(shí)施過(guò)程中，安排過(guò)渡過(guò)程，采用跟蹤微分器對(duì)被控對(duì)象提取微分信號(hào)；通過(guò)跟蹤參考輸入信號(hào)，柔化參考輸入信號(hào)的變化，減少系統(tǒng)輸出的超調(diào)，增強(qiáng)魯棒性。由非線性擴(kuò)張觀測(cè)器實(shí)現(xiàn)對(duì)未知擾動(dòng)的估計(jì)；擴(kuò)展?fàn)顟B(tài)觀測(cè)器除了觀測(cè)過(guò)渡過(guò)程輸出值外，進(jìn)一步的估計(jì)了系統(tǒng)未知擾動(dòng)，為后面對(duì)擾動(dòng)的補(bǔ)償提供了基礎(chǔ)；引入擾動(dòng)補(bǔ)償過(guò)程。最終實(shí)現(xiàn)了更好的抗干擾能力及適用更大范圍的參數(shù)設(shè)計(jì)。

本領(lǐng)域普通技術(shù)人員可以理解上述實(shí)施例的各種方法中的全部或部分步驟是可以通過(guò)程序來(lái)指令相關(guān)的硬件來(lái)完成，該程序可以存儲(chǔ)于一計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)中，存儲(chǔ)介質(zhì)可以包括：只讀存儲(chǔ)器(ROM，Read Only Memory)、隨機(jī)存取存儲(chǔ)器(RAM，Random Access Memory)、磁盤(pán)或光盤(pán)等。

另外，以上對(duì)本發(fā)明實(shí)施例所提供的基于四旋翼無(wú)人機(jī)的姿態(tài)控制的方法及其系統(tǒng)進(jìn)行了詳細(xì)介紹，本文中應(yīng)用了具體個(gè)例對(duì)本發(fā)明的原理及實(shí)施方式進(jìn)行了闡述，以上實(shí)施例的說(shuō)明只是用于幫助理解本發(fā)明的方法及其核心思想；同時(shí)，對(duì)于本領(lǐng)域的一般技術(shù)人員，依據(jù)本發(fā)明的思想，在具體實(shí)施方式及應(yīng)用范圍上均會(huì)有改變之處，綜上所述，本說(shuō)明書(shū)內(nèi)容不應(yīng)理解為對(duì)本發(fā)明的限制。