王林榮 黃濤

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

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

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

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

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

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

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

2.1? 機(jī)器人平臺

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

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

底層控制板是STM32F1系列，用于控制運(yùn)動(dòng)裝置以及獲取碼盤和陀螺儀信息。上層控制板則是運(yùn)行Linux系統(tǒng)的樹莓派，再執(zhí)行次級系統(tǒng)機(jī)器人操作系統(tǒng)（Robot Operating System，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線的碼盤減速電機(jī)，TB6612FNG芯片作為電機(jī)驅(qū)動(dòng)。系統(tǒng)通過PWM脈沖寬度調(diào)制來控制電機(jī)速度。機(jī)器人獲取方位以及姿態(tài)調(diào)整需要使用到陀螺儀，再聯(lián)合碼盤電機(jī)采用PID算法進(jìn)行閉環(huán)控制，使得機(jī)器人平臺更加穩(wěn)定。電路如圖2所示。

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

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

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

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

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

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

粒子濾波的思想基于蒙特卡洛方法來表示概率，可以用在任何形式的狀態(tài)空間模型上。簡單來說，粒子濾波法是指通過尋找一組在狀態(tài)空間傳播的隨機(jī)樣本，對概率密度函數(shù)進(jìn)行近似，以樣本均值代替積分運(yùn)算（狀態(tài)方程），從而獲得狀態(tài)最小方差分布的過程。因此，粒子濾波能夠比較精確地表達(dá)基于觀測量和控制量的后驗(yàn)概率分布，可以用于解決SLAM問題[3]。粒子濾波算法主要分成5個(gè)步驟：

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

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

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

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

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

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

機(jī)器人的導(dǎo)航系統(tǒng)主要使用ros-navigation導(dǎo)航功能包集合，導(dǎo)總結(jié)來看可以分為數(shù)據(jù)收集層（傳感器數(shù)據(jù)收集）、全局規(guī)劃層（global_planner）、局部規(guī)劃層（local_planner）、行為層（結(jié)合機(jī)器人狀態(tài)和上層指令給出機(jī)器人當(dāng)前行為）、控制器層（與下位機(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)航過程中出問題后的行為。具體的邏輯流程如下：

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

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

3.3? 系統(tǒng)軟件平臺

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

總而言之，Rviz可以幫助使用者實(shí)現(xiàn)所有測量數(shù)據(jù)信息的圖形化顯示，同時(shí)還可以通過按鈕、點(diǎn)擊或修改控制變量的方式，控制定位導(dǎo)航機(jī)器人的行為。

4? ? 結(jié)語

室內(nèi)服務(wù)機(jī)器人將會(huì)是未來一個(gè)很大的發(fā)展方向，在室內(nèi)環(huán)境中GPS無法精準(zhǔn)定位，所以SLAM技術(shù)應(yīng)運(yùn)而生。機(jī)器人可以自身感知室內(nèi)環(huán)境，實(shí)現(xiàn)建圖、定位和導(dǎo)航功能。通過單線激光雷達(dá)獲取室內(nèi)二維平面地圖，完成到目標(biāo)點(diǎn)的路徑規(guī)劃，再完成相應(yīng)的工作。

室內(nèi)建圖主要使用Gmapping和Hector_SLAM算法，通過激光雷達(dá)的返回?cái)?shù)據(jù)構(gòu)建地圖信息，使用蒙特卡洛定位算法實(shí)現(xiàn)機(jī)器人的定位，通過move_base導(dǎo)航包，采用A*算法尋找到最優(yōu)路徑。未來工作可以在此機(jī)器人上加上機(jī)械臂，以實(shí)現(xiàn)相關(guān)的功能操作。