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

技术特征：
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根据最小二乘法可解得jj＝(b
t
b)
‑1b
t
y在实际训练过程中，通过实时的手臂旋转矩阵可算得患者肩关节中心偏移量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
t
a)
‑1a
t
y
ꢀꢀꢀꢀꢀꢀ
(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所述的绳索康复机器人运动学参数标定和解算系统，其特征在于：所述计算机程序包括运动学参数标定模块和运动学参数解算模块，运动学参数标定模块用于在使用绳索康复机器人进行康复训练前对患者的运动学参数进行标定；运动学参数解算模块用于在使用绳索康复机器人进行康复训练过程中根据患者的手臂姿态实时解算出当前肩关节中心位置。

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


技术研发人员：宋爱国 厉叶 李会军 石珂
受保护的技术使用者：东南大学
技术研发日：2021.05.24
技术公布日：2021/10/18