CN114028153B - 一种康复机器人及其控制方法 - Google Patents

Abstract

本发明提供了一种康复机器人及其控制方法，涉及机器人技术领域，上述康复机器人控制方法包括：控制虚拟现实装置从第一侧机械臂和第二侧机械臂的协同工作空间中获取目标位置，接收虚拟现实装置发送的目标位置；基于人工势场算法确定第一侧机械臂和第二侧机械臂在无碰撞下到达目标位置各关节的输出力矩；基于输出力矩控制第一侧机械臂和第二侧机械臂对各关节施加辅助力矩或阻力矩，以使第一侧机械臂和第二侧机械臂的末端分别到达目标位置。本发明能够避免上肢康复训练中双臂之间发生碰撞，实现了对双侧手臂的协调训练，提升了康复机器人的实用性。

Description

一种康复机器人及其控制方法

技术领域

本发明涉及机器人技术领域，尤其是涉及一种康复机器人及其控制方法。

背景技术

现有的上肢康复机器人通常针对单侧手臂进行设计，无法进行协调性训练，少数双侧上肢康复机器人针对双臂进行康复训练，但是主要采用镜像训练方法，即一侧手臂主动运动，另一侧手臂被动地复制主动手臂的运动，在训练过程中双侧机械臂可能会产生碰撞，影响训练效果，训练的局限性较大，实用性较差。

发明内容

有鉴于此，本发明的目的在于提供一种康复机器人及其控制方法，能够实现对第一侧机械臂和第二侧机械臂的动态避障和静态避障，避免康复训练中双臂之间发生碰撞，同时可以实现对双侧手臂的协调训练，提升了康复机器人的实用性。

为了实现上述目的，本发明实施例采用的技术方案如下：

第一方面，本发明实施例提供了一种康复机器人控制方法，应用于康复机器人的控制器，所述康复机器人包括第一侧机械臂、第二侧机械臂、控制器和虚拟现实装置；所述康复机器人控制方法包括：控制所述虚拟现实装置从所述第一侧机械臂和所述第二侧机械臂的协同工作空间中获取目标位置，接收所述虚拟现实装置发送的目标位置；基于人工势场算法确定所述第一侧机械臂和所述第二侧机械臂在无碰撞下到达所述目标位置各关节的输出力矩；基于所述输出力矩控制所述第一侧机械臂和所述第二侧机械臂对各关节施加辅助力矩或阻力矩，以使所述第一侧机械臂和所述第二侧机械臂的末端分别到达所述目标位置。

进一步，本发明实施例提供了第一方面的第一种可能的实施方式，其中，所述康复机器人控制方法还包括：采用蒙特卡洛采样算法确定所述第一侧机械臂和所述第二侧机械臂的协同工作空间，将所述协同工作空间发送至所述虚拟现实装置中。

进一步，本发明实施例提供了第一方面的第二种可能的实施方式，其中，所述康复机器人控制方法还包括：控制所述虚拟现实装置向用户展示预存的训练模式，将用户选择的训练模式作为目标训练模式，基于所述目标训练模式从所述协同工作空间中选取任意一点作为目标位置。

进一步，本发明实施例提供了第一方面的第三种可能的实施方式，其中，所述基于人工势场算法确定所述第一侧机械臂和所述第二侧机械臂在无碰撞下到达所述目标位置各关节的输出力矩的步骤，包括：基于所述人工势场算法及所述目标位置分别计算所述第一侧机械臂和所述第二侧机械臂所受的吸引力和排斥力；接收所述虚拟现实装置发送的目标训练模式，根据所述目标训练模式确定对应的辅助力矩系数；基于所述吸引力、所述排斥力及所述辅助力矩系数确定所述第一侧机械臂和所述第二侧机械臂各关节的输出力矩。

进一步，本发明实施例提供了第一方面的第四种可能的实施方式，其中，所述基于所述吸引力、所述排斥力及所述辅助力矩系数确定所述第一侧机械臂和所述第二侧机械臂各关节的输出力矩的步骤，包括：基于所述吸引力和所述排斥力分别计算所述第一侧机械臂和所述第二侧机械臂所受合力；基于所述辅助力矩系数及所述第一侧机械臂和所述第二侧机械臂所受合力，分别计算所述第一侧机械臂和所述第二侧机械臂各关节的输出力矩。

进一步，本发明实施例提供了第一方面的第五种可能的实施方式，其中，所述第一侧机械臂各关节输出力矩的计算算式为：

其中，τ_L为所述第一侧机械臂各关节输出力矩，α为所述辅助力矩系数，分别为所述第一侧机械臂的各关节角度、角速度和角加速度，n为所述第一侧机械臂的关节自由度，J_L∈R^3×n为所述第一侧机械臂的线速度雅克比矩阵，F_L为所述第一侧机械臂所受合力，M_L(q_L)∈R^n×n为所述第一侧机械臂的惯性矩阵，/>为所述第一侧机械臂的克里斯托弗尔矩阵，G_L(q_L)∈Rⁿ为所述第一侧机械臂的重力项，为所述第一侧机械臂的摩擦力项；

