基于交互力识别运动意图的欠驱动康复机器人的控制方法与流程

本发明属于外骨骼机器人控制相关技术领域，更具体地，涉及一种基于交互力识别运动意图的欠驱动康复机器人的控制方法。

背景技术：

目前，大部分康复机器人仅能完成被动训练任务，无法根据患者的主动运动意图完成主动康复训练，如申请号为201410627428.9的专利公开了一种外骨骼式上肢康复训练系统，其是辅助康复医师完成康复训练的一种医疗设备，可以实现肩部三个自由度、肘部一个自由度和腕部一个自由度的运动，真实再现患者的日常生活的动作训练，所述外骨骼式上肢康复训练系统由两部分组成：第一耦合运动机构及第二耦合运动机构。所述第一耦合运动机构将肩部外展和内旋两个自由度耦合，采用一个电机驱动；所述第二耦合运动机构将肩部前伸、肘部屈肘及腕部翻转三个自由度耦合，采用一个电机驱动，故所述外骨骼式上肢康复训练系统采用两个电机控制上肢五个关节的运动，其为欠驱动系统。该外骨骼式上肢康复训练系统的优势在于既减少了驱动电机的数量，使得控制可靠，又将驱动电机转移到了非机械臂处，减小了外骨骼的尺寸大小，提高患者的安全性，但其无法根据训练者的主动运动意图完成主动康复训练。

然而，在肢体康复训练中，患者主动运动康复训练是必须的康复过程。若要实现主动康复训练，就要能够准确判断患者肢体的运动意图，进行主动训练的患者肌体肌力不具备支撑肢体及康复器械的能力，需要康复器械施加一定的力矩辅助患者运动。相比于被动康复训练，主动康复训练被认为对肢体运动功能恢复更为有效。为了实现主动康复训练就需要判断患者的运动意向，大多采用肌电信号来实现判断人体主动运动意向的，但由于每个人的实际情况不同，所产生的肌电信号强弱不一致，肌电信号的干扰因素很多，所以在实际过程中要经过反复检测才能使用，增加了检测成本。

现阶段，本领域相关技术人员已经做了一些研究，如申请号为201510895139.1的专利记载了根据患者的关节力矩作为判断患者运动意图的方式，用力来判断患者运动意图的方式比肌电信号可靠，但是采集患者关节力矩值的方式不仅要求患者的上肢关节完全与康复机器人的关节重合，而且要求其运动角度、角速度和角加速度均与外骨骼康复机器人一致，这些要求并不符合实际康复训练情况，偏瘫患者在上肢外骨骼机器人上做康复训练时，上肢各关节或多或少偏离外骨骼机器人的关节，此种情况采集到的患者关节的力矩值并不准确。

技术实现要素：

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于交互力识别运动意图的欠驱动康复机器人的控制方法，其基于欠驱动康复机器人的工作特点，针对基于交互力识别运动意图的欠驱动康复机器人的控制方法进行了设计。所述控制方法能够借助六维力传感器较容易的实时判断出患者的运动意图，并将运动意图转化为欠驱动康复机器人的驱动电机所应做出的响应，实现了以患者意图主导的主动康复训练，降低了结构复杂程度，减小了体积，安全可靠，有利于欠驱动康复机器人的应用。此外，所述控制方法通过对速度矢量系数的改变可以调节欠驱动康复机器人助力的大小，使用方便，灵活性较高。

为实现上述目的，本发明提供了一种基于交互力识别运动意图的欠驱动康复机器人的控制方法，其包括以下步骤：

(1)提供一个欠驱动康复机器人，并确定所述欠驱动康复机器人的驱动电机数量及驱动角度矢量，所述欠驱动康复机器人的手柄部位设置有六维力传感器；

(2)根据所述欠驱动康复机器人的关节数量确定运动关节角度矢量；

(3)根据所述欠驱动康复机器人的关节初始角度确定初始关节角度矢量；

(4)根据所述欠驱动康复机器人的结构耦合情况计算出传动矩阵；

(5)根据所述驱动角度矢量、所述运动关节角度矢量、初始关节角度矢量及所述传动矩阵求得驱动电机与关节转角之间的关系式；

(6)根据运动关节角度矢量确定所述欠驱动康复机器人的刚体雅可比矩阵；

(7)根据所述传动矩阵及所述雅可比矩阵确定所述欠驱动康复机器人末端的速度映射矩阵；

