基于動力學(xué)模型預(yù)測控制的Stewart型輪足腿控制方法

汪首坤， 劉大江， 郭非， 岳彬凱， 陳志華， 王軍政

(北京理工大學(xué) 自動化學(xué)院，北京 100081)

Stewart平臺是一種并聯(lián)機構(gòu)，具有精度高、剛性好的優(yōu)點.將Stewart平臺應(yīng)用于機器人，可以使其具有姿態(tài)控制精度高、承重能力強的特點.如圖1所示輪足機器人,它的每條輪足腿均包含一個倒置的Stewart平臺.改變Stewart平臺的位姿可以使輪足腿的位姿發(fā)生變化從而改變機器人的位姿.

Stewart平臺是一種包含6個作動器的并聯(lián)機構(gòu)，具有非線性和強耦合的特點.其運動控制方法可分為作動器獨立控制以及平臺整體控制.作動器獨立控制方法不需要考慮平臺整體的動力學(xué)特性，每個作動器采用相同的控制器且獨立運行.雖然控制器設(shè)計簡單易實現(xiàn)，但無法保證Stewart平臺在不同位姿條件下的控制性能.而平臺整體控制方法由于考慮了平臺動力學(xué)特性并對作動器進行整體協(xié)調(diào)控制，所以可以保證不同位姿狀態(tài)下的控制性能.基于平臺整體控制的控制器設(shè)計上有不同方法：哈爾濱工業(yè)大學(xué)的劉磊等[1]通過動力學(xué)模型建立了多變量魯棒控制器，將其用于Stewart平臺的隔振方面，可以有效抑制微擾動.北京理工大學(xué)的郝仁劍等[2]利用力矩解耦的方法構(gòu)建動力學(xué)模型，采用自適應(yīng)魯棒控制實現(xiàn)了平臺的魯棒控制；哈爾濱工業(yè)大學(xué)的郭洪波[3]提出基于期望補償?shù)姆蔷€性自適應(yīng)控制器，可以克服動力學(xué)模型的不確定性對系統(tǒng)性能的影響.

圖1 Stewart型輪足腿機器人

模型預(yù)測控制(MPC)廣泛應(yīng)用于多變量、帶約束的控制問題，它通過預(yù)測模型得到被控制對象的優(yōu)化控制量，因而在Stewart平臺這種并聯(lián)機構(gòu)的控制方法上具有研究價值.

1 Stewart型輪足腿控制原理

圖2為Stewart型輪足腿的控制框圖,通過速度位置雙閉環(huán)實現(xiàn)對位姿輸入的跟蹤.內(nèi)環(huán)速度環(huán)采用MPC控制器,外環(huán)位置環(huán)采用比例控制器+前饋控制.對于速度環(huán)，MPC控制器根據(jù)目標速度、作動器的實際速度和預(yù)測模型完成作動器速度控制.對于位置環(huán)，由3個位移量和3個姿態(tài)角構(gòu)成位姿輸入向量經(jīng)過運動學(xué)逆解得到6個作動器的目標長度.目標長度與從積分器輸出的作動器實際長度進行比較，其偏差經(jīng)過比例控制器并疊加前饋信號輸入到MPC控制器.Stewart型輪足腿的作動器速度經(jīng)過積分器輸出作動器的長度，再經(jīng)過運動學(xué)正解[4]得到Stewart型輪足腿的位姿輸出，實現(xiàn)位姿輸出對位姿輸入的跟蹤.相對于單閉環(huán)控制，采用雙閉環(huán)控制可以降低MPC控制器預(yù)測模型中的狀態(tài)向量的維數(shù)，從而降低在高采樣率實驗條件下對硬件運算、存儲能力的需求.

圖2 Stewart平臺控制框圖

2 Stewart平臺動力學(xué)模型

圖3為簡化后的負載受力示意圖(不考慮作動器質(zhì)量、慣量)，通過對負載進行力和力矩的分析可以得到平臺動力學(xué)模型.其中C為負載的質(zhì)心，其位置矢量為RC.f1～f6為6個作動器產(chǎn)生的推力,作動器受到的摩擦力相對于負載的慣性力可視為未建模參數(shù)忽略不計.ρ1～ρ6是質(zhì)心C到作動器推力作用點的徑矢(為了簡明起見，圖中只標注了f1和ρ1).連體坐標系B以質(zhì)心C為原點并跟隨負載一起運動，慣性坐標系N以基座圓心作為原點.

首先，在慣性坐標系N中對質(zhì)心進行受力分析，可以得到負載質(zhì)心的牛頓運動方程

(1)

在連體坐標系B中對負載進行力矩分析可以得到如下歐拉方程[5]

(2)

圖3 Stewart平臺受力圖

為了將在連體坐標系中的向量坐標表示為慣性坐標系中的坐標，引入旋轉(zhuǎn)矩陣R

