微小型無(wú)人直升機(jī)建模與航跡跟蹤控制

聶 航，代冀陽(yáng)，應(yīng) 進(jìn)，孫翊君

(南昌航空大學(xué)信息工程學(xué)院，江西 南昌 330063)

1 引言

近年來(lái)，由于直升機(jī)具有一些獨(dú)特的特性，例如垂直起降，懸停和低空低速飛行等，人們對(duì)其關(guān)注度越來(lái)越高[1]。無(wú)人直升機(jī)可在任何環(huán)境飛行，使其在軍事和民用領(lǐng)域具有很高的應(yīng)用價(jià)值[2，3]。無(wú)人直升機(jī)是一種重要的動(dòng)態(tài)耦合和高度非線性、強(qiáng)耦合，欠驅(qū)動(dòng)系統(tǒng)，具有六個(gè)自由度和四個(gè)直接控制量[2，4]，使得其研究難度非常大。

目前，無(wú)人直升機(jī)控制算法中，PID控制方法是使用最廣泛的方法[5，6]，傳統(tǒng)的PID控制方法具有實(shí)現(xiàn)簡(jiǎn)單，魯棒性好，可靠性高的優(yōu)點(diǎn)，但其參數(shù)調(diào)整困難，一般是根據(jù)經(jīng)驗(yàn)和仿真調(diào)試獲得的，控制效果難以達(dá)到期望值[7]。PID控制器的關(guān)鍵問(wèn)題是精確有效地調(diào)整參數(shù)[7]。利用群體智能方法優(yōu)化PID控制參數(shù)是一種可行而有效的方法[8]。文獻(xiàn)[6]中提出設(shè)計(jì)時(shí)間與絕對(duì)誤差積分(ITAE)性能指標(biāo)下PID控制算法，該性能指標(biāo)在縮短抑制時(shí)間中效果較好，但僅加入評(píng)價(jià)指標(biāo)對(duì)PID控制器整定來(lái)說(shuō)稍顯欠缺。文獻(xiàn)[9]中選擇粒子群優(yōu)化的適應(yīng)度函數(shù)作為“積分絕對(duì)誤差”(IAE)方法，該方法在抑制小偏差中效果較好，但是在抑制大偏差和抑制響應(yīng)誤差中效果較差，且抑制時(shí)間較長(zhǎng)，而無(wú)人直升機(jī)模型較為復(fù)雜，不太適用于此類方法。文獻(xiàn)[7]中提出對(duì)PSO的慣性權(quán)重進(jìn)行動(dòng)態(tài)調(diào)整，擴(kuò)大了搜索面積，但是粒子的搜索的速度減慢，使得其收斂時(shí)間變長(zhǎng)。文獻(xiàn)[10]中提出一種均值粒子群優(yōu)化算法，該方法對(duì)粒子群優(yōu)化的快速性以及收斂性較好，但因缺少評(píng)價(jià)指標(biāo)而使得錯(cuò)過(guò)最優(yōu)解。文獻(xiàn)[11]中提出自適應(yīng)粒子群優(yōu)化算法，提高了算法的效率，但算法過(guò)于復(fù)雜，且收斂時(shí)間變慢。文獻(xiàn)[12]中提出采用了一種基于相似度動(dòng)態(tài)調(diào)整慣性權(quán)重的方法(LPSO)，提高了算法的收斂時(shí)間，但該算法易陷入局部極小值點(diǎn)。

控制器參數(shù)直接影響著無(wú)人直升機(jī)模型性能。為提高控制器的性能，對(duì)PID控制器參數(shù)整定十分關(guān)鍵。本文在均值粒子群優(yōu)化算法的基礎(chǔ)上加入了評(píng)價(jià)指標(biāo)，解決了文獻(xiàn)[10]中容易錯(cuò)過(guò)最優(yōu)解問(wèn)題；同時(shí)對(duì)慣性因子進(jìn)行了改進(jìn)，解決了文獻(xiàn)[6]中全局搜索和局部搜索存在一定的互斥關(guān)系問(wèn)題；另外，在傳統(tǒng)粒子群算法中加入簡(jiǎn)單的變異因子，簡(jiǎn)化了自適應(yīng)粒子群算法，擴(kuò)大了搜索范圍，解決了文獻(xiàn)[12]中算法易陷入局部極小值問(wèn)題。通過(guò)仿真驗(yàn)證該算法對(duì)控制器的有效性以及快速收斂性。

