電站載物爬樓機器人的動態穩定性建模與控制

胡安超，佃松宜，趙 濤

(四川大學電氣工程學院，成都 610065)

爬樓機器人因具有結構簡單、操作容易等優點，可作為殘疾人上下樓、貨物運輸等場景的移動平臺，因此很多學者針對其建模、控制與優化等方面進行研究。目前，人們已研制出多種類型的爬樓機器人，如輪式、履帶式、復合式[1]等。其中，履帶式機器人不易打滑、重心變化小、直線運動效果較好、適用范圍較廣[2-3]，因此，基于履帶式機器人進行研究。在上下樓過程中，由于樓梯傾斜角度及其他因素的變化，會使機器人的重心和俯仰角產生相應的改變。為了防止機器人在爬樓過程中發生傾翻、載物滑落等意外現象，須通過控制系統調節爬樓機器人的重心位置，維持其動態穩定和平衡。

針對爬樓機器人動態穩定性的控制問題，文獻[4]根據傾角變化，利用電機控制曲柄滑塊機構，進而實現對機器人傾角的控制；文獻[5]介紹了一種凸輪結構，以補償運動單元旋轉過程中對輪椅穩定性產生的波動；文獻[6]通過4個連桿機構連接至輪椅基座，利用平行四邊形特性保持基座水平；文獻[7]介紹了爬樓輪椅的臺階攀登和軸定位過程之間相互獨立的關系，簡化了同步過程，進而更好地通過連桿使輪椅保持平衡；文獻[8]提出的結構由6根連桿、絲杠和導軌組成，通過控制滾珠絲杠來調節座椅重心和相對水平軸的角度位置；文獻[9]提出關節式履帶機器人與樓梯一點、兩點和多點接觸的一種動態穩定性準則判別方法，并建立動力學方程。

為了確保爬樓機器人能安全、穩定地爬樓，關于爬樓機器人的動態穩定性問題，歸納出機器人的運動模式并建立數學模型。結合量子粒子群優化(quantum particle swarm optimization，QPSO) 算法優化的一型模糊邏輯控制器(1 type of fuzzy logic，1T1FLC)對爬樓機器人的動態穩定性進行控制，并將其與比例-積分-微分(proportion-integral-derivative, PID)、T1FLC及粒子群優化算法(particles swarm optimization, PSO)優化的T1FLC進行對比，觀察QPSO-T1FLC方法的控制效果。與現有研究方法相比，主要創新點如下：①歸納出機器人的運動模式，利用幾何關系和運動學關系構建被控對象的數學模型；②根據爬樓機器人上下兩部分的重心距離與期望距離間的誤差、誤差變化率及電動推桿伸縮量的關系，設計動態穩定性控制系統的T1FLC；③針對模糊規則參數難以確定的問題，采用QPSO優化隸屬度函數參數。

1 電站載物爬樓機器人

研究對象是針對電站工件運輸所設計的載物爬樓機器人，結構如圖1所示。該載物爬樓機器人的結構由行走單元、穩定性調節單元、運輸平臺等部件組成。行走單元負責執行上、下樓的基本動作；穩定性調節單元通過控制電動推桿的伸縮量來調節機器人的重心位置；運輸平臺用于裝載工件等。

圖1 電站載物爬樓機器人結構Fig.1 Power station load climbing robot structure

電站載物爬樓機器人的整個爬樓過程可以分為4個階段，如圖2所示。

圖2 爬樓過程Fig.2 Climbing processes

圖2中，履帶與水平軸的夾角θ1變化如式(1)～式(4)所示。

過程1：

(1)

過程2：

(L1-v1t)sinθ2=b

(2)

過程3：

(3)

過程4：

(4)

式中：θ1為長履帶部分與水平軸間夾角；θ2為運輸平臺與履帶間夾角；θ3為短履帶部分與水平軸間夾角；θ4為樓梯斜平面與水平軸間夾角；L1、L2分別為長履帶和短履帶的長度；L3為履帶前端伸出樓梯的長度；h為履帶前端至樓梯的垂直高度；a、b分別為樓梯的寬度和高度；v1為履帶前進、后退速度。

