基于ROS的激光SLAM室內(nèi)建圖定位導(dǎo)航智能機(jī)器人設(shè)計(jì)

作者：王林榮黃濤

來(lái)源：《無(wú)線互聯(lián)科技》第04期

摘;;要：文章基于Linux系統(tǒng)上的ROS平臺(tái)實(shí)現(xiàn)基本的SLAM算法，機(jī)器人能夠?qū)崿F(xiàn)構(gòu)建室內(nèi)地圖、室內(nèi)自動(dòng)避障導(dǎo)航及室內(nèi)定位等功效。底層重要以STM32和樹(shù)莓派為核心，通過(guò)陀螺儀和碼盤(pán)電機(jī)得到機(jī)器人方位和行走里程，使用激光雷達(dá)獲取室內(nèi)地圖信息。在ROS上執(zhí)行對(duì)應(yīng)腳本，調(diào)用Gmapping，ACML算法實(shí)現(xiàn)室內(nèi)地圖構(gòu)建和定位。

核心詞：即時(shí)定位與地圖構(gòu)建;機(jī)器人操作系統(tǒng);建圖;導(dǎo)航;定位

1;;機(jī)器人研究現(xiàn)狀及發(fā)展方向

近年來(lái)，隨著計(jì)算機(jī)視覺(jué)和人工智能等高新技術(shù)的逐步成熟，其被越來(lái)越廣泛地應(yīng)用于人們生活中的每個(gè)角落。移動(dòng)機(jī)器人在各個(gè)領(lǐng)域都有著很大的市場(chǎng)需求，使得有關(guān)技術(shù)有著更進(jìn)一步地研究和應(yīng)用。在國(guó)防領(lǐng)域里，無(wú)人戰(zhàn)機(jī)、無(wú)人執(zhí)勤車(chē)等被用于情報(bào)收集和地形勘探，減少戰(zhàn)爭(zhēng)風(fēng)險(xiǎn)。在家庭和商業(yè)中心，掃地機(jī)器人、服務(wù)機(jī)器人和安保機(jī)器人給人們帶來(lái)便捷服務(wù)的同時(shí)還能夠減少人力成本。在物流領(lǐng)域里，搬運(yùn)小車(chē)更是成為當(dāng)中不可或缺的一部分，重要應(yīng)用于物流的分揀和搬運(yùn)以及機(jī)場(chǎng)港口的行李貨柜搬運(yùn)。其中，最被人熟知的就是在亞馬遜物流中心工作的Kiva機(jī)器人[1]?，F(xiàn)在有超出15000臺(tái)被派駐到10個(gè)物流中心進(jìn)行工作。Kiva系統(tǒng)將物流中心的儲(chǔ)存能力相較傳統(tǒng)倉(cāng)庫(kù)提高了50%以上[2]。

即時(shí)定位與地圖構(gòu)建（SimultaneousLocalizationandMapping，SLAM）技術(shù)能夠很精確地實(shí)現(xiàn)環(huán)境的地圖構(gòu)建，定位以及多點(diǎn)導(dǎo)航?，F(xiàn)在SLAM技術(shù)能夠分為激光SLAM和視覺(jué)SLAM，激光SLAM采用的傳感器為激光雷達(dá)，而視覺(jué)激光則采用深度攝像頭。激光SLAM技術(shù)較為成熟、誤差少，且足以滿足現(xiàn)在環(huán)境的使用，因此本項(xiàng)目采用了此項(xiàng)技術(shù)。

當(dāng)機(jī)器人有了SLAM的功效，就相稱(chēng)于機(jī)器人有了一雙眼睛。其能夠掃描未知環(huán)境從而獲取地圖，以及實(shí)時(shí)獲取自己的現(xiàn)在位置;能夠自行途徑規(guī)劃達(dá)成指定地圖上的地點(diǎn)。如智能掃地機(jī)就能夠通過(guò)SLAM實(shí)現(xiàn)精確的地面避障遍歷，又自行回到充電地點(diǎn)。在無(wú)人駕駛以及無(wú)人機(jī)上亦同樣合用，其需要實(shí)現(xiàn)一種共同的目的：避障、定位、途徑規(guī)劃。為了讓機(jī)器人更加智能化，可在實(shí)現(xiàn)此項(xiàng)技術(shù)上再加上手機(jī)APP的控制和語(yǔ)音識(shí)別控制。

