陳小宇，陽夢雪，李常對，趙鵬程

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

2.武漢大學遙感信息工程學院，湖北武漢430079

迭代最近點（iterative closest point,ICP）是一種常用的激光點云匹配算法，ICP 算法通常需要大量的計算時間。文獻[1-3] 的研究表明，減少ICP 計算時間的方法包括減少迭代次數(shù)、減少同名點數(shù)量和加速K最近鄰（K-nearest neighbor,KNN）搜索3 種，基于ICP 方法的基準顯示，KNN 搜索花費的時間占ICP 算法總計算時間的75% 左右，其效率是影響ICP效率的主要因素。

目前常用的KNN 搜索大都是基于中央處理器（central processing unit,CPU）、圖形處理器（graphics processing unit,GPU）、專用集成電路（application specific integrated circuit,ASIC）、現(xiàn)場可編程門陣列（field programmable gate array,FPGA）平臺。文獻[4] 利用統(tǒng)一計算設備架構（compute unified device architecture,CUDA）對KNN 算法問題進行加速，并且對比了KNN 距離計算和排序在CPU、GPU 上的運行時間，結果顯示基于GPU 平臺進行KNN 距離計算和排序的耗時分別為79.81 ms 和0.53 ms，而基于CPU 平臺處理耗時為5 647.45 ms 和15.32 ms。文獻[5-8] 對CPU、ASIC、FPGA 平臺上KNN 搜索的時間進行了對比，耗時分別為8 423 ms、1 169 ms、720 ms，因此基于FPGA 平臺進行KNN 搜索可減少搜索耗時。文獻[9-10] 基于FPGA 異構計算平臺的特點，設計了KNN 算法的異構實現(xiàn)方案KB-KNN，并且對并行方案CU-KNN 和KB-KNN 在CPU、GPU、FPGA 平臺上的性能進行了對比，結果表明KB-KNN 方案與移植到FPGA 平臺上的CU-KNN 方案相比獲得了1.7 倍的加速比，與原始CU-KNN 方案相比獲得了1.5 倍的能效比。因此合理的優(yōu)化方法將使FPGA 作為一種新型異構計算加速設備具有更高的能量效率。

綜上可知，基于FPGA 平臺進行的三維激光點云KNN 搜索具有功耗低、耗時短的優(yōu)勢。本文提出了一種利用MPSoC FPGA 實現(xiàn)三維激光點云線性KNN 快速搜索的方法，在保證鄰近點搜索精度的前提下，發(fā)揮FPGA 高速并行的優(yōu)勢，通過存儲管理模塊、距離計算模塊、K最鄰近比較模塊、K鄰近特征結果寫回模塊以及調度管控模塊相互協(xié)調配合，大大減少了三維激光點云K最近鄰搜索耗時。

1 KNN 算法

KNN 算法是一種理論完善、簡單有效的惰性學習算法[11-12]，1968 年由Cover 和Hart提出。K最近鄰是指最近的K個鄰居，每個樣本都可以用其最接近的K個鄰居來代表[13]。在深度學習領域中，可以用于分類的方法有決策樹、神經(jīng)網(wǎng)絡、遺傳算法、支持向量機、KNN分類等，其中KNN 作為一種較為成熟的算法，是向量空間模型（vector space model,VSM）下最好的分類算法之一[14]。

KNN 算法的基本思想是把每個樣本假設為空間中對應的各個點，同時也把新樣本假設成空間中的一個點，當輸入需要分類的新樣本時，通過距離函數(shù)計算這些新樣本點與空間中各個點之間的距離并排序，選擇K個距離最小的樣本，再將需要進行分類的未知樣本歸類到K個距離值中出現(xiàn)頻率最高的類[15]。將KNN 算法簡要描述為如下流程圖[16]。

2 總體設計框架

激光雷達可以從環(huán)境中獲取被測目標的精確距離及激光回波強度信息，被廣泛應用于地圖匹配定位[17]。利用三維激光點云進行地圖匹配定位時，需要在分別在兩幀激光點云和激光點云地圖中進行KNN 搜索，總體框架如圖2 所示。

圖1 KNN 算法流程圖Figure 1 Flow chart of KNN algorithm

圖2 總體框架圖Figure 2 Overall frame diagram

幀間搜索：從雙數(shù)據(jù)率同步動態(tài)隨機存儲器（double data rate synchronous dynamic random access memory,DDR）中實時加載當前幀三維激光點云數(shù)據(jù)，依次與已經(jīng)部署在嵌入式塊存儲器（block random access memory,BRAM）中的上一幀激光點云數(shù)據(jù)進行K最近鄰搜索，并且將搜索結果寫回DDR 中。

地圖搜索：從DDR 中加載一幀位姿變換后的激光點云數(shù)據(jù)，與已經(jīng)從DDR 中加載并存儲在BRAM 中的地圖特征點數(shù)據(jù)進行KNN 搜索，并且將搜索結果寫回DDR 中。