2 無(wú)人直升機(jī)動(dòng)力學(xué)建模

(1)

無(wú)人直升機(jī)的動(dòng)力學(xué)方程與外力矩作用下的分力以及力矩方程

(2)

(3)

由上述式可推出，無(wú)人直升機(jī)的非線性動(dòng)力學(xué)方程為

(4)

式中：m表示無(wú)人直升機(jī)質(zhì)量；Ixx、Iyy、Izz分別表示直升機(jī)對(duì)OX軸、OY軸和OZ軸的慣性積。

無(wú)人直升機(jī)非線性系統(tǒng)是一個(gè)高階、強(qiáng)耦合的復(fù)雜系統(tǒng)[2，4]。因此，本文采用文獻(xiàn)[12]中的線性化模型以及辨識(shí)參數(shù)進(jìn)行建模與求解。

無(wú)人直升機(jī)線性化處理，既能簡(jiǎn)化方程組的計(jì)算，又能充分描述無(wú)人直升機(jī)的實(shí)際情況。平衡點(diǎn)處采用泰勒級(jí)數(shù)展開方式，以基準(zhǔn)運(yùn)動(dòng)狀態(tài)和小擾動(dòng)偏差量為基礎(chǔ)，忽略二階及其以上導(dǎo)數(shù)。考慮配平條件為懸停狀態(tài)。本文利用參數(shù)化模型和狀態(tài)輸入的擾動(dòng)關(guān)系，得到線性狀態(tài)空間模型。線性狀態(tài)空間模型表達(dá)式為：

(5)

狀態(tài)變量為

(6)

控制向量為

(7)

建立好線性狀態(tài)空間、狀態(tài)變量和控制變量后，將文獻(xiàn)[13]中的參數(shù)代入到狀態(tài)空間的矩陣A與矩陣B中，得到亞拓600型無(wú)人直升機(jī)的線性化模型。

3 進(jìn)粒子群算法

3.1 傳統(tǒng)粒子群算法

文獻(xiàn)[14]中提出傳統(tǒng)粒子群算法，并提出了每次迭代的個(gè)體極值和全局極值的速度與位置更新公式為

vt+1=wvt+c1r1+(Pt-xt)+c2r2(Gt-xt)

(8)

xt+1=xt+vt+1

(9)

式中：v為粒子速度；w表示慣性因子；x表示粒子位置；c1、c2表示加速常量；r1、r2表示范圍[0，1]之間的隨機(jī)數(shù)。Pt和Gt分別表示當(dāng)前搜索到的個(gè)體極值和全局極值最優(yōu)位置。

傳統(tǒng)粒子群算法缺陷：精度低、易發(fā)散。如果加速系數(shù)或最大速度等參數(shù)過(guò)大時(shí)，容易錯(cuò)過(guò)最優(yōu)解而出現(xiàn)不收斂情況。在收斂情況下，粒子群易出現(xiàn)同一化而失去了多樣性，使得后期收斂速度變慢，無(wú)法繼續(xù)優(yōu)化，而出現(xiàn)精度不高等問(wèn)題?，F(xiàn)如今對(duì)粒子群的改進(jìn)方法很多，如協(xié)同粒子群算法、隨機(jī)離子群算法和耗散粒子群算法等。

3.2 改進(jìn)粒子群算法

粒子群算法對(duì)于全局搜索能力要求很高。但當(dāng)Vmax過(guò)小時(shí)，無(wú)法達(dá)到最大搜索能力，只能局部搜索，無(wú)論慣性因子w取多大；如果Vmax過(guò)大時(shí)，可以通過(guò)慣性因子w，提供很多的搜索范圍。由此可知，最大速度Vmax和慣性因子w間接或直接影響全局搜索能力，必須找一個(gè)平衡全局搜索和局部搜索的慣性因子w的值。

1)改進(jìn)慣性因子

為解決上述問(wèn)題，本文采用逐漸降低慣性因子w的方式進(jìn)行改進(jìn)，從而使其搜索面積更大，可以較快的選取最優(yōu)解的大致區(qū)域。而隨著慣性因子w的值降低，粒子搜索速度減慢，搜索更加細(xì)致，提高了算法的性能。其改進(jìn)的慣性因子公式為

