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

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

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

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

圖1 Stewart型輪足腿機(jī)器人

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

1 Stewart型輪足腿控制原理

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

圖2 Stewart平臺(tái)控制框圖

2 Stewart平臺(tái)動(dòng)力學(xué)模型

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

首先，在慣性坐標(biāo)系N中對(duì)質(zhì)心進(jìn)行受力分析，可以得到負(fù)載質(zhì)心的牛頓運(yùn)動(dòng)方程

(1)

在連體坐標(biāo)系B中對(duì)負(fù)載進(jìn)行力矩分析可以得到如下歐拉方程[5]

(2)

圖3 Stewart平臺(tái)受力圖

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

RRT=I3×3

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

(3)

對(duì)式(2)進(jìn)行變量替換得到慣性坐標(biāo)系下的歐拉方程

(4)

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

(5)

(6)

牛頓運(yùn)動(dòng)方程可以寫為

(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ù)測(cè)控制器

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

3.1 預(yù)測(cè)模型

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

(15)

預(yù)測(cè)模型的第i步預(yù)測(cè)狀態(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ù)測(cè)模型的第i步預(yù)測(cè)輸出如下

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

(17)

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

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

(18)

式中：yr(k+i|k)為從當(dāng)前時(shí)間k起始的預(yù)測(cè)時(shí)域p內(nèi)的目標(biāo)值，是在預(yù)測(cè)時(shí)域p內(nèi)的目標(biāo)軌跡；控制時(shí)域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)為兩個(gè)時(shí)刻控制向量之差.式(18)右側(cè)的前一項(xiàng)反映了對(duì)跟蹤目標(biāo)性能的需求，后一項(xiàng)反映了對(duì)抑制控制量變化的需求.此外，通過加入松弛因子可以保證在違反約束條件情況下最小化目標(biāo)函數(shù)得到可行解，松弛因子ε>0，ρ為其權(quán)重.

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

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)為被控制對(duì)象輸出的最大、最小值；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 目標(biāo)函數(shù)最小化求解

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

x(k)TKx)Δuk+const

(23)

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

(24)

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

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

(25)

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

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

圖4 Simulink中的控制模塊圖

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

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

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

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

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

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

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

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

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

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

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

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

5 實(shí)驗(yàn)與分析

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

圖7 實(shí)驗(yàn)裝置圖

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

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

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

圖9 MPC控制器與PI控制器位移跟蹤對(duì)比

6 結(jié) 論

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