邱 雪，鐘 超，侯月陽(yáng)，夏澤群，董洋洋，張子建

(1.南京航空航天大學(xué)航天學(xué)院，江蘇 南京 210016)(2.上海航天控制技術(shù)研究所，上海 200233)

隨著空間技術(shù)的高速發(fā)展，特別是空間站、航天飛機(jī)等的誕生及成功應(yīng)用，空間機(jī)械臂作為完成目標(biāo)捕獲、在軌組裝與服務(wù)、星表樣本采集等任務(wù)的關(guān)鍵裝備發(fā)揮著越來(lái)越重要的作用[1-2]。空間七自由度機(jī)械臂相比非冗余機(jī)械臂其操作靈活性大為提高，在各國(guó)航天領(lǐng)域受到越來(lái)越廣泛的關(guān)注[3]。但針對(duì)七自由度機(jī)械臂的動(dòng)力學(xué)建模更加復(fù)雜，且計(jì)算量較大，難以應(yīng)用于機(jī)械臂系統(tǒng)的實(shí)時(shí)控制與仿真。

動(dòng)力學(xué)模型是空間機(jī)械臂規(guī)劃與控制的基礎(chǔ)，建立正確、高效的動(dòng)力學(xué)方程對(duì)于設(shè)計(jì)高性能控制器及機(jī)械臂系統(tǒng)仿真等具有重要意義，國(guó)內(nèi)外許多學(xué)者在機(jī)械臂動(dòng)力學(xué)建模與控制方面做了大量研究工作。目前，機(jī)械臂動(dòng)力學(xué)建模方法主要有以牛頓-歐拉法為代表的矢量力學(xué)方法[4-6]、以拉格朗日方法為代表的分析力學(xué)方法[7-9]以及兼具矢量力學(xué)和分析力學(xué)特點(diǎn)的凱恩法[10-11]，它們的計(jì)算復(fù)雜度分別為O(n3)、O(n4)和O(n2)。上述傳統(tǒng)動(dòng)力學(xué)建模方法中拉格朗日方法應(yīng)用相對(duì)較多，王磊等[12]利用拉格朗日方法建立了兩自由度機(jī)械臂動(dòng)力學(xué)模型；劉文超[13]采用拉格朗日方法建立了三自由度機(jī)械臂動(dòng)力學(xué)模型并進(jìn)行軌跡跟蹤控制；李鵬飛[14]采用拉格朗日方法建立了六自由度機(jī)械臂動(dòng)力學(xué)模型并分析其動(dòng)力學(xué)特性。這種傳統(tǒng)的方法對(duì)于低自由度機(jī)械臂動(dòng)力學(xué)建模是方便可行的，但隨著系統(tǒng)自由度增加，方程復(fù)雜度呈指數(shù)增長(zhǎng)，其算法計(jì)算效率低，不能滿足復(fù)雜機(jī)械臂系統(tǒng)實(shí)時(shí)控制和仿真的需要[15-16]。因此，建立計(jì)算效率高的機(jī)械臂動(dòng)力學(xué)模型成為重要研究方向之一。肖爽等[17]采用拉格朗日方法建立五自由度機(jī)械臂動(dòng)力學(xué)模型并利用機(jī)械臂特殊構(gòu)型對(duì)方程進(jìn)行簡(jiǎn)化，最終實(shí)現(xiàn)了對(duì)機(jī)械臂的實(shí)時(shí)控制，不過(guò)該方法不具備普適性。

本文針對(duì)空間七自由度機(jī)械臂基座位姿固定和自由漂浮兩種工作狀態(tài)，研究了計(jì)算量為O(n)次的高效率動(dòng)力學(xué)建模方法[18-21]，推導(dǎo)了基于旋量和鉸接體慣量理論的正向動(dòng)力學(xué)模型和逆向動(dòng)力學(xué)模型,并基于逆向動(dòng)力學(xué)模型建立了機(jī)械臂位置控制系統(tǒng)，實(shí)現(xiàn)了機(jī)械臂實(shí)時(shí)軌跡跟蹤控制。

1 動(dòng)力學(xué)建?；A(chǔ)

為了建立能夠用于機(jī)械臂實(shí)時(shí)控制和仿真的高效、簡(jiǎn)潔的動(dòng)力學(xué)模型，使用空間矢量描述機(jī)械臂系統(tǒng)的各物理量，并引入鉸接體概念[21]。對(duì)于機(jī)械臂系統(tǒng)，連桿(i-1)的子樹(shù)即為鉸接體i，以此類推，如圖1所示。

圖1 鉸接體示意圖

