本發(fā)明屬于機(jī)器人領(lǐng)域，尤其涉及一種搜救機(jī)器人手臂輔助同時定位及構(gòu)建環(huán)境地圖系統(tǒng)及方法。

背景技術(shù)：

機(jī)器人(Robot)是自動執(zhí)行工作的機(jī)器裝置。它既可以接受人類指揮，又可以運行預(yù)先編排的程序，也可以根據(jù)以人工智能技術(shù)制定的原則綱領(lǐng)行動。它的任務(wù)是協(xié)助或取代人類工作的工作，例如生產(chǎn)業(yè)、建筑業(yè)，或是危險的工作。搜救機(jī)器人，為救援而采取先進(jìn)科學(xué)技術(shù)研制的機(jī)器人，如地震救援機(jī)器人，它是一種專門用于大地震后在地下商場的廢墟中尋找幸存者執(zhí)行救援任務(wù)的機(jī)器人。

目前，在搜救機(jī)器人同時定位和構(gòu)建環(huán)境地圖(simultaneous localization and mapping,SLAM)時，有危險的目標(biāo)如爆炸物在構(gòu)建的環(huán)境地圖中未能作標(biāo)記，這樣易傷害搜救中的人員；在搜救和構(gòu)建地圖區(qū)域探索被困者和犧牲者過程中，多會忽略一些遮蓋物，易使搜救人員未注意到犧牲者；在災(zāi)難現(xiàn)場搜救中，同時定位和構(gòu)建地圖所形成的環(huán)境地圖缺少感知性。上述均為現(xiàn)有技術(shù)的不足之處。

技術(shù)實現(xiàn)要素：

為了解決現(xiàn)有技術(shù)的不足，本發(fā)明提供了一種搜救機(jī)器人手臂輔助同時定位及構(gòu)建環(huán)境地圖系統(tǒng)，其具有同時定位和構(gòu)建環(huán)境地圖效果，解決了搜救機(jī)器人構(gòu)建環(huán)境地圖時的感知性問題。

本發(fā)明的一種搜救機(jī)器人手臂輔助同時定位及構(gòu)建環(huán)境地圖系統(tǒng)，包括：

機(jī)器人本體，所述機(jī)器人本體上安裝有機(jī)器人控制器、定位傳感器模塊、圖像采集模塊和機(jī)器人手臂，所述機(jī)器人手臂與手臂驅(qū)動機(jī)構(gòu)相連，所述手臂驅(qū)動機(jī)構(gòu)與機(jī)器人控制器相連，所述定位傳感器模塊和圖像采集模塊分別與機(jī)器人控制器相連；

所述定位傳感器模塊用于實時檢測指定目標(biāo)的位置信息并傳送至機(jī)器人控制器內(nèi)，所述圖像采集模塊還用于實時采集機(jī)器人周圍的圖像信息并傳送至機(jī)器人控制器內(nèi)；所述機(jī)器人控制器用于根據(jù)接收到的圖像信息以及指定目標(biāo)的位置信息進(jìn)行點云匹配來構(gòu)建出目標(biāo)環(huán)境地圖；所述機(jī)器人控制器用于根據(jù)構(gòu)建出的目標(biāo)環(huán)境地圖來實時向手臂驅(qū)動機(jī)構(gòu)輸出脈沖信號來控制機(jī)器人手臂實現(xiàn)搜救工作。

進(jìn)一步的，所述定位傳感器模塊包括機(jī)器人前向定位聲納傳感器、機(jī)器人后向定位聲納傳感器、機(jī)器人激光測距儀和陀螺儀定位傳感器。本發(fā)明通過機(jī)器人前向定位聲納傳感器、機(jī)器人后向定位聲納傳感器，其中，聲納傳感器的原理為：聲納傳感器發(fā)出一個聲波信號，當(dāng)遇到物體后會反射回來，依據(jù)反射時間及波型去計算它的距離及位置超聲波傳感器是利用超聲波的特性研制而成的傳感器。聲納傳感器能夠準(zhǔn)確檢測物體的大小。

進(jìn)一步的，所述機(jī)器人手臂為多自由度手臂。多自由度手臂能夠?qū)崿F(xiàn)機(jī)器人手臂的多維度運動，使得機(jī)器人手臂更加靈活。

