自動(dòng)清糞機(jī)器人路徑規(guī)劃方法的研究與實(shí)現(xiàn)

侯云濤，堯李慧，蔡曉華，王 強(qiáng)

(1.黑龍江省農(nóng)業(yè)機(jī)械工程科學(xué)研究院，哈爾濱 150081；2.黑龍江八一農(nóng)墾大學(xué)，黑龍江 大慶 163319；3.中國(guó)農(nóng)業(yè)機(jī)械化科學(xué)研究院呼和浩特分院，呼和浩特 010010)

?

自動(dòng)清糞機(jī)器人路徑規(guī)劃方法的研究與實(shí)現(xiàn)

侯云濤1，堯李慧2，蔡曉華1，王 強(qiáng)3

(1.黑龍江省農(nóng)業(yè)機(jī)械工程科學(xué)研究院，哈爾濱 150081；2.黑龍江八一農(nóng)墾大學(xué)，黑龍江 大慶 163319；3.中國(guó)農(nóng)業(yè)機(jī)械化科學(xué)研究院呼和浩特分院，呼和浩特 010010)

自動(dòng)清糞機(jī)器人的工作環(huán)境復(fù)雜，不僅有靜態(tài)障礙物(如牛欄)，且動(dòng)態(tài)障礙物眾多(如人和牲畜等)。為此，提出了一種基于行為方法的路徑規(guī)劃法。該方法非常適合于動(dòng)態(tài)環(huán)境下機(jī)器人的運(yùn)動(dòng)規(guī)劃，其原理簡(jiǎn)單、計(jì)算快捷、容易實(shí)現(xiàn)。根據(jù)該算法，確立了一套完整的軟件系統(tǒng)和設(shè)計(jì)方案，并在此基礎(chǔ)上制造出了樣機(jī)。經(jīng)過(guò)樣機(jī)在奶牛養(yǎng)殖場(chǎng)的工作和測(cè)試，驗(yàn)證了該方法的實(shí)時(shí)性、準(zhǔn)確性和可操作性。

自動(dòng)清糞機(jī)器人；行為方法；路徑規(guī)劃算法；動(dòng)態(tài)環(huán)境

0 引言

隨著養(yǎng)殖業(yè)中大規(guī)?；膛ｐB(yǎng)殖場(chǎng)的日益興起，如何有效地對(duì)奶牛糞便進(jìn)行收集及處理，日益成為一個(gè)嚴(yán)峻棘手的問(wèn)題。如不能有效處理奶牛糞便，不僅影響奶牛的出奶量和品質(zhì)，還會(huì)給奶牛及養(yǎng)殖場(chǎng)附近居民的健康帶來(lái)極大威脅。清理奶牛糞污的工作在規(guī)模化牛場(chǎng)用人力完成是不可能的，因此迫切需要開(kāi)發(fā)一種自走式智能化牛舍清糞機(jī)器人，能夠自動(dòng)化、全天候地工作，并且行走速度適中、噪音極小，對(duì)牛群的日常飼養(yǎng)和奶牛起居不造成任何影響和傷害，替代傳統(tǒng)繁重的人工清潔工作。

由于清糞機(jī)器人工作環(huán)境復(fù)雜，障礙物眾多(如牛欄、奶牛等)，且許多奶牛不時(shí)在牛舍中走動(dòng)，對(duì)于機(jī)器人來(lái)說(shuō)，無(wú)疑于眾多的移動(dòng)障礙物。因此，如何設(shè)計(jì)出最優(yōu)的行走路徑，有效規(guī)避牛舍中的障礙物，成為清糞機(jī)器人控制算法的關(guān)鍵問(wèn)題。

路徑規(guī)劃問(wèn)題按照機(jī)器人的工作條件，可以分為兩種工作模型：一是基于靜止障礙物的規(guī)化模型,養(yǎng)牛場(chǎng)的全部地面障礙信息都是預(yù)知的;二是基于傳感器的路徑規(guī)劃算法,養(yǎng)牛場(chǎng)的地面障礙信息是全部未知或部分未知的[1]。清糞機(jī)器人的工作條件無(wú)疑屬于第2種。

對(duì)于養(yǎng)牛場(chǎng)的地面工作障礙信息部分未知或者完全未知的情況，機(jī)器人利用裝配的傳感器實(shí)時(shí)檢測(cè)周邊環(huán)境，控制系統(tǒng)利用傳感器接收到的數(shù)據(jù)決策機(jī)器人的運(yùn)動(dòng)軌跡。局部路徑規(guī)劃的常用方法有人工勢(shì)場(chǎng)法及模糊邏輯等[2]。