2;;硬件系統(tǒng)設(shè)計(jì)

2.1;機(jī)器人平臺(tái)

選擇機(jī)器人平臺(tái)需要考慮的核心因素是穩(wěn)定性，減少機(jī)械構(gòu)造問(wèn)題帶來(lái)的數(shù)據(jù)誤差，微小的誤差在定位和導(dǎo)航上都會(huì)造成錯(cuò)誤。同時(shí)，亦需要考慮定位性能，這是機(jī)器人能夠根據(jù)本身運(yùn)動(dòng)進(jìn)行位置估算的重要原則。定位導(dǎo)航算法需要獲取機(jī)器人的里程計(jì)信息，因此在控制平臺(tái)上必須含有統(tǒng)計(jì)行駛路程以及方位的功效。機(jī)器人動(dòng)力系統(tǒng)使用兩個(gè)390線的碼盤(pán)電機(jī)，使用碼盤(pán)作為機(jī)器人的里程計(jì)，通過(guò)兩輪差速實(shí)現(xiàn)轉(zhuǎn)向，并使用九軸陀螺儀控制方位。

在定位導(dǎo)航上的測(cè)量單元重要有激光測(cè)量、圖像測(cè)量。其中激光測(cè)距單元比較精確、高效，并且其輸出不需要太多的解決。激光SLAM在構(gòu)建地圖的時(shí)候精度較高，思嵐科技的RPLIDAR系列構(gòu)建的地圖精度可達(dá)成2cm左右，而半徑可達(dá)成12m，足以在室內(nèi)環(huán)境下使用。

底層控制板是STM32F1系列，用于控制運(yùn)動(dòng)裝置以及獲取碼盤(pán)和陀螺儀信息。上層控制板則是運(yùn)行Linux系統(tǒng)的樹(shù)莓派，再執(zhí)行次級(jí)系統(tǒng)機(jī)器人操作系統(tǒng)（RobotOperatingSystem，ROS），激光雷達(dá)掃描室內(nèi)環(huán)境返回?cái)?shù)據(jù)信息傳輸?shù)絇C端進(jìn)行建圖、定位和導(dǎo)航。機(jī)器人實(shí)物如圖1所示。

2.2;硬件電路設(shè)計(jì)

運(yùn)動(dòng)裝置采用390線的碼盤(pán)減速電機(jī)，TB6612FNG芯片作為電機(jī)驅(qū)動(dòng)。系統(tǒng)通過(guò)PWM脈沖寬度調(diào)制來(lái)控制電機(jī)速度。機(jī)器人獲取方位以及姿態(tài)調(diào)節(jié)需要使用到陀螺儀，再聯(lián)合碼盤(pán)電機(jī)采用PID算法進(jìn)行閉環(huán)控制，使得機(jī)器人平臺(tái)更加穩(wěn)定。電路如圖2所示。

3;;軟件系統(tǒng)設(shè)計(jì)

3.1;地圖構(gòu)建算法Gmapping

Gmapping由GiorgioGrisetti和CyrillStachniss等在以Fast-SLAM方案為基本原理提出，是一種基于Rao-Blackwellized粒子濾波的二維激光SLAM辦法，現(xiàn)在在機(jī)器人定位導(dǎo)航方面應(yīng)用廣泛[3]。

使用Gmapping算法進(jìn)行機(jī)器人定位導(dǎo)航，在掃描到的環(huán)境中用每一種粒子代表機(jī)器人可能出現(xiàn)的位置。獲取精確位置，必須預(yù)計(jì)機(jī)器人的位置和姿態(tài)，通過(guò)不停地運(yùn)動(dòng)和測(cè)量，獲取其所處環(huán)境中的地圖信息，逐步減少本身位置的不擬定性，最后獲得精確的位置信息。在SLAM中，姿態(tài)和地圖都是狀態(tài)變量，因此一種粒子需要同時(shí)保存開(kāi)始到現(xiàn)在的全部位姿和地圖信息。這相稱(chēng)于將SLAM中分為定位和建圖兩部分，減少了算法的計(jì)算量和復(fù)雜度，可簡(jiǎn)樸描述為機(jī)器人用上一刻的定位和地圖預(yù)測(cè)現(xiàn)在時(shí)刻的位置信息，然后計(jì)算權(quán)重、重采樣，接著更新粒子的姿態(tài)和地圖信息。

