畢偉平，張　歡，瞿振林，丁永前*，余洪鋒，王　波

(1.南京農(nóng)業(yè)大學(xué)工學(xué)院，江蘇 南京 210031；2.江蘇省現(xiàn)代設(shè)施農(nóng)業(yè)技術(shù)與裝備工程實驗室，江蘇 南京 210031)

?

基于雙目視覺的主從式果園作業(yè)車輛自主跟隨系統(tǒng)設(shè)計

畢偉平1,2，張歡1,2，瞿振林1,2，丁永前1,2*，余洪鋒1,2，王波1,2

(1.南京農(nóng)業(yè)大學(xué)工學(xué)院，江蘇 南京 210031；2.江蘇省現(xiàn)代設(shè)施農(nóng)業(yè)技術(shù)與裝備工程實驗室，江蘇 南京 210031)

摘 要：將黑白棋盤格特征板固定于引導(dǎo)車輛作為跟隨特征，通過固定于跟隨車輛的雙目視覺系統(tǒng)，獲取黑白棋盤特征板上各角點的三維信息，分析該信息，得到引導(dǎo)車輛相對于跟隨車輛的車輛間距、橫向偏移及航向偏角，利用該導(dǎo)航信息實現(xiàn)跟隨車輛的自動跟隨，建立了主從式果園作業(yè)車輛的跟隨系統(tǒng)。雙目視覺系統(tǒng)在不同光強下靜態(tài)性能測試結(jié)果表明，雙目系統(tǒng)生成導(dǎo)航參數(shù)偏差與日間光強無明顯相關(guān)性，體現(xiàn)出雙目系統(tǒng)對外界光環(huán)境變化有良好的適應(yīng)性。在果園環(huán)境下，對該系統(tǒng)進行性能測試，結(jié)果系統(tǒng)導(dǎo)航信息生成頻率為4.0 Hz，橫向偏移平均偏差為3.68 cm，車輛間距平均偏差為8.24 cm，航向偏角平均偏差為2.32°，表明所構(gòu)建的跟隨系統(tǒng)能實現(xiàn)對引導(dǎo)車輛的自動實時跟隨。

關(guān) 鍵 詞：主從式果園車輛；雙目視覺；自動導(dǎo)航

投稿網(wǎng)址：http://xb.ijournal.cn

目前，農(nóng)業(yè)車輛自主導(dǎo)航研究主要集中在單個車輛在農(nóng)田環(huán)境中的自動導(dǎo)航[1–4]，但在很多情況下農(nóng)業(yè)車輛作業(yè)需要2輛及以上車輛協(xié)同操作才能完成[5]，如在果園收獲過程中，農(nóng)業(yè)車輛作業(yè)采用主從式跟隨方式，果實采摘車(引導(dǎo)車)與果實運輸車(跟隨車)之間跟隨操作[6–8]。日本京都大學(xué)農(nóng)業(yè)機械化研究生院研究了水稻收割過程中的協(xié)同車輛系統(tǒng)[9]，采用超聲波模塊測量車輛相對位置，方法較為簡單，但超聲波模塊易受周圍障礙物干擾，不具有分辨干擾物與目標車輛的能力；北海道大學(xué)農(nóng)業(yè)車輛系統(tǒng)工程研究生院研究了在農(nóng)田作業(yè)的主從式機器人系統(tǒng)[10]，其主要研究了跟隨車輛跟隨算法及到達特定位置的運行算法，但其跟隨距離最小為12.5 m，近距離跟隨時效果未知；上海交通大學(xué)智能車實驗室實現(xiàn)了基于GPS的城市環(huán)境多智能車跟隨協(xié)作系統(tǒng)[11]，主要針對城市道路環(huán)境下多車運行，無主從之分，當車輛間距離較遠時各自運行，距離較近時車輛相互配合實現(xiàn)換道超車等操作。當前主從式車輛跟隨研究中，主要采用紅外測距、高精度GPS及激光掃描等方式[12–14]，成本高且易受環(huán)境影響。

用機器視覺方式實現(xiàn)主從式車輛跟隨鮮有研究，因為不同車輛其特征不同且提取算法復(fù)雜，單純通過識別車輛特征也較難判斷其實際位置。黑白棋盤格作為一種標準的視覺系統(tǒng)參數(shù)標定對象[15–16]，其角點提取算法成熟，易于實現(xiàn)。筆者改裝了2輛小型電動車輛使其構(gòu)成主從車輛跟隨系統(tǒng)，采用黑白棋盤格板固定于引導(dǎo)車輛后方，作為其特征標識物，同時利用固定于跟隨車輛前方的雙目視覺系統(tǒng)，依據(jù)雙目立體視覺原理[17–19]提取特征板各角點三維信息，分析獲取2車之間的相對位姿信息，指導(dǎo)跟隨車輛的運行。在實際果園環(huán)境中，對建立的主從車輛跟隨系統(tǒng)進行了驗證試驗，現(xiàn)將結(jié)果報道如下。