對(duì)本文研究的機(jī)械臂系統(tǒng)作如下假設(shè)：1)文中涉及的機(jī)械臂系統(tǒng)為剛體系統(tǒng)；2)機(jī)械臂與基座、相鄰連桿間只有一個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)，關(guān)節(jié)無(wú)柔性；3)機(jī)械臂系統(tǒng)拓?fù)浣Y(jié)構(gòu)為鏈?zhǔn)剑瑹o(wú)支路且末端自由；4)空間微重力環(huán)境下忽略重力的影響。

基于上述假設(shè)，對(duì)空間七自由度機(jī)械臂正動(dòng)力學(xué)建模進(jìn)行研究。存在鉸接體慣量定理，使得機(jī)械臂系統(tǒng)中的某個(gè)體受力為[19]：

(1)

首先根據(jù)機(jī)械臂連桿之間運(yùn)動(dòng)學(xué)量的遞推關(guān)系可以得到鉸接體加速度的表達(dá)式；再對(duì)空間機(jī)械臂系統(tǒng)中單個(gè)連桿進(jìn)行受力分析，可解出關(guān)節(jié)角加速度；最后結(jié)合式(1)可得到遞推形式的鉸接體慣量和鉸接體偏向力表達(dá)式，進(jìn)而得到機(jī)械臂正向動(dòng)力學(xué)模型。

2 基座位姿固定機(jī)械臂動(dòng)力學(xué)建模

2.1 正向動(dòng)力學(xué)

基座位姿固定情況下基座的速度和加速度均為零，利用機(jī)械臂各關(guān)節(jié)角加速度的表達(dá)式，可得到遞推形式的機(jī)械臂正向動(dòng)力學(xué)模型。

1)運(yùn)動(dòng)學(xué)量向外遞推。

i=1，2，…，n循環(huán)：

(2)

2)鉸接體慣量和鉸接體偏向力向內(nèi)遞推。

i=n，n-1，…，2循環(huán)：

(3)

3)加速度向外遞推計(jì)算。

i=1，2，…，n循環(huán)：

(4)

由上述模型可以得到初始時(shí)刻機(jī)械臂各關(guān)節(jié)的角加速度，為求得下一時(shí)刻各關(guān)節(jié)角度及角速度，本文采用經(jīng)典的龍格庫(kù)塔方法進(jìn)行數(shù)值積分，最終實(shí)現(xiàn)了對(duì)機(jī)械臂運(yùn)動(dòng)狀態(tài)的仿真。

2.2 逆向動(dòng)力學(xué)

對(duì)于基座位姿固定的逆向動(dòng)力學(xué)建模，同樣已知基座速度、加速度均為零，傳統(tǒng)的牛頓-歐拉建模方法重復(fù)運(yùn)算多，計(jì)算效率不高，采用旋量代替三維向量描述各運(yùn)動(dòng)學(xué)和力學(xué)物理量，克服了上述缺點(diǎn)，提高了計(jì)算效率。

1)運(yùn)動(dòng)學(xué)量向外遞推。

i=1，2，…，n循環(huán)：

(5)

2)力學(xué)量向內(nèi)遞推。

i=n，n-1，…，1循環(huán)：

(6)

由以上逆向動(dòng)力學(xué)模型，根據(jù)機(jī)械臂運(yùn)動(dòng)狀態(tài),可快速地求解出各關(guān)節(jié)驅(qū)動(dòng)力矩，用于固定基座機(jī)械臂的實(shí)時(shí)控制。

3 基座自由漂浮機(jī)械臂動(dòng)力學(xué)建模

3.1 正向動(dòng)力學(xué)

與基座位姿固定的動(dòng)力學(xué)建模不同，漂浮基座的速度與加速度不為零，可以看成是基座通過(guò)6-DOF的虛擬鉸鏈與慣性坐標(biāo)系連接。因此，機(jī)械臂系統(tǒng)自由度大大增加，使得動(dòng)力學(xué)建模更加困難，模型更加復(fù)雜。本文在固定基座正向動(dòng)力學(xué)模型的基礎(chǔ)上進(jìn)一步擴(kuò)展，得到了漂浮基座的正向動(dòng)力學(xué)模型。

對(duì)于基座自由漂浮的空間機(jī)械臂，將整個(gè)系統(tǒng)視為鉸接體0，由于鉸接體0不受外力作用，因此有：

(7)

(8)

(9)

1)運(yùn)動(dòng)學(xué)量向外遞推。

i=1，2，…，n循環(huán)：

(10)

2)鉸接體慣量和鉸接體偏向力向內(nèi)遞推。

i=n，n-1，…，2循環(huán)：

