直線二級倒立擺的一種穩(wěn)定控制算法研究

蔣凌云，徐炳吉，張峰華，張欽琛

(中國地質大學(北京)信息工程學院，北京100083)

1 引言

倒立擺被譽為是“控制理論的試金石[1]”，是檢驗控制理論與算法是否可靠，能否實施的極佳平臺，并將研究的控制算法應用到航天，機器人等很多領域中。倒立擺是一個具有非線性，不穩(wěn)定，多變量，強耦合等特性的系統(tǒng)，任何重心在上、支點在下的控制問題研究都與倒立擺的控制相似，比如，機器人行走中的平衡控制，電動平衡車的平衡控制，火箭垂直度控制等等[2][3]。研究倒立擺的控制不僅具有理論意義也具實際意義。

倒立擺的控制分為起擺控制和穩(wěn)定控制兩部分。本文研究的是直線二級倒立擺的平衡點附近的穩(wěn)定控制?，F有的倒立擺穩(wěn)定控制方法有LQR，PID控制等等。彭錦，尹遜和，曹戈等人[2][3][4]對直線二級倒立擺進行分析，并在倒立擺平衡點處進行線性化處理，得到倒立擺的狀態(tài)空間方程，設計了LQR對其進行穩(wěn)定控制，但僅僅只是通過人工試湊得到LQR的一組相對合適的參數，這樣的“最優(yōu)控制”并不是嚴格意義上的最優(yōu)，人為因素太多。由于直線二級倒立擺狀態(tài)變量較多，人工試湊LQR的參數很繁瑣，利用粒子群算法來優(yōu)化LQR參數是個很好的選擇。武俊峰，鄒忱忱等人[5][6]分別利用基本PSO和慣性權重線性遞減的PSO來優(yōu)化LQR，并對二級倒立擺進行穩(wěn)定控制，控制效果優(yōu)于人工試湊法。但是基本粒子群算法存在收斂速度慢，容易陷入局部最優(yōu)等缺點，算法搜索過程是一個復雜的非線性過程，慣性權重線性遞減也未必是一個好的改進措施[7]。吳永紅[8]等人從慣性權重和學習因子兩個角度來改進基本PSO，改進后效果要優(yōu)于基本PSO，但是收斂速度和精度依然存在一定問題。南潔瓊[9]等人采用慣性權重隨機正弦調整策略來改進基本PSO。陳林[10]等人采用慣性權重隨機正弦調整策略和概率變異策略相結合來改基本PSO。但是這兩種方法中，慣性權重在初期減少得比較快，不利于權衡全局搜索能力和局部搜索能力。

本文從慣性權重非線性動態(tài)調整，學習因子非線性動態(tài)調整以及基于自然選擇機理三個角度來改進基本粒子群算法。慣性權重非線性動態(tài)調整有利于權衡算法的前期全局搜索能力和后期局部搜索能力；學習因子非線性動態(tài)調整有利于權衡粒子不同階段自我學習能力和社會學習能力。進行自然選擇有利于提高算法的收斂速度和尋優(yōu)能力。將改進后的算法應用于LQR的參數優(yōu)化，實現對直線二級倒立擺的穩(wěn)定控制，并與基本粒子群算法，慣性權重線性遞減粒子群算法，遺傳算法優(yōu)化的LQR控制效果相對比。仿真結果表明，改進后的算法優(yōu)化的LQR具有更佳的控制效果。

2 直線二級倒立擺數學模型與穩(wěn)定控制描述

直線二級倒立擺簡化圖如圖1所示，小車質量為M，擺桿1和擺桿質量分別為m1，m2，質量塊質量為m3，擺桿1和擺桿2質心到轉動軸心的距離分別為l1，l2，F為作用在小車上的合外力。x為小車位移，θ1和θ2分別為擺桿1和擺桿2與豎直方向上的夾角。

圖1 直線二級倒立擺簡化圖

為了簡化建模過程，忽略空氣所帶來的阻力，認為兩個擺桿都是剛體且質量分布均勻，小車受到導軌對它的摩檫力正比于小車的速度，擺桿1所受到的轉動摩擦力矩正比于它相對于小車的相對角速度，擺桿2所受到的轉動摩擦力矩正比于它相對于擺桿1的相對角速度。

