廖志祥師鵬趙育善鄭翰清

(1北京航空航天大學(xué)宇航學(xué)院,北京100191)

(2上海航天控制技術(shù)研究所,上海200233)

具有初始動量的空間機械臂零反作用軌跡跟蹤

廖志祥1師鵬1趙育善1鄭翰清2

(1北京航空航天大學(xué)宇航學(xué)院,北京100191)

(2上海航天控制技術(shù)研究所,上海200233)

對具有初始動量的自由漂浮空間機械臂系統(tǒng)進行了研究,考慮其執(zhí)行在軌任務(wù)時便已經(jīng)具有的動量,根據(jù)系統(tǒng)的構(gòu)型參數(shù)建立運動學(xué)模型,再采用拉格朗日第二類方程建立動力學(xué)模型。利用空間機械臂系統(tǒng)具有冗余自由度這一特性,推導(dǎo)運動學(xué)和動力學(xué)方程,得到了零反作用運動方程,消除了基座和機械臂之間的角速度耦合作用。末端運動軌跡用多項式插值函數(shù)來逼近,根據(jù)給定的機械臂末端初始和終止狀態(tài)來規(guī)劃其運動軌跡,并將機械臂末端的工作空間轉(zhuǎn)換到關(guān)節(jié)空間中。根據(jù)已知的動力學(xué)模型,設(shè)計合適的比例-微分(PD)控制率。仿真結(jié)果表明,通過選擇合適的增益反饋矩陣,機械臂末端能平穩(wěn)地跟蹤目標軌跡,同時,機械臂的運動不會對基座的姿態(tài)產(chǎn)生擾動,保證了基座姿態(tài)的穩(wěn)定性及航天器的正常運行。

空間機械臂;軌跡跟蹤;零反運動;比例-微分控制;多項式插值;航天器

1 引言

空間機械臂系統(tǒng)由大型航天器基座、若干機械臂桿件和末端抓手三大部分組成[1],在對空間站進行維護和抓捕目標等任務(wù)中具有無可比擬的優(yōu)越性。因此,利用空間機械臂系統(tǒng)代替航天員來完成外太空的各種任務(wù)在經(jīng)濟和安全兩方面都具有重要意義,并且這將會成為未來發(fā)展的必然趨勢[2]。

一般而言,當空間機械臂系統(tǒng)執(zhí)行在軌服務(wù)時,為節(jié)省燃料,不對基座施加控制;但是,由于外太空的特殊環(huán)境,機械臂的運動會對基座產(chǎn)生反作用力和力矩,從而改變基座的位置和姿態(tài),所以需要合理規(guī)劃機械臂的運動軌跡,保證機械臂沿此軌跡運動時可以實現(xiàn)機械臂與基座的協(xié)調(diào)運動[3]。另外,基座的姿態(tài)對于電源、通信等分系統(tǒng)有著至關(guān)重要的作用,國內(nèi)外很多學(xué)者對于如何保證基座的姿態(tài)穩(wěn)定進行了大量研究。文獻[4]研究了機械臂末端受到碰撞后,基座和機械臂的協(xié)調(diào)控制;文獻[5]提出了利用循環(huán)運動來調(diào)整基座姿態(tài)的方法,利用機械臂和基座的耦合關(guān)系,機械臂沿著特定軌跡循環(huán)運動,從而慢慢地調(diào)整基座姿態(tài);文獻[6]提出了反作用零空間概念,用于處理自由漂浮空間機械臂系統(tǒng)的基座和機械臂的動力學(xué)耦合問題。

然而,在這些研究中,機械臂系統(tǒng)在初始時刻都被假定為靜止狀態(tài),這在實際情況中基本上是不可能出現(xiàn)的。當空間機械臂系統(tǒng)被送至目標軌道并開始正常運行時,整個系統(tǒng)便已經(jīng)具有了一定量的線速度和角速度。在空間機械臂執(zhí)行在軌任務(wù)之前,不可能也完全沒有必要將其運動狀態(tài)變?yōu)殪o止。因此,實際情況就是空間機械臂系統(tǒng)的線動量和角動量始終都不為零,這也是本文研究的重點和難點。有了初始動量,在運動過程中末端執(zhí)行器的速度便不能僅用機械臂轉(zhuǎn)動的角速度表示出來,基座的廣義速度也不能僅用機械臂轉(zhuǎn)動的角速度表示,還需要考慮初始動量的影響。

2 空間機械臂運動學(xué)和動力學(xué)建模

