文/陳昌宇 邱廣萍 謝偉彬

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

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

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

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

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

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

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

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

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

2 模塊設(shè)計

2.1 地圖構(gòu)建

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

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

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

2.1.1 回環(huán)檢測

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

2.1.2 算法比較

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

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

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

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

2.1.3 建圖

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

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

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

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

2.2 自主導(dǎo)航

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

2.2.1 實(shí)現(xiàn)定位

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

2.2.2 實(shí)現(xiàn)導(dǎo)航

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

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

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

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

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

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

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

3.1 前端配準(zhǔn)

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

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

3.2 后端優(yōu)化

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

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

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

4 結(jié)語

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