CN100464720C - 基于光学跟踪闭环控制的脑外科机器人系统及实现方法 - Google Patents

Abstract

一种基于光学跟踪闭环控制脑外科机器人系统，是进行接收医学影像信息、测定并确定病灶位置、辅助进行手术规划、进行手术导引的脑外科机器人；由计算机、五自由度机器人、光学跟踪设备和被动标识器构成一个闭环的机器人位姿测量与实时反馈控制系统，被动标识器安装在五自由度机器人末端；五自由度机器人包括机械臂和机械臂控制器；辅助手术规划与导引软件包括数字影像输入与预处理模块、病灶提取与三维重构模块、手术规划模块、以及手术实施模块；其优点是：提高机器人系统的轨迹跟踪和定位精度，解决限制外科手术机器人推广使用的绝对精度问题，降低机器人设计和制造成本；简化注册标定过程，减轻医生工作量和出错概率，减轻病人的痛苦。

Description

基于光学跟踪闭环控制的脑外科机器人系统及实现方法

技术领域

本发明属于机器人技术领域，特别涉及一种基于光学跟踪闭环控制的脑外科机器人系统及实现方法，具体地说是指一种可以辅助医生进行精细脑外科微创手术的高精度机器人系统及其实现方法。

背景技术

脑外科微创手术机器人是一种可以辅助医生施行无框架立体定向手术的机器人系统，一般都包括有能够辅助进行手术规划与导引的计算机、以及执行立体定向辅助手术操作的机器人。其中，辅助进行手术规划与导引的计算机，安装配套的辅助手术规划与导引软件，主要完成病人脑部医学影像信息的处理和三维重构，辅助医生进行虚拟手术规划，并在手术过程中向机器人控制器发出运动指令以控制机器人运动。该软件通常包括数字影像输入与预处理、病灶提取与三维重构、手术规划、以及手术实施等功能模块。数字影像输入与预处理模块的作用是接收外部输入的病人脑部的医学影像数据，并转换成计算机可以显示的格式，在此基础上对图像进行预处理，包括图像的强化、去燥、直方统计等；病灶提取与三维重构模块的作用是让医生以交互的方式在图像上对病灶进行分割，并提取头颅轮廓，从而生成病灶和头颅的三维医学模型；在手术规划模块中，医生可以进行靶点与标记点的标识工作，并可以在虚拟现实环境中进行手术路径规划；在手术实施模块中，可以按照预先规划好的手术路径，适时向机器人控制器发出运动指令以控制机器人运动，同时虚拟现实功能可以为手术提供实时的监控。

系统所采用的机器人由机械臂和控制器组成，一般具有5个关节自由度，每个关节都有驱动机构，用于驱动关节运动，能够保证手术器械以任意姿态到达手术空间中的任意一点。机器人控制器能够接受计算机发出的运动指令，实时控制机器人的各个关节运动到达指定位置。

现有脑外科机器人存在的最大问题是定位精度不能满足精细脑外科手术的高精确性要求。外科机器人不仅要求重复精度高，对器械运动的绝对精度要求更严格，因为在虚拟的三维医学模型空间进行的手术规划，最终要由机器人的运动实现。由于机构和控制误差的因素，期望的位置命令和机器人实际达到的位置之间不可避免地存在误差。这种误差通常可能达到几个毫米到一个厘米左右，远远超过微创伤外科手术的精度要求，必须设法加以克服。在工业应用中，机器人的绝对定位误差可以通过各种方式的标定来克服，但外科机器人作为一种手术设备，其应用情况差别很大，一般没有严格固定的安置位置。显然，对每一例手术都要求进行严格、繁琐的现场标定是不适合的，况且一些非几何误差因素如机构弹性形变、关节间隙和柔性等的影响，也难以依靠标定解决。目前，许多工作都是从机械上考虑提高精度的方法，依靠高精密度的机构加工、装配以及高精度的控制和补偿来保证系统精度，很难达到理想效果，而且大大增加了机器人的设计和制造成本。现有的脑外科机器人从全局上是开环的，即从计算机给出位置命令到引导机器人运动和定位的过程，缺少机器人末端位姿的反馈和校正，各种误差因素无法得到全局的闭环校正，绝对位姿精度很难保证。

另外，从已经公开的报道(例如中国专利公开号CN1243690A)，确定病灶相对机器人操作空间的位置的所谓标测定位方法过于复杂，需要用到立体定位框架、标测钉、或双层模板、六关节数字化机械臂、力控制人机交互技术等，既费时又费力，也给病人带来痛苦和压力；也有运用视觉测量技术进行注册的方法(例如中国专利公开号CN1554315A)，但是精度和环境适应性还需要提高，较难实用。

发明内容

本发明的目的是提供一种基于光学跟踪闭环控制的脑外科机器人系统及实现方法，可以克服现有技术的缺点。它是将光学跟踪设备与脑外科机器人结合，利用光学跟踪设备实时跟踪机器人末端器械的当前位置和姿态，在光学测量空间进行实时的位姿闭环控制，使得脑外科机器人末端能够精确地按照预先规划的理想手术路径进行运动和定位，实现能够满足精细脑外科手术要求的高精度脑外科机器人系统。同时，借助于光学跟踪技术，大大简化标测三维医学模型空间与机器人基座坐标空间的映射变换的过程和方法。使用本发明能够使脑外科机器人达到高精度、低成本，而且省时、便捷、实用、易操作。

本发明提出的基于光学跟踪闭环控制的脑外科机器人系统包括：

五自由度机器人、计算机(选用型号PC P4 2.0GHz/256M)、光学跟踪设备(选用型号NDI公司POLARIS光学跟踪设备)、光学注册工具(选用型号NDI公司POLARIS标配被动工具)、被动标识器(选用型号NDI公司POLARIS标配球形被动标识器)、医学标志器(医学上使用的一种用钽金属材料制成的、对射电高度不透明的小球形标志点，与生物体兼容，可长期植入人体)、塑形枕和手术床设备组成的硬件部分；辅助手术规划与导引软件部分，即进行接收医学影像信息、测定并确定病灶的位置、辅助进行手术规划、并进行手术导引的脑外科机器人；其特征是：