所述第一侧机械臂所受合力的计算算式为：

F_L＝F_{att_L}+F_{rep_L2R2}+F_{rep_L2R1}+F_{rep_L2B}∈R³

其中，F_{att_L}∈R³为第一侧机械臂末端受到目标位置的吸引力向量，F_{rep_L2R2}∈R³为第一侧前臂L2受到第二侧前臂R2的排斥力向量，F_{rep_L2R1}∈R³为第一侧前臂L2受到第二侧上臂R1的排斥力向量，F_{rep_L2B}∈R³为第一侧前臂L2受到身体B的排斥力向量。

所述第二侧机械臂各关节的输出力矩的计算算式为：

其中，τ_R为所述第二侧机械臂各关节输出力矩，分别为所述第二侧机械臂的各关节角度、角速度和角加速度，m为所述第二侧机械臂的关节自由度，J_R∈R^{3 ×m}为所述第二侧机械臂的线速度雅克比矩阵，F_R为所述第二侧机械臂所受合力，M_R(q_R)∈R^{m ×m}为所述第二侧机械臂的惯性矩阵，/>为所述第二侧机械臂的克里斯托弗尔矩阵，G_R(q_R)∈R^m为所述第二侧机械臂的重力项，/>为所述第二侧机械臂的摩擦力项。

所述第二侧机械臂所受合力的计算算式为：

F_R＝F_{att_R}+F_{rep_R2L2}+F_{rep_R2L1}+F_{rep_R2B}

其中，F_{att_R}∈R³为第二侧机械臂末端受到目标位置的吸引力向量，F_{rep_R2L2}∈R³为第二侧前臂R2受到第一侧前臂L2的排斥力向量，F_{rep_R2L1}∈R³为第二侧前臂R2受到第一侧上臂L1的排斥力向量，F_{rep_R2B}∈R³为第二侧前臂R2受到身体B的排斥力向量。进一步，本发明实施例提供了第一方面的第六种可能的实施方式，其中，所述训练模式包括被动助力模式、主动助力模式和阻尼模式，所述被动助力模式对应的辅助力矩系数α＝100％，所述主动助力模式对应的辅助力矩系数α＝0～99％，所述阻尼模式对应的辅助力矩系数α＝-1～-100％。

第二方面，本发明实施例还提供了一种康复机器人，包括第一侧机械臂、第二侧机械臂、控制器和虚拟现实装置；所述虚拟现实装置用于从所述第一侧机械臂和所述第二侧机械臂的协同工作空间中获取目标位置，将所述目标位置发送至所述控制器；所述控制器用于基于人工势场算法确定所述第一侧机械臂和所述第二侧机械臂无碰撞下到达所述目标位置各关节的输出力矩，将所述输出力矩对应发送至所述第一侧机械臂和所述第二侧机械臂；所述第一侧机械臂和所述第二侧机械臂分别用于根据所述输出力矩对各关节施加辅助力矩或阻力矩。

进一步，所述控制器还用于采用蒙特卡洛采样算法确定所述第一侧机械臂和所述第二侧机械臂的协同工作空间，将所述协同工作空间发送至所述虚拟现实装置中；所述虚拟现实装置还用于向用户展示预存的训练模式，将用户选择的训练模式作为目标训练模式，基于所述目标训练模式从所述协同工作空间中选取任意一点作为目标位置。

进一步，所述控制器还用于基于所述人工势场算法及所述目标位置分别计算所述第一侧机械臂和所述第二侧机械臂所受的吸引力和排斥力；接收所述虚拟现实装置发送的目标训练模式，根据所述目标训练模式确定对应的辅助力矩系数；基于所述吸引力和所述排斥力分别计算所述第一侧机械臂和所述第二侧机械臂所受合力基于所述辅助力矩系数及所述第一侧机械臂和所述第二侧机械臂所受合力，分别计算所述第一侧机械臂和所述第二侧机械臂各关节的输出力矩。

本发明实施例提供了一种康复机器人及其控制方法，首先控制虚拟现实装置从第一侧机械臂和第二侧机械臂的协同工作空间中获取目标位置，接收虚拟现实装置发送的目标位置；其次基于人工势场算法确定第一侧机械臂和第二侧机械臂在无碰撞下到达目标位置各关节的输出力矩；最后基于输出力矩控制第一侧机械臂和第二侧机械臂对各关节施加辅助力矩或阻力矩，以使第一侧机械臂和第二侧机械臂的末端分别到达目标位置。本发明通过基于人工势场算法计算各机械臂关节的输出力矩，可以分别控制两侧机械臂动作，同时实现对第一侧机械臂和第二侧机械臂的动态避障和静态避障，避免上肢康复训练中双臂之间发生碰撞，实现了对双侧手臂的协调训练，提升了康复机器人的实用性。

