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

权利要求书

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

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

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

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

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

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

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

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

3.根据权利要求1或2所述的基于MEMS传感器实现机械车体航向角检测的方法，其特征在于，所述的步骤(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)，否则提示校验失败。

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

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

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

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

ψ=tan-1(ax/ay2+az2);]]>

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

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

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

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

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。