三维上肢康复训练仪重力补偿方法及控制系统与流程

本发明涉及重力补偿，具体涉及一种三维上肢康复训练仪重力补偿方法及控制系统。

背景技术：

1、对于一些由于脑损伤或神经性疾病引起的上肢活动关节功能障碍的患者，康复训练是消除或者缓解上肢障碍一个重要的环节，基于康复医学理论，通过对患肢的反复运动训练，辅助患肢失去的功能逐渐恢复。三维上肢康复训练仪实时模拟人体上肢运动规律，能够在多个维度实现上肢的运动，而且结合情景互动、训练反馈信息和强大的评估系统后，患者可以在完全零肌力下进行康复训练，提早了患者康复训练的进程。

2、三维上肢康复训练仪包括机械臂和驱动组件，机械臂包括前臂和后臂，前臂和后臂之间活动连接，机械臂通过后臂的后端安装于驱动组件上，驱动组件可以驱动后臂上下、左右运动以及前臂相对于后臂的转动，共三个串联关节的运动，每个关节都有对应的扭矩传感器和角度传感器。康复者的手臂固定在前臂的末端进行康复训练，由于不同角度位置下，由于重力作用（仅考虑静态或低速运动的情况），机械臂三个关节自身的重力会对扭矩传感器采集的数据值有影响，不能完全反应前臂末端作用力的情况，这样就不能通过作用力进行机械臂的运动控制，因此为了准确反应末端作用力，需要进行重力补偿。

3、申请号为202210064467.7的中国专利公开了一种上肢康复机器人重力补偿方法，首先获取目标位置，并通过目标位置参数计算得到各个关节的目标关节角度；再由所述目标关节角度及与所述目标关节角度对应的实际关节角度、目标关节角速度、实际关节角速度计算得到各个关节所需的手臂支撑力矩；最后由所述各个关节的目标运动参数和实际运动参数计算得到所述各个关节的输出力矩，以及根据所述各个关节的输出力矩计算得到所述各个关节的驱动量；进而根据驱动量控制待训练关节施加辅助力矩，以进行康复训练。

4、上述方法在计算目标关节角度时需要对上肢康复机器人建立dh坐标系，确定末端齐次变换矩阵、线速度雅可比矩阵，设置计算收敛阈值和迭代次数阈值，并依次循环计算以下参数：位置误差、关节角度误差、关节角度、循环次数加一；需要由目标关节角度及与所述目标关节角度对应的实际关节角度、目标关节角速度、实际关节角速度基于迭代学习的方法计算得到所述各个关节所需的手臂支撑力矩；另外，还要通过一系列的公式计算重力矩误差、各个关节的驱动量等。因而上述方法的建模计算较为复杂，会大大增加工程量，尤其对多个关节的运动计算量较大，而且每个数据提取过程和数据计算过程都会产生误差，同时重力补偿后会引入新的误差，众多误差积累到一起会导致最终的重力补偿不准确，从而导致难以实现理想的重力补偿效果，进而影响康复训练的运动精度。

技术实现思路

1、本发明针对背景技术中的不足，提供一种三维上肢康复训练仪重力补偿方法及控制系统，针对多个关节的运动，建模计算较为简单，适合复杂的运动结构，计算量较小。

2、为解决以上技术问题，本发明采用以下技术方案：

3、一种三维上肢康复训练仪重力补偿方法，包括如下步骤：

4、步骤1，对三个关节全姿态空间进行位置扫描，并记录各关节在不同角度下的关节角和重力矩，进行重力标定，最后存储到对应的数据存储单元；

5、步骤2，通过对机械臂的位置实时测量得到实时角度，然后从对应的数据存储单元提取重力矩；

6、步骤3，分别测量机械臂三个关节的实时合力矩，并根据步骤2中提取的重力矩进行重力补偿运算。

7、进一步地，在步骤3中，所述重力补偿运算的运算公式如下：

