一种基于立体视觉的主从式遥操作手术机器人控制系统 |
|||||||
| 申请号 | CN201710254581.5 | 申请日 | 2017-04-18 | 公开(公告)号 | CN106903665A | 公开(公告)日 | 2017-06-30 |
| 申请人 | 中国科学院重庆绿色智能技术研究院; | 发明人 | 向洋; 熊亮; 谢毅; 傅舰艇; 王黎; 张敏锐; | ||||
| 摘要 | 本 发明 公开了一种基于 立体视觉 的主从式遥操作手术 机器人 控制系统，包括运动控制系统和立体视觉系统，运动控制系统包括运动控制 算法 模 块 和底层控 制模 块；立体视觉系统包括立体视觉算法模块；各从手 机械臂 实时接收主手操作臂传递过来的 位置 控制指令，运动控制算法模块解算得到各关节期望位置，底层 控制模块 控制伺服 驱动器 使从手机械臂运动到 指定 位置；立体视觉系统实时采集从手机械臂所处位置信息并传递给视觉算法计算线程，通过立体视觉算法模块计算机械臂相对于世界 坐标系 的位置，通过以太网将位置信息反馈至运动控制系统；操作者通过立体显示装置观察从臂运动情况操作主手操作臂完成手术任务。本发明在系统 稳定性 、实时性和高 精度 等方面达到了较高的 水 平。 | ||||||
| 权利要求 | 1.一种基于立体视觉的主从式遥操作手术机器人控制系统，手术机器人包括主手操作端和从手执行端；主手操作端为7DOF并联式机器人，从手执行端由一个固定立体视觉系统的6DOF串联式持镜臂、两个7DOF串联式器械臂和一个6DOF辅助臂；主手操作端与从手执行端通过以太网通信，其特征在于：机器人控制系统包括运动控制系统和立体视觉系统，运动控制系统与立体视觉系统之间通过以太网通信； |
||||||
| 说明书全文 | 一种基于立体视觉的主从式遥操作手术机器人控制系统技术领域背景技术[0002] 机器人微创手术通过从根本上改变了医生的手术环境及手术方式，有效的避免了医生长时间手术时的疲劳感及由此引起的手术误操作，有利于进一步提高微创手术的手术效果；同时，机器人微创手术能够很好的实现医生的手眼协调，便于开展更为复杂精细的手术操作，极大地拓展了医生的手术能力。主从式遥操作手术机器人控制系统通常由一个主手操作端和若干个从手执行臂组成。主从式遥操作手术机器人控制系统通常由一个主手操作端和若干个从手执行臂组成。从手执行臂末端安装内窥镜及手术器械通过创口直达病灶处，医生通过操作主手即可控制从手末端器械完成各种手术操作。将立体视觉和主从式遥操作控制系统于一体的手术机器人系统，需要进行主从手及各从手之间算法的实时计算及网络实时通信，这就提高了遥操作控制系统的复杂性，现有技术在稳定性、实时性和高精度等方面还有欠缺。 发明内容[0003] 有鉴于此，本发明提供一种基于立体视觉的主从式遥操作手术机器人控制系统，通过在从手端采用立体视觉功能，以提高医生实施遥操作手术时的真实感，更好的操控机器人完成手术。 [0004] 本发明的目的是通过这样的技术方案实现的，一种基于立体视觉的主从式遥操作手术机器人控制系统，手术机器人包括主手操作端和从手执行端；主手操作端为7DOF并联式机器人，从手执行端由一个固定立体视觉系统的6DOF串联式持镜臂、两个7DOF串联式器械臂和一个6DOF辅助臂；主手操作端与从手执行端通过以太网通信，机器人控制系统包括运动控制系统和立体视觉系统，运动控制系统与立体视觉系统之间通过以太网通信； [0005] 所述运动控制系统包括运动控制算法模块和底层控制模块；所述立体视觉系统包括立体视觉算法模块；各从手机械臂控制程序的通信模块通过以太网实时接收主手操作臂传递过来的位置控制指令，通过运动控制算法模块中的逆运动学算法解算得到各关节期望位置，通过任务执行线程调用底层控制模块控制伺服驱动器使从手机械臂运动到指定位置；立体视觉系统通过图像采集线程实时采集从手机械臂所处位置信息并传递给视觉算法计算线程，通过立体视觉算法模块计算机械臂相对于世界坐标系的位置，通过以太网将位置信息反馈至运动控制系统；操作者通过立体显示装置观察从臂运动情况操作主手操作臂完成手术任务。 [0006] 进一步，所述立体视觉算法模块包括单个相机内部参数和外部参数确定、左相机与右相机之间变换矩阵确定、景深计算、共轭点对特征提取等算法。 [0007] 进一步，所述的单个相机内部参数和外部参数确定算法，其特征在于，包括以下步骤： [0008] S1、获取目标物体在相机坐标系与世界坐标系的关系xc＝Rxw+T，xc为目标物体在相机坐标系中的坐标，xw为目标物体在世界坐标系中的坐标， 为相机的外部参数； 表示世界坐标系到相机坐标系的旋转矩阵， 相机坐标系到世界坐标系的平移向量。 [0009] S2、根据关系式 和 确定世界坐标和像素坐标之间的映射关系，其中(x,y,z)为世界坐标，(r,c)为像素坐标，(or,oc)表示包含主点的像素的像素阵列坐标，sx和sy表示一个像素在水平和垂直方向的尺度， λ＝kz，k为未知的正 值常数； [0010] 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所述的世界坐标和像素坐标间的映射关系式，得到[0011] [0012] 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为正交矩阵的约束关系求得所有参数。 [0013] 进一步，所述左相机与右相机之间变换矩阵确定包括以下步骤： [0014] 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方向的分量； [0015] 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乘子法基于平方误差 最小的原则求得最优解。 [0016] 进一步，所述的景深计算具体为：根据S52所述展开式得到， [0017] [0018] 由于采用了上述技术方案，本发明具有如下的优点： [0020] 为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步的详细描述，其中： [0023] 图3为本发明控制系统控制流程示意图。 具体实施方式[0024] 以下将结合附图，对本发明的优选实施例进行详细的描述；应当理解，优选实施例仅为了说明本发明，而不是为了限制本发明的保护范围。 [0025] 附图1为本发明的控制系统硬件架构，一种基于立体视觉的主从式遥操作手术机器人控制系统，手术机器人包括主手操作端和从手执行端；主手操作端为7DOF并联式机器人，从手执行端由一个固定立体视觉系统的6DOF串联式持镜臂、两个7DOF串联式器械臂和一个6DOF辅助臂；主手操作端与从手执行端通过以太网通信，机器人控制系统包括运动控制系统和立体视觉系统，运动控制系统与立体视觉系统之间通过以太网通信； [0026] 控制系统包括运动控制系统和立体视觉系统，运动控制系统由工控机、运动控制卡、以太网卡、数字量与模拟量采集卡、伺服驱动器及电机等组成；立体视觉系统由工控机、图像采集卡、以太网卡、微光源和两个微相机等组成；运动控制系统与立体视觉系统之间通过以太网通信。 [0027] 附图2为本发明的控制系统软件架构示意图，所述运动控制系统包括人机界面模块、控制程序流程模块、通信模块、底层控制模块、运动学及动力学算法模块；所述控制程序流程模块包括通信线程、运动学及动力学计算线程、任务执行线程、底层控制线程等。所述运动学及动力学算法模块包括机械臂运动学正逆解、动力学正逆解、轨迹规划及雅可比矩阵等。所述立体视觉系统包括通信模块、视觉程序流程模块、图像采集模块、图像雅可比矩阵及视觉算法模块；所述的立体视觉算法模块包括单个相机内部参数和外部参数确定、左相机与右相机之间变换矩阵确定、景深计算、共轭点对特征提取等算法；所述视觉程序流程模块包括通信线程、视觉算法计算线程、图像采集线程及任务执行线程等。 [0028] 附图3为本发明的控制系统控制流程示意图，其控制流程包括，各从手机械臂的通信模块通过以太网实时接收主手操作臂传递过来的位置控制指令，通过运动控制算法模块中的逆运动学解算得到各关节期望位置，通过任务执行线程调用底层控制模块控制伺服驱动器使从手机械臂运动到指定位置；立体视觉系统通过图像采集线程实时采集从手机械臂所处位置信息并传递给视觉算法计算线程，通过立体视觉算法计算机械臂相对于世界坐标系的位置，通过以太网将位置信息反馈至运动控制系统；操作者通过立体显示装置观察从臂运动情况操作主手操作臂实现手术任务。 [0029] 所述的单个相机内部参数和外部参数确定算法，其特征在于，包括以下步骤： [0030] S1、获取目标物体在相机坐标系与世界坐标系的关系xc＝Rxw+T，xc为目标物体在相机坐标系中的坐标，xw为目标物体在世界坐标系中的坐标， 为相机的外部参数； 表示世界坐标系到相机坐标系的旋转矩阵， 相机坐标系到世界坐标系的平移向量。 [0031] S2、根据关系式 和 确定世界坐标和像素坐标之间的映射关系，其中(x,y,z)为世界坐标，(r,c)为像素坐标，(or,oc)表示包含主点的像素的像素阵列坐标，sx和sy表示一个像素在水平和垂直方向的尺度， λ＝kz，k为未知的正 值常数； [0032] 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所述的世界坐标和像素坐标间的映射关系式，得到[0033] [0034] 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为正交矩阵的约束关系求得所有参数。 [0035] 所述左相机与右相机之间变换矩阵确定包括以下步骤： [0036] 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方向的分量； [0037] 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乘子法基于平方误差 最小的原则求得最优解。 [0038] 所述的景深计算具体为：根据S52所述展开式得到， [0039] 本发明提出了一种基于立体视觉的主从式遥操作手术机器人控制系统，按照所述流程搭建控制系统硬件并编写控制程序实现了集立体视觉及主从式遥操作控制系统功能。 |
||||||