在上述假設條件的基礎上，以小車位移x，擺桿1的角度θ1和擺桿2的角度θ2為廣義坐標，采用第二類Lagrange方程對倒立擺系統(tǒng)進行建模。

(1)

L為Lagrange算子，T為系統(tǒng)的動能，V為系統(tǒng)的勢能，q=[x，θ1，θ2]。

系統(tǒng)在廣義坐標x所受合外力為F，在廣義坐標θ1和θ2并未受到外力，所以有：

(2)

(3)

(4)

對小車，擺桿1和擺桿2和質量塊進行能量分析，得到系統(tǒng)動能和勢能，得出Lagrange算子，利用式(2)(3)(4)得到系統(tǒng)的非線性微分方程，詳細推導見文獻[2][3]。由于在穩(wěn)擺控制過程中，θ1和θ2很小，所以可以認為cos(θ2-θ1)，sin(θ2-θ1)，cosθ2≈cosθ1，sinθ2≈θ2，sinθ1≈θ1[11]。

取狀態(tài)向量：

令u=F，在系統(tǒng)的平衡位置[0，0，0，0，0，0]T進行線性化處理，詳細推導及數據見文獻[2][3]，得到直線二級倒立擺線性處理后的狀態(tài)空間模型

(5)

(6)

根據線性系統(tǒng)能控性和能觀性判據，得

所以直線二級倒立擺在平衡位置線性化處理后得到狀態(tài)空間模型能控能觀，可以設計LQR控制器使得系統(tǒng)穩(wěn)定。直線二級倒立擺的穩(wěn)定控制就是通過設計一個合適的控制器，對小車施加合適的控制信號，讓小車位置發(fā)生改變，使得兩個擺桿維持豎直向上狀態(tài)。

3 線性二次型最優(yōu)調節(jié)器的設計

3.1 線性二次型調節(jié)器(LQR)原理

假定一個線性時不變(LTI)系統(tǒng)

(7)

y(t)=Cx(t)

(8)

其中x、u、y分別為n維狀態(tài)變量，r維輸入矢量，m維輸出矢量；A∈Rn×n，B∈Rn×r，C∈Rm×n分別為系統(tǒng)狀態(tài)矩陣，輸入矩陣，輸出矩陣。引入最優(yōu)控制二次型性能指標

(9)

其中，Q為n×n維半正定狀態(tài)加權矩陣，Q=diag(q11，q22，q33，q44，q55，q66)；R為r×r維正定輸入加權矩陣，R=[q77]；S為n×n維半正定終端狀態(tài)加權矩陣。設計一個輸入量u(t)，使得J最小化，達到能量和誤差綜合最優(yōu)。由線性二次型最優(yōu)控制理論可得，要使得J最小化，控制信號應為

u*(t)=-R-1BTP(t)x(t)=-K(t)x(t)

(10)

其中，P(t)為n×n維實對稱正定矩陣，K(t)為r×n維最優(yōu)反饋增益矩陣。P(t)滿足黎卡提(Riccati)矩陣微分方程

(11)

(12)

式(11)變?yōu)槔杩ㄌ?Riccati)矩陣代數方程

0=-P(t)A-ATP(t)+P(t)BR-1BTP(t)-Q

(13)

解式(13)即可得到P(t)，控制信號為：u*(t)=-R-1BTP(t)x(t)=-K(t)x(t)，使得J最小。

3.2 直線二級倒立擺LQR仿真研究與分析

通常情況下，LQR控制器的Q和R矩陣選取沒有一個固定解析類方法，所以控制器的性能好壞基本取決于人工經驗對Q和R的選取。而且直線二級倒立擺的各狀態(tài)變量存在強耦合性，不斷通過調整各個狀態(tài)變量的權重，進行試湊來達到理想的控制效果，十分繁瑣，并且不能確定性能指標是否滿足限定范圍。

直線二級倒立擺是個非線性系統(tǒng)，而LQR的設計是建立在其平衡點線性化處理后得到的線性模型基礎上，因此要考慮系統(tǒng)的狀態(tài)變量變化不能超過這個線性范圍，即擺桿的角度和角速度不能過大；并且由于導軌有一定長度限制，小車的位移也不能過大。倒立擺初始狀態(tài)設置為[0，0，0，0，0，0]T，R設置為[1]，然后施加階躍信號，本文通過試湊多組參數，最終選取一組合適參數：Q=diag(1500，1000，600，0，0，0)，R=[1]，得到K=[38.7298，128.6358，-277.4202，35.4025，0.5085，-46.0550]，系統(tǒng)的階躍響應曲線如圖2。

