双目立体摄像机机器人视觉伺服控制装置及其使用方法

摘要

本发明涉及医疗机器人技术领域，是一种双目立体摄像机机器人视觉伺服控制装置及其使用方法，其包括机器人子系统和视觉控制子系统，所述的机器人子系统包括机器人控制器和关节型六自由度机器人，所述的视觉控制子系统包括双目立体摄像机和视觉控制器；机器人控制器的输出端与关节型六自由度机器人的输入端之间电连接，机器人控制器与视觉控制器之间双向通信连接，双目立体摄像机的输出端与视觉控制器的输入端之间电连接。本发明通过采用固定式双目立体摄像机实时检测机器人与目标的相对位置，计算位置误差，保证机器人随动的快速与准确，避免发生碰撞，有效提高了在医疗手术中机器人跟踪目标的准确性，确保手术的安全性，降低了手术风险系数。

说明书

技术领域

本发明涉及医疗机器人技术领域，是一种双目立体摄像机机器人视觉伺服控制装置及其使用方法。

背景技术

在现阶段，视觉传感器是机器人系统中最重要的传感器之一，它的引入改变了机器人对操作对象及环境必须精确建模的要求。依靠视觉信息的反馈，机器人可实现在动态下的操作。

在医疗领域，视觉伺服技术通过获取手术器械与患者的相对位置关系，辅助医生进行复杂的外科手术。例如在进行胸腔手术和脑手术时，借助于机器视觉的特点，可利用射线透视图，核磁共振图像对病灶位置做详细分析和定位，并控制机器人末端手术刀具精确地进行手术，完成以前单纯靠人力无法完成的高难度手术。在机器人辅助骨科钻孔手术过程中如果患者的肢体没有被固定或固定不牢，可能在钻孔操作时产生突然运动，碰到刀具，伤及健康部位，造成医疗事故,因此需要检测手术器械与患者肢体的相对位置关系，来控制机器人运动。

由于受视觉处理的速度及精度等瓶颈的限制，早期的机器视觉系统多采用静态控制结构，即“先看后动”，难以实现对运动目标的跟踪，直到计算机及图像处理技术发展成熟后，才使视觉信息可用于连续反馈，于是人们提出了基于视觉的伺服控制形式，相对于对静止目标的操作，视觉伺服系统强调运动目标跟踪的实时性。

根据控制系统中视觉传感器数目差异，分为单目、双目和多目视觉。根据视觉传感器安装的位置，分为手眼系统(EyeinHand)和固定摄像机系统(EyetoHand)。根据视觉反馈信号是3维笛卡尔空间坐标值还是2维图像特征，分为基于位置(Position-Based)的视觉控制系统和基于图像(Imaged-Based)的视觉控制系统。采用EyeinHand的单目视觉系统结构简单，视野范围较大，这种形式在工业机器人中应用比较广泛。但由于视野处在变化之中，不能保证目标一直在视场内，有时会存在丢失目标现象，而且摄像机的成像深度随机器人运动不断变化，增加了图像处理的计算量。

发明内容

本发明提供了一种双目立体摄像机机器人视觉伺服控制装置和使用方法，克服了上述现有技术之不足，其能有效解决现有技术采用手眼系统采集目标图像视野范围大易造成目标丢失的现象，更有效解决了由于摄像机成像深度需不断随机器人运动采集变化的目标，造成图像处理计算量大的问题。

本发明的技术方案之一是通过以下措施来实现的：一种双目立体摄像机机器人视觉伺服控制装置，包括机器人子系统和视觉控制子系统，所述的机器人子系统包括机器人控制器和关节型六自由度机器人，所述的视觉控制子系统包括双目立体摄像机和视觉控制器；机器人控制器的输出端与关节型六自由度机器人的输入端之间电连接，机器人控制器与视觉控制器之间双向通信连接，双目立体摄像机的输出端与视觉控制器的输入端之间电连接。

下面是对上述发明技术方案的进一步优化或/和改进：

上述视觉控制子系统还可包括末端执行器光学靶标和目标运动体光学靶标，所述的末端执行器光学靶标固定安装在关节型六自由度机器人的末端执行器上，所述的目标运动体光学靶标安装在目标期望位置，末端执行器光学靶标和目标运动体光学靶标分别记载有期望位置和当前位置的位置坐标信息。

上述视觉控制器为视觉PID控制器。

上述机器人控制器可为嵌入式控制器，其包括功率放大器和关节角度传感器。

本发明的技术方案之二是通过以下措施来实现的：一种使用双目立体摄像机机器人视觉伺服控制装置的方法，包括以下步骤：

