專利名稱：基于紅外測距的微機(jī)器人粒子濾波定位方法
技術(shù)領(lǐng)域：
本發(fā)明涉及的是一種微型機(jī)器人定位方法，具體是一種基于紅外傳感器和粒子濾波估計的微型機(jī)器人定位方法。
背景技術(shù)：
隨著MEMS技術(shù)及其相關(guān)技術(shù)的發(fā)展，移動微型機(jī)器人已經(jīng)成為機(jī)器人學(xué)的一個重要發(fā)展方向。機(jī)器人的微型化為機(jī)器人的應(yīng)用開闊了更為廣闊的前景，如微尺度空間內(nèi)的探測、代替人在微型工廠里工作、危險廢棄物的清除，微尺寸管道的檢測等，這些應(yīng)用極大地擴(kuò)大了機(jī)器人的應(yīng)用范圍。為了能完成這些任務(wù)，移動微機(jī)器人必須能夠在工作環(huán)境中自定位。所謂的移動機(jī)器人自定位是根據(jù)先驗地圖信息，結(jié)合當(dāng)前位姿觀測值以及傳感器輸入信息確定機(jī)器人位姿的過程。定位技術(shù)是移動機(jī)器人領(lǐng)域中的一項核心技術(shù)，幾乎所有的任務(wù)都要求機(jī)器人知道自己在環(huán)境中的位置信息。例如機(jī)器人到達(dá)目的地進(jìn)行檢測，需要機(jī)器人知道當(dāng)前的位置從而規(guī)劃一條到目標(biāo)的路徑。機(jī)器人在執(zhí)行探索任務(wù)時，要求機(jī)器人對自身位姿的評估以確定一個集體的地帶是否被探測過。從概率的角度來看，定位問題就是移動機(jī)器人狀態(tài)估計問題，基于概率的定位方法具有很高的魯棒性。當(dāng)前解決方法主要集中于以貝葉斯濾波為基礎(chǔ)理論的概率定位方法，如卡爾曼濾波，多峰值估計法，馬爾科夫定位，粒子濾波等。粒子濾波方法應(yīng)用于移動機(jī)器人定位研究，即Monte CarloLocalization(MCL)方法,近十年來受到廣泛的關(guān)注。對現(xiàn)有技術(shù)的文獻(xiàn)檢索發(fā)現(xiàn)，當(dāng)前存在很少的微機(jī)器人自主定位方法，公開文獻(xiàn)有Buguera等人在SENSORS (傳感器雜志)2009年發(fā)表的《Sonar Sensor Modelsand TheirApplication to Mobile Robot))(聲納傳感器模型及其在機(jī)器人上的應(yīng)用)和 Biber 等人在 Proceedings of the 2003 IEEE/RJS International Coference onIntelligenceRobots and Systems (智能機(jī)器人和系統(tǒng)、IEEE2003關(guān)于智能機(jī)器人和系統(tǒng)的會議)發(fā)表的《The Normal Distributions Transform:A New Approach to Laser ScanMatching》(正態(tài)分布轉(zhuǎn)換一種新的激光掃描匹配方法)。以上兩文所涉及的對象都是相對比較大機(jī)器人，所采用的基于聲納傳感器和激光傳感器粒子濾波的定位方法，聲納傳感器和激光傳感器尺寸很大故很難應(yīng)用到尺寸在數(shù)十個立方厘米之內(nèi)的微型機(jī)器人上。