進(jìn)一步的，所述機(jī)器人控制器還與機(jī)器人工控機(jī)相連，所述機(jī)器人工控機(jī)與顯示裝置相連。機(jī)器人工控機(jī)用于實時接收機(jī)器人控制器輸出的目標(biāo)環(huán)境地圖，進(jìn)而在顯示裝置上實時顯示。

進(jìn)一步的，所述機(jī)器人工控機(jī)還和云端服務(wù)器相連，所述云端服務(wù)器與遠(yuǎn)程監(jiān)控終端相連。

進(jìn)一步的，所述機(jī)器人手臂的末端還安裝末端執(zhí)行器，所述末端執(zhí)行器還與夾持器相連，所述末端執(zhí)行器與機(jī)器人控制器相連。本發(fā)明利用末端執(zhí)行器來實時調(diào)整夾持器的工作狀態(tài)，實現(xiàn)救援和搜救工作。

本發(fā)明還提供了一種搜救機(jī)器人手臂輔助同時定位及構(gòu)建環(huán)境地圖系統(tǒng)的工作方法。

本發(fā)明的搜救機(jī)器人手臂輔助同時定位及構(gòu)建環(huán)境地圖系統(tǒng)的工作方法，包括：

定位傳感器模塊和圖像采集模塊分別實時檢測指定目標(biāo)的位置信息和機(jī)器人周圍的圖像信息，并傳送至機(jī)器人控制器內(nèi)；

機(jī)器人控制器根據(jù)接收到的圖像信息以及指定目標(biāo)的位置信息進(jìn)行點云匹配來構(gòu)建出目標(biāo)環(huán)境地圖；

機(jī)器人控制器根據(jù)構(gòu)建出的目標(biāo)環(huán)境地圖來實時向手臂驅(qū)動機(jī)構(gòu)輸出脈沖信號來控制機(jī)器人手臂實現(xiàn)搜救工作。

進(jìn)一步的，在機(jī)器人控制器內(nèi)采用分布粒子濾波算法來同時定位和構(gòu)建目標(biāo)環(huán)境地圖。

進(jìn)一步的，該方法還包括：機(jī)器人控制器將構(gòu)建的目標(biāo)環(huán)境地圖輸出至機(jī)器人工控機(jī)，在與機(jī)器人工控機(jī)相連的顯示裝置上實時顯示。

進(jìn)一步的，該方法還包括：遠(yuǎn)程監(jiān)控終端還通過云端服務(wù)器實時調(diào)取機(jī)器人工控機(jī)接收到的目標(biāo)環(huán)境地圖并進(jìn)行遠(yuǎn)程實時監(jiān)控。

與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果是：

本發(fā)明在搜救機(jī)器人構(gòu)建環(huán)境地圖過程中，將有危險的目標(biāo)物標(biāo)識于地圖上，從而防止搜救人員受傷害；機(jī)器人手臂通過控制器作用將遮蓋物翻動遠(yuǎn)離被困者和犧牲者的區(qū)域，防止搜救人員未發(fā)現(xiàn)搜救目標(biāo)；搜救機(jī)器人在災(zāi)難現(xiàn)場探索時，同時定位和建圖中所形成的地圖更加精確和具備感知性，給搜救人員使用，防止惡劣災(zāi)難現(xiàn)場搜救中的人員傷害事故及被困者和犧牲者未被發(fā)現(xiàn)情況。

附圖說明

構(gòu)成本申請的一部分的說明書附圖用來提供對本申請的進(jìn)一步理解，本申請的示意性實施例及其說明用于解釋本申請，并不構(gòu)成對本申請的不當(dāng)限定。

圖1是本發(fā)明的一種搜救機(jī)器人手臂輔助同時定位及構(gòu)建環(huán)境地圖系統(tǒng)結(jié)構(gòu)示意圖；

圖2是本發(fā)明的一種搜救機(jī)器人手臂輔助同時定位及構(gòu)建環(huán)境地圖流程圖；

圖3是本發(fā)明的一種搜救機(jī)器人手臂控制流程；

圖4是本發(fā)明的一種搜救機(jī)器人手臂輔助同時定位及構(gòu)建環(huán)境地圖實驗和仿真過程；

