基於向量模型設計與實現蒙地卡羅定位與路徑規劃於輪型移動機器人

全文

(1)國立臺灣師範大學機電工程學系碩士論文指導教授:陳美勇博士基於向量模型設計與實現蒙地卡羅定位與路徑規劃於輪型移動機器人 Design and Implementation of Monte-Carlo Localization and Path Planning for Wheeled Mobile Robot Based on Vector Model. 研究生 : 鍾秉剛撰中. 華. 民. 國. 1. 0. 4. 年. 1. 月.

(2) 摘要本論文主要研究之目的是改良輪型機器人的定位與路徑規畫演算法。在定位方面，本論文克服定點定位、路徑追蹤與機器人綁架所面臨之問題，並且提出了基於向量模型的改良式蒙地卡羅定位演算法。具體的改善措施包含以下內容：第一，引入離散形式的輪型機器人運動模型進行動態更新；第二，以向量模型取代柵格地圖，解決定位的解析度問題；第三，除了使用競爭法與菁英保留策略外，在重新採樣的程序中引入了粒子變異機制，其變異程度由當前粒子的分佈情形決定，使演算法可以保持指數地收斂；最後，粒子重新分佈的機制亦被納入演算法中，使得演算法具備更佳的強健性針對定位失敗與綁架問題。在路徑規劃方面，本論文將 A*演算法建立於向量地圖中，使用地圖中的角點為此演算法內的節點，藉以改善在 A*演算法面臨移動方向限制的困擾與大量轉折點的問題。為使上述方法可行，本文修改 A*演算法流程並加入判別節點間移動可行性的函數，只有獲得許可的路徑會被納入花費函數的考量以確保能正確地規劃路徑。為避免路徑過於靠近障礙物而發生碰撞，本文引入影像侵蝕方法去除過於接近障礙物的區域，使機器人在移動過程中能與障礙物保持適當距離。此外，本文亦加入一項路徑圓弧優化方法讓路徑更為平順，使機器人以弧型移動通過各節點而降低整體移動花費時間。整體而言，本論文的定位演算法可以更有效率地提供機器人定位結果，提供的路徑能在最短路徑與避障能力上保持良好的平衡，並使機器人除了避障外亦能快速到達終點。關鍵字：蒙地卡羅定位法、Ａ*演算法、地圖向量化. i.

(3) Abstract The main purpose of research is to improve the localization performance and result of path planning of wheeled mobile robot. In the area of localization, we solve the problems from localization, path tracking and robot kidnapped, and propose an improved Monte Carlo localization based on vector model. The specific improvements of the method are as follows: First, we include the discrete kinematic model of wheeled mobile robot in prediction process. Second, use vector model instead of bitmap to solve the resolution problem. Third, not only tournament selection and elitism, but also variation mechanism are used in the resampling process. The extent of Variation is decided by distribution of particles. Last, the re-initialize process is added into the algorithm, so that the system can be more robust and better to robot kidnapped problem. In the area of path planning, we include A* algorithm into vector model instead of traditional bitmap, and use the corners as nodes in A* algorithm to solve the limited of orientation of robot problem, and reduce the amount of turning points. To do this, we modify the process of A* algorithm and add the function that checks particles are moveable or not. Only pass particles can be considered into cost function. To avoid collision, we include the image erosion to remove the area which is too close to obstacles, so there is enough distance between robot and obstacles. In addition, path smoothing process is added into the algorithm to get smoother path after planning, so that the robot can pass through all nodes more efficiently. Overall, our research can let the localization algorithm be with more efficacy, and plan a path with good balance in shortest path and avoidance ability, so the robot can move quickly and stably to the goal. Keywords: Monte Carlo localization, A* algorithm, map vectorization. ii.

(4) 誌謝「一個階段的結束，也是另一個階段的開始」，經過這麼多年的訓練與成長，充滿了酸甜苦辣的過程，終於在今天完成了碩士論文，也是對得起自己與眾人的交代了。首先，我要感謝陳美勇教授的指導，進入實驗室之後學習到許多嶄新的知識，領導團隊的技巧，以及做實驗乃至於未來人生所需的嚴謹態度；當然也要感謝口試委員許陳鑑博士、李敏凡博士以及蕭志清博士，針對論文描述不足之處給予指正，使得本論文得以更加周延，也因此能使性能再提升一個層次。此外，我也要感謝實驗室的同學：昀漢、智翔、玠虢、高遠、煒騰、威任等人，在我有需要的時候可以搶劫你們的電腦進行我海量參數的模擬，也感謝你們願意聽我的抱怨與不斷地問你們問題，讓我能夠在進行自我的專業之餘有接觸別的領域的機會，以及如何針對研究進行改善。願你們在將來能一帆風順，勇往直前地追尋自己的目標。最後，我還要感謝我的家人，我的父母還有三位姐姐們，你們不求回饋地提供我需要的資源，讓溫暖的避風港充滿了歡樂；當然也要謝謝我的女友乃瑄這麼多年來的陪伴，能夠為彼此打氣加油，讓我們在面對困難時能一起度過，願我們可以一起攜手前進。. 鍾秉剛. 謹誌. 民國一零四年一月精密運動實驗室. iii.

(5) 目錄摘要………………………………………………………………………………i Abstract ………………………………………………………………………ii 致謝……………………………………………………………………………..iii 目錄……………………………………………………………………………..iv 圖目錄……………………………………………………………………….viii 表目錄…………………………………………………………………………xiii. 第一章緒論…………………………………………………………………….1 1.1 前言………………………………………………………………1 1.2 文獻回顧…………………………………………………………4 1.2.1 定位研究之回顧………………..………………………4 1.2.2 規劃研究之回顧………………………………………11 1.2.3 控制研究之回顧………………………...…………….13 1.3 研究動機與目的…………………………………..……..………14 1.4 本論文之貢獻……………………………………………………14 1.5 本論文之架構……………………………………………..…..…15 第二章理論基礎………………………………………………………….......16 2.1 蒙地卡羅方法……………………………………………………16 iv.

(6) 2.2.1 預測……………………………………………………17 2.2.2 權重分配………………………………………………17 2.2.3 重新採樣………………………………………………18 2.2 A*演算法………………………………………………………….19 2.3 點陣圖與向量圖轉換機制………………………………………23 2.4 輪型機器人之結構分析並以約束條件推導運動模型..………24 2.5 離散型輪型機器人運動模型………………………………….25 2.6 梯形與 S 形速度控制訊號設計方法…………………………….31 第三章機器人控制…………………………………………………...............37 3.1 機器人控制架構………………………………………………....37 3.2 地圖向量化………………………………………………………37 3.2.1 角點偵測………………………………………………38 3.2.1.1 邊緣偵測…………………………………...38 3.1.1.2 搜尋候選點…………………………....…...40 3.1.1.3 集合候選點…………………………….......42 3.1.1.4 決定角點……………………………...........42 3.1.2 形狀構成…………………………….………………...43 3.1.3 地圖建立………………………………………………44 3.2 機器人定位………………………………………………………44 v.

(7) 3.2.1 演算法初始化…………………………………………45 3.2.2 預測(prediction)……………………………………….46 3.2.3 權重分配(weight assignment)………………………...47 3.2.3.1 環境模擬…………………………………...48 3.2.3.2 誤差計算…………………………………...51 3.2.3.3 權重計算……………...……………………53 3.2.4 重新採樣(resampling)………………..……………….54 3.2.5 重新分佈………………………………………………56 3.2.6 逆向收斂………………………………………………58 3.3 機器人路徑規劃…………………………………………………59 3.3.1 移動通行性函數設計…………………………………59 3.3.2 基於向量模型 A*路徑規劃演算法…………………..60 3.3.3 路徑之避障設計………………………………………61 3.3.4 路徑弧形優化……………………………………...….62 3.3.5 馬達參考轉速轉換機制………………………………64 第四章實驗設備……………………………………………………………...65 第五章實驗結果與討論……………………………………………………67 5.1 角點偵測與地圖向量化…………………………………………67 5.2 不同繪圖法在 matlab 上之時間花費比較………………………69 vi.

