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

激光雷達(dá)機(jī)器人車(chē)輛地頭轉(zhuǎn)向路徑規(guī)劃*

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

(南京農(nóng)業(yè)大學(xué) 工學(xué)院，江蘇 南京 210031)

對(duì)差速轉(zhuǎn)向機(jī)器人車(chē)輛進(jìn)行果園行尾地頭轉(zhuǎn)向路徑規(guī)劃研究，通過(guò)車(chē)輛運(yùn)動(dòng)分析，建立車(chē)輛運(yùn)動(dòng)學(xué)模型；進(jìn)行U型地頭轉(zhuǎn)向和K型地頭轉(zhuǎn)向的路徑生成與分析；采用層次分析法對(duì)生成路徑優(yōu)化選擇，確定行尾地頭轉(zhuǎn)向路徑；針對(duì)U型地頭轉(zhuǎn)向和K型地頭轉(zhuǎn)向進(jìn)行實(shí)驗(yàn)驗(yàn)證。對(duì)于最佳生成路徑對(duì)應(yīng)的實(shí)測(cè)路徑，U型地頭轉(zhuǎn)向時(shí)方向偏角與橫向偏移均值為15.0°,13.7 cm，K型地頭轉(zhuǎn)向時(shí)方向偏角與橫向偏移均值為13.7°，13.9 cm。

激光雷達(dá)傳感器； 機(jī)器人車(chē)輛； 行尾地頭轉(zhuǎn)向； 路徑規(guī)劃

0 引 言

果園機(jī)器人車(chē)輛自主導(dǎo)航的主要任務(wù)為行引導(dǎo)與行尾地頭轉(zhuǎn)向，但是大多數(shù)研究集中于果樹(shù)行內(nèi)的自主導(dǎo)航，早期有些學(xué)者應(yīng)用電磁導(dǎo)航與機(jī)械導(dǎo)航方式實(shí)現(xiàn)果園植保機(jī)械的自動(dòng)行走[1,2]。由于在果園中不適宜采用全球定位系統(tǒng)(GPS)導(dǎo)航，目前大多數(shù)研究基于機(jī)器視覺(jué)、激光雷達(dá)等技術(shù)。馮娟等人利用機(jī)器視覺(jué)提取樹(shù)干與地面交點(diǎn)生成果園行內(nèi)直線(xiàn)導(dǎo)航基準(zhǔn)線(xiàn)[3]；有學(xué)者基于激光雷達(dá)實(shí)現(xiàn)機(jī)器人車(chē)輛在果樹(shù)行間的直線(xiàn)行駛[4～6]；還有學(xué)者采用機(jī)器視覺(jué)或激光雷達(dá)進(jìn)行果園內(nèi)彎曲路面的路徑檢測(cè)[7,8]；而Subramanian Vijay等人基于機(jī)器視覺(jué)與激光雷達(dá)開(kāi)展柑橘園中直線(xiàn)與曲線(xiàn)路徑的導(dǎo)航研究[9]；國(guó)內(nèi)外一些學(xué)者還利用激光雷達(dá)進(jìn)行障礙物檢測(cè)，實(shí)現(xiàn)果園自主車(chē)輛的自動(dòng)避障[10,11]。

目前，在果園行尾開(kāi)展地頭轉(zhuǎn)向路徑規(guī)劃的研究較少，但是機(jī)器人車(chē)輛自主完成路徑規(guī)劃并正確地定位以進(jìn)入下一行是至關(guān)重要的。

本文基于激光雷達(dá)開(kāi)展差速轉(zhuǎn)向機(jī)器人車(chē)輛行尾地頭轉(zhuǎn)向路徑規(guī)劃研究，這也是對(duì)果園車(chē)輛自主導(dǎo)航內(nèi)容的重要補(bǔ)充。

1 機(jī)器人車(chē)輛運(yùn)動(dòng)學(xué)模型

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

圖2 行尾地頭寬度測(cè)定示意

建立車(chē)輛運(yùn)動(dòng)學(xué)模型，對(duì)其在地頭轉(zhuǎn)向過(guò)程中運(yùn)動(dòng)情況作出假設(shè)：1)轉(zhuǎn)向時(shí)車(chē)速較低，車(chē)輛質(zhì)心在轉(zhuǎn)向過(guò)程中不變，忽略瞬時(shí)質(zhì)心偏移和整車(chē)轉(zhuǎn)動(dòng)慣量的影響。2)僅考慮輪胎接地段地面變形阻力和轉(zhuǎn)向阻力的影響，且兩側(cè)車(chē)輪輪胎的地面變形阻力相同。差速轉(zhuǎn)向機(jī)器人車(chē)輛模型如圖3所示，車(chē)輛質(zhì)心C處坐標(biāo)為(XC,YC)，位姿向量P=(XC,YC,θ)T。根據(jù)運(yùn)動(dòng)學(xué)知識(shí)可得其運(yùn)動(dòng)學(xué)方程為

