CN113545851A - 重建器械术野中心的控制方法、系统、设备和存储介质 - Google Patents

Abstract

本申请涉及一种重建器械术野中心的控制方法、系统、设备和可读存储介质，获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，根据动平台的当前位姿获取重建器械术野中心后，静平台在机械坐标系下的目标位姿，根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换；根据动平台的当前位姿和静平台的目标位姿获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位，解决了相关技术中存在的并联机器人难以实现姿态回零的问题，提升了重建器械术野中心的控制准确度，保证并联平台的运动性能。

Description

重建器械术野中心的控制方法、系统、设备和存储介质

技术领域

本申请涉及机器人技术领域，特别是涉及重建器械术野中心的控制方法、系统、设备和存储介质。

背景技术

机器人在进行应用操作前需要进行摆位，使器械末端到达相应的操作空间，对并联机器人来说，摆位首先依赖于串联机械臂的运动，使器械末端到达操作空间，再通过操控并联平台使器械末端到达操作空间的中间位置，实现精确的摆位。

在将器械转移到另一较远的位置时，若直接通过主动臂将器械移至该目标点，此时并联平台可能已经处于工作空间的边缘位置，因此需要在器械位置不变的情况下，将并联平台调整至零位状态。

并联机器人的主动臂运动范围小，在完成摆位后，并联平台通常不处于零点位置，这可能导致并联平台在运动空间中无法覆盖应用的操作空间，从而使并联平台卡在极限位置无法完成操作。由于被动臂的运动自由度有限，仅通过被动臂的运动无法完全实现姿态回零。

目前针对相关技术中存在的并联机器人难以实现姿态回零的问题，尚未提出有效的解决方案。

发明内容

本申请实施例提供了一种重建器械术野中心的控制方法、系统、设备和可读存储介质，以至少解决相关技术中存在的并联机器人难以实现姿态回零的问题。

第一方面，本申请实施例提供了一种重建器械术野中心的控制方法，包括：

获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，其中，机械坐标系是动平台所在机械臂的全局坐标系；

根据动平台的当前位姿获取重建器械术野中心后，静平台在机械坐标系下的目标位姿，其中，动平台和静平台是机械臂中通过伸缩元件连接的并联平台，静平台的目标位姿的中心在动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与动平台平行；

根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换；

根据动平台的当前位姿和静平台的目标位姿获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位。

在其中一些实施例中，根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换包括以下步骤：

根据动平台的当前位姿和静平台的目标位姿获取经重建并回归零位状态时，静坐标系到动坐标系间的第一转换矩阵；静坐标系是静平台所在的坐标系，动坐标系是动平台所在的坐标系；

获取经重建但未回归零位状态时，静坐标系到动坐标系间的第二转换矩阵；以及重建前机械坐标系到静坐标系间的第三转换矩阵；

根据第一转换矩阵、第二转换矩阵和第三转换矩阵获取重建后机械坐标系到静坐标系间的第四转换矩阵；

根据第四转换矩阵获取机械臂中被动臂的关节坐标系间的第五转换矩阵，根据第五转换矩阵对被动臂的关节的姿态参数进行变换，其中，被动臂与静平台连接。

在其中一些实施例中，被动臂包括串联的第一关节、第二关节和第三关节，其中，第一关节和第三关节的姿态参数不变，第三关节与静平台连接；

根据第五转换矩阵对被动臂的关节的姿态参数进行变换包括以下步骤：

获取机械坐标系到第一关节的坐标系的第六转换矩阵，以及第二关节的坐标系到第三关节的坐标系间的第七转换矩阵；

根据第五转换矩阵、第六转换矩阵和第七转换矩阵获取第一关节到第二关节间的第八转换矩阵；

根据第八转换矩阵对第二关节的姿态参数进行变换。

在其中一些实施例中，第二关节包括依次连接的第一转动副、第一移动副、第二转动副、第二移动副和第三转动副，所述第一转动副与所述第一关节转动连接，所述第三转动副与所述第三关节转动连接；

所述根据第八转换矩阵对所述第二关节的驱动参数进行变换包括以下步骤：

获取所述第一关节到所述第三转动副的驱动参数变换关系，根据所述第八转换矩阵和所述驱动参数变换关系，求解得到所述第一转动副、所述第一移动副、所述第二转动副、所述第二移动副和所述第三转动副的驱动参数。

在其中一些实施例中，根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换包括以下步骤：

根据动平台的当前位姿和静平台的目标位姿获取伸缩元件的第一伸缩量，在对静平台进行位姿变换时，根据第一伸缩量对伸缩元件进行调节，以使动平台在机械坐标系下的当前位姿保持不变。

在其中一些实施例中，根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换包括以下步骤：

获取在重建前静平台的原始位姿，对静平台的原始位姿和目标位姿进行插补，获取静平台在位姿变换过程中的中间位姿；

根据中间位姿和动平台的当前位姿对静平台进行位置变换和姿态变换，其中，位置变换包括以远心不动点为圆心的圆弧运动变换和指向圆心的直线运动变换，姿态变换包括静坐标系绕机械坐标系Z轴的旋转变换和静坐标系绕自身Y轴的旋转变换。

在其中一些实施例中，根据中间位姿和动平台的当前位姿对静平台进行位置变换和姿态变换包括以下步骤：

将中间位姿转换为静坐标系至机械坐标系间的第九转换矩阵和静坐标系至动坐标系间的第十转换矩阵；静坐标系是静平台所在的坐标系，动坐标系是动平台所在的坐标系；

根据第九转换矩阵和第十转换矩阵对被动臂的关节的姿态参数进行变换，其中，被动臂与静平台连接；

根据第九转换矩阵和第十转换矩阵获取伸缩元件的第二伸缩量，根据第二伸缩量对伸缩元件进行调节，以使动平台在机械坐标系下的当前位姿保持不变。

在其中一些实施例中，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位包括以下步骤：

对扭转角度进行插补，获取动平台在旋转变换过程中的中间角度；

根据所述中间角度和所述动平台的当前角度对所述动平台进行旋转变换。

在其中一些实施例中，根据所述中间角度和所述动平台的当前角度对所述动平台进行旋转变换包括以下步骤：

根据中间角度获取静坐标系和动坐标系之间的第十一转换矩阵，根据第十一转换矩阵获取伸缩元件的第三伸缩量，根据第三伸缩量对伸缩元件进行调节，以使并联平台回归零位。

在其中一些实施例中，重建器械术野中心的控制方法还包括以下步骤：在所述并联平台回归零位后，根据重建术野后所述静坐标系与所述机械坐标系之间的转换关系，获取所述远心不动点在重建术野后所述静坐标系下的坐标，其中，所述远心不动点在所述机械坐标系下的坐标不变。

第二方面，本申请实施例提供了一种重建器械术野中心的控制系统，包括：

第一获取模块，用于获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，其中，机械坐标系是动平台所在机械臂的全局坐标系；