圖5是本發(fā)明的一種搜救機(jī)器人構(gòu)建的環(huán)境地圖。

具體實施方式

應(yīng)該指出，以下詳細(xì)說明都是例示性的，旨在對本申請?zhí)峁┻M(jìn)一步的說明。除非另有指明，本文使用的所有技術(shù)和科學(xué)術(shù)語具有與本申請所屬技術(shù)領(lǐng)域的普通技術(shù)人員通常理解的相同含義。

需要注意的是，這里所使用的術(shù)語僅是為了描述具體實施方式，而非意圖限制根據(jù)本申請的示例性實施方式。如在這里所使用的，除非上下文另外明確指出，否則單數(shù)形式也意圖包括復(fù)數(shù)形式，此外，還應(yīng)當(dāng)理解的是，當(dāng)在本說明書中使用術(shù)語“包含”和/或“包括”時，其指明存在特征、步驟、操作、器件、組件和/或它們的組合。

圖1是本發(fā)明的一種搜救機(jī)器人手臂輔助同時定位及構(gòu)建環(huán)境地圖系統(tǒng)結(jié)構(gòu)示意圖。

如圖1所示，本發(fā)明的一種搜救機(jī)器人手臂輔助同時定位及構(gòu)建環(huán)境地圖系統(tǒng)，包括：

機(jī)器人本體，所述機(jī)器人本體上安裝有機(jī)器人控制器、定位傳感器模塊、圖像采集模塊和機(jī)器人手臂，所述機(jī)器人手臂與手臂驅(qū)動機(jī)構(gòu)相連，所述手臂驅(qū)動機(jī)構(gòu)與機(jī)器人控制器相連，所述定位傳感器模塊和圖像采集模塊分別與機(jī)器人控制器相連；

所述定位傳感器模塊用于實時檢測指定目標(biāo)的位置信息并傳送至機(jī)器人控制器內(nèi)，所述圖像采集模塊還用于實時采集機(jī)器人周圍的圖像信息并傳送至機(jī)器人控制器內(nèi)；所述機(jī)器人控制器用于根據(jù)接收到的圖像信息以及指定目標(biāo)的位置信息進(jìn)行點云匹配來構(gòu)建出目標(biāo)環(huán)境地圖；所述機(jī)器人控制器用于根據(jù)構(gòu)建出的目標(biāo)環(huán)境地圖來實時向手臂驅(qū)動機(jī)構(gòu)輸出脈沖信號來控制機(jī)器人手臂實現(xiàn)搜救工作。

其中，機(jī)器人本體可以為履帶式機(jī)器人或輪式機(jī)器人。

定位傳感器模塊包括機(jī)器人前向定位聲納傳感器、機(jī)器人后向定位聲納傳感器、機(jī)器人激光測距儀和陀螺儀定位傳感器。

本發(fā)明通過機(jī)器人前向定位聲納傳感器、機(jī)器人后向定位聲納傳感器，其中，聲納傳感器的原理為：聲納傳感器發(fā)出一個聲波信號，當(dāng)遇到物體后會反射回來，依據(jù)反射時間及波型去計算它的距離及位置超聲波傳感器是利用超聲波的特性研制而成的傳感器。聲納傳感器能夠準(zhǔn)確檢測物體的大小。

在本實施例中，機(jī)器人手臂為多自由度手臂。多自由度手臂能夠?qū)崿F(xiàn)機(jī)器人手臂的多維度運動，使得機(jī)器人手臂更加靈活。

機(jī)器人控制器還與機(jī)器人工控機(jī)相連，所述機(jī)器人工控機(jī)與顯示裝置相連。機(jī)器人工控機(jī)用于實時接收機(jī)器人控制器輸出的目標(biāo)環(huán)境地圖，進(jìn)而在顯示裝置上實時顯示。

在另一實施例中，所述機(jī)器人工控機(jī)還云端服務(wù)器相連，所述云端服務(wù)器與遠(yuǎn)程監(jiān)控終端相連。

