擴(kuò)張狀態(tài)觀測(cè)器下的FFSR末端軌跡跟蹤控制

陳志華，胡安正

（1.襄陽(yáng)汽車職業(yè)技術(shù)學(xué)院，湖北 襄陽(yáng) 441021；2.湖北文理學(xué)院物理與電子工程學(xué)院，湖北 襄陽(yáng) 441053）

1 引言

空間機(jī)器人，作為一種智能操作系統(tǒng)，能夠高效地執(zhí)行航天探測(cè)、在軌維修、空間建造等諸多任務(wù)，從而把航天員從復(fù)雜的太空環(huán)境中解放出來(lái)，近年來(lái)發(fā)展迅速且得到了航天人員以及控制領(lǐng)域?qū)＜覍W(xué)者的廣泛關(guān)注[1?3]。根據(jù)基座的控制方式的不同，空間機(jī)器人的控制可分為基座位置受控、基座姿態(tài)受控、基座位姿受控和自由漂浮四種模式[4]。根據(jù)控制空間的不同，空間機(jī)器人的控制又可分為關(guān)節(jié)空間控制即通過(guò)對(duì)關(guān)節(jié)角的控制實(shí)現(xiàn)軌跡跟蹤，以及任務(wù)空間控制即直接對(duì)機(jī)械臂末端控制以實(shí)現(xiàn)機(jī)械臂抓手的軌跡跟蹤[5]。自由漂浮空間機(jī)器人（Free Floating Space Robot，F(xiàn)FSR）任務(wù)空間軌跡跟蹤控制由于具備無(wú)需額外負(fù)載燃料、姿態(tài)靈活可調(diào)整、直接控制末端更符合實(shí)際工程需求等優(yōu)點(diǎn)，逐步發(fā)展成為空間機(jī)器人控制領(lǐng)域研究重點(diǎn)。文獻(xiàn)[6]針對(duì)模型參數(shù)不精確條件下的FFSR任務(wù)空間軌跡跟蹤控制問(wèn)題，提出了一種逆鏈逼近的自適應(yīng)控制方法，實(shí)現(xiàn)了機(jī)械臂末端軌跡的精確跟蹤控制；文獻(xiàn)[7]考慮FFSR的模型誤差，提出一種基于SDRE的末端軌跡跟蹤控制方法，在實(shí)現(xiàn)了末端軌跡精確跟蹤的同時(shí)考慮了輸出力矩的優(yōu)化；文獻(xiàn)[8]利用神經(jīng)網(wǎng)絡(luò)的逼近特性和自適應(yīng)控制器的學(xué)習(xí)特性，提出了一種自適應(yīng)神經(jīng)網(wǎng)絡(luò)控制方法，實(shí)現(xiàn)了FF?SR的末端軌跡跟蹤；文獻(xiàn)[9]基于拉蓋爾模型提出了一種離散模型預(yù)測(cè)控制方法，實(shí)現(xiàn)了FFSR末端的位置和速度跟蹤。

在上述文獻(xiàn)的研究中，均較好的實(shí)現(xiàn)了FFSR的任務(wù)空間軌跡跟蹤控制，且考慮了模型參數(shù)以及外部干擾等復(fù)雜因素，但在控制系統(tǒng)設(shè)計(jì)上均需實(shí)時(shí)測(cè)量關(guān)節(jié)角和末端的位置信息以及速度信息，而空間機(jī)器人多處于低速運(yùn)動(dòng)狀態(tài)、空間環(huán)境噪聲大，速度測(cè)量易出現(xiàn)較大誤差，且測(cè)速系統(tǒng)將增加系統(tǒng)質(zhì)量和運(yùn)行成本，引入觀測(cè)器是一種較好解決方案。

