搭載激光雷達(dá)的智能小車系統(tǒng)開發(fā)

熊云龍，李志揚(yáng)

（華中師范大學(xué) 物理科學(xué)與技術(shù)學(xué)院，湖北 武漢430079）

近年來，隨著無人駕駛汽車[1]、文字識(shí)別[2]、人臉識(shí)別[3]、語音識(shí)別[4]和指紋識(shí)別[5]等技術(shù)逐漸進(jìn)入實(shí)用階段，人工智能進(jìn)入了一個(gè)蓬勃發(fā)展期，形成了一股“人工智能+”[6]的大潮流，并且正在向生活中的各個(gè)領(lǐng)域快速滲透。基于人工智能技術(shù)的智能管家機(jī)器[7]逐漸成為人們的研究熱點(diǎn)，但是可以進(jìn)入普通家庭的人形機(jī)器人管家還需要一個(gè)漫長的研究過程[8]。

開發(fā)了一款基于智能小車的輕便型實(shí)驗(yàn)室智能管家系統(tǒng)，該系統(tǒng)搭載激光雷達(dá)進(jìn)行環(huán)境測量，以便實(shí)現(xiàn)準(zhǔn)確避障和路徑規(guī)劃。

本文重點(diǎn)介紹了智能小車的硬件設(shè)計(jì)和激光雷達(dá)測距數(shù)據(jù)的分析處理，特別是在噪聲較大的情況下，如何通過粒子群算法實(shí)現(xiàn)激光雷達(dá)測距數(shù)據(jù)的準(zhǔn)確匹配和拼接。

1 智能小車系統(tǒng)硬件設(shè)計(jì)

本文開發(fā)的智能小車硬件系統(tǒng)構(gòu)架如圖1所示，其中車載激光雷達(dá)傳感器主要對(duì)小車周圍環(huán)境信息進(jìn)行掃描探測，STM32控制器對(duì)激光雷達(dá)傳感器所感知的數(shù)據(jù)信息進(jìn)行處理，以提取道路環(huán)境、障礙物等信息，同時(shí)拼接各處掃描數(shù)據(jù)形成整體環(huán)境地圖，并定位小車當(dāng)前位置，進(jìn)行合適的路徑規(guī)劃，最后驅(qū)動(dòng)直流電機(jī)控制小車執(zhí)行前進(jìn)、后退、轉(zhuǎn)彎等動(dòng)作，確保小車在整個(gè)實(shí)驗(yàn)室環(huán)境內(nèi)暢通無阻地四處巡查，執(zhí)行相應(yīng)的高端監(jiān)測等任務(wù)，為智能管家系統(tǒng)提供一個(gè)穩(wěn)定的運(yùn)動(dòng)平臺(tái)。

圖1 系統(tǒng)架構(gòu)示意圖

本文開發(fā)的智能小車實(shí)物如圖2所示。智能小車采用六輪底盤，其中電池（容量2200mAh）和STM32控制主板安裝在底盤夾板內(nèi)。為了避免動(dòng)作沖突，同時(shí)簡化驅(qū)動(dòng)控制，小車每側(cè)三個(gè)輪子的直流電機(jī)分別并聯(lián)在一起，在同一脈寬調(diào)制（PWM）指令下同步運(yùn)轉(zhuǎn)。當(dāng)左右兩側(cè)小輪轉(zhuǎn)速相同時(shí)，小車直線前行，當(dāng)小車某一側(cè)的小輪轉(zhuǎn)速超過另一側(cè)時(shí)，小車左轉(zhuǎn)或右轉(zhuǎn)。車載激光雷達(dá)采用由SLAMTEC公司開發(fā)的二維激光雷達(dá)RPLIDARA1[9]，如圖2中箭頭所指。該雷達(dá)采用了激光三角測距技術(shù)，配合SLAMTEC研發(fā)的高速的視覺采集處理機(jī)構(gòu)，可對(duì)12m半徑范圍內(nèi)的物體展開360°全方位距離測量，掃描測距速度每秒高達(dá)4000次[10]。STM32微控制器與激光雷達(dá)按照事先規(guī)定的16進(jìn)制數(shù)據(jù)幀格式進(jìn)行串行通信，實(shí)時(shí)傳輸距離探測數(shù)據(jù)以及激光雷達(dá)轉(zhuǎn)速、分辨率、健康等狀態(tài)信息。

圖2 搭載激光雷達(dá)的智能小車實(shí)物圖

2 基于粒子群算法的激光雷達(dá)數(shù)據(jù)處理

隨著智能小車運(yùn)動(dòng)，車載激光雷達(dá)在不同位置掃描到的點(diǎn)云數(shù)據(jù)不同，智能小車位于A和B兩個(gè)不同位置時(shí)車載激光雷達(dá)的輸出數(shù)據(jù)，如圖3所示。從圖3可以看出，由于智能小車旋轉(zhuǎn)、平移，導(dǎo)致室內(nèi)環(huán)境輪廓也發(fā)生了旋轉(zhuǎn)，同時(shí)2幅圖中的輪廓并非完全相同，由于噪聲和遮擋等因素存在很大差異。如何對(duì)此類存在嚴(yán)重噪聲的雷達(dá)數(shù)據(jù)進(jìn)行匹配拼接從而實(shí)現(xiàn)小車定位與實(shí)驗(yàn)室完整環(huán)境重建，是一個(gè)較大的挑戰(zhàn)。

圖3 激光雷達(dá)在A和B兩個(gè)不同位置掃描到的點(diǎn)云數(shù)據(jù)

