CN104677353A

CN104677353A - 基于mems传感器实现机械车体航向角检测的方法

Info

Publication number: CN104677353A
Application number: CN201510116771.1A
Authority: CN
Inventors: 任强; 王杰俊; 沈雪峰; 戴文鼎
Original assignee: Shanghai Huace Navigation Technology Ltd
Current assignee: Shanghai Huace Navigation Technology Ltd
Priority date: 2015-03-17
Filing date: 2015-03-17
Publication date: 2015-06-03

Abstract

本发明涉及一种基于MEMS传感器实现机械车体航向角检测的方法，其中包括：MEMS传感器采集机械车辆在转动过程中X、Y、Z三轴的磁场强度数据；MEMS传感器采集机械车辆的运动加速度；微控制处理器根据空间向量之间的关系计算出机械车辆当前俯仰角和翻滚角；微控制处理器对所述的磁场强度数据进行修正；微控制处理器计算得到航向角。采用本发明的基于MEMS传感器实现机械车体航向角检测的方法，通过MEMS传感器采集数据，实现二维平面内数据校准和补偿，获得更为精确的航向角，运行稳定、抗干扰能力强，运算精度高，具有更广泛的应用范围。

Description

基于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°采集所述的机械车辆的运动加速度。

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

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

ψ = \tan^{- 1} (a_{x} / \sqrt{{a_{y}}^{2} + {a_{z}}^{2}});

其中，ψ为俯仰角，翻滚角，a_x、a_y、a_z分别为MEMS传感器的X、Y、Z轴的输出值。

更进一步地，所述的微控制处理器对所述的磁场强度数据进行修正，具体为：

所述的微控制处理器根据以下公式对所述的磁场强度进行修正：

其中，Xh为X平面内经倾角补偿后的X轴的地磁场强度值，Yh为Y平面内经倾角补偿后的Y轴的地磁场强度值，X、Y、z分别为MEMS传感器的X、Y、Z轴输出值对应的地磁场强度值。

更进一步地，所述的微控制处理器计算得到航向角，具体为：

所述的微控制处理器通过以下公式计算得到航向角α：

α＝180-arcTan(Yh/Xh)，Xh<0；

α＝arcTan(Yh/Xh)，Xh>0，YH<0；

α＝360-arcTan(Yh/Xh)，Xh>0，YH>0

α＝90，Xh＝0，YH<0；

α＝270，Xh＝0,YH>0。

采用了本发明的基于MEMS传感器实现机械车体航向角检测的方法，具有以下效益：

(1)本发明采用了3+3六轴集成MEMS传感器，体积小，便于模块化集成到应用系统当中，且性价比高；

(2)本发明采用的六轴集成传感器，降低了多传感器的安装误差、轴系不对称误差和耦合误差等；

(3)本发明通过两组三维空间数据，实现二维平面内二维平面磁场强度零偏修正算法，解决了农业机械难于实现空间8字校准动作，提高了磁场强度数据输出的准确度；

(4)本发明采用的姿态修正算法可以解决农业机械在作业过程中由于路况复杂而引起的车身倾斜造成的误差；

(5)本发明具有运行稳定、抗干扰能力强和精度较高(静态误差0.2°)等特点，尤其满足农业机械等体积较大物体航向角的实时采集。

附图说明

图1为本发明的基于MEMS传感器实现机械车体航向角检测的方法的流程图。

图2为在一个具体实施例中实现本发明方法的装置的结构示意图。

图3为本发明的一个具体实施例的流程图。

具体实施方式

为了能够更清楚地描述本发明的技术内容，下面结合具体实施例来进行进一步的描述。

如图1所示，在一种实施方式中，本发明的基于MEMS传感器实现机械车体航向角检测的方法的步骤包括：

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

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

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

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

在一种优选的实施方式中，所述的步骤(1)和(2)之间，还包括以下步骤：

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

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

在一种优选的实施方式中，所述的MEMS传感器采集所述的机械车辆的运动加速度，具体为：

在一种优选的实施方式中，所述的微控制处理器根据空间向量之间的关系计算出机械车辆当前俯仰角和翻滚角，具体为：

ψ = \tan^{- 1} (a_{x} / \sqrt{{a_{y}}^{2} + {a_{z}}^{2}});

其中，ψ为俯仰角，翻滚角，a_x、a_y、a_z分别为MEMS传感器的X、Y、Z轴的输出值。。

