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

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

雙目立體視覺線驅動蛇形手術機器人的形狀重建

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

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

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

柔性手術機器人； 形狀重建； 雙目視覺

0 引 言

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

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

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

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

1.1三維測量原理

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

圖1 雙目視覺成像原理

Fig.1Supportbeamspacingeffectsonstressanddeformationstrain

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

(1)

由此，可以計算出物體特性點在左攝像機坐標系下的三維坐標為：

(2)

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

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

1.2關節(jié)標記點的形狀跟蹤算法

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

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

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

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

兩臺攝像機的標定誤差分別如圖3和圖4所示，誤差范圍集中分布在±0.6 mm的范圍內， 通過分析可知，標定誤差是在合理分布范圍。

圖3 左攝像機標定誤差分布

圖4 右攝像機標定誤差分布

2 柔性機器人三維形狀的重建

2.1機器人結構

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

圖5 線驅動機器人結構

2.2實驗平臺系統(tǒng)的搭建

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

圖6 柔性機器人系統(tǒng)實驗平臺

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

a s形軌跡

b c形軌跡

a

b

Fig.8Constrainttrajectoryunderpredictedcurveandactualcurve

3 結束語

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

[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)的研究與設計[J]. 計算機技術與發(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

黑龍江省教育廳科學技術研究項目(12541713)

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