CN115157268A - 空间姿态估计方法 - Google Patents

Abstract

本发明公开了空间姿态估计方法，获取空间中至少一个向量及至少一个空间位置；设置固定轴方向；根据所述向量及空间位置，以及所述固定轴方向，规划预设空间姿态；利用控制系统将物体按照所述预设空间姿态移动至目标规划姿态。通过预设空间姿态，一方面，根据预设空间姿态，能够有效避免设备旋转，造成绕线；另一方面，将空间姿态估计方法应用于机器人手臂旋转控制，能够有效避免起点姿态，更有效地控制机械臂。

Description

空间姿态估计方法

技术领域

本发明涉及经颅磁刺激医疗技术领域，更具体地，涉及一种空间姿态估计方法。

背景技术

现有临床应用经颅磁刺激治疗时存在诸多困难，极大的约束了经颅磁刺激技术在精神类和神经类疾病的治疗领域的应用和推广。主要困难有：第一，TMS刺激线圈的定位依赖医生经验和技术，主观性较强，刺激线圈放置不准确将影响治疗效果。第二，放置刺激线圈的过程中，患者的大脑结构不可见，而每个人的头部结构不同，定位帽不具有通用性，精度太差。第三，经颅磁刺激治疗每次治疗时间持续15至30分钟，此过程如果患者头部稍有移动，将导致刺激线圈的放置位置发生变化。若将患者头部固定，随着刺激时间的增长，肌肉收缩紧张，患者会感到不适，上述三个问题是目前影响经颅磁刺激治疗精度的主要原因，也是经颅磁刺激技术在推广应用中需要解决的技术难题。

使用TMS机器人治疗系统，在一定程度上提高刺激线圈定位的准确性，减轻人工手持线圈的不稳定性，然而在机械臂追踪的同时会引入刺激线圈绕线的情况，导致无法持续追踪患者头部靶点位置。

现有文献(201922312995.6)公开了用于经颅磁刺激拍的多触点定位装置，包括控制器、与控制器连接的机械手和多个检测触头；机械手通过夹具与刺激拍可拆卸连接，用于调整刺激拍的空间姿态；多个检测触头安装在刺激拍的拍面上，且检测触头的检测端位于刺激拍外部，用于检测刺激拍与头部接触时的姿态信息；多个检测触头在刺激拍上安装位置的几何中心与刺激拍内刺激线圈的几何中心重合；多个检测触头的检测端所在的平面与刺激拍的拍面平行，该方案是通过在磁刺激拍上设置多个检测触头，该检测触头用于检测刺激拍接触头部时相对于头部的空间姿态信息，并非通过姿态规划算法来实现刺激线圈绕线现象。

发明内容

有鉴于此，本发明提供了一种空间姿态估计方法，包括以下步骤：

获取空间中至少一个向量及至少一个空间位置；

设置固定轴方向；

根据所述向量及空间位置，以及所述固定轴方向，规划预设空间姿态；

利用控制系统将物体按照所述预设空间姿态移动至目标规划姿态。

可选地，还包括：

构建所述物体坐标系。

可选地，所述设置固定轴方向包括确定干扰位置的坐标轴方向。

可选地，所述构建所述物体坐标系还包括：

根据所述物体坐标系输入固定轴，其中，所述固定轴在世界坐标系中空间位置坐标的投影保持不变。

可选地，还包括：

构建自动控制装置的坐标系。

可选地，所述设置固定轴方向包括确定自动控制装置的坐标轴方向。

可选地，所述获取空间中至少一个向量及至少一个空间位置包括：

设定空间中的坐标系为世界坐标系；

根据所述世界坐标系输入至少一个向量以及至少一个空间位置。

与现有技术相比，本发明提供的空间姿态估计方法，至少实现了如下的有益效果：

一方面，根据预设空间姿态，能够有效避免设备旋转，造成绕线；

另一方面，将空间姿态估计方法应用于机器人手臂旋转控制，能够有效避免起点姿态，更有效地控制机械臂。

当然，实施本发明的任一产品必不特定需要同时达到以上所述的所有技术效果。