所说的计算机、五自由度机器人、光学跟踪设备、和被动标识器构成一个闭环的机器人位姿测量与实时反馈控制系统，被动标识器安装在五自由度机器人的末端，始终被光学跟踪设备跟踪；所说的五自由度机器人包括五自由度机械臂和机械臂控制器；所说的辅助手术规划与导引软件包括数字影像输入与预处理模块、病灶提取与三维重构模块、手术规划模块、以及手术实施模块。

具体操作步骤是：

一.在进行手术前准备时：在病人头颅上粘贴四个医学标志器，对脑部进行医学影像扫描，并将扫描所得医学影像信息输入计算机，运用辅助手术规划与导引软件确定病灶，重构病灶和头颅的三维医学模型，进行手术靶点与医学标志影像位置的标识，并规划手术路径。

二.在进行注册标定时：让病人躺在手术床上，头部使用塑形枕与手术床相对固定，一方面用光学注册工具测定病人头颅上的四个医学标志器的坐标，测量值由光学跟踪设备给出，并送入计算机，由计算机计算三维医学模型空间与光学测量空间之间的映射变换；另一方面用五自由度机器人基座坐标空间与光学测量空间映射变换的自动标测方法，由五自由度机器人、光学跟踪设备、被动标识器、和计算机配合，自动测定五自由度机器人基座坐标空间与光学测量空间之间的映射变换。

三.在进行手术实施时：首先，由计算机将在三维医学模型空间预先规划的手术路径变换到光学测量空间，并计算出在当前路径点五自由度机器人末端需要达到的理想位姿；然后，由计算机根据当前路径点的理想位姿，进行由光学测量空间到五自由度机器人基座坐标空间的坐标变换，并通过求解五自由度机器人逆运动学方程，得到五自由度机器人各个关节的理想位置；然后，由计算机向五自由度机器人输入各个关节的理想位置，控制五自由度机器人运动；同时，由光学跟踪设备和安装在五自由度机器人末端的被动标识器配合，实时测定五自由度机器人的末端位姿并送入计算机，用五自由度机器人末端位姿的闭环控制方法对五自由度机器人的末端位姿进行实时控制，实现精确的轨迹跟踪和定位；最后，五自由度机器人锁定，医生在五自由度机器人的辅助下进行手术操作。

五自由度机械臂由手臂和手腕组成，具有五个关节，采取两个滑动关节和三个转动关节的PPRRR构型；手臂部分具有三个关节，分别为I、II和III关节，第I关节采用滑动关节，运动方向垂直于水平面；第II关节采用滑动关节，与第I关节成“T”形垂直；第III关节采用转动关节，轴线与第I关节运动方向平行；手腕部分具有两个关节，分别为IV和V关节，第IV关节采用转动关节，轴线与第III关节轴线平行；第V关节采用转动关节，轴线与第IV关节轴线垂直；在机械臂的第V关节安装末端器械，安装轴线与第IV关节轴线平行。每个关节都有独立的驱动机构，由步进电机、减速器组成，滑动关节的驱动机构还包括有滚珠丝杆。

机械臂控制器由可编程逻辑控制器PLC和步进电机驱动器组成，采用三个PLC和五个步进电机驱动器组合，对应控制五自由度机械臂的五个关节，机械臂控制器通过RS232C串口通信组件与外部计算机进行通信，可接受外部计算机输入的关节位置命令，控制机械臂运动到达指定位置。

NDI公司的POLARIS光学跟踪设备，是目前医疗行业中广泛应用的一种空间测量定位仪器。被动式的POLARIS设备包括一个能发射并接收红外照明光的位置传感器，配套提供若干被动标识器或安装有被动标识器的被动工具；POLARIS设备通过测量工具上被动标识器的空间位置，就可以实时地确定工具的位置和方向，3D均方误差范围通常在0.35mm范围内；POLARIS设备与计算机之间通过RS-232/RS-422通讯，连续跟踪的数据更新频率达到60HZ。

闭环的机器人位姿测量和实时反馈控制系统的方法是：由光学跟踪设备测定五自由度机器人的末端位姿，由计算机根据测量得到的位姿和期望五自由度机器人达到的位姿计算控制量，控制五自由度机器人进一步运动以修正偏差。

光学跟踪设备通过跟踪安装在五自由度机器人末端的被动标识器来测量五自由度机器人的末端位姿。

被动标识器安装在五自由度机器人的末端，数量至少为3个。

被动标识器在五自由度机器人末端安装的几何位置关系满足：任意两个被动标识器之间的距离不能小于50mm、任意两条线段不平行、而且任意两条线段之间的夹角不能低于0.5度。

基于光学跟踪闭环控制的脑外科机器人系统的实现方法，其特征是，借助于光学跟踪技术进行位姿测量，包括：三维医学模型空间与光学测量空间映射变换的简便标测方法、五自由度机器人基座坐标空间与光学测量空间映射变换的自动标测方法及五自由度机器人末端位姿的闭环控制方法。

所述三种方法涉及五个坐标系，如图2：在三维医学模型空间中建立一个三维医学模型坐标系{V}；在真实的病人头颅上建立一个患者坐标系{P}；在光学测量空间中建立一个光学测量坐标系{M}；在五自由度机器人的基座上建立一个机器人基座坐标系{R}；在五自由度机器人的末端建立一个末端工具坐标系{T}；整个系统以光学测量坐标系{M}为基准参考系；其中，患者坐标系是基于粘贴在病人头颅上的四个医学标志器所在的点来描述的，该坐标系在四个医学标志器中选取任意一个标志器所在的点M₀作为坐标系原点，同时以M₀与其他三个标志器所在的点M₁、M₂、M₃的连线作为三个坐标轴向。

