[发明专利]双目立体摄像机机器人视觉伺服控制装置及其使用方法

摘要

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

主权项

一种双目立体摄像机机器人视觉伺服控制装置的使用方法，其特征在于包括以下步骤：步骤1：启动机器人子系统和视觉控制子系统，调整双目立体摄像机的位置以确定末端执行器光学靶标和目标运动体光学靶标均在双目立体摄像机的视野范围内，之后进入步骤2；步骤2：通过双目立体摄像机采集初始化状态下末端执行器光学靶标和目标运动体光学靶标的位置信息图像，根据摄像机模型计算出末端执行器光学靶标在摄像机坐标系下的位姿齐次矩阵表达式M2V和目标运动体光学靶标在摄像机坐标系下的位姿齐次矩阵表达式M1V，之后进入步骤3；步骤3：通过测量工具测出末端执行器光学靶标在机器人末端执行器坐标系下的位姿齐次矩阵表达式M2T；通过关节角度传感器获取机器人的六个关节角确定机器人末端执行器在初始位置上的位姿齐次矩阵，再经机器人运动学正解得到MTB；根据坐标变换关系，计算出末端执行器光学靶标在机器人基座标系B下的齐次矩阵为M2B＝MTB·M2T，之后进入步骤4；步骤4：确定机器人基座标系的转换矩阵，根据机器人末端执行器光学靶标在摄像机坐标系下和机器人基座标系下的齐次矩阵计算出从摄像机坐标系变换到机器人基座标系的转换矩阵Mmap＝M2B·M2V‑1，之后进入步骤5；步骤5：计算目标运动体和机器人末端执行器的位置关系Mbind，关节型六自由度机器人在进行目标跟踪的过程中机器人末端执行器与目标运动体之间始终保持一个固定的偏置关系，根据步骤2中双目立体摄像机获取到末端执行器光学靶标齐次矩阵M2V和目标运动体光学靶标齐次矩阵M1V后计算出偏置关系：Mbind＝M2V‑1·M1V，之后进入步骤6；步骤6：计算末端执行器光学靶标和目标运动体光学靶标在摄像机坐标系V下的位置误差，双目立体摄像机以一定的频率不断地计算出新的M1V和M2V，通过MerrV＝M1V‑M2V·Mbind计算得到在摄像机视觉坐标系V下的位置误差MerrV；再通过公式MerrB＝Mmap·MerrV计算出在机器人基坐标B下的位置误差MerrB，之后进入步骤7；步骤7：判定误差值，齐次矩阵MerrB中包含了三维空间中机器人末端执行器和目标运动体在X、Y、Z方向上的位置误差ΔX、ΔY、ΔZ，如果误差值在容许的范围ε内，则进入步骤2，否则进入步骤8；步骤8：根据PID算法利用误差值计算出在机器人末端执行器坐标系T下的速度控制量VX、VY、VZ，并发送给机器人控制器让机器人末端执行器按照给定速度运动，之后进入步骤2，实现在一定的空间范围内关节型六自由度机器人快速而准确的跟踪运动的目标；其中：该双目立体摄像机机器人视觉伺服控制装置包括机器人子系统和视觉控制子系统，所述的机器人子系统包括机器人控制器和关节型六自由度机器人，所述的视觉控制子系统包括双目立体摄像机和视觉控制器；机器人控制器的输出端与关节型六自由度机器人的输入端之间电连接，机器人控制器与视觉控制器之间双向通信连接，双目立体摄像机的输出端与视觉控制器的输入端之间电连接；其中：视觉控制子系统还包括末端执行器光学靶标和目标运动体光学靶标，所述的末端执行器光学靶标固定安装在关节型六自由度机器人的末端执行器上，所述的目标运动体光学靶标安装在目标期望位置，末端执行器光学靶标和目标运动体光学靶标分别记载有期望位置和当前位置的位置坐标信息；视觉控制器为视觉PID控制器；机器人控制器为嵌入式控制器，其包括功率放大器和关节角度传感器。