王爽, 任晗, 李楨, 郭超, 李單青, 張向慧

(1.北方工業(yè)大學(xué)機(jī)械與材料工程學(xué)院, 北京 100114; 2.中國科學(xué)院自動化研究所多模態(tài)人工智能全國重點實驗室,北京 100090;3.北京協(xié)和醫(yī)學(xué)院北京協(xié)和醫(yī)院, 北京 100730)

與傳統(tǒng)的剛性機(jī)器人相比,柔性連續(xù)體機(jī)器人具有良好的靈巧性和柔順性,更適用于空間狹小、路徑復(fù)雜的場景,如應(yīng)用于航空、醫(yī)療等領(lǐng)域。在醫(yī)療手術(shù)機(jī)器人領(lǐng)域,目前繩驅(qū)動連續(xù)體機(jī)器人主要有離散關(guān)節(jié)串聯(lián)構(gòu)型和柔性骨架的連續(xù)體構(gòu)型這兩種構(gòu)型。離散關(guān)節(jié)串聯(lián)的連續(xù)體機(jī)器人,該結(jié)構(gòu)變形程度由每個關(guān)節(jié)可變形的角度決定,但是存在關(guān)節(jié)間的約束難以建立精確模型的問題。Hong等[1]提出了一種應(yīng)用于上頜竇手術(shù)的球型關(guān)節(jié)鉸接構(gòu)型的連續(xù)體機(jī)器人。Ji等[2]提出了一種具有球型關(guān)節(jié)和彈簧構(gòu)成的微創(chuàng)手術(shù)機(jī)器人。另一種是柔性骨架構(gòu)成的連續(xù)體機(jī)器人,該結(jié)構(gòu)主要由彈性中心骨架和節(jié)盤構(gòu)成,利用骨架自身的形變來完成彎曲和旋轉(zhuǎn)等運動,并采用多根驅(qū)動絲來進(jìn)行控制,該結(jié)構(gòu)具有良好的柔順性且適用于負(fù)載能力需求不高的場景,如醫(yī)療領(lǐng)域。Wilkening等[3]提出了一種應(yīng)用于骨科的由兩側(cè)交替切口鎳鈦骨架構(gòu)成的連續(xù)體機(jī)器人。Kato等[4]提出了一種應(yīng)用于神經(jīng)外科的導(dǎo)絲盤和柔性骨架構(gòu)型的連續(xù)體機(jī)器人。Li等[5]研發(fā)了單孔手術(shù)機(jī)器人,該柔性體段由多個關(guān)節(jié)鉸接和柔性骨架構(gòu)成,具有比傳統(tǒng)的繩驅(qū)動蛇形機(jī)械臂具有更大的空間。

運動學(xué)模型的精準(zhǔn)度將影響機(jī)器人的整體控制效果,針對多段連續(xù)體機(jī)械臂的運動學(xué)建模,多位學(xué)者開展了研究工作。連續(xù)體機(jī)器人的運動學(xué)通常采用基于模型的方法或無模型的方法[6]。Hwang等[7]對具有切口式繩驅(qū)動的連續(xù)體機(jī)器人通過D-H(Diffie-Hellman)參數(shù)法建立了運動學(xué)模型。王騰等[8]利用增廣雅克比矩陣求解冗余機(jī)械臂的運動學(xué)模型。常曲率運動學(xué)模型具有計算速度快的特點。周圓圓等[9]提出了一種可運動解耦的連續(xù)體機(jī)器人,提出了遠(yuǎn)端與近端的連續(xù)體機(jī)器人的解耦方法。陳元科等[10]提出了分段常曲率與粒子群算法結(jié)合的運動學(xué)模型,用于絲驅(qū)動的連續(xù)體機(jī)器人。Chen等[11]針對微創(chuàng)手術(shù)提出了一種基于Cosserat桿理論的多段連續(xù)體機(jī)器人的運動學(xué)模型。Chen等[12]提出了考慮環(huán)境影響的分段常曲率方法,應(yīng)用于軟機(jī)械臂。盧佳佳等[13]提出了基于分段常曲率結(jié)合D-H參數(shù)法建立了連續(xù)體機(jī)械臂的正逆運動學(xué)模型。Wu等[14]采用了多段解耦的方法對腹腔鏡機(jī)器人進(jìn)行運動學(xué)建模。