空間機械臂系統(tǒng)如圖1所示,其中包含n個關(guān)節(jié)和n+1個剛體,為方便描述,將基座編號為0,機械臂連桿依次編號為1至n,末端用e表示,各向量和矩陣均在慣性坐標系中表示。對本文所涉及的變量和符號說明如下:CM為空間機械臂系統(tǒng)的質(zhì)心,ri為機械臂第i根連桿質(zhì)心的位置矢量, r0為基座質(zhì)心的位置矢量,re為機械臂末端的位置矢量,pi為關(guān)節(jié)i的位置矢量,rg為空間機械臂系統(tǒng)質(zhì)心的位置矢量,vi為機械臂第i根連桿質(zhì)心的速度,v0為基座質(zhì)心的速度,ωi為機械臂第i根連桿的角速度,ω0為基座的角速度,ki為第i根連桿旋轉(zhuǎn)軸方向的單位矢量,M為系統(tǒng)的總質(zhì)量,θ=[θ1, θ2,…,θn]T為機械臂各關(guān)節(jié)角變量,mi為機械臂第i根連桿的質(zhì)量,I0為基座相對于其質(zhì)心的轉(zhuǎn)動慣量,Ii為第i根連桿相對于其質(zhì)心的轉(zhuǎn)動慣量,定義矩陣~r為向量r的叉乘斜對稱矩陣[7]。

圖1 空間機械臂系統(tǒng)Fig.1 Space manipulator system

由圖1中機械臂的構(gòu)型可以很容易得到機械臂末端執(zhí)行器的廣義速度表達式:

式中 Jb是與基座相關(guān)聯(lián)的雅可比矩陣;Jm是與機械臂相關(guān)聯(lián)的雅可比矩陣,且E表示三階單位陣,其具體表達式為

系統(tǒng)的線動量P為

角動量L為

對于處于自由漂浮模式的空間機械臂系統(tǒng),可以認定其動量在運動過程中始終保持不變,若初始線動量和角動量分別為P0和L0,其動量守恒方程為

本文采用拉格朗日方程對空間機械臂系統(tǒng)進行動力學(xué)建模。首先令˙xb=v0ω[ ]0T,求得系統(tǒng)的動能(由于空間機械臂系統(tǒng)處于自由漂浮狀態(tài),可忽略系統(tǒng)所受萬有引力,因此勢能為零),將系統(tǒng)的動能代入到拉格朗日方程中,得到

式中 Hb為基座的慣量矩陣;Hbm為基座與機械臂的耦合慣量矩陣;Hm為機械臂的慣量矩陣; cb、cm為非線性項;N0為基座受到的外力F0和外力矩T0,且有N0=[F0T0]T;τm為機械臂關(guān)節(jié)的驅(qū)動力矩;Ne為機械臂末端受到的外力Fe和外力矩Te,且有Ne=[FeTe]T。

3 具有非零初始動量的空間機械臂零反作用運動

在對空間機械臂系統(tǒng)零反作用運動研究中,一般都設(shè)定系統(tǒng)的初始動量為零,當機械臂執(zhí)行任務(wù)時,只需要保持基座的角速度始終為零即可,既簡化了理論推導(dǎo)過程,也使得控制變得更加容易[8]。然而,這樣的設(shè)定與實際情況是不相符的,本文提出一種更加接近于實際情況的設(shè)定,即空間機械臂系統(tǒng)在執(zhí)行在軌任務(wù)之前,便已經(jīng)具有了非零的初始動量,因此,為保持基座的姿態(tài)運動不受機械臂運動的影響,在機械臂運動過程中,就要保證基座的角速度始終不變且為初始狀態(tài)。本文定義的零反作用運動指的是使基座姿態(tài)運動始終不受影響的機械臂運動,在末端執(zhí)行器接近目標軌跡的過程中,以及抓捕目標完成后系統(tǒng)的穩(wěn)定等方面,零反運動都有著重大作用[9]。

由式(4)中的線動量守恒方程可以解出:

將v0的表達式帶入到式(4)的角動量守恒方程中:

化簡為

其中

可以證明,矩陣Hs是非奇異的,因而解得基座角速度的表達式為

進一步求得基座質(zhì)心的線速度為

其中

那么機械臂末端執(zhí)行器的速度

其中

從式(11)與Ps、Jg_v的表達式中可以清楚看到,當初始動量不為零時,末端執(zhí)行器的速度不僅與關(guān)節(jié)角運動相關(guān),還與初始動量和雅可比矩陣相關(guān)。

