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

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

0 引言

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

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

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

1 連續(xù)體機(jī)器人運(yùn)動(dòng)模型

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

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

(1)

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

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。

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

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

(3)

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

(4)

其中R1=0R1P1=0P1

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

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

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

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

(5)

根據(jù)幾何分析法，則關(guān)節(jié)空間到驅(qū)動(dòng)空間的映射關(guān)系為

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

(6)

式中rh——驅(qū)動(dòng)繩孔半徑

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

相鄰坐標(biāo)系間的速度可分別通過位置變換向量i-1pi和旋轉(zhuǎn)變換矩陣i-1Ri對時(shí)間求導(dǎo)得出，其中線速度可直接通過對位置向量求導(dǎo)得出，即

(7)

式中i-1vi——線速度

Jxv——線速度對應(yīng)的雅可比矩陣

但直接對旋轉(zhuǎn)變換矩陣i-1Ri求導(dǎo)比較復(fù)雜，可通過歐拉變換原理求解當(dāng)前坐標(biāo)系相對于前一坐標(biāo)系的角速度。

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

(8)

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

(9)

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

Jxω——角速度對應(yīng)的雅可比矩陣

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

(10)

其中

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

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

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

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

(11)

(12)

2 連續(xù)機(jī)器人動(dòng)力學(xué)模型

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

2.1 彈性力分析

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

其中

(13)

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

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

(14)

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

(15)

2.2 重力分析

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

(16)

式中mi——第i段彎曲單元質(zhì)量

g——重力加速度

2.3 驅(qū)動(dòng)力分析

如圖3所示，每個(gè)連接盤具有3個(gè)導(dǎo)向繩孔，且相鄰連接盤間的驅(qū)動(dòng)繩為直線，則驅(qū)動(dòng)繩的運(yùn)動(dòng)方向可由連接盤導(dǎo)向孔的方向決定。在當(dāng)前坐標(biāo)系下，連接盤的3個(gè)導(dǎo)向孔的位置向量為

(j=1,2,3)

(17)

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

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

(18)

其中

(19)

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

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

(20)

最終施加在每個(gè)連接盤上的等效驅(qū)動(dòng)力Fi,act及驅(qū)動(dòng)力矩Mi,act為

(21)

2.4 慣性力分析

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

(22)

其中

式中IIi——連接盤的轉(zhuǎn)動(dòng)慣量

Ixx、Izz——徑向和軸向轉(zhuǎn)動(dòng)慣量

2.5 動(dòng)力學(xué)模型

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

(23)

(24)

其中

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

Mi,ex=Mi,inr+Mi,act

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

(25)

其中

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

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

Q——等效廣義力

3 動(dòng)力學(xué)模型仿真

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

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

3.1 靜態(tài)平衡時(shí)

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

3.2 彎曲運(yùn)動(dòng)時(shí)

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

4 導(dǎo)向輪間傳動(dòng)特性分析

在實(shí)際運(yùn)動(dòng)過程中驅(qū)動(dòng)繩索需要通過導(dǎo)向輪后與伺服電機(jī)轉(zhuǎn)軸相連(圖6)，則繩-輪傳動(dòng)系統(tǒng)中的摩擦力勢必會(huì)對張力的傳遞造成影響，從而降低機(jī)器人的運(yùn)動(dòng)精度。

由文獻(xiàn)[21-22]可知，驅(qū)動(dòng)繩與導(dǎo)向輪間摩擦力與接觸面正壓力滿足能量法則

f=αNn(n≤1)

(26)

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

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

結(jié)合圖6，則驅(qū)動(dòng)繩與導(dǎo)向輪間的繩張力、剪切力和彎曲力矩的平衡方程為

(27)

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

(28)

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

M——剪切力矩

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

r——驅(qū)動(dòng)繩半徑

Rp——導(dǎo)向輪半徑

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

(29)

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

(30)

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

5 實(shí)驗(yàn)驗(yàn)證

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

5.1 平面內(nèi)的彎曲運(yùn)動(dòng)

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

5.2 空間內(nèi)的圓弧運(yùn)動(dòng)

假設(shè)機(jī)器人在三維空間的圓弧軌跡為

(31)

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

6 結(jié)論

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

(2)實(shí)驗(yàn)結(jié)果同時(shí)也驗(yàn)證了運(yùn)動(dòng)學(xué)建模時(shí)的常曲率圓弧假設(shè)，忽略了連接盤間距參數(shù)對連續(xù)體機(jī)器人運(yùn)動(dòng)學(xué)建模誤差的影響。

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