然而,柔性連續(xù)體機(jī)器人的末端位置誤差大多數(shù)是由非線性因素引起的,非線性建模復(fù)雜,且模型精度有限。因此,一些學(xué)者已經(jīng)開始采用無模型的方法。Tan等[15]學(xué)者提出了一種基于歸零神經(jīng)動力學(xué)方法的連續(xù)體機(jī)器人的無模型控制方法。Wang等[16]提出了一種混合運動學(xué)模型,利用混合自適應(yīng)框架來為機(jī)器人提供額外的驅(qū)動量。張震等[17]采用粒子群優(yōu)化的徑向基函數(shù)神經(jīng)網(wǎng)絡(luò)來求解機(jī)械臂的運動學(xué)模型。

綜上所述,針對經(jīng)自然腔道手術(shù)的操作需求以及靈巧性的要求,設(shè)計一種基于繩驅(qū)動的多段柔性連續(xù)骨架構(gòu)型的連續(xù)體機(jī)械臂,具有全維度大角度彎曲能力。并且基于分段常曲率方法建立了連續(xù)體機(jī)械臂的正運動學(xué)映射;以連續(xù)體機(jī)械臂任務(wù)空間的末端位置為輸入,驅(qū)動空間的驅(qū)動繩拉伸量為輸出構(gòu)建神經(jīng)網(wǎng)絡(luò),擬合連續(xù)體機(jī)械臂的逆運動學(xué)映射,解決逆運動學(xué)求解困難且非線性誤差難以補(bǔ)償?shù)膯栴}。通過彎曲運動實驗和軌跡跟蹤實驗驗證連續(xù)體機(jī)械臂的運動性能以及運動學(xué)模型的有效性。

1 連續(xù)體機(jī)械臂結(jié)構(gòu)設(shè)計

繩驅(qū)動連續(xù)體機(jī)械臂結(jié)構(gòu)如圖1所示,所設(shè)計的連續(xù)體機(jī)械臂由雙段柔性連續(xù)體、三個導(dǎo)繩盤以及若干間隔盤串聯(lián)構(gòu)成。近端柔性連續(xù)體L1由三根近端驅(qū)動繩控制,三根驅(qū)動繩在圓周上間隔120°均勻分布,同樣地,遠(yuǎn)端柔性連續(xù)體L2由三根遠(yuǎn)端驅(qū)動繩控制。驅(qū)動繩采用直徑為d1規(guī)格的鎳鈦繩。遠(yuǎn)端柔性連續(xù)體的三根驅(qū)動繩需要穿過近端柔性連續(xù)體。

圖1 繩驅(qū)動連續(xù)體機(jī)械臂結(jié)構(gòu)Fig.1 The structure of the rope-driven continuum manipulator

其中,柔性連續(xù)體中的彈性骨架為鎳鈦合金骨架經(jīng)過激光切割加工一體化成型,設(shè)計了具有切口梁式的中心彈性骨架結(jié)構(gòu),具有直徑為D1的內(nèi)部工作通道,利用鎳鈦合金的超彈性來實現(xiàn)連續(xù)體機(jī)械臂的彎曲運動。導(dǎo)繩盤的內(nèi)側(cè)具有驅(qū)動繩的通道,從近端到遠(yuǎn)端的導(dǎo)繩盤分別為s1、s2、s3,導(dǎo)繩盤的外徑為D2。

2 連續(xù)體機(jī)械臂運動學(xué)模型

采用分段常曲率方法,求解正運動學(xué)映射;基于神經(jīng)網(wǎng)絡(luò)模型,求解逆運動學(xué)映射。構(gòu)建連續(xù)體機(jī)械臂末端位置與驅(qū)動繩拉伸量的運動學(xué)模型。任務(wù)空間、配置空間和驅(qū)動空間三個空間之間的運動映射關(guān)系如圖2所示。任務(wù)空間變量為(x,y,z),其中,x,y,z為笛卡爾坐標(biāo)系下末端連續(xù)體機(jī)械臂的坐標(biāo);配置空間變量為(α1,θ1,α2,θ2),其中,α為連續(xù)體機(jī)械臂的旋轉(zhuǎn)角,θ為彎曲角;驅(qū)動空間變量為(Δl1,Δl2,Δl3,Δl4,Δl5,Δl6),其中,Δl為六根驅(qū)動繩的拉伸量。

圖2 運動學(xué)映射關(guān)系圖Fig.2 Kinematic mapping diagram

2.1 連續(xù)體機(jī)械臂正運動學(xué)映射