本发明实施例的其他特征和优点将在随后的说明书中阐述，或者，部分特征和优点可以从说明书推知或毫无疑义地确定，或者通过实施本发明实施例的上述技术即可得知。

为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1示出了本发明实施例所提供的一种康复机器人控制方法流程图；

图2示出了本发明实施例所提供的一种康复机器人结构示意图；

图3示出了本发明实施例所提供的一种康复训练起始状态示意图；

图4示出了本发明实施例所提供的一种康复训练完成状态示意图；

图5示出了本发明实施例所提供的一种机械臂受力示意图。

图标：

21-控制器；22-第一侧机械臂；23-第二侧机械臂；24-虚拟现实装置；31-人体模块；L1-第一侧上臂；L2-第一侧前臂；R1-第二侧上臂；R2-第二侧前臂；311-第一侧机械臂末端位置；312-第二侧机械臂末端位置。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的技术方案进行描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。

目前，考虑到现有的上肢康复机器人在进行康复训练时，左右机械臂通常为镜像训练，机械臂运动时容易出现碰撞，且训练模式单一，为改善此问题，本发明实施例提供了一种康复机器人及其控制方法，以下对本发明实施例进行详细介绍。

本实施例提供了一种康复机器人控制方法，应用于康复机器人的控制器，参见图1所示的康复机器人控制方法流程图，上述康复机器人控制方法包括以下步骤S102～步骤S106：

步骤S102，控制虚拟现实装置从第一侧机械臂和第二侧机械臂的协同工作空间中获取目标位置，接收虚拟现实装置发送的目标位置。

上述康复机器人可以是双侧上肢康复机器人，参见如图2所示的康复机器人结构示意图，该康复机器人包括控制器21、第一侧机械臂22、第二侧机械臂23和虚拟现实装置24。

在康复训练开始之前，虚拟现实模块获取第一侧机械臂和第二侧机械臂的协同工作空间，即第一侧机械臂和第二侧机械臂都能到达的空间位置集合，并保存数据。在一种具体的实施方式中，上述第一侧机械臂是康复机器人的左侧机械臂，第二侧机械臂是康复机器人的右侧机械臂，或者相反，因此，上述第一侧机械臂和第二侧机械臂也可以称为左侧机械臂和右侧机械臂。

当用户将双侧手臂分别连接第一侧机械臂和第二侧机械臂开始训练时，从协同工作空间中随机抽取一点或者将某一设定点作为目标位置，将目标位置的坐标发送至控制器。

步骤S104，基于人工势场算法确定第一侧机械臂和第二侧机械臂在无碰撞下到达目标位置各关节的输出力矩。

采用人工势场算法控制机械臂运动至目标位置，分别计算两侧机械臂所受的吸引力和排斥力，吸引力用于将机械臂末端拉向目标位置，排斥力用于避免双侧机械臂产生碰撞，根据吸引力和排斥力确定双侧机械臂的各关节输出力矩。

步骤S106，基于输出力矩控制第一侧机械臂和第二侧机械臂对各关节施加辅助力矩或阻力矩，以使第一侧机械臂和第二侧机械臂的末端分别到达目标位置。

第一侧机械臂和第二侧机械臂根据计算得到的每个关节的输出力矩，分别对各个关节施加辅助力矩或阻力矩，以控制第一侧机械臂和第二侧机械臂的末端到达目标位置。

当执行下一个训练动作时，虚拟现实装置确定新的目标位置，并重复执行上述步骤S102～步骤S106，以实现对用户上肢的康复训练。

本实施例提供的上述康复机器人控制方法，通过基于人工势场算法计算各机械臂关节的输出力矩，可以分别控制两侧机械臂动作，同时实现对第一侧机械臂和第二侧机械臂的动态避障和静态避障，避免康复训练中双臂之间发生碰撞，实现了对双侧手臂的协调训练，提升了康复机器人的实用性。

在一种可行的实施方式中，在上述步骤S102之前，上述康复机器人的控制方法还包括以下步骤(1)～步骤(2)：

步骤(1)：采用蒙特卡洛采样算法确定第一侧机械臂和第二侧机械臂的协同工作空间，将协同工作空间发送至虚拟现实装置中。

在康复训练开始之前，控制器采用蒙特卡洛采样算法对第一侧机械臂和第二侧机械臂周围预设距离进行点位置直接采样，根据第一侧机械臂和第二侧机械臂的尺寸长度，确定第一侧机械臂和第二侧机械臂所能够到达的空间位置集合，将第一侧机械臂和第二侧机械臂所能够到达的空间位置集合作为第一侧机械臂和第二侧机械臂的协同工作空间，并将得到第一侧机械臂和第二侧机械臂的协同工作空间发送至虚拟现实装置。

步骤(2)：控制虚拟现实装置向用户展示预存的训练模式和康复训练场景，将用户选择的训练模式作为目标训练模式，基于目标训练模式从协同工作空间中选取任意一点作为目标位置。

