基于视觉和惯导融合的多臂连续体形状检测

王 宁,陈 柏,鞠 锋,郭 昊

(南京航空航天大学机电学院,江苏 南京 210016)

连续体机器人在人体膀胱肿瘤微创手术、飞机发动机检测、地震被困人员搜救、核电站管道内部维修等狭小、受限、障碍物较多的工作环境中,可利用其自身柔顺、灵活度高的特点,通过改变自身形状来适应工作空间。为了更好地让连续体机器人准确、安全地运动,对连续体机器人形状信息的获取十分重要。连续体机器人是一种仿章鱼触手、象鼻和蛇等的柔性机构,没有离散关节和刚性连杆,无法像传统机器人那样使用电位计或编码器来获得位姿信息,故而大多数连续体机器人没有形状感知能力,因此研究一种可以在狭小或者障碍物多的工作空间内检测连续体机器人形状的方法尤为重要。

目前,连续体机器人形状检测方法主要有基于视觉的形状检测、基于智能材料的形状感知和基于传感器的形状检测等方法,其中基于视觉的形状检测方法应用最为广泛。基于视觉的形状检测方法是一种模拟人的视觉功能从客观图像中提取信息的方法,视觉检测的方法根据相机个数不同,分为单目、双目、三目或者多目视觉检测几类[1]。美国学者Hannan等[2]利用单目高速相机获取连续体机器人图像,通过捕捉均匀布置的标记点,将灰度图二值化,再通过算法得到连续体机器人二维形状,该方法图片处理时间长、精度低,不能满足控制的实时性要求;Chitrakaran等[3]在文献[2]的基础上进行了改进,结合运动学分解获得了连续体机器人三维空间形状,并且通过视觉信息完成了对连续体机器人的控制;Croom等[4]提出了一种基于双目视觉无标记的形状检测方法,利用自组织映射算法SOM对图像进行处理得到二维曲线,进而重构三维形状;美国学者Camarillo等[5]研究出一种在三维空间中对连续体机器人形状进行检测的方法,通过立体视觉来检测标记点,估计其形状。基于智能材料的形状感知方法其智能材料主要有聚偏氟乙烯(PVDF)和离子交换聚合金属材料(IPMC)。以色列学者Shapiro等[6]提出一种采用PVDF材料制作的嵌入式压电薄膜实时跟踪柔性机器人,通过PVDF和运动捕捉系统最终实现了二维形状重构;Bahramzadeh等[7]针对IPMC材料的悬臂梁利用上位机计算梁的弯曲角度变化,以此来进行形状重构。基于传感器的形状检测方法主要有基于光纤光栅(FBG)传感器的检测方法和基于电磁传感器的检测方法。荷兰学者Roesthuis等[8]在三维空间扩展了基于FBG传感器的形状检测方法,设计了一种直径为1 mm、长为172 mm、集成了12个FBG点的传感器,通过检测应变来计算曲率,从而重构出三维形状;Song等[9]采用永磁体作为连续体机器人末端跟踪的目标,通过磁传感器阵列平台测量磁通密度,得到机器人末端的位置和姿态,然后根据贝塞尔曲线拟合连续体形状,但是这种方法还需要在机器人外部增加额外的检测设备。

针对在狭小或者障碍物较多的工作空间中存在视觉遮挡和缺失的问题,本文提出一种视觉和惯导融合的多臂连续体机器人形状检测方法,通过获取连续体机器人头部标识物位姿信息,利用位姿信息进行曲线拟合,从而重构出连续体机器人三维形状,完成对多臂连续体机器人的形状检测。

1 多臂连续体机器人形状检测系统

1.1 连续体机器人结构设计

针对膀胱介入手术,课题组提出由多个结构和功能相同的弯曲单元串联而成的绳驱动连续体机器人来实现手术操作,其骨架结构如图1所示。绳索驱动控制简单,密封性要求低,便于微型化,是一种很好的连续体机器人驱动方式。

图1 连续体机器人导管骨架

图2所示为多臂连续体机器人机构,机械臂长度为220 mm,直径为30 mm,由4根绳索驱动,导管头部内侧安装了惯导传感器,外侧安装了球形标识物。

图2 多臂连续体机器人机构

1.2 视觉与惯导融合形状检测系统

视觉检测在连续体机器人形状检测中应用较为广泛,但是将其直接应用于多臂连续体机器人时有一定的局限性。在狭小空间或障碍物较多的工作空间中会存在一定的视觉遮挡和缺失,仅通过视觉无法获得完整的形状信息。若将惯性传感器布满整个连续体机器人,可以利用机器人运动学解算方位得到形状,但是过程过于复杂,无法实时检测连续体形状。为了解决上述问题,本文将视觉与惯导进行多传感融合,通过视觉与惯导获取连续体机器人头部标识物位姿,然后采用形状拟合重构的方法,获得多臂连续体机器人在工作空间中的形状。

1.2.1视觉检测系统

本文的连续体机器人视觉检测系统主要由图像采集模块、图像处理器和相机万向支架组成。其中图像采集模块包括两个相机,图像处理器由图像处理PC(personal computer)构成。首先由相机采集连续体机器人图像,然后通过串口将图像信息传输到图像处理PC,最终由LabVIEW进行运算处理。

对相机进行标定,即建立实际场景点与图像坐标系之间的关系,根据已知特征点的世界坐标和图像坐标求解相机内外参数。在针孔成像模型[10]基础上,研究4种坐标系的关系,如图3所示。

