[发明专利]一种基于惯性传感器的位姿检测与能耗计算方法

权利要求书

1.一种基于惯性传感器的位姿检测与能耗计算方法，位姿包括位移和姿态角，其特征在于：在手部抓握一个负载，所述负载内置惯性传感器模块，根据惯性传感器模块返回的参数解算手部末端负载的实时位姿，计算负载消耗的能耗，检测计算过程包括以下步骤：

步骤一：在0时刻，对惯性传感器在初始位标定，在O点以所述惯性传感器的位姿定义基准坐标系M:xyz；

步骤二：在t时刻，在O”点以所述惯性传感器的位姿定义载体坐标系M”:x”y”z”；

步骤三：定义辅助坐标系N：x’y’z’,所述辅助坐标系以O为原点，坐标轴指向与所述载体坐标系相同；

所述载体坐标系，在0-t时刻，可看做先发生旋转变换，再发生平移变换(M→N→M”)；

在t时刻,所述惯性传感器返回滚转角α、俯仰角β、偏航角γ；故旋转变换矩阵如下：

于是变换矩阵：

在t时刻,所述惯性传感器返回沿着x”方向的加速度返回沿着y”方向的加速度返回沿着z”方向的加速度根据所述旋转变换矩阵，对返回的加速度信号进行解耦，消除重力加速度g的影响，

所述解耦步骤如下：

步骤一：定义x”、y”、z”(同x’、y’、z’)与-z轴的夹角分别为∠1、∠2、∠3；

步骤二：定义N中存在沿着x’轴的向量其在M中的描述:

定义M内存在沿着-z轴的向量则

同理定义N中存在沿着y’轴的向量存在沿着z’轴的向量可得：

步骤三：根据三元一次方程组：

求解得：在所述载体坐标系(同所述辅助坐标系)下，所述负载的运动加速度a_x”、a_y”、a_z”；

对所述负载装置的运动加速度，将载体坐标系(同辅助坐标系)下的描述，转换到基准坐标系下，其运动加速度a_x、a_y、a_z：

对a_x、a_y、a_z分别进行一次积分、两次积分可以得到负载装置的速度V_x、V_y、V_z与位移S_x、S_y、S_z；

克服哑铃做功：

W＝G*S_z。

2.根据权利要求1所述的基于惯性传感器的位姿检测与能耗计算方法，其特征在于，为了进一步精确计算能耗，

式中：Q_x、Q_y、Q_z表示负载在X轴、Y轴和Z轴方向上的矩的作用，W表示克服负载重力做的功。

3.根据权利要求2所述的基于惯性传感器的位姿检测与能耗计算方法，其特征在于，计算整体能耗,Q＝k_xQ_x+k_yQ_y+k_zQ_z+kW；

式中：Q表示由于负载引起的能耗，系数k_x、k_y、k_z可以通过针对上肢的具体动作训练实验标定测得，k表示功能转换系数，具体可以通过标定实验测得。

4.根据权利要求1-3任一所述的基于惯性传感器的位姿检测与能耗计算方法，其特征在于，上诉计算方法不局限于仅有上肢进行运动时的情况，当复合了人体自身的运动时，可以在腰背部固定同样的惯性传感器模块，同上诉计算方法所述，负载相对自身的运动等于负载的绝对位姿减去自身的绝对位姿。

5.根据权利要求1-3任一所述的基于惯性传感器的位姿检测与能耗计算方法，其特征在于，所述惯性传感器是三轴加速度传感器与三轴陀螺仪组合的六轴惯性传感器，或九轴姿态传感器。