基于改進粒子群的繩驅(qū)連續(xù)型機械臂逆運動學(xué)分析*

余 鑫，武海雷，丁 萌，鄭先杰

(1.南京理工大學(xué) 自動化學(xué)院·南京·210094；2.上海航天控制技術(shù)研究所·上?！?01109)

0 引 言

隨著人類對太空的不斷探索，空間機械臂已被應(yīng)用到多種在軌任務(wù)中，如飛行器對接、航天器故障維修等[1]。然而，傳統(tǒng)剛性機械臂自由度受限，工作空間相對固定，難以適用于狹小的非結(jié)構(gòu)化空間。連續(xù)型機械臂因其自身的彈性結(jié)構(gòu)，具有良好的彎曲性能，能夠靈活地改變自身的形狀。因此，利用連續(xù)型機械臂代替?zhèn)鹘y(tǒng)剛性機械臂，可以更好地滿足狹窄空間作業(yè)任務(wù)需求。

連續(xù)型機械臂設(shè)計靈感來源于對象鼻、章魚臂等生物的仿生研究?，F(xiàn)有的連續(xù)型機械臂按照驅(qū)動方式的不同，主要分為：氣動和液壓驅(qū)動連續(xù)型機械臂[2-3]、同心管連續(xù)型機械臂[4]、記憶合金機械臂[5]以及繩驅(qū)連續(xù)型機械臂[6]。連續(xù)型機械臂與具有剛性連桿和傳統(tǒng)關(guān)節(jié)形式的剛性機械臂不同，其通過柔性構(gòu)件的彈性變形產(chǎn)生連續(xù)彎曲運動，運動學(xué)建模更為復(fù)雜。Geng S.N.等[7]利用分段常曲率假設(shè)，建立三關(guān)節(jié)連續(xù)型機械臂的運動學(xué)模型，得到驅(qū)動空間、關(guān)節(jié)空間以及操作空間的映射關(guān)系，但該模型沒有考慮多關(guān)節(jié)之間的耦合。R.Roy等[8]利用偽剛體的思想，將連續(xù)型機械臂簡化為多個相連的剛性連桿，相鄰的連桿之間具有彎曲和旋轉(zhuǎn)兩個自由度，利用修正后的D-H參數(shù)得到運動學(xué)模型，但這種模型只適用于彎曲幅度較小的運動，并且所采用的關(guān)節(jié)參數(shù)不能直觀反映機械臂的連續(xù)變化。F.Renda等[9-10]分析了連續(xù)型機械臂的結(jié)構(gòu)特點，利用Cosserat桿理論，將連續(xù)型機械臂離散成有限的節(jié)段和自由度，建立其運動學(xué)模型。

由于連續(xù)型機械臂的正運動學(xué)模型表達(dá)式過于復(fù)雜，難以得到逆運動學(xué)模型的解析解。B.A.Jones等[11]分析了操作空間到關(guān)節(jié)空間的映射關(guān)系，得到了單關(guān)節(jié)連續(xù)型機械臂的逆運動學(xué)關(guān)系，但該方法難以推廣到多關(guān)節(jié)機械臂。劉闖等[12]利用分段常曲率假設(shè)建立正運動學(xué)模型，得到連續(xù)型機械臂的雅可比矩陣，通過牛頓迭代法得到逆運動學(xué)數(shù)值解。K.Dmitrii等[13]采用FABRIK算法解決逆運動學(xué)問題，通過迭代的方式逐步搜索關(guān)節(jié)角，直到機械臂末端完全到達(dá)目標(biāo)點。然而，基于迭代的逆運動學(xué)求解方法通常計算量大，且迭代精度和收斂速度均受到迭代初值的影響，給連續(xù)型機械臂的實時操作帶來困難。A.Amouri等[14-15]將連續(xù)型機械臂的逆運動學(xué)問題轉(zhuǎn)化為優(yōu)化問題，利用元啟發(fā)式算法，解決了具有超冗余性與復(fù)雜耦合性的機械臂逆運動學(xué)求解問題，獲得了具有更高精度的逆運動學(xué)解，但是該逆運動學(xué)模型只建立了機械臂末端位置和繩長的關(guān)系，不能反映機械臂的形態(tài)變化。

本文針對一類兩節(jié)繩驅(qū)連續(xù)型機械臂，基于分段常曲率假設(shè)建立了正運動學(xué)模型，并分析了多節(jié)之間的運動耦合關(guān)系。利用改進的粒子群優(yōu)化(Particle Swarm Optimization，PSO)算法求解得到連續(xù)型機械臂的逆運動學(xué)模型。搭建了繩驅(qū)連續(xù)型機械臂的物理樣機，對所建運動學(xué)模型進行了實驗驗證。

