壟間無人運輸車自主導航糾偏方法*

隨連杰 ， 劉義亭 ， 郁漢琪

（1.南京工程學院研究生院，江蘇 南京 211167；2.南京工程學院自動化學院，江蘇 南京 211167；3.東南大學信息科學與工程學院，江蘇 南京 210096；4.南京工程學院工業中心，江蘇 南京 211167）

0 引言

《中國制造2025》將農業機械自動化列入我國發展的十大重點領域，旨在2025年全面實現農業產品結構升級并突破高端產品技術應用[1]。農業機器人是結合機器人技術、自動控制技術、無線傳輸技術等技術共同開發出來的一種具體的以農業生產為背景的生產技術[2-4]。近年來，隨著物聯網技術、深度學習、大數據等理論知識的普及，農業機器人也逐漸產生并完善，在具體的農業生產中也有應用[5]。Plessen[6]提出了兩種規劃模式，通過使用減輕重量的控制方法和單次田間覆蓋方式來減輕土壤壓實。Jensen等[7]提出了一種用于農業作業中運輸單元的路徑規劃方法，涉及田間和田間運輸，以優化時間或行程標準。Zhang等[8]舉例說明了一種路徑模式，通過避免在地頭耗費時間，引導履帶式聯合收割機從外到內工作。此外，地頭轉彎是農場工作中的一個重要考慮因素，在很大程度上影響工作效率[9]。相應地，各種自動導航系統由研究人員和制造商開發研制，以促進農業生產的自動化[10-11]。

無人運輸車在進行溫室大棚壟間作業時，單純只靠激光雷達一種傳感器，容易出現兩側預留空間不夠而導致導航失敗的情況，甚至產生碰撞危險。因此，設計融合激光測距儀、激光雷達以及陀螺儀三種傳感器的導航糾偏系統，該系統適用于溫室大棚內壟間作業時自動糾偏的導航運輸車設備。

1 整機結構與硬件設備

1.1 總體架構

如圖1所示，農業無人運輸車導航系統可由三個模塊構成：1）PC端從機與工控機交互模塊，該模塊主要功能是將建圖與導航過程可視化；2）激光雷達、里程計、激光測距儀和IMU傳感器模塊，該模塊主要功能是為導航系統提供機器人位姿信息與環境信息；3）建圖與導航功能模塊，該模塊主要功能是通過SLAM與路徑規劃算法實現機器人建圖與導航。

圖1 農業無人運輸車總體架構圖

1.2 硬件設計

本文所用的導航無人運輸車的硬件結構為兩輪差速驅動結構，如圖2所示。左右驅動輪線速度的計算公式如下：

式中，φ1表示右驅動輪轉速，單位m/s；φ2表示左驅動輪轉速，單位m/s；r表示驅動輪半徑，單位m。

則無人運輸車的導航線速度Vd和導航角速度ωd分別為：

式中，L表示導航無人車中心到兩輪的距離，單位m。

如圖2所示，陀螺儀安裝在導航無人車的前方中心，用于測量導航無人車的三軸角速度。激光測距儀分別安裝在導航無人車的右前、右后、左前、左后位置，用于測量導航無人車與兩側田壟的距離。

圖2 無人運輸車硬件結構示意圖

2 融合定位與糾偏策略

2.1 擴展卡爾曼濾波融合IMU

系統矩陣和測量矩陣的線性化將導致誤差協方差矩陣和卡爾曼增益成為狀態估計的函數。當卡爾曼濾波器使用先前的最優估計而不是預定義值應用于線性化時，則稱為擴展卡爾曼濾波器[12]。最佳估計值用于初始化卡爾曼濾波器，然后將新的估計值作為下一時期的輸入值。因此，擴展卡爾曼濾波器使用閉環濾波器的架構。在閉環校正結構中，將估計的誤差反饋給IMU進行校正，這樣可以保持系統模型的線性假設；相反，在開環系統中，誤差或校正狀態不會在每次迭代時反饋給IMU。因此，本文采用擴展卡爾曼濾波的閉環架構來進行融合，將其運用到非線性系統的過程如下所示。

1）狀態模型：

式中，^xk表示k時刻的狀態向量；uk表示控制向量，如加速度、角加速度等；Qk表示預測狀態的高斯噪聲的協方差陣，它用來衡量模型的準確度，模型越準確其值越小表示狀態向量的協方差矩陣，代表著狀態向量每個元素之間的關系。

2）更新模型：

