基于視覺與慣性的農機組合導航的方法研究

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

(湖北工業大學 農業機械工程研究設計院，武漢 430068)

0 引言

現代農機自動導航系統一般由檢測單元、控制單元、執行單元及監控單元4部分組成，主要任務是對農機進行橫向位置控制，使其與預設路徑之間的橫向誤差保持在一定的精度范圍之內，滿足農業自動化生產的需要[1]。農機作業在復雜多變、非結構化的野外農田環境中，每種位置測量方法都有一定的局限性。例如，基于全球導航衛星系統GNSS的導航方法應用成本較高，且信號傳輸易受外界環境干擾；基于機器視覺導航方法受視覺傳感器分辨率、光照及作物生長狀況的影響，導航路徑易受干擾，從而導致定位失效[2]；基于慣性導航方法受傳感器積分誤差的影響，導航誤差隨時間的累積而增加，從而使得導航失效。采用組合導航的方式將各種導航信息進行融合，能夠實現各導航方式優勢互補，提升導航系統的檢測精度和穩定性。

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

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

1 組合導航系統架構

1.1 組合導航系統結構設計

導航系統按功能可分解為導航測量模塊和導航控制模塊：導航控制模塊由導航控制器和執行機構組成；導航測量模塊由視覺檢測模塊、慣性檢測模塊組成。該系統以電動微耕機為試驗平臺進行車載試驗，系統框架如圖1所示。

圖1 組合導航系統框架示意圖Fig.1 System framework of Integrated navigation

導航測量模塊利用陀螺儀和加速度計這兩個慣性測量單元，通過運動學方程獲得農機在地理坐標系下的位姿信息，同時從視覺模塊獲取在導航坐標系下的位姿信息，采用卡爾曼濾波器進行信息融合，從而獲得最終農機位姿信息，并且對慣性導航的累積誤差進行修正。導航測量模塊將當前農機位姿信息的最優估計同期望跟隨路徑對比，通過導航控制模塊完成農機對期望路徑的跟隨任務。

1.2 組合導航硬件組成

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

1.RaspberryPi 3-B微型電腦 2.JY901慣性導航模塊 3.USB攝像頭 4.電動微耕機

2 農機運動學分析與建模

2.1 農機工作路徑描述

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

假設農機的期望路徑為既定直線，并以該直線為導航參考坐標系，通過視覺模塊實時檢測該直線相對載體的位姿信息，通過慣性模塊實時檢測載體相對起始點的位姿信息。在利用慣性模塊測量信息時，需要把信息轉換到參考坐標系下，再與視覺模塊檢測結果進行信息融合[10]。載體在參考坐標系下的位姿示意圖如圖3所示。

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

2.2 視覺檢測位姿確定

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

定義[xcyczc1]T為相機參考系下的坐標，[xwywzw]T為車載坐標系下的坐標，L為兩坐標系原點的距離。相機坐標系與車載坐標系之間的轉換關系為

(1)

如圖5所示，導航線在車載坐標系下的直線表達式可表示為

(2)

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

圖5 導航路徑在載體坐標系的投影Fig.5 Projection of the target path on the vehicle coordinate

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

(3)

同時，在導航線L在圖像中的投影為Lc，其表達式可寫為

(4)

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

(5)

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

tanψ'=tanψc·sinφ

(6)

(7)

圖6 導航路徑在相機坐標系的投影Fig.6 Projection of the target path on the camera coordinate

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

2.3 微耕機運動學分析

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

(8)

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

圖7 載體二維運動示意圖Fig.7 Two-dimensional motion analysis of vehicle

陀螺儀和角加速度計測得的角速度和加速度存在著測量誤差，將加速度和角速度的測量值表達為

(9)

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

(10)

3 基于卡爾曼濾波的組合導航

3.1 卡爾曼濾波

卡爾曼濾波是一種遞推的狀態空間方法。在離散過程中，該方法利用狀態方程描述系統狀態的轉移過程，并結合上次的最優估計獲取系統狀態的當前估計；利用觀測方程獲取狀態的觀測值，再結合二者得出當前的最優估計。它只用狀態的前一個估計值和最近一個觀察值，就可以在線性無偏最小方差估計準則下對當前狀態做出最優估計。其遞推公式[11]為

(11)

其中，Kk為卡爾曼增益矩陣，Pk/k-1為預測誤差協方差矩陣，Pk為估計誤差協方差矩陣，Rk為測量方差矩陣,Qk-1為過程方差矩陣。

3.2 組合導航系統方程

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

Xk=ΦXk-1+ΓW

(12)

由系統的視覺測量模塊的測量變量可確定，卡爾曼濾波的測量向量為

(13)

根據選定的狀態向量和量測向量可得系統的量測方程為

Zk=HXk+V

(14)

其中

根據狀態方程(12)和測量方程(14)，即可結合前面的卡爾曼濾波公式(11)進行擴展卡爾曼濾波器的設計，進而對系統的狀態進行估計。

4 仿真與試驗

4.1 仿真初始條件

1)系統噪聲和測量噪聲均為高斯白噪聲，陀螺漂移設為均值0.01(°)/h、標準差為0.005(°)/h的隨機偏差，加速度計零偏值設為10-2g(g=9.5m/s2)、標準差為5×10-3g的隨機偏差。

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)

人工控制載體在預訂的路徑上行駛，并實時采集行駛過程中的導航數據，同時對真實的行駛軌跡進行采樣、擬合。將卡爾曼濾波器融合的結果與行駛軌跡的擬合結果進行對比，即可得到試驗結論。

4.2 仿真結果分析

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

圖8 仿真最優估計與真實狀態對比圖Fig.8 Result of simulation and actual

5 結論

介紹了一種農機多傳感器組合導航方法。該方法通過建立以導航線為參考系的位姿描述方程及對載體進行運動學建模，完成了基于視覺和慣性傳感器組合導航系統平臺搭建，并采用卡爾曼擴展濾波方法進行導航信息融合，能夠實時可靠地計算出車輛的位姿與速度信息。實驗結果表明：該方法在一定條件下能較好地滿足農機作業高精度自動導航的要求。