如图2所示，上述虚拟现实装置包括显示屏和交互装置(诸如触摸屏或输入按键)，虚拟现实装置基于显示屏向用户显示康复机器人的训练模式和康复训练场景，用户在进行康复训练时根据需要的康复等级，通过交互装置选择需要的训练模式和康复训练场景，虚拟现实装置的显示屏显示用户选择的康复训练场景、目标位置及双侧机械臂的当前位置。

虚拟现实装置将用户选择的训练模式记为目标训练模式，根据该目标训练模式及用户选择的训练场景或训练动作，从机械臂能够达到的空间位置中选取任意一点作为机械臂末端要到达的目标位置。

为了在双侧机械臂末端到达目标位置的过程中实现精准无碰撞控制，本实施例提供了基于人工势场算法确定第一侧机械臂和第二侧机械臂在无碰撞下到达目标位置各关节的输出力矩的实施方式，具体可参照如下步骤1)～步骤3)执行：

步骤1)：基于人工势场算法及目标位置分别计算第一侧机械臂和第二侧机械臂所受的吸引力和排斥力。

基于上述虚拟现实装置实时显示当前的目标位置及双侧机械臂的当前动作，参见如图3所示的康复训练起始状态示意图，图3中示出了人体模型31和目标位置A，其中，人体模块包括身体B、第一侧上臂L1(即第一侧机械臂的上臂)、第一侧前臂L2、第一侧机械臂末端位置311、第二侧上臂R1、第二侧前臂R2和第二侧机械臂末端位置312。

控制器根据当前设置的目标位置A计算得到第一侧机械臂和第二侧机械臂在无碰撞下到达目标位置A各关节的输出力矩，并控制第一侧机械臂和第二侧机械臂对各关节施加辅助力矩或阻力矩，以使第一侧机械臂和第二侧机械臂的末端分别到达目标位置A，参见如图4所示的康复训练完成状态示意图，双侧机械臂的末端均到达目标位置A。

由于康复训练中的训练动作可能包括多个，当虚拟现实装置获取到下一个目标位置C时，控制器基于人工势场算法确定第一侧机械臂和第二侧机械臂在无碰撞下到达目标位置各关节的输出力矩，以使第一侧机械臂和第二侧机械臂的末端分别到达目标位置C，在计算各关机输出力矩时，首先计算第一侧机械臂和第二侧机械臂所受的吸引力和排斥力，参见如图5所示的机械臂受力示意图，图5中示出了双侧机械臂末端要从目标位置A处移动到目标位置C处时所受的吸引力和排斥力。

吸引力用于将机械臂末端拉向目标位置，吸引力的计算算式为：

其中，F_{att_L}∈R³为第一侧机械臂(左侧机械臂)末端受到目标位置的吸引力向量，F_{att_R}∈R³为第二侧机械臂(右侧机械臂)末端受到目标位置的吸引力向量，K_al为第一侧机械臂的引力系数(通过这个系数调节吸引力的强弱)，K_ar为第二侧机械臂的引力系数，P_L∈R³为第一侧机械臂末端的笛卡尔坐标，P_R∈R³为第二侧机械臂末端的笛卡尔坐标，P_d∈R³为目标位置的笛卡尔坐标，P_d-P_L∈R³为第一侧机械臂末端指向目标位置的向量，P_d-P_R∈R³为第二侧机械臂末端指向目标位置的向量，‖*‖表示向量*的模，d₁是吸引力距离常数，用于限制吸引力的最大值，当机械臂末端位置与目标位置的距离小于d₁时吸引力随着距离的增大而增大，当距离大于d₁时吸引力达到恒定的最大值。

排斥力用于避免双侧机械臂产生碰撞，排斥力包含以下三个部分：

其中，F_{rep_L2R2}∈R³为左侧前臂L2受到右侧前臂R2的排斥力向量，F_{rep_R2L2}∈R³表示第二侧前臂R2受到第一侧前臂L2的排斥力向量，K_rl和K_rr分别表示第一侧机械臂和第二侧机械臂的排斥力系数(通过这个系数可以调节排斥力的强弱)，ρ_L2R2∈R³表示第一侧前臂L2与第二侧前臂R2的最短距离向量，d₂是排斥力第一距离常数，当第一侧前臂L2与第二侧前臂R2的最小距离小于d₂时排斥力随着距离的减小而增大，从而避免第一侧前臂L2与第二侧前臂R2碰撞，当距离大于d₂时排斥力为零。

其中，F_{rep_L2R1}∈R³表示第一侧前臂L2受到第二侧上臂R1的排斥力向量，F_{rep_R2L1}∈R³表示第二侧前臂R2受到第一侧上臂L1的排斥力向量，ρ_L2R1∈R³表示第一侧前臂L2与第二侧上臂R1的最短距离向量，ρ_R2L1∈R³表示第二侧前臂R2与第一侧上臂L1的最短距离向量，d₃是排斥力第二距离常数，当第一侧前臂L2与第二侧上臂R1(或第二侧前臂R2与左侧上臂L1)的最小距离小于d₃时排斥力随着距离的减小而增大，从而避免第一侧前臂L2与第二侧上臂R1(或第二侧前臂R2与第一侧上臂L1)碰撞，当距离大于d₃时排斥力为零。

