齊 飛 佘世剛 高書苑 陳 柏 吳洪濤

(1.常州大學機械與軌道交通學院， 常州 213016； 2.南京航空航天大學機電學院， 南京 210016)

0 引言

連續(xù)體機器人具有獨特的柔順性、安全性和靈活性，能在一些曲折、狹小的工作環(huán)境中作業(yè)，因此在工業(yè)管道、航空發(fā)動機及醫(yī)療手術等領域得到了廣泛研究。建立高效、準確的運動模型是機器人運動控制的基礎和前提條件，但連續(xù)體機器人是一種超冗余、強耦合的非線性系統(tǒng)，獲取其適用的數(shù)學模型難度較大。為了實現(xiàn)對連續(xù)體機器人精確的運動控制，需要對其進行動力學建模研究。

一般情況下，連續(xù)體機器人的變形過程可以通過運動學模型進行描述，但在受到外界環(huán)境干擾及運動速度較大時，運動學模型將無法準確地表示機器人的動態(tài)變形過程[1-5]。常見的動力學建模方法有拉格朗日法、牛頓歐拉法和凱恩法[6-9]。HE等[10]基于拉格朗日法建立了多節(jié)連續(xù)體機器人的動力學模型，但忽略了傳動摩擦損失的影響。RONE等[11]基于虛功原理推導了連續(xù)體機器人動力學模型，同時對驅動繩與連接盤間的摩擦影響進行了研究。WANG等[12]基于融合的Cosserat梁理論和凱恩方法建立了一種繩驅動的軟體機器人動力學模型，該模型考慮了軟體材料粘性變形所帶來的阻尼作用。RENDA等[13]基于幾何分析法推導了仿象鼻軟體機器人的動力學模型，并分析了關節(jié)耦合力與外界相互作用力的影響。XU等[14]針對一種繩驅動冗余機械臂進行了運動學和動力學建模，并提出了一種驅動解耦方法。XU等[15]基于分段分析法推導了一種桿驅動軟體機器人的靜力學模型，并通過幾何相容性約束和橢圓積分法分析了機器人的運動和剛度特性，但這種方法建模復雜，且需要進行多次積分。

本文針對自主研發(fā)的介入手術連續(xù)體機器人的運動模型及控制策略進行研究。首先，基于幾何分析法對運動學模型進行分析，構建考慮慣性力、彈性內(nèi)力、重力及驅動力影響的動力學模型；分析繩-輪傳動系統(tǒng)的非線性摩擦對運動傳遞的影響，提出一種基于誤差補償?shù)那梆伩刂撇呗裕蛔詈笸ㄟ^仿真和實驗對所建模型和控制方法的有效性進行驗證。

1 連續(xù)體機器人運動模型

圖1為本文所提出的一種繩驅動連續(xù)體機器人，該機器人主要由NiTi合金芯柱、連接盤、硅膠外殼等組成。連接盤等間距固定在芯柱上，每個連接盤圓周上均勻分布3個導向孔。驅動繩通過導向孔后固定在機器人末端，通過調節(jié)驅動繩長度來實現(xiàn)機器人在三維空間中的變形運動[16]。為方便建模，假設機器人質量和慣性由連接盤、芯柱質量和慣性所決定，忽略了彎曲過程中機器人扭轉和剪切應變。

如圖2所示，每段彎曲單元用參數(shù)β、θ來表示，其中β為彎曲切面方向角，θ為彎曲角，定義向量qi=(θi,βi)T來描述圖2b所示的坐標系，則對于n個單元的連續(xù)體機器人有

(1)

在分段常曲率圓弧假設下基于幾何分析法建立第i個連接盤中心相對于前一坐標的位置向量為

i-1pi=(cβi(1-cθi)l/θi，sβi(1-cθi)l/θi，sθil/θi)T

(2)

式中l(wèi)——彎曲單元長度

pi——位置向量

其中s表示sin,c表示cos。

其中當彎曲角為0時，為避免矩陣變換陷入奇異性(分母為0)，令pi=(0,0,l)T。

而第i個坐標系的方向則由3個歐拉變換矩陣組成(圖2b)，分別為繞前一坐標系zi-1軸旋轉βi，繞當前坐標系y1軸旋轉θi，最后再繞zi軸旋轉-βi。

(3)

則第i節(jié)彎曲單元相對于基坐標系的位姿變換矩陣Ti為

(4)

其中R1=0R1P1=0P1

Ri=Ri-1i-1RiPi=Ri-1i-1pi+Pi-1(i>1)