第二获取模块，用于根据动平台的当前位姿获取重建器械术野中心后，静平台在机械坐标系下的目标位姿，其中，动平台和静平台是机械臂中通过伸缩元件连接的并联平台，静平台的目标位姿的中心在动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与动平台平行；

第一变换模块，用于根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换；

第二变换模块，用于根据动平台的当前位姿和静平台的目标位姿获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位。

第三方面，本申请实施例提供了一种重建器械术野中心的控制设备，包括机械臂和控制装置；

控制装置用于根据上述第一方面的重建器械术野中心的控制方法对机械臂上的静平台进行位姿变换，对机械臂上的动平台进行旋转变换。

第四方面，本申请实施例提供了一种可读存储介质，其上存储有可执行程序，该可执行程序被处理器执行时实现如上述第一方面的重建器械术野中心的控制方法。

相比于相关技术，本申请实施例提供的重建器械术野中心的控制方法、系统、设备和可读存储介质，获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，根据动平台的当前位姿获取重建器械术野中心后，静平台在机械坐标系下的目标位姿，根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换；根据动平台的当前位姿和静平台的目标位姿获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位，解决了相关技术中存在的并联机器人难以实现姿态回零的问题，提升了重建器械术野中心的控制准确度，保证并联平台的运动性能。

本申请的一个或多个实施例的细节在以下附图和描述中提出，以使本申请的其他特征、目的和优点更加简明易懂。

附图说明

此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1是根据本申请实施例的重建器械术野中心的控制方法的流程示意图；

图2是根据本申请实施例的重建器械术野中心的控制方法的应用示意图；

图3是根据本申请实施例的机械臂的关节示意图；

图4是根据本申请实施例的并联平台中静平台的位姿变换示意图；

图5是根据本申请实施例的并联平台中静平台的姿态变换原理图；

图6是根据本申请实施例的并联平台的系统默认初始位置示意图；

图7是根据本申请实施例的重建器械术野中心的控制系统的结构框图；

图8是根据本申请实施例的重建器械术野中心的控制设备的结构框图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行描述和说明。应当理解，此处所描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。基于本申请提供的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

在本申请中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域普通技术人员显式地和隐式地理解的是，本申请所描述的实施例在不冲突的情况下，可以与其它实施例相结合。

除非另作定义，本申请所涉及的技术术语或者科学术语应当为本申请所属技术领域内具有一般技能的人士所理解的通常意义。本申请所涉及的“一”、“一个”、“一种”、“该”等类似词语并不表示数量限制，可表示单数或复数。本申请所涉及的术语“包括”、“包含”、“具有”以及它们任何变形，意图在于覆盖不排他的包含；例如包含了一系列步骤或模块(单元)的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可以还包括没有列出的步骤或单元，或可以还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。本申请所涉及的“连接”、“相连”、“耦接”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电气的连接，不管是直接的还是间接的。本申请所涉及的“多个”是指两个或两个以上。“和/或”描述关联对象的关联关系，表示可以存在三种关系，例如，“A和/或B”可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。本申请所涉及的术语“第一”、“第二”、“第三”等仅仅是区别类似的对象，不代表针对对象的特定排序。

本实施例提供了一种重建器械术野中心的控制方法。图1是根据本申请实施例的重建器械术野中心的控制方法的流程示意图，如图1所示，该流程包括如下步骤：

步骤S110：获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，其中，机械坐标系是动平台所在机械臂的全局坐标系；

在本步骤中，器械术野中心是指器械在手术视野中心的位置，重建器械术野中心是指并联机器人的机械臂末端的术野中心非目标区域，需要在不改变并联机器人的远心不动点的情况下重新确定，使术野中心在目标区域；在重建器械术野中心的过程中，一般是通过并联机器人的并联平台上的动平台的移动来实现术野中心的转移，但此时并联平台上的动平台和静平台已不处于零点位置；机械臂自身所在的全局坐标系是适用于机械臂各个部件的机械坐标系，动平台和静平台的位姿映射到机械坐标系上，有利于两者之间的位姿转换计算。

具体的，机械坐标系可以是图3中的整个机械臂所在的全局坐标系，该全局坐标系的原点可为机械臂的底座中心。

步骤S120：根据动平台的当前位姿获取重建器械术野中心后，静平台在机械坐标系下的目标位姿，其中，动平台和静平台是机械臂中通过伸缩元件连接的并联平台，静平台的目标位姿的中心在动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与动平台平行；

在本步骤中，并联平台是机械臂的重要组成部分，包括动平台、伸缩元件和静平台，由于静平台和动平台在零位时的相对位姿是确定的，静平台的目标位姿的中心在动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与动平台平行，利用重建器械术野中心后动平台的当前位姿可确定静平台在机械坐标系下的目标位姿。

步骤S130：根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换；

在本步骤中，利用动平台的当前位姿和静平台的目标位姿作为变换的目标，对静平台进行位姿变换，使静平台运动至目标位姿，在这一过程中，动平台在机械坐标系下的当前位姿保持不变。

步骤140：根据动平台的当前位姿和静平台的目标位姿获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位；

在本步骤中，在静平台进行位姿变换后，并联平台还无法完全回到零位，动平台和静平台之间还可能存在一个动平台中心轴方向上的扭转，由于机械臂上的被动臂的自由度限制，已无法通过调整静平台的姿态来使并联平台回到零位，故而选择控制动平台绕中心轴线转动来使并联平台回到零位，动平台的转动会导致器械绕中心轴线转动，但这并不会违背远心不动点原则，也不会改变器械的术野中心，可以有效地完成术野中心重建。

在本实施例中，通过上述步骤，获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，根据动平台的当前位姿获取重建器械术野中心后，静平台在机械坐标系下的目标位姿，根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换；根据动平台的当前位姿和静平台的目标位姿获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位，解决了相关技术中存在的并联机器人难以实现姿态回零的问题，提升了重建器械术野中心的控制准确度，保证并联平台的运动性能。

需要说明的是，本申请的重建器械术野中心的控制方法可应用于多种应用场景。如图2所示，在前置摆位操作中，前置助手通过移动被动臂各关节来确立远心不动点的同时，建立器械术野中心，使器械末端点的初始位置位于术野的中心。由于该过程器械并未被插入待检物内，而是由前置助手目测大致确定，因此可能与理想的器械术野中心存在较大偏差。当器械被插入待检物内，发现存在较大偏差时，可通过本申请的方案，在不改变远心不动点的条件下重建器械术野中心，以消除偏差，使器械获得理想的活动空间。

又如，受伸缩元件(如动静铰链)摆动角限制，并联平台工作空间相对较小，而某些场景可能要求更大的活动空间。当并联平台当前工作空间无法满足需求，或当前工作空间内已完成阶段性操作时，可通过本申请的方案重建器械术野中心，在不改变远心不动点的条件下移动并联平台工作空间，间接获得更大的活动空间。

在其中一些实施例中，根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换包括以下步骤：

根据动平台的当前位姿和静平台的目标位姿获取经重建并回归零位状态时，静坐标系到动坐标系间的第一转换矩阵；静坐标系是静平台所在的坐标系，动坐标系是动平台所在的坐标系；