2 爬樓機器人的動態穩定性建模與分析

假設機器人在上、下樓過程中：樓梯不發生形變；兩側履帶運動狀態一致；履帶與樓梯之間沒有發生相對滑動造成機器人轉向等意外情況。

2.1 爬樓穩定性條件

假設機器人整體模型如圖3所示，在爬樓過程中履帶的中心軸線與地面夾角不斷變大，重心不斷升高和前移。

圖3 機器人成功攀爬樓梯Fig.3 Robot successfully climbs the stairs

以樓梯的尖角A點為支點，履帶翻越樓梯的條件可用式(5)描述：

(T1P1)Δt+wL*<(GP2)Δt

(5)

式(5)中：T1為履帶推力；G為機器人整體重力；P1、P2分別為T1、G對A點的力臂；w為履帶抬起的角速度；L*為轉動慣量。由式(5)可知，在爬樓過程中，機器人整體的重心G與履帶翻越支點A保持在一條垂直線上或者重心G靠后，即P2≥0時，載物爬樓機器人能夠保持動態穩定，不會發生傾翻、貨物滑落現象。

2.2 建模與分析

為了研究方便，將電站載物爬樓機器人的模型抽象為上下兩部分。上部分表示運輸平臺，依靠中間的電動推桿調節狀態；下部分表示履帶。考慮到運輸貨物置放位置，分3種情況進行建模分析。

情況1：運輸平臺的重心位于中間時，受力情況如圖4所示。機器人坐標系XOY位于機器人履帶的左下端支點，XiOiYi、XwOwYw均是履帶左上端支點的坐標系。機器人上部分質量為m1；下部分質量為m2；l1、l2分別為上下兩部分的長度；h1、h2分別為上下兩部分的高度；L、L′均為G1、G2在坐標系XOY下橫坐標之間的距離；上部分、下部分和機器人整體的重心分別位于G1(x1,y1)、G2(x2,y2)、G3(x3,y3)。

圖4 情況1Fig.4 Situation one

在坐標系XOY下：

(6)

(7)

L=x1-x2

(8)

式(8)中：L=0為期望值。

L′=x′1-x′2

(9)

ΔL=L-L′

(10)

(11)

(12)

根據式(13)、式(14)可得運輸平臺轉動的角度θ2。

(13)

(14)

式(14)為機器人整體重心G3的約束條件；Hmax、Hmin分別為重心G3高度的上下限值。

根據式(15)可得電動推桿的輸出量為

(15)

式(15)中：l4、l5和l分別表示運輸平臺旋轉支點至電動推桿首端距離、運輸平臺旋轉支點至電動推桿尾端距離、電動推桿總長度。

情況2：運輸平臺的重心位于后端時，L>0為期望值。由于情況2和情況1類似，所以不再具體分析該情況。

情況3：運輸平臺的重心位于前端時，L<0為期望值。根據分析，電動推桿需要很大的量程才能保證G1、G2共線，同時在實際過程中，該情況易發生傾翻現象，因此不考慮該情況。

根據式(6)～式(15),可得電站載物爬樓機器人的動態穩定非線性模型，如式(16)和式(17)所示。

(16)

式(16)中：

(17)

3 量子粒子群優化算法原理

3.1 標準粒子群優化算法

PSO[10-11]是一種基于群體的隨機優化技術，已廣泛應用于各種優化問題。在粒子群優化算法中，每個粒子都代表優化問題的一個潛在解。假設在D維的優化空間中，粒子群的規模為N，則在t時刻第i個粒子的位置表示為Xi(t)=[Xi,1(t),Xi,2(t),…,Xi,D(t)]，第i個粒子的速度可表示為Vi(t)=[Vi,1(t),Vi,2(t),…,Vi,D(t)]，第i個粒子當前最優位置記為Pi(t)=[Pi,1,Pi,2,…,Pi,D],種群最優位置記為G(t)=[Pg,1,Pg,2,…,Pg,D]，其中1≤g≤N。第i個粒子的最優位置可由式(18)確定，即

(18)

種群最優位置可由式(19)確定，即