RRT=I3×3

式中：c和s分別表示三角函數(shù)余弦和正弦；φ，θ和ψ分別為負載在慣性坐標系中的橫滾、俯仰及航向姿態(tài)角.如下關(guān)系式反映了負載角速度，徑矢以及推力向量在連體坐標系和慣性坐標系中坐標表示之間關(guān)系

(3)

對式(2)進行變量替換得到慣性坐標系下的歐拉方程

(4)

可將式(4)中的向量積?運算替換為矩陣運算[6]

(5)

(6)

牛頓運動方程可以寫為

(7)

(8)

ωN(k+1)=(I3×3-TsRJ-1RTΩNRJRT)ωN(k)+

(9)

將式(8)與式(9)合并

(10)

式中：

(11)

式(10)可化為

(12)

xp(k+1)=Ax kxp(k)+Bu ku(k)+Bvkv(k)

(13)

輸出方程可以寫為

yp(k)=xp(k)

(14)

3 模型預(yù)測控制器

Stewart平臺采用模型預(yù)測控制器(MPC控制器)進行速度閉環(huán)控制，控制流程見圖2中的速度環(huán)閉環(huán)控制部分.目標向量yr(k)是Stewart平臺作動器的目標速度，MPC控制器對平臺的狀態(tài)向量xp(k)進行估計得到控制器的狀態(tài)向量xc(k)，再利用平臺的預(yù)測模型得到Stewart平臺預(yù)測時域p內(nèi)的輸出向量y(k)，y(k+1)，……，y(k+p-1)，然后通過使優(yōu)化目標函數(shù)最小化和約束條件求解得到最優(yōu)化控制向量u(k)，最后該控制向量輸入到被控對象Stewart平臺實現(xiàn)輸出向量yp(k)對目標向量yr(k)的跟蹤.

3.1 預(yù)測模型

由已知動力學(xué)模型式(13)、(14)得到式(15)狀態(tài)觀測器，其中xc(k)從xp(k)的值估計得到；Axk、Buk及Bvk在每個控制周期更新一次，并在預(yù)測時域p內(nèi)保持不變

(15)

預(yù)測模型的第i步預(yù)測狀態(tài)如下

xc(k+i+1|k)=Ax kxc(k+i|k)+

Bu ku(k+i|k)+Bv kv(k),0≤i≤p-1

(16)

預(yù)測模型的第i步預(yù)測輸出如下

y(k+i|k)=xc(k+i|k),0≤i≤p-1

(17)

3.2 目標函數(shù)及其約束

通過設(shè)定目標函數(shù)并使其最小化從而得到最優(yōu)控制.目標函數(shù)可以寫為如下表達式[7]

(18)

式中：yr(k+i|k)為從當(dāng)前時間k起始的預(yù)測時域p內(nèi)的目標值，是在預(yù)測時域p內(nèi)的目標軌跡；控制時域m滿足m

Δuk=[Δu(k|k)TΔu(k+1|k)T…

Δu(k+m-1|k)T]T

(19)

式中Δu(k+i|k)=u(k+i|k)-u(k+i-1|k)為兩個時刻控制向量之差.式(18)右側(cè)的前一項反映了對跟蹤目標性能的需求，后一項反映了對抑制控制量變化的需求.此外，通過加入松弛因子可以保證在違反約束條件情況下最小化目標函數(shù)得到可行解，松弛因子ε>0，ρ為其權(quán)重.

在實際系統(tǒng)中，往往需要對控制量和被控制對象的輸出進行約束.其中被控對象輸出量約束為

ymin(k+i|k)-εvmin≤y(k+i|k)≤

ymax(k+i|k)+εvmax, 0≤i≤p-1

(20)

式中：ymax(k+i|k)和ymin(k+i|k)為被控制對象輸出的最大、最小值；vmax>0和vmin>0為軟化約束向量.

控制量約束為

umin(k+i|k)≤u(k+i|k)≤

umax(k+i|k), 0≤i≤m-1

(21)

式中umax(k+i|k)和umin(k+i|k)為控制量的最大、最小值.

控制量變化約束為

Δumin(k+i|k)≤Δu(k+i|k)≤

Δumax(k+i|k), 0≤i≤m-1

(22)

式中Δumax(k+i|k)和Δumin(k+i|k)為控制量變化的最大、最小值.

3.3 目標函數(shù)最小化求解

式(18)可以進一步寫成如下形式

x(k)TKx)Δuk+const

(23)

式中：KΔ u，Kyr，Kv，Ku，Kx均為常數(shù)矩陣；const為常數(shù)標量；u(k-1)為k前一時刻的控制量.J(ε,Δuk)寫為式(23)形式，最小化問題便轉(zhuǎn)化為求解帶有約束的二次規(guī)劃(quadratic programming ，QP)問題，可以計算出控制時域內(nèi)的最優(yōu)控制增量序列