通过以下参照附图对本发明的示例性实施例的详细描述，本发明的其它特征及其优点将会变得清楚。

附图说明

被结合在说明书中并构成说明书的一部分的附图示出了本发明的实施例，并且连同其说明一起用于解释本发明的原理。

图1是本发明提供的空间姿态估计方法的流程图；

图2是本发明提供的三维空间示意图；

图3是本发明提供的三维俯视图。

具体实施方式

现在将参照附图来详细描述本发明的各种示例性实施例。应注意到：除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。

以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。

对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为说明书的一部分。

在这里示出和讨论的所有例子中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它例子可以具有不同的值。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

图1是本发明提供的空间姿态估计方法的流程图；图2是本发明提供的三维空间示意图；图3是本发明提供的三维俯视图；参见图1-图3所示，本实施例提供一种空间姿态估计方法，包括以下步骤：

S1获取空间中至少一个向量及至少一个空间位置；

S2设置固定轴方向；

S3根据所述向量及空间位置，以及所述固定轴方向，规划预设空间姿态；

S4利用控制系统将物体按照所述预设空间姿态移动至目标规划姿态。

具体地，设定空间中的坐标系为世界坐标系；根据所述世界坐标系输入至少一个向量以及至少一个空间位置，具体而言，首先已知空间中一个向量及空间位置，如向量的末端点或起始点，假设空间中的坐标系为世界坐标系，在控制系统中需要输入一个空间位置及一个方向，方式可以是其他控制系统的输出，或者手动输入控制系统。输入方向可以是向量或者其他形式的矢量；当然也可以获取空间中两个向量及空间位置，获取两个向量及空间位置可以重复做两次一个的情况即可；

根据向量及空间位置，设置固定轴方向；

针对设置固定轴方向根据物体建立三维坐标系，如物体坐标系；坐标轴中x，y，z方向可以随意；系统需要用户输入固定轴，如坐标轴中x，y，z，固定轴在规划姿态的时候，会保持该固定轴在世界坐标系中空间位置坐标中的投影保持不变(如xy，xz或yz)，已达到防止该轴随意变动导致系统故障，该固定轴也可以是物体坐标系中某个方向，可以是向量或者其他形式的矢量，只是在第一种情况下增加了某个固定的旋转。

根据所述向量及空间位置，并且按照所述固定轴方向的位置点，规划预设空间姿态；控制系统中可以建立物体坐标系和世界坐标系，如机械臂末端坐标系，以世界坐标系为参照世界坐标系举例，将物体按照预设空间姿态移动至目标规划姿态。

通过上述实施例可知，本发明提供的空间姿态估计方法，至少实现了如下的有益效果：

一方面，根据预设空间姿态，能够有效避免设备旋转，造成绕线；

另一方面，将空间姿态估计方法应用于机器人手臂旋转控制，能够有效避免起点姿态，更有效地控制机械臂。

可选地，还包括构建所述物体坐标系，设置固定轴方向包括确定干扰位置的坐标轴方向。

具体地，对设备进行坐标系建立，为物体坐标系，明确出线位置的坐标轴方向；

针对构建所述物体坐标系可以由用户输入，也可以系统规定，方法就是明确一个X轴方向，然后再明确一个Y轴方向，根据右手定则确定Z轴方向，没有特殊要求，因为坐标系之间有相互关系，若为设备，则默认设备的基座即为物体坐标系，当然也可以根据左手定则确定Z轴，具体采用左手定则还是右手定则，需要根据实际情况确认；

针对确定干扰位置的坐标轴方向在手臂坐标系中，可以由控制系统设置手臂方向为干扰方向坐标轴，也可以随意设置某一方向，如可以用户输入，也可以由控制系统设定。

防止设备绕线的空间姿态规划的应用场景如下，将设备进行坐标系建立，明确干扰位置的坐标轴方向；使用空间姿态估计方法，给出规划姿态；按照设置固定轴方向及姿态的姿态将设备移动到指定位置；按照空间姿态估计方法，能有效避免设备旋转，造成绕线。

可选地，构建自动控制装置的坐标系，所述设置固定轴方向包括确定自动控制装置的坐标轴方向。