在另一實施例中，所述機(jī)器人手臂的末端還安裝末端執(zhí)行器，所述末端執(zhí)行器還與夾持器相連，所述末端執(zhí)行器與機(jī)器人控制器相連。本發(fā)明利用末端執(zhí)行器來實時調(diào)整夾持器的工作狀態(tài)，實現(xiàn)救援和搜救工作。

圖2是本發(fā)明的一種搜救機(jī)器人手臂輔助同時定位及構(gòu)建環(huán)境地圖流程圖。

如圖2所示，本發(fā)明的搜救機(jī)器人手臂輔助同時定位及構(gòu)建環(huán)境地圖系統(tǒng)的具體流程，包括：

定位傳感器模塊和圖像采集模塊分別實時檢測指定目標(biāo)的位置信息和機(jī)器人周圍的圖像信息，并傳送至機(jī)器人控制器內(nèi)；

機(jī)器人控制器根據(jù)接收到的圖像信息以及指定目標(biāo)的位置信息進(jìn)行點云匹配來構(gòu)建出目標(biāo)環(huán)境地圖；

機(jī)器人控制器根據(jù)構(gòu)建出的目標(biāo)環(huán)境地圖來實時向手臂驅(qū)動機(jī)構(gòu)輸出脈沖信號來控制機(jī)器人手臂實現(xiàn)搜救工作。

其中，在機(jī)器人控制器內(nèi)采用分布粒子濾波算法來同時定位和構(gòu)建目標(biāo)環(huán)境地圖。

該方法還包括：機(jī)器人控制器將構(gòu)建的目標(biāo)環(huán)境地圖輸出至機(jī)器人工控機(jī)，在與機(jī)器人工控機(jī)相連的顯示裝置上實時顯示。

該方法還包括：遠(yuǎn)程監(jiān)控終端還通過云端服務(wù)器實時調(diào)取機(jī)器人工控機(jī)接收到的目標(biāo)環(huán)境地圖并進(jìn)行遠(yuǎn)程實時監(jiān)控。

圖3是本發(fā)明的一種搜救機(jī)器人手臂控制流程。

如圖3所示，搜救機(jī)器人手臂的速度可由公式(1)來表示：

W:q的向量函數(shù)；

α：標(biāo)量常數(shù)，使得達(dá)到極小值時，V取得期望值；

V：長度為m維的末端執(zhí)行器的狀態(tài)向量；

q：長度為n維的關(guān)節(jié)位置向量，表示手臂關(guān)節(jié)到達(dá)的位置，其物理含義為關(guān)節(jié)到達(dá)的位置組成的一個向量；

q的時間微分，代表關(guān)節(jié)到達(dá)位置時的速度。

F:所有標(biāo)準(zhǔn)的加權(quán)和，是核心速度控制的標(biāo)準(zhǔn)向量，這個向量既可以是單個優(yōu)化標(biāo)準(zhǔn)，也可以是多個優(yōu)化標(biāo)準(zhǔn)的聯(lián)合。

w_i:第i個標(biāo)準(zhǔn)的權(quán)重值；

F_i：第i個標(biāo)準(zhǔn)條件；

J:操作器m×n維的雅可比(Jacobian)矩陣，用以控制手臂運動；

N_J:n×(n-m)維包含零空間的雅可比矩陣的向量組集合，其秩為(n-m)，定義為

JN_J＝0 公式(3)

β(q,V):健壯廣義因子，提供運動奇異點的魯棒性。

本發(fā)明的控制器所用控制系統(tǒng)具備關(guān)節(jié)靈活度和能量消耗控制，運動算法保證在手臂運動的關(guān)節(jié)極限無控制操作的情況下，能夠避障，而且使得運動能量消耗最小?？刂葡到y(tǒng)用于約束機(jī)器人手臂按期望的方式運動，同時避免關(guān)節(jié)極限、奇異點，以及與環(huán)境中物體的碰撞。

