微小型無人直升機建模與航跡跟蹤控制

聶 航，代冀陽，應 進，孫翊君

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

1 引言

近年來，由于直升機具有一些獨特的特性，例如垂直起降，懸停和低空低速飛行等，人們對其關注度越來越高[1]。無人直升機可在任何環境飛行，使其在軍事和民用領域具有很高的應用價值[2，3]。無人直升機是一種重要的動態耦合和高度非線性、強耦合，欠驅動系統，具有六個自由度和四個直接控制量[2，4]，使得其研究難度非常大。

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

控制器參數直接影響著無人直升機模型性能。為提高控制器的性能，對PID控制器參數整定十分關鍵。本文在均值粒子群優化算法的基礎上加入了評價指標，解決了文獻[10]中容易錯過最優解問題；同時對慣性因子進行了改進，解決了文獻[6]中全局搜索和局部搜索存在一定的互斥關系問題；另外，在傳統粒子群算法中加入簡單的變異因子，簡化了自適應粒子群算法，擴大了搜索范圍，解決了文獻[12]中算法易陷入局部極小值問題。通過仿真驗證該算法對控制器的有效性以及快速收斂性。

2 無人直升機動力學建模

(1)

無人直升機的動力學方程與外力矩作用下的分力以及力矩方程

(2)

(3)

由上述式可推出，無人直升機的非線性動力學方程為

(4)

式中：m表示無人直升機質量；Ixx、Iyy、Izz分別表示直升機對OX軸、OY軸和OZ軸的慣性積。

無人直升機非線性系統是一個高階、強耦合的復雜系統[2，4]。因此，本文采用文獻[12]中的線性化模型以及辨識參數進行建模與求解。

無人直升機線性化處理，既能簡化方程組的計算，又能充分描述無人直升機的實際情況。平衡點處采用泰勒級數展開方式，以基準運動狀態和小擾動偏差量為基礎，忽略二階及其以上導數。考慮配平條件為懸停狀態。本文利用參數化模型和狀態輸入的擾動關系，得到線性狀態空間模型。線性狀態空間模型表達式為：

(5)

狀態變量為

(6)

控制向量為

(7)

建立好線性狀態空間、狀態變量和控制變量后，將文獻[13]中的參數代入到狀態空間的矩陣A與矩陣B中，得到亞拓600型無人直升機的線性化模型。

3 進粒子群算法

3.1 傳統粒子群算法

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

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]之間的隨機數。Pt和Gt分別表示當前搜索到的個體極值和全局極值最優位置。

傳統粒子群算法缺陷：精度低、易發散。如果加速系數或最大速度等參數過大時，容易錯過最優解而出現不收斂情況。在收斂情況下，粒子群易出現同一化而失去了多樣性，使得后期收斂速度變慢，無法繼續優化，而出現精度不高等問題。現如今對粒子群的改進方法很多，如協同粒子群算法、隨機離子群算法和耗散粒子群算法等。

3.2 改進粒子群算法

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

1)改進慣性因子

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

(10)

2)粒子群速度修正

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

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

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

(11)

3)加入自適應變異

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

(12)

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

算法步驟：

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

Step2：隨機產生粒子群的速度Vi與位置Xi，并確定粒子群的當前個體最優值Pt與全局最優值Gt；

Step3：將粒子群中的粒子賦值給PID控制器參數Kp、Ki、Kd，然后運行系統的Simulink模型，輸出評價指標反饋值(即粒子適應值)執行Step10；

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

Step5：根據式(12)判斷是否超過限定值，若超過限定值，則執行Step6，若沒有執行Step7；

Step6：重新初始化粒子；

Step7：計算每個粒子適應度；

Step8：每個粒子個體適應值Pi與其個體最優位置Pt適應值進行比較，若Pi>Pt，則將其替換為Pt；

Step9：每個粒子全局適應值Gi與全局最優位置Gt適應值進行比較，若Gi>Gt，則將其替換為Gt，返回步驟3；

Step10：若沒有滿足終止條件，則返回Step4，否則退出算法，得到最優解。

4 基本人工勢場法航跡規劃

4.1 傳統人工勢場法

傳統人工勢場法表示目標對機器人產生一個引力場，而障礙物會對機器人產生一個斥力場。機器人在運動過程中，受到目標的吸引力而向目標移動，同時還受到障礙給它的排斥力而遠離障礙。

圖1 傳統人工勢場法

傳統人工勢場中，無人直升機受到的引力場函數為

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

(13)

無人直升機受到的斥力場函數為：

(14)

對上面的引力場與斥力場分別求取負梯度，可得引力場負梯度為

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

(15)

斥力場負梯度為

-?Uao(X)

(16)

