運用雷射測距儀之機器人定位設計

國立交通大學

電機與控制工程學系

碩士論文

運用雷射測距儀之機器人定位設計

Mobile Robot Localization Using

A Laser Range Finder

研 究 生：鍾鎮謙

指導教授：宋開泰 博士

運用雷射測距儀之機器人定位設計

Mobile Robot Localization Using A Laser Range Finder

研 究 生：鍾鎮謙 Student: Chen-Chien Chung

指導教授：宋開泰 博士 Advisor: Dr. Kai-Tai Song

國 立 交 通 大 學 電 機 與 控 制 工 程 學 系

碩 士 論 文

A Thesis

Submitted to Department of Electrical and Control Engineering College of Electrical and Computer Engineering

National Chiao Tung University in Partial Fulfillment of the Requirements

for the Degree of Master in

Electrical and Control Engineering September 2006

Hsinchu, Taiwan, Republic of China

運用雷射測距儀之機器人定位設計

學生:鍾鎮謙 指導教授:宋開泰 博士 國立交通大學電機與控制工程學系

摘要

智慧型機器人必須有在移動環境中自我定位的能力，以達成各種實際應用。 本論文的目的在於設計一機器人之自我定位系統來估測機器人在環境中的姿 態。本論文利用粒子濾波器設計位置估測的方法，藉由里程計的資訊先對機器人 的位置做預測，再經由從距離感測器對環境的量測結果得到的機器人位置資訊來 對預測的機器人位置做修正，最後由粒子濾波器估測出機器人的位置。本論文並 以將感測資訊與環境地圖做比對的方式估測朝向角，將雷射掃描儀量測的環境距 離資料以最小平方法擬合成包含若干線段的線段組，再結合機器人的位置資訊與 內建的環境地圖做比對，藉由比對結果可以估測出機器人的朝向角。本論文使用 的距離感測器為雷射掃描儀，其具有相當高的精確度與可靠度，利用它來得到用 來擬合成線段的環境距離資料。最後經由機器人繞行環境實驗的驗證，結果顯示 所設計的機器人之定位系統確實有預期的功效。

Mobile Robot Localization Using A Laser Range Finder

Student: Chen-Chien Chung Advisor: Dr. Kai-Tai Song

Department of Electrical and Control Engineering National Chiao Tung University

ABSTRACT

Mobile robots should have the ability of self-localization for navigation in an environment. The objective of this thesis is to design a mobile robot localization system using a laser scanner. A particle filter is designed to estimate robot’s position in this study. The particle filter predicts robot’s position from odometer data, and updates the predicted robot’s position by using the distance measurements of the environment. The localization system estimates robot’s orientation by comparing the information from laser range finder with the environmental map. We can estimate the robot’s orientation by first fitting the distance data from laser range finder to a line set, then combining the line set with robot’s position information to match with the environment map. The system has been demonstrated using an experimental mobile robot. Several navigation experiments in an indoor environment have been carried out. Satisfactory experimental results are presented in this thesis.

誌謝

謹向我的指導老師宋開泰教授致上感謝之意，感謝他二年來的指導，以他豐富的學 識與經驗，使本論文得以順利完成。感謝口試委員胡竹生教授、陳傳生教授與王傑智教 授的指導及建議，讓本論文能更加的完整。 感謝學長任詔、奇謚、孟儒、嘉豪和奕龍在理論與實作上的指點，給予我許多寶貴 的經驗。感謝與我一起奮鬥的同學晉懷、忠憲、宏宜及富全的相互鼓勵與提攜。感謝學 弟富聖、俊瑋、振暘、濬尉、志昇與裕宏在生活上帶來的樂趣，及對於完成此論文所提 供的協助。感謝我的朋友們，在我覺得沒力時，給我鼓勵與打氣。感謝所有在交大幫助 過我的人，讓我能更順利地完成學業。 感謝我的兩位哥哥，在我的成長過程中各方面的關照。最後，特別感謝我的父母， 由於他們的辛苦栽培，在生活上給予我細心地關懷與照料，使我才能夠順利完成此論 文，在此我願以此論文獻給我最感激的父母親。

目錄

中文摘要 ... i 英文摘要 ...ii 誌謝 ...iii 目錄 ... iv 圖例 ... vi 表格 ...viii 第一章 緒論 ... 1 1.1 研究動機…. ... 1 1.2 相關研究回顧. ... 1 1.3 問題描述 ... 6 1.4 章節說明 ... 6 第二章 粒子濾波器……….………7 2.1 貝氏濾波器………7 2.2 粒子濾波器原理………11 第三章 自走式機器人之定位設計.……….…….16 3.1 設計概念…….………..………16 3.2 位置估測方法..……….………..………19 3.2.1 預測(Prediction) ………..………...19 3.2.2 更新(Update)………..22 3.3 朝向角估測方法………..………....24 3.3.1 線段擷取.………25 3.3.2 線段比對……….…29 第四章 模擬與實驗結果..………..……….32 4.1 機器人定位系統硬體架構機器人系統………..……32

4.1.1 機器人系統……….…32 4.1.2 雷射掃描儀………..33 4.2 模擬結果……...………...………37 4.3 實驗結果………...43 4.3.1 繞行實驗(一)…………..………..……….…….43 4.3.2 繞行實驗(二)………..……….………...48 4.3.3 繞行實驗(三)………..……….………...50 第五章 結論與未來展望………...53 5.1 結論………..53 5.2 未來展望………..53 參考文獻………...………55

圖例

圖 1-1 機器人姿態定義………..2 圖 1-2 基於 Bayesian filter 之定位演算法架構圖[9]……….5 圖 2-1 動態貝氏網路示意圖……….………...8 圖 3-1 機器人對其位置的不確定性範圍示意圖………..……16 圖 3-2 自走式機器人定位系統架構圖………..….18 圖 3-3 擷取線段座標轉換關係圖………22 圖 3-4 線段方程式參數定義示意圖..………...25 圖 3-5 判斷資料點是否包含於線段之示意圖………..……….……..27 圖 3-6 線段擷取流程圖……….………...28 圖 3-7 機器人在環境中擷取線段…..……….28 圖 4-1 機器人硬體裝置架構圖………..……….32 圖 4-2 機器人系統控制架構圖..……….33 圖 4-3 雷射掃描儀………..…..34 圖 4-4 雷射掃描儀工作原理[11]………...34 圖 4-5 雷射掃描儀角度掃描範圍[11]………..………..….…….34 圖 4-6 雷射掃描儀量測結果………...…37 圖 4-7 模擬環境與機器人繞行軌跡……….…..……….………….…..……37 圖 4-8 模擬一之機器人定位系統姿態估測誤差..………….……….….39 圖 4-9 模擬二之機器人定位系統姿態估測誤差……...………...………....………...41 圖 4-10 模擬三之機器人定位系統姿態估測誤差………...………..………...42 圖 4-11 繞行實驗(一)之環境示意圖…….……….………...43 圖 4-12 機器人與目標點相對位置示意圖………44 圖 4-13 繞行實驗(一)之實驗過程…….……….…46 圖 4-14 繞行實驗(一)之結果….………..….47

圖 4-15 繞行實驗(二)之環境示意圖..……….48

圖 4-16 繞行實驗(二)之結果……….……..49

圖 4-17 繞行實驗(三)之環境示意圖…..……….….50

表格

表 3-1 機器人在圖 3-7 環境中擷取的線段資料………...……...….29 表 4-1 要求回傳量測值指令格式[11]………..………...……...35 表 4-2 要求回傳量測值命令之反應[11]………...36 表 4-3 模擬一之機器人姿態估測絕對誤差………..39 表 4-4 模擬二未加雜訊部分之機器人姿態估測絕對誤差………..41 表 4-5 模擬二加雜訊部分之機器人姿態估測絕對誤差……..…..………..41 表 4-6 模擬三之機器人姿態估測絕對誤差………42

第一章 緒論

1.1 研究動機