1 跟隨車輛系統(tǒng)的構(gòu)建

主從式果園車輛如圖1所示。跟隨車輛系統(tǒng)包括雙目視覺系統(tǒng)與車輛運動平臺。雙目視覺系統(tǒng)采用雙目相機與PC連接的方式構(gòu)成，車輛運動平臺在改裝電動車基礎(chǔ)上通過基于CAN總線的控制節(jié)點實現(xiàn)與雙目視覺系統(tǒng)通信及車輛控制。

圖1　主從式果園車輛Fig. 1 Master-slave vehicles in orchard

1.1 雙目視覺系統(tǒng)

雙目攝像頭采用維視數(shù)字圖像技術(shù)有限公司的 MV–VS030FC型工業(yè)數(shù)字相機。采用幀曝光CCD作為傳感器，圖像質(zhì)量較高，顏色還原性較好。CCD相機最大分辨率640×480。該相機采用 IEEE 1394 作為輸出接口。在安裝有 1394采集卡的 PC平臺同時連接2臺該工業(yè)相機，2臺工業(yè)相機平行固定于相機支架上，其成像中心間距離為90 mm，構(gòu)成雙目視覺系統(tǒng)。

1.2 車輛跟隨系統(tǒng)

車輛控制系統(tǒng)采用易于擴展的CAN總線結(jié)構(gòu)，各CAN總線節(jié)點以STM32F102VET6為核心；主節(jié)點通過串口與PC端雙目視覺導(dǎo)航系統(tǒng)傳輸導(dǎo)航信息，各個從節(jié)點通過編碼器實現(xiàn)測量前輪轉(zhuǎn)角和車輛前進速度、通過控制步進電機配合鏈輪傳動機構(gòu)帶動方向盤旋轉(zhuǎn)實現(xiàn)車輛轉(zhuǎn)向、通過控制后輪差速電機實現(xiàn)車輛驅(qū)動和差速轉(zhuǎn)彎。

1.3 車輛控制方法

果園路面狀況較為復(fù)雜，因此跟隨車輛在雙目視覺系統(tǒng)獲取車輛的位姿參數(shù)后，需要經(jīng)過相關(guān)控制算法才能實現(xiàn)穩(wěn)定運行。轉(zhuǎn)向控制系統(tǒng)根據(jù)航向偏角θ和橫向偏差f 與目標控制期望值之間的偏差,通過航向控制器輸出控制轉(zhuǎn)向電機的PWM脈沖，從而控制車輛轉(zhuǎn)向輪的偏轉(zhuǎn)[20]。車速控制系統(tǒng)根據(jù)車輛間距d與目標控制期望偏差通過速度控制器輸出相關(guān)模擬電壓從而控制車輛前進差速電機，實現(xiàn)車輛行駛速度的調(diào)整，以保持設(shè)定的車輛間距[21]。2個控制系統(tǒng)同時作用，實現(xiàn)車輛跟隨操作。車輛跟蹤控制系統(tǒng)如圖2所示。

圖2　跟隨控制系統(tǒng)Fig. 2 Diagram of follow-control system

2 車輛跟隨方法

2.1 車輛運動學(xué)模型

跟隨車輛運動學(xué)分析如圖3(左)所示，跟隨車輛與引導(dǎo)車輛相對位置如圖3(右)所示。當車輛前后跟隨時，理想情況為保持設(shè)定的跟隨距離的同時，2車中軸線相互重合，因此，當引導(dǎo)車輛運行狀態(tài)變化或運行環(huán)境受到影響時，2車間距d(m)發(fā)生變化，同時產(chǎn)生距離為f(m)的橫向偏移及角度為θ(°)的航向偏角，跟隨車輛控制系統(tǒng)需要通過控制車輛前進速度及前輪偏轉(zhuǎn)角度，使2車間距趨于目標間距、橫向偏移趨于0和相對航向偏角趨向于0，從而實現(xiàn)跟隨。圖中2車輪中間淺色陰影區(qū)域為2車輪的等效模型，O為跟隨車轉(zhuǎn)向運行時的速度瞬心。

圖3　主從式果園作業(yè)車輛跟隨系統(tǒng)運動學(xué)分析Fig.3 Kinematical analysis of autonomous following system for master-slave vehicles in orchard

