[0001] 本发明涉及机械臂技术领域，特别涉及一种机械臂控制方法、康复机器人、计算机及存储介质。

[0002] 随着人口老龄化的加剧，脑卒中患病率、致残率逐年升高，肢体运动障碍严重影响患者生活质量，给家庭、社会带来极大负担。上肢参与了日常活动中的大部分动作，其功能损伤严重影响患者运动能力与生活质量，因此，上肢运动功能训练是脑卒中患者康复过程中极其重要的环节。

[0003] 针对对于上肢运动障碍的不同程度的患者，有不同的训练模式，主动训练模式以患者的主动运动为主导，机械臂用于提供助力，以提高康复训练效果；被动训练模式由机械臂控制为主导，以预设的运动轨迹带动患者上肢运动，来提高康复训练效果。

[0004] 现有技术中，需要根据需求手动切换不同的康复训练模式，不仅增加了操作复杂度，还可能导致训练效果的不稳定。此外，手动切换模式还可能因为操作不及时或不准确，而影响患者的康复进程。

[0023] 为了便于理解本发明，下面将参照相关附图对本发明进行更全面的描述。附图中给出了本发明的若干实施例。但是，本发明可以以许多不同的形式来实现，并不限于本文所描述的实施例。相反地，提供这些实施例的目的是使对本发明的公开内容更加透彻全面。

[0024] 需要说明的是，当元件被称为“固设于”另一个元件，它可以直接在另一个元件上或者也可以存在居中的元件。当一个元件被认为是“连接”另一个元件，它可以是直接连接到另一个元件或者可能同时存在居中元件。本文所使用的术语“垂直的”、“水平的”、“左”、“右”以及类似的表述只是为了说明的目的。

[0025] 除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“及／或”包括一个或多个相关的所列项目的任意的和所有的组合。

[0026] 请参阅图1，所示为本发明第一实施例中的机械臂控制方法的流程图，如图所示，该方法包括以下步骤：步骤S100，实时获取机械臂末端的三维力传感器及握力传感器的采集数据，以基于所述采集数据获取手部的姿态数据及指关节握力数据。

[0027] 便于理解地，在本实施例中，上述上肢机械臂末端连接小臂托，患者小臂固定在臂托上，臂托前端设置握力传感器作为握把。手掌握住握力传感器，握力传感器下方连接一个三维力传感器，握力传感器可精确获取指关节握力大小，三维力传感器可以精确获取手部施力方向。

[0028] 步骤S200，判断所述指关节握力数据是否大于握力阈值。可以理解地，上述握力阈值表示需要切换模式时的最小握力大小，当指关节握力数据小于握力阈值时，无论三维力传感器多大，都不会对模式进行判断与切换，防止在外力作用下对姿态数据造成影响，而用户实际并没有握住握力传感器握把导致的模式误切换。

[0029] 步骤S300，若所述指关节握力数据大于所述握力阈值，且所述机械臂的控制模式为被动模式时，基于预设采集频率实时获取第一预设时间内与各所述姿态数据对应的多个第一合力数据，以基于各所述第一合力数据计算得到总合力数据。

[0030] 步骤S400，根据切换模式所需的三维力标定值及所述指关节握力数据计算当前模式切换所需的切换力数据，若所述总合力数据大于等于所述切换力数据，以基于所述切换力数据将所述机械臂的被动模式切换为主动模式。

[0031] 具体来说，在本实施例中，所述总合力数据的计算式如下：；
；
式中， 为所述总合力数据， 表示第b条第一合力数据，n为所述第一合
力数据的数量， 为所述第一合力数据， 、 、 分别为三维力传感器x、y、z三轴方向上的采集数据。

[0032] 所述切换力数据的计算式如下：；
式中， 为所述切换力数据， 为所述指关节握力数据，K为所述握力阈值，为所述三维力标定值。

[0033] 优选地，在本实施例中，上述预设采集频率为50ms一次，每50ms基于三维力传感器计算一次合力数据，计算1s‑2s内的数据的总值为总合力数据，握力越大，切换成主动模式需要的力也越大，从而基于上述总合力数据得到切换力数据。当上述总合力数据大于等于三维力标定值时，基于切换力数据将被动模式切换为主动模式；当上述总合力数据小于三维力标定值时，则不进行模式切换，维持被动模式，持续读取传感器数据，通过以上方式切换模式的速度也能按照患者想法调节，患者希望能快速的切换成主动模式，只需要手部施加更大的力量，最快1s内只需20条合力数据相加便能满足切换需求。而患者施力偏小时，最慢2s，即40条数据也可完成模式的自适应切换。