步骤1：启动机器人子系统和视觉控制子系统，调整双目立体摄像机的位置以确定末端执行器光学靶标和目标运动体光学靶标均在双目立体摄像机的视野范围内，之后进入步骤2；

步骤2：通过双目立体摄像机采集初始化状态下末端执行器光学靶标和目标运动体光学靶标的位置信息图像，根据摄像机模型计算出末端执行器光学靶标在摄像机坐标系下的位姿齐次矩阵表达式和目标运动体光学靶标在摄像机坐标系下的位姿齐次矩阵表达式，之后进入步骤3；

步骤3：通过测量工具测出末端执行器光学靶标在机器人末端执行器坐标系下的位姿齐次矩阵表达式；通过关节角度传感器获取机器人的六个关节角确定机器人末端执行器在初始位置上的位姿齐次矩阵，再经机器人运动学正解得到；根据坐标变换关系，计算出末端执行器光学靶标在机器人基座标系B下的齐次矩阵为，之后进入步骤4；

步骤4：确定机器人基座标系的转换矩阵，根据机器人末端执行器光学靶标在摄像机坐标系下和机器人基座标系下的齐次矩阵计算出从摄像机坐标系变换到机器人基座标系的转换矩阵，之后进入步骤5；

步骤5：计算目标物体和机器人末端执行器的位置关系，关节型六自由度机器人在进行目标跟踪的过程中机器人末端执行器与目标运动体之间始终保持一个固定的偏置关系，根据步骤2中双目立体摄像机获取到末端执行器光学靶标齐次矩阵和目标运动体光学靶标齐次矩阵后计算出偏置关系：，之后进入步骤6；

步骤6：计算末端执行器光学靶标和目标运动体光学靶标在摄像机坐标系V下的位置误差，双目立体摄像机以一定的频率不断地计算出新的和，通过计算得到在摄像机视觉坐标系V下的位置误差；再通过公式计算出在机器人基坐标B下的位置误差，之后进入步骤7；

步骤7：判定误差值，齐次矩阵中包含了三维空间中机器人末端执行器和目标运动体在X、Y、Z方向上的位置误差、、，如果误差值在容许的范围内，则进入步骤2，否则进入步骤8；

步骤8：根据PID算法利用误差值计算出在机器人末端执行器坐标系T下的速度控制量、、，并发送给机器人控制器让机器人末端执行器按照给定速度运动，之后进入步骤2，实现在一定的空间范围内关节型六自由度机器人快速而准确的跟踪运动的目标。

本发明通过采用固定式双目立体摄像机实时采集机器人末端执行器上的手术器械与目标运动体的相对位置信息，直接在笛卡尔空间内计算出位置误差作为视觉PID控制器的输入，机器人控制器检测到目标运动后立即发出控制指令使机器人末端执行器跟随其运动，保证机器人随动的快速与准确，避免发生碰撞，从而实现对运动目标快速、准确的跟踪。本发明有效提高了在医疗手术中机器人跟踪目标的准确性，确保手术的安全性，降低了手术风险系数。

附图说明

图1为本发明的系统结构示意图。

图2为本发明的视觉伺服控制装置原理框图。

图3为本发明的视觉伺服控制装置的工作流程图。

附图中的编码分别为：1为机器人控制器，2为关节型六自由度机器人，3为双目立体摄像机，4为视觉控制器，5为末端执行器光学靶标，6为目标运动体光学靶标，7为机器人末端执行器。

具体实施方式

本发明不受下述实施例的限制，可根据本发明的技术方案与实际情况来确定具体的实施方式。

在本发明中，为了便于描述，各部件的相对位置关系的描述均是根据说明书附图1的布图方式来进行描述的，如：前、后、上、下、左、右等的位置关系是依据说明书附图的布图方向来确定的。

下面结合实施例及附图对本发明作进一步描述：

实施例一：如附图1、2所示，该双目立体摄像机机器人视觉伺服控制装置，包括机器人子系统和视觉控制子系统，所述的机器人子系统包括机器人控制器1和关节型六自由度机器人2，所述的视觉控制子系统包括双目立体摄像机3和视觉控制器4；机器人控制器1的输出端与关节型六自由度机器人2的输入端之间电连接，机器人控制器1与视觉控制器4之间双向通信连接，双目立体摄像机3的输出端与视觉控制器4的输入端之间电连接。在实际应用中，关节型六自由度机器人2可以是丹麦UniversalRobots公司生产的UR5型机器人，其结构紧凑、重量轻、安全性高；机器人控制器1基于Linux系统开发，能通过以太网接口接收外部计算机传来的运动控制指令并发送给机器人去执行。双目立体摄像机3与视觉控制器4之间通过1394线相连接。本发明的视觉控制器4采用DellPrecisionM4700移动工作站，是关节型六自由度机器人视觉伺服控制装置的核心，它根据三维空间中的位置误差，采用PID控制算法计算出控制量，实现机器人对运动目标的跟踪与随动。

