基于GNSS航向微分和MEMS陀螺儀的農(nóng)機輪角測量方法

何 杰，高維煒，王 輝，岳斌斌，張 帆，張智剛

(華南農(nóng)業(yè)大學 工程學院/南方農(nóng)業(yè)機械與裝備關鍵技術教育部重點實驗室，廣東 廣州 510642)

農(nóng)業(yè)機械自動導航技術已經(jīng)得到較為普遍的應用[1-3]，前輪轉角測量是農(nóng)業(yè)機械自動導航的關鍵技術之一[4]。輪角測量傳感器的測量精度和安裝可靠性是直接影響自動導航控制性能的重要因素[5]。目前，自動導航農(nóng)機的輪角傳感器主要有連桿式直接安裝的測量方式和安裝較為簡潔的間接測量方式[6]。連桿式直接安裝的角度傳感器可直接測量農(nóng)機的轉向輪角，常采用的傳感器有電位計[7]、電阻式角度傳感器[8-10]、絕對式編碼器[11-13]、霍爾式傳感器[14-15]等。這類傳感器一般成本較低，但是在實際生產(chǎn)中存在安裝較麻煩、需準確標定等問題，且連桿易松動或折斷[16]。間接測量常采用陀螺儀及MEMS陀螺儀[17-18]、磁敏傳感器[19-20]等非接觸式傳感器測量車輪的相對轉動角速率或角度，通過信息處理的方式獲得車輪轉向角。

本文提出基于GNSS航向微分和MEMS陀螺儀的轉向輪角測量方法，通過MEMS陀螺儀測量車身和車輪的合轉動速率以及對GNSS天線輸出的航向和速度信息進行處理，設計自適應卡爾曼濾波器進行信息的融合和校正，提高輪角測量的精度，并在自動導航拖拉機上試驗進行驗證。

1 材料與方法

1.1 試驗材料

如圖1a所示，選取安裝農(nóng)業(yè)機械導航系統(tǒng)的雷沃歐豹M904–D型拖拉機為試驗研究平臺，農(nóng)機導航系統(tǒng)采用華南農(nóng)業(yè)大學和雷沃重工股份有限公司聯(lián)合研制的AGCS–I系統(tǒng)，其定位定向模塊采用上海司南導航技術股份有限公司生產(chǎn)的K728 GNSS板卡，RTK定向更新頻率為10 Hz，航向角測量精度為0.2°/R(R為雙天線基線長，1.6 m)。試驗選用美國BEI公司生產(chǎn)的DUNCAN 9360型霍爾式角度傳感器作為輪角測量的參考對象，如圖1b所示安裝于農(nóng)機左前輪輪軸上，DUNCAN 9360型角度傳感器的測量精度為 ± 0.6%，輸出頻率最低響應為 1 kHz。選用日本 Silicon sensing 公司生產(chǎn)的CRS03?02型硅性模擬量輸出的陀螺儀測量車輪轉動合角速率，安裝于農(nóng)機左前輪轉向軸，如圖1c所示，CRS03–02陀螺儀啟動時間小于0.2 s，分辨率小于± 0.1°/s/g，非線性度小于 ± 0.5%，帶寬為 10 Hz(?3 dB)。

1.2 基于GNSS航向微分和MEMS陀螺儀的輪角測量方法

如圖2所示，獲取GNSS雙天線測量車身航向微分，設計自適應卡爾曼濾波器提高雙GNSS天線航向微分的精度，獲得車身航向微分；采用MEMS陀螺儀測量農(nóng)機轉向輪的轉動合速率，導航控制器采集MEMS陀螺儀數(shù)據(jù)與GNSS雙天線測量的車身航向作差值，濾除車輪MEMS測得的車身航向微分，獲得純車輪轉動的速率，然后進行積分獲得車輪轉向角度。

圖2 基于GNSS航向和MEMS陀螺儀輪角測量方法Fig.2 Wheel angle measurement method based on GNSS heading and MEMS

由于MEMS陀螺儀存在零偏、隨機漂移等噪聲，測得的車輪轉向角速率被噪聲污染，且積分過程中MEMS陀螺儀的噪聲誤差會隨時間累積，嚴重影響轉向輪角測量結果的準確性，因此以農(nóng)機運動學方程計算轉向輪角觀測值(α)。

式中，?為車身航向微分，v為拖拉機后輪軸中心速度，L為拖拉機軸距。

以式(1)作為觀測方程設計自適應卡爾曼濾波器，濾除MEMS陀螺儀的輸出信號與航向差值積分過程的噪聲干擾，對角度積分過程進行負反饋誤差校正，獲得車輪轉向角的精準測量結果。

1.3 自適應卡爾曼濾波器