圖2 倒立擺階躍響應

可以看見擺桿擺角波動小，小車位移很小，響應迅速，滿足擺桿的角度和角速度不能過大，小車的位移不能過大的條件。

4 LQR的優(yōu)化

4.1 基本粒子群算法原理

粒子群算法(PSO)是一種隨機搜索算法、迭代進化算法，啟發(fā)式優(yōu)化算法[7]，適用于動態(tài)，多目標優(yōu)化問題的尋優(yōu)，與一些傳統(tǒng)優(yōu)化算法比較具有快速的計算速度和更好的全局尋優(yōu)能力。

假定被優(yōu)化函數的解空間是D維空間，粒子群包含N個粒子，PSO首先初始化N個隨機粒子，即被優(yōu)化函數的N個隨機解，每個粒子都有自己的位置(被優(yōu)化函數的潛在解)Xi和速度Vi，第i個粒子搜索到的最優(yōu)位置(解)PBEST(i)稱為個體極值，整個粒子群搜索到的最優(yōu)位置(解)GBEST，分別記為

PSO通過迭代搜索被優(yōu)化函數的全局最優(yōu)解，確定個體最優(yōu)位置和全局最優(yōu)位置后，通過公式更新個體位置和速度

(14)

(15)

k為當前迭代次數；ω為慣性權重；C1，C2為取值非負的學習因子，兩者通常取2；r1，r2為[0，1]范圍內產生的均勻隨機數。通常粒子的速度會有一個限制范圍，粒子的位置也會有邊界限制范圍，速度和粒子位置超過限制范圍則采用其邊界值。然后代入被優(yōu)化函數評價出新的個體最優(yōu)位置，全局最優(yōu)位置和全局最優(yōu)適應值，循環(huán)上述式(14)(15)，直至達到最大迭代次數或者滿足其它設定條件，算法尋優(yōu)結束，輸出最優(yōu)解和相應的適應值。

基本粒子群算法有一定的缺陷，算法容易產生早熟收斂或者收斂速度慢，環(huán)境的變化會影響算法性能，容易陷入局部最優(yōu)，不能保證全局收斂[7][8]。針對這些問題，本文從慣性權重，學習因子以及基于自然選擇機理來改進基本粒子群算法。

4.2 標準粒子群算法改進

4.2.1 權重改進

粒子的慣性權重ω的選取十分重要，較大的ω有利于全局搜索，較小的ω有利于局部搜索和收斂，因此動態(tài)調整ω十分重要。由于搜索過程是非線性的，采用固定ω或者慣性權重線性遞減方式

ω=ωmax-(ωmax-ωmin)*(k-1)/(kmax-1)

(16)

ω隨著迭代次數增加從ωmax線性遞減至ωmin，并不能很好地反映搜索過程，不能很好地權衡全局搜索和局部搜索。所以本文使用一種非線性遞減方式

ω=ωmax*cos[π(k-1)/(2(kmax-1))]

+ωmin*{1-cos[π(k-1)/(2(kmax-1))]}

(17)

其中，ωmax和ωmin分別為選取的最大和最小慣性權重；k為當前迭代次數，kmax為最大迭代次數。ωmax取1，ωmin取0.35。

如圖3前期隨著k增大，可以看出慣性權重線性遞減方式中，ω減小得快，這樣會導致全局搜索能力下降得快，容易過早陷入局部最優(yōu)；而本文使用的慣性權重非線性遞減方式中ω減小緩慢，有利于維持全局搜索；中后期減小得較快，有利于局部搜索和收斂，提高搜索精度。

圖3 慣性權重隨迭代次數變化曲線

4.2.2 學習因子改進

學習因子C1和C2分別反映了對自我經驗學習能力和對社會的學習能力，越大反映相應的學習能力越強，學習因子也能像慣性權重一樣起到平衡局部搜索和全局搜索的作用。通常情況下，C1和C2都取一個固定值，這樣不利于迭代初期和后期的搜索能力的權衡。在迭代初期，C1較大，C2較小，能更好地進行全局搜索；迭代后期C1較小，C2較大，有利于局部搜索和收斂。因此本文設置學習因子如下

