一種基于無線傳感器網(wǎng)絡(luò)的未知區(qū)域地圖構(gòu)建系統(tǒng)

王興偉，陳 浩，師 普，史金炆，劉 煜

（中國石油大學(xué)(華東） 山東 青島 266580）

1 基于無線傳感器網(wǎng)絡(luò)的未知區(qū)域地圖構(gòu)建系統(tǒng)

針對地圖構(gòu)建的問題，現(xiàn)在一些陸地機(jī)器人已經(jīng)實(shí)現(xiàn)，但只能構(gòu)建二維地圖，對于機(jī)器人的控制，目前也只限于人工控制，一旦失去通信信號，將無法實(shí)現(xiàn)對機(jī)器人的有效控制，除此之外，傳統(tǒng)救援機(jī)器人由于缺少多機(jī)器人間的協(xié)同性，救援效率不高。現(xiàn)階段，災(zāi)后救援機(jī)器人控制方式單一（人工控制），且環(huán)境信息采集能力較弱，對于災(zāi)后完全陌生的環(huán)境不具備自定位和導(dǎo)航功能。災(zāi)害發(fā)生后，若不能及時(shí)的獲得災(zāi)區(qū)環(huán)境內(nèi)部情況，就不能實(shí)時(shí)的展開有效的救援以及疏散工作。本系統(tǒng)提出的通過多機(jī)器人編隊(duì)式互聯(lián)來實(shí)現(xiàn)及時(shí)定位與環(huán)境地圖構(gòu)建的方案，能有效地開展救援工作與環(huán)境信息的采集。結(jié)合雙目視覺，人工智能等技術(shù)，在災(zāi)后現(xiàn)場第一時(shí)間自主的進(jìn)行地圖構(gòu)建和救援準(zhǔn)備工作。

2 地圖構(gòu)建系統(tǒng)原理

本系統(tǒng)在實(shí)現(xiàn)受災(zāi)環(huán)境的地圖構(gòu)建時(shí)：由機(jī)器人從一個(gè)未知點(diǎn)開始移動，利用激光測距雷達(dá)發(fā)射電磁波，當(dāng)電磁波遇到阻礙就會反射回來，通過記錄電磁波往返的時(shí)間，根據(jù)電磁波在介質(zhì)中的傳播速度從而計(jì)算出機(jī)器人到障礙物的距離，同時(shí)將障礙物標(biāo)記為路標(biāo)點(diǎn)，然后根據(jù)機(jī)器人與路標(biāo)點(diǎn)之間的相對位置和里程計(jì)的讀數(shù)估計(jì)出機(jī)器人自身和路標(biāo)點(diǎn)的位置，構(gòu)成全局坐標(biāo)系，機(jī)器人繼續(xù)運(yùn)動，進(jìn)行路標(biāo)識別和自身定位，最終構(gòu)建出完整的路標(biāo)地圖。

里程計(jì)和激光測距雷達(dá)放置在密封殼內(nèi)，從而完成機(jī)器人的定位和誤差校正；使用超遠(yuǎn)距離傳輸模塊Xbeepro進(jìn)行數(shù)據(jù)傳輸，確保數(shù)據(jù)的可靠性。樹莓派控制器安裝在密封殼內(nèi)主板上，內(nèi)部移植ROS操作系統(tǒng)實(shí)現(xiàn)完整SLAM?，F(xiàn)有SLAM算法的實(shí)現(xiàn)是在MATLAB仿真軟件的環(huán)境下實(shí)現(xiàn)的，本作品在樹莓派上安裝Ubuntu系統(tǒng)，再將ROS機(jī)器人操作系統(tǒng)移植到Ubuntu系統(tǒng)中，調(diào)用ROS中相應(yīng)的功能包實(shí)現(xiàn)仿真。并將SLAM算法移植到ROS中，對機(jī)器人電機(jī)轉(zhuǎn)速進(jìn)行控制，處理傳感器測得的數(shù)據(jù)信息，實(shí)現(xiàn)對機(jī)器人的實(shí)際控制，最終構(gòu)建特征地圖，實(shí)現(xiàn)完整的SLAM過程。

地圖信息采集上，通過在樹莓派系統(tǒng)上移植機(jī)器人控制操作系統(tǒng)的方法，實(shí)現(xiàn)對機(jī)器人的運(yùn)動控制，配合激光測距雷達(dá)、里程計(jì)等，采用SLAM技術(shù)（全稱即時(shí)定位與地圖構(gòu)建），實(shí)現(xiàn)機(jī)器人自主導(dǎo)航和地圖構(gòu)建功能。同時(shí)實(shí)現(xiàn)地圖構(gòu)建數(shù)據(jù)的處理、存儲與傳輸，救援人員可在遠(yuǎn)程手持設(shè)備上查看地圖和圖像，便于開展救援決策、發(fā)送指揮指令，機(jī)器人搭載的聲光報(bào)警設(shè)備可引導(dǎo)被困人員撤離。

2.1 地圖構(gòu)建算法的移植

我們所采用的SLAM技術(shù)是智能機(jī)器人的基礎(chǔ)技術(shù)，解決機(jī)器人自主導(dǎo)航過程中的三個(gè)問題：“我在哪”，“到哪去”和“怎么去”。機(jī)器人通過應(yīng)周圍環(huán)境，定位自己，構(gòu)建地圖，尋找目標(biāo)點(diǎn)，規(guī)劃行駛路線。現(xiàn)有的機(jī)器人的定位方法大體可以分為三種：相對定位，絕對定位和組合定位。相對定位是依靠機(jī)器人自身的感應(yīng)器進(jìn)行的定位，絕對定位是依靠外界發(fā)來的定位信息對自己進(jìn)行定位，組合定位是相對定位和絕對定位一起進(jìn)行綜合定位。在作品功能實(shí)現(xiàn)的過程中，我們對機(jī)器人運(yùn)動模型公式進(jìn)行了改進(jìn)。

外部傳感器通過觀測周圍的環(huán)境，解析觀測信息，來對自己進(jìn)行定位。某個(gè)環(huán)境特征相對于機(jī)器人自己的位置信息為Z（k），則公式1所示：

其中xi和yi是某個(gè)環(huán)境特征位置的坐標(biāo)，xvy（k）和xvx（k）為機(jī)器人的位置坐標(biāo)。

內(nèi)部傳感器通過感應(yīng)機(jī)器人的運(yùn)動信息來定位，下圖為機(jī)器人的運(yùn)動模型，設(shè)當(dāng)前時(shí)刻為i，下一時(shí)刻的時(shí)間就是i+1。那么相對應(yīng)的當(dāng)前位置的橫坐標(biāo)為x（i）,縱坐標(biāo)為y（i）,面向角為θ（i），下一時(shí)刻的位置為橫坐標(biāo)為x（i+1）,縱坐標(biāo)為y（i+1）,面向角為θ（i+1）。感應(yīng)器測得的位移數(shù)據(jù)橫坐標(biāo)為△x（i）,縱坐標(biāo)為△y（i）,面向角為△θ（i），最后的是感應(yīng)器測量時(shí)的誤差。

公式2就是機(jī)器人運(yùn)動過程中通過自身感應(yīng)器通過之前的位置信息和測得的移動信息對當(dāng)前的位置信息進(jìn)行推算的公式，即為運(yùn)動模型公式。

2.2 基于SLAM技術(shù)的地圖構(gòu)建

圖1 基于SLAM技術(shù)的地圖構(gòu)建流程圖

系統(tǒng)能夠?qū)崿F(xiàn)災(zāi)后環(huán)境的地圖構(gòu)建，并且標(biāo)定出被救援目標(biāo)的位置，為救援人員進(jìn)場搜救奠定基礎(chǔ)。是通過在樹莓派嵌入式環(huán)境上移植ROS機(jī)器人操作系統(tǒng)，同時(shí)搭載激光雷達(dá)與攝像頭等多種傳感器配合來實(shí)現(xiàn)的。