(8) 5.3 不同數量粒子隨機分布在地圖上………………………………70 5.4 比較有無去頭之均方根誤差於估算誤差之表現差異…………70 5.5 重新採樣參數對收斂速度影響之研究…………………………72 5.6 重新採樣參數對收斂成功率影響之研究………………………81 5.7 本論文提出之改定位法與其他定位演算法之性能比較………85 5.8 本論文路徑規劃之表現………………………………………..107 第六章結論及未來展望..…………………………………………………...109 參考文獻……………………………………………………………………...111. vii.

(9) 圖目錄圖1-1. 移動機器人的系統架構圖……………………………………..2. 圖1-2. 網狀地圖的移動方向…………………………………………3. 圖1-3. 蜂巢狀地圖的移動方向………………………………………3. 圖1-4. MCL 粒子收斂…………………………………………………5. 圖1-5. GSAMCL 粒子收斂……………………………………………5. 圖1-6. MCL 與 GSAMCL 誤差收斂圖…………………………………5. 圖1-7. 使用 3D 建模後與實際獲得之色彩與深度影像資訊…….........6. 圖1-8. 定位執行結果…………………………………………………7. 圖1-9. 似然函數構成之輪廓表面……………………………………..7. 圖1-10 定位演算流程圖………………………………………………7 圖1-11 機器人的運動更新虛擬碼……………………………………..8 圖1-12 不同參數設置之運動更新分布………………………………8 圖1-13 超音波感測裝置………………………………………………9 圖1-14 SAMCL 之運作流程圖………………………………………10 圖1-15 基於向量模型規劃路徑之結果……………………………….11 圖1-16 不同參數的安全花費對於地圖之影響………………………12 圖1-17 三種路徑規畫之結果…………………………………………13 圖2-1. 隨機分佈於地圖上粒子………………………………………16 viii.

(10) 圖2-2. 蒙地卡羅定位之結果…………………………………………16. 圖2-3. 蒙地卡羅定位之虛擬碼………………………………………18. 圖2-4. 以 A*演算法規劃而得之路徑範例…………………………19. 圖2-5. A*路徑規劃演算法之虛擬碼…………………………………22. 圖2-6. 機器人單步運動示意圖………………………………………28. 圖2-7. 機器人單步節點示意圖………………………………………28. 圖2-8. 梯形速度控制之 v-t 圖………………………………………31. 圖2-9. P0 示意圖………………………………………………………32. 圖2-10 S 型速度控制法之位置、速度、加速度、急衝度曲線圖………33 圖3-1. 地圖向量化運作流程圖………………………………………38. 圖3-2. 局部地圖與使用 Roberts、Prewitt、Sobel、LoG、Canny、zerocross、erode、dilate 方法邊緣偵測的結果……………………39. 圖3-3. 候選角點比較圖………………………………………………41. 圖3-4. 交代模糊不清的情況…………………………………………41. 圖3-5. 實際角點附近的候選角點……………………………………42. 圖3-6. 粒子以隨機均勻分布方式散布於向量地圖上………………45. 圖3-7. 環境模擬資訊…………………………………………………49. 圖3-8. 環境模擬的虛擬碼……………………………………………51. 圖3-9. 感測器誤差擴增問題範例示意圖…………………………....52 ix.

(11) 圖3-10 環境模擬的虛擬碼……………………………………………53 圖3-11 權重計算的虛擬碼……………………………………………53 圖3-12 節點定義於向量地圖之範例………………………………....61 圖3-13 機器人避障示意圖……………………………………………62 圖3-14 圓弧優化之各節點相對之幾何位置………………………....63 圖3-15 以弧形優化機制建立之路徑範例…………………………....64 圖4-1. X80SV 輪型機器人之外觀……………………………………65. 圖4-2. X80SV 輪型機器人之感測器安裝位置………………………65. 圖4-3. X80SV 輪型機器人之人機開發介面…………………………66. 圖5-1. 地圖向量化與角點偵測之結果………………………………68. 圖5-2. 不同數量粒子隨機分布在地圖上之結果……………………70. 圖5-3. 兩種計算感測誤差方式之誤差-步數圖……………………71. 圖5-4. 感測訊號範圍估測……………………………………………71. 圖5-5. 第 1-6 組數據之常態分佈圖…………………………………..74. 圖5-6. 第 7-12 組數據之常態分佈圖…………………………………75. 圖5-7. 第 13-18 組數據之常態分佈圖………………………………..76. 圖5-8. 重新採樣參數對收斂速度影響之第一次多重比較結果……77. 圖5-9. 重新採樣參數對收斂速度影響之第二次多重比較結果……78. 圖5-10 重新採樣參數對收斂速度影響之第三次多重比較結果……79 x.

(12) 圖5-11 重新採樣參數對收斂速度影響之第四次多重比較結果……80 圖5-12 重新採樣參數與收斂成功率影響之長條圖…………………82 圖5-13 感測器等級 1 之全域定位結果……………………………….86 圖5-14 感測器等級 1 之低速簡單路徑追蹤………………………….87 圖5-15 感測器等級 1 之高速複雜路徑追蹤………………………….88 圖5-16 感測器等級 1 之機器人綁架………………………………….89 圖5-17 感測器等級 2 之全域定位結果……………………………….90 圖5-18 感測器等級 2 之低速簡單路徑追蹤………………………….91 圖5-19 感測器等級 2 之高速複雜路徑追蹤………………………….92 圖5-20 感測器等級 2 之機器人綁架………………………………….93 圖5-21 感測器等級 3 之全域定位結果……………………………….94 圖5-22 感測器等級 3 之低速簡單路徑追蹤………………………….95 圖5-23 感測器等級 3 之高速複雜路徑追蹤………………………….96 圖5-24 感測器等級 3 之機器人綁架………………………………….97 圖5-25 感測器等級 4 之全域定位結果……………………………….98 圖5-26 感測器等級 4 之低速簡單路徑追蹤………………………….99 圖5-27 感測器等級 4 之高速複雜路徑追蹤………………………...100 圖5-28 感測器等級 4 之機器人綁架……………………………….101 圖5-29 實際感測數據之第 1 全域定位結果………………………102 xi.

(13) 圖5-30 實際感測數據之第 2 全域定位結果………………………...103 圖5-31 實際感測數據之路徑追蹤…………………………………..104 圖5-32 實際感測數據之機器人綁架………………………………..105 圖5-33 第一組路徑規劃結果比較…………………………………107 圖5-34 第二組路徑規劃結果比較…………………………………..108. xii.

(14) 表目錄表1-1. MCL、SAMCL 與 IMCLROE 之定位均方根誤差……………10. 表2-1. 各區段對應之運動函數…………………………………………….34. 表2-2. 各種狀況之限制條件與對應之參數表……………………....35. 表3-1. 位於圖 3-5 之最佳角點計算結果……………………………43. 表4-1. 各感測器之位置參數資訊……………………………………66. 表5-1. 不同繪製方法所花費之時間數據(單位：秒)…………………69. 表5-2. 不同繪圖法在 matlab 上時間花費之 ANOVA 分析結果……..69. 表5-3. 重新採樣參數與收斂速度影響之數據 1……………………72. 表5-4. 重新採樣參數與收斂速度影響之數據 2……………………73. 表5-5. 每個等級常態分布圖的相關係數……………………………73. 表5-6. 重新採樣參數對收斂速度影響之第一次 ANOVA 分析結果..77. 表5-7. 重新採樣參數對收斂速度影響之第二次 ANOVA 分析結果..78. 表5-8. 重新採樣參數對收斂速度影響之第三次 ANOVA 分析結果..79. 表5-9. 重新採樣參數對收斂速度影響之第四次 ANOVA 分析結果..80. 表5-10 重新採樣參數與收斂速度影響之數據………………………81 表5-11 重新採樣參數與收斂成功率影響之數據……………………81 表5-12 群組 1 與群組 2 的成功次數之 ANOVA 分析結果……………82 表5-13 整體迭代次數與 k 值關係表…………………………………84 xiii.

