TWI387727B - Traveling path planning method for self-propelled machine and navigation method for avoiding dynamic obstacle - Google Patents

Description

用於自走機器裝置的行走路徑規劃方法與避開動態障礙物的導航方法

本發明係關於一種行走路徑的規劃方法以及導航方法，其特別係關於可用於自走機器裝置(Mobile Robot Device)的行走路徑規劃方法與避開動態障礙物的導航方法。

對自走機器裝置(例如自走機器人)的課題來說，路徑規劃主要是指在有障礙物的工作環境中，尋找一條恰當路徑，亦即從起始點到目的點的運動路徑。在這個路徑中，要使機器人在運動過程中能安全、無碰撞地繞過所有的障礙物。

在習知技術之一中，乃採取遇到新障礙物時，則重新作路徑規劃，而放棄原先的規劃路徑。在另一習知技術中，在起始點到目的點之間定義一條直線來作為規劃路徑，若此直線中有障礙物，即向左轉或向右轉，而繞開障礙物。然而若遇上連續障礙物時(例如牆壁)，就可能無法繞開障礙物，也就無法到達目的點，或者，就算能夠繞開障礙物，而到達目的點，也常會多繞相當多之距離。

本發明之發明人有鑑於相關習知技術仍有改良之處，乃亟思發明而改良出用於自走機器裝置的行走路徑規劃方法與避開動態障礙物的導航方法，可解決上述習知技術的缺失。

本發明之第一目的在提供一種行走路徑規劃方法，可應用在自走機器裝置，使得在一實際場所的自走機器裝置，能夠在起始點與目的點規劃出行走路徑。

本發明之第二目的在提供一種避開動態障礙物的導航方法，可應用在自走機器裝置，使得在一實際場所的自走機器裝置，能夠在起始點與目的點規劃出行走路徑，能夠避開動態障礙物，同時繼續往目的地前進。

為達成本發明上述第一目的，本發明提供如申請專利範圍第1項所述之一種用於自走機器裝置的行走路徑規劃方法。

為達成本發明上述第二目的，本發明提供如申請專利範圍第7項所述之一種用於自走機器裝置的避開動態障礙物的導航方法。

關於本發明之優點與精神可以藉由以下的發明詳述及所附圖式得到進一步的瞭解。

第一圖係顯示本發明用於自走機器裝置的行走路徑規劃方法的流程圖，以及第二A圖至第二D圖係顯示依據本發明行走路徑規劃方法對一實體場所的實施示意圖。本發明用於自走機器裝置的行走路徑規劃方法10乃包括步驟101與步驟103。步驟101係基於一個實體場所5產生相對的網狀格狀圖20，網狀格狀圖20乃包含帶有障礙參數值的複數個地圖網格(Map grid)201，其中每個地圖網格201的該障礙參數值係取決於在相對位置上實體場所5中的障礙物的位置。

請參見第二A圖，為了使得自走機器裝置(未描繪)(例如機器人)可依據行走路徑的規劃結果在實體場所5(例如工廠)中行走(Moving)，實體場所5乃包含障礙物12a~12b。首先讀入實體場所5的平面地圖，並基於實體場所5產生相對的網狀格狀圖20。在網狀格狀圖20中，主要包含分別帶有障礙參數值的複數個地圖網格201。每個地圖網格201的大小例如可採行為1cm*1cm。

為了在完成路徑規劃後，能提供關於行走路徑的座標資訊，在網狀格狀圖20中的複數個地圖網格201可付予一個識別編號。

在實體場所5所出現的障礙物12a~12b，乃必須在網狀格狀圖20中標註障礙物12a~12b所佔據的位置。每個地圖網格201的障礙參數值取決於在相對位置上實體場所5中的障礙物12a~12b的位置。如第二B圖中所標示斜線的區塊16，屬於區塊16的該些地圖網格201的障礙參數值均被設成例如1。而其它無被佔據的地圖網格201的障礙參數值則被設成例如0。此外，為了避免自走機器裝置會行走到網狀格狀圖20的外部，網狀格狀圖20的邊緣可被定義成連續障礙物，而該些地圖網格201的障礙參數值均被設成1。

步驟103係開始自起始點14a與目的點14b的地圖網格201向鄰近地圖網格201持續擴展，直到所擴展的地圖網格201彼此相遇，並定義能彼此相遇到的擴展軌跡中每個地圖網格201為行走路徑。

