CN114910045A - 一种基于六轴获取偏航角的方法 - Google Patents

Abstract

本发明公开了一种基于六轴获取偏航角的方法，一种基于六轴获取偏航角的方法，包括以下步骤：S1，通过加速度计推算出粗略的偏航角；S2，通过陀螺仪获得角速度；S3，将粗略的偏航角和角速度通过六轴融合方法进行运算得到最终的偏航角。本发明的方法首先通过加速度计粗略推算得到偏航角，再通过陀螺仪得到角速度，最后将两个数值通过六轴融合方法准确地获得偏航角，并且可以消除温漂和零漂产生的影响，抗干扰性强。

Description

一种基于六轴获取偏航角的方法

技术领域

本发明涉及门锁设备技术领域，尤其是一种基于六轴获取偏航角的方法。

背景技术

三维空间中经常用到俯仰角(pitch)、偏航角(yaw)和翻滚角(roll)，俯仰角是围绕x轴旋转，偏航角是围绕y轴旋转，翻滚角是围绕z轴旋转。其中俯仰角和翻滚角通常采用陀螺仪+加速度计的方法来测量，通过陀螺仪积分得到的角度和加速度计与重力加速度的夹角转换得到的角度进行整合计算出实际的俯仰角和翻滚角。由于偏航角所在平面一直与重力加速度方向垂直，导致加速度计无法通过重力加速度转换成方位角移动相对角度。因此上述的陀螺仪+加速度计测量方法不能用来测量偏航角。测量偏航角目前常用的方法有两种：

1、陀螺仪检测；陀螺仪可以测量当前设备在相对惯性空间转角或角速度。通过陀螺仪积分得到角度，推算出偏航角；

2、磁力计检测；磁力计可以测量出当前设备与东南西北四个方向上的夹角。通过磁力计可以推算出偏航角。

只有陀螺仪单独测量角度时，由于没有加速度计进行补偿，陀螺仪采样角速度数据时会受到外部作用力的影响。但陀螺仪测量精度的问题，推算值会发生偏移。而且陀螺仪存在温漂和零漂的问题，会导致角度失真。使用磁力计测量角度时，虽然能较为准确地推算出偏航角。但是磁力计受外界磁性物质干扰较大，当磁性物质的磁场较大时，可能会出现误判的情况。并且磁力计也会存在零漂的问题，导致角度失真。

在中国专利文献上公开的“一种基于毫米波雷达的车辆偏航角计算方法”，其公开号为CN108909721B，该发明涉及一种基于毫米波雷达的车辆偏航角计算方法，通过采用毫米波雷达来检测高速公路旁的静止物体，如护栏、树木，并进行直线、曲线拟合获得道路边界，利用该边界来代替车道的几何特性；采用长距毫米波雷达获得前方道路边界来预估道路为直道或弯道，采用中距毫米波雷达拟合两侧道路边界，进而计算车辆偏航角。相比于基于机器视觉的车辆偏航角计算，本方法对于天气、光照、阴影、摄像头抖动及交通标志线等因素的抗干扰能力更强，有助于车道偏离预警系统的准确决策。但是该发明需要毫米波雷达的检测，计算过程繁琐。

发明内容

本发明解决了现有技术中使用磁力计测量角度时由于受外界磁性物质干扰较大导致出现误判的问题，提出一种基于六轴获取偏航角的方法，首先通过加速度计粗略推算得到偏航角，再通过陀螺仪得到角速度，最后将两个数值通过六轴融合方法准确地获得偏航角，并且可以消除温漂和零漂产生的影响，抗干扰性强。

为了实现上述目的，本发明采用以下技术方案：一种基于六轴获取偏航角的方法，包括以下步骤：

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

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

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

