基于SDRE控制的雙臂空間機器人軌跡優化跟蹤

顧建華，孫翠玲，賁能軍，3

（1.鹽城工業職業技術學院機電工程學院，江蘇 鹽城 224000；2.鹽城師范學院信息工程學院，江蘇 鹽城 224001；3.安徽工業大學，安徽 馬鞍山 243002）

1 引言

相比于單臂空間機器人系統，雙臂空間機器人系統抓取物體載荷更大、精度更高、穩定性更好；同時，雙臂空間機器人系統的運動規劃和動力學控制過程，會由于所處的微重力環境和由其帶來的強耦合及非線性增強等問題，而變得更加復雜。這些因素使得雙臂空間機器人系統引起了國內外研究人員的廣泛關注［1-3］。

近年來，絕大多數研究主要集中在空間機器人參數未知及外界擾動的問題上，文獻［4］使用神經網絡在線建模逼近系統中的非線性模型，并通過魯棒控制器來消除逼近誤差及外界有界擾動，以解決空間機器人高精度軌跡跟蹤問題；文獻［5］針對自由漂浮空間機器人系統慣性參數不確定問題，設計了一類基于逆動力學法的自適應控制器；文獻［6］基于確定學習理論，設計了一種新的自適應神經網絡控制算法，對未知閉環系統進行了局部準確逼近；梁捷［7］設計了一種基于標稱力矩控制器附加模糊自適應補償控制器的復合控制方案，克服了系統參數未知的影響。

然而，上述研究均沒有具體給出衡量機械臂軌跡跟蹤精度及各關節輸出力矩能量的性能指標，難以對所設計控制方法的有效性進行定量分析，無法滿足實際工程應用中的控制精度要求。狀態依賴Riccati方程（State-dependent Riccati Equation，SDRE）方法是一種高效的非線性優化控制方法［8］，長期以來得到了十分重要的理論發展和實際應用［9-11］。在機械臂方面，文獻［12］設計了一種基于SDRE的控制器，首次將該方法應用于工程實踐中，并通過物理實驗驗證了該方法的可行性；文獻［13］基于SDRE理論，設計了一種機械臂點到點運動控制器，并通過神經網絡補償控制器來克服未知參數帶來的影響，以提高控制器的魯棒性。然而，上述神經網絡補償控制器降低了控制輸入的優化性能，且這些設計都僅僅針對的是地面機械臂，無法直接應用到模型更加復雜且存在動力學耦合的空間機器人中。

這里將SDRE理論應用于慣性參數未知條件下空間機械臂的軌跡跟蹤問題中，提出一種離線標稱SDRE控制器結合在線補償SDRE控制器的軌跡跟蹤控制方法。首先，建立雙臂空間機器人自由漂浮狀態下的關節空間動力學模型；而后將標稱狀態量和實際狀態量作為標稱動力學方程的反饋輸入量，借助增廣變量法，分別得到空間機器人關于關節角度和角速度的類線性標稱狀態方程和實際狀態方程；根據標稱狀態方程設計離線標稱SDRE控制器，并建立其與實際狀態方程的誤差模型，由此設計在線補償SDRE控制器補償標稱控制器的跟蹤誤差。仿真結果驗證了所設計控制器的有效性。

2 系統動力學建模

以雙臂兩連桿平面自由漂浮空間機器人系統為研究對象，如圖1所示。

圖1 雙臂空間機器人系統Fig.1 Dual-Arm Space Robot System

設該系統由一個自由漂浮基座M0及四個機械臂M1、M2、M3、M4組成，建立各分體M（ii=0，1，2，3，4）的主軸坐標系（Oi-xiy）i，并設ei(i=1，2，3，4)為xi軸的基矢量，其中：O0與M0的質心Oc0重合，O1，O2，O3，O4分別為連接M1與M0、M2與M1、M3與M0、M4與M3的轉動幅中心，xi(i=1，2，3，4)為機械臂的對稱軸。設O0在O0e0軸上與O1的距離為l0，M（ii=0，1，2，3，4）沿軸ei(i=1，2，3，4)的長度為li(i=1，2，3，4)；質心Oci在軸ei上與Oi的距離為ai(i=1，2，3，4)；各分體的質量和中心慣量張量分別為mi和Ji，M=為系統總質量，O為系統總質心。