式中，Rk表示測量值的高斯噪聲的協方差陣，代表著傳感器測量的誤差。Fk和Hk分別是f和h的偏導雅可比矩陣，可由式（10）、（11）表示：

2.2 糾偏判斷方法

橫向偏移量DP通過同側兩個激光測距儀所測距離與目標距離設定值作差得到，橫向偏移量DP的計算公式如下：

式中，C1、C2表示同側激光測距儀所測得的數據，單位m；Dset表示右側目標距離設定值，為0.5 m。

航向偏移量AP通過同側兩個激光測距儀所測距離的差值與同側激光測距儀的距離相除后的反正切值得到，航向偏移量AP的計算公式如下：

式中，pdist表示同側激光測距儀的距離，單位m。

將計算所得到的橫向偏移量DP與初始設定的距離標準值Dm相比較，同時根據路況信息對是否需要糾偏作出判斷。根據多次測量比較，最終距離標準值Dm設置為30 mm。若橫向偏移量DP大于距離標準值Dm，同時路況信息判斷該路況為非轉彎路段，則判定該導航無人車未進行直線前進，需要在Y方向上控制橫向偏移量，直到所測100組激光測距儀的數據的橫向偏移量絕對值小于或等于距離標準值的數量大于等于50組。

圖3為無人運輸車在進行Y方向上調偏時，同側激光測距儀測量數據以及橫向偏差量數據。如圖3（a）所示，右側前后兩個激光測距儀測量距離在未出現導航調偏系統介入時，測量的距離均保持在距離設定值0.5 m上下波動。當前后兩側激光測距儀測量距離驟減，同時圖3（b）中橫向偏移量DP絕對值大于Dm，則糾偏系統開始介入糾偏，直至右側前后兩個激光測距儀測量距離開始返回距離設定值，橫向偏移量絕對值小于或等于距離標準值的數量大于等于50組，調偏結束。

圖3 同側激光測距儀測量距離及橫向偏差

在進行Y方向上的糾偏時，給定無人車以斜45°方向進行糾偏，則此時的調偏角速度ωt為：

式中，N表示此次調偏采集激光測距儀數據的次數；Δt表示相鄰兩次采集數據時間差，單位s。

糾偏所需要的調偏線速度Vt計算公式如下：

式中，ΔD表示橫向偏差量，單位m；Δt表示相鄰兩次采集激光測距儀數據的時間差，單位s。

將計算所得到的航向偏移量AP與初始設定的航向標準值Am進行比較，同時根據路況信息對是否需要糾偏作出判斷，根據多次測量比較，最終航向標準值Am設置為5°。若航向偏移量AP大于航向標準值Am，同時路況信息判斷該路況為非轉彎路段，則判定該導航無人車已偏航，需要在Z方向上控制航向偏移量，直到陀螺儀的數據的航向偏移量回零。

如圖4所示，導航無人車發生航向偏移時的300組航向偏移量數據，一旦導航無人車航向偏移量大于航向標準值，糾偏系統介入并進行Z方向上的調偏，即圖中一號區域。在進行Z方向上的調偏時，糾偏系統只通過計算旋轉角速度發送給導航控制話題move_base，不發送線速度，即糾偏系統介入時的導航無人車會停下來進行原地旋轉調整，直至與兩側障礙物平行，糾偏結束。糾偏所需要的Z方向上的糾偏角速度ωt′計算公式如下：

圖4 航向糾偏時航向偏移量數據

式中，ΔA表示航向偏差量，單位°；Δt′表示相鄰兩次采集陀螺儀數據的時間差，單位s；ωt′表示Z方向上的糾偏角速度，單位rad/s。

2.3 導航調偏方法

導航無人運輸車調偏流程如下。

步驟1：以導航無人車自身坐標系建立車身坐標系，導航無人車中心為原點，導航無人車前進方向為X方向，左右方向為Y方向，上下方向為Z方向；

步驟2：安裝陀螺儀和激光測距儀，通過陀螺儀和激光測距儀實時采集導航無人車位姿信息；

步驟3：根據陀螺儀和激光測距儀實時采集的數據，判斷路況信息并計算橫向偏移量DP和航向偏移量AP；

步驟4：主控單元根據路況信息和偏移量判斷無人導航無人車是否需要進行糾偏，如需糾偏，則進一步計算糾偏所需要的調偏線速度Vt和調偏角速度ωt，結合導航角速度ωd和導航線速度Vd，得到目標線速度Vx和目標角速度ω，進入步驟5；

步驟5：通過移動控制節點將目標線速度Vx和目標角速度ω發布至速度話題上；