本发明中，由于在非匀加速转动改变偏航角时，偏航角所在平面与重力方向垂直，所以加速度计无法通过重力获得偏航角，因此传统的用卡尔曼滤波将陀螺仪获得的角度作为系统的估计值，和将加速度计获得的角度作为状态的测量值进行融合的方法在此情况下不适用，虽然加速度计无法通过重力获得偏航角，但是在外力作用下可以获得偏航角所在水平面的瞬间加速度。本发明的步骤S1正是依据上述原理推算出粗略的偏航角，又用传统的陀螺仪获得角速度，最后用卡尔曼滤波进行融合，能准确的测量偏航角，并且抗干扰强。

作为优选，所述步骤S1具体包括以下步骤：

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

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

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

本发明中，加速度计获得的瞬间加速度在采样时间一定的情况下，采样时间可以作为常量不计，瞬间加速度的大小能间接反应出偏航角的大小；综合牛顿第二定律和力的合成原则，计算出总的作用力大小，最后转化为加速度进行作差，计算方便。

作为优选，所述近似求解的方法具体为由于加速度计只能测量x轴、y轴或者z轴上的加速度，作用力F1施加到加速度计上时，测量得到三个x轴、y轴和z轴上的分加速度分别为ax，ay和az，故可得在x轴、y轴和z轴上三个分力分别是Fx、Fy和Fz，根据力的合成，计算出：

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

本发明中，求解粗略偏航角的过程忽略外界因素干扰，将三个分加速度转化成作用力后进行合成，无需传感器即可求出作用力。

作为优选，所述步骤S13具体为利用质量m一定，根据作用力F1和F2求出合成加速度a1和a2，两者作差的值即为粗略的偏航角。

本发明中，a1和a2的差近似等同于偏航角的前提是采样时间t非常小的时候。

作为优选，所述步骤S2具体还包括通过陀螺仪的角速度积分推算出角度。

本发明中，陀螺仪能测量当前设备在相对惯性空间的角速度，并通过积分计算出角度。

作为优选，所述步骤S3具体包括通过加速度计采样时间内获取的粗略偏航角作为对系统的估计值，将角速度积分得到的角度值作为状态的测量值，通过卡尔曼滤波将加速度计推算出的偏航角和陀螺仪得到的角度进行融合得到最终的偏航角。

本发明中，平常实用的卡尔曼滤波融合获得俯仰角和翻滚角时，是将角速度采样时间内得到的角度值Δθ＝ωΔt作为对系统的估计值，将基于加速度计获得的加速度a作为状态的测量值，然后进行融合得到最终的角度，而本发明则不同，本发明的方法能有效融合得到准确的偏航角，另外采样时间内的偏航角是通过作差得到的，若当前采样出现干扰会在下一次采样时作差抵消，这样的话可以去除温漂和零漂带来的影响，确保使用的过程中不会对偏航角带来误差。

本发明的有益效果是：

1、本发明直接通过六轴获取偏航角，不需要额外增加传感器获取偏航角；

2、本发明中的六轴融合方法抗干扰性强，可以有效的去除温漂和零漂；

3、本发明不受硬件的限制，只要是带有相关功能的硬件都可以使用，替换性强。

附图说明

图1是本发明的流程图；

图2是本发明求解F1过程中的受力分析图。

具体实施方式

实施例：

本实施例提出一种基于六轴获取偏航角的方法，参考图1，首先，由加速度计算出粗略的偏航角；其次通过陀螺仪获取角速度并计算出角度，具体通过角速度积分的方法计算出；最后将偏航角和角速度这两个数据通过六轴融合方法准确获取偏航角。

具体的，参考图2，粗略计算偏航角大致的过程为：首先，计算出作用力F1，由于加速度计直接测量得到的数据为沿x轴、y轴或者z轴上的加速度，故使用近似求解的方法，当作用力F1作用到加速度计上的时候，可检测得到三个轴向方向的分加速度，分别为ax，ay和az，由牛顿第二定律可求出三个轴向方向对应的分作用力，分别为Fx、Fy和Fz，最后对力进行合成，先算出F'：