具体地，对自动控制装置进行坐标系建立，为自动控制装置的坐标系，明确出线位置的坐标轴方向；

针对构建所述物体坐标系可以由用户输入，也可以系统规定，方法就是明确一个X轴方向，然后再明确一个Y轴方向，根据右手定则确定Z轴方向，没有特殊要求，因为坐标系之间有相互关系，若为机械臂，机械臂基座的默认坐标系为物体坐标系；当然也可以根据左手定则确定Z轴，具体采用左手定则还是右手定则，需要根据实际情况确认；

机器人手臂旋转控制的应用场景如下，将机器人手臂进行坐标系建立，明确机器人手臂末端坐标轴方向，使用空间姿态估计方法，指定机械臂末端坐标轴方向，按照预设空间姿态将设备移动到指定位置，按照空间姿态估计方法，能有效避免起点姿态，更有效的控制机械臂。

导航定位方法包括以下步骤：S1确定医疗器械与自动控制装置之间的坐标系转换关系；S2获取目标对象数据；S3根据所述目标对象数据，利用姿态规划算法对所述医疗器械进行姿态规划，得到自动控制装置坐标系下医疗器械的姿态矩阵；S4根据自动控制装置下医疗器械的姿态矩阵与自动控制装置的逆运动求解方法，使所述医疗器械移动至目标规划位置。

具体的，上述医疗器械可以为刺激线圈，自动控制装置可以为机械臂，目标对象数据可以为脑部MRI影像数据，刺激线圈可以经颅磁线圈，已知患者头部核磁共振MRI影像数据、刺激靶点及距刺激靶点最近的头皮上的刺激点(头皮上的坐标点)，同时完成机械臂和视觉相机的手眼标定，建立机械臂坐标系与视觉相机坐标系之间的关联关系，也就是机械臂坐标系与视觉相机坐标系之间的转换矩阵T1，将机械臂与刺激线圈进行固定，可以将刺激线圈固定于机械臂的末端，假设以将刺激线圈刺激原点为脑中心点(也就是说以刺激靶点为患者脑中心举例)，以刺激面正前方为Y轴，刺激线圈拍面正上方为Z轴，右侧为X轴，由于机械臂与刺激线圈位置固定，因此，比较容易确定刺激线圈与机械臂之间的坐标系转换关系。

由于刺激靶点位置只能位于患者脑部上半部分(头顶为上方向，即RAS坐标系，右手为X轴，正面前方为Y轴，头顶为Z轴)，所以以患者脑中心为坐标原点(MRI影像数据的原点就在患者脑中心)，可以把脑部上半部分分为2个象限，为相对设置的左侧象限与右侧象限，具体的还可以把脑部上半部分分为4个象限，分别为左前上象限、右前上象限、左后上象限和右后上象限，其中，左前上象限与右前上象限相对设置，左后上象限与右后上象限相对设置。

需要说明的是：将MRI影像数据放到RAS坐标系，坐标原点位于脑中心位置，由于人脑部形状为非圆状态，且每个人的人脑的差异，上述患者脑中心的位置可能会有些偏移，假设它在脑中心位置，稍微有些偏移不会影响刺激线圈的姿态规划，因为本发明考虑的是脑的上部，它在坐标系的Z轴大于0，若RAS坐标系位置过于偏上或过于偏下，可以采用坐标系的转换方式，将其转换到正常的RAS坐标系下即可。

S3根据脑部MRI影像数据，利用姿态规划算法对刺激线圈进行姿态规划，得到机械臂坐标系下刺激线圈的姿态矩阵，其中，根据MRI影像数据，利用姿态规划算法对刺激线圈进行姿态规划，得到自动控制装置坐标系下医疗器械的姿态矩阵；S4根据自动控制装置下医疗器械的姿态矩阵与自动控制装置的逆运动求解方法，使所述医疗器械移动至目标规划位置。