下面给出所述上述方法的详细步骤和数学描述。

一.三维医学模型空间与光学测量空间的映射变换

借助于光学跟踪技术进行位姿测量，三维医学模型空间与光学测量空间映射变换的简便标测方法是：由医生在病人头部粘贴四个医学标志器，这四个标志器不在同一平面，且任意三个标志器不在同一条直线上；对病人头部进行CT或MRI医学影像扫描，所得扫描影像输入计算机；让病人躺在手术床上，头部使用塑形枕与手术床相对固定，用光学注册工具测定病人头部的四个医学标志器在光学测量空间的坐标，测量值由光学跟踪设备输入计算机；由计算机计算三维医学模型空间与光学测量空间的映射变换。

该映射变换由两组坐标转换组成，即从三维医学模型坐标系{V}到患者坐标系{P}的转换、和从患者坐标系{P}到光学测量坐标系{M}的转换，两组转换都是基于在病人头颅上粘贴的四个医学标志器来确定。

显然，根据患者坐标系{P}的定义，四个标志器在{P}中的坐标分别为：M₀(0，0，0)，M₁(1，0，0)，M₂(0，1，0)，M₃(0，0，1)。

另外，由于四个医学标志器在医学扫描影像中是可以识别的，它们在三维医学模型坐标系{V}中的坐标也可以获得，假设记为：M′₀(x_v0，y_v0，z_v0)，M′₁(x_v1，y_v1，z_v1)，M′₂(x_v2，y_v2，z_v2)，M′₃(x_v3，y_v3，z_v3)。

由于病人脑部的三维医学模型是通过病人的脑部扫描数据重构的，因此可以认为三维医学模型坐标系{V}与患者坐标系{P}的映射是刚性变换(包括平移、旋转和拉伸)，可以用一个齐次变换矩阵^VT_P完成两个坐标系中的位置映射。由{P}向{V}的齐次变换矩阵为：

$T_P^V=\left[\begin{array}{cccc}{x}_{v1}-x_{v0}& x_{v2}-x_{v0}& x_{v3}-x_{v0}& x_{v0}\\ y_{v1}-y_{v0}& y_{v2}-y_{v0}& y_{v3}-y_{v0}& y_{v0}\\ z_{v1}-z_{v0}& z_{v2}-z_{v0}& z_{v3}-z_{v0}& z_{v0}\\ 0& 0& 0& 1\end{array}\right]---\left(1\right)$

同理，可以确定患者坐标系{P}向光学测量坐标系{M}的转换矩阵^MT_P。四个医学标志器在光学测量坐标系{M}中的位置，可以通过用光学注册工具分别点到四个医学标志器来获得，设为：M₀(x_m0，y_m0，z_m0)，M₁(x_m1，y_m1，z_m1)，M₂(x_m2，y_m2，z_m2)，M₃(x_m3，y_m3，z_m3)。则由{P}向{M}的齐次变换矩阵为：

$T_P^M=\left[\begin{array}{cccc}{x}_{m1}-x_{m0}& x_{m2}-x_{m0}& x_{m3}-x_{m0}& x_{m0}\\ y_{m1}-y_{m0}& y_{m2}-y_{m0}& y_{m3}-y_{m0}& y_{m0}\\ z_{m1}-z_{m0}& z_{m2}-z_{m0}& z_{m3}-z_{m0}& z_{m0}\\ 0& 0& 0& 1\end{array}\right]---\left(2\right)$

最后，由三维虚拟模型坐标系{V}到光学测量坐标系{M}的齐次变换为^MT_V即为：

^MT_V＝^MT_P ^PT_V＝^MT_P(^VT_P)^-1        (3)

二.五自由度机器人基座坐标空间与光学测量空间的映射变换

借助于光学跟踪技术进行位姿测量，五自由度机器人基座坐标空间与光学测量空间映射变换的自动标测方法是：首先，选定五自由度机器人工作空间内的四个点，这四个点同时应该处于光学跟踪设备的测量范围内，并且四个点不共面，任意三个点不共线；由计算机按照预设的程序，向五自由度机器人发出到达上述四个点的关节位置命令，控制五自由度机器人末端依次运动到上述四个点，计算机记录下上述四个点在五自由度机器人基座坐标空间中的坐标；同时，由光学跟踪设备和安装在五自由度机器人末端的被动标识器配合，依次测定五自由度机器人运动到上述四个点时其末端在光学测量空间中的坐标，并送入计算机；最后，由计算机根据上述四个点在五自由度机器人基座坐标空间和光学测量空间的坐标值，计算出五自由度机器人基座坐标空间与光学测量空间的映射变换，从而完成自动标测。

假设所述四个点在机器人基座坐标系{R}中的坐标为^RP_i(r_i，s_i，t_i)，(i＝1，2，3，4)，同时，所述四个点在光学测量坐标系中的坐标为^MP_i(u_i，v_i，w_i)，(i＝1，2，3，4)，则由机器人基座坐标系{R}到光学测量坐标系{M}的齐次变换矩阵为：

$T_R^M=\left[\begin{array}{cccc}{u}_1& u_2& u_3& u_4\\ v_1& v_2& v_3& v_4\\ w_1& w_2& w_3& w_4\\ 1& 1& 1& 1\end{array}\right]{\left[\begin{array}{cccc}{r}_1& r_2& r_3& r_4\\ s_1& s_2& s_3& s_4\\ t_1& t_2& t_3& t_4\\ 1& 1& 1& 1\end{array}\right]}^{-1}---\left(4\right)$

三.五自由度机器人末端位姿的闭环控制方法

