本實用新型屬于導(dǎo)航控制和位置跟蹤領(lǐng)域，特別涉及一種MEMS慣性測量器件與衛(wèi)星定位系統(tǒng)組合導(dǎo)航測角的方法。

背景技術(shù)：

由自動駕駛儀控制的農(nóng)用車輛是高效、精準農(nóng)田作業(yè)的必要設(shè)備。與人力作業(yè)相比，自動駕駛儀控制的車輛能夠完成更精準的駕駛，并且重復(fù)作業(yè)時不會感到疲憊。因此，自動駕駛農(nóng)用車輛能夠有效地提高農(nóng)田作業(yè)效率，減少作業(yè)成本。

在自動駕駛系統(tǒng)中，車輪相對于車輛主體的轉(zhuǎn)動角度是進行車輛控制必不可少的信息?，F(xiàn)有的自動駕駛系統(tǒng)中采用的機械式角度傳感器雖然能夠提供符合要求的角度信息，但安裝、檢修及更換過程復(fù)雜，耗費時間和人力。中國專利申請?zhí)?00680045195.5中所述的使用慣性測量單元（IMU）作為角度傳感器，通過算法計算車輪轉(zhuǎn)動角度，但系統(tǒng)在車輛主體和車輪上分別安裝慣性測量器件，增加了系統(tǒng)成本，并且慣性器件受車體振動影響較大，GPS天線解算速度與實際計算速度之間存在桿臂誤差，使得系統(tǒng)精度受到影響。

技術(shù)實現(xiàn)要素：

為了解決上述技術(shù)問題，克服現(xiàn)有技術(shù)的不足，本實用新型提出了一種廉價、簡單的雙GNSS系統(tǒng)及單MEMS陀螺組成的系統(tǒng)，完成自動駕駛系統(tǒng)中車輪相對于主體轉(zhuǎn)角的測量問題。

本實用新型解決其技術(shù)問題所采用的技術(shù)解決方案是：

基于雙GNSS天線及單軸MEMS陀螺的前輪測角系統(tǒng)，其主要由能安裝于車輛車輪的單軸MEMS陀螺，能安裝于車體兩側(cè)的兩個GNSS接收機以及導(dǎo)航控制計算機組成；所述兩個GNSS接收機以一定距離安裝在車體兩側(cè)，安裝位置之間的直線垂直于車輛車頭到車尾的中軸線；單軸MEMS陀螺，安裝在車輪上，敏感軸垂直于地面。

利用單軸MEMS陀螺和兩個GNSS接收機對系統(tǒng)的每個部分測量一組運動信息數(shù)據(jù)，對每部分的運動信息數(shù)據(jù)進行信息處理獲得主體的運動姿態(tài)信息和主體與轉(zhuǎn)動機構(gòu)之間的相對角運動信息。

GNSS天線和單軸MEMS陀螺完成各運動部分數(shù)據(jù)的采集，導(dǎo)航控制計算機對GNSS天線解算的速度進行桿臂誤差補償后代入卡爾曼濾波器模型，估計得到車輪相對于主體的轉(zhuǎn)動角度。

所述運動信息數(shù)據(jù)包括：雙GNSS天線系統(tǒng)測量系統(tǒng)主體的運動線速度，航向角信息，經(jīng)緯度信息等，運動部分MEMS陀螺測量相關(guān)子系統(tǒng)的轉(zhuǎn)動角速度。

所述信息處理包括對雙GNSS天線系統(tǒng)輸出主體的運動速率、航向角及經(jīng)緯度等信息的處理；運動子系統(tǒng)安裝的MEMS陀螺進行數(shù)據(jù)處理后積分獲得轉(zhuǎn)動角度信息，并與雙GNSS輸出信息進行組合卡爾曼濾波進行校正，輸出高精度的轉(zhuǎn)角信息。

進一步地，所述雙GNSS接收機定位采用偽距相對差分進行定位運算，保證兩個GNSS接收機觀測到同一組衛(wèi)星，這樣將兩個GNSS接收機的定位數(shù)據(jù)進行差分運算，可以去除或降低大氣層誤差，電離層誤差，衛(wèi)星軌道誤差等共有誤差，輸出精度較高的系統(tǒng)運動速度、航向角及經(jīng)緯度等信息。