本發(fā)明的手臂運動控制器設(shè)計具備通用性，控制系統(tǒng)被加載于手臂控制器?？刂破鞯母鞣N可能的控制算法可以實現(xiàn)不同控制標(biāo)準(zhǔn)的運動形式和各種結(jié)構(gòu)目標(biāo)物體的抓取，但是，手臂關(guān)節(jié)運動極限避免和與環(huán)境物體的避障是控制器的運動控制算法的最頻繁使用的兩個優(yōu)化標(biāo)準(zhǔn)。在本發(fā)明的手臂控制器中，優(yōu)化范圍設(shè)定為這兩個標(biāo)準(zhǔn)。本發(fā)明設(shè)計的手臂運動控制器只包含避免關(guān)節(jié)運動極限和與環(huán)境物體避障兩個優(yōu)化標(biāo)準(zhǔn)。通過對每個選擇的優(yōu)化標(biāo)準(zhǔn)設(shè)定權(quán)重因子來控制和優(yōu)化手臂的運動。

在本發(fā)明中，機(jī)器人手臂輔助構(gòu)建環(huán)境地圖步驟：

步驟一：使用關(guān)節(jié)末端執(zhí)行器集合制機(jī)器人手臂的一組關(guān)節(jié)角，關(guān)節(jié)末端執(zhí)行器集合是公式(1)所述的手臂末端執(zhí)行器到達(dá)的位置的向量V所需要的控制命令組成的集合；

步驟二：利用點末端執(zhí)行器集合控制機(jī)器人手臂到達(dá)某一點，點末端執(zhí)行器集合是公式(1)所述的手臂關(guān)節(jié)到達(dá)的位置向量b所需要控制命令組成的集合；

步驟三：利用框架末端執(zhí)行器集合調(diào)整機(jī)器人手臂的抓取位姿，框架末端執(zhí)行器集合是公式(1)所述的操縱機(jī)器人手臂整體協(xié)調(diào)運動的雅可比矩陣向量組N_J的控制命令集合；

步驟四：開合機(jī)器人手臂連接的夾持器；

步驟五：打開并運行之前創(chuàng)建的一系列操作的程序代碼示例集，本發(fā)明支持離線建立機(jī)器人手臂運動命令集合，生成控制程序代碼示例集，加載于手臂控制器的控制系統(tǒng)，提供機(jī)器人手臂抓取物體的控制參數(shù)；

步驟六：利用關(guān)節(jié)控制移動機(jī)器人手臂，并利用末端執(zhí)行器控制移動夾持器；

步驟七：計算所需的距離和方位角，移動機(jī)器人手臂的關(guān)節(jié)狀態(tài)，達(dá)到所期望的位姿；這個探測過程的軌跡可以是直線或曲線；

步驟八：以給定的速度移動機(jī)器人手臂的末端執(zhí)行器；

步驟九：切換不同的末端執(zhí)行器集合，控制機(jī)器人手臂運動，輔助機(jī)器人定位和構(gòu)建地圖過程。

本發(fā)明支持離線建立機(jī)器人手臂運動命令集合，生成控制程序代碼示例集，存儲于手臂控制器的控制系統(tǒng)，提供機(jī)器人手臂抓取物體的控制參數(shù)，同時可以實時地在控制器上編輯和設(shè)定機(jī)器人手臂的位姿控制命令語句。這些控制命令包括公式(1)所述的手臂末端執(zhí)行器到達(dá)的位置的向量V所需要的控制命令集合、手臂關(guān)節(jié)到達(dá)的位置向量b所需要控制命令集合、操縱機(jī)器人手臂整體協(xié)調(diào)運動的雅可比矩陣向量組N_J的控制命令集合，這些控制命令集合由關(guān)節(jié)末端執(zhí)行器集合、點末端執(zhí)行器集合、框架末端執(zhí)行器集合組成。

本發(fā)明在同時定位和構(gòu)建地圖過程采用分布粒子濾波(DP,Distributed Particle)算法。

本發(fā)明中所用搜救機(jī)器人還包含內(nèi)置工控機(jī)(industrial PC,IPC)，安裝機(jī)器人地圖構(gòu)建控制系統(tǒng)，且機(jī)器人內(nèi)置的工控機(jī)能夠接收遠(yuǎn)程PC計算機(jī)控制中心的控制。機(jī)器人手臂控制器的控制系統(tǒng)、分布粒子濾波器及其粒子濾波運動估計器都安裝和運行于搜救機(jī)器人內(nèi)置的工控機(jī)IPC上。手臂控制器的控制系統(tǒng)、分布粒子濾波器及其粒子濾波運動估計器同為搜救機(jī)器人構(gòu)建地圖控制器的組成部分。分布粒子濾波器算法持續(xù)估計搜救機(jī)器人的位置和姿態(tài)，以使機(jī)器人位姿配合手臂進(jìn)行感知探測，當(dāng)機(jī)器人手臂獲取到搜救的興趣目標(biāo)地點或物體時，則附加的粒子被記錄，作為地圖構(gòu)建信息，以確定機(jī)器人所處位置和方向，并在所構(gòu)建的搜救區(qū)域的地圖上標(biāo)識出興趣目標(biāo)地點或物體。

