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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

附圖說(shuō)明

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

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

具體實(shí)施方式

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

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

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

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

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

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

由于車(chē)輛運(yùn)動(dòng)學(xué)計(jì)算車(chē)輛車(chē)輪轉(zhuǎn)角時(shí)，需要的速度是車(chē)輛固定輪軸的速度，但GNSS天線(xiàn)安裝位置與固定軸之間存在桿臂，車(chē)輛轉(zhuǎn)動(dòng)時(shí)引起速度誤差，所以需要補(bǔ)償GNSS天線(xiàn)到固定軸之間的桿臂引起的速度誤差。

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

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

為航向角變化角度，為前輪相對(duì)車(chē)輛主體轉(zhuǎn)動(dòng)角度。

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

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

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

車(chē)輛轉(zhuǎn)動(dòng)角速率：；

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

將GNSS天線(xiàn)測(cè)量的速度按照運(yùn)動(dòng)航向角方向分解到正東和正北兩個(gè)方向，即，則經(jīng)過(guò)桿臂誤差補(bǔ)償?shù)乃俣葹?。最后將速度在航向角方向上合成得到最終的速度信息。

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

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

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