一种基于立体视觉的主从式遥操作手术机器人控制系统技术方案

本发明专利技术公开了一种基于立体视觉的主从式遥操作手术机器人控制系统，包括运动控制系统和立体视觉系统，运动控制系统包括运动控制算法模块和底层控制模块；立体视觉系统包括立体视觉算法模块；各从手机械臂实时接收主手操作臂传递过来的位置控制指令，运动控制算法模块解算得到各关节期望位置，底层控制模块控制伺服驱动器使从手机械臂运动到指定位置；立体视觉系统实时采集从手机械臂所处位置信息并传递给视觉算法计算线程，通过立体视觉算法模块计算机械臂相对于世界坐标系的位置，通过以太网将位置信息反馈至运动控制系统；操作者通过立体显示装置观察从臂运动情况操作主手操作臂完成手术任务。本发明专利技术在系统稳定性、实时性和高精度等方面达到了较高的水平。

A master slave teleoperation robot control system based on stereo vision

The invention discloses a master-slave tele operation control system of stereo vision based on surgical robot, including motion control system and stereo vision system, motion control system, including the algorithm module and the underlying control module of motion control; stereo vision system including stereo vision algorithm module; the mobile phone from the command position control arm receiving real-time master hand manipulator transfer, motion control algorithm module calculated the joint expected position, bottom control module to control the servo drive the mechanical arm movement from the mobile phone to the specified location; stereo vision system for real-time collection from mobile phone arm position information for visual computing threads, through the stereo vision algorithm module calculates the manipulator relative to the world coordinate the position, through the Ethernet location information feedback to the operator through the vertical motion control system; The body display device performs the operation by observing the movement of the arm and operating the main hand operating arm. The invention achieves a higher level in system stability, real-time and high precision, etc..

