基于雙GNSS天線及單陀螺的車輪轉角測量系統

繆存孝 楚煥鑫 孫志輝 徐紀洋 馬 飛

(1.北京科技大學機械工程學院， 北京 100083； 2.上海聯適導航技術有限公司， 上海 201702)

基于雙GNSS天線及單陀螺的車輪轉角測量系統

繆存孝1楚煥鑫1孫志輝1徐紀洋2馬 飛2

(1.北京科技大學機械工程學院， 北京 100083； 2.上海聯適導航技術有限公司， 上海 201702)

針對農業機械自動導航中，傳統絕對角度傳感器連接件多、安裝復雜且容易出現故障，而陀螺測量前輪轉角雖然安裝容易，但陀螺零偏等儀器誤差造成測量誤差隨時間累積的問題，提出了基于雙GNSS天線和單軸MEMS陀螺組合測角系統，該系統通過雙GNSS天線解算的航向角、速度等信息計算觀測量，通過卡爾曼濾波器對陀螺計算的角度進行實時校正，提高了車輪轉角的測量精度。實車試驗結果表明，該系統具有較好的合理性和準確性，車輪轉角測量結果與絕對角度傳感器輸出結果比較：直線試驗誤差在0.5°以內，曲線試驗誤差在1°以內，滿足了農業機械自動導航的測角精度要求。

車輪轉角； 雙GNSS天線； 單軸MEMS陀螺； 卡爾曼濾波

引言

隨著精準農業的快速發展，農業機械自動導航系統因其有利于減少農作物生產的投入成本，減少作業能耗等優點得到了越來越廣泛的應用[1-3]。農業機械自動導航中車輪轉角信息是影響導航效果的重要因素之一[4-5]。

目前農機自動導航采用的角度測量方法有2種：一種是采用電位移傳感器、絕對編碼器等絕對角度傳感器測量車輪轉向的絕對角度[6-7]；另一種是采用陀螺等慣性器件檢測車輪的相對轉動角度[8]，對安裝在車輪轉動部件上的陀螺輸出的角速率信息處理后積分得到車輪的轉動角度信息[9-11]。

絕對角度傳感器雖然角度輸出可以滿足轉角精度要求，但傳感器連接件多，安裝復雜，標定工作繁瑣，并且容易出故障，檢修和更換過程復雜，耗費大量時間和人力[12]。陀螺作為慣性器件，安裝簡便，工作壽命長[13]，但陀螺輸出中包括零偏、隨機漂移等誤差項，造成誤差隨時間累積，測量精度下降[14]。文獻[15]對陀螺數據用時間平滑模型建模后通過卡爾曼濾波來估計陀螺輸出，減少了陀螺隨機誤差的影響，但該過程沒有對常值誤差進行處理，誤差還是隨時間累積。本文擬設計由雙全球導航衛星系統(Global navigation satellite system，GNSS)天線和單軸微機電系統(Micro-electro-mechanical system，MEMS)陀螺組成的測角系統，通過雙GNSS天線輸出的速度、航向角等信息對車輪角度計算結果進行實時校正，提高角度測量精度，并安裝在拖拉機上進行實車測試，驗證算法的有效性。

1 系統設計

1.1 測角理論

如圖1所示安裝關系，系統主要包括2個GNSS天線組成的雙天線導航系統和1個單軸MEMS陀螺。雙GNSS系統的2個天線安裝在車輛主體的兩側，基線垂直于車頭與車尾的中軸線，雙GNSS系統能夠輸出車體的運動速度、姿態角及經緯度等信息，能夠完成自動駕駛系統需要的車輛姿態信息測量，同時為車輪轉角校正提供信息；單軸MEMS陀螺安裝在車輪轉動部件上，敏感軸垂直于地面，測量車輪的轉動速度。

圖1 系統安裝示意圖Fig.1 Diagram of system installation1.車輪 2.單軸MEMS陀螺 3.GNSS天線1 4.GNSS天線2

