文/陳昌宇 邱廣萍 謝偉彬

清潔勞動是一項重復性很強的工作，隨著人們生活質(zhì)量的改善，這項工作對現(xiàn)代年輕人來說毫無吸引力，但是清潔工作又是必不可少的，所以，一部室外智能清潔機器人的出現(xiàn)是大勢所趨。

本系統(tǒng)研究內(nèi)容有以下幾方面：

（1）地圖構(gòu)建，能夠在一定區(qū)域范圍內(nèi)獲取云點數(shù)據(jù)，最后通過二維柵格建圖形成可用于其他功能的地圖。

（2）自主導航和自動清潔，室外智能清潔機器人可在地圖內(nèi)進行定點導航并清掃。

（3）動態(tài)避障，遇到障礙物時室外智能清潔機器人自動繞過障礙物到達指定地點。

（4）規(guī)避懸崖，室外智能清潔機器人運行在懸崖邊上，機器人的一端檢測到懸崖信號，機器人會立即停止運行并重新規(guī)劃路線，絕不會掉入懸崖或深谷。

1 系統(tǒng)總體方案

本系統(tǒng)選用樹莓派和STM32 單片機為控制器，由電源電路模塊、電機驅(qū)動模塊、激光雷達、攝像頭、IMU 九軸慣性傳感器、編碼器、懸崖檢測模塊等其他外圍設(shè)備組成。完成構(gòu)建地圖、規(guī)避障礙物、自動跟隨、自主導航、自動清掃垃圾等功能。

系統(tǒng)總體框圖如圖1所示。

2 模塊設(shè)計

2.1 地圖構(gòu)建

機器人在環(huán)境中實現(xiàn)無人駕駛的功能需要解決定位與建圖的問題。目前比較成熟的技術(shù)有激光雷達和視覺傳感器。由于激光雷達相對于攝像頭視覺距離測量比較準確，誤差模型較為簡單，在強光直射以外的環(huán)境中能穩(wěn)定運行，點云的處理也比較容易，相比之下更適合用于室外建圖。同時，點云信息本身包含直接的幾何關(guān)系，使得機器人的路徑規(guī)劃和導航變得直觀，激光SLAM（Simultaneous Localization and Mapping 同步定位與建圖）理論研究也相對成熟，對CPU 的計算能力要求遠低于視覺SLAM。所以在室外智能清潔機器人頂部中央安裝了RPLIDAR-A1 激光雷達，由發(fā)射系統(tǒng)、接收系統(tǒng)、信息處理等部分組成。

系統(tǒng)運行時，通過激光雷達、編碼器和9軸姿態(tài)慣性傳感器重復采集得到地圖特征定位自身位置和姿態(tài)，再根據(jù)自身位置增量式地構(gòu)建地圖，從而達到同步定位和地圖構(gòu)建的目的。并使用概率統(tǒng)計的方法，通過多特征匹配來達到定位和減少定位的誤差。

建圖系統(tǒng)框圖如圖2所示。

2.1.1 回環(huán)檢測

激光雷達獲得的點云數(shù)據(jù)會由于傳感器的移動而導致采集的數(shù)據(jù)產(chǎn)生畸變，由此產(chǎn)生累計誤差。為了減小累計誤差，引進回環(huán)檢測。在SLAM中，位姿約束建立在前一幀的基礎(chǔ)上，不可避免產(chǎn)生逐幀誤差?；丨h(huán)檢測就是把歷史所有幀和當前幀做匹配，建立位姿約束的歷史幀，減少約束數(shù)，起到了減小累計誤差的作用?；丨h(huán)檢測對SALM 系統(tǒng)的意義重大，關(guān)系到估計的軌跡和地圖在長時間下的正確性，當然也提供了當前數(shù)據(jù)與所有歷史數(shù)據(jù)的關(guān)聯(lián)。回環(huán)檢測對整個SLAM 系統(tǒng)精度與穩(wěn)健性的提升是非常明顯的。

2.1.2 算法比較

激光SLAM 大致有Gmapping、Hector、Cartographer 三種算法。其中hector 不適合復雜的環(huán)境，如果環(huán)境特別凌亂的話，建圖的效果不是很好，相對來說gmapping 建圖的效果會更好；Cartographer 雖然采用了回環(huán)檢測，魯棒性好，可以消除累計誤差，但是運算量要遠大于Gmapping 和Hector ，所以采用openslam_gampping 在ROS 中二次封裝的軟件包slam_gmapping。真正的核心代碼實現(xiàn)都在openslam_gampping 里面。