(10)

2)粒子群速度修正

除了慣性因子w，速度對(duì)全局搜索能力也影響十分明顯。在搜索時(shí)，粒子們要綜合考慮自身與同伴之間的優(yōu)劣不斷調(diào)整自身方向。本文將修正速度公式，對(duì)當(dāng)前的個(gè)體極值與全局極值取均值。這樣在搜索中更能快速考慮到自身與同伴之間的優(yōu)劣，更快的搜索到全局最優(yōu)值。粒子群速度修正公式為

vt+1=wvt+c1r1((Pt+Gt)/2-xt)…

+c2r2((Pt+Gt)/2-xt)

(11)

3)加入自適應(yīng)變異

針對(duì)傳統(tǒng)粒子群算法易出現(xiàn)早收斂、搜索精度低、后期收斂速度變慢等缺點(diǎn)。本文將借鑒遺傳算法的變異想法，對(duì)粒子群算法進(jìn)行變異操作，維持種群的多樣性。變異操作可以擴(kuò)大種群的搜索空間，使得粒子可以跳出之前搜索的最優(yōu)值的位置，提高了算法尋找最優(yōu)解的能力。本文在粒子群算法的基礎(chǔ)上引入一個(gè)簡(jiǎn)單變異算子，粒子每次更新后，以一定的概率對(duì)粒子進(jìn)行初始化。

(12)

式中：R表示更新的粒子；L表示重新初始化的粒子；r表示限定值。

算法步驟：

Step1：初始化粒子群。對(duì)粒子群各個(gè)參數(shù)進(jìn)行設(shè)置：種群大小n，搜索空間M維，加速常量c1、c2，Kp、Ki、Kd以及終止條件等；

Step2：隨機(jī)產(chǎn)生粒子群的速度Vi與位置Xi，并確定粒子群的當(dāng)前個(gè)體最優(yōu)值Pt與全局最優(yōu)值Gt；

Step3：將粒子群中的粒子賦值給PID控制器參數(shù)Kp、Ki、Kd，然后運(yùn)行系統(tǒng)的Simulink模型，輸出評(píng)價(jià)指標(biāo)反饋值(即粒子適應(yīng)值)執(zhí)行Step10；

Step4：將式(10)代入式(11)中更新粒子速度，根據(jù)式(9)更新當(dāng)前粒子的位置；

Step5：根據(jù)式(12)判斷是否超過(guò)限定值，若超過(guò)限定值，則執(zhí)行Step6，若沒(méi)有執(zhí)行Step7；

Step6：重新初始化粒子；

Step7：計(jì)算每個(gè)粒子適應(yīng)度；

Step8：每個(gè)粒子個(gè)體適應(yīng)值Pi與其個(gè)體最優(yōu)位置Pt適應(yīng)值進(jìn)行比較，若Pi>Pt，則將其替換為Pt；

Step9：每個(gè)粒子全局適應(yīng)值Gi與全局最優(yōu)位置Gt適應(yīng)值進(jìn)行比較，若Gi>Gt，則將其替換為Gt，返回步驟3；

Step10：若沒(méi)有滿足終止條件，則返回Step4，否則退出算法，得到最優(yōu)解。

4 基本人工勢(shì)場(chǎng)法航跡規(guī)劃

4.1 傳統(tǒng)人工勢(shì)場(chǎng)法

傳統(tǒng)人工勢(shì)場(chǎng)法表示目標(biāo)對(duì)機(jī)器人產(chǎn)生一個(gè)引力場(chǎng)，而障礙物會(huì)對(duì)機(jī)器人產(chǎn)生一個(gè)斥力場(chǎng)。機(jī)器人在運(yùn)動(dòng)過(guò)程中，受到目標(biāo)的吸引力而向目標(biāo)移動(dòng)，同時(shí)還受到障礙給它的排斥力而遠(yuǎn)離障礙。

圖1 傳統(tǒng)人工勢(shì)場(chǎng)法

傳統(tǒng)人工勢(shì)場(chǎng)中，無(wú)人直升機(jī)受到的引力場(chǎng)函數(shù)為

Uat(X)=0.5αtρ2(X，Xt)