全局地圖分割為柵格單元，每個柵格單元獨立完成定位和構(gòu)建局部地圖過程。每個柵格單元包含一定數(shù)量的隨機(jī)粒子，根據(jù)每個粒子的權(quán)重因子，計算柵格地圖的占有率，相應(yīng)柵格單元通過分配空閑率被占有。

每一個柵格地圖是一個二維柵格單元內(nèi)的隨機(jī)粒子，柵格單元的隨機(jī)粒子的概率定義為：

P(G^k|x_k,m_k)＝P(G^k|p^k)P(p^k|x_k,m_k) 公式(4)

其中：

P(G^k|x_k,m_k):柵格地圖單元隨機(jī)粒子占有的概率；

m_k:表示第k個粒子所處的柵格單元；

x_k:表示第k個粒子當(dāng)前所處的位置；

p^k:第k個粒子的柵格單元占有概率；

G^k:柵格單元的空閑率；

P(G^k|p^k):柵格單元空閑的條件概率；

P(p^k|x_k,m_k):柵格單元占有的概率，是確定性參數(shù)α，根據(jù)不同柵格單元在0～1之間取值，因此公式(4)表示為公式(5):

P(G^k|x_k,m_k)＝αP(G^k|p^k),α∈(0～1) 公式(5)

粒子濾波算法的柵格地圖在空閑集群中以一種進(jìn)化的方式編寫程序代碼。按照柵格地圖的占有概率，采用相應(yīng)的值進(jìn)入空閑空間。當(dāng)柵格地圖進(jìn)化并生成一個擴(kuò)大的邊界時，集群和空閑空間得到更新。在粒子濾波器中，從給定的標(biāo)準(zhǔn)地圖配置生成地圖，并產(chǎn)生多個不同粒子群的柵格地圖。

粒子濾波運動估計器計算可用的柵格單元，當(dāng)?shù)貓D的柵格單元數(shù)目增加時，搜救移動機(jī)器人的定位信息得到更新。粗的柵格分辨率將會導(dǎo)致不準(zhǔn)確的定位信息和地圖構(gòu)建，例如，較大的柵格單元尺寸將會涉及較少的位置信息。

相比于現(xiàn)有技術(shù)，分布粒子濾波器算法能夠改善基于信息熵的探測靈敏度分析的閉環(huán)覆蓋性能。同時，分布粒子濾波器算法的粒子滲透占有性和柵格地圖構(gòu)建的進(jìn)化性，適合于搜救移動機(jī)器人在非結(jié)構(gòu)的、災(zāi)后障礙物簇集場景的定位和環(huán)境地圖構(gòu)建。

柵格地圖單元和機(jī)器人的位姿在粒子濾波器中保持一個聯(lián)合分布，柵格地圖是一個全局像柵格一樣占有陣列，而且每一個柵格單元與機(jī)器人的位姿相關(guān)。分布粒子濾波器分開儲存每一個粒子的柵格地圖的副本，在地圖構(gòu)建過程中，關(guān)聯(lián)機(jī)器人的位姿信息。

根據(jù)記錄的粒子數(shù)量的增加，更新原有的標(biāo)準(zhǔn)地圖和機(jī)器人的位姿。機(jī)器人在n時刻的位姿ρ_n表示為

ρ_n＝[x_n,y_n,θ_n]^T 公式(6)

其中，(x_n,y_n)是笛卡爾坐標(biāo)位置，θ_n代表方位角。