借助于光学跟踪技术进行位姿测量，五自由度机器人末端位姿的闭环控制方法是：以光学测量空间为基准参考空间，首先，由计算机将在三维医学模型空间预先规划的手术路径变换到光学测量空间，并计算出在当前路径点五自由度机器人末端需要达到的理想位姿；然后，由计算机根据当前路径点的理想位姿，进行由光学测量空间到五自由度机器人基座坐标空间的坐标变换，并通过求解五自由度机器人逆运动学方程，得到五自由度机器人各个关节的理想位置；然后，由计算机向五自由度机器人输入各个关节的理想位置，控制五自由度机器人运动；同时，由光学跟踪设备和安装在五自由度机器人末端的被动标识器配合，实时测定五自由度机器人末端的位姿，并送入计算机；然后，由计算机将实时测定的位姿信息与预先规划的当前路径点的理想位姿进行比较，得到两者的位姿偏差；最后，由计算机根据位姿偏差按照预先确定的控制规律计算校正控制量，修正当前路径点的理想位姿，并开始新一轮的运动控制，从而实现精确的轨迹跟踪和定位。

由计算机根据位姿偏差计算校正控制量时所采用的控制规律是：位置控制采用比例-积分-微分调节；姿态控制采用比例调节。

五自由度机器人的末端位姿是在光学测量空间中进行闭环控制。

如图5，设末端工具坐标系{T}在光学测量坐标系{M}中的位姿用X＝[P^T φ^T]^T表示。其中P＝[x y z]^T表示{T}的位置；φ＝[α β γ]^T表示{T}的姿态，由一组Z-Y-X欧拉角组成，

记X_d＝[P_d ^T φ_d ^T]^T表示理想位姿，P_d＝[x_d y_d z_d]^T，φ_d＝[α_d β_d γ_d]^T；X_r＝[P_r ^T φ_r ^T]^T表示实际测量得到的位姿，P_r＝[x_r y_r z_r]^T，φ_r＝[α_r β_r γ_r]^T；ΔX＝[ΔP^T Δφ^T]^T表示X_d和X_r的偏差，ΔP＝[Δx Δy Δz]^T，Δφ＝[Δα Δβ Δγ]^T，

位置偏差可以直接计算，即：

ΔP＝P_d-P_r              (5)

姿态偏差的计算涉及旋转变换，用R_d表示{T}相对于{M}的理想旋转变换矩阵；R_r表示根据测量值得到的实际变换矩阵；ΔR表示由R_r向R_d的变换，则：

$R_d=R_Z\left({\mathrm{\α}}_d\right)R_Y\left({\mathrm{\β}}_d\right)R_X\left({\mathrm{\γ}}_d\right)$

$=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_d\cos {\mathrm{\β}}_d& \cos {\mathrm{\α}}_d\sin {\mathrm{\β}}_d\sin {\mathrm{\γ}}_d-\sin {\mathrm{\α}}_d\cos {\mathrm{\γ}}_d& \cos {\mathrm{\α}}_d\sin {\mathrm{\β}}_d\cos {\mathrm{\γ}}_d+\sin {\mathrm{\α}}_d\sin {\mathrm{\γ}}_d\\ \sin {\mathrm{\α}}_d\cos {\mathrm{\β}}_d& \sin {\mathrm{\α}}_d\sin {\mathrm{\β}}_d\sin {\mathrm{\γ}}_d+\cos {\mathrm{\α}}_d\cos {\mathrm{\γ}}_d& \sin {\mathrm{\α}}_d\sin {\mathrm{\β}}_d\cos {\mathrm{\γ}}_d-\cos {\mathrm{\α}}_d\sin {\mathrm{\γ}}_d\\ -\sin {\mathrm{\β}}_d& \cos {\mathrm{\β}}_d\sin {\mathrm{\γ}}_d& \cos {\mathrm{\β}}_d\cos {\mathrm{\γ}}_d\end{array}\right]$

                                                     (6)

$R_r=R_Z\left({\mathrm{\α}}_r\right)R_Y\left({\mathrm{\β}}_r\right)R_X\left({\mathrm{\γ}}_r\right)$

$=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_r\cos {\mathrm{\β}}_r& \cos {\mathrm{\α}}_r\sin {\mathrm{\β}}_r\sin {\mathrm{\γ}}_r-\sin {\mathrm{\α}}_r\cos {\mathrm{\γ}}_r& \cos {\mathrm{\α}}_r\sin {\mathrm{\β}}_r\cos {\mathrm{\γ}}_r+\sin {\mathrm{\α}}_r\sin {\mathrm{\γ}}_r\\ \sin {\mathrm{\α}}_r\cos {\mathrm{\β}}_r& \sin {\mathrm{\α}}_r\sin {\mathrm{\β}}_r\sin {\mathrm{\γ}}_r+\cos {\mathrm{\α}}_r\cos {\mathrm{\γ}}_r& \sin {\mathrm{\α}}_r\sin {\mathrm{\β}}_r\cos {\mathrm{\γ}}_r-\cos {\mathrm{\α}}_r\sin {\mathrm{\γ}}_r\\ -\sin {\mathrm{\β}}_r& \cos {\mathrm{\β}}_r\sin {\mathrm{\γ}}_r& \cos {\mathrm{\β}}_r\cos {\mathrm{\γ}}_r\end{array}\right]$

                                                     (7)

记： $\mathrm{\ΔR}=R_d{R}_r^{-1}=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]---\left(8\right)$

又有：

$\mathrm{\ΔR}=R_Z\left(\mathrm{\Δ\α}\right)R_Y\left(\mathrm{\Δ\β}\right)R_X\left(\mathrm{\Δ\γ}\right)$

