雙目立體視覺線驅(qū)動蛇形手術(shù)機(jī)器人的形狀重建

王宏民， 王 平， 呂雄飛， 蘇鳳武， 康紅明

(1. 五邑大學(xué) 機(jī)電工程學(xué)院， 廣州 江門 529020； 2.黑龍江科技大學(xué) 電氣工程與控制學(xué)院， 哈爾濱 150022)

雙目立體視覺線驅(qū)動蛇形手術(shù)機(jī)器人的形狀重建

王宏民1，2， 王 平2， 呂雄飛2， 蘇鳳武2， 康紅明2

(1. 五邑大學(xué) 機(jī)電工程學(xué)院， 廣州 江門 529020； 2.黑龍江科技大學(xué) 電氣工程與控制學(xué)院， 哈爾濱 150022)

針對柔性手術(shù)機(jī)器人較難實(shí)時(shí)獲取末端柔性段位置和形狀信息的問題，提出一種基于雙目立體視覺技術(shù)的線驅(qū)動蛇形機(jī)器人形狀重建算法。該算法采用雙目攝像機(jī)采集20組蛇形機(jī)器人柔性段不同角度的圖片信息作為樣本，利用Matlab標(biāo)定工具箱標(biāo)定圖片樣本，提取蛇形機(jī)器人標(biāo)記點(diǎn)特征與形狀重建信息。標(biāo)定好的三維立體視覺系統(tǒng)實(shí)時(shí)獲取機(jī)器人柔性段關(guān)節(jié)標(biāo)記點(diǎn)圖像，根據(jù)獲取的關(guān)節(jié)標(biāo)記點(diǎn)信息實(shí)現(xiàn)曲線擬合和形狀重建。實(shí)驗(yàn)結(jié)果表明：實(shí)驗(yàn)值和預(yù)測值存在一定的誤差，但幾乎在同一條曲線上，證明該方法應(yīng)用的可行性。

柔性手術(shù)機(jī)器人； 形狀重建； 雙目視覺

0 引 言

微創(chuàng)外科手術(shù)機(jī)器人具有失血少、恢復(fù)快、創(chuàng)傷小等優(yōu)點(diǎn)，給病人的治療帶來了很大的益處[1]。目前，商業(yè)化的微創(chuàng)手術(shù)機(jī)器人安裝的大多是剛性機(jī)械臂，然而，剛性機(jī)械臂具有工作空間小，開孔多等缺點(diǎn)。線驅(qū)動柔性手術(shù)機(jī)器人和同心管手術(shù)機(jī)器人在自然腔道內(nèi)窺鏡手術(shù)和單孔介入手術(shù)中得到了廣泛應(yīng)用[2-3]。

柔性手術(shù)機(jī)器人采用的連續(xù)體機(jī)構(gòu)相對于剛性機(jī)械臂屬于線驅(qū)動系統(tǒng)，因此，較難獲取關(guān)節(jié)信息。為了達(dá)到較好的閉環(huán)控制效果和操作安全性，實(shí)時(shí)獲取柔性手術(shù)機(jī)器人位置反饋和形狀信息。位置和形狀的預(yù)測多用運(yùn)動學(xué)模型和等曲率假設(shè)相結(jié)合的方法，只能預(yù)測無負(fù)載情況下柔性機(jī)器人的形狀，在有外部負(fù)載的情況下，等曲率假設(shè)就是不合理的[4]。有些學(xué)者利用克斯?fàn)枟U理論和靜力學(xué)模型預(yù)測連續(xù)體機(jī)器人形狀，然而，這些方法并沒有考慮摩擦等外部擾動信息。目前，很多形狀信息的獲取采用FBG或者EMT傳感器的方法對柔性機(jī)械臂的形狀重建[5-6]。

盡管此類方法比較有效，但價(jià)格較貴并且需要有效的曲線擬合算法，受環(huán)境影響也較大。筆者采用雙目立體視覺的方法對線驅(qū)動蛇形手術(shù)機(jī)器人進(jìn)行三維形狀重建，通過對柔性關(guān)節(jié)的標(biāo)定點(diǎn)獲取其空間位置信息，實(shí)時(shí)擬合柔性段的形狀。該方法只擬采用兩臺攝像頭結(jié)合Matlab的標(biāo)定工具箱就可以完成蛇形手術(shù)機(jī)器人的形狀重建。