進一步地，所述利用單軸MEMS陀螺進行數(shù)據(jù)測量的處理，是陀螺數(shù)據(jù)與雙GNSS航向角度微分得到的角速度相減得到運動子系統(tǒng)相對于主體的轉(zhuǎn)動角速率的處理，通過對處理后的相對轉(zhuǎn)動角速率進行積分得到相對轉(zhuǎn)動角度，然后通過主體運動與相對轉(zhuǎn)角之間的運動模型，建立卡爾曼濾波器的狀態(tài)方程和量測方程，選取航向角輸入為量測量，對相對轉(zhuǎn)角誤差進行估計，并對相對轉(zhuǎn)角輸出結(jié)果進行校正。

進一步地，所述測量計算時需要的速度信息是車輛固定輪軸的速度，而GNSS天線與固定輪軸之間存在桿臂，當(dāng)車輛轉(zhuǎn)向時，由于桿臂的存在造成GNSS天線測量的速度與所需速度之間有桿臂引起的速度誤差，因此需要對桿臂誤差進行補償。

與現(xiàn)有實用新型相比，本實用新型的主要優(yōu)點在于：

1）本實用新型采用的雙GNSS天線系統(tǒng)能夠消除或降低大氣層誤差，電離層誤差，衛(wèi)星軌道誤差等共有誤差，輸出精度較高的系統(tǒng)運動速度信息。并通過相應(yīng)的算法輸出系統(tǒng)主體的航向、橫滾等信息，比慣導(dǎo)系統(tǒng)輸出的姿態(tài)信息有更高的精度，為導(dǎo)航控制提供保障。

2）本實用新型采用單軸MEMS陀螺采集系統(tǒng)的運動信息，降低了系統(tǒng)成本，簡化了系統(tǒng)安裝過程。同時，車輛主體的姿態(tài)和運動信息由雙GNSS天線系統(tǒng)獲得，降低了車輛運動過程中振動對數(shù)據(jù)的影響，提高了系統(tǒng)角度輸出的精度。

3）補償了GNSS天線速度中的桿臂引起的速度誤差，提高了卡爾曼濾波器的輸入精度，進而提高最終輸出的角度結(jié)果精度。

附圖說明

圖1為本實用新型可應(yīng)用于的車輛簡化結(jié)構(gòu)示意圖。

圖2 為本實用新型車輪相對轉(zhuǎn)角數(shù)據(jù)處理原理框圖。

具體實施方式

如圖1所示，該實用新型專利的一項實例的結(jié)構(gòu)主要包括：安裝在車輛主體兩側(cè)的GNSS天線（1）和GNSS天線（2），安裝在前輪結(jié)構(gòu)上的單軸MEMS陀螺（3），以及相應(yīng)的硬件電路和導(dǎo)航計算機。GNSS天線（1）和（2）分別安裝在車體兩側(cè)，安裝位置之間基線垂直于車頭到車尾的中軸線；雙天線系統(tǒng)負責(zé)接收主體運動的速度、位置等信息，并通過雙GNSS定姿算法計算出載體的航向角。單軸MEMS陀螺3安裝在車輪轉(zhuǎn)向部件上，隨車輪轉(zhuǎn)動，敏感軸垂直于地面，測量車輪轉(zhuǎn)動角速率。

圖2所示的是在本實用新型專利中前輪轉(zhuǎn)角的精確估計結(jié)果的數(shù)據(jù)處理框圖。

對于各傳感器采集到的信息進行處理，對單軸MEMS陀螺（3）采集角速度進行積分可得車輪在慣性系內(nèi)的轉(zhuǎn)動角度，用此結(jié)果減去雙GNSS系統(tǒng)（1,2）解算的車體航向角度變化可得車輪相對于車輛主體的角度；由于陀螺零偏，積分結(jié)果存在隨時間積累的誤差，需要通過卡爾曼濾波器進行濾波處理，對轉(zhuǎn)向角進行估計得到精確結(jié)果。

