双目立体摄像机机器人视觉伺服控制装置及其使用方法
【技术领域】
[0001] 本发明涉及医疗机器人技术领域，是一种双目立体摄像机机器人视觉伺服控制装 置及其使用方法。
【背景技术】
[0002] 在现阶段，视觉传感器是机器人系统中最重要的传感器之一，它的引入改变了机器人 对操作对象及环境必须精确建模的要求。依靠视觉信息的反馈，机器人可实现在动态下的 操作。
[0003] 在医疗领域，视觉伺服技术通过获取手术器械与患者的相对位置关系，辅助医生 进行复杂的外科手术。例如在进行胸腔手术和脑手术时，借助于机器视觉的特点，可利用射 线透视图，核磁共振图像对病灶位置做详细分析和定位，并控制机器人末端手术刀具精确 地进行手术，完成以前单纯靠人力无法完成的高难度手术。在机器人辅助骨科钻孔手术过 程中如果患者的肢体没有被固定或固定不牢，可能在钻孔操作时产生突然运动，碰到刀具， 伤及健康部位，造成医疗事故，因此需要检测手术器械与患者肢体的相对位置关系，来控 制机器人运动。
[0004] 由于受视觉处理的速度及精度等瓶颈的限制，早期的机器视觉系统多采用静态控 制结构，即"先看后动"，难以实现对运动目标的跟踪，直到计算机及图像处理技术发展成熟 后，才使视觉信息可用于连续反馈，于是人们提出了基于视觉的伺服控制形式，相对于对静 止目标的操作，视觉伺服系统强调运动目标跟踪的实时性。
[0005] 根据控制系统中视觉传感器数目差异，分为单目、双目和多目视觉。根据视觉传感 器安装的位置，分为手眼系统（Eye in Hand)和固定摄像机系统（Eye to Hand)。根据视觉 反馈信号是3维笛卡尔空间坐标值还是2维图像特征，分为基于位置（Position-Based)的 视觉控制系统和基于图像（Imaged-Based)的视觉控制系统。采用Eye in Hand的单目视 觉系统结构简单，视野范围较大，这种形式在工业机器人中应用比较广泛。但由于视野处在 变化之中，不能保证目标一直在视场内，有时会存在丢失目标现象，而且摄像机的成像深度 随机器人运动不断变化，增加了图像处理的计算量。
【发明内容】
[0006] 本发明提供了一种双目立体摄像机机器人视觉伺服控制装置和使用方法，克服了 上述现有技术之不足，其能有效解决现有技术采用手眼系统采集目标图像视野范围大易造 成目标丢失的现象，更有效解决了由于摄像机成像深度需不断随机器人运动采集变化的目 标，造成图像处理计算量大的问题。
[0007] 本发明的技术方案之一是通过以下措施来实现的：一种双目立体摄像机机器人视 觉伺服控制装置，包括机器人子系统和视觉控制子系统，所述的机器人子系统包括机器人 控制器和关节型六自由度机器人，所述的视觉控制子系统包括双目立体摄像机和视觉控制 器；机器人控制器的输出端与关节型六自由度机器人的输入端之间电连接，机器人控制器 与视觉控制器之间双向通信连接，双目立体摄像机的输出端与视觉控制器的输入端之间电 连接。
[0008] 下面是对上述发明技术方案的进一步优化或/和改进： 上述视觉控制子系统还可包括末端执行器光学靶标和目标运动体光学靶标，所述的末 端执行器光学靶标固定安装在关节型六自由度机器人的末端执行器上，所述的目标运动体 光学靶标安装在目标期望位置，末端执行器光学靶标和目标运动体光学靶标分别记载有期 望位置和当前位置的位置坐标信息。
[0009] 上述视觉控制器为视觉PID控制器。
[0010] 上述机器人控制器可为嵌入式控制器，其包括功率放大器和关节角度传感器。
[0011] 本发明的技术方案之二是通过以下措施来实现的：一种使用双目立体摄像机机器 人视觉伺服控制装置的方法，包括以下步骤： 步骤1:启动机器人子系统和视觉控制子系统，调整双目立体摄像机的位置以确定末 端执行器光学靶标和目标运动体光学靶标均在双目立体摄像机的视野范围内，之后进入步 骤2; 步骤2:通过双目立体摄像机采集初始化状态下末端执行器光学靶标和目标运动体光 学靶标的位置信息图像，根据摄像机模型计算出末端执行器光学靶标在摄像机坐标系下的 位姿齐次矩阵表达式和目标运动体光学靶标在摄像机坐标系下的位姿齐次矩阵表达 式^^，之后进入步骤3; 步骤3:通过测量工具测出末端执行器光学靶标在机器人末端执行器坐标系下的位姿 齐次矩阵表达式％ ;通过关节角度传感器获取机器人的六个关节角确定机器人末端执行 器在初始位置上的位姿齐次矩阵，再经机器人运动学正解得到％ ;根据坐标变换关系，计 算出末端执行器光学靶标在机器人基座标系B下的齐次矩阵为
之后进 入步骤4; 步骤4:确定机器人基座标系的转换矩阵，根据机器人末端执行器光学靶标在摄像机 坐标系下和机器人基座标系下的齐次矩阵计算出从摄像机坐标系变换到机器人基座标系 的转换矩阵
之后进入步骤5; 步骤5:计算目标物体和机器人末端执行器的位置关系，关节型六自由度机器人 在进行目标跟踪的过程中机器人末端执行器与目标运动体之间始终保持一个固定的偏置 关系，根据步骤2中双目立体摄像机获取到末端执行器光学靶标齐次矩阵和目标运动 体光学靶标齐次矩阵遍后计算出偏置关系：
… 之后进入步骤6; 步骤6:计算末端执行器光学靶标和目标运动体光学靶标在摄像机坐标系V下 的位置误差，双目立体摄像机以一定的频率不断地计算出新的和^，通过
计算得到在摄像机视觉坐标系V下的位置误差;再通过 公式：
：:计算出在机器人基坐标B下的位置误差，之后进入步骤7; 步骤7:判定误差值，齐次矩阵%*中包含了三维空间中机器人末端执行器和目标运 动体在X、Y、Z方向上的位置误差Ajf.、Ar、A2，如果误差值在容许的范围本内，则进入步骤 2,否则进入步骤8; 步骤8:根据PID算法利用误差值计算出在机器人末端执行器坐标系T下的速度控制 量&、匕，并发送给机器人?制器&机器人末端执行器按照给定速度运动，之后进人 步骤2,实现在一定的空间范围内关节型六自由度机器人快速而准确的跟踪运动的目标。
[0012] 本发明通过采用固定式双目立体摄像机实时采集机器人末端执行器上的手术器 械与目标运动体的相对位置信息，直接在笛卡尔空间内计算出位置误差作为视觉PID控制 器的输入，机器人控制器检测到目标运动后立即发出控制指令使机器人末端执行器跟随其 运动，保证机器人随动的快速与准确，避免发生碰撞，从而实现对运动目标快速、准确的跟 踪。本发明有效提高了在医疗手术中机器人跟踪目标的准确性，确保手术的安全性，降低了 手术风险系数。
【附图说明】
[0013] 图1为本发明的系统结构示意图。
[0014] 图2为本发明的视觉伺服控制装置原理框图。
[0015] 图3为本发明的视觉伺服控制装置的工作流程图。
[0016] 附图中的编码分别为：1为机器人控制器，2为关节型六自由度机器人，3为双目立 体摄像机，4为视觉控制器，5为末端执行器光学靶标，6为目标运动体光学靶标，7为机器人 末端执行器。
【具体实施方式】
[0017] 本发明不受下述实施例的限制，可根据本发明的技术方案与实际情况来确定具体 的实施方式。
[0018] 在本发明中，为了便于描述，各部件的相对位置关系的描述均是根据说明书附图1 的布图方式来进行描述的，如：前、后、上、下、左、右等的位置关系是依据说明书附图的布图 方向来确定的。
[0019] 下面结合实施例及附图对本发明作进一步描述： 实施例一：如附图1、2所示，该双目立体摄像机机器人视觉伺服控制装置，包括机器人 子系统和视觉控制子系统，所述的机器人子系统包括机器人控制器1和关节型六自由度机 器人2,所述的视觉控制子系统包括双目立体摄像机3和视觉控制器4;机器人控制器1的 输出端与关节型六自由度机器人2的输入端之间电连接，机器人控制器1与视觉控制器4 之间双向通信连接，双目立体摄像机3的输出端与视觉控制器4的输入端之间电连接。在 实际应用中，关节型六自由度机器人2可以是丹麦Universal Robots公司生产的UR5型机 器人，其结构紧凑、重量轻、安全性高；机器人控制器1基于Linux系统开发，能通过以太网 接口接收外部计算机传来的运动控制指令并发送给机器人去执行。双目立体摄像机3与视 觉控制器4之间通过1394线相连接。本发明的视觉控制器4采用Dell Precision M4700 移动工作站，是关节型六自由度机器人视觉伺服控制装置的核心，它根据三维空间中的位 置误差，采用PID控制算法计算出控制量，实现机器人对运动目标的跟踪与随动。
[0020] 可根据实际需要，对上述双目立体摄像机机器人视觉伺服控制装置作进一步优化 或/和改进： 如附图1所示，视觉控制子系统还包括末端执行器光学靶标5和目