[0034] 优选地，在本实施例中，所述方法还包括：步骤S500，若所述指关节握力数据大于所述握力阈值，且所述机械臂的控制模式为主动模式时，基于预设采集频率实时获取与各所述姿态数据对应的多个第二合力数据；
若第二预设时间内获取的各所述第二合力数据均小于三维力标定值，将所述机械臂的主动模式切换为被动模式。

[0035] 可以理解地，当指关节握力数据小于握力阈值时，同样不会对模式进行切换，上述第二合力数据的计算式和第一合力数据的相同。

[0036] 具体来说，在本实施例中，将所述机械臂的主动模式切换为被动模式的步骤具体包括：设置软件定时器，若所述第二预设时间内获取的各所述第二合力数据均小于三维力标定值，控制所述软件定时器发出超时信号，以将所述机械臂的主动模式切换为被动模式。

[0037] 优选地，当指关节握力数据大于握力阈值时，每50ms计算一次第二合力数据，设置软件定时器，计算4s内第二合力数据的大小是否持续小于切换模式的阈值（三维力标定值），如果持续小于三维力标定值，4s时间到达时，定时器发出超时信号，设置模式切换成被动模式；如果在4s时间内，当第二合力数据大于上述三维力标定值时，则关闭软件定时器，而当出现第二合力数据大于上述三维力标定值时，才再次开启定时器，定时器关闭或未达到超时期间内，保持主动模式。

[0038] 在本实施例的一些应用场景中，当患者想施力控制机械臂时，可切换成主动模式，由患者自主控制康复训练，当患者不想施力控制机械臂时，可切换成被动模式，由机械臂带动患者进行康复训练。通过传感器确认患者当前的使用状态，可自适应的切换成患者的需要的康复训练模式，提高训练康复效果。

[0039] 优选地，在本实施例中，被动训练模式的控制方式如下：以所述机械臂的基座为原点，建立空间直角坐标系；
根据所述机械臂的被动模式中手腕关节的预设运动路径，基于所述空间直角坐标系获取与所述预设运动路径对应的多个路径点；
建立所述机械臂的D‑H坐标系，基于运动学的逆向求解算法，依次计算出各所述路径点所对应的电机旋转角度。

[0040] 具体来说，被动训练由机械臂控制，带动患者上肢运动，电机控制采用位置控制模式，以机械臂基座为原点，建立空间直角坐标系，确定末端手腕关节位置的运动路径，按固定500ms间隔取出路径上的所有坐标点空间坐标。建立机械臂的D‑H坐标系，通过运动学的逆向求解方法，依次将各坐标点空间坐标计算求出各个电机的旋转角度，相减得到每一段角度的间隔。上述电机转速的计算公式如下：；
式中，是电机的转速， 是每一段角度间隔， 是时间间隔500ms，算出每一小段路径的电机速度，电机每一段运行按求得的速度以及角度设置，机械臂便按照指定的路径运行。

[0041] 优选地，在本实施例中，主动训练模式的控制方式如下：计算重力对目标电机扭矩值的影响值；
当所述机械臂的控制模式为主动模式时，将所述影响值设置为所述目标电机的前馈扭矩大小。

[0042] 具体来说，主动训练由患者控制机械臂，机械臂跟随患者手臂运动，电机控制采用阻抗控制模式，解决机械臂在不同姿态下对患者自主运动产生的阻力大小，阻力大小主要来源于其他电机或连杆的重力对当前电机产生的扭矩，因此在控制上，首先需要消除重力带来的影响，对每个电机做一定的重力补偿，减少机械臂对患者自主运动的影响，使患者上肢运动时尽可能的感受不到机械臂的存在。

[0043] 进一步地，在本实施例中，计算重力对目标电机扭矩值的影响值的步骤具体包括：根据运动学的正向求解算法，计算得到与所述机械臂对应的所有电机以及连杆的坐标；
获取当前姿态下需要进行重力补偿的目标电机的坐标，并根据肩部关节对应电机的坐标得到所述目标电机的旋转轴坐标及所述旋转轴的向量；
根据所述机械臂中除所述目标电机外的其他电机及各连杆的重量，获取其他电机及各连杆在所述目标电机的旋转平面的重力向量，以基于所述重力向量计算得到重力对目标电机扭矩值的影响值。