在n時刻的柵格地圖用一個二維矩陣表示，N_x是行數(shù)，N_y是列數(shù)，在m_n中的像素灰度值表明柵格單元的空閑、占有和未被探測三種狀態(tài)。搜救機(jī)器人在n時刻的測量值是S_n，u_n∈R³和z_n∈R¹⁸¹.S_n定位信息包含ρ_nθ_n和m_n；u_n∈R³估計機(jī)器人位置的三個參數(shù)x_n,y_n,θ_n；z_n∈R¹⁸¹包含激光測距儀估計機(jī)器人前方超過180⁰的視角范圍。

機(jī)器人的定位信息是一個概率密度函數(shù)，定義為：

bel(S_n)＝ρ(S_n|z₁,...,z_n,u₁,...,u_n) 公式(7)

在分布粒子濾波算法中，產(chǎn)生一系列粒子來估計概率密度函數(shù)bel(S_n)，每一個粒子含有一組機(jī)器人位姿信息和局部柵格地圖。當(dāng)機(jī)器人收集到一組新的測量數(shù)據(jù)，就分配給每個粒子一個影響因子。一個粒子對當(dāng)前測量的支持越高，分配的影響因子就越高。因此，原先的柵格地圖被更新，新的位姿通過重采樣的粒子得到確認(rèn)，而且，一個粒子的影響因子越高，越容易被選取。通過計算柵格地圖占有率，重復(fù)這樣一個重采樣步驟，相同的粒子可以被多次選取。

本發(fā)明中所用機(jī)器人在給定的搜救區(qū)域周期性地進(jìn)行全局覆蓋式探索，這樣每個特定粒子群與一個柵格地圖相聯(lián)系，形成各個柵格局部地圖。每一個局部柵格地圖有各自獨立的定位和地圖構(gòu)建過程。然后根據(jù)各柵格地圖的粒子的權(quán)重因子，即公式(5)所述確定性參數(shù)α，由公式(4)-(5),計算每個局部柵格地圖的占有率。隨著粒子數(shù)量的增加，由公式(6)-(7)更新機(jī)器人位姿信息、各柵格局部地圖的定位信息，根據(jù)各局部柵格地圖的占有概率，輸出搜救區(qū)域的一個全局地圖。

圖4是本發(fā)明的一種搜救機(jī)器人手臂輔助同時定位及構(gòu)建環(huán)境地圖實驗和仿真過程。

在進(jìn)行仿真實驗過程中，本發(fā)明的軟件編譯環(huán)境為Cmake、Visual studio 2013、Matlab；Matlab用來仿真MobileRobot移動機(jī)器人控制平臺控制機(jī)器人探索和建圖，Visual studio 2013用來控制程序生成，Cmake用來進(jìn)行程序編譯環(huán)境配置。

通過上述實驗，得到的搜救機(jī)器人構(gòu)建的環(huán)境地圖，如圖5所示。根據(jù)圖5可知：本發(fā)明在搜救機(jī)器人構(gòu)建環(huán)境地圖過程中，將有危險的目標(biāo)物標(biāo)識于地圖上，從而防止搜救人員受傷害；機(jī)器人手臂通過控制器作用將遮蓋物翻動遠(yuǎn)離被困者和犧牲者的區(qū)域，防止搜救人員未發(fā)現(xiàn)搜救目標(biāo)；搜救機(jī)器人在災(zāi)難現(xiàn)場探索時，同時定位和建圖中所形成的地圖更具備精確性，給搜救人員使用，防止惡劣災(zāi)難現(xiàn)場搜救中的人員傷害事故及被困者和犧牲者未被發(fā)現(xiàn)情況。機(jī)器人控制器用于搜救機(jī)器人手臂輔助構(gòu)建環(huán)境地圖，補(bǔ)償了機(jī)器人自身定位和建圖中的現(xiàn)有技術(shù)的感知性不足。

上述雖然結(jié)合附圖對本發(fā)明的具體實施方式進(jìn)行了描述，但并非對本發(fā)明保護(hù)范圍的限制，所屬領(lǐng)域技術(shù)人員應(yīng)該明白，在本發(fā)明的技術(shù)方案的基礎(chǔ)上，本領(lǐng)域技術(shù)人員不需要付出創(chuàng)造性勞動即可做出的各種修改或變形仍在本發(fā)明的保護(hù)范圍以內(nèi)。