根據(jù)運動學(xué)理論可以得出以下計算關(guān)系：

式中：L為跟隨車輛前后車輪輪軸間距離(m)；θ 為引導(dǎo)車和跟隨車相對航向偏轉(zhuǎn)角(°)；δ為當前跟隨車輛前輪的偏轉(zhuǎn)角度(rad)；vω為當前跟隨車輛的轉(zhuǎn)向運行時的角速度(rad/s)；v 為當前跟隨車輛的前進方向速度(m/s)；u為當前跟隨車輛的轉(zhuǎn)向時橫向速度(m/s)；d為引導(dǎo)車與跟隨車間距(m)；f為引導(dǎo)車與跟隨車相對橫向偏移(m)。

2.2 車輛導(dǎo)航參數(shù)確定方法

主從車輛跟隨系統(tǒng)的主要特點是黑白棋盤格板固定于引導(dǎo)車輛作為車輛特征，運用黑白棋盤格板內(nèi)角點提取算法及雙目視覺原理，可以得到黑白棋盤格各個內(nèi)角點在雙目系統(tǒng)坐標系下的三維信息，而雙目系統(tǒng)固定于跟隨車輛，因此其角點三維信息代表了引導(dǎo)車輛與跟隨車輛之間的相對位姿，通過建立整個系統(tǒng)坐標系分析各個角點三維信息，可以得到2車間距、橫向偏移及航向偏角。

如圖4–a所示，建立主從系統(tǒng)的坐標系，以雙目系統(tǒng)左相機圖像中心O為坐標原點，橫向偏移方向標記為OX，2車間距方向即深度方向標記為OZ，垂直于XOZ平面的軸記為OY，則空間點坐標可表示為(x,y,z)。

黑白棋盤格被固定在引導(dǎo)車車尾并垂直于地面，2車同向且無橫向偏差行駛時，OZ軸與黑白棋盤格平面垂直，其交點與黑白棋盤格中心點A連線垂直于地面。引導(dǎo)車和跟隨車的相對位置關(guān)系可以由黑白棋盤格角點的三維信息確定。如圖4–b所示，記A點為黑白棋盤格的中心點，其在OX和OZ方向的坐標記為(c，d)，則c、d分別為引導(dǎo)車輛與跟隨車輛的橫向偏移與車輛間距。設(shè)黑白棋盤格角點在OX和OZ方向的三維坐標為(xi，zj)，角點總數(shù)為n，則車輛間距及橫向偏移可以表示為：

圖4　引導(dǎo)車與跟隨車位置Fig. 4 The position between two vehicles

圖4–b中，BC為黑白棋盤特征板載XOZ平面的投影位置，假設(shè)點B、C為黑白棋盤格上處于同一水平線的2個特征點，B、C 2點在坐標系下的橫向距離為b，深度距離為a，航向偏角θ可由a、b的三角關(guān)系得到，若 B、C2點坐標分別為(x1，z1) 和(x2，z2)，則a、b 可表示為：

于是可得：

至此，2車間距、橫向偏移、航向偏角3個導(dǎo)航參數(shù)均得到。

3 試驗驗證及結(jié)果分析

3.1 靜態(tài)精度試驗

為驗證系統(tǒng)得到導(dǎo)航參數(shù)的準確性，設(shè)計了靜態(tài)導(dǎo)航參數(shù)的生成試驗。制作內(nèi)角點為4×4、單個方格邊長為10 cm的黑白棋盤格特征板，固定在可隨垂直于地面軸360°旋轉(zhuǎn)的工作臺上，通過移動工作臺，雙目視覺系統(tǒng)獲取導(dǎo)航參數(shù)。

縱向移動：以10 cm為間隔縱向移動工作臺，移動范圍為1.5～2.0 m。

橫向移動：在1.5～2.0 m各間距點上，以10 cm為間隔橫向移動工作臺，移動范圍為–500～500 mm。

航向旋轉(zhuǎn)：在 1.5～2.0 m各間距點上，以 10°為步進，分別按順時針和逆時針方向旋轉(zhuǎn)工作臺，旋轉(zhuǎn)范圍為–60°～60°。

在以上試驗條件下，通過雙目系統(tǒng)測量相應(yīng)間距、橫向偏移及航向偏角，每個位置測量持續(xù)時間為3 min。通過觀察測量結(jié)果的波動情況，評價系統(tǒng)穩(wěn)定性，通過分析雙目系統(tǒng)與實際固定間距、橫向偏移及偏角偏差，評價系統(tǒng)準確性。

