基于偽譜法的自由漂浮柔性空間機器人姿態(tài)運動規(guī)劃與反饋控制

姚其家， 戈新生

(北京信息科技大學(xué) 機電工程學(xué)院， 北京 100192)

空間機器人可以取代宇航員進行部分艙外作業(yè)，是人類進行太空活動的重要工具之一[1-2]?？臻g機器人的工作模式可以分成自由漂浮模式，自由飛行模式和基座姿態(tài)受控模式三種。其中，自由漂浮模式是空間機器人最為常用的工作模式。該模式不僅可以節(jié)省燃料，延長空間機器人的在軌使用壽命，還可以避免在捕獲過程中由于過控制引起的機械臂末端與目標之間的碰撞。對于自由漂浮空間機器人，由于機械臂與基座之間的強動力學(xué)耦合作用，可以通過機械臂的關(guān)節(jié)運動實現(xiàn)基座的姿態(tài)改變，從而使機械臂和基座同時到達期望姿態(tài)。此外，在許多實際操作中，還需要考慮機械臂柔性變形對系統(tǒng)姿態(tài)造成的影響。由于非完整約束與剛?cè)狁詈享椀拇嬖?，這類空間機器人系統(tǒng)的姿態(tài)控制問題要比一般的多體系統(tǒng)復(fù)雜很多。

國內(nèi)外學(xué)者對空間機器人的動力學(xué)與控制問題進行了大量的研究工作。運動規(guī)劃方法(包括遺傳算法、粒子群算法和雙向逼近方法等)[3-8]和主動控制方法(包括PID控制、模糊控制、模型預(yù)測控制和自適應(yīng)控制等)[9-14]都在空間機器人的姿態(tài)最優(yōu)控制和振動抑制等方面取得廣泛應(yīng)用。總體來說，對于空間機器人系統(tǒng)動力學(xué)與控制問題的研究已經(jīng)非常深入，選取的模型較為復(fù)雜，考慮的問題也相當全面。近年來，偽譜法逐漸成為航天領(lǐng)域求解最優(yōu)控制問題的一種重要方法，該方法也同樣適用于求解空間機器人系統(tǒng)的姿態(tài)控制問題。戈新生等[15]利用Legendre偽譜法研究了自由漂浮空間機器人的姿態(tài)運動規(guī)劃問題，在建模過程中將機械臂簡化為剛體，沒有考慮機械臂的柔性效應(yīng)。Liao等[16]采用Gauss偽譜法和直接打靶法相結(jié)合的混合控制策略研究了柔性空間機器人的減振運動規(guī)劃問題。需要指出的是，偽譜法從本質(zhì)上講是一種開環(huán)控制方法，參數(shù)不確定性和外界干擾都會對計算結(jié)果產(chǎn)生很大的影響。因此，本文在利用偽譜法規(guī)劃柔性空間機器人關(guān)節(jié)運動的基礎(chǔ)上，進一步設(shè)計了最優(yōu)反饋控制律對偽譜法得到的參考軌跡進行修正。

針對初始狀態(tài)不確定性對系統(tǒng)姿態(tài)造成的影響，在空間機器人系統(tǒng)動力學(xué)模型的基礎(chǔ)上，本文提出一種基于間接偽譜法的運動規(guī)劃與反饋控制相結(jié)合的姿態(tài)控制策略，通過規(guī)劃機械臂的關(guān)節(jié)運動，使自由漂浮空間機器人系統(tǒng)從初始位置運動到指定姿態(tài)，并滿足系統(tǒng)的能耗最小和控制輸入受限。在運動規(guī)劃方法的設(shè)計中，利用Legendre偽譜法將空間機械臂非完整運動規(guī)劃問題轉(zhuǎn)化為非線性規(guī)劃問題，再利用序列二次規(guī)劃(SQP)算法求解此非線性規(guī)劃問題得到系統(tǒng)的優(yōu)化運動軌跡和最優(yōu)控制輸入。在反饋控制方法的設(shè)計中，將狀態(tài)方程在已知的參考軌跡上線性化處理，利用Pontryagin極大值原理將對參考軌跡的跟蹤問題轉(zhuǎn)化為兩點邊值問題，再利用Legendre偽譜法離散此兩點邊值問題，將其轉(zhuǎn)化為線性代數(shù)方程組進行求解，該過程無需任何積分計算，具有很好的實時性。最后，文末給出仿真算例。