激光雷達(dá)的測(cè)量信息可覺(jué)得Gmapping算法提供一種相對(duì)集中的粒子分布，把粒子采樣范疇更改到激光雷達(dá)測(cè)量的區(qū)域，使得后來(lái)的粒子分布更靠近真實(shí)分布。而碼盤(pán)電機(jī)和陀螺儀能夠提供機(jī)器人的位姿信息，結(jié)合內(nèi)容隨機(jī)采樣多個(gè)點(diǎn)，再根據(jù)這些點(diǎn)的里程計(jì)和觀察模型計(jì)算均值和方差。其計(jì)算過(guò)程如圖3所示。

Gmapping算法只適合室內(nèi)環(huán)境，場(chǎng)景越大，需要的粒子越多，會(huì)造成建圖失敗。在機(jī)器人起始位置應(yīng)當(dāng)選擇地圖特性豐富的地方，避免大片的空地，從而更容易提高對(duì)的粒子的權(quán)重。建圖系統(tǒng)框架如圖4所示。

粒子濾波的思想基于蒙特卡洛辦法來(lái)表達(dá)概率，能夠用在任何形式的狀態(tài)空間模型上。簡(jiǎn)樸來(lái)說(shuō)，粒子濾波法是指通過(guò)尋找一組在狀態(tài)空間傳輸?shù)碾S機(jī)樣本，對(duì)概率密度函數(shù)進(jìn)行近似，以樣本均值替代積分運(yùn)算（狀態(tài)方程），從而獲得狀態(tài)最小方差分布的過(guò)程。因此，粒子濾波能夠比較精確地體現(xiàn)基于觀察量和控制量的后驗(yàn)概率分布，能夠用于解決SLAM問(wèn)題[3]。粒子濾波算法重要分成5個(gè)環(huán)節(jié)：

（1）預(yù)測(cè)階段，粒子濾波首先根據(jù)狀態(tài)轉(zhuǎn)移函數(shù)預(yù)測(cè)生成大量的采樣，這些采樣就被稱(chēng)之為粒子，運(yùn)用這些粒子的加權(quán)和來(lái)逼近后驗(yàn)概率密度。

（2）校正階段，隨著觀察值的依次達(dá)成，為每個(gè)粒子計(jì)算對(duì)應(yīng)的重要性權(quán)值。該權(quán)值代表了預(yù)測(cè)的位姿取第N個(gè)粒子時(shí)獲得觀察的概率。因此，對(duì)全部粒子都進(jìn)行評(píng)價(jià)，越有可能獲得觀察的粒子，獲得的權(quán)重越高。

（3）重采樣階段，根據(jù)權(quán)值的比例重新分布采樣粒子。由于近似逼近持續(xù)分布的粒子數(shù)量有限，因此這個(gè)環(huán)節(jié)非常重要。下一輪濾波中，再將重采樣過(guò)后的粒子集輸入到狀態(tài)轉(zhuǎn)移方程中，就能夠獲得新的預(yù)測(cè)粒子了。

（4）濾波階段，將重采樣后的粒子放進(jìn)狀態(tài)轉(zhuǎn)移方程通過(guò)計(jì)算獲取新的預(yù)測(cè)粒子位姿和地圖信息，然后再次進(jìn)行預(yù)測(cè)、校正和重采樣過(guò)程，通過(guò)不停地循環(huán)，使得粒子的范疇值處在靠近真實(shí)值的區(qū)域內(nèi)，從而計(jì)算出機(jī)器人的具體定位。