式中Pi——第i節(jié)彎曲單元相對于基坐標系的位置向量

Ri——第i節(jié)彎曲單元相對于基坐標系的旋轉矩陣

若已知機器人末端位姿，則關節(jié)空間與操控空間的逆變換為

(5)

根據(jù)幾何分析法，則關節(jié)空間到驅動空間的映射關系為

(i=1,2,…，n;j=1,2,3)

(6)

式中rh——驅動繩孔半徑

Δli,j——第j根繩長的變化量

相鄰坐標系間的速度可分別通過位置變換向量i-1pi和旋轉變換矩陣i-1Ri對時間求導得出，其中線速度可直接通過對位置向量求導得出，即

(7)

式中i-1vi——線速度

Jxv——線速度對應的雅可比矩陣

但直接對旋轉變換矩陣i-1Ri求導比較復雜，可通過歐拉變換原理求解當前坐標系相對于前一坐標系的角速度。

令旋轉變換矩陣i-1Ri每一列分別對應切向量i-1ti、法向量i-1ni、副法線向量i-1bi，即i-1Ri=[i-1nii-1bii-1ti]，則當前坐標系下機器人的角速度由式(8)可得

(8)

將式(8)展開并化簡得角速度為

(9)

式中i-1ωi——角速度

Jxω——角速度對應的雅可比矩陣

對于n段連續(xù)體機器人，其末端連接盤相對于初始坐標系的速度為

(10)

其中

式中Jn——n節(jié)連續(xù)體機器人的雅可比矩陣

Ji——第i節(jié)彎曲單元對應的雅可比矩陣，i=1,2,…,n

Ai——第i節(jié)彎曲單元對應的伴隨矩陣

而第i節(jié)彎曲單元相對于基坐標系的線加速度ai和角加速度ψi可通過對線速度和角速度相對于時間求導得出，即

(11)

(12)

2 連續(xù)機器人動力學模型

為分析連續(xù)體機器人的動態(tài)變形特性，基于凱恩方法對機器人進行動力學建模。為簡化模型，忽略了機器人在運動過程中的剪切、扭轉及壓縮等變形，只考慮機器人的彎曲變形運動。

2.1 彈性力分析

根據(jù)Cosserat梁理論[17-18]，當機器人彎曲變形時其中心骨架產(chǎn)生的彎曲和扭轉應變可由曲率向量Ki表示，則第i段彎曲單元在彎曲過程中產(chǎn)生的彈性勢能密度Ep為

其中

(13)

式中E——楊氏模量G——剪切模量

Ii——截面慣性矩Ipi——極慣性矩

(14)

考慮到相鄰關節(jié)間的耦合影響，則第i個離散單元的彈性力矩Mi,el為

(15)

2.2 重力分析

根據(jù)前文假設，第i段彎曲單元所受的重力Fi,g表示為

(16)

式中mi——第i段彎曲單元質量

g——重力加速度

2.3 驅動力分析

如圖3所示，每個連接盤具有3個導向繩孔，且相鄰連接盤間的驅動繩為直線，則驅動繩的運動方向可由連接盤導向孔的方向決定。在當前坐標系下，連接盤的3個導向孔的位置向量為

(j=1,2,3)

(17)

根據(jù)式(17)，則驅動繩張力的方向可通過驅動繩位置向量pi,j的單位向量表示為

fi,j=pi,j/‖pi,j‖

(18)

其中

(19)

式中fi,j——第i連接盤第j根繩張力向量

第i連接盤第j根繩的張力Fi,j為

(20)

最終施加在每個連接盤上的等效驅動力Fi,act及驅動力矩Mi,act為

(21)

2.4 慣性力分析

根據(jù)式(10)～(12)求解出的速度及加速度，則每個連接盤所受到的慣性力Fi,inr和慣性力矩Mi,inr為

(22)

其中

式中IIi——連接盤的轉動慣量

Ixx、Izz——徑向和軸向轉動慣量

2.5 動力學模型

根據(jù)凱恩(Kane)方法[19-20]，機器人系統(tǒng)在外力和外力矩下所做的虛功P(式(23))為0時從而推導出機器人的動力學模型。假設機器人每個離散的彎曲系統(tǒng)所受的外力和外力矩分別為Fi,ex、Mi,ex，結合由式(10)得出的對應角速度和線速度，則連續(xù)體機器人系統(tǒng)的動力學模型為

(23)

(24)

其中

Fi,ex=Fi,inr+Fi,g+Fi,act

Mi,ex=Mi,inr+Mi,act

為了便于分析，可將式(24)簡化為一般形式的動力學模型，即