單段連續(xù)體機(jī)械臂具有3個驅(qū)動自由度,長度為Li。其幾何理論模型如圖3所示,在連續(xù)體機(jī)械臂的底部中心建立坐標(biāo)系{Oi},頂部中心建立坐標(biāo)系{Oi2},坐標(biāo)系的x軸方向為指向三根驅(qū)動繩中的一根,z軸方向為沿著軸線方向,y軸由右手定則決定。坐標(biāo)系{Oi}經(jīng)過多次的旋轉(zhuǎn)與平移變換后得到坐標(biāo)系{Oi2}。因此,可得出坐標(biāo)系{Oi}到{Oi2}的齊次變換矩陣,即

trans[Ri(1-cosθi),0,0]rot(y,θi)rot(z,αi)

(1)

式(1)中:i表示連續(xù)體機(jī)械臂段數(shù);Ri為該段連續(xù)體機(jī)械臂的彎曲半徑。

兩段柔性連續(xù)體機(jī)械臂由兩個單段柔性連續(xù)體及三個導(dǎo)繩盤平移段串聯(lián)而成。本文中采用分段常曲率方法建立運動學(xué)模型,其幾何模型如圖1所示,各坐標(biāo)系的設(shè)置如下:在第一個平移段的底部中心建立基坐標(biāo)系{O0},近端柔性連續(xù)體底部中心建立坐標(biāo)系{O1},第二個平移段的底部中心建立坐標(biāo)系{O12},遠(yuǎn)端柔性連續(xù)體底部中心建立坐標(biāo)系{O2},第三個平移段的底部中心建立坐標(biāo)系{O22},柔性連續(xù)體機(jī)械臂的末端中心建立坐標(biāo)系{O3}。其中,坐標(biāo)系的方向設(shè)定法則與單段連續(xù)體機(jī)械臂設(shè)置一致。基于單段的運動學(xué)模型,采用分段常曲率方法進(jìn)行雙段連續(xù)體機(jī)械臂建模。柔性連續(xù)體機(jī)械臂的末端位置可由配置空間通過全局坐標(biāo)系和局部坐標(biāo)系的齊次變換矩陣轉(zhuǎn)換得出。

沿著第一個平移段的z軸方向平移其軸線長度s1,得到基坐標(biāo)系{O0}到坐標(biāo)系{O1}的齊次變換矩陣,即

(2)

沿著第二平移段的z軸方向平移其軸線長度s2,得到基坐標(biāo)系{O12}到坐標(biāo)系{O2}的齊次變換矩陣,即

(3)

近端柔性連續(xù)體的齊次變換矩陣由三部分的齊次變換矩陣連乘得出,即

(4)

式(4)中:R1為3×3的旋轉(zhuǎn)矩陣;P1為近端柔性連續(xù)體機(jī)械臂的末端位置向量,其表達(dá)式為

(5)

(6)

式(6)中:s表示正弦函數(shù)sin;c表示余弦函數(shù)cos。

沿著第三平移段的z軸方向平移其軸線長度s3,得到基坐標(biāo)系{O22}到坐標(biāo)系{O3}的齊次變換矩陣,即

(7)

遠(yuǎn)端柔性連續(xù)體機(jī)械臂的齊次變換矩陣由五部分的齊次變換矩陣連乘得出,即

(8)

式(8)中:R2為3×3的旋轉(zhuǎn)矩陣;P2為遠(yuǎn)端柔性連續(xù)體機(jī)械臂的末端位置向量,其表達(dá)式為

P2=[x3y3z3]T

(9)

(10)

兩段連續(xù)體機(jī)械臂的末端需要實現(xiàn)全維度的導(dǎo)向功能,其運動涉及解耦分析。當(dāng)近端連續(xù)體機(jī)械臂進(jìn)行彎曲和旋轉(zhuǎn)運動時,遠(yuǎn)端連續(xù)體機(jī)械臂為了保持相對于近端的靜止?fàn)顟B(tài),需要考慮近端的耦合影響,驅(qū)動空間的變量(Δl1,Δl2,Δl3,Δl4,Δl5,Δl6)隨之發(fā)生改變。其中,(Δl1,Δl3,Δl5)為近端連續(xù)體機(jī)械臂的驅(qū)動量,(Δl2,Δl4,Δl6)為遠(yuǎn)端連續(xù)體機(jī)械臂的驅(qū)動量,(Δl12,Δl14,Δl16)為遠(yuǎn)端驅(qū)動繩受近端連續(xù)體機(jī)械臂彎曲耦合影響的各驅(qū)動繩拉伸量。因此,配置空間中旋轉(zhuǎn)角、彎曲角與驅(qū)動空間中驅(qū)動繩的變化量關(guān)系為

