[发明专利]一种姿态矩阵的计算方法及设备

权利要求书

1.一种姿态矩阵的计算方法，应用于便携设备，其特征在于，所述便携设备包括传感器，所述传感器包括陀螺仪，所述方法涉及传感器坐标系、脚坐标系和地面坐标系，所述脚坐标系根据脚和小腿的方向定义，所述方法包括：

根据第一坐标变换矩阵和第i个采样时刻对应的所述陀螺仪的测量值，计算所述第i个采样时刻对应的第二坐标变换矩阵，所述第一坐标变换矩阵为常数矩阵，所述第一坐标变换矩阵为所述传感器坐标系与所述脚坐标系之间的坐标变换矩阵，所述第二坐标变换矩阵为所述脚坐标系与所述地面坐标系之间的坐标变换矩阵，i为大于0的整数；

根据所述第一坐标变换矩阵和所述第i个采样时刻对应的所述第二坐标变换矩阵，计算所述第i个采样时刻对应的姿态矩阵，所述姿态矩阵为所述传感器坐标系与所述地面坐标系之间的坐标变换矩阵。

2.根据权利要求1所述的方法，其特征在于，所述姿态矩阵为所述传感器坐标系到所述地面坐标系的坐标变换矩阵，所述第一坐标变换矩阵为所述传感器坐标系到所述脚坐标系的坐标变换矩阵，所述第二坐标变换矩阵为所述脚坐标系到所述地面坐标系的坐标变换矩阵。

3.根据权利要求1所述的方法，其特征在于，所述传感器还包括加速度计，在所述根据所述第一坐标变换矩阵和所述第i个采样时刻对应的所述第二坐标变换矩阵，计算所述第i个采样时刻对应的姿态矩阵之后，所述方法还包括：

根据所述第i个采样时刻对应的所述加速度计的测量值和所述第i个采样时刻对应的姿态矩阵，计算所述第i个采样时刻对应的所述传感器在所述地面坐标系中的加速度；

根据所述第i个采样时刻对应的所述传感器在所述地面坐标系中的加速度，计算所述第i个采样时刻对应的所述传感器在所述地面坐标系中的速度；

根据q个采样时刻对应的所述传感器在所述地面坐标系中的速度，计算所述传感器在所述地面坐标系中的距离，q为大于或者等于2的整数。

4.根据权利要求2所述的方法，其特征在于，所述传感器还包括加速度计，在所述根据所述第一坐标变换矩阵和所述第i个采样时刻对应的所述第二坐标变换矩阵，计算所述第i个采样时刻对应的姿态矩阵之后，所述方法还包括：

根据所述第i个采样时刻对应的所述加速度计的测量值和所述第i个采样时刻对应的姿态矩阵，计算所述第i个采样时刻对应的所述传感器在所述地面坐标系中的加速度；

根据所述第i个采样时刻对应的所述传感器在所述地面坐标系中的加速度，计算所述第i个采样时刻对应的所述传感器在所述地面坐标系中的速度；

根据q个采样时刻对应的所述传感器在所述地面坐标系中的速度，计算所述传感器在所述地面坐标系中的距离，q为大于或者等于2的整数。

5.根据权利要求3所述的方法，其特征在于，所述方法还包括：

对第j个校正时刻与第j-1个校正时刻之间的时间段内，各采样时刻分别对应的所述传感器在所述地面坐标系的第n坐标轴的速度进行校正，以使得在所述第j个校正时刻，校正后的所述传感器在所述地面坐标系的第n坐标轴的速度为0，j为大于0的整数，n的取值为一、二或者三；

其中，所述校正时刻为在包括脚着地时刻的预设时长内，所述传感器在所述地面坐标系的第二坐标轴的速度的极小值对应的时刻，所述脚着地时刻为所述加速度计的测量值表示的向量的模长的极大值对应的时刻。

6.根据权利要求4所述的方法，其特征在于，所述方法还包括：

对第j个校正时刻与第j-1个校正时刻之间的时间段内，各采样时刻分别对应的所述传感器在所述地面坐标系的第n坐标轴的速度进行校正，以使得在所述第j个校正时刻，校正后的所述传感器在所述地面坐标系的第n坐标轴的速度为0，j为大于0的整数，n的取值为一、二或者三；

其中，所述校正时刻为在包括脚着地时刻的预设时长内，所述传感器在所述地面坐标系的第二坐标轴的速度的极小值对应的时刻，所述脚着地时刻为所述加速度计的测量值表示的向量的模长的极大值对应的时刻。