隨連杰 ， 劉義亭 ， 郁漢琪

（1.南京工程學(xué)院研究生院，江蘇 南京 211167；2.南京工程學(xué)院自動(dòng)化學(xué)院，江蘇 南京 211167；3.東南大學(xué)信息科學(xué)與工程學(xué)院，江蘇 南京 210096；4.南京工程學(xué)院工業(yè)中心，江蘇 南京 211167）

0 引言

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

無(wú)人運(yùn)輸車在進(jìn)行溫室大棚壟間作業(yè)時(shí)，單純只靠激光雷達(dá)一種傳感器，容易出現(xiàn)兩側(cè)預(yù)留空間不夠而導(dǎo)致導(dǎo)航失敗的情況，甚至產(chǎn)生碰撞危險(xiǎn)。因此，設(shè)計(jì)融合激光測(cè)距儀、激光雷達(dá)以及陀螺儀三種傳感器的導(dǎo)航糾偏系統(tǒng)，該系統(tǒng)適用于溫室大棚內(nèi)壟間作業(yè)時(shí)自動(dòng)糾偏的導(dǎo)航運(yùn)輸車設(shè)備。

1 整機(jī)結(jié)構(gòu)與硬件設(shè)備

1.1 總體架構(gòu)

如圖1所示，農(nóng)業(yè)無(wú)人運(yùn)輸車導(dǎo)航系統(tǒng)可由三個(gè)模塊構(gòu)成：1）PC端從機(jī)與工控機(jī)交互模塊，該模塊主要功能是將建圖與導(dǎo)航過程可視化；2）激光雷達(dá)、里程計(jì)、激光測(cè)距儀和IMU傳感器模塊，該模塊主要功能是為導(dǎo)航系統(tǒng)提供機(jī)器人位姿信息與環(huán)境信息；3）建圖與導(dǎo)航功能模塊，該模塊主要功能是通過SLAM與路徑規(guī)劃算法實(shí)現(xiàn)機(jī)器人建圖與導(dǎo)航。

圖1 農(nóng)業(yè)無(wú)人運(yùn)輸車總體架構(gòu)圖

1.2 硬件設(shè)計(jì)

本文所用的導(dǎo)航無(wú)人運(yùn)輸車的硬件結(jié)構(gòu)為兩輪差速驅(qū)動(dòng)結(jié)構(gòu)，如圖2所示。左右驅(qū)動(dòng)輪線速度的計(jì)算公式如下：

式中，φ1表示右驅(qū)動(dòng)輪轉(zhuǎn)速，單位m/s；φ2表示左驅(qū)動(dòng)輪轉(zhuǎn)速，單位m/s；r表示驅(qū)動(dòng)輪半徑，單位m。

則無(wú)人運(yùn)輸車的導(dǎo)航線速度Vd和導(dǎo)航角速度ωd分別為：

式中，L表示導(dǎo)航無(wú)人車中心到兩輪的距離，單位m。

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

圖2 無(wú)人運(yùn)輸車硬件結(jié)構(gòu)示意圖

2 融合定位與糾偏策略

2.1 擴(kuò)展卡爾曼濾波融合IMU

系統(tǒng)矩陣和測(cè)量矩陣的線性化將導(dǎo)致誤差協(xié)方差矩陣和卡爾曼增益成為狀態(tài)估計(jì)的函數(shù)。當(dāng)卡爾曼濾波器使用先前的最優(yōu)估計(jì)而不是預(yù)定義值應(yīng)用于線性化時(shí)，則稱為擴(kuò)展卡爾曼濾波器[12]。最佳估計(jì)值用于初始化卡爾曼濾波器，然后將新的估計(jì)值作為下一時(shí)期的輸入值。因此，擴(kuò)展卡爾曼濾波器使用閉環(huán)濾波器的架構(gòu)。在閉環(huán)校正結(jié)構(gòu)中，將估計(jì)的誤差反饋給IMU進(jìn)行校正，這樣可以保持系統(tǒng)模型的線性假設(shè)；相反，在開環(huán)系統(tǒng)中，誤差或校正狀態(tài)不會(huì)在每次迭代時(shí)反饋給IMU。因此，本文采用擴(kuò)展卡爾曼濾波的閉環(huán)架構(gòu)來(lái)進(jìn)行融合，將其運(yùn)用到非線性系統(tǒng)的過程如下所示。

1）狀態(tài)模型：

式中，^xk表示k時(shí)刻的狀態(tài)向量；uk表示控制向量，如加速度、角加速度等；Qk表示預(yù)測(cè)狀態(tài)的高斯噪聲的協(xié)方差陣，它用來(lái)衡量模型的準(zhǔn)確度，模型越準(zhǔn)確其值越小表示狀態(tài)向量的協(xié)方差矩陣，代表著狀態(tài)向量每個(gè)元素之間的關(guān)系。

2）更新模型：

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

2.2 糾偏判斷方法