C1=C1i-(C1i-C1f)[1-cos(π(k-1)/(2(kmax-1)))]

(18)

C2=C2i-(C2i-C2f)[1-cos(π(k-1)/(2(kmax-1)))]

(19)

C1i和C1f分別為C1的初始值和終值，C2i和C2f分別為C2的初始值和終值；k為當前迭代次數，kmax為最大迭代次數。

4.2.3 基于自然選擇改進

借鑒達爾文自然學說中優(yōu)勝劣汰，適者生存這一原理，淘汰一部分位置不是太好的粒子，以達到提高算法的收斂速度和尋優(yōu)能力的目的。在算法每一次迭代后，將每一個粒子按照其適應值大小從左到右進行升序排列，用排序后靠近左端的1/N適應值小的粒子的速度和位置去替換對應的靠近右端的1/N適應值大的粒子的速度和位置，同時保留粒子所記憶的歷史最優(yōu)適應值。為了避免陷入局部最優(yōu)，本文只把百分之十最靠左的粒子的速度和位置去替換百分之十最靠右的粒子的速度和位置。

4.3 改進粒子群算法優(yōu)化LQR參數

(20)

Q=diag(q11，q22，q33，q44，q55，q66)，R=[q77]

ω1=0.4，ω2=0.6，tf為仿真時間，選取一個合適的值以確保倒立擺動態(tài)響應曲線已經進入了穩(wěn)態(tài)，本文設置為50s。

改進粒子群算法優(yōu)化LQR參數步驟：

Step1：初始化粒子群的粒子個數，慣性權重變化范圍，學習因子變化范圍，粒子速度范圍限制，粒子每一維位置限制，迭代步數等參數。隨機初始粒子位置和速度，計算粒子個體適應值和全局最優(yōu)個體適應值。

Step2：開始迭代，由式(17)(18)(19)計算粒子慣性權重和學習因子。

Step3：由式(14)(15)更新粒子速度和位置，計算個體適應值，更新粒子個體歷史最優(yōu)適應值和最優(yōu)位置，全局最優(yōu)個體適應值和位置。

Step4：將粒子按照適應值從小到大排序，進行自然選擇操作。

Step5：判斷是否達到最大迭代次數，未達到最大次數返回到Step2，繼續(xù)循環(huán)迭代。達到最大迭代次數則跳出循環(huán)。

Step6：輸出適應值函數的全局最優(yōu)適應值和最優(yōu)位置，輸出相應的LQR最佳參數。

上述改進粒子群算法優(yōu)化LQR參數流程圖如圖4。

圖4 改進粒子群算法優(yōu)化LQR參數的過程

5 仿真結果與分析

5.1 模型搭建與算法參數設置

如圖5是根據直線二級倒立擺狀態(tài)方程以及適應值函數搭建的直線二級倒立擺simulink仿真圖。

本文分別采用慣性權重固定的基本粒子群算法，慣性權重線性遞減粒子群算法，遺傳算法(GA)，以及提出的改進粒子群算法對LQR進行參數優(yōu)化。

基本粒子群算法的慣性權重ω設置為1，粒子個數為60個，學習因子均設為2，迭代最大次數為150次。

慣性權重線性遞減粒子群算法的權重初始值和終值分別為1，0.35，粒子個數為60個，學習因子均設為2，迭代最大次數為150次。

遺傳算法種群大小設置為60，進化次數為150次，交叉概率0.8，變異概率0.05。

本文提出的改進粒子群算法的權重初始值和終值分別為1，0.35，粒子個數為60個，學習因子C1初始值C1i設置為2，終值C1f設置為1.5；C2初始值C2i設置為1.5，C2f設置為2，迭代最大次數為150次。

粒子的位置下限都為[1，1，1，1，1，1，1]，上限為[1500，1500，1500，1500，1500，1500，10]。

圖5 simulink直線二級倒立擺模型

5.2 仿真結果與分析