單軸MEMS陀螺測量的車輪轉動速率包括車體航向角的變化速率和車輪相對于主體的轉動角速率，即

(1)

(2)

式中θ——車輪轉角真值

由于陀螺輸出中有零偏、隨機漂移等誤差項，所以實際應用中ωA還包含輸出誤差，造成角度積分的結果誤差隨時間累積。因此需要設計卡爾曼濾波器對誤差進行實時校正。

角度計算值為

(3)

對式(3)求導得

(4)

式(4)中計算角速率等于陀螺測量真值加陀螺零偏減去航向角變化速率，即

(5)

根據式(5)與式(1)得角度計算的誤差微分方程為

(6)

MEMS陀螺零偏為一階馬爾科夫過程，一階馬爾科夫過程微分方程表示為

(7)

根據車輛運動學模型[16]可知

(8)

則由雙GNSS系統輸出的車輛速度和航向角變化速率可以算出期望的車輪轉角為

(9)

1.2 速度桿臂效應誤差補償

圖2 GNSS天線桿臂示意圖Fig.2 Diagram of lever-arm between GNSS and rear axle

主GNSS天線與后輪軸中心的桿臂長度為

式中φ——航向角γ——橫滾角

車輛轉動角速度矢量為

則桿臂效應造成的速度誤差δv表示為

(10)

將GNSS天線測量的水平速度按照運動航向角方向分解到正東和正北2個方向，即

則經過桿臂誤差補償的速度為

(11)

1.3 卡爾曼濾波器設計

設計基于雙GNSS天線及單陀螺的車輪轉角測量系統，系統數據處理流程框圖如圖3所示。

圖3 測角系統框架簡圖Fig.3 Algorithmic block diagram

陀螺數據與雙GNSS天線航向角信息得到車輪轉向角速度，積分后得到轉向角度，雙GNSS系統速度信息進行桿臂誤差補償后與航向角速率計算車輪轉角理想值，將計算值與理想值作差作為量測值代入卡爾曼濾波器進行最優估計，卡爾曼濾波輸出的角度誤差和陀螺零偏對角度計算積分過程進行反饋校正，得到車輪轉角的精確信息。

根據系統特點及數據處理過程，定義系統狀態量為

式中δθk——k時刻的角度誤差εk——k時刻的零偏

建立本系統的卡爾曼濾波狀態方程[20]

Xk=Φk,k-1Xk-1+Wk

(12)

其中

(13)

式中Xk——k時刻的系統狀態向量Φk,k-1——離散系統狀態轉移矩陣Wk——離散時間過程噪聲，為N(0,Qk) 的高斯白噪聲

Qk——系統噪聲的方差矩陣，本文系統中為陀螺隨機白噪聲及零偏的激勵白噪聲

Δt——濾波時間間隔

F(tk-1)——連續時間狀態轉移矩陣

根據式(6)、(7)，計算得出

(14)

系統的觀測量根據式(3)、(9)選取角度估計誤差為

(15)

式中Zk——k時刻的系統觀測向量

建立本系統的觀測方程為

Zk=HkXk+Vk

(16)

其中

式中Hk——觀測矩陣Vk——觀測噪聲，為N(0,Rk) 的高斯白噪聲

Rk——系統噪聲的方差矩陣，本系統中量測計算的誤差白噪聲

卡爾曼濾波算法可由下述方程描述：

一步預測方程

Xk,k-1=Φk,k-1Xk-1

(17)

狀態遞推方程

Xk=Xk,k-1+Kk(Zk-HkXk,k-1)

(18)

增益方程

Kk=Pk,k-1H′k(HkPk,k-1H′k+Rk)′

(19)

均方誤差陣

Pk,k-1=Φk,k-1Pk-1Φ′k,k-1+Qk-1

(20)

Pk=(I-KkHk)Pk,k-1

(21)

式中Kk——k時刻的濾波器增益Pk——k時刻系統對象的狀態估計誤差協方差

I——單位矩陣

2 試驗驗證