本文采用一種基于行為的方法，可以實(shí)現(xiàn)機(jī)器人的局部最優(yōu)路徑規(guī)劃，行為的綜合結(jié)果用來(lái)指導(dǎo)機(jī)器人的運(yùn)動(dòng)，非常適用于時(shí)變未知環(huán)境下的路徑規(guī)劃。經(jīng)過(guò)樣機(jī)的現(xiàn)場(chǎng)測(cè)試表明，本文方法的準(zhǔn)確性和實(shí)時(shí)性極高。

1 自動(dòng)清糞機(jī)器人的運(yùn)動(dòng)軌跡模型

本文設(shè)計(jì)的自動(dòng)清糞機(jī)器人的兩個(gè)后輪作為完全獨(dú)立的驅(qū)動(dòng)輪，分別由2個(gè)直流伺服電機(jī)控制，利用轉(zhuǎn)速差實(shí)現(xiàn)轉(zhuǎn)向。其具有自重輕、承載大、機(jī)構(gòu)簡(jiǎn)單、驅(qū)動(dòng)和控制相對(duì)方便、行走速度快、機(jī)動(dòng)靈活及工作效率高等優(yōu)點(diǎn)[3]。

機(jī)器人裝配有以下3種傳感器：

1)慣性測(cè)量部件，用來(lái)檢測(cè)機(jī)器人的轉(zhuǎn)角；

2)超聲波傳感器，檢測(cè)機(jī)器人與障礙物的距離；

3)電流檢測(cè)傳感器，用來(lái)檢測(cè)驅(qū)動(dòng)電機(jī)的電流，一旦電流值超過(guò)正常值，即可判斷機(jī)器人前端接觸到障礙物。

根據(jù)Muri和Neuman提出的輪式移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)建模理論，兩輪驅(qū)動(dòng)的自動(dòng)清糞機(jī)器人作為幾何中心和質(zhì)心不重合的約束非理想的簡(jiǎn)化模型，如圖1所示[4]。簡(jiǎn)化后的清糞機(jī)器人的運(yùn)行機(jī)構(gòu)由兩個(gè)同軸的驅(qū)動(dòng)后輪和1個(gè)刮糞板組成，三點(diǎn)確定一個(gè)平面，刮糞板起到萬(wàn)向從動(dòng)輪轉(zhuǎn)向的作用。清糞機(jī)器人的兩個(gè)后輪分別由兩個(gè)獨(dú)立的直流伺服電機(jī)驅(qū)動(dòng)，前方刮板看作是支撐輪，在后方兩個(gè)驅(qū)動(dòng)輪的作用下，機(jī)器人可以沿目標(biāo)方向向前移動(dòng)。(xM,yM)為機(jī)器人質(zhì)量中心M在XOY世界坐標(biāo)系中的橫縱坐標(biāo)；θ為清潔機(jī)器人運(yùn)動(dòng)方向與世界坐標(biāo)系X軸的夾角；C為清潔機(jī)器人連接兩個(gè)驅(qū)動(dòng)輪的輪軸的幾何中心，(xC，yC)為清潔機(jī)器人的平面投影幾何中心C在世界坐標(biāo)系中的橫縱坐標(biāo)；r為兩個(gè)驅(qū)動(dòng)輪半徑，2b為兩驅(qū)動(dòng)輪之間的距離；d為M、C之間的距離。

圖1 自動(dòng)清糞機(jī)器人運(yùn)動(dòng)模型Fig.1 Motion model of the barn cleaner

設(shè)機(jī)器人平臺(tái)質(zhì)量中心 M 的前進(jìn)速度為 VM，它垂直于平面的車輪軸，在世界坐標(biāo)系中坐標(biāo)軸上的分量分別為

(1)

(2)

由圖 1 可知：清潔機(jī)器人質(zhì)量中心 M 和平面投影幾何中心 C有如下位置關(guān)系，即

xc=xM-dcosθ

(3)

yc=yM-dsinθ

(4)

對(duì)式(3)和式(4)求導(dǎo)可得

(5)

(6)

將式(1)、式(2)分別代入式(5)和式(6)，經(jīng)過(guò)運(yùn)算可以得平面投影幾何中心C的速度關(guān)系式為