發(fā)明內(nèi)容
針對現(xiàn)有技術(shù)中的缺陷，本發(fā)明的目的是提供一種基于紅外測距的微機(jī)器人粒子濾波定位方法。根據(jù)本發(fā)明的一個方面，提供一種基于紅外測距的微機(jī)器人粒子濾波定位方法，包括如下步驟步驟一根據(jù)狀態(tài)先驗分布建立初始狀態(tài)粒子集；步驟二 采樣，具體為根據(jù)運動模型，采樣得到新粒子集；
步驟三權(quán)值計算，具體為根據(jù)感知模型計算所述新粒子集中粒子的權(quán)值，并以所述新粒子集中的粒子、以及所述粒子的權(quán)值為元素得到含權(quán)重的粒子集；步驟四狀態(tài)輸出，具體為根據(jù)所述含權(quán)重的粒子集、以及狀態(tài)后驗分布，計算狀態(tài)；步驟五重采樣，具體為根據(jù)所述粒子的權(quán)值從所述含權(quán)重的粒子集中重新抽取粒子。優(yōu)選地，所述運動模型采用的是計步方式。優(yōu)選地，所述感知模型是采用紅外測距掃描提取系統(tǒng)狀態(tài)的粒子特征，進(jìn)行地圖匹配而獲得的，地圖采用的是柵格地圖，其中，基于傳感器獲得的范圍數(shù)據(jù)和地圖信息來預(yù)測當(dāng)前粒子的姿態(tài)的概率并獲取當(dāng)前粒子的權(quán)值。優(yōu)選地，所述柵格地圖采用成塊存儲的方式，存儲塊的大小由紅外測距傳感器的最大范圍rmax決定，存儲塊的外接圓的半徑為rmax，整個地圖由相切的數(shù)個存儲塊組成。優(yōu)選地，根據(jù)機(jī)器人的所述運動模型確定使用的地圖從而快速加載塊地圖進(jìn)行粒子特征匹配。我們假設(shè)微型機(jī)器人在平面內(nèi)作業(yè)，工作在結(jié)構(gòu)化的小于I立方米環(huán)境之內(nèi)。作為優(yōu)選的技術(shù)方案，在所述步驟二中，微機(jī)器人采用計步的方法存儲機(jī)器人的運動信息。因為微機(jī)器人采用步進(jìn)馬達(dá)驅(qū)動，所以允許其采用計步的方法存儲機(jī)器人的運動信息，根據(jù)運動信息采樣得到新粒子集。作為優(yōu)選的技術(shù)方案，在所述步驟三中，采用紅外測距掃描提取系統(tǒng)狀態(tài)的粒子特征，紅外的中心角要比較小，用多個紅外測距傳感器以保證其360度的輻射范圍。結(jié)構(gòu)化的環(huán)境地圖采用成塊存儲，存儲塊的大小由紅外測距傳感器的最大范圍rmax決定，存儲塊的外接圓的半徑為rmax，整個結(jié)構(gòu)化環(huán)境由相切的數(shù)個存儲塊組成。根據(jù)機(jī)器人的運動信息決定用哪塊地圖從而快速加載塊地圖進(jìn)行粒子特征匹配。與現(xiàn)有技術(shù)相比，本發(fā)明采用基于紅外測距的微機(jī)器人粒子濾波定位方法在結(jié)構(gòu)化的環(huán)境中實現(xiàn)微機(jī)器人的自定位。針對微機(jī)器人尺寸小的特點，采用小尺寸的紅外傳感器作為微型機(jī)器人的測距傳感器，感知模型采用的紅外測距地圖匹配方式比較適合尺寸小的微型機(jī)器人；微機(jī)器人的存儲能力有限，采用分塊存儲地圖的方法，通過結(jié)構(gòu)化的全局地圖成塊讀取方式，節(jié)省了存儲空間，便于微機(jī)器人的快速加載匹配，從而加速定位過程；由于尺寸效應(yīng)的影響，微機(jī)器人無法安裝位置傳感器，采用計步的方法存儲機(jī)器人的運動信息比較適合微型機(jī)器人。因此本發(fā)明可以實現(xiàn)微機(jī)器人在結(jié)構(gòu)化的環(huán)境中的自定位，而且定位效率比較高，能夠保證一定的定位精度。
具體實施例方式下面結(jié)合具體實施例對本發(fā)明進(jìn)行詳細(xì)說明。以下實施例將有助于本領(lǐng)域的技術(shù)人員進(jìn)一步理解本發(fā)明，但不以任何形式限制本發(fā)明。應(yīng)當(dāng)指出的是，對本領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明構(gòu)思的前提下，還可以做出若干變形和改進(jìn)。這些都屬于本發(fā)明的保護(hù)范圍。設(shè)X為被估計量，其先驗分布為P (X)，Z1:k為X的k個觀測值，其條件概率密度函數(shù)為P (z1:k IX)，則可利用貝葉斯公式得到后驗概率密度函數(shù)
貝葉斯公式
權(quán)利要求
1.一種基于紅外測距的微機(jī)器人粒子濾波定位方法，其特征在于，包括如下步驟 步驟一根據(jù)狀態(tài)先驗分布建立初始狀態(tài)粒子集； 步驟二 采樣，具體為根據(jù)運動模型，采樣得到新粒子集； 步驟三權(quán)值計算，具體為根據(jù)感知模型計算所述新粒子集中粒子的權(quán)值，并以所述新粒子集中的粒子、以及所述粒子的權(quán)值為元素得到含權(quán)重的粒子集； 步驟四狀態(tài)輸出，具體為根據(jù)所述含權(quán)重的粒子集、以及狀態(tài)后驗分布，計算狀態(tài)； 步驟五重采樣，具體為根據(jù)所述粒子的權(quán)值從所述含權(quán)重的粒子集中重新抽取粒子。
2.根據(jù)權(quán)利要求1所述的基于紅外測距的微機(jī)器人粒子濾波定位方法，其特征在于，所述運動模型采用的是計步方式。
3.根據(jù)權(quán)利要求1所述的基于紅外測距的微機(jī)器人粒子濾波定位方法，其特征在于，所述感知模型是采用紅外測距掃描提取系統(tǒng)狀態(tài)的粒子特征，進(jìn)行地圖匹配而獲得的，地圖采用的是柵格地圖，其中，基于傳感器獲得的范圍數(shù)據(jù)和地圖信息來預(yù)測當(dāng)前粒子的姿態(tài)的概率并獲取當(dāng)前粒子的權(quán)值。
4.根據(jù)權(quán)利要求3所述的基于紅外測距的微機(jī)器人粒子濾波定位方法，其特征在于，所述柵格地圖采用成塊存儲的方式，存儲塊的大小由紅外測距傳感器的最大范圍rmax決定，存儲塊的外接圓的半徑為rmax，整個地圖由相切的數(shù)個存儲塊組成。
5.根據(jù)權(quán)利要求4所述的基于紅外測距的微機(jī)器人粒子濾波定位方法，其特征在于，根據(jù)機(jī)器人的所述運動模型確定使用的地圖從而快速加載塊地圖進(jìn)行粒子特征匹配。
全文摘要
本發(fā)明提供一種基于紅外測距的微機(jī)器人粒子濾波定位方法，包括步驟步驟一根據(jù)狀態(tài)先驗分布建立初始狀態(tài)粒子集；步驟二采樣；步驟三權(quán)值計算；步驟四狀態(tài)輸出；步驟五重采樣。本發(fā)明針對微機(jī)器人尺寸小的特點，采用小尺寸的紅外傳感器作為微型機(jī)器人的測距傳感器；微機(jī)器人的存儲能力有限，采用分塊存儲地圖的方法；由于尺寸效應(yīng)的影響，微機(jī)器人無法安裝位置傳感器，運動模型采用的是計步方法。因此本發(fā)明可以實現(xiàn)微機(jī)器人在結(jié)構(gòu)化的環(huán)境中的自定位，而且定位效率比較高，能夠保證一定的定位精度。
文檔編號G06F19/00GK103020427SQ20121048459
公開日2013年4月3日 申請日期2012年11月23日 優(yōu)先權(quán)日2012年11月23日
發(fā)明者陳佳品, 毛玲, 張大偉, 李振波, 唐曉寧 申請人:上海交通大學(xué)