一种控制机械臂移动的方法和装置与流程

本发明属于医疗器械技术领域，尤其涉及一种控制机械臂移动的方法和装置。

背景技术：

根据中国疾病预防控制中心精神卫生中心统计，目前，我国精神疾病患者总数已超过1亿，精神病专科医院已经超过600家，其中，经颅磁刺激(transcranialmagneticstimulation,tms)是对精神病治疗的一种基础和有效的治疗手段。利用tms治疗时，将线圈与大脑皮层接触，通过脉冲磁场在大脑皮层中产生电流以暂时激活或抑制该皮层。tms可以治疗精神分裂症(阴性症状)、抑郁症、强迫症、躁狂症、创伤后应激障碍(ptsd)等精神疾病，其中对抑郁症的治疗在美国已经通过fda的认证。另外，对运动皮层进行tms可以产生肌肉抽动或阻挡运动。对枕叶皮质进行刺激可以产生光幻视或盲点。tms还可以在刺激之后的一段时间内改善大脑功能。

传统的tms治疗系统，通过人工来调节线圈与大脑皮层的位置，如果人转动头部，则tms系统则无法追踪人体头部运动。这样使tms治疗系统对大脑皮层刺激的位置不准确，从而降低治疗效果。

技术实现要素：

本发明提供一种控制机械臂移动的方法和装置，旨在解决由于传统的tms系统无法追踪人体头部运动，在治疗过程中当人体头部转动时，会使得tms对对大脑皮层刺激的位置不准确，从而降低治疗效果的问题。

本发明第一方面提供的一种控制机械臂移动的方法，所述方法包括：

获取用户头部的预设部位的位置和机械臂末端的线圈的初始位置；

根据所述预设部位的位置和所述初始位置，确定所述线圈的运行轨迹；

根据所述运行轨迹进行运动学反解计算，得到所述机械臂中每个关节的旋转角度；

控制所述关节按照所述旋转角度旋转，以使所述线圈到达所述预设部位。

本发明第二方面提供的一种控制机械臂移动的装置，所述装置包括：获取模块、确定模块、计算模块和控制模块；

获取模块，用于获取用户头部的预设部位的位置和机械臂末端的线圈的初始位置；

确定模块，用于根据所述预设部位的位置和所述初始位置，确定所述线圈的运行轨迹；

计算模块，用于根据所述运行轨迹进行运动学反解计算，得到所述机械臂中每个关节的旋转角度；

控制模块，用于控制所述关节按照所述旋转角度旋转，以使与所述线圈到达所述预设部位。

本发明提供的一种控制机械臂移动的方法和装置，通过获取运动后的人头部的预设部位和线圈的初始位置，确定线圈的运行轨迹，并根据运行轨迹计算机械臂中关节的旋转角度。如此，当人头部运动时，即使预设部位发生变化，线圈也可以追踪到预设部位，可以准确的对预设部位进行追踪，从而提高了治疗的效果。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例。

图1是本发明第一实施例提供的一种控制机械臂移动的方法的实现流程示意图；

图2是本发明第二实施例提供的一种控制机械臂移动的方法的实现流程示意图；

图3是本发明第三实施例提供的一种柔性驱动关节的结构示意图；

图4是本发明第四实施例提供的一种控制机械臂移动的装置的结构示意图；

图5是本发明第五实施例提供的一种控制机械臂移动的装置的结构示意图。

具体实施方式

为使得本发明的发明目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而非全部实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1，图1为本发明第一实施例提供的控制机械臂移动的方法的实现流程示意图，图1所示的接领人识别方法主要包括以下步骤：

s101、获取用户头部的预设部位的位置和机械臂末端的线圈的初始位置；

预设部位为用户头部需要进行经颅磁刺激的位置。在实际应用中，控制机械臂移动的装置包括由多个关节组成的机械臂，其中，机械臂的一端与线圈连接。线圈为用于与用户头部的预设部位进行接触，以输出脉冲磁场的线圈。

s102、根据该预设部位的位置和该初始位置，确定该线圈的运行轨迹；

可以将预设部位的位置和初始位置的确定的直线作为线圈的运行轨迹，也可以将预设部位、初始位置以及预设的中间位置确定的弧线作为线圈的运行轨迹。