获取经重建但未回归零位状态时，静坐标系到动坐标系间的第二转换矩阵；以及重建前机械坐标系到静坐标系间的第三转换矩阵；

根据第一转换矩阵、第二转换矩阵和第三转换矩阵获取重建后机械坐标系到静坐标系间的第四转换矩阵；

根据第四转换矩阵获取机械臂中被动臂的关节坐标系间的第五转换矩阵，根据第五转换矩阵对被动臂的关节的姿态参数进行变换，其中，被动臂与静平台连接。

在本实施例中，在重建器械术野中心前后，动坐标系、静坐标系和机械坐标系之间存在转换关系，可通过转换矩阵进行描述。此处采用四种转换矩阵：经重建并回归零位状态时，静坐标系到动坐标系间的第一转换矩阵；经重建但未回归零位状态时，静坐标系到动坐标系间的第二转换矩阵；重建前机械坐标系到静坐标系间的第三转换矩阵；重建后机械坐标系到静坐标系间的第四转换矩阵。第一转换矩阵可根据动平台的当前位姿和静平台的目标位姿得到，第二转换矩阵可根据动平台和静平台之间的转换关系得到，第三转换矩阵可根据机械臂的被动臂的关节信息转换得到，由于动平台的位姿在变换过程中保持不变，第四转换矩阵和第一转换矩阵的乘积与第三转换矩阵和第二转换矩阵的乘积相同，利用这一等价关系，可得到第四转换矩阵，其反映的是机械臂中被动臂至并联平台的转换关系，由此可获得机械臂中被动臂的关节坐标系间的第五转换矩阵，并以第五转换矩阵对被动臂的关节的姿态参数进行变换，通过被动臂的姿态变换实现并联平台中静平台的位姿变换。

需要说明的是，如图2所示，静坐标系是静平台所在的坐标系，重建术野前，以静平台的中心S为原点，Xs轴和Ys轴平行于静平台平面，Zs轴垂直于静平台平面；重建术野后，以静平台的中心S’为原点，X’s轴和Y’s轴平行于静平台平面，Z’s轴垂直于静平台平面；动坐标系是动平台所在的坐标系，重建术野前，以动平台的中心M为原点，X_M轴和Y_M轴平行于动平台平面，Z_M轴垂直于动平台平面；重建术野后，以动平台的中心M’为原点，X’_M轴和Y’_M轴平行于动平台平面，Z’_M轴垂直于动平台平面。

在其中一些实施例中，被动臂包括串联的第一关节、第二关节和第三关节，其中，第一关节和第三关节的姿态参数不变，第三关节与静平台连接；

根据第五转换矩阵对被动臂的关节的姿态参数进行变换包括以下步骤：

获取机械坐标系到第一关节的坐标系的第六转换矩阵，以及第二关节的坐标系到第三关节的坐标系间的第七转换矩阵；

根据第五转换矩阵、第六转换矩阵和第七转换矩阵获取第一关节到第二关节间的第八转换矩阵；

根据第八转换矩阵对第二关节的姿态参数进行变换。

在本实施例中，被动臂包括多个串联的关节，在对被动臂的关节的姿态参数进行变换时，其中部分关节的姿态参数不变，只改变其中另一部分关节的姿态参数，由于第一关节和第三关节的姿态参数不变，机械坐标系到第一关节的坐标系的第六转换矩阵，以及第二关节的坐标系到第三关节的坐标系间的第七转换矩阵都是确定的，结合变换目标的第五转换矩阵可得到针对第二关节的姿态参数的第八转换矩阵，基于第八转换矩阵对第二关节的姿态参数进行变换，如此可采用较少的运动关节数量，减少关节的姿态参数的计算量，并尽快实现对被动臂进行调节。

进一步的，第一关节、第二关节和第三关节均可包括一个或多个关节，而且由于第三关节与静平台连接，在完成操作退出并联平台时，保证第三关节充分的运动范围。

在其中一些实施例中，第二关节包括依次连接的第一转动副、第一移动副、第二转动副、第二移动副和第三转动副，所述第一转动副与所述第一关节转动连接，所述第三转动副与所述第三关节转动连接；

所述根据第八转换矩阵对所述第二关节的驱动参数进行变换包括以下步骤：

获取所述第一关节到所述第三转动副的驱动参数变换关系，根据所述第八转换矩阵和所述驱动参数变换关系，求解得到所述第一转动副、所述第一移动副、所述第二转动副、所述第二移动副和所述第三转动副的驱动参数。

在本实施例中，第二关节可包括多个关节，如依次连接的第一转动副、第一移动副、第二转动副、第二移动副和第三转动副，第一转动副与所述第一关节转动连接，所述第三转动副与所述第三关节转动连接，第二关节包括的多个关节可使被动臂有较大的运动空间，由于第二关节中的各个关节的连接关系，各个关节的驱动参数之间存在一定的变换关系，结合第八转换矩阵，可具体求解出各个关节的具体驱动参数，以此实现被动臂第二关节的调节。

进一步的，在第二关节中，第一转动副的驱动参数可为转动角θ5，第一移动副的驱动参数可为移动距离d6，第二转动副的驱动参数可为转动角θ7，第二移动副的驱动参数可为移动距离d8，第三转动副的驱动参数可为转动角θ9，具体如图3所示。

在其中一些实施例中，根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换包括以下步骤：

根据动平台的当前位姿和静平台的目标位姿获取伸缩元件的第一伸缩量，在对静平台进行位姿变换时，根据第一伸缩量对伸缩元件进行调节，以使动平台在机械坐标系下的当前位姿保持不变。

在本实施例中，除了通过被动臂来调整静平台的位姿以外，还需要对动平台和静平台之间的伸缩元件进行调节，以保证在对静平台进行位姿变换时，动平台在机械坐标系下的当前位姿保持不变，只让静平台的位姿进行变换，以实现并联平台的回归零位。

具体的，本申请的方案可应用于Stewart平台，其可关联10个串联的关节。

在具体工作过程中，可划分为“位姿变换阶段”和“动坐标系回归零位阶段”。

串联机械臂的控制通常基于DH参数或改进的DH参数进行坐标系的转换。相连接的两个关节为相邻关节。相邻两个关节的关节坐标系的变换通常采用DH参数或改进的DH参数来表示。以DH参数为例，相邻两个关节坐标系通过绕Z轴旋转θ、平移d后，再绕X轴旋转α、平移a，就能够相互重合，上述的θ、d、α和a即为DH参数。由此可知，如果DH参数越简洁，则相邻两个关节坐标系的转换越简单。

已知各关节的DH参数，根据机器人正运动学可知两相邻关节之间的转换矩阵为

从第m关节(包含m关节的自由度)到第n关节的转换矩阵可表示为：

在器械术野中心重建后，根据Stewart平台运动前后动平台在机械坐标系下的位姿不变，可建立如下等式关系：

