基于三維正態分布變換改進算法的移動機器人實時定位

摘要：針對點云配準三維正態分布變換（3D-NDT）在未確定初始位姿情況下配準精度較低、配準時間較長，無法滿足移動機器人實時定位要求，提出1種改進的3D-NDT點云配準融合算法。在原始點云的降采樣過程中，使用源點云中的點替代計算得到的重心，降低算法運算量并保留點云的特征信息；通過引入信賴半徑動態調節迭代步長，提高降采樣后的精度和點云配準速度；通過融合三維激光點云數據與9軸慣性測量單元（IMU）數據，解決2組點云數據位姿差異過大無法收斂或進入局部極值的問題。采用實驗室自搭建的移動機器人平臺對改進的3D-NDT算法進行仿真實驗，驗證改進算法實時定位的可靠性和準確度。結果表明：與傳統3D-NDT算法相比，改進3D-NDT算法在室外和室內環境下的匹配精度分別提升106%，108%，匹配成功率分別提升8.29%，6.35%，平均匹配耗時分別降低51.1%，47.9%，移動機器人實時定位的配準精度得到較大提升，單次配準

時間也大幅降低，改進的3D-NDT算法可滿足移動機器人實時定位的需求。

關鍵詞：正態分布變換；點云數據；點云配準；慣性測量單元（IMU）；數據融合；動態信賴半徑；自主定位；移動機器人

中圖分類號：TP 242文獻標志碼：A doi：10.12415/j.issn.1671?7872.24066

Real-time Positioning of Mobile Robots Based onImproved 3D Normal Distributions Transform Algorithm

ZHAO Weidong，LYUHongbing，LIULilei，ZHOUDachang

（School of Electricalamp;InformationEngineering，Anhui University of Technology，Maanshan 243032，China）

Abstract：To address the issues of low registration accuracy and long registration time in 3D normal distributions transform（3D-NDT）point cloud registration when initial poses are not accurately known，which fail to meet the real-time localization requirements of mobile robots，an improved 3D-NDT point cloud registration fusion algorithm was proposed.During the downsampling process of the raw point cloud，points from the source point cloud were used to replace the calculated centroids，thereby reducing computational complexity while preserving the feature information of the point cloud.By introducing a trust radius to dynamically adjust the iteration step size，the accuracy after downsampling and the speed of point cloud registration could be improved.Additionally，by integrating 3D laser point cloud data with 9-axis inertial measurement unit（IMU）data，problems such as excessive pose differences between two sets of point cloud data leading to non-convergence or falling into local minima were resolved.The improved 3D-NDT algorithm was subjected to simulation experiments using a self-built mobile robot platform in thelaboratory to verify the reliability and accuracy of the real-time localization of the enhanced algorithm.The results show that compared with the traditional 3D-NDT algorithm，the improved 3D-NDT algorithm achieves a matching accuracy improvement of 106%outdoors and 108%indoors，with success rates increasing by 8.29%and 6.35%，respectively.Average matching times were reduced by 51.1%and 47.9%，respectively.This significant enhancement in registration accuracy and substantial reduction in single-registration time for mobile robot real-time localization indicate that the improved 3D-NDT algorithm can meet the demands of real-time positioning for mobile robots.

Keywords：normal distribution stransform;point cloud data;point cloud registration;inertial measurement unit（IMU）;data fusion;dynamic trust radius;autonomouslocalization;mobile robots

近年，隨著機器人和無人駕駛技術的飛速發展，產生了諸如清潔機器人、巡檢無人車、包裹配送無人機等眾多移動機器人的應用場景。自主定位是移動機器人建圖與導航的基礎，關鍵在于能夠使機器人在復雜環境中確定自身在已知地圖中的具體位置[1]。車載激光雷達可為移動機器人建圖和定位提供硬件基礎。對于較為簡單的室內環境，二維激光雷達可滿足移動機器人的自主定位需求。對于較為復雜的室內和室外環境，二維激光雷達無法滿足要求，需采用三維激光雷達實現移動機器人的自主定位[2]。但是，三維激光雷達掃描的點云數量巨大，而移動機器人處理器的算力有限，對點云數據與現實地圖進行點云配準定位耗時長，無法滿足移動機器人快速反應的場景定位要求。因此，探討輕量化移動機器人定位運算、進一步提高實時定位精度和配準速度，對于移動機器人適應不同場景的自主定位具有重要現實意義。