對於一個多功能的智慧型機器人來說，自我定位是其必備的功能之一。智慧 型機器人能夠自我定位，即它能知道自己目前在所處環境中的位置，機器人就可 以在一個平面空間裡自由移動。有了自我定位的能力，機器人可以執行的任務將 增加許多，並且更加能夠展現其智慧型行為的能力。例如，人可以命令機器人到 指定的地方去取物品、可以引領人到平面空間中的任何人們想要到達的地方。拓 展了機器人的應用面。 未來可將有定位能力的機器人應用於保全工作中，藉由機器人的自主移 動，讓機器人巡邏整個環境區域。由於機器人能夠自我定位，機器人可以持續的 巡邏而不會迷失自己的方位。另外，具定位能力之機器人亦可應用於家庭看護 中，對於行動不便的老年人，機器人可以輔助其移動或移動到家庭中的某一地點 去取得老年人指定的物品。而機器人要能夠移動到家庭中的某個地點，首先它要 知道自己目前所處的位置，亦即要有自我定位功能的機器人才能夠來完成這項任 務。除此之外，亦可將有定位能力的機器人應用於博物館中，作為導覽機器人， 可以帶領遊客到博物館內的各個區域參觀，當遊客迷失方位的時候，也可藉由機 器人的帶領，而到達遊客想參觀的區域。 以上的應用皆須藉著機器人的自我定位與對於周遭環境的了解能力才能達 成，即機器人能夠知道自己目前的位置且對於周遭的環境能探索了解而不會迷失 自己的方位。所以這個研究主題是相當有趣且重要的，值得去做進一步的探討。

1.2 相關研究回顧

自走式機器人定位問題是要求得機器人在移動環境中的姿態(Pose)，即機器 人的位置與朝向角，(x,y,θ)值，其定義如圖 1-1 所示。其中朝向角定義為機器

人正前方與座標系x軸的夾角。 X Y Θ Robot O _x y Oc (x,y,Θ) Heading dirction 圖 1-1 機器人姿態定義 使 用 里 程 計 (odometer) 的 資 訊 來 達 成 機 器 人 自 我 定 位 的 方 法 稱 作 dead-reckoning。由輪型機器人兩個輪子個別轉動的圈數，並可將其轉換成機器 人左右輪個別的線性移動距離，加上已知的前一時刻的位置與朝向角資訊，便可 以估測出機器人目前所在的位置。不過，里程計會受到輪子打滑與路面不平的影 響，使得從里程計得到的機器人移動距離與實際上機器人的移動情形不一致，並 且由此所造成的機器人位置估測誤差是不斷地累積的，所以 dead-reckoning 只適 合在短距離移動的機器人應用中。為使機器人在更長距離的移動中能持續地自我 定位，加入其它感測器的使用，以及一套有效的絕對位置定位演算法是必要的。 在以地圖為基礎的定位方法中，以內建的全域地圖與外部環境感測器的測量 資料來決定機器人的姿態。機器人將由感測器得到的局部地圖與全域地圖做比對 (Map-matching)，以全域地圖為基準來修正目前機器人的姿態。在相關文獻中， 外部環境感測器可以是影像感測器(Image sensor)[1]、超音波感測器(Ultrasonic sensor)[2]或雷射掃描儀(Laser scanner)等。在這些感測器中，雷射掃描儀因有高 精確度的優點，所以廣泛的被運用。使用雷射掃描儀為感測器而產生的定位演算 法 可 以 依 用 來 比 對 的 資 料 型 態 不 同 而 分 成 point-based 定 位 演 算 法 [3] 與 feature-based 定位演算法[4]。Point-based 定位演算法比較連續兩筆掃描資料，運 用最小平方法得到點對點(Point-to-point)的最佳對應關係，並以此推算機器人的

姿態。另一方面，feature-based 定位演算法則從掃描的距離資料中擷取環境特徵， 如牆壁或牆角，將擷取出的特徵和全域地圖比對來得到機器人的姿態。 本論文設計的定位系統將使用 feature-based 的地圖比對方法，以環境中的牆 面作為環境特徵，從雷射掃描的距離資料擷取出代表牆面的線段並與環境地圖比 對並由比對結果來估測機器人的朝向角。與 point-based 的方法比較，feature-based 方法的比對結果其精確度較高，這是因為 feature-based 方法是以掃描資料整體的 形狀(如代表牆面的形狀)作為地圖比對的依據，而 point-based 方法則是個別的去 比對每一筆距離資料，又個別的距離資料會受到量測誤差的影響，降低了比對結 果的正確性，所以 point-based 方法的比對結果並不如 feature-based 方法來得精 確。不過，point-based 方法可以使用在非多邊形(Non-polygonal)為主的環境中， 而本論文使用的 feature-based 方法則只能運用在多邊形(Polygonal)為主的環境 中。 另外有些研究以在環境中設置地標的方式，透過感測器對地標的量測來得到 機器人在環境中的姿態資訊。因此必須要先記錄所設置之地標在環境中的位置， 則機器人對於地標的量測資料才能成為有用的定位資訊。文獻[5]採用影像處理 的方式，在環境中設置特定顏色圖案的地標，讓機器人上的影像感測器能在環境 中辨認出此地標，並對地標做量測來得到機器人的姿態資訊。文獻[6]則以圓柱 狀的物體作為地標設置在環境中，並使用雷射掃描儀作為感測器來辨認圓柱狀地 標的幾何形狀。辨認出地標後便可得到機器人的姿態資訊。 許多的研究以機率理論來解決機器人定位問題。目前以機率理論發展出來的 機器人定位演算法中，主要有卡曼濾波器(Kalman Filter)演算法[7]和 MCL(Monte Carlo Localization)演算法[8]。因為機器人在對其所在位置做估測時存在著不確定 性，為了要將此不確定性以數學來表示，所以需要以機率理論來作為定位演算法 的理論基礎。另外，機器人在估測其所在位置時，必需將裝在機器人上的感測器 所得到資訊作為輸入，經過定位演算法處理後才能估測出機器人之所在位置。不 過，感測器在做測量時必定存在著量測誤差，而量測誤差雜訊經常是複雜的且不

容易將它對測量資訊所造成的影響消除。在機率理論中將雜訊以隨機變數 (Random Variable)來表示，所謂的隨機變數即是它在每一時刻的數值皆是隨機 的，並以機率密度函數(Probability Density Function)來表示，所以以量測雜訊的 機率密度函數來當作感測器的感測誤差模型，便可以處理感測誤差所造成的影 響。而這是以機率理論來建構機器人定位演算法的另一個主要原因。由以上的這 些原因，不難看出為何以機率統計的方法來解決機器人定位問題會如此重要了。 卡曼濾波器(Kalman Filter)演算法利用了目前機器人的感測器量得的資訊以 及機器人目前的位置來推測機器人在下一時刻的位置，以達成機器人的自我定 位。所以我們可以發現在使用卡曼濾波器演算法來解決機器人自我定位問題時， 我們必須要有機器人的運動模型(Motion Model)以及感測器的感測誤差模型 (Measurement Model)才能對機器人的位置進行估測。在卡曼濾波器演算法中假設 所使用的機率模型為高斯分佈(Gaussian Distribution)，亦即常態分佈(Normal Distribution)，而後經過遞迴的運算來估測出機器人的位置狀態。卡曼濾波器定 位演算法其優點為精確度高且收斂速度快，而缺點為使用的機率模型必須是高斯 機率分佈，若不是高斯分佈，則其估測結果的準確度將會降低。 MCL 演算法，亦即以粒子濾波器(Particle Filter)理論為基礎產生的機器人定 位演算法，是以取樣(Sampling)的方式(Sample-based)來表示機器人姿態的機率密 度函數，並給予每一個取樣點(Sample)或稱為粒子(Particle)一個機率值，隨後再 加入感測器的資訊對每一個粒子給予不同的權重(Weight)，最後再對所有的取樣 點重新取樣(Resampling)，而重新取樣後得到的新的一組取樣點將成為下一個循 環的開始。所以 MCL 的定位演算法以不斷地取樣、給予取樣點權重以及重新取 樣的方式來對機器人的位置狀態做估測與修正。MLC 定位演算法的優點在於能 夠表示具有多種或不同的機率密度函數，這是因為 MLC 演算法是以取樣的方式 來表示機器人姿態的機率密度函數。 將卡曼濾波器與 MCL 這兩種定位演算法做比較，可以發現卡曼濾波器定位 演算法只適用於機率模型為高斯分佈(Gaussian distribution)或接近於高斯分佈的