$=\left[\begin{array}{ccc}\cos \mathrm{\Δ\α}\cos \mathrm{\Δ\β}& \cos \mathrm{\Δ\α}\sin \mathrm{\Δ\β}\sin \mathrm{\Δ\γ}-\sin \mathrm{\Δ\α}\cos \mathrm{\Δ\γ}& \cos \mathrm{\Δ\α}\sin \mathrm{\Δ\β}\cos \mathrm{\Δ\γ}+\sin \mathrm{\Δ\α}\sin \mathrm{\Δ\γ}\\ \sin \mathrm{\Δ\α}\cos \mathrm{\Δ\β}& \sin \mathrm{\Δ\α}\sin \mathrm{\Δ\β}\sin \mathrm{\Δ\γ}+\cos \mathrm{\Δ\α}\cos \mathrm{\Δ\γ}& \sin \mathrm{\Δ\α}\sin \mathrm{\Δ\β}\cos \mathrm{\Δ\γ}-\cos \mathrm{\Δ\α}\sin \mathrm{\Δ\γ}\\ -\sin \mathrm{\Δ\β}& \cos \mathrm{\Δ\β}\sin \mathrm{\Δ\γ}& \cos \mathrm{\Δ\β}\cos \mathrm{\Δ\γ}\end{array}\right]$

                                                        (9)

由(6)、(7)、(8)、(9)式联合可以解得姿态偏差Δφ＝[Δα Δβ Δγ]^T，其中：

a.当sinΔβ≠0时，

$\mathrm{\Δ\β}=A\tan 2(\sqrt{r_{31}^2+r_{32}^2},r_{33}),$

Δa＝Atan 2(r₂₃/sinΔβ，r₁₃/sinΔβ)，

Δγ＝Atan 2(r₃₂/sinΔβ，-r₃₁/sinΔβ)         (10)

b.当sinΔβ＝0时，若Δβ＝0.0，则：

Δα＝0.0，

Δγ＝Atan 2(-r₁₂，r₁₁)                      (11)

若Δβ＝180.0°，则：

Δα＝0.0，

Δγ＝Atan 2(r₁₂，-r₁₁)                      (12)

下面分别讨论位置校正和姿态校正问题，采用离散形式列写公式，括号中的下标k表示控制循环的第k步。

位置校正采用比例-积分-微分(PID)控制，校正控制量记为U_P＝[u_x u_y u_z]^T，则：

$U_P\left(k\right)=K_{\mathrm{PP}}\mathrm{\ΔP}\left(k\right)+K_{\mathrm{PI}}T\underset{j=0}{\overset{k}{\mathrm{\Σ}}}\mathrm{\ΔP}\left(j\right)+K_{\mathrm{PD}}\frac{\mathrm{\ΔP}\left(k\right)-\mathrm{\ΔP}(k-1)}{T}---\left(13\right)$

其中T是系统的采样控制周期，比如按POLARIS设定的最高测量数据更新率60HZ，T可取为16.7ms；K_PP是由三个比例控制系数组成的对角矩阵；K_PI是由三个积分控制系数组成的对角矩阵；K_PD是由三个微分控制系数组成的对角矩阵。将上式改写成增量形式：

$U_P\left(k\right)=U_P(k-1)+(K_{\mathrm{PP}}+{\mathrm{TK}}_{\mathrm{PI}}+\frac{K_{\mathrm{PD}}}{T})\mathrm{\ΔP}\left(k\right)$

$-(K_{\mathrm{PP}}+\frac{2K_{\mathrm{PD}}}{T})\mathrm{\ΔP}(k-1)+\frac{K_{\mathrm{PD}}}{T}\mathrm{\ΔP}(k-2)$       (14)

记 $A=K_{\mathrm{PP}}+{\mathrm{TK}}_{\mathrm{PI}}+\frac{K_{\mathrm{PD}}}{T},$ $B=K_{\mathrm{PP}}+\frac{2K_{\mathrm{PD}}}{T},$ $C=\frac{K_{\mathrm{PD}}}{T},$ 则进一步将算法写成计算机实现的形式：