基于三維激光雷達采集的點云數據進行移動機器人定位的算法有最近點迭代算法（iterative closest point，ICP），但ICP算法運算耗時較長，不適合作為移動機器人的實時定位算法。Magnus提出了1種新的基于高斯分布的點云配準算法，即三維正態分布變換（3D-normal distributions transform，3D-NDT）[3]算法，將點云數據劃分為多個小體素（voxels），在每個體素上擬合高斯分布，通過計算目標點云和參考點云體素之間的相似度，估計2個點云之間的剛性變換，再通過最小化體素之間的誤差來實現配準[4]。針對3D-NDT算法在大規模點云匹配中計算效率較低的問題，Zhang等[5]引入基于體素網格的優化策略和更高效的數據結構，使3D-NDT算法從小規模點云拓展到大規模場景，但處理現實場景更大規模點云數據的速度較慢。張桂楊等[6]將3D-NDT算法移植到圖形處理單元（graphics processing unit，GPU），利用其多線程計算的優勢，加速場景點云配準過程，但移動機器人采集的點云數據與場景點云地圖位姿差異較大時，配準定位會出現無法收斂和進入局部極值的問題；Yousuf等[7]引入自適應網格細化的方法，在關鍵區域增加計算精度，提高收斂速度。但是，現有3D-NDT的改進算法仍存在不足：將點云劃分成多個等大方格的體素時，會直接把所有點的重心認作當前體素的降采樣點，易產生誤差；在沒有初始位姿的情況下，點云配準更易陷入局部極值無法收斂。鑒于此，提出1種改進的3D-NDT融合算法，采用優化降采樣的方法在保留源點云特征的前提下降低點云數量，在提高點云配準速度的同時保證點云配準精度；引入局域半徑調整迭代步長，并融合IMU和里程計數據獲取點云數據初始位姿，避免出現陷入局部極值和無法收斂的情況，以期提高移動機器人實時定位的準確度和實時性。

1 3D-NDT算法的改進

3D-NDT算法是由NDT算法改進而來的三維點云配準算法，具有配準速度快、精度高的特點[8]。3D-NDT采用標準最優法進行點云數據和參考點云數據的匹配，將采集的三維點云數據均分為大小相同的小方塊，對每個小方塊內的點云數據進行正態分布處理，即把點云數據轉變為用高斯概率分布表示的數學模型[9]。由于每個小方塊只有1個點參與運算，從而減少了待匹配點的數量[10]。3D-NDT算法進行點云配準的主要步驟如下：

1）將采集的三維點云數據進行三維分割，分割成大小相同的多個小方塊，也稱之為體素[11]。小方塊的體積決定點云降采樣的程度，小方塊越大，降采樣程度越大，配準速度越快。

2）將1個小方格中的點集記為X={x1，x2，···，xn}，其中n為1個小方格內點的總數。計算小方格內點C的公式如下[12]：

3）將小方塊中每個點的概率分布使用高斯概率函數P（x）表示，既正態分布建模：

其中c為常數。

4）對采集的點云數據使用初始變換參數p將其映射到目標點云，使點云數據變換為正態分布，即為1個多段平滑的3D空間[13]表示。

5）根據最大似然函數φ求解[14]最優的變換參數p。

式中T（p，xi）表示使用姿態變換T來移動點xi。

反復計算概率分布函數，當最大似然函數取最大值時[15]，求解得出最優變換參數p，通過求解出的p即可計算出移動機器人在點云地圖中的位置。

1.1體素濾波降采樣的改進

移動機器人的實時定位對響應速度要求較高，因此點云地圖配準時間應盡可能縮短。對移動機器人采集的三維點云數據進行點云地圖匹配前，需進行降采樣操作，以減少數據量并提升匹配速度。根據3D-NDT算法進行點云匹配的主要步驟，三維點云數據被分割成多個大小相同的小方塊，并分別計算每個小方塊的重心，以此代表方塊內的所有點。然而，由于重心可能并非原始點云數據中的實際點，會丟失點云數據的細節信息，導致細微誤差。因此，對降采樣濾波方法進行改進，使用距離重心最近的原始點代表小方塊內的所有點。如圖1所示，將點云數據劃分為多個體積相等的體素小方塊。