[0044] 更进一步地，上述旋转轴坐标及所述旋转轴的向量的计算式如下：；
；
式中，Z为所述旋转轴坐标， 为所述肩部关节对应电机的坐标，
为所述目标电机的坐标， 为所述旋转轴的单位向量， 为所述旋转轴
的向量；
所述影响值的计算式如下：
；
；
；
；
；
式中，T为所述影响值，为其他电机及各连杆在所述目标电机的旋转平面的力臂的向量， 为 的单位向量， 为其他电机及各连杆的重量在所述目标电机的旋转平面的分量， 为其他电机及各连杆的重量向量，为其他电机及各连杆的坐标， 为 垂直于力臂 的向量。

[0045] 综上，本发明上述实施例当中的机械臂控制方法，通过集成三维力传感器和握力传感器，实现了对患者手部姿态及握力的精准监测，能够根据患者的实际握力和手部施力情况，智能地切换机械臂的训练模式，从而提高了康复训练的针对性和有效性，在被动训练模式中，通过建立空间直角坐标系和D‑H坐标系，利用运动学的逆向求解算法计算出机械臂的运动路径和电机旋转角度，从而确保机械臂能够按照预设的轨迹带动患者进行康复训练；在主动训练模式中，通过计算重力对目标电机扭矩值的影响值，并进行重力补偿，以减少机械臂对患者自主运动的影响，提高训练的舒适性和效果。

[0046] 本发明第二实施例提供了一种康复机器人，包括机械臂及机械臂控制系统，如图2所示，为本发明第二实施例中的机械臂控制系统的结构框图，如图所示，该系统包括：采集模块100，用于实时获取机械臂末端的三维力传感器及握力传感器的采集数据，以基于所述采集数据获取手部的姿态数据及指关节握力数据；
判断模块200，用于判断所述指关节握力数据是否大于握力阈值；
计算模块300，用于若所述指关节握力数据大于所述握力阈值，且所述机械臂的控制模式为被动模式时，基于预设采集频率实时获取第一预设时间内与各所述姿态数据对应的多个第一合力数据，以基于各所述第一合力数据计算得到总合力数据；
第一切换模块400，用于根据切换模式所需的三维力标定值及所述指关节握力数据计算当前模式切换所需的切换力数据，若所述总合力数据大于等于所述切换力数据，以基于所述切换力数据将所述机械臂的被动模式切换为主动模式。

[0047] 优选地，在本实施例中，上述系统还包括：第二切换模块，用于若所述指关节握力数据大于所述握力阈值，且所述机械臂的控制模式为主动模式时，基于预设采集频率实时获取与各所述姿态数据对应的多个第二合力数据；
若第二预设时间内获取的各所述第二合力数据均小于三维力标定值，将所述机械臂的主动模式切换为被动模式。

[0048] 优选地，在本实施例中，所述第二切换模块具体用于：设置软件定时器，若所述第二预设时间内获取的各所述第二合力数据均小于三维力标定值，控制所述软件定时器发出超时信号，以将所述机械臂的主动模式切换为被动模式。

[0049] 优选地，在本实施例中，所述系统还包括：被动训练模块，用于：
以所述机械臂的基座为原点，建立空间直角坐标系；
根据所述机械臂的被动模式中手腕关节的预设运动路径，基于所述空间直角坐标系获取与所述预设运动路径对应的多个路径点；
建立所述机械臂的D‑H坐标系，基于运动学的逆向求解算法，依次计算出各所述路径点所对应的电机旋转角度。

[0050] 优选地，在本实施例中，所述系统还包括：主动训练模块，用于：
计算重力对目标电机扭矩值的影响值；
当所述机械臂的控制模式为主动模式时，将所述影响值设置为所述目标电机的前馈扭矩大小。

[0051] 优选地，在本实施例中，所述主动训练模块具体用于：根据运动学的正向求解算法，计算得到与所述机械臂对应的所有电机以及连杆的坐标；
获取当前姿态下需要进行重力补偿的目标电机的坐标，并根据肩部关节对应电机的坐标得到所述目标电机的旋转轴坐标及所述旋转轴的向量；
根据所述机械臂中除所述目标电机外的其他电机及各连杆的重量，获取其他电机及各连杆在所述目标电机的旋转平面的重力向量，以基于所述重力向量计算得到重力对目标电机扭矩值的影响值。