在一种更优选的实施方式中，所述的微控制处理器对所述的磁场强度数据进行修正，具体为：

在一种更优选的实施方式中，所述的微控制处理器计算得到航向角，具体为：

所述的微控制处理器通过以下公式计算得到航向角α：

α＝180-arcTan(Yh/Xh)，Xh<0；

α＝arcTan(Yh/Xh)，Xh>0，Xh<0；

α＝360-arcTan(Yh/Xh)，Xh>0，Xh>0

α＝90，Xh＝0，Xh<0；

α＝270，Xh＝0，Xh>0。

结合图2和上述步骤(1)～(5)，以农业机械车体航向角检测为例，实现发明方法的装置包括：六轴MEMS(Micro-Electro-Mechanical-System，微电子机械系统)传感器和MCU处理器，MEMS传感器实时获取车体所处地磁场强度(X、Y、Z)和车体运动加速度(X、Y、Z)，二维平面磁场强度校准算法(椭圆修正和圆心修正)、车体姿态角计算和姿态修正以及航向角计算在MCU处理器中进行。

其中，六轴MEMS传感器为六轴MEMS，包括三轴地磁场数据获取模块和三轴加速度数据获取模块，解决多传感器之间坐标系不对称和耦合性问题，ACC和MAG分别为六轴MEMS传感器输出的各个轴的加速度值和磁场强度值；MCU处理器包括磁场数据二维平面校准算法模块、车体姿态计算修正模块和航向角计算模块。

结合上述硬件，六轴地磁和加速度传感器LSM303D实时采集农业机械所位于的地球磁场大小和农业机械车体的运动加速度数据，通过采集三维数据进行二维平面磁场强度零偏修正算法(椭圆修正和圆心修正)，消除检测装置所处环境铁磁材料的影响；通过加速度数据计算获得车体姿态角(横滚角和俯仰角)，实时对磁场强度进行补偿，最后通过计算获得较高精度的航向信息。

本发明在校准过程中并非简单的旋转一周经行数据采样、分析和计算，而是考虑到拖拉机实际情况(不能做3D动作而进行空间校准)，采用平面内6点采样计算，即在360°范围内每隔60°进行较长时间地磁和加速度计传感器数据采样，经滑动均值滤波后经行平面倾角补偿，从而计算校准参数。这样做法可以充分采样车体周围磁场环境(尤其是软磁和不固定磁场的影响)，从而提高航向计算精度。

如图3所示，本发明中二维平面磁场强度校准算法、车体姿态角计算和姿态修正以及航向角计算按照以下步骤进行：

第一步，硬件初始化配置，包括MEMS传感器初始化配置和MCU初始化配置，具体为时钟配置、MCU的I2C通信配置、MEMS传感器输出方式和输出频率配置、片内滤波器配置和传感器量程配置等；

第二步，软件判定是否接收到开始校准命令，前一次校准结果会保存在配置文件中，只有新的校准命令到来才会重新校准，否则使用之前的校准结果；

第三步，平面内转动农业机械一圈半，MCU读取MEMS传感器X、Y、Z三轴磁场强度数据，同时与判断是否满量程，记录采集到的当前环境磁场强度并计算出每个轴的最大值和最小值；

“判断是否满量程”的作用是防止传感器数据溢出，因为传感器一个分辨率对应一个量程，如果获得的数据在量程之外，可认为此数据是错误和无效的数据，因为此分辨率只是在本量程内有很好的线性。

第四步，地磁场磁力线易受附近铁质材料(硬铁和软铁)的影响，所以需做二维平面校准，所需操作为继续开动农业机械旋转半圈，在旋转过程中利用第三步的值进行校准，具体校准过程参照以下步骤：

1、利用最大最小值计算X与Y轴确定的二维平面内的圆半径或者椭圆的长半轴和短半轴大小；

2、利用最大最小值计算X和Y轴圆心位置，而Z轴的中心位置为Z轴采集数据的均值与未装机之前采集数据的差值；

因为电子罗盘的校准应该是3D空间内的校准，但是农业机械只能在平面内做校准动作，所以只能计算出X和Y轴的偏移量，而Z轴数据不能在360°动作内采样，所以采用此办法可以粗略地估计出周围磁场对Z轴的影响。