為保證機械臂的運動不對基座姿態(tài)運動產(chǎn)生擾動,即基座的角速度始終保持初始值不變,機械臂轉(zhuǎn)動的角速度應(yīng)滿足

式中 ω0(t0)為基座的初始角速度。因此,空間機械臂系統(tǒng)的零反作用軌跡跟蹤方程為

當機械臂轉(zhuǎn)動的角速度滿足式(13)時,機械臂末端執(zhí)行器既能跟蹤任務(wù)空間中的期望軌跡,同時,機械臂的運動不會對基座姿態(tài)運動產(chǎn)生任何擾動,使基座始終保持期望的姿態(tài)角速度。

4 基于逆運動學(xué)的軌跡規(guī)劃

空間機械臂系統(tǒng)所處環(huán)境的特殊性決定了其運動的特殊性,可以證明其動量矩守恒方程不可積分?；谶@一特點,推導(dǎo)出其運動受非完整約束,這樣軌跡規(guī)劃問題可以簡化為:根據(jù)已知的初始和終止狀態(tài),確定一條使系統(tǒng)從初始狀態(tài)運動到終止狀態(tài)的軌跡[10-11]。由式(13)可知機械臂末端執(zhí)行器工作空間中的軌跡與構(gòu)型空間中的軌跡之間的關(guān)系,因此,只需要獲得末端位置的運動規(guī)律,并以此為約束條件,就能將末端工作空間運動規(guī)律轉(zhuǎn)化為構(gòu)型空間的運動規(guī)律。

本文所研究的重點在于機械臂末端執(zhí)行器的位置,對其姿態(tài)不加要求。機械臂末端執(zhí)行器的運動狀態(tài)是時間的函數(shù),因此,可以采用多項式插值的方法來逼近末端位置的狀態(tài)。考慮到速度和加速度的要求,并讓運動軌跡盡量平滑,不至于出現(xiàn)速度級的突變,將機械臂末端執(zhí)行器的位置狀態(tài)函數(shù)設(shè)為

5 已知模型的比例-微分(PD)控制器設(shè)計

為了節(jié)省燃料,在非必要的情況下,空間機械臂系統(tǒng)不會對基座位置進行主動控制,在抓捕目標物體之前,機械臂末端執(zhí)行器也沒有受到外力作用,所以N0=Ne=0[12]。則系統(tǒng)的動力學(xué)方程可寫成如下欠驅(qū)動形式

消去方程中¨xb變量,并將方程變形得到

在得到新的動力學(xué)方程之后,本文采用基于已知模型的PD控制方法

其中u為控制輸入,對于指定的期望u,可令

式中 KP為關(guān)節(jié)角的增益反饋矩陣;KD為關(guān)節(jié)角速度的增益反饋矩陣;r(t)為參考輸入,因此有

式中 θD為機械臂關(guān)節(jié)角的期望值,設(shè)跟蹤誤差為e=θD-θ且滿足

由式(18)和式(19)推導(dǎo)得出控制輸入量為

綜上各式可以得到基于已知模型的PD控制方案,選擇了合適的增益反饋矩陣KP和KD,機械臂末端抓手就能跟蹤期望的運動軌跡。

6 仿真分析

為驗證可行性,本文以平面三連桿自由漂浮機械臂為模型,基座以及機械臂的物理參數(shù)如表1所示。

表1 空間機械臂系統(tǒng)慣性參數(shù)Tab.1 Inertial parameters of space manipulator system

初始時刻基座質(zhì)心位置為R0=[0 0]T,姿態(tài)角為零,只有橫向速度vx和垂直于運動平面的角速度ωz,且vx=0.01 m/s,ωz=0.005 rad/s,各機械臂的初始關(guān)節(jié)角為θ0=[π/3 -2π/3 π/3]T,機械臂相對于基座為靜止狀態(tài),即˙θ=0,機械臂末端執(zhí)行器初始位置為re(t0)=[2.5 0]T。在仿真中,末端執(zhí)行器跟蹤的軌跡為圓,其軌跡方程為

假設(shè)經(jīng)過2.8 s,末端執(zhí)行器開始與期望軌跡重合。選取合適的增益反饋矩陣,保證控制的精度和誤差達到要求,仿真時間為10 s,采樣周期為1 ms。