請參見第二C圖，基於上述步驟101的前處理結果，開始自起始點14a與目的點14b的地圖網格201分別向鄰近地圖網格持續擴展，直到所擴展的地圖網格201彼此相遇，並定義能彼此相遇到的擴展軌跡中每個地圖網格201為行走路徑18。換言之，這就好像是對水池扔下兩顆石頭，所形成的水波逐漸擴大，最終會相遇。舉例來說，在自起始點14a與目的點14b的地圖網格201向鄰近地圖網格201持續擴展時，可從起始點14a的地圖網格201與目的點14b的地圖網格201，採以十字狀(即為上、下、左、右)向鄰近地圖網格201持續擴展，被擴展的該些地圖網格201再向上、下、左、右的四個鄰近地圖網格201持續擴展，直到由起始點14a與目的點14b所擴展的地圖網格201彼此相遇，即相遇點18a所在位置。

然後，從相遇點18a所在位置分成兩個方向，分別逆 溯回起始點14a與目的點14b，而形成從相遇點18a分別逆溯回至起始點14a與目的點14b的擴展軌跡(即其所經過的地圖網格201)，並以這兩條軌跡定義成行走路徑18，請參見第二D圖。

除了採以十字狀向鄰近地圖網格201持續擴展以外，亦可以改採米字狀向八個鄰近地圖網格201持續擴展。

由於自走機器裝置佔有一定投影面積(例如：40cm * 40cm)，需依據自走機器裝置的大小，使網狀格狀圖5中被障礙物12a~12b所佔據的地圖網格，再相對地向外調整被佔據的地圖網格201。舉例來說，由自走機器裝置中心點計算，往外擴張20cm，並可再加上保護範圍10cm，故總共需向外擴張30cm，也就是說，被障礙物12a、12b所佔據的地圖網格，再相對地向外調整30cm，如此再調整增加被佔據的地圖網格201。同理，該些位於網狀格狀圖20邊緣的該些地圖網格201，再相對地向外調整30cm，如此再調整增加被佔據的地圖網格201。

理想上，規劃的行走路徑18已能避開既有障礙，但是為了能夠避開動態障礙物(即不屬於實體場所5中原先具有的障礙物12a、12b)，可在自走機器裝置上設置有偵測器(Sensor)，例如紅外線偵測器或超音波偵測器，以便隨時偵測在行走過程中所遇到的動態障礙物。

第三圖顯示本發明用於自走機器裝置的避開動態障礙物的導航方法的流程圖，以及第四A圖至第四C圖係顯示自走機器裝置依據本發明導航方法，在實體場所行走的路徑示意圖。本發明用於自走機器裝置的避開動態障礙物的導航(Navigating)方法30乃包括行走路徑規劃步驟31與避開動態障礙物的行走(moving)步驟33。行走路徑規劃步驟31的兩個步驟311、313係分別相同於上述步驟101、103，因此不再重述。

請參見第四A圖，如果行走路徑18中並無任何的動態障礙物，依據步驟333以及步驟335進行行走的自走機器 裝置，乃會行走通過識別編號201a、201b、201c、201d、201e、201f、201g、201n、201o、201p的該些地圖網格201，最後抵達目的點14b。

在避開動態障礙物的行走步驟33的步驟331中，依據行走路徑規劃步驟31的行走路徑18行走，當要進入中途位置之前時，偵測到不屬於實體場所中原先的障礙物，然後將該障礙物定義成動態障礙物22。請參見第四B圖，在行走路徑18的中途位置，該中途位置的所對應的地圖網格201其識別編號為201d，乃有一個障礙物。自走機器裝置依據行走路徑規劃步驟31的行走路徑18行走，行走通過識別編號為201a~201c的該些地圖網格201。當自走機器裝置將要行走到這個中途位置之前，亦即在識別編號為201c的地圖網格201乃會偵測到障礙物，然後將這障礙物定義成動態障礙物22。

接著，在避開動態障礙物的行走步驟33的步驟333中，依據該中途位置的下個轉彎位置的轉彎方向，在該中途位置依照該轉彎方向進行轉彎，轉彎後，接著如果剛才轉彎方向係向左轉彎，則沿著右手側(right-hand)的動態障礙物22繼續行走；如果剛才轉彎方向係向右轉彎，則沿著左手側(left-hand)的動態障礙物22行走。

請參見第四B圖與第五圖，就該中途位置而言，該中途位置的下個轉彎位置即為識別編號為201g的地圖網格201。在識別編號為201g的地圖網格201的轉彎方向係向右轉。自走機器裝置依據下個轉彎位置的轉彎方向，在識別編號為201c的地圖網格201向右轉。右轉後，接著，自走機器裝置判斷如果剛才轉彎方向係向左轉彎，則沿著右手側(right-hand)的動態障礙物22繼續行走；如果剛才轉彎方向判斷係向右轉彎，則沿著左手側(left-hand)的動態障礙物22行走。