其中，F_{rep_L2B}∈R³表示第一侧前臂L2受到身体B的排斥力向量，F_{rep_R2B}∈R³表示第二侧前臂R2受到身体B的排斥力向量，ρ_L2B∈R³和ρ_R2B∈R³分别表示第一侧前臂L2和第二侧前臂R2与身体B的最短距离向量，d₄是排斥力第三距离常数，当第一侧前臂L2或第二侧前臂R2与身体B的最小距离小于d₄时排斥力随着距离的减小而增大，从而避免第一侧前臂L2或第二侧前臂R2与身体B碰撞，当距离大于d₄时排斥力为零。

步骤2)：接收虚拟现实装置发送的目标训练模式，根据目标训练模式确定对应的辅助力矩系数。

将用户选择的训练模式作为目标训练模式，其中，上述训练模式包括被动助力模式、主动助力模式和阻尼模式，被动助力模式对应的辅助力矩系数α＝100％，主动助力模式对应的辅助力矩系数α＝0～99％，阻尼模式对应的辅助力矩系数α＝-1～-100％(负号表示力与运动方向相反，是阻力)。

步骤3)：基于吸引力、排斥力及辅助力矩系数确定第一侧机械臂和第二侧机械臂各关节的输出力矩。

基于吸引力和排斥力分别计算第一侧机械臂和第二侧机械臂所受合力。

第一侧机械臂所受合力的计算算式为：

F_L＝F_{att_L}+F_{rep_L2R2}+F_{rep_L2R1}+F_{rep_L2B}∈R³

由于合力F_L包含向上的排斥力分量，因此第一侧机械臂末端P_L实际的运动轨迹会是一个外凸的曲线，可避免与第二侧机械臂碰撞，如图5上的虚线曲线S_L所示。

第二侧机械臂所受合力的计算算式为：

F_R＝F_{att_R}+F_{rep_R2L2}+F_{rep_R2L1}+F_{rep_R2B}

由于合力F_R包含向下排斥力的分量，因此第二侧机械臂末端P_R实际的运动轨迹会是一个内凹的曲线，可避免与第一侧机械臂碰撞，如图5上的虚线曲线S_R所示。

基于辅助力矩系数及第一侧机械臂和第二侧机械臂所受合力，分别计算第一侧机械臂和第二侧机械臂各关节的输出力矩。

第一侧机械臂各关节输出力矩的计算算式为：

其中，τ_L为第一侧机械臂各关节输出力矩，α为辅助力矩系数，分别为第一侧机械臂的各关节角度、角速度和角加速度，n为第一侧机械臂的关节自由度，J_L∈R^3×n为第一侧机械臂的线速度雅克比矩阵，f_L为第一侧机械臂所受合力，M_L(q_L)∈R^n×n为第一侧机械臂的惯性矩阵，/>为第一侧机械臂的克里斯托弗尔矩阵(Christoffel矩阵)，G_L(q_L)∈Rⁿ为第一侧机械臂的重力项，/>为第一侧机械臂的摩擦力项。

第二侧机械臂各关节的输出力矩的计算算式为：

其中，τ_R为第二侧机械臂各关节输出力矩，分别为第二侧机械臂的各关节角度、角速度和角加速度，m为第二侧机械臂的关节自由度，J_R∈R^3×m为第二侧机械臂的线速度雅克比矩阵，F_R为第二侧机械臂所受合力，M_R(q_R)∈R^m×m为第二侧机械臂的惯性矩阵，/> 为第二侧机械臂的克里斯托弗尔矩阵，G_R(q_R)∈R^m为第二侧机械臂的重力项，/>为第二侧机械臂的摩擦力项。

上述康复机器人双侧手臂均采用外骨骼机械臂的形式，每个单侧外骨骼的训练自由度包括以下7个自由度至少之一：肩关节外摆/内收自由度、前屈/后伸自由度，旋内/旋外自由度，肘关节屈曲/伸展自由度，前臂旋前/旋后自由度，腕关节背屈/掌屈、尺曲/桡曲自由度。双侧机械臂的各个关节自由度既可以单独进行训练，也可以任意组合在一起进行训练。

在基于上述康复机器人进行康复训练时，针对不同康复等级的患者可以采用不同的康复模式：

(1)对于Brunnstrom I-II期的患者，上述康复机器人可以提供被动助力(工作在被动助力模式)的康复训练方案。提供单关节康复训练动作包括：肩关节前屈/后伸；肩关节水平内收/外展；肩关节内旋/外旋；肘关节屈曲/伸直；前臂旋前/旋后等。提供多关节康复训练动作包括：PNF-D1屈伸；PNF-D2屈伸；模拟搬运动作；PNF双侧对称和非对称模式；各个方向的够取动作；模拟喝水、洗脸、穿袖子等。