(13)

無(wú)人直升機(jī)受到的斥力場(chǎng)函數(shù)為：

(14)

對(duì)上面的引力場(chǎng)與斥力場(chǎng)分別求取負(fù)梯度，可得引力場(chǎng)負(fù)梯度為

-?Uat(X)=-αtρ(X，Xt)

(15)

斥力場(chǎng)負(fù)梯度為

-?Uao(X)

(16)

傳統(tǒng)人工勢(shì)場(chǎng)法的缺陷分析：傳統(tǒng)人工勢(shì)場(chǎng)法容易出現(xiàn)目標(biāo)不可達(dá)問(wèn)題；容易陷入了局部極小值點(diǎn)；傳統(tǒng)人工勢(shì)場(chǎng)法缺少航路評(píng)價(jià)機(jī)制，無(wú)法判斷航路優(yōu)劣。

4.2 改進(jìn)人工勢(shì)場(chǎng)法

1)目標(biāo)不可達(dá)問(wèn)題改進(jìn)方案：

針對(duì)上述問(wèn)題，本節(jié)將對(duì)上節(jié)中的引力場(chǎng)函數(shù)和斥力場(chǎng)函數(shù)為進(jìn)行改進(jìn)。改進(jìn)之后的引力場(chǎng)函數(shù)式為

(17)

改進(jìn)之后的斥力場(chǎng)函數(shù)式為

(18)

類似的，求得引力函數(shù)負(fù)梯度為

(19)

求得斥力函數(shù)負(fù)梯度為

Fao(X)=-?Uao(X)

(20)

式中：ρ1=max{ρomax，ρa(bǔ)max}，ρ2=min{ρomin，ρa(bǔ)min}；αt1和αt2表示無(wú)人直升機(jī)受到的引力增益常數(shù)；βo表示無(wú)人直升機(jī)受到的斥力增益常數(shù)。ρ(X，Xt)和ρ(X，Xi)分別表示無(wú)人直升機(jī)重心位置到目標(biāo)和威脅的空間距離；ρa(bǔ)min表示無(wú)人直升機(jī)偵查到威脅影響的最小范圍；ρa(bǔ)max表示無(wú)人直升機(jī)偵查到威脅影響的最大范圍；ρomin表示無(wú)人直升機(jī)受到威脅影響的最小范圍；ρomax表示無(wú)人直升機(jī)受到威脅影響的最大范圍。

2)容易陷入局部極小值點(diǎn)問(wèn)題改進(jìn)方案：

針對(duì)上述問(wèn)題，本節(jié)根據(jù)文獻(xiàn)[15]引入一種“回環(huán)力”。該力定義在威脅斥力的垂直面上，方向與目標(biāo)引力在斥力垂直面的投影方向一致，回環(huán)力大小可以通過(guò)斥力數(shù)值大小與回環(huán)力增益系數(shù)的乘積得到[15]?；丨h(huán)力函數(shù)表達(dá)式為

Fac(X)=

(21)

式中：γ1表示回環(huán)力增益常數(shù)；η1表示回環(huán)力作用距離常數(shù)。

4.3 無(wú)人直升機(jī)軌跡跟蹤控制器設(shè)計(jì)

單機(jī)控制器主要運(yùn)用改進(jìn)人工勢(shì)場(chǎng)法規(guī)劃的路徑作為期望軌跡，實(shí)時(shí)將規(guī)劃的路徑輸入到無(wú)人直升機(jī)模型。運(yùn)用Matlab與Simulink交互，將Matlab數(shù)據(jù)實(shí)時(shí)傳輸?shù)絊imulink模型里面。路徑規(guī)劃時(shí)均預(yù)留了10米距離，保證無(wú)人直升機(jī)飛行過(guò)程安全的同時(shí)，也為后續(xù)真實(shí)無(wú)人直升機(jī)航跡規(guī)劃預(yù)留足夠安全距離。

