馬志艷，歐陽方熙，楊光友，余昌舜

(湖北工業(yè)大學(xué) 農(nóng)業(yè)機(jī)械工程研究設(shè)計(jì)院，武漢 430068)

0 引言

現(xiàn)代農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)一般由檢測(cè)單元、控制單元、執(zhí)行單元及監(jiān)控單元4部分組成，主要任務(wù)是對(duì)農(nóng)機(jī)進(jìn)行橫向位置控制，使其與預(yù)設(shè)路徑之間的橫向誤差保持在一定的精度范圍之內(nèi)，滿足農(nóng)業(yè)自動(dòng)化生產(chǎn)的需要[1]。農(nóng)機(jī)作業(yè)在復(fù)雜多變、非結(jié)構(gòu)化的野外農(nóng)田環(huán)境中，每種位置測(cè)量方法都有一定的局限性。例如，基于全球?qū)Ш叫l(wèi)星系統(tǒng)GNSS的導(dǎo)航方法應(yīng)用成本較高，且信號(hào)傳輸易受外界環(huán)境干擾；基于機(jī)器視覺導(dǎo)航方法受視覺傳感器分辨率、光照及作物生長(zhǎng)狀況的影響，導(dǎo)航路徑易受干擾，從而導(dǎo)致定位失效[2]；基于慣性導(dǎo)航方法受傳感器積分誤差的影響，導(dǎo)航誤差隨時(shí)間的累積而增加，從而使得導(dǎo)航失效。采用組合導(dǎo)航的方式將各種導(dǎo)航信息進(jìn)行融合，能夠?qū)崿F(xiàn)各導(dǎo)航方式優(yōu)勢(shì)互補(bǔ)，提升導(dǎo)航系統(tǒng)的檢測(cè)精度和穩(wěn)定性。

目前，市場(chǎng)上已有的農(nóng)機(jī)導(dǎo)航系統(tǒng)大多基于GNSS。隨著組合導(dǎo)航技術(shù)的發(fā)展，不同的導(dǎo)航信息組合已經(jīng)應(yīng)用到組合導(dǎo)航技術(shù)中，并取得了一定成果。如Nagasaka等研究的DGPS和陀螺的組合導(dǎo)航系統(tǒng)，例如，0.7m/s 的車載試驗(yàn)中，直線行走誤差控制在12cm以內(nèi)，但轉(zhuǎn)彎誤差較大[3]。Gao JC等基于卡爾曼濾波算法的GPS/INS車載式傳感器組合導(dǎo)航算法，對(duì)比單獨(dú)使用GPS信號(hào)的方法，在GPS信號(hào)中斷40s的情況下，可提升定位精度15%[4]。Gao GJ等提出的INS輔助高精度GPS導(dǎo)航方法，能夠在GPS信號(hào)受到較大干擾的情況下，提供有效的定位信息[5]。Wang等提出的基于模糊邏輯和專家系統(tǒng)的INS/ GPS卡爾曼濾波組合方案，證明了該方法的可行性[6]。高為廣等設(shè)計(jì)的GPS/INS組合導(dǎo)航自適應(yīng)濾波算法，能有效地控制觀測(cè)異常和狀態(tài)擾動(dòng)異常對(duì)參數(shù)估計(jì)的影響[7]。相對(duì)于GNSS/INS組合導(dǎo)航系統(tǒng)，視覺/INS組合導(dǎo)航系統(tǒng)具有成本低、體積小、易維護(hù)等優(yōu)點(diǎn)。視覺傳感器除了能提取出導(dǎo)航信息之外還能提供豐富的感知信息(如作物生長(zhǎng)情況、障礙物及實(shí)時(shí)路況等)，這為農(nóng)機(jī)進(jìn)一步的自主導(dǎo)航提供了可靠的信息來源。