图3 针孔成像模型

由于视觉检测系统设备的局限性、安装位置的准确性、环境灯光以及系统各部分连接过程中存在的问题,图像会产生畸变,因此综合考虑形状检测视觉系统成像的多种畸变因素,对系统进行校准,找出图像与真实世界坐标系之间的映射关系。本文采用圆点标定盘(7×7圆点标定盘,点阵中纵向和横向相邻点之间的距离为20 mm,圆点的直径为10 mm),为视觉系统提供这些点的已知信息,包括在真实世界坐标系中点与点的距离,结合视觉系统拍摄的图像中各像素点与真实世界之间的映射关系,将得到的标定关系应用到之后的图像采集处理中,可以降低各种畸变对系统检测结果的影响,完成对畸变图像的矫正。

为了获得连续体机器人头部标识物位姿信息,要进行标识物的图像模板匹配工作,通过分析标识物图像与实时图像中边缘、灰度、外形结构及对应关系等特征的相似性,从实时图像中找到与标识物相似的区域。这一过程包括学习和匹配两个阶段,在学习阶段提取标识物的特征信息,在匹配阶段找到并提取相似的标识物特征信息,从而实时抓取标识物,得到连续体机器人头部标识物的位姿信息。

1.2.2惯性传感器

本文采用惯性传感器来解决视觉遮挡和视觉缺失的问题,将惯性传感器安装在连续体机器人头部内侧,来获取连续体机器人头部标识物姿态,如图4所示。

图4 惯导与标识物

采用东北天坐标系将惯性传感器的测量值与地球的主方向联系起来,地理坐标系到惯性传感器自身坐标系的转化矩阵为:

(1)

式中:C为姿态矩阵;n代表地理坐标系;b代表机器人本体坐标系;T为姿态矩阵相乘得到矩阵中的值;γ为滚转角;θ为俯仰角;Ψ为航向角。

由姿态矩阵计算出滚转角γ、俯仰角θ、航向角Ψ,公式如下:

(2)

根据得到的连续体机器人头部位标识物位姿信息,通过贝塞尔曲线拟合重构连续体机器人在三维空间中的形状。三次贝塞尔曲线的显示形式为[11]:

B(t)=(1-t)3P1+3t(1-t)2P2+3t2(1-t)P3+t3P4

(3)

式中:B(t)为贝塞尔曲线函数;t为时间;P1为起点位姿矩阵;P2为终点位姿矩阵;P3和P4为控制点位姿矩阵。

通过本文提出的视觉和惯导融合的多臂连续体机器人形状检测方法,获得多臂连续体机器人头部标识物位姿,从而在三维空间中拟合重构出连续体机器人的三维形状。

2 实验平台搭建与形状重构

2.1 实验平台

测试平台由多臂连续体机器人、视觉系统和惯性传感器组成。两个相机分别安装在框架结构的左侧和前部,用于采集连续体机器人头部位置信息。连续体机器人由尼龙材料3D打印而成,连续体头部凹槽用于安装惯性传感器(JY901),头部外侧凸出的球体作为采集位置信息的标识物。多臂连续体机器人形状检测实验平台如图5所示。

图5 多臂连续体机器人形状检测实验平台

本文采用Vision Assistant标定工具进行视觉系统的标定,用Calibration函数进行畸变矫正,如图6所示。完成视觉系统校准工作后,再进行标识物的模板匹配,通过相机拍摄识别标识物,从而获得多臂连续体机器人头部标识物位置信息,如图7所示。

图6 标定

图7 模板匹配

2.2 连续体形状检测实验及拟合重构

首先,对单臂连续体机器人在平面内的弯曲运动进行实验,得到平面内一组不同位置的单臂连续体弯曲形状,然后根据本文形状检测方法,通过实验平台得到机器人头部位姿,再进行贝塞尔曲线拟合,最终重构出单臂连续体机器人形状,如图8所示。

图8 不同位置下的单臂连续体及拟合结果

将不同位置下的单臂连续体形状用毫米方格纸记录下来,与拟合结果进行对比,结果表明,其最大误差为1.832 mm,如图9所示。

图9 不同位置下的单臂连续体实际形状

接下来进行三维空间中的多臂连续体机器人形状检测实验,在绳索驱动下,多臂连续体机器人形状不断发生改变,如图10所示。在存在视觉遮挡和视觉缺失的情况下,通过得到的连续体机器人头部位姿信息,重构多臂连续体机器人的三维形状,结果如图11所示。

图10 存在视觉遮挡的多臂连续体形状检测实验

图11 多臂连续体形状重构结果

通过激光跟踪仪对三维空间内的多臂连续体机器人实际形状进行测量,然后与拟合结果进行对比,结果表明,最大误差为2.386 mm。实验结果表明,本文提出的形状检测方法有着较高的拟合度,在工作空间有限、存在视觉遮挡和缺失的情况下,能够有效重构出连续体机器人形状。

3 结束语

本文提出了一种基于视觉和惯导融合的连续体机器人形状检测方法,在工作空间有限、存在视觉遮挡的情况下,通过两个相机与三个惯性传感器获取多臂连续体机器人头部标识物的位姿信息,利用位姿信息进行三次贝塞尔曲线拟合,最终重构出多臂连续体机器人形状。