圖2為三個關(guān)節(jié)的實際角速度與期望角速度的對比,可以看出每個關(guān)節(jié)的實際角速度與期望角速度都非常接近,并且整個運動非常平滑自然,沒有出現(xiàn)劇烈震蕩的現(xiàn)象,角速度大小也始終保持在可達空間內(nèi),沒有出現(xiàn)過大的角速度致使機械臂轉(zhuǎn)動太過劇烈,最終無法維持系統(tǒng)穩(wěn)定的現(xiàn)象。

圖3為基座的角速度變化曲線,在整個運動過程中,基座的角速度始終維持在初始值附近,且波動幅度極小,即沒有對基座姿態(tài)運動產(chǎn)生擾動。圖4為機械臂末端的實際軌跡與期望軌跡的對比,機械臂末端執(zhí)行器首先穿過目標所在的圓軌跡,此時并沒有追蹤上目標軌跡;在圓軌跡的左上方時,滿足位移、速度和加速度一致的條件,這時,末端執(zhí)行器就追蹤上了目標軌跡?？梢钥闯稣麄€運動軌跡非常平滑,末端執(zhí)行器在到達目標軌跡處后跟蹤目標軌跡做圓周運動,從之前的多項式運動過渡到圓周運動非常自然,而且,實際軌跡與期望軌跡的偏差很小,隨時間增長逐漸趨近于期望軌跡。圖5為基座質(zhì)心的運動軌跡,初始時刻基座只有x軸正向的速度,由于機械臂和基座之間的耦合作用,在機械臂運動過程中,致使基座產(chǎn)生了y軸方向上的速度,因此,基座質(zhì)心產(chǎn)生了微小的y軸方向上的位移,并且其運動軌跡呈現(xiàn)一定的周期規(guī)律,這與末端執(zhí)行器跟蹤目標軌跡進行圓周運動的周期性一致。

圖2 關(guān)節(jié)一、二、三的實際與期望角速度Fig.2 Actual and desired angular velocity of joint 1,2,and 3

圖3 基座角速度變化曲線Fig.3 Angular velocity of the base

圖4 機械臂末端運動的期望軌跡與實際軌跡Fig.4 Desired and actual paths of manipulator′s end-effector

圖5 基座質(zhì)心的運動軌跡Fig.5 Trajectory of the base′s centroid

此外,在整個理論推導(dǎo)過程中,對系統(tǒng)的模型沒有特殊要求,并沒有局限于二維空間中。在實際的編程仿真過程中,為精簡計算,令所有剛體只有z軸方向上的角速度,并且其線速度在z軸方向上為零,即在二維平面內(nèi)求解,但所有的物理量,包括速度、角速度、動量及轉(zhuǎn)動慣量等參數(shù)都是在三維空間中表示的,因此本算例同樣適用于三維空間的情況。

7 結(jié)束語

針對具有初始動量的自由漂浮空間機械臂系統(tǒng),本文提出的零反作用軌跡跟蹤算法既能使機械臂末端執(zhí)行器平穩(wěn)跟蹤目標軌跡,又能保證基座的姿態(tài)穩(wěn)定性;并且,通過簡單實用的PD控制就能實現(xiàn)其功能,為航天器的在軌任務(wù)節(jié)省了寶貴的燃料資源。在考慮了初始的動量之后,更加符合實際情況,這對航天的在軌維修服務(wù)、抓捕失效衛(wèi)星等任務(wù)具有現(xiàn)實意義。由于軌跡規(guī)劃包含了逆運動學(xué)過程,導(dǎo)致在計算過程中可能會有奇異的情況發(fā)生,因此,下一步的研究重點,就是在規(guī)劃機械臂的運動時如何避免奇異。

[1] SATOKO ABIKO,GERD HIRZINGER.Adaptive control for a torque controlled free-floating space robot with kinematic and dynamic model uncertainty[C].The 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems,St.Louis,USA,October 11-15,2009:2359-2364.

[2] 徐文福,詹文法,梁斌,等.自由漂浮空間機器人系統(tǒng)基座姿態(tài)調(diào)整路徑規(guī)劃方法的研究 [J].機器人, 2006,28(3):291-296.

XU WENFU,ZHAN WENFA,LIANG BIN,et al.Path planning for base attitude adjustment of a free-floating space robot system[J].Robot,2006,28(3):291-296.

[3] 徐栓鋒,楊保華,張篤周,等.面向非合作目標抓捕的機械臂軌跡規(guī)劃方法 [J].中國空間科學(xué)技術(shù), 2014,34(4):8-15.