(15) 表5-14 四種定位演算法穩定性比較………………………………..106 表5-15 四種定位演算法性能比較…………………………………..106 表5-16 本論文提出方法與傳統 A*路徑規劃法之性能比較……….108. xiv.

(16) 第一章緒論 1.1 前言在科技快速進步的年代，電腦運算速度的提升與普及化，使得機器人應用的涵蓋範圍更加廣泛，從早期的軍事、工業領域乃至現今於家庭的生活、娛樂方面皆有展現。機器人根據是否具備移動能力分為移動型與固定型機器人，前者可自由移動而具備較佳的靈活性，因此經常應用於探索或服務領域；後者則擁有固定式基座，且無須自備電源而更加穩定，故在生產線上常見其蹤影，又因結構與人類的手臂相似而被稱為機械手臂。移動機器人的研究相當複雜而廣泛，包含硬體方面的機構設計、電路設計、機電整合，以及軟體方面的演算法設計、機器人控制與程式撰寫等，本論文著墨於控制領域，並且改良演算法的效率與表現性能。在機器人控制領域中，依照賦予之任務可分為定位、規劃與控制三大主題。「定位」是為了讓機器人獲得其當前位置，以提供控制與規劃所需資訊，廣義而言可視為系統之回授，用以估測機器人之狀態。「規劃」之目的在於事前設計任務執行之方式，並提供機器人完成任務所需之步驟。以本論文而言，路徑規劃是此主題的內容，但對於更為複雜之系統，還包括機器手臂之運動規劃或者群組機器人間之協調合作等方面。規劃機器人的執行方式後，還需使用回授資訊與參考訊號「控制」機器人的運行，使機器人能正確地執行任務，因此建構良好的控制方法是控制的核心關鍵。移動機器人的基本架構可以表示成圖 1-1，此圖說明前述三大主題彼此的關係。 X t 代表機器人在時間 t 的實際狀態， Z t 是機器人獲得的感測訊號， Rt 為規劃至目標點 G 的參考訊號，U t 為控制量， Xˆ t 則是估算的機器人狀態。一個良好的機器人系統，估算要與實際相符，即 Xˆ t  X t ，且亦與參考訊號相符，即 X t  Rt 。另外，規劃 Rt 的程序通常在機器人移動前執行，除非發生特殊事件，如非預期之障礙物入侵，否則不會在運動過程中調整。 1.

(17) 圖 1-1 移動機器人的系統架構圖本論文在移動機器人控制領域的三個主題皆有涉獵，但主要仍集中於定位與規劃兩個主題的演算法改良。在定位方面，蒙地卡羅方法(Monte Carlo method)因具備優異的抗干擾能力與執行效率，故本論文使用此演算法作為機器人定位之主要架構，並改善所面臨之問題。根據定義，蒙地卡羅方法用於定位時會分為感測與運動模型，藉由感測器蒐集環境數據，加以分析後推估機器人所在的位置是感測模型的任務；而透過馬達的回授估測機器人的移動量則為運動模型的工作。兩個模型間不斷地交換資訊，可以改善使用單一模型而導致的缺點，進而獲得更為精確的機器人位置。在感測模型估測的過程中，當粒子以當前位置模擬應得的感測環境時，會因為需要對照其所在的像素點之座標，而嚴重侷限解析度，使得地圖的解析度直接決定了粒子群之收斂程度。若意圖以增加地圖解析度的方式處理此問題時，卻也同時因為像素數量的增加，使得計算上的負擔更為沉重，所以本論文針對地圖的形式進行調整，以收斂的解析度不再受像素影響。在規劃方面，本論文的研究為路徑規劃。A*演算法與 Dijkstra 演算法都是能獲得最短路徑的演算法，所以適合在應用於路徑規劃。比較此二演算法，A*演算法屬於深度優先的方法，在搜尋的花費上會較屬於廣度優先的 Dijkstra 演算法更為節省，因而受更多使用者青睞，本論文亦使用 A*演算法實現路徑規劃。最常見的 A*路徑規劃然演算法，也是使用與定位相同的地圖格式，且每一個步伐都是在鄰近的像素間移動，造成路徑方向受像素的排列方式決定。以最常見的柵格地圖 2.

(18) 圖 1-2 網狀地圖的移動方向. 圖 1-3 蜂巢狀地圖的移動方向. 而言，此種移動方式只會有 8 種不同的方向，如圖 1-2；而較少見的蜂巢狀地圖則只剩 6 個。若考慮在一步內可移動兩個像素，也只分別提升至 16 與 12 個，此結果導致規劃的路徑充滿許多菱角，且方向的限制更不能滿足真正在幾何空間的最短路徑，此部分也可以藉由調整地圖的形式而獲得改善。綜觀前兩者所面臨的困擾，地圖的調整都可以針對問題進行處理，因此本論文也會說明調整的方式，而此方式可視為將地圖向量化的過程，即用向量圖形取代原先地圖中的邊界，其中使用影像處理技巧，如膨脹、侵蝕、角點偵測、邊緣偵測的延伸應用與像素計算、分析與決策等。向量後地圖所具備函數的表示，可以解決定位受限於解析度的問題，同時角點應用於路徑規劃上能大幅降低演算步驟以及前述的問題，使得演算法在性能與效能皆有所提升。除了地圖像量化外，本論文亦針對定位與路徑規劃的其他方面進行研究，包括輪型機器人的正逆向運動學、蒙地卡羅方法內的細步調整機制、路徑圓弧化乃至於控制方面，將在論文後續部分詳細說明其原理與應用方式。在前述方法之結合下，期望機器人能沿著路徑前進，在無碰觸障礙物之情況下到達目的地。本論文包含的模擬與實驗部分皆在 Matlab 中進行，機器人則使用 X80SV 。 3.

(19) 1.2 文獻回顧有關機器人控制的研究，基本圍繞著此三個主題進行，大部分的論文針對單一主題深入研究，其他少數則針對不同主題有跨領域研究，因此，本論文仍「定位」、「規劃」與「控制」進行分類，並依序介紹數篇相關之研究。. 1.2.1 定位研究之回顧定位可以讓機器人獲得在環境中的位置，使用全球定位系統 (Global Positioning System , GPS)是相當適合的方法之一，但仍有以下的問題需要克服： 1.. 以現有的技術仍然具有數公尺的誤差，不能提供更為精細的定位。. 2.. 室內環境因受到遮蔽物影響無法使用。. 由於本論文以室內環境作為考量，故不考慮全球定位系統而改用蒙地卡羅定位 (Monte Carlo localization, MCL)，此方法為蒙地卡羅方法的延伸應用，發展於 20 世紀中期由 John von Neumann, Nicholas, and Stanislaw Ulam[1]提出的一種非常重要的數值計算方法，而後續學者針對此方法進行改進。 Xiao Kang, KeJie Li and Wei Zhu[2]學者根據蒙地卡羅定位的基礎，將基因演算法的概念融入後而建構遺傳模擬退火蒙地卡羅定位法 (Genetic Simulated Annealing Monte Carlo Localization , GSAMCL)。此篇以感測器的訊號強度作為測距方式，同時將 GPS 位置資訊納入考量，並在預測與權重分配程序間，加入基因演算法，其中適應函數(fitness function)表示為  2 1   i     fitness  exp  RSSI d  RSSI  t pred i 2    2 d t  . (1-1). 其中訊號強度與距離的關係可表示為 RSSI (d )   A  10n ln d . 而距離 d ti 是 GPS 位置資訊 LGPS 與粒子座標 lti 的距離，即 4. (1-2).

