激光雷達機器人車輛地頭轉向路徑規劃*

封 永, 張欣欣, 黃林青, 薛金林

激光雷達機器人車輛地頭轉向路徑規劃*

封 永, 張欣欣, 黃林青, 薛金林

(南京農業大學 工學院，江蘇 南京 210031)

對差速轉向機器人車輛進行果園行尾地頭轉向路徑規劃研究，通過車輛運動分析，建立車輛運動學模型；進行U型地頭轉向和K型地頭轉向的路徑生成與分析；采用層次分析法對生成路徑優化選擇，確定行尾地頭轉向路徑；針對U型地頭轉向和K型地頭轉向進行實驗驗證。對于最佳生成路徑對應的實測路徑，U型地頭轉向時方向偏角與橫向偏移均值為15.0°,13.7 cm，K型地頭轉向時方向偏角與橫向偏移均值為13.7°，13.9 cm。

激光雷達傳感器； 機器人車輛； 行尾地頭轉向； 路徑規劃

0 引 言

果園機器人車輛自主導航的主要任務為行引導與行尾地頭轉向，但是大多數研究集中于果樹行內的自主導航，早期有些學者應用電磁導航與機械導航方式實現果園植保機械的自動行走[1,2]。由于在果園中不適宜采用全球定位系統(GPS)導航，目前大多數研究基于機器視覺、激光雷達等技術。馮娟等人利用機器視覺提取樹干與地面交點生成果園行內直線導航基準線[3]；有學者基于激光雷達實現機器人車輛在果樹行間的直線行駛[4～6]；還有學者采用機器視覺或激光雷達進行果園內彎曲路面的路徑檢測[7,8]；而Subramanian Vijay等人基于機器視覺與激光雷達開展柑橘園中直線與曲線路徑的導航研究[9]；國內外一些學者還利用激光雷達進行障礙物檢測，實現果園自主車輛的自動避障[10,11]。

目前，在果園行尾開展地頭轉向路徑規劃的研究較少，但是機器人車輛自主完成路徑規劃并正確地定位以進入下一行是至關重要的。

本文基于激光雷達開展差速轉向機器人車輛行尾地頭轉向路徑規劃研究，這也是對果園車輛自主導航內容的重要補充。

1 機器人車輛運動學模型

圖1為果園行尾地頭轉向研究用的機器人車輛，該機器人車輛可在果園內巡游進行果園信息采集與管理自動化[12～14]。車輛尺寸為1.2 m×0.6 m×0.75 m(長×寬×高)，輪距d=0.55 m，車輪半徑rω=0.19 m。在機器人平臺正前方距離地面0.6 m處安裝LMS291—S05型激光雷達[15]。激光雷達檢測到的果園行尾信息傳輸至上位機(便捷式計算機)；上位機對數據處理分析后計算轉向路徑，然后發送指令至下位機(BasicAtom微控制器)；下位機輸出脈寬調制(PWM)信號至電機控制器以控制兩側驅動電機的角速度，實現機器人車輛的差速轉向。由激光雷達同時獲取果園地頭寬度D大小(如圖2)。上位機根據地頭寬度D大小選擇不同的轉向方式，進行行尾地頭轉向路徑規劃。

圖2 行尾地頭寬度測定示意

建立車輛運動學模型，對其在地頭轉向過程中運動情況作出假設：1)轉向時車速較低，車輛質心在轉向過程中不變，忽略瞬時質心偏移和整車轉動慣量的影響。2)僅考慮輪胎接地段地面變形阻力和轉向阻力的影響，且兩側車輪輪胎的地面變形阻力相同。差速轉向機器人車輛模型如圖3所示，車輛質心C處坐標為(XC,YC)，位姿向量P=(XC,YC,θ)T。根據運動學知識可得其運動學方程為

(1)

圖3 機器人車輛模型

將式(1)進一步整理為

(2)

2 行尾地頭轉向路徑生成與分析

