[发明专利]一种基于六轴获取偏航角的方法

权利要求书

1.一种基于六轴获取偏航角的方法，其特征在于，包括以下步骤：

S1，通过加速度计推算出粗略的偏航角；

S2，通过陀螺仪获得角速度；

S3，将粗略的偏航角和角速度通过六轴融合方法进行运算得到最终的偏航角。

2.根据权利要求1所述的一种基于六轴获取偏航角的方法，其特征在于，所述步骤S1具体包括以下步骤：

S11，用近似求解的方法求出作用力F1；

S12，经过采样时间t后，求出作用力F2，求解方法与求解作用力F1的方法相同；

S13，求出a1和a2并作差得到粗略的偏航角。

3.根据权利要求2所述的一种基于六轴获取偏航角的方法，其特征在于，所述近似求解的方法具体为由于加速度计只能测量x轴、y轴或者z轴上的加速度，作用力F1施加到加速度计上时，测量得到三个x轴、y轴和z轴上的分加速度分别为ax，ay和az，故可得在x轴、y轴和z轴上三个分力分别是Fx、Fy和Fz，根据力的合成，计算出：

式中，F1”为三个分力的合力；用合力F1”来代替并求出作用力F1。

4.根据权利要求2所述的一种基于六轴获取偏航角的方法，其特征在于，所述步骤S13具体为利用质量m一定，根据作用力F1和F2求出合成加速度a1和a2，两者作差的值即为粗略的偏航角。

5.根据权利要求1所述的一种基于六轴获取偏航角的方法，其特征在于，所述步骤S2具体还包括通过陀螺仪的角速度积分推算出角度。

6.根据权利要求1所述的一种基于六轴获取偏航角的方法，其特征在于，所述步骤S3具体包括通过加速度计采样时间内获取的粗略偏航角作为对系统的估计值，将角速度积分得到的角度值作为状态的测量值，通过卡尔曼滤波将加速度计推算出的偏航角和陀螺仪得到的角度进行融合得到最终的偏航角。