双目立体视觉线驱动蛇形手术机器人的形状重建

王宏民， 王 平， 吕雄飞， 苏凤武， 康红明

(1. 五邑大学 机电工程学院， 广州 江门 529020； 2.黑龙江科技大学 电气工程与控制学院， 哈尔滨 150022)

双目立体视觉线驱动蛇形手术机器人的形状重建

王宏民1，2， 王 平2， 吕雄飞2， 苏凤武2， 康红明2

(1. 五邑大学 机电工程学院， 广州 江门 529020； 2.黑龙江科技大学 电气工程与控制学院， 哈尔滨 150022)

针对柔性手术机器人较难实时获取末端柔性段位置和形状信息的问题，提出一种基于双目立体视觉技术的线驱动蛇形机器人形状重建算法。该算法采用双目摄像机采集20组蛇形机器人柔性段不同角度的图片信息作为样本，利用Matlab标定工具箱标定图片样本，提取蛇形机器人标记点特征与形状重建信息。标定好的三维立体视觉系统实时获取机器人柔性段关节标记点图像，根据获取的关节标记点信息实现曲线拟合和形状重建。实验结果表明：实验值和预测值存在一定的误差，但几乎在同一条曲线上，证明该方法应用的可行性。

柔性手术机器人； 形状重建； 双目视觉

0 引 言

微创外科手术机器人具有失血少、恢复快、创伤小等优点，给病人的治疗带来了很大的益处[1]。目前，商业化的微创手术机器人安装的大多是刚性机械臂，然而，刚性机械臂具有工作空间小，开孔多等缺点。线驱动柔性手术机器人和同心管手术机器人在自然腔道内窥镜手术和单孔介入手术中得到了广泛应用[2-3]。

柔性手术机器人采用的连续体机构相对于刚性机械臂属于线驱动系统，因此，较难获取关节信息。为了达到较好的闭环控制效果和操作安全性，实时获取柔性手术机器人位置反馈和形状信息。位置和形状的预测多用运动学模型和等曲率假设相结合的方法，只能预测无负载情况下柔性机器人的形状，在有外部负载的情况下，等曲率假设就是不合理的[4]。有些学者利用克斯尔杆理论和静力学模型预测连续体机器人形状，然而，这些方法并没有考虑摩擦等外部扰动信息。目前，很多形状信息的获取采用FBG或者EMT传感器的方法对柔性机械臂的形状重建[5-6]。

尽管此类方法比较有效，但价格较贵并且需要有效的曲线拟合算法，受环境影响也较大。笔者采用双目立体视觉的方法对线驱动蛇形手术机器人进行三维形状重建，通过对柔性关节的标定点获取其空间位置信息，实时拟合柔性段的形状。该方法只拟采用两台摄像头结合Matlab的标定工具箱就可以完成蛇形手术机器人的形状重建。

1 双目立体视觉形状重建算法

1.1三维测量原理

柔性机器人的形状重建算法是基于双目立体视觉成像技术。双目立体视觉进行三维重建是指利用摄像机从不同角度获取同一物体的两幅图形，利用三维重建原理，由计算机重建物体的三维形状，恢复出物体的空间位置信息，三维测量成像原理，如图1所示。

图1 双目视觉成像原理

Fig.1Supportbeamspacingeffectsonstressanddeformationstrain

两台摄像机的投影中心连线的距离定义为基准距l，两台摄像机同时观看空间物体的特征点为p(xc,yc,zc)，分别在左右视图上获取P点的图像，图像坐标分别为和p1(xr,yr)。假设两台摄像机的图像位于同一平面，即特征点P的图像y轴相同，满足条件y1=yr=y，由透视变换原理得到：

(1)

由此，可以计算出物体特性点在左摄像机坐标系下的三维坐标为：

(2)

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

因此，左摄像机图像上的任意一点只要能够在右摄像机图像上找到对应的匹配点，就可以根据摄像机的固有几何约束确定该点三维坐标[7]。

1.2关节标记点的形状跟踪算法