1 雙目立體視覺形狀重建算法

1.1三維測量原理

柔性機(jī)器人的形狀重建算法是基于雙目立體視覺成像技術(shù)。雙目立體視覺進(jìn)行三維重建是指利用攝像機(jī)從不同角度獲取同一物體的兩幅圖形，利用三維重建原理，由計(jì)算機(jī)重建物體的三維形狀，恢復(fù)出物體的空間位置信息，三維測量成像原理，如圖1所示。

圖1 雙目視覺成像原理

Fig.1Supportbeamspacingeffectsonstressanddeformationstrain

兩臺攝像機(jī)的投影中心連線的距離定義為基準(zhǔn)距l(xiāng)，兩臺攝像機(jī)同時(shí)觀看空間物體的特征點(diǎn)為p(xc,yc,zc)，分別在左右視圖上獲取P點(diǎn)的圖像，圖像坐標(biāo)分別為和p1(xr,yr)。假設(shè)兩臺攝像機(jī)的圖像位于同一平面，即特征點(diǎn)P的圖像y軸相同，滿足條件y1=yr=y，由透視變換原理得到：

(1)

由此，可以計(jì)算出物體特性點(diǎn)在左攝像機(jī)坐標(biāo)系下的三維坐標(biāo)為：

(2)

式中，D——視差，D=|x1-xr|。

因此，左攝像機(jī)圖像上的任意一點(diǎn)只要能夠在右攝像機(jī)圖像上找到對應(yīng)的匹配點(diǎn)，就可以根據(jù)攝像機(jī)的固有幾何約束確定該點(diǎn)三維坐標(biāo)[7]。

1.2關(guān)節(jié)標(biāo)記點(diǎn)的形狀跟蹤算法

基于Matlab標(biāo)定工具箱完成立體視覺重建，三維立體視覺重建系統(tǒng)由三個(gè)步驟組成：立體視覺標(biāo)定、標(biāo)記點(diǎn)特征提取和形狀重建。立體視覺系統(tǒng)參數(shù)標(biāo)定分為兩個(gè)過程：根據(jù)給定的圖像標(biāo)定每臺攝像機(jī)的內(nèi)容參數(shù)，根據(jù)內(nèi)部參數(shù)標(biāo)定結(jié)果標(biāo)定立體視覺系統(tǒng)的外部參數(shù)。黑白相間的棋盤格標(biāo)定塊的尺寸為270 mm×210 mm，每個(gè)柵格的規(guī)格為30 mm×30 mm，將其固定在支架的平面板上。利用兩平行的攝像頭采集標(biāo)定板不同角度的圖像，根據(jù)經(jīng)驗(yàn)每個(gè)攝像頭拍攝20副圖像用于標(biāo)定。

根據(jù)采集的圖像依次完成所有圖像的角點(diǎn)提取后，完成對攝像機(jī)的立體標(biāo)定，得到來兩臺攝像機(jī)之間的位置關(guān)系的平移向量和旋轉(zhuǎn)矩陣信息，攝像機(jī)外部參數(shù)三維顯示如圖2所示。

提取網(wǎng)絡(luò)角點(diǎn)后，就可以計(jì)算出攝像頭的內(nèi)部參數(shù)。同樣讀入右攝像頭的圖像，對其進(jìn)行標(biāo)定，標(biāo)定方法同上。得到兩個(gè)攝像頭的內(nèi)部參數(shù)后，即可完成兩個(gè)攝像頭構(gòu)成的立體視覺系統(tǒng)的標(biāo)定，在得到左右攝像頭內(nèi)部參數(shù)的基礎(chǔ)上，可以得到攝像頭的外部參數(shù)。

圖2 攝像機(jī)外部參數(shù)三維顯示

