CN113509352A - 一种绳索康复机器人运动学参数标定和解算方法及系统 - Google Patents

Abstract

本发明提供了一种绳索康复机器人运动学参数标定和解算方法及系统。本发明在一种绳索牵引上肢康复机器人的基础上，利用机器人自带的编码器和IMU(惯性测量单元)两种传感器实现，首先在使用绳索康复机器人进行康复训练前对患者的运动学参数进行标定，再在使用绳索康复机器人进行康复训练过程中根据患者的手臂姿态实时解算出当前肩关节中心位置。本发明仅使用编码器和IMU两类传感器即可有效标定绳索机器人的运动学参数，并在康复训练过程中实时解算相关参数，提高了运动学模型的准确性和控制精度，成本低，安装与操作简单。

Description

一种绳索康复机器人运动学参数标定和解算方法及系统

技术领域

本发明属于康复机器人技术领域，涉及一种基于编码器和IMU的绳索康复机器人运动学参数标定和解算方法及系统。

背景技术

由于社会老龄化的加剧，我国脑卒中的发病率不断上升，且大部分脑卒中患者在术后会遗留功能障碍。偏瘫是脑卒中后常见残疾，其主要表现为患侧肌力减退、肢体痉挛以及活动受限，对患者日常生活造成严重影响。

研究表明，通过康复训练能够促进大脑损伤区域或冗余神经重新学习，恢复人体运动功能，近年来，机器人行业发展迅猛，使用机器人进行康复训练也愈发普及，相关研究也表明机器人康复训练具有较好的效果且能显著降低治疗成本。

上肢康复训练机器人经过多年发展，大体可以分为末端式和外骨骼式两种，末端牵引式机器人，通过与人手臂末端相固定，进而牵引手臂进行运动康复，这类机器人结构通常较为简单且不会引起训练过程中的关节错位问题，但是其工作范围有限且运动灵活性相对较差；外骨骼机器人通过与人上肢多个关节绑定来驱动手臂运动，相对末端机器人具有较好的灵活性和较大的工作空间，但是其结构通常比较复杂，并且，由于需要让机器人关节旋转中心与人体关节旋转中心相对准，增大了设计难度，在肩关节等非中心固定关节处，更是较难满足要求。

上述问题在传统的刚性机器人上多有发生。近年来，柔性外骨骼机器人得到了广泛的发展。与刚性机器人相比，柔性机器人使用绳索或人工肌肉模拟人体肌肉收缩，因而对人体的适应性更强，且结构更加简单轻便。同时由于柔性外骨骼机器人常将人体关节作为机器人结构的一部分，因此有效避免了传统外骨骼机器人关节轴对准和关节长度调整的问题。

而对于此类柔性外骨骼机器人来说，其柔性的结构通常会导致不确定的运动学参数，如穿戴时患者的肩关节中心位置及设备穿戴位置等运动学参数会相对于预设值发生偏移。同时考虑到人肩关节的运动不能简单理解为一个球状关节的运动，在实际的肩关节运动中，上臂的抬举动作会使得肩关节的旋转中心同步抬升，给机器人的运动学建模和运动控制带来影响。目前国内外的柔性外骨骼上肢康复机器人基本上将肩关节中心视为定点进行参数解算，会给上肢的运动学模型引入误差，因此研究柔性外骨骼机器人的运动学参数的标定和实时解算问题具有实际的意义以及较高的价值。

发明内容

为解决上述问题，本发明提供了一种基于编码器和IMU的绳索康复机器人运动学参数标定和解算方法及系统。本发明在一种绳索牵引上肢康复机器人的基础上，利用机器人自带的编码器和IMU(惯性测量单元)两种传感器实现，首先在使用绳索康复机器人进行康复训练前对患者的运动学参数进行标定，再在使用绳索康复机器人进行康复训练过程中根据患者的手臂姿态实时解算出当前肩关节中心位置。

为了达到上述目的，本发明提供如下技术方案：

一种绳索康复机器人运动学参数标定和解算方法，包括如下步骤：

步骤一，运动学参数标定