T_0_4×T_4_9₁×T_9_10×T_s_end_m_end

＝T_machine_s_begin×T_s_begin_m_end (3)

其中，T_0_4表示机械坐标系到第4关节坐标系间转换矩阵；T_4_9₁表示重建并回归零位状态后的第4关节坐标系到第9关节坐标系间转换矩阵；T_9_10表示第9关节坐标系到第10关节坐标系间转换矩阵；T_s_end_m_end表示经重建并回归零位状态时Stewart平台静坐标系到动坐标系间转换矩阵；T_machine_s_begin表示重建前，机械坐标系到静坐标系间转换矩阵；T_s_begin_m_end表示重建后Stewart平台未回归零位状态时，静坐标系到动坐标系间转换矩阵。

由式(3)可得

其中，等式左侧为包含待求参数的转换矩阵，等式右侧均为已知量，T_4_9₀表示第5-9关节DH参数还未更新时的转换矩阵，T_4_9₁表示重建并回归零位状态后的第4关节坐标系到第9关节坐标系间转换矩阵。由于第1-4、10被动关节臂DH参数整个过程无变动，故据此可求得各矩阵表达式为：

T_machine_s_begin＝T_0_4×T_4_9₀×T_9_10 (6)

式(8)中m_oz表示零位状态时，动静坐标系在静坐标系Z轴方向上间距。

令φ_x、φ_y、φ_z分别为绕x、y、z轴的欧拉角，m_x、m_y、m_z分别为任意时刻动坐标系在静坐标系下的坐标位置，则任意时刻静坐标系到动坐标系的转换矩阵为：

如图3所示，第4关节坐标系到第9关节坐标系的转换矩阵可求得表达式为：

由等式(4)两侧矩阵对应元素相等，即可求得待更新的第5-9关节DH参数。在建立等式关系时，由于T_4_9存在0元素，故首先必须满足：

T_4_9₁[3,3]＝0 (11)

经推导：

T_4_9₁[3,3]＝sinθ₉(sinφ_xcosφ_z+cosφ_x sinφ_ysinφ_z)-cosθ₉cosφ_ysinφ_z

则有

sinθ₉(sinφ_xcosφ_z+cosφ_xsinφ_ysinφ_z)-cosθ₉cosφ_ysinφ_z＝0 (12)

其中，θ₉为串联臂第9关节臂DH参数，φ_x为动坐标系绕自身X轴的欧拉角，φ_z为动坐标系绕自身Z轴的欧拉角。由等式(12)可得：

当sinθ₉cosφ_xsinφ_y-cosθ₉cosφ_y＝0时，φ_z＝90°；

当sinθ₉cosφ_xsinφ_y-cosθ₉cosφ_y≠0时，结合式子(2)、(3)和θ₉可得：

在整个重建过程中，动静坐标系间Z轴方向的转动运动对应的转换矩阵记为T_rota_z：

则，重建术野过程中，静平台调整至与动平台平行而动平台还未运动时动静坐标系之间的转换矩阵T_s_m_input为

T_s_m_input由根据主手传递的位置信息计算得到，为已知量。根据式(15)可计算出动坐标系绕自身X、Y轴的欧拉角：

φ_x＝arcsin(T_s_m_input(3,2))

φ_y＝arcsin(T_s_m_input(1,3))

至此可将术野重建过程分为“位姿变换阶段”和“动坐标系回归零位阶段”。其中“位姿变换阶段”为保持动平台在机械坐标系下的位姿不动，将静平台运动至与动平台平行且Z轴共线位置的过程；“动坐标系回归零位阶段”为动平台绕自身Z轴旋转φ_z使Stewart平台回到零位的过程。

重建前后，静坐标系转换矩阵、动静坐标系转换矩阵有如下关系：

T_machine_s_begin×T_s_begin_m_end＝T_machine_s_end×T_s_end_m_end (16)

故，重建后表示静坐标系位姿的转换矩阵为：

T_machine_s_end＝T_machine_s_begin×T_s_begin_m_end×T_s_end_m_end^-1 (17)

其中T_machine_s_end为重建后机械坐标系到静坐标系转换矩阵，T_s_end_m_end为动静坐标系零位转换矩阵，且

至此，可明确“位姿变换阶段”的运动目标：

①.串联关节臂运动目标：T_machine_s_begin→T_machine_s_end；

②.Stewart平台运动目标：T_s_m_input→T_s_end_m_end×T_rota_z

(φ_z此时取为-φ_z。由于动平台绕自身Z轴转动，总角度为φ_z，则相对于零位状态，转动过程中，动静坐标系间欧拉角描述应为-φ_z)。

在其中一些实施例中，根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换包括以下步骤：

获取在重建前静平台的原始位姿，对静平台的原始位姿和目标位姿进行插补，获取静平台在位姿变换过程中的中间位姿；

根据中间位姿和动平台的当前位姿对静平台进行位置变换和姿态变换，其中，位置变换包括以远心不动点为圆心的圆弧运动变换和指向圆心的直线运动变换，姿态变换包括静坐标系绕机械坐标系Z轴的旋转变换和静坐标系绕自身Y轴的旋转变换。

在本实施例中，对静平台的原始位姿和目标位姿进行插补，得到静平台在位姿变换过程中的中间位姿，利用中间位姿可规划静平台回归零位时的路径，在该路径上对静平台进行位置变换和姿态变换，使静平台在运动过程中保持平滑顺畅，并保证静平台的运动精度。

进一步的，位置变换中，以远心不动点为圆心的圆弧运动变换和指向圆心的直线运动变换可同时进行；姿态变换中，静坐标系绕机械坐标系Z轴的旋转变换和静坐标系绕自身Y轴的旋转变换也可同时进行。

具体的，对“位姿变换阶段”静平台的运动进行插补及运动学解算。根据确认的运动目标，对静坐标系的运动过程进行位姿插补，如图4所示。

(1)位置插补

位置插补目标为将静坐标系从重建前所在空间位置，沿一定的轨迹由图4所示S₀运动到重建后所在目标位置S₁。首先通过运动规划，寻找该运动轨迹，然后建立以远心点为原点的坐标系，将运动轨迹曲线各点映射到该坐标系下，最后通过新建立的坐标系转换矩阵，求得运动轨迹点在机械坐标系下的坐标。

如图3所示，位置插补过程分为两个运动。运动1：以远心点为圆心，r0为半径的圆弧运动；运动2：沿半径r指向圆心方向的直线运动，即半径r0不断变化的运动。二者合成之后所形成的运动轨迹如图4静坐标系运动轨迹，为一半径从r0变化为r1的曲线运动。

其中，象征圆弧运动的变量θr以Δθ为增量，从0变化到θ，象征直线运动的变量r以Δr为变化量，从r0变化到r1以及从0变化到θ的过程中，设插值点数目为N，则第i个插值点与初始位置夹角θr以及半径r：

以远心点F为原点OF，

向量为XF轴正向，

结果为ZF轴正向，YF轴正向满足右手定则，建立如图3中所示的远心点坐标系O_F-X_FY_FZ_F，则在远心点坐标系下，第i个插值点坐标值为：

