專利名稱：基于電子羅盤的割草機器人局部路徑規(guī)劃方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種控制割草機器人路徑的方法，尤其是一種基于電子羅盤的割草機器人局部路徑規(guī)劃方法，可精確控制和優(yōu)化割草機器人工作時的路徑。
背景技術(shù)：
隨著經(jīng)濟的發(fā)展，生活水平的提高，人們的環(huán)境意識越來越強，綠色草坪在大小城市中已隨處可見，但割草作業(yè)的任務(wù)量大且?guī)в泻軓姷闹貜?fù)性，因此對智能割草機器人的研究是社會發(fā)展的需要。智能割草機器人是一個相當(dāng)復(fù)雜的系統(tǒng)，它集多種功能和多種技術(shù)于一體，因此對智能割草機器人的研究涉及很多方面，相比于以往割草機器人運動路徑失控，重復(fù)性大的問題，本發(fā)明研究基于電子羅盤的割草機器人局部路徑規(guī)劃方法，可精確控制和優(yōu)化割草機器人的工作路徑，以求得復(fù)雜環(huán)境下割草機器人平滑、穩(wěn)定、精確的工作，解決了以上問題。

發(fā)明內(nèi)容
本發(fā)明的目的在于提供一種基于電子羅盤的割草機器人局部路徑規(guī)劃方法，該方法利用電子羅盤硬件模塊對割草機器人所處位置的方位進(jìn)行檢測，分析得到割草機器人此刻在磁場中X、Y、Z軸上的分量，從而根據(jù)輸出的航向角來精確校正割草機器人的運動路徑。減少了電能消耗和提高了割草效率，精確控制運動路徑并優(yōu)化軌跡。本發(fā)明采用的技術(shù)方案是
基于電子羅盤的割草機器人局部路徑規(guī)劃方法，割草機器人與電子羅盤硬件模塊是通過I2 C協(xié)議來實現(xiàn)通訊。電子羅盤
硬件模塊檢測割草機器人所處位置的方位并將方位信號傳遞給路徑規(guī)劃控制模塊。同時， 根據(jù)外部環(huán)境狀況，監(jiān)控感應(yīng)模塊判斷割草機器人的運動狀態(tài)割草狀態(tài)、邊界狀態(tài)、導(dǎo)航狀態(tài)，并將狀態(tài)信號傳遞給路徑規(guī)劃控制模塊。路徑規(guī)劃控制模塊對接收到的方位信號和狀態(tài)信號進(jìn)行分析處理，判斷割草機器人下一步的精確位置及運動狀態(tài)。本發(fā)明采用的技術(shù)方案的步驟如下
O電子羅盤硬件模塊記錄割草機器人當(dāng)前和歷史的方位信息，并轉(zhuǎn)換為方位信號傳送給路徑規(guī)劃控制模塊；
2)監(jiān)控感應(yīng)模塊根據(jù)外部環(huán)境來判斷割草機器人當(dāng)前的環(huán)境狀況，并轉(zhuǎn)換為狀態(tài)信號傳送給路徑規(guī)劃控制模塊；
3)路徑規(guī)劃控制模塊分析處理接收到的方位信號和狀態(tài)信號，優(yōu)化得到相應(yīng)的運動狀態(tài)及路徑。最后將得到的數(shù)字信號發(fā)送給割草機器人的運動系統(tǒng)控制模塊；
4)運動系統(tǒng)控制模塊根據(jù)接收到的數(shù)字信號來控制割草機器人按相應(yīng)的命令運動到下一位置。所述步驟I)中的電子羅盤硬件模塊包含放大器、濾波器及A/D轉(zhuǎn)換器。通過電子羅盤測量割草機器人所處位置在磁場中Χ、γ軸方向上的分量，同時測得Z軸的分量，并對這三個數(shù)值進(jìn)行放大濾波和A/D轉(zhuǎn)換，再通過I 2 C通訊傳給割草機器人的路徑規(guī)劃控制模塊；同時結(jié)合電子羅盤測量得到的俯仰角和翻滾角對X、Y軸的磁場強度作補償校正，傳送給割草機器人的路徑規(guī)劃控制模塊，分析計算得到割草機器人的地理航向角和姿態(tài)角。它能確定割草機器人當(dāng)前和歷史的方位，進(jìn)而控制割草機器人下一步的運動方位，來實現(xiàn)軌跡的優(yōu)化。所述步驟2)中的監(jiān)控感應(yīng)模塊判斷割草機器人當(dāng)前的環(huán)境狀況，根據(jù)環(huán)境狀況得到狀態(tài)信號，確定割草機器人下一步的運動狀態(tài)，總共有三個狀態(tài)割草狀態(tài)，邊界狀態(tài)，導(dǎo)航狀態(tài)。所述步驟3)中的路徑規(guī)劃控制模塊，根據(jù)接收的方位信號和狀態(tài)信號來判斷割草機器人的精確運動位置。其運動控制特征如下
O割草狀態(tài)
在割草狀態(tài)輸入的條件下，割草機器人沿某一方向行走，遇到障礙(包括導(dǎo)航標(biāo)志、邊界、墻、階梯等)后退，利用電子羅盤硬件模塊的方位信號來控制割草機器人相對于原來方向順時針轉(zhuǎn)動某一設(shè)定的角度，并判斷障礙是否還在監(jiān)控感應(yīng)模塊設(shè)定的感應(yīng)范圍內(nèi)。若是，重復(fù)后退轉(zhuǎn)彎的步驟；若不是，則利用電子羅盤硬件模塊的方位信號來控制割草機器人沿轉(zhuǎn)后的方向繼續(xù)行走并判斷。如此循環(huán)此過程，直到整個區(qū)域被覆蓋。2)邊界狀態(tài)
本發(fā)明中邊界狀態(tài)有兩種形式，一種是割草時遇到的邊界，此時在割草機器人中作為障礙處理，在割草狀態(tài)中已說明。另一種則是在回基站的過程中遇到的邊界，具有不同的處理方式。在回基站過程中，由于導(dǎo)航標(biāo)志設(shè)置在邊界處，故識別到邊界時，利用電子羅盤硬件模塊的方位信號來控制割草機器人沿著邊界行走，尋找導(dǎo)航標(biāo)志。具體為當(dāng)遇到邊界時，利用電子羅盤硬件模塊的方位信號來控制割草機器人順時針轉(zhuǎn)一個固定角度，判斷邊界是否還在監(jiān)控感應(yīng)范圍內(nèi)，若是，利用電子羅盤硬件模塊的方位信號來控制割草機器人再轉(zhuǎn)一個設(shè)定的角度；若不是，則利用電子羅盤硬件模塊的方位信號來控制割草機器人直線前進(jìn)一段距離，在此過程中監(jiān)控感應(yīng)模塊仍在不斷的判斷中。如此反復(fù)，直到發(fā)現(xiàn)標(biāo)志。3)導(dǎo)航狀態(tài)
本發(fā)明中當(dāng)割草機器人在割草過程中電量達(dá)到某一設(shè)定值時，割草機器人就要回基站充電。其基本的方式是不管割草機器人在草地的哪個位置，割草機器人得到這一信號后先停止割草，然后利用電子羅盤硬件模塊的方位信號來控制割草機器人向前直線行走直到發(fā)現(xiàn)邊界，再以邊界狀態(tài)的方式進(jìn)行處理。找到導(dǎo)航標(biāo)志后，判斷標(biāo)志是否在割草機器人的右側(cè)，若是，利用電子羅盤硬件模塊的方位信號來控制割草機器人左轉(zhuǎn)一個角度，并移動一段距離，再判斷；若不是，利用電子羅盤硬件模塊的方位信號來控制割草機器人右轉(zhuǎn)一個角度，并移動一段距離，再判斷。如此反復(fù)，直到順著標(biāo)志走到充電處。所述步驟4)中的運動系統(tǒng)控制模塊，控制割草機器人按相應(yīng)的執(zhí)行命令運動到下一位置。本發(fā)明具有的有益效果是
帶有電子羅盤的割草機器人與傳統(tǒng)割草機器人相比，能精確控制割草路徑，在割草模式，邊界模式，導(dǎo)航模式中實現(xiàn)有規(guī)律的路徑變化，避免了傳統(tǒng)割草方式的無規(guī)律性，盲目性和出現(xiàn)的路徑失控問題，減少了電能消耗和提高了割草效率，充分覆蓋整個區(qū)域。