(20) d ti  lti  LGPS. 圖 1-4 MCL 粒子收斂. (1-3). 圖 1-5 GSAMCL 粒子收斂. 圖 1-6 MCL 與 GSAMCL 誤差收斂圖圖 1-4 與 1-5 為改良前後粒子的收斂情形，圖 1-6 則是此二演算法之誤差響應圖。結果顯示，改良後的 GSAMCL 的收斂精確性與效能有所提升，誤差從原本的 3.4 公尺下降至 1.8 公尺，且收斂所需時間降低近百秒，此方面是得利於基因演算法的優勢與適當地整合，但也可以發現誤差仍然存在而不會再隨時間持續 5.

(21) 收斂，此為這篇論文提供的演算法仍然存在的問題。 Maurice F. Fallon, Hordur Johannsson and John J. Leonard[3]學者則提出以具色彩與深度攝影機(RGB-D)作為感測器，與立體環境模型比對而得位置資訊的蒙地卡羅方法。此篇貢獻在於擴展到立體結構上，且只使用單一攝影機進行回授即可定位，而非一般昂貴的雷射測距儀。圖 1-7 說明在模型與實際上得到的色彩與深度資訊，其中深度資訊與環境結構有關，作為定位的感測器資訊是相當適合的。. (a) 模擬色彩資訊. (b) 模擬深度資訊. (c) 實際色彩資訊. (d) 實際深度資訊. 圖 1-7 使用 3D 建模後與實際獲得之色彩與深度影像資訊圖 1-8 展示粒子群在此模型中定位結果，以及深度感測資訊與模型的擬合程度，藍色點為感測資訊應出現於模型中的座標，綠色點則為粒子的位置。圖 1-9 是計算地圖中各區域的似然函數(likelihood function)，進而繪製的曲面圖，越接近機器人實際位置的地方，感測資訊擬合的程度越高，也因此能使粒子往機器人移動，最終定位機器人的座標。 6.

(22) 圖 1-8 定位執行結果. 圖 1-9 似然函數構成之輪廓表面. Tae Gyun Kim and Nak Yong Ko[4]學者也是針對蒙地卡羅定位進行改良，包括加入輪型機器人的運動模型，以及考慮模型在具有誤差下演算法的調整方式。圖 1-10 為此論文的定位演算架構，第一步以馬達的光學編碼器回授資訊，在考慮輪胎與地面間沒有打滑情況下估算機器人的速度與角速度，並使用於第二步驟的運動更新；第三與第四步則與原本的演算法相同。. 圖 1-10 定位演算流程圖 7.

(23) 圖 1-11 機器人運動更新的虛擬碼. 此篇論文提供運動更新的虛擬碼於圖 1-11，其中  1 至  6 皆為高斯分佈模型亂數，表示運動模型的不確定性。使用不同的變異數配置，其運動模型的機率分布情形亦有所變化，圖 1-12 舉例不同參數的四種分布結果。. 圖 1-12 不同參數設置之運動更新分布 8.

(24) (a) 發射器. (b) 訊號產生器. (c) 接收器. 圖 1-13 超音波感測裝置此篇論文的感測器分為三個部分，首先訊號產生器產生訊號，其次由安裝於環境中四個角落的四個發射器發射訊號，最後再由安裝於機器人的接收器接收訊號，並以訊號強度推估機器人與發射器的距離。單一感測器與距離的模型如下：. d sensor  dtrue  fb  N 0, fbd2 . (1-4). f b  34.71  0.03dtrue. (1-5). f bd. 1.222  10 4  1.390d ture  2 1  0.163  7.680e 0.05d true. (1-6). 且機率分布表示如下：. . . N d sensor; f b  d true , f bd2 . 1 2f bd2. . e. d sensor  fb  dtrue 2 2 f bd. (1-7). 方程式(1-7)計算粒子的感測資訊於真實距離之機率，並用於建構蒙地卡羅定位之權重分配。此篇論文最後考慮在發射器失效下，是否仍具備定位能力。在具三個發射器下，僅影響粒子群收斂速度而仍然能夠成功定位；但僅剩二個發射器時，機器人在移動時能成功定位；僅一個發射器器時，無法成功定位。 9.

(25) Chen-Chien Hsu, Chia-Jui Kuo, Wen-Chung Kao[5]提出一種改良之蒙地卡羅定位法為 Improved Monte Carlo Localization with Robust Orientation Estimation 或簡稱為 IMCLROE ，此方法以自適應蒙地卡羅定位 (Self-Adapted Monte Carlo Localization, SAMCL)為基礎，並結合機器人方向預測機制，再使用更為強健之競爭法取代輪盤選擇法於蒙地卡羅定位法之重新取樣中。此篇與蒙地卡羅定位法、自適應蒙地卡羅定位法評估不同粒子數量在定位精度上之優劣如表 1-1，並以均方根誤差(Root mean square error, RMSE)進行比較，均方根誤差定義為：. RMSE ( xˆt ) . 1 NT. NT.  ( x  xˆ ) t 1. t. 2. t. (1-8). 此篇論文結果顯示不論粒子的數量多寡，IMCLROE 之表現優於 SAMCL 與標準的蒙地卡羅定位法，且在粒子數量較少時，IMCLROE 的表現優勢會更為顯著。. 圖 1-14 SAMCL 之運作流程圖表 1-1 MCL、SAMCL 與 IMCLROE 之定位均方根誤差. 10.

(26) 1.2.2 規劃研究之回顧在本論文中，路徑規劃是此主題的研究方向，其目的在於規劃一條路徑，可讓機器人順利地從起點移動至目的地。由於起點為機器人當前座標，故在全域定位後、機器人移動前，或者面臨突發狀況如障礙物入侵路徑時執行。完成路徑規劃後，便能讓機器人嘗試沿著路徑移動至終點。 Yajie Liu, Hangyu Wang and Jun Xie[6]學者提出一種基於向量模型的路徑規劃方法，考慮機器人與障礙物的形狀後，利用角點進行路徑規劃，最終得到一條由單一或多個線段構成路徑。此篇論文考慮機器人之尺寸，因此能夠保留足夠空間使機器人在移動時不會碰觸障礙物。圖 1-15 為此論文提供路徑規劃之結果，若空間很大時可以直線抵達終點，否則以轉折方式前進，但若空間過小時則因機器人尺寸的限制而無法產生路徑。. (a) 直線抵達. (b) 轉折抵達. (c) 轉折抵達. (d) 因空間過小而無法產生路徑. 圖 1-15 基於向量模型規劃路徑之結果 11.

(27) Chia-Jun Yu, Yi-Hong Chen, and Ching-Chang Wong[7]學者則提出兩個概念改良 A*路徑規劃演算法：第一為安全花費，靠近障礙物的花費會提高，(1-9)為安全花費函數，不同參數的設置的結果表現於圖 1-16；第二為轉彎花費，當規劃的路徑轉彎時 w  1.3，否則維持 w  1.0 。此設計使轉彎增加額外的花費，進而導致結果傾向轉彎次數少的路徑。方程式(1-10)為結合兩種改良後的結果。 2    Dwall n       exp      , if Dwall n   Dmax  D  safe(n)   max      0, else. g w (n)  w  g (n)  safe(n). (1-9). (1-10). (b)   150 ,   4.5 , and Dmax  30. (a)   150 ,   2.5 ,and Dmax  50. 圖 1-16 不同參數的安全花費對於地圖之影響圖 1-16 提供了三種路徑規劃結果，第一種使用 A*路徑規劃演算法；第二種為僅包含安全花費；最後一種則再涵蓋轉彎花費。A*路徑規劃演算法具備最短路徑，但轉彎次數多且靠近障礙物；考慮安全花費後則路徑變長但相對可以與障礙物保持距離；加入轉彎花費後，可使路徑更為平順，且對路徑長度影響不大。 12.