1 動力學(xué)建模

考慮一個作平面運動的自由漂浮柔性空間機器人系統(tǒng)如圖1所示。該系統(tǒng)由剛性基座B0和n關(guān)節(jié)柔性機械臂各連桿Bi(i=1,2,…,n)組成，其中，各分體之間通過轉(zhuǎn)動鉸Oi(i=1,2,…,n)聯(lián)結(jié)。以系統(tǒng)的總質(zhì)心Oc為原點，建立慣性坐標系Σc。以基座B0的質(zhì)心O0為原點，建立基座B0的連體坐標系Σ0。以各轉(zhuǎn)動鉸中心Oi(i=1,2,…,n)為原點，建立柔性連桿Bi的連體坐標系Σi(i=1,2,…,n)。設(shè)φ為基座B0相對慣性系的絕對姿態(tài)角，θi(i=1,2,…,n)為Oi鉸鄰接分體之間的關(guān)節(jié)轉(zhuǎn)角。mi(i=0,1,…,n)表示分體Bi的質(zhì)量。l0表示基座B0的質(zhì)心O0與轉(zhuǎn)動鉸O1之間的距離，li(i=1,2,…,n)表示柔性連桿Bi的長度。這里，忽略柔性連桿微小變形對系統(tǒng)總質(zhì)心位置造成的影響，假設(shè)系統(tǒng)總質(zhì)心保持不動。

將柔性連桿視作Euler-Bernoulli梁，采用假設(shè)模態(tài)法對柔性連桿的彈性變形進行離散，即

(1)

圖1 自由漂浮柔性空間機器人系統(tǒng)Fig.1 Free-floating flexible space robot system

忽略微弱的重力梯度影響，結(jié)合動量守恒定律和第二類Lagrange方程，得到圖1所示的自由漂浮柔性空間機器人系統(tǒng)的動力學(xué)方程為[13-14]

(2)

按照系統(tǒng)的廣義坐標對應(yīng)的廣義力是否為零，把廣義坐標η劃分為ηr=[θ1,θ2,…,θn]T和ηe=[φ,q1,q2,qn]T兩部分，將式(2)按照相應(yīng)的維數(shù)分解得到

(3)

(4)

式中：Kee為對應(yīng)于坐標ηe的廣義剛度矩陣。

在空間機器人系統(tǒng)姿態(tài)運動規(guī)劃過程中，式(3)用于系統(tǒng)運動已知的情況下求解關(guān)節(jié)力矩。式(4)具有非完整約束方程形式，一方面，可以規(guī)劃不同的機械臂關(guān)節(jié)運動來完成同樣的操作任務(wù)；另一方面，系統(tǒng)質(zhì)量矩陣的子矩陣Mre、Mee以及速度耦合項h2均與機械臂關(guān)節(jié)的運動規(guī)律有關(guān)，當關(guān)節(jié)運動規(guī)律改變時，Mre、Mee和h2也隨之改變，機械臂的彈性變形也將發(fā)生變化。

2 運動規(guī)劃方法

2.1 最優(yōu)控制問題

(5)

根據(jù)最小能量原理，選擇機械臂關(guān)節(jié)相對轉(zhuǎn)動的耗散能作為系統(tǒng)的優(yōu)化性能指標，相應(yīng)的目標函數(shù)為

(6)

邊界條件約束包括空間機器人系統(tǒng)運動過程初末端的位形約束和控制輸入邊界。

Θ(t0)=Θ0,Θ(tf)=Θf,

u(t0)=0,u(tf)=0

(7)

此外，在實際操作中還需要考慮空間機器人系統(tǒng)姿態(tài)角的范圍約束和控制輸入受限等不等式路徑約束。

|θi(t)|≤θm,

|ui(t)|≤um

(8)

式中：θm>0和um>0分別表示姿態(tài)角和控制輸入的上限。

將空間機器人系統(tǒng)非完整運動規(guī)劃問題描述成最優(yōu)控制問題，即：求解最優(yōu)控制輸入u(t)，系統(tǒng)從初始位形Θ0運動到終端位形Θf，使得性能指標(6)最小，并滿足狀態(tài)方程(5)、邊界條件約束(7)和不等式路徑約束(8)。