對上述自由漂浮空間機器人系統作出如下假設：（1）雙臂空間機器人系統每個關節均受主動控制，且在同一平面內運動；（2）不考慮空間環境下的微弱重力影響，系統處于不受外力作用的自由漂浮狀態；（3）各構件均為剛性體，系統為剛性系統。由系統的動量守恒關系及Lagrange第二類方程，根據擴展機械臂方法［14］，可導出自由漂浮雙臂空間機器人系統的動力學方程為［15］：

式中：M(q)∈R4×4—系統質量矩陣；C(q，q)q∈R4×1—包含哥氏力和離心力的廣義力矢量，τ機械臂4個關節幅的控制力矩組成的4維列矢量，τ=[τ1，τ2，τ3，τ4]T，q—系統坐標組成的列矢量，q=[θ1，θ2，θ3，θ4]T。

動力學方程式（1）具備如下性質：（1）慣性矩陣M(q)∈R4×4為對稱正定矩陣。（2）通過選取合適的C(q，q)q，可以使(M-2C)為斜對稱矩陣。

3 SDRE基本理論

考慮如下形式的非線性系統：

式中：x∈Rn—系統的狀態向量；u∈Rn—輸入向量，且t∈[0，∞)，B(x)≠0，f(0)=0。則可以給出具有如下形式的無限時間性能指標：

其中Q(x)>0，R(x)>0。

引理1［8］.基于擴展線性化方法，當式（2）滿足：

（1）f(0)=0；

（2）在正常的取值范圍內，f(x)具有一階連續微分時；

則可將該狀態方程轉化為狀態依賴參數SDC（Sdate Depen?dent Coefficient）的類線性結構方程：

引理2［9］.當式（5）中的SDC矩陣A(x)和B(x)滿足如下條件：

（1）{A(x)，B(x)}逐點可控；

（2）{A(x)，Q12(x)}逐點可觀；

則上述狀態依賴Riccati 方程（SDRE）可以看成是線性二次型調節器（LQR）方法在非線性控制的推廣，針對由非線性狀態方程（2）和性能指標函數（3）描述的無限時間狀態調節問題，可設計局部漸進穩定優化控制律為：

其中P(x)滿足代數Riccati方程：

需要指出的是，SDC矩陣的選取沒有唯一的標準方法，同時可以通過調整權值矩陣Q(x)、R(x)，根據實際工程需求，對系統的跟蹤誤差和控制量的大小進行有效的折衷，這說明SDRE只是一種優化控制而并非最優控制，但這也體現了該方法的靈活性，是其它非線性方法所不具備的。

4 優化控制器設計

本節所設計的控制器由離線標稱SDRE控制器和在線補償SDRE 控制器兩部分組成。根據給定的離線固定慣性參數獲得系統標稱動力學方程，并由此選取離線SDC 矩陣，設計離線SDRE控制器；而后，將實際狀態量作為標稱動力學方程的反饋輸入量，得到實際狀態方程，建立其與標稱狀態的誤差方程，并由此設計在線補償控制器。所設計的控制器較好的解決了慣性參數未知帶來的系統模型不確定問題，并且給出了性能優化指標，是一種優化控制器。其控制系統框圖，如圖（2）所示。

圖2 控制器系統框圖Fig.2 Control System Framework

4.1 離線標稱SDRE控制器設計

式中：qc—標稱模型輸出關節角速度，令狀態量為：x1c=qc、x2c=qc，可得：

其中，

選取關節角度和角速度x=[x1c x2c]作為系統的增廣變量，則該系統的標稱狀態方程變為：

則可設計離線標稱SDRE控制律為：