(28) (a) A*演算法. (b) 考慮安全花費. (c) 考慮安全與轉彎花費. 圖 1-17 三種路徑規畫之結果. 1.2.3 控制研究之回顧在控制方面，可再分為兩個子主題：機器人模型與控制器設計，前者可應用於定位與路徑規劃上，故本論文在此方面之研究以機器人模型為主。在 C. Ming Wang 學者發表的論文 Location Estimation And Uncertainty Analysis for Mobile Robots[8]中，推導輪型機器人的離散型式運動模型。此篇論文以在時間間隔內，機器人的運動皆可視為圓周運動，進而導出機器人移動量 xn , yn ,  T 與輪胎切線移動量 Dr , Dl T 的關係為(1-10)，其中 L 為輪距。 xn .   sin  / 2   Dn cos n1  n   / 2 2  .   sin  / 2  Dn sin  n1  n   / 2 2   Dr  Dl D  2 Dr  Dl   L y n . (1-11). Pu-Sheng Tsai, Ying-Hao Lin, and Chung-Fu Tsai 學者由機構自由度與約束條件推得運動模型，在輪胎與地面無打滑、機器人的從動輪保持在地面而無翻滾情形時，得到的結果為(1-11)，其中 b 為輪距之半， 為輪胎之角速度，r 為輪距半徑。 13.

(29)  r  r 2  x r   1  cos  2    r  r 2  y r   1  sin  2   r   1   2  2b. (1-12). 比較兩者模型，以及微分與差分之關係後，當時間間隔 t 趨近於 0 時，兩者的結果相同。然而實際在電腦上運算時皆為數位系統，當時間間隔變大時，會因離散模型是假設為圓周運動之故，滿足驅動輪在固定速度但有速差情況下之表現，而更接近真實的運動情形，因此本論文在實際應用上仍以(1-11)為主。. 1-3 研究動機與目的 1.. 本論文期望能實現移動機器人的定位、路徑規劃與追跡能力，並得以在室內環境進行自主式移動。. 2.. 在機器人定位中，改善使用點陣圖作為地圖儲存格式所面臨解析度之限制，以及克服定位失敗時所面臨無法脫離區域解的問題，並加速定位的收斂效率。. 3.. 在路徑規劃方面，改善規劃之路徑品質，包含具備迴避障礙物能力、移動更為迅速與順暢。. 1.4 本論文之貢獻 1.. 提供向量地圖與點陣地圖轉換機制，使定位與路徑規劃演算法可選用較具有優勢之地圖形式做為環境參考依據。. 2.. 改善蒙地卡羅定位法，藉由向量地圖之特性使演算法之解析度不受限於點陣地圖像素尺寸與數量，並修改給予擾動機制加速收斂效率，以及設計逃脫區域解機制。. 3.. 改善 A*路徑規劃演算法，以地圖中角點位置做為演算法之節點，使其能獲得 14.

(30) 幾何上最短路徑；透過影像形態學之侵蝕處理使規劃之路徑具備迴避障礙物能力；再透過路徑圓弧化機制，改善路徑方向不連續之問題。 4.. 藉由輪型機器人之運動模型，將規劃之路徑轉換為輪軸馬達之參考轉速與位置資訊，以提供為輸入之控制訊號。. 1.5 本論文之架構本論文區分為六個章節，各章節之標題及內容說明如下： [第一章]: 緒論說明研究背景與目標，並藉由參考文獻回顧，得知在過去相關領域研究中，移動機器人的定位與路徑規劃方式，以及對其效能之評估。 [第二章]: 理論基礎闡述本研究使用的演算法原理與基本介紹，包含蒙地卡羅方法(Monte Carlo method)、A*演算法、點陣圖與向量圖轉換機制、輪型機器人的正逆向運動學，以及梯形與 S 形速度控制訊號設計方法。 [第三章]: 機器人控制以第二章之理論基礎，建構或改良機器人定位、路徑規劃演算法，包含地圖轉換機制、建立蒙地卡羅定位與 A*演算法在向量地圖之演算機制、建立定位失敗逃脫機制、改善定位時給予擾動之機制、路徑弧形優化之計制。 [第四章]: 實驗設備介紹 X80SV 移動輪型機器人與人機介面開發環境。 [第五章]: 實驗結果與討論針對機器人控制中改良部分進行比較，並展現建立機制之進行成果。 [第六章]: 結論與未來展望根據整體研究的效果及實驗的目標達成程度進行結論，並說明未來目標。. 15.

(31) 第二章理論基礎 2.1 蒙地卡羅方法蒙地卡羅方法(Monte Carlo method)，又稱粒子濾波器(particle filter)，是一種以貝氏機率為基礎的數值方法，藉由迭代法求得近似解。由於此方法具備優異強健性，非常適合針對於計算過於複雜或者難以獲得解析解之問題，因此除了在工業領域外，在金融、數學、生物或者物理等領域亦有卓越之貢獻。在本篇中，蒙地卡羅方法被應用於機器人定位，因此亦被稱為蒙地卡羅定位法(Monte Carlo localization, MCL)，藉由環境感測器，如紅外線測距儀、雷射測距儀等，得到移動機器人與障礙物的距離，並用來預測機器人所在位置，以做為運動規劃、路徑規劃、或其他任務執行時所具備之必要資訊。. 圖 2-1 隨機分佈於地圖上粒子. 圖 2-2 蒙地卡羅定位之結果. 在此演算法中，粒子群 X t 代表著機器人可能出現位置的機率密度分布，並表示成 X t  {xti ; i  1,2,..., mt }，其中 xti 表示第 i 個粒子的座標， t 表示時間，在此演算法中亦為迭代次數，而 mt 則為在時間 t 時粒子的數量，可為定值或具可變性。考慮機器人移動在二維平面，且為直角座標系統，則每個粒子的座標表示為 16.

(32) xti  {xti , x , xti , y , xti , }，其中 xti , x、xti , y 分別表示兩軸方向的座標，xti , 則為面向的角度。此外，權重值是粒子的另一項變數，表示為 wti ，代表此粒子的信任程度。圖 2-1 顯示粒子群在演算法初始化時，以均勻隨機分布於地圖上，且每一個粒子的面向亦為隨機的，但已經先確保所有粒子都落於移動範圍內，隨著演算法的執行，最終粒子群會收斂至機器人周圍，如圖 2-2，因此能估測機器人的座標。蒙地卡羅定位法是一種迭代演算法，除了初始化外，其餘過程皆會在迭代中進行，藉由去蕪存菁的步驟逐步提升定位的品質。而迭代的過程則由三個程序組成，分別為預測、權重分配與重新分佈，並演算法依序執行。. 2.1.1 預測 (Prediction) 在預測的程序中，機器人藉由給出的控制命令估算其位置，並且將其應用於所有的粒子中，且由於此程序考慮的因素為機器人之移動量，而又可稱為運動更新(motion update)。對於第 i 個粒子而言，其位置可以表示成(2-1)，其中 xti1 表示此粒子的前次位置資訊， ut 為控制命令， p 為系統狀態轉移機率。. xti ~ p( xt | xti1 , ut ). (2-1). 以輪型機器人為例，最為常見而廣泛使用的 ut 是驅動輪的角度回授，進而估算機器人的移動量。然而在現實上，沒有任何致動器是完美的，且輪胎與地面間的滑動狀態也是難以估計的，因此在預測的程序中需要考慮噪動的部分，所以若沒有感測環境的感測器存在，其粒子群的分佈會隨時間發散。. 2.1.2 權重分配(Weight Assignment) 在權重分配的過程中，粒子會以其座標推估環境感測器應得到的數據，並與真正回授的感測器數據進行比對，進而估算權重值。因此，對於第 i 個粒子而言，. 17.