這里我們采用的算法核心是擴(kuò)展卡爾曼濾波器（EKF）。它用于結(jié)合信息估計(jì)機(jī)器人準(zhǔn)確位置，選取的特征一般稱作地標(biāo)。EKF在這個(gè)過程中將持續(xù)不斷的估計(jì)周圍環(huán)境中機(jī)器人位置和地標(biāo)位置。

圖2 EKFSLAM算法的MATLAB仿真圖

測距單元的輸出表示的距離是機(jī)器人與最近的障礙物的距離。機(jī)器人自身位置的一個(gè)估計(jì)可以通過對機(jī)器人輪胎運(yùn)行圈數(shù)的估計(jì)得到，由此得到的數(shù)據(jù)可以被看作EKF的初值。在確定完需要提取的特征后，需要從測距單元獲得的信息中準(zhǔn)確的提取出為救援人員進(jìn)入所提供的特征。我們通過在實(shí)驗(yàn)中驗(yàn)證如何從激光雷達(dá)得到的信息中提取有效特征得出結(jié)論。兩種典型的特征提取方法，Spike方法和RANSAC方法會產(chǎn)生適合適合環(huán)境條件的結(jié)果。

SLAM過程的目標(biāo)就是利用環(huán)境信息構(gòu)建環(huán)境地圖，在此基礎(chǔ)上更新機(jī)器人的位置姿態(tài)實(shí)現(xiàn)機(jī)器人定位。對環(huán)境信息進(jìn)行特征提取，與已知地圖進(jìn)行特征匹配，當(dāng)機(jī)器人再次運(yùn)動時(shí)進(jìn)行再次觀測，進(jìn)行特征點(diǎn)更新。這些特征被稱為路標(biāo)，根據(jù)路標(biāo)點(diǎn)可靠地估計(jì)機(jī)器人的位置信息。不斷進(jìn)行循環(huán)迭代，逐步減小誤差，完成傳感器網(wǎng)絡(luò)的組網(wǎng)和地圖構(gòu)建。

3 應(yīng)用前景

本系統(tǒng)通過及時(shí)定位與地圖構(gòu)建（SLAM）來為救援人員進(jìn)入災(zāi)區(qū)制定路線，可以自行規(guī)避路途中遇到的障礙物，進(jìn)而到達(dá)救援現(xiàn)場的指定位置并營救被困人員，極大地縮短了救援時(shí)間。同時(shí)不需要采用交流電，可以應(yīng)付各類復(fù)雜狀況。本系統(tǒng)還具有普適性，對比系統(tǒng)封閉的現(xiàn)階段救援產(chǎn)品，我們可以根據(jù)需求隨時(shí)搭載各種所需要的傳感器進(jìn)行工作。因此，此地圖構(gòu)建系統(tǒng)可以應(yīng)用于各類自然災(zāi)害現(xiàn)場，如火災(zāi)、地震災(zāi)害等現(xiàn)場的探測及救援，同時(shí)可以作為探險(xiǎn)救援隊(duì)的裝備來推廣。