改進的降采樣算法具體步驟為：

1）接收移動機器人采集的點云數據集X={x1，x2，···，xn}，將此點云數據按照空間劃分為m個小立方體。

2）計算每個小立方體的重心坐標（xg，yg，zg），將所有重心點表示為1個新點云集N，其中V是點云數據集X的體積。

3）使用最鄰域搜索法[16]找出和重心點最近的原始點，由多維二叉樹遍歷點云集X尋找出替代點，這些替代點形成1個新點集Y，該點集即為體素濾波后的便于運算的點云集，同時該點云集包含原點云集的重要幾何信息。

1.2迭代過程及評分函數的優化

移動機器人采集的點云數據與點云地圖配準可能會出現無法收斂和進入局部極值的問題，原因在于2組點云數據位姿差異較大。對此，引入可調節的局域半徑（u），通過調節u來調整迭代步長，避免步長不合適而導致陷入局部極值和無法收斂的情況。u是根據因子（ρ）來動態調節的，在每次迭代過程中ρ也會改變。

式中：f（x），J（x）分別為函數在點x處的泰勒展開函數及增量方程；Δx為點x的增量。當計算得到的ρ遠小于1時，需縮小u；當ρ接近1時，則可擴大u以增大迭代步長。

給定第一次迭代步長l0，局域半徑的計算公式為：u0 L0=JT（l0）J（l0）（7）

其中：u0為初始局域半徑；L0為初始變換矩陣；α為常量，手動設置；lii（0）為L0的對角線元素。

當第k次迭代時，根據計算因子來調整局域半徑，進而求得迭代步長（lk+1）：

式中D（x）為x的方差值。根據計算因子ρ求得此時的迭代步長lk+1=lk+Δlk。

對于上述求得的新點云集Y和點云地圖的配準，將點集Y映射到點云地圖中，其中變換矩陣T=（tx，ty）T。

根據正態分布函數，計算點集X中的每個點轉換到點云地圖的概率密度P（x），根據3D-NDT算法配準得分將每個小方塊內計算的概率密度[17]求和：

根據牛頓優化算法對評分函數進行優化，求解出S取最大值時的變換參數p，求解出變換參數以及評分函數梯度是牛頓優化算法的關鍵步驟。評分函數的S值由每個小方塊的梯度s值求和得到[18]：

梯度方向可由矩陣求導公式和鏈式求導法則[19]求得：

雅可比矩陣[20]是均值q對變換參數pi的偏導數即

根據求得的s，可繼續求s對變量pi，pj的二階偏導Hij[21]：

Hij==??pj=

?qTΣ?1 exp/?pj

均值q對變換參數pi的二階導數向量為：

通過迭代，重復執行上述操作式（13）～（16）。

1.3激光雷達點云畸變的去除

移動機器人使用激光雷達采集1幀現場點云數據，需雷達完成1次全方位的旋轉。然而，由于機器人在不斷運動，采集的點云數據與移動機器人之間的相對距離會發生變化，從而導致點云數據出現運動畸變[22]。當移動機器人轉彎或速度很快時，點云運動畸變明顯，這就需要融合慣性測量單元（IMU）數據，分析移動機器人的運動狀態并對采集的點云數據進行運動補償以去除點云運動畸變。文中選用型號為N100的9軸IMU，設置測量頻率為400 Hz。9軸IMU在運行過程中存在零偏噪聲，會對數據產生影響，需對9軸IMU進行內參標定。將9軸IMU安裝于移動機器人適當位置，采集2 h靜止的IMU數據，再運行標定程序，將運行產生的配置文件放入IMU的驅動包。去除點云運動畸變的原理：從激光雷達掃描器開始旋轉至旋轉1周完成1幀點云數據的采集，積分IMU數據獲取雷達掃描1周期間移動機器人的位姿變換，然后使用這一位姿變換將雷達所有的點轉換至掃描初始位置，去除點云運動畸變。去除點云運動畸變流程如圖2。