雙天線GNSS板卡航向輸出頻率為10 Hz，直接對航向微分處理得到的航向角速率噪聲大，不能直接作為車身航向角速率參與卡爾曼濾波器運算，因此本文設計了一種自適應卡爾曼濾波器，利用實時觀測數(shù)據(jù)進行濾波的同時，通過不斷改變模型參數(shù)或噪聲統(tǒng)計特性進行實時估計和修正，實現(xiàn)濾波器的在線改進，縮小實際濾波誤差，改善卡爾曼濾波器的動態(tài)性能，該方法能將系統(tǒng)辨識與濾波估計有效融為一體。

假設農(nóng)機自動導航直線行駛時車身的角加速度(a)為0，且存在一個正態(tài)分布的隨機噪聲，在直線導航狀態(tài)下可以利用該假設對航向角速率進行卡爾曼濾波，令：

卡爾曼濾波器的狀態(tài)變量即為農(nóng)機車身角速率，無控制輸入，故狀態(tài)轉移矩陣A=1；包含噪聲的觀測值是狀態(tài)變量的直接體現(xiàn)，故測量矩陣H=1。建立該卡爾曼濾波器的狀態(tài)方程和觀測方程分別如式(3)和(4)所示：

狀態(tài)方程：

觀測方程：

式中，Xk為k時刻農(nóng)機車身角速率，Xk?1為k?1時刻農(nóng)機車身角速率，Zk為k時刻雙GNSS航向微分后的車身轉動速率觀測值，v為高斯白噪聲。

根據(jù)濾波器迭代原理，建立卡爾曼濾波器時間更新方程式(5)和(6)，測量更新方程式(7)、(8)和(9)。

由于農(nóng)機曲線行駛時車身角加速度不能近似為0，卡爾曼濾波器在農(nóng)機曲線行駛時模型失配。本文提出了基于農(nóng)機橫向跟蹤偏差進行自適應調(diào)節(jié)卡爾曼濾波器的過程激勵噪聲協(xié)方差矩陣Q，卡爾曼濾波器判斷農(nóng)機為上線過程時，可通過增大Q值改變卡爾曼濾波器的權重值解決模型失配的問題。

式中，penow為當前農(nóng)機的橫向跟蹤偏差，pemax、pemin分別為最大上線距離和最小上線距離，qmax和qmin分別為自適應卡爾曼濾波器過程激勵噪聲協(xié)方差矩陣Q初始化的最大值和最小值。

1.4 試驗方法

為驗證本文設計的GNSS航向微分和MEMS陀螺儀轉向輪角測量方法的準確性，分別設計了如下試驗。

1.4.1 自適應卡爾曼濾波器性能驗證試驗 以安裝于拖拉機車身的MEMS陀螺儀測量的車身角速率作為參照，對比檢驗雙天線航向微分采用自適應卡爾曼濾波前后的效果。在華南農(nóng)業(yè)大學增城基地的水泥路面自動導航駕駛拖拉機直線行駛、人工駕駛拖拉機曲線行駛，分別采集后輪軸中心速度、MEMS陀螺輸出角速率、雙天線航向角等原始數(shù)據(jù)進行仿真分析，對原始數(shù)據(jù)和濾波效果進行評價。自適應卡爾曼濾波器初始化參數(shù)為P(0)=100，Q(0)=0.005，Q自適應公式為：

R=0.05，初始化輪角值為0°，令農(nóng)機后輪軸中心速度大于0.2 m /s 時，自適應卡爾曼濾波器工作。

1.4.2 對比試驗 分別以GNSS雙天線航向微分和MEMS陀螺儀測量的輪角和DUNCAN 9360型角度傳感器測量的輪角作為導航系統(tǒng)的輪角反饋，選取拖拉機偏離目標航線1.5和2.5 m作為上線距離，拖拉機上線速度為2.88 km/h，進行對比試驗。試驗時，卡爾曼濾波器初始化參數(shù)與驗證試驗的相同，初始化輪角值為0°。試驗采集MEMS陀螺儀測量輪角的觀測值、卡爾曼濾波值和連桿式輪角傳感器獲得的角度基準值，比較上線過程卡爾曼濾波器收斂速度和輪角測量的準確性和動態(tài)響應性能。

1.4.3 田間直線導航應用試驗 采用“1.4.1”所述卡爾曼濾波器初始化參數(shù)，以4 km/h行進速度進行田間直線導航試驗。試驗采集并統(tǒng)計對比前輪轉向角度測量值和橫向位置偏差，通過評價拖拉機導航行駛作業(yè)性能，間接考察拖拉機雙天線GNSS航向微分和MEMS陀螺儀轉向輪角測量性能。

2 結果與分析

2.1 自適應卡爾曼濾波器性能驗證試驗