本文以種植行距為1.5m的主干形密植桃園為例開展行尾地頭轉向路徑規劃研究，樹高不超過2m，樹冠半徑在1m以內。通過實驗得知，當地頭寬度D超過1m時，機器人車輛有相對寬闊的空間以U型轉向方式進行地頭轉向；當地頭寬度D小于1m時，地頭空間較窄，機器人車輛通過三點地頭轉向，即K型轉向。本文以地頭寬度1m為界限，對果樹地頭寬度充足(D>1m)情況和地頭寬度不足(D<1m)情況分別進行行尾地頭轉向路徑研究。

通過式(2)可以生成地頭轉向路徑，且生成的路徑純粹與左右兩側車輪角速度有關。因此，機器人車輛采用角速度控制模式，即通過改變車輪角速度ωr，ωl大小控制路徑形狀。圖4為生成的任意地頭轉向路徑。對于差速轉向車輛，理論上可以實現原地轉向，但實際上會出現較大的滑移。為了減少滑移，同時考慮到機器人車輛最大設計車速，對其地頭轉向時的左、右兩輪角速度進行限定，即左右兩輪最小角速度不低于0.5rad/s，最大角速度不高于5rad/s。

圖4 地頭轉向路徑

圖5(a)為地頭寬度充足情況下機器人車輛U型地頭轉向的生成路徑，路徑1、路徑2、路徑3的右側車輪角速度分別為3，4.25，5rad/s，左側車輪角速度分別為1.5，2，2rad/s。

圖5(b)為地頭寬度不足情況下機器人車輛K型地頭轉向的生成路徑。每條路徑都由前進轉向、后退轉向和再前進轉向三個轉向過程組成。路徑4、路徑5、路徑6前進轉向時右側車輪角速度分別為2.5，2.5，2rad/s，左側車輪角速度分別為2，1.55，1.5rad/s；后退轉向時右側車輪角速度分別為1.5，1，1.5rad/s，左側車輪角速度分別為2，1.4，2rad/s；再前進轉向時右側車輪角速度都為2rad/s，左側車輪角速度都為2rad/s。

圖5 U型和K型地頭轉向生成路徑

3 機器人車輛生成路徑評價

3.1 路徑評價方法

本文采用層次分析法(analytichierarchyprocess，AHP)對所生成的路徑進行評價。如圖6所示。與前文對應，在地頭寬度充足情況下，方案層D包含路徑1、路徑2和路徑3；在地頭寬度不足情況下，方案層D包含路徑4、路徑5和路徑6。

各層次因素間的相對重要性分為同等重要、稍微重要、明顯重要、強烈重要、極度重要5等級(對應標度值為1,3,5,7,9)。假設某層次中元素ai與元素aj的相對重要性用ai/aj表示，則有

進行全面規劃，應充分考慮長遠利益與當前利益，積極推進省內重點水利工程建設，適時推動鄱陽湖水利樞紐工程建設；持續推進大型灌區續建配套工程，加強建設和改造中小型灌區，加快排灌泵站更新改造，不斷完善灌溉排澇系統；[2，3]大力開展縣級以上城市防洪排澇體系建設，以及山洪災害防治、中小河流治理和鄱陽湖蓄滯洪區安全建設，從而提高設防中心城市防洪標準。

(3)

為了防止層次分析法分析問題產生偏差，需要進行一致性檢驗，用一致性比例C.R.表示，即

C.R.=C.I./R.I.

(4)

式中 C.I.=(λmax-n)/(n-1)為一致性指標，其中n為判斷矩陣的階數，λmax為矩陣的最大特征值；R.I.為平均隨機一致性指標，由查表確定，對于5階判斷矩陣，R.I.=1.12。

當C.R.<0.1時，認為判斷矩陣的一致性是可以接受的;否則,應該修改矩陣使之符合一致性要求。

圖6 轉向路徑層次結構模型圖

3.2 行尾地頭轉向路徑評價與選擇

3.2.1U型地頭轉向路徑評價與選擇

構造準則層C相對目標層A的判斷矩陣

通過計算判斷矩陣的特征值λmax=4.174和特征向量W=[0.567 0.05 0.151 0.232]，判斷矩陣一致性比例C.R.=0.066<0.1，說明判斷矩陣的一致性可以接受。

構造方案層D各元素相當于準則層C各元素的判斷矩陣