圖1：系統(tǒng)總體框圖

圖2：建圖系統(tǒng)框圖

Gmapping 是根據(jù)激光雷達點云數(shù)據(jù)來構(gòu)建二維柵格地圖的Rao-Blackwellized 粒子濾波SLAM 算法。其中所有粒子都相當于一個獨立的地圖，該軟件包采用自適應蒙特卡洛算法在損失較小精度的前提下，減少重采樣的次數(shù)和采樣的粒子數(shù)，提高算法的效率。Hector算法與Gmapping 算法對原圖處理后的對比圖如圖3所示。

2.1.3 建圖

建圖時，設(shè)定坐標系，坐標原點，調(diào)用GridSlamProcessor:init ,初始化粒子數(shù)，設(shè)定子地圖大小。初始化一些重要的參數(shù)以及采樣函數(shù)隨機種子，將SLAM 里的參數(shù)傳遞到openslam 里，用節(jié)點 scan 獲取激光器云點數(shù)據(jù)和運動姿態(tài)，每隔throttle_scans_幀計算一次數(shù)據(jù)。

接著進行掃描匹配，在score 函數(shù)里，計算障礙物坐標phit，再將phit 轉(zhuǎn)換成網(wǎng)格坐標iPhit，計算光束上與障礙物相鄰的非障礙物網(wǎng)格坐標pfree,pfree 由phit 沿激光束方向移動一個網(wǎng)格的距離得到，把pfree 轉(zhuǎn)換成網(wǎng)格坐標ipfree（增量，并不是實際值）在iphit 及其附近8 個柵格，根據(jù)最優(yōu)準則搜索最優(yōu)可能是障礙物的柵格。接著進行了一次權(quán)重計算updateTreeWeights(false)，重采樣之后再進行了一次權(quán)重計算。重采樣得到的粒子集對目標分布的近似越差，則權(quán)重的方差越大。最后利用最優(yōu)粒子對地圖進行更新。

圖3：Hector 算法與Gmapping 算法對原圖處理后的對比

在SLAM 建立地圖模型中，構(gòu)建的地圖就是激光雷達采集的所有路標點的集合，一旦確定了各個路標點的位置，就可以說完成了建圖。

2.2 自主導航

機器人的導航需要先做好自身的可靠定位，系統(tǒng)通過機器人的位姿和觀測得到的特征運用概率定位法與建好的地圖進行特征對比定位；導航應用高效的A*算法，它能從起點到終點探測代價最小路徑的廣度優(yōu)先搜索方式，如果失敗則選擇其它路徑直到得到最優(yōu)路徑。

2.2.1 實現(xiàn)定位

使用ROS 中的amcl 定位包，通過自適應的KLD 法進行粒子濾波更新粒子來定位的，屬于蒙特卡洛定位的升級版本。更新得到的粒子與建好的地圖作對比，賦予粒子一個概率，再根據(jù)這些概率重新生成粒子，概率越高生成的概率越大，如此循環(huán)，不斷迭代，機器人的位置就可以推算出來了。

2.2.2 實現(xiàn)導航

設(shè)定好導航目標。用IMU 九軸慣性傳感器獲得室外智能清潔機器人的加速度、陀螺儀和方向角數(shù)據(jù)，進行姿態(tài)融合解算，得到室外智能清潔機器人的運行姿態(tài)，同時用編碼器檢測室外智能清潔機器人的速度和運行距離。然后把運行姿態(tài)、運行速度以及運行距離反饋給樹莓派上位機，通過 ROS 的中用于讓機器人在制定框架內(nèi)移動到目標位置move_base 包進行融合解算，得到室外智能清潔機器人的運行路徑，再通過樹莓派上位機傳送信號給下位機來控制L298N 電機驅(qū)動，最終實現(xiàn)自主導航功能，到達目標地點。

2.3 動態(tài)避障和規(guī)避懸崖

激光雷達通過飛行時間測距。飛行時間測距指激光器發(fā)射一個激光脈沖，并由計時器記錄下出射的時間，回返光經(jīng)接收器接收，并由計時器記錄下回返的時間。兩個時間相減即得到了光的“飛行時間”，而光速是一定的，因此在已知速度和時間后很容易就可以計算出距離。測量的距離=（（回返的時間-出射的時間）*光速）/2。