1 繩驅(qū)連續(xù)型機械臂結(jié)構(gòu)

兩節(jié)繩驅(qū)連續(xù)型機械臂示意圖如圖1所示，該連續(xù)型機械臂由中心彈性桿、間隔盤以及驅(qū)動繩組成，機械臂由2節(jié)組成，每節(jié)包含5個間隔盤以及3根驅(qū)動繩。間隔盤固定在彈性桿上并等距分布，作為剛性支撐，間隔盤的結(jié)構(gòu)如圖2所示，其中圖2(a)為第1節(jié)機械臂的間隔盤結(jié)構(gòu)，6個驅(qū)動繩引導(dǎo)孔均勻分布在中心孔周圍，圖2(b)為第2節(jié)機械臂的間隔盤結(jié)構(gòu)，3個驅(qū)動繩引導(dǎo)孔均勻分布。驅(qū)動繩穿過中間間隔盤，固連在相應(yīng)關(guān)節(jié)最后一個間隔盤上，利用電機對驅(qū)動繩進行收放可以使機械臂發(fā)生彎曲形變。

圖1 繩驅(qū)連續(xù)型機械臂示意圖Fig.1 Sketch of the cable-driven continuous manipulator

(a)第1節(jié)間隔盤

2 正運動學(xué)

第i節(jié)機械臂彎曲示意圖如圖3所示，為了建立繩驅(qū)連續(xù)型機械臂的正運動學(xué)模型，在基座間隔盤上建立坐標(biāo)系Oi-1Xi-1Yi-1Zi-1，其中，坐標(biāo)系原點Oi-1位于間隔盤中心點，Xi-1軸方向為中心點到第一根驅(qū)動繩的方向，Zi-1軸方向為沿著中心桿垂直于間隔盤向上，Yi-1軸方向由右手定則確定。然后，以坐標(biāo)系Oi-1Xi-1Yi-1Zi-1為基坐標(biāo)系，在末端間隔盤上建立坐標(biāo)系OiXiYiZi，其初始方向與基坐標(biāo)系一致。

圖3 連續(xù)型機械臂彎曲示意圖Fig.3 Sketch of the continuous manipulator bending

繩驅(qū)連續(xù)型機械臂通過驅(qū)動繩的收放使得中心彈性桿發(fā)生彎曲，進而達(dá)到機械臂末端運動的目的。機械臂每節(jié)有2個自由度，包括由彈性桿形變產(chǎn)生的彎曲自由度和圍繞Z軸旋轉(zhuǎn)產(chǎn)生的旋轉(zhuǎn)自由度，分別用θi和φi表示這2個自由度。

n節(jié)繩驅(qū)連續(xù)型機械臂的正運動學(xué)模型，即由關(guān)節(jié)空間的變量θi和φi求解操作空間中機械臂的末端位姿，可以表示為末端坐標(biāo)系OnXnYnZn和基坐標(biāo)系O0X0Y0Z0的映射關(guān)系0Tn，由上述分析可知

0Tn=0T11T2…n-1Tn

(1)

基于分段常曲率假設(shè)，在忽略外力、重力以及驅(qū)動繩和間隔盤之間的摩擦力后，連續(xù)型機械臂在運動時，每節(jié)可近似為一段等曲率的曲線，由此，式(1)中齊次變換矩陣i-1Ti可表示為

(2)

式中，Li表示第i節(jié)的長度，cφi=cos(φi)，sφi=sin(φi)，cθi=cos(θi)，sθi=sin(θi)。

3 基于PSO的逆運動學(xué)求解

繩驅(qū)連續(xù)型機械臂的逆向運動學(xué)，需要建立操作空間到關(guān)節(jié)空間以及關(guān)節(jié)空間到驅(qū)動空間的映射關(guān)系，也即在已知末端位姿時求取關(guān)節(jié)變量和繩長的變化量。為了避免復(fù)雜的雅可比矩陣的求解，本文將逆運動學(xué)問題轉(zhuǎn)化為優(yōu)化問題，并利用改進的粒子群算法對優(yōu)化目標(biāo)進行求解。

3.1 操作空間到關(guān)節(jié)空間的映射

由式(1)和式(2)可知，單節(jié)連續(xù)型機械臂的逆運動學(xué)求解較簡單，但對于多節(jié)連續(xù)型機械臂，其正運動學(xué)表達(dá)式非常復(fù)雜，難以像傳統(tǒng)的六自由度剛性機械臂一樣求解出解析解。此外，由于冗余性的存在，對于同一個目標(biāo)位置，可能會存在多組θi和φi與之對應(yīng)，因此，如何選取合適的解也是必須要解決的問題。本文為了簡單高效地完成逆運動學(xué)的求解工作，從優(yōu)化的角度出發(fā)，利用粒子群算法完成操作空間到關(guān)節(jié)空間的映射求解。

