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

姚其家， 戈新生

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

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

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

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

1 動力學建模

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

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

(1)

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

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

(2)

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

(3)

(4)

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

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

2 運動規劃方法

2.1 最優控制問題

(5)

根據最小能量原理，選擇機械臂關節相對轉動的耗散能作為系統的優化性能指標，相應的目標函數為

(6)

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

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

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

(7)

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

|θi(t)|≤θm,

|ui(t)|≤um

(8)

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

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

(9)

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

2.2 參考軌跡生成

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

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

(10)

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

(11)

(12)

(13)

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

(14)

狀態變量由插值多項式近似表示后，其導數可以轉化為對基函數的微分運算，即

(15)

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

(16)

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

k=0,1,K

(17)

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

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

(18)

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

k=0,1,…,K

(19)

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

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

(20)

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

(21)

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

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

3 反饋控制方法

3.1 軌跡跟蹤問題的轉化

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

(22)

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

(23)

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

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

(24)

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

根據Pontryagin極大值原理，引入系統的協態變量δλ(t)，構造如下Hamilton函數

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

(25)

根據最優控制的必要條件，得到如下正則方程

(26)

和邊值條件

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

(27)

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

(28)

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

3.2 基于間接偽譜法的最優反饋控制律

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

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

(29)

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

(30)

(31)

對式(30)和(31)求導得到

(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、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)，當初始時刻的狀態偏差量δx0已知時，最優反饋控制律在離散節點處的值可以表示為

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

(48)

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

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

4 數值仿真

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

表1 空間機器人系統質量幾何參數Tab.1 Mass and geometry parameters of the space robot system

給定系統的初始位形和終端目標位形分別為

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

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

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

圖2 空間機器人系統優化運動軌跡Fig.2 Optimal trajectory of the space robot system

圖3 空間機器人系統最優控制輸入Fig.3 Optimal control inputs of the space robot system

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

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

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

|Δφ(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的結果作為空間機器人姿態運動的參考軌跡，再利用反饋控制方法分別對情況1和情況2的擾動進行反饋控制，其中，加權矩陣Pf、Q和R均設計為對角矩陣，具體元素由BRYSON法則確定[21]。

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

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

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

5 結 論

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