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

权利要求书

1.一种绳索康复机器人运动学参数标定和解算系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于：绳索康复机器人包括4根绳索，4根绳索穿过肩部支架后连接到上臂环，上臂环是一个穿戴在手臂上的套筒，上臂环与患者上肢结合形成一个完全约束的机械系统，可控制上肢肩关节3自由度运动；机器人绳索的长度通过编码器测量，上肢的3自由度旋转通过IMU测量，所述计算机程序被加载至处理器时用于实现绳索康复机器人运动学参数标定和解算方法，方法包括如下步骤：

步骤一，运动学参数标定

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

(2)患者进行手臂各种动作，记录患者运动过程中的编码器数据和IMU数据，即可得到绳长变化量Δl_i和上肢旋转矩阵参数，患者手臂在任一位姿下的Δl_i和为一组数据，采集n组数据；

(3)对算法迭代量赋初值，即对待求解参数初始化，初始值通过手动测量近似确定，分别赋值为其中，为肩关节中心相对于系统坐标原点的偏移值初始值；为手臂处于某一姿态下的四根绳索初始绳长，其中i＝1,2,3,4,为上臂环中心到肩关节中心的距离；

(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：

记

其中

记

记

记

则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根绳索在上臂环上的连接点坐标，会随着上肢运动相对于坐标系{O}运动，即l_ij为第j组数据中第i根绳索的长度，即||^OS_i^OU_ij||；Δl_ij为第j组数据中第i根绳索长度相对于初始绳长的绳长差；为在运动学参数标定过程中,第j次测量中得到的第i根绳索的绳长相对于初始绳长的绳长差的估计值；δl_ij为在运动学参数标定过程中,第j次测量中得到的第i根绳索的绳长差理论值和估计值的差；

(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

在实际训练过程中，通过实时的手臂旋转矩阵可算得患者肩关节中心偏移量