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

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

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

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

1 材料與方法

1.1 試驗材料

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

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

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

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

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

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

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

1.3 自適應(yīng)卡爾曼濾波器

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

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

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

狀態(tài)方程：

觀測方程：

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

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

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

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

1.4 試驗方法

為驗證本文設(shè)計的GNSS航向微分和MEMS陀螺儀轉(zhuǎn)向輪角測量方法的準(zhǔn)確性，分別設(shè)計了如下試驗。

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

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

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

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

2 結(jié)果與分析

2.1 自適應(yīng)卡爾曼濾波器性能驗證試驗

圖3 GNSS航向微分計算值(藍(lán)線)、車身MEMS陀螺儀測量值(黑線)和自適應(yīng)卡爾曼濾波(紅線)對比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)

雙天線航向微分計算值、車身陀螺測量值和自適應(yīng)卡爾曼濾波值的驗證試驗結(jié)果如圖3所示。從圖3可以看出，自適應(yīng)卡爾曼濾波值基本與MEMS陀螺儀測量值吻合，在直線行駛和轉(zhuǎn)彎部分均得到了較好的濾波效果。以車身MEMS陀螺儀測量值為參考，對濾波前和后的3組采樣數(shù)據(jù)進(jìn)行統(tǒng)計分析，結(jié)果如表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；自適應(yīng)卡爾曼濾波器濾波后最大誤差為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；自適應(yīng)卡爾曼濾波器濾波后最大誤差為15.69 °/s ，平均絕對誤差最大為1.45°/s，均方根誤差最大為2.34°/s。3次曲線行駛數(shù)據(jù)角速率平均誤差均值由濾波前的1.57°/s降低為濾波后的1.38°/s，均方根誤差均值由濾波前的2.61°/s降低為濾波后的2.19°/s。

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

2.2 對比試驗

圖4 不同上線距離前輪轉(zhuǎn)向角度觀測值(紅線)、濾波值(黑線)和角度傳感器測量值(藍(lán)線)對比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

對比試驗結(jié)果如圖4所示。從圖4可知，卡爾曼濾波器初始化的輪角為0°，真實角度值分別為?1.0°和6.2°，濾波值逼近真實值時間分別為1.4和1.1 s；前輪轉(zhuǎn)向角度的觀測值噪聲大，影響濾波效果。以濾波逼近真實值時刻為開始，濾波前和濾波后角度值與連桿式角度傳感器輸出差值誤差統(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°。濾波后誤差大幅下降，表明自適應(yīng)卡爾曼濾波器濾波效果較好。

圖5為采用GNSS航向微分和MEMS陀螺儀測量轉(zhuǎn)向輪角導(dǎo)航時2.5 m 和1.5 m上線距離下拖拉機(jī)的上線時間，圖6為采用角度傳感器導(dǎo)航時2.5和1.5 m上線距離下拖拉機(jī)的上線時間。在1.5和2.5 m的上線距離和相同速度下，采用傳統(tǒng)角度傳感器時上線時間分別為15.0 s和14.2 s，采用GNSS航向微分和MEMS陀螺儀測量輪角時上線時間分別為29.4和23.5 s，較采用傳統(tǒng)角度傳感器時的上線時間長，動態(tài)響應(yīng)性能稍遜。主要原因是模型失配導(dǎo)致角度測量誤差較大，上線超調(diào)量較大，但采用的自適應(yīng)卡爾曼濾波器收斂較快，且未出現(xiàn)發(fā)散和震蕩，上線時間在可接受范圍內(nèi)，能滿足導(dǎo)航作業(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陀螺儀測量轉(zhuǎn)向輪角導(dǎo)航時的上線時間Fig.5 On-line time at different on-line distances when measuring steering wheel angle with GNSS heading differential and MEMS gyroscope

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

2.3 田間直線導(dǎo)航應(yīng)用試驗

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

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

圖7 田間試驗中前輪轉(zhuǎn)向角觀測值（紅線）、濾波值（黑線）和角度傳感器測量值（藍(lán)線）對比圖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 拖拉機(jī)直線導(dǎo)航作業(yè)時橫向位置偏差Fig.8 Deviation of lateral position during linear navigation operation of tractor

3 結(jié)論

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

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

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

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

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

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