[发明专利]一种基于陀螺仪的割草机直线行走的航向校准方法

说明书

本发明公开了一种基于陀螺仪的割草机直线行走的航向校准方法，属于姿态解算领域。本发明首先获取陀螺仪Z轴静态数据，得出N个采样周期时间内的静止状态下的航向漂移累积角度。其次解算航向角，得到航向角，并根据状态阈值判断割草机运动状态。然后去除航向角漂移误差，得到校准后的航向角。最后电机根据角度阈值进一步判断并调整双轮速度来进行航向调整，不断更新角度，做直线运动。本发明可以解算出更加精确的航向角，进行精确的航向校正。

技术领域

本发明属于姿态解算领域，具体涉及一种基于陀螺仪的割草机直线行走的航向校准方法。

技术背景

室外割草机设备的姿态解算一般依赖于六轴传感器利用四元数解算姿态角(航向角、俯仰角和横滚角)，六轴传感器包括陀螺仪和加速度计，但是由于陀螺仪存在零漂问题，所以采用六轴解算出来的航向角误差会随着时间累积越来越大，割草机走直线也会逐渐偏航，所以需要解决在割草机运行过程中的零漂问题。

现有技术针对上述问题，主要有两种方案，一种是对陀螺仪进行误差分析建立模型，然后再利用四元数和误差补偿算法最终解算姿态角，但是该方式适用于环境较为稳定的情况使用，例如静止状态下能够取得良好的效果，而对于在凹凸不平的草地上运行的割草机效果并不明显；另一种是在六轴基础上再加上磁力计融合算法最终实现校准航向角。但由于磁力计易受干扰，户外环境不稳定且不可预测，导致依旧难以解决上述问题。

发明内容

针对上述问题，本发明提出了一种基于陀螺仪的割草机直线行走的航向校准方法，通过对陀螺仪Z轴零漂误差的周期性较准，减小航向角的累积误差，同时采用陀螺仪Z轴做为解算的数据来源，具备强抗环境干扰能力，并且可以让割草机在运行过程中通过划分状态达到更好的去除零漂误差的效果。

本发明采用以下技术方案：

利用陀螺仪的静态零漂解决割草机运行过程中的漂移问题，根据陀螺仪角速度值在动态和静态时的区分设置一个判断割草机动态还是静态的状态阈值，根据状态阈值判定状态可以采用静态和动态两种模式解算航向角，从而可以更高精度的解决由于陀螺仪零漂导致的航向角随时间累积误差的问题。具体包含以下步骤：

A、获取陀螺仪Z轴静态数据

A-1、在割草机初始化开机时，此时割草机处于静止状态，获得N个连续的、静止的以周期T为采样间隔的陀螺仪的Z轴的角速度值。

A-2、对A-1中获取的陀螺仪静态的Z轴角速度值对周期T进行累积求和，得出该N个采样周期时间内静止状态下的航向漂移累积角度。

B、解算航向角

从割草机开机后，割草机就以T为周期获取陀螺仪的数据，每获取一次数据都将Z轴角速度值和T乘积并且累加到上次求出来的航向角中，以此不断叠加，记为开机到当前时刻的总的航向角。

C、判断运动状态

C-1、根据陀螺仪Z轴角速度值在运动和静止状态下的不同的特性，设置一个状态阈值区分割草机状态(该状态阈值可以结合使用的陀螺仪的量程、灵敏度和应用该陀螺仪的场景综合决定)。

C-2、为了防止是陀螺仪的数据突变造成的个别数值大于阈值而导致状态误判，采用滑动窗口，即每次在滑动窗口中存放最新的M个连续的Z轴角速度值，算其平均值，若大于阈值，则判定为运动状态，若小于状态阈值，则为静止状态。

D、去除航向角漂移误差

D-1、根据C中判断割草机的静态和动态的状态阈值，当判断为静止状态时，将Z轴角速度值直接赋值为0，因此可以不引入静态漂移误差。

D-2、当判断为运动状态时，每累积N个T与Z轴角速度的乘积得到的航向角，都要减去一个A-2中解算出来的N个采样周期时间内静止状态下的航向漂移累积角度，去除动态过程中的静态漂移误差，得到校准后的航向角。