可根据实际需要，对上述双目立体摄像机机器人视觉伺服控制装置作进一步优化或/和改进：

如附图1所示，视觉控制子系统还包括末端执行器光学靶标5和目标运动体光学靶标6，所述的末端执行器光学靶标5固定安装在关节型六自由度机器人2的末端执行器上，所述的目标运动体光学靶标6安装在目标期望位置，末端执行器光学靶标5和目标运动体光学靶标6分别记载有期望位置和当前位置的位置坐标信息。在实际使用时，该光学靶标是以一定模式设计的高对比度黑白图像且光学靶标设置在关节型六自由度机器人周围。双目立体摄像机3固定在工作空间的预定位置，使机器人末端执行器7上固连的光学靶标和目标运动体上的光学靶标都在其视野范围内。双目立体摄像机3以一定的频率连续采集左右两幅图像，并根据立体摄像机模型及投影变换关系确定末端执行器光学靶标5和目标运动体光学靶标6的三维空间位置。

如附图1所示，视觉控制器4为视觉PID控制器。采用视觉PID控制器，可根据光学靶标的位置和姿态误差，计算出其速度控制量，然后将速度指令传给机器人控制器，控制机器人的运动。

如附图1所示，机器人控制器1为嵌入式控制器，其包括功率放大器和关节角度传感器。所述的功率放大器用于在机器人运动时提供动力，所述的关节角度传感器用于检测机器人的各个关节转角，向视觉控制器4提供闭环反馈。

实施例二：如图1、2、3所示，一种使用上述双目立体摄像机机器人视觉伺服控制装置的方法，包括以下步骤：

步骤1：启动机器人子系统和视觉控制子系统，调整双目立体摄像机3的位置以确定末端执行器光学靶标5和目标运动体光学靶标6均在双目立体摄像机3的视野范围内，之后进入步骤2；

步骤2：通过双目立体摄像机3采集初始化状态下末端执行器光学靶标5和目标运动体光学靶标6的位置信息图像，根据摄像机模型计算出末端执行器光学靶标5在摄像机坐标系V下的位姿齐次矩阵表达式和目标运动体光学靶标6在摄像机坐标系V下的位姿齐次矩阵表达式，之后进入步骤3；

步骤3：通过测量工具测出末端执行器光学靶标5在机器人末端执行器坐标系T下的位姿齐次矩阵表达式；通过关节角度传感器获取机器人的六个关节角确定机器人末端执行器7在初始位置上的位姿齐次矩阵，再经机器人运动学正解得到；根据坐标变换关系，计算出末端执行器光学靶标5在机器人基座标系B下的齐次矩阵为，之后进入步骤4；

步骤4：确定机器人基座标系的转换矩阵，根据机器人末端执行器光学靶标5在摄像机坐标系V下和机器人基座标系B下的齐次矩阵计算出从摄像机坐标系变换到机器人基座标系的转换矩阵，之后进入步骤5；

步骤5：计算目标运动体和机器人末端执行器7的位置关系，关节型六自由度机器人2在进行目标跟踪的过程中机器人末端执行器7与目标运动体之间始终保持一个固定的偏置关系，根据步骤2中双目立体摄像机3获取到末端执行器光学靶标5齐次矩阵和目标运动体光学靶标6齐次矩阵后计算出偏置关系：，之后进入步骤6；

步骤6：计算末端执行器光学靶标5和目标运动体光学靶标6在摄像机坐标系V下的位置误差，双目立体摄像机3以一定的频率不断地计算出新的和，通过计算得到在摄像机视觉坐标系V下的位置误差；再通过公式计算出在机器人基坐标B下的位置误差，之后进入步骤7；

步骤7：判定误差值，齐次矩阵中包含了三维空间中机器人末端执行器7和目标运动体在X、Y、Z方向上的位置误差、、，如果误差值在容许的范围内，则进入步骤2，否则进入步骤8；

步骤8：根据PID算法利用误差值计算出在机器人末端执行器7坐标系T下的速度控制量、、，并发送给机器人控制器1，让机器人末端执行器7按照给定速度运动，之后进入步骤2，实现在一定的空间范围内关节型六自由度机器人快速而准确的跟踪运动的目标。

以上技术特征构成了本发明的实施例，其具有较强的适应性和实施效果，可根据实际需要增减非必要的技术特征，来满足不同情况的需求。