卡爾曼濾波器的狀態(tài)量為轉(zhuǎn)向角計算誤差和陀螺零偏，陀螺零偏為一階馬爾科夫過程；觀測量選擇為通過車輛運動學(xué)所計算的車輪相對于主體的轉(zhuǎn)動角度與積分計算的轉(zhuǎn)向角度之差；通過上述模型估計出角度計算的誤差，對角度進行校正。

GNSS天線（1）和GNSS天線（2）組成的雙天線系統(tǒng)接收衛(wèi)星信號，通過解算得到車輛主體的前向運動線速度、運動航向角、車輛橫滾角度等信息。對雙GNSS系統(tǒng)輸出的航向角信息進行微分可以得到車頭的轉(zhuǎn)向角速度；

單軸MEMS陀螺（3）測量數(shù)據(jù)為車輛車輪在慣性系內(nèi)的轉(zhuǎn)向速度，包括車頭的轉(zhuǎn)向速度和車輪相對于車輛主體的相對轉(zhuǎn)動速度。導(dǎo)航控制計算機將上述傳感器的信息進行分析處理，通過相應(yīng)的算法計算得到車輛車輪相對于主體的轉(zhuǎn)動角度。

由于車輛運動學(xué)計算車輛車輪轉(zhuǎn)角時，需要的速度是車輛固定輪軸的速度，但GNSS天線安裝位置與固定軸之間存在桿臂，車輛轉(zhuǎn)動時引起速度誤差，所以需要補償GNSS天線到固定軸之間的桿臂引起的速度誤差。

車輪上安裝的單軸MEMS陀螺的輸出角速率為航向角變化速率和前輪轉(zhuǎn)動速率：

通過對前輪安裝的單軸MEMS陀螺輸出結(jié)果進行積分得到前輪的轉(zhuǎn)動角度，

為航向角變化角度，為前輪相對車輛主體轉(zhuǎn)動角度。

通過對陀螺數(shù)據(jù)積分結(jié)果與雙天線系統(tǒng)輸出的航向角變化角度作差得到轉(zhuǎn)動角度

。同時，利用雙天線系統(tǒng)實時差分系統(tǒng)輸出的速度和軸距等信息計算得前輪的絕對轉(zhuǎn)向角。主體航向角變化速率與前輪絕對轉(zhuǎn)向角之間關(guān)系可表示為：

其中，為車輛軸距，為航向角變化速率，可由雙天線系統(tǒng)輸出的航向角數(shù)據(jù)實時微分得到。速度在計算時需要進行桿臂誤差補償，GNSS天線（1）與固定軸中心的桿臂，由雙GNSS系統(tǒng)輸出的車輛航向角及橫滾角信息微分得到車輛在相應(yīng)方向上的角速率和角速率，考慮實際工作條件，車輛行駛過程中俯仰角的變化忽略不計。車輛載體系到導(dǎo)航系（地理系）之間的坐標轉(zhuǎn)換矩陣為：

車輛轉(zhuǎn)動角速率：；

則由桿臂造成的速度誤差。

將GNSS天線測量的速度按照運動航向角方向分解到正東和正北兩個方向，即，則經(jīng)過桿臂誤差補償?shù)乃俣葹椤Ｗ詈髮⑺俣仍诤较蚪欠较蛏虾铣傻玫阶罱K的速度信息。

通過上述過程所描述的各個參數(shù)，建立系統(tǒng)方程，通過卡爾曼濾波器，對角度進行最優(yōu)估計，得到精度較高的車輪轉(zhuǎn)動角度信息。

本實用新型說明書中未作詳細描述的內(nèi)容屬于本領(lǐng)域?qū)I(yè)技術(shù)人員公知的現(xiàn)有技術(shù)。

以上所述僅是本實用新型的優(yōu)選實施方式，應(yīng)當(dāng)指出，對于本技術(shù)領(lǐng)域的普通技術(shù)人員來說，在不脫離本實用新型原理的前提下，還可以做出若干改進和潤飾，這些改進和潤飾也應(yīng)視為本實用新型的保護范圍。