(1)首先取患者手臂处于某一姿态下的状态为初始状态，此时的绳长即为初始绳长，亦是待求参数，并记此时手臂的旋转矩阵为

即无相对旋转；

(2)患者进行手臂各种动作，记录患者运动过程中的编码器数据和IMU数据，即可得到绳长变化量Δl_i和上肢旋转矩阵

参数，患者手臂在任一位姿下的Δl_i和

为一组数据，采集n组数据；

(3)对算法迭代量赋初值，即对待求解参数初始化，初始值通过手动测量近似确定，分别赋值为

其中，

为肩关节中心相对于系统坐标原点的偏移值初始值；

为手臂处于某一姿态下的四根绳索初始绳长，其中i＝1,2,3,4,

为上臂环中心到肩关节中心的距离；

(4)进行迭代，算得迭代参数的修正值，对迭代参数进行修正；

(5)将第m次迭代后的迭代参数D^m与第m-1次的迭代参数D^m-1进行比较，当||D^m-D^m-1||_∞＜ε时，停止迭代，待求解参数的解即为当前迭代参数，为

否则返回步骤(4)，继续迭代；其中D^m是第m次迭代后的迭代参数，

ε是设定的误差限，m为迭代次数；

步骤二，运动学参数解算

对n组

和

进行多元线性回归

记标定过程中的数据

和

的关系系数

则BJ＝Z

根据最小二乘法可解得J

J＝(B^TB)^-1B^TY

在实际训练过程中，通过实时的手臂旋转矩阵

可算得患者肩关节中心偏移量

进一步的，所述步骤(4)中，具体迭代过程如下：

考虑任意其中一组数据，忽略下标j，则在该组数据中，第i根绳索的绳长相对于初始绳长的绳长差用如下方程表示

而l_i＝||^OS_i ^OU_i|| (2)

将式(1)代入式(2)，可得

假定肩关节中心坐标估计值

上臂环中心到肩关节中心的距离估计值值为

第i根绳索的初始绳长估计值为

在待求解参数附近进行泰勒展开，并取至一次项；则

将式(4)代入式(3)，展开后并写成

假设共采集了n组数据，则根据公式(5)可列出4n组方程，对应的待求解参数为3n组肩关节中心偏移[^OG_jx,^OG_jy,^OG_jz]^T，4组初始绳长

和1组上臂环中心到肩关节中心的距离

共3n+5个变量，其中，j＝1,2,...,n，i＝1,2,3,4；

因此至少需要采集5组数据方可解出方程，即n≥5：

记

其中

记δl_ij＝Δl_ij-Δl_0ij

记

记

则AX＝Y

根据最小二乘法有

X＝(A^TA)^-1A^TY (6)

最后对待求解参数估计值进行迭代

上述各式中，^OG_j＝[^OG_jx,^OG_jy,^OG_jz]^T为在坐标系{O}下第j组数据中肩关节中心坐标；

表示第j组数据中通过IMU测得的手臂的旋转矩阵；

为在坐标系{G}下第i根绳索在上臂环上的连接点坐标初始值；^OS_i为在坐标系{O}下第i根绳索在肩部支架的连接点坐标；^OU_ij为在坐标系{O}下第j组数据中第i根绳索在上臂环上的连接点坐标；l_ij为第j组数据中第i根绳索的长度，即||^OS_i ^OU_ij||；Δl_ij为第j组数据中第i根绳索长度相对于初始绳长的绳长差。

进一步的，所述步骤二进行解算时，参数

视为常量。

进一步的，所述步骤一中某一姿态指患者手臂自然下垂时的姿态。

进一步的，所述步骤一中各种动作包括手臂外展/内收、前屈/后伸、水平屈/伸动作。

本发明还提供了绳索康复机器人运动学参数标定和解算系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于：所述计算机程序被加载至处理器时用于实现绳索康复机器人运动学参数标定和解算方法。