其中，F1'为x轴和y轴方向的合力，之后又和z轴方向的力进行合成，求得：

其中，F1”为三个分力的合力，综合可得：

虽然有外界因素存在，但合力F1”和作用力F1还是近乎相等的。

经过采样时间t后，求出作用力F2，求解过程完全与作用力F1的求解过程相同。最后，由F1和F2再次求出对应的合成加速度，分别为a1和a2，a2-a1得到的值就可以直接作为每次采样后偏航角的变化值，即为粗略偏航角。

最后，通过卡尔曼滤波将加速度计推算出的偏航角和陀螺仪得到的角度进行融合。具体的，本方法通过加速度计采样时间内获取的粗略偏航角即a2-a1作为对系统的估计值，将角速度积分得到的角度值作为状态的测量值，角度值为

θ(k)＝θ(k-1)+ωΔt

其中θ(k)为当前累加角度值，θ(k-1)为上次累加角度值；最后通过卡尔曼滤波将加速度计推算出的偏航角和陀螺仪得到的角度进行融合得到最终的偏航角。

本发明中，由于在非匀加速转动改变偏航角时，偏航角所在平面与重力方向垂直，所以加速度计无法通过重力获得偏航角，因此传统的用卡尔曼滤波将陀螺仪获得的角度作为系统的估计值，和将加速度计获得的角度作为状态的测量值进行融合的方法在此情况下不适用，虽然加速度计无法通过重力获得偏航角，但是在外力作用下可以获得偏航角所在水平面的瞬间加速度。本发明的步骤S1正是依据上述原理推算出粗略的偏航角，又用传统的陀螺仪获得角速度，最后用卡尔曼滤波进行融合，能准确的测量偏航角，并且抗干扰强。

本发明中，加速度计获得的瞬间加速度在采样时间一定的情况下，采样时间可以作为常量不计，瞬间加速度的大小能间接反应出偏航角的大小；综合牛顿第二定律和力的合成原则，计算出总的作用力大小，最后转化为加速度进行作差，计算方便。

本发明中，平常实用的卡尔曼滤波融合获得俯仰角和翻滚角时，是将角速度采样时间内得到的角度值Δθ＝ωΔt作为对系统的估计值，将基于加速度计获得的加速度a作为状态的测量值，然后进行融合得到最终的角度，而本发明则不同，本发明的方法能有效融合得到准确的偏航角，另外采样时间内的偏航角是通过作差得到的，若当前采样出现干扰会在下一次采样时作差抵消，这样的话可以去除温漂和零漂带来的影响，确保使用的过程中不会对偏航角带来误差。

本发明的卡尔曼滤波方法具体如下：

X(k|k-1)＝A X(k-1|k-1)+B U(k)

其中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，A和B是系统参数；

P(k|k-1)＝A P(k-1|k-1)A’+Q

其中，P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差，A’表示A的转置矩阵，Q是系统过程的协方差；

X(k|k)＝X(k|k-1)+Kg(k)(Z(k)-H X(k|k-1))

其中，Kg(k)为卡尔曼增益，Z(k)是k时刻的测量值，H是测量系统的参数；

Kg(k)＝P(k|k-1)H’/(H P(k|k-1)H’+R)

其中，R是系统过程的协方差；

P(k|k)＝(I-Kg(k)H)P(k|k-1)

其中，I为1的矩阵，对于单模型单测量，I＝1；当系统进入k+1状态时，P(k|k)就是本段第二个式子中的P(k-1|k-1)。

上述实施例是对本发明的进一步阐述和说明，以便于理解，并不是对本发明的任何限制，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (6)

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具体包括通过加速度计采样时间内获取的粗略偏航角作为对系统的估计值，将角速度积分得到的角度值作为状态的测量值，通过卡尔曼滤波将加速度计推算出的偏航角和陀螺仪得到的角度进行融合得到最终的偏航角。

Images