可變形機(jī)器人在線同步定位與地圖創(chuàng)建系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)

王 玲，房雅洪，王明輝

（1. 沈陽(yáng)理工大學(xué) 信息科學(xué)與工程學(xué)院，沈陽(yáng) 110159；2. 中國(guó)科學(xué)院 沈陽(yáng)自動(dòng)化研究所 機(jī)器人學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室，沈陽(yáng) 110016）

0 引言

由于全球定位系統(tǒng)受到信號(hào)強(qiáng)度的影響無(wú)法在室內(nèi)環(huán)境或廢墟環(huán)境進(jìn)行全局地圖絕對(duì)定位，因此在室內(nèi)復(fù)雜環(huán)境或廢墟環(huán)境中進(jìn)行移動(dòng)機(jī)器人的同步定位與地圖創(chuàng)建[1]（Sim u ltaneous Localization and Mapping，SLAM）問(wèn)題的研究尤為重要。

同步定位與地圖創(chuàng)建問(wèn)題是指在一個(gè)未知環(huán)境中，移動(dòng)機(jī)器人利用其搭載的傳感器增量式地創(chuàng)建未知環(huán)境的連續(xù)地圖，同步確定其在地圖中的位置。它是移動(dòng)機(jī)器人[2]實(shí)現(xiàn)自主化的一項(xiàng)關(guān)鍵技術(shù)。

本文研究的同步定位與地圖創(chuàng)建系統(tǒng)以中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所自主研制的可變形機(jī)器人[3]為實(shí)驗(yàn)平臺(tái)，在室內(nèi)復(fù)雜環(huán)境或廢墟環(huán)境中實(shí)現(xiàn)機(jī)器人的自主定位和環(huán)境地圖的創(chuàng)建更新。根據(jù)可變形機(jī)器人多種構(gòu)型[4]的結(jié)構(gòu)特點(diǎn)擴(kuò)大了環(huán)境的適應(yīng)范圍，使同步定位與地圖創(chuàng)建系統(tǒng)在實(shí)際中可廣泛運(yùn)用。

1 同步定位與地圖創(chuàng)建系統(tǒng)結(jié)構(gòu)設(shè)計(jì)

同步定位與地圖創(chuàng)建系統(tǒng)的功能：實(shí)現(xiàn)移動(dòng)機(jī)器人在未知環(huán)境中利用其搭載的傳感器進(jìn)行自定位和創(chuàng)建環(huán)境地圖。移動(dòng)機(jī)器人在全局環(huán)境中自主定位需要實(shí)時(shí)獲取機(jī)器人的位置信息和姿態(tài)信息，并根據(jù)獲取的信息進(jìn)行機(jī)器人的航位推算，產(chǎn)生當(dāng)前時(shí)刻機(jī)器人的位置信息和姿態(tài)信息的預(yù)測(cè)。創(chuàng)建未知環(huán)境的地圖需要完成環(huán)境測(cè)距信息的獲取和處理，完成環(huán)境特征的提取和特征的數(shù)據(jù)關(guān)聯(lián)，進(jìn)行地圖的創(chuàng)建更新。根據(jù)同步定位與地圖創(chuàng)建系統(tǒng)的功能需求設(shè)計(jì)系統(tǒng)結(jié)構(gòu)框圖如圖1所示。

圖1 同步定位與地圖創(chuàng)建系統(tǒng)結(jié)構(gòu)框圖

根據(jù)同步定位與地圖創(chuàng)建系統(tǒng)結(jié)構(gòu)框圖分析：同步定位與地圖創(chuàng)建系統(tǒng)由環(huán)境信息獲取子系統(tǒng)、機(jī)器人狀態(tài)信息獲取子系統(tǒng)、定位和地圖在線更新子系統(tǒng)組成。