靜態(tài)精度試驗結(jié)果列于表 1。結(jié)果表明，在固定測量位置時，本系統(tǒng)實時生成的位置信息與實際位置偏差較小，說明系統(tǒng)具有較高的準確性，同時系統(tǒng)輸出參數(shù)波動率較低，說明系統(tǒng)具有較高的穩(wěn)定性。

表1　靜態(tài)試驗結(jié)果Table 1 Static test results

3.2 光環(huán)境適應(yīng)性試驗

為驗證系統(tǒng)的光環(huán)境適應(yīng)性，白天在室外進行了不同時段自然光下的導(dǎo)航參數(shù)生成的靜態(tài)試驗。設(shè)置車輛間距為1.6 m，橫向偏移為0 cm，航向偏角0°，在 08:00、12:00、17:00，分別通過雙目系統(tǒng)測量3個導(dǎo)航參數(shù)，通過計算不同光照條件下雙目系統(tǒng)獲得導(dǎo)航參數(shù)與實際設(shè)置參數(shù)的偏差，分析雙目系統(tǒng)對光環(huán)境的適應(yīng)性。光照度裝置采用?，敻呔葦?shù)字光照度計AS823，測量范圍為1～200 000 lx，天氣晴朗，導(dǎo)航參數(shù)平均偏差見表2。

表2　不同光環(huán)境下的導(dǎo)航參數(shù)Table 2 The experimental results under different light conditions

在不同光照度下，雙目視覺系統(tǒng)生成的車輛間距、橫向偏移與航向偏角與實際設(shè)置參數(shù)的平均偏差較小，表明雙目視覺系統(tǒng)對不同光環(huán)境有較好的適應(yīng)性。

由于黑白棋盤格板制作采用普通KT板，表面為塑料覆膜，在陽光強烈及特定角度下會產(chǎn)生一定反光，因此棋盤格需要注意采用磨砂或紙面材料。

3.3 果園跟隨導(dǎo)航試驗

于2015年11月下旬，在南京農(nóng)業(yè)大學(xué)江浦農(nóng)場果園路徑中進行了果園導(dǎo)航試驗。路徑長25 m，寬2.6 m，引導(dǎo)車采用人工控制，沿果園路徑中心線運行，目標運行速度為2 km/h。跟隨車輛自動跟隨引導(dǎo)車，目標跟隨距離為1.6 m，目標橫向偏移0 cm，目標航向偏角為0(°)。各導(dǎo)航參數(shù)變化曲線如圖5所示。

圖5　實際試驗車輛導(dǎo)航參數(shù)Fig. 5 Navigation parameters of the actual test

導(dǎo)航參數(shù)穩(wěn)定性分析如表3。果園環(huán)境下實時導(dǎo)航運行過程中，系統(tǒng)輸出性能穩(wěn)定，偏差較小，說明在整個過程中跟隨車輛能夠穩(wěn)定跟隨引導(dǎo)車輛。

表3　導(dǎo)航參數(shù)試驗結(jié)果Table 3 The test results for navigation parameters

參考文獻：

[1] Reid J F，Zhang Q，Noguchi N，et al．Agricultural automatic guidance research in North America[J]. Computers and Electronics in Agriculture，2000，25 (1/2)：155–167．

[2] Keicher R，Seufer T H．Automatic guidance for agricultural vehicles in Europe[J]．Computers and Electronics in Agriculture，2000，25(1/2)：169–194．

[3] Torii T．Research in autonomous agriculture vehicles in Japan[J]．Computers and Electronics in Agriculture，2000，25(1/2)：133–153．

[4] Li M，Imou K，Wakabayayashi K，et al．Review of research on agricultural vehicle autonomous guidance[J]. Int J Agric and Biol Eng，2009，2(3)：1–16．

[5] Stavros G V．A distributed control framework for motion coordination of teams of autonomous agricultural vehicles[J]．Biosys Tems Engineering，2012，113(3)：284–297．

[6] Goragl A，Amatyas S，Karkee M．Sensors and systems for fruit detection and localization：a review[J]. Computers and Electronics in Agriculture，2015，116：8–19．

[7] Thomas R，Marco K．Robotic fruit harvesting：a stateof-the-art review[J]．Journal of Agricultural Engineering Research，1993，54(3)：265–280．

[8] Barawid O C，Akira M，Kazunobu I，et al．Development of an autonomous navigation system using a twodimensional laser scanner in an orchard application[J]. Biosystems Engineering，2007，96(2)：139–149．

[9] Iida M，Kudou M，Ono K，et al．Automatic following control for Agricutural vehicle[C]//6th International Workshop on Advanced Motion Control，Proceedings. Nagoya：AMC，2000：158–162．