(9)

式中:Φ(·)和C(·)表示空間機器人系統(tǒng)的邊界條件約束和不等式路徑約束，分別對應(yīng)式(7)和式(8)。式(9)為Bolza型最優(yōu)控制問題的標準形式。

2.2 參考軌跡生成

對于式(9)所描述的最優(yōu)控制問題，采用Legendre偽譜法將其在Legendre-Gauss-Lobatto(LGL)點處離散，轉(zhuǎn)化為非線性規(guī)劃問題進行求解。Fahroo等[17]證明了Legendre偽譜法的協(xié)態(tài)變量映射定理，使用該方法求得的解滿足傳統(tǒng)間接法的一階最優(yōu)必要條件，避免了直接法的不足之處。

空間機器人系統(tǒng)最優(yōu)控制問題的時間區(qū)間為[t0,tf]，而Legendre偽譜法定義的時間區(qū)間為[-1,1]，因此對時間變量t作如下變換

(10)

最優(yōu)控制問題(9)改寫為如下形式

(11)

(12)

(13)

式中，Lagrange插值基函數(shù)Li(τ)定義為

(14)

狀態(tài)變量由插值多項式近似表示后，其導(dǎo)數(shù)可以轉(zhuǎn)化為對基函數(shù)的微分運算，即

(15)

式中，微分矩陣D∈R(K+1)×(K+1)由式(16)確定[17]

(16)

將式(15)代入狀態(tài)方程，得到狀態(tài)方程的離散表達式，

k=0,1,K

(17)

式中:為了表述方便，將X(τk)和U(τk)簡寫為Xk和Uk。

利用Gauss積分方法離散目標函數(shù)的積分項，得到目標函數(shù)的離散形式為

(18)

式中:μk(k=0,1,…,K)為Gauss-Lobatto權(quán)重，由式(19)確定

k=0,1,…,K

(19)

同理，將邊界條件約束和不等式路徑約束在LGL點處進行離散，

Φ(X0,t0,Xf,tf)=0

(20)

C(Xk,Uk,τk;t0,tf)≤0

(21)

經(jīng)過上述過程，將空間機器人系統(tǒng)最優(yōu)控制問題轉(zhuǎn)化為非線性規(guī)劃問題，即：通過求解離散控制變量U，使得性能指標(19)最小，并滿足狀態(tài)方程約束(17)、邊界條件約束(20)和不等式路徑約束(21)。對于離散得到的非線性規(guī)劃問題，本文通過MATLAB工具箱SNOPT求解，該工具箱采用SQP算法，是求解這類參數(shù)尋優(yōu)問題最有效的算法之一，具有很高的收斂效率和計算精度[18]。

這里需要指出的是，采用Legendre偽譜法求解式(11)描述的非線性規(guī)劃問題，若選取的離散節(jié)點較少，則得到的結(jié)果精度較差，達不到要求。若離散節(jié)點選取較多，則設(shè)計變量的數(shù)目十分龐大，為SQP算法選取設(shè)計變量初值的過程變得相當困難。另外，如果變量初值選取不當，往往會使問題收斂不到可行解或陷入局部最小化而得不到全局最優(yōu)解。針對這些問題，我們先選取較少的LGL節(jié)點K1，求解非線性規(guī)劃問題，得到狀態(tài)變量和控制變量的可行解，再利用更多的LGL節(jié)點K2離散原非線性規(guī)劃問題，這些節(jié)點處狀態(tài)變量和控制變量的初值通過對可行解線性插值獲得，再利用SQP算法求解精度更高的空間機器人系統(tǒng)優(yōu)化運動軌跡。該方法可以使程序更為有效地收斂。

3 反饋控制方法

3.1 軌跡跟蹤問題的轉(zhuǎn)化

考慮初始狀態(tài)不確定性對系統(tǒng)姿態(tài)造成的影響，空間機器人的實際運動軌跡與利用運動規(guī)劃方法得到的參考軌跡之間存在偏差。定義δx(t)∈Rm為狀態(tài)變量實際值x與參考值x*之間的偏差量，δu(t)∈Rn為控制變量u的修正量，將對參考軌跡的跟蹤問題轉(zhuǎn)化成一個線性時變的姿態(tài)調(diào)節(jié)問題。對狀態(tài)方程(5)線性化處理得到如下方程

