基于mems傳感器實(shí)現(xiàn)機(jī)械車體航向角檢測的方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及機(jī)械車體航向角檢測技術(shù)領(lǐng)域，尤其涉及農(nóng)業(yè)機(jī)械車體航向角檢測技 術(shù)領(lǐng)域，具體是指一種基于MEMS傳感器實(shí)現(xiàn)機(jī)械車體航向角檢測的方法。
【背景技術(shù)】
[0002] 隨著傳感器和控制技術(shù)的發(fā)展和國家對(duì)農(nóng)業(yè)扶持力度的進(jìn)一步加大，精準(zhǔn)農(nóng)業(yè)正 在快速變成一種趨勢，而在農(nóng)業(yè)機(jī)械輔助駕駛控制過程中，車體航向角能夠?yàn)槠涮峁┬旭?方向，進(jìn)而為控制算法提供數(shù)據(jù)輸入。
[0003] 車體航向角多利用地球磁場方向的水平分量永遠(yuǎn)指向磁北極的特性，通過磁性傳 感器實(shí)時(shí)獲取和計(jì)算得到航向角。但是，地球磁場易受周圍鐵質(zhì)材料（硬鐵和軟鐵）干擾 的影響，使得磁性傳感器直接輸出的數(shù)據(jù)根本無法使用，固在傳感器使用前必須進(jìn)行校準(zhǔn)； 車體姿態(tài)的變化（俯仰和橫滾角）對(duì)計(jì)算輸出的航向角也存在較大的影響，故需要通過姿 態(tài)角對(duì)磁場強(qiáng)度實(shí)時(shí)進(jìn)行補(bǔ)償。
[0004] 目前，三維磁場強(qiáng)度多采用空間8字方法對(duì)磁性傳感器進(jìn)行偏移校準(zhǔn)，將平臺(tái)指 向3個(gè)已知的方向，或?qū)⑺鼛L動(dòng)和俯仰地移動(dòng)一圈，或24個(gè)指向，手持平臺(tái)卻可以做24 點(diǎn)校正，但農(nóng)業(yè)機(jī)械等不可能。對(duì)于農(nóng)業(yè)機(jī)械等大體積物體在使用過程中來說，做一個(gè)三 維空間動(dòng)作基本上是不可能的。而且如果車身俯仰和橫滾角通過其它傾角傳感器獲取，多 傳感器之間的X、Y、Z坐標(biāo)系不對(duì)稱，及其多傳感器之間的耦合性都會(huì)影響航向角的計(jì)算精 度。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的是克服了上述現(xiàn)有技術(shù)的缺點(diǎn)，提供了一種通過 MEMS(Micro-electromechanical Systems，微電子機(jī)械系統(tǒng)）傳感器采集數(shù)據(jù)，實(shí)現(xiàn)二維平 面內(nèi)數(shù)據(jù)校準(zhǔn)和補(bǔ)償，獲得更為精確的航向角的基于MEMS傳感器實(shí)現(xiàn)機(jī)械車體航向角檢 測的方法。
[0006] 為了實(shí)現(xiàn)上述目的，本發(fā)明的基于MEMS傳感器實(shí)現(xiàn)機(jī)械車體航向角檢測的方法 具有如下構(gòu)成：
[0007] 該基于MEMS傳感器實(shí)現(xiàn)機(jī)械車體航向角檢測的方法，其主要特點(diǎn)是，所述的方法 包括以下步驟：
[0008] (1)所述的MEMS傳感器采集所述的機(jī)械車輛在轉(zhuǎn)動(dòng)過程中X、Y、Z三軸的磁場強(qiáng) 度數(shù)據(jù)；
[0009] (2)所述的MEMS傳感器采集所述的機(jī)械車輛的運(yùn)動(dòng)加速度；
[0010] (3)所述的微控制處理器根據(jù)空間向量之間的關(guān)系計(jì)算出機(jī)械車輛當(dāng)前俯仰角和 翻滾角；
[0011] ⑷所述的微控制處理器對(duì)所述的磁場強(qiáng)度數(shù)據(jù)進(jìn)行修正；
[0012] (5)所述的微控制處理器計(jì)算得到航向角。
[0013] 其中，所述的MEMS傳感器采集所述的機(jī)械車輛在轉(zhuǎn)動(dòng)過程中X、Y、Z三軸的磁場強(qiáng) 度數(shù)據(jù)，具體為：
[0014] 所述的MEMS傳感器每隔60°采集所述的機(jī)械車輛在轉(zhuǎn)動(dòng)過程中Χ、Υ、Ζ三軸的磁 場強(qiáng)度數(shù)據(jù)。
[0015] 進(jìn)一步地，所述的步驟（1)和（2)之間，還包括以下步驟：
[0016] (I. 1)所述的微控制處理器讀取所述的X、Y、Z三軸的磁場強(qiáng)度數(shù)據(jù)，并比較出所 述的X、Y、Z三軸的磁場強(qiáng)度數(shù)據(jù)中的最大值和最小值；
[0017] (1. 2)所述的微控制處理器根據(jù)所述的最大值和最小值計(jì)算得到由X與Y軸確定 的二維平面內(nèi)的橢圓的長半軸和短半軸大??；
[0018] (1. 3)所述的微控制處理器根據(jù)所述的X軸和Y軸的最大值和最小值計(jì)算得到所 述的二維平面內(nèi)的橢圓的圓心位置，Z軸的中心位置為Z軸采集的磁場強(qiáng)度數(shù)據(jù)的均值與 未裝機(jī)之前采集的數(shù)據(jù)的差值；
[0019] (1. 4)所述的微控制處理器修正所述的圓心位置；
[0020] (1. 5)所述的微控制處理器進(jìn)行橢圓修正將所述的橢圓修正為圓形；
[0021] (1. 6)所述的微控制處理器根據(jù)所述的圓的半徑和自身內(nèi)置的閾值計(jì)算得到一個(gè) 圓環(huán)，并判斷所述的X、Y、Z三軸的磁場強(qiáng)度數(shù)據(jù)是否位于圓環(huán)內(nèi)；
[0022] (1. 7)如果判斷結(jié)果為所述的X、Y、Z三軸的磁場強(qiáng)度數(shù)據(jù)是否位于圓環(huán)內(nèi)，則繼 續(xù)步驟（2)，否則提示校驗(yàn)失敗。
[0023] 其中，所述的MEMS傳感器采集所述的機(jī)械車輛的運(yùn)動(dòng)加速度，具體為：
[0024] 所述的MEMS傳感器每隔60°采集所述的機(jī)械車輛的運(yùn)動(dòng)加速度。
[0025] 進(jìn)一步地，所述的微控制處理器根據(jù)空間向量之間的關(guān)系計(jì)算出機(jī)械車輛當(dāng)前俯 仰角和翻滾角，具體為：
[0026] 所述的微控制處理器根據(jù)以下公式計(jì)算得出當(dāng)前俯仰角和翻滾角：
【主權(quán)項(xiàng)】
1. 一種基于MEMS傳感器實(shí)現(xiàn)機(jī)械車體航向角檢測的方法，其特征在于，所述的MEMS傳 感器與微控制處理器連接，所述的方法包括以下步驟： (1) 所述的MEMS傳感器采集所述的機(jī)械車輛在轉(zhuǎn)動(dòng)過程中X、Y、Z三軸的磁場強(qiáng)度數(shù) 據(jù)； (2) 所述的MEMS傳感器采集所述的機(jī)械車輛的運(yùn)動(dòng)加速度； (3) 所述的微控制處理器根據(jù)空間向量之間的關(guān)系計(jì)算出機(jī)械車輛當(dāng)前俯仰角和翻滾 角； (4) 所述的微控制處理器對(duì)所述的磁場強(qiáng)度數(shù)據(jù)進(jìn)行修正； (5) 所述的微控制處理器計(jì)算得到航向角。
2. 根據(jù)權(quán)利要求1所述的基于MEMS傳感器實(shí)現(xiàn)機(jī)械車體航向角檢測的方法，其特征在 于，所述的MEMS傳感器采集所述的機(jī)械車輛在轉(zhuǎn)動(dòng)過程中X、Y、Z三軸的磁場強(qiáng)度數(shù)據(jù)，具 體為： 所述的MEMS傳感器每隔60°采集所述的機(jī)械車輛在轉(zhuǎn)動(dòng)過程中X、Y、Z三軸的磁場強(qiáng) 度數(shù)據(jù)。
3. 根據(jù)權(quán)利要求1或2所述的基于MEMS傳感器實(shí)現(xiàn)機(jī)械車體航向角檢測的方法，其特 征在于，所述的步驟（1)和（2)之間，還包括以下步驟： (1. 1)所述的微控制處理器讀取所述的X、Y、Z三軸的磁場強(qiáng)度數(shù)據(jù)，并比較出所述的 X、Y、Z三軸的磁場強(qiáng)度數(shù)據(jù)中的最大值和最小值； (1. 2)所述的微控制處理器根據(jù)所述的最大值和最小值計(jì)算得到由X與Y軸確定的二 維平面內(nèi)的橢圓的長半軸和短半軸大??； (1. 3)所述的微控制處理器根據(jù)所述的X軸和Y軸的最大值和最小值計(jì)算得到所述的 二維平面內(nèi)的橢圓的圓心位置，Z軸的中心位置為Z軸采集的磁場強(qiáng)度數(shù)據(jù)的均值與未裝 機(jī)之前采集的數(shù)據(jù)的差值； (1. 4)所述的微控制處理器修正所述的圓心位置； (1. 5)所述的微控制處理器進(jìn)行橢圓修正將所述的橢圓修正為圓形； (1. 6)所述的微控制處理器根據(jù)所述的圓形的半徑和自身內(nèi)置的閾值計(jì)算得到一個(gè)圓 環(huán)，并判斷所述的X、Y、Z三軸的磁場強(qiáng)度數(shù)據(jù)是否位于圓環(huán)內(nèi)； (1. 7)如果判斷結(jié)果為所述的X、Y、Z三軸的磁場強(qiáng)度數(shù)據(jù)是否位于圓環(huán)內(nèi)，則繼續(xù)步 驟（2)，否則提示校驗(yàn)失敗。
4. 根據(jù)權(quán)利要求1至3中任一項(xiàng)所述的基于MEMS傳感器實(shí)現(xiàn)機(jī)械車體航向角檢測的 方法，其特征在于，所述的MEMS傳感器采集所述的機(jī)械車輛的運(yùn)動(dòng)加速度，具體為： 所述的MEMS傳感器每隔60°采集所述的機(jī)械車輛的運(yùn)動(dòng)加速度。
5. 根據(jù)權(quán)利要求1至3中任一項(xiàng)所述的基于MEMS傳感器實(shí)現(xiàn)機(jī)械車體航向角檢測的 方法，其特征在于，所述的微控制處理器根據(jù)空間向量之間的關(guān)系計(jì)算出機(jī)械車輛當(dāng)前俯 仰角和翻滾角，具體為： 所述的微控制處理器根據(jù)以下公式計(jì)算得出當(dāng)前俯仰角和翻滾角：
其中，Φ為俯仰角，P翻滾角，ax、ay、az分別為MEMS傳感器的X、Y、Z軸的輸出值。
6. 根據(jù)權(quán)利要求5所述的基于MEMS傳感器實(shí)現(xiàn)機(jī)械車體航向角檢測的方法，其特征在 于，所述的微控制處理器對(duì)所述的磁場強(qiáng)度數(shù)據(jù)進(jìn)行修正，具體為： 所述的微控制處理器根據(jù)以下公式對(duì)所述的磁場強(qiáng)度進(jìn)行修正：
其中，Xh為X平面內(nèi)經(jīng)傾角補(bǔ)償后的X軸的地磁場強(qiáng)度值，Yh為Y平面內(nèi)經(jīng)傾角補(bǔ)償 后的Y軸的地磁場強(qiáng)度值，X、Υ、ζ分別為MEMS傳感器的X、Y、Z軸輸出值對(duì)應(yīng)的地磁場強(qiáng) 度值。
7. 根據(jù)權(quán)利要求6所述的基于MEMS傳感器實(shí)現(xiàn)機(jī)械車體航向角檢測的方法，其特征在 于，所述的微控制處理器計(jì)算得到航向角，具體為： 所述的微控制處理器通過以下公式計(jì)算得到航向角α : a = 18〇-arcTan (Yh/Xh), Xh<0 ； α = arcTan (Yh/Xh), Xh>0, Xh<0 ； a = 360-arcTan (Yh/Xh), Xh>0, Xh>0 a = 90, Xh = 0, Xh〈0 ; a = 270, Xh = 0, Xh>0。
【專利摘要】本發(fā)明涉及一種基于MEMS傳感器實(shí)現(xiàn)機(jī)械車體航向角檢測的方法，其中包括：MEMS傳感器采集機(jī)械車輛在轉(zhuǎn)動(dòng)過程中X、Y、Z三軸的磁場強(qiáng)度數(shù)據(jù)；MEMS傳感器采集機(jī)械車輛的運(yùn)動(dòng)加速度；微控制處理器根據(jù)空間向量之間的關(guān)系計(jì)算出機(jī)械車輛當(dāng)前俯仰角和翻滾角；微控制處理器對(duì)所述的磁場強(qiáng)度數(shù)據(jù)進(jìn)行修正；微控制處理器計(jì)算得到航向角。采用本發(fā)明的基于MEMS傳感器實(shí)現(xiàn)機(jī)械車體航向角檢測的方法，通過MEMS傳感器采集數(shù)據(jù)，實(shí)現(xiàn)二維平面內(nèi)數(shù)據(jù)校準(zhǔn)和補(bǔ)償，獲得更為精確的航向角，運(yùn)行穩(wěn)定、抗干擾能力強(qiáng)，運(yùn)算精度高，具有更廣泛的應(yīng)用范圍。
【IPC分類】G01C21-08
【公開號(hào)】CN104677353
【申請(qǐng)?zhí)枴緾N201510116771
【發(fā)明人】任強(qiáng), 王杰俊, 沈雪峰, 戴文鼎
【申請(qǐng)人】上海華測導(dǎo)航技術(shù)股份有限公司
【公開日】2015年6月3日
【申請(qǐng)日】2015年3月17日