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

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

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

?

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

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

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

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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

設(shè)機(jī)器人平臺質(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)

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

(5)

(6)

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

(7)

(8)

(9)

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

(10)

(11)

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

(12)

由于機(jī)器人的質(zhì)量中心 M 和平面投影幾何中心 C并不重合，在兩個后置驅(qū)動輪滿足與地面無滑動、純滾動的運(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 基本行為集

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

3種行為之間的優(yōu)先級關(guān)系如圖2所示。其中，位于高層的行為抑制下層的行為，S表示抑制，即change_direction行為抑制follow_wall行為，follow_wall行為抑制move_to_alone行為。在某時刻，只有確定的唯一種行為對機(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)入到軌跡中的人或牲畜等動態(tài)障礙物時，通過電流傳感器可以檢測到驅(qū)動電機(jī)的電流瞬間增大，機(jī)器人會立即停止前進(jìn)，并在向后行駛一定的距離后，開始旋轉(zhuǎn)；之后開啟此行為模式，沿著障礙物的邊界開始行駛。開啟該模式時，follow_wall行為和Move_to_alone行為均被鎖死。

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

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

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

(17)

(18)

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

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

(19)

(20)

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

2.2 follow_wall行為

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

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

2.3 Move_to_alone行為

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

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

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

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

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

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

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

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

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

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

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

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

[5] 屈 鴻，黃利偉，柯星.動態(tài)環(huán)境下基于改進(jìn)蟻群算法的機(jī)器人路徑規(guī)劃研究[J].電子科技大學(xué)學(xué)報,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

“十二五”國家科技支撐計劃項目(2014BAD08B10)

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

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

S24；TP242

A

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