3 KNN 算法的模塊設計

本文利用MPSoC FPGA 實現(xiàn)激光點云KNN 搜索，利用處理系統(tǒng)（processing system,PS）進行任務調度，可編程邏輯（programmable logic,PL）進行KNN 加速，采用多個模塊并行計算，以減少KNN 搜索耗時，如圖3，包括存儲管理模塊、距離計算模塊、K最鄰近比較模塊、K鄰近特征結果寫回模塊以及調度管控模塊。為了提高激光點云匹配的精度，分別利用角特征點和面特征點進行幀間KNN 搜索和地圖KNN 搜索，幀間KNN 搜索和地圖KNN搜索實現(xiàn)過程相似，本文將以地圖面特征點的KNN 搜索為例進行說明。

圖3 KNN 算法的并行模塊Figure 3 Parallel module of KNN algorithm

3.1 存儲管理模塊

在地圖面特征點的KNN 搜索中，需要將位姿變換后的一幀面點數(shù)據(jù)與地圖面點數(shù)據(jù)進行KNN 搜索。為減少KNN 搜索耗時，首先需要將位姿變換后的面點數(shù)據(jù)與地圖面點數(shù)據(jù)分別部署到BRAM 中，如圖4 所示，位姿變換后的面點數(shù)據(jù)被部署到BRAM163 中，地圖面點數(shù)據(jù)被部署到BRAM1,BRAM2,···,BRAM96 中，使位姿變換后的面點數(shù)據(jù)同時與96 個地圖面點數(shù)據(jù)進行歐氏距離計算，從而提高KNN 搜索效率。

圖4 地圖面點搜索數(shù)據(jù)存儲圖Figure 4 Map surface point search data storage diagram

3.2 距離計算模塊

在存儲管理模塊的每個BRAM 后跟隨一個距離計算模塊Cal_PE。將存儲管理模塊中部署在BRAM 里的數(shù)據(jù)按照行列方向依次取出，每一列在前一列的時間上延遲一個時鐘節(jié)拍，形成pipeline 流水結構，這樣可以避免特征點的數(shù)據(jù)路徑太長或扇出太大導致的時序違規(guī)。X、Y、Z方向上分別延一拍讀出特征點的X、Y、Z坐標，然后進行歐氏距離計算。如圖5 為地圖面點搜索距離計算圖。

圖5 地圖面點搜索距離計算圖Figure 5 Map surface point search distance calculation diagram

3.3 K 最鄰近比較模塊

從經(jīng)過位姿變換的點云數(shù)據(jù)中每提取一個特征點，都需要在地圖的特征點中找出與其最鄰近的K個特征點，因此經(jīng)過歐氏距離計算的特征點，都需要輸入到K最鄰近比較模塊中，找出K個最鄰近的坐標值。

在K最鄰近比較模塊中，將比較模塊Com_PE 中較小結果和對應的地址addr1 以及所在列序號num1 向流水線的右側進行傳輸，將每個比較模塊Com_PE 中較大結果和對應的地址addr2 以及所在列序號num2 向流水線的下側進行傳輸。由于K最鄰近比較模塊的并行度與距離計算模塊一樣，因此在減少歐氏距離計算耗時的同時，可以節(jié)省從歐氏距離計算結果中找出最小值所用的時間。圖6 為地圖面點搜索最鄰近比較圖，其中K取5。

3.4 K 鄰近特征結果寫回模塊

依次根據(jù)K最鄰近比較模塊輸出的地址和所在列序號，將對應地址中特征點的3 個維度坐標同時讀出進行數(shù)據(jù)位拼接，再將K個最鄰近點的三維坐標寫回DDR 中，即可完成一次KNN 搜索。

3.5 調度管控模塊

如圖7 為調度管控模塊處理圖，調度管控模塊類似一個狀態(tài)機，配合并行計算架構和調度管控等模塊協(xié)調處理數(shù)據(jù)流之間的時序關系。在程序啟動時，調度管控模塊控制從DDR 中讀取激光點云數(shù)據(jù)，并且控制將搜索結果寫回DDR 中，實現(xiàn)三維激光點云數(shù)據(jù)的連續(xù)讀取、計算和寫回。

4 系統(tǒng)仿真與驗證

整個工程在vivado 2019.2 環(huán)境中設計開發(fā)，結合自帶的仿真軟件進行仿真。工作時鐘為210 MHz 經(jīng)過測試系統(tǒng)功能實現(xiàn)正常。圖8 為歐氏距離計算仿真波形圖，圖中的result 就是兩個點進行歐氏距離計算的結果，例如，result_1 是bram_douta_163 和douta_1 的三維坐標進行歐氏距離計算的結果。

圖8 歐氏距離計算仿真圖Figure 8 Simulation diagram of Euclidean distance calculation