(33) 權重分配的過程可以表示成(2-2)，其中 z t 為感測器回授的資訊。相對於預測部分，此程序仰賴能探索環境的感測器，因此又稱為感測更新(sensor update)。. wti  p( zt | xti ). (2-2). 2.1.3 重新採樣(Resampling) 重新採樣是以權重值作為機率，從粒子群 X t 1 中抽取權重高的粒子並產生新一代的粒子群 X t，因此權重值會影響該粒子是否能迭代的主要因素。在此程序中，粒子群會因為新增、複製或刪除部分的粒子，使得往整體更佳的方向前進。上述的三個步驟會構成一個完整的迭代，且粒子群會隨著迭代次數的增加而逐漸收斂，最終得到機器人的位置，圖 2-2 即為圖 2-1 經過數次迭代後而獲得的結果。為了更進一步說明，本論文將蒙地卡羅定位迭代演算法的虛擬碼表示於圖 2-3，其中， m 為地圖資訊，並應用於權重分配的過程。 MCL_Algorithm( X t 1 , ut , zt , m ) set X t and X t as empty set set mt is the size of X t 1 for i  1 to mt. xti = prediction( xti1 , u t ). wti = weight_assigment( xti , z t , m ) Add < xti , wti > into X t end_for for i  1 to mt draw xti from X t with probability  wti add xti into X t end_for return X t. 圖 2-3 蒙地卡羅定位之虛擬碼 18.

(34) 2.2 A*演算法 A*演算法(A* algorithm)，又名 A 星演算法，是一種在有限節點的平面上求出最低通過成本的演算法，故廣泛應用於移動花費之計算，其他領域亦有如語音辨識之應用。A*演算法與 Dijkstra 演算法非常相似，皆可獲得全域最佳解，但因能進行啟發式的搜尋而相對更有效率，所以在本篇中將採用 A*演算法為演算基礎，圖 2-4 為一般常見的 A*演算法能提供之路徑。. 圖 2-4 以 A*演算法規劃而得之路徑範例此演算法包含兩大階段：搜尋與重構階段。在搜尋階段時，演算法會從起點開始依照既定流程蒐集、計算與更新通過之節點資訊；重複此過程並接觸終點時，則進入重構階段，即以蒐集之資訊還原成路徑。為說明演算流程，本論文定義節點 n 為任何可以通過的位置，起點與終點則分別為 n 0 與 n g ，因此演算法即為尋找一條從 n 0 開始至 n g 結束且花費最低的路徑；其次由基本定義得到花費函數 f (n)  g (n)  h(n) ，其中 f (n) 代表從起點至終點的整體預估花費，可由 g (n) 與 h(n) 相加而得， g (n) 則代表從起點 n 0 至節點 n 的實際最低花費，而 h(n) 則為節點. n 至終點 n g 的預估花費。此函數具有以下之特性：當不具備預估花費時，即 f (n)  g (n) ，則此演算法會轉變為 Dijkstra 演算法；若 h(n) 小於從節點 n 至終點 19.

(35) 的實際花費時，則必定可以得到最短路徑；若 h(n) 非常接近乃至於相等於實際花費時，則此演算法會具備最佳的效率；但若 h(n) 大於實際花費時，則不能保證獲得最佳解，因此在實務操作上，多以該節點至終點的直線距離定為該節點的 h(n) ，以確保能得最佳解。一般而言，每個節點至少需要包含以下資訊：節點座標、來源節點 c(n) 、實際最低花費 g (n) 與預估花費 h(n) ，其中來源節點是用來記錄通過此節點的上一個節點，於重構階段時所需之資訊。A*演算法的流程如下：第一步：建立開啟清單(OPEN list)與關閉清單(CLOSE list)。開啟清單是即將進行估算的節點之集合。由於演算法皆由起點開始，故初始時必定只包含初始節點 n 0 ；關閉清單則為已經被估算節點的集合，由於初始時未有任何節點已被估算，故為空集合。第二步：由於 g (n) 為起點 n 0 至節點 n 的實際最低花費，故可得 g (n 0 ) 必為 0。在此步驟同時估算 h(n 0 ) ，且因 g (n0 )  0 得到 f (n 0 )  h(n 0 ) 。第三步：建構一個包含第四步至第十四步的迴圈，當開啟清單非空集合時進入迴圈，表示此地圖仍然有從起點出發可抵達但尚未探索的區域，否則表示在此地圖中，無法建立一條從起點至終點的完整路徑，其中最常見的是分別包含起點與終點之區域已經被實質分割成兩區，且中間已無任何通道相連之狀況，因此需要回傳規劃失敗的訊息。第四步：從開啟清單找出具有最小的 f (n) 的節點，並將它定為當前節點 x 。第五步：判斷當前節點是否為終點 n g ，若是則表示完成搜尋階段，跳出由第三步建構的迴圈，直接進入第十五步之重構階段。第六步：從開啟清單中移除目標節點並加入至關閉清單。第七步：搜尋在當前節點附近且可達的節點並定義成 y 。. 20.

(36) 第八步：建構一個迴圈包含第八步至第十四步，在迴圈內針對每個節點 y i 進行判斷，直到所有鄰近的節點都判斷完畢。第九步：判斷節點 y i 是否已經在關閉清單內，若是則跳過此節點，直接進行下一個鄰近節點的判斷，否則繼續下一個步驟。第十步：估算節點 y i 的 gˆ (y i ) ，此函數定義為通過當前節點 x 的實際花費函數，並且可以表示成 gˆ (y i )  g (x)  dis(x, y i ) ，其中 dis() 函數表示兩個節點間的直線距離。第十一步：設定一個布林參數 k 為 false，用來記錄節點資訊是否需要更新。第十二步：判斷節點 y i 是否存在於開起清單內，若不在則將節點 y i 加入至開起清單，並將參數 k 設為 true，否則再次判斷 gˆ (y ) i 是否小於節點 y i 本身具有的 g (y ) i ，若小於則亦將參數 k 設為 true，以表示節點的資料需要新增或修改。第十三步：判斷參數 k ，若為 false 則不進行任何動作直接進行第十三步；若為 true 則進行資料更新，包含：設定節點 y i 的來源節點為 x ，表示是由當前節點 x 建立而得； g (y ) 由 gˆ (y ) 取代，表示有花費更低的路徑。第十四步：復歸參數 k 為 false 後即完成一次迴圈，然後回到第八步。第十五步：將終點 n g 存入暫存路徑 p 的第一個位置，即 p (1)  n g 。第十六步：建構一個迴圈，依照 p (i  1)  c(p (i)) 逐步將節點資訊填入暫存路徑，直到將起點加入至暫存路徑，即滿足 p (k )  n 0 。第十七步：將暫存路徑之順序翻轉，即 p(i)  p (k  i  1), i  1, 2, ... , k  1，便得正確順序之路徑 p ，即完成整個 A*演算法。 21.

(37) A*(start, goal) CLOSElist := empty set OPENlist := {start} g[start] = 0 while OPENlist is not empty x = the node in OPENlist with lowest f_cost if x = goal while came_from[x] is not start path.append(x) x = came_from[x] end_while return rev(path) end_if remove x from OPENlist add x to CLOSElist for each y in neighbor_nodes(x) if y in CLOSElist continue end_if gt = g[y]+dis(x,y) k = false if y is not in OPENlist add y to OPENlist k = true else if gt < g[y] k = true end_if if k = true came_from[y] = x g[y] = gt f[y] = g[y] + h(y, goal) end_if end_for end_while return failure. 圖 2-5 A*路徑規劃演算法之虛擬碼. 22.