(22)

式中：δx0為初始時刻實際狀態(tài)量與參考狀態(tài)量之間的差值。A(t)和B(t)分別為狀態(tài)方程相對狀態(tài)變量和控制變量的偏導(dǎo)數(shù)在參考軌跡上取值所構(gòu)成的時變矩陣，表示為

(23)

空間機器人系統(tǒng)的姿態(tài)調(diào)節(jié)問題可以描述為：確定控制輸入修正量δu(t)以及狀態(tài)偏差量δx(t)，滿足式(22)，并使得如下的二次型性能指標最小

δuT(t)R(t)δu(t)]

(24)

式中：Pf∈Rm×m表示半正定的末端狀態(tài)加權(quán)矩陣，Q(t)∈Rm×m表示半正定的狀態(tài)加權(quán)矩陣，R(t)∈Rn×n表示正定的控制加權(quán)矩陣。

根據(jù)Pontryagin極大值原理，引入系統(tǒng)的協(xié)態(tài)變量δλ(t)，構(gòu)造如下Hamilton函數(shù)

δλT(t)[A(t)δx(t)+B(t)δu(t)]

(25)

根據(jù)最優(yōu)控制的必要條件，得到如下正則方程

(26)

和邊值條件

δx(t0)=δx0,δλ(tf)=Pfδx(tf)

(27)

δu(t)=-R-1(t)BT(t)δλ(t)

(28)

可以看出，通過求解式(26)和(27)所描述的兩點邊值問題，獲得協(xié)態(tài)變量δλ(t)，即可進一步求得控制輸入修正量δu(t)。

3.2 基于間接偽譜法的最優(yōu)反饋控制律

對于式(26)和(27)所描述的兩點邊值問題，由于本文的動力學(xué)模型較為復(fù)雜，如果采用MATLAB的lqr函數(shù)等傳統(tǒng)的對Riccati方程進行積分求解的方法，一方面要消耗大量的計算時間，另一方面還可能出現(xiàn)數(shù)值不穩(wěn)定的情況。因此，本文利用間接Legendre偽譜法離散此兩點邊值問題，將其轉(zhuǎn)化為一個線性方程組進行求解。

首先，通過時域變換(10)將式(26)和(27)所描述的兩點邊值問題的時間區(qū)間轉(zhuǎn)換到[-1,1]上，表示為

(29)

同第2.2節(jié)一樣，將狀態(tài)偏差量δx(t)和協(xié)態(tài)變量δλ(t)用Lagrange插值多項式近似

(30)

(31)

對式(30)和(31)求導(dǎo)得到

(32)

(33)

式中：微分矩陣D的表達式由式(16)給出。將式(32)和(33)代入式(29)所描述的兩點邊值問題，得到其離散表達式為

k=0,1,…,K

(34)

k=0,1,…,K

(35)

式中：為了表述方便，將δx(τk)，δλ(τk)，A(τk)，B(τk)，R(τk)和Q(τk)分別簡寫為δxk，δλk，Ak，Bk，Rk和Qk。

(36)

(37)

式中，F(xiàn)、G、M、N∈Rm(K+1)×m(K+1)，具體表達式分別為[19]

(38)

(39)

(40)

(41)

式中：Em表示m×m階單位矩陣，0m表示m×m階零矩陣。這里，我們的目標是求解式(36)和(37)，并滿足兩點邊值問題的邊值條件(27)。綜合式(27)、(36)和(37)并將其寫成下列分塊的形式

(42)

式中：S1=[0m,…,0m,Pf]∈Rm×m(K+1)，S2=[0m,…,0m,-Em]∈Rm×m(K+1)。這里，將V分為V=[V0,Ve]兩部分，式(42)寫成如下形式

V0δx0+VeZe=0

(43)

Ze=-VeV0δx0=Wδx0

(44)

式中:符號“”表示MATLAB的左除運算。進一步有

(45)

式中：將矩陣[Em,W]T重新分塊為Wx∈Rm(K+1)×m和Wλ∈Rm(K+1)×m，使得

X=Wxδx0

(46)

Λ=Wλδx0

(47)

將式(47)代入式(28)，當初始時刻的狀態(tài)偏差量δx0已知時，最優(yōu)反饋控制律在離散節(jié)點處的值可以表示為