承續第四B圖的範例說明，自走機器裝置在識別編號為201c的地圖網格201向右轉。右轉後，接著，自走機器 裝置判斷剛才轉彎方向係向右轉彎，則沿著左手側(left-hand)的動態障礙物22行走，再行走通過識別編號201h、201i、201j、201e的該些地圖網格201。

接著，在避開動態障礙物的行走步驟33的步驟335中，如果在前方再偵測到另一個新動態障礙物時，而無法繼續沿著動態障礙物22側邊行走，則在該偵測到另一個新動態障礙物的位置向後轉彎，向後轉彎後，接著則沿著另一隻手(the-other-hand)的該動態障礙物繼續行走。

請參見第四C圖與第五圖，當偵測到動態障礙物22後，由於下一轉彎點為右轉的關係，當時的自走機器裝置會依據下一轉彎點而右轉，並沿著動態障礙物22之左牆行走(即自走機器裝置左手側)，若動態障礙物22佔據的地圖網格多達使自走機器裝置還必需要持續延著左牆走時，例如第四C圖的動態障礙物22佔據了地圖網格201d、201i，另一個動態障礙物22’若被偵測到在自走機器裝置的前方，即第四C圖之地圖網格201q已被動態障礙物22’佔據，也就是說，自走機器裝置沿左牆走時仍遭遇到阻礙，在此種情形下，自走機器裝置向後轉彎，掉頭轉向。

此時，自走機器裝置行在識別編號201h的地圖網格201向後轉(即180°轉)，向後轉彎後，接著則沿著另一隻手(the-other-hand)側的動態障礙物22繼續行走，此時的另一隻手側係為右手側。沿著右手側的動態障礙物22繼續行走的自走機器裝置，會行走通過識別編號201c、201k、201l、201m、201e的該些地圖網格201。

接著，在避開動態障礙物的行走步驟33的步驟337中，在步驟333以及步驟335的行走時，自走機器裝置判斷是否已走到行走路徑18上，如果為真，則再依行走路徑18繼續行走。

依據步驟333以及步驟335進行行走的自走機器裝置，乃會到達識別編號201e的地圖網格，當到達識別編號201e的地圖網格201時，自走機器裝置判斷出已走到行走 路徑18上。

接著，在避開動態障礙物的行走步驟33的步驟339中，重覆步驟331至步驟337，一直到行走至目的點為止。通過避開動態障礙物22、22’的自走機器裝置，乃再依據步驟331至步驟337的行走方式，將會行走通過識別編號201f、201g、201n、201o、201p的該些地圖網格201，最後抵達目的點14b。

第六圖顯示應用本發明方法的自走機器裝置的架構示意圖。應用本發明方法的自走機器裝置的硬體構成是可以採用習知技藝。本發明的行走路徑規劃方法10以及導航方法30可以編寫(Programming)成程式碼(Program codes)，而該程式碼是燒錄於快閃記憶體403。至少一個以上的偵測器405是用來偵測障礙物。中央處理器401執行該程式碼，並且接收該些偵測器405所產生的信號。行走機構單元409是用來至少使得自走機器裝置能夠行走前進、轉彎、以及停止前進，且行走機構單元409是受控於中央處理器401。基於實體場所5所產生相對的網狀格狀圖20的數位資料是儲存於記憶體407，記憶體407可以採用依電記憶體或非依電記憶體。

綜上所述，本發明行走路徑規劃方法以及避開動態障礙物的導航方法，可應用在自走機器裝置，只要對自走機器裝置輸入對應於實體場所的數位地圖資料後，自走機器裝置就能夠在起始點與目的點規劃出行走路徑，同時依據導航方法，來快速地閃避動態障礙物，同時繼續往目的地前進，此即為本發明效益所在。

藉由以上較佳具體實施例之詳述，係希望能更加清楚描述本發明之特徵與精神，而並非以上述所揭露的較佳具體實施例來對本發明之範疇加以限制。相反地，其目的是希望能涵蓋各種改變及具相等性的安排於本發明所欲申請之專利範圍的範疇內。

5‧‧‧實體場所

10‧‧‧行走路徑規劃方法

20‧‧‧網狀格狀圖

12a~12b‧‧‧障礙物

14a‧‧‧起始點

14b‧‧‧目的點

16‧‧‧區塊

18‧‧‧行走路徑

22、22’‧‧‧動態障礙物

30‧‧‧導航方法

31‧‧‧行走路徑規劃步驟

33‧‧‧避開動態障礙物的行走步驟

101~103‧‧‧步驟

201a~201q‧‧‧地圖網格

311~313‧‧‧步驟

331~339‧‧‧步驟