x＝r×cosθ_r，y＝r×sinθ_r，z＝0 (21)

记为location_pre(i)＝[r×cosθ_r；r×sinθ_r；0；1]

远心点坐标系O_F-X_FY_FZ_F在机械坐标系下的位姿矩阵T_machine_dm记为：

其中n_x，n_y，n_z为Xdm轴相对于全局x，y，z轴夹角的余弦值；o_x，o_y，o_z为Ydm轴相对于全局x，y，z轴夹角的余弦值；a_x，a_y，a_z为Zdm轴相对于全局x，y，z轴夹角的余弦值；X_F，Y_F，Z_F为远心点在机械坐标系下的坐标。

根据前文坐标系的建立，可求得各参数如下，其中fix为重建前远心点在静坐标系下坐标，为已知量：

[x_F；y_F；z_F；1]＝T_machine_s_begin×fix (23)

将式子(23)-(26)代入式(22)，即可得到全局坐标系下远心点坐标系的位姿矩阵T_machine_dm，则第i个插值点在全局坐标系下坐标值location(i)为：

location(i)＝T_machine_dm×location_pre(i) (27)

至此，位置插补工作结束，运动轨迹中每一个插值点的全局坐标均存储在数组location中。下面将进行姿态插补。

(2)姿态插补

姿态插补目标为，将静坐标系重建前姿态按照一定的法则，变换为重建后的目标姿态，如图5所示。由于机械结构限制，在姿态变换过程中，静坐标系Y0轴需时刻与全局坐标系XY平面平行，故如图4所示，姿态变换分为2个部分：①静坐标系绕全局Z轴旋转；②静坐标系绕自身Y0轴旋转。故首先应通过重建前后静坐标系姿态关系，求得2个旋转角度，再将2个角度分别线性插值，最后一系列由各插值点组成的连续变换数组。

由于2个变换过程均满足平行条件，可同时进行。因此，姿态变换过程可描述为下式：

R_rotate_posture＝R_z×R_machine_s_begin×R_y (28)

其中，R_rotate_posture表示第i个插值点在全局坐标系下旋转矩阵；R_machine_s_begin为重建前静坐标系旋转转换矩阵，其由转换矩阵T_machine_s_begin第1-3行、第1-3列9个元素构成；R_z表示绕全局Z轴变换过程；R_y表示绕静坐标系Y₀轴变换过程。

其中θ_z、θ_y分别表示第i个插值点与重建前静坐标系间绕全局Z轴和自身Y0轴旋转角度，由式(28)可得，重建前后姿态旋转矩阵应满足：

R＝R_machine_s_end×R_y^-1＝R_z×R_machine_s_begin(31)

其中R_machine_s_end为重建后静坐标系旋转矩阵，由转换矩阵T_machine_s_end第1-3行、第1-3列9个元素构成；

取等式(31)两侧矩阵元素R(1,2)和R(2,2)可得

根据上述方程组，两式相除，即可解得由重建前状态变换至重建后状态所需转角θ_z，记为α_z。

令

同理，取式子(31)矩阵元素R(3,1)和R(3,3)可得

根据上述方程组，两式相除，即可解得由重建前状态变换至重建后状态所需转角θ_y，记为α_y。

令

在整个姿态变换过程中，θ_z、θ_y分别以Δθ_z、Δθ_y为增量，从0变化至α_z、α_y，插值点数目与位置插补相同，为N，则第i个插值点转角参数θ_z、θ_y可表示为：

将第i个插值点参数记录在数组posture中，可表示为：

至此，姿态插补工作结束，所有插值点姿态参数记录在数组posture。

在其中一些实施例中，根据中间位姿和动平台的当前位姿对静平台进行位置变换和姿态变换包括以下步骤：

将中间位姿转换为静坐标系至机械坐标系间的第九转换矩阵和静坐标系至动坐标系间的第十转换矩阵；静坐标系是静平台所在的坐标系，动坐标系是动平台所在的坐标系；

根据第九转换矩阵和第十转换矩阵对被动臂的关节的姿态参数进行变换，其中，被动臂与静平台连接；

根据第九转换矩阵和第十转换矩阵获取伸缩元件的第二伸缩量，根据第二伸缩量对伸缩元件进行调节，以使动平台在机械坐标系下的当前位姿保持不变。

在本实施例中，根据中间位姿和动平台的当前位姿对静平台进行位置变换和姿态变换，需要通过被动臂的关节的姿态参数变换和伸缩元件的伸缩调节来实现。将中间位姿转换为静坐标系至机械坐标系间的第九转换矩阵和静坐标系至动坐标系间的第十转换矩阵，利用第九转换矩阵和第十转换矩阵来获取被动臂的关节的姿态参数以及伸缩元件的第二伸缩量，进而进行中间位姿变换，使静平台在运动过程中保持平滑顺畅，并保证静平台的运动精度。

进一步的，在获得被动臂的关节的姿态参数和伸缩元件的第二伸缩量后，可进而解析被动臂的关节的驱动参数，以及伸缩元件的驱动参数，来实现上述被动臂的关节的姿态参数和伸缩元件的第二伸缩量。

具体的，被动臂的关节的姿态参数和伸缩元件的第二伸缩量的解析可称为位姿插补运动学解算。运动学解算部分通过(1)位置插补和(2)姿态插补获得的参数转换为描述静坐标系位姿的转换矩阵和动静坐标系间转换矩阵，再分别解算至串联关节臂和Stewart平台的运动驱动上，最后获得所需各驱动参数。

由(1)位置插补和(2)姿态插补可知，第i个插值点位置描述由数组location获得，姿态描述由数组posture获得，通过式(28)可将第i个插值点转动角度转换为旋转矩阵R_rotate_posture(i)，则机械坐标系到该点静坐标系转换矩阵可表示为：

此时，动静坐标系间转换矩阵可表示为：

T_s_m_i＝T_machine_s_i^-1×T_machine_m_mid (38)

其中，T_machine_m_mid＝T_machine_s_begin×T_s_m_input。

①串联关节臂驱动参数求解：

将T_s_m_input对式(4)中的T_s_begin_m_end进行替换，相应地，将T_s_m_i对式(4)中的T_s_end_m_end进行替换，依然能使等式成立，得到

其中T_4_9_i表示第i个插值点第4关节坐标系到第9关节坐标系间转换矩阵。

由式(38)可得

根据等式(40)两侧矩阵对应项相等，综合考虑matlab反三角函数求解取值范围，可得到串联关节臂中各待求参数表达式：

θ_7i＝arcsin[T_4_9_i(2,3)×cosθ₅-T_4_9_i(1,3)×sinθ₅] (41)

d_8i＝-T_4_9_i(3,4) (42)

θ_9i＝arcsin T_4_9_i(3,2) (43)

a)若T_4_9_i(2,4)＝0，则：

θ_5i＝90° (44)

d_6i＝a₄-T_4_9_i(1,4) (45)

