CN109159120B - 基于康复机械臂关节电机电流反馈的主动控制方法及系统 - Google Patents

Abstract

本发明提供了一种基于康复机械臂关节电机电流反馈的主动控制方法及系统，涉及上肢关节主动康复领域。包括以下过程：步骤1、建立康复机械臂的运动学模型和动力学模型：步骤2、计算因外力而引起的关节电流变化：步骤3、根据电流变化主动控制电机运动。本发明能够对病患输入到康复机械臂输入的力进行检测，但不需要力传感器，利用电机电流反馈代替力传感器检测，通过实时编码器反馈代入动力学方程，减小运动中不同位姿下重力负载对电流的干扰影响，建立反馈电流和电机转速的运动关系，从而实现康复机械臂的主动控制。

Description

基于康复机械臂关节电机电流反馈的主动控制方法及系统

技术领域

本发明属于上肢关节康复技术领域，具体涉及一种基于康复机械臂关节电机电流反馈的主动控制方法及系统。

背景技术

康复医学的临床研究表明，偏瘫患者的中枢神经系统可以通过各种方式来连通相关的突触链，进行重组，让患侧功能恢复。主动式训练(即有患者运动意图主动参与的康复训练)对于患者神经系统重建和运动功能恢复更加有效。由此需要完成两个主要任务,一个是需要精确采集患者的运动意图，以便有效驱动机器人按照患者的意愿运动；另一个，则是需要根据患者病情设计有效的康复运动动作。

目前广泛使用的基于力传感器来获取病患的运动意图，力有大小和方向，在轨迹控制上有天然的优势，但力传感器昂贵且安装调试不便，以及温漂、识别精度还有待提高；肌电和脑电识别患者运动意图的方法也获得广泛研究，但在识别准确率、识别模型的鲁棒性方面还需进一步提高。

人体肩部关节的运动比较复杂，各关节既有单独运动，又有相互间的协同运动，肩关节的各关节在运动时形成一个完整的统一体。肩关节的运动类似于球关节，自由度较多。为了方便研究一般将肩关节运动简化分解为3个基本的正交运动，即绕额状轴做屈/伸运动，绕矢状轴做外展/内收运动，绕垂直轴做内旋/外旋运动，其在运动时都要依靠肩关节周围的韧带和肌肉之间互相收缩实现。

病患在做肩关节康复动作时，一般较为柔和缓慢，以很低的速度和加速度做重复运动，这对电机来说要以很小的加速度来运动，减少了过大的加速度对关节电流的扰动，减小控制上的难度并且可以提高控制精度。

肩关节康复可以由屈曲伸展、外展内收、内旋外旋三个基本动作完成，根据三自由度康复机械臂的特殊构型，这三个动作是分别控制肩关节一个自由度运动；每个动作可以由一个关节完成，再结合相应的肩关节康复动作，即可达到康复机械臂主动康复的效果。

因此，本发明使用一种基于关节电流反馈的主动控制方法，结合肩关节康复的三组基本动作，实现对病患肩关节的主动康复训练。

发明内容

本发明提供了一种基于康复机械臂关节电机电流反馈的主动控制方法，以电流反馈替代力传感器反馈，实现康复机械臂的主动控制。

本发明公开了一种基于康复机械臂关节电机电流反馈的主动控制方法，所述康复机械臂包括转动关节、各转动关节之间的连杆和驱动转动关节的电机，利用关节电流反馈来检测电机输入力的变化，达到主动控制的效果，包括以下步骤：

步骤1：建立康复机械臂的运动学模型和动力学模型；

步骤2：计算因外力而引起的关节电流变化；

步骤3：根据电流变化控制电机运动。

所述步骤1具体操作如下：

S1-1：选定基础坐标系，根据各连杆的旋转方向，建立连杆坐标系，形成DH参数表；

S1-2：根据连杆坐标系，得到相互连杆坐标系间的变换矩阵；根据相互连杆坐标系间的变换矩阵和DH参数表，得到末端连杆坐标系到基础坐标系的变换矩阵，该末端连杆坐标系到基础坐标系的变换矩阵相乘得到机械臂运动学正解；

S1-3：通过建模和所述康复机械臂进行仿真分析，得到三自由度康复臂的动力学方程：

式中，τ为力矩，D表示惯性矩阵，对角元素表示连杆本身的惯性大小，非对角元素则表示各连杆之间的耦合惯性大小，H表示表示离心力与哥氏力之和的系数矩阵参数，G表示为重力矩参数，q、

和

表示为关节的角度、角速度、和角加速度；

所述步骤2具体操作如下：