[0052] 本申请的第三实施例提供了一种计算机，该计算机可以包括处理器81以及存储有计算机程序命令的存储器82。可以理解地，本实施例中的机械臂控制系统中提及的原理与本申请第一实施例中的机械臂控制方法相对应，未见描述的相关原理详见可对应参照第一实施例，在此不多赘述。

[0053] 具体地，上述处理器81可以包括中央处理器（CPU），或者特定集成电路（Application Specific Integrated Circuit，简称为ASIC），或者可以被配置成实施本申请实施例的一个或多个集成电路。

[0054] 其中，存储器82可以包括用于数据或命令的大容量存储器。举例来说而非限制，存储器82可包括硬盘驱动器（Hard Disk Drive，简称为HDD）、软盘驱动器、固态驱动器（Solid State Drive，简称为SSD）、闪存、光盘、磁光盘、磁带或通用串行总线（Universal Serial Bus，简称为USB）驱动器或者两个或更多个以上这些的组合。在合适的情况下，存储器82可包括可移除或不可移除（或固定）的介质。在合适的情况下，存储器82可在数据处理装置的内部或外部。在特定实施例中，存储器82是非易失性（Non‑Volatile）存储器。在特定实施例中，存储器82包括只读存储器（Read‑Only Memory，简称为ROM）和随机存取存储器（Random Access Memory，简称为RAM）。在合适的情况下，该ROM可以是掩模编程的ROM、可编程ROM（Programmable Read‑Only Memory，简称为PROM）、可擦除PROM（Erasable Programmable Read‑Only Memory，简称为EPROM）、电可擦除PROM（Electrically Erasable Programmable Read‑Only Memory，简称为EEPROM）、电可改写ROM（Electrically Alterable Read‑Only Memory，简称为EAROM）或闪存（FLASH）或者两个或更多个以上这些的组合。在合适的情况下，该RAM可以是静态随机存取存储器（Static Random‑Access Memory，简称为SRAM）或动态随机存取存储器（Dynamic Random Access Memory，简称为DRAM），其中，DRAM可以是快速页模式动态随机存取存储器（Fast Page Mode Dynamic Random Access Memory，简称为FPMDRAM）、扩展数据输出动态随机存取存储器（Extended Date Out Dynamic Random Access Memory，简称为EDODRAM）、同步动态随机存取内存（Synchronous Dynamic Random‑Access Memory，简称SDRAM）等。

[0055] 存储器82可以用来存储或者缓存需要处理和/或通信使用的各种数据文件，以及处理器81所执行的可能的计算机程序命令。

[0056] 处理器81通过读取并执行存储器82中存储的计算机程序命令，以实现上述实施例中的任意一种机械臂控制方法。

[0057] 在其中一些实施例中，计算机还可包括通信接口83和总线80。其中，如图3所示，处理器81、存储器82、通信接口83通过总线80连接并完成相互间的通信。

[0058] 通信接口83用于实现本申请实施例中各模块、装置、单元和/或设备之间的通信。通信接口83还可以实现与其他部件例如：外接设备、图像/数据采集设备、数据库、外部存储以及图像/数据处理工作站等之间进行数据通信。

[0059] 总线80包括硬件、软件或两者，将计算机的部件彼此耦接在一起。总线80包括但不限于以下至少之一：数据总线（Data Bus）、地址总线（Address Bus）、控制总线（Control Bus）、扩展总线（Expansion Bus）、局部总线（Local Bus）。举例来说而非限制，总线80可包括图形加速接口（Accelerated Graphics Port，简称为AGP）或其他图形总线、增强工业标准架构（Extended Industry Standard Architecture，简称为EISA）总线、前端总线（Front Side Bus，简称为FSB）、超传输（Hyper Transport，简称为HT）互连、工业标准架构（Industry Standard Architecture，简称为ISA）总线、无线带宽（InfiniBand）互连、低引脚数（Low Pin Count，简称为LPC）总线、存储器总线、微信道架构（Micro Channel Architecture，简称为MCA）总线、外围组件互连（Peripheral Component Interconnect，简称为PCI）总线、PCI‑Express（PCI‑X）总线、串行高级技术附件（Serial Advanced Technology Attachment，简称为SATA）总线、视频电子标准协会局部（Video Electronics Standards Association Local Bus，简称为VLB）总线或其他合适的总线或者两个或更多个以上这些的组合。在合适的情况下，总线80可包括一个或多个总线。尽管本申请实施例描述和示出了特定的总线，但本申请考虑任何合适的总线或互连。

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

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