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

说明书

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

技术领域

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

背景技术

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

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

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

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

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

发明内容

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

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

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

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

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

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

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

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

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