S2-1：根据编码器得到当前关节位置信息，得到因机械臂当前位姿而引起的力矩τ_q；

S2-2：根据电机转矩常数k和公式τ＝i*k，计算因力矩τ_q而引起的关节电流i_q；读取当前关节电流i，计算出当前关节力矩τ_now；

S2-3：根据因力矩τ_q而引起的关节电流i_q、当前关节电流i和公式i_c＝i-i_q，计算出因外力引起的关节电流变化i_c。

所述步骤3具体操作如下：

S3-1：设置电机运动的最小变化电流i_min、最大运行速度v_max和加速度a；

S3-2：计算电机运动速度v和变化电流i_c的对应关系为f；

S3-3：当电机运动按照设定的轨迹方向，当i_c大于i_min时，电机以加速度a运动，达到速度v后匀速，最大速度为v_max；当i_c小于i_m时，电机以加速度a减速到0速度；当电机转动方向与设定的轨迹方向不一致时，电机的立即写入0速度。

本发明还公开了一种基于康复机械臂关节电机电流反馈的主动控制系统，包括康复机械臂，所述康复机械臂包括转动关节、各转动关节之间的连杆和驱动转动关节的电机，所述各转动关节之间的连杆相互垂直，且一处连杆为可伸缩连杆，所述电机由上述基于康复机械臂关节电机电流反馈的主动控制方法控制运行。

有益效果：本发明与现有技术相比，具有以下优点：本发明能够对病患输入到康复机械臂输入的力进行检测，但不需要力传感器，利用电机电流反馈代替力传感器检测，通过实时编码器反馈代入动力学方程，减小运动中不同位姿下重力负载对电流的干扰影响，建立反馈电流和电机转速的运动关系，从而实现康复机械臂的主动控制。病患通过自身肩部发力，带动康复臂运动，而康复臂按照设计的康复动作运动，从而完成病患自发的进行康复训练，从而达到良好的主动康复效果。

附图说明

图1是本发明的三自由度机械臂构型；

图2是三自由度康复机械臂穿戴效果图；

图3是基于电流反馈的主动控制方法的系统框图；

图4是基于电流反馈的主动控制方法的系统流程图。

具体实施方式

下面结合附图和实施例进一步阐述本发明。

本实施例使用一种三自由度肩关节康复机械臂作为实验平台，实验平台构型见图1，q1、q2、q3为转动关节，含有一个可调节连杆长度的机构，图中标示为move，q3的电机没有标出。

穿戴效果图如图2所示，将病患的大臂固定于q3关节处，当做屈/伸动作时，只需要控制q2关节即可；当做绕矢状轴做外展/内收运动，控制q1电机；当做绕垂直轴做内旋/外旋运动时，控制q3电机。

设计流程框图如图3所示，流程图如图4所示，本发明的主动控制方法可以利用关节电流反馈达到主动康复的效果，利用电流反馈检测输入力的变化，应用于上肢关节康复领域。

具体包括以下步骤：

S1、选定基础坐标系，根据各连杆的旋转方向，建立康复机械臂的各连杆坐标系，建立DH参数表，如表1所示；

表1 DH参数表

S2、根据所建立的连杆坐标系，得到相互连杆坐标系间的变换矩阵；

其中c代表cos()函数，同理s代表sin()函数，α代表连杆转角度，a代表连杆长度，d代表连杆偏移，θ代表关节变量，

代表从n-1关节到n关节的变换矩阵，例如

表示关节1到关节2的变换矩阵。

S3、根据相互连杆坐标系间的变换矩阵，代入DH参数，得到末端连杆坐标系到基础坐标系的变换矩阵

相乘得到机械臂运动学正解

S4、通过ADAMS软件三维精确建模和UG软件对三自由度康复机械臂进行仿真分析，验证运动学模型的可行性，然后对其进行动力学的研究，采用拉格朗日法建立上肢康复机器人的动力学方程，并通过ADAMS对其进行动力学仿真分析，得到动力学方程的关键参数，包括惯性矩阵D，离心力与哥氏力之和的系数矩阵参数矩阵参数H，重力矩参数G，得到机器人系统的动力学计算公式的一般表达式为；

式中，τ为力矩，D表示惯性矩阵，对角元素表示连杆本身的惯性大小，非对角元素则表示各连杆之间的耦合惯性大小，H表示表示离心力与哥氏力之和的系数矩阵参数，前者与关节自身的运动速度有关，后者则与两个关节的运动速度有关。G表示为重力矩参数，q、

和

表示为关节的角度、角速度、和角加速度。