(25)

其中

式中H(q)——機器人慣性矩陣

G(q)——機器人自身重力和中心骨架的彈性力

Q——等效廣義力

3 動力學模型仿真

以一個由4節(jié)離散單元組成的繩驅動連續(xù)體機器人為仿真對象，分別在靜態(tài)平衡和彎曲運動狀態(tài)下對機器人的動態(tài)性能進行分析，通過求解該模型的數(shù)值解來驗證所建動力學模型的正確性。機器人材料特性和結構參數(shù)如表1所示。

表1 材料與結構特性參數(shù)Tab.1 Material and structure parameters

3.1 靜態(tài)平衡時

首先研究未加負載時機器人的動態(tài)特性，即驅動張力為0。令θi=0.000 1 rad、βi=0 rad作為數(shù)值求解動力學模型時的初始關節(jié)參數(shù)。由于輸入張力為0，機器人理論上處于靜態(tài)平衡狀態(tài)，即廣義速度及加速度均為0。圖4為零驅動狀態(tài)下機器人各離散關節(jié)參數(shù)的動態(tài)特性。由圖4可知，零驅動時，連續(xù)體機器人各離散單元彎曲角θ隨時間的變化規(guī)律為繞某一固定值的正弦曲線，而旋轉角隨時間變化始終為0。同時由于各驅動繩的輸入張力為0，系統(tǒng)總能量始終保持不變。而圖4b展示了零驅動時連續(xù)體機器人的運動形狀為繞某一平衡位置做振蕩運動。

3.2 彎曲運動時

考慮到機器人在三維空間中相互對稱運動特性，對彎曲運動時機器人的動態(tài)特性進行分析。假設機器人初始驅動張力為T1=5 N,T2=0 N,T3=0 N，以各個關節(jié)參數(shù)θi=π/12,βi=0.001 rad作為求解動力學模型的初始值，則運動過程中連續(xù)體機器人各關節(jié)彎曲角的動態(tài)變化如圖5所示。由圖5可知，機器人在單一驅動力下做平面運動，則旋轉角βi始終為0。由圖5a可知，從前至后各離散彎曲單元的彎曲角幅值依次減少，并最終收斂于某一穩(wěn)定位置，這可能是由于傳動系統(tǒng)中的摩擦所引起的。而圖5b則展示了運動過程中機器人中心骨架的形狀隨時間呈振蕩變化，且最終收斂于彎曲角為π/6的平衡位置。

4 導向輪間傳動特性分析

在實際運動過程中驅動繩索需要通過導向輪后與伺服電機轉軸相連(圖6)，則繩-輪傳動系統(tǒng)中的摩擦力勢必會對張力的傳遞造成影響，從而降低機器人的運動精度。

由文獻[21-22]可知，驅動繩與導向輪間摩擦力與接觸面正壓力滿足能量法則

f=αNn(n≤1)

(26)

式中f——摩擦力N——法向力

α、n——常量，與接觸材料特性有關

結合圖6，則驅動繩與導向輪間的繩張力、剪切力和彎曲力矩的平衡方程為

(27)

由于dφ→0，將式(27)化簡可得

(28)

式中T——繩張力Q——剪切力

M——剪切力矩

φ——傳動系統(tǒng)微小包角

r——驅動繩半徑

Rp——導向輪半徑

由于所采用的驅動繩為超高分子聚乙烯編制的PE線，由文獻[23-25]可知，驅動繩與導向輪在運動過程中將產(chǎn)生粘彈性的變形。假設ρ=Rp/r為接觸面半徑，將式(26)代入到方程(28)中，可得

(29)

式(29)為考慮了彎曲剛度和非線性摩擦力影響的改進Capstan方程，該模型能夠準確地描述驅動繩與導向輪間的粘彈性變形。為便于求解，通過Matlab中的ODEs函數(shù)來解其數(shù)值解。首先將微分方程轉換為1階微分的形式，即

(30)