方案層D相對于目標層A的權重是通過準則層C相對于目標層A和方案層D相對于準則層C的權重組合而得到，計算結果如表1。路徑1,2,3的總權重向量W=[0.138 0.720 0.142]T，即生成路徑2為最優路徑。總排序一致性比例C.R.為

(5)

式中C.I.i為準則層Ci相應的方案層D中判斷矩陣的一致性指標；R.I.i為準則層Ci相應的方案層D中判斷矩陣的平均隨機一致性指標；ci是準則層C相對目標層A的權重值。層次總排序一致性比例C.R.=0.018<0.1，說明層次總排序一致性可以接受。

表1 轉向路徑1,2,3總權重

方案C1C2C3C40．5670．0500．1510．232總權重D10．1190．6490．1130．0910．138D20．7470．2790．7090．7580．720D30．1340．0720．1780．1510．142

3.2.2 K型地頭轉向路徑評價與選擇

構造準則層C相對目標層A的判斷矩陣

通過計算判斷矩陣的特征值λmax=4.258和特征向量W=[0.140 0.052 0.250 0.558]，判斷矩陣一致性比例C.R.=0.096<0.1，說明判斷矩陣的一致性可以接受。

構造方案層D各元素相當于準則層C各元素的判斷矩陣

同理，計算出方案層D相對于目標層A的權重，如表2。路徑4,5,6的總權重向量W=[0.147 0.727 0.126]T，即生成路徑5為最優路徑。經計算層次總排序一致性可以接受(C.R.=0.018<0.1)。

表2 轉向路徑4,5,6總權重

方案C1C2C3C40．1400．0520．2500．558總權重D10．2580．2790．1190．1190．147D20．6370．6490．7470．7470．727D30．1050．0720．1340．1340．126

4 實驗與結果分析

實驗場地是江浦農場的密植桃園，每次機器人車輛在行中定點出發，初始轉向角為0°±1°，初始偏移量為(0±1)cm。在地頭寬度D=1.4 m對路徑1,2,3進行實驗；在地頭寬度D=0.8 m對路徑4,5,6進行實驗。每條路徑實驗3次，分別記錄左右輪角速度、轉向時間、轉向次數、最終位置(位置與方向角),實驗數據如表3。

表3 地頭轉向路徑實驗數據

路徑實驗次數右側車輪角速度/(rad/s)左側車輪角速度/(rad/s)轉向時間/s轉向次數/次最終位置(與行中心線的夾角)/(°)最終位置(與行中心線的距離)/cm1131．57．6213035．21231．57．5712326．31331．57．5512022．4214．2525．1111512．7224．2524．9911210．3234．2525．6111818．231523．8213539．832523．7613336．633523．5013033．1412．5，-1．5，22，-2，213．003-30-36．2422．5，-1．5，22，-2，212．523-25-28．8432．5，-1．5，22，-2，212．983-26-30．0512．5，-1，21．55，-1．4，29．3531515．6522．5，-1，21．55，-1．4，29．3231211．6532．5，-1，21．55，-1．4，29．3531414．4612，-1．5，21．5，-2，212．5532528．8622，-1．5，21．5，-2，212．3132225．0632，-1．5，21．5，-2，212．4532326．3

注：車輛縱向中心線偏向果樹行中心線左側為正，反之為負；車頭偏向左為正，反之為負；角速度值為負表示車輛倒退行駛。

根據表3，在3次實驗中機器人車輛都能夠在地頭寬度充足情況下以U型轉向生成轉向路徑1，2,3，及在地頭寬度不足情況下以K型轉向生成轉向路徑4,5,6，很好地完成果園行尾地頭轉向操作，盡管有不同程度的方向偏角與橫向偏移，這主要是因為轉向中存在微小的滑移。在U型轉向時路徑2的方向偏角與橫向偏移均值都最小，分別為15.0°與13.7 cm；在K型轉向時路徑5的偏角與偏移均值都最小，分別為13.7°與13.9 cm。根據上位機對各層次因素相對重要性的分析結果，在U型轉向中路徑2的總權重最大，為0.720；在K型轉向中路徑5的總權重最大，為0.727。

5 結 論