进一步的，所述计算机程序包括运动学参数标定模块和运动学参数解算模块，运动学参数标定模块用于在使用绳索康复机器人进行康复训练前对患者的运动学参数进行标定；运动学参数解算模块用于在使用绳索康复机器人进行康复训练过程中根据患者的手臂姿态实时解算出当前肩关节中心位置。

与现有技术相比，本发明具有如下优点和有益效果：

本发明仅使用编码器和IMU两类传感器即可有效标定绳索机器人的运动学参数，并在康复训练过程中实时解算相关参数，提高了运动学模型的准确性和控制精度，成本低，安装与操作简单。

附图说明

图1为绳索上肢康复机器人示意图，图中，4根绳索(虚线表示)穿过肩部支架后连接到上臂环，上臂环与患者上肢结合形成一个完全约束的机械系统，可控制上肢肩关节3自由度运动。机器人绳索的长度通过编码器测量，上肢的3自由度旋转通过IMU测量；

图2为绳索上肢康复机器人标定及使用状态示意图，其中(a)为手臂的前屈/后伸动作，(b)为手臂的外展/内收动作，(c)为手臂的水平屈/伸动作。

图3为本发明方法流程示意图。

具体实施方式

以下将结合具体实施例对本发明提供的技术方案进行详细说明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

图1为绳索上肢康复机器人示意图，图中，4根绳索(虚线表示)穿过肩部支架后连接到上臂环，上臂环是一个穿戴在手臂上的套筒，上臂环与患者上肢结合形成一个完全约束的机械系统，可控制上肢肩关节3自由度运动。机器人绳索的长度通过编码器测量，上肢的3自由度旋转通过IMU测量。图2为绳索上肢康复机器人标定及使用状态示意图。

将绳索上肢康复机器人穿戴在患者手臂上，应用本发明提供的基于编码器和IMU的绳索康复机器人运动学参数标定和解算方法，如图3所示，本发明方法具体包括如下步骤：

(一)运动学参数标定

首先统一对本发明需要的参数进行说明如下：

下标i:第i根绳索对应参数，i＝1,2,3,4；

下标j:第j组数据对应参数，标定过程共采集n组数据，j对应其中某一组数据，j＝1,2,…,n；

顶标∧：用迭代参数算得的估计量；

表示第j组数据中，坐标系{G}相对于坐标系{O}的旋转矩阵，在本发明中主要指代通过IMU测得的手臂的旋转矩阵；

^OG_j：^OG_j＝[^OG_jx,^OG_jy,^OG_jz]^T，在坐标系{O}下，第j组数据中，肩关节中心坐标；

在坐标系{G}下，第i根绳索在上臂环上的连接点坐标初始值；

^OS_i:在坐标系{O}下，第i根绳索在肩部支架的连接点坐标，相对于坐标系{O}静止；

^OU_ij:在坐标系{O}下，第j组数据中，第i根绳索在上臂环上的连接点坐标，会随着上肢运动相对于坐标系{O}运动，即

l_ij：第j组数据中，第i根绳索的长度，即||^OS_i ^OU_ij||；

第i根绳索的初始长度，i＝1,2,3,4；

Δl_ij：第j组数据中，第i根绳索长度相对于初始绳长的绳长差；

待求解的参数是G_jx，G_jy，G_jz，

其各自含义如下：

G_jx，G_jy，G_jz：肩关节中心相对于系统坐标原点的偏移^OG_j＝[^OG_jx,^OG_jy,^OG_jz]^T，(j＝1,2,...,n)。考虑到正常人手臂在抬升过程中，肩关节的旋转中心会同步抬升，因此n组数据对应n组肩关节中心偏移；

取手臂处于某一姿态下的四根绳索长度记为初始绳长

通常取手臂正常下垂时的绳长值作为初始绳长。

上臂环中心到肩关节中心的距离为

具体标定过程如下：

1.首先取患者手臂自然下垂时的状态为初始状态，此时的绳长即为初始绳长，亦是待求参数，并记此时手臂的旋转矩阵为

即无相对旋转。

2.如图2所示，患者在医师的帮助下进行手臂的外展/内收，前屈/后伸、水平屈/伸等动作，记录运动过程中的编码器数据和IMU数据，即可得到绳长变化量Δl_i和上肢旋转矩阵