圖I是系統(tǒng)體系結(jié)構(gòu)圖。圖2是三個狀態(tài)流程圖。圖3是基于電子羅盤的系統(tǒng)框圖。圖4是三軸磁阻傳感器引腳及外圍電路圖。圖5是I 2 C通訊電路圖。
具體實施例方式下面結(jié)合附圖和具體實例對本發(fā)明做進(jìn)一步詳述
圖I所示，是系統(tǒng)體系結(jié)構(gòu)圖。由路徑規(guī)劃控制模塊，電子羅盤硬件模塊，監(jiān)控感應(yīng)模塊和運動系統(tǒng)控制模塊組成。所述的電子羅盤硬件模塊記錄割草機器人當(dāng)前和歷史的方位信息，并轉(zhuǎn)換為方位信號傳送給路徑規(guī)劃控制模塊。所述的監(jiān)控感應(yīng)模塊根據(jù)外部環(huán)境來判斷割草機器人當(dāng)前的環(huán)境狀況，并轉(zhuǎn)換為狀態(tài)信號傳送給路徑規(guī)劃控制模塊。所述的路徑規(guī)劃控制模塊分析處理接收到的狀態(tài)信號和方位信號，優(yōu)化得到相應(yīng)的運動狀態(tài)及路徑。得到的運動狀態(tài)若是割草狀態(tài)，則以割草狀態(tài)下的路徑控制割草機器人的運動，實現(xiàn)了在割草狀態(tài)下精準(zhǔn)校正螺旋式軌跡，精準(zhǔn)校正迂回式軌跡，精準(zhǔn)校正隨機式軌跡；得到的運動狀態(tài)若是邊界狀態(tài)，則以邊界狀態(tài)下的路徑控制割草機器人的運動，實現(xiàn)了在邊界狀態(tài)下精準(zhǔn)校正邊界轉(zhuǎn)彎角度和優(yōu)化邊界軌跡速度；得到的運動狀態(tài)若是導(dǎo)航狀態(tài)，則以導(dǎo)航狀態(tài)下的路徑控制割草機器人的運動，實現(xiàn)了在導(dǎo)航狀態(tài)下精準(zhǔn)校正導(dǎo)航轉(zhuǎn)彎角度。所述的運動系統(tǒng)控制模塊控制割草機器人按相應(yīng)的命令運動到下一位置。圖2所示，是三個狀態(tài)流程圖。由監(jiān)控感應(yīng)模塊根據(jù)外部環(huán)境判斷割草機器人是否處于割草狀態(tài)。若處于割草狀態(tài)，則利用電子羅盤硬件模塊的方位信號來控制割草機器人沿原來方向繼續(xù)行走。在此過程中判斷是否發(fā)現(xiàn)障礙(包括邊界，標(biāo)志，墻或者樓梯等)，若未發(fā)現(xiàn)障礙，則利用電子羅盤硬件模塊的方位信號來控制割草機器人沿原來方向繼續(xù)行走；若發(fā)現(xiàn)障礙，則后退，利用電子羅盤硬件模塊的方位信號來控制割草機器人相對于原來方向順時針轉(zhuǎn)過一定角度后再判斷障礙能否被發(fā)現(xiàn)。如此反復(fù)，直至完全覆蓋草地。實現(xiàn)了在割草狀態(tài)下精準(zhǔn)校正螺旋式軌跡，精準(zhǔn)校正迂回式軌跡，精準(zhǔn)校正隨機式軌跡。若得到的是回基站狀態(tài)，則判斷有無發(fā)現(xiàn)邊界。若未發(fā)現(xiàn)邊界，利用電子羅盤硬件模塊的方位信號來控制割草機器人沿原來方向繼續(xù)行走；若發(fā)現(xiàn)邊界，則后退，利用電子羅盤硬件模塊的方位信號來控制割草機器人相對于原來方向順時針轉(zhuǎn)過一定角度，再判斷能否發(fā)現(xiàn)邊界。如此反復(fù)來控制割草機器人沿著邊界行走，直至找到導(dǎo)航標(biāo)志。實現(xiàn)了在邊界狀態(tài)下精準(zhǔn)校正邊界轉(zhuǎn)彎角度和優(yōu)化邊界軌跡速度。發(fā)現(xiàn)導(dǎo)航標(biāo)志后，根據(jù)監(jiān)控感應(yīng)模塊來判斷標(biāo)志相對于割草機器人是在其左側(cè)還是右側(cè)。若是左側(cè)，則利用電子羅盤硬件模塊的方位信號來控制割草機器人右轉(zhuǎn)一個角度，再判斷左右側(cè)；若是右側(cè)，則利用電子羅盤硬件模塊的方位信號來控制割草機器人左轉(zhuǎn)一個角度，再判斷左右側(cè)。如此反復(fù)，直至走過導(dǎo)航區(qū)，到達(dá)充電處充電。實現(xiàn)了在導(dǎo)航狀態(tài)下精準(zhǔn)校正導(dǎo)航轉(zhuǎn)彎角度。圖3所示，是基于電子羅盤的系統(tǒng)框圖。其功能的實現(xiàn)主要包括以下三個部分信息獲取、信號調(diào)理及輸出。以類似于霍尼韋爾公司的三軸磁阻傳感器，型號為HMC5883LBY 為例。測得割草機器人所處方位X、Y軸的分量；以重力加速度傳感器，型號為ADXL345為例，測得Z軸的分量。三個數(shù)據(jù)放大濾波后經(jīng)過A/D轉(zhuǎn)換，變?yōu)閿?shù)字信號。將此數(shù)據(jù)通過 I 2 C通訊協(xié)議傳遞給割草機器人的路徑規(guī)劃控制模塊。同時結(jié)合重力加速度傳感器測量得到的俯仰角和翻滾角對X、Y軸的磁場強度作補償校正，傳送給割草機器人的路徑規(guī)劃控制模塊，分析計算得到割草機器人的地理航向角和姿態(tài)角，來控制割草機器人的轉(zhuǎn)彎角度， 實現(xiàn)軌跡的優(yōu)化。圖4所示，是三軸磁阻傳感器引腳及外圍電路圖。本發(fā)明中三軸磁阻傳感器是以類似于霍尼韋爾公司，型號為HMC5883LBY的傳感器芯片為例。其中3，5，6，7，14腳不連接， 4腳與13腳連接供應(yīng)電源，I腳是串行時鐘-I a C總線主/從時鐘，16腳是串行時鐘-I a C總線主/從數(shù)據(jù)，8腳和12腳是置位/復(fù)位連接，11腳接地，10腳是存儲電容器連接，15 腳是數(shù)據(jù)準(zhǔn)備，中斷引腳。圖5所示，是I 2 C通訊電路圖。以型號為AT24C02A的芯片為例。其中1，2，3，7 腳均接地，8腳接電源電壓。6腳是串行時鐘，與HMC5883LBY芯片的I腳連接。5腳是串行數(shù)據(jù)，與HMC5883LBY芯片的16腳連接。
權(quán)利要求
1.基于電子羅盤的割草機器人局部路徑規(guī)劃方法，其特征在于它由路徑規(guī)劃控制模塊，電子羅盤硬件模塊，監(jiān)控感應(yīng)模塊和運動系統(tǒng)控制模塊組成；該方法的步驟如下1)電子羅盤硬件模塊記錄割草機器人當(dāng)前和歷史的方位信息，并轉(zhuǎn)換為方位信號傳送給割草機器人的路徑規(guī)劃控制模塊；2)監(jiān)控感應(yīng)模塊根據(jù)外部環(huán)境來判斷割草機器人當(dāng)前的環(huán)境狀況，并轉(zhuǎn)換為狀態(tài)信號傳送給割草機器人的路徑規(guī)劃控制模塊；3)路徑規(guī)劃控制模塊分析處理接收到的方位信號和狀態(tài)信號，優(yōu)化得到相應(yīng)的運動狀態(tài)及路徑，最后將得到的數(shù)字信號發(fā)送給割草機器人的運動系統(tǒng)控制模塊；4)運動系統(tǒng)控制模塊根據(jù)接收到的數(shù)字信號來控制割草機器人按相應(yīng)的命令運動到下一位置。
2.根據(jù)權(quán)利要求I所述的基于電子羅盤的割草機器人局部路徑規(guī)劃方法，其特征在于所述步驟I)中的電子羅盤硬件模塊包含放大器、濾波器及A/ D轉(zhuǎn)換器；通過電子羅盤測量割草機器人所處位置在磁場中X、Y軸方向上的分量，同時測得Z軸的分量，并對這三個數(shù)值進(jìn)行放大濾波和A/D轉(zhuǎn)換，再通過I2C通訊將信號傳給割草機器人的路徑規(guī)劃控制模塊；同時結(jié)合電子羅盤測量得到的俯仰角和翻滾角對X、Y軸的磁場強度作補償校正，傳送給割草機器人的路徑規(guī)劃控制模塊，分析計算得到割草機器人的地理航向角和姿態(tài)角；它能確定割草機器人當(dāng)前和歷史的方位， 進(jìn)而控制割草機器人下一步的運動方位，來實現(xiàn)軌跡的優(yōu)化。
3.根據(jù)權(quán)利要求I所述的基于電子羅盤的割草機器人局部路徑規(guī)劃方法，其特征在于所述步驟3)中的路徑規(guī)劃控制模塊，是根據(jù)電子羅盤硬件模塊和監(jiān)控感應(yīng)模塊提供的方位信息和環(huán)境狀況來實現(xiàn)路徑規(guī)劃；監(jiān)控感應(yīng)模塊根據(jù)外部環(huán)境判斷割草機器人是否處于割草狀態(tài)；若處于割草狀態(tài)，則利用電子羅盤硬件模塊的方位信號來控制割草機器人沿原來方向繼續(xù)行走；在此過程中判斷是否發(fā)現(xiàn)障礙，若未發(fā)現(xiàn)障礙，則利用電子羅盤硬件模塊的方位信號來控制割草機器人沿原來方向繼續(xù)行走；若發(fā)現(xiàn)障礙，則后退，利用電子羅盤硬件模塊的方位信號來控制割草機器人相對于原來方向順時針轉(zhuǎn)過一定角度后再判斷障礙能否被發(fā)現(xiàn)；如此反復(fù)，直至完全覆蓋草地；實現(xiàn)了在割草狀態(tài)下精準(zhǔn)校正螺旋式軌跡，精準(zhǔn)校正迂回式軌跡，精準(zhǔn)校正隨機式軌跡；若得到的是回基站狀態(tài)，則判斷有無發(fā)現(xiàn)邊界；若未發(fā)現(xiàn)邊界，利用電子羅盤硬件模塊的方位信號來控制割草機器人沿原來方向繼續(xù)行走；若發(fā)現(xiàn)邊界，則后退，利用電子羅盤硬件模塊的方位信號來控制割草機器人相對于原來方向順時針轉(zhuǎn)過一定角度，再判斷能否發(fā)現(xiàn)邊界；如此反復(fù)來控制割草機器人沿著邊界行走，直至找到導(dǎo)航標(biāo)志；實現(xiàn)了在邊界狀態(tài)下精準(zhǔn)校正邊界轉(zhuǎn)彎角度和優(yōu)化邊界軌跡速度；發(fā)現(xiàn)導(dǎo)航標(biāo)志后，根據(jù)監(jiān)控感應(yīng)模塊來判斷標(biāo)志相對于割草機器人是在其左側(cè)還是右側(cè)；若是左側(cè)，則利用電子羅盤硬件模塊的方位信號來控制割草機器人右轉(zhuǎn)一個角度，再判斷左右側(cè)；若是右側(cè)，則利用電子羅盤硬件模塊的方位信號來控制割草機器人左轉(zhuǎn)一個角度，再判斷左右側(cè)；如此反復(fù)，直至走過導(dǎo)航區(qū)，到達(dá)充電處充電；實現(xiàn)了在導(dǎo)航狀態(tài)下精準(zhǔn)校正導(dǎo)航轉(zhuǎn)彎角度。
4.根據(jù)權(quán)利要求I所述的基于電子羅盤的割草機器人局部路徑規(guī)劃方法，其特征在于所述步驟2)中的監(jiān)控感應(yīng)模塊判斷割草機器人當(dāng)前的環(huán)境狀況，根據(jù)環(huán)境狀況得到狀態(tài)信號，確定割草機器人下一步的運動狀態(tài)，總共有三個狀態(tài)割草狀態(tài)，邊界狀態(tài)，導(dǎo)航狀態(tài)。
5.根據(jù)權(quán)利要求I所述的基于電子羅盤的割草機器人局部路徑規(guī)劃方法，其特征在于所述步驟4)中的運動系統(tǒng)控制模塊來控制割草機器人按相應(yīng)的命令運動到下一位置。
全文摘要
本發(fā)明公開了一種基于電子羅盤的割草機器人局部路徑規(guī)劃方法。包括路徑規(guī)劃控制模塊，電子羅盤硬件模塊，監(jiān)控感應(yīng)模塊和運動系統(tǒng)控制模塊。電子羅盤硬件模塊和監(jiān)控感應(yīng)模塊為路徑規(guī)劃控制模塊提供割草機器人當(dāng)前的方位信息和環(huán)境狀況。路徑規(guī)劃控制模塊實現(xiàn)割草狀態(tài)、邊界狀態(tài)、導(dǎo)航狀態(tài)下路徑的精準(zhǔn)校正及軌跡的優(yōu)化，并由運動系統(tǒng)控制模塊來控制割草機器人執(zhí)行相應(yīng)的動作命令。本發(fā)明能實現(xiàn)多種工作方式的轉(zhuǎn)換，精準(zhǔn)控制割草機器人在各個狀態(tài)下的軌跡，解決了以往割草機器人工作時運動路徑失控的問題，用于草坪的自動修剪，能適應(yīng)復(fù)雜地形的作業(yè)，并充分覆蓋整個區(qū)域，減少了電能消耗和提高了割草機器人的工作效率。
文檔編號A01D75/00GK102591342SQ20121002514
公開日2012年7月18日 申請日期2012年2月6日 優(yōu)先權(quán)日2012年2月6日
發(fā)明者朱培培, 江興旺, 趙亮 申請人:浙江大學(xué)