【技术实现步骤摘要】
一种基于立体视觉的主从式遥操作手术机器人控制系统
本专利技术属于医疗手术机器人领域，具体涉及一种基于立体视觉的主从式遥操作手术机器人控制系统，属于人机交互领域。
技术介绍
机器人微创手术通过从根本上改变了医生的手术环境及手术方式，有效的避免了医生长时间手术时的疲劳感及由此引起的手术误操作，有利于进一步提高微创手术的手术效果；同时，机器人微创手术能够很好的实现医生的手眼协调，便于开展更为复杂精细的手术操作，极大地拓展了医生的手术能力。主从式遥操作手术机器人控制系统通常由一个主手操作端和若干个从手执行臂组成。主从式遥操作手术机器人控制系统通常由一个主手操作端和若干个从手执行臂组成。从手执行臂末端安装内窥镜及手术器械通过创口直达病灶处，医生通过操作主手即可控制从手末端器械完成各种手术操作。将立体视觉和主从式遥操作控制系统于一体的手术机器人系统，需要进行主从手及各从手之间算法的实时计算及网络实时通信，这就提高了遥操作控制系统的复杂性，现有技术在稳定性、实时性和高精度等方面还有欠缺。
技术实现思路
有鉴于此，本专利技术提供一种基于立体视觉的主从式遥操作手术机器人控制系统，通过在从手端采用立体视觉功能，以提高医生实施遥操作手术时的真实感，更好的操控机器人完成手术。本专利技术的目的是通过这样的技术方案实现的，一种基于立体视觉的主从式遥操作手术机器人控制系统，手术机器人包括主手操作端和从手执行端；主手操作端为7DOF并联式机器人，从手执行端由一个固定立体视觉系统的6DOF串联式持镜臂、两个7DOF串联式器械臂和一个6DOF辅助臂；主手操作端与从手执行端通过以太网通信，机器人控制系统包括运动控制系统和立体视觉系统，运动控制系统与立体视觉系统之间通过以太网通信；所述运动控制系统包括运动控制算法模块和底层控制模块；所述立体视觉系统包括立体视觉算法模块；各从手机械臂控制程序的通信模块通过以太网实时接收主手操作臂传递过来的位置控制指令，通过运动控制算法模块中的逆运动学算法解算得到各关节期望位置，通过任务执行线程调用底层控制模块控制伺服驱动器使从手机械臂运动到指定位置；立体视觉系统通过图像采集线程实时采集从手机械臂所处位置信息并传递给视觉算法计算线程，通过立体视觉算法模块计算机械臂相对于世界坐标系的位置，通过以太网将位置信息反馈至运动控制系统；操作者通过立体显示装置观察从臂运动情况操作主手操作臂完成手术任务。进一步，所述立体视觉算法模块包括单个相机内部参数和外部参数确定、左相机与右相机之间变换矩阵确定、景深计算、共轭点对特征提取等算法。进一步，所述的单个相机内部参数和外部参数确定算法，其特征在于，包括以下步骤：S1、获取目标物体在相机坐标系与世界坐标系的关系xc＝Rxw+T，xc为目标物体在相机坐标系中的坐标，xw为目标物体在世界坐标系中的坐标，为相机的外部参数；表示世界坐标系到相机坐标系的旋转矩阵，相机坐标系到世界坐标系的平移向量。S2、根据关系式和确定世界坐标和像素坐标之间的映射关系，其中(x,y,z)为世界坐标，(r,c)为像素坐标，(or,oc)表示包含主点的像素的像素阵列坐标，sx和sy表示一个像素在水平和垂直方向的尺度，λ＝kz，k为未知的正值常数；S3、根据相机坐标系中的点P在世界坐标系中的表示xc＝r11x+r12y+r13z+Tx，yc＝r21x+r22y+r23z+Ty，zc＝r31x+r32y+r33z+Tz，(r11，r12，r13为相机坐标系到世界坐标系的旋转矩阵的第一行；r21，r22，r23为相机坐标系到世界坐标系的旋转矩阵的第二行；r31，r32，r33为相机坐标系到世界坐标系的旋转矩阵的第三行；Tx，Ty，Tz分别为相机坐标系到世界坐标系的平移向量的x轴,y轴,z轴的分量)结合S2所述的世界坐标和像素坐标间的映射关系式，得到S4、对于利用相机采集到的N个点的数据点集(ri、ci、xi、yi、zi)，ri表示像素坐标的行值，ci表示像素坐标的列值，xi、yi、zi分别表示世界坐标系中的x轴，y轴，z轴的分量；根据S3所述的方程，得到rify(r21x+r22y+r23z+Ty)＝-cifx(r11x+r12y+r13z+Tx)，写成矩阵形式Ax＝0，其中，A为N个方程组成的矩阵，x＝[r21,r22,r23,Ty,αr11,αr12,αr13,αTx]T，α＝fx/fy，利用R为正交矩阵的约束关系求得所有参数。进一步，所述左相机与右相机之间变换矩阵确定包括以下步骤：S51、同一个点P在左相机坐标系下的坐标rl＝(xl,yl,zl)T和在右相机坐标系下的坐标rr＝(xr,yr,zr)T之间的关系满足rr＝Rrl+ro，R是3×3的正交矩阵，表示两个相机坐标系的旋转；ro是3维向量，表示两个相机坐标系的平移，其展开式为r11xl+r12yl+r13zl+r14＝xr，r21xl+r22yl+r23zl+r24＝yr，r31xl+r32yl+r33zl+r34＝zr；r14，r24，r34分别表示从右相机坐标系到左相机坐标系的平移向量的x方向，y方向，z方向的分量；S52、对于焦距为f的两个相机，有和和根据S51所述的展开式得到，r11x′l+r12y′l+r13f+r14f/zl＝x′rzr/zl，r21x′l+r22y′l+r23f+r24f/zl＝y′rzr/zl，r31x′l+r32y′l+r33f+r34f/zl＝fzr/zl；定义e＝(ex,ey,ez)为误差向量，对于第i个样本点的误差向量表示为ei＝(Rrl,i+ro)-rr,i，R表示左相机坐标系到右相机坐标系的旋转矩阵，ro表示左相机坐标系到右相机坐标系的平移向量，rl,i表示左相机坐标系下的坐标值，rr,i表示右相机坐标系下的坐标值；选择多对共轭点对，根据正交矩阵的约束条件，利用Lagrange乘子法基于平方误差最小的原则求得最优解。进一步，所述的景深计算具体为：根据S52所述展开式得到，由于采用了上述技术方案，本专利技术具有如下的优点：本专利技术将运动控制系统与立体视觉系统分别进行处理，通过以太网进行数据传输，在系统稳定性、实时性和高精度等方面达到了较高的水平。附图说明为了使本专利技术的目的、技术方案和优点更加清楚，下面将结合附图对本专利技术作进一步的详细描述，其中：图1为本专利技术控制系统硬件架构示意图；图2为本专利技术控制系统软件架构示意图；图3为本专利技术控制系统控制流程示意图。具体实施方式以下将结合附图，对本专利技术的优选实施例进行详细的描述；应当理解，优选实施例仅为了说明本专利技术，而不是为了限制本专利技术的保护范围。附图1为本专利技术的控制系统硬件架构，一种基于立体视觉的主从式遥操作手术机器人控制系统，手术机器人包括主手操作端和从手执行端；主手操作端为7DOF并联式机器人，从手执行端由一个固定立体视觉系统的6DOF串联式持镜臂、两个7DOF串联式器械臂和一个6DOF辅助臂；主手操作端与从手执行端通过以太网通信，机器人控制系统包括运动控制系统和立体视觉系统，运动控制系统与立体视觉系统之间通过以太网通信；控制系统包括运动控制系统和立体视觉系统，运动控制系统由工控机、运动控制卡、以太网卡、数字量与模拟量采集卡、伺服驱动器及电机等组成；立体视觉系统由工控机、图像采集卡、以本文档来自技高网...