等参数，患者手臂在任一位姿下的Δl_i和

为一组数据，采集n组数据(n≥5)。

3.对算法迭代量赋初值，即对待求解参数初始化，初始值通过手动测量近似确定，分别赋值为

4.进行迭代，算得迭代参数的修正值，对迭代参数进行修正。

具体迭代原理如下：

考虑任意其中一组数据，忽略下标j,则在该组数据中，第i根绳索的绳长相对于初始绳长的绳长差可用如下方程表示

而l_i＝||^OS_i ^OU_i|| (2)

将式(1)代入式(2)，可得

假定肩关节中心坐标估计值

上臂环中心到肩关节中心的距离估计值值为

第i根绳索的初始绳长估计值为

在待求解参数附近进行泰勒展开，并取至一次项。则

将式(4)代入式(3)，展开后并写成

假设共采集了n组数据，则根据公式(5)可列出4n组方程，对应的待求解参数为3n组肩关节中心偏移[^OG_jx,^OG_jy,^OG_jz]^T(j＝1,2,...,n)，4组初始绳长

和1组上臂环中心到肩关节中心的距离

共3n+5个变量。

因此至少需要采集5组数据方可解出方程。

记

其中

记δl_ij＝Δl_ij-Δl_0ij

记

记

则AX＝Y

根据最小二乘法有

X＝(A^TA)^-1A^TY (6)

最后对待求解参数估计值进行迭代

5.将第m次迭代后的迭代参数D^m与第m-1次的迭代参数D^m-1进行比较，当||D^m-D^m-1||_∞＜ε时(ε是设定的误差限)，停止迭代，待求解参数的解为

(m为迭代次数)，否则返回步骤4，继续迭代。

(二)运动学参数解算

根据上述运动学参数标定方法，能够在机器人训练前对患者进行运动学参数标定，获得待求解运动学参数在迭代解算后的近似解

(m为迭代次数)。

其中参数

仅与患者手臂长度，上臂环穿戴位置等因素有关，不随患者手臂在训练过程的运动而变化，因此将其视为常量。而参数

表示患者肩关节旋转中心在坐标系{O}下的坐标，会随着患者手臂在训练过程的运动而发生改变，属于需要实时解算的参数。解算过程如下：

在上述运动学参数标定过程中，对于n组数据，共有n组手臂旋转矩阵

和n组肩关节中心偏移量标定值

运动学解算的目标是找到标定过程中的数据

和

的关系系数J，并在训练过程中，根据关系系数J和实时测得的手臂旋转矩阵

实时解算出患者肩关节中心偏移量^OG。

为便于计算，对n组

和

进行多元线性回归

记

则BJ＝Z

根据最小二乘法可解得J

J＝(B^TB)^-1B^TY

在实际训练过程中，通过实时的手臂旋转矩阵

可算得患者肩关节中心偏移量

基于相同的发明构思，本发明还提供了一种绳索康复机器人运动学参数标定和解算系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，该计算机程序被加载至处理器时实现前述的一种绳索康复机器人运动学参数标定和解算方法方法。计算机程序包括运动学参数标定模块和运动学参数解算模块，运动学参数标定模块用于在使用绳索康复机器人进行康复训练前对患者的运动学参数进行标定，具体实现上述步骤一内容；运动学参数解算模块用于在使用绳索康复机器人进行康复训练过程中根据患者的手臂姿态实时解算出当前肩关节中心位置，具体实现上述步骤二内容。

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (7)

1.一种绳索康复机器人运动学参数标定和解算方法，其特征在于，包括如下步骤：

步骤一，运动学参数标定

(1)首先取患者手臂处于某一姿态下的状态为初始状态，此时的绳长即为初始绳长，亦是待求参数，并记此时手臂的旋转矩阵为

即无相对旋转；

(2)患者进行手臂各种动作，记录患者运动过程中的编码器数据和IMU数据，即可得到绳长变化量Δl_i和上肢旋转矩阵