(2)对于Brunnstrom III期的患者，康复机器人可以提供主动助力(工作在主动助力模式)的康复训练方案。提供单关节康复训练动作包括：肩关节前屈/后伸；肩关节水平内收/外展；肩关节内旋/外旋；肘关节屈曲/伸直；前臂旋前/旋后等。提供多关节康复训练动作包括：4象限够取动作；肘伸直位肩水平内收/外展；模拟擦桌子、擦玻璃、挂衣服；肘伸直位肩前屈0-90°；模拟从土地中拔菜动作；肘屈曲下肩零度位前臂旋前旋后；模拟翻书的动作；掌心朝上，肘伸直位下肩外展0-180°；肩外展50，肩前屈20，肩和前臂中立位下的肘屈伸，可模拟喝水、进食动作，肩前屈0度，肘关节屈伸，模拟手握菜刀切菜动作，肩0度，肘屈曲90度下肩内外旋活动，模拟把菜扫进盘子里等。

(3)对于Brunnstrom IV期的患者，康复机器人可以提供主动助力(工作在主动助力模式)的康复训练方案。提供单关节康复训练动作包括：肩关节前屈/后伸；肩关节水平内收/外展；肩关节内旋/外旋；肘关节屈曲/伸直；前臂旋前/旋后；固定肩肘关节，旋前时--朝左/上，旋后时--朝右/下等。提供多关节康复训练动作包括：肩水平内收肘屈曲+肩水平外展肘伸直；肩内收肘屈曲+肩前屈肘伸直；肩内收肘屈曲+肩前屈外展肘伸直；前臂固定在中立位/旋前位，肩0度位肘屈曲+肩前屈肘伸直；肩关节前屈位固定，肘屈曲前臂旋前+肘伸直前臂中立/旋后等。

(4)对于Brunnstrom V期的患者，康复机器人可以提供主动助力(工作在主动助力模式)的康复训练方案。提供多关节康复训练动作包括：肩前屈位下肘屈曲前臂旋前+肩外展肘伸直前臂中立/旋后；肩内收前屈肘伸直前臂旋前+肩外展(前屈)肘伸直前臂旋前等。

(5)对于Brunnstrom VI期的患者，康复机器人可以提供抗阻(工作在阻尼模式)的康复训练方案。提供多关节康复训练动作包括：肩内收前屈外旋肘伸直前臂旋后+肩外展内旋肘伸直前臂旋前；肩前屈位下肘屈曲前臂旋前+肩外展肘伸直前臂中立/旋后；肩内收前屈肘伸直前臂旋前+肩外展(前屈)肘伸直前臂旋前等。

本是实施例提供的上述康复机器人控制方法，通过采用人工势场算法对双侧机械臂进行运行控制，能够对动态环境快速响应，可以实现防碰撞移动，并结合虚拟现实技术，使用者在沉浸式的环境中大脑和肢体同时得到训练，提升了训练效果；通过设置不同的辅助力矩系数，使康复机器人能够实现主动助力、被动助力和阻尼训练三种训练模式，能够适用于不同康复等级的用户，通用性较强。

在前述实施例的基础上，本实施例提供了一种应用前述康复机器人控制方法的示例，具体可参照如下步骤1～步骤4：

步骤1：训练开始前使用蒙特卡洛采样方法确定双机械臂的协同工作空间(即左右两侧机械臂末端都能到达的空间位置集合)并保存数据。

步骤2：训练开始时，待训练用户手臂分别连接左/右侧机械臂，接收用户根据康复程度选择的康复训练场景及康复训练相关参数(辅助力矩系数、目标位置或训练动作等)。

步骤3：虚拟现实模块从协同工作空间中随机抽取一点作为目标位置P并发送给控制器，控制器基于人工势场算法计算到达目标位置双侧机械臂各个关节的输出力矩值。

步骤4：左/右侧机械臂根据每个关节对应的关节力矩值，对各关节施加辅助力矩或阻力矩，以对待训练患者进行上肢康复训练。

对应于上述实施例所提供的康复机器人控制方法，本发明实施例提供了一种康复机器人，该康复机器人包括：第一侧机械臂、第二侧机械臂、控制器和虚拟现实装置。

虚拟现实装置用于从第一侧机械臂和第二侧机械臂的协同工作空间中获取目标位置，将目标位置发送至控制器；

控制器用于基于人工势场算法确定第一侧机械臂和第二侧机械臂无碰撞下到达目标位置各关节的输出力矩，将输出力矩对应发送至第一侧机械臂和第二侧机械臂；

第一侧机械臂和第二侧机械臂分别用于根据输出力矩对各关节施加辅助力矩或阻力矩。

本实施例提供的上述康复机器人，通过基于人工势场算法计算各机械臂关节的输出力矩，可以分别控制两侧机械臂动作，同时实现对第一侧机械臂和第二侧机械臂的动态避障和静态避障，避免康复训练中双臂之间发生碰撞，实现了对双侧手臂的协调训练，提升了康复机器人的实用性。