可选地，在获取脑部MRI影像数据之前，将机械臂移动至患者附近，需要保证机械臂上的刺激线圈的拍柄位于脑部的切平面，可以在脑上半部分任何位置去做切平面，若患者移动，刺激线圈的拍柄的位置均不会改变，参照图2和图3所示，将脑部上半部分分为左前上象限、左后上象限、右前上象限和右后上象限中，上述每个象限中两个黑点相连线的位置刺激线圈的拍柄所在位置，通常刺激线圈的拍柄的末端是需要连接线路的，同时需要保证机械臂上的刺激线圈的线路正常。

若在治疗过程中患者移动，仅需重复步骤S3和S4，就能保证在实时追踪的时候刺激线圈姿态不会绕线；

具体地，根据机械臂坐标系和RAS坐标系的转换关系，将机械臂坐标系下的刺激线圈姿态矩阵转换到RAS坐标系下，判断刺激线圈姿态是否为目标姿态，若不是目标姿态，利用本发明的姿态规划算法进行姿态规划，先确定RAS坐标系下刺激线圈的目标姿态矩阵，然后利用机械臂坐标和RAS坐标的转换关系，将RAS坐标系下刺激线圈的目标姿态矩阵转到机械臂坐标系下，最后利用机械臂的逆运动求解，控制运动。

若加入机械臂路径规划，也可以运用上述机械臂姿态规划的方法计算机械臂路径上的姿态，首先计算机械臂路径上每个位置上线圈与患者的最佳位置(刺激线圈平面始终是患者头皮的切平面，并且在刺激线圈移动中保持与患者脑部一定的垂直距离)，可以得到向量，利用步骤S3就能够保证在实时追踪的时候刺激线圈姿态不会绕线，也就说：在路径规划过程中，在路径规划中应用姿态规划算法避免刺激线圈做不规则运动，防止实时追踪时出现线圈绕线的情况，增加追踪的稳定性，扩大适用范围。

需要说明的是：为了与机械臂姿态规划中向量V和向量P做区别，也就是说，得到向量V’和向量P’，则可以利用机械臂姿态规划方法进行姿态规划算法的计算，从而避免刺激线圈做不规则运动。

通过上述实施例可知，本发明提供的导航定位方法，至少实现了如下的有益效果：

一方面，根据姿态规划，保证机械臂上的刺激线圈位置不会绕线，增加机械臂实时追踪的易用性；

另一方面，可以将机械臂末端刺激线圈的姿态规划计算应用于机械臂路径规划中，在机械臂路径规划中应用姿态规划算法，避免刺激线圈做不规则运动，防止实时追踪时出现刺激线圈绕线情况，增加了机械臂追踪的稳定性。

虽然已经通过例子对本发明的一些特定实施例进行了详细说明，但是本领域的技术人员应该理解，以上例子仅是为了进行说明，而不是为了限制本发明的范围。本领域的技术人员应该理解，可在不脱离本发明的范围和精神的情况下，对以上实施例进行修改。本发明的范围由所附权利要求来限定。

Claims (7)

1.一种空间姿态估计方法，其特征在于：包括以下步骤：

获取空间中至少一个向量及至少一个空间位置；

设置固定轴方向；

根据所述向量及空间位置，以及所述固定轴方向，规划预设空间姿态；

利用控制系统将物体按照所述预设空间姿态移动至目标规划姿态。

2.根据权利要求1所述的空间姿态估计方法，其特征在于，还包括：构建所述物体坐标系。

3.根据权利要求2所述的空间姿态估计方法，其特征在于，所述设置固定轴方向包括确定干扰位置的坐标轴方向。

4.根据权利要求2所述的空间姿态估计方法，其特征在于，所述构建所述物体坐标系还包括：

根据所述物体坐标系输入固定轴，其中，所述固定轴在世界坐标系中空间位置坐标的投影保持不变。

5.根据权利要求1所述的空间姿态估计方法，其特征在于，还包括：构建自动控制装置的坐标系。

6.根据权利要求5所述的空间姿态估计方法，其特征在于，所述设置固定轴方向包括确定自动控制装置的坐标轴方向。

7.根据权利要求1-6任一项所述的空间姿态估计方法，其特征在于，所述获取空间中至少一个向量及至少一个空间位置包括：

设定空间中的坐标系为世界坐标系；

根据所述世界坐标系输入至少一个向量以及至少一个空间位置。

Images