一種基于單天線的gnss-ins車輛定姿方法
【專利摘要】本發(fā)明公開了一種基于單天線的GNSS?INS車輛定姿方法，包括以下步驟：將GNSS天線固定安裝于車輛中心質(zhì)心位置，同時將MEMS傳感器的IMU測量單元固定安裝于車輛的轉(zhuǎn)向車軸上；通過GNSS天線獲取車輛位置和速度信息，通過IMU測量單元獲取車輛的航向角速率信息；通過加速度計和陀螺儀融合的方法進(jìn)行車輛姿態(tài)角解算；根據(jù)車輛位置、速度和航向角速率信息進(jìn)行車輛航向角解算。采用單天線GNSS與低成本IMU/MEMS傳感器組組合，基于農(nóng)用車輛的運(yùn)動學(xué)模型進(jìn)行定姿、定向，具有短期精度高、長期穩(wěn)定性高的優(yōu)點(diǎn)，避免了使用陀螺儀時的發(fā)散現(xiàn)象，同時降低GNSS測姿的噪聲水平，測姿精度可提高數(shù)倍。
【專利說明】
-種基于單天線的GNSS-1 NS車輛定姿方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明設(shè)及農(nóng)業(yè)車輛自動化領(lǐng)域，尤其設(shè)及一種基于單天線的GNSS-INS車輛定姿 方法。
【背景技術(shù)】
[0002] 隨著W生物技術(shù)、GNSS技術(shù)、信息技術(shù)為先導(dǎo)的現(xiàn)在科學(xué)技術(shù)發(fā)展及其在農(nóng)業(yè)上 的廣泛應(yīng)用，精準(zhǔn)農(nóng)業(yè)應(yīng)用而生，而農(nóng)機(jī)自動駕駛系統(tǒng)作為其核屯、載體，其在耕地、播種、施 月己、灌概、植保和收獲等各種農(nóng)藝作業(yè)中，將有著十分重要的意義。
[0003] 為提高農(nóng)業(yè)車輛自動駕駛系統(tǒng)的精度，需要測量農(nóng)業(yè)車輛位置坐標(biāo)、航向角W及 姿態(tài)角等導(dǎo)航精度，且測量值應(yīng)盡量準(zhǔn)確。特別是當(dāng)裝備了 GNSS接收機(jī)的農(nóng)用車輛在田間 行走時，由于輪胎在±壤里受力不均，車體發(fā)生顛鑛導(dǎo)致GNSS接收天線的位置傾斜，其結(jié)果 是GNSS天線與車體質(zhì)屯、不重合，因此需要精確已知車體的姿態(tài)角和航向角。
[0004] 目前常采用多天線GNSS測姿的方法，但是該方法具有成本高、天線接收延遲、實時 性差、安裝難度大等缺點(diǎn)；而陸地導(dǎo)航系統(tǒng)的中常用慣性導(dǎo)航INS進(jìn)行定姿，也由于其成本 高等原因不適用于農(nóng)用車輛的定姿定向。

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