(19)

標準粒子群第i個粒子的速度和位置更新可由式(20)確定，即

(20)

式(20)中：w為非負數的慣性因子；c1表示粒子飛向自身最好位置的因子；c2為粒子飛向全局最好位置的因子；r1和r2為(0,1)上均勻分布的隨機數。由于粒子在搜索最優位置時，存在飛離搜索范圍的潛在風險，所以為了降低粒子飛離空間的概率，需要對粒子的速度和位置進行限制，即

(21)

3.2 量子粒子群優化算法

針對PSO算法依賴于速度-位移軌道模型及全局尋優能力不足的缺陷，文獻[12-14]提出了QPSO算法。與標準PSO算法相比，QPSO算法內的粒子以概率密度函數來表示其真實的狀態，同時引入了粒子的平均最好位置，在整個優化空間中尋找最優解，因此QPSO算法是一種優于PSO的優化算法。

在QPSO算法中，粒子的量子態通過波函數來描述，具體粒子位置的更新方式由式(22)確定，即

(22)

式(22)中：ui,j和φj均表示為[0,1]上均勻分布的隨機數；Li,j由式(23)確定，即

Li,j(t+1)=2β|m-xi,j(t)|

(23)

式(23)中：β表示收縮-擴張系數，通常可以選取固定值或者隨著迭代次數增加呈現線性遞減的規律，β=1-0.5(iter/item)。其中：m表示粒子群個體最優位置Pi(t)的平均值，由式(24)確定；iter表示當前粒子的迭代次數，而item表示為粒子群最大迭代次數。

(24)

4 模糊控制器的設計及優化

4.1 模糊控制器設計

本節設計模糊控制器進而實現電站載物爬樓機器人系統的動態穩定性。

根據電站載物爬樓機器人的非線性模型[式(16)～式(17)]，定義輸出跟蹤誤差：

e(k)=Lr(k)-L(k)

(25)

式(25)中：Lr(k)是機器人上下兩部分重心距離差值的期望值；L(k)是系統輸出值。

模糊控制器采取if-then形式來描述非線性系統，對于多輸入單輸出(multi-input single-output, MISO)系統，電站載物爬樓機器人控制系統的模糊規則為

(26)

規則前件中e∈[-3,3]表示系統輸出值與期望值之間的偏差值，ec∈[-3,3]表示系統輸出值與期望值之間的偏差變化率；規則后件中u∈[-1.5,+1.5]表示模糊控制器的輸出，即電動推桿的伸縮速度。

電站載物爬樓機器人動態穩定性控制系統的整體原理圖如圖5所示。

圖5 動態穩定性控制系統原理圖Fig.5 Dynamic stability control system schematic

圖5中：ke與kec是量化因子，將輸入量的基本論域轉化為模糊論域；ku是比例因子，將輸出量的模糊論域轉換為基本論域；f表示外部干擾。

設定輸入前件和輸出后件的模糊子集為{負大、負中、負小、零、正小、正中、正大}，簡記為{NB,NM,NS,ZO,PS,PM,PB}。選擇三角形函數分別作為輸入、輸出的隸屬度函數，控制器模糊推理的類型為Mamdani，去模糊類型為Centroid。確定了7×7模糊規則表，具體如表1所示。

表1 系統穩定性控制模糊規則表Table 1 System stability control fuzzy rule table

4.2 模糊隸屬度函數優化

隸屬度函數參數一般根據專家知識及現場操作經驗進行選擇，過程繁瑣，因此針對一個強耦合非線性系統，所設計的控制器很難達到最佳的控制效果。為了獲取更好的控制器性能，利用QPSO算法對模糊控制器的隸屬度函數參數進行優化。

在優化參數時，適應度函數用來衡量每個個體的優良程度是QPSO算法的關鍵。種群個體進入下一代的概率由適應度的大小所決定，越優良的個體，其適應度值也越大。為了提高控制器的性能，QPSO算法的適應度函數采用時間和誤差絕對值的積分(ITAE)作為最小目標函數，即

(27)

式(27)中：λ為計算步長。

QPSO算法的優化步驟如下。