基于Matlab标定工具箱完成立体视觉重建，三维立体视觉重建系统由三个步骤组成：立体视觉标定、标记点特征提取和形状重建。立体视觉系统参数标定分为两个过程：根据给定的图像标定每台摄像机的内容参数，根据内部参数标定结果标定立体视觉系统的外部参数。黑白相间的棋盘格标定块的尺寸为270 mm×210 mm，每个栅格的规格为30 mm×30 mm，将其固定在支架的平面板上。利用两平行的摄像头采集标定板不同角度的图像，根据经验每个摄像头拍摄20副图像用于标定。

根据采集的图像依次完成所有图像的角点提取后，完成对摄像机的立体标定，得到来两台摄像机之间的位置关系的平移向量和旋转矩阵信息，摄像机外部参数三维显示如图2所示。

提取网络角点后，就可以计算出摄像头的内部参数。同样读入右摄像头的图像，对其进行标定，标定方法同上。得到两个摄像头的内部参数后，即可完成两个摄像头构成的立体视觉系统的标定，在得到左右摄像头内部参数的基础上，可以得到摄像头的外部参数。

图2 摄像机外部参数三维显示

两台摄像机的标定误差分别如图3和图4所示，误差范围集中分布在±0.6 mm的范围内， 通过分析可知，标定误差是在合理分布范围。

图3 左摄像机标定误差分布

图4 右摄像机标定误差分布

2 柔性机器人三维形状的重建

2.1机器人结构

自然界中的蛇、象鼻和章鱼触手等，具有良好的灵巧性和运动能力，根据该类动物组织结构的特点设计了应用于微创外科手术的线驱动蛇形手术机器人，机器人柔性臂机构如图5所示。柔性段采用线驱动方式，线驱动蛇形机器人由线驱动关节、约束机构和牵引绳组成，每一个关节都具有一个中孔和四个导向孔，中孔可以通过约束杆，导向孔用来穿牵引线。

图5 线驱动机器人结构

2.2实验平台系统的搭建

通过在每个柔性关节上的标记点提取关节信息，每一台摄像机根据摄像头检测的像素点获取关节特性点在空间的三维坐标，拟合后即可完成三维形状重建。 为了获取每一个关节的位置信息，两台Logitech网络摄像头(HD webcam c255)安装后保证位置固定。机器人由10段关节组成，每段关节的长度为10 mm，关节旋转角度为14°，每一段关节上都有标记点。机器人通过步进电机驱动完成在工作空间的运动，实验平台，如图6所示。

图6 柔性机器人系统实验平台

根据标定好的三维立体视觉系统实验平台，实时获取机器人在两台摄像机上的标记点图像，柔性机器人的弯曲形状就可以根据获取的关节标记点信息进行曲线拟合重建。在实验中由于限制了机器人在平面上运动，因此，给出二维xy平面下的重建曲线，如图7所示。为了验证基于双目立体视觉形状重建方法的性能，根据给定的机器人弯曲轨迹，投影到坐标纸上如图8所示，由图8可见，实际值和预测值几乎在同一条曲线上。

a s形轨迹

b c形轨迹

a

b

Fig.8Constrainttrajectoryunderpredictedcurveandactualcurve

3 结束语

通过研究双目立体视觉的线驱动蛇形手术机器人形状的重建，基于双目立体视觉成像技术构建了机器人形状重建的算法，在Matlab工具箱完成了立体视觉标定、标记点特征提取和形状重建。通过柔性关节的标记点提取了关节信息在实验平台系统完成三维形状重建，在标定好的三维立体视觉系统实验平台，实时获取了机器人在两台摄像机上的标记点图像，柔性机器人的弯曲形状可以根据获取的关节标记点信息进行曲线拟合重建。根据给定的机器人弯曲轨迹和重建的拟合曲线对比，验证了基于双目立体视觉形状重建方法的性能，虽然存在一定的误差，但是实验值和预测值几乎在同一条曲线上，证明了该方法的有效性，下一步可以采用误差补偿进一步提高形状重建的精度。

[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] 于 勇， 张 晖， 林茂松. 基于双目立体视觉三维重建系统的研究与设计[J]. 计算机技术与发展, 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。