(11)

(12)

式中:r為驅(qū)動繩通道圓心距連續(xù)體機(jī)械臂的軸心距離。

2.2 連續(xù)體機(jī)械臂逆運動學(xué)映射

連續(xù)體機(jī)械臂由于機(jī)械裝配、摩擦等非線性誤差,導(dǎo)致難以建立準(zhǔn)確的運動學(xué)模型。前饋神經(jīng)網(wǎng)絡(luò)具有良好的自適應(yīng)能力,通過對樣本的學(xué)習(xí)訓(xùn)練后能擬合非線性的輸入輸出關(guān)系,且結(jié)構(gòu)簡單,可操作性強(qiáng)。因此,本文中采用前饋神經(jīng)網(wǎng)絡(luò)進(jìn)行連續(xù)體機(jī)械臂的逆運動學(xué)求解。

神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)如圖4所示,建立了3輸入6輸出的雙層神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu),任務(wù)空間中機(jī)械臂的末端位置(x,y,z)為神網(wǎng)絡(luò)的輸入,驅(qū)動空間中各驅(qū)動繩拉伸量(Δl1,Δl2,Δl3,Δl4,Δl5,Δl6)為輸出。

w、b、f分別為權(quán)值、閾值和傳遞函數(shù)圖4 神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)Fig.4 Neural network structure

通過實驗采集的連續(xù)體機(jī)械臂末端位置和驅(qū)動繩拉伸量建立樣本數(shù)據(jù)集,在工作空間中共采集了1 400組樣本數(shù)據(jù),其中,70%的數(shù)據(jù)作為訓(xùn)練集,30%的數(shù)據(jù)作為測試集。學(xué)習(xí)率設(shè)置為0.3,前饋神經(jīng)網(wǎng)絡(luò)的流程圖如圖5所示,首先,生成訓(xùn)練集測試集,構(gòu)建神經(jīng)網(wǎng)絡(luò),進(jìn)行權(quán)值和閾值初始化,接下來,訓(xùn)練神經(jīng)網(wǎng)絡(luò),更新迭代權(quán)值和閾值,當(dāng)訓(xùn)練的輸出誤差達(dá)到訓(xùn)練精度要求時,將停止訓(xùn)練;未滿足結(jié)束條件,將繼續(xù)更新權(quán)值和閾值,最后,利用測試數(shù)據(jù)集進(jìn)行預(yù)測,測試神經(jīng)網(wǎng)絡(luò)訓(xùn)練效果并計算訓(xùn)練及測試誤差大小。

圖5 前饋神經(jīng)網(wǎng)絡(luò)流程圖Fig.5 Feedforward neural network flow chart

仿真結(jié)果如下:神經(jīng)網(wǎng)絡(luò)訓(xùn)練平均絕對誤差為0.215 mm,測試平均絕對誤差為0.192 mm。該神經(jīng)網(wǎng)絡(luò)的誤差量約為連續(xù)體機(jī)械臂長度的1%以下,該誤差量對連續(xù)體機(jī)械臂的運動控制影響較小,因此,可采用神經(jīng)網(wǎng)絡(luò)對連續(xù)體機(jī)械臂進(jìn)行逆運動學(xué)模型映射。

3 實驗及結(jié)果分析

本文中對連續(xù)體機(jī)械臂的彎曲能力和所提出的運動學(xué)模型的控制效果開展驗證實驗。連續(xù)體機(jī)械臂的實驗系統(tǒng)如圖6所示,該系統(tǒng)由操控模塊、上位機(jī)、驅(qū)動模塊、連續(xù)體機(jī)械臂、光學(xué)追蹤平臺(NDI Polaris Vega)組成。其中,上位機(jī)發(fā)送運動指令給驅(qū)動模塊,也可以采用經(jīng)操控模塊和上位機(jī)發(fā)送運動指令給驅(qū)動模塊,驅(qū)動模塊驅(qū)動連續(xù)體機(jī)械臂進(jìn)行運動,光學(xué)追蹤平臺用于采集連續(xù)體機(jī)械臂的末端坐標(biāo)信息。連續(xù)體機(jī)械臂的設(shè)計參數(shù)如表1所示。