(8)根据所述速度映射矩阵的列矢量构建出驱动电机的驱动空间到所述欠驱动康复机器人操作空间的投影矩阵；

(9)根据所述六维力传感器测量出的数据计算出患者的手施加在所述欠驱动康复机器人的手柄处的力矢量；

(10)将所述力矢量转化为患者渴望的速度矢量；

(11)将所述速度矢量左乘所述投影矩阵以得到所述驱动电机的转速矢量，即得到所需控制的电机转速。

进一步的，所述欠驱动康复机器人为上肢外骨骼康复机器人，其驱动电机的数量为两个。

进一步的，所述欠驱动康复机器人的关节数量为五个；所述初始关节角度矢量其中，分别表示五个关节的初始转动角度。

进一步的，所述欠驱动康复机器人包括两个耦合部分，两个所述耦合部分分别为肩关节耦合及肩肘关节耦合。

进一步的，所述传动矩阵为各个关节的传动比构成的：式中，¹μ₁、¹μ₂、²μ₁、²μ₂、²μ₃分别为各个关节的传动比。

进一步的，所述六维力传感器可采集施加在其表面的力和力矩值，并以下式输出：

F_s＝[τ_x,τ_y,τ_z,f_x,f_y,f_z]^T，式中，τ为力矩，f为力，x,y,z的方向分别与六维力传感器坐标系的三个坐标轴x_f，y_f，z_f同向，所述六维力传感器坐标系的坐标原点位于所述六维力传感器的几何中心。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，本发明提供的基于交互力识别运动意图的欠驱动康复机器人的控制方法，其能够借助六维力传感器较容易的实时判断出患者的运动意图，并将运动意图转化为欠驱动康复机器人的驱动电机所应做出的响应，实现了以患者意图主导的主动康复训练，降低了结构复杂程度，减小了体积，安全可靠，有利于欠驱动康复机器人的应用。此外，所述控制方法通过对速度矢量系数的改变可以调节欠驱动康复机器人助力的大小，使用方便，灵活性较高。

附图说明

图1是发明较佳实施方式提供的基于交互力识别运动意图的欠驱动康复机器人的控制方法的流程图。

图2是图1中的基于交互力识别运动意图的欠驱动康复机器人的控制方法涉及的欠驱动康复机器人的结构示意图。

图3是图2中的欠驱动康复机器人的局部坐标设置示意图。

图4是图2中的欠驱动康复机器人的另一局部坐标设置示意图。

在所有附图中，相同的附图标记用来表示相同的元件或结构，其中：1-第一电机，2-第一驱动盘，3-第二电机，4-第二驱动盘，5-第一肩部从动盘，6-第一肘部从动盘，7-六维力传感器，8-第一绳索，9-第二绳索，10-第二肩部从动盘，11-第三肩部从动盘，12-第三绳索，13-第四绳索，14-第四肩部从动盘，15-第五肩部从动盘，16-第二肘部从动盘，17-腕部弧形盘，18-手柄。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

请参阅图1至图4，本发明较佳实施方式提供的基于交互力识别运动意图的欠驱动康复机器人的控制方法，所述控制方法借助六维力传感器来达到稳定识别患者的运动意图，进而完成欠驱动康复机器人带着患者主动康复训练的目的。

本发明提供的基于交互力识别运动意图的欠驱动康复机器人的控制方法主要包括以下步骤：

步骤一，提供一个欠驱动康复机器人，并确定所述欠驱动康复机器人的驱动电机数量及驱动角度矢量，所述欠驱动康复机器人的手柄部位安装有六维力传感器7。本实施方式中，所述欠驱动康复机器人为上肢外骨骼康复机器人。

具体地，所述欠驱动康复机器人的关节数量大于自由度数量，其操作空间不是一个自由空间。所述欠驱动康复机器人包括电机组件、绳索组件及上肢外骨骼组件，所述电机组件包括第一电机1、第一驱动盘2、第二电电机3及第二驱动盘4。所述绳索组件包括第一绳索8、第二绳索9、第三绳索12及第四绳索13。所述上肢外骨骼组件包括第一肩部从动盘5、第二肩部从动盘10、第三肩部从动盘11、第四肩部从动盘14、第五肩部从动盘15、第一肘部从动盘6、第二肘部从动盘16、腕部弧形盘17、六维力传感器7及手柄18。所述六维力传感器7连接于所述手柄18，其用于测量患者作用于所述手柄18的力。