b)若T_4_9_i(2,4)≠0，则：

θ_5i＝arccos(T_4_9_i(2,4)/d₆) (46)

并将计算得到的N组DH参数存储在数组dh_update_po中。

②Stewart驱动解算过程：

根据Stewart坐标系建立，计算动铰接点在动坐标系、静铰接点在静坐标系的相对坐标。以静铰接点坐标计算为例，根据前文叙述，先将静铰接点S_i(i＝1～6)由静坐标系原点O_S移动到静坐标系X_S轴和静坐标系平台分布圆周交点处(如图6(c)所示)，此时坐标值为(r_s，0)。

根据平面坐标旋转定理，任意坐标点为(x，y)绕原点旋转θ角度后的坐标表示为

x′＝xcosθ-ysinθ，y′＝xsinθ+ycosθ (48)

将坐标(r_s，0)作为x，y的值带入公式(48)，则其坐标可表示为

(ss_ix，ss_iy)＝(r_scosθ_si，r_ssinθ_si) (49)

其中θ_si为以静坐标系原点为圆心从交点旋转到相应铰接点的旋转角度。

静平台坐标系原点定义在静铰接点所在平面中心处，因此任意静铰接点在静坐标系下的ZS轴向坐标为0，同时考虑到坐标旋转和平移变换，可得到静铰接点在静平台的齐次坐标：

S_i＝(ss_ix，ss_iy，0，1)

如图6(c)所示，相应铰接点变换过程中的旋转角度由表1给出。

表1静铰接点相应变换旋转角度

同理，获得动铰接点在动坐标系下的齐次坐标：

M_i＝(mm_ix，mm_iy，0，1)

如图6(c)所示，相应铰接点变换过程中的旋转角度由表2给出。

表2动铰接点相应变换旋转角度

由式(38)可得，此时动铰接点在静坐标系下坐标为：

由三维空间两点间的距离公式，计算任意一对动、静铰接点之间的距离：

将此数值与初始杆长相减，即可得到各杆驱动参数。

将计算得到的N组各杆驱动参数存储在数组L_mark_po中。

串联臂和Stewart平台分别依次读取数组dh_update_po和数组L_mark_po中的参数，即可在动平台位姿相对机械坐标系静止的情况下使静平台以确定的轨迹运动至与动平台平行的位置。至此“位姿变换阶段”完成。

在其中一些实施例中，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位包括以下步骤：

对扭转角度进行插补，获取动平台在旋转变换过程中的中间角度；

根据所述中间角度和所述动平台的当前角度对所述动平台进行旋转变换。

在其中一些实施例中，根据所述中间角度和所述动平台的当前角度对所述动平台进行旋转变换包括以下步骤：

根据中间角度获取静坐标系和动坐标系之间的第十一转换矩阵，根据第十一转换矩阵获取伸缩元件的第三伸缩量，根据第三伸缩量对伸缩元件进行调节，以使并联平台回归零位。

在本实施例中，在对动平台进行旋转变换时，也可以对扭转角度进行插补，得到动平台在旋转变换过程中的中间角度，利用中间角度可转换得到静坐标系和动坐标系之间的第十一转换矩阵，并进而得到伸缩元件的第三伸缩量，对伸缩元件进行调节后，使动平台回归零位，实现并联平台回归零位。

进一步的，可将第三伸缩量转换为伸缩元件的驱动参数，并对伸缩元件进行驱动调节。

在其中一些实施例中，重建器械术野中心的控制方法还包括以下步骤：

在所述并联平台回归零位后，根据重建术野后所述静坐标系与所述机械坐标系之间的转换关系，获取所述远心不动点在重建术野后所述静坐标系下的坐标，其中，所述远心不动点在所述机械坐标系下的坐标不变。

在本实施例中，可重新确认远心不动点在重建术野后所述静坐标系下的坐标，以便于手术位置的确认。

具体的，经过“位姿变换阶段”，静坐标系位姿已满足重建后的位姿要求，Stewart平台动坐标系相对于静坐标系间还存在一绕动坐标系Z轴的转动，故该阶段串联臂保持不动，Stewart平台对前文描述的绕自身Z轴转动角度进行插值并解算Stewart相应驱动参数。

目标为：T_s_m_rota→T_s_end_m_end。对“动坐标系回归零位阶段”动平台的运动进行插补及运动学解算如下。

该阶段运动为：动平台绕自身Z轴转动，总角度为φ_z，则相对于零位状态，转动过程中，动静坐标系间欧拉角描述应为-φ_z。

设插值点数目为M，则第i个插补点的动静坐标系间欧拉角描述eula_z应为：

第i个插补点的动静坐标系间转换矩阵T_s_m_rota_z_i为：

根据上述②Stewart驱动解算过程的方法，用T_s_m_rota_z_i替换其中的T_s_m_i，再用同样的方法求得M组各驱动杆参数，并存入数组L_mark_rota_z，与L_mark_po共同组成整个重建过程stewart平台的驱动L_mark_re_update：

L_mark_re_update＝[L_mark_po；L_mark_rota_z] (54)

重建过程串联关节臂的DH参数即为位姿插补时的参数：

dh_re_update＝dh_update_po (55)

Stewart平台完成“位姿变换阶段”后，依次读取数组L_mark_rota_z中的数组参数，即可使动坐标系绕自身Z轴旋转φ_z，使Stewart回到零位。至此“动坐标系回归零位阶段”完成，即整个术野重建过程完成。

另外，为使手术正常进行，还需要重新获得静坐标系下远心不动点的坐标并输入给系统。术野重建过程中，静平台位姿改变，但远心不动点在机械坐标系下坐标不变，根据重建术野后得到的静坐标与机械坐标的转换关系，可得到远心不动点在重建术野后静坐标系下的坐标，记为fix_new:

fix_new＝T_machine_s_end^-1×T_machine_s_begin×fix (56)

在一个具体实施例中，重建器械术野中心的控制步骤如下：

1.位姿变换阶段：

①通过被动臂的关节信息，解算重建前静坐标系在机械坐标系下的位姿转换矩阵T_machine_s_begin。

②根据动平台在机械坐标系下的位姿信息，解算重建后静坐标系在机械坐标系下的位姿转换矩阵T_machine_s_end。由于被动臂只有5个自由度，仅通过被动臂的运动无法完全实现Stewart姿态回零，因此这里解算得到的静坐标系与动坐标系之间还可能存在一个Z轴方向的扭转。

③根据术野重建前后的静坐标系位姿矩阵，对中间轨迹和姿态进行位置插补和姿态插补，得到一系列中间过程中静坐标系在机械坐标系下的位姿矩阵T_machine_s，并通过被动臂逆运动学解算每个插补点之间被动臂5个关节的运动信息dh_update_po，使静坐标系根据插补点轨迹运动至重建后的预设位姿。