環(huán)境信息獲取子系統(tǒng)：提取并處理環(huán)境信息，解析環(huán)境中障礙物的相對(duì)距離信息，把處理后的測(cè)距信息傳輸給定位和地圖在線更新子系統(tǒng)，為地圖的創(chuàng)建提供數(shù)據(jù)基礎(chǔ)。

機(jī)器人狀態(tài)信息獲取子系統(tǒng)：實(shí)現(xiàn)機(jī)器人的位置信息和姿態(tài)信息的獲取。并把機(jī)器人的位置信息和姿態(tài)信息傳遞給定位和地圖在線更新子系統(tǒng)，為機(jī)器人的航位推算提供數(shù)據(jù)基礎(chǔ)。

定位和地圖在線更新子系統(tǒng)：根據(jù)機(jī)器人的狀態(tài)信息獲取子系統(tǒng)的數(shù)據(jù)在全局地圖下進(jìn)行機(jī)器人航位推算并且產(chǎn)生當(dāng)前時(shí)刻的機(jī)器人的位置信息和姿態(tài)信息預(yù)測(cè)，實(shí)現(xiàn)移動(dòng)機(jī)器人的自主定位；對(duì)環(huán)境信息獲取子系統(tǒng)觀測(cè)到的相對(duì)距離信息進(jìn)行預(yù)處理，完成環(huán)境特征的提取和特征的數(shù)據(jù)關(guān)聯(lián)；結(jié)合當(dāng)前環(huán)境地圖信息、機(jī)器人的狀態(tài)預(yù)測(cè)信息、當(dāng)前時(shí)刻的環(huán)境實(shí)際觀測(cè)數(shù)據(jù)信息實(shí)現(xiàn)地圖創(chuàng)建更新。

2 同步定位與地圖創(chuàng)建系統(tǒng)功能實(shí)現(xiàn)

2.1 環(huán)境信息獲取子系統(tǒng)功能實(shí)現(xiàn)

環(huán)境信息獲取子系統(tǒng)完成環(huán)境相對(duì)距離信息的提取。由于視覺(jué)傳感器受室內(nèi)復(fù)雜環(huán)境或廢墟環(huán)境光線較弱、易顛簸等特點(diǎn)的局限不宜使用，因此環(huán)境信息獲取子系統(tǒng)采用激光傳感器和環(huán)境信息獲取處理器組成方式。激光傳感器需要完成環(huán)境信息的采集，環(huán)境信息獲取處理器需要控制激光傳感器測(cè)距工作并把激光傳感器采集回來(lái)的信息經(jīng)過(guò)處理轉(zhuǎn)化為環(huán)境相對(duì)距離信息。

激光傳感器有串口通信和USB通信兩種通信方式，環(huán)境信息獲取處理器和激光傳感器采用串口通訊方式。激光傳感器的指令集L命令、G命令分別控制激光傳感器的開(kāi)啟和關(guān)閉、環(huán)境信息數(shù)據(jù)的采集。

環(huán)境信息獲取處理器發(fā)送激光傳感器開(kāi)啟命令，激光傳感器返回的狀態(tài)標(biāo)志位為0，說(shuō)明激光傳感器被開(kāi)啟；如果返回的狀態(tài)標(biāo)志位不為0，那么環(huán)境信息獲取處理器控制激光傳感器開(kāi)啟命令出錯(cuò)，則需要重新發(fā)送激光傳感器開(kāi)啟命令。環(huán)境信息獲取處理器發(fā)送激光測(cè)距命令，可以設(shè)置激光傳感器測(cè)距的起止范圍和測(cè)距的數(shù)據(jù)量大小，如果激光傳感器返回的測(cè)距狀態(tài)位為0，說(shuō)明激光傳感器開(kāi)始采集環(huán)境信息，環(huán)境信息處理器根據(jù)激光傳感器測(cè)距的起止范圍和測(cè)距的數(shù)據(jù)量大小進(jìn)行環(huán)境信息數(shù)據(jù)的提取和解析；如果激光傳感器返回的測(cè)距狀態(tài)位不為0，那么環(huán)境信息獲取處理器發(fā)送激光測(cè)距命令出錯(cuò)，需要重新發(fā)送激光測(cè)距命令。