(38) 從上述流程可得知，搜尋階段為第一至第十四步驟，重構階段為第十五至第十七步驟，而階段轉換位於第五步驟的跳脫條件，由於重構階段相對簡單而且無巢狀迴圈，因此演算時間花費集中於搜尋階段。此外，路徑是否能夠建立仰賴於搜尋階段的開啟清單之節點數量，演算法一方面持續地消耗開啟清單之節點，同時將地圖的節點加入至開啟清單內，一旦無節點可以加入開啟清單內，則開啟清單內之節點數量便會下降，若在被消耗殆盡前仍然無法接觸終點，意味著路徑建立失敗，因此雖於第三步建立非可預期長度之迴圈，但在正常操作下並不會造成演算法無窮循環。為了更詳盡地說明整體的運作流程楚，本論文提供此演算法之虛擬碼，表示於圖 2-5。由於 A*演算法必須建立於有限節點的平面上，並且需給予節點間的連結，因此問題本身要能網格化或者建立拓樸圖方得使用；而網格化的細膩程度直接權衡了路徑細膩度以及運算時間，這往往是開發者所需面臨之問題。. 2.3 點陣圖與向量圖轉換機制在機器人控制領域中，環境地圖是定位與路徑規劃不可或缺之資訊，且由於機器人多在平面上移動，因此以圖片作為儲存方法是被相當廣泛應用的。圖片的格式很多，依構成方式可分為點陣圖與向量圖兩類形式，其中點陣圖是使用像素陣列組合而得之影像，尺寸與像素數量相關；而向量圖則是由點、線條或幾何圖形構成之影像，故尺寸主要由影像複雜度決定。在傳統上由於操作方便、計算簡單與易於處理複雜地形，定位與路徑規劃所使用之地圖多建立於點陣圖的形式上，但相對可能會面臨品質與解析度等問題，便有以向量圖形為基礎之研究，使部分原先面臨的問題可以獲得解決或改善。然而，使用向量圖的方式卻可能導致產生大量數學及複雜的邏輯運算，或者需要針對各種情況分別處理，因此在操作與實現上相對困難。為了權衡兩種處理方法之優缺點，針對面臨情況採用較為方便或優異的方法，建立兩種圖片形式的轉換機制便有其價值存在。由於向量圖主要由點、線或幾何形狀構成，因此在點陣轉向量圖的過程，主 23.

(39) 要的演算法仰賴於角點與邊緣偵測，如 Harris 角點偵測與哈克轉換法而得，而轉換流程如下：先透過角點與邊緣判斷地圖中線段存在的位置，再透過多條線段組合而得幾何圖形，同時定義該區域是否為可通行區域，因此向量圖形可視為多個可移動與不可移動區域的組合。從向量圖轉換成點陣圖則相對簡單，首先決定網格密度，並依其建構對應之矩陣後，評估矩陣內每點對應位置是否在可移動範圍內而對每個元素賦值，便可得轉換後的點陣圖。由於目前已經有許多套裝軟體可以在兩種型式上轉換，因此對於靜態地圖而言已經相當容易取得兩種型式的地圖，甚可不需使用上述之轉換機制。然而，若要進行地圖的動態處理時，即考慮在機器人運行中地圖可能有所變化而須修改的情況，則必須要解決將機器人控制程式與轉換軟體相連的問題，或者使用上述之轉換機制撰寫轉換程式，動態建立所需之點陣或向量地圖。. 2.4 輪型機器人之結構分析並以約束條件推導運動模型在本研究中所使用之輪型機器人是由五個剛體組成，分別為平台、分別安裝於兩側之固定輪、後方之自由輪，以及前方可自由旋轉之攝影鏡頭。平台是機器人之主體結構，無線聽訊模組、馬達驅動電路、電池與環境感測器如紅外線、超音波距離感測器等皆安裝於此區域內；左右固定輪直接安裝於兩組獨立之伺服馬達且具有共同之軸心，藉由對其之驅動來控制機器人進行相關運動，並以光學編碼器回授角度資訊；自由輪則是用於平衡用，而不以動力驅動；攝影鏡頭能擷取機器人周遭之影像，並能藉由兩軸步進馬達改變鏡頭角度。本章節之目的在於獲得機器人之運動方程式，因此需先分析其結構，再以幾何與運動約束推導之。首先，針對機器人之每個剛體進行定義：令 body1 為平台，. body 2 與 body 3 分別為機器人左右兩側之固定輪， body 4 為自由輪，而 body 5 為攝影鏡頭，其中自由輪在不考慮摩擦力的情況下，與攝影鏡頭皆對運動影響甚微， 24.

(40) 因此在推導過程中並不加以考慮。此外，亦針對機器人之其他參數加以定義： wr 為車輪之半徑，而 L 則表示雙輪間距之一半。在三維之空間中，每個剛體需要以 6 個位形變數描述其移動與轉動關係，故在本研究中以變數. x  ( xi , yi , zi , i ,  i , i ), i  1, 2, 3 表示，其中 ( xi , yi , zi ) 描述該剛體在慣性座標軸之座標，(i ,  i ,i ) 則描述該剛體之姿態角，分別為滾轉角(roll angle)、俯仰角(pitch angle) 與方位角(yaw angle)，而 i 則是對應前敘之剛體編號。由於姿態角是定義於機器人上，故與其運動有關。在此定義 X 軸為機器人之移動方向，Z 軸為朝地心方向， Y 軸與 X、Z 軸則互成右旋座標系統，因此根據此定義可得 (i ,  i ,i ) 分別是對 X、 Y、Z 軸三軸旋轉之角度。由上述得知此機器人架構包含 18 個位形變數，下一步將透過約束條件推導運動方程式。首先考慮機器人運動於平坦之二維平面，因此相對於地面之高度不應有所變化，故可得到 3 組約束條件：. z1  c1 z 2  wr. (2-3). z 3  wr 其中 c1 為一個常數表示平台質心與地面之距離。由於機器人具備自由輪輔助平衡，若整體運動保持穩定，即平台沒有翻滾或側傾現象產生時，可得下列 2 個約束條件：. 1  0 1  0. (2-4). 考慮機器人之輪胎是直接安裝於伺服馬達上，並能與地面保持垂直，在無晃動之情況下，可得到下列 2 個約束條件： 25.

(41) 2  0 3  0. (2-5). 此外，因馬達具備共同之軸心並垂直於移動方向，因此車輪與平台之方向角一致而得到：.  2  1  3  1. (2-6). 再者，各剛體間之位置受機器人之結構限制，因此具有以下之約束條件：. x2  x1  d cos1  L sin 1 y 2  y1  d sin 1  L cos1 x3  x1  d cos1  L sin 1. (2-7). y3  y1  d sin 1  L cos1 以上探討 13 個幾何約束條件於機器人運動模型上，下一步將引入運動約束條件推導之。考慮車輪與地面間為純滾動狀態下，其移動速度會與本身之轉動角速度和機器人之移動方向有關，即下列 4 個運動約束條件：. x 2  wr  2 cos1 y 2  wr  2 sin 1 x 3  wr  3 cos1 y 3  wr  3 sin 1. (2-8). 從(2-7)與(2-8)可得到：. x2  x3  2L sin 1 將(2-9)微分後得到：. 26. (2-9).

(42) x 2  x3  2L cos1  1. (2-10). 將運動約束條件(2-8)代入並整理得： wr  2   3   2L1. (2-11). 由於此式可以積分，因此可以被轉變成幾何約束條件成. 1 . wr  2  3   c2 2L. (2-12). 其中 c2 為一常數，表示  1 之初始值。此外，從(2-8)可再得： x2  x3  2 x1  2d cos1 y 2  y3  2 y1  2d sin 1. (2-13). 分別將 x1 與 y1 移至等號左側並微分後得：. 1 x 2  x3   d cos1  1 2 1 y1   y 2  y 3   d cos1  1 2. x1 . (2-14). 將運動約束條件 (2-8)代入後並整理而得到：. wr  2  3  cos1  d cos1  1 2 w y1  r  2   3  sin 1  d cos1  1 2. x1 . (2-15). 相較於平台座標 ( x1 , y1 ) ，左右輪軸之中心參考點 Pr ( xr , yr ) 更常為使用，且由於車輪安裝於伺服馬達上，因此可以直接控制其運動，故在此令 2  u1 以及 3  u2 ，最終可以獲得機器人之運動模型： 27.