S5、根据编码器反馈得到当前位置,代入动力学方程得到因机械臂当前位姿而引起的力矩τ_q，该力矩是电机为了反抗自身重力而产生的力矩；

S6、根据电机转矩常数k(单位电流产生的单位力矩)，计算因力矩τ_q而引起的关节电流i_q,计算公式为

S7、读取当前关节电流i，根据电机转矩常数k，计算出当前关节力矩τ_now；

S8、根据公式计算出因外力引起的关节电流变化i_c，计算公式为i_c＝i-i_q，即

S9、康复运动时，因为肩关节康复动作需要尽量平缓和柔和，配置电机的最大运行速度v_max，加速度为a；为确保康复运动的安全性，电机在加速以及减速阶段会引起电流波动，以及防止轻微的晃动造成机械臂的自运动，设置电机运动的最小变化电流为i_min，防止发生意外；

S10、病患在做康复运动时，病患主动施加力F引起电机关节力矩变化，力矩的变化引起电流的变化，设置速度(rpm)和电流(ma)的对应关系为v＝f(i)；

根据实物调试以及期望的对应关系确定f，可以设置为线性或非线性关系。当电机变化电流i_c大于i_min时，电机将以速度v运动，直到达到v_max时保持匀速；当电机变化电流i_c小于i_min时，设置电机速度为零，电机将以加速度-a进行减速；最终得到的速度计算公式为；

S11、电机运动按照预定的康复运动轨迹，对于屈曲伸展、外展内收、内旋外旋这三个基本动作，只需要控制一个电机；当电机转动方向和轨迹的终点位置方向一致时，电机按照S10的方式运动；当电机转动方向和轨迹的终点位置方向不一致时，电机的立即写入0速度。

Claims (2)

1.一种基于康复机械臂关节电机电流反馈的主动控制方法，其特征在于：所述康复机械臂包括转动关节、各转动关节之间的连杆和驱动转动关节的电机，利用关节电流反馈来检测电机输入力的变化，达到主动控制的效果，包括以下步骤：

步骤1：建立康复机械臂的运动学模型和动力学模型；具体操作如下：

S1-1：选定基础坐标系，根据各连杆的旋转方向，建立连杆坐标系，形成DH参数表；

S1-2：根据连杆坐标系，得到相互连杆坐标系间的变换矩阵：

其中c代表cos()函数，同理s代表sin()函数，α代表连杆转角度，a代表连杆长度，d代表连杆偏移，θ代表关节变量，

代表从n-1关节到n关节的变换矩阵；

根据相互连杆坐标系间的变换矩阵和DH参数表，得到末端连杆坐标系到基础坐标系的变换矩阵，该末端连杆坐标系到基础坐标系的变换矩阵相乘得到机械臂运动学正解；

S1-3：通过建模和所述康复机械臂进行仿真分析，得到三自由度康复臂的动力学方程：

式中，τ为力矩，D表示惯性矩阵，对角元素表示连杆本身的惯性大小，非对角元素则表示各连杆之间的耦合惯性大小，H表示离心力与哥氏力之和的系数矩阵参数，G表示为重力矩参数，q、

和

表示为关节的角度、角速度、和角加速度；

步骤2：计算因外力而引起的关节电流变化；具体操作如下：

S2-1：根据编码器得到当前关节位置信息，得到因机械臂当前位姿而引起的力矩τ_q；

S2-2：根据电机转矩常数k和公式τ＝i*k，计算因力矩τ_q而引起的关节电流i_q：

读取当前关节电流i，计算出当前关节力矩τ_now；

S2-3：根据因力矩τ_q而引起的关节电流i_q、当前关节电流i和公式i_c＝i-i_q，计算出因外力引起的关节电流变化i_c：

步骤3：根据电流变化控制电机运动；具体操作如下：

S3-1：设置电机运动的最小变化电流i_min、最大运行速度v_max和加速度a；

S3-2：计算电机运动速度v和变化电流i_c的对应关系为f；

S3-3：当电机运动按照设定的轨迹方向，当i_c大于i_min时，电机以加速度a运动，达到速度v后匀速，最大速度为v_max；当i_c小于i_m时，电机以加速度a减速到0速度；当电机转动方向与设定的轨迹方向不一致时，电机的立即写入0速度。

2.一种基于康复机械臂关节电机电流反馈的主动控制系统，包括康复机械臂，所述康复机械臂包括转动关节、各转动关节之间的连杆和驱动转动关节的电机，所述各转动关节之间的连杆相互垂直，且一处连杆为可伸缩连杆，所述电机由权利要求1所述的一种基于康复机械臂关节电机电流反馈的主动控制方法控制运行。

Images