δu(τk)=-R-1(τk)BT(τk)Wλkδx0

(48)

式中：Wλk為Wλ對應(yīng)τk時刻的子矩陣。通過計算式(46)、(47)和(48)得到兩點邊值問題的離散解，在此基礎(chǔ)上，非離散節(jié)點處的控制輸入修正量的值可以通過對離散節(jié)點處控制輸入修正量插值獲得。整個算法在執(zhí)行過程中不需要任何積分計算，具有很強的實時性和在線計算能力。

另外需要指出的是，該閉環(huán)跟蹤算法本質(zhì)上講是一類最優(yōu)反饋控制，其穩(wěn)定性與轉(zhuǎn)化得到的兩點邊值問題的求解精度緊密相關(guān)[19-20]。選取更多的LGL節(jié)點離散式(29)中的狀態(tài)偏差量和協(xié)態(tài)變量，則兩點邊值問題的求解精度越高，算法的穩(wěn)定性越好。然而，隨著離散節(jié)點的增多，兩點邊值問題的離散過程和最優(yōu)反饋控制律的求解過程都將消耗更多的計算時間。因此，我們需要選取適當?shù)碾x散節(jié)點個數(shù)，來同時兼顧閉環(huán)算法的穩(wěn)定性和計算效率。

4 數(shù)值仿真

以平面運動的兩桿自由漂浮柔性空間機器人的姿態(tài)控制問題作為仿真算例。設(shè)置空間機器人的運行時間t=20 s，系統(tǒng)的質(zhì)量幾何參數(shù)，如表1所示。

表1 空間機器人系統(tǒng)質(zhì)量幾何參數(shù)Tab.1 Mass and geometry parameters of the space robot system

給定系統(tǒng)的初始位形和終端目標位形分別為

Θ0=[0,-π/3,π/2]T,Θf=[-π/6,π/3,π/6]T

即空間機器人系統(tǒng)基座B0姿態(tài)φ由初始位置順時針轉(zhuǎn)動π/6到達-π/6位置，前桿B1姿態(tài)θ1由初始位置逆時針轉(zhuǎn)動2π/3到達π/3位置，后桿B2姿態(tài)θ2由初始位置順時針轉(zhuǎn)動π/3到達π/6位置。

首先，采用運動規(guī)劃方法生成空間機器人的參考運動軌跡。仿真在MATLAB平臺上編程實現(xiàn)，求解可行解選取LGL節(jié)點個數(shù)K1=5，求解精度更高的數(shù)值解選取LGL節(jié)點個數(shù)K2=40，仿真結(jié)果如圖2～5所示，其中，曲線上的標記代表LGL節(jié)點的分布情況。圖2為空間機器人系統(tǒng)基座姿態(tài)φ以及機械臂關(guān)節(jié)角θ1和θ2的優(yōu)化運動軌跡，運動曲線光滑平穩(wěn)，滿足空間機器人系統(tǒng)初末位置的位形約束和姿態(tài)角的范圍約束。圖3為空間機器人系統(tǒng)最優(yōu)控制輸入規(guī)律，初末位置邊界值均為0，并且滿足控制輸入的范圍約束。圖4為空間機器人系統(tǒng)在運動過程中柔性機械臂中點處的一階彈性變形情況，連桿的彈性變形在合理范圍之內(nèi)，且殘余振動基本為0，符合工程實際。圖5給出了自由漂浮空間機器人系統(tǒng)在不同時刻的運動投影，圖中，三角形(Δ)標注的曲線表示機械臂末端點的運動軌跡。系統(tǒng)的總質(zhì)心始終在原點保持不動，空間機器人能夠有效地完成事先規(guī)劃好的點對點操作任務(wù)。從仿真結(jié)果可以看出，以機械臂關(guān)節(jié)角速度作為控制輸入信號，空間機器人系統(tǒng)姿態(tài)是可控的，且生成的優(yōu)化運動軌跡能夠滿足系統(tǒng)的能耗最小和控制輸入受限等要求。

圖2 空間機器人系統(tǒng)優(yōu)化運動軌跡Fig.2 Optimal trajectory of the space robot system

圖3 空間機器人系統(tǒng)最優(yōu)控制輸入Fig.3 Optimal control inputs of the space robot system

