搭載激光雷達的智能小車系統開發

熊云龍，李志揚

（華中師范大學 物理科學與技術學院，湖北 武漢430079）

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

開發了一款基于智能小車的輕便型實驗室智能管家系統，該系統搭載激光雷達進行環境測量，以便實現準確避障和路徑規劃。

本文重點介紹了智能小車的硬件設計和激光雷達測距數據的分析處理，特別是在噪聲較大的情況下，如何通過粒子群算法實現激光雷達測距數據的準確匹配和拼接。

1 智能小車系統硬件設計

本文開發的智能小車硬件系統構架如圖1所示，其中車載激光雷達傳感器主要對小車周圍環境信息進行掃描探測，STM32控制器對激光雷達傳感器所感知的數據信息進行處理，以提取道路環境、障礙物等信息，同時拼接各處掃描數據形成整體環境地圖，并定位小車當前位置，進行合適的路徑規劃，最后驅動直流電機控制小車執行前進、后退、轉彎等動作，確保小車在整個實驗室環境內暢通無阻地四處巡查，執行相應的高端監測等任務，為智能管家系統提供一個穩定的運動平臺。

圖1 系統架構示意圖

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

圖2 搭載激光雷達的智能小車實物圖

2 基于粒子群算法的激光雷達數據處理

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

圖3 激光雷達在A和B兩個不同位置掃描到的點云數據

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

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

導入激光雷達在位置A和位置B掃描到的點云數據，并將其從極坐標轉化為直角坐標，得到其中M和N分別為A、B兩組數據中的點云個數。

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

計算轉換后的點（xb′n，yb′n）與A組數據中每個點之間的距離：

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

終止條件判斷。不斷重復迭代搜索求解進行迭代，直到達到預先設定的迭代次數，或者全局最佳匹配效果CGbest停止增加達到一定次數，則迭代結束。此時全局最佳位置PGbest所代表的全局最佳平移、旋轉量即為最終搜索結果。

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

3 結語

綜上所述，本文開發了一款搭載激光雷達的智能小車系統，并著重討論了基于粒子群算法的激光雷達測距數據處理。實驗表明該算法可以有效地對存在大量噪聲的測距數據進行準確處理，通過多次迭代找到全局最優解，避免錯誤的局部極小解，保證測距數據正確地匹配和拼接，從而實現智能小車的定位和實驗室整體環境重建，為智能管家系統提供一個安全可靠的運動平臺。

圖4 經過粒子群算法合成后的實驗室大致輪廓