(1)

圖3 機(jī)器人車(chē)輛模型

將式(1)進(jìn)一步整理為

(2)

2 行尾地頭轉(zhuǎn)向路徑生成與分析

本文以種植行距為1.5m的主干形密植桃園為例開(kāi)展行尾地頭轉(zhuǎn)向路徑規(guī)劃研究，樹(shù)高不超過(guò)2m，樹(shù)冠半徑在1m以?xún)?nèi)。通過(guò)實(shí)驗(yàn)得知，當(dāng)?shù)仡^寬度D超過(guò)1m時(shí)，機(jī)器人車(chē)輛有相對(duì)寬闊的空間以U型轉(zhuǎn)向方式進(jìn)行地頭轉(zhuǎn)向；當(dāng)?shù)仡^寬度D小于1m時(shí)，地頭空間較窄，機(jī)器人車(chē)輛通過(guò)三點(diǎn)地頭轉(zhuǎn)向，即K型轉(zhuǎn)向。本文以地頭寬度1m為界限，對(duì)果樹(shù)地頭寬度充足(D>1m)情況和地頭寬度不足(D<1m)情況分別進(jìn)行行尾地頭轉(zhuǎn)向路徑研究。

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

圖4 地頭轉(zhuǎn)向路徑

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

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

圖5 U型和K型地頭轉(zhuǎn)向生成路徑

3 機(jī)器人車(chē)輛生成路徑評(píng)價(jià)

3.1 路徑評(píng)價(jià)方法

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

各層次因素間的相對(duì)重要性分為同等重要、稍微重要、明顯重要、強(qiáng)烈重要、極度重要5等級(jí)(對(duì)應(yīng)標(biāo)度值為1,3,5,7,9)。假設(shè)某層次中元素ai與元素aj的相對(duì)重要性用ai/aj表示，則有

進(jìn)行全面規(guī)劃，應(yīng)充分考慮長(zhǎng)遠(yuǎn)利益與當(dāng)前利益，積極推進(jìn)省內(nèi)重點(diǎn)水利工程建設(shè)，適時(shí)推動(dòng)鄱陽(yáng)湖水利樞紐工程建設(shè)；持續(xù)推進(jìn)大型灌區(qū)續(xù)建配套工程，加強(qiáng)建設(shè)和改造中小型灌區(qū)，加快排灌泵站更新改造，不斷完善灌溉排澇系統(tǒng)；[2，3]大力開(kāi)展縣級(jí)以上城市防洪排澇體系建設(shè)，以及山洪災(zāi)害防治、中小河流治理和鄱陽(yáng)湖蓄滯洪區(qū)安全建設(shè)，從而提高設(shè)防中心城市防洪標(biāo)準(zhǔn)。

(3)

為了防止層次分析法分析問(wèn)題產(chǎn)生偏差，需要進(jìn)行一致性檢驗(yàn)，用一致性比例C.R.表示，即

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

(4)

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

當(dāng)C.R.<0.1時(shí)，認(rèn)為判斷矩陣的一致性是可以接受的;否則,應(yīng)該修改矩陣使之符合一致性要求。

圖6 轉(zhuǎn)向路徑層次結(jié)構(gòu)模型圖

3.2 行尾地頭轉(zhuǎn)向路徑評(píng)價(jià)與選擇

3.2.1U型地頭轉(zhuǎn)向路徑評(píng)價(jià)與選擇

構(gòu)造準(zhǔn)則層C相對(duì)目標(biāo)層A的判斷矩陣

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

構(gòu)造方案層D各元素相當(dāng)于準(zhǔn)則層C各元素的判斷矩陣

方案層D相對(duì)于目標(biāo)層A的權(quán)重是通過(guò)準(zhǔn)則層C相對(duì)于目標(biāo)層A和方案層D相對(duì)于準(zhǔn)則層C的權(quán)重組合而得到，計(jì)算結(jié)果如表1。路徑1,2,3的總權(quán)重向量W=[0.138 0.720 0.142]T，即生成路徑2為最優(yōu)路徑?？偱判蛞恢滦员壤鼵.R.為

(5)

式中C.I.i為準(zhǔn)則層Ci相應(yīng)的方案層D中判斷矩陣的一致性指標(biāo)；R.I.i為準(zhǔn)則層Ci相應(yīng)的方案層D中判斷矩陣的平均隨機(jī)一致性指標(biāo)；ci是準(zhǔn)則層C相對(duì)目標(biāo)層A的權(quán)重值。層次總排序一致性比例C.R.=0.018<0.1，說(shuō)明層次總排序一致性可以接受。