針對差速轉向機器人車輛開展了果園行尾地頭轉向路徑規劃研究，分析了在行尾地頭寬度充足與不足情況下的行尾地頭轉向路徑，并通過層次分析法對生成路徑進行優化選擇。對果樹行尾地頭寬度充足或不足的情況分別進行實驗驗證。結果表明:盡管有一定的方向偏角與橫向偏移，但是機器人車輛能很好地完成果園行尾的地頭轉向。

[1] Torii T.Research in autonomous agriculture vehicles in Japan[J].Computers and Electronics in Agriculture,2000,25(1-2):133-153．

[2] Gabriely Y,Rimon E.C-space characterization of contact preserving paths with application to tactile-sensor based mobile robot navigation[C]∥2008 IEEE International Conference on Robotics and Automation,Pasadena,CA:IEEE,2008:1792-1797.

[3] 馮 娟,劉 剛,司永勝,等.果園視覺導航基準線生成算法[J].農業機械學報,2012,43(7):185-189,184.

[4] 劉 沛,陳 軍,張明穎.基于激光導航的果園拖拉機自動控制系統[J].農業工程學報,2011,27(3):196-199.

[5] 薛金林,張順順.基于激光雷達的農業機器人導航控制研究[J].農業機械學報,2014,45(9):55-60.

[6] Barawid O C,Mizushima A,Ishii K,et al.Development of an autonomous navigation system using a two-dimensional laser scanner in an orchard application[J].Biosystems Engineering,2007,96(2):139-149.

[7] 林桂潮,鄒湘軍,羅陸鋒,等.改進隨機樣本一致性算法的彎曲果園道路檢測[J].農業工程學報,2015,31(4):168-174.

[8] 陳 軍,蔣浩然,劉 沛,等.果園移動機器人曲線路徑導航控制[J].農業機械學報,2012,43(4):179-182,187.

[9] Subramanian Vijay,Burks F T,Arroyo A A.Development of machine vision and laser radar based autonomous vehicle guidance systems for citrus grove navigation[J].Computers and Electronics in Agriculture,2006,53:130-143.

[10] 郝永軍.果園打藥機自動行走控制系統研究[D].保定:河北農業大學,2013.

[11] Freitas G,Hamner B,Bergerman M,et al.A practical obstacle detection system for autonomous orchard vehicles[C]∥IEEE/RSJ International Conference on Intelligent Robots and Systems,Algarve,Portugal,2012:3391-3398.

[12] 朱磊磊.果園管理機器人平臺的自動導航系統研究[D].西安:西北農林科技大學,2010.

[13] 王海波,劉鳳之,王孝娣,等.我國果園機械研發與應用概述[J].果樹學報,2013,30(1):165-170.

[14] 董希玲.蘋果果園管理信息系統的構建[D].保定:河北農業大學,2010.

[15] 王 斌,劉昭度,何 瑋,等.車用測距雷達研究進展[J].傳感器與微系統,2006,25(3):7-9.

Path planning of end-of-row turning for robot vehicle based on laser radar*

FENG Yong, ZHANG Xin-xin, HUANG Lin-qing, XUE Jin-lin

(College of Engineering,Nanjing Agricultural University,Nanjing 210031,China)

Path planning of end-of-row turning for robot vehicle with differential steering in orchard is researched.Kinematics model is established in light of an analysis on movement for robot vehicle.U-turn and K-turn paths are generated and analyzed according to model.Analytic hierarchy process(AHP)is adopted to select generated paths, thus to choose the optimal path for end-of-row turning.Experiments are carried out to verify in the case with U-turn and K-turn.For the measured paths corresponding to the optimal generated paths,mean value of direction angle and lateral shift are 15.0°,13.7 cm for U-turn and 13.7°,13.9 cm for K-turn.

laser radar sensor; robot vehicle; end-of-row turning; path planning

10.13873/J.1000—9787(2017)02—0028—04

2016—02—24

中央高校基本科研業務費專項資金資助項目(KYZ2011)； 江蘇省自然科學基金資助項目(BK20151436)

TP 242

A

1000—9787(2017)02—0028—04

封 永(1987-)，男，碩士研究生，主要研究方向為車輛自主轉向智能化。

薛金林(1974-)，男，通訊作者，教授，主要從事農業智能化研究工作，E-mail:xuejinlin@njau.edu.cn。