(7)

(8)

(9)

由式(1)、式(5)、式(8)和式(9)可得

(10)

(11)

當(dāng)不考慮驅(qū)動(dòng)輪的滑動(dòng)時(shí)，由式(9) ～ 式(11)可以得到機(jī)器人的運(yùn)動(dòng)學(xué)方程為

(12)

由于機(jī)器人的質(zhì)量中心 M 和平面投影幾何中心 C并不重合，在兩個(gè)后置驅(qū)動(dòng)輪滿足與地面無(wú)滑動(dòng)、純滾動(dòng)的運(yùn)行條件下，清糞機(jī)器人所受的非完整約束關(guān)系為

(13)

可將式(13)改寫成如下形式，即

(14)

(15)

速度V(t)為

V(t)=[v，ω]T

(16)

其中，v表示機(jī)器人平面投影幾何中心C在世界坐標(biāo)系下的線速度;ω表示C 的笛卡兒坐標(biāo)系下角速度。

2 基本行為集

針對(duì)清糞機(jī)器人的復(fù)雜作業(yè)環(huán)境，本文為清糞機(jī)器人設(shè)計(jì)了3種基本行為：follow_wall行為、change_direction行為和move_to_alone行為。其中，follow_wall行為的功能是：根據(jù)機(jī)器人當(dāng)前探測(cè)到的環(huán)境信息，通過(guò)超聲波傳感器及慣性測(cè)量部件以避開(kāi)環(huán)境中的靜態(tài)障礙物，并利用機(jī)器人距離靜態(tài)障礙的距離及慣性測(cè)量檢測(cè)出的轉(zhuǎn)角，使機(jī)器人按照預(yù)先設(shè)定的軌跡行走。change_direction行為的功能是：當(dāng)人或者奶牛突然出現(xiàn)在預(yù)先設(shè)定的軌跡上時(shí)，控制系統(tǒng)根據(jù)電流傳感器檢測(cè)到的驅(qū)動(dòng)電機(jī)電流值，可以使機(jī)器人迅速停止運(yùn)動(dòng)，以免造成傷害，之后能夠繞開(kāi)障礙物，并且使機(jī)器人回到預(yù)先設(shè)定的軌跡。move_to_alone行為的功能是：當(dāng)環(huán)境中沒(méi)有可以利用測(cè)距的靜態(tài)障礙物，并且尚未碰撞到移動(dòng)障礙物時(shí)，那么機(jī)器人將進(jìn)行直行動(dòng)作，由于糾偏措施較少，所以該行為進(jìn)行的時(shí)間不能過(guò)長(zhǎng)，以免偏離預(yù)定軌跡。

3種行為之間的優(yōu)先級(jí)關(guān)系如圖2所示。其中，位于高層的行為抑制下層的行為，S表示抑制，即change_direction行為抑制follow_wall行為，follow_wall行為抑制move_to_alone行為。在某時(shí)刻，只有確定的唯一種行為對(duì)機(jī)器人起作用。

圖2 基于行為控制的結(jié)構(gòu)Fig.2 The control structure based on the behavior

2.1 Change_direction行為

當(dāng)機(jī)器人在預(yù)設(shè)軌跡上行駛、碰撞到突然進(jìn)入到軌跡中的人或牲畜等動(dòng)態(tài)障礙物時(shí)，通過(guò)電流傳感器可以檢測(cè)到驅(qū)動(dòng)電機(jī)的電流瞬間增大，機(jī)器人會(huì)立即停止前進(jìn)，并在向后行駛一定的距離后，開(kāi)始旋轉(zhuǎn)；之后開(kāi)啟此行為模式，沿著障礙物的邊界開(kāi)始行駛。開(kāi)啟該模式時(shí)，follow_wall行為和Move_to_alone行為均被鎖死。

以機(jī)器人當(dāng)前位置R點(diǎn)為原點(diǎn)、當(dāng)前運(yùn)動(dòng)方向?yàn)閄軸，建立極坐標(biāo)系，如圖3所示[5]。

圖3 Change_direction行為示意圖Fig.3 Change_direction behavior diagram

機(jī)器人下一步運(yùn)行的運(yùn)動(dòng)方向應(yīng)當(dāng)滿足

(17)

(18)