④在步骤③进行的同一时间内，即在插补过程中，根据动平台在机械坐标系下的位姿信息，以及静平台插补位姿在机械坐标系下的姿态矩阵，得到插补点动静坐标系间转换矩阵T_s_m，根据Stewart平台逆运动学解算Stewart平台六个驱动杆的长度L_mark_po，使静平台运动的过程中动平台在机械坐标系下的位姿保持静止。

2.动坐标系回归零位阶段：

根据位姿变换阶段第②点中所解释的，位姿变换阶段无法使Stewart平台完全回到零位，动静平台之间还可能存在一个Z轴方向的扭转，由于被动臂的自由度限制已无法通过调整静平台的姿态来使Stewart平台回到零位，故而选择控制动平台绕Z轴转动来使Stewart平台回到零位。动平台的转动会导致器械绕Z轴转动，但这并不会违背远心不动点原则，也不会改变器械的术野中心，可以有效地完成术野中心重建。

根据位姿变换阶段完成后动静坐标系在机械坐标系下的位姿矩阵，计算动静之间的扭转角度，对该角度进行多点插值，再根据Stewart平台逆运动学解算每个差值点Stewart平台六个驱动杆的长度L_mark_po，使动平台以相应的速度绕Z轴旋转，直至Stewart平台回到零点位置。

本实施例还提供了一种重建器械术野中心的控制系统，该系统用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”等可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的系统较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。

图7是根据本申请实施例的重建器械术野中心的控制系统的结构框图，如图7所示，该系统包括：

第一获取模块210，用于获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，其中，机械坐标系是动平台所在机械臂的全局坐标系；

第二获取模块220，用于根据动平台的当前位姿获取重建器械术野中心后，静平台在机械坐标系下的目标位姿，其中，动平台和静平台是机械臂中通过伸缩元件连接的并联平台，静平台的目标位姿的中心在动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与动平台平行；

第一变换模块230，用于根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换；

第二变换模块240，用于根据动平台的当前位姿和静平台的目标位姿获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位。

在其中一些实施例中，第一变换模块230还用于根据动平台的当前位姿和静平台的目标位姿获取经重建并回归零位状态时，静坐标系到动坐标系间的第一转换矩阵；静坐标系是静平台所在的坐标系，动坐标系是动平台所在的坐标系；获取经重建但未回归零位状态时，静坐标系到动坐标系间的第二转换矩阵；以及重建前机械坐标系到静坐标系间的第三转换矩阵；根据第一转换矩阵、第二转换矩阵和第三转换矩阵获取重建后机械坐标系到静坐标系间的第四转换矩阵；根据第四转换矩阵获取机械臂中被动臂的关节坐标系间的第五转换矩阵，根据第五转换矩阵对被动臂的关节的姿态参数进行变换，其中，被动臂与静平台连接。

在其中一些实施例中，被动臂包括串联的第一关节、第二关节和第三关节，其中，第一关节和第三关节的姿态参数不变，第三关节与静平台连接；

第一变换模块230还用于获取机械坐标系到第一关节的坐标系的第六转换矩阵，以及第二关节的坐标系到第三关节的坐标系间的第七转换矩阵；根据第五转换矩阵、第六转换矩阵和第七转换矩阵获取第一关节到第二关节间的第八转换矩阵；根据第八转换矩阵对第二关节的姿态参数进行变换。

在其中一些实施例中，第一变换模块230还用于根据动平台的当前位姿和静平台的目标位姿获取伸缩元件的第一伸缩量，在对静平台进行位姿变换时，根据第一伸缩量对伸缩元件进行调节，以使动平台在机械坐标系下的当前位姿保持不变。

在其中一些实施例中，第一变换模块230还用于获取在重建前静平台的原始位姿，对静平台的原始位姿和目标位姿进行插补，获取静平台在位姿变换过程中的中间位姿；根据中间位姿和动平台的当前位姿对静平台进行位置变换和姿态变换，其中，位置变换包括以远心不动点为圆心的圆弧运动变换和指向圆心的直线运动变换，姿态变换包括静坐标系绕机械坐标系Z轴的旋转变换和静坐标系绕自身Y轴的旋转变换。

在其中一些实施例中，第一变换模块230还用于将中间位姿转换为静坐标系至机械坐标系间的第九转换矩阵和静坐标系至动坐标系间的第十转换矩阵；静坐标系是静平台所在的坐标系，动坐标系是动平台所在的坐标系；根据第九转换矩阵和第十转换矩阵对被动臂的关节的姿态参数进行变换，其中，被动臂与静平台连接；根据第九转换矩阵和第十转换矩阵获取伸缩元件的第二伸缩量，根据第二伸缩量对伸缩元件进行调节，以使动平台在机械坐标系下的当前位姿保持不变。

在其中一些实施例中，第二变换模块240还用于对扭转角度进行插补，获取动平台在旋转变换过程中的中间角度；根据中间角度获取静坐标系和动坐标系之间的第十一转换矩阵，根据第十一转换矩阵获取伸缩元件的第三伸缩量，根据第三伸缩量对伸缩元件进行调节，以使并联平台回归零位。

如图8所示，本申请实施例还提供了一种重建器械术野中心的控制设备，包括机械臂310和控制装置320；

控制装置320用于根据上述第一方面的重建器械术野中心的控制方法对机械臂310上的静平台进行位姿变换，对机械臂310上的动平台进行旋转变换。

上述重建器械术野中心的控制设备，通过控制装置320对机械臂310上的静平台进行位姿变换，对机械臂310上的动平台进行旋转变换，实现了并联平台回归零位，解决了相关技术中存在的并联机器人难以实现姿态回零的问题，提升了重建器械术野中心的控制准确度，保证并联平台的运动性能。

本申请实施例还提供了一种可读存储介质，其上存储有可执行程序，该可执行程序被处理器执行时实现如上述重建器械术野中心的控制方法。

上述可读存储介质，通过在处理器上运行可执行程序，可以实现并联平台回归零位，解决了相关技术中存在的并联机器人难以实现姿态回零的问题，提升了重建器械术野中心的控制准确度，保证并联平台的运动性能。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成。所述的程序可以存储于可读取存储介质中。该程序在执行时，包括上述方法所述的步骤。所述的存储介质，包括：ROM/RAM、磁碟、光盘等。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (13)

1.一种重建器械术野中心的控制方法，其特征在于，所述方法包括以下步骤：

获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，其中，所述机械坐标系是所述动平台所在机械臂的全局坐标系；

根据所述动平台的当前位姿获取重建器械术野中心后，静平台在所述机械坐标系下的目标位姿，其中，所述动平台和所述静平台是所述机械臂中通过伸缩元件连接的并联平台，所述静平台的目标位姿的中心在所述动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与所述动平台平行；

根据所述动平台的当前位姿和所述静平台的目标位姿对所述静平台进行位姿变换；

根据所述动平台的当前位姿和所述静平台的目标位姿获取所述动平台和所述静平台之间在所述中心轴线上的扭转角度，根据所述扭转角度对所述动平台进行旋转变换，以使所述并联平台回归零位。