s103、根据该运行轨迹进行运动学反解计算，得到该机械臂中每个关节的旋转角度；

运行轨迹为线圈由初始位置到达用户头部的预设部位的运行轨迹。具体的，确定运行轨迹之后，计算机械臂中关节在运行轨迹中的位置坐标，并根据该关节的位置坐标来计算关节的旋转角度。其中，关节为可以转动的装置，例如电机和传动杆组成的装置，电机通过转动带动传动杆转动实现旋转。

s104、控制该关节按照该旋转角度旋转，以使该线圈到达该预设部位。

控制机械臂移动的装置控制机械臂中每个关节按照对应的旋转角度旋转，可以使机械臂末端的线圈到达用于头部的预设部位。

本发明第一实施例提供的一种控制机械臂移动的方法，通过获取运动后的人头部的预设部位和线圈的初始位置，确定线圈的运行轨迹，并根据运行轨迹计算机械臂中关节的旋转角度。如此，当人头部运动时，即使预设部位发生变化，线圈也可以追踪到预设部位，可以准确的对预设部位进行追踪，从而提高了治疗的效果。

请参阅图2，图2为本发明第二实施例提供的控制机械臂移动的方法的实现流程示意图，图2所示的经颅磁刺激的追踪方法应用于控制机械臂移动的装置。该方法主要包括以下步骤：

s201、获取待用户头部的预设部位的位置和机械臂末端的线圈的初始位置；

预设部位为用户头部需要进行经颅磁刺激的位置。在实际应用中，控制机械臂移动的装置包括由多个关节组成的机械臂，其中，机械臂的一端与线圈连接。线圈为用于与用户头部的预设部位进行接触，以输出脉冲磁场的线圈。在实际应用中，在用户头部和线圈上放置多个红外反射球，控制机械臂移动的装置通过获取红外反射球反射的红外线来获取待测人员的头部的预设部位和线圈的初始位置。

s202、根据该预设部位的位置和该初始位置，确定该线圈的运行轨迹；

进一步地，根据该预设部位和该初始位置，确定该线圈的运行轨迹，具体为：将该预设部位的位置坐标、该初始位置的位置坐标和预设的中间位置的位置坐标确定的弧线作为该线圈的运行轨迹。其中，该中间位置的位置坐标、预设部位的位置坐标和初始位置的位置坐标不共线，三个位置坐标可以唯一确定一个圆弧，即，由上述三个位置坐标确定的圆弧为线圈的运行轨迹。可选地，将由预设部位的位置和该初始位置确定的直线，作为该线圈的运行轨迹。

s203、根据该运行轨迹进行运动学反解计算，得到该机械臂中每个关节的旋转角度；

进一步地，根据该运行轨迹进行运动学反解计算，得到该机械臂中每个关节的旋转角度，具体包括：

利用圆弧插补法和直线插补法分别计算该运行轨迹中每个点的位置坐标；

运行轨迹中每个点的位置坐标包括：xyz坐标系的位置坐标和姿态角，其中，利用圆弧插补法计算运行轨迹中点的xyz坐标系的位置坐标，利用直线插补法计算运行轨迹中每个点的姿态角。

具体的，利用圆弧插补法计算运行轨迹中点的xyz坐标系中的位置坐标，包括：

在xyz坐标系中，根据预设部位的坐标p1(x1,y1,z1)、初始位置的坐标p2(x2,y2,z2)和中间位置的坐标p3(x3,y3,z3)计算上述三个位置坐标构成的圆形的圆心坐标po和半径r。将圆心坐标po作为原点or，建立uvw坐标系，并将p1、p2和p3转化为uvw坐标系中的坐标p’1(u1,v1,w1)、p’2(u2,v2,w2)和p’3(u3,v3,w3)。

假设从p’1到p’2的圆弧上的点的坐标在uvw坐标系的坐标为p’(u,v,w)，根据运行轨迹中预设点的个数，计算步长为α，则从p’1到p’扫过的角度为θ，而从p’1到p’3扫过的总角度为θ3。需要说明的是，预设的点的个数可以由追踪时间确定，追踪时间越长，预设点的个数越多，步长越短，计算出的运行轨迹上的点的位置坐标越精确。设定

当v3>0时，θ3＝atan2(v3,u3)；