401‧‧‧中央處理器

403‧‧‧快閃記憶體

405‧‧‧偵測器

407‧‧‧記憶體

409‧‧‧行走機構單元

第一圖係顯示本發明用於自走機器裝置的行走路徑規劃方法的流程圖。

第二A圖至第二D圖係顯示依據本發明行走路徑規劃方法對一實體場所的實施示意圖。

第三圖顯示本發明用於自走機器裝置的避開動態障礙物的導航方法的流程圖。

第四A圖至第四C圖係顯示自走機器裝置依據本發明導航方法，在實體場所行走的路徑示意圖。

第五圖顯示本發明偵測到動態障礙物時，如何避開動態障礙物的流程圖。

第六圖顯示應用本發明方法的自走機器裝置的架構示意圖。

30‧‧‧導航方法

31‧‧‧行走路徑規劃步驟

33‧‧‧避開動態障礙物的行走步驟

311~313‧‧‧步驟

331~339‧‧‧步驟

Claims (7)

1. 一種用於自走機器裝置的行走路徑規劃方法，係用來提供一自走機器裝置在一實體場所的行走路徑，包括下列步驟：基於該實體場所產生相對的一網狀格狀圖，該網狀格狀圖包含帶有障礙參數值的複數個地圖網格，其中每個地圖網格的該障礙參數值係取決於在相對位置上該實體場所中的一障礙物的位置；以及開始自一起始點與一目的點的地圖網格向鄰近地圖網格持續擴展，直到所擴展的地圖網格彼此相遇，並定義能彼此相遇到的擴展軌跡中每個地圖網格為該行走路徑。
2. 如申請專利範圍第1項所述之用於自走機器裝置的行走路徑規劃方法，其中該網狀格狀圖的邊緣被定義成連續障礙物。
3. 如申請專利範圍第1項所述之用於自走機器裝置的行走路徑規劃方法，其中該網狀格狀圖中的複數個地圖網格，係進一步包含一識別編號。
4. 如申請專利範圍第1項所述之用於自走機器裝置的行走路徑規劃方法，其中該開始自於該起始點與該目的點的地圖網格向鄰近地圖網格持續擴展的步驟，係為可自該起始點與該目的點的地圖網格以十字狀向鄰近地圖網格持續擴展。
5. 如申請專利範圍第1項所述之用於自走機器裝置的行走路徑規劃方法，其中該開始自於該起始點 與該目的點的地圖網格向鄰近地圖網格持續擴展的步驟，係為可自該起始點與該目的點的地圖網格以米字狀向鄰近地圖網格持續擴展。
6. 如申請專利範圍第1項所述之用於自走機器裝置的行走路徑規劃方法，係進一步取決於依據該自走機器的大小，使該網狀格狀圖中被該障礙物所佔據的地圖網格，再相對地向外調整被佔據的地圖網格，以及設定被調整為佔據的該些地圖網格的障礙參數值。
7. 一種用於自走機器裝置的避開動態障礙物的導航方法，包括下列步驟：行走路徑規劃步驟：基於一實體場所產生相對的一網狀格狀圖，該網狀格狀圖包含帶有障礙參數值的複數個地圖網格，其中每個地圖網格的該障礙參數值係取決於在相對位置上該實體場所中的一障礙物的位置；開始自一起始點與一目的點的地圖網格向鄰近地圖網格持續擴展，直到所擴展的地圖網格彼此相遇，並定義能彼此相遇到的擴展軌跡中每個地圖網格為該行走路徑；避開動態障礙物的行走(moving)步驟：(a).依據該行走路徑規劃步驟的該行走路徑行走，當要進入一中途位置之前時，偵測到不屬於該實體場所中原先具有的該障礙物時，將該障礙物定義成一動態障礙物； (b).依據該中途位置的下個轉彎位置的轉彎方向，在偵測到該動態障礙物的位置處依照該轉彎方向進行轉彎，轉彎後，接著如果剛才轉彎方向係向左轉彎，則沿著右手側(right-hand)的該動態障礙物繼續行走，如果剛才轉彎方向係向右轉彎，則沿著左手側(left-hand)的該動態障礙物行走；(c).如果在前方再偵測到另一個新動態障礙物時，而無法繼續沿著該動態障礙物側邊行走，則在該偵測到另一個新動態障礙物的位置向後轉彎，向後轉彎後，接著則沿著另一隻手(the-other-hand)的該動態障礙物繼續行走；(d).在步驟(b)以及(c)的行走時，判斷是否已走到該行走路徑上，如果為真，則再依該行走路徑繼續行走；(e).重覆步驟(a)至步驟(d)，一直到行走至該目的點為止。