表1 連續(xù)體機(jī)械臂的設(shè)計參數(shù)

圖6 連續(xù)體機(jī)械臂實驗系統(tǒng)組成Fig.6 Continuum manipulator experiment system composition

3.1 彎曲性能驗證實驗

連續(xù)體機(jī)械臂的彎曲性能驗證實驗設(shè)置為機(jī)械臂分別進(jìn)行向上、向下、向左、向右四個方向的彎曲運動。圖7為四個方向的彎曲運動效果圖,顯示雙段連續(xù)體機(jī)械臂可完成彎曲角度約為180°的大角度彎曲運動。彎曲性能實驗驗證了所設(shè)計的連續(xù)體機(jī)械臂具有大角度彎曲運動的能力,具有在狹窄、復(fù)雜的腔道內(nèi)彎曲的潛力。

圖7 連續(xù)體機(jī)械臂彎曲性能驗證實驗圖Fig.7 Verification experiment of bending performance of continuum manipulator

3.2 軌跡跟蹤實驗

軌跡跟蹤實驗的目的是為了驗證連續(xù)體機(jī)械臂運動學(xué)模型的效果。在雙段連續(xù)體機(jī)械臂末端以及連續(xù)體機(jī)械臂的底部粘貼光學(xué)標(biāo)記紙,利用光學(xué)追蹤平臺識別標(biāo)記紙的位置信息,采集經(jīng)神經(jīng)網(wǎng)絡(luò)擬合的運動學(xué)模型驅(qū)動后連續(xù)體機(jī)械臂的末端位置坐標(biāo)。

在連續(xù)體機(jī)械臂的工作空間中,進(jìn)行半徑為25 mm的圓形軌跡跟蹤,連續(xù)體機(jī)械臂的運動軌跡如圖8所示,紅色為預(yù)期軌跡,藍(lán)色為實際軌跡,實際軌跡與預(yù)期軌跡存在一定的誤差。在各坐標(biāo)軸方向上的軌跡跟蹤誤差如圖9所示,重復(fù)進(jìn)行5次圓軌跡跟蹤實驗,得到連續(xù)體機(jī)械臂末端在x軸、y軸、z軸方向的平均絕對位置誤差為1.743、1.334 mm和1.172 mm。實驗結(jié)果表明所提出的運動學(xué)模型有效的補(bǔ)償了由于摩擦、裝配等產(chǎn)生的非線性誤差。但經(jīng)過分析,神經(jīng)網(wǎng)絡(luò)雖然補(bǔ)償了一些位置誤差,但是仍有一些誤差的引入,如光學(xué)標(biāo)記貼紙位置的偏差。

圖8 連續(xù)體機(jī)械臂運動軌跡圖Fig.8 Motion trajectory diagram of continuum manipulator

圖9 連續(xù)體機(jī)械臂軌跡跟蹤誤差圖Fig.9 The continuum manipulator trajectory tracking error

4 結(jié)論

本文設(shè)計了一種應(yīng)用于經(jīng)自然腔道手術(shù)的繩驅(qū)動的多段連續(xù)體機(jī)械臂,并基于分段常曲率和神經(jīng)網(wǎng)絡(luò)的方法,構(gòu)建了任務(wù)空間和驅(qū)動空間的正逆運動學(xué)映射,擬合出了連續(xù)體機(jī)械臂末端位置和驅(qū)動繩拉伸量的映射關(guān)系,仿真測試誤差約為連續(xù)體機(jī)械臂長度的1%以下,表明所提出的方法可用于連續(xù)體機(jī)械臂的運動學(xué)模型。最后,對連續(xù)體機(jī)械臂的彎曲能力和運動學(xué)模型效果進(jìn)行了驗證實驗,連續(xù)體機(jī)械臂的彎曲性能實驗表明該機(jī)械臂具有良好的彎曲能力,可以實現(xiàn)大角度彎曲變形;軌跡跟蹤實驗結(jié)果顯示,基于運動學(xué)模型的驅(qū)動,在半徑為25 mm的圓軌跡上,連續(xù)體機(jī)械臂末端在x軸、y軸、z軸方向的平均絕對位置誤差為1.743 、1.334和1.172 mm,表明所提出的雙段連續(xù)體機(jī)械臂的運動學(xué)模型的有效性,可用于連續(xù)體機(jī)械臂的運動控制。接下來,我們將在采集連續(xù)體機(jī)械臂更全面的位置信息和實時閉環(huán)位置控制上展開研究工作。