3、对于修正硬铁干扰，它主要产生于永久磁铁和被磁化的金属，这些干扰保持大小恒定，与传感器的指向无关，而与所处环境中周围的铁质材料有关，它所产生的干扰在输出曲线上表现为圆心位置的移动，所以修正硬铁干扰则为修正圆心位置。利用第2步中计算出来的圆心实时修正采集到得磁场强度，即可消除硬铁干扰；

4、对于软磁干扰，它来源于地球磁场和罗盘附近的任何磁性材料之间的相互作用，象硬铁材料一样，软金属也干扰地球的磁力线，不同点是，软磁的干扰程度，与罗盘的方向有关。修正软磁干扰将磁场读数旋转45°，标记出主轴线，将椭圆形改为圆形，然后，将读数反转45°，得到圆形输出，即椭圆修正。

5、利用以上计算结果判断是否校准成功，利用第1步计算的半径值和预设定的一个阈值，计算出来一个圆环大小，采集数据通过上述3和4步骤对原始数据进行校准，然后判断此数据是否位于圆环内，若在，则校准成功，否则，校准失败；

6、在农业机械半圈转动过程中重复5过程；

第五步，如果第四步校准成功，则将校准参数保存在配置文件中，以便下次继续使用；

第六步，车体姿态修正，采集车体运动加速度并做超量程判断，根据空间向量之间的关系计算出车体当前俯仰角和翻滚角，其计算公式为：其中ψ为俯仰角，翻滚角

ψ = \tan^{- 1} (a_{x} / \sqrt{{a_{y}}^{2} + {a_{z}}^{2}});

并根据以下公式对磁场强度进行修正：

第七步，航向角计算，考虑到航向角为0～360°四个象限，所以采用以下公式分象限计算航向角α：

α＝180-arcTan(Yh/Xh)，Xh<0；

α＝arcTan(Yh/Xh)，Xh>0，YH<0；

α＝360-arcTan(Yh/Xh)，Xh>0，YH>0

α＝90，Xh＝0，YH<0；

α＝270，Xh＝0，YH>0。

需要注意的是，磁北极和地理位置上的真北极并不重合，但是这个角度的差值一般为一个固定值，通过查询手册即可修正。

第八步，重复第七步即可得到恒定车体航向角输出。

在此说明书中，本发明已参照其特定的实施例作了描述。但是，很显然仍可以作出各种修改和变换而不背离本发明的精神和范围。因此，说明书和附图应被认为是说明性的而非限制性的。

Claims

1.一种基于MEMS传感器实现机械车体航向角检测的方法，其特征在于，所述的MEMS传感器与微控制处理器连接，所述的方法包括以下步骤：

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

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

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

2.根据权利要求1所述的基于MEMS传感器实现机械车体航向角检测的方法，其特征在于，所述的MEMS传感器采集所述的机械车辆在转动过程中X、Y、Z三轴的磁场强度数据，具体为：

3.根据权利要求1或2所述的基于MEMS传感器实现机械车体航向角检测的方法，其特征在于，所述的步骤(1)和(2)之间，还包括以下步骤：

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

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

4.根据权利要求1至3中任一项所述的基于MEMS传感器实现机械车体航向角检测的方法，其特征在于，所述的MEMS传感器采集所述的机械车辆的运动加速度，具体为：

5.根据权利要求1至3中任一项所述的基于MEMS传感器实现机械车体航向角检测的方法，其特征在于，所述的微控制处理器根据空间向量之间的关系计算出机械车辆当前俯仰角和翻滚角，具体为：

ψ = \tan^{- 1} (a_{x} / \sqrt{{a_{y}}^{2} + {a_{z}}^{2}});

6.根据权利要求5所述的基于MEMS传感器实现机械车体航向角检测的方法，其特征在于，所述的微控制处理器对所述的磁场强度数据进行修正，具体为：

7.根据权利要求6所述的基于MEMS传感器实现机械车体航向角检测的方法，其特征在于，所述的微控制处理器计算得到航向角，具体为：

所述的微控制处理器通过以下公式计算得到航向角α：

α＝180-arcTan(Yh/Xh)，Xh<0；

α＝arcTan(Yh/Xh)，Xh>0，Xh<0；

α＝360-arcTan(Yh/Xh)，Xh>0，Xh>0

α＝90，Xh＝0，Xh<0；

α＝270，Xh＝0，Xh>0。