如圖6(a)所示，為本文改進的粒子群算法和其它三種算法適應值變化曲線，基本粒子群算法在迭代120次以上開始收斂，慣性權重線性遞減的粒子群算法在迭代85次開始收斂，改進粒子群算法在迭代少于65次就開始收斂，改進粒子群算法收斂速度明顯快于另外兩種粒子群算法。雖然遺傳算法在一開始迭代就搜索到最優(yōu)解附近，然后開始收斂進行局部精細尋優(yōu)，但是從圖6(b)可以看出本文提出的改進的粒子算法尋優(yōu)精度高于遺傳算法。說明改進的粒子群算法收斂速度快，尋優(yōu)精度高。

四種算法在迭代150次后，各自輸出的反饋增益矩陣K如表1所示：

表1 人工試湊和四種算法獲取的反饋增益矩陣K

為了仿真繪圖方便，本文把慣性權重線性遞減PSO簡寫成wLDPSO。

實驗1：倒立擺初始狀態(tài)為[0，0，0，0，0，0]T，t=0s時刻，施加一個幅值為6的階躍信號，模擬倒立擺中的小車受到外界一個持續(xù)大小為6N的恒力時的穩(wěn)定控制。人工試湊和采用四種不同算法的直線二級倒立擺的擺桿1和擺桿2的角度響應曲線分別如圖7(a)、7(b)所示。

由此可以看出五種控制器都能實現倒立擺的穩(wěn)擺控制，本文改進的粒子群算法相對于人工試湊和其它三種算法，擺桿1和擺桿2的角度響應具有超調量小，響應快的特點。

由此可以看出五種控制器都能實現倒立擺的穩(wěn)擺控制，本文改進的粒子群算法相對于人工試湊和其它三種算法，擺桿1和擺桿2的角度響應具有超調量小，響應快的特點。

實驗2：倒立擺初始狀態(tài)為[0，0，0，0，0，0]T，2s時刻施加一個寬為 0.1 s，幅值為6的脈沖干擾脈沖信號，模擬小車受到一個持續(xù)時間為0.1s，大小為6N的外力擾動時的穩(wěn)定控制。擺桿1和擺桿2的角度響應曲線分別如圖8(a)、8(b)所示?？梢钥闯?，改進粒子算法優(yōu)化后的控制器使得系統(tǒng)抗干擾能力強，反應速度快，擺桿角度響應的超調量更小。

實驗3：無擾動情況下，倒立擺初始狀態(tài)為[0，0，0，0，0，0]T，期望狀態(tài)為[0.1，0，0，0，0，0]T。如圖9(a)、圖9(b)，經過改進的粒子群算法優(yōu)化后，擺桿1，擺桿2的角度響應超調量最小，恢復到平衡位置快。

實驗4：在倒立擺初始狀態(tài)為[0，0，0，0，0，0]T時，加入一個寬為 0.1 s，幅值為0.1 rad 的脈沖干擾，模擬擺桿2受到擾動。經過改進粒子群算法優(yōu)化后，在擺桿受到擾動情況下，擺桿1、擺桿2的角度響應超調量均比人工試湊，基本粒子算法以及慣性權重線性遞減粒子群算法的要?。蝗斯ぴ嚋?，擺桿恢復到平衡位置時間更短，但是超調量很大，幾乎不滿足前面討論的擺角要小這個條件；比遺傳算法優(yōu)化后的超調量略小，均能很快恢復到平衡位置，有很強抗干擾能力。

6 結論

本文利用LQR來實現直線二級倒立擺的穩(wěn)定控制，但是直線二級倒立擺狀態(tài)變量較多，傳統(tǒng)人工試湊得到LQR參數的方法過于繁瑣且具有人為意義上的最優(yōu)，以及基本粒子群算法存在收斂速度慢，容易陷入局部最優(yōu)等缺點。針對這些問題，從慣性權重非線性動態(tài)調整，學習因子非線性動態(tài)調整以及基于自然選擇機理三個角度來改進粒子群算法，來優(yōu)化LQR參數，提升穩(wěn)定控制效果。在算法迭代尋找適應值函數最優(yōu)值方面，改進粒子群算法與基本粒子群算法、慣性權重線性遞減粒子群算法以及遺傳算法相比，具有收斂快，尋優(yōu)精度高的優(yōu)點。仿真結果表明，改進粒子群算法優(yōu)化后的LQR對倒立擺的穩(wěn)定控制效果與人工試湊以及其它算法優(yōu)化后的控制效果相比，擺桿角度響應具有更小的超調量，響應快，抗干擾能力強。