$\left\{\begin{array}{c}{U}_P\left(k\right)=\mathrm{A\ΔP}\left(k\right)+F(k-1)\\ F\left(k\right)=U_P\left(k\right)-\mathrm{B\ΔP}\left(k\right)+\mathrm{C\ΔP}(k-1)\end{array}\right.---\left(15\right)$

初值可取F(k-1)＝0，ΔP(k-1)＝0，算法的每一步都要计算ΔP(k)、U_P(k)、F(k)，其中F(k)用于下一步计算U_P(k)。

姿态校正采用比例(P)控制，校正控制量记为U_φ＝[u_α u_β u_γ]^T，对应的旋转矩阵为则：

U_φ(k)＝K_φPΔφ(k)         (16)

其中K_φP是由三个比例控制系数组成的对角矩阵，

$\mathrm{\Δ}\hat{R}=R_Z\left(u_{\mathrm{\α}}\right)R_Y\left(u_{\mathrm{\β}}\right)R_X\left(u_{\mathrm{\γ}}\right)---\left(17\right)$

记修正后的位姿指令为 ${\hat{X}}_d={\left[\begin{array}{cc}{{\hat{P}}_d}^T& {{\widehat{\mathrm{\φ}}}_d}^T\end{array}\right]}^T,$ ${\hat{P}}_d={\left[\begin{array}{ccc}{\hat{x}}_d& {\hat{y}}_d& {\hat{z}}_d\end{array}\right]}^T,$ ${\widehat{\mathrm{\φ}}}_d={\left[\begin{array}{ccc}{\widehat{\mathrm{\α}}}_d& {\widehat{\mathrm{\β}}}_d& {\widehat{\mathrm{\γ}}}_d\end{array}\right]}^T,$ 对应的旋转矩阵为

则：

${\hat{P}}_d\left(k\right)=P_d\left(k\right)+U_P\left(k\right)---\left(18\right)$

${\hat{R}}_d\left(k\right)=R_Z\left({\widehat{\mathrm{\α}}}_d\left(k\right)\right)R_Y\left({\widehat{\mathrm{\β}}}_d\left(k\right)\right)R_X\left({\widehat{\mathrm{\γ}}}_d\left(k\right)\right)=R_d\left(k\right)\mathrm{\Δ}\hat{R}\left(k\right)---\left(18\right)$

由(6)、(17)、(19)式联合求解 ${\widehat{\mathrm{\φ}}}_d\left(k\right)={\left[\begin{array}{ccc}{\widehat{\mathrm{\α}}}_d\left(k\right)& {\widehat{\mathrm{\β}}}_d\left(k\right)& {\widehat{\mathrm{\γ}}}_d\left(k\right)\end{array}\right]}^T,$ 其过程可参照上述由(6)、(7)、(8)、(9)式联合求解Δφ＝[Δα Δβ Δγ]^T，不再赘述。

最后，

代替X_d作为位姿指令送出，用于引导机器人运动。

本发明具有的优点是：通过对机器人末端位姿进行实时的闭环控制，使影响机器人运动和定位的主要误差因素，如机器人本体的机构和控制误差，以及手术过程中的各种测量和计算误差等因素都能得到校正，从而有效地保证了手术机器人系统全局的运动和定位精度，解决了现有外科手术机器人存在的轨迹跟踪和定位精度不能满足临床精细手术运用要求的问题。另一方面，采用了全局的位姿控制以后，对机器人本体的机构设计、制造及控制精度的要求等都可以放宽，从而降低机器人本体的设计和制造成本，并有条件更多地关注机器人的手术灵活性和可操作性，设计出人机更加协调的手术机器人机构系统。本发明提出的方法同样适合于某些对绝对精度要求严格的工业机器人应用领域。

本发明还可以自动标定机器人基座坐标空间与光学测量空间的映射变换，免去了繁琐的机器人基座坐标系的人工标定过程，而且通过在病人头部粘贴医学标志器、采用光学注册工具测定医学标志器的坐标，就可以很简捷地获得由三维医学模型空间与光学测量空间的映射变换。

总之，本发明一方面可以明显提高机器人系统的轨迹跟踪和定位精度，不仅解决了限制外科手术机器人推广使用的关键性的绝对精度问题，而且有利于降低机器人本体的设计和制造成本；另一方面大大简化了注册标定过程，不仅减轻了医生的工作量、心理负担和出现差错的概率，而且减轻了病人的痛苦和压力。

附图说明

图1是本发明的结构示意图。

图2是本发明的工作原理框图和所涉及坐标系定义的示意图。

图3是本发明的三维医学模型空间与光学测量空间映射变换的标测方法的流程框图。

图4是本发明的五自由度机器人基座坐标空间与光学测量空间映射变换的自动标测方法的流程框图。

图5是本发明的五自由度机器人末端位姿的闭环控制方法的流程和原理框图。

图6是本发明的五自由度机器人机械臂结构示意图。

具体实施方式

参照附图对本发明作详细说明：

如图所示，设备组成和图中个标号的含义是，它是由计算机1、五自由度机器人2、光学跟踪设备3、光学注册工具4、被动标识器5、医学标志器6、塑形枕7、手术床8硬件部分，和辅助手术规划与导引软件9部分组成的，以及五自由度机械臂11和机械臂控制器10。

由所说的计算机1、五自由度机器人2、光学跟踪设备3、和被动标识器5构成一个闭环的机器人位姿测量与实时反馈控制系统，被动标识器5安装在五自由度机器人2的末端，始终被光学跟踪设备3跟踪；所说的五自由度机器人2包括五自由度机械臂11和机械臂控制器10；所说的辅助手术规划与导引软件9包括数字影像输入与预处理模块、病灶提取与三维重构模块、手术规划模块、以及手术实施模块。被动标识器5的数量至少为3个。

具体操作步骤是：

一.在进行手术前准备时：在病人头颅上粘贴四个医学标志器6，对脑部进行医学影像扫描，并将扫描所得医学影像信息输入计算机1，运用辅助手术规划与导引软件9确定病灶，重构病灶和头颅的三维医学模型，进行手术靶点与医学标志影像位置的标识，并规划手术路径；

二.在进行注册标定时：让病人躺在手术床8上，头部使用塑形枕7与手术床8相对固定，一方面用光学注册工具4测定病人头颅上的四个医学标志器6的坐标，测量值由光学跟踪设备3给出，并送入计算机1，由计算机1计算三维医学模型空间与光学测量空间之间的映射变换；另一方面用五自由度机器人基座坐标空间与光学测量空间映射变换的自动标测方法，由五自由度机器人2、光学跟踪设备3和计算机1配合，自动测定五自由度机器人基座坐标空间与光学测量空间之间的映射变换；

三.在进行手术实施时：首先，由计算机1将在三维医学模型空间预先规划的手术路径变换到光学测量空间，并计算出在当前路径点五自由度机器人末端需要达到的理想位姿；然后，由计算机1根据当前路径点的理想位姿，进行由光学测量空间到五自由度机器人基座坐标空间的坐标变换，并通过求解五自由度机器人逆运动学方程，得到五自由度机器人2各个关节的理想位置；然后，由计算机1向五自由度机器人2输入各个关节的理想位置，控制五自由度机器人2运动；同时，由光学跟踪设备3和安装在五自由度机器人末端的被动标识器5配合，实时测定五自由度机器人的末端位姿并送入计算机1，用五自由度机器人末端位姿的闭环控制方法对五自由度机器人的末端位姿进行实时控制，实现精确的轨迹跟踪和定位；最后，五自由度机器人2锁定，医生在五自由度机器人2的辅助下进行手术操作。

五自由度机械臂由手臂和手腕组成，具有五个关节，采取两个滑动关节和三个转动关节的PPRRR构型；手臂部分具有三个关节，分别为I、II和III关节，第I关节11-1采用滑动关节，运动方向垂直于水平面；第II关节11-2采用滑动关节，与第I关节成“T”形垂直；第III关节11-3采用转动关节，轴线与第I关节运动方向平行；手腕部分具有两个关节，分别为IV和V关节，第IV关节11-4采用转动关节，轴线与第III关节轴线平行；第V关节11-5采用转动关节，轴线与第IV关节轴线垂直；在机械臂的第V关节安装末端器械，安装轴线与第IV关节轴线平行。每个关节都有独立的驱动机构，由步进电机、减速器组成，滑动关节的驱动机构还包括有滚珠丝杆。

机械臂控制器10包括编程逻辑控制器PLC和步进电机驱动器，采用三个PLC和五个步进电机驱动器组合，对应控制五自由度机械臂11的五个关节，机械臂控制器10通过RS232C串口通信组件与计算机1进行通信，接受计算机1输入的关节位置命令，控制五自由度机械臂11运动到达指定位置。

闭环的机器人位姿测量与实时反馈控制系统的方法是：由光学跟踪设备3通过跟踪安装在五自由度机器人末端的被动标识器5来测定五自由度机器人2的末端位姿，由计算机1根据测量得到的位姿和期望五自由度机器人2达到的位姿计算控制量，控制五自由度机器人2进一步运动以修正偏差；光学跟踪设备3与计算机1之间通过RS232/RS422串口进行通信；五自由度机器人2与计算机1之间通过RS232C串口进行通信。

所述的脑外科机器人系统，被动标识器5在五自由度机器人2末端安装的几何位置关系满足：任意两个被动标识器5之间的距离不能小于50mm，任意两条线段不平行，而且任意两条线段之间的夹角不能低于0.5度。

基于光学跟踪闭环控制的脑外科机器人系统的实现方法，借助于光学跟踪技术进行位姿测量，包括：三维医学模型空间与光学测量空间映射变换的简便标测方法、五自由度机器人基座坐标空间与光学测量空间映射变换的自动标测方法、五自由度机器人末端位姿的闭环控制方法。

三维医学模型空间与光学测量空间映射变换的简便标测方法是：

a.由医生在病人头部粘贴四个医学标志器6，要求这四个标志器不在同一平面，且任意三个标志器不在同一条直线上；

b.对病人头部进行CT或MRI医学影像扫描，所得扫描影像输入计算机1；

c.确定四个医学标志器的扫描成像点在三维医学模型空间的坐标；

d.让病人躺在手术床8上，头部使用塑形枕7与手术床8相对固定，用光学注册工具4测定病人头部的四个医学标志器6在光学测量空间的坐标，测量值由光学跟踪设备3输入计算机1；

e.由计算机1计算三维医学模型空间与光学测量空间的映射变换。

五自由度机器人基座坐标空间与光学测量空间映射变换的自动标测方法是：

a.预先选定五自由度机器人2工作空间内的四个点，要求这四个点应该处于光学跟踪设备3的测量范围内，并且四个点不共面，任意三个点不共线；

b.由计算机1向五自由度机器人2发出到达上述四个点的关节位置命令，控制五自由度机器人2依次运动到上述四个点；

c.由计算机1记录下上述四个点在五自由度机器人基座坐标空间中的坐标；同时，由光学跟踪设备3和安装在五自由度机器人末端的被动标识器5配合，依次测定五自由度机器人2运动到上述四个点时其末端在光学测量空间中的坐标，并送入计算机1；

d.由计算机1根据上述四个点在五自由度机器人基座坐标空间和光学测量空间的坐标值，计算出五自由度机器人2基座坐标空间与光学测量空间的映射变换，从而完成自动标测。

五自由度机器人末端位姿的闭环控制方法是：

a.由计算机1将在三维医学模型空间预先规划的手术路径变换到光学测量空间，并计算出在当前路径点五自由度机器人末端需要达到的理想位姿；

b.由计算机1根据当前路径点的理想位姿，进行由光学测量空间到五自由度机器人基座坐标空间的坐标变换，并通过求解五自由度机器人逆运动学方程，得到五自由度机器人各个关节的理想位置；

c.由计算机1向五自由度机器人2输入各个关节的理想位置，控制五自由度机器人2运动；

d.由光学跟踪设备3和安装在五自由度机器人末端的被动标识器5配合，实时测定五自由度机器人末端的位姿，并送入计算机1；

e.由计算机1将实时测定的位姿信息与预先规划的当前路径点的理想位姿进行比较，得到两者的位姿偏差；

f.由计算机1根据位姿偏差按照预先确定的控制规律，位置控制采用比例-积分-微分调节；姿态控制采用比例调节，计算校正控制量，修正当前路径点的理想位姿，并开始新一轮的运动控制，从而实现精确的轨迹跟踪和定位。

Claims (8)

1、一种基于光学跟踪闭环控制的脑外科机器人系统，是进行接收医学影像信息、测定并确定病灶的位置、辅助进行手术规划并进行手术导引的脑外科机器人系统，包括计算机(1)、五自由度机器人(2)、光学跟踪设备(3)、光学注册工具(4)、被动标识器(5)、医学标志器(6)、塑形枕(7)和手术床(8)设备组成的硬件部分，和辅助手术规划与导引软件(9)部分；其特征是，由所说的计算机(1)、五自由度机器人(2)、光学跟踪设备(3)和被动标识器(5)构成一个闭环的机器人位姿测量与实时反馈控制系统，被动标识器(5)安装在五自由度机器人(2)的末端，始终被光学跟踪设备(3)跟踪；所说的五自由度机器人(2)包括五自由度机械臂(11)和机械臂控制器(10)；所说的辅助手术规划与导引软件(9)包括数字影像输入与预处理模块、病灶提取与三维重构模块、手术规划模块以及手术实施模块；

具体操作步骤是：

一.在进行手术前准备时：在病人头颅上粘贴四个医学标志器(6)，对脑部进行医学影像扫描，并将扫描所得医学影像信息输入计算机(1)，运用辅助手术规划与导引软件(9)确定病灶，重构病灶和头颅的三维医学模型，进行手术靶点与医学标志影像位置的标识，并规划手术路径；

二.在进行注册标定时：让病人躺在手术床(8)上，头部使用塑形枕(7)与手术床(8)相对固定，一方面用光学注册工具(4)测定病人头颅上的四个医学标志器(6)的坐标，测量值由光学跟踪设备(3)给出，并送入计算机(1)，由计算机(1)计算三维医学模型空间与光学测量空间之间的映射变换；另一方面用五自由度机器人基座坐标空间与光学测量空间映射变换的自动标测方法，由五自由度机器人(2)、光学跟踪设备(3)和计算机(1)配合，自动测定五自由度机器人基座坐标空间与光学测量空间之间的映射变换；

三.在进行手术实施时：首先，由计算机(1)将在三维医学模型空间预先规划的手术路径变换到光学测量空间，并计算出在当前路径点五自由度机器人末端需要达到的理想位姿；然后，由计算机(1)根据当前路径点的理想位姿，进行由光学测量空间到五自由度机器人基座坐标空间的坐标变换，并通过求解五自由度机器人逆运动学方程，得到五自由度机器人(2)各个关节的理想位置；然后，由计算机(1)向五自由度机器人(2)输入各个关节的理想位置，控制五自由度机器人(2)运动；同时，由光学跟踪设备(3)和安装在五自由度机器人末端的被动标识器(5)配合，实时测定五自由度机器人的末端位姿并送入计算机(1)，用五自由度机器人末端位姿的闭环控制方法对五自由度机器人的末端位姿进行实时控制，实现精确的轨迹跟踪和定位；最后，五自由度机器人(2)锁定，医生在五自由度机器人(2)的辅助下进行手术操作。

2.根据权利要求1所述的脑外科机器人系统，其特征是，五自由度机械臂(11)由手臂和手腕组成，具有五个关节，采取两个滑动关节和三个转动关节的PPRRR构型；手臂部分具有三个关节，分别为I、II和III关节，第I关节(11-1)采用滑动关节，运动方向垂直于水平面；第II关节(11-2)采用滑动关节，与第I关节成“T”形垂直；第III关节(11-3)采用转动关节，轴线与第I关节运动方向平行；手腕部分具有两个关节，分别为IV和V关节，第IV关节(11-4)采用转动关节，轴线与第III关节轴线平行；第V关节(11-5)采用转动关节，轴线与第IV关节轴线垂直；在机械臂的第V关节安装末端器械，安装轴线与第IV关节轴线平行；每个关节都有独立的驱动机构，由步进电机、减速器组成，滑动关节的驱动机构还包括有滚珠丝杆。

3.根据权利要求1所述的脑外科机器人系统，其特征是，机械臂控制器(10)包括编程逻辑控制器和步进电机驱动器，采用三个编程逻辑控制器和五个步进电机驱动器组合，对应控制五自由度机械臂(11)的五个关节，机械臂控制器(10)通过RS232C串口通信组件与计算机(1)进行通信，接受计算机(1)输入的关节位置命令，控制五自由度机械臂(11)运动到达指定位置。

4.根据权利要求1所述的脑外科机器人系统，其特征是，闭环的机器人位姿测量与实时反馈控制系统的方法是：由光学跟踪设备(3)通过跟踪安装在五自由度机器人末端的被动标识器(5)来测定五自由度机器人(2)的末端位姿，由计算机(1)根据测量得到的位姿和期望五自由度机器人(2)达到的位姿计算控制量，控制五自由度机器人(2)进一步运动以修正偏差；光学跟踪设备(3)与计算机(1)之间通过RS232/RS422串口进行通信；五自由度机器人(2)与计算机(1)之间通过RS232C串口进行通信。

5.根据权利要求1所述的脑外科机器人系统，其特征是，被动标识器(5)的数量至少为3个。

6.根据权利要求1所述的脑外科机器人系统，其特征是，被动标识器(5)在五自由度机器人(2)末端安装的几何位置关系满足：任意两个被动标识器(5)之间的距离不能小于50mm、任意两条由被动标识器(5)连线构成的线段不平行而且任意两条线段之间的夹角不能低于0.5度。

7.根据权利要求1所述的脑外科机器人系统，其特征是，五自由度机器人基座坐标空间与光学测量空间映射变换的自动标测方法是：

a.预先选定五自由度机器人(2)工作空间内的四个点，要求这四个点应该处于光学跟踪设备(3)的测量范围内，并且四个点不共面，任意三个点不共线；

b.由计算机(1)向五自由度机器人(2)发出到达上述四个点的关节位置命令，控制五自由度机器人(2)依次运动到上述四个点；

c.由计算机(1)记录下上述四个点在五自由度机器人基座坐标空间中的坐标；同时，由光学跟踪设备(3)和安装在五自由度机器人末端的被动标识器(5)配合，依次测定五自由度机器人(2)运动到上述四个点时其末端在光学测量空间中的坐标，并送入计算机(1)；

d.由计算机(1)根据上述四个点在五自由度机器人基座坐标空间和光学测量空间的坐标值，计算出五自由度机器人(2)基座坐标空间与光学测量空间的映射变换，从而完成自动标测。

8.根据权利要求1所述的脑外科机器人系统，其特征是，五自由度机器人末端位姿的闭环控制方法是：

a.由计算机(1)将在三维医学模型空间预先规划的手术路径变换到光学测量空间，并计算出在当前路径点五自由度机器人末端需要达到的理想位姿；

b.由计算机(1)根据当前路径点的理想位姿，进行由光学测量空间到五自由度机器人基座坐标空间的坐标变换，并通过求解五自由度机器人逆运动学方程，得到五自由度机器人各个关节的理想位置；

c.由计算机(1)向五自由度机器人(2)输入各个关节的理想位置，控制五自由度机器人(2)运动；

d.由光学跟踪设备(3)和安装在五自由度机器人末端的被动标识器(5)配合，实时测定五自由度机器人末端的位姿，并送入计算机(1)；

e.由计算机(1)将实时测定的位姿信息与预先规划的当前路径点的理想位姿进行比较，得到两者的位姿偏差；

f.由计算机(1)根据位姿偏差按照预先确定的控制规律，位置控制采用比例-积分-微分调节；姿态控制采用比例调节，计算校正控制量，修正当前路径点的理想位姿，并开始新一轮的运动控制，从而实现精确的轨迹跟踪和定位。

Images