系統，而 MCL 演算法則可以適用與具有多種或不同機率模型之系統中。不過， 從另一方面來看，只要卡曼濾波器演算法其使用的機率模型接近高斯分佈，則其 所得到的位置狀態(State)估測結果將會比 MLC 演算法要來得精確，而且機率模 型愈接近高斯分佈，使用卡曼濾波器演算法得到的位置狀態估測結果將會愈精 確。雖然卡曼濾波器演算法比 MCL 演算法還要早被提出，但是兩種用來定位的 演算法各有其長處，所以個別皆有不少的專家、學者在進行研究與運用來解決機 器人的自我定位問題，甚至持續地提出許多改良的法方。 不論是粒子濾波器演算法或卡曼濾波器演算法，皆是以 Bayesian filter 為基 本架構發展而來的。在 Bayesian filter 演算法中，主要分成兩個步驟，分別是預 測(Pridict)和更新(Update)。因此，在以粒子濾波器演算法或卡曼濾波器演算法來 解決機器人定位問題的過程中，也可被分成預測階段(Prediction)與感測器資訊更 新階段(Measurement update)，如圖 1-2 所示。 在 Prediction 階段，定位演算法利用 odometer 的資料來對機器人的位置資訊 做預測。不過以 odometer 的資料來估測機器人的位置，會因為路面不平或輪子 打滑的影響而產生誤差，並且誤差會隨著機器人的移動而持續累積。所以在 Update 階段時，必須依靠其它感測器得到的資訊來對在 Prediction 階段預測出的 機器人位置做修正，如此才能正確地估測出機器人的位置。而在 Update 階段所 使用的感測器一般來說大部分是影像感測器、超音波感測器或雷射掃描儀等。 圖 1-2 : 基於 Bayesian filter 之定位演算法架構圖[9] Odometry Sensors

1.3 問題描述

為了讓自走式機器人能夠完成更多樣的任務與應用，自走式機器人必須要有 在環境中自我定位的能力。因此，如何能讓機器人得到它所處的環境資訊與在所 處的環境區域中能夠自我定位，即機器人能知道自己目前所處的位置及朝向角， 是這個研究主題所要達成的目標。 以 Dead-reckoning 來估測機器人的姿態，會有累積誤差的問題，造成所估測 的機器人姿態誤差愈來愈大。因此，在定位系統中需加入其它感測器的使用以及 有效的定位演算法，才能在機器人長距離的移動過程中不使估測誤差累積，而仍 可有效的估測機器人姿態。 文獻[5][6]在環境中設置地標，藉由感測器對地標的量測，來得到機器人在 環境中的姿態參考資訊。此法雖然實用，不過當機器人移動到新的環境時，就需 重新設置地標，使機器人定位系統的適用環境不夠彈性。若能從感測器的資訊擷 取出環境中原有的特徵，並將此環境特徵當作修正機器人姿態的參考資訊，便可 不用設立特定地標。在一般室內環境中，牆壁為主要的結構，而牆壁可以簡單的 線段來表示，所以採用牆壁構成的線段當作環境特徵。因此，本論文希望以粒子 濾波器演算法為基礎結合 feature-based 的地圖比對方式，來設計機器人在已知初 始姿態與環璄地圖資訊的情況下的自我定位系統。

1.4 章節說明

本論文共分為五章。第一章為緒論，介紹研究動機與相關的研究背景。第二 章介紹粒子濾波器的理論背景，並包含貝氏濾波器的介紹。第三章介紹以粒子濾 波器為基礎的機器人定位演算法設計與機器人朝向角的估測方法。第四章介紹模 擬與實驗結果，以驗證定位系統的設計。第五章為結論與未來工作。

第二章 粒子濾波器(Particle Filter)

本章之重點在於介紹粒子濾波器的理論基礎。粒子濾波器演算法是由貝氏濾 波器(Bayesian filter)演算法衍生而來的，故先介紹貝氏濾波器演算法，說明貝氏 濾波器演算法如何的以遞迴(Recursive)的方式來對系統狀態做估測。隨後才是粒 子濾波器演算法理論的介紹。

2.1 貝氏濾波器(Bayesian Filter)

在介紹貝氏濾波器演算法[12]之前，必須先說明幾個貝氏濾波器應用在自走 式機器人領域的重要名詞，以便後續的介紹：

(1) 狀態轉移機率(State transition probability)：