文獻(xiàn)[10]針對(duì)控制力矩受限條件下的空間機(jī)械臂軌跡跟蹤控制問(wèn)題，采用速度濾波器對(duì)速度信號(hào)進(jìn)行估計(jì)，提出了一種自適應(yīng)控制方法；文獻(xiàn)[11]采用神經(jīng)網(wǎng)絡(luò)觀測(cè)器對(duì)系統(tǒng)速度信息進(jìn)行估計(jì)，提出了一種神經(jīng)網(wǎng)絡(luò)自適應(yīng)魯棒控制方法；文獻(xiàn)[12]基于自抗擾技術(shù)，設(shè)計(jì)了擴(kuò)張狀態(tài)觀測(cè)器，并結(jié)合模糊控制實(shí)現(xiàn)了機(jī)械臂的軌跡跟蹤控制。文獻(xiàn)[10?12]雖然解決了測(cè)速問(wèn)題，但其主要針對(duì)空間機(jī)器人關(guān)節(jié)空間軌跡跟蹤控制，而對(duì)于FFSR系統(tǒng)，由于基座位置易受反作用力運(yùn)動(dòng)，因此即便實(shí)現(xiàn)了關(guān)節(jié)角的精確跟蹤，仍會(huì)由于基座位姿的改變使末端軌跡跟蹤產(chǎn)生誤差。

綜合考慮上述問(wèn)題，針對(duì)存在模型參數(shù)不精確以及外部擾動(dòng)等不確定性條件下的FFSR末端軌跡跟蹤控制問(wèn)題，提出一種基于擴(kuò)張狀態(tài)觀測(cè)器的優(yōu)化控制方法。

首先，在建立FFSR 關(guān)節(jié)空間動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)模型的基礎(chǔ)上，采用增廣向量法獲得FFSR的偽線性狀態(tài)空間方程；而后，設(shè)計(jì)擴(kuò)張觀測(cè)器對(duì)狀態(tài)變量以及系統(tǒng)不確定項(xiàng)進(jìn)行在線估計(jì)，并采用Lyapunov方法對(duì)所設(shè)計(jì)觀測(cè)器的穩(wěn)定性進(jìn)行分析；進(jìn)而，將觀測(cè)器的輸出作為控制器的輸入，提出一種基于SDRE的優(yōu)化控制方法，實(shí)現(xiàn)FFSR系統(tǒng)末端軌跡的精確跟蹤控制。數(shù)值仿真證明了所提控制方法的有效性。

2 FFSR系統(tǒng)建模

不失一般性，單臂空間機(jī)器人通常由基座B0以及搭載在其上的若干連桿機(jī)械臂Bn以及末端抓手組成H0，機(jī)械臂與基座有且僅有一個(gè)連接點(diǎn)J1，如圖1所示。

圖1 單臂空間機(jī)器人系統(tǒng)結(jié)構(gòu)圖Fig.1 FFSR Structure Diagram

3 控制系統(tǒng)設(shè)計(jì)

在不測(cè)量角速度以及位置速度信息條件下，為實(shí)現(xiàn)FFSR系統(tǒng)末端軌跡精確跟蹤控制，設(shè)計(jì)控制系統(tǒng)結(jié)構(gòu)，如圖2所示。

圖2 控制系統(tǒng)結(jié)構(gòu)Fig.2 Control System Structure

從圖中可以看出控制系統(tǒng)設(shè)計(jì)的基本原理為：首先以角度和末端位置信息為輸入，設(shè)計(jì)擴(kuò)張狀態(tài)觀測(cè)器，對(duì)狀態(tài)變量以及不確定項(xiàng)進(jìn)行在線估計(jì)；進(jìn)而，將觀測(cè)器的輸出作為控制器的輸入，提出一種基于SDRE的優(yōu)化控制方法，并將觀測(cè)器獲得的干擾觀測(cè)量反饋至控制器中，獲得最終控制律。

3.1 擴(kuò)張狀態(tài)觀測(cè)器設(shè)計(jì)

為便于狀態(tài)觀測(cè)器的設(shè)計(jì)，需將式（5）所示的關(guān)節(jié)空間模型轉(zhuǎn)變?yōu)闋顟B(tài)空間模型。