圖4 柔性機械臂連桿的彈性變形情況Fig.4 Elastic deformation of flexible manipulator links

圖5 空間機器人系統(tǒng)總體運動過程Fig.5 Whole motion process of the space robot system

另外，這里考慮初始狀態(tài)不確定性對系統(tǒng)姿態(tài)造成的影響，設(shè)定空間機器人系統(tǒng)在初始時刻存在外部擾動的范圍如下

|Δφ(0)|≤π/30,|Δθ1(0)|≤π/15,

|Δθ2(0)|≤π/20

考慮正負兩個極限情況為情況1和情況2，空間機器人的實際初始位形分別為Θ0=[π/30,-4π/15,11π/20]T(情況1)和Θ0=[-π/30,-2π/5,9π/20]T(情況2)。將圖2的結(jié)果作為空間機器人姿態(tài)運動的參考軌跡，再利用反饋控制方法分別對情況1和情況2的擾動進行反饋控制，其中，加權(quán)矩陣Pf、Q和R均設(shè)計為對角矩陣，具體元素由BRYSON法則確定[21]。

初始狀態(tài)量存在偏差情況下空間機器人系統(tǒng)的優(yōu)化運動軌跡如圖6所示，其中，實線表示不存在初始狀態(tài)量偏差情況下空間機器人系統(tǒng)的參考運動軌跡，虛線和點劃線分別表示反饋控制作用于系統(tǒng)后情況1和情況2的實際運動軌跡。這兩種情況下終端狀態(tài)量相對于參考軌跡的偏差值由表2給出?？梢钥闯?，在反饋控制律的作用下，空間機器人的實際運動軌跡逐漸偏向參考軌跡，一段時間后，實際運動軌跡和參考軌跡基本重合，終端狀態(tài)量的偏差值在10-4rad量級，滿足系統(tǒng)點對點操作的精度要求。仿真結(jié)果說明采用的反饋控制方法可以有效消除初始狀態(tài)不確定性對系統(tǒng)姿態(tài)運動造成的影響，本文提出的運動規(guī)劃與反饋控制相結(jié)合的姿態(tài)控制策略對于一定范圍內(nèi)的初始狀態(tài)量偏差具有很好的魯棒性。另外，兩種極限情況下反饋控制律的解算時間均少于反饋周期，反饋控制律能夠滿足實時性的要求。

表2 初始狀態(tài)量存在偏差情況下系統(tǒng)的終端狀態(tài)量偏差Tab.2 Terminal state deviations subject to initial state errors

圖6 初始狀態(tài)量存在偏差情況下系統(tǒng)的運動軌跡Fig.6 Trajectory of the system subject to initial state errors

5 結(jié) 論

本文研究了自由漂浮柔性空間機器人系統(tǒng)的動力學(xué)建模與姿態(tài)控制問題，提出一種基于偽譜法的運動規(guī)劃與反饋控制相結(jié)合的姿態(tài)控制策略。其中，利用偽譜法離散設(shè)計變量的思想在控制策略中兩次應(yīng)用。第一次是利用偽譜法離散狀態(tài)變量和控制變量，將空間機器人姿態(tài)優(yōu)化問題離散為非線性規(guī)劃問題進行求解。第二次是利用偽譜法離散狀態(tài)偏差量和協(xié)態(tài)變量，將軌跡跟蹤問題轉(zhuǎn)化得到的兩點邊值問題離散為線性方程組進行計算。通過數(shù)值仿真，系統(tǒng)優(yōu)化運動軌跡光滑平穩(wěn)，柔性機械臂連桿的全局振動和殘余振動在合理范圍之內(nèi)，反饋控制有效消除系統(tǒng)初始狀態(tài)量偏差對系統(tǒng)姿態(tài)造成的影響，空間機器人系統(tǒng)能夠從初始位置運動到指定姿態(tài)，并滿足系統(tǒng)的能耗最小和控制輸入受限，仿真結(jié)果驗證了該方法的有效性和魯棒性。盡管本文工作是針對作平面運動的空間機器人系統(tǒng)姿態(tài)控制問題展開的，但提出的基于間接偽譜法的姿態(tài)控制策略也適用于作空間運動的空間機器人系統(tǒng)的姿態(tài)控制以及其他非完整系統(tǒng)的優(yōu)化控制問題。