基于改進粒子群的繩驅連續型機械臂逆運動學分析*

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

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

0 引 言

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

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

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

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

1 繩驅連續型機械臂結構

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

圖1 繩驅連續型機械臂示意圖Fig.1 Sketch of the cable-driven continuous manipulator

(a)第1節間隔盤

2 正運動學

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

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

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

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

0Tn=0T11T2…n-1Tn

(1)

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

(2)

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

3 基于PSO的逆運動學求解

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

3.1 操作空間到關節空間的映射

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

3.1.1 優化目標

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

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

Δ=

(3)

(4)

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

(5)

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

(6)

其中，f′(q)表示轉換后的目標函數；g′(q)為懲罰項；λ表示懲罰系數。

3.1.2 利用PSO求解

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

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

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

(7)

(8)

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

(9)

3.2 關節空間到驅動空間的映射

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

(10)

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

由此，關節空間到驅動空間的映射關系如下

(11)

(12)

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

3.3 仿真驗證

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

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

表2 基于標準粒子群算法的逆運動學求解結果

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

4 實驗分析

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

圖5 連續型機械臂物理樣機系統Fig.5 Prototype of continuum manipulator system

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

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

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

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

表3 單節機械臂彎曲結果

5 結 論

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