圖3 GNSS航向微分計算值(藍線)、車身MEMS陀螺儀測量值(黑線)和自適應卡爾曼濾波(紅線)對比Fig.3 Comparison of the calculated values of GNSS heading differential (bule line), measured values of MEMS gyroscope(black line)and self-adaptive Kalman filter (red line)

雙天線航向微分計算值、車身陀螺測量值和自適應卡爾曼濾波值的驗證試驗結果如圖3所示。從圖3可以看出，自適應卡爾曼濾波值基本與MEMS陀螺儀測量值吻合，在直線行駛和轉彎部分均得到了較好的濾波效果。以車身MEMS陀螺儀測量值為參考，對濾波前和后的3組采樣數(shù)據(jù)進行統(tǒng)計分析，結果如表1所示。

表1 GNSS雙天線航向微分濾波前后誤差Table 1 Errors of GNSS dual antenna heading differential values before and after Kalman filtering

以車身陀螺儀測量值作為參考，直線行駛時，濾波前最大誤差為5.40 °/s，平均絕對誤差最大為0.94°/s ，均方根誤差最大為1.21°/s；自適應卡爾曼濾波器濾波后最大誤差為4.14°/s，平均絕對誤差最大為0.66 °/s ，均方根誤差最大為0.85°/s。3次直線行駛數(shù)據(jù)角速率平均絕對誤差均值由濾波前的0.91°/s 降低為濾波后的0.59°/s，均方根誤差均值由濾波前的1.16°/s 降低為濾波后的0.76°/s。

曲線行駛時，濾波前最大誤差為18.00°/s，平均絕對誤差最大為1.69°/s，均方根誤差最大為2.75°/s；自適應卡爾曼濾波器濾波后最大誤差為15.69 °/s ，平均絕對誤差最大為1.45°/s，均方根誤差最大為2.34°/s。3次曲線行駛數(shù)據(jù)角速率平均誤差均值由濾波前的1.57°/s降低為濾波后的1.38°/s，均方根誤差均值由濾波前的2.61°/s降低為濾波后的2.19°/s。

自適應卡爾曼濾波器有效去除了直線行駛和曲線行駛時GNSS雙天線航向微分計算的車身角速率值噪聲，平滑了車身角速率值，降低了平均誤差和均方根誤差，濾波效果明顯。驗證試驗結果表明，設計的自適應卡爾曼濾波器對車身航向角速率濾波效果良好，滿足測量精度要求，可作為信息源替代車身MEMS陀螺儀的輸出值參與前輪轉向角度的計算。

2.2 對比試驗

圖4 不同上線距離前輪轉向角度觀測值(紅線)、濾波值(黑線)和角度傳感器測量值(藍線)對比Fig.4 Comparison of observed (red line)and filter values (black line)of front wheel steering angle and measured values of angle sensor (blue line)at different on-line distances

對比試驗結果如圖4所示。從圖4可知，卡爾曼濾波器初始化的輪角為0°，真實角度值分別為?1.0°和6.2°，濾波值逼近真實值時間分別為1.4和1.1 s；前輪轉向角度的觀測值噪聲大，影響濾波效果。以濾波逼近真實值時刻為開始，濾波前和濾波后角度值與連桿式角度傳感器輸出差值誤差統(tǒng)計信息如表2所示，上線距離為2.5 m時，平均絕對誤差由2.66°降低至1.13°，均方根誤差由3.48°降低至0.90°；上線距離為1.5 m時，平均絕對誤差由1.98°降低至 0.87°，均方根誤差由 2.89°降低至0.68°。濾波后誤差大幅下降，表明自適應卡爾曼濾波器濾波效果較好。

圖5為采用GNSS航向微分和MEMS陀螺儀測量轉向輪角導航時2.5 m 和1.5 m上線距離下拖拉機的上線時間，圖6為采用角度傳感器導航時2.5和1.5 m上線距離下拖拉機的上線時間。在1.5和2.5 m的上線距離和相同速度下，采用傳統(tǒng)角度傳感器時上線時間分別為15.0 s和14.2 s，采用GNSS航向微分和MEMS陀螺儀測量輪角時上線時間分別為29.4和23.5 s，較采用傳統(tǒng)角度傳感器時的上線時間長，動態(tài)響應性能稍遜。主要原因是模型失配導致角度測量誤差較大，上線超調(diào)量較大，但采用的自適應卡爾曼濾波器收斂較快，且未出現(xiàn)發(fā)散和震蕩，上線時間在可接受范圍內(nèi)，能滿足導航作業(yè)的基本需求。

表2 濾波前后角度值與連桿傳感器輸出值對比的統(tǒng)計數(shù)據(jù)Table 2 Statistics of angle values before and after filtering compared to the output values of link-type angle sensor

圖5 不同上線距離GNSS航向微分和MEMS陀螺儀測量轉向輪角導航時的上線時間Fig.5 On-line time at different on-line distances when measuring steering wheel angle with GNSS heading differential and MEMS gyroscope