本文以棚室電動(dòng)微耕機(jī)為試驗(yàn)平臺(tái)，在Raspberry Pi 3-B開發(fā)板上搭載JY901高精度慣性導(dǎo)航模塊及320×480像素USB攝像頭構(gòu)建視覺/SINS組合導(dǎo)航系統(tǒng)。該導(dǎo)航系統(tǒng)一方面利用慣性測(cè)量模塊，通過姿態(tài)矩陣將加速度計(jì)在載體坐標(biāo)系下的檢測(cè)量轉(zhuǎn)換到地理坐標(biāo)系中，進(jìn)而積分得到載體在地理坐標(biāo)系下的位置和速度信息；同時(shí)，利用視覺測(cè)量模塊得到載體在導(dǎo)航坐標(biāo)系下的位姿信息；最后，通過卡爾曼濾波對(duì)兩類數(shù)據(jù)進(jìn)行融合，實(shí)現(xiàn)導(dǎo)航信息的最優(yōu)估計(jì)。通過微耕機(jī)實(shí)際行駛數(shù)據(jù)進(jìn)行仿真，驗(yàn)證了算法的有效性。

1 組合導(dǎo)航系統(tǒng)架構(gòu)

1.1 組合導(dǎo)航系統(tǒng)結(jié)構(gòu)設(shè)計(jì)

導(dǎo)航系統(tǒng)按功能可分解為導(dǎo)航測(cè)量模塊和導(dǎo)航控制模塊：導(dǎo)航控制模塊由導(dǎo)航控制器和執(zhí)行機(jī)構(gòu)組成；導(dǎo)航測(cè)量模塊由視覺檢測(cè)模塊、慣性檢測(cè)模塊組成。該系統(tǒng)以電動(dòng)微耕機(jī)為試驗(yàn)平臺(tái)進(jìn)行車載試驗(yàn)，系統(tǒng)框架如圖1所示。

圖1 組合導(dǎo)航系統(tǒng)框架示意圖Fig.1 System framework of Integrated navigation

導(dǎo)航測(cè)量模塊利用陀螺儀和加速度計(jì)這兩個(gè)慣性測(cè)量單元，通過運(yùn)動(dòng)學(xué)方程獲得農(nóng)機(jī)在地理坐標(biāo)系下的位姿信息，同時(shí)從視覺模塊獲取在導(dǎo)航坐標(biāo)系下的位姿信息，采用卡爾曼濾波器進(jìn)行信息融合，從而獲得最終農(nóng)機(jī)位姿信息，并且對(duì)慣性導(dǎo)航的累積誤差進(jìn)行修正。導(dǎo)航測(cè)量模塊將當(dāng)前農(nóng)機(jī)位姿信息的最優(yōu)估計(jì)同期望跟隨路徑對(duì)比，通過導(dǎo)航控制模塊完成農(nóng)機(jī)對(duì)期望路徑的跟隨任務(wù)。

1.2 組合導(dǎo)航硬件組成

組合導(dǎo)航系統(tǒng)采用Raspberry Pi 3-B微型電腦為核心的嵌入式計(jì)算機(jī)，通過USB接口搭載JY901高精度慣性模塊和320×480USB攝像頭視覺模塊，構(gòu)成導(dǎo)航信息融合的主要硬件框架。該系統(tǒng)基于農(nóng)機(jī)運(yùn)動(dòng)學(xué)分析，將多傳感器的導(dǎo)航信息進(jìn)行融合，估計(jì)出農(nóng)機(jī)的位置、速度和航向信息，并將這些信息換算成電機(jī)的控制信號(hào)，最后通過Raspberry Pi 3-B微型電腦的I/O接口將控制數(shù)據(jù)發(fā)送到相應(yīng)的電機(jī)驅(qū)模塊實(shí)現(xiàn)農(nóng)機(jī)自動(dòng)導(dǎo)航，各硬件模塊如圖2所示。

1.RaspberryPi 3-B微型電腦 2.JY901慣性導(dǎo)航模塊 3.USB攝像頭 4.電動(dòng)微耕機(jī)