(24)

將此控制序列的第一個元素與u(k-1)相加得到k時刻優(yōu)化控制量

u(k)=u(k-1)+Δu*(k|k)

(25)

4 仿真與控制器參數(shù)整定

根據(jù)圖2的控制框圖在Matlab/Simulink建立控制模塊圖，如圖4所示.式(13)(14)包含在的動力學(xué)模型模塊中，MPC控制器完成預(yù)測輸出和控制量的優(yōu)化求解.控制量輸入到Stewart平臺Adams模型，同時模型作動器的長度和速度測量值反饋回控制器及其他模塊.階躍信號用于速度環(huán)MPC控制器參數(shù)的整定；正弦信號用于位置環(huán)比例控制器參數(shù)的整定.

圖4 Simulink中的控制模塊圖

Stewart平臺的Adams模型的主要參數(shù)如表1所示,將摩擦因數(shù)和預(yù)緊力等未建模參數(shù)也考慮進去，可以評估MPC控制器的魯棒性.仿真和控制器參數(shù)整定按照先速度環(huán)后位置環(huán)的順序進行，采樣時間Ts設(shè)定為0.5 ms.

表1 Adams模型主要參數(shù)

4.1 速度環(huán)仿真及控制器參數(shù)整定

首先對速度環(huán)的MPC控制器參數(shù)進行整定，主要的整定參數(shù)為預(yù)測時域p和控制時域m.將幅值為0.15 m/s的階躍信號輸入到MPC控制器，控制量的約束范圍為±700 N，比例因子為12 000.通過比較不同p和不同m條件下的階躍響應(yīng)確定它們的優(yōu)化值.

從圖5可以得知：預(yù)測時域p的變化對階躍響應(yīng)的影響較小，而控制時域m的變化對階躍響應(yīng)的影響較大.表2總結(jié)了m變化的階躍響應(yīng).

圖5 p，m變化條件下的階躍響應(yīng)

表2 m變化的階躍響應(yīng)

對表2分析可知：隨著m的增大，階躍響應(yīng)上升時間和穩(wěn)態(tài)相對誤差均減小.考慮到增大m會導(dǎo)致計算量增大，故MPC控制器的控制時域取中間值即m=2，同時MPC控制器對未建模的摩擦力顯示出良好的魯棒性.

4.2 位置環(huán)仿真及控制器參數(shù)整定

對位置環(huán)比例控制器參數(shù)進行整定.將頻率2 Hz，幅值為0.15 rad的正弦波作為俯仰軸姿態(tài)的輸入?yún)⒖夹盘?，逐漸增大比例控制器增益且輸出不發(fā)生振蕩.不同增益下的正弦波響應(yīng)如圖6所示.

圖6 m=2,p=5的正弦波響應(yīng)

由圖6可知，當(dāng)位置環(huán)控制器增益為0.12，正弦波響應(yīng)的幅值誤差為0.000 4 rad，延時為0.5 ms，滿足姿態(tài)跟蹤精度及動態(tài)特性的要求.

5 實驗與分析

圖7為實驗裝置圖，開發(fā)計算機對控制算法編譯后下發(fā)給目標計算機.目標計算機運行控制算法并將控制量輸入到伺服驅(qū)動器控制電動缸運動，同時通過編碼器接口讀取電動缸的位置速度信息.

圖7 實驗裝置圖

首先進行姿態(tài)跟蹤實驗,選擇正弦波信號作為俯仰軸的姿態(tài)輸入.頻率分別為1,2和4 Hz，幅值為0.087 3 rad.MPC控制器和作為對比的PI控制器(參數(shù)設(shè)置:P=25，I=0.1,)實驗曲線如圖8所示.跟蹤性能如表3所示，在頻率4 Hz條件下，MPC控制器優(yōu)勢更為明顯.

圖8 MPC控制器與PI控制器姿態(tài)跟蹤對比

表3 姿態(tài)跟蹤性能對比

圖9 MPC控制器與PI控制器位移跟蹤對比

6 結(jié) 論

根據(jù)Stewart型輪足腿多變量、帶約束的控制特點，采用MPC控制器作為速度環(huán)控制器，對輪足腿進行速度位置雙閉環(huán)控制.通過Stewart平臺動力學(xué)模型建立MPC控制器的預(yù)測模型并對目標函數(shù)最小化求解得到控制量，再利用Adams 和Matlab聯(lián)合仿真對MPC控制器參數(shù)進行整定,最后通過實驗驗證了MPC控制器能夠提高Stewart型輪足腿的跟蹤精度、動態(tài)特性，其性能優(yōu)于PI控制器并對未建模的摩擦力具有良好的魯棒性.