[发明专利]一种三维姿态角的计算方法与系统

权利要求书

1.一种三维姿态角的计算方法，用于计算一运动物体对应的三维姿态角，其特征在于，包括如下步骤：

获取所述运动物体对应的运动信息，所述运动信息包括三维加速度、三维角速度以及三维磁场强度；

对所述运动信息进行CRC校验以得到一校验结果，判断所述校验结果是否与预设校验结果相同；

若是，则对所述运动信息进行形式转换，所述形式转换包括数据格式转换以及方位坐标变换；

对所述形式转换后的所述运动信息进行校正处理，根据所述校正处理后的所述运动信息计算以得到一对应的四元数，其中所述校正处理为分别对所述三维加速度进行加速度参数标定，对所述三维角速度进行角速度温度补偿以及对所述三维磁场强度进行磁场强度参数标定；

根据所述四元数计算得到对应的所述三维姿态角。

2.根据权利要求1所述的三维姿态角的计算方法，其特征在于，对所述三维角速度进行角速度温度补偿的步骤包括：

根据ΔT＝T_i-T₀计算温度的变化值，其中ΔT为温度的变化值，T_i为i时刻的温度值，T₀为基准温度值；

根据计算所述三维角速度的变化量，其中为第i时刻的所述三维角速度，为初始时刻的所述三维角速度；

根据MATLAB计算中以及对应的系数值；

根据对所述三维角速度进行计算以完成所述角速度温度补偿。

3.根据权利要求1所述的三维姿态角的计算方法，其特征在于，对所述三维加速度进行所述加速度参数标定的步骤包括：

根据对所述三维加速度进行所述加速度参数标定，其中A'_X、A'_Y以及A'_Z为实际的所述三维加速度，A_X、A_Y以及A_Z为测量得到的所述三维加速度，a、b以及c为第一比例因子，d、e以及f为第一偏移因子。

4.根据权利要求1所述的三维姿态角的计算方法，其特征在于，对所述三维磁场强度进行所述磁场强度参数标定的步骤包括：

根据对所述三维磁场强度进行所述磁场强度标定，其中M'_X、M'_Y以及M'_Z为实际的所述三维磁场强度，M_X、M_Y以及M_Z为测量得到的所述三维磁场强度，a'、b'以及c'为第二比例因子，d'、e'以及f'为第二偏移因子。

5.根据权利要求1所述的三维姿态角的计算方法，其特征在于，所述数据格式转换为将所述运动信息转换为十进制格式。

6.根据权利要求1所述的三维姿态角的计算方法，其特征在于，计算所述四元数的步骤包括：

根据T＝[EX EY EZ]得到一变换矩阵，其中为所述三维加速度组成的矩阵，为所述三维磁场强度组成的矩阵，EY＝EY/(SQRT(EY·EY))，SQRT为数学函数开平方；

根据所述变换矩阵T计算得到所述四元数，所述四元数的表达式分别为：

q₀＝SQRT(1+T(1，1)+T(2，2)+T(3，3))/2；

q₁＝SIGN(T(3，2)-T(2，3))*SQRT(1+T(1，1)-T(2，2)-T(3，3))/2；

q₂＝SIGN(T(1，3)-T(3，1))*SQRT(1+T(1，1)+T(2，2)-T(3，3))/2；

q₃＝SIGN(T(2，1)-T(1，2))*SQRT(1-T(1，1)-T(2，2)+T(3，3))/2，其中SIGN(X)

表示当X大于0时取1，X小于0时取-1。