所述第一电机1连接于所述第一驱动盘2，所述第二肩部从动盘10连接于所述第三肩部从动盘11，所述第一绳索8及所述第二绳索9连接所述第一驱动盘2及所述第二肩部从动盘10。所述第四肩部从动盘14连接于所述第三肩部从动盘11，所述第五肩部从动盘15连接于所述第四肩部从动盘14，所述第二肘部从动盘16连接于所述第五肩部从动盘15，所述腕部弧形盘17连接于所述第二肘部从动盘16，所述六维力传感器7连接所述腕部弧形盘17及所述手柄18。所述第二电机3连接于所述第二驱动盘4，所述第三绳索12及所述第四绳索13连接所述第二驱动盘4及所述第五肩部从动盘15。

所述欠驱动康复机器人由两个耦合部分组成，第一耦合部分为肩关节耦合，第二耦合部分为肩肘关节耦合。当所述欠驱动康复机器人的肩关节部分运动时，所述第一电机1驱动所述第一驱动盘2运动，所述第一驱动盘2通过所述第一绳索8及所述第二绳索9驱动所述第二肩部从动盘10运动，以完成肩部内旋的动作。同时，由于所述第三肩部从动盘11固定不动，连接所述第三肩部从动盘11及所述第四肩部从动盘14的绳索会带动所述第四肩部从动盘14转动以完成肩部外展的动作，即所述第一耦合部分将肩部内旋及肩部外展两个自由度耦合，采用所述第一电机1来控制。

当所述欠驱动康复机器人的肘关节运动时，所述第二电机3驱动所述第二驱动盘4运动，所述第二驱动盘4通过第三绳索12及所述第四绳索13来驱动所述第五肩部从动盘15转动以完成肩部前伸的动作；此时，由于所述第一肩部从动盘5固定不动，连接所述第一肩部从动盘5及所述第一肘部从动盘6的绳索会带动所述第一肘部从动盘6转动以完成肘部屈肘的动作；同时，所述第二肘部从动盘16与机械臂固定连接，在肘部屈肘时，连接所述第二肘部从动盘16及所述腕部弧形盘17之间的绳索会带动所述腕部弧形盘17转动，以完成腕部翻转的动作，即第二耦合部分将肩部前伸、肘部屈肘及腕部翻转三个自由度耦合，采用所述第二电机3来控制。

建立空间坐标系{s}及手柄坐标系{h}，所述空间坐标系在空间固定，其与机械臂的上端的位置关系相对固定。所述空间坐标系的三个坐标轴分别为x_s，y_s，z_s。所述手柄坐标系的原点固定在所述手柄的几何中心，其三个坐标轴分别为x_h，y_h，z_h。ω₁，ω₂，ω₃，ω₄，ω₅分别为所述第二肩部从动盘10、所述第四肩部从动盘14、所述肩部从动盘15、所述第二肘部从动盘16和所述腕部弧形盘17的转轴方向，其中ω₁，ω₂，ω₃两两垂直且交于空间一点，ω₄与ω₃平行且与ω₁相交；位于初始位置时，ω₅与ω₁共线。

此外，建立六维力传感器坐标系{f}，所述六维力传感器坐标系的原点固定在所述六维力传感器7的几何中心，x_f，y_f，z_f为所述六维力传感器的三个坐标轴。

所述欠驱动康复机器人的驱动电机的数量为两个，分别是所述第一电机1及所述第二电机3；分别用u₁，u₂来分别表示所述第一电机1及所述第二电机3的角位移，则驱动角度矢量为u＝(u₁,u₂)^T。

步骤二，根据所述欠驱动康复机器人的关节数量确定运动关节角度矢量。具体地，所述欠驱动康复机器人的关节数量为5个，用θ₁，θ₂，θ₃，θ₄，θ₅分别表示ω₁，ω₂，ω₃，ω₄，ω₅所对应关节的转动角度，运动关节角度矢量为θ＝(θ₁,θ₂,θ₃,θ₄,θ₅)^T。

步骤三，根据欠驱动康复机器人的关节初始角度确定初始关节角度矢量。具体地，用分别表示ω₁，ω₂，ω₃，ω₄，ω₅所对应关节上的初始转动角度，初始关节角度矢量初始关节角度根据使患者上肢佩戴所述欠驱动康复机器人时舒适的角度确定，根据所述欠驱动康复机器人确定如下角度，