当v3<0时，θ3＝2π+atan2(v3,u3)。

其中a为设定值。

则p’1到p’扫过的角度θ＝αθ3。进而可求得p’在uvw坐标系中的坐标p’(u,v,w)：

将p’(u,v,w)在uvw坐标系中的坐标转换为xyz坐标系中的位置坐标p(x,y,z)。

具体的，利用直线插补法计算运行轨迹上的点的姿态角时，假设机械臂末端的起点姿态角为p1，目标姿态角为p2，每步运动的步长为α，则利用直线插补法计算第i步的姿态角p(i)，得到：p(i)＝p1+αi。其中，步长为相邻的两个插补点之间姿态角的变化。可选地，可以利用圆弧插补法来计算姿态角的位置坐标。

按照该运行轨迹模拟该机械臂的运行，并从该运行轨迹中提取该关节的位置坐标；

根据该关节的位置坐标以及预存的与该关节连接的该连杆的结构参数，进行该运动学反解计算，得到该关节的旋转角度。

具体的，根据运动学反解等式，其中，可由与关节连接的连杆的结构参数、该关节的xyz坐标系中的位置坐标和姿态角求出，分别为每个关节的矩阵变换公式，该公式中包含了每个关节的旋转角度θ，其中，第一个关节的旋转角度为θ1，第二个关节的旋转角度为θ2，第n个关节的旋转角度为θn。利用上述运动学反解等式可以求出多组解，选取最小的转角作为关节的转角。需要说明的是，连杆的结构参数包括相连连杆之间的距离、角度和高度。

s204、控制该关节按照该旋转角度旋转，以使该线圈到达该预设部位；

具体的，机械臂中的每个关节按照对应的旋转角进行转动，使线圈到达预设部位。在实际应用中，每个连杆与电机相连，即，连杆的关节为电机，通过电机的转动来实现关节的转动。

s205、向该线圈施压，以使该线圈压紧该预设部位；

s206、当施加的压力达到预设的压力值时，开启脉冲磁场以刺激该预设部位的大脑皮层。

机械臂包括柔性驱动关节，请参照图3，图3为柔性驱动关节的结构示意图，其中，柔性驱动关节包括电机301、主动轮302、从动轮303、柔索304和关节305、其中，从动轮303包括：第一挡板313、第二挡板323、第一弹簧333和第二弹簧343，其中，柔索304连接主动轮302和从动轮303，第一挡板313和第二挡板323固定于从动轮303内侧，第一弹簧333的一端与第一挡板313固定，另一端与关节305的一端固定，第二弹簧343的一端与第二挡板323的一端固定，另一端与关节305的另一端固定。关节305沿从动轮303的中心转动。于本发明的一个实施例，机械臂包括6个关节，其中柔性驱动关节作为机械臂的中间部位的关节。当电机顺时针或逆时针旋转使，带动主动轮302顺时针或逆时针旋转，当从动轮303顺时针旋转时，第一弹簧333被压缩，第二弹簧343被拉伸，当从动轮303逆时针旋转时，第一弹簧333被拉伸，第二弹簧343被压缩，控制机械臂移动的装置可以通过第一弹簧333和第二弹簧343的压缩或拉伸的长度，进而计算出线圈施加的压力。

具体的，控制机械臂移动的装置通过柔性驱动关节计算施加的压力，并判段施加的压力是否达到预设的压力值，若施加的压力大于或等于预设的压力值，则停止施加压力，并开启脉冲磁场来刺激预设部位的大脑皮层。

本发明第二实施例提供的一种控制机械臂移动的方法，通过获取运动后的人头部的预设部位和线圈的初始位置，确定线圈的运行轨迹，并根据运行轨迹计算机械臂中关节的旋转角度。如此，当人头部运动时，即使预设部位发生变化，线圈也可以追踪到预设部位，可以准确的对预设部位进行追踪，从而提高了治疗的效果。

请参阅图4，图4是本发明第三实施例提供的一种控制机械臂移动的装置的结构示意图，为了便于说明，仅示出了与本发明实施例相关的部分。图4示例的控制机械臂移动的装置可以是前述图1所示实施例提供的接领人识别方法的执行主体，或者还可以是该执行主体中的一个功能模块。图4示例的控制机械臂移动的装置，主要包括：获取模块401、确定模块402、计算模块403和控制模块404。以上各功能模块详细说明如下：