根據(jù)式（4）可得：

定義增廣狀態(tài)變量x=[x1x2]T=[q?m XE]T，輸出變量y=XE，由式（5）、式（6）可得：

式中：x?1、x?2、f?——狀態(tài)觀測(cè)以及誤差觀測(cè)向量；α1、α2、α3—正實(shí)數(shù)；ε>0；且多項(xiàng)式s3+α1s2+α2s+α3滿足Hurwitz條件。

3.2 擴(kuò)張狀態(tài)觀測(cè)器穩(wěn)定性分析

為證明所設(shè)計(jì)的擴(kuò)張狀態(tài)觀測(cè)器式（8）能夠?qū)崿F(xiàn)對(duì)狀態(tài)變量以及誤差變量的在線估計(jì)，即：當(dāng)t→∞時(shí)，x?1→x1、x?2→x2、f?→f。定義：

3.3 控制器設(shè)計(jì)

擴(kuò)張狀態(tài)觀測(cè)器重點(diǎn)解決的是控制系統(tǒng)狀態(tài)變量以及擾動(dòng)變量的在線觀測(cè)問(wèn)題，為實(shí)現(xiàn)FFSR系統(tǒng)末端的軌跡跟蹤控制，進(jìn)一步針對(duì)式（7）所示的增廣狀態(tài)空間模型，將觀測(cè)器的輸出作為控制器的輸入，提出一種基于SDRE的優(yōu)化控制方法。

首先，不考慮系統(tǒng)模型不精確以及外部擾動(dòng)帶來(lái)的不確定性，則式（7）轉(zhuǎn)化為：

定義FFSR系統(tǒng)期望末端軌跡為yd(t)，實(shí)際軌跡為y(t)，軌跡跟蹤誤差為e=yd(t)?y(t)，則按SDRE 控制原理[15]，針對(duì)形式如式（15）所示的狀態(tài)空間模型，進(jìn)一步結(jié)合狀態(tài)觀測(cè)器的觀測(cè)結(jié)果，可設(shè)計(jì)控制器以及性能評(píng)價(jià)指標(biāo)分別如下所示：

4 仿真實(shí)例

為驗(yàn)證所提控制方法的有效性，以文獻(xiàn)[7]中的兩連桿FFSR為例，進(jìn)行Matlab∕Simulink數(shù)值仿真，空間機(jī)器人各項(xiàng)參數(shù)，如表1所示。

表1 系統(tǒng)參考慣性參數(shù)Tab.1 Inertial Parameters of System

表2 系統(tǒng)真實(shí)慣性參數(shù)Tab.2 Inertial Parameters of System

外部干擾為：τd=0.2+e+1.5e?（單位：N ?m，e=yd(t)?y(t)為跟蹤誤差）。設(shè)置仿真時(shí)長(zhǎng)t=20.0s，仿真結(jié)果，如圖3～圖7所示。其中。FFSR系統(tǒng)末端軌跡跟蹤情況。直接反應(yīng)了所設(shè)計(jì)的控制器的控制效果，如圖3所示。FFSR機(jī)械臂兩個(gè)關(guān)節(jié)的輸出力矩，反應(yīng)了控制器輸出力矩優(yōu)化情況，如圖4所示。擴(kuò)張狀態(tài)觀測(cè)器分別對(duì)狀態(tài)變量x1、x2以及系統(tǒng)不確定項(xiàng)f的觀測(cè)范數(shù)值與實(shí)際范數(shù)值的跟蹤情況，直接反應(yīng)了擴(kuò)張狀態(tài)觀測(cè)器的穩(wěn)定性以及有效性，如圖5～圖7所示。

圖3 末端軌跡跟蹤情況Fig.3 End Track Tracking