環(huán)境信息獲取處理器和激光傳感器進(jìn)行環(huán)境信息數(shù)據(jù)提取的功能流程圖如圖2所示。

圖2 環(huán)境信息提取的功能流程圖

2.2 機(jī)器人狀態(tài)信息獲取子系統(tǒng)功能實(shí)現(xiàn)

機(jī)器人狀態(tài)信息獲取子系統(tǒng)獲取移動(dòng)機(jī)器人在采樣時(shí)間內(nèi)移動(dòng)的距離信息和轉(zhuǎn)過(guò)的角度信息。采用實(shí)驗(yàn)估算的方法估測(cè)可變形機(jī)器人[4]在并排構(gòu)型下的線速度和角速度的值，并用軟件記錄機(jī)器人在移動(dòng)過(guò)程中直線前進(jìn)運(yùn)動(dòng)的時(shí)間和轉(zhuǎn)向移動(dòng)的時(shí)間，根據(jù)機(jī)器人在并排構(gòu)型下的線速度、角速度、直線前進(jìn)時(shí)間、轉(zhuǎn)向移動(dòng)時(shí)間等信息獲得機(jī)器人在采樣時(shí)間內(nèi)移動(dòng)的相對(duì)距離信息和轉(zhuǎn)過(guò)的航向偏向角信息。根據(jù)機(jī)器人在采樣時(shí)間內(nèi)移動(dòng)的相對(duì)距離信息和航向偏向角信息產(chǎn)生當(dāng)前時(shí)刻機(jī)器人的位置信息和姿態(tài)信息的預(yù)測(cè)。移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型[5]如圖3所示。

圖3 移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型

根據(jù)上圖的移動(dòng)機(jī)器人在 時(shí)刻移動(dòng)到 時(shí)刻的位置和姿態(tài)信息預(yù)測(cè)可表示為：

2.3 定位和地圖在線更新子系統(tǒng)功能實(shí)現(xiàn)

定位和地圖在線更新子系統(tǒng)根據(jù)環(huán)境信息獲取子系統(tǒng)中激光傳感器的觀測(cè)數(shù)據(jù)和機(jī)器人狀態(tài)信息獲取子系統(tǒng)中機(jī)器人在采樣時(shí)間內(nèi)移動(dòng)的相對(duì)距離信息和航向偏向角信息數(shù)據(jù)，實(shí)現(xiàn)了機(jī)器人自主定位和全局地圖的在線更新。算法流程如圖4所示。

圖4 定位和地圖在線更新子系統(tǒng)功能流程圖

全局特征地圖的創(chuàng)建更新需要采用擴(kuò)展卡爾曼濾波原理估計(jì)每個(gè)時(shí)刻的運(yùn)動(dòng)預(yù)測(cè)、觀測(cè)更新。

傳感器觀測(cè)的數(shù)據(jù)提取的特征為一個(gè)新的環(huán)境特征，則根據(jù)當(dāng)前傳感器觀測(cè)數(shù)據(jù)，對(duì)當(dāng)前的估計(jì)狀態(tài)進(jìn)行擴(kuò)展，相當(dāng)于更新了特征地圖。同時(shí)一個(gè)新的局部地圖進(jìn)行初始化。

3 實(shí)驗(yàn)研究