第1步：初始化粒子種群，即設置粒子群規模數為N且每個粒子有D維，令X={X1,X2,…,XD}，設置最大迭代數k、因子c1和c2。

第2步：設置收縮-擴張系數β、個體最優位置Pi(0)、種群最優位置G(0)，設置t=0。

第3步：將每個粒子代入控制器中進行控制、求解，根據式(18)計算粒子群當前平均最優位置及位置適應值。

第4步：根據式(19)計算種群最優位置，根據式(22)更新當前粒子的位置，設置t=t+1。

第5步：判斷是否達到迭代次數，若是，進行下一步；否則，返回至第3步。

第6步：輸出優化后的模糊隸屬度函數參數。

5 仿真結果與分析

針對電站載物爬樓機器人的動態穩定性模型，采用QPSO后的模糊邏輯控制器對其進行控制。為了更好地說明模型的正確性與控制器的有效性，分別對QPSO的T1FLC、PSO優化的T1FLC、未優化的T1FLC及位置式PID控制效果進行仿真對比。

位置式PID控制算法如下：

(28)

式(28)中：Kp、Ki和Kd分別代表比例、積分和微分。經過大量試湊，得出3個參數值為Kp=0.08、Ki=0.04、Kd=0.1。

機器人系統相關參數如表2所示。

表2 電站載物爬樓機器人相關參數Table 2 Power station load-climbing robot related parameters

QPSO前/后的前件、后件隸屬度函數分別如圖6(a)、圖6(b)所示。

圖6 控制器的前件、后件隸屬度函數Fig.6 Controller’s front and rear membership functions

根據式(1)～式(4)，可得到機器人履帶與水平軸夾角θ1及θ1增量變化曲線如圖7、圖8所示。

圖7 機器人履帶與水平軸夾角變化曲線Fig.7 The curve of the angle between the robot track and the horizontal axis

圖8 機器人履帶與水平軸夾角增量變化曲線Fig.8 The incremental curve of the angle between the robot track and the horizontal axis

圖7中，(a)、(b)、(c)、(d)段分別表示機器人在爬樓第1、2、3、4階段θ1角度變化的趨勢。

為了研究方便，針對時刻改變的θ1及θ1增量，選取L輸出值為0和1的方波作為2.2節中情況1和情況2的期望輸出。

關于電站爬載物樓機器人動態穩定性的控制，首先在不考慮外界干擾的條件下，對比位置式PID、T1FLC、PSO-T1FLC與QPSO-T1FLC方法的控制效果。

圖9所示為電站載物爬樓機器人在無干擾時動態穩定性控制的效果。由圖9可知，采用QPSO后的模糊控制方法響應快、超調小、穩定性好。

圖9 無干擾時動態穩定性控制Fig.9 Dynamic stability control without interference

考慮到電站載物爬樓機器人會受到外界及自身不確定性因素的影響，在t=155 s時對4種方法均增加了干擾。

圖10所示為加入干擾后4種方法的控制輸出曲線對比圖，從圖10中可知，QPSO-T1FLC能以更快的反應速度使系統達到指定位置，抗干擾性能較好。

圖10 干擾時動態穩定性控制Fig.10 Dynamic stability control with interference

6 實驗驗證及分析

為了進一步驗證所建模型的準確性及控制優化方法的有效性，進行現場實驗，如圖11所示。

根據圖11可知，在整個爬樓過程中，爬樓機器人均能很好地調節系統穩定性，保證了承載物的安全運輸，進一步說明了本文模型及控制方法的合理性。

圖11 實驗驗證Fig.11 Experimental verification

7 結論

針對電站載物爬樓機器人動態穩定性的控制問題，根據系統的特性建立非線性模型，并設計了模糊控制器。針對模糊控制器中隸屬度函數的參數難以確定的問題，采用QPSO方式得出最佳值。仿真、實驗表明，與位置式PID、T1FLC、PSO-T1FLC方法對比，QPSO-T1FLC控制方法響應速度快、超調小、抗干擾性能更好，同時表明了本文所建非線性模型的準確性和控制優化方法的有效性。