在建好的地圖的基礎(chǔ)上，導航系統(tǒng)把機器人位置、傳感器姿態(tài)、障礙物信息和作為SLAM 地圖的結(jié)果而獲得的占用網(wǎng)格地圖調(diào)用到固定地圖，用作占用區(qū)域、自由區(qū)域和未知區(qū)域。使用move_base 提供的ROS 導航功能，設(shè)置四個配置文件base_local_planner_params、costmap_common_params、global_costmap_params、local_costmap_params，分別定義了障礙物的代價、機器人半徑、路徑規(guī)劃時要考慮未來多長的路、移動速度等等。

在自動導航運行時，會通過SLAM 以機器人為中心的部分限定范圍中規(guī)劃路徑時，計算障礙物區(qū)域、預計會和障礙物碰撞的區(qū)域以及機器人可移動區(qū)域，生成躲避障礙物時用的地圖，接著系統(tǒng)自動重新規(guī)劃路徑并繞過障礙物，實現(xiàn)動態(tài)避障的功能。

規(guī)避懸崖是通過超聲波傳感器測距實現(xiàn)的，超聲波測量室外智能清潔機器人的底座到地面距離，設(shè)置一個最大值參數(shù)，若大于該參數(shù)，判斷為存在懸崖，系統(tǒng)重新規(guī)劃路線并繞過該懸崖。

3 數(shù)據(jù)處理

3.1 前端配準

前端配準就是將兩個或兩個以上坐標系中的點云數(shù)據(jù)轉(zhuǎn)換到統(tǒng)一坐標系統(tǒng)中的空間坐標變換。配準操作實際是要找到從坐標系1 到坐標系2 的一個剛性變換。

通過ICP 算法把不同坐標的點云數(shù)據(jù)合并到同一坐標系統(tǒng)中。ICP 算法是為了將點云中的數(shù)據(jù)點進行匹配。ICP 配準過程：計算點云P 中的每一個點云在點云Q 中的對應最近點；解算使對應點對平均距離最小的剛體變換，得到平移參數(shù)和旋轉(zhuǎn)參數(shù)；對P 使用得到的平移參數(shù)和旋轉(zhuǎn)參數(shù)，得到變換后的點云P’；新的變換后，如果點云與參考點集滿足以上目標函數(shù)需求，就是兩個點云的平均距離小于給定的閾值，那么就停止迭代計算，否則新的變換點集P’作為新的P 繼續(xù)進行迭代，最終達到目標函數(shù)的要求。

3.2 后端優(yōu)化

前端配準可以給出一個短時間內(nèi)的軌跡和地圖，由于存在累計誤差，地圖在長時間內(nèi)并不是非常準確的。所以在前端配準的基礎(chǔ)上，加上了后端優(yōu)化，用于長時間的最優(yōu)軌跡和地圖。

粒子濾波算法的核心思想是利用一系列隨機樣本的加權(quán)和近似后驗概率密度函數(shù)，通過求和來近似積分操作。粒子濾波器簡單、易于實現(xiàn)，提供了有效解決非線性動態(tài)系統(tǒng)的方法。粒子濾波根據(jù)狀態(tài)轉(zhuǎn)移方程解算生成大量的采樣，這些采樣就稱之為粒子，通過加權(quán)這些粒子的和逼近后驗概率密度。采樣到達一定數(shù)量，每個粒子計算對應的重要性權(quán)值，代表了預測位姿取第個粒子時獲得觀測的概率。粒子獲得觀測的概率越高，權(quán)重越高。接著根據(jù)權(quán)值的比例重新分布采樣粒子。下一輪濾波中，再將重采樣過后的粒子集輸入到狀態(tài)轉(zhuǎn)移方程中，就能獲得新的預測粒子。最后對每個采樣粒子，通過其采樣的軌跡與觀測計算出相應的地圖估計。

粒子濾波算法可以有效作為SLAM 的問題，但是算法存在著一些問題。其中最主要的問題是需要用大量的樣本數(shù)量才能很好地近似系統(tǒng)的后驗概率密度。機器人構(gòu)建越復雜的環(huán)境時，描述后驗概率分布所需要的樣本數(shù)量就越多，算法的復雜度就越高。因此，該算法的重點是能夠有效地減少樣本數(shù)量的自適應采樣策略。

4 結(jié)語

室外智能清潔機器人通過樹莓派上位機和STM32 下位機為控制核心，加上其他模塊構(gòu)成，實際檢驗表明，該機器人偶爾存在定位不夠精確，導航路線規(guī)劃不夠理想的問題；在戶外環(huán)境可以實現(xiàn)小區(qū)域建立地圖，自主規(guī)劃路線，動態(tài)避障，最終實現(xiàn)清掃垃圾。