[发明专利]基于MEMS传感器实现机械车体航向角检测的方法

说明书

技术领域

本发明涉及机械车体航向角检测技术领域，尤其涉及农业机械车体航向角检测技术领域，具体是指一种基于MEMS传感器实现机械车体航向角检测的方法。

背景技术

随着传感器和控制技术的发展和国家对农业扶持力度的进一步加大，精准农业正在快速变成一种趋势，而在农业机械辅助驾驶控制过程中，车体航向角能够为其提供行驶方向，进而为控制算法提供数据输入。

车体航向角多利用地球磁场方向的水平分量永远指向磁北极的特性，通过磁性传感器实时获取和计算得到航向角。但是，地球磁场易受周围铁质材料(硬铁和软铁)干扰的影响，使得磁性传感器直接输出的数据根本无法使用，固在传感器使用前必须进行校准；车体姿态的变化(俯仰和横滚角)对计算输出的航向角也存在较大的影响，故需要通过姿态角对磁场强度实时进行补偿。

目前，三维磁场强度多采用空间8字方法对磁性传感器进行偏移校准，将平台指向3个已知的方向，或将它带滚动和俯仰地移动一圈，或24个指向，手持平台却可以做24点校正，但农业机械等不可能。对于农业机械等大体积物体在使用过程中来说，做一个三维空间动作基本上是不可能的。而且如果车身俯仰和横滚角通过其它倾角传感器获取，多传感器之间的X、Y、Z坐标系不对称，及其多传感器之间的耦合性都会影响航向角的计算精度。

发明内容

本发明的目的是克服了上述现有技术的缺点，提供了一种通过MEMS(Micro-electromechanical Systems，微电子机械系统)传感器采集数据，实现二维平面内数据校准和补偿，获得更为精确的航向角的基于MEMS传感器实现机械车体航向角检测的方法。

为了实现上述目的，本发明的基于MEMS传感器实现机械车体航向角检测的方法具有如下构成：

该基于MEMS传感器实现机械车体航向角检测的方法，其主要特点是，所述的方法包括以下步骤：

(1)所述的MEMS传感器采集所述的机械车辆在转动过程中X、Y、Z三轴的磁场强度数据；

(2)所述的MEMS传感器采集所述的机械车辆的运动加速度；

(3)所述的微控制处理器根据空间向量之间的关系计算出机械车辆当前俯仰角和翻滚角；

(4)所述的微控制处理器对所述的磁场强度数据进行修正；

(5)所述的微控制处理器计算得到航向角。

其中，所述的MEMS传感器采集所述的机械车辆在转动过程中X、Y、Z三轴的磁场强度数据，具体为：

所述的MEMS传感器每隔60°采集所述的机械车辆在转动过程中X、Y、Z三轴的磁场强度数据。

进一步地，所述的步骤(1)和(2)之间，还包括以下步骤：

(1.1)所述的微控制处理器读取所述的X、Y、Z三轴的磁场强度数据，并比较出所述的X、Y、Z三轴的磁场强度数据中的最大值和最小值；

(1.2)所述的微控制处理器根据所述的最大值和最小值计算得到由X与Y轴确定的二维平面内的椭圆的长半轴和短半轴大小；

(1.3)所述的微控制处理器根据所述的X轴和Y轴的最大值和最小值计算得到所述的二维平面内的椭圆的圆心位置，Z轴的中心位置为Z轴采集的磁场强度数据的均值与未装机之前采集的数据的差值；

(1.4)所述的微控制处理器修正所述的圆心位置；

(1.5)所述的微控制处理器进行椭圆修正将所述的椭圆修正为圆形；

(1.6)所述的微控制处理器根据所述的圆的半径和自身内置的阈值计算得到一个圆环，并判断所述的X、Y、Z三轴的磁场强度数据是否位于圆环内；

(1.7)如果判断结果为所述的X、Y、Z三轴的磁场强度数据是否位于圆环内，则继续步骤(2)，否则提示校验失败。

其中，所述的MEMS传感器采集所述的机械车辆的运动加速度，具体为：

所述的MEMS传感器每隔60°采集所述的机械车辆的运动加速度。

进一步地，所述的微控制处理器根据空间向量之间的关系计算出机械车辆当前俯仰角和翻滚角，具体为：

所述的微控制处理器根据以下公式计算得出当前俯仰角和翻滚角：