(43)  2  u1  3  u 2 wr u1  u 2  cos1 2 w y r  r u1  u 2  sin 1 2 w 1  r u1  u 2  2L x r . (2-16). 2.5 離散型輪型機器人運動模型在上一章節中，微分形式之運動模型已經導出，然因實作要求需轉換成離散系統，因此推導離散型差分方程式更能精確描述機器人之移動。首先，考慮當取樣間隔足夠小時，機器人在間隔內之移動可視為圓周運動，因此其運動示意圖可如圖 2-6 描述。由於 Dl 與 Dr 分別定義為左右輪在平面上之移動軌跡，因此 Dl 與. Dr 是其在時間間隔內之移動量， R 則是內輪之旋轉半徑，而 D 則對應至輪軸中心點 Pr ( xr , yr ) 。若考慮輪胎與地面間是純滾動狀態，則輪胎移動速度會與其轉動的切線速度相同，而有以下之關係： Dl  wr  2 Dr  wr  3. 圖 2-6 機器人單步運動示意圖. (2-17). 圖 2-7 機器人單步節點示意圖. 28.

(44) 且移動量與機器人之方向角之關係可描述成 Dl  R. Dr  R  2 L . (2-18). 由於輪軸中心點 Pr 之旋轉半徑為 R  L ，故 D  R  L ，且經過代換後可得：. D . Dl  Dr  2. (2-19). 此外，  亦可表示成：.  . Dr  Dl  2L. (2-20). 本論文加入參考點如圖 2-7，O 為旋轉中心，A 與 B 分別是移動之起點與終點，C 是 A 與 B 沿移動切線交點，而 E 是起點 A 朝 x 軸方向延伸之一點。此外， D' 定義為 A 與 B 之直線距離，假設 EAB 為  a，則 xr 與 yr 可以分別表示成：. xr  D' cos a  y r  D' sin a . (2-21). 由圖 2-7 所知， EAC 是機器人在前一個時間點之方向角，即 EAC   n  1 ，而根據幾何關係可以得到 CAB   / 2 ，因此我們有 xr  D' cos n  1   / 2 y r  D' sin n  1   / 2. (2-22). 此外， D' 與 D 可以分別被描述成：.    D'  2 sin   R  L   2  D    R  L  29. (2-23).

(45) 因此我們可以得到： D' . sin / 2 D  / 2. (2-24). 最終運動模型可以獲得並描述成：. xr . sin / 2    D cos n  1    / 2 2  . sin / 2    y r  D sin n  1    / 2 2  . (2-25). 或者以差分之形式表示 xr n   xr n  1 . sin / 2    D cos n  1    / 2 2  . sin / 2    D sin n  1    / 2 2   Dr  Dl   r n    r n  1  2L y r n   y r n  1 . (2-26). 然而在實際應用上，需要將規劃路徑轉換成馬達參考之輸入，因此需要以逆向運動學反推導出結果。由於路徑是規劃出來時， xr 、 yr 已經轉變為已知，而目標是要獲得  2 與 3 ，在參考(2-25)後可得： D . ( / 2) x 2  y 2 sin( / 2). (2-27). 因此再引入(2-18)至(2-0)後，即可獲得逆向運動學之最終結果：  2 . D  L wr. D  L  3  wr 30. (2-28).

(46) 2.6 梯形與 S 形速度控制訊號設計方法梯形速度控制具備加減速之設計，故是一種經常被使用於馬達加減速控制的方式，因速度訊號具備連續性，使得馬達運轉具備更佳的穩定性；而 S 形速度則是梯形速度的進一步優化，使馬達速度訊號具備連續可微分性，而在速度轉折處有更好的表現，且本身因具備位置的二階可微分性，使得在控制器設計時得以實現平滑的微分項。梯形速度控制的速度時間圖如圖 2-8 所示，其中包含的參數有：最大速度限制 vmax 、加速度 a a 與減加速度 ad 、速度變換時間點 t1 、 t 2 與 t 3 ，以及預定之位移量 P。由於 P 值之正負號不影響時間點之計算，故以 P  0 之情況討論之，而 P  0 可. 藉由 P  0 之模式類推而得。對於梯形速度控制而言，其結果可分成兩種情況：第一種為速度已被限制於 vmax 而具有等速度時段，另一種則無法或恰好達到 vmax 而僅有等加與等減速度區段。為區分兩種狀況，首先計算 a a 以加速至 vmax 並以 a d 減速至 0 之位移量，並定義為 P0 ： 2 2 vmax vmax P0   2a a 2a d. 圖 2-8 梯形速度控制之 v-t 圖. (2-29). 圖 2-9 P0 示意圖. 31.

(47) 圖 2-9 為 P0 所代表意義之示意圖。我們可藉由 P0 來區分兩種狀況，若 P0  P ，則為第一種具有等速時段之結果，維持等速之時間，即 t 2  t1 可以滿足. P  P0  vmax  t 2  t1 . (2-30). 因此可得時間點分別如下：. t1 . v max aa. P  P0 . t 2  t1 . v max. t3  t 2 . (2-31). v max ad. 但若 P0  P ，即為第二種不具有等速時段之結果，時間點之計算分別如下：. t1 . v max aa. P P0. t 2  t1 t3  t 2 . (2-32) v max ad. P P0. 由速度變換時間點 t1 、t 2 與 t 3 進行速度模式的切換，可以設計梯形速度控制訊號。相對於梯型速度控制法，S 型速度控制法具有 7 段不同之速度函數，以及 3 個限制參數。起始、終止與切換時間點分別由 t 0 至 t 7 表示；3 個限制參數分別為：最大速度限制 v M 、最大加速度限制 a M 與最大急衝度限制 j M 。其他的參數則定義為移動量 xt 、急衝度時間 t s 、加速度時間 t a 與運動時間 tt 。每一個參數對應之關係表示於圖 2-10，並將每段運動方程式描述於表 2-1。根據條件可分成五種狀況，每種狀況建立時間與限制參數之關係與修正之限制參數整理於表 2-2。 32.

(48) (a) 位置-時間曲線圖. (b) 速度-時間曲線圖. .. (c) 加速度-時間曲線圖. . (d) 急衝度-時間曲線圖. 圖 2-10 S 型速度控制法之位置、速度、加速度、急衝度曲線圖. 33.

(49) 表 2-1 各區段對應之運動函數時間範圍. x(t ) . v(t ) . a(t ) . j (t ) . t0  t  t1. 1 jM t 3 6. 1 jM t 2 2. jM t. jM. v1  aM t  t1 . aM. 0. aM  jM t  t 2 .  jM. 0. 0.  jM t  t4 .  jM.  aM. 0. t1  t  t2. t 2  t  t3. t3  t  t 4. t 4  t  t5. t5  t  t6. t6  t  t7. x1  v1 (t  t1 ) 1 2 a M t  t1  2 x2  v2 (t  t 2 ). . 1 2 a M t  t 2  2 1 3  j M t  t 3  6 . x3  vM t  t3  x4  vM t  t 4  1  j M (t  t 4 ) 3 6 x5  v2 t  t 5  1 2 a M t  t 5  2 x6  v1 t  t 6 . . 1 2  a M t  t 6  2 1 3  j M t  t 6  6. v2  a M t  t 2  . 1 2 j M t  t 2  2. vM. vM . 1 2 jM t  t4  2. v2  aM t  t5 . v1  a M t  t 6  . 1 2 j M t  t 6  2. 34.  aM  jM t  t6 . jM.

(50) 表 2-2 各種狀況之限制條件與對應之參數表 2. 限制條件. 時間參數. 修正限制參數. ta. t s  vM a M. 1. tt. t a  xt vM. 1. aM. aM. vM. vM 距離. 2. 2. 2a M v v a  xt  M  M M 2 aM jM jM. 速度 vM . aM jM. 1. ts. aM jM. ta. t s  vM a M. 1. tt. 2ta. aM. aM. vM. xt  t  t a . 限制條件. 時間參數. 1. aM jM. 3. 修正限制參數. a 速度 vM  M jM. ts. 限制條件. 時間參數. 2. v v a 距離 xt  M  M M aM jM. 1. 2a 距離 xt  M2 jM. 3. 2. a 速度 vM  M jM. ts. 3. ta. xt 2 jM 2ts. 35.