無(wú)人直升機(jī)模型分別從姿態(tài)、速度和位置控制器進(jìn)行設(shè)計(jì)，來(lái)介紹無(wú)人直升機(jī)的軌跡跟蹤器問(wèn)題。其整體設(shè)計(jì)如圖2所示，圖中九個(gè)輸出量分別為u、v、w、θ、φ、ψ、q、p、r。其中u、v、w為線速度，θ、φ、ψ為姿態(tài)角，q、p、r為姿態(tài)角速度。本文將采用串級(jí)PID控制的方法對(duì)無(wú)人直升機(jī)軌跡跟蹤進(jìn)行控制，其中位置控制器將包含坐標(biāo)轉(zhuǎn)換環(huán)節(jié)，速度控制器與姿態(tài)控制器將分別控制無(wú)人直升機(jī)飛行速度與姿態(tài)變化。期望軌跡實(shí)質(zhì)就是引導(dǎo)無(wú)人直升機(jī)飛行，通過(guò)無(wú)人直升機(jī)模型，得到與真實(shí)無(wú)人直升機(jī)相近的軌跡。

圖2 無(wú)人直升機(jī)控制器設(shè)計(jì)結(jié)構(gòu)

5 仿真分析

對(duì)粒子群算法進(jìn)行非線性函數(shù)極小值點(diǎn)尋優(yōu)實(shí)驗(yàn)，實(shí)驗(yàn)選取函數(shù)如下所示

非線性函數(shù)1

(22)

非線性函數(shù)2

f(x，y)=x2+y2-10*cos(2πx)…

-10*cos(2πy)+20

(23)

非線性函數(shù)3

(24)

將本文提出的改進(jìn)粒子群算法(IPSO)與文獻(xiàn)[6]提出的PSO算法、文獻(xiàn)[12]中的提出的LPSO算法進(jìn)行對(duì)比實(shí)驗(yàn)。實(shí)驗(yàn)過(guò)程中，保證各組粒子群的初始化參數(shù)一致，并對(duì)每組算法進(jìn)行20次重復(fù)實(shí)驗(yàn)，記錄非線性函數(shù)運(yùn)行20次后的最優(yōu)值和平均值，以及平均迭代次數(shù)，最終匯總到表1與表2進(jìn)行對(duì)比分析。具體結(jié)果如表1、表2所示。

表1 非線性函數(shù)運(yùn)行20次后的結(jié)果比較

表2 非線性函數(shù)運(yùn)行20次后的結(jié)果比較

從表1中可以看出，不論是三個(gè)函數(shù)的最優(yōu)解還是20次運(yùn)行結(jié)果的平均值，IPSO的算法精度都是最高的。從表2中可以看出來(lái)，IPSO算法的平均迭代次數(shù)也是最少的。這說(shuō)明，IPSO算法不僅搜索能力強(qiáng)，而且能夠快速找到最優(yōu)解。

首先，根據(jù)粒子群算法對(duì)無(wú)人直升機(jī)控制器參數(shù)整定，具體參數(shù)整定如表3所示，其各個(gè)通道的適應(yīng)度值，如圖3所示。航跡規(guī)劃算法首先設(shè)置一個(gè)三維空間，在三維空間中設(shè)置7個(gè)障礙物，并初始化改進(jìn)人工勢(shì)場(chǎng)法的各個(gè)參數(shù)。航跡規(guī)劃起始位置為(-50，-50，0)，無(wú)人直升機(jī)真實(shí)位置為(0，0，0)，目標(biāo)位置為(500，450，0)。本文無(wú)人直升機(jī)軌跡由改進(jìn)人工勢(shì)場(chǎng)法航跡規(guī)劃路徑進(jìn)行引導(dǎo)，理論航跡與無(wú)人直升機(jī)飛行軌跡圖，如圖4所示。無(wú)人直升機(jī)姿態(tài)響應(yīng)曲線，如圖5所示。無(wú)人直升機(jī)誤差響應(yīng)曲線，如圖6所示。

表3 粒子群算法整定的各控制器參數(shù)

圖3 各個(gè)通道的適應(yīng)度值

圖4 理論航跡與無(wú)人直升機(jī)航跡圖

圖5 無(wú)人直升機(jī)姿態(tài)響應(yīng)曲線

圖6 無(wú)人直升機(jī)誤差響應(yīng)曲線