橫向偏移量DP通過同側(cè)兩個(gè)激光測(cè)距儀所測(cè)距離與目標(biāo)距離設(shè)定值作差得到，橫向偏移量DP的計(jì)算公式如下：

式中，C1、C2表示同側(cè)激光測(cè)距儀所測(cè)得的數(shù)據(jù)，單位m；Dset表示右側(cè)目標(biāo)距離設(shè)定值，為0.5 m。

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

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

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

圖3為無(wú)人運(yùn)輸車在進(jìn)行Y方向上調(diào)偏時(shí)，同側(cè)激光測(cè)距儀測(cè)量數(shù)據(jù)以及橫向偏差量數(shù)據(jù)。如圖3（a）所示，右側(cè)前后兩個(gè)激光測(cè)距儀測(cè)量距離在未出現(xiàn)導(dǎo)航調(diào)偏系統(tǒng)介入時(shí)，測(cè)量的距離均保持在距離設(shè)定值0.5 m上下波動(dòng)。當(dāng)前后兩側(cè)激光測(cè)距儀測(cè)量距離驟減，同時(shí)圖3（b）中橫向偏移量DP絕對(duì)值大于Dm，則糾偏系統(tǒng)開始介入糾偏，直至右側(cè)前后兩個(gè)激光測(cè)距儀測(cè)量距離開始返回距離設(shè)定值，橫向偏移量絕對(duì)值小于或等于距離標(biāo)準(zhǔn)值的數(shù)量大于等于50組，調(diào)偏結(jié)束。

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

在進(jìn)行Y方向上的糾偏時(shí)，給定無(wú)人車以斜45°方向進(jìn)行糾偏，則此時(shí)的調(diào)偏角速度ωt為：

式中，N表示此次調(diào)偏采集激光測(cè)距儀數(shù)據(jù)的次數(shù)；Δt表示相鄰兩次采集數(shù)據(jù)時(shí)間差，單位s。

糾偏所需要的調(diào)偏線速度Vt計(jì)算公式如下：

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

將計(jì)算所得到的航向偏移量AP與初始設(shè)定的航向標(biāo)準(zhǔn)值A(chǔ)m進(jìn)行比較，同時(shí)根據(jù)路況信息對(duì)是否需要糾偏作出判斷，根據(jù)多次測(cè)量比較，最終航向標(biāo)準(zhǔn)值A(chǔ)m設(shè)置為5°。若航向偏移量AP大于航向標(biāo)準(zhǔn)值A(chǔ)m，同時(shí)路況信息判斷該路況為非轉(zhuǎn)彎路段，則判定該導(dǎo)航無(wú)人車已偏航，需要在Z方向上控制航向偏移量，直到陀螺儀的數(shù)據(jù)的航向偏移量回零。

如圖4所示，導(dǎo)航無(wú)人車發(fā)生航向偏移時(shí)的300組航向偏移量數(shù)據(jù)，一旦導(dǎo)航無(wú)人車航向偏移量大于航向標(biāo)準(zhǔn)值，糾偏系統(tǒng)介入并進(jìn)行Z方向上的調(diào)偏，即圖中一號(hào)區(qū)域。在進(jìn)行Z方向上的調(diào)偏時(shí)，糾偏系統(tǒng)只通過計(jì)算旋轉(zhuǎn)角速度發(fā)送給導(dǎo)航控制話題move_base，不發(fā)送線速度，即糾偏系統(tǒng)介入時(shí)的導(dǎo)航無(wú)人車會(huì)停下來(lái)進(jìn)行原地旋轉(zhuǎn)調(diào)整，直至與兩側(cè)障礙物平行，糾偏結(jié)束。糾偏所需要的Z方向上的糾偏角速度ωt′計(jì)算公式如下：

圖4 航向糾偏時(shí)航向偏移量數(shù)據(jù)

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

2.3 導(dǎo)航調(diào)偏方法

導(dǎo)航無(wú)人運(yùn)輸車調(diào)偏流程如下。

步驟1：以導(dǎo)航無(wú)人車自身坐標(biāo)系建立車身坐標(biāo)系，導(dǎo)航無(wú)人車中心為原點(diǎn)，導(dǎo)航無(wú)人車前進(jìn)方向?yàn)閄方向，左右方向?yàn)閅方向，上下方向?yàn)閆方向；

步驟2：安裝陀螺儀和激光測(cè)距儀，通過陀螺儀和激光測(cè)距儀實(shí)時(shí)采集導(dǎo)航無(wú)人車位姿信息；

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

步驟4：主控單元根據(jù)路況信息和偏移量判斷無(wú)人導(dǎo)航無(wú)人車是否需要進(jìn)行糾偏，如需糾偏，則進(jìn)一步計(jì)算糾偏所需要的調(diào)偏線速度Vt和調(diào)偏角速度ωt，結(jié)合導(dǎo)航角速度ωd和導(dǎo)航線速度Vd，得到目標(biāo)線速度Vx和目標(biāo)角速度ω，進(jìn)入步驟5；

