基于ROS的激光SLAM室內建圖定位導航智能機器人設計

王林榮 黃濤

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

關鍵詞：即時定位與地圖構建;機器人操作系統(tǒng);建圖;導航;定位

1? ? 機器人研究現狀及發(fā)展方向

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

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

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

2? ? 硬件系統(tǒng)設計

2.1? 機器人平臺

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

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

底層控制板是STM32F1系列，用于控制運動裝置以及獲取碼盤和陀螺儀信息。上層控制板則是運行Linux系統(tǒng)的樹莓派，再執(zhí)行次級系統(tǒng)機器人操作系統(tǒng)（Robot Operating System，ROS），激光雷達掃描室內環(huán)境返回數據信息傳輸到PC端進行建圖、定位和導航。機器人實物如圖1所示。

2.2? 硬件電路設計

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

3? ? 軟件系統(tǒng)設計

3.1? 地圖構建算法Gmapping

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

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

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

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

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

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

（2）校正階段，隨著觀測值的依次到達，為每個粒子計算相應的重要性權值。該權值代表了預測的位姿取第N個粒子時獲得觀測的概率。因此，對所有粒子都進行評價，越有可能獲得觀測的粒子，獲得的權重越高。

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

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

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

3.2? 自主避障導航和多點導航

機器人的導航系統(tǒng)主要使用ros-navigation導航功能包集合，導總結來看可以分為數據收集層（傳感器數據收集）、全局規(guī)劃層（global_planner）、局部規(guī)劃層（local_planner）、行為層（結合機器人狀態(tài)和上層指令給出機器人當前行為）、控制器層（與下位機通信）[4]。

機器人使用navigation棧導航時，move_base模塊負責整個navigation行為的調度，包括初始化costmap與planner，監(jiān)視導航狀態(tài)適時更換導航策略等監(jiān)視導航狀態(tài)、適時更換導航策略等。涉及行為的控制，move_base具體實現就是有限狀態(tài)機，定義了若干recovery_behavior，指定機器人導航過程中出問題后的行為。具體的邏輯流程如下：

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

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

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

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

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

4? ? 結語

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

室內建圖主要使用Gmapping和Hector_SLAM算法，通過激光雷達的返回數據構建地圖信息，使用蒙特卡洛定位算法實現機器人的定位，通過move_base導航包，采用A*算法尋找到最優(yōu)路徑。未來工作可以在此機器人上加上機械臂，以實現相關的功能操作。