由圖3可知，改進(jìn)粒子群對(duì)姿態(tài)環(huán)的收斂速度最快，是由于姿態(tài)環(huán)處于內(nèi)環(huán)。而位置環(huán)收斂速度最慢，是由于其處于外環(huán)。但從適應(yīng)度值來(lái)看，速度環(huán)的適應(yīng)度值最好。從三個(gè)環(huán)適應(yīng)度曲線均在迭代10次時(shí)收斂，說(shuō)明改粒子群算法對(duì)PID控制器的收斂速度很快。從適應(yīng)度值看，三個(gè)環(huán)的適應(yīng)度值普遍都很小，說(shuō)明改進(jìn)算法的尋優(yōu)能力較快。

由圖4可知，紅色為無(wú)人直升機(jī)曲線，黑色為航跡規(guī)劃曲線。藍(lán)色的點(diǎn)為航跡規(guī)劃起點(diǎn)，綠色點(diǎn)為目標(biāo)終點(diǎn)。無(wú)人直升機(jī)首先從真實(shí)位置向航跡規(guī)劃起點(diǎn)飛行，然后再根據(jù)航跡規(guī)劃線路飛行。從圖中可以看出，無(wú)人直升機(jī)真實(shí)的紅色軌跡，與航跡規(guī)劃的黑色軌跡重合程度高，且最終到達(dá)目標(biāo)點(diǎn)。

由圖5可知，由于航跡規(guī)劃的為無(wú)規(guī)律軌跡，圖中的姿態(tài)變化也較為復(fù)雜，從圖中可以看出，其滾轉(zhuǎn)角與俯仰角前期變化較大，但后期變化較小，而偏航角一直處于變化中，由于航跡規(guī)劃中，無(wú)人直升機(jī)一直受到目標(biāo)點(diǎn)的吸引力、障礙物的排斥力以及回環(huán)力而處于不規(guī)則運(yùn)動(dòng)狀態(tài)，所以偏航角一直處于左右運(yùn)動(dòng)狀態(tài)。

由圖6可知，三個(gè)軸的軌跡誤差中，X軸與Y軸初始誤差較大，是由于無(wú)人直升機(jī)起始點(diǎn)為(0，0，0)，而規(guī)劃起始點(diǎn)為(-50，-50，0)，但模型的響應(yīng)速度很快，在短時(shí)間飛行到航跡路徑后，X軸和Y軸的誤差隨即變得很小。Z軸誤差較大處，為直升機(jī)上升轉(zhuǎn)下降點(diǎn)處，且該處障礙物較多，導(dǎo)致誤差超過(guò)2米，且直升機(jī)出現(xiàn)徘徊誤差，但在之前航跡規(guī)劃中已經(jīng)預(yù)留10米的安全距離，所以誤差處于被控范圍內(nèi)，出現(xiàn)徘徊誤差，是由于航跡規(guī)劃時(shí)直升機(jī)受到多個(gè)力而出現(xiàn)航跡徘徊。根據(jù)上述實(shí)驗(yàn)可知，該控制器對(duì)無(wú)規(guī)律航跡的跟蹤較好。

6 結(jié)論

1)采用改進(jìn)粒子群算法對(duì)無(wú)人直升機(jī)控制器進(jìn)行參數(shù)整定時(shí)，改進(jìn)慣性因子提高了算法的尋優(yōu)范圍；修正粒子群速度，改善了算法的收斂速度；加入自適應(yīng)變異，解決了易陷入局部極小值問(wèn)題。

2)采用改進(jìn)人工勢(shì)場(chǎng)法進(jìn)行航跡規(guī)劃。通過(guò)改進(jìn)勢(shì)場(chǎng)函數(shù)，解決了目標(biāo)不可達(dá)問(wèn)題；在飛行過(guò)程中引入“回環(huán)力”，解決了傳統(tǒng)勢(shì)場(chǎng)法易收斂到局部極小值的問(wèn)題。

3)采用無(wú)人直升機(jī)模型與航跡規(guī)劃路徑相結(jié)合，對(duì)規(guī)劃路徑以及控制器效果進(jìn)行驗(yàn)證。由實(shí)驗(yàn)結(jié)果可知，無(wú)人直升機(jī)的跟蹤誤差軌跡收斂度較高，實(shí)際飛行軌跡與期望軌跡基本相同，且最終能夠順利到達(dá)目標(biāo)點(diǎn)。本文設(shè)計(jì)的控制器能達(dá)到較好的控制效果，符合設(shè)計(jì)要求。