在一种实施方式中，上述控制器还用于采用蒙特卡洛采样算法确定第一侧机械臂和第二侧机械臂的协同工作空间，将协同工作空间发送至虚拟现实装置中；

在一种实施方式中，虚拟现实装置还用于向用户展示预存的训练模式，将用户选择的训练模式作为目标训练模式，基于目标训练模式从协同工作空间中选取任意一点作为目标位置。

在一种实施方式中，控制器还用于基于人工势场算法及目标位置分别计算第一侧机械臂和第二侧机械臂所受的吸引力和排斥力；接收虚拟现实装置发送的目标训练模式，根据目标训练模式确定对应的辅助力矩系数；基于吸引力和排斥力分别计算第一侧机械臂和第二侧机械臂所受合力；基于辅助力矩系数及第一侧机械臂和第二侧机械臂所受合力，分别计算第一侧机械臂和第二侧机械臂各关节的输出力矩。

在一种实施方式中，第一侧机械臂各关节输出力矩的计算算式为：

其中，τ_L为第一侧机械臂各关节输出力矩，α为辅助力矩系数，分别为第一侧机械臂的各关节角度、角速度和角加速度，n为第一侧机械臂的关节自由度，J_L∈R^3×n为第一侧机械臂的线速度雅克比矩阵，F_L为第一侧机械臂所受合力，M_l(q_L)∈R^n×n为第一侧机械臂的惯性矩阵，/>为第一侧机械臂的克里斯托弗尔矩阵，G_L(q_L)∈Rⁿ为第一侧机械臂的重力项，/>为第一侧机械臂的摩擦力项；

在一种实施方式中，第二侧机械臂各关节的输出力矩的计算算式为：

其中，τ_R为第二侧机械臂各关节输出力矩，分别为第二侧机械臂的各关节角度、角速度和角加速度，m为第二侧机械臂的关节自由度，J_R∈R^3×m为第二侧机械臂的线速度雅克比矩阵，F_R为第二侧机械臂所受合力，M_R(q_R)∈R^m×m为第二侧机械臂的惯性矩阵，/> 为第二侧机械臂的克里斯托弗尔矩阵，G_R(q_R)∈R^m为第二侧机械臂的重力项，/>为第二侧机械臂的摩擦力项。

在一种实施方式中，上述训练模式包括被动助力模式、主动助力模式和阻尼模式，被动助力模式对应的辅助力矩系数α＝100％，主动助力模式对应的辅助力矩系数α＝0～99％，阻尼模式对应的辅助力矩系数α＝-1～-100％。

本是实施例提供的上述康复机器人，通过采用人工势场算法对双侧机械臂进行运行控制，能够对动态环境快速响应，可以实现防碰撞移动，并结合虚拟现实技术，使用者在沉浸式的环境中大脑和肢体同时得到训练，提升了训练效果；通过设置不同的辅助力矩系数，使康复机器人能够实现主动助力、被动助力和阻尼训练三种训练模式，能够适用于不同康复等级的用户，通用性较强。

本实施例所提供的康复机器人，其实现原理及产生的技术效果和前述实施例相同，为简要描述，康复机器人实施例部分未提及之处，可参考前述方法实施例中相应内容。

本发明实施例提供了一种计算机可读介质，其中，所述计算机可读介质存储有计算机可执行指令，所述计算机可执行指令在被处理器调用和执行时，所述计算机可执行指令促使所述处理器实现上述实施例所述的方法。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统具体工作过程，可以参考前述实施例中的对应过程，在此不再赘述。

本发明实施例所提供的康复机器人及其控制方法的计算机程序产品，包括存储了程序代码的计算机可读存储介质，所述程序代码包括的指令可用于执行前面方法实施例中所述的方法，具体实现可参见方法实施例，在此不再赘述。

另外，在本发明实施例的描述中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

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

在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。

最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (4)

1.一种康复机器人控制方法，其特征在于，应用于康复机器人的控制器，所述康复机器人包括第一侧机械臂、第二侧机械臂、控制器和虚拟现实装置；所述康复机器人控制方法包括：

采用蒙特卡洛采样算法确定所述第一侧机械臂和所述第二侧机械臂的协同工作空间，将所述协同工作空间发送至所述虚拟现实装置中；

控制所述虚拟现实装置向用户展示预存的训练模式，将用户选择的训练模式作为目标训练模式，基于所述目标训练模式从所述协同工作空间中选取任意一点作为目标位置；

控制所述虚拟现实装置从所述第一侧机械臂和所述第二侧机械臂的协同工作空间中获取目标位置，接收所述虚拟现实装置发送的目标位置；

基于人工势场算法确定所述第一侧机械臂和所述第二侧机械臂在无碰撞下到达所述目标位置各关节的输出力矩；

基于所述输出力矩控制所述第一侧机械臂和所述第二侧机械臂对各关节施加辅助力矩或阻力矩，以使所述第一侧机械臂和所述第二侧机械臂的末端分别到达所述目标位置；