選取2m×10m的室內(nèi)走廊環(huán)境進(jìn)行實(shí)驗(yàn)，驗(yàn)證在線同步定位與地圖創(chuàng)建系統(tǒng)的有效性。實(shí)驗(yàn)環(huán)境如圖5所示，該實(shí)驗(yàn)環(huán)境中，門框和墻壁形成的直線交點(diǎn)稱為角點(diǎn)，該角點(diǎn)特征作為全局地圖的特征。實(shí)驗(yàn)采用激光傳感器URG-04LX，掃描的角度范圍240°，角度分辨率為0.36°，測(cè)量的最大距離為4095mm，測(cè)量的距離在1000mm以內(nèi)精度±10mm，超過(guò)1000mm精度±1%。激光傳感器每3s采集一次環(huán)境信息數(shù)據(jù)，進(jìn)行數(shù)據(jù)的提取和解析。在整個(gè)實(shí)驗(yàn)過(guò)程中，機(jī)器人在移動(dòng)的同時(shí)，緩慢地創(chuàng)建和更新環(huán)境地圖。實(shí)驗(yàn)結(jié)果如圖6所示。

圖5 實(shí)際走廊環(huán)境

圖6 實(shí)驗(yàn)結(jié)果

根據(jù)實(shí)驗(yàn)結(jié)果可以分析出：圖6(a)表示可變形機(jī)器人單程創(chuàng)建地圖過(guò)程；圖6(b)表示可變形機(jī)器人單程創(chuàng)建地圖結(jié)束；圖6(c)表示可變形機(jī)器人返程創(chuàng)建地圖過(guò)程；圖6(d)表示可變形機(jī)器人創(chuàng)建地圖結(jié)束。實(shí)驗(yàn)中可變形機(jī)器人增量式地在線創(chuàng)建環(huán)境地圖，機(jī)器人移動(dòng)過(guò)程中不斷更新和校正地圖，驗(yàn)證了該同步定位與地圖創(chuàng)建系統(tǒng)的有效性。創(chuàng)建的環(huán)境地圖由于受到里程計(jì)信息的累積誤差和機(jī)器人在側(cè)向滑移誤差等干擾出現(xiàn)了一些小凸起。

4 結(jié)束語(yǔ)

本文設(shè)計(jì)的可變形機(jī)器人在線同步定位與地圖創(chuàng)建系統(tǒng)，根據(jù)可變形機(jī)器人的結(jié)構(gòu)特點(diǎn)和運(yùn)動(dòng)特性，能夠在室內(nèi)復(fù)雜環(huán)境或廢墟環(huán)境中進(jìn)行機(jī)器人的自主定位和環(huán)境地圖在線創(chuàng)建和更新。實(shí)驗(yàn)驗(yàn)證了該同步定位與地圖創(chuàng)建系統(tǒng)的算法實(shí)時(shí)性好，創(chuàng)建的環(huán)境地圖效果好。今后的工作：改進(jìn)同步定位與地圖創(chuàng)建系統(tǒng)的結(jié)構(gòu)設(shè)計(jì)和優(yōu)化算法，減小實(shí)驗(yàn)中傳感器誤差和機(jī)器人側(cè)向滑移誤差等干擾。

[1] 郭帥,馬書(shū)根,李斌,王明輝,王越超.基于Vo ronoi地圖表示方法的同步定位與地圖創(chuàng)建[J].自動(dòng)化學(xué)報(bào),2011,37(9):1095-1104.

[2] 孫榮川,馬書(shū)根,李斌,王明輝,王越超.SLASEM中的廣義距離的數(shù)據(jù)關(guān)聯(lián)[J].機(jī)器人.2011, 33(1):208-214.

[3] 王楠,吳成東,王明輝.可變形災(zāi)難救援機(jī)器人控制站系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[J].機(jī)器人.2011,33(1):203-207.

[4] 常鍵,吳成東,李斌.一種可變形移動(dòng)機(jī)器人環(huán)境地圖創(chuàng)建方法[J].儀器儀表學(xué)報(bào)增刊.2010,31(8):207-210.

[5] 蘇麗穎,王琨,余躍慶.柵格地圖中基于改進(jìn)粒子濾波的SLAM[J].華中科技大學(xué)學(xué)報(bào).2011,39(9):165-168.