步驟5：通過移動(dòng)控制節(jié)點(diǎn)將目標(biāo)線速度Vx和目標(biāo)角速度ω發(fā)布至速度話題上；

步驟6：導(dǎo)航無(wú)人車根據(jù)速度話題上的速度，分別計(jì)算出左驅(qū)動(dòng)輪和右驅(qū)動(dòng)輪所需要的脈沖個(gè)數(shù)，進(jìn)行導(dǎo)航無(wú)人車自主導(dǎo)航糾偏。

3 實(shí)驗(yàn)及數(shù)據(jù)分析

3.1 導(dǎo)航路線規(guī)劃實(shí)驗(yàn)

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

圖5 壟間導(dǎo)航路徑規(guī)劃圖

3.2 路況信息識(shí)別實(shí)驗(yàn)

課題組設(shè)計(jì)的導(dǎo)航糾偏系統(tǒng)通過讀取陀螺儀和兩側(cè)激光測(cè)距儀的數(shù)據(jù)來(lái)判斷此時(shí)所處的路況信息以及導(dǎo)航調(diào)偏效果反饋。圖6～圖9為導(dǎo)航糾偏系統(tǒng)介入轉(zhuǎn)彎路況時(shí)陀螺儀和兩側(cè)激光測(cè)距儀輸出數(shù)據(jù)圖。

如圖6、圖7所示，在導(dǎo)航無(wú)人運(yùn)輸車進(jìn)行轉(zhuǎn)彎操作時(shí)，陀螺儀PITCH軸和ROLL軸輸出信號(hào)趨于平穩(wěn)，即導(dǎo)航無(wú)人運(yùn)輸車轉(zhuǎn)彎操作時(shí)沒有X方向和Y方向的速率變化。如圖8所示，陀螺儀YAW軸輸出信號(hào)中有一段極速變化的數(shù)據(jù)，即此時(shí)導(dǎo)航無(wú)人運(yùn)輸車Z軸旋轉(zhuǎn)角速率快速增加，這與圖9兩側(cè)激光測(cè)距儀輸出距離圖中右側(cè)距離快速增加、左側(cè)距離快速下降的一段相對(duì)應(yīng)。同時(shí)，根據(jù)激光測(cè)距儀左側(cè)距離減小、右側(cè)距離增加，可以判斷此時(shí)為左轉(zhuǎn)操作。

圖6 陀螺儀PITCH軸輸出信號(hào)圖

圖7 陀螺儀ROLL軸輸出信號(hào)圖

圖8 陀螺儀YAW軸輸出信號(hào)圖

圖9 兩側(cè)激光測(cè)距儀輸出距離圖

3.3 導(dǎo)航糾偏效果實(shí)驗(yàn)

本實(shí)驗(yàn)是通過將自主導(dǎo)航時(shí)的最大橫向偏差和最大航向偏差與導(dǎo)航糾偏系統(tǒng)介入時(shí)的最大橫向偏差和最大航向偏差對(duì)比，進(jìn)而可以明顯地看出本導(dǎo)航糾偏系統(tǒng)對(duì)于壟間無(wú)人運(yùn)輸車的糾偏效果。

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

表1 自主導(dǎo)航與導(dǎo)航調(diào)偏介入時(shí)偏差數(shù)據(jù)對(duì)比

4 結(jié)論

課題組首先對(duì)壟間導(dǎo)航無(wú)人車系統(tǒng)進(jìn)行總體框架設(shè)計(jì)，并分析了其硬件結(jié)構(gòu)；其次利用擴(kuò)展卡爾曼濾波融合IMU對(duì)導(dǎo)航無(wú)人進(jìn)行先驗(yàn)定位；然后分析通過陀螺儀和激光測(cè)距儀兩種傳感器數(shù)據(jù)的策略，可以對(duì)此時(shí)的路況信息進(jìn)行準(zhǔn)確的判斷，計(jì)算偏差并介入導(dǎo)航控制系統(tǒng)，最終提高了無(wú)人運(yùn)輸車導(dǎo)航糾偏的準(zhǔn)確性。

實(shí)驗(yàn)表明，應(yīng)用課題組提出的導(dǎo)航糾偏方法，規(guī)劃路線會(huì)與兩側(cè)保持一定安全距離。10次測(cè)試中，最大橫向偏差為38.02 mm，最大航向偏差為16.1°，平均最大橫向偏差為24.88 mm，平均最大航向偏差為9.7°。導(dǎo)航糾偏系統(tǒng)介入，可以綜合調(diào)整距離和方向偏差，保證壟間導(dǎo)航無(wú)人運(yùn)輸車可以在相鄰田壟中線附近行駛，避免壟間導(dǎo)航無(wú)人運(yùn)輸車與兩側(cè)田壟相碰撞，為壟間導(dǎo)航無(wú)人運(yùn)輸車的偏差調(diào)整提供了一種準(zhǔn)確、高效、可靠的控制方法。