2.根据权利要求1所述的重建器械术野中心的控制方法，其特征在于，所述根据所述动平台的当前位姿和所述静平台的目标位姿对所述静平台进行位姿变换包括以下步骤：

根据所述动平台的当前位姿和静平台的目标位姿获取经重建并回归零位状态时，静坐标系到动坐标系间的第一转换矩阵；所述静坐标系是所述静平台所在的坐标系，所述动坐标系是所述动平台所在的坐标系；

获取经重建但未回归零位状态时，所述静坐标系到所述动坐标系间的第二转换矩阵；以及重建前所述机械坐标系到所述静坐标系间的第三转换矩阵；

根据所述第一转换矩阵、所述第二转换矩阵和所述第三转换矩阵获取重建后所述机械坐标系到所述静坐标系间的第四转换矩阵；

根据所述第四转换矩阵获取所述机械臂中被动臂的关节坐标系间的第五转换矩阵，根据所述第五转换矩阵对所述被动臂的关节的驱动参数进行变换，其中，所述被动臂与所述静平台连接。

3.根据权利要求2所述的重建器械术野中心的控制方法，其特征在于，所述被动臂包括串联的第一关节、第二关节和第三关节，其中，所述第一关节和第三关节的驱动参数不变，所述第三关节与所述静平台连接；

所述根据所述第五转换矩阵对所述被动臂的关节的驱动参数进行变换包括以下步骤：

获取所述机械坐标系到所述第一关节的坐标系的第六转换矩阵，以及所述第二关节的坐标系到所述第三关节的坐标系间的第七转换矩阵；

根据所述第五转换矩阵、所述第六转换矩阵和所述第七转换矩阵获取所述第一关节到所述第二关节间的第八转换矩阵；

根据所述第八转换矩阵对所述第二关节的驱动参数进行变换。

4.根据权利要求3所述的重建器械术野中心的控制方法，其特征在于，所述第二关节包括依次连接的第一转动副、第一移动副、第二转动副、第二移动副和第三转动副，所述第一转动副与所述第一关节转动连接，所述第三转动副与所述第三关节转动连接；

所述根据第八转换矩阵对所述第二关节的驱动参数进行变换包括以下步骤：

获取所述第一关节到所述第三转动副的驱动参数变换关系，根据所述第八转换矩阵和所述驱动参数变换关系，求解得到所述第一转动副、所述第一移动副、所述第二转动副、所述第二移动副和所述第三转动副的驱动参数。

5.根据权利要求1所述的重建器械术野中心的控制方法，其特征在于，所述根据所述动平台的当前位姿和所述静平台的目标位姿对所述静平台进行位姿变换包括以下步骤：

根据所述动平台的当前位姿和所述静平台的目标位姿获取所述伸缩元件的第一伸缩量，在对所述静平台进行位姿变换时，根据所述第一伸缩量对所述伸缩元件进行调节，以使所述动平台在所述机械坐标系下的当前位姿保持不变。

6.根据权利要求1至5中任意一项所述的重建器械术野中心的控制方法，其特征在于，所述根据所述动平台的当前位姿和所述静平台的目标位姿对所述静平台进行位姿变换包括以下步骤：

获取在重建前所述静平台的原始位姿，对所述静平台的原始位姿和目标位姿进行插补，获取所述静平台在位姿变换过程中的中间位姿；

根据所述中间位姿和所述动平台的当前位姿对所述静平台进行位置变换和姿态变换，其中，所述位置变换包括以远心不动点为圆心的圆弧运动变换和指向所述圆心的直线运动变换，所述姿态变换包括静坐标系绕所述机械坐标系Z轴的旋转变换和所述静坐标系绕自身Y轴的旋转变换。

7.根据权利要求6所述的重建器械术野中心的控制方法，其特征在于，所述根据所述中间位姿和所述动平台的当前位姿对所述静平台进行位置变换和姿态变换包括以下步骤：

将所述中间位姿转换为静坐标系至所述机械坐标系间的第九转换矩阵和所述静坐标系至动坐标系间的第十转换矩阵；所述静坐标系是所述静平台所在的坐标系，所述动坐标系是所述动平台所在的坐标系；

根据所述第九转换矩阵和所述第十转换矩阵对被动臂的关节的姿态参数进行变换，其中，所述被动臂与所述静平台连接；

根据所述第九转换矩阵和所述第十转换矩阵获取所述伸缩元件的第二伸缩量，根据所述第二伸缩量对所述伸缩元件进行调节，以使所述动平台在所述机械坐标系下的当前位姿保持不变。

8.根据权利要求7所述的重建器械术野中心的控制方法，其特征在于，所述根据所述扭转角度对所述动平台进行旋转变换，以使所述并联平台回归零位包括以下步骤：

对所述扭转角度进行插补，获取所述动平台在旋转变换过程中的中间角度；

根据所述中间角度和所述动平台的当前角度对所述动平台进行旋转变换。

9.根据权利要求8所述的重建器械术野中心的控制方法，其特征在于，所述根据所述中间角度和所述动平台的当前角度对所述动平台进行旋转变换包括以下步骤：

根据所述中间角度获取所述静坐标系和所述动坐标系之间的第十一转换矩阵，根据所述第十一转换矩阵获取所述伸缩元件的第三伸缩量，根据所述第三伸缩量对所述伸缩元件进行调节，以使所述并联平台回归零位。

10.根据权利要求9所述的重建器械术野中心的控制方法，其特征在于，还包括以下步骤：

在所述并联平台回归零位后，根据重建术野后所述静坐标系与所述机械坐标系之间的转换关系，获取所述远心不动点在重建术野后所述静坐标系下的坐标，其中，所述远心不动点在所述机械坐标系下的坐标不变。

11.一种重建器械术野中心的控制系统，其特征在于，所述系统包括：

第一获取模块，用于获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，其中，所述机械坐标系是所述动平台所在机械臂的全局坐标系；

第二获取模块，用于根据所述动平台的当前位姿获取重建器械术野中心后，静平台在所述机械坐标系下的目标位姿，其中，所述动平台和所述静平台是所述机械臂中通过伸缩元件连接的并联平台，所述静平台的目标位姿的中心在所述动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与所述动平台平行；

第一变换模块，用于根据所述动平台的当前位姿和所述静平台的目标位姿对所述静平台进行位姿变换；

第二变换模块，用于根据所述动平台的当前位姿和所述静平台的目标位姿获取所述动平台和所述静平台之间在所述中心轴线上的扭转角度，根据所述扭转角度对所述动平台进行旋转变换，以使所述并联平台回归零位。

12.一种重建器械术野中心的控制设备，其特征在于，所述设备包括机械臂和控制装置；

所述控制装置用于根据权利要求1至10中任意一项所述的重建器械术野中心的控制方法对所述机械臂上的静平台进行位姿变换，对所述机械臂上的动平台进行旋转变换。

13.一种可读存储介质，其上存储有可执行程序，其特征在于，所述可执行程序被处理器执行时实现权利要求1至10中任意一项所述的重建器械术野中心的控制方法的步骤。

Images