自由漂浮雙臂空間機器人末端軌跡跟蹤控制

王 琪，孔德彭

（1.浙江省機電技師學院電氣工程系，浙江 義烏 322000；2.浙江工業大學機器人教育研究中心，浙江 杭州 310000）

1 引言

空間機器人可以代替人類更安全、更精確、更便捷地完成空天科研實驗和生產作業，得到廣泛的研究并已大量應用于太空在軌服務任務[1?4]。由于太空環境中機器人的基座與機械臂之間運動相互干擾，導致其運動控制問題十分復雜且種類各異[5?8]。其中，根據機器人基座受控情況，可區分為自由漂浮、姿態受控、位置受控和基座固定四種狀態下的運動控制；根據機械臂的數量區分，可分為單臂和多臂空間機器人運動控制；根據研究的空間不同，又可分為關節空間和慣性空間運動控制。

綜合考慮燃料損耗、系統穩定性、執行任務可靠性等實際工程因素，自由漂浮狀態下多臂空間機器人慣性空間末端的運動控制問題更具研究價值。文獻[9?10]結合增廣變量思想，分別提出一種魯棒控制和自適應控制方法，實現了自由漂浮雙臂空間機器人（Dual?Arm Free Floating Space Robot，DFFSR）末端抓手跟蹤期望軌跡運動控制，但均需要實時測量基座角速度和角加速度信息；文獻[11]針對慣性參數未知DFFSR慣性空間運動控制問題，設計了一種基于速度濾波器的魯棒控制方法，實現了末端軌跡精確跟蹤，但該控制方法未考慮外界擾動；文獻[12]針對具有外部擾動和不確定載荷情況下的DFFSR末端運動控制問題，設計了一種擬增廣魯棒與自適應混合控制方法，實現了末端軌跡精確跟蹤且大大減少了計算量。上述文獻較好的解決了DFFSR動力學方程慣性參數呈非線性關系的問題，但均未考慮能量優化問題。

針對DFFSR系統機械臂末端軌跡跟蹤控制問題，提出了一種增廣優化控制方法。首先，基于DFFSR關節空間的動力學方程和運動學方程，推導出其機械臂慣性運動空間動力學方程；而后，基于增廣變量思想，將動力學方程擴展成以末端位置和線速度為狀態變量的偽線性狀態方程，獲取系統SDC（State?depen?dent Coefficient）矩陣，以此提出一種增廣SDRE優化控制方法，并給出能量優化指標，通過Lyapunov方法證明系統漸近穩定性；一個針對平面二連桿的DFFSR系統數值仿真驗證了所提控制方法的可行性。

2 系統建模

2.1關節空間動力學方程

DFFSR系統由基座M0及機械臂M1、M2、M3、M4組成，如圖1所示。

式中：M(q) ∈R5×5—正定?對稱質量矩陣；C(q，q?)q?h∈R5—包含哥氏力和離心力的廣義列向量，且滿足如下性質：

（1）M(q) ∈R5×5對稱正定且有界。

（2）對于任意向量Z∈R5，存在zTM?(q)z= 2zTC(q，q?)z。

2.2 系統運動學方程

式中：K∈R1×4—基座姿態角速度與關節角速度的雅克比關系向量；Jmh(q) ∈R2×5—基座線速度與姿態角及關節角速度的雅克比關系矩陣；J(q) ∈R4×7—雙臂末端抓手線速度與基座線速度、姿態以及關節角速度之間的廣義雅克比關系矩陣，其表達式為：

2.3 系統慣性空間動力學方程

3 控制器設計

3.1 預備知識

考慮仿射非線性系統：

3.2 基于增廣變量優化控制器設計

3.3 穩定性分析

4 數值仿真

以圖1所示平面二連桿DFFSR系統為對象，進行數值仿真。選定其慣性參數為：

仿真時長t= 20.0s，如圖2～圖4所示。其中，DFFSR左臂末端位置及其在x、y軸坐標分量的軌跡跟蹤效果，如圖2 所示。DFFSR右臂末端位置及其在x、y軸坐標分量的軌跡跟蹤效果，如圖3所示。DFFSR機械臂各關節控制力矩變化曲線，如圖4所示。

在圖2 和圖3 的仿真結果中，左∕右機械臂末端在慣性坐標系下x方向以及y方向分量的軌跡跟蹤情況，圖中橫坐標為仿真運行時間，縱坐標為機械臂位置的分量坐標值，如圖2（a）、圖3（a）所示。從圖中可以看出，兩個分量坐標能夠克服初始誤差迅速實現對期望軌跡的跟蹤；左∕右機械臂末端在慣性坐標系下運動軌跡的跟蹤情況，如圖2（b）、圖3（b）所示。圖中橫坐標為機械臂位置的x坐標分量，縱坐標為機械臂位置的y坐標分量，期望運動軌跡為一個圓形軌跡，從圖中可以看出，機械臂末端能夠從初始點位迅速運動到期望點位并對期望軌跡進行跟蹤；左∕右機械臂末端在慣性坐標系下x方向以及y方向的軌跡跟蹤誤差，如圖2（c）圖3（c）所示。圖中橫坐標為仿真運行時間，縱坐標為期望軌跡與實際軌跡的跟蹤誤差值，從圖中可以看出，x方向和y方向的軌跡跟蹤誤差均保持在一較小范圍內，進一步證明了控制器的有效性。

在圖4的仿真結果中，左∕右機械臂各關節鉸的輸出力矩，如圖4（a）、圖4（b）所示。圖中橫坐標為仿真運行時間，縱坐標為關節鉸輸出力矩值，從圖中可以看出，左∕右機械臂的兩個關節鉸出初始輸出力矩較大外，后續持續輸出力矩均很小，在一定程度上實現了輸出力矩的優化，滿足實際工程需求。

綜合分析仿真結果，圖2、圖3驗證了所設計控制器能實現機械臂末端對期望軌跡的快速跟蹤，且誤差保持在合理范圍內；圖4表明所提優化控制方法在保持跟蹤精度的同時對輸出力矩進行了能量優化，除存在初始沖擊力矩外，后期控制輸出力矩值均很小。

圖2 左臂末端位置跟蹤效果Fig.2 The End Position Tracking Effect of Left Arm

圖3 右臂末端跟蹤效果Fig.3 The End Position Tracking Effect of Right Arm

圖4 各關節控制力矩Fig.4 The Control Torque of Each Joint

5 結論

根據DFFSR關節空間的動力學方程和運動學方程，推導出其機械臂慣性運動空間動力學方程，并基于增廣變量思想，將動力學方程擴展成以末端位置和線速度為狀態變量的偽線性狀態方程，以此提出一種增廣狀態依賴Riccati方程（SDRE）優化控制方法，實現了DFFSR系統末端位置的快速有效跟蹤。所提控制方法通過增廣變量實現了末端位置和速度的同步跟蹤，大大提高了跟蹤速率，且考慮了能量優化問題，使得輸出控制力矩較小。