第27卷第6期2017年11月黑龙江科技大学学报JournalofHeilongjiangUniversityofScience＆TechnologyVol．27No．6Nov．2017双目立体视觉线驱动蛇形手术机器人的形状重建王宏民1，2，王平2，吕雄飞2，苏凤武2，康红明2(1．五邑大学机电工程学院，广州江门529020;2．黑龙江科技大学电气工程与控制学院，哈尔滨150022)摘要:针对柔性手术机器人较难实时获取末端柔性段位置和形状信息的问题，提出一种基于双目立体视觉技术的线驱动蛇形机器人形状重建算法。该算法采用双目摄像机采集20组蛇形机器人柔性段不同角度的图片信息作为样本，利用Matlab标定工具箱标定图片样本，提取蛇形机器人标记点特征与形状重建信息。标定好的三维立体视觉系统实时获取机器人柔性段关节标记点图像，根据获取的关节标记点信息实现曲线拟合和形状重建。实验结果表明:实验值和预测值存在一定的误差，但几乎在同一条曲线上，证明该方法应用的可行性。关键词:柔性手术机器人;形状重建;双目视觉doi:10．3969/j．issn．2095－7262．2017．06．024中图分类号:TN953文章编号:2095－7262(2017)06－0694－04文献标志码:AShapereconstructionforwire-drivensnake-likerobotsbasedonbinocularstereovisionWangHongmin1，2，WangPing2，Lu．．Xiongfei2，SuFengwu2，KangHongming2(1．Mechanical＆ElectricalEngineeringInstitute，WuyiUniversity，Jiangmen529020，China;2．SchoolofElectrical＆ControlEngineering，HeilongjiangUniversityofScience＆Technology，Harbin150022，China)Abstract:Thispaperisaresponsetothechallengesfacingflexiblesurgicalrobots，asinthecaseofreal-timeaccesstothepositionandshapeinformationoftheendflexiblesegmentandpresentsanalgo-rithmbehindtheshapereconstructionofwire-drivensnake-likebasedonthebinocularstereovisiontech-nology．Thisalgorithmworksfirstlybyusingthebinocularcameratocollectthepictureinformationof20groupsofserpentineroboticflexiblesegmentfromdifferentangles;usingMatlabCalibrationToolboxtocalibratethepicturesample;andcompletingfeatureextractionandshapereconstructionofsnakerobotmarkerpointsinsuchawaythatthecalibrated3Dstereovisionsystemiscapableofbothrealtimeacqui-sitionofjointmarkimageofrobotflexiblesegmentandcurvefittingandshapereconstructionbasedonac-quiredjointmarkpointsinformation．Theexperimentalresultsverifythatthealgorithmcouldworkbetterthankstothefactthattheexperimentalvaluesandthepredictedvaluesarealmostonthesamecurvere-gardlessofasmallerrorfoundbycomparingthetrajectoryoftheflexiblesegmentoftherobotwiththere-constructedtrajectory，Keywords:flexiblesurgicalrobot;shapereconstruction;binocularvision收稿日期:2017－09－25基金项目:黑龙江省教育厅科学技术研究项目(12541713)第一作者简介:王宏民(1978－)，男，河北省承德人，副教授，硕士，研究方向:自动控制、机器人控制，E-mail:wang_hongmin@163．com。中国煤炭期刊网 www.chinacaj.net