在所設(shè)計(jì)的控制器作用下，F(xiàn)FSR機(jī)械臂末端軌跡跟蹤情況，如圖3所示為。其中，機(jī)械臂末端軌跡的x、y方向分量跟情況，如圖3（a）、圖3（b）所示。從仿真結(jié)果可以看出，期望軌跡xd為正弦運(yùn)動(dòng)函數(shù)、yd為余弦運(yùn)動(dòng)函數(shù)，在控制器作用下，實(shí)際運(yùn)動(dòng)分量與期望分量基本保持一致，跟蹤誤差均控制在0.01以內(nèi)；機(jī)械臂末端在慣性坐標(biāo)系下運(yùn)動(dòng)軌跡的直接反映，從仿真結(jié)果可以看出，期望運(yùn)動(dòng)軌跡為一橢圓，在控制器作用下，末端抓手迅速?gòu)某跏架壽E點(diǎn)運(yùn)動(dòng)到指定點(diǎn)，且后續(xù)始終沿期望軌跡運(yùn)動(dòng)，如圖3（c）所示?？刂破鬏敵隽兀鐖D4所示。從仿真結(jié)果可以看出，不僅考慮了控制器控制效果，即使機(jī)械臂末端實(shí)現(xiàn)精確軌跡跟蹤，且同時(shí)考慮了控制器輸出力矩的大小，所設(shè)計(jì)的控制器除初始力矩稍大外，整個(gè)控制過(guò)程輸出力矩均較小，實(shí)現(xiàn)了能量的優(yōu)化。

圖4 關(guān)節(jié)控制力矩Fig.4 Joint Control Torque

設(shè)計(jì)擴(kuò)張狀態(tài)觀測(cè)器的觀測(cè)情況，如圖5～圖7所示。

圖5 狀態(tài)變量x1觀測(cè)情況Fig.5 Observation of State Variable x1

圖6 狀態(tài)變量x2觀測(cè)情況Fig.6 Observation of State Variable x2

圖7 不確定項(xiàng)觀測(cè)情況Fig.7 Observation of Uncertainty

從仿真結(jié)果可以看出，所設(shè)計(jì)的擴(kuò)張狀態(tài)觀測(cè)器能夠較好的對(duì)系統(tǒng)狀態(tài)變量以及系統(tǒng)不確定項(xiàng)的在線觀測(cè)，即實(shí)現(xiàn)x?1→x1、x?2→x2、f?→f。

5 結(jié)論

傳統(tǒng)的空間機(jī)器人控制方法往往需實(shí)時(shí)測(cè)量關(guān)節(jié)角速度和末端的位置信息，而空間機(jī)器人多處于低速運(yùn)動(dòng)狀態(tài)、空間環(huán)境噪聲大，速度測(cè)量易出現(xiàn)較大誤差，且測(cè)速系統(tǒng)將增加系統(tǒng)質(zhì)量和運(yùn)行成本。重點(diǎn)針對(duì)這一問(wèn)題，同時(shí)考慮輸出力矩的優(yōu)化，提出了一種基于擴(kuò)張狀態(tài)觀測(cè)器的優(yōu)化控制方法，實(shí)現(xiàn)了末端抓手的精確軌跡跟蹤控制。所提方法在以下幾個(gè)方面具有一定優(yōu)越性：（1）實(shí)現(xiàn)了對(duì)FFSR系統(tǒng)末端抓手的直接控制，較傳統(tǒng)關(guān)節(jié)空間控制更具工程應(yīng)用價(jià)值；（2）通過(guò)設(shè)計(jì)擴(kuò)張狀態(tài)觀測(cè)器實(shí)現(xiàn)了對(duì)擴(kuò)張狀態(tài)空間方程的狀態(tài)變量以及系統(tǒng)不確定項(xiàng)的在線觀測(cè)，從而無(wú)需測(cè)量關(guān)節(jié)角速度以及末端位置信息；（3）基于SDRE控制原理提出的優(yōu)化控制方法實(shí)現(xiàn)了能量的優(yōu)化，控制力矩保持在較小范圍。