式中：qd—雙臂空間機器人系統機械臂各關節在關節空間的期望運動軌跡，P1(x)滿足代數Riccati方程：

4.2 在線補償控制器設計

在慣性參數未知條件下，給出雙臂空間機器人系統的實際動力學模型：

式中：M(q)、C(q，q)—實際慣性矩陣。

根據實際模型的狀態量，設計補償控制器使得在標稱條件下滿足：

令狀態變量為x1=q、x2=，根據式（15）可得實際狀態方程：

將實際狀態式（16）與標稱模型狀態式（9）相減，可得：

選取A2(ε)、B2(ε)作為誤差模型中的SDC矩陣，選取優化指標為：

則可設計在線補償SDRE控制器：

其中P2(ε)滿足代數Riccati方程：

4.3 穩定性分析

定理1 由標稱控制器式（12）和補償控制器式（20）組成的閉環控制系統是局部漸進穩定的，即當t→∞時，e→0。

證明定義：

則由性質（1）可知空間機器人慣性矩陣M1為對稱正定矩陣，可得出{A1(x)，B1(x)}滿足逐點可控條件；同時由于SDC 矩陣A1(x)為離線可選定的標稱慣性矩陣、權值矩陣Q1為一組可調整的常值矩陣，因此總能通過選取合適的標稱慣性參數及Q1陣，使得{A1(x)，Q112}滿足逐點可觀條件。由引理2可得，當標稱控制器中的SDC矩陣滿足上述條件時，可保證標稱控制器構成的閉環控制系統的局部漸進穩定性，即當t→∞時，e1→0。將矩陣A2(ε)寫出分塊矩陣的形式：

式中：D1(ε)、D2(ε)—一個4×4的矩陣。

定義：

式中：C11(ε)—一個4×5的零陣

5 仿真實例

以這里所提自由漂浮雙臂空間機器人系統為對象，系統的標稱模型參數與真實模型參數，如表1、表2所示。

表1 標稱模型參數Tab.1 Nominal Model Parameters

表2 真實模型參數Tab.2 Actual Model Parameters

設雙臂空間機器人系統機械臂各關節在關節空間的期望運動規律為：

選取權值矩陣：

考慮系統慣性參數未知情況，使用標稱控制律式（12）聯立補償控制律式（20）對機械臂軌跡跟蹤運動情況進行仿真，整個追蹤過程耗時t=20.0s。自由漂浮雙臂空間機器人左、右機械臂各關節軌跡跟蹤情況，如圖3（a）～圖3（b）所示。對應的各關節軌跡跟蹤誤差，如圖3（c）所示。各關節的輸出力矩，如圖4（a）～圖4（b）所示。

圖3 空間機器人系統各關節軌跡跟蹤情況Fig.3 Trajectory Tracking Situation of Each Arm on Space Robot System

圖4 空間機器人系統各關節輸出力矩Fig.4 Control Torque of Each Joints on Space Robot System

圖3表明，在t=2.5s的時間內，各關節基本能夠對期望軌跡進行跟蹤，為了進一步驗證所設計的兩級SDRE控制器對慣性參數未知情況下空間機器人軌跡跟蹤的有效性，下面給出不加補償控制器的軌跡跟蹤仿真情況。由圖5可以看出，當不加SDRE補償控制器時，標稱SDRE控制器的跟蹤誤差不收斂，無法實現對軌跡的精確跟蹤。

圖5 空間機器人系統各關節軌跡跟蹤情況Fig.5 Trajectory Tracking Situation of Each Arm on Space Robot System

6 結論

這里將SDRE理論應用于慣性參數未知條件下空間機械臂的軌跡跟蹤問題中，設計了一種離線標稱SDRE控制器結合在線補償SDRE控制器的軌跡跟蹤優化控制方法，給出了衡量機械臂軌跡跟蹤精度及各關節輸出力矩能量的具體性能指標函數，所設計的優化控制器能夠保證該優化指標最小，符合實際工程需求，通過數值仿真，驗證了該方法的有效性。