假設激光雷達掃描起始時刻為t0，掃描完成1周的時刻為t1，在三維點云數據中第i個點的坐標為是oi，該點的時間節點為ti（t0lt;tilt;t1），該點相對于雷達掃描起始時刻t0的位姿轉換oi，wi為：

式中：Δo為雷達掃描1周期間移動機器人的平移量；Δw為雷達掃描1周期間移動機器人的旋轉量；r為比例常數；G（）為四元數單面插值。將ti時刻點的坐標平移量Δo和旋轉量Δw轉換至t0時刻，便可去除點云運動畸變：

式中oi、為去除點云運動畸變后的坐標。

三維激光雷達和9軸IMU的安裝位置不同，還需進行雷達和IMU的外參標定，以此求得雷達和IMU的相對位置關系。錄制1 min的數據包可運行標定程序，將根據標定程序得到的四元數數據手動轉換為旋轉矩陣，將旋轉矩陣放入配置文件即可完成雷達與IMU外參標定。錄制數據前應確保雷達和IMU安裝穩固，錄制數據時需將2臺設備整體在坐標系3個軸x，y，z方向充分移動和旋轉。

2移動機器人的實時定位流程

移動機器人實時定位的前提條件是建立供點云匹配的點云地圖，高精度的點云地圖可滿足移動機器人實時定位的精確度和穩定性。將構建的點云地圖劃分為多個體素，計算每個體素內點的概率密度分布函數，然后計算其協方差矩陣和均值。將三維激光雷達采集的點云分割成多個大小相同的小方塊，計算每個小方塊的重心；使用最鄰域搜索法找到距離重心最近的原始點，并用該點代替當前小方格內的所有點，完成點云的降采樣處理，顯著提高點云配準的速度，滿足移動機器人定位的實時性要求。通過監聽移動機器人在機器人操作系統中（robot operating system，ROS）的初始位姿話題（initial pose）獲取點云的初始姿態，將降采樣后的點云數據映射到點云地圖中計算每個映射點的概率密度，將每個體素內映射點的概率密度相加得到配準得分，并使用牛頓優化算法求出使配準得分最大時的變換參數。依據變換參數計算變換矩陣，轉換出移動機器人在點云地圖中的位姿。采用改進的3D-NDT算法實現移動機器人實時定位的流程，如圖3。

3實驗結果與分析

為驗證改進3D-NDT算法實時定位的可靠性和準確度，采用實驗室自搭建的移動機器人平臺，如圖4。移動機器人平臺分為三大單元，底部為運動控制單元，由輪系和運動編碼器組成。中間部分為控制系統和顯示單元，控制系統采用工控機為運算處理器，硬件配置為Intel（R）Core（TM）i7–7820HQ CPU@2.90 GHz*8，RAM 8 GB；顯示單元采用外接便攜顯示屏方案，便于調試移動機器人。上部為信息采集單元，由32線3D激光雷達與9軸IMU構成。

3.1現場地圖的構建

選用室外校園和室內長廊場景作為實驗環境，由移動機器人對2個實驗場景進行建圖，使用32線3D激光雷達建立點云地圖，移動機器人提前錄制的高精度點云地圖如圖5。由圖5可看出：室外場景下的地圖包含教學樓、樹木、機動車等靜態障礙物，點云數據三維特征明顯，有利于移動機器人實時定位；室內封閉環境下，點云地圖的建筑特征較為單一，點云數據特征不夠明顯，影響移動機器人定位的準確性，融合IMU數據可提升移動機器人的定位精度。

3.2 3D-NDT定位實驗

以建立的高精度點云地圖為基礎，采用改進的3D-NDT算法對移動機器人進行定位實驗。為減少參與點云地圖配準的點云數量，同時保留點云數據的三維特征，對移動機器人采集的三維點云數據進行體素濾波優化降采樣處理。如圖6所示，紅色點云為三維激光雷達掃描的原始點云，白色點云為通過體素濾波優化降采樣后的點云。從圖6可看出：降采樣處理后，白色點的數量明顯少于紅色點，同時白色點云保留紅色點云的三維特征。

為驗證本文算法在配準速度和精度方面的提升效果，分別采用原始和改進的3D-NDT算法在2種實驗場景下進行定位實驗，結果如圖7。圖中，灰白色點云為提前建立的點云地圖，紅色點云為移動機器人實時采集的點云數據。