其中，D0為一裕量，用來(lái)確保機(jī)器人與障礙物保持一定距離；θf(wàn)w(j)在滿足上式的條件下，使得Dfw((θf(wàn)w(j))取得最小值，記為θf(wàn)w(m)。當(dāng)沒(méi)有發(fā)現(xiàn)合適的θf(wàn)w(j)時(shí)，機(jī)器人原地旋轉(zhuǎn)θao(固定值)。

于是，change_direction行為規(guī)劃的控制輸入為

(19)

(20)

當(dāng)機(jī)器人與障礙物的距離Dfw(θf(wàn)w(j))大于某個(gè)定值時(shí)，控制系統(tǒng)通過(guò)慣性測(cè)量部件檢測(cè)到的機(jī)器人的轉(zhuǎn)角值，可以使得機(jī)器人通過(guò)旋轉(zhuǎn)一定的角度來(lái)調(diào)整運(yùn)行的方向，從而重新回到預(yù)設(shè)軌跡,開(kāi)啟follow_wall行為和Move_to_alone行為。

2.2 follow_wall行為

在機(jī)器人按照預(yù)先設(shè)計(jì)定的軌跡正常行駛，并且周邊環(huán)境具有固定的、可供測(cè)距的靜態(tài)障礙物存在時(shí)(如牛欄，水槽等)，該行為模式被激活。

在此行為模式下，機(jī)器人通過(guò)超聲波傳感器不斷檢測(cè)與周邊靜態(tài)障礙物的距離，控制系統(tǒng)通過(guò)判斷距離是否符合預(yù)先的設(shè)定值，來(lái)判斷運(yùn)動(dòng)軌跡是否偏離。當(dāng)發(fā)生偏離時(shí)，就需要控制系統(tǒng)進(jìn)行及時(shí)調(diào)整。調(diào)整過(guò)程中，可以通過(guò)超聲波傳感器測(cè)距來(lái)保證調(diào)整后機(jī)器人與周邊靜態(tài)障礙物的距離符合預(yù)先的設(shè)定，通過(guò)慣性測(cè)量部件測(cè)轉(zhuǎn)角來(lái)保證調(diào)整后的運(yùn)動(dòng)方向符合預(yù)先的設(shè)定。

2.3 Move_to_alone行為

當(dāng)機(jī)器人在正常的行走過(guò)程中，周邊環(huán)境沒(méi)有可供超聲波傳感器測(cè)距的靜態(tài)障礙物存在時(shí)，該行為模式被激活。

在此行為模式下，只有慣性測(cè)量部件檢測(cè)到的轉(zhuǎn)角作為控制系統(tǒng)的糾偏輸入，超聲波傳感器無(wú)法為控制系統(tǒng)提供有效的輸入。所以，在規(guī)劃?rùn)C(jī)器人的行走路徑時(shí)，要盡量減少該行為模式的出現(xiàn)次數(shù)。

3 機(jī)器人運(yùn)行軌跡規(guī)劃算法的優(yōu)化實(shí)現(xiàn)

通過(guò)本文之前的說(shuō)明，可以得出以下結(jié)論：本文設(shè)計(jì)的機(jī)器人路徑規(guī)劃算法中，Change_direction行為是機(jī)器人在預(yù)定行駛軌跡中遇到突發(fā)的動(dòng)態(tài)障礙物時(shí)的處理方法；follow_wall行為和Move_to_alone行為是機(jī)器人在預(yù)定軌跡上正常行駛時(shí)的行為模式。

圖4簡(jiǎn)易說(shuō)明了follow_wall行為和Move_to_alone行為如何構(gòu)成了機(jī)器人在沒(méi)有動(dòng)態(tài)障礙物的情況下的行駛軌跡。

圖4 運(yùn)動(dòng)軌跡示意圖Fig.4 Trajectory diagram

運(yùn)動(dòng)軌跡由以下路徑組成：(1)沿墻或牛欄走(follow_wall行為)；(2)左轉(zhuǎn)(45°)；(3)直行(Move_to_alone行為)；(4)右轉(zhuǎn)(45°)；(5)沿墻或牛欄走(follow_wall行為)；(6)右轉(zhuǎn)45°；(7)直行(Move_to_alone行為)；(8)左轉(zhuǎn)45°；(9)沿墻或牛欄走(follow_wall行為)；(10)沿墻或牛欄走(follow_wall行為)；(11)左轉(zhuǎn)10°，之后右轉(zhuǎn)5°；(12)直行(Move_to_alone行為)；(13)左轉(zhuǎn)180°；(14)直行(Move_to_alone行為)；(15)沿墻或牛欄走(follow_wall行為)；(16)沿墻或牛欄走(follow_wall行為)；(17)直行(Move_to_alone行為)；(18)左轉(zhuǎn)90°；(19)沿墻或牛欄走(follow_wall行為)；(20)左轉(zhuǎn)90°；(21)直行(Move_to_alone行為)；(22)右轉(zhuǎn)10°；(23)沿墻或牛欄走(follow_wall

行為)。