兩臺攝像機(jī)的標(biāo)定誤差分別如圖3和圖4所示，誤差范圍集中分布在±0.6 mm的范圍內(nèi)， 通過分析可知，標(biāo)定誤差是在合理分布范圍。

圖3 左攝像機(jī)標(biāo)定誤差分布

圖4 右攝像機(jī)標(biāo)定誤差分布

2 柔性機(jī)器人三維形狀的重建

2.1機(jī)器人結(jié)構(gòu)

自然界中的蛇、象鼻和章魚觸手等，具有良好的靈巧性和運(yùn)動能力，根據(jù)該類動物組織結(jié)構(gòu)的特點(diǎn)設(shè)計(jì)了應(yīng)用于微創(chuàng)外科手術(shù)的線驅(qū)動蛇形手術(shù)機(jī)器人，機(jī)器人柔性臂機(jī)構(gòu)如圖5所示。柔性段采用線驅(qū)動方式，線驅(qū)動蛇形機(jī)器人由線驅(qū)動關(guān)節(jié)、約束機(jī)構(gòu)和牽引繩組成，每一個(gè)關(guān)節(jié)都具有一個(gè)中孔和四個(gè)導(dǎo)向孔，中孔可以通過約束桿，導(dǎo)向孔用來穿牽引線。

圖5 線驅(qū)動機(jī)器人結(jié)構(gòu)

2.2實(shí)驗(yàn)平臺系統(tǒng)的搭建

通過在每個(gè)柔性關(guān)節(jié)上的標(biāo)記點(diǎn)提取關(guān)節(jié)信息，每一臺攝像機(jī)根據(jù)攝像頭檢測的像素點(diǎn)獲取關(guān)節(jié)特性點(diǎn)在空間的三維坐標(biāo)，擬合后即可完成三維形狀重建。 為了獲取每一個(gè)關(guān)節(jié)的位置信息，兩臺Logitech網(wǎng)絡(luò)攝像頭(HD webcam c255)安裝后保證位置固定。機(jī)器人由10段關(guān)節(jié)組成，每段關(guān)節(jié)的長度為10 mm，關(guān)節(jié)旋轉(zhuǎn)角度為14°，每一段關(guān)節(jié)上都有標(biāo)記點(diǎn)。機(jī)器人通過步進(jìn)電機(jī)驅(qū)動完成在工作空間的運(yùn)動，實(shí)驗(yàn)平臺，如圖6所示。

圖6 柔性機(jī)器人系統(tǒng)實(shí)驗(yàn)平臺

根據(jù)標(biāo)定好的三維立體視覺系統(tǒng)實(shí)驗(yàn)平臺，實(shí)時(shí)獲取機(jī)器人在兩臺攝像機(jī)上的標(biāo)記點(diǎn)圖像，柔性機(jī)器人的彎曲形狀就可以根據(jù)獲取的關(guān)節(jié)標(biāo)記點(diǎn)信息進(jìn)行曲線擬合重建。在實(shí)驗(yàn)中由于限制了機(jī)器人在平面上運(yùn)動，因此，給出二維xy平面下的重建曲線，如圖7所示。為了驗(yàn)證基于雙目立體視覺形狀重建方法的性能，根據(jù)給定的機(jī)器人彎曲軌跡，投影到坐標(biāo)紙上如圖8所示，由圖8可見，實(shí)際值和預(yù)測值幾乎在同一條曲線上。

a s形軌跡

b c形軌跡

a

b

Fig.8Constrainttrajectoryunderpredictedcurveandactualcurve

3 結(jié)束語