8、，其中，、、为三个关节分别受到的外力矩，康复者的手臂对机械臂的前臂末端施加作用力，所述外力矩为所述作用力传递到各个关节上的力，、、为三个关节从其位置所对应的数据存储单元提取的重力矩，、、为三个关节分别实测的实时合力矩，所述合力矩为关节在承受到外力矩和重力矩后的力矩。

9、进一步地，在步骤1进行重力标定时，采用每隔5°进行一次标定采样。

10、进一步地，所述三个关节分别为关节一、关节二和关节三，所述机械臂包括前臂和后臂；

11、所述关节一通过第一驱动组件驱动，实现机械臂的上下摆动，所述关节二通过第二驱动组件驱动，实现机械臂的左右转动；所述关节三通过第三驱动组件驱动，实现前臂的后端相对于后臂的前端转动。

12、进一步地，在步骤1中，所述位置扫描的方法如下，

13、先将各个关节回归至初始角度位置；

14、然后关节一从初始角度开始，每隔5°逐步改变角度位置，每个角度位置扫描所对应的重力矩；关节一的每个角度位置会对应关节二和关节三不同角度位置的多个重力矩；

15、在扫描关节一的每个角度位置时，先将关节二和关节三回归到初始角度位置，然后关节三每隔5°逐步改变角度位置，直至关节三的角度位置超过终止角度位置时，关节二每隔5°逐步改变角度位置，在关节二的每个位置，关节三均从初始角度位置运行到终止角度位置，直至关节二的角度位置超过终止角度位置时，关节一每隔5°逐步改变角度位置，直至关节一的角度位置到达终止角度位置。

16、进一步地，在步骤2中，提取重力矩后，根据提取的重力矩计算实时角度所对应的重力矩的方法如下：

17、假设实时角度x在标定角度x0和x1之间，则计算公式如下：

18、

19、其中：、、分别为角度x、x0和x1下的重力矩。

20、进一步地，每个关节处均设有扭矩传感器和角度传感器，在步骤1中进行位置扫描时，通过角度传感器和扭矩传感器分别记录各个关节在不同角度对应的扭矩；在步骤2中通过角度传感器测得实时角度；在步骤3中扭矩传感器测得机械臂三个关节的实时合力矩。

21、还包括一种三维上肢康复训练仪重力补偿控制系统，

22、包括控制器、角度传感器和扭矩传感器、位置扫描模块、数据存储单元和实时测量模块。

23、控制器，所述控制器包括插值计算模块和补偿运算模块，所述插值计算模块提取数据存储单元内所对应的重力矩并计算出实时角度的重力矩；所述补偿运算模块获得实时角度并根据实时角度的重力矩和每个关节的实时合力矩计算出三个关节分别受到的外力矩，从而进行重力补偿；

24、所述位置扫描模块用于对每个关节全姿态空间进行位置扫描，并记录各关节在不同角度下的关节角和重力矩，进行重力标定；

25、所述数据存储单元用于存储重力标定时对应关节角的重力矩，以供控制器提取；

26、所述实时测量模块根据机械臂的位置实时测量得到实时角度，以供控制器提取；

27、所述角度传感器和扭矩传感器分别记录各个关节在每个位置的角度和扭矩。

28、本发明采用以上技术方案后，与现有技术相比，具有以下优点：

29、1.重力标定可以是在出厂时一次标定好，并存储到对应的数据存储单元，不需要每次都进行标定和计算，以后便可以随时提取已标定好的重力矩。

30、2.所述重力补偿方法实现复杂力学环境下机械臂对重力的补偿，拟采用重力标定的方式，对先对各机构在重力影响下的重力进行扫描标定，然后在从存储数据中匹配出实时角度下的重力补偿，是将三个关节分别实测的实时合力矩减去三个关节从其位置所对应的数据存储单元提取的重力矩从而得到三个关节分别受到的外力矩，这种运算模式数学构造模型简单，计算量较小，仅需要通过角度传感器和扭矩传感器检测出所处位置的角度和各关节的实时合力矩，并提取提取出该位置重力矩，然后实时合力矩减去位置的重力矩就是该位置重力补偿后的外力矩。

31、下面结合附图和实施例对本发明进行详细说明。