假設繩-輪傳動系統(tǒng)總包角φ=[0，π]，繩-輪半徑比ρ為1或10，接觸摩擦力系數(shù)α為0.15或0.6，參數(shù)n為0.67或1。若假設經(jīng)繩-輪傳動系統(tǒng)后的初始輸出張力為1，則在運動過程中傳動系統(tǒng)輸入和輸出張力的比值隨包角的變化規(guī)律如圖7所示。由圖7可知，隨著接觸包角的變大，繩-輪傳動系統(tǒng)的能量損耗越大，即輸入到系統(tǒng)中的繩張力也越大。與經(jīng)典的Capstan方程相比，考慮非線性摩擦及彎曲剛度的改進模型的摩擦損失小，且其張力比隨彎曲度的變化相對比較平穩(wěn)，即在輸出相同張力時所需的繩輸入張力小。同時分析了不同參數(shù)對繩-輪傳動模型的影響，對比圖7a、7b可知，改進后的傳動模型中繩張力損失較小，且非線性摩擦對張力損耗的影響比彎曲剛度的大。當摩擦力系數(shù)α為0.6時，輸入和輸出的繩張力比比摩擦力系數(shù)α為0.15時變化快，表明摩擦力系數(shù)是影響傳動效率的最主要參數(shù)。

5 實驗驗證

為驗證所建模型及所提控制方法的有效性，根據(jù)圖8所示的機器人運動控制框圖搭建了單節(jié)連續(xù)體機器人樣機平臺，如圖9所示。該機器人本體由一個直徑為0.8 mm的NITi合金芯柱、5個3D打印的外徑為10 mm的連接盤、硅膠外殼和4根直徑為0.8 mm的大力馬聚合編織的繩組成。機器人的材料特性如表1所示。機器人總長度為90 mm，直徑為10 mm，彎曲所需的驅動力主要由3個Maxon電機(A-max22, Switzerland)提供，同時每根驅動繩均連有一個張力傳感器(JLBS-MD-10kg)，用于測量運動過程中繩的實際張力。通過Matlab/Simulink編寫相應的控制程序，并基于RTW/xPC實時控制平臺控制機器人的變形運動，從而來驗證機器人運動模型及控制方法的有效性。

5.1 平面內(nèi)的彎曲運動

首先控制連續(xù)體機器人在單一平面xz內(nèi)進行彎曲運動(圖10)，則機器人僅在驅動繩1張力作用下進行平面彎曲運動，其他繩張力均為0。根據(jù)所建的運動模型，則在運動過程中機器人的理論運動軌跡及驅動繩長的變化量如圖11a、11b所示。由圖可知，連續(xù)體機器人的末端運動軌跡為一橢圓弧曲線，且在平面內(nèi)以同一彎曲角速度0.26 rad/s進行彎曲運動。而圖11c、11d展示了運動過程中連續(xù)體的理論驅動力和實測驅動力。由圖可知，該機器人僅在驅動繩1張力F1作用下進行彎曲運動，其余繩張力均為0。同時根據(jù)圖11d理論驅動繩張力與實測輸入驅動力間的誤差可知，其張力損失量隨著機器人彎曲角的增大而增大，當彎曲角為1.6 rad時，最大驅動力偏差為1.26 N，可能是機器人加工誤差所引起，但占比較小，可忽略不計。

5.2 空間內(nèi)的圓弧運動

假設機器人在三維空間的圓弧軌跡為

(31)

其具體的運動過程如圖12所示。首先根據(jù)運動學模型可得機器人理論運動軌跡，如圖13a所示，而運動過程中關節(jié)參數(shù)和驅動繩長隨時間的變化曲線如圖13b、13c所示。由圖可知，連續(xù)體機器人在三維空間中以旋轉角速度0.53 rad/s，恒定的彎曲角π/3作圓弧運動。由圖13d、13e可知，連續(xù)體機器人在彎曲運動中所需的理論驅動力基本上與實際施加的驅動力一致，但存在一定量的偏差(理論驅動力略小于實際輸入驅動力)，其主要原因是用于補償繩-輪傳遞系統(tǒng)中摩擦力所引起的繩張力損耗。運動過程中繩驅動的張力損失如圖13e所示，同時該誤差也包含了加工誤差和裝配誤差所引起的張力損失，由于這項誤差較小(均值均為0.42 N)可忽略不計。但從實驗結果可知，所提的動力學模型基本上能夠較為精確地描述連續(xù)體機器人的動態(tài)運動過程。

6 結論

(1)所建立的動力學模型能夠準確地描述機器人的動態(tài)變形過程，并通過實驗進行了驗證。但在運動過程中，機器人實際電機輸入驅動力略大于理論驅動力，其主要原因是需要補償運動過程中繩-輪傳動系統(tǒng)的摩擦損失。

(2)實驗結果同時也驗證了運動學建模時的常曲率圓弧假設，忽略了連接盤間距參數(shù)對連續(xù)體機器人運動學建模誤差的影響。

(3)連續(xù)體機器人理論彎曲形狀與實際形狀間存在微小偏差，這可能是由制造加工誤差及裝配誤差等導致，可忽略不計。