國
立
交
通
大
學
機械工程學系
博 士 論 文
針對動態路徑規劃之 D++演算法研究
及其應用
The Research and Application of D++
Algorithm for Dynamic Path-Planning
研 究 生：陳品均
指導教授：鄭璧瑩 博士
針對動態路徑規劃之 D++演算法研究及其應用
The Research and Application of D++ Algorithm for
Dynamic Path-Planning
研 究 生： 陳品均 Student： Pin-Jyun Chen 指導教授： 鄭璧瑩 博士 Advisor： Dr. Pi-Ying Cheng
國 立 交 通 大 學 機 械 工 程 學 系
博 士 論 文
A Dissertation
Submitted to Department of Mechanical Engineering
College of Engineering
National Chiao Tung University
in partial Fulfillment of the Requirements
for the Degree of
PhD
in
Mechanical Engineering
June 2013
i
針對動態路徑規劃之 D++演算法研究及其應用
研 究 生 ： 陳 品 均 指 導 教 授 ： 鄭 璧 瑩 博 士 國立交通大學機械工程學系摘要
移動機器人的導航科技是機器人科學中一項重要的技術。在過去數 十年間，移動機器人的導航技術吸引了許多學界與產業界的注意，並發 展出許多的技術。在已知或未知的環境所進行的主要導航科技，包括了 探索、環境建置、定位、運動控制、路徑規劃。路徑規劃技術為引導機 器人從一個起點走到它的目標點。現今的主要路徑規劃大致分為全域路 徑規劃法，與區域路徑規劃法。全域路徑規劃法的優點為可找出最短路 徑，然而計算量龐大且耗時久，故大多用於靜態環境。區域路徑規劃法 的優點為可快速更新環境資訊並找出可行路徑，故適用於動態環境。然 而區域路徑規劃法的缺點為不能保證找到最短路徑，尤其是處理類似迷 宮的環境，非常容易卡在某特定區域(區域解)而無法找到終點。 在本研究中，我們改良舊有的 Dijkstra 演算法，並發展成一種新的演 算法：D++演算法；並且應用它在移動機器人的導航科技中。我們結合了 Dijkstra 演算法與環境感測方法，讓原本屬於全域搜尋的 Dijkstra 演算法 轉變區域搜尋的 D++演算法，因此可讓機器人能即時決定下一步該怎麼 移動。雖然 D++演算法大部分時間為在有限的區域進行區域搜索，但它 也可以在需要的時候擴大搜索範圍以避開區域解的狀況。因此 D++演算 法綜合了全域搜尋與區域搜尋的優點，不斷可快速反應處理動態環境問 題，即使面對迷宮的環境，也可輕易解決區域解問題而成功找到終點。ii 而在本論文後面的章節，我們也應用 D++演算法在實際的移動機器 人上，並使其航行在數個測試環境中。由實驗結果我們驗證了 D++演算 法可有效地實現在真實的移動機器人上。因此我們可以期望未來可應用 D++演算法於野外型探勘機器人，以應付複雜、未知、動態、廣大的真實 環境。 關鍵字：機器人；路徑規劃；Dijkstra 演算法
iii
The Research and Application of D++ Algorithm for Dynamic
Path-Planning
Student：Cheng, Kuei-Jen Advisors：Dr. Cheng, Pi-Ying Department of Mechanical Engineering
National Chiao Tung University
ABSTRACT
The navigation of mobile robots is a vital aspect of technology in robotics. During the past few decades, the navigation of mobile robots has attracted notable attention, and considerable research was developed. The main technology of navigation in unknown or uncertain environments includes exploration, mapping, localization, motion control, and path-planning. Path planning is one of the main technologies to direct a robot from a starting point to its destination. The main methods of path planning at present can be classified by global search type and local search type. The advantage of global search path-planning is that it can find a shortest path. However, it need plenty of computation and takes a long time. Therefore it often only use static environment. The advantage of local search path-planning is that it can find probable moving direction very fast. Therefore, it is very suitable for dynamic environment. However, the shortcoming of local search path-planning is that it can not ensure to find a shortest path, especially for some environments of maze. It is very easily to get stuck at some area which is a local solution so that it can not a path to destination eventually.
In this paper, we applied the D++ algorithm, which is a novel and improved path-planning algorithm, to the navigation of mobile robots. The D++ algorithm combines Dijkstra’s algorithm with the idea of a sensor-based
iv
method, such that Dijkstra’s algorithm is adapted to local search, and the robot can determine its next move in real-time. Although the D++ algorithm frequently runs local search with limited ranges, it can compute optimum paths by expanding the size of the searching range to avoid local minima. Therefore D++ algorithm combines the advantages of global seach and local search. It not only can deal with dynamic environment rapidly, but also avoid to the problem of local solution for the environment of maze.
In the later chapters, we applied the D++ algorithm to a real mobile robot in a number of environments. According to the results of experiments we verified that the D++ algorithm is very practicability for real mobile robot. Therefore we can expect that use of the D++ algorithm enables robots to navigate efficiently in complex, unknown, dynamic and large environments.
v
誌謝
在博士班求學期間，首先感謝指導教授鄭璧瑩博士這些年來的指導 與教誨，使我能如期順利地完成博士論文，並學習到許多寶貴的知識與 經驗。另外感謝口試委員徐瑞坤教授、吳隆庸教授、歐耿良院長、陳昭 亮教授、曾釋鋒博士的許多重要意見，使得本論文的內容更加地充實、 完備。 接著要感謝在求學期間，在工作上非常照顧我的長官同仁們，雷晟 科技的唐沛澤總經理、程建智經理，金屬中心的黃昆明處長、吳春甫副 處長、魏江銘組長，與其他曾一起共事的夥伴們。讓我可以在工作閒暇 之餘，順利完成我的博士學位。 最後，論文能順利完成還要感謝我的父親陳正宏、母親徐翠蓮、姐 姐陳萱蓉、與女友官愛蓮，在這段時間給予的支持與鼓勵，讓我能夠無 後顧之憂的完成博士學業。以上我所感謝的人，對於你們的幫助，紙短 情長，無法表達我的感激之情於萬一。僅將本論文的成果與榮耀與你們 分享。vi
目錄
頁次中文摘要 ... I
英文摘要 ... III
誌謝 ... V
目錄 ... VI
表目錄 ... VIII
圖目錄 ... IX
一、緒論 ... 1
1.1 研究背景 ... 1
1.2 文獻回顧 ... 2
1.3 研究目的 ... 7
1.4 論文架構 ... 8
二、D++演算法 ... 9
2.1 Dijkstra 演算法 ... 9
2.2 D++演算法之理論 ... 15
2.3 區域解問題 ... 22
2.4 與其他演算法的比較... 25
2.5 偵測範圍的最佳化設定 ... 35
2.6 機器手臂之模擬範例... 37
2.6.1 模擬條件 ... 38
vii
2.6.2 模擬結果 ... 40
三、D++演算法與移動機器人之整合實作 ... 43
3.1 系統架構 ... 43
3.2 Arduino 控制開發板 ... 46
3.3 運動控制用之硬體 ... 50
3.3.1 自製編碼器 ... 50
3.3.2 雙輪同動控制 ... 53
3.3.3 直流馬達驅動板 ... 58
3.3.4 電子陀螺儀 ... 62
3.4 紅外線感測器 ... 65
3.5 無線電通訊模組 ... 68
四、D++演算法應用於移動機器人之實驗結果 ... 72
4.1 靜態環境 ... 72
4.2 動態環境 ... 76
五、結論與建議 ... 78
5.1 結論 ... 78
5.2 建議 ... 79
參考文獻 ... 80
附錄一、D++演算法程式碼 ... 86
附錄二、ARDUINO 開發板程式碼 ... 93
viii
表目錄
頁次表 1、本研究實驗之電腦硬體規格 ... 22
表 2、路經規劃結果比較表 ... 32
表 3、各路經規劃法的優缺點比較 ... 34
表 4、L298P 接腳功能表 ... 59
表 5、L298P 真值表 ... 60
表 6、陀螺儀 ADXRS613 的規格特性（來源：[61]） ... 64
表 7、APC220 無線電通訊模組接腳定義 ... 69
表 8、APC220 無線電通訊模組的參數設置表 ... 70
ix
圖目錄
頁次
圖一、Dijkstra 演算法與 A*演算法之比較圖：(a) Dijkstra 演算法
(b) A*演算法 (來源：Amit's A* Pages[6]) ... 2
圖二、D*演算法：(a)直接搜索 (b)先使用粗略地圖進行搜索 (來
源：[7]) ... 3
圖三、人工位能法之範例(來源：[17]) ... 4
圖四、蟻群演算法之範例（來源： [26]） ... 5
圖五、模糊演算法之範例(來源：[40]) ... 6
圖六、類神經網路系統之範例(來源：[37]) ... 6
圖七、使用 Dijkstra 演算法的 3×3 地圖 ... 11
圖八、Dijkstra 演算法流程圖 ... 13
圖九、Dijkstra 演算法無障礙範例 ... 14
圖十、Dijkstra 演算法具障礙物範例 ... 14
圖十一、D++演算法之簡易流程圖 ... 16
圖十二、本研究所用到之圖例說明 ... 16
圖十三、D++演算法主要流程圖 ... 21
圖十四、偵測範圍為 5 格，總花費時間約 4 秒 ... 23
圖十五、偵測範圍為 20 格，總花費時間約 3 秒 ... 23
圖十六、當表 Select_List 中無節點時，D++演算法的行為模式
... 24
x
圖十七、靜態環境中使用 D++演算法之路徑規劃結果(探訪節點
數:2626, 路徑長度:151) ... 25
圖十八、靜態環境中使用 Dijkstra 演算法之路徑規劃結果(探訪
節點數:6286, 路徑長度:122) ... 26
圖十九、靜態環境中使用 A*演算法之路徑規劃結果(探訪節點
數:6209, 路徑長度:122) ... 26
圖二十、靜態環境中使用 D*演算法之路徑規劃結果(探訪節點數:
6209, 路徑長度:122) ... 27
圖二十一、靜態環境中使用人工位能之路徑規劃結果(未能抵達
終點) ... 27
圖二十二、有九個隨機移動物體的動態環境 ... 28
圖二十三、動態環境以 D++演算法之路徑規劃結果 (偵測範圍為
10 格) ... 29
圖二十四、動態環境以 D*演算法之路徑規劃結果 ... 30
圖二十五、動態環境以人工位能法之路徑規劃結果 ... 31
圖二十六、一個 100×100 格、無障礙物的空白地圖 ... 36
圖二十七、使用不同偵測範圍的模擬實驗之路徑規劃時間統計
圖 ... 36
圖二十八、具兩軸之舉重機器手臂 ... 38
圖二十九、本章節舉重機器手臂軌跡規劃目標 ... 39
圖三十、於卡氏座標系統的模擬結果比較(a) 負重為 0kg 之結果
(b) 負重為 4kg 之結果 ... 41
xi
圖三十一、機器手臂舉起負重 0kg 的模擬情形(10 毫秒的循環週
期，且每軸之解析角度為 1 度) ... 41
圖三十二、機器手臂舉起負重 4kg 的模擬情形(10 毫秒的循環週
期，且每軸之解析角度為 1 度) ... 42
圖三十三、具兩個驅動輪(左右)與一個萬向輪(前)之移動機器人
... 44
圖三十四、移動機器人之系統架構圖 ... 44
圖三十五、機器人控制流程 ... 45
圖三十六、Arduino 軟體開發除錯環境(來源：[56]) ... 47
圖三十七、Arduino UNO 控制板(來源：[56]) ... 48
圖三十八、本研究所使用之自製光學編碼器 ... 50
圖三十九、本研究之自製編碼盤尺寸圖 ... 51
圖四十、HD74LS14 之內部結構圖（來源：[57]） ... 52
圖四十一、使用史密特觸發反相器穩定光遮斷器之訊號（來源：
[58]） ... 52
圖四十二、本研究移動機器人之運動控制邏輯(a)前進(b)後退(c)
左轉(d)右轉 ... 53
圖四十三、本研究雙輪同動 PID 控制方塊圖 ... 55
圖四十四、無使用 PID 同步控制之馬達運轉結果 ... 57
圖四十五、使用 PID 同步控制之馬達運轉結果 ... 57
圖四十六、具 L298P 之直流馬達驅動板(來源：[59]) ... 58
圖四十七、L298P 外觀與尺寸（來源：[60]） ... 59
xii
圖四十八、不同 PWM 輸入產生的等效電壓輸入（來源：[56]）
... 61
圖四十九、陀螺儀 ADXRS613 之外觀圖（來源：[61]） ... 62
圖五十、陀螺儀 ADXRS613 之內部方塊圖（來源：[61]） ... 62
圖五十一、ADXRS613 所量測角速度的方向與輸出電壓大小參
考圖（來源：[61]） ... 63
圖五十二、梯形軟體積分法示意圖 ... 64
圖五十三、紅外線感測器於本研究移動機器人平台之配置 ... 65
圖五十四、紅外線感測器之原理圖 ... 66
圖五十五、GP2D120 之輸出電壓與量測距離關係圖（來源：[62]）
... 67
圖五十六、APC220 無線電通訊模組實體圖（來源：[59]） .... 68
圖五十七、APC220 無線電通訊模組尺寸圖（來源：[59]） .... 68
圖五十八、一個迷宮地形的靜態環境 ... 73
圖五十九、移動機器人航行在圖五十七之地形環境的實際情形
（偵測範圍為 3 格） ... 73
圖六十、使用偵測範圍為 3 格的機器人航行實驗監視畫面 ... 74
圖六十一、使用偵測範圍為 20 格的機器人航行實驗監視畫面
... 75
圖六十二、使用偵測範圍為 20 格、且已知環境資訊的機器人航
行實驗監視畫面 ... 75
圖六十三、一個動態環境試驗(障礙物由上至下移動) ... 76
xiii
圖六十四、移動機器人航行在動態環境的實際情形（偵測範圍
為 5 格） ... 77
圖六十五、使用偵測範圍為 5 格於動態環境的機器人航行實驗
1
一、緒論
1.1 研究背景 在過去的二十年中，由於人工智慧理論[1]與電腦硬體科技的快速進 步，以往僅存在於電影小說的機器人科技，受到了越來越多研究人員的 注意與研究，並且廣泛應用於工業製造、太空探索與軍事領域上。例如 移動機器人可以替代人們探索未知與危險的環境、或是變成自動駕駛的 汽車…等等。其中路徑規劃技術為移動機器人科技中，一項重要且關鍵 的技術。另外除了工業、航太與軍事領域外，路徑規劃技術已成功地應 用到一些日常生活產品，例如電玩遊戲、導航軟體、吸塵機器人等。 根據所需處理環境資訊的差異，路徑規劃主要可分為兩種類型：（1） 全域路徑規劃：是指所有的環境資訊是已先行透過偵查或感測獲得，並 且之後也不會再變動的，一般稱之為「靜態環境」。因此，機器人可輕易 地透過全域路徑規劃方法，從起點找到一條最短或最佳路徑到終點。（2） 區域路徑規劃：是指環境資訊是未知、或部分已知；有關障礙物的形狀、 大小和位置，必須透過事後偵查或感測獲得，並且之後也許會再變動的， 一般則稱為「動態環境」。在動態環境中，機器人必須不斷地觀察目前環 境，以取得有限的資訊，並立即作出決策。因此，區域路徑規劃法的環 境資訊可以不斷改變。目前，產、學、研界在全域路徑規劃已獲得許多 成果，並發展了許多有效的方法；然而目前全域路徑規劃大部分僅應用 於電玩遊戲或導航軟體等非實際環境之應用，對於實際的移動機器人或 自動車而言，則難以使用全域路徑規劃於瞬息萬變的實際動態環境中。 故難度較高的動態環境問題，則成為近年來越來越受到重視的研究方向。2
1.2 文獻回顧
一般來說，對於靜態環境問題，路徑規劃是屬於一次性演算法，可 找到最短或最佳路徑。最有名的方法是 Dijkstra 演算法[2]與其衍生出來 的 A*算法[3]-[5]。Dijkstra 演算法為 1959 年由 Edsger Dijkstra 所提出，為 一種圖形式搜索法。該演算法可以找到最短路徑或最佳路徑，然而 Dijkstra 演算法會探索空間中非常大量的節點，所以 Dijkstra 演算法一般被認為效 率不佳。1972 年，Peter Hart 提出了 A*演算法，採用了啟發式預估的設 計，所以會優先處理空間中較有探索價值的節點，而忽略一些明顯不合 適的節點。因此，A*演算法可以比 Dijkstra 演算法更快速地找到路徑， 並且獲得與使用 Dijkstra 演算法所得到的路徑，非常接近的結果(如圖一)。 然而對於動態環境而言，像 Dijkstra 與 A*這種一次性搜尋法，是相 當沒有效率的。因為當環境發生變化時（例如，出現新的障礙物），Dijkstra 或 A*演算法必須從目前位置重新搜索到終點之路徑。一旦環境變化是持 續不斷的，且目前位置離終點仍有很長的距離時，使用 Dijkstra 或 A*演 算法將會花費大量的搜索時間，而顯得非常沒有效率。 (a) (b)
圖一、Dijkstra 演算法與 A*演算法之比較圖：(a) Dijkstra 演算法 (b) A* 演算法 (來源：Amit's A* Pages[6])
3
因此，Anthony Stentz 於 1994 年提出了 D*演算法[7][8]。主要為了解 決 Dijkstra 和 A*演算法不能處理動態環境的問題。2005 年，Sven Koenig 提出了 D*-Lite 演算法[9]。D*-Lite 演算法比傳統 D *算法更容易被理解， 並且擁有更好的效率。其實 D*-Lite 演算法並非由 D *算法發展而來，而 是由同樣 Sven Koenig 提出的 LPA*演算法[10] 發展而來。然而，不論是 D*或是 D*-Lite 演算法，它們還是必須一開始做一次全域路徑規劃，所 以在第一次搜尋結束前，仍然會有等待時間，特別是針對大型地圖的環 境。所以使用 D*演算法於實際大型地圖時，往往要先使用粗略地圖做初 期全域路徑規劃(如圖二所示)。 (a) (b) 圖二、D*演算法：(a)直接搜索 (b)先使用粗略地圖進行搜索 (來源：[7]) 在過去 20 年，由 Khatib [16]提出的人工位能法，已被廣泛用於移動 機器人或機械手臂的路徑規劃問題。起初，人工位能法是設計給機械手 臂，以避免碰撞障礙物，並抓取目標物體。但後來研究人員發現，人工 位能法也非常適合處理移動機器人的路徑規劃問題，並且可以建立一條
4 非常平滑的軌跡路徑。雖然人工位能法的理論很簡單，可以即時地處理 動態環境問題；但是，人工位能法的缺點也非常顯而易見。例如，最大 的缺點就是區域解問題。這是因為當終點產生的引力與障礙物產生的斥 力之合力為零時，機器人將停住而無法繼續前進。雖然有許多方法來解 決這個問題，例如，添加一個隨機擾動力量，讓移動機器人離開區域解； 或是結合其他路徑搜尋方法[20][21]，幫助機器人離開區域解之地區。這 些方法或多或少可降低機器人產生區域解的問題。但當障礙物的尺寸與 形狀非常龐大與複雜時，這些方法便顯得十分沒有效率。除此之外，人 工位能法還有其他問題，例如在狹窄的通道會震盪、或是無法通過更窄 的通道。在參考文獻[18][19]有更詳細的描述。 圖三、人工位能法之範例(來源：[17]) 另外還有如蟻群演算法[22]-[25]等最佳化方法，也是近年來常被來應 用在路徑規劃上的一種方法。它由 Marco Dorigo 於 1992 年提出，其靈感 來源於螞蟻在尋找食物過程中發現路徑的行為。自然界螞蟻出外覓食 時，會在行經的路徑上留下一種叫費洛蒙 (Pheromone)的物質，以利後續 出發的螞蟻決定是否依循此路徑前進。覓食初期蟻群會漫無目的地隨機 四處移動，並留下費洛蒙。路徑愈短，螞蟻會較快地往返巢穴與覓食處， 所留下費洛蒙濃度愈高。則較短的路徑將會吸引更多螞蟻循此路覓食，
5 最後所有螞蟻均循該路線覓食（如圖四所示）。不過蟻群演算法需要較 多的計算時間來求得路徑解，且也未能如 Dijkstra 演算法等全域搜尋法能 保證求得最短路徑。故蟻群演算法並不普遍應用在機器人的路徑規劃問 題上。 圖四、蟻群演算法之範例（來源： [26]） 其他人工智慧方法[27]，如遺傳算法[28]-[32]、類神經網路系統[33]- [38]，模糊演算法[39][40]...等等。這些方法大部分著重於機器人面對當前 障礙物的因應對策，因此對於解決動態環境的即時避碰問題，有相當不 錯的效率與成果。然而，這些方法大多未對整個環境做實際的路境規劃， 所以當碰到比較複雜的地形（如迷宮式的地圖），這些方法往往產生不盡 理想的結果，甚至難以求得路徑解。例如圖五為使用模糊遺傳演算法導 引機器人避開障礙物，而這些障礙物均為簡單圓柱體，因此機器人只要 繞過障礙物並繼續往終點方向前進即可，並無區域解的問題，而圖六為 使用類神經網路系統，控制機器人追逐目標物，此類問題也並未實際執 行路徑規劃的動作。
6
圖五、模糊演算法之範例(來源：[40])
7 1.3 研究目的 對於移動機器人，有一些現實狀況中會遇到的問題，必須加以解決 的。（1）若移動機器人可以在一個靜態、並事先獲得所有資訊的環境中， 進行路徑規劃，當然是最理想的狀況。然而，在大多數現實情況下，在 每次路徑規劃前，是很難獲得所有的環境資訊的。特別對於大型環境而 言，收集環境資訊和建立其模型是非常艱難與複雜的工作。（2）即使移 動機器人在路徑規劃前，已獲得的所有環境資訊。但是現實中環境資訊 通常一直在改變(如:行人或行車)。因此，移動機器人必須有能力即時地 處理未知和動態環境中，否則很容易導致事故和災害。 故本研究期望開發一套新演算法，能整合先前路徑規劃法之優缺 點，以解決實際狀況中，移動機器人所會碰到的種種問題。因此本研究 開發之演算法不僅需能解決大型空間、複雜地形(迷宮)、動態環境等等目 前路徑規劃法所難以解決的狀況；另外此演算法還需具有線上計算、即 時反應的特性，對於應用在真實的移動機器人上，才有較高的實用性。
8 1.4 論文架構 本論文架構總共分為五章，第一章為緒論，說明研究之背景、動機 以及目的。第二章詳細說明古典的 Dijkstra 演算法，與本研究提出的 D++ 演算法之演算流程。在第二章中還有將 D++與幾個重要的路徑規劃法做 比較，並探討其優缺點。在第二章的最後則是應用 D++演算法在舉重機 器手臂之模擬實驗。第三章則介紹如何將 D++演算法與實際的移動機器 人進行軟硬體的整合，與介紹本研究移動機器人之硬體架構。第四章為 展示 D++演算法應用在移動機器人的實驗結果，以驗證 D++演算法具有 很高的實用性。最後，第五章為本論文之結論與建議，本章討論 D++演 算法在實際應用中的心得，並提出未來需要改善的建議。
9
二、D++演算法
本研究所提出的新型演算法之所以命名為D++演算法，是因為該演算 法乃改良原有的Dijkstra演算法而來的。改良後的D++演算法對於未知與 動態環境問題，可以獲得良好的結果。在本章節中，我們將具體描述D++ 演算法的觀念，與其演算過程。並在本章節的末段展示使用D++演算法的 模擬過程與結果 。 2.1 Dijkstra 演算法 由於本研究提出的D++演算法是由Dijkstra演算法改良而成的，故本 節中將先介紹Dijkstra演算法的基本理論。Dijkstra演算法是由Edsger Wybe Dijkstra[2]於1959年提出的一個最短路徑演算法，也是目前公認求解最短 路徑的高效經典演算法之一。例如Cheng等人[42]曾採用Dijkstra演算法， 求解規劃任意三次元曲面兩點間最短路徑的問題。Dijkstra演算法可以保 證求出某一節點到其他所有節點的最短路徑。目前最常用的最短路徑演 算法－A*演算法，其實也是由Dijkstra演算法衍生而來。Dijkstra演算法與 A*演算法的優缺點在於：Dijkstra演算法具有最佳答案的保證性，但搜尋 的空間與時間複雜度卻很大；反之，A*演算法的空間與時間複雜度相對 於Dijkstra演算法要小很多，但卻不能保證搜尋出的答案為最佳的。 Dijkstra 演算法的主要特點是以起始點為中心向外一層一層擴展，直 到擴展到終點為止，所以 Dijkstra 演算法能夠求得最短路徑的最佳答案。 Dijkstra 演算法一般對於節點的紀錄有兩種方式，一種用永久和臨時標號 方式，一種是用 OPEN、CLOSE 表方式，在本篇論文中採用 OPEN、CLOSE 表的方式。其搜尋過程簡介如下所述：10 1. 建立兩個節點紀錄－OPEN 與 CLOSE。OPEN 表紀錄所有已生成 而未選取過的節點，CLOSED 表中記錄已選取過的節點。 2. 計算起點的目標函數值，並把它放入 OPEN 表中等待檢查。 3. 從 OPEN 表中找出目標函數最小的節點，找出這個節點的所有子 節點，並計算這些子節點的目標函數值。 A. 若子節點已經在 OPEN 表中了，比較子節點新舊目標函數 值，保留目標函數值較小的子節點資料。 B. 若子節點已經在 CLOSE 表中了，比較子節點新舊目標函數 值。若子節點的新目標函數值較小，更新子節點資料並重新 加入 OPEN 表中。 C. 不在 OPEN 表也不在 CLOSE 表中，則直接將子節點加入 OPEN 表中。 4. 重複第 3 步。直到挑選的節點為終點則完成搜尋；或是 OPEN 表 裡沒有節點則表示沒有路徑可以到終點。 上述的節點資料包括：(1)節點的座標(2)節點的父節點座標：因為等到搜 尋結束時，可以從終點回溯父節點到起點以得到路徑。(3)節點的目標函 數值：每個節點的目標函數值為從父節點累加，所以當父節點改變時， 節點可能會有不一樣的目標函數值。下面我們將舉一個簡單的例子示範 Dijkstra 演算法。如圖七為一個 3×3 的 2D 網格圖，每個網格到鄰近網格 的距離均為 1。欲求Ｓ點到 G 點的最短路徑，以 Dijkstra 演算法求解的步 驟如下。在下列參考範例中的(Aa,B)，A 表示節點編號，a 表示 A 的父節
11 S 1 4 2 3 6 5 7 G 圖七、使用 Dijkstra 演算法的 3×3 地圖 1. 挑選：(S,0) 子節點：(1S,1),(2S,1),(3S,1) OPEN 表：(1S,1),(2S,1),(3S,1) CLOSE 表：(S,0) 2. 挑選：(1S,1) 子節點：(S1,1),(21,2),(31,2),(41,2),(61,2) OPEN 表：(2S,1),(3S,1),(41,2),(61,2) CLOSE 表：(S,0),(1S,1) 3. 挑選：(2S,1) 子節點：(S2,2),(12,2),(32,2),(52,2),(72,2) OPEN 表：(3S,1),(41,2),(52,2),(61,2),(72,2) CLOSE 表：(S,0),(1S,1),(2S,1) 4. 挑選：(3S,1) 子節點：(S3,2),(13,2),(23,2),(43,2),(53,2),(63,2),(73,2),(G3,2) OPEN 表：(41,2),(52,2),(61,2),(72,2),(G3,2)
12 CLOSE 表：(S,0),(1S,1),(2S,1),(3S,1) 5. 挑選：(41,2) 子節點：(14,3),(34,3),(64,3) OPEN 表：(52,2),(61,2),(72,2),(G3,2) CLOSE 表：(S,0),(1S,1),(2S,1),(3S,1),(41,2) 6. 挑選：(52,2) 子節點：(25,3),(35,3),(75,3) OPEN 表：(61,2),(72,2),(G3,2) CLOSE 表：(S,0),(1S,1),(2S,1),(3S,1),(41,2),(52,2) 7. 挑選：(61,2) 子節點：(16,3), (36,3), (46,3), (76,3),(G6,3) OPEN 表：(72,2),(G3,2) CLOSE 表：(S,0),(1S,1),(2S,1),(3S,1),(41,2),(52,2),(61,2) 8. 挑選：(72,2) 子節點：,(27,3),(37,3),(57,3),(67,3),(G7,3) OPEN 表：(G3,2) CLOSE 表：(S,0),(1S,1),(2S,1),(3S,1),(41,2),(52,2),(61,2),(72,2) 9. 挑選：(G3,2) =>挑選到終點，則搜尋結束。 10. 從終點回溯各節點的父節點直到起點為止：(G3,2)->(3S,1)-> (S,0)則搜 尋結果之最短路徑：S->3->G 有關 Dijkstra 演算法的詳細搜尋流程，可參考圖八所示。接著我們以 兩個 2D 平面空間最短路徑規劃的例子，來展示 Dijkstra 演算法的成果， 如下面圖九與圖十所示。其中圖九為無障礙範例，而圖十為具障礙物範
13 例。在圖中有顏色的格子為在搜尋中，有被討論到的格子；而顏色越深 者表示目標函數值越小、離起點越近，反之則越大。 圖八、Dijkstra 演算法流程圖 將起點加入 open 表中 從 open 表中挑選目標函數最 小的最佳節點，並將之從 open 表移到 close 表中 產生子節點 最佳節點是否為 終點或 open 表內是否已 無任何節點 結束搜尋 子節點是否 在 open 表中 是 是 否 否 子節點不在 open 表與 close 表中，則將之加入 open 表 子節點的新目標函 數值是否比舊 的小 子節點的新目標函 數值是否比舊的小 更新子節點資料 更新子節點資 料並將之重新 移到 open 表中 是 是 否 否 否 子節點是否 在 close 表中 是
14
圖九、Dijkstra 演算法無障礙範例 (來源：Amit's A* Pages[6])
圖十、Dijkstra 演算法具障礙物範例 (來源：Amit's A* Pages[6])
15 2.2 D++演算法之理論 由於Dijkstra演算法屬於一次性搜尋的全域路徑規劃法。因此對於大 型的地圖或環境，Dijkstra演算法需要許多搜尋時間去計算最短路徑，才 能令機器人開始移動。另外Dijkstra演算法還需要事先獲得環境中所有的 資訊，所以它無法在未知的環境中使用。為了克服這些限制，我們參考 了如使用超音波感測器的環境感測路徑規劃方法[43]-[45]，在Dijkstra演 算法中加入了的「偵測範圍」概念；如此可使原來的Dijkstra演算法只需 搜尋區域環境即可[46]。D++演算法中的偵測範圍類似於一個被感測器偵 測到的區域，每次只需讓機器人在偵測範圍中搜尋一個最接近終點的中 繼點，並相中繼點前進。僅搜尋偵測範圍內的中繼點可讓機器人在極短 的時間內即可決定下一步移動的方向與位置。透過這樣的方法，機器人 可以一步一步地逐漸靠近並抵達終點。圖十一描述了D++演算法的概念流 程圖，而圖十二則說明了本篇論文中所用到的圖例。如圖十一中之概念 流程所示，流程中每次循環迴圈的計算時間將會非常短，這是因為每次 搜尋的範圍(即偵測範圍)為有限的小型空間。因此機器人可迅速第一次完 成搜尋並開始移動，而無原本Dijkstra演算法常有的等待時間。另外，機 器人只需附近的環境資訊即可開始搜尋路徑，而不需要事先取得環境中 所有的資訊。
16
圖十一、D++演算法之簡易流程圖
17 在圖十一的D++演算法簡易流程中，其步驟二包含了兩個主要執行函 式：(1) GET_DETECTIVE_RANGE() 和 (2) GET_NEXT_MOVE()。其中 第一個函式 GET_DETECTIVE_RANGE()的執行目的為使用Dijkstra演算 法，從目前位置不停擴張子節點範圍到所設定的偵測範圍大小。至於第 二個執行函式GET_NEXT_MOVE()則是搜尋偵測範圍內，最接近終點的 中繼點並獲得從目前位置到中繼點的路徑，進而獲得下一步移動的方向 與位置。執行函式GET_DETECTIVE_RANGE() 的程式條列如下： Function: GET_DETECTIVE_RANGE() L1 for(;;) L2 X = GET_BEST_NODE() L3 if (X is GOAL) then L4 break L5 if (X is NEW) then
L6 add X into SELECT_LIST
L7 if (COST_X > DETECTIVE_RANGE) then L8 if (SELECT_LIST is not empty) then L9 break
L10 else
L11 set X as OLD
L12 remove X from OPEN_LIST L13 add X into CLOSE_LIST L14 for(;;)
L15 Y = GET_CHILD_NODE(X)
L16 if (Y is in OPEN_LIST & NEW_COST_Y < OLD_COST_Y)
then
18
L18 else if (Y is in CLOSE_LIST & NEW_COST_Y <
OLD_COST_Y) then
L19 refresh data base of Y
L20 remove Y from CLOSE_LIST L21 add Y into OPEN_LIST
L22 else if (Y is not in OPEN_LIST & CLOSE_LIST) then L23 write data base of Y
L24 add Y into OPEN_LIST
L25 if (X has no other child node) then L26 Break 在執行函式GET_DETECTIVE_RANGE()中還包括了兩個子執行函 式：GET_BEST_NODE()與GET_CHILD_NODE()。其中第一個子執行函 式GET_BEST_NODE()為從OPEN_LIST中挑出移動成本最小的節點X。 而第二個子執行函式GET_CHILD_NODE()則是取得離節點X最近的各個 子節點。 而在執行函式GET_DETECTIVE_RANGE()中的OPEN_LIST， 儲存了已被搜尋到但尚未被挑選的節點。而CLOSE_LIST則儲存了已被挑 選過的節點。因為執行函式GET_DETECTIVE_RANGE() 在函式一開始 即清除OPEN_LIST 和CLOSE_LIST的內容，所以這兩個表在一個搜尋循 環後，並無法保留環境中節點曾被拜訪過的紀錄。這樣可能會造成機器 人不斷地拜訪某些舊節點，而在某個區域裡打轉。為了避免發生這樣的 情形，D++演算法參考了D*演算法[7][8]的方法，以在一個搜尋循環後持 續記錄節點被拜訪的情形。因此D++演算法在搜尋一開始(即圖十一中的 第一步驟)，即將環境中所有的節點設定為”NEW”的狀態(即為未被拜訪過 狀態)。如果節點一旦被偵測或拜訪過後，它的狀態就會被變更為”OLD”。 另外D++演算法還使用了SELECT_LIST。在偵測範圍內的節點被搜尋到
19 時，如果節點的狀態為NEW，則它將被加入到SELECT_LIST中；否則它 將 被 忽 略 不 予 討 論 。 之 後 每 次 搜 尋 循 環 的 中 繼 點 則 將 由 位 於 SELECT_LIST的節點中被挑選。在執行函式GET_DETECTIVE_RANGE() 所有在節點之COST指的是從目前機器人的位置移動到目前討論節點的 路徑長度，如式(2-1)。 2 2 costC costL (XCXL) (YCYL) (2-1) 其中costC 為經計算後之目前節點的移動成本，costL表示上一節點的 總移動成本，XC 表示目前節點的X座標，XL表示上一節點的X座標，YC 表示目前節點的Y座標，而YL 則表示上一節點的Y座標。 至於第二個主要執行函式GET_NEXT_MOVE()的執行程式條列如下 表所示： Function: GET_NEXT_MOVE() L1 for(;;) L2 X = GET_NODE_FROM_SELECT_LIST() L3 EXPECTED_COST_X = GET_EXPECTED_COST(X) L4 if (EXPECTED_COST_X < MIN_EXPECTED_COST) then L5 WAYPOINT = X
L6 if (SELECT_LIST has no other node) then L7 Break
L8 Y = WAYPOINT
L9 for(;;)
L10 Z = GET_FATHER_NODE (Y)
L11 if (Z = robot’s current location) then L12 NEXT_MOVE = Y
20 L13 Break L14 Else L15 Y = Z 在執行函式GET_NEXT_MOVE()中，還包含了三個執行子函式，分 別 為 (1) GET_NODE_FROM_SELECT_LIST() 、 (2) GET_COST() 與 (3) GET_FATHER_NODE()。其中子函式GET_NODE_FROM_SELECT_LIST() 為從SELECT_LIST中取得節點。而GET_EXPECTED_COST()則為計算從 節點到終點的預估移動成本，如式(2-2)。 2 2 costC (XGXC) (YGYC) (2-2) 其中costC 為經計算後、從SELECT_LIST挑選出節點的移動成本， XC 表示目前節點的X座標，XG表示終點的X座標，YC表示目前節點的Y 座標，而YG 則表示終點的Y座標。之後在表SELECT_LIST中且離終點 GOAL 最 近 的 中 繼 點 WAYPOINT 便 會 被 挑 選 出 來 。 透 過 子 函 式 GET_FATHER_NODE()，D++演算法可由中繼點不斷地回溯各子點的父 節點，以獲得從目前機器人的位置到中繼點的路徑。如此機器人也可獲 得移動下一步的位置與方向。在一次搜尋循環結束後和下一次搜尋循環 開始前，表SELECT_LIST 、表OPEN_LIST和表CLOSE_LIST的內容都將 被清除。接著我們總結整個D++演算法的主要流程，並將其繪製如圖十三 所示，以俾利更容易了解D++演算法。
21 圖十三、D++演算法主要流程圖 1. 設定起點、終點與偵測範圍；並將所有節點狀態設定為 NEW。 2. 清空 OPEN、CLOSE 與 SELECT 表。 3. 根據 Dijkstra 演算法擴展節點。 5. 目前搜尋到的節點狀 態是否為 NEW？ 6. 增加節點到 SELECT 表，並變更節點狀態為 OLD。 7. 搜尋範圍是否超過偵測範 圍？或是終點已在 SELECT 表？ 8. SELECT 表中是否有任何節 點？ 9. 從 SELECT 表中挑選出中繼點。 10. 從中繼點追溯回機器人位置並獲得下一步位置。 11. 機器人移動到下一步。 12. 目前機器人的位置是否抵 達終點？ 13. 結束搜尋 是 否 4. 目前搜尋到的節點 是否為終點？ 是 否 是 是 是 否 否 否
22 2.3 區域解問題 在本節中，我們將展示一些使用 D++演算法進行路徑規劃可能會碰 到區域解的模擬實驗。在這些模擬實驗中，每次的循環週期為 20 毫秒。 而在本研究中，所有模擬實驗所使用的電腦體規格則條列於表 1 中： 表 1、本研究實驗之電腦硬體規格 Item Specification
CPU Intel Core 2 Duo T6600 2.2 GHz RAM DDR2-800 2 GB
VGA ATI Radeon HD4650 512 MB OS Microsoft Windows XP IDE SharpDevelop 在這個靜態環境路徑規劃的模擬實驗中，將展示兩個例子。其中一 個例子的偵測範圍設定為 5 格(如圖十四所示)，而另一個例子的偵測範圍 則設定為 20 格(如圖十五所示)。圖十四中路徑的長度明顯比圖十五的路 徑還要長的許多，這是因為 5 格的偵測範圍不夠讓機器人提早發現無效 的節點與路徑，因此在 U 型障礙物中間遊走了一段路程才脫離出來。相 對地，由於 20 格的偵測範圍可即早地預測到 U 型障礙物中間的無效節點 與路徑，因此圖十五中，機器人幾乎沒有進入 U 型障礙物便轉換方向移 動，如此規劃出來的路徑既短又有效率。
23
圖十四、偵測範圍為 5 格，總花費時間約 4 秒
24 一般情況下，當一次搜尋循環的搜尋範圍到達了所設定的偵測範圍 大小，D++演算法會結束該循環並從表 SELECT_LIST 中挑選中繼點 WAYPOINT。然而，如果機器人進入一條死路中，例如一個 U 型障礙物， 則那時的偵測範圍內的節點狀態可能都會是”OLD”(如圖十六 b 所示)。如 此表 SELECT_LIST 中將沒有任何節點，而機器人將無法決定如何移動下 一步。所以當機器人遇到了這樣的狀況時，D++演算法特別地會擴大偵測 範圍直到一個狀態為” NEW”的節點被發現(如圖十六 c 所示)。。透過這 樣的方式，機器人可持續規劃到終點的移動路徑並輕易地脫離該區域。 圖十六、當 Select_List 中無節點時，D++演算法的行為模式 (a)機器人進入 U 型障礙物(b)偵測範圍內沒有狀態為”NEW”的節點(c)D++ 演算法擴大偵測範圍直到發現有狀態為”NEW”的節點(d)機器人往狀態 為”NEW”的節點移動
25 2.4 與其他演算法的比較 (1)靜態環境： 圖十七為一個 100×100 格、且具有模擬野外障礙地形的地圖。起點 被設定在地圖的左方，而終點被設定在地圖右方。圖十七為使用 D++演 算法規劃路徑後的結果，此模擬實驗使用 15 格為偵測範圍，並以 20 毫 秒為搜尋週期，搜尋過程中共探訪了 2626 個格點，最後出來的路徑長度 為 151 個格點。圖十八為使用 Dijkstra 演算法規劃路徑後的結果，搜尋過 程中共探訪了 6286 個格點，最後出來的路徑長度為 122 個格點，計算搜 尋時間約為 2.5 秒。圖十九為使用 A*演算法規劃路徑後的結果，搜尋過 程中共探訪了 6209 個格點，最後出來的路徑長度同樣為 122 個格點，計 算搜尋時間約為 2.4 秒。圖二十為使用 D*演算法規劃路徑後的結果，因 演算概念與 A*演算法相同，故搜尋過程中一樣共探訪了 6209 個格點， 最後出來的路徑長度同樣為 122 個格點，計算搜尋時間約為 2.4 秒。最後 圖二十一為使用人工位能法，並以 20 毫秒為計算週期之路徑規劃結果， 最後卡在地圖中央的區域解，並無法求得路徑解而抵達終點。 圖十七、靜態環境中使用 D++演算法之路徑規劃結果(探訪節點數:2626, 路徑長度:151)
26
圖十八、靜態環境中使用 Dijkstra 演算法之路徑規劃結果(探訪節點 數:6286, 路徑長度:122)
圖十九、靜態環境中使用 A*演算法之路徑規劃結果(探訪節點數:6209, 路 徑長度:122)
27
圖二十、靜態環境中使用 D*演算法之路徑規劃結果(探訪節點數: 6209, 路徑長度:122)
28 (2) 動態環境： 在這個模擬實驗中，我們設計一個 100×100 的大型地圖（如圖二十 二所示），一開始在地圖中間有九個圓形物體。這些物體固定以每 100 毫 秒的週期，隨機地往或前、或後、或左、或右移動。機器人的起始位置 在地圖的左下方，目的地在地圖的右上方。我們以 100 毫秒為計算週期， 分別以 D++演算法、D*演算法與人工位能法進行動態路徑規劃，並分別 得到如圖二十三、圖二十四與圖二十五之結果。圖二十三中使用 D++演 算法共花費 14 秒使機器人從起點移動到終點；圖二十四中使用 D*演算 法共花費 1 分 11 秒使機器人從起點移動到終點；圖二十五中使用人工位 能法共花費 13 秒使機器人從起點移動到終點。 圖二十二、有九個隨機移動物體的動態環境
29
30
31
32 (3) 結果比較討論： 我們將前述之靜態環境與動態環境的兩個範例，分別應用了 Dijkstra 演算法、A*演算法、D*演算法、人工位能法和 D++演算法的實驗結果， 總結整理如表 2 所示。 表 2、路經規劃結果比較表 靜態環境 動態環境 搜尋節點數 路徑長度 機器人啟動 時間(秒) 總花費時間 (秒) 移動總花費 時間(秒) Dijkstra 6286 122 2.5 124.5 X A* 6209 122 2.4 124.4 X D* 6209 122 2.4 124.4 71 人工位 能法 X X 0.02 X 13 D++ 2626 151 0.02 154.02 14 從表 2 中我們可以看到在靜態環境的實驗中，人工位能法因為陷入區域 解的關係，並沒有搜尋出可到達終點的路徑。而 Dijkstra、A*與 D*則找 到比 D++更短的路徑，不過它們也搜尋了比 D++多出一倍的節點數量， 因此大約花了 2.5 秒的時間找到路徑解，才可讓機器人開始移動。而 D++ 雖然求得的路徑稍長，但由於一開始僅做 10 格偵測範圍的區域搜尋，故 可順利地在 20 毫秒內完成中繼點的搜尋，並立即讓機器人開始移動。不 過若假設機器人移動一個格點需費時 1 秒，並將搜尋時間與機器人移動 時間總和，Dijkstra、A*與 D*僅需約 124 秒左右，而 D++則需花費約 154 秒左右。
33 而動態環境中，Dijkstra 與 A*由於屬於一次性搜尋路徑規劃法，故 無法處理動態環境的問題。至於 D++與人工位能法則發揮了區域搜尋的 優勢，故能觀察動態環境的變化後，便立即快速地做出適當的反應；因 此兩者均在 13~14 秒左右，即讓機器人完成在此動態環境從起點移動到 終點。而 D*由於在移動的初期具有很大的搜尋範圍，故在此段時期所耗 費的路徑規劃時間會較久；而雖然在後段時期因為搜尋範圍變小了而增 快了反應速度，然而機器人從起點移動到終點的總花費時間還是高達了 71 秒，為使用 D++與人工位能法的六倍左右。 故從這兩個實驗結果可以得知，Dijkstra 與 A*因為屬於一次性搜尋 法，故無法處理動態環境問題；而人工位能法則因為容易遇到區域解問 題，故無法處理某些複雜環境問題。而 D*雖然對於動態環境與複雜環境 問題均可以處理，但是對於大型地圖仍然有搜尋範圍過大導致搜尋時間 過長的問題。特別對於大型動態環境，使用 D*可能因為搜尋時間過久讓 機器人停滯，而撞上高速移動的障礙物。因此相較起來，D++比其他方法， 雖然分別對於靜態環境與動態環境問題，並非其中最有效率的方法。但 D++可同時適用於靜態環境與動態環境的特性，對於應用在機器人問題 上，D++相較於其他方法，則具有較高的泛用性與實用性。綜合以上討論， 我們整理各路徑規劃法之優缺點比較於表 3，以俾利更簡單、清楚地了解 各方法在各條件下之優勢與劣勢。
34 表 3、各路經規劃法的優缺點比較 最短路徑 避免 區域解 小型、靜態、 已知環境 大型、動態、 未知環境 Dijkstra ◎ ◎ ◎ △ A* ◎ ◎ ◎ △ D* ◎ ◎ ◎
○
人工位能法 △ △○
◎ D++○
○
○
◎ ◎：好 ○：普通 △：劣35 2.5 偵測範圍的最佳化設定 從圖十四與圖十五的結果我們可以得知，偵測範圍的尺寸將影響 D++演算法的效能與路徑結果。然而多大的偵測範圍才是適當的?由 D++ 演算法規劃的路徑可能不是最短、或最佳路徑，這是因為 D++演算法每 次搜尋循環僅在區域範圍搜尋中繼點。因此偵測範圍照理說應該儘可能 地設定越大越好，只要機器人的控制硬體效能可以在容許的週期時間內 完成每次的搜尋循環。例如圖二十六為一個 100×100 格、無障礙物的空 白地圖；起點在地圖的左下角，而終點在地圖的右上角。設計無障礙物 的地圖的原因是讓不論偵測範圍的大小為多少，使用 D++演算法規劃出 的路徑均為從左下角直線移動到右上角。如此我們可以統一路徑的總長 度。 接著我們設定偵測範圍從 1 格到 20 格各使用 D++演算法跑一次模擬 結果，並設定以 20 毫秒為每次的搜尋循環週期。在此 20 次模擬實驗中， 路徑規劃與機器人移動的總時間花費統計如圖二十七所示。透過該圖表 的顯示，我們可以看出當偵測範圍小於或等於 15 格時，偵測範圍變大並 不會影響路徑規劃與機器人移動的總時間花費；而當偵測範圍大於 15 格 時，路徑規劃與機器人移動的總時間花費隨著偵測範圍變大而增加。因 此我們可以推斷：對於此機器人的控制硬體而言，若要以每次 20 毫秒的 循環週期內完成對偵測範圍大小的環境搜尋中繼點，則對此機器人的控 制硬體效能而言，我們將偵測範圍設定為 15 格，將獲得最佳的搜尋效率 與路徑結果。
36
(a) (b)
圖二十六、一個 100×100 格、無障礙物的空白地圖
37 2.6 機器手臂之模擬範例 本節將使用 D++演算法應用於機器手臂的軌跡規劃上[47]。在 2001 年時，Wang[48]等人使用了梯度計算方法以解決機器手臂的舉重最佳化 問題。他們的研究目標為如何令機器手臂以最省力的方式，以舉起所能 負荷的重物。Wang 等人分別展示了成功地令一軸、三軸、與六軸的機器 手臂舉重模擬結果。然而梯度法需要事先列出並計算複雜的數學公式， 另外也要給定合適的初始值方能收斂至全域最佳解。故這種方式雖然可 找到很好的結果，但僅限於一次計算結果後便不斷重複執行同一動作的 機器手臂應用，不適合於需自我決策與即時反應的機器人應用上。同樣 於 2001 年時, Rosenstein[49]等人提出了直接方針法建立了一組三軸機器 手臂舉重學習平台。藉著設計適當的參數式方針與提供合適的手端軌跡 中繼點，Rosenstein 等人提出的方法可有效地找出良好的結果。然而需要 人工設定的方針與中繼點，以及同樣冗長的試誤學習過程，同樣讓這類 方法並不適用於需自我決策與即時反應的機器人應用上。另外在 2007 年 時，Cheng[50]等人使用了 Dijkstra 演算法，為一組三軸的舉重機器人建 立了手臂移動軌跡規劃方法。然而正如之前提到的，Dijkstra 演算法是一 種一次性計算的演算法，所以對於機器手臂軌跡規劃問題，特別是高解 析度軌跡規劃，其搜尋空間將十分龐大，因此其搜尋時間將非常久，導 致機器手臂無法即時反應的動作。
38 2.6.1 模擬條件 圖二十九中為一組具兩軸之舉重機器手臂，其相關的尺寸、重量與 限制條件如下： 1. 連桿長度： L1 = 1 m, L2 = 1 m 2. 連桿重量： m1 = 1 kg, m2 = 0.5 kg 3. 軸之轉動範圍： -150°≦θ1≦150°, 0°≦θ2≦150° 4. 軸之轉矩限制： τ1≦50 Nm, τ2≦50 Nm 本節使用 D++演算法的軌跡規劃目標，為使此舉重手臂以手端移動距離 較短方式將重物由機器手臂的手端最低點（θ1 = -90°, θ2 = 0°），舉升到手 端的最高點（θ1 = 90°, θ2 = 0°），如圖二十九所示。 因此我們將原先應用 在 X、Y 平面座標的 D++演算法，改成應用在 θ1 、θ2平面座標。而此θ1 、 θ2座標平面上的障礙物即為超過上述角度與轉矩限制的座標點。 圖二十八、具兩軸之舉重機器手臂 而此舉重機器手臂的手端位置可根據式(2-3)與式(2-4)計算： 1
cos
1 2cos(
1 2)
hX
L
L
(2-3)39 1sin 1 2 sin( 1 2) h Y L
L
(2-4) 式中 Xh 表示機器手臂手端在 X 方向的座標，, Yh表示機器手臂手端在 Y 方向的座標。 圖二十九、本章節舉重機器手臂軌跡規劃目標 因此在本問題中， D++演算法的目標函式（cost function）可以式(2-5) 表 示： 2 2 costC costL (XCXL) (YCYL) (2-5) 式中 costC 表示 D++演算法中的子節點目標函數值，costL 表示上一節點 的目標函數值，XC表示子節點的手端ｘ方向的位置，XL表示上一節點的 手端ｘ方向的位置，YC表示子節點的手端ｙ方向的位置，YL表示上一節 點的手端ｙ方向的位置。為了簡化問題，我們根據了靜力學理論列出了 機器手臂每一軸承受力矩之公式，如式(2-6)與 (2-7)所示。 1 2 1 2 1cos 1 2cos( 1 2) 2 2 m m g m M L M L (2-6) 2 2 2cos( 1 2) 2 m g M L
(2-7)40 式中 M 表示負重的重量，g 表示重力加速度常數。接著本問題中的障礙 物定義如下： 1
50Nm
(2-8) 2 50Nm (2-9) 1 150 or 1 150 (2-10) 2 150 or 2 0 (2-11) 2.6.2 模擬結果 因此根據上述之理論與公式，使用 D++演算法計算機器手臂的舉重 軌跡之模擬結果如圖三十所示，其中圖三十(a)為負重 0kg 之模擬結果， 而圖三十(b) 為負重 4kg 之模擬結果。這兩個模擬結果均具有 10 毫秒的 循環週期，且每軸之解析角度(θ1、θ２)為 1 度。而圖三十中，機器手臂的 舉起 0kg 與 4kg 負重的模擬情形，則分別如圖三十一與圖三十二所展示。 從圖三十的結果比較我們可以看到，圖三十(b)中的路徑比起圖三十(a)的 路徑繞行地更遠，這是因為圖三十(b)中的負重較大，相較於圖三十(a)的 例子將更容易超過轉矩限制。所以圖三十(b)中 θ1與 θ２的平面座標上，相 較於圖三十(a)的平面座標多了許多的障礙物(圖三十(b)中較小的圓點)。 因此圖三十(b)中路徑必須繞過較多的障礙物方能抵達終點。 而觀察機器手臂舉起負重的模擬結果，圖三十一的例子由於無負重， 所以為了快速從起點到達終點，機器手臂的手端位置離肩部(θ1 之軸處) 較遠，也可以在轉矩限制下完成舉重動作。而圖三十二的例子由於有 4kg 的負重，所以為了在轉矩限制下完成舉重動作，機器手臂的手端位置必 須離肩部較近以降低力矩臂與力矩，方可順利地從起點到達終點。41 圖三十、於卡氏座標系統的模擬結果比較(a) 負重為 0kg 之結果(b) 負重 為 4kg 之結果 圖三十一、機器手臂舉起負重 0kg 的模擬情形(10 毫秒的循環週期，且每 軸之解析角度為 1 度)
42 圖三十二、機器手臂舉起負重 4kg 的模擬情形(10 毫秒的循環週期，且每 軸之解析角度為 1 度) 透過本模擬實驗的進行，我們成功且有效地運用了 D++演算法於機器手 臂的路徑規劃上，並具有快速反應使用者命令需求的能力，避免因需大 量計算而導致機器手臂響應過慢，以致無法處理動態環境等問題。對於 未來用於家庭、或醫療照顧等需即時反應的智慧型機器手臂應用，可具 有較高的實用性。
43
三、D++演算法與移動機器人之整合實作
3.1 系統架構 在前兩章節中，我們介紹了 D++演算法的基本概念與流程，並以電 腦模擬程式驗證了 D++演算法的可行性。本章將介紹本研究如何實際製 作移動機器人，並於一實驗性室內環境進行路境規劃與移動，以證明 D++ 演算法具有很高的實用性。本研究將設計製造具兩輪之移動機器人(如圖 三十三所示)。該移動機器人使用兩個直流馬達驅動兩個輪子轉動，並使 用 Arduino 開發板的 PWM 輸出功能控制直流馬達的轉速與與轉向。為了 精簡移動機器人的製作成本與構造，D++演算法等高階控制流程將透過個 人電腦(PC)或筆記型電腦(NoteBook)實現；而電腦與 Arduino 開發板之溝 通則以 RF 無線電傳輸實現。其系統架構如圖三十四所示。Arduino 開發 板負責接收 RF 無線電傳輸指令，並擷取紅外線感測器的訊號回報給遠端 控制電腦規劃路徑，以獲得各種決策如前進或轉向等。44
圖三十三、具兩個驅動輪(左右)與一個萬向輪(前)之移動機器人
45
46 3.2 Arduino 控制開發板 Arduino[51]是一個的「開放式硬體（open source）」微電腦控制板， 在 2005 年 1 月 由 米 蘭 互 動 設 計 學 院 的 教 授 David Cuartielles 和 Massimo Banzi 所設計出來了，原始構想是希望讓設計師及藝術家們，透 過 Arduino 很快的學習電子和感測器的基本知識，快速的設計、製作作 品的原型，很容易與目前設計系所學的 FLASH, MAX/MSP, Virtool 等軟 體整合，使得虛擬與現實的互動更加容易。互動的內容設計才是設計師 的主要訴求，至於怎麼拼湊一個單晶片開發板，或是當中涉及如何構築 電路之類的知識，就並非設計師需要了解的，因此非常適合不具電子背 景的人使用，以設計出各種不同的互動裝置。
Arduino 控制板主要是採用 Atmel 公司的 AVR 系列微處理器。它的 硬體很簡潔，沒有什麼特殊的設計，除了開放式硬體架構之外，重點在 於它提供了一個基於 C/C++的程式語言來控制 Arduino，並且有 Windows, Mac OS X 和 Linux 等跨平台環境的程式編輯工具(如圖三十六)。除了當 成可獨立運作的微電腦控制器之外，它也能透過許多愛好者開發出的程 式 模 組 ， 和 電 腦 上 的 其 他 程 式 語 言 通 訊 ， 例 如 ： Flash ActionScript, Processing, Python, PHP,…等等。微電腦自動控制的專業人士，大多優先 選擇「組合語言」來撰寫程式，因為組合語言的執行速度快，而且佔用 的記憶體空間小。如果嫌高階語言不專業，使用者也可以安裝 Atmel 公 司的 AVR Studio 開發工具，用組合語言或者 AVR C 語言撰寫 Arduino 的 程式。
47
圖三十六、Arduino 軟體開發除錯環境(來源：[51])
因為本研究所建置的移動機器人，還需要使用到專為 Arduino 開發的 馬達驅動板（如圖四十六所示），該馬達模組可直接層疊至 Arduino Uno 上(如圖三十三所示)，不需另外的傳輸線，故我們採用的是 Arduino Uno 這塊開發控制板。Arduino Uno 為使用 ATmega328 微控器的控制板。它 具有 13 個數位輸入/輸出埠，其中 Pin3、5、6、9、10、11 具有 PWM 輸 出功能，Pin2,3 具有中斷輸入功能，Pin0, Pin1 為一組串列通訊埠(0=Rx, 1=Tx)；Pin13 具有 LED 輸出功能。Arduino Uno 還有 6 個類比輸入埠 (A0~A5)。Arduino Uno 使用 16MHz 的振盪器，並具有 128K 的記憶體。 至於下載程式的介面，則是使用 ATmega8U2 晶片來處理 USB 通訊
48
(B-Type 接頭)，並可支援多平台作業系統 windows/Linux/Mac。電源方面 使用 2.1mm 的 Power Jack 接頭，可輸入 6~20V 的 DC 電源，官方則建議 使用 7~12V 的 DC 電源。然而為了電源的長續航力，本移動機器人平台 使用 5V/1A、具 USB 埠的行動電源來提供 Arduino 控制板所需要的電源。
圖三十七、Arduino UNO 控制板(來源：[51]) 在本研究中，我們使用 Arduino Uno 來擔任移動機器人本體的硬體控 制器。接著我們針對本研究所使用到 Arduino 控制板的主要功能，做出更 進一步的介紹。在單晶片控制技術中，輸入輸出是單晶片最基本的介面， 通過這個介面，單晶片可以用數位或類比的方式，和周邊電路連接，並 進行控制電路訊號和訊號檢測。在上一段中，我們已概略介紹 Arduino 開發板具有哪些輸入輸出功能，接著我們針對這些功能，來說明在本研 究中所應用的方式： (1) 數位輸出輸入： 在本研究中，我們使用數位輸出輸入，來取得編碼器的輸入訊號， 並控制馬達驅動器的輸出訊號。 (2) 類比輸入：
49 在本研究中，測量障礙物的位置主要是由紅外線感測器所偵測，而 我們使用的紅外線感測器之感測距離，為使用類比電壓輸出。故我 們使用 Arduino 開發板上的 A0~A5 來接收紅外線感測器與電子陀螺 儀的訊號。在 Arduino 開發板上，類比訊號為 10bit 的資料，故可將 得到的類比電壓值細分為 0~1023 數位資料。 (3) PWM： 在本研究中，PWM 主要是控制移動機器人的 DC 馬達轉速，每一顆 DC 馬達需要 1 組 PWM 控制其轉速，1 條數位輸出控制其正反轉。 所以在本研究中，我們需要 2 組 PWM 來控制兩個 DC 馬達的轉速， 2 Pin 的數位輸出埠控制兩個 DC 馬達的正反轉，。 (4) 計時中斷： 在本研究中，計時中斷主要負責機器人的馬達轉動的同步轉動控 制、移動距離控制與轉向時的角度控制。為本研究移動機器人運動 控制最重要的部分。 (5) 輸入觸發中斷： 在本研究中，我們使用了 2 pin 的輸入觸發中斷，來取得馬達編碼器 傳至 Arudino 板的訊號，以獲得目前機器人的移動距離，進而換算 機器人目前之位置。使用觸發中斷埠的優點是能保證不漏掉訊號。 (6) 串列通訊埠： 在本研究中，串列通訊埠主要是透過無線電 RF 傳輸模組，與遠端控 制電腦溝通。因此 Arduino 控制板負責移動機器人馬達驅動與感測 器接收等硬體工作，而電腦則負責較複雜的路徑規劃計算工作，以 達到多工目的。
50 3.3 運動控制用之硬體 3.3.1 自製編碼器 本節說明本研究所使用之自製光學編碼器的工作原理及應用，它被 安裝在輪型機器人左右兩側車輪軸心上，當車輪轉動時將產生穩定的脈 衝輸出，在單位時間內計數脈衝個數可以轉換得知左、右車輪移動的距 離，進而得知機器人的方位及座標，並導引機器人到達期望的目標，這 種方法被稱為車輪測程法。 光學編碼器是由一組光遮斷器及齒輪狀之圓盤所組合而成(如圖三十 八)，當固定在車輪上的圓盤轉動時，光遮斷器之光通過圓盤齒隙沒有被 阻擋的話，其輸出為高電位，藉由提升電阻拉為高電位。反之則輸出為 低電位。 圖三十八、本研究所使用之自製光學編碼器
51 本研究所使用的光學編碼器，圓盤上平均分配 30 個齒，每旋轉一周可產 生 30 個脈波或 60 個邊緣觸發，參考圖三十九所示。 圖三十九、本研究之自製編碼盤尺寸圖 若左右兩車輪的輪距為 2b，車輪半徑為 r，車輪每旋轉一周，編碼器送 出 N 個脈波，假設兩車輪同方向同步轉動了 n 個脈波，輪型機器人在直 線上移動的距離可計算如下: d 2 nr N (3.1) 以本研究所採用的輪型機器人及光學編碼器為例，車輪半徑為 30mm，每 轉一圈送出 60 個邊緣觸發，在輪胎不打滑的前提下，輪型機器人約向前 移動了 188.5 mm 的距離，每個邊緣處發信號的精度為 3.14mm。 由於從光遮斷器所獲取的訊號為弦波而不是方波，加上馬達轉動時 容易對週遭電路產生干擾，故若是我們直接擷取從光遮斷器來的訊號， 非常容易產生誤判的情形，造成編碼器嚴重的計數誤差。為了得到正確
52 的編碼器計數值，我們使用史密特觸發反相器 IC－HD74LS14（如圖四 十），將從光遮斷器擷取來的不穩定弦波訊號（輸入至Ａ），透過反相器 來整形成穩定的方波訊號（由 Y 輸出），以獲得正確的編碼器計數值（如 圖四十一所示）。 圖四十、HD74LS14 之內部結構圖（來源：[54]） 圖四十一、使用史密特觸發反相器穩定光遮斷器之訊號（來源：[55]）
53 3.3.2 雙輪同動控制 如同本章第一節所提到，本研究所建立的移動機器人平台，具一個 萬向輪(前)與兩個驅動輪(左右)。萬向輪的作用僅為支撐機器人不翻倒， 真正令移動機器人的前進、後退、轉向為位於左右的驅動輪。左右驅動 輪同時向移動機器人前方轉動時，則移動機器人便往前進；反之左右驅 動輪同時向移動機器人後方轉動時，則移動機器人便會後退。若是左右 驅動輪一個向前、一個向後轉動，則移動機器人便會根據驅動輪轉動的 方向，而向左或向右轉向（如圖四十二所示）。 (a) (b) (c) (d) 圖四十二、本研究移動機器人之運動控制邏輯(a)前進(b)後退(c)左轉(d) 右轉
54 然而以上的控制邏輯，必須在左右兩個驅動輪具有相同轉動位移或 速度的條件下，方能達成。若是左右兩個驅動輪不同動，便會造成斜向 地前進或後退，或是非以移動機器人為中心轉向。由於本研究之移動機 器人平台，並未架設外部的機器人絕對位置定位裝置（如 GPS 系統、CCD 照相機或室內網路定位系統）；因此本研究之移動機器人的定位方式，為 透過編碼器與上述前進、後退、轉向的動作，自行計算其相對的移動位 置，以換算可能的絕對位置。因此若是移動機器人因為左右兩個驅動輪 不同動，造成斜向地前進或後退，或是非以其中心轉向，便會使得換算 的位置與實際有很大的誤差。因此我們利用 PID 控制法則，透過編碼器 的訊號回授，來控制兩個驅動輪以相同轉動位移與速度同時轉動，以達 到我們利用編碼器與相對位移換算其絕對位置的目的。 PID 控制法則[60][61]問世至今已有近 60 年的歷史，此種控制具有結 構簡單、穩定性好、工作可靠、調整方便等四項優點而成為工業控制技 術的工具。假若受控制對象的組態與參數無法完全掌握，或是無法獲知 精確的數學模型時，系統的控制器的參數就必須依靠經驗和實際調試來 確定，此時，應用 PID 控制技術最為方便。PID 控制法則由比例增益( P)、 積分增益( I) 和微分增益(D)組成，通過 Kp， Ki 和 Kd 三個參數的設定， 使得在單一輸入及單一輸出的控制下可以達到非常好的穩定性。我們可 以認為積分是過去偏差的累積值，比例是現在偏差的大小，微分則是針 對將來的偏差變化動向做各種對應。因此控制系統的設計重點是如何決 定 Kp、Ki、Kd 這三個參數值，我們將之稱為 PID 控制器的調整，就是 替控制器參數尋找一個最佳設定值。而比例增益( P)、積分增益( I) 和微 分增益(D)的功用說明如下： (1) 比例（P）控制：是一種最簡單的控制方式。其控制器的輸出與 輸入誤差信號成比例關系。當僅有比例控制時系統輸出存在穩態
55 誤差（Steady-state error）。 (2) 積分（I）控制：在積分控制中，控制器的輸出與輸入誤差信號的 積分成正比關系。積分項對誤差取決於時間的積分，隨著時間的 增加，積分項會增大。這樣，即便誤差很小，積分項也會隨著時 間的增加而加大，它推動控制器的輸出增大使穩態誤差進一步減 小，直到等於零。因此，比例+積分(PI)控制器，可以使系統在進 入穩態後無穩態誤差。 (3) 微分（D）控制：在微分控制中，控制器的輸出與輸入誤差信號 的微分（即誤差的變化率）成正比關系。比例項的作用僅是放大 誤差的幅值，而微分項能預測誤差變化的趨勢，這樣，具有比例 +微分的控制器，就能夠提前使抑制誤差的控制作用等於零，甚 至為負值，從而避免了被控量的嚴重超調。所以對有較大慣性或 滯後的被控對象，比例+微分(PD)控制器能改善系統在調節過程 中的動態特性。 在本研究的雙輪同動 PID 控制問題中，由於是我們使用 Arduino 板 上的 PWM 輸出功能來控制直流馬達的轉速，所以同動 PID 控制中的輸 入為預期左右驅動輪的馬達轉速，輸出為使用 PID 調整後的馬達轉速， 而回授訊號為左右驅動輪上的編碼器數值（如圖四十三所示）。 圖四十三、本研究雙輪同動 PID 控制方塊圖