圖9 是利用激光雷達連續(xù)采集得到的三維激光點云數(shù)據(jù)進行幀間和地圖的KNN 搜索中的一幀地圖面點KNN 搜索結果圖，其中圖9(a) 是基于ARM 進行地圖面點的KNN 搜索，而圖9(b) 是基于MPSoC FPGA 進行地圖面點的KNN 搜索，圖中紅色的點是位姿變換后的一幀面點數(shù)據(jù)與地圖面點數(shù)據(jù)進行KNN 搜索得到的K個最鄰近點，白色點是地圖面點。對比兩幅圖的紅色點可以看出基于MPSoC FPGA 和ARM 進行地圖的KNN 搜索結果一致。

圖9 基于ARM 和MPSoC FPGA 的地圖面點KNN 搜索結果對比Figure 9 Comparison of map surface point KNN search results based on ARM and MPSoC FPGA

圖10 辦公室實景圖Figure 10 Real picture of the office

圖11 點云實時處理結果圖Figure 11 Real-time processing results of point cloud

將激光雷達置于辦公室環(huán)境中，并且使激光雷達按照畫8 的軌跡運動，實時采集當前幀和地圖點云，進行幀間和地圖的KNN 搜索，最后將搜索后的激光雷達運動軌跡實時顯示。如點云實時處理結果圖中，白色的點是當前幀點，藍色的是地圖點云，紅色的點是激光雷達的運動軌跡，從點云實時處理結果圖中可以看出激光雷達的運動軌跡與實際軌跡相符，說明基于MPSoC FPGA 進行地圖KNN 搜索在實際的運動場景中搜索精度高。

根據(jù)Vivado2019.2 綜合實現(xiàn)后的報告給出MZU15A 硬件消耗情況可以看出，如表1 所示，本工程中共使用207 382.0 個LUT，236 904.0 個觸發(fā)器，580.5 個BRAM 和2 880.0 個DSP，充分利用了MZU15A 開發(fā)板的硬件資源。通過將距離計算模塊的歐氏距離計算結果直接輸入到K最鄰近比較模塊中比較大小，節(jié)省了BRAM 的資源消耗，位寬為96 位的114 688.0 個點云數(shù)據(jù)進行KNN 搜索只需要580.5 個BRAM，大大減少了BRAM 資源的占用。

表1 資源消耗對比Table 1 Comparison of resource consumption

ARM 性能的提升，使得它在激光雷達數(shù)據(jù)處理中得到了廣泛的應用。本文采用和ARM對比的方式來驗證基于MPSoC FPGA 平臺實現(xiàn)三維激光點云KNN 算法的加速效果。ARM的型號為Qual-core ARM Cortex-A53 @1 333 MHz，MPSoC FPGA 采用米聯(lián)客MZU15A 開發(fā)板，工作時鐘為210 MHz。測試ARM 和MPSoC FPGA 的耗時情況如表2 所示，可以看出，基于MPSoC FPGA 的幀間KNN 搜索中1 024×1 024 個角點搜索耗時為0.67 ms，相比ARM 加速了8.22 倍，3 072×3 072 個面點搜索耗時為2.01 ms，相比ARM 加速了24.65倍；地圖KNN 搜索中1 024×20 480 個角點搜索耗時為4.66 ms，相比ARM 加速了23.62倍，3 072×81 920 個面點搜索耗時為13.98 ms，相比ARM 加速了94.74 倍?？梢姡贛PSoC FPGA 平臺實現(xiàn)三維激光點云KNN 算法耗時更短。由于開發(fā)板的BRAM 資源有限，地圖面點的KNN 搜索選擇了96 倍的并行度。若開發(fā)板BRAM 資源充足，基于MPSoC FPGA 平臺實現(xiàn)三維激光點云KNN 搜索算法耗時將會更短，加速效果更好。

表2 耗時對比Table 2 Comparison of time consuming

5 結語

本文利用FPGA 高速并行的優(yōu)勢，設計了存儲管理模塊、距離計算模塊、K最鄰近比較模塊、K鄰近特征結果寫回模塊以及調度管控模塊，通過各模塊的協(xié)調合作，實現(xiàn)了三維激光點云K最近鄰快速搜索及FPGA 加速。在真實場景中，利用載有MZU15A 開發(fā)板和激光雷達的小車對本文的方法進行了測試驗證，結果表明本文提出的基于MPSoC FPGA 的線性KNN 搜索算法具有明顯的加速效果，同時兼具精度高、功耗較低的優(yōu)點。此外，該搜索方法采用了參數(shù)化的設計方案，可靈活配置激光點云特征點數(shù)、維度、鄰近特征點個數(shù)和數(shù)據(jù)位寬等參數(shù)，以便根據(jù)不同的FPGA 資源進行適當調整部署，大大方便了后期的使用。