获取模块401，用于获取用户头部的预设部位的位置和机械臂末端的线圈的初始位置；

确定模块402，用于根据该预设部位的位置和该初始位置，确定该线圈的运行轨迹；

计算模块403，用于根据该运行轨迹进行运动学反解计算，得到该机械臂中每个关节的旋转角度；

控制模块404，用于控制该关节按照该旋转角度旋转，以使与该线圈到达该预设部位。

本发明实施例中的未尽细节，请参照图1和图2所示的第一实施例和第二实施例，在此不再赘述。

本发明第三实施例提供的一种控制机械臂移动的装置，通过获取运动后的人头部的预设部位和线圈的初始位置，确定线圈的运行轨迹，并根据运行轨迹计算机械臂中关节的旋转角度。如此，当人头部运动时，即使预设部位发生变化，线圈也可以追踪到预设部位，可以准确的对预设部位进行追踪，从而提高了治疗的效果。

请参阅图5，图5是本发明第四实施例提供的一种控制机械臂移动的装置的结构示意图，为了便于说明，仅示出了与本发明实施例相关的部分。图5示例的控制机械臂移动的装置可以是前述图1所示实施例提供的接领人识别方法的执行主体，或者还可以是该执行主体中的一个功能模块。图3示例的控制机械臂移动的装置，主要包括：获取模块501、确定模块502、计算模块503和控制模块505。以上各功能模块详细说明如下：

获取模块501，用于获取用户头部的预设部位的位置和机械臂末端的线圈的初始位置；

确定模块502，用于根据该预设部位的位置和该初始位置，确定该线圈的运行轨迹；

计算模块503，用于根据该运行轨迹进行运动学反解计算，得到该机械臂中每个关节的旋转角度；

控制模块504，用于控制该关节按照该旋转角度旋转，以使与该线圈到达该预设部位。

进一步地，

确定模块502，还用于将该预设部位的位置坐标、该初始位置的位置坐标和预设的中间位置的位置坐标确定的弧线作为该线圈的运行轨迹。

进一步地，计算模块503还包括：

坐标计算模块513，用于利用圆弧插补法和直线插补法分别计算该运行轨迹中每个点的位置坐标；

提取模块523，用于按照该运行轨迹模拟该机械臂的运行，并从该运行轨迹中提取该关节的位置坐标；

反解计算模块533，用于根据该关节的位置坐标以及预存的与该关节连接的该连杆的结构参数，进行该运动学反解计算，得到该关节的该旋转角度。

进一步地，

控制模块504，还用于向该线圈施压，以使该线圈压紧该预设部位；

该装置还包括：

启动模块505，用于当施加的压力达到预设的压力值时，开启脉冲磁场以刺激该预设部位的大脑皮层。

本发明实施例中的未尽细节，请参照图1和图2所示的第一实施例和第二实施例，在此不再赘述。

本发明第四实施例提供的一种控制机械臂移动的装置，通过获取运动后的人头部的预设部位和线圈的初始位置，确定线圈的运行轨迹，并根据运行轨迹计算机械臂中关节的旋转角度。如此，当人头部运动时，即使预设部位发生变化，线圈也可以追踪到预设部位，可以准确的对预设部位进行追踪，从而提高了治疗的效果。

在本申请所提供的多个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述模块的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个模块或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信链接可以是通过一些接口，装置或模块的间接耦合或通信链接，可以是电性，机械或其它的形式。

所述作为分离部件说明的模块可以是或者也可以不是物理上分开的，作为模块显示的部件可以是或者也可以不是物理模块，即可以位于一个地方，或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能模块可以集成在一个处理模块中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。

所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccessmemory)、磁碟或者光盘等各种可以存储程序代码的介质。

需要说明的是，对于前述的各方法实施例，为了简便描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其它顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定都是本发明所必须的。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其它实施例的相关描述。

以上为对本发明所提供的控制机械臂移动的方法和装置的描述，对于本领域的技术人员，依据本发明实施例的思想，在具体实施方式及应用范围上均会有改变之处，综上，本说明书内容不应理解为对本发明的限制。