以條件機率函數p(x_t |x_t−1,u_t)來表示，其中x 與_t x_t−1分別表示在 t 和 t-1 時刻 的系統狀態，在機器人定位系統中即是機器人在 t 與 t-1 時刻的姿態(Pose)， 而u 則是在 t 時刻的機器人控制命令。此機率函數說明了在已知目前控制命_t 令與前一時刻機器人狀態的條件下，目前機器人狀態的機率分佈情形。 (2) 量測機率(Measurement probability)： 以條件機率函數p(z_t|x_t)來表示，其中z 為系統在 t 時刻的量測資訊，在機_t 器人定位系統中即是機器人在時刻 t 由感測器測得的感測資訊。量測機率函 數說明了機器人在目前的狀態下(x )，從感測器量得的環境資訊的機率分佈_t t z 。 (3) 相信度函數(Belief)： 以bel(x_t)符號表示狀態x 的相信度函數，並可寫成 _t

) , | ( ) (x_t p x_t z_1:t u_1:t bel = (2-1) 其中p(x_t|z_1:t,u_1:t)通常稱作 Posterior，是系統狀態x 在已知所有過去的感測_t 資訊z_1:t與控制命令u_1:t的情況下的機率分佈函數。在未包含 t 時刻的量測資

訊z 前的 Posterior 通常稱做 Prior，而未包含_t z 的相信度函數以_t bel(x_t)符號 表示，可寫成如下的式子： t t t t t p x z u x bel( )= ( | _1:−1, _1:) (2-2) 在貝氏濾波器演算法中bel(xt)是 Prediction 階段之後得到的，而在加入 t 時

刻的量測資訊z 後，便可從t bel(xt)算出bel(xt)，這個過程稱作 Measurement

update。

圖 2-1 說明了在貝氏濾波器中系統狀態、控制命令與量測資訊之間隨時間的

演化過程，在時間 t 的系統狀態是根據同時刻的控制命令u 與前一時刻的系統狀_t

態而來，而 t 時刻的量測資訊z 則是根據系統在狀態為_t x 時的量測而來。如此的_t

演化模式稱作動態貝氏網路(Dynamic Bayes network,DBN)[12]。

x

t

z

t+1

x

t-1

x

t+1

u

t+1

z

t

z

t-1

u

t

u

t-1 圖 2-1 動態貝氏網路示意圖 貝氏濾波器演算法的目的是要求得系統狀態的相信度函數bel(x_t)，並且以遞

迴的方式從感測資訊與控制命令來求得bel(x_t)，所以貝氏濾波器的輸入是 t-1 時刻的相信度函數bel(x_t−1)與 t 時刻的感測資訊與控制命令，而輸出即是 t 時刻 的相信度函數bel(x_t)。 貝氏濾波器演算法主要分成兩個步驟來求得bel(x_t)，分別是 Prediction 與 Update： 1.預測階段(Prediction)：

∫

− − − = ( | , ₁) ( ₁) ₁ ) (x_t p x_t u_t x_t bel x_t dx_t bel (2-3) 2.更新階段(Update)： ) ( ) | ( ) (x_t p z_t x_t bel x_t bel =η (2-4) 在 Prediction 階段，貝氏濾波器利用了前一時刻的相信度函數bel(x_t−1)與控制命 令u ，並對兩個機率函數，狀態轉移函數_t p(x_t |x_t−1,u_t)與相信度函數bel(xt−1)相 乘的結果做積分，從積分的結果我們可以得到bel(x_t)。而在 Update 階段，貝氏 濾波器則是將前一步驟得到的bel(x_t)乘上量測機率p(z_t |x_t)，相乘後的結果可以 得到最後的結果相信度函數bel(x_t)。因為兩個機率函數相乘的結果不一定仍為一 機率函數，即將兩機率函數相乘的結果積分不一定為 1，所以在(2-4)式中必須要 乘上一係數η使得bel(x_t)為正常的機率函數。 接著是數學的推導，將証明時刻 t 的相信度函數bel(xt)確實可由前一時刻的 相信度函數bel(x_t−1)經由貝氏濾波器演算法的計算求出。以下的推導必須有兩個 假設為前提： 1. 假設系統狀態

x

_t包含了所有過去時間的量測資訊與控制命令的資訊(Markov Assumption)。

2. 假設控制命令是隨機產生的並且已知。

有了假設之後，首先我們將bel(xt)，即 target posterior p(xt |z1:t,u1:t)以貝氏

定理展開成： ) , | ( ) , , | ( ) , | ( ) | | ( ) , , | ( ) , | ( : 1 : 1 : 1 1 : 1 : 1 : 1 : 1 1 : 1 : 1 1 : 1 : 1 : 1 t t t t t t t t t t t t t t t t t t t t u z x p u z x z p u z z p u z x p u z x z p u z x p − − − = =

η

(2-5) 接著利用假設 1，如果我們已知系統狀態x_t，而且我們想要得到量測資料

z

_t的資 訊，則過去時間的所有控制命令

u

_1:t−1與量測資料

z

_1:t−1皆無關於系統對於

z

_t的量 測結果。以數學表式可得到： ) | ( ) , , | (z_t x_t z_{1:t 1} u_1:t p z_t x_t p − = (2-6) 所以我們便可以將(2-5)式簡化成： ) , | ( ) | ( ) , | (x_t z_1:t u_1:t p z_t x_t p x_t z_{1:t 1} u_1:t p =

η

₋ (2-7) 可以得到： ) ( ) | ( ) (x_t p z_t x_t bel x_t bel =η (2-8) (2-8)式即是貝式濾波器在 Update 階段所使用的方程式。

另外，我們利用 Theorem of total probability 將bel(x_t)展開可以得到：

1 : 1 1 : 1 1 : 1 1 1 : 1 1 : 1 ) , | ( ) , , | ( ) , | ( ) ( − − − − − −

∫

= = t t t t t t t t t t t t x d u z x p u z x x p u z x p x bel (2-9) 再一次利用假設 1，若已知系統狀態x ，則我們可以不用考慮 t-1 時刻之前的控_t−1

制命令與量測資訊，但不包括u ，可以得到： _t ) , | ( ) , , | (xt xt 1 z1:t 1 u1:t p xt xt 1 ut p − − = − (2-10) 最後利用假設 2，使得u 可以從機率函數_t p(x_t−1|z_1:t−1u_1:t)中去除，所以我們可以從 (2-9)式得到： 1 1 : 1 1 : 1 1 1, ) ( | , ) | ( ) (xt =

∫

p xt xt− ut p xt− zt− u t− dxt− bel (2-11) (2-11)式即是使用在貝式濾波器中 Prediction 階段的式子。所以經由上面的証明可 以得到貝氏濾波器確實可以正確地由前一時刻的相信度函數bel(x_t−1)加上控制命 令與量測資訊，得到目前時刻的bel(x_t)。

2.2 粒子濾波器原理

粒子濾波器演算法是基於貝氏濾波器發展而來的[12]，它的特色是將我們想

要的 Posterior density function bel(x_t)以一組隨機的取樣點(Sample)或稱為粒子

(Particle)來表示，並且每一個 Sample 都有其相對應的權重(Weight)，再利用這一 組取樣點與其對應的權重來估測系統狀態。所以粒子濾波器能夠表示更多樣的機 率函數。 通常可以將有 M 個粒子的粒子組(Particle set) X 表示成： _t ] [ ] 2 [ ] 1 [ ,..., , : _{t t t}M t x x x X = (2-12) 每一個 Particle [m] t x (1<m<M)都代表著在 t 時刻系統的某一個狀態。故粒子濾波

器演算法的概念是以這一組 Particle X ，來近似 Posterior density_t bel(x_t)。既然是

用來近似 Posterior density bel(x_t)，每一個 Particle 都應該與bel(x_t)成一定的比例

) , | ( ~ _{1: 1:} ] [ t t t m t p x z u x (2-13) 從(2-12)式可以得知，若在系統狀態空間中某個狀態附近有 particle 分佈得特別密 集，則此狀態很可能就是真實的系統狀態。 如同貝氏濾波器演算法，粒子濾波器演算法亦是遞迴(Recursive)演算的架

構，從前一時刻的 Posterior bel(x_t−1)求得目前時刻的 Posterior bel(x_t)。但是粒子

濾波器是以 Particle Set 來表示bel(x_t)，故粒子濾波器是遞迴地從前一時刻的

Particle set X_t−1經由演算法的計算而求得目前的 Particle set X 。所以粒子濾波_t

器演算法的輸入就是前一時刻的 Particle set X_t−1與目前的控制命令u 以及量測_t 資訊z ，先將_t X_t−1裡的每一個 particle [ ] 1 m t x₋ 處理後，變成一組用來表示 Prior 相信

度函數bel(x_t)的 Particle Set Xt，隨後再將Xt轉換成用來近似 Posterior 相信度

函數bel(x_t)的 Particle Set X 。 _t

粒子濾波器演算法的流程主要可以分成三個步驟，分別是預測(Prediction)、 設定權重(Assign weight)與重新取樣(Resampling)：

1. 預測(Prediction)：

在這個步驟中，從前一時刻的 Particle SetX_t−1與控制命令u 產生另一組_t

Particle Set Xt，而Xt即是用來近似belt的 Particle Set。產生的方式為對狀

態轉移機率函數p(x_t |u_t,x_t−1)做取樣。即： ) , | ( ~ [ ] 1 ] [ m t t t m t p x u x x ₋ (2-14) 2. 設定權重(Assign weight)： 這個步驟將產生每一個 Particle x_t[m]其相對應的權重 [m] t w 。計算權重 [m] t w 的 目的是要將量測資訊z 加入到 Particle Set 中，計算的方式為： _t

) | ( [ ] ] [ m t t m t p z x w = (2-15) 因為權重 [m] t

w 的加入使得原來的 Particle Set 從用來表示bel(x_t)，變成用來表

示bel(x_t)，所以下式也可代表權重wt[m]的涵意： on distributi proposal on distributi target ] [m = t w (2-16)

其中 target distribution 與 proposal distribution 分別對應bel(x_t)與bel(x_t)。

3. 重新取樣(Resampling)：

這個步驟根據前一步驟產生的權重w ，將原來的_t[m] Particle Set 轉換成另一組

數目相同的 Particle Set，轉換的方式是在權重大的 Particle 附近取更多的 Particle，反之將權重小的 Particle 減少或直接消去，如此 Particle 會集中在 權重大的區域附近，使得 Particle Set 愈接近真實的系統狀態。所以在

Resampling 步驟之前，Particle Set 是根據 Prior bel(x_t)而分佈的，而在

Resampling 步驟之後，Particle Set 的分佈則是近似於 Posterior )bel(x_t 。

粒子濾波器的這三個主要步驟，第 1 步驟 Predicton 與貝氏濾波器相同，而步驟 2 和 3， Assign weight 與 Resampling，則是對應貝氏濾波器的 Update 階段。

接著是數學的推導，將証明(2-15)的 target distribution bel(x_t)的確可經由權

重 [m]

t

w 與 proposal distribution 的運算來求得。在此我們將考慮 state sequence：

t

t

x

x

x

x

_0:

=

₀

,

₁

,...,

(2-17)

而非先前的單一個 state x 。所以考慮整個_t state sequence 的 Posteriorbel(x_{0 t:})可

) , | ( ) (x_0:t p x_0:t u_1:t z_1:t bel = (2-18) 並且可進一步化簡成： ) , | ( ) , | ( ) | ( ) , | ( ) , , | ( ) | ( ) , | ( ) | ( ) , | ( ) , , | ( ) , | ( 1 : 1 1 : 1 1 : 0 1 : 1 1 : 1 1 : 0 : 1 1 : 1 1 : 0 : 1 1 : 1 : 0 : 1 1 : 1 : 0 : 1 1 : 1 : 0 : 1 : 1 : 0 − − − − − − − − − − − = = = = t t t t t t t t t t t t t t t t t t t t t t t t t t t t t t t t u z x p u x x p x z p u z x p u z x x p x z p u z x p x z p u z x p u z x z p z u x p

η

η

η

η

(2-19) (2-18)中的第 2 與第 4 個等號使用了 Markov assumption 來化簡。此外，我們假設 Proposal distribution 為： ) , | ( ) , | ( ) ( ) , | (x_t x_t−1 u_t bel x_0:t−1 = p x_t x_t−1 u_t p x_0:t−1 z_1:t−1 u_1:t−1 p (2-20) 所以由(2-18)和(2-19)式，我們可以將(2-15)式展開成： ) | ( ) , | ( ) , | ( ) , | ( ) , | ( ) | ( on distributi proposal on distributi target 1 : 1 1 : 1 1 : 0 1 1 : 1 1 : 1 1 : 0 1 ] [ t t t t t t t t t t t t t t t t m t x z p u z x p u x x p u z x p u x x p x z p w

η

η

= = = − − − − − − − − (2-21) 將(2-20)式的結果與(2-14)式做比較，兩式相一個差係數η，而在 Resampling 時 根據的是每一個 Particle 的權重大小之間的相互比例關係，並非權重的數值，所 以係數η是不重要的，(2-20)式仍和(2-14)式相符合。 因此粒子濾波器演算法在 Resampling 步驟中根據權重大小重新產生的

Particle Set，其分佈情況的確是與 Proposl distribution 和權重兩者相乘的結果符 合，亦即： ) ( ) , | ( ) , | ( _{1 0: 1 0: 1 0: 1 0:} ] [ t t t t t t t m t p x x u p x z u bel x w _{− − − −} =

η

(2-22)

所以 target distribution bel(x_t)確實可經由權重 [m]

t w 與 proposal distribution 經由 粒子濾波器演算法的運算得到。 由上面的介紹以粒子濾波器來做機器人定位演算法設計的步驟為： Step1：以粒子組X_t ={(x_t[m],w_t[m]),m=1,…,M}代表機器人的姿態。 Step2：利用控制命令u 與時刻t t−1的粒子組Xt−1產生新的粒子組Xt來代表預 測的機器人姿態。產生方式為：x_t[m] ~ p(x_t|u_t,x_t[₋m₁])。 Step3：根據外部環境感測器的資訊來修正前一步驟預測的機器人姿態，根據 量測資訊z 更新每個粒子的權重。 _t ) | ( [ ] ] [ m t t m t p z x w = Step4：將權重正規化，使

∑

[ ]=1 m m t w 。 Step5：設定權重臨界值w 。 _th Step6：對每一個粒子，若 th m t w w[ ]> ，則保留此粒子，否則消去此粒子。 Step7：從保留的粒子產生新的粒子直到總粒子數為 M。 接著重覆 Step2 到 Step7。

第三章 自走式機器人之定位設計

本章將介紹機器人自我定位演算法之設計，說明以粒子濾波器演算法結合 地圖比對的方法，來設計自走式機器人之定位系統。一般來說，機器人的定位 問題就是要求得機器人在環境中的姿態(Pose)，也就是位置與朝向角(x, y,θ )。在 本論文所設計的機器人定位系統中，位置的估測是經由粒子濾波器的處理求 得，而機器人的朝向角估測則是經由感測器得到的環境資訊與機器人已知的環 境地圖作比對來求得。

3.1 設計概念

一般在自走式機器人的移動過程中，可以使用里程計(Odometer)的回傳資訊 以及已知的機器人初始位置與朝向角來估測機器人目前的位置與朝向角，但是使 用這種定位方法最常遇到的問題就是累積誤差(Accumulate Error)。也就是說，機 器人在每次移動之後，其對本身位置資訊的不確定性範圍就會不斷變大。如圖 3-1 所示，當機器人依序經過 A Æ B Æ C Æ D Æ E 時，我們可以發現其對本身 Robot A B _C D E Uncertainty Region 圖 3-1 機器人對其位置的不確定性範圍示意圖

位置的不確定性將會愈來愈高，這種情況在自走式機器人的應用當中是不被允許 的，因為這容易造成機器人產生錯誤的後續動作。因此在機器人定位系統中還必 須加上其它感測器的使用與一套有效的定位演算法，才能確實解決自走式機器人 定位問題。

本論文以粒子濾波器演算法結合地圖比對的方式設計機器人定位系統，並使 用雷射掃描儀(Laser Range Finder)作為用來得到環境資訊的感測器。在機器人朝 向角估測方面，將雷射掃描儀量測的機器人與環境周圍的相對距離，以最小平方 法做線段擬合可以得到用來表示機器人周圍環境的線段組合，而這些線段組合是 代表機器人在目前的位置與朝向角所面對的局部環境地圖。所以我們將此線段組 合與機器人已知的內建地圖做比對，進行比對的時候須加上機器人前一時刻的位 置與朝向角資訊，如此可以減少比對結果的多種可能性(Ambiguity)，而從最後的 比對結果我們便可以估測出機器人的朝向角。 因此要由線段比對來估測機器人朝向角，必須要有機器人的位置資訊當作輸 入，位置資訊愈準確則得到的朝向角也會愈準確。所以在設計上，我們以粒子濾 波器來進行位置的估測，希望能估測出精確的位置資訊，進而使線段比對的朝向 角估測方法也能精確地估測朝向角。 首先，將雷射掃描儀所量測的機器人與環境周圍的相對距離，以最小平方法 產生用來表示機器人周圍環境的線段組合，然後將此環境線段組合的線段表示法 從以機器人為座標中心的區域座標系轉換成以內建地圖座標原點為中心的全域 座標系，如此便可得到環境線段組合與內建地圖的對應關係，再由機器人與環境 線段組合中線段的相對距離可以得到機器人在環境中的位置資訊，最後經由粒子 濾波器對機器人的位置做估測。 根據上面陳述的概念，本論文所設計的自走式機器人定位系統的架構圖，如 圖 3-2 所示，系統包含了兩個感測器，分別是里程計(軸編碼器)與雷射掃描儀。

Odometer Predict Update Laser range finder Line extraction Map matching Robot orientation θk Particle filter k

X

) , , (

θ

_k k k k

x

y

X

= 1 − k

X

k Z Line set Environment map Range data 圖 3-2 自走式機器人定位系統架構圖 機器人定位系統的輸入為兩個感測器的量測資料以及前一時刻的機器人姿態 ) , , ( _{1 1 1} 1 − − − − = t t t t x y X θ ，輸出為機器人目前的姿態X_t =(x_t,y_t,θ_t)。 一開始里程計的資訊與前一時刻的機器人姿態X_t−1輸入到系統的粒子濾波 器中，由粒子濾波器對機器人目前的姿態做預測得到Xt，接著利用雷射掃描儀 的量測資料來產生環境線段組合，計算機器人與線段組的相對距離並將結果輸入 到粒子濾波器中，經由濾波器中 Update 階段的處理，可以得到機器人的位置資 訊。另外，由雷射掃描儀對環境的距離量測，經由線段擷取(Line extraction)可得 到一組線段組合，再將此線段組合與環境地圖做比對，可得到機器人的朝向角資 訊。最後將位置和朝向角資訊結合，就可得到系統的輸出，即機器人目前的姿態 t X 。以上所描述的便是設計的機器人定位系統的運作流程。

綜合以上的討論，本論文所設計的機器人定位系統運作之前必須有兩個已知 條件： 1) 機器人在環境中的初始位置與朝向角。 2) 機器人所處環境的地圖。