2.1 試驗平臺

為檢驗本文所建系統的正確性，搭建了車輪轉角測量系統的試驗平臺，如圖4所示。

圖4 測角系統試驗平臺Fig.4 Experiment platform1.MEMS陀螺儀 2.霍爾效應角度傳感器 3.雙GNSS天線

試驗平臺包括：黃海金馬904-1型拖拉機1臺、聯適R60s系列雙天線定位定向系統1套、日本SSS公司CRM 100型高精度單軸陀螺儀1只及相應數據處理電路、RTY050HVEAX型Honeywell霍爾效應絕對角度傳感器1個、相應的數據傳輸線路及1臺導航計算機。2個GNSS接收天線安裝在車體兩側，基線距離為2.05 m，R60S系統輸出航向精度優于0.05°，橫滾角精度優于1°；MEMS陀螺和絕對角度傳感器安裝在車輛前輪的轉向部件，測量同一側轉向輪的轉角，各傳感器與導航計算機之間通過相應數據線連接。

相應算法編寫成DLL函數形式，編寫軟件從串口讀取各個傳感器信息并調用DLL函數完成前輪轉動角度的計算，并將結果與絕對傳感器輸出角度保存。

試驗中當車輛速度為零時，根據式(9)量測計算值分母為接近零的小值，計算結果無法反映當前車輪實際轉角，所以在實際應用中需要進行速度判定：在車輛靜止時車輪轉角保持不變；當車輛開始運動時，陀螺數據積分并開始調用卡爾曼濾波過程，校正車輪的轉角信息。系統中由于天線定位滯后等造成的延遲誤差與系統測量精度量級相當，作為卡爾曼濾波器的觀測噪聲以消除對結果精度的影響。

2.2 試驗結果與分析

在同一運動狀態下，測量絕對角度傳感器與MEMS陀螺測角系統輸出的角度結果進行比較，驗證本文提出的雙GNSS天線及單軸MEMS陀螺車輪轉角測量系統的正確性和有效性。

駕駛拖拉機在試驗場地分別走一段直線路徑和一段S曲線路徑，在行駛過程中實時采集各傳感器的數據和雙GNSS天線及單軸MEMS陀螺組成的測角系統輸出的車輪轉角信息，以便分析和處理。

圖5 直線試驗路線Fig.5 Straight road test result

直線行駛路徑如圖5所示，整個過程人工駕駛，所以行駛路徑不是嚴格的直線，前輪有轉動。以起始點坐標為原點，利用經緯度計算各采集點與原點間的距離，畫出直線行駛的路線圖。圖6為直線試驗航向角度及航向角微分得到的航向角速率數據曲線。

直線試驗數據處理過程中，卡爾曼濾波器的增益矩陣元素和估計誤差協方差矩陣元素如圖7所示，卡爾曼濾波器在一段時間后估計誤差協方差與濾波增益收斂到穩定值，卡爾曼濾波器的狀態估計收斂，說明所設計濾波器是穩定的。

圖6 直線試驗航向角度和角速率曲線Fig.6 Heading angle and angular rate of straight road

圖7 直線試驗卡爾曼濾波增益矩陣K元素和誤差協方差陣P元素Fig.7 Gain matrix and error covariance matrix of Kalman filter

圖8 直線行駛角度輸出對比曲線Fig.8 Angle output contrast of straight road test