從圖7可看出：2種算法應用在室內場景和室外場景下，移動機器人采集的點云數據和供參考的點云地圖均可很好地匹配；高密度的紅色點云與白色點云重合度較高，但是原始算法下低密度的紅色點云無法和灰白色點云重合，本文算法下低密度紅色點云與灰白色點云基本重合。由此表明：2種算法均能實現不同場景下的定位，但本文算法的定位精度優于原始算法。

歐式適應度指點云地圖和實時采集的三維點云地圖中各對應點之間歐式距離的平方和。配準時間指三維激光雷達采集到點云數據至工控機輸出變換矩陣定位時的時間間隔。為進一步比較分析改進3D-NDT算法的定位效果，選用歐式適應度和配準時間分別為點云數據配準精度和移動機器人定位實時性的評價指標，選取移動機器人在實驗場景下移動的100幀點云匹配，計算其歐式適應度和配準時間的平均值，結果如表1。

對比表1中的數據可看出：室外環境下，改進3D-NDT算法在匹配精度上比原始3D-NDT算法提升了106%，匹配成功率提升了8.29%，且平均匹配耗時大大降低，接近原始3D-NDT算法的1/2；室內環境下，改進3D-NDT算法在匹配精度上比傳統3D-NDT算法提升了108%，匹配成功率提升了6.35%，平均匹配耗時也大大降低。

綜上可看出：本文算法顯著降低了平均配準時間，可滿足移動機器人定位的實時性要求；此外，本文算法在室外和室內不同場景下均表現出良好的適應性和魯棒性，在復雜多變的環境中能夠可靠地支持移動機器人自主定位。

4結論

針對傳統算法配準速度較慢，無法滿足移動機器人定位的實時性要求，提出1種改進的3D-NDT算法對移動機器人進行自主定位。通過優化點云數據的降采樣，篩選體素小方塊內距離當前小方塊重心最近的點，并把此點作為當前體素內所有點的特征點；融合IMU數據分析移動機器人的運動狀態，完成機器人的定位。通過實驗室自搭建的移動機器人平臺，在室外校園和室內長廊環境下進行驗證實驗。結果表明：本文算法在室外和室內環境下的匹配精度比傳統3D-NDT算法分別提升106%，108%，平均匹配耗時分別降低51.1%，47.9%，本文算法在提高移動機器人定位精度的同時，大幅降低移動機器人單次定位的匹配時間，可滿足移動機器人定位的實時性要求；在室外復雜和室內封閉環境，本文算法均能保持穩定的性能，進一步證明了本文算法具有廣泛的適用性和可靠性，可為移動機器人在多樣化場景中的實時定位提供有力支持。

參考文獻：

[1]鄧永勝.煤礦井下無軌膠輪車的現狀及應用[J].礦業信息，2023（2）：165?167.

[2]HUANG J G，TAO B，ZENG F.Point cloud registration algorithm based on ICP algorithm and 3D-NDT algorithm[J].International Journal of Wireless and Mobile Computing，2022，22（2）：125?130.

[3]王太學，江智，江德港，等.融合PCA的改進ICP激光點云配準算法[J].遙感信息，2022，37（2）：70?76.

WANG T X，JIANG Z，JIANG D G，etal.Improved ICP laser point cloud registration algorithm based on PCA[J].Remote Sensing Information，2022，37（2）：70?76.

[4]BESL P J，MCKAY N D.A method for registration of 3D shapes[J].IEEE Transactions on Pattern Analysis and Machine Intelligence，2002，14（2）：239?256.

[5]ZHANG J，SINGH S.Low-drift and real-time lidar odometry and mapping[J].Autonomous Robots，2017，41（2）：401?416.

[6]張桂楊，苑壯，陶剛.基于NDT和ICP融合的點云配準方法[J].北京測繪，2019，33（12）：1465?1469.

ZHANG G Y，YUAN Z，TAO G.A point cloud registration method based on NDT and ICP fusion[J].Beijing Surveying and Mapping，2019，33（12）：1465?1469.

[7]YOUSUF S，KADRI M B.Information fusion of GPS，INS and odometer sensors for improving localization accuracy of mobile robots in indoor and outdoor applications[J].Robotica，2021，39（2）：250?276.