步驟6：導航無人車根據速度話題上的速度，分別計算出左驅動輪和右驅動輪所需要的脈沖個數，進行導航無人車自主導航糾偏。

3 實驗及數據分析

3.1 導航路線規劃實驗

全局路徑規劃算法使用傳統的A*算法，其原理重點是從初始狀態向外擴展，計算每個相鄰狀態的評估函數值，選擇評估函數值最小的狀態作為下一個遍歷狀態。然后重復擴展過程，直到遍歷目標狀態。如圖5所示，藍色路徑（虛線）為A*算法導航規劃路徑，可以明顯地看出該算法為了獲取更短的搜索時間和最短路徑，沒有著重注意與障礙物的最小距離，導致其與障礙物的距離較小，容易發生碰撞。紅色路徑（實線）為導航糾偏系統介入時的導航規劃路徑，可以清楚地看出在路徑規劃時，糾偏系統會避免與障礙物有過小的距離，在保證安全距離的前提下，指導導航系統在田壟中心線行駛，避免產生碰撞危險。

圖5 壟間導航路徑規劃圖

3.2 路況信息識別實驗

課題組設計的導航糾偏系統通過讀取陀螺儀和兩側激光測距儀的數據來判斷此時所處的路況信息以及導航調偏效果反饋。圖6～圖9為導航糾偏系統介入轉彎路況時陀螺儀和兩側激光測距儀輸出數據圖。

如圖6、圖7所示，在導航無人運輸車進行轉彎操作時，陀螺儀PITCH軸和ROLL軸輸出信號趨于平穩，即導航無人運輸車轉彎操作時沒有X方向和Y方向的速率變化。如圖8所示，陀螺儀YAW軸輸出信號中有一段極速變化的數據，即此時導航無人運輸車Z軸旋轉角速率快速增加，這與圖9兩側激光測距儀輸出距離圖中右側距離快速增加、左側距離快速下降的一段相對應。同時，根據激光測距儀左側距離減小、右側距離增加，可以判斷此時為左轉操作。

圖6 陀螺儀PITCH軸輸出信號圖

圖7 陀螺儀ROLL軸輸出信號圖

圖8 陀螺儀YAW軸輸出信號圖

圖9 兩側激光測距儀輸出距離圖

3.3 導航糾偏效果實驗

本實驗是通過將自主導航時的最大橫向偏差和最大航向偏差與導航糾偏系統介入時的最大橫向偏差和最大航向偏差對比，進而可以明顯地看出本導航糾偏系統對于壟間無人運輸車的糾偏效果。

如表1所示，在自主導航實驗中，最大橫向偏差為68.50 mm，最大航向偏差為21.7°，平均最大橫向偏差為54.84 mm，平均最大航向偏差為17.38°?？梢钥闯龃藭r的偏差較大，對于農業溫室大棚環境，相鄰田壟間距固定且狹窄，導航無人運輸車會與兩側田壟距離較近，有時會直接與田壟碰撞，導致導航失敗，同時會有一定危險。在有導航糾偏系統介入實驗中，最大橫向偏差為38.02 mm，最大航向偏差為16.1°，平均最大橫向偏差為24.88 mm，平均最大航向偏差為9.7°。導航無人運輸車會盡量保持在兩側田壟的中線上行駛，雖然存在一定偏差，但能根據陀螺儀和兩側激光測距儀傳感器傳輸回來的數據及時對偏航狀態進行調整，不會導致導航系統意外終止，避免壟間導航無人運輸車與兩側田壟相碰撞的危險。

表1 自主導航與導航調偏介入時偏差數據對比

4 結論

課題組首先對壟間導航無人車系統進行總體框架設計，并分析了其硬件結構；其次利用擴展卡爾曼濾波融合IMU對導航無人進行先驗定位；然后分析通過陀螺儀和激光測距儀兩種傳感器數據的策略，可以對此時的路況信息進行準確的判斷，計算偏差并介入導航控制系統，最終提高了無人運輸車導航糾偏的準確性。

實驗表明，應用課題組提出的導航糾偏方法，規劃路線會與兩側保持一定安全距離。10次測試中，最大橫向偏差為38.02 mm，最大航向偏差為16.1°，平均最大橫向偏差為24.88 mm，平均最大航向偏差為9.7°。導航糾偏系統介入，可以綜合調整距離和方向偏差，保證壟間導航無人運輸車可以在相鄰田壟中線附近行駛，避免壟間導航無人運輸車與兩側田壟相碰撞，為壟間導航無人運輸車的偏差調整提供了一種準確、高效、可靠的控制方法。