[发明专利]一种用于手臂姿态重建的校准方法及系统

权利要求书

1.一种用于手臂姿态重建的校准方法，其特征在于，包括：

获取手臂姿态数据和手臂末端移动数据；所述手臂姿态数据包括大臂姿态数据和小臂姿态数据；所述大臂姿态数据是手臂末端在已知面上随意移动时，佩戴在大臂上的惯性测量单元测量到的数据；所述小臂姿态数据是手臂末端在已知面上随意移动时，佩戴在小臂上的惯性测量单元测量到的数据；所述手臂末端移动数据由手臂末端在已知面上随意移动的位置点组成；所述已知面为已知空间分布特征的平面或曲面；

依据所述手臂姿态数据构建第一捷联矩阵；所述第一捷联矩阵包括大臂捷联矩阵和小臂捷联矩阵；所述大臂捷联矩阵为佩戴在大臂上的惯性测量单元到大地坐标参考系的捷联矩阵；所述小臂捷联矩阵为佩戴在小臂上的惯性测量单元到大地坐标参考系的捷联矩阵；

根据不重合角度，构建第二捷联矩阵；所述第二捷联矩阵包括大臂旋转变换捷联矩阵和小臂旋转变换捷联矩阵；所述大臂旋转变换捷联矩阵为佩戴在大臂上的惯性测量单元到手臂关节坐标系的捷联矩阵；所述小臂旋转变换捷联矩阵为佩戴在小臂上的惯性测量单元到手臂关节坐标系的捷联矩阵；所述不重合角度为惯性测量单元的姿态轴与手臂关节的姿态轴之间不重合的角度；

由大臂长度、小臂长度、所述手臂末端移动数据、所述第一捷联矩阵和所述第二捷联矩阵构建校准目标函数，并采用梯度下降法对所述校准目标函数进行求解，得到最优解；所述最优解包括最优不重合角度、最优大臂长度和最优小臂长度；

由所述最优解对手臂空间移动数据进行校正，得到校正后的手臂空间移动数据；所述手臂空间移动数据由手臂在空间中作出设定动作时移动的位置点组成；

所述校准目标函数为

其中，(x_n,y_n,z_n)表示第n次移动手臂末端后的位置点在大地坐标参考系下的坐标；x_n表示第n次移动手臂末端后的位置点在大地坐标参考系x轴上的坐标，y_n表示第n次移动手臂末端后的位置点在大地坐标参考系y轴上的坐标，z_n表示第n次移动手臂末端后的位置点在大地坐标参考系z轴上的坐标；[C]ⁿ_IMU1toE表示大臂捷联矩阵，[C]ⁿ_IMU2toE表示小臂捷联矩阵，[C]_JtoIMU1表示大臂旋转变换捷联矩阵，[C]_JtoIMU2表示小臂旋转变换捷联矩阵；L₁表示大臂长度，L₂表示小臂长度；

所述由所述最优解对手臂空间移动数据进行校正，得到校正后的手臂空间移动数据，具体为：

其中，(x_r,y_r,z_r)表示手臂第r次在空间中作出设定动作后移动的位置点的校正值；[C]^r_IMU1toE表示手臂第r次在空间中作出设定动作后，佩戴在大臂上的惯性测量单元输出的大臂姿态数据，[C]^r_IMU2toE表示手臂第r次在空间中作出设定动作后，佩戴在小臂上的惯性测量单元输出的小臂姿态数据；[C]^k_JtoIMU1表示最优大臂旋转变换捷联矩阵，[C]^k_JtoIMU1是由大臂对应的最优不重合角度确定的；[C]^k_JtoIMU2表示最优小臂旋转变换捷联矩阵，[C]^k_JtoIMU2是由小臂对应的最优不重合角度确定的；L^k₁表示最优大臂长度；L^k₂表示最优小臂长度。