（5）地圖預(yù)計(jì)，對(duì)于每個(gè)采樣的粒子，通過(guò)其采樣的軌跡與觀察計(jì)算出對(duì)應(yīng)的地圖預(yù)計(jì)，選用最靠近真實(shí)值的粒子，即可生成規(guī)劃區(qū)域的柵格地圖。

3.2;自主避障導(dǎo)航和多點(diǎn)導(dǎo)航

機(jī)器人的導(dǎo)航系統(tǒng)重要使用ros-navigation導(dǎo)航功效包集合，導(dǎo)總結(jié)來(lái)看能夠分為數(shù)據(jù)收集層（傳感器數(shù)據(jù)收集）、全局規(guī)劃層（global_planner）、局部規(guī)劃層（local_planner）、行為層（結(jié)合機(jī)器人狀態(tài)和上層指令給出機(jī)器人現(xiàn)在行為）、控制器層（與下位機(jī)通信）[4]。

機(jī)器人使用navigation棧導(dǎo)航時(shí)，move_base模塊負(fù)責(zé)整個(gè)navigation行為的調(diào)度，涉及初始化costmap與planner，監(jiān)視導(dǎo)航狀態(tài)適時(shí)更換導(dǎo)航方略等監(jiān)視導(dǎo)航狀態(tài)、適時(shí)更換導(dǎo)航方略等。涉及行為的控制，move_base具體實(shí)現(xiàn)就是有限狀態(tài)機(jī)，定義了若干recovery_behavior，指定機(jī)器人導(dǎo)航過(guò)程中出問(wèn)題后的行為。具體的邏輯流程以下：

move_base首先啟動(dòng)了global_planner和local_planner兩個(gè)規(guī)劃器，負(fù)責(zé)全局途徑規(guī)劃和局部途徑規(guī)劃，通過(guò)costmap組件生成自己的代價(jià)地圖（global_costmap和local_costmap）。通過(guò)全局途徑規(guī)劃，計(jì)算出機(jī)器人到目的位置的全局路線，實(shí)現(xiàn)的辦法是基于柵格地圖的cost搜索找最優(yōu)。然后通過(guò)局部規(guī)劃，負(fù)責(zé)做局部避障的規(guī)劃，具體navigation中實(shí)現(xiàn)的算法是DynamicWindowsApproach，具體流程是：由移動(dòng)底盤(pán)的運(yùn)動(dòng)學(xué)模型得到速度的采樣空間;在采樣空間中，計(jì)算每個(gè)樣本的目的函數(shù);得到盼望速度，插值成軌跡輸出，如圖5所示。

在機(jī)器人運(yùn)行過(guò)程中，根據(jù)機(jī)器人的狀態(tài)做出規(guī)劃（喚醒規(guī)劃器），操作（計(jì)算正當(dāng)速度并公布），清理（recovery_behavior）等操作。

3.3;系統(tǒng)軟件平臺(tái)

ROS中提供了一個(gè)3D可視化工具Rviz，能夠在顯示面板區(qū)域選擇需要顯示的選項(xiàng)，還涉及了一種3D可視化區(qū)域，用于顯示所加載的全部項(xiàng)[5]。在此界面上，能夠顯示構(gòu)建的地圖和已規(guī)劃的途徑，還能夠獲得激光測(cè)距儀、里程計(jì)以及仿真機(jī)器人等多個(gè)信息。顧客可從不同角度對(duì)機(jī)器人的運(yùn)動(dòng)狀態(tài)進(jìn)行觀察，可直觀地獲取機(jī)器人現(xiàn)在的位姿和創(chuàng)立的地圖等，如圖6所示。

總而言之，Rviz能夠協(xié)助使用者實(shí)現(xiàn)全部測(cè)量數(shù)據(jù)信息的圖形化顯示，同時(shí)還能夠通過(guò)按鈕、點(diǎn)擊或修改控制變量的方式，控制訂位導(dǎo)航機(jī)器人的行為。

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

室內(nèi)服務(wù)機(jī)器人將會(huì)是將來(lái)一種很大的發(fā)展方向，在室內(nèi)環(huán)境中GPS無(wú)法精擬定