[10] Noboru N，Jeff W．Development of a master-slave robot system for farm operations[J]．Computers and Electronics in Agriculture，2004，44(1)：1–19．

[11] 彭新榮．基于智能小車平臺的多車協(xié)作研究[D]．上海：上海交通大學(xué)，2010．

[12] 沈文龍，薛金林，張永．農(nóng)業(yè)車輛電控液壓轉(zhuǎn)向系統(tǒng)的設(shè)計[J]．湖南農(nóng)業(yè)大學(xué)學(xué)報(自然科學(xué)版)，2014，40(3)：325–329．

[13] 周俊，姬長英．自主車輛導(dǎo)航系統(tǒng)中的多傳感器融合技術(shù)[J]．農(nóng)業(yè)機械學(xué)報，2002，33(5)：113–116．

[14] 陳無畏，施文武，王啟瑞，等．新型自動引導(dǎo)車導(dǎo)航與控制系統(tǒng)[J]．農(nóng)業(yè)機械學(xué)報，2002，33(2)：70–73．

[15] 馮煥飛．三維重建中的相機標定方法研究[D]．重慶：重慶交通大學(xué)，2013．

[16] 舒娜．攝像機標定方法的研究[D]．南京：南京理工大學(xué)，2014．

[17] Rovira M F，Reid J F，Zhang Q．Stereovision data processing with 3D density maps for agricultural vehicles[J]. Transactions of the ASABE，2006，49(4)：1213–1222．

[18] Kise M，Zhang Q，Rovira F．A stereovision-based crop row detection method for tractor-automated guidance [J]. Biosystems Engineering，2005，90(4)：357–367．

[19] Rovira M F．Applications of stereoscopic vision to agriculture[D]．Urbana：University of Illinois at Urbana-Champaign，2003．

[20] 丁永前，王致情，林相澤，等．自主跟隨車輛航向控制系統(tǒng)[J]．農(nóng)業(yè)機械學(xué)報，2015，46(1)：8–13．

[21] 王致情．農(nóng)業(yè)車輛自主跟隨控制系統(tǒng)的設(shè)計與實現(xiàn)[D]．南京：南京農(nóng)業(yè)大學(xué)，2014．

責任編輯：羅慧敏

英文編輯：吳志立

Design of autonomous following system for master-slave vehicles operating in orchard based on binocular stereo vision

Bi Weiping1,2, Zhang Huan1,2, Qu Zhenlin1,2, Ding Yongqian1,2*, Yu Hongfeng1,2, Wang Bo1,2
(1.College of Engineering, Nanjing Agricultural University, Nanjing 210031, China; 2.Jiangsu Province Engineering Laboratory for Modern Facility Agriculture Technology & Equipment, Nanjing 210031, China)

Abstract:A checkerboard plate was fixed on the back part of the master-vehicle as a guidance feature for the slave-vehicle. The three-dimension(3D) information of each corner point on the checkerboard plate can be easily obtained by the stereo vision system which was fixed on the front of the slave-vehicle. Through the analysis of the 3D information of each corner point, the distance, the relative lateral offset and heading angle can be obtained between the two vehicles. These information can be used to guide the slave-vehicle to follow the master-vehicle autonomously. According to this method, a master-slave following system was built for vehicles operating in orchard. The test results of stereo vision system performance in different light environment showed that the navigation parameter deviation is not associated with the light intensity during the day, so the stereo vision system have good adaptability to the natural light environment. The performance of the master-slave vehicles system was tested in an orchard. The update rate of the navigation information was 4.0 Hz. During the operation, the average deviation of lateral offset, the distance, heading angle was 3.68 cm, 8.24 cm and 2.32° respectively. The experimental results showed that the slave vehicle can follow the master-vehicle in real time successfully.

Keywords:master-slave vehicles in orchard; binocular stereovision; autonomous navigation

中圖分類號：S225.93

文獻標志碼：A

文章編號：1007?1032(2016)03?0344?05

收稿日期：2016–01–15 修回日期：2016–03–16

基金項目：國家“十二·五”科技支撐計劃項目(2013BAD08B04–1)；江蘇省農(nóng)機局–南京農(nóng)業(yè)大學(xué)科研啟動基金項目(6KH–0603)

作者簡介：畢偉平(1989—)，男，山西太原人，碩士研究生，主要從事車輛導(dǎo)航與信息融合技術(shù)研究，bwp2012@163.com；*通信作者，丁永前，博士，副教授，主要從事農(nóng)業(yè)信息和自動化研究，yongqiand@njau.edu.cn