(11)

3)加速度向外遞推計(jì)算。

i=1，2，…，n循環(huán)：

(12)

3.2 逆向動(dòng)力學(xué)

逆向動(dòng)力學(xué)模型是根據(jù)機(jī)械臂運(yùn)動(dòng)狀態(tài)求解各關(guān)節(jié)驅(qū)動(dòng)力矩。對(duì)于基座自由漂浮的空間機(jī)械臂，各關(guān)節(jié)加速度已知，但基座的加速度未知，因此求解該逆向動(dòng)力學(xué)模型是一個(gè)動(dòng)力學(xué)混合問(wèn)題?；杂善〉目臻g機(jī)械臂系統(tǒng)不受外力作用，系統(tǒng)的動(dòng)量守恒，結(jié)合動(dòng)量守恒定理對(duì)固定基座機(jī)械臂的逆向動(dòng)力學(xué)算法進(jìn)行改進(jìn)，得到了基座自由漂浮的機(jī)械臂逆向動(dòng)力學(xué)模型。

1)運(yùn)動(dòng)學(xué)量向外遞推。

i=1，2，…，n循環(huán)：

(13)

2)組合體慣量和組合體偏向力向內(nèi)遞推。

i=n，n-1，…，1循環(huán)：

(14)

3)關(guān)節(jié)力矩向外遞推。

i=1，2，…，n循環(huán)：

(15)

根據(jù)已知的機(jī)械臂運(yùn)動(dòng)狀態(tài)及基座初始運(yùn)動(dòng)狀態(tài)，運(yùn)用上述基座自由漂浮的空間機(jī)械臂逆向動(dòng)力學(xué)算法，可以得到機(jī)械臂各關(guān)節(jié)需要的驅(qū)動(dòng)力矩及機(jī)械臂運(yùn)動(dòng)對(duì)基座產(chǎn)生的擾動(dòng)力和力矩，進(jìn)而判斷機(jī)械臂運(yùn)動(dòng)對(duì)基座載體產(chǎn)生的影響，為任務(wù)規(guī)劃提供依據(jù)。

4 仿真試驗(yàn)及結(jié)果分析

利用MATLAB編寫(xiě)空間七自由度機(jī)械臂正、逆動(dòng)力學(xué)算法(簡(jiǎn)稱ABA)，并在SimMechanics中建立該機(jī)械臂物理模型，比較二者之間的仿真結(jié)果。仿真步長(zhǎng)設(shè)置為0.01 s，仿真時(shí)間為10 s?？臻g七自由度機(jī)械臂及其固連坐標(biāo)系如圖2所示。

圖2 空間機(jī)械臂固連坐標(biāo)系

機(jī)械臂質(zhì)量、固連坐標(biāo)系原點(diǎn)位置、連桿質(zhì)心位置、轉(zhuǎn)動(dòng)慣量等動(dòng)力學(xué)參數(shù)見(jiàn)表1。其中，坐標(biāo)系原點(diǎn)位置是相對(duì)于上一級(jí)連桿固連坐標(biāo)系，連桿質(zhì)心位置是相對(duì)于本級(jí)連桿固連坐標(biāo)系，另外慣性系與基座本體系初始位姿相同。

表1 空間機(jī)械臂動(dòng)力學(xué)參數(shù)

4.1 基座位姿固定機(jī)械臂動(dòng)力學(xué)仿真

假設(shè)機(jī)械臂各關(guān)節(jié)驅(qū)動(dòng)力矩為sint、0.3cost、0.3sint、0.2cost、0.2sint、0.03sint、0.02sintN·m，由正動(dòng)力學(xué)模型可以計(jì)算得到機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)情況?；蛔斯潭ㄇ闆r下的仿真結(jié)果如圖3、圖4所示。

圖3 基座位姿固定機(jī)械臂各關(guān)節(jié)角度曲線

圖4 空間機(jī)械臂各關(guān)節(jié)角速度曲線

假設(shè)機(jī)械臂各關(guān)節(jié)角度變化曲線均為0.1costrad，通過(guò)逆動(dòng)力學(xué)模型可以計(jì)算得到各關(guān)節(jié)所需的驅(qū)動(dòng)力矩?；蛔斯潭ㄇ闆r下機(jī)械臂各關(guān)節(jié)驅(qū)動(dòng)力矩仿真曲線如圖5所示。