【技术保护点】
一种基于立体视觉的主从式遥操作手术机器人控制系统，手术机器人包括主手操作端和从手执行端；主手操作端为7DOF并联式机器人，从手执行端由一个固定立体视觉系统的6DOF串联式持镜臂、两个7DOF串联式器械臂和一个6DOF辅助臂；主手操作端与从手执行端通过以太网通信，其特征在于：机器人控制系统包括运动控制系统和立体视觉系统，运动控制系统与立体视觉系统之间通过以太网通信；所述运动控制系统包括运动控制算法模块和底层控制模块；所述立体视觉系统包括立体视觉算法模块；各从手机械臂控制程序的通信模块通过以太网实时接收主手操作臂传递过来的位置控制指令，通过运动控制算法模块中的逆运动学算法解算得到各关节期望位置，通过任务执行线程调用底层控制模块控制伺服驱动器使从手机械臂运动到指定位置；立体视觉系统通过图像采集线程实时采集从手机械臂所处位置信息并传递给视觉算法计算线程，通过立体视觉算法模块计算机械臂相对于世界坐标系的位置，通过以太网将位置信息反馈至运动控制系统；操作者通过立体显示装置观察从臂运动情况操作主手操作臂完成手术任务。

【技术特征摘要】
1.一种基于立体视觉的主从式遥操作手术机器人控制系统，手术机器人包括主手操作端和从手执行端；主手操作端为7DOF并联式机器人，从手执行端由一个固定立体视觉系统的6DOF串联式持镜臂、两个7DOF串联式器械臂和一个6DOF辅助臂；主手操作端与从手执行端通过以太网通信，其特征在于：机器人控制系统包括运动控制系统和立体视觉系统，运动控制系统与立体视觉系统之间通过以太网通信；所述运动控制系统包括运动控制算法模块和底层控制模块；所述立体视觉系统包括立体视觉算法模块；各从手机械臂控制程序的通信模块通过以太网实时接收主手操作臂传递过来的位置控制指令，通过运动控制算法模块中的逆运动学算法解算得到各关节期望位置，通过任务执行线程调用底层控制模块控制伺服驱动器使从手机械臂运动到指定位置；立体视觉系统通过图像采集线程实时采集从手机械臂所处位置信息并传递给视觉算法计算线程，通过立体视觉算法模块计算机械臂相对于世界坐标系的位置，通过以太网将位置信息反馈至运动控制系统；操作者通过立体显示装置观察从臂运动情况操作主手操作臂完成手术任务。2.根据权利要求1所述的一种基于立体视觉的主从式遥操作手术机器人控制系统，其特征在于：所述立体视觉算法模块包括单个相机内部参数和外部参数确定、左相机与右相机之间变换矩阵确定、景深计算、共轭点对特征提取算法。3.根据权利要求2所述的一种基于立体视觉的主从式遥操作手术机器人控制系统，其特征在于：所述的单个相机内部参数和外部参数确定算法，其特征在于，包括以下步骤：S1、获取目标物体在相机坐标系与世界坐标系的关系xc＝Rxw+T，xc为目标物体在相机坐标系中的坐标，xw为目标物体在世界坐标系中的坐标，为相机的外部参数；表示世界坐标系到相机坐标系的旋转矩阵，相机坐标系到世界坐标系的平移向量；S2、根据关系式和确定世界坐标和像素坐标之间的映射关系，其中(x,y,z)为世界坐标，(r,c)为像素坐标，(or,oc)表示包含主点的像素的像素阵列坐标，sx和sy表示一个像素在水平和垂直方向的尺度，λ＝kz，k为未知的正值常数；S3、根据相机坐标系中的点P在世界坐标系中的表示xc＝r11x+r12y+r13z+Tx，yc＝r21x+r22y+r23z+Ty，zc＝r31x+r32y+r33z+Tz，结合S2所述的世界坐标和像素坐标间的映射关系式，得到r11，r12，r13为相机坐标系到世界坐标系的旋转矩阵的第一行；r21，r22，r23为相机坐标系到世界坐标系的...

【专利技术属性】
技术研发人员：向洋，熊亮，谢毅，傅舰艇，王黎，张敏锐，
申请(专利权)人：中国科学院重庆绿色智能技术研究院，
类型：发明
国别省市：重庆,50