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

王 琪，孔德彭

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

1 引言

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

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

針對(duì)DFFSR系統(tǒng)機(jī)械臂末端軌跡跟蹤控制問題，提出了一種增廣優(yōu)化控制方法。首先，基于DFFSR關(guān)節(jié)空間的動(dòng)力學(xué)方程和運(yùn)動(dòng)學(xué)方程，推導(dǎo)出其機(jī)械臂慣性運(yùn)動(dòng)空間動(dòng)力學(xué)方程；而后，基于增廣變量思想，將動(dòng)力學(xué)方程擴(kuò)展成以末端位置和線速度為狀態(tài)變量的偽線性狀態(tài)方程，獲取系統(tǒng)SDC（State?depen?dent Coefficient）矩陣，以此提出一種增廣SDRE優(yōu)化控制方法，并給出能量優(yōu)化指標(biāo)，通過Lyapunov方法證明系統(tǒng)漸近穩(wěn)定性；一個(gè)針對(duì)平面二連桿的DFFSR系統(tǒng)數(shù)值仿真驗(yàn)證了所提控制方法的可行性。

2 系統(tǒng)建模

2.1關(guān)節(jié)空間動(dòng)力學(xué)方程

DFFSR系統(tǒng)由基座M0及機(jī)械臂M1、M2、M3、M4組成，如圖1所示。

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

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

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

2.2 系統(tǒng)運(yùn)動(dòng)學(xué)方程

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

2.3 系統(tǒng)慣性空間動(dòng)力學(xué)方程

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

3.1 預(yù)備知識(shí)

考慮仿射非線性系統(tǒng)：

3.2 基于增廣變量優(yōu)化控制器設(shè)計(jì)

3.3 穩(wěn)定性分析

4 數(shù)值仿真

以圖1所示平面二連桿DFFSR系統(tǒng)為對(duì)象，進(jìn)行數(shù)值仿真。選定其慣性參數(shù)為：

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

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

在圖4的仿真結(jié)果中，左∕右機(jī)械臂各關(guān)節(jié)鉸的輸出力矩，如圖4（a）、圖4（b）所示。圖中橫坐標(biāo)為仿真運(yùn)行時(shí)間，縱坐標(biāo)為關(guān)節(jié)鉸輸出力矩值，從圖中可以看出，左∕右機(jī)械臂的兩個(gè)關(guān)節(jié)鉸出初始輸出力矩較大外，后續(xù)持續(xù)輸出力矩均很小，在一定程度上實(shí)現(xiàn)了輸出力矩的優(yōu)化，滿足實(shí)際工程需求。

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

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

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

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

5 結(jié)論

根據(jù)DFFSR關(guān)節(jié)空間的動(dòng)力學(xué)方程和運(yùn)動(dòng)學(xué)方程，推導(dǎo)出其機(jī)械臂慣性運(yùn)動(dòng)空間動(dòng)力學(xué)方程，并基于增廣變量思想，將動(dòng)力學(xué)方程擴(kuò)展成以末端位置和線速度為狀態(tài)變量的偽線性狀態(tài)方程，以此提出一種增廣狀態(tài)依賴Riccati方程（SDRE）優(yōu)化控制方法，實(shí)現(xiàn)了DFFSR系統(tǒng)末端位置的快速有效跟蹤。所提控制方法通過增廣變量實(shí)現(xiàn)了末端位置和速度的同步跟蹤，大大提高了跟蹤速率，且考慮了能量優(yōu)化問題，使得輸出控制力矩較小。