2 農(nóng)機(jī)運(yùn)動(dòng)學(xué)分析與建模

2.1 農(nóng)機(jī)工作路徑描述

在農(nóng)機(jī)實(shí)際作業(yè)過程中，主要以完成農(nóng)田環(huán)境中的直線行走任務(wù)為主。本文借鑒郭孔輝等提出的“預(yù)瞄-跟隨”駕駛員建模理論[8]，選用位置偏差d和航向角偏差ψ兩個(gè)變量，作為導(dǎo)航控制變量及導(dǎo)航過程中載體的位姿變量。其中，位置偏差為車輛行駛軌跡同預(yù)期規(guī)劃路徑的距離誤差，航向角偏差為車輛行駛航向角同預(yù)期路徑方向的夾角[9]。

假設(shè)農(nóng)機(jī)的期望路徑為既定直線，并以該直線為導(dǎo)航參考坐標(biāo)系，通過視覺模塊實(shí)時(shí)檢測(cè)該直線相對(duì)載體的位姿信息，通過慣性模塊實(shí)時(shí)檢測(cè)載體相對(duì)起始點(diǎn)的位姿信息。在利用慣性模塊測(cè)量信息時(shí)，需要把信息轉(zhuǎn)換到參考坐標(biāo)系下，再與視覺模塊檢測(cè)結(jié)果進(jìn)行信息融合[10]。載體在參考坐標(biāo)系下的位姿示意圖如圖3所示。

圖3 載體位姿描述示意圖Fig.3 Position and orientation of vehicle

2.2 視覺檢測(cè)位姿確定

將相機(jī)與車體固定，并且使相機(jī)坐標(biāo)系的yc-oc-zc平面垂直于地面，俯視角為φ。車載坐標(biāo)系原點(diǎn)位于相機(jī)坐標(biāo)系原點(diǎn)oc與地面的投影點(diǎn)o′軸指向載體的縱軸方向，Z軸方向垂直于地面向上，Y軸方向指向車輛方向的左側(cè)。組合導(dǎo)航坐標(biāo)系示意圖如圖4所示。

定義[xcyczc1]T為相機(jī)參考系下的坐標(biāo)，[xwywzw]T為車載坐標(biāo)系下的坐標(biāo)，L為兩坐標(biāo)系原點(diǎn)的距離。相機(jī)坐標(biāo)系與車載坐標(biāo)系之間的轉(zhuǎn)換關(guān)系為

(1)

如圖5所示，導(dǎo)航線在車載坐標(biāo)系下的直線表達(dá)式可表示為

(2)

其中，d為載體坐標(biāo)系原點(diǎn)到導(dǎo)航線的距離，并且近似將其當(dāng)作載體到導(dǎo)航線的偏離；ψ′為導(dǎo)航線與載體坐標(biāo)系Z軸的夾角。

圖5 導(dǎo)航路徑在載體坐標(biāo)系的投影Fig.5 Projection of the target path on the vehicle coordinate

導(dǎo)航線在相機(jī)成像平面的投影示意圖如圖6所示。由式(1) 、式(2) 可得導(dǎo)航線在xcocyc投影坐標(biāo)系的投影表達(dá)式為

(3)

同時(shí)，在導(dǎo)航線L在圖像中的投影為L(zhǎng)c，其表達(dá)式可寫為

(4)

其中，λ為投影坐標(biāo)系中導(dǎo)航線投影Lc到坐標(biāo)原點(diǎn)的距離；ψc為投影坐標(biāo)系中導(dǎo)航線投影Lc與Y軸偏角。由式(2)～式(4)可知，導(dǎo)航線L在平面o′x′z′的表達(dá)式為

(5)

在圖像中容易得到導(dǎo)航線的投影與xc軸的截距a，又因xw、zw有無窮組解，解得

tanψ'=tanψc·sinφ