0引言微创外科手术机器人具有失血少、恢复快、创伤小等优点，给病人的治疗带来了很大的益处［1］。目前，商业化的微创手术机器人安装的大多是刚性机械臂，然而，刚性机械臂具有工作空间小，开孔多等缺点。线驱动柔性手术机器人和同心管手术机器人在自然腔道内窥镜手术和单孔介入手术中得到了广泛应用［2－3］。柔性手术机器人采用的连续体机构相对于刚性机械臂属于线驱动系统，因此，较难获取关节信息。为了达到较好的闭环控制效果和操作安全性，实时获取柔性手术机器人位置反馈和形状信息。位置和形状的预测多用运动学模型和等曲率假设相结合的方法，只能预测无负载情况下柔性机器人的形状，在有外部负载的情况下，等曲率假设就是不合理的［4］。有些学者利用克斯尔杆理论和静力学模型预测连续体机器人形状，然而，这些方法并没有考虑摩擦等外部扰动信息。目前，很多形状信息的获取采用FBG或者EMT传感器的方法对柔性机械臂的形状重建［5－6］。尽管此类方法比较有效，但价格较贵并且需要有效的曲线拟合算法，受环境影响也较大。笔者采用双目立体视觉的方法对线驱动蛇形手术机器人进行三维形状重建，通过对柔性关节的标定点获取其空间位置信息，实时拟合柔性段的形状。该方法只拟采用两台摄像头结合Matlab的标定工具箱就可以完成蛇形手术机器人的形状重建。1双目立体视觉形状重建算法1.1三维测量原理柔性机器人的形状重建算法是基于双目立体视觉成像技术。双目立体视觉进行三维重建是指利用摄像机从不同角度获取同一物体的两幅图形，利用三维重建原理，由计算机重建物体的三维形状，恢复出物体的空间位置信息，三维测量成像原理，如图1所示。图1双目视觉成像原理Fig．1Supportbeamspacingeffectsonstressanddeform-ationstrain两台摄像机的投影中心连线的距离定义为基准距l，两台摄像机同时观看空间物体的特征点为p(xc，yc，zc)，分别在左右视图上获取P点的图像，图像坐标分别为和p1(xr，yr)。假设两台摄像机的图像位于同一平面，即特征点P的图像y轴相同，满足条件y1=yr=y，由透视变换原理得到:x1=fxcz()c，xr=fxc－lz()c，y=fycz()c。(1)由此，可以计算出物体特性点在左摄像机坐标系下的三维坐标为:xc=lx1D，yc=lyD，zc=lfD，(2)式中，D———视差，D=|x1－xr|。因此，左摄像机图像上的任意一点只要能够在右摄像机图像上找到对应的匹配点，就可以根据摄像机的固有几何约束确定该点三维坐标［7］。1.2关节标记点的形状跟踪算法基于Matlab标定工具箱完成立体视觉重建，三维立体视觉重建系统由三个步骤组成:立体视觉标定、标记点特征提取和形状重建。立体视觉系统参数标定分为两个过程:根据给定的图像标定每台摄像机的内容参数，根据内部参数标定结果标定立体视觉系统的外部参数。黑白相间的棋盘格标定块的尺寸为270mm×210mm，每个栅格的规格为30mm×30mm，将其固定在支架的平面板上。利用两平行的摄像头采集标定板不同角度的图像，根据经验每个摄像头拍摄20副图像用于标定。根据采集的图像依次完成所有图像的角点提取后，完成对摄像机的立体标定，得到来两台摄像机之间的位置关系的平移向量和旋转矩阵信息，摄像机外部参数三维显示如图2所示。提取网络角点后，就可以计算出摄像头的内部参数。同样读入右摄像头的图像，对其进行标定，标定方法同上。得到两个摄像头的内部参数后，即可完成两个摄像头构成的立体视觉系统的标定，在得596第6期王宏民，等:双目立体视觉线驱动蛇形手术机器人的形状重建中国煤炭期刊网 www.chinacaj.net