所述基于人工势场算法确定所述第一侧机械臂和所述第二侧机械臂在无碰撞下到达所述目标位置各关节的输出力矩的步骤，包括：基于所述人工势场算法及所述目标位置分别计算所述第一侧机械臂和所述第二侧机械臂所受的吸引力和排斥力；

接收所述虚拟现实装置发送的目标训练模式，根据所述目标训练模式确定对应的辅助力矩系数；

基于所述吸引力、所述排斥力及所述辅助力矩系数确定所述第一侧机械臂和所述第二侧机械臂各关节的输出力矩；

所述基于所述吸引力、所述排斥力及所述辅助力矩系数确定所述第一侧机械臂和所述第二侧机械臂各关节的输出力矩的步骤，包括：基于所述吸引力和所述排斥力分别计算所述第一侧机械臂和所述第二侧机械臂所受合力；基于所述辅助力矩系数及所述第一侧机械臂和所述第二侧机械臂所受合力，分别计算所述第一侧机械臂和所述第二侧机械臂各关节的输出力矩。

2.根据权利要求1所述的康复机器人控制方法，其特征在于，所述第一侧机械臂各关节输出力矩的计算算式为：

其中，为所述第一侧机械臂各关节输出力矩，/>为所述辅助力矩系数，分别为所述第一侧机械臂的各关节角度、角速度和角加速度，/>为所述第一侧机械臂的关节自由度，/>为所述第一侧机械臂的线速度雅克比矩阵，/>为所述第一侧机械臂所受合力，/>为所述第一侧机械臂的惯性矩阵，为所述第一侧机械臂的克里斯托弗尔矩阵，/>为所述第一侧机械臂的重力项，/>为所述第一侧机械臂的摩擦力项；

所述第一侧机械臂所受合力的计算算式为：

其中，为第一侧机械臂末端受到目标位置的吸引力向量，/>为第一侧前臂L2受到第二侧前臂R2的排斥力向量，/>为第一侧前臂L2受到第二侧上臂R1的排斥力向量，/>为第一侧前臂L2受到身体B的排斥力向量；

所述第二侧机械臂各关节的输出力矩的计算算式为：

其中，为所述第二侧机械臂各关节输出力矩，/>分别为所述第二侧机械臂的各关节角度、角速度和角加速度，/>为所述第二侧机械臂的关节自由度，/>为所述第二侧机械臂的线速度雅克比矩阵，/>为所述第二侧机械臂所受合力，为所述第二侧机械臂的惯性矩阵，/>为所述第二侧机械臂的克里斯托弗尔矩阵，/>为所述第二侧机械臂的重力项，为所述第二侧机械臂的摩擦力项；

所述第二侧机械臂所受合力的计算算式为：

其中，为第二侧机械臂末端受到目标位置的吸引力向量，/>为第二侧前臂R2受到第一侧前臂L2的排斥力向量，/>为第二侧前臂R2受到第一侧上臂L1的排斥力向量，/>为第二侧前臂R2受到身体B的排斥力向量。

3.根据权利要求1所述的康复机器人控制方法，其特征在于，所述训练模式包括被动助力模式、主动助力模式和阻尼模式，所述被动助力模式对应的辅助力矩系数，所述主动助力模式对应的辅助力矩系数/>，所述阻尼模式对应的辅助力矩系数。

4.一种康复机器人，其特征在于，包括第一侧机械臂、第二侧机械臂、控制器和虚拟现实装置；

所述虚拟现实装置用于从所述第一侧机械臂和所述第二侧机械臂的协同工作空间中获取目标位置，将所述目标位置发送至所述控制器；

所述控制器用于基于人工势场算法确定所述第一侧机械臂和所述第二侧机械臂无碰撞下到达所述目标位置各关节的输出力矩，将所述输出力矩对应发送至所述第一侧机械臂和所述第二侧机械臂；

所述第一侧机械臂和所述第二侧机械臂分别用于根据所述输出力矩对各关节施加辅助力矩或阻力矩；

所述控制器还用于基于所述人工势场算法及所述目标位置分别计算所述第一侧机械臂和所述第二侧机械臂所受的吸引力和排斥力；接收所述虚拟现实装置发送的目标训练模式，根据所述目标训练模式确定对应的辅助力矩系数；基于所述吸引力和所述排斥力分别计算所述第一侧机械臂和所述第二侧机械臂所受合力；基于所述辅助力矩系数及所述第一侧机械臂和所述第二侧机械臂所受合力，分别计算所述第一侧机械臂和所述第二侧机械臂各关节的输出力矩；

所述控制器还用于采用蒙特卡洛采样算法确定所述第一侧机械臂和所述第二侧机械臂的协同工作空间，将所述协同工作空间发送至所述虚拟现实装置中；

所述虚拟现实装置还用于向用户展示预存的训练模式，将用户选择的训练模式作为目标训练模式，基于所述目标训练模式从所述协同工作空间中选取任意一点作为目标位置。