傳統人工勢場法的缺陷分析：傳統人工勢場法容易出現目標不可達問題；容易陷入了局部極小值點；傳統人工勢場法缺少航路評價機制，無法判斷航路優劣。

4.2 改進人工勢場法

1)目標不可達問題改進方案：

針對上述問題，本節將對上節中的引力場函數和斥力場函數為進行改進。改進之后的引力場函數式為

(17)

改進之后的斥力場函數式為

(18)

類似的，求得引力函數負梯度為

(19)

求得斥力函數負梯度為

Fao(X)=-?Uao(X)

(20)

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

2)容易陷入局部極小值點問題改進方案：

針對上述問題，本節根據文獻[15]引入一種“回環力”。該力定義在威脅斥力的垂直面上，方向與目標引力在斥力垂直面的投影方向一致，回環力大小可以通過斥力數值大小與回環力增益系數的乘積得到[15]。回環力函數表達式為

Fac(X)=

(21)

式中：γ1表示回環力增益常數；η1表示回環力作用距離常數。

4.3 無人直升機軌跡跟蹤控制器設計

單機控制器主要運用改進人工勢場法規劃的路徑作為期望軌跡，實時將規劃的路徑輸入到無人直升機模型。運用Matlab與Simulink交互，將Matlab數據實時傳輸到Simulink模型里面。路徑規劃時均預留了10米距離，保證無人直升機飛行過程安全的同時，也為后續真實無人直升機航跡規劃預留足夠安全距離。

無人直升機模型分別從姿態、速度和位置控制器進行設計，來介紹無人直升機的軌跡跟蹤器問題。其整體設計如圖2所示，圖中九個輸出量分別為u、v、w、θ、φ、ψ、q、p、r。其中u、v、w為線速度，θ、φ、ψ為姿態角，q、p、r為姿態角速度。本文將采用串級PID控制的方法對無人直升機軌跡跟蹤進行控制，其中位置控制器將包含坐標轉換環節，速度控制器與姿態控制器將分別控制無人直升機飛行速度與姿態變化。期望軌跡實質就是引導無人直升機飛行，通過無人直升機模型，得到與真實無人直升機相近的軌跡。

圖2 無人直升機控制器設計結構

5 仿真分析

對粒子群算法進行非線性函數極小值點尋優實驗，實驗選取函數如下所示

非線性函數1

(22)

非線性函數2

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

-10*cos(2πy)+20

(23)

非線性函數3

(24)

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

表1 非線性函數運行20次后的結果比較

表2 非線性函數運行20次后的結果比較

從表1中可以看出，不論是三個函數的最優解還是20次運行結果的平均值，IPSO的算法精度都是最高的。從表2中可以看出來，IPSO算法的平均迭代次數也是最少的。這說明，IPSO算法不僅搜索能力強，而且能夠快速找到最優解。

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

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

圖3 各個通道的適應度值

圖4 理論航跡與無人直升機航跡圖

圖5 無人直升機姿態響應曲線

圖6 無人直升機誤差響應曲線

由圖3可知，改進粒子群對姿態環的收斂速度最快，是由于姿態環處于內環。而位置環收斂速度最慢，是由于其處于外環。但從適應度值來看，速度環的適應度值最好。從三個環適應度曲線均在迭代10次時收斂，說明改粒子群算法對PID控制器的收斂速度很快。從適應度值看，三個環的適應度值普遍都很小，說明改進算法的尋優能力較快。

由圖4可知，紅色為無人直升機曲線，黑色為航跡規劃曲線。藍色的點為航跡規劃起點，綠色點為目標終點。無人直升機首先從真實位置向航跡規劃起點飛行，然后再根據航跡規劃線路飛行。從圖中可以看出，無人直升機真實的紅色軌跡，與航跡規劃的黑色軌跡重合程度高，且最終到達目標點。

由圖5可知，由于航跡規劃的為無規律軌跡，圖中的姿態變化也較為復雜，從圖中可以看出，其滾轉角與俯仰角前期變化較大，但后期變化較小，而偏航角一直處于變化中，由于航跡規劃中，無人直升機一直受到目標點的吸引力、障礙物的排斥力以及回環力而處于不規則運動狀態，所以偏航角一直處于左右運動狀態。

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

6 結論

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

2)采用改進人工勢場法進行航跡規劃。通過改進勢場函數，解決了目標不可達問題；在飛行過程中引入“回環力”，解決了傳統勢場法易收斂到局部極小值的問題。

3)采用無人直升機模型與航跡規劃路徑相結合，對規劃路徑以及控制器效果進行驗證。由實驗結果可知，無人直升機的跟蹤誤差軌跡收斂度較高，實際飛行軌跡與期望軌跡基本相同，且最終能夠順利到達目標點。本文設計的控制器能達到較好的控制效果，符合設計要求。