到左右摄像头内部参数的基础上，可以得到摄像头的外部参数。图2摄像机外部参数三维显示Fig．23Ddisplayofcameraexternalparameters两台摄像机的标定误差分别如图3和图4所示，误差范围集中分布在±0．6mm的范围内，通过分析可知，标定误差是在合理分布范围。图3左摄像机标定误差分布Fig．3Leftcameracalibrationerrordistribution图4右摄像机标定误差分布Fig．4Errordistributionofrightcameracalibration2柔性机器人三维形状的重建2.1机器人结构自然界中的蛇、象鼻和章鱼触手等，具有良好的灵巧性和运动能力，根据该类动物组织结构的特点设计了应用于微创外科手术的线驱动蛇形手术机器人，机器人柔性臂机构如图5所示。柔性段采用线驱动方式，线驱动蛇形机器人由线驱动关节、约束机构和牵引绳组成，每一个关节都具有一个中孔和四个导向孔，中孔可以通过约束杆，导向孔用来穿牵引线。图5线驱动机器人结构Fig．5Structureoflinedrivenrobot2.2实验平台系统的搭建通过在每个柔性关节上的标记点提取关节信息，每一台摄像机根据摄像头检测的像素点获取关节特性点在空间的三维坐标，拟合后即可完成三维形状重建。为了获取每一个关节的位置信息，两台Logitech网络摄像头(HDwebcamc255)安装后保证位置固定。机器人由10段关节组成，每段关节的长度为10mm，关节旋转角度为14°，每一段关节上都有标记点。机器人通过步进电机驱动完成在工作空间的运动，实验平台，如图6所示。图6柔性机器人系统实验平台Fig．6Experimentalplatformofflexiblerobotsystem根据标定好的三维立体视觉系统实验平台，实时获取机器人在两台摄像机上的标记点图像，柔性机器人的弯曲形状就可以根据获取的关节标记点信息进行曲线拟合重建。在实验中由于限制了机器人在平面上运动，因此，给出二维xy平面下的重建曲线，如图7所示。为了验证基于双目立体视觉形状重建方法的性能，根据给定的机器人弯曲轨迹，投影到坐标纸上如图8所示，由图8可见，实际值和预测值几乎在同一条曲线上。696黑龙江科技大学学报第27卷中国煤炭期刊网 www.chinacaj.net

as形轨迹bc形轨迹图7线驱动机器人形状重建曲线Fig．7Shapereconstructioncurveoflinedriverobotab图8约束轨迹下预测和实验曲线Fig．8Constrainttrajectoryunderpredictedcurveandactualcurve3结束语通过研究双目立体视觉的线驱动蛇形手术机器人形状的重建，基于双目立体视觉成像技术构建了机器人形状重建的算法，在Matlab工具箱完成了立体视觉标定、标记点特征提取和形状重建。通过柔性关节的标记点提取了关节信息在实验平台系统完成三维形状重建，在标定好的三维立体视觉系统实验平台，实时获取了机器人在两台摄像机上的标记点图像，柔性机器人的弯曲形状可以根据获取的关节标记点信息进行曲线拟合重建。根据给定的机器人弯曲轨迹和重建的拟合曲线对比，验证了基于双目立体视觉形状重建方法的性能，虽然存在一定的误差，但是实验值和预测值几乎在同一条曲线上，证明了该方法的有效性，下一步可以采用误差补偿进一步提高形状重建的精度。参考文献:［1］LiZ，FeilingJ，ＲenH，etal．Anoveltele-operatedflexiblerobottargetedforminimallyinvasiverobotsurgery［J］．Engineering，2015(1):73－78．［2］XuW．Afeed-forwardfrictioncompensationmotioncontrollerforatendon-sheath-drivenflexibleroboticsandBiomimetics(ＲOBIO)，2013IEEEInternationalConferenceonＲoboticsandAutomation，Karlsruhe:IEEE，2016(1):69－80．［3］HuB，YuJJ，YLu，etal．Staticsandstiffnessmodelofserial-parallelmanipulatorformedbykparallelmanipulatorsconnectedinseries［J］．JournalofMechanismsandＲobotics-TransactionsofTheASME，2012，20(2):66－70．［4］CamarilloDB，MilneCF，CarlsonCＲ，etal．Mechanicsmodel-ingoftendon-drivencontinuummanipulators［J］．IEEETransac-tionsonＲobotics，2008，24(6):1262－1273．［5］ＲoesthuisＲJ，MisraS．Steeringofmultisegmentcontinuumma-nipulatorsusingrigid-linkmodelingandFBG-basedshapesensing［J］．IEEETransactionsonＲobotics，2016，32(2):372－382．［6］YunY，LiY．Modelingandcontrolanalysisofa3-PUPUdualcompliantparallelmanipulatorformicropositioningandactivevi-brationisolation［J］．Journalofdynamicsystemmeasurementandcontrol，TransactionsofASME，2012，134(2):21－25．［7］于勇，张晖，林茂松．基于双目立体视觉三维重建系统的研究与设计［J］．计算机技术与发展，2009，19(6):127－130．(编校李德根)796第6期王宏民，等:双目立体视觉线驱动蛇形手术机器人的形状重建中国煤炭期刊网 www.chinacaj.net