直線行駛時，絕對傳感器輸出角度、陀螺積分角度及經過濾波后的車輪轉角角度輸出結果如圖8所示。陀螺積分初值取0°，由圖8可知，陀螺積分直接計算角度誤差隨著時間累積，120 s試驗時間內誤差累積達到10°以上，這一誤差會嚴重影響自動駕駛系統的導航精度，甚至造成系統失衡。而濾波角度在速度為零時保持初值不變，在車輛啟動后，調用卡爾曼濾波過程經過雙GNSS天線輸出的航向角速率、速度等信息計算期望車輪轉角對陀螺計算角度進行校正，濾波器收斂后系統的角度輸出的誤差不隨時間累積，能夠保證拖拉機自動駕駛的長時間穩定工作。為了分析卡爾曼濾波器對角度誤差的補償效果，表1列出了霍爾傳感器、濾波前后角度在穩態情況下輸出的部分數據，雙GNSS系統及單陀螺組成的測角系統直線試驗時輸出結果與角度真值之間誤差收斂后平均值為-0.064°，均方差為0.309°。本文設計的轉角測量系統直線行駛時與霍爾效應角度傳感器輸出對比誤差在0.5°以內，滿足自動駕駛系統要求的車輪轉角測量精度要求。

表1 霍爾傳感器、濾波前后角度輸出Tab.1 Hall effect sensor output, angle after and before Kalman filter (°)

圖9 曲線試驗路線Fig.9 Curve road test result

S曲線行駛路線如圖9所示，車輛行駛路徑為S曲線，通過補償桿臂誤差和不補償桿臂誤差角度輸出對比，驗證系統的精度。圖10為S曲線試驗航向角度及航向角微分得到的航向角速率數據曲線。

S曲線補償桿臂試驗數據處理過程中，卡爾曼濾波器的增益矩陣元素和估計誤差協方差矩陣元素如圖11所示，估計誤差協方差與濾波增益收斂到穩定值，卡爾曼濾波器的狀態估計收斂，說明所設計濾波器是穩定的。

圖10 曲線試驗航向角度和角速率曲線Fig.10 Heading angle and angular rate of curve road test

圖11 曲線試驗卡爾曼濾波增益矩陣K元素和誤差協方差陣P元素Fig.11 Gain matrix and error covariance matrix of Kalman filter

S曲線行駛時，桿臂補償前的角度輸出結果及經過桿臂誤差補償的車輪轉角角度輸出結果與絕對角度傳感器輸出的對比如圖12所示。結果表明，在大角度轉彎未進行桿臂誤差補償時，雙GNSS系統及單陀螺測角系統的角度輸出在曲線行駛時誤差較大，誤差在曲線處達10°以上；經過桿臂誤差補償后，系統在曲線行駛時角度輸出誤差減小。為了分析桿臂補償前后效果，表2列出了霍爾傳感器、桿臂補償前后角度輸出的部分數據，通過桿臂補償算法，系統在曲線試驗時角度輸出與霍爾效應傳感器誤差平均值為0.299°，均方差為1.009°。本文設計的轉角測量系統曲線行駛時與霍爾效應角度傳感器輸出對比誤差在1°以內，滿足自動駕駛系統對車輪轉角的測量精度要求。

圖12 曲線行駛角度輸出對比曲線Fig.12 Angle output contrast of curve running test

綜上所述，本文提出的雙GNSS系統及單陀螺組成的車輪轉角測量系統能夠代替傳統絕對角度測量裝置，并且能夠解決陀螺測角由于零偏等器件誤差引起的角度誤差累積問題，同時，在曲線運動時通過補償桿臂效應造成的速度誤差提高了曲線運動時系統角度輸出的精度，驗證了本文提出系統的可行性和有效性。

表2 霍爾傳感器、桿臂補償前后角度輸出Tab.2 Hall effect sensor output, angle after and before lever-arm compensation (°)

3 結束語

針對農機自動駕駛系統中車輪轉角測量問題，設計了一種雙GNSS系統及單軸MEMS陀螺組成的轉角測量系統。從理論方面推導了陀螺測量車輪轉角的可行性及誤差來源，針對陀螺輸出中零偏等誤差項使角度輸出誤差隨時間累積的問題，設計了相應的卡爾曼濾波器對角度測量進行最優估計，并對速度進行了桿臂效應補償，提高觀測量精度。安裝在拖拉機上進行路面試驗結果表明，本文提出的測角系統精度與霍爾角度傳感器輸出角度之間誤差直線行駛時在0.5°左右，曲線行駛時在1°以內，滿足自動駕駛系統的測量精度要求，驗證了測角系統的可行性和有效性。