經(jīng)過反復(fù)實(shí)驗(yàn)，摸索出一套基于粒子群的匹配算法。激光雷達(dá)輸出的數(shù)據(jù)以小車當(dāng)前位置為坐標(biāo)原點(diǎn)，如果小車位置B相對(duì)位置A有一個(gè)平移量和旋轉(zhuǎn)量，則在位置B掃描測量的點(diǎn)云數(shù)據(jù)必須經(jīng)過相同平移量和旋轉(zhuǎn)坐標(biāo)變換后才能與位置A掃描測量的點(diǎn)云數(shù)據(jù)重合。但激光雷達(dá)輸出的數(shù)據(jù)并不包含自身位置，因此需要求解使A和B兩點(diǎn)所得點(diǎn)云數(shù)據(jù)相互重疊的最佳平移量和旋轉(zhuǎn)量。為了避免陷入局部極小，本文采用了粒子群搜索算法。其核心思想是：在較大范圍內(nèi)隨機(jī)初始化設(shè)置大量粒子，每個(gè)粒子代表一個(gè)平移量和旋轉(zhuǎn)量。然后讓每個(gè)粒子隨機(jī)游走，并計(jì)算評(píng)估B點(diǎn)點(diǎn)云經(jīng)過該粒子所代表的平移和旋轉(zhuǎn)變換后與A點(diǎn)數(shù)據(jù)的匹配程度。在迭代過程中，每個(gè)粒子都會(huì)向自己的歷史最佳值和所有粒子的歷史最佳值靠攏，這樣總會(huì)有粒子找到正確的全局最小值，具體步驟如下。

估算平移量和旋轉(zhuǎn)量的最大變化范圍，并初始化K個(gè)粒子，使得它們的初始位置Pi（xi，yi，θi）和初始速度Vi，i=1，2，…，K，在此范圍內(nèi)隨機(jī)分布，其中xi、yi和θi分別代表位置B相對(duì)位置A的平移量和旋轉(zhuǎn)量。

導(dǎo)入激光雷達(dá)在位置A和位置B掃描到的點(diǎn)云數(shù)據(jù)，并將其從極坐標(biāo)轉(zhuǎn)化為直角坐標(biāo)，得到其中M和N分別為A、B兩組數(shù)據(jù)中的點(diǎn)云個(gè)數(shù)。

計(jì)算每個(gè)粒子i的初始匹配效果。將B組中的每個(gè)數(shù)據(jù)點(diǎn)n的坐標(biāo)（xbn，ybn）用粒子i所代表的平移量xi，yi和旋轉(zhuǎn)量θi分別轉(zhuǎn)換到A位置坐標(biāo)，得到（xb′n，yb′n），變換關(guān)系如下：

計(jì)算轉(zhuǎn)換后的點(diǎn)（xb′n，yb′n）與A組數(shù)據(jù)中每個(gè)點(diǎn)之間的距離：

如果dnm的最小值小于A組數(shù)據(jù)中相鄰兩點(diǎn)之間的距離d=rmα，其中rm為A中第m個(gè)點(diǎn)離激光雷達(dá)的距離，α為相鄰兩次測量之間的夾角，則認(rèn)為點(diǎn)（xb′n-yb′n）與A組數(shù)據(jù)匹配成功。遍歷B組中所有點(diǎn)云，統(tǒng)計(jì)匹配成功的點(diǎn)的數(shù)目Cibest，將其定義為第i個(gè)粒子的初始匹配效果。進(jìn)一步將所有粒子的最大匹配值定義為全局最佳匹配效果。而Cibest所對(duì)應(yīng)的位置定義為第i個(gè)粒子的最佳位置Pibest，Cibest所對(duì)應(yīng)的位置定義為全局最佳位置Pibest。

終止條件判斷。不斷重復(fù)迭代搜索求解進(jìn)行迭代，直到達(dá)到預(yù)先設(shè)定的迭代次數(shù)，或者全局最佳匹配效果CGbest停止增加達(dá)到一定次數(shù)，則迭代結(jié)束。此時(shí)全局最佳位置PGbest所代表的全局最佳平移、旋轉(zhuǎn)量即為最終搜索結(jié)果。

以圖3實(shí)驗(yàn)數(shù)據(jù)為例，最終匹配結(jié)果如圖4所示，最佳平移和旋轉(zhuǎn)為XGbest=538.81mm，YGbest=-544.54mm，θGbest=30.49°。從圖4可以看出，兩組數(shù)據(jù)中的部分點(diǎn)云并沒有完全重合，這可能是由遮擋和噪聲等因素引起的。在對(duì)多個(gè)位置的激光雷達(dá)掃描測量數(shù)據(jù)進(jìn)行拼接時(shí)，把所有只出現(xiàn)1次的點(diǎn)云都丟棄，只有在2個(gè)以上位置都探測到的數(shù)據(jù)才納入實(shí)驗(yàn)室的整體輪廓。

3 結(jié)語

綜上所述，本文開發(fā)了一款搭載激光雷達(dá)的智能小車系統(tǒng)，并著重討論了基于粒子群算法的激光雷達(dá)測距數(shù)據(jù)處理。實(shí)驗(yàn)表明該算法可以有效地對(duì)存在大量噪聲的測距數(shù)據(jù)進(jìn)行準(zhǔn)確處理，通過多次迭代找到全局最優(yōu)解，避免錯(cuò)誤的局部極小解，保證測距數(shù)據(jù)正確地匹配和拼接，從而實(shí)現(xiàn)智能小車的定位和實(shí)驗(yàn)室整體環(huán)境重建，為智能管家系統(tǒng)提供一個(gè)安全可靠的運(yùn)動(dòng)平臺(tái)。

圖4 經(jīng)過粒子群算法合成后的實(shí)驗(yàn)室大致輪廓