3.1.1 優(yōu)化目標(biāo)

由于逆運動學(xué)求解存在多解的情況，在實際的機械臂運動中，通常需要選擇一組最優(yōu)解作為目標(biāo)解，使得機械臂在運動過程中滿足能量消耗和安全性等方面的約束。本文在進行逆運動學(xué)求解時，從能量最優(yōu)的角度確定優(yōu)化目標(biāo)。

設(shè)機械臂的目標(biāo)位置為[xd，yd，zd]，其中xd，yd，zd為位置坐標(biāo)，當(dāng)前位置為[xc(q)，yc(q)，zc(q)]，其中，q代表機械臂的4個關(guān)節(jié)變量，即q=[θ1，φ1，θ2，φ2]，那么機械臂末端的實際位置和期望位置的誤差Δ為

Δ=

(3)

(4)

由此，本文的優(yōu)化目標(biāo)為在滿足末端位置誤差Δ小于允許的最大誤差Δmax時，關(guān)節(jié)角度的變化最小，如式(5)所示

(5)

其中，g(q)表示約束函數(shù)，代表末端位置誤差與最大允許誤差之間的差值。為方便后續(xù)處理，利用罰函數(shù)法將式(5)所示的約束最優(yōu)問題轉(zhuǎn)化為無約束最優(yōu)問題，如式(6)所示

(6)

其中，f′(q)表示轉(zhuǎn)換后的目標(biāo)函數(shù)；g′(q)為懲罰項；λ表示懲罰系數(shù)。

3.1.2 利用PSO求解

粒子群算法模擬了鳥群覓食的過程，鳥群中的每只鳥(粒子)通過對自身最優(yōu)位置和群體最優(yōu)位置的學(xué)習(xí)而不斷更新自身的位置，從而達(dá)到全局最優(yōu)解。粒子群算法的基本流程如圖4所示。

圖4 粒子群算法基本流程Fig.4 Basic flow of PSO

首先，設(shè)置基本粒子量x，針對繩驅(qū)連續(xù)型機械臂逆運動學(xué)的實際情況，將關(guān)節(jié)變量(θ1，φ1，θ2，φ2)作為基本粒子。其次，采用隨機生成的方式初始化粒子種群X=(x1，x2，…，xn)以及每個粒子的初始速度V=(v1，v2，…，vn)，每次循環(huán)中，通過對適應(yīng)度函數(shù)的計算，每個粒子可以得到自身歷史最優(yōu)位置和全部粒子的最優(yōu)位置，從而利用式(3)和式(4)更新自身的速度和位置。

(7)

(8)

慣性權(quán)重ω(k)是粒子群速度更新的一個重要參數(shù)，它的大小決定著粒子的搜索能力。經(jīng)典的粒子群算法中，利用線性遞減函數(shù)作為慣性權(quán)重，這種方式使得在算法初期粒子速度變化較大，具有良好的全局搜索能力，在迭代后期粒子速度變化較小，局部搜索性能好。但是慣性權(quán)重采取這種線性遞減的方式，使得在算法的迭代次數(shù)較大時，粒子幾乎沒有搜索能力，一旦粒子陷入局部最優(yōu)就很難跳出來，本文中改進的慣性權(quán)重如式(9)所示

(9)

3.2 關(guān)節(jié)空間到驅(qū)動空間的映射

在利用粒子群算法得到操作空間到關(guān)節(jié)空間的映射后，利用幾何分析法建立關(guān)節(jié)空間到操作空間的映射關(guān)系。如兩關(guān)節(jié)連續(xù)型機械臂在運動過程中，兩節(jié)之間存在運動耦合，在第1節(jié)單獨運動時，會造成第2節(jié)驅(qū)動繩的繩長也發(fā)生變化，因此在求取關(guān)節(jié)空間到驅(qū)動空間的映射時需要進行解耦。假設(shè)第1關(guān)節(jié)的關(guān)節(jié)量為θ1，φ1，第2關(guān)節(jié)不發(fā)生運動，則第2關(guān)節(jié)的耦合量為

(10)

式中，Δl′2i(i=1，2，3)表示第1關(guān)節(jié)單獨運動時對第2節(jié)3根驅(qū)動繩造成的耦合量；r2表示中心彈性到第2關(guān)節(jié)驅(qū)動繩之間的距離；n1為第1關(guān)節(jié)間隔盤數(shù)量；L1為第1節(jié)的長度。

由此，關(guān)節(jié)空間到驅(qū)動空間的映射關(guān)系如下

(11)

(12)