(6)

(7)

圖6 導(dǎo)航路徑在相機(jī)坐標(biāo)系的投影Fig.6 Projection of the target path on the camera coordinate

由此，建立了圖像檢測(cè)模塊對(duì)車輛載體的位姿描述方程。

2.3 微耕機(jī)運(yùn)動(dòng)學(xué)分析

通常微耕機(jī)工作在在平整的農(nóng)田環(huán)境中，垂直方向上的變化可忽略。因此，由圖7可得簡(jiǎn)化的水平面位姿推算模型為

(8)

其中，Vn為載體沿導(dǎo)航路徑法向方向的速度；ax'和az'分別為載體沿載體坐標(biāo)系x軸和z軸的加速度；ω為載體沿載體坐標(biāo)系Y軸的角速度。公式中，ax'、az'和ω均可以由慣性導(dǎo)航模塊測(cè)量得到，如圖7所示。

圖7 載體二維運(yùn)動(dòng)示意圖Fig.7 Two-dimensional motion analysis of vehicle

陀螺儀和角加速度計(jì)測(cè)得的角速度和加速度存在著測(cè)量誤差，將加速度和角速度的測(cè)量值表達(dá)為

(9)

其中，ωc、ax′c、az′c為陀螺儀和加速度計(jì)在X、Z軸的測(cè)量值；ω、ax′、az′為對(duì)應(yīng)的真值，wω、wax′、waz′為對(duì)應(yīng)的測(cè)量噪聲。由式(8)、式(9)可得水平面位姿推算帶測(cè)量噪聲的方程，即

(10)

3 基于卡爾曼濾波的組合導(dǎo)航

3.1 卡爾曼濾波

卡爾曼濾波是一種遞推的狀態(tài)空間方法。在離散過程中，該方法利用狀態(tài)方程描述系統(tǒng)狀態(tài)的轉(zhuǎn)移過程，并結(jié)合上次的最優(yōu)估計(jì)獲取系統(tǒng)狀態(tài)的當(dāng)前估計(jì)；利用觀測(cè)方程獲取狀態(tài)的觀測(cè)值，再結(jié)合二者得出當(dāng)前的最優(yōu)估計(jì)。它只用狀態(tài)的前一個(gè)估計(jì)值和最近一個(gè)觀察值，就可以在線性無偏最小方差估計(jì)準(zhǔn)則下對(duì)當(dāng)前狀態(tài)做出最優(yōu)估計(jì)。其遞推公式[11]為

(11)

其中，Kk為卡爾曼增益矩陣，Pk/k-1為預(yù)測(cè)誤差協(xié)方差矩陣，Pk為估計(jì)誤差協(xié)方差矩陣，Rk為測(cè)量方差矩陣,Qk-1為過程方差矩陣。

3.2 組合導(dǎo)航系統(tǒng)方程

經(jīng)典的卡爾曼濾波只適用于線性且滿足高斯分布的系統(tǒng)，但此時(shí)的系統(tǒng)并不是線性的，需要應(yīng)用擴(kuò)展卡爾曼濾波(EKF)來解決這種情況。設(shè)置系統(tǒng)的采樣時(shí)間為Δt，在估計(jì)狀態(tài)值處進(jìn)行泰勒級(jí)數(shù)展開，忽略二次級(jí)以上的高階項(xiàng)，一階線性化處理后的系統(tǒng)狀態(tài)方程[12]為

Xk=ΦXk-1+ΓW

(12)

由系統(tǒng)的視覺測(cè)量模塊的測(cè)量變量可確定，卡爾曼濾波的測(cè)量向量為

(13)

根據(jù)選定的狀態(tài)向量和量測(cè)向量可得系統(tǒng)的量測(cè)方程為

Zk=HXk+V

(14)

其中