圖6 不同上線距離角度傳感器測量轉向輪角導航時的上線時間Fig.6 On-line time at different on-line distances when measuring steering wheel angle with angle sensor

2.3 田間直線導航應用試驗

圖7為4 km/h速度下前輪轉向角度濾波值、觀測值和角度傳感器測量的基準值對比試驗結果，由圖7可知GNSS航向微分和MEMS陀螺儀測量的輪角和角度傳感器測量值的基準角度趨勢一致，測量結果穩(wěn)定，設計的自適應卡爾曼濾波器能很快收斂，并濾除雜波。以連桿式輪角傳感器測量結果為基準，對采用卡爾曼濾波前后GNSS航向微分和MEMS陀螺儀的測量結果進行統(tǒng)計分析可得，濾波前平均絕對誤差為1.07°，均方根誤差為 2.12 °；濾波后平均絕對誤差為 0.44 °，均方根誤差為0.87 °。應用試驗結果表明自適應卡爾曼濾波器效果良好，能夠替代角度傳感器用于農(nóng)業(yè)機械導航。

圖8為4 km/h速度下采用GNSS航向微分和MEMS陀螺儀測量值作為輪角反饋進行直線導航時采集的拖拉機橫向位置偏差。從導航路徑跟蹤效果看，最大橫向位置偏差為5.6 cm，平均偏差為1.8 cm，橫向位置偏差均方根為2.2 cm，極差為10.5 cm，跟蹤精度基本能滿足拖拉機旱地作業(yè)要求。試驗結果表明，GNSS航向微分和MEMS陀螺儀測量輪角方法基本可滿足拖拉機自動導航作業(yè)場景，且成本相對較低，具有一定的應用前景。

圖7 田間試驗中前輪轉向角觀測值（紅線）、濾波值（黑線）和角度傳感器測量值（藍線）對比圖Fig.7 Comparison chart of observed (red line)and filtered values (black line)of front wheel steering angle and measured values of angle sensor (blue line)in field experiment

圖8 拖拉機直線導航作業(yè)時橫向位置偏差Fig.8 Deviation of lateral position during linear navigation operation of tractor

3 結論

本文分析了基于GNSS航向微分和MEMS陀螺儀的農(nóng)機輪角測量方法，設計了自適應卡爾曼濾波器抑制GNSS雙天線航向微分測量噪聲，提高了農(nóng)機輪角測量的精度和穩(wěn)定性。

設計了自適應卡爾曼濾波器性能驗證試驗，對獲取的雙天線GNSS航向微分和MEMS陀螺儀測量的車身角速率以及自適應卡爾曼濾波后的車身轉動速率進行比較，驗證試驗結果表明，卡爾曼濾波器在直線行駛和曲線行駛時對雙天線航向微分計算的車身角速率值的噪聲進行了有效去除且平滑了車身角速率值，降低了平均絕對誤差和均方根誤差，濾波效果良好，滿足測量精度要求。

GNSS航向微分和MEMS陀螺儀測量值和角度傳感器測量值對比試驗結果表明，該方法轉向輪角測量精度與傳統(tǒng)輪角傳感器測量結果相當，2.5和1.5 m上線距離均方根誤差分別為0.90°和0.68°，基本滿足農(nóng)機自動導航輪角測量需求；動態(tài)響應性能比傳統(tǒng)角度傳感器性能稍遜，分別采用本文方法和輪角傳感器進行導航，拖拉機偏離目標航線2.5 m時上線時間分別為 29.4 和 15.0 s，偏離目標航線 1.5 m時上線時間分別為23.5 和14.2 s，較采用傳統(tǒng)角度傳感器時上線時間長。

4 km/h速度直線導航應用試驗結果表明，GNSS航向微分和MEMS陀螺儀測量的輪角與角度傳感器測量的輪角平均絕對誤差為0.44°，導航路徑跟蹤橫向位置偏差均方根為2.2 cm，滿足拖拉機旱地作業(yè)要求。

3組試驗結果表明，本文提出的GNSS航向微分和MEMS陀螺儀測量方法性能與連桿式輪角傳感器相當，且成本相對較低，能夠替代角度傳感器用于農(nóng)業(yè)機械較低速導航，具有一定的應用前景。

由于轉向輪上MEMS陀螺儀存在零偏未校正，GNSS航向微分和MEMS陀螺儀測量的輪角值存在初始化角度值不準確的缺陷，后續(xù)研究應進行MEMS陀螺儀零偏校正。由于GNSS板卡更新頻率低，在高速導航作業(yè)時GNSS航向微分值噪聲較大，前輪轉向角度測量系統(tǒng)穩(wěn)定性會變差，農(nóng)機高速導航作業(yè)時輪角測量精度和穩(wěn)定性有待驗證。