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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

附圖說明

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

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

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

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

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

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

具體實(shí)施方式

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

JN_J＝0 公式(3)

β(q,V):健壯廣義因子，提供運(yùn)動(dòng)奇異點(diǎn)的魯棒性。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

每一個(gè)柵格地圖是一個(gè)二維柵格單元內(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個(gè)粒子所處的柵格單元；

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

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

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)化并生成一個(gè)擴(kuò)大的邊界時(shí)，集群和空閑空間得到更新。在粒子濾波器中，從給定的標(biāo)準(zhǔn)地圖配置生成地圖，并產(chǎn)生多個(gè)不同粒子群的柵格地圖。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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