步骤四，根据所述欠驱动康复机器人的结构耦合情况计算出传动矩阵。具体地，根据所述欠驱动康复机器人的两个耦合部分，可得传动矩阵如下：

传动比

各个盘半径¹r₂＝40，¹r₁₀＝50¹r₁₁＝66，¹r₁₄＝75²r₄＝40，²r₁₅＝50²r₅＝74，²r₆＝56，²r₁₆＝37，²r₁₇＝74，单位为毫米，将上述参数带入可得传动矩阵：

步骤五，根据所述驱动角度矢量、所述运动关节角度矢量、初始关节角度矢量及所述传动矩阵获得驱动电机转角与关节转角之间的关系式。具体地，驱动电机转角与关节转角之间的关系式确定为展开如下:

步骤六，根据运动关节角度矢量确定所述欠驱动康复机器人的刚体雅可比矩阵。具体地，对于纯转动关节，由每个角速度方向的单位矢量ω可以算出该关节的运动旋量：

其中，q为末端转动坐标系的原点到关节旋转轴线的向量，对于本欠驱动康复机器人来说为手柄坐标系原点到关节旋转轴线的向量，

若则

由运动旋量B₁,B₂,B₃,B₄,B₅和关节角位移θ₁，θ₂，θ₃，θ₄，θ₅可以求得刚体的雅可比矩阵J＝[J₁ J₂ J₃ J₄ J₅]；

其中，

J₅＝B₅。

由刚体螺旋运动公式可知

式中，对于纯转动关节，h为0

若

步骤七，根据所述传动矩阵及所述雅可比矩阵确定所述欠驱动康复机器人末端的速度映射矩阵。具体地，所述欠驱动康复机器人末端的速度映射矩阵为Γ＝Jη。

步骤八，根据所述速度映射矩阵的列矢量构建出驱动电机的驱动空间到所述欠驱动康复机器人操作空间的投影矩阵。具体地，Riemannian矩阵由公式可知α和β分别是角速度和线速度对投影速度的影响因子。

对于所述欠驱动康复机器人α取0.3，β取1，使得所述欠驱动康复机器人对线速度更敏感，得Riemannian矩阵G为

进而求得投影矩阵P为

步骤九，根据所述六维力传感器7测量出的数据计算出患者的手施加在所述欠驱动康复机器人的手柄处的力矢量。具体地，所述六维力传感器7可采集施加在其表面的力和力矩值，并以如下形式输出：

F_s＝[τ_x,τ_y,τ_z,f_x,f_y,f_z]^T，τ为力矩，f为力，x,y,z方向分别与坐标轴x_f，y_f，z_f相同。

将六维力传感器坐标系中的力F_s变换到手柄坐标系F_h中

其中，

R_hs为手柄坐标系到六维力传感器坐标系的旋转矩阵，p_hs为手柄坐标系原点到六维力传感器坐标系原点的向量，R为旋转矩阵。

步骤十，将所述力矢量转化为患者所渴望的速度矢量。具体地，将力矢量转化为速度矢量可利用公式V^d＝ACF，其中V^d是主动训练时患者所渴望的速度，F是患者手施加在手柄上的力，AC是导纳矩阵，为了不改变力的方向，导纳矩阵为对角矩阵。

A＝λI_6×6，为了放慢速度，λ取0.05。

其中α和β分别是对所采集的力矩和力的放大倍数，α＝10，β＝1，使其将力矩放大。

步骤十一，将所述速度矢量左乘所述投影矩阵以得到所述驱动电机的转速矢量，即得到所需控制的电机转速。具体地，将步骤十中所获得的速度V^d投影到机器人的操作空间的平面上其中P是步骤八中所获得的投影矩阵，是驱动电机的转速矢量，和分别是所述第一电机1及所述第二电机3的转速。

经过如上步骤就建立起了患者施加于手柄的力和电机转速的关系，也就是患者运动意图和机器运动的关系。

本发明提供的基于交互力识别运动意图的欠驱动康复机器人的控制方法，其能够借助六维力传感器较容易的实时判断出患者的运动意图，并将运动意图转化为欠驱动康复机器人的驱动电机所应做出的响应，实现了以患者意图主导的主动康复训练，降低了结构复杂程度，减小了体积，安全可靠，有利于欠驱动康复机器人的应用。此外，所述控制方法通过对速度矢量系数的改变可以调节欠驱动康复机器人助力的大小，使用方便，灵活性较高。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。