XU SHUANFENG,YANG BAOHUA,ZHANG DUZHOU,et al.Trajectory planning algorithm for manipulators capturing non-cooperative objects[J].Chinese Space Science and Technology,2014,34(4): 8-15.

[4] DRAGOMIR N NENCHEV,KAZUYA YOSHIDA.Impact Analysis and post-impact motion control issues of a free-floating space robot subject to a force impulse[J].IEEE Transactions on Robotics and Automation, 1999,15(3):548-557.

[5] VAFA Z,DUBOWSKY S.On the dynamics of space manipulator using the virtual manipulator with application to path planning[J].Journal of the Astronautical Science,1990,38(4):441-472.

[6] NENCHEV D,UMETANI Y,YOSHIDA K.Analysis of a redundant free-flying spacecraft/manipulator system[J].IEEE Transactions on Robotics and Automation,1992,8(1):1-6.

[7] 王委鋒,羅建軍,馬衛(wèi)華.自由漂浮空間機器人動力學(xué)建模與仿真研究[J].科學(xué)技術(shù)與工程,2011 11(13): 3004-3008.

WANG WEIFENG,LUO JIANJUN,MA WEIHUA.Study on dynamic modeling and simulation of free-floating space robot[J].Science Technology and Engineering,2011 11(13):3004-3008.

[8] P PIERSIGILLI,I SHARF,A K MISRA.Reactionless capture of a satellite by a two degree-of-freedom manipulator[J].Acta Astronautic,2010,66(5):183-192.

[9] 徐文福.空間機器人目標捕獲的路徑規(guī)劃與實驗研究[D].哈爾濱:哈爾濱工業(yè)大學(xué),2007:60-63.

XU WENFU.Path planning and experiment study of space robot for target capturing[D].Harbin Institute of Technology,2007:60-63.

[10] IOANNIS TORTOPIDIS,EVANGELOS PAPADOULOS.On point-to-point motion planning for underactuated space manipulator systems[J].Robotics and Autonomous Systems,2007,55(7):122-131.

[11] EVANGELOS PAPADOPOULOS,IOANNIS TORTOPIDIS,KOSTAS NANOS.Smooth planning for free-floating space robots using polynomials[C].The 2005 IEEE International Conference on Robotics and Automation,Barcelona,Spain,April,2005:4272-4277.

[12] 洪昭斌.漂浮基空間機械臂沖擊動力學(xué)建模、控制[C].第32屆中國控制會議,中國,西安,2013,7月26—28日:503-506.

HONG ZHANBIN.Modeling of impact dynamics and control for coordinated motion of space manipulator[C]. The 32ndChinese Control Conference,Xi′an,China,July 26-28,2013:503-506.

Zero Reaction Motion Trajectory Tracking for Space Manipulator with Non-zero Initial Momentum

LIAO Zhixiang1SHI Peng1ZHAO Yushan1Zheng Hanqing2
(1 School of Astronautics,Beihang University,Beijing 100191)
(2 Shanghai Aerospace Control Technology Institute,Shanghai 200233)

The free-floating manipulator system with non-zero initial momentum was studied. The configuration parameters of the manipulator system and the Lagrange equations were used to build the dynamic model.In order to eliminate the coupling effects between the base and the manipulators in angular velocity,the zero reaction motion equation was derived by using the manipulator system redundancy.A polynomial interpolation method was used to approach the trajectory of end-effector and the trajectory was planned due to the initial and final position of end-effector which could turn workspace of end-effector into joints space.A suitable proportiondifferentiation(PD)control algorithm was designed to drive manipulators movement.Simulation results demonstrate that the end-effector can smoothly track the target and the movement of the manipulator doesn′t make any disturbance to the base′s attitude at the same time and the spacecraft also can work normally because of the stability of base.

Space manipulator;Trajectory tracking;Zero reaction motion;PD control; Polynomial interpolation;Spacecraft

10.3780/j.issn.1000-758X.2015.05.003

(編輯:車曉玲)

國家自然科學(xué)基金(11102007),國家基本科研業(yè)務(wù)費(YWF-14-YHXY-012)資助項目

2015-04-28。收修改稿日期:2015-07-14

廖志祥 1991年生,2015年獲北京航空航天大學(xué)飛行器設(shè)計與工程專業(yè)碩士學(xué)位,現(xiàn)為北京航空航天大學(xué)飛行器設(shè)計與工程專業(yè)碩士研究生。研究方向為空間機械臂系統(tǒng)的軌跡規(guī)劃與動力學(xué)控制。