3.2 位置估測方法

本節介紹以粒子濾波器理論為基礎而設計的機器人位置估測方法，其中包 含了粒子濾波器演算法的兩個主要步驟，預測階段(Prediction)與更新(Update)階 段。說明如何利用感測器的資訊與前一時刻機器人的姿態，經由粒子濾波器的處 理來估測機器人的位置資訊。而機器人朝向角的部分，將在下一節以線段擷取與 比對的方式加入此節產生的位置資訊當作輸入來求得，並未包含在粒子濾波器的 設計中。 3.2.1 預測(Prediction) 將粒子濾波器應用在機器人位置估測問題中，粒子在環境中散佈的位置就 是粒子濾波器對於機器人位置的估測結果。如下式所示： ) , ( m m m t x y s = (3-1) 其中 m t s 代表在時間 t 的第 m 個粒子 一開始因為我們已知機器人的初始位置，所以將粒子濾波器的所有粒子 (Particles)均勻散佈在初始位置的周圍，並且我們將每一個粒子的權重(Weight)皆 設定為 M 1 ，如下式所示：

M w_tm = 1 (3-2) 其中 m t w 代表粒子 m t s 的權重 M 為粒子濾波器的粒子數目 接著利用里程計的資訊，來得到機器人在一個取樣周期內的移動距離與朝向 角改變的角度，(dx,dy,dθ)，將此資訊加上前一時刻的機器人的位置及朝向角， ) , , ( _{1 1 1} 1 − − − − = t t t t x y X θ ，我們可以對機器人目前的位置與朝向角做預測而得到預測 結果，以Xt =(xt,y_t,θt)表示。 一般來說，(dx,dy,dθ)的計算方式是經由機器人輪子驅動馬達的軸編碼器 (Encoder)回傳的脈波(Pulse)數來得到機器人左右兩輪各自移動的距離，再由兩輪 移動距離推算出(dx,dy,dθ)。數學式子如下[20]： )) ( ) ( ( 2 1 ) (t ds t ds t ds = _r + _l (3-3) )) ( ) ( ( 1 ) ( ds t ds t W t dθ = _r − _l (3-4) ) 2 ) 1 ( ) ( cos( ) ( ) (t =ds t × t + t− dx θe θe (3-5) ) 2 ) 1 ( ) ( sin( ) ( ) (t =ds t × t + t− dy θe θe (3-6) 其 中 ds_l(t)與ds_r(t)分 別 為 左 右 輪 在 t-1 至 t 時 間 內 的 移 動 距 離 ) (t ds 為 機 器 人 在 t-1 至 t 時 間 內 的 移 動 距 離 ) (t e θ 為 里 程 計 對 機 器 人 朝 向 的 估 測 結 果

W 為 機 器 人 的 兩 輪 間 距 從 (3-5)式 與 (3-6)式 我 們 可 以 發 現 ， 式 子 中 利 用 了 里 程 計 對 機 器 人 朝 向 的 估 測 結 果θ_e(t)來 求 得dx(t)與 dy(t)，但 由 前 面 的 討 論 可 知，用 里 程 計 來 估 測 機 器 人 的 位 置 及 朝 向 角 會 有 誤 差 累 積 的 問 題，所 以 使 用θ_e(t) 來 計 算 dx(t)與 dy(t)，會 產 生 不 正 確 的 結 果，使 得 預 測 的 機 器 人 姿 態 有 著 更 大 的 誤 差。為 了 改 善 這 個 問 題，我 們 將θ_e(t)以 所 設 計 的 定 位 系 統 估 測 的 朝 向 角θ_est(t)來 代 替 ， 如 此 可 以 得 到 較 精 確 的 dx(t)與 dy(t)。 不 過 ， 可 以 如 此 替 換 的 前 提 是θ_est(t)的 正 確 性 要 比θ_e(t)高 ， 而 這 點 可 以 從 下 一 節 的 討 論 結 果 得 到。所 以 我 們 可 以 將 (3-5)式 與 (3-6)式 改 寫 成 ： ) 2 ) 1 ( ) ( cos( ) ( ) (t =ds t × t + t− dx θest θest (3-7) ) 2 ) 1 ( ) ( sin( ) ( ) (t =ds t × t + t− dy θest θest (3-8) 可 以 正 確 的 得 到(dx,dy,dθ)之後，接著在實際做法上，因為粒子所在的 位置就是代表機器人可能存在的位置，所以將每一個粒子所表示的位置都加上 ) , (dx dy ，以此來完成預測機器人位置的步驟。而加上(dx,dy)後的粒子分佈，即 用來代表預測的機器人位置(x ,t y_t)。綜合以上，這個步驟可以下式來表示： ) , | ( ~ m₁ t t t m t p x u s s ₋ , m=1~M (3-9) 其中u 在此是代表里程計的資訊_t 意義為讓所有粒子的分佈符合機率函數p(x_t|u_t,s_tm₋₁)，而p(x_t |u_t,s_tm₋₁)表示在已知 里程計資訊與前一時刻粒子分佈的情況下，機器人位置的機率分佈。因此，將每 一個粒子加上(dx,dy)即可達成(3-9)式。

3.2.2 更新(Update) 在這個階段中，我們使用預測階段得到的粒子組(Particle Set)與雷射掃描儀 的量測資訊來估測機器人的位置。首先將雷射掃描儀量測的距離資訊以最小平方 法產生一表示環境的線段組，並以極座標的直線方程式參數(α,ρ)來表示線段(詳 述於 3.3 節)。其中ρ為機器人與線段的垂直距離，另外以dRL代表。為了得到機 器人的位置參考資訊，我們必須知道由量測資訊產生的環境線段組與內建地圖的 對應關係。所以將環境線段組的直線方程式參數由以機器人為中心的區域座標系 轉換到以內建地圖原點為中心的全域座標系來表示，轉換公式如下： ) 2 (θ π α α_G = _L+ − (3-10) G G L G ρ x α y α ρ = + cos + sin (3-11) 其中(x, y,θ )為機器人的姿態，而(α_L,ρ_L)與(α_G,ρ_G)分別為區域座標系與全域座 標系的直線方程式參數，如圖 3-3 所示。所以我們可以得知d_RL是機器人與內建 地圖中的那一條線段之間的相對距離，藉此可以得到機器人在環境的位置參考資 訊。接著我們就可以對預測階段產生的粒子組做權重給予(Assign weight)與更新 (Update)的步驟。 圖 3-3 擷取線段座標轉換關係圖

在粒子濾波器演算法中，每一個粒子的權重是根據下列式子來決定： ) | ( m t t m t p z x w = , m=1~M (3-12) 其中z 為量測資訊 _t 所以決定了機率分佈函數p(z_t|x_tm)就可以決定權重。而機率函數p(z_t |x_tm)代表 的意義為在已知粒子的位置情況下，量測資訊z 的機率分佈情形。 _t 在我們的設計中，量測資訊z 為機器人與環境線段之間的相對距離_t d_RL。另 外，對每一個粒子再求出其與環境線段之間的相對距離 m PL d ，因為我們已知環境 線段在全域座標系的直線方程式參數(α_G,ρ_G)以及每個粒子的位置座標 ) , ( m t m t y x ，所以可以經由距離公式求得 m PL d 。接著對於每一個粒子我們比較d_RL與 m PL d ，當d_RL與dPLm 之間的差距愈小代表此粒子的位置與機器人在環境中的真實位 置愈符合。相反的，若d_RL與 m PL d 之間的差距愈大，則表示此粒子的位置與機器 人的真實位置相距愈大。所以我們可以根據d_RL與dPLm 間的差值來給予每個粒子 合理的權重，差值愈小則權重愈大，反之，當差值愈大則權重愈小，如下式： ) | ( t tm m t p z x w = Å _m PL RL d d − 1 (3-13) 接著還必須對每個權重做正規化(Normalize)的動作，即將每個權重 m t w 除以所有 權重的總合

∑

= M m m t w 1 ，藉此得到正規化後的粒子權重。 求得每一個粒子的權重後，須將權重太小的粒子從粒子組中消去，在我們的 設計中是將權重小於 M 1 的粒子消去，其中 M 為粒子組包含的粒子數目，而其餘 的粒子則持續存在於粒子組中。另外，為了使粒子組的粒子數目在每個循環都保 持相同，我們須產生與被消去的粒子數目一樣多的新的粒子。在我們的設計中， 是從存留下來的粒子去產生新的粒子。這是因為粒子的位置代表著機器人在環境

中的位置，而存留下來的粒子其位置能夠符合於機器人在環境中的真實位置，所 以存留下來的粒子來產生新的粒子，會使得新的粒子組的位置分佈亦集中在真實 機器人位置的周圍。而實際做法是先設定一個距離範圍，然後在存留下來粒子的 位置的設定之距離範圍內隨機地產生新的粒子。如此新的粒子的位置也會集中在 實際機器人的位置周圍。不過所設定的距離範圍不可過大，若過大會造成新的粒 子與存留粒子相距太大，使新的粒子組的位置分佈過於分散而與機器人的實際位 置不符合。當新的粒子組產生後，再將所有粒子的權重都設為 M 1 ，接著新的粒 子組便進入下一個循環。以上的處理過程，即是粒子濾波器演算法中的重新取樣 (Resampling)步驟。 重新取樣後，可得一組新的粒子組，並將粒子組中每個粒子的位置座標取平 均值，而得到平均位置的座標即是粒子濾波器對機器人位置的估測結果，如下式 所示： M x x m m t t

∑

= (3-14) M y y m m t t

∑

= (3-15) 所以經由(3-14)與(3-15)式，我們可以得到估測的機器人位置座標(x ,_t y_t)。

3.3 朝向角估測方法

本節將說明如何以最小平方法將由雷射掃描儀得到的環境資訊擷取成線段 組合，並將此線段組合結合粒子濾波器預測的機器人姿態Xt的資訊與已知的環 境地圖做比對，最後從比對的結果來估測機器人的朝向角。

3.3.1 線段擷取 從雷射掃描儀我們可以得到以點為基礎的資料，為了將這些資料與環境地圖 做線段比對，所以必須將這些用來表示距離的點以線段的方式來描述。所使用的 雷射掃描儀的角度掃描範圍(Angular range)為 0 到 180 度，若以 1 度為間隔來量 取距離資料，可以得到 181 筆距離資料。每一筆距離資料可以以(r_i,φ_i)來表示， 其中φ_i代表掃描的角度，而r 則代表對應於_i φ_i角度的距離值。

接著使用最小平方誤差法(Least Square Error Method)將 181 筆距離資料

(r_i,φ_i)以線段來表示。一般的直線方程式如下式所示： b ax y= + (3-16) 其中a代表直線的斜率，b代表直線與 Y 軸的截距，不過以這種方程式來表示線 段會遇到斜率為無窮大的情況，所以我們改用極座標的線段表示方式： ρ α α + sin = cos y x (3-17) 其中ρ代表 XY 平面原點到直線的垂直距離，而α則是垂直距離ρ代表的直線與 X 軸間的夾角。在我們的應用當中，ρ代表機器人與牆面之間的垂直距離，而α 則是ρ代表的直線與機器人 0 度方向的夾角，如圖 3-4 所示。

α

ρ

Robot X Wall surface Y 圖 3-4 線段方程式參數定義示意圖

另外，資料點(r_i,φ_i)到直線(ρ,α)的距離d 可由下式得到： _i ρ α φ − − = _icos( _i ) i r d (3-18) 接著最小平方法即是以使所有d 平方的總和為最小值的方式來求得最適合所有_i 資料點的直線參數(ρ,α)，若有 n 個資料點，則平方總和 S 可由下式求得： ∑− − − = = 1 0 2 ) ) cos( ( n i i i S _r φ α ρ (3-19) 然後將平方和 S 分別對ρ及α作偏微分： 0 ) ) cos( ( 1 0 2 = ∂ − − ∂

∑

− = ρ ρ α φ n i i i r (3-20) 0 ) ) cos( ( 1 0 2 = ∂ − − ∂

∑

− = α ρ α φ n i i i r (3-21) 整理(3-20)式與(3-21)式，可以得到：

∑

− = − = 1 0 ) cos( 1n i i i r n φ α ρ (3-22)

∑

∑∑

∑

∑∑

− = − = − = − = − = − = + − + − = ₁ 0 1 0 1 0 2 1 0 1 0 1 0 2 ) cos( 2 cos ) sin( 2 sin ) 2 tan( _n i n i n j j i j i i n i n i n j j i j i i r r r n r r r n φ φ φ φ φ φ α (3-23) 所以最佳解(ρ,α)可以由(3-20)式及(3-21)式求得。 可以求得資料點的最佳線段後，接下來就是要將由雷射掃描儀讀入的 181 筆資料點擬合成線段，但因為機器人所在的環境中通常不會只有一面牆，讀入的 資料點通常會被擬合成多條線段來代表他們所描述的環境特徵(牆面)，稱為線段 組。所以我們要將所有的資料點做分組使得符合同一條線段的資料點分成同一組 後再進行線段擷取，步驟如下[13]：

1.首先將前兩筆資料點D 及₀ D₁(0 度與 1 度)以最小平方法誤差法求得線段L₁， (ρ ,1 α )。 1 2.接著判斷第三筆資料點D2是否也屬於前一步驟求得的線段L1，判斷方式是 去計算資料點D₂到直線L₁的距離 d，如圖 3-5 所示，若得到的距離值小於 一預先設定的臨界值dis ，則我們可以將_th D₂包含於L₁內，並回到步驟 1 將 0 D ~D₂以最小平方誤差法求得新的線段。但若資料點D₂到直線L₁的距離 d 大於dis ，表示_th D₂不包含於L₁內，則L₁即為擷取出的線段，並將D₂與下一 資料點D 代入步驟 1，進行下一條線段的擷取。 ₃ D0 D1 D2 L1 d > disth D0 D1 D2 L1 d < disth 圖 3-5 判斷資料點是否包含於線段之示意圖 如此重覆的進行步驟 1、2 直到最後一筆資料點結束，我們可以得到若干線段的 線段組： } ... 1 | {L i N L= _i = (3-24) 其中 L_i =(α_i,ρ_i) (3-25) 而線段擷取的流程如圖 3-6 所示。

Laser scanner data D0~D180 Least square line fit Check next point Included in the line Not included in the line The extracted line Li 圖 3-6 線段擷取流程圖 假設機器人在如圖 3-7 的環境裡，機器人的座標為(100,200)朝向角為負 90 度(X 軸方向為 0 度)。使用上面陳述的線段擷取方法，我們可以得到三條 線段如圖中所示的 Line1、Line2 及 Line3，其線段參數(ρ,α)如表 3-1 所列， 其中第三欄為每條線段所包含的資料點數目。 圖 3-7 機器人在環境中擷取線段

表 3-1 機器人在圖 3-7 環境中擷取的線段資料

ρ α(deg) Data number

Line 1 99.66 0.16 65 Line 2 201.51 89.89 53 Line 3 101.53 179.9 63 3.3.2 線段比對 線段比對的目的是要求出機器人的朝向角。因為我們已知以線段表示的環 境地圖以及粒子濾波器預測的機器人姿態Xt =(xt,y_t,θt)的資訊，所以可以經由 模擬得到機器人在環境地圖中的(xt,y_t)位置而朝向角為θt時雷射掃描儀掃描的 距離資料，並經由線段擷取可得到一組線段組： } ... 1 | { M_{k M} M N k L L = = (3-26) 其中 ( , kM) M k M k L = α ρ (3-27) 另外，將機器人上的雷射掃描儀量到的實際環境距離資料以線段擷取步驟處理， 同樣地我們可以得到另一組線段組： } ... 1 | { L L i L N i L L = = (3-28) 其中 LL_i =(α_iL,ρ_iL) (3-29) 接著便可以將 L L 與L 兩線段組進行比對，而比對的方式為分別比較M L 中每一條L

線段 L i L 與L 中的每一條線段M L 的直線方程式參數，當相互比較的兩條線段的M_k 參數符合下面兩式時，則將這兩條線段視為相互符合。 th M k L i α α α − ≤ (3-30) th M k L i ρ ρ ρ − ≤ (3-31) 其中α_th與ρ_th為預先設定的臨界值。 當 L L 與L 中各別的每一條線段都相互比較過之後，我們可以得知M L 與L LM 兩線段組的互相符合程度，並且由此我們可以判定預測的機器人朝向角θt與機 器人實際的朝向角是否符合。通常θt與機器人實際朝向角之間的誤差不大，這 是因為θt是由前一時刻定位系統估測的機器人朝向角θ_t−1加上里程計的資訊得 來，所以我們認定機器人實際的朝向角會落在以θt為中心向正負方向展開的角 度範圍內。 在我們的設計中，是取以θt角度為中心正負 15 度的範圍。因此我們可以將 機器人的朝向角設定成此角度範圍內的角度，再經由模擬可得到對應每個朝向角 角度的線段組 M L ，共會產生 31 組。最後，我們將這 31 組線段組分別與由實際 雷射掃描的量測資料點產生的線段組 L L 做比對，接著從比對的結果我們可以得 到 31 組線段組中那一組與 L L 最符合，而最符合的線段組所對應的朝向角即是估 測的機器人朝向角θ_t。 不論是位置估測方法或朝向角估測方法都需要用到環境中牆面的資訊，因此 環境中牆面的多寡將會影響定位系統所估測的機器人姿態。當環境中只有一面牆 時，因為朝向角估測方法是去比對代表牆面的線段參數(α,ρ)來估測朝向角，故 在只有一面牆的情況下仍可估測出朝向角。另一方面，在位置估測方法上，是使

用機器人與牆面的垂直距離作為量測資訊來進行位置估測，而機器人位置(x,y) 是二維資料，需要環境中至少有兩面牆來提供量測資訊，位置估測方法才能有效 的估測出機器人的位置，所以環境中只有一面牆是不夠的。因此機器人必需處於 有兩面或兩面以上牆壁的室內環境中，所設計的機器人定位系統才能正確地估測 機器人的姿態。

第四章 模擬與實驗結果

本章的目的是為了驗證第三章所設計的自走式機器人定位系統。先介紹用來 做為實驗平台的機器人系統與雷射掃描儀，接著呈現模擬與實驗結果。在實驗部 分，第一個實驗讓機器人繞行環境，藉以驗證機器人在繞行的過程中是否能夠持 續地定位。第二個實驗則增長了繞行的環境，以此測試機器人在更大的環境中是 否同樣具有定位的能力。第三個實驗則將繞行環境改變成 L 型走廊來驗證所設 計的定位系統。

4.1 機器人定位系統硬體架構

4.1.1 機器人系統 本論文以實驗室自行開發的機器人做為實驗平台，如圖 4-1 所示。其系統核 心為工業電腦，負責處理由各個感測器傳來的資訊並估測機器人的姿態，以及決 定機器人的運動控制命令並透過 RS232 介面傳送到 DSP 馬達控制系統。在感測 器方面，雷射掃儀是透過 RS422 介面將量測資訊傳送到工業電腦，同時亦接收 由工業電腦傳來的指令。圖 4-2 為機器人系統控制架構。 方塊說明： 1. 雷射掃描儀 2. 工業電腦 3. DSP 馬達控制卡 圖 4-1 機器人硬體裝置架構圖 1 3 2

IPC DSP Motor Control Card Laser Scanner (LMS 291-S05) Left Wheel L298 Servo Driver Right Wheel L298 Servo Driver DC Servo Motor DC Servo Motor Encoder Encoder RS422 RS232 (v_l,v_r) ) , (r_i φ_i 圖 4-2 機器人系統控制架構圖 在機器人之馬達控制系統部分採用實驗室學長所開發完成之 DSP 馬達控制 卡，再加上兩塊 L298 驅動電路，來做為機器人的兩輪移動控制(速度回授控制)， 其與工業電腦端之訊息傳遞透過 RS232 介面來實現，並且資料傳輸鮑率為 19200bps。另外，兩個馬達各附有一組軸編碼器(Encoder)，經由軸編碼器的回傳 資料可以得知兩輪的移動情形。 4.1.2 雷射掃描儀

本論文所使用的雷射掃描儀為 SICK 公司的 LMS291-S05，如圖 4-3，其規 格如下： – 最大量測距離：80 公尺

– 角度量測範圍 : 0 ~ 180 度 – 角度解析度： 0.25 / 0.5 / 1 度 – 量測距離解析度 : 10 公釐 – 資料傳輸速率：9.6 / 19.2 / 38.4 / 500 k baud 圖 4-3 雷射掃描儀 圖 4-4 雷射掃描儀工作原理[11] 圖 4-5 雷射掃描儀角度掃描範圍[11] 在我們的使用上是將傳輸鮑率設成 38400，並使用 1 公分的距離解析度，0.5 度 的角度解析度，與 180 度的角度掃描範圍(如圖 4-5)，所以在每一次掃描之後雷

射掃描儀可得到 0~180 度的 361 筆距離資料。 在雷射掃描儀的內部結構中，主要包含三個部分，分別是不斷旋轉的鏡面、 光學折射裝置與接收反射回來雷射光的接收器，如圖 4-4 所示。其工作原理為當 雷射光從雷射掃描儀發射出去後，經由光學折射裝置射向前方，待雷射光碰到物 體反射回雷射掃描儀，再經由折射裝置將反射回來的雷射光送到雷射光的接收 器，雷射掃描器便利用此雷射光往返的時間(time-of-flight)去計算出雷射掃描器 與障礙物間的距離。而不斷旋轉的鏡面則將雷射光打至 0~180 度之間不同的角 度，如此便可計算出每個角度與環境障礙物之間的距離。 另外，我們可以對 LMS 291-S05 雷射掃描儀下達不同的指令參數來對掃描 儀做不同的要求，但當我們給予的指令不是雷射掃描儀所能明白的格式，這時掃 描儀的輸出將不會有變化，也就是不會有訊號產生，而當我們所輸入的命令是雷 射掃描所能明白時，它會先輸出一個確認訊號(ACK，06H)表示雷射掃描器已經 收到命令，緊接在確認性號後的則是對應所下達命令的反應(Response)。 在 LMS 291-S05 雷射掃描儀的指令當中最常用到的是要求回傳量測值指令 (Requesting measured value, 30H)，其指令格式如下表所示：

表 4-1 要求回傳量測值指令格式[11]

其中 STX 代表同步檢查碼，固定為 02H；Address 代表在使用多個雷射掃描儀的 系統中要對第幾個雷射掃描儀下命令(從 0 開始)；Length 代表粗黑框框中 Command 與 Data 的長度，第 3 個 Byte 表示為低位元，第 4 個 Byte 表示為高位 元，在本例中 Command 與 Data 的總長度為兩個 Byte，因此第三個 Byte 為 02H， 第四個 Byte 為 00H；Command 代表所欲送出命令的代碼，30H 是代表要求雷射