1 LI M, KENJI I, KATSUHIRO W, et al. Review of research on agricultural vehicle autonomous guidance[J]. International Journal and Biological Engineering, 2009,2(3):1-16.

2 林志海,白洪波. 淺談GNSS衛星定位和自動駕駛系統在現代化農機具中的應用[J]. 農業機械, 2015(1):137-141.

3 崔冰. 簡述農業機械化現狀與發展趨勢[J]. 現代農業, 2010(11):78-79.

4 ROVIRAMAS F. Sensor architecture and task classification for agricultural vehicles and environments[J]. Sensors, 2010, 10(12):11226-11247.

5 胡靜濤,高雷,白曉平,等. 農業機械自動導航技術研究進展[J]. 農業工程學報, 2015, 31(10):1-10. HU Jingtao, GAO Lei, BAI Xiaoping, et al. Review of research on automatic guidance of agricultural vehicles[J]. Transactions of the CSAE, 2015, 31(10):1-10. (in Chinese)

6 SANTANA F, GOMEZ J. Design and implementation of a GPS guidance system for agricultural tractors using augmented reality technology[J].Sensors, 2010,10(11):10435-10447.

7 王鶴, 胡靜濤, 高雷. 農業機械自動導航車輪轉角測量誤差補償模型[J/OL]. 農業機械學報, 2014,45(8):33-37. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20140806&journal_id=jcsam. DOI：10.6041/j.issn.1000-1298.2014.08.006. WANG He, HU Jingtao, GAO Lei. Compensation model for measurement error of wheel turning angle in agricultural vehicle guidance[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2014,45(8):33-37. (in Chinese)

8 MEISER V, HESSELBARTH H, HENKE R, et al. Autonomous unmanned ground vehicle as sensor carrier for agricultural survey tasks[C]∥Proceedings International Conference of Agricultural Engineering,2014:Ref-CO266.

9 連志鵬. 自動駕駛農機轉向控制研究[D]. 上海:上海工程技術大學, 2015.

10 陳艷,張漫,馬文強，等. 基于GPS 和機器視覺的組合導航定位方法[J]. 農業工程學報, 2011,27(3): 126-130. CHEN Yan, ZHANG Man, MA Wenqiang, et al. Positioning method of integrated navigation based on GPS and machine vision[J].Transactions of the CSAE, 2011,27(3):126-130. (in Chinese)

11 周俊,姬長英. 自主車輛導航系統中的多傳感器融合技術[J].農業機械學報, 2002,33(5): 113-116. ZHOU Jun, JI Changying. Multi-sensor fusion in navigation of autonomous vehicle[J]. Transactions of the Chinese Society for Agricultural Machinery, 2002, 33(5):113-116. (in Chinese)

12 賈全. 拖拉機自動導航系統關鍵技術研究[D]. 北京:中國農業機械化科學研究院,2015.

13 PAUL D. Navigation using inertial sensors[J]. IEEE Aerospace & Electronic System Magazine, 2015,2(2):42-69.

14 李軍,馬蓉. 拖拉機自動轉向系統設計研究[J]. 拖拉機與農用運輸車, 2012, 39(5):51-54,58. LI Jun, MA Rong. Design and study of tractor automatic steering system[J]. Tractor & Farm Transporter, 2012, 39(5):51-54,58. (in Chinese)

15 王朝陽. 基于陀螺儀的車輛前輪轉角測量方法研究[D]. 上海:上海工程技術大學,2015.

16 李逃昌,胡靜濤,高雷,等. 基于模糊自適應純追蹤模型的農業機械路徑跟蹤方法[J/OL].農業機械學報,2013,44(1):205-210. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20130139&journal_id=jcsam. DOI：10.6041/j.issn.1000-1298.2013.01.039. LI Taochang, HU Jingtao, GAO Lei, et al. Agricultural machine path tracking method based on fuzzy adaptive pure pursuit model[J/OL]. Transactions of the Chinese Society for Agricultural Machinery,2013,44(1):205-210. (in Chinese)

17 于海業,馬成林,孫瑞東. 角速度計檢測信號數字濾波處理的研究[J]. 農業機械學報, 1997,38(4):86-89. YU Haiye, MA Chenglin, SUN Ruidong. A study on signals detected by angle velocity meter digital filter processing[J]. Transactions of the Chinese Society for Agricultural Machinery, 1997,38(4):86-89. (in Chinese)

18 嚴恭敏,嚴衛生,徐德民. 捷聯慣性測量組件中內桿臂效應分析與補償[J]. 中國慣性技術學報，2008，16(2): 148-153. YAN G M，YAN W S，XU D M． Analysis and compensation on inner lever arm effect of strapdown inertial measurement unit[J]. Journal of Chinese Inertial Technology, 2008,16(2): 148-153. (in Chinese)

19 高偉,張亞,孫騫，等. 傳遞對準中桿臂效應的誤差分析與補償[J]. 儀器儀表學報,2013,34(3):560-565. GAO Wei, ZHANG Ya, SUN Qian, et al. Error analysis and compensation for lever-arm effect in transfer alignment[J]. Chinese Journal of Scientific Instrument,2013,34(3):560-565. (in Chinese)

20 周建軍,王秀,張睿,等. 農機車載GPS和DR組合導航系統定位方法[J/OL]. 農業機械學報, 2012, 43(增刊):262-265. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=2012s54&journal_id=jcsam. DOI：10.6041/j.issn.1000-1298.2012.S0.054. ZHOU Jianjun, WANG Xiu, ZHANG Rui, et al. GPS/DR integrated navigation positioning method for agricultural machinery[J/OL]. Transactions of the Chinese Society for Agricultural Machinery,2012,43(Supp.):262-265. (in Chinese)

WheelTurningAngleMeasurementSystemBasedonDoubleGNSSAntennasandSingleGyro

MIAO Cunxiao1CHU Huanxin1SUN Zhihui1XU Jiyang2MA Fei2

(1.SchoolofMechanicalEngineering,UniversityofScienceandTechnologyBeijing,Beijing100083,China2.ShanghaiAllyNavTechnologyCo.,Ltd.,Shanghai201702,China)

For the problem that the traditional angle sensor’s complex mechanical structure and prone to failure and the gyro bias cause the error accumulates over time in automatic driving system, a wheel turning angle measurement system based on double GNSS antennas and single gyro was proposed. The system’s sensors mainly included two GNSS antennas and a MEMS gyro. The double GNSS antennas were mounted on both side of the vehicle and provided the speed, attitude angle, latitude and longitude of the vehicle. The single MEMS gyro was mounted on the wheel and the angular rate was measured. An algorithm used the above data was designed to integrate the angular rate to obtain the steering angle. In order to solve the problem that the error accumulates over time, a Kalman filter based on the vehicle dynamics model was designed to calibrate the error of integration of the gyro data. At the same time, the lever-arm compensation algorithm was used to solve the speed error caused by lever-arm. The straight line experiment was carried out to verify the effectiveness of the system and the curve line experiment was carried out to verify the effectiveness of the lever-arm compensation algorithm. Compared the steering angle of the proposed system with the Hall effect angular sensor, the average error of the straight line experiment was -0.064° and the error variance was 0.309° and the cure line experiment’s mean error was 0.299° and the error variance was 1.009°. The result of the experiments showed that this system could replace the traditional angle sensor and it was easy to install and overhaul.

wheel turning angle; double GNSS antennas; uniaxial MEMS gyro; Kalman filter

S24; TP271

A

1000-1298(2017)09-0017-07

10.6041/j.issn.1000-1298.2017.09.002

2017-03-24

2017-07-03

國家自然科學基金項目(61603035)

繆存孝(1981—)，男，講師，博士，主要從事慣性器件應用研究，E-mail: miao_cunxiao@163.com