[0005] 鑒于目前農(nóng)業(yè)車輛自動化領(lǐng)域存在的上述不足，本發(fā)明提供一種基于單天線的 GNSS-INS車輛定姿方法，能夠通過單天線實現(xiàn)對車輛定姿定向，提高了定姿精度。
[0006] 為達(dá)到上述目的，本發(fā)明的實施例采用如下技術(shù)方案：
[0007] -種基于單天線的GNSS-INS車輛定姿方法，所述基于單天線的GNSS-INS車輛定姿 方法包括W下步驟：
[000引將GNSS天線固定安裝于車輛中屯、質(zhì)屯、位置，同時將MEMS傳感器的IMUii量單元固 定安裝于車輛的轉(zhuǎn)向車軸上；
[0009] 通過GNSS天線獲取車輛位置和速度信息，通過IMU測量單元獲取車輛的航向角速 率?目息；
[0010] 通過加速度計和巧螺儀融合的方法進(jìn)行車輛姿態(tài)角解算；
[0011] 根據(jù)車輛位置、速度和航向角速率信息進(jìn)行車輛航向角解算。
[0012] 依照本發(fā)明的一個方面，所述將MEMS傳感器的IMU測量單元固定安裝于車輛的轉(zhuǎn) 向車軸上時，需保證IMU測量單元的Ξ軸坐標(biāo)系與機(jī)車Ξ軸坐標(biāo)系一致。
[0013] 依照本發(fā)明的一個方面，所述通過加速度計和巧螺儀融合的方法進(jìn)行車輛姿態(tài)角 解算包括：
[0014] 當(dāng)車輛處于靜止或者勻速運(yùn)動的情況下時，其加速度值為零，根據(jù)加速度計的原 理，可準(zhǔn)確得到車輛的橫滾角和俯仰角：
[0015] 目= sin_i(ax)
[0016]
[0017] 式中，ax、ay為載體坐標(biāo)系的X軸和y軸的加速度;θ為俯仰角；φ為橫滾角；
[0018] 當(dāng)車輛運(yùn)動或者抖動比較大時，引入巧螺儀，采用加速度計和巧螺儀融合的方法 進(jìn)行，具體如下：
[0019] 首先構(gòu)建橫滾角和俯仰角組合濾波器，具體濾波器如下：
[0020]
[0021] 其中待估參數(shù)如下：
[0022] x(t) =[白 Φ ωχ cOy c0z]T;
[0023] 觀測向量如下：
[0024] Z(t) = [bfx 6fy b …bx 6"iby 6"ibz]T，其中bfx,y和b?ibx,y,z為加速計和巧螺儀輸出 值；
[0025] 狀態(tài)轉(zhuǎn)移矩陣如下：
[0026]
[0027] 觀測設(shè)計矩陣如下：
[002引 f(x(t)) = [sin0 003φ ωχ cOy c0z]T
[0029] 根據(jù)上述參數(shù)計算得到車輛的橫滾角和俯仰角。
[0030] 依照本發(fā)明的一個方面，所述通過加速度計和巧螺儀融合的方法進(jìn)行車輛姿態(tài)角 解算包括：
[0031] 當(dāng)車輛處于靜止或者勻速運(yùn)動的情況下時，其加速度值為零，根據(jù)加速度計的原 理，可準(zhǔn)確得到車輛的橫滾角和俯仰角：
[0034] 式中，ax、ay為載體坐標(biāo)系的X軸和y軸的加速度;Θ為俯仰角；Φ為橫滾角；
[0035] 當(dāng)車輛運(yùn)動或者抖動比較大時，引入巧螺儀，采用加速度計和巧螺儀融合的方法 進(jìn)行，具體如下：
[0036] 由于傾角(橫滾角或者俯仰角)和傾角角速度存在導(dǎo)數(shù)關(guān)系，因此可將系統(tǒng)傾斜真 實角度^可用來做一個狀態(tài)向量，采用加速度計估計出巧螺儀常值偏差b，W此偏差作為狀 態(tài)向量得到相應(yīng)的狀態(tài)方程和觀測方程：
[0037]
[00；3 引
[0039] 式中，Wgyr。為包含固定偏差的巧螺儀輸出角速度，巧,ηρ為加速度計經(jīng)處理后得到 的角度值，Wg為巧螺儀測量噪聲，Wa為加速度計測量噪聲，b為巧螺儀漂移誤差，Wg與Wa相互 獨(dú)立，并假設(shè)二者為滿足正態(tài)分布的白色噪聲。同時設(shè)定卡爾曼濾波器的系統(tǒng)過程噪聲協(xié) 方差陣QW及測量誤差的協(xié)方差矩陣R，其形式如下：
[0040]
[0041 ]式中，q_ac C e和9_旨7 r 0分別是加速度計和巧螺儀測量的協(xié)方差，;r_ac C e為加速度 計測量噪聲；
[0042] 根據(jù)上述參數(shù)計算得到車輛的橫滾角和俯仰角。
[0043] 依照本發(fā)明的一個方面，所述根據(jù)車輛位置、速度和航向角速率信息進(jìn)行車輛航 向角解算包括：
[0044] 航向角可由東、北向速度計算得到，即：
[0045] i])p = arctan(vE/vN)
[0046] 式中，Φρ為GNSS航向角，VE、VN為東北向速度；
[0047] 當(dāng)車輛靜止或者速度非常小時，引入Ζ軸巧螺儀和GNSS進(jìn)行組合來進(jìn)行航向角的 高精度解算，系統(tǒng)方程和觀測方程如下：
[0050] 式中，恥NSS為GNSS輸出的航向角，br為巧螺儀漂移誤差，Whd為航向過程噪聲，Tb為一 階馬爾科夫相關(guān)時間。
[0051] 線性化之后如下：
[0054] 當(dāng)車輛運(yùn)動時，Hyaw=[l 0]，其它情況下Hyaw=[0 0]。
[0055] 依照本發(fā)明的一個方面，所述MEMS傳感器為六軸MEMS傳感器。
[0056] 本發(fā)明實施的優(yōu)點(diǎn)：本發(fā)明所述的基于單天線的GNSS-INS車輛定姿方法，包括W 下步驟:將GNSS天線固定安裝于車輛中屯、質(zhì)屯、位置，同時將MEMS傳感器的IMU測量單元固定 安裝于車輛的轉(zhuǎn)向車軸上;通過GNSS天線獲取車輛位置和速度信息，通過IMU測量單元獲取 車輛的航向角速率信息;通過加速度計和巧螺儀融合的方法進(jìn)行車輛姿態(tài)角解算;根據(jù)車 輛位置、速度和航向角速率信息進(jìn)行車輛航向角解算。采用單天線GNSS與低成本IMU/MEMS 傳感器組組合，基于農(nóng)用車輛的運(yùn)動學(xué)模型進(jìn)行定姿、定向，該方法吸收了IM啡它螺儀短期 精度高和GNSS單天線測姿長期穩(wěn)定性高的優(yōu)點(diǎn)，避免了使用巧螺儀時的發(fā)散現(xiàn)象，同時降 低GNSS測姿的噪聲水平，測姿精度可提高數(shù)倍。
[0057]下面將對本發(fā)明實施例中的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，所描述的實施 例僅僅是本發(fā)明一部分實施例，而不是全部的實施例?；诒景l(fā)明中的實施例，本領(lǐng)域普通 技術(shù)人員在沒有做出創(chuàng)造性勞動前提下所獲得的所有其他實施例，都屬于本發(fā)明保護(hù)的 范圍。
【具體實施方式】
[0化引
[0化9] 實施例一
[0060] -種基于單天線的GNSS-INS車輛定姿方法，所述基于單天線的GNSS-INS車輛定姿 方法包括W下步驟：
[0061 ] 步驟S1:將GNSS天線固定安裝于車輛中屯、質(zhì)屯、位置，同時將MEMS傳感器的IMU測量 單元固定安裝于車輛的轉(zhuǎn)向車軸上；
[0062] 所述步驟S1將GNSS天線固定安裝于車輛中屯、質(zhì)屯、位置，同時將MEMS傳感器的IMU 測量單元固定安裝于車輛的轉(zhuǎn)向車軸上的【具體實施方式】可為:將GNSS天線固定安裝于機(jī)車 中屯、質(zhì)屯、位置，W精確測量出載體機(jī)車的位置和速度，傳感器MEMS-IMU測量單元固定安裝 于載體機(jī)車上時需保證IMU的Ξ軸坐標(biāo)系與機(jī)車Ξ軸坐標(biāo)系一致;同時IMU測量單元需隨機(jī) 車車輪轉(zhuǎn)向而實時轉(zhuǎn)向，即將IMU單元固裝于車軸W便IMU能精確敏感機(jī)車的航向角速率， 使IMU敏感航向信息與GNSS測量的航向信息一致。
[0063] 步驟S2:通過GNSS天線獲取車輛位置和速度信息，通過IMU測量單元獲取車輛的航 向角速率信息；
[0064] 所述步驟S2通過GNSS天線獲取車輛位置和速度信息，通過IMU測量單元獲取車輛 的航向角速率信息，采用了 3+3六軸集成MEMS-IMU傳感器，能精確獲得車輛的航向角速率信 息。
[0065] 步驟S3:通過加速度計和巧螺儀融合的方法進(jìn)行車輛姿態(tài)角解算；
[0066] 所述步驟S3通過加速度計和巧螺儀融合的方法進(jìn)行車輛姿態(tài)角解算具體可包括：
[0067] 因 MEMS-IMU的巧螺測量精度無法敏感地球自轉(zhuǎn)角速率，故MEMS系統(tǒng)無法進(jìn)行全自 主對準(zhǔn)；當(dāng)車輛處于靜止或者勻速運(yùn)動的情況下時，其加速度值為零，根據(jù)加速度計的原 理，可準(zhǔn)確得到車輛的橫滾角和俯仰角：
[0070] 式中，ax、ay為載體坐標(biāo)系的X軸和y軸的加速度;Θ為俯仰角；Φ為橫滾角；
[0071] 當(dāng)車輛運(yùn)動或者抖動比較大時，由于其它方向的加速度的影響，采用上式不能準(zhǔn) 確計算出姿態(tài)角，為此引入巧螺儀，采用加速度計和巧螺儀融合的方法進(jìn)行解算，具體如 下：
[0072] 首先構(gòu)建橫滾角和俯仰角組合濾波器，具體濾波器如下：
[0073]
[0074] 其中待估參數(shù)如下：
[0075] x(t) =[白 Φ ωχ cOy c0z]T;
[0076] 觀測向量如下：
[0077] Z(t) = [bfx 6fy b …bx 6"iby 6"ibz]T，其中bfx,y和b?ibx,y,z為加速計和巧螺儀輸出 值；
[0078] 狀態(tài)轉(zhuǎn)移矩陣如下：
[0079]
[0080] 觀測設(shè)計矩陣如下：
[0081] f(x(t)) = [sin目 ο03φ ωχ cOy c0z]T
[0082] 根據(jù)上述參數(shù)計算得到車輛的橫滾角和俯仰角，從而解算出了車輛的姿態(tài)角。
[0083] 步驟S4:根據(jù)車輛位置、速度和航向角速率信息進(jìn)行車輛航向角解算。
[0084] 所述步驟S4根據(jù)車輛位置、速度和航向角速率信息進(jìn)行車輛航向角解算的具體實 施方式可為：
[0085] 單天線GNSS測姿主要利用GNSS速度，其航向角可由東、北向速度計算得到，即：
[0086] i])p = arctan(vE/vN)
[0087] 式中，Φρ為GNSS航向角，VE、VN為東北向速度；
[0088] 在實際應(yīng)用中，GNSS航向角可直接從GNSS接收機(jī)輸出的NMEA語句中提取，但是當(dāng) 車輛靜止或者速度非常小時，數(shù)學(xué)上造成數(shù)值不穩(wěn)定性，而且速度測量誤差可能會淹沒真 實速度值。
[0089] 為提高其航向精度，引入Z軸巧螺儀和GNSS進(jìn)行組合，巧螺儀沿載體坐標(biāo)系Z軸向 上安裝。系統(tǒng)方程和觀測方程如下：
[0092] 式中，恥NSS為GNSS輸出的航向角，br為巧螺儀漂移誤差，whd為航向過程噪聲，Tb為一 階馬爾科夫相關(guān)時間。
[0093] 線性化之后如下：
[0094]
[0095]
[0096] 當(dāng)車輛運(yùn)動時，Hyaw=[l 0]，其它情況下Hyaw=[0 0]。
[0097] 通過上述的車輛姿態(tài)角解算及航向角解算，最終實現(xiàn)了基于單天線的GNSS-INS車 輛的定姿。
[0098] 本發(fā)明所述的基于單天線的GNSS-INS車輛定姿方法，包括W下步驟:將GNSS天線 固定安裝于車輛中屯、質(zhì)屯、位置，同時將MEMS傳感器的IMU測量單元固定安裝于車輛的轉(zhuǎn)向 車軸上;通過GNSS天線獲取車輛位置和速度信息，通過IMU測量單元獲取車輛的航向角速率 信息;通過加速度計和巧螺儀融合的方法進(jìn)行車輛姿態(tài)角解算;根據(jù)車輛位置、速度和航向 角速率信息進(jìn)行車輛航向角解算。采用單天線GNSS與低成本IMU/MEMS傳感器組組合，基于 農(nóng)用車輛的運(yùn)動學(xué)模型進(jìn)行定姿、定向，該方法吸收了IM啡它螺儀短期精度高和GNSS單天線 測姿長期穩(wěn)定性高的優(yōu)點(diǎn)，避免了使用巧螺儀時的發(fā)散現(xiàn)象，同時降低GNSS測姿的噪聲水 平，測姿精度可提高數(shù)倍。本實施例所述的測姿方法結(jié)合IMU傳感器，經(jīng)測試其航向角誤差 小于0.2°，俯仰角和橫滾角誤差小于0.08°。在實際應(yīng)用中，采用了3+3六軸集成IMU傳感器， 體積小，重量輕，性價比高，模塊化設(shè)計便于集成到農(nóng)業(yè)機(jī)械輔助駕駛控制系統(tǒng)之中。
[0099] 在實際應(yīng)用中，還可使用傾角傳感器代替IMU傳感器實現(xiàn)該定姿方案。
[0100] 實施例二
[0101] -種基于單天線的GNSS-INS車輛定姿方法，所述基于單天線的GNSS-INS車輛定姿 方法包括W下步驟：
[0102] 步驟S1:將GNSS天線固定安裝于車輛中屯、質(zhì)屯、位置，同時將MEMS傳感器的IMU測量 單元固定安裝于車輛的轉(zhuǎn)向車軸上；
[0103] 所述步驟S1將GNSS天線固定安裝于車輛中屯、質(zhì)屯、位置，同時將MEMS傳感器的IMU 測量單元固定安裝于車輛的轉(zhuǎn)向車軸上的【具體實施方式】可為:將GNSS天線固定安裝于機(jī)車 中屯、質(zhì)屯、位置，W精確測量出載體機(jī)車的位置和速度，傳感器MEMS-IMU測量單元固定安裝 于載體機(jī)車上時需保證IMU的Ξ軸坐標(biāo)系與機(jī)車Ξ軸坐標(biāo)系一致;同時IMU測量單元需隨機(jī) 車車輪轉(zhuǎn)向而實時轉(zhuǎn)向，即將IMU單元固裝于車軸W便IMU能精確敏感機(jī)車的航向角速率， 使IMU敏感航向信息與GNSS測量的航向信息一致。
[0104] 步驟S2:通過GNSS天線獲取車輛位置和速度信息，通過IMU測量單元獲取車輛的航 向角速率信息；
[0105] 所述步驟S2通過GNSS天線獲取車輛位置和速度信息，通過IMU測量單元獲取車輛 的航向角速率信息，采用了 3+3六軸集成MEMS-IMU傳感器，能精確獲得車輛的航向角速率信 息。
[0106] 步驟S3:通過加速度計和巧螺儀融合的方法進(jìn)行車輛姿態(tài)角解算；
[0107] 所述步驟S3通過加速度計和巧螺儀融合的方法進(jìn)行車輛姿態(tài)角解算具體可包括：
[0108] 因 MEMS-IMU的巧螺測量精度無法敏感地球自轉(zhuǎn)角速率，故MEMS系統(tǒng)無法進(jìn)行全自 主對準(zhǔn)；當(dāng)車輛處于靜止或者勻速運(yùn)動的情況下時，其加速度值為零，根據(jù)加速度計的原 理，可準(zhǔn)確得到車輛的橫滾角和俯仰角：
[0109] 0 = sin-i(ax)
[0110]
[0111] 式中，ax、ay為載體坐標(biāo)系的X軸和y軸的加速度;θ為俯仰角；φ為橫滾角；
[0112] 當(dāng)車輛運(yùn)動或者抖動比較大時，由于其它方向的加速度的影響，采用上式不能準(zhǔn) 確計算出姿態(tài)角，為此引入巧螺儀，采用加速度計和巧螺儀融合的方法進(jìn)行解算，具體如 下：
[0113] 由于傾角(橫滾角或者俯仰角)和傾角角速度存在導(dǎo)數(shù)關(guān)系，因此可將系統(tǒng)傾斜真 實角度^可用來做一個狀態(tài)向量，采用加速度計估計出巧螺儀常值偏差b，W此偏差作為狀 態(tài)向量得到相應(yīng)的狀態(tài)方程和觀測方程：
[0116] 式中，Wgyr。為包含固定偏差的巧螺儀輸出角速度，巧為加速度計經(jīng)處理后得到 的角度值，Wg為巧螺儀測量噪聲，Wa為加速度計測量噪聲，b為巧螺儀漂移誤差，Wg與Wa相互 獨(dú)立，并假設(shè)二者為滿足正態(tài)分布的白色噪聲。同時設(shè)定卡爾曼濾波器的系統(tǒng)過程噪聲協(xié) 方差陣QW及測量誤差的協(xié)方差矩陣R，其形式如下：
[0117]
[0118] 式中，9_曰(3。6和9_旨71'〇分別是加速度計和巧螺儀測量的協(xié)方差，;r_acce為加速度 計測量噪聲；
[0119] 根據(jù)上述參數(shù)計算得到車輛的橫滾角和俯仰角，從而解算出了車輛的姿態(tài)角。
[0120] 步驟S4:根據(jù)車輛位置、速度和航向角速率信息進(jìn)行車輛航向角解算。
[0121] 所述步驟S4根據(jù)車輛位置、速度和航向角速率信息進(jìn)行車輛航向角解算的具體實 施方式可為：
[0122] 單天線GNSS測姿主要利用GNSS速度，其航向角可由東、北向速度計算得到，即：
[012引 4p = arctan(VE/vN)
[0124] 式中，Φρ為GNSS航向角，VE、VN為東北向速度；
[0125] 在實際應(yīng)用中，GNSS航向角可直接從GNSS接收機(jī)輸出的NMEA語句中提取，但是當(dāng) 車輛靜止或者速度非常小時，數(shù)學(xué)上造成數(shù)值不穩(wěn)定性，而且速度測量誤差可能會淹沒真 實速度值。
[01%]為提高其航向精度，引入Z軸巧螺儀和GNSS進(jìn)行組合，巧螺儀沿載體坐標(biāo)系Z軸向 上安裝。系統(tǒng)方程和觀測方程如下：
[0127]
[012 引
[0129] 式中，恥Nss為GNSS輸出的航向角，br為巧螺儀漂移誤差，whd為航向過程噪聲，Tb為一 階馬爾科夫相關(guān)時間。
[0130] 線性化之后如下：
[0133] 當(dāng)車輛運(yùn)動時，Hyaw=[l 0]，其它情況下Hyaw=[0 0]。
[0134] 通過上述的車輛姿態(tài)角解算及航向角解算，最終實現(xiàn)了基于單天線的GNSS-INS車 輛的定姿。
[01對包括W下步驟:將GNSS天線固定安裝于車輛中屯、質(zhì)屯、位置，同時將MEMS傳感器的 IMU測量單元固定安裝于車輛的轉(zhuǎn)向車軸上;通過GNSS天線獲取車輛位置和速度信息，通過 IMU測量單元獲取車輛的航向角速率信息;通過加速度計和巧螺儀融合的方法進(jìn)行車輛姿 態(tài)角解算;根據(jù)車輛位置、速度和航向角速率信息進(jìn)行車輛航向角解算。采用單天線GNSS與 低成本IMU/MEMS傳感器組組合，基于農(nóng)用車輛的運(yùn)動學(xué)模型進(jìn)行定姿、定向，該方法吸收了 IM啡它螺儀短期精度高和GNSS單天線測姿長期穩(wěn)定性高的優(yōu)點(diǎn)，避免了使用巧螺儀時的發(fā) 散現(xiàn)象，同時降低GNSS測姿的噪聲水平，測姿精度可提高數(shù)倍。本實施例所述的測姿方法結(jié) 合IMU傳感器，經(jīng)測試其航向角誤差小于0.2°，俯仰角和橫滾角誤差小于0.08°。在實際應(yīng)用 中，采用了3+3六軸集成IMU傳感器，體積小，重量輕，性價比高，模塊化設(shè)計便于集成到農(nóng) 業(yè)機(jī)械輔助駕駛控制系統(tǒng)之中。
[0136] 在實際應(yīng)用中，還可使用傾角傳感器代替IMU傳感器實現(xiàn)該定姿方案。
[0137] 本發(fā)明實施的優(yōu)點(diǎn):本發(fā)明所述的基于單天線的GNSS-INS車輛定姿方法包括W下 步驟:將GNSS天線固定安裝于車輛中屯、質(zhì)屯、位置，同時將MEMS傳感器的IMU測量單元固定安 裝于車輛的轉(zhuǎn)向車軸上;通過GNSS天線獲取車輛位置和速度信息，通過IMU測量單元獲取車 輛的航向角速率信息;通過加速度計和巧螺儀融合的方法進(jìn)行車輛姿態(tài)角解算;根據(jù)車輛 位置、速度和航向角速率信息進(jìn)行車輛航向角解算。采用單天線GNSS與低成本IMU/MEMS傳 感器組組合，基于農(nóng)用車輛的運(yùn)動學(xué)模型進(jìn)行定姿、定向，該方法吸收了IM啡它螺儀短期精 度高和GNSS單天線測姿長期穩(wěn)定性高的優(yōu)點(diǎn)，避免了使用巧螺儀時的發(fā)散現(xiàn)象，同時降低 GNSS測姿的噪聲水平，測姿精度可提高數(shù)倍。本發(fā)明所述的定姿方法結(jié)合IMU傳感器，經(jīng)測 試其航向角誤差小于0.2°，俯仰角和橫滾角誤差小于0.08°。采用了3+3六軸集成IMU傳感 器，體積小，重量輕，性價比高，模塊化設(shè)計便于集成到農(nóng)業(yè)機(jī)械輔助駕駛控制系統(tǒng)之中。
[0138] W上所述，僅為本發(fā)明的【具體實施方式】，但本發(fā)明的保護(hù)范圍并不局限于此，任何 熟悉本領(lǐng)域技術(shù)的技術(shù)人員在本發(fā)明公開的技術(shù)范圍內(nèi)，可輕易想到的變化或替換，都應(yīng) 涵蓋在本發(fā)明的保護(hù)范圍之內(nèi)。因此，本發(fā)明的保護(hù)范圍應(yīng)W所述權(quán)利要求的保護(hù)范圍為 準(zhǔn)。
【主權(quán)項】
1. 一種基于單天線的GNSS-INS車輛定姿方法，其特征在于，所述基于單天線的GNSS-INS車輛定姿方法包括以下步驟： 將GNSS天線固定安裝于車輛中心質(zhì)心位置，同時將MEMS傳感器的IMU測量單元固定安 裝于車輛的轉(zhuǎn)向車軸上； 通過GNSS天線獲取車輛位置和速度信息，通過IMU測量單元獲取車輛的航向角速率信 息； 通過加速度計和陀螺儀融合的方法進(jìn)行車輛姿態(tài)角解算； 根據(jù)車輛位置、速度和航向角速率信息進(jìn)行車輛航向角解算。2. 根據(jù)權(quán)利要求1所述的基于單天線的GNSS-INS車輛定姿方法，其特征在于，所述將 MEMS傳感器的頂U(kuò)測量單元固定安裝于車輛的轉(zhuǎn)向車軸上時，需保證頂U(kuò)測量單元的三軸坐 標(biāo)系與機(jī)車三軸坐標(biāo)系一致。3. 根據(jù)權(quán)利要求1所述的基于單天線的GNSS-INS車輛定姿方法，其特征在于，所述通過 加速度計和陀螺儀融合的方法進(jìn)行車輛姿態(tài)角解算包括： 當(dāng)車輛處于靜止或者勻速運(yùn)動的情況下時，其加速度值為零，根據(jù)加速度計的原理，可 準(zhǔn)確得到車輛的橫滾角和俯仰角： 9. sin_1(ax)式中，ax、ay為載體坐標(biāo)系的X軸和y軸的加速度;Θ為俯仰角；φ為橫滾角； 當(dāng)車輛運(yùn)動或者抖動比較大時，引入陀螺儀，采用加速度計和陀螺儀融合的方法進(jìn)行， 具體如下： 首先構(gòu)建橫滾角和俯仰角組合濾波器，具體濾波器如下：其中待估參數(shù)如下： χ(?) = [θ φ ωχ ωγ ωζ]τ； 觀測向量如下： z(t) = [bfxbfybcoibxbcoibybcoibz]T，其中吖"和^讓^^為加速計和陀螺儀輸出值； 狀態(tài)轉(zhuǎn)移矩陣如下：觀測設(shè)計矩陣如下： f (x(t)) = [sinB cos Φ ωχ ωy ω ζ]τ 根據(jù)上述參數(shù)計算得到車輛的橫滾角和俯仰角。4. 根據(jù)權(quán)利要求1所述的基于單天線的GNSS-INS車輛定姿方法，其特征在于，所述通過 加速度計和陀螺儀融合的方法進(jìn)行車輛姿態(tài)角解算包括： 當(dāng)車輛處于靜止或者勻速運(yùn)動的情況下時，其加速度值為零，根據(jù)加速度計的原理，可 準(zhǔn)確得到車輛的橫滾角和俯仰角： 9. sin_1(ax)式中，ax、ay為載體坐標(biāo)系的X軸和y軸的加速度;Θ為俯仰角；φ為橫滾角； 當(dāng)車輛運(yùn)動或者抖動比較大時，引入陀螺儀，采用加速度計和陀螺儀融合的方法進(jìn)行， 具體如下： 由于傾角(橫滾角或者俯仰角)和傾角角速度存在導(dǎo)數(shù)關(guān)系，因此可將系統(tǒng)傾斜真實角 度免可用來做一個狀態(tài)向量，采用加速度計估計出陀螺儀常值偏差b，以此偏差作為狀態(tài)向 量得到相應(yīng)的狀態(tài)方程和觀測方程：式中，《gyr。為包含固定偏差的陀螺儀輸出角速度.為加速度計經(jīng)處理后得到的角 度值，wg為陀螺儀測量噪聲，《^為加速度計測量噪聲，b為陀螺儀漂移誤差，《^與&相互獨(dú)立， 并假設(shè)二者為滿足正態(tài)分布的白色噪聲。同時設(shè)定卡爾曼濾波器的系統(tǒng)過程噪聲協(xié)方差陣 Q以及測量誤差的協(xié)方差矩陣R，其形式如下：式中，分別是加速度計和陀螺儀測量的協(xié)方差，r_acce為加速度計測 量噪聲； 根據(jù)上述參數(shù)計算得到車輛的橫滾角和俯仰角。5.根據(jù)權(quán)利要求1至4之一所述的基于單天線的GNSS-INS車輛定姿方法，其特征在于， 所述根據(jù)車輛位置、速度和航向角速率信息進(jìn)行車輛航向角解算包括： 航向角可由東、北向速度計算得到，BP: itP = arctan(vE/vN) 式中，ΦΡ為GNSS航向角，ve、vn為東北向速度； 當(dāng)車輛靜止或者速度非常小時，引入z軸陀螺儀和GNSS進(jìn)行組合來進(jìn)行航向角的高精 度解算，系統(tǒng)方程和觀測方程如下：式中，ikNss為GNSS輸出的航向角，br為陀螺儀漂移誤差，Whd為航向過程噪聲，Tb為一階馬 爾科夫相關(guān)時間。 線性化之后如下：當(dāng)車輛運(yùn)動時，Hyaw=[l 0]，其它情況下Hyaw=[0 0]。6.根據(jù)權(quán)利要求5所述的基于單天線的GNSS-INS車輛定姿方法，其特征在于，所述MEMS 傳感器為六軸MEMS傳感器。
【文檔編號】G01C21/16GK105823481SQ201510969458
【公開日】2016年8月3日
【申請日】2015年12月21日
【發(fā)明人】沈雪峰, 車相慧, 任強(qiáng), 曹廣節(jié), 董光陽, 涂睿
【申請人】上海華測導(dǎo)航技術(shù)股份有限公司