根據(jù)狀態(tài)方程(12)和測(cè)量方程(14)，即可結(jié)合前面的卡爾曼濾波公式(11)進(jìn)行擴(kuò)展卡爾曼濾波器的設(shè)計(jì)，進(jìn)而對(duì)系統(tǒng)的狀態(tài)進(jìn)行估計(jì)。

4 仿真與試驗(yàn)

4.1 仿真初始條件

1)系統(tǒng)噪聲和測(cè)量噪聲均為高斯白噪聲，陀螺漂移設(shè)為均值0.01(°)/h、標(biāo)準(zhǔn)差為0.005(°)/h的隨機(jī)偏差，加速度計(jì)零偏值設(shè)為10-2g(g=9.5m/s2)、標(biāo)準(zhǔn)差為5×10-3g的隨機(jī)偏差。

2)卡爾曼濾波初始條件：X0取0向量，則

P0=diag{(0.2)2m,(0.2)2m/s,(0.001672)°}

(15)

Q=diag{(5×10-3)2m,(5×10-3)2m/s,(0.00082)°}

(16)

人工控制載體在預(yù)訂的路徑上行駛，并實(shí)時(shí)采集行駛過程中的導(dǎo)航數(shù)據(jù)，同時(shí)對(duì)真實(shí)的行駛軌跡進(jìn)行采樣、擬合。將卡爾曼濾波器融合的結(jié)果與行駛軌跡的擬合結(jié)果進(jìn)行對(duì)比，即可得到試驗(yàn)結(jié)論。

4.2 仿真結(jié)果分析

實(shí)際試驗(yàn)過程中，選擇較為平整的溫室大棚田地作為場(chǎng)地，設(shè)定車輛作業(yè)的行進(jìn)速度為1m/s，信息采樣的周期為200ms，保證預(yù)設(shè)路徑的圖像始終出現(xiàn)在相機(jī)的成像平面中。整個(gè)試驗(yàn)過程，車輛的作業(yè)距離大約為50m。對(duì)采集的一組數(shù)據(jù)進(jìn)行實(shí)驗(yàn)仿真，計(jì)算結(jié)果表明：該算法能保證組合導(dǎo)航系統(tǒng)狀態(tài)收斂，有效提高導(dǎo)航的精度。圖8為融合導(dǎo)航信息與實(shí)時(shí)行駛參數(shù)的對(duì)比結(jié)果。其中，實(shí)線為對(duì)行駛軌跡真實(shí)采樣點(diǎn)的擬合，由圖8(a)到圖8(c)分別記錄了載體行進(jìn)過程中，載體相對(duì)于目標(biāo)導(dǎo)航線的法向偏移、法向速度和行偏角。由圖8可以看出：雖然載體在復(fù)雜的農(nóng)田環(huán)境中行駛軌跡、車身姿態(tài)難以達(dá)到理想狀態(tài)，但融合的導(dǎo)航數(shù)據(jù)基本能快速收斂于真實(shí)數(shù)據(jù)且具有較高精度。其中，法向偏移的最大誤差不超過10cm，航偏角的最大誤差不超過1.0°。

圖8 仿真最優(yōu)估計(jì)與真實(shí)狀態(tài)對(duì)比圖Fig.8 Result of simulation and actual

5 結(jié)論

介紹了一種農(nóng)機(jī)多傳感器組合導(dǎo)航方法。該方法通過建立以導(dǎo)航線為參考系的位姿描述方程及對(duì)載體進(jìn)行運(yùn)動(dòng)學(xué)建模，完成了基于視覺和慣性傳感器組合導(dǎo)航系統(tǒng)平臺(tái)搭建，并采用卡爾曼擴(kuò)展濾波方法進(jìn)行導(dǎo)航信息融合，能夠?qū)崟r(shí)可靠地計(jì)算出車輛的位姿與速度信息。實(shí)驗(yàn)結(jié)果表明：該方法在一定條件下能較好地滿足農(nóng)機(jī)作業(yè)高精度自動(dòng)導(dǎo)航的要求。