[8]PARK H G，AHN K K，PARK M K，etal.Study on robust lateral controller for differential GPS-based autonomous vehicles[J].International Journal of Precision Engineering and Manufacturing，2018，19（3）：367?376.

[9]李繞波，袁希平，甘淑，等.一種基于對偶四元素描述的線面特征約束的點云配準方法[J].光學學報，2022，42（2）：167?175.

LI R B，YUAN X P，GAN S，etal.Point cloud registration method based on dual quaternion description of line-planar feature constraints[J].Acta Optica Sinica，2022，42（2）：167?175.

[10]POMERLEAU F，COLAS F，SIEGWART R，etal.Comparing ICP variants on real-world data sets[J].Autonomous Robots，2013，34（3）：133?148.

[11]彭真，呂遠健，渠超，等.基于關鍵點提取與優化迭代最近點的點云配準[J].激光與光電子學進展，2020，57（6）：68?79.

PENG Z，LüY J，QU C，etal.Accurate registration of 3D point clouds based on keypoint extraction and improved iterative closest point algorithm[J].Laseramp;Optoelectronics Progress，2020，57（6）：68?79.

[12]王慶閃，張軍，劉元盛，等.基于NDT與ICP結合的點云配準算法[J].計算機工程與應用，2020，56（7）：88?95.

WANG Q S，ZHANG J，LIU Y S，etal.Point cloud registration algorithm based on combination of NDT and ICP[J].Computer Engineering and Applications，2020，56（7）：88?95.

[13]MAGNUSSONM.The three-dimensional normal-distributions transform：an efficient representation for registration surface analysis and loop detection[J].Renewable Energy，2009，28（4）：655?663.

[14]JIANG J W，WANG J K，WANG P，etal.LiPMatch：lidar point cloud plane based loop-closure[J].IEEE Robotics and Automation Letters，2020，5（4）：6861?6868.

[15]BURNETT K，SCHOELLIG A P，BARFOOT T D.Do we need to compensate for motion distortion and Doppler effects in spinning radar navigation？[J].IEEE Robotics and Automation Letters，2021，6（2）：771?778.

[16]SUN R Y，ZHANG E Z，MU D Q，etal.Optimization of the 3D point cloud registration algorithm based on FPFH features[J].Applied Sciences，2023，13（5）：3096.

[17]孫文瀟，王健，梁周雁，等.法線特征約束的激光點云精確配準[J].武漢大學學報（信息科學版），2020，45（7）：988?995.

SUN W X，WANG J，LIANG Z Y，etal.Accurate registration of laser point cloud based on normal feature constraint[J].Geomatics and Information Science of Wuhan University，2020，45（7）：988?995.

[18]關麗敏，張倩，楚慶玲，等.基于改進ICP算法的路側雙激光雷達數據融合[J].激光雜志，2021，42（9）：38?44.

GUAN L M，ZHANG Q，CHU Q L，etal.Roadside dual LIDAR data fusion based on improved ICP algorithm[J].Laser Journal，2021，42（9）：38?44.

[19]衣明悅，石晶，趙梓杉.基于激光雷達的點云配準算法綜述[J].汽車實用技術，2021，46（17）：212?214.

YI M Y，SHI J，ZHAO Z S.Survey of point cloud registration algorithms based on lidar[J].Automobile Applied Technology，2021，46（17）：212?214.

[20]RUAN J Y，FANG Z，LI B，etal.Evaluation of GP-SLAM in real-world environments[C]//2019 Chinese Automation Congress.Hangzhou，China：IEEE，2019：3076?3081.

[21]楊奇峰，曲道奎，徐方.基于3D-NDT的移動機器人定位算法研究[J].控制工程，2020，27（4）：613?619.

YANG Q F，QU D K，XU F.Localization algorithm of outdoor mobile robot based on 3D-NDT[J].Control Engineering of China，2020，27（4）：613?619.

[22]譚國威，伍吉倉.基于FPFH特征的三維點云配準方法研究[J].工程勘察，2022，50（4）：52?56.

TAN G W，WU J C.Research on 3D point cloud registration based on FPFH features[J].Geotechnical Investigationamp;Surveying，2022，50（4）：52?56.

責任編輯：何莉