参数，患者手臂在任一位姿下的Δl_i和

为一组数据，采集n组数据；

(3)对算法迭代量赋初值，即对待求解参数初始化，初始值通过手动测量近似确定，分别赋值为

其中，

为肩关节中心相对于系统坐标原点的偏移值初始值；

为手臂处于某一姿态下的四根绳索初始绳长，其中i＝1,2,3,4,

为上臂环中心到肩关节中心的距离；

(4)进行迭代，算得迭代参数的修正值，对迭代参数进行修正；

(5)将第m次迭代后的迭代参数D^m与第m-1次的迭代参数D^m-1进行比较，当

时，停止迭代，待求解参数的解即为当前迭代参数，为

否则返回步骤(4)，继续迭代；其中D^m是第m次迭代后的迭代参数，

ε是设定的误差限，m为迭代次数；

步骤二，运动学参数解算

对n组

和

进行多元线性回归

记标定过程中的数据

和

的关系系数

则BJ＝Z

根据最小二乘法可解得J

J＝(B^TB)^-1B^TY

在实际训练过程中，通过实时的手臂旋转矩阵

可算得患者肩关节中心偏移量

2.根据权利要求1所述的绳索康复机器人运动学参数标定和解算方法，其特征在于，所述步骤(4)中，具体迭代过程如下：

考虑任意其中一组数据，忽略下标j，则在该组数据中，第i根绳索的绳长相对于初始绳长的绳长差用如下方程表示

而

将式(1)代入式(2)，可得

假定肩关节中心坐标估计值

上臂环中心到肩关节中心的距离估计值值为

第i根绳索的初始绳长估计值为

在待求解参数附近进行泰勒展开，并取至一次项；则

将式(4)代入式(3)，展开后并写成

假设共采集了n组数据，则根据公式(5)可列出4n组方程，对应的待求解参数为3n组肩关节中心偏移

4组初始绳长

和1组上臂环中心到肩关节中心的距离

共3n+5个变量，其中，j＝1,2,...,n，i＝1,2,3,4；

因此至少需要采集5组数据方可解出方程，即n≥5：

记

其中

记δl_ij＝Δl_ij-Δl_0ij

记

记

则AX＝Y

根据最小二乘法有

X＝(A^TA)^-1A^TY (6)

最后对待求解参数估计值进行迭代

上述各式中，

为在坐标系{O}下第j组数据中肩关节中心坐标；

表示第j组数据中通过IMU测得的手臂的旋转矩阵；

为在坐标系{G}下第i根绳索在上臂环上的连接点坐标初始值；

为在坐标系{O}下第i根绳索在肩部支架的连接点坐标；

为在坐标系{O}下第j组数据中第i根绳索在上臂环上的连接点坐标；l_ij为第j组数据中第i根绳索的长度，即

Δl_ij为第j组数据中第i根绳索长度相对于初始绳长的绳长差。

3.根据权利要求1所述的绳索康复机器人运动学参数标定和解算方法，其特征在于，所述步骤二进行解算时，参数

视为常量。

4.根据权利要求1所述的绳索康复机器人运动学参数标定和解算方法，其特征在于，所述步骤一中某一姿态指患者手臂自然下垂时的姿态。

5.根据权利要求1所述的绳索康复机器人运动学参数标定和解算方法，其特征在于，所述步骤一中各种动作包括手臂外展/内收、前屈/后伸、水平屈/伸动作。

6.一种绳索康复机器人运动学参数标定和解算系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于：所述计算机程序被加载至处理器时用于实现权利要求1-5中任意一项所述的绳索康复机器人运动学参数标定和解算方法。

7.根据权利要求1所述的绳索康复机器人运动学参数标定和解算系统，其特征在于：所述计算机程序包括运动学参数标定模块和运动学参数解算模块，运动学参数标定模块用于在使用绳索康复机器人进行康复训练前对患者的运动学参数进行标定；运动学参数解算模块用于在使用绳索康复机器人进行康复训练过程中根据患者的手臂姿态实时解算出当前肩关节中心位置。

Images