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

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

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

1 引言

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

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

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

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

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

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

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

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

(1)

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

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

(2)

(3)

(4)

對小車，擺桿1和擺桿2和質(zhì)量塊進行能量分析，得到系統(tǒng)動能和勢能，得出Lagrange算子，利用式(2)(3)(4)得到系統(tǒng)的非線性微分方程，詳細推導(dǎo)見文獻[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進行線性化處理，詳細推導(dǎo)及數(shù)據(jù)見文獻[2][3]，得到直線二級倒立擺線性處理后的狀態(tài)空間模型

(5)

(6)

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

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

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

3.1 線性二次型調(diào)節(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)控制二次型性能指標(biāo)

(9)

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

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)矩陣代數(shù)方程

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

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

圖2 倒立擺階躍響應(yīng)

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

4 LQR的優(yōu)化

4.1 基本粒子群算法原理

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

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

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

(14)

(15)

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

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

4.2 標(biāo)準(zhǔn)粒子群算法改進

4.2.1 權(quán)重改進

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

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

(16)

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

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

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

(17)

其中，ωmax和ωmin分別為選取的最大和最小慣性權(quán)重；k為當(dāng)前迭代次數(shù)，kmax為最大迭代次數(shù)。ωmax取1，ωmin取0.35。

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

圖3 慣性權(quán)重隨迭代次數(shù)變化曲線

4.2.2 學(xué)習(xí)因子改進

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

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為當(dāng)前迭代次數(shù)，kmax為最大迭代次數(shù)。

4.2.3 基于自然選擇改進

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

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

(20)

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

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

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

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

Step2：開始迭代，由式(17)(18)(19)計算粒子慣性權(quán)重和學(xué)習(xí)因子。

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

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

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

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

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

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

5 仿真結(jié)果與分析

5.1 模型搭建與算法參數(shù)設(shè)置

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

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

基本粒子群算法的慣性權(quán)重ω設(shè)置為1，粒子個數(shù)為60個，學(xué)習(xí)因子均設(shè)為2，迭代最大次數(shù)為150次。

慣性權(quán)重線性遞減粒子群算法的權(quán)重初始值和終值分別為1，0.35，粒子個數(shù)為60個，學(xué)習(xí)因子均設(shè)為2，迭代最大次數(shù)為150次。

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

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

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

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

5.2 仿真結(jié)果與分析

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

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

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

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

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

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

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

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

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

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

6 結(jié)論

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