表1 轉(zhuǎn)向路徑1,2,3總權(quán)重

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

3.2.2 K型地頭轉(zhuǎn)向路徑評(píng)價(jià)與選擇

構(gòu)造準(zhǔn)則層C相對(duì)目標(biāo)層A的判斷矩陣

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

構(gòu)造方案層D各元素相當(dāng)于準(zhǔn)則層C各元素的判斷矩陣

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

表2 轉(zhuǎn)向路徑4,5,6總權(quán)重

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

4 實(shí)驗(yàn)與結(jié)果分析

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

表3 地頭轉(zhuǎn)向路徑實(shí)驗(yàn)數(shù)據(jù)

路徑實(shí)驗(yàn)次數(shù)右側(cè)車(chē)輪角速度/(rad/s)左側(cè)車(chē)輪角速度/(rad/s)轉(zhuǎn)向時(shí)間/s轉(zhuǎn)向次數(shù)/次最終位置(與行中心線(xiàn)的夾角)/(°)最終位置(與行中心線(xiàn)的距離)/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

注：車(chē)輛縱向中心線(xiàn)偏向果樹(shù)行中心線(xiàn)左側(cè)為正，反之為負(fù)；車(chē)頭偏向左為正，反之為負(fù)；角速度值為負(fù)表示車(chē)輛倒退行駛。

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

5 結(jié) 論

針對(duì)差速轉(zhuǎn)向機(jī)器人車(chē)輛開(kāi)展了果園行尾地頭轉(zhuǎn)向路徑規(guī)劃研究，分析了在行尾地頭寬度充足與不足情況下的行尾地頭轉(zhuǎn)向路徑，并通過(guò)層次分析法對(duì)生成路徑進(jìn)行優(yōu)化選擇。對(duì)果樹(shù)行尾地頭寬度充足或不足的情況分別進(jìn)行實(shí)驗(yàn)驗(yàn)證。結(jié)果表明:盡管有一定的方向偏角與橫向偏移，但是機(jī)器人車(chē)輛能很好地完成果園行尾的地頭轉(zhuǎn)向。

[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] 馮 娟,劉 剛,司永勝,等.果園視覺(jué)導(dǎo)航基準(zhǔn)線(xiàn)生成算法[J].農(nóng)業(yè)機(jī)械學(xué)報(bào),2012,43(7):185-189,184.

[4] 劉 沛,陳 軍,張明穎.基于激光導(dǎo)航的果園拖拉機(jī)自動(dòng)控制系統(tǒng)[J].農(nóng)業(yè)工程學(xué)報(bào),2011,27(3):196-199.

[5] 薛金林,張順順.基于激光雷達(dá)的農(nóng)業(yè)機(jī)器人導(dǎo)航控制研究[J].農(nóng)業(yè)機(jī)械學(xué)報(bào),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ìn)隨機(jī)樣本一致性算法的彎曲果園道路檢測(cè)[J].農(nóng)業(yè)工程學(xué)報(bào),2015,31(4):168-174.

[8] 陳 軍,蔣浩然,劉 沛,等.果園移動(dòng)機(jī)器人曲線(xiàn)路徑導(dǎo)航控制[J].農(nóng)業(yè)機(jī)械學(xué)報(bào),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] 郝永軍.果園打藥機(jī)自動(dòng)行走控制系統(tǒng)研究[D].保定:河北農(nóng)業(yè)大學(xué),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] 朱磊磊.果園管理機(jī)器人平臺(tái)的自動(dòng)導(dǎo)航系統(tǒng)研究[D].西安:西北農(nóng)林科技大學(xué),2010.

[13] 王海波,劉鳳之,王孝娣,等.我國(guó)果園機(jī)械研發(fā)與應(yīng)用概述[J].果樹(shù)學(xué)報(bào),2013,30(1):165-170.

[14] 董希玲.蘋(píng)果果園管理信息系統(tǒng)的構(gòu)建[D].保定:河北農(nóng)業(yè)大學(xué),2010.

[15] 王 斌,劉昭度,何 瑋,等.車(chē)用測(cè)距雷達(dá)研究進(jìn)展[J].傳感器與微系統(tǒng),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

中央高?；究蒲袠I(yè)務(wù)費(fèi)專(zhuān)項(xiàng)資金資助項(xiàng)目(KYZ2011)； 江蘇省自然科學(xué)基金資助項(xiàng)目(BK20151436)

TP 242

A

1000—9787(2017)02—0028—04

封 永(1987-)，男，碩士研究生，主要研究方向?yàn)檐?chē)輛自主轉(zhuǎn)向智能化。

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