通過研究雙目立體視覺的線驅(qū)動蛇形手術(shù)機(jī)器人形狀的重建，基于雙目立體視覺成像技術(shù)構(gòu)建了機(jī)器人形狀重建的算法，在Matlab工具箱完成了立體視覺標(biāo)定、標(biāo)記點(diǎn)特征提取和形狀重建。通過柔性關(guān)節(jié)的標(biāo)記點(diǎn)提取了關(guān)節(jié)信息在實(shí)驗(yàn)平臺系統(tǒng)完成三維形狀重建，在標(biāo)定好的三維立體視覺系統(tǒng)實(shí)驗(yàn)平臺，實(shí)時(shí)獲取了機(jī)器人在兩臺攝像機(jī)上的標(biāo)記點(diǎn)圖像，柔性機(jī)器人的彎曲形狀可以根據(jù)獲取的關(guān)節(jié)標(biāo)記點(diǎn)信息進(jìn)行曲線擬合重建。根據(jù)給定的機(jī)器人彎曲軌跡和重建的擬合曲線對比，驗(yàn)證了基于雙目立體視覺形狀重建方法的性能，雖然存在一定的誤差，但是實(shí)驗(yàn)值和預(yù)測值幾乎在同一條曲線上，證明了該方法的有效性，下一步可以采用誤差補(bǔ)償進(jìn)一步提高形狀重建的精度。

[1] Li Z, Feiling J, Ren H, et al. A novel tele-operated flexible robot targeted for minimally invasive robot surgery [J].Engineering, 2015(1): 73-78.

[2] Xu W. A feed-forward friction compensation motion controller for a tendon-sheath-driven flexible robotics and Biomimetics (ROBIO), 2013 IEEE International Conference on Robotics and Automation, Karlsruhe: IEEE, 2016(1): 69-80.

[3] Hu B, Yu J J, Y Lu, et al. Statics and stiffness model of serial-parallel manipulator formed by k parallel manipulators connected in series [J]. Journal of Mechanisms and Robotics-Transactions of The ASME, 2012, 20(2): 66-70.

[4] Camarillo D B, Milne C F, Carlson C R, et al. Mechanics modeling of tendon-driven continuum manipulators [J]. IEEE Transactions on Robotics, 2008, 24(6)： 1262-1273.

[5] Roesthuis R J, Misra S. Steering of multisegment continuum manipulators using rigid-link modeling and FBG-based shape sensing [J]. IEEE Transactions on Robotics, 2016, 32(2): 372-382.

[6] Yun Y， Li Y. Modeling and control analysis of a 3-PUPUdual compliant parallel manipulator for micropositioning and active vibration isolation [J]．Journal of dynamic system measurement and control, Transactions of ASME， 2012， 134(2): 21-25．

[7] 于 勇， 張 暉， 林茂松. 基于雙目立體視覺三維重建系統(tǒng)的研究與設(shè)計(jì)[J]. 計(jì)算機(jī)技術(shù)與發(fā)展, 2009, 19(6): 127-130.

(編校李德根)

Shapereconstructionforwire-drivensnake-likerobotsbasedonbinocularstereovision

(1.Mechanical & Electrical Engineering Institute, Wuyi University， Jiangmen 529020, China; 2.School of Electrical & Control Engineering， Heilongjiang University of Science & Technology， Harbin 150022, China)

This paper is a response to the challenges facing flexible surgical robots, as in the case of real-time access to the position and shape information of the end flexible segment and presents an algorithm behind the shape reconstruction of wire-driven snake-like based on the binocular stereo vision technology. This algorithm works firstly by using the binocular camera to collect the picture information of 20 groups of serpentine robotic flexible segment from different angles; using Matlab Calibration Toolbox to calibrate the picture sample; and completing feature extraction and shape reconstruction of snake robot marker points in such a way that the calibrated 3D stereo vision system is capable of both real time acquisition of joint mark image of robot flexible segment and curve fitting and shape reconstruction based on acquired joint mark points information. The experimental results verify that the algorithm could work better thanks to the fact that the experimental values and the predicted values are almost on the same curve regardless of a small error found by comparing the trajectory of the flexible segment of the robot with the reconstructed trajectory,

flexible surgical robot; shape reconstruction; binocular vision

10.3969/j.issn.2095-7262.2017.06.024

TN953

2095-7262(2017)06-0694-04

A

2017-09-25

黑龍江省教育廳科學(xué)技術(shù)研究項(xiàng)目(12541713)

王宏民(1978-)，男，河北省承德人，副教授，碩士，研究方向：自動控制、機(jī)器人控制，E-mail：wang_hongmin@163.com。