由圖3和圖4可以看出，正動(dòng)力學(xué)算法仿真得到的機(jī)械臂各關(guān)節(jié)的角度、角速度曲線與SimMechanics得到的一致；由圖5可以看出，逆動(dòng)力學(xué)算法得到的機(jī)械臂各關(guān)節(jié)驅(qū)動(dòng)力矩曲線與SimMechanics得到的一致。由此，可以驗(yàn)證基座位姿固定情況下空間機(jī)械臂動(dòng)力學(xué)模型算法的準(zhǔn)確性。

圖5 空間機(jī)械臂各關(guān)節(jié)力矩曲線

4.2 基座自由漂浮機(jī)械臂動(dòng)力學(xué)仿真

假設(shè)機(jī)械臂各關(guān)節(jié)驅(qū)動(dòng)力矩為cost、0.5sint、0.5cost、0.1sint、0.1cost、0.05cost、0.01costN·m，由正動(dòng)力學(xué)模型可以計(jì)算得到機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)情況及基座位姿擾動(dòng)。基座自由漂浮情況下機(jī)械臂各關(guān)節(jié)角度曲線如圖6所示，基座位姿擾動(dòng)曲線如圖7所示。

假設(shè)空間七自由度機(jī)械臂各關(guān)節(jié)角度變化曲線均為0.1sintrad，通過(guò)逆動(dòng)力學(xué)模型可以計(jì)算得到各關(guān)節(jié)所需的驅(qū)動(dòng)力矩及基座受到的擾動(dòng)力。基座擾動(dòng)力曲線如圖8所示。

由圖6和圖7可以看出，正動(dòng)力學(xué)算法仿真得到的機(jī)械臂各關(guān)節(jié)的角度和基座位姿擾動(dòng)曲線與SimMechanics得到的一致；由圖8可以看出，逆動(dòng)力學(xué)算法得到的機(jī)械臂基座擾動(dòng)力曲線與SimMechanics得到的一致。由此，可以驗(yàn)證基座自由漂浮情況下空間機(jī)械臂動(dòng)力學(xué)模型算法的準(zhǔn)確性。

圖6 基座自由漂浮機(jī)械臂各關(guān)節(jié)角度曲線

圖7 基座位姿擾動(dòng)

圖8 基座擾動(dòng)力

4.3 基于逆動(dòng)力學(xué)模型的位置控制仿真

為證明本文建立的動(dòng)力學(xué)算法具有計(jì)算量小、計(jì)算效率高、可用于機(jī)械臂實(shí)時(shí)控制等優(yōu)點(diǎn)，基于上述逆動(dòng)力學(xué)模型設(shè)計(jì)了位置控制器，實(shí)現(xiàn)了空間七自由度機(jī)械臂對(duì)期望軌跡的實(shí)時(shí)跟蹤控制?？刂瓶驁D如圖9所示。

位置控制器的基本設(shè)計(jì)思路為利用逆動(dòng)力學(xué)模型在內(nèi)控制回路中引入非線性補(bǔ)償，將機(jī)械臂化為一個(gè)更容易控制的線性定常系統(tǒng)。控制律的完整表達(dá)式如下：

(16)

為驗(yàn)證基于逆動(dòng)力學(xué)算法的位置控制器的控制效果，假定機(jī)械臂各關(guān)節(jié)的期望關(guān)節(jié)角度分別為sint、cost、0.5sint、0.5cost、sint、cost、0.5sintrad，對(duì)控制系統(tǒng)進(jìn)行仿真得到關(guān)節(jié)軌跡跟蹤誤差，誤差曲線如圖10所示。

圖10 空間機(jī)械臂各關(guān)節(jié)角度跟蹤誤差曲線

由圖10 可以看出，盡管期望關(guān)節(jié)角度幅值比較大、變化較快，但控制器仍能夠很好地跟蹤期望關(guān)節(jié)角度，可以證明本文建立的基于逆動(dòng)力學(xué)的位置控制器動(dòng)態(tài)性能良好，進(jìn)而表明本文推導(dǎo)的動(dòng)力學(xué)算法能夠應(yīng)用于七自由度機(jī)械臂的實(shí)時(shí)控制與仿真。

5 結(jié)束語(yǔ)

本文針對(duì)空間七自由度機(jī)械臂基座位姿固定和基座自由漂浮兩種情況，建立了基于空間矢量和鉸接體慣量的機(jī)械臂正、逆動(dòng)力學(xué)模型，并將上述逆動(dòng)力學(xué)模型應(yīng)用于位置控制器，得到了很好的控制效果。本文研究的動(dòng)力學(xué)建模方法擴(kuò)展性強(qiáng)，可以應(yīng)用于空間機(jī)械臂與被抓捕物體的組合體建模、閉環(huán)系統(tǒng)建模及基座自由飛行情況下的動(dòng)力學(xué)建模。