式中，Δl1i、Δl2i(i=1，2，3)分別表示兩節(jié)3根驅(qū)動繩的變化量；r1為中心桿到第1節(jié)驅(qū)動繩之間的距離；n2為第2關(guān)節(jié)間隔盤數(shù)量；L2為第2節(jié)的長度。

3.3 仿真驗證

為了驗證改進的粒子群算法求解繩驅(qū)連續(xù)型機械臂逆運動學(xué)的有效性，將其與基于標(biāo)準(zhǔn)粒子群算法的求解結(jié)果進行對比仿真分析。仿真平臺采用的CPU為i7-7700HQ，主頻2.8GHz。設(shè)置種群大小為50，最大迭代次數(shù)為100，自身學(xué)習(xí)因子c1=1.3，全局學(xué)習(xí)因子c2=1.7，罰函數(shù)系數(shù)λ=1，最大允許誤差Δmax=5。兩節(jié)繩驅(qū)連續(xù)型機械臂的相關(guān)參數(shù)：L1=L2=300mm，n1=n2=5，r1=18mm，r2=16mm。隨機選取5個末端位置，利用本文所述改進粒子群算法以及標(biāo)準(zhǔn)粒子群算法進行逆運動學(xué)的求解，所求得的關(guān)節(jié)變量值、計算耗時以及誤差分別列于表1和表2中。

表1 基于改進粒子群算法的逆運動學(xué)求解結(jié)果

表2 基于標(biāo)準(zhǔn)粒子群算法的逆運動學(xué)求解結(jié)果

如表1和表2所示，對隨機選取的末端位姿，利用改進的粒子群算法能夠快速得到相應(yīng)的逆運動學(xué)結(jié)果，并且求解誤差在允許范圍之內(nèi)，滿足實際使用的需求。

4 實驗分析

為了驗證本文基于分段常曲率假設(shè)建立的繩驅(qū)連續(xù)型機械臂的運動學(xué)模型，本文搭建了單節(jié)機械臂的物理樣機系統(tǒng)，如圖5所示。該系統(tǒng)由機械臂本體、驅(qū)動部分、視覺部分等組成，其中機械臂包含10個間隔盤以及1個基座盤，各盤之間間隔60mm，機械臂總長600mm，由3根繩子驅(qū)動，驅(qū)動繩引導(dǎo)孔到中心桿孔的距離是25mm。

圖5 連續(xù)型機械臂物理樣機系統(tǒng)Fig.5 Prototype of continuum manipulator system

利用上述物理樣機系統(tǒng)進行單節(jié)機械臂的彎曲實驗。實驗中，機械在初始狀態(tài)時，關(guān)節(jié)角θ=0°、φ=0°。在彎曲平面角φ保持不變的情況下，通過理論模型計算機械臂彎曲10°、20°、30°和40°時的繩長變化量，將這些變化量施加到實際物理樣機系統(tǒng)中，可得到機械臂的彎曲情況如圖6所示。

圖6 單節(jié)機械臂彎曲實驗示意圖Fig.6 Schematic diagram of bending experiment

利用雙目視覺系統(tǒng)可以測量各個彎曲階段的末端位置，記錄在表3中。表3還記錄了機械臂彎曲相同度數(shù)時，理論模型下的末端位置。

表3中的誤差計算方式為兩種情況下的末端位置到初始末端位置之間距離的差值和機械臂總長的比值。從實驗結(jié)果可以看出，理論模型與實驗結(jié)果之間的誤差較小，證明了基于常曲率的運動學(xué)模型可以在一定程度上描述繩驅(qū)連續(xù)型機械臂的運動特征。另一方面，隨著機械臂彎曲角度的變大，誤差逐漸增加，考慮到機械臂本身質(zhì)量較小，受重力的影響較小，因此，誤差的主要來源是理論模型所依賴的常曲率假設(shè)。

表3 單節(jié)機械臂彎曲結(jié)果

5 結(jié) 論

本文針對一種多節(jié)繩驅(qū)連續(xù)型機械臂，基于常曲率假設(shè)建立了機械臂的正運動學(xué)模型，并對機械臂關(guān)節(jié)之間的運動學(xué)耦合關(guān)系進行了分析，在此基礎(chǔ)上提出了利用改進PSO算法對連續(xù)型機械臂的逆運動學(xué)進行求解。仿真結(jié)果表明，該方法能快速求解逆運動學(xué)，且誤差相對較小。最后，搭建了繩驅(qū)連續(xù)型機械臂的物理樣機系統(tǒng)，對本文所建正運動學(xué)模型進行驗證，分析了分段常曲率假設(shè)的誤差大小。本文所提方法可推廣應(yīng)用于多節(jié)繩驅(qū)連續(xù)型機械臂的逆運動學(xué)求解，但是對于多節(jié)連續(xù)型機械臂的逆運動學(xué)多解問題，還需進一步開展研究。