4 結(jié)論

本文提出的路徑規(guī)劃方法非常適合于動(dòng)態(tài)環(huán)境下(即不時(shí)出現(xiàn)移動(dòng)障礙物情況下)的機(jī)器人的運(yùn)動(dòng)路線規(guī)劃。本文在為機(jī)器人設(shè)計(jì)了3種基本行為后，根據(jù)機(jī)器人的工作目標(biāo)、周邊的工作環(huán)境等，可以通過(guò)進(jìn)行在線調(diào)整來(lái)提高機(jī)器人適應(yīng)環(huán)境的能力，包括改變行為的激活狀態(tài)和運(yùn)行狀態(tài)、改變行為算法模型的接口參數(shù)及改變行為的綜合決策判斷模式等。最后，控制系統(tǒng)針對(duì)各個(gè)被激活的基本行為進(jìn)行綜合決策，驅(qū)動(dòng)執(zhí)行機(jī)構(gòu)執(zhí)行決策。

按照本文提出的路徑規(guī)劃方法進(jìn)行了軟件編程，并組裝了測(cè)試樣機(jī)。通過(guò)樣機(jī)在奶牛養(yǎng)殖場(chǎng)的相關(guān)測(cè)試、運(yùn)行，驗(yàn)證了本文提出方法的實(shí)時(shí)性、準(zhǔn)確性和可操作性。

[1] 王菁華,張翠敏.智能機(jī)器人綜合路徑規(guī)劃算法在Matlab中的實(shí)現(xiàn)[J].天津工程師范學(xué)院學(xué)報(bào), 2006,16(3):47-50.

[2] 于奎龍,賈小平，曹有輝，等.基于混合算法的局部路徑規(guī)劃[J].裝甲兵工程學(xué)院學(xué)報(bào), 2008,22(2):43-45.

[3] 張倩.差動(dòng)式移動(dòng)機(jī)器人的運(yùn)動(dòng)性能分析[D].天津：河北工業(yè)大學(xué)，2013.

[4] 李江昊，李振波，陳佳品.毫米級(jí)全方位移動(dòng)微型裝配機(jī)器人設(shè)計(jì)[J].機(jī)器人,2008,30(1):63-69.

[5] 屈 鴻，黃利偉，柯星.動(dòng)態(tài)環(huán)境下基于改進(jìn)蟻群算法的機(jī)器人路徑規(guī)劃研究[J].電子科技大學(xué)學(xué)報(bào),2015,44(2):260-265.

Research and Realization of Automatic Barn Cleaner Path Planning

Hou Yuntao1, Yao Lihui2, Cai Xiaohua1, Wang Qiang3

(1.Scientific Research Institute of Agricultural Mechanical Engineering in Heilongjiang，Harbin 150081, China；2.Heilongjiang BAYI Agriculture university，Daqing 163319, China; 3.Hohhot Branch of Chinese Academy of Agricultural Mechanization Sciences 010010, China)

The working environment of barn cleaner is complex,not only static obstacles,such as the stable,and dynamic obstacles, such as people and livestock,etc.Aimed at the situation ,this paper proposes a method of path planning algorithm based on behavior .This method is very suitable for robot in dynamic environment, its principle is simple, quick calculation, easy to implement.According to this algorithm, we established a complete set of software system and the design scheme,And on this basis, make the prototype.After the prototype in dairy cow farm work and test, this method was verified real-time, accuracy and operability.

barn cleaner; behavior method; path planning; dynamic environment

2016-07-21

“十二五”國(guó)家科技支撐計(jì)劃項(xiàng)目(2014BAD08B10)

侯云濤(1984-),男，哈爾濱人，工程師，(E-mail)736126 975@qq.com。

蔡曉華(1968-),男，哈爾濱人，研究員級(jí)高級(jí)工程師，(E-mail) 12369757@qq.com。

S24；TP242

A

1003-188X(2017)06-0023-04