第一部分 設計概述

1.1 設計目的

近年來，機器學習，無人駕駛等領域是十分熱門的研究話題。在這些領域中，電腦對環境的感知十分重要。因此三維的環境重建是必然的趨勢，方便我們實現虛擬世界與現實世界之間的交互。

在三維重建中，主流的方式有視覺和激光雷達兩種。視覺技術的測距方法是基于三角的測距法，最大范圍是 5-8m，并不適用于較大的空間，且這種方法受光線的影響很大。相比之下，激光雷達可以用于的場景更為廣泛。

1.2 應用領域

三維建模的應用很廣泛，例如無人機、無人駕駛汽車的即時避障，路徑規劃。同時也可以與機器視覺、3D 打印等技術也可以融合。在文物復刻和重建也是十分有意義的應用。本次設計的基于 ZYNQ 的激光雷達三維建模系統，可以深入目標環境， 進行點云采集，重建三維空間模型，測量等機器視覺方面的應用。

1.3 主要技術特點

首先，本系統采用 pcl 點云庫中的 icp 算法，可將激光雷達返回的數據進行多次迭代計算，實現精確配準。

其次，本系統將激光雷達搭載在麥輪小車上，通過小車陀螺儀和電機編碼器，實時傳輸小車的位置和速度信息，通過坐標系變換可得到激光雷達的實時精準定位。

本設計可以對距離為 1~5m，盲區較少的室內物體進行實時的三維重建， 不易受光線的影響。激光雷達搭載在可遠程控制的小車上，可以用于勘察人無法進入的環境。

1.4 關鍵性能指標

1.5 主要創新點

（1） 可通過移動 app 對小車進行遠程控制；

（2） 算法基于 PCL 點云庫；

（3） 通過 Icp 算法可達到實時數據配準；

（4） 利用 pynq 的 PL 模塊對 icp 算法進行 HLS 加速。

第二部分 系統組成及功能說明

2.1 整體介紹

系統由激光雷達、基于 STM32 的麥輪小車 和 Xilinx 公司 PYNQ-Z2 組成的。其中激光雷達通過網口將采集到的點云數據傳給 PYNQ-Z2，麥輪小車上搭載了電機編碼器、陀螺儀和藍牙模塊。轉向和移動可y由手機藍牙程序控制。在運動過程中，將位移和姿態信息傳遞給 STM32 單片機，通過 UART 協議 STM32 將這些信息傳遞給 PYNQ-Z2，ZYNQ 根據位移和姿態信息計算出激光雷達的位移和姿態偏移。ZYNQ 根據姿態和位置信息的偏移將點云數據利用 ICP 算法拼接起來，拼接后用網口傳出數據。

在本設計中，我們采用小車搭載激光雷達進行移動掃描，采集左、右和頂三面的信息進行重構。

2.2 各模塊介紹

2.2.1 R-Fans-16 激光雷達

本系統中激光雷達采集采用的是 R-Fans-16 導航型雷達，它通過 16 線 360°掃描實現三維探測成像。基于高精度激光回波信號測量技術，R-Fans-16 具備測程遠（探測能力最遠達到 200m），測量精度高（測距精度優于 2cm），回波強度準確（目標反射回波強度達到 8 位）等技術特點，同時兼顧了俯仰方向的角度覆蓋和角分辨率。運行激光雷達時，通過網口將實時點云數據傳輸給 PYNQ-Z2。

2.2.2 基于 STM32 的麥輪小車

在這個麥輪小車上搭載了 STM32 單片機，本次實驗中，利用了小車上的陀螺儀、編碼器和藍牙。小車上的陀螺儀和電機編碼器通過 SPI 協議將數據傳給 STM32 單片機，單片機通過計算得到小車的姿態和輪胎轉速，然后利用 UART 協議，以 115200 的波特率將數據循環實時發送給 ZYNQ。小車的移動和轉向利用藍牙遠程控制。

2.2.3 坐標系轉換

本設計中采用的是 R-Fans-16 導航型雷達，它采集的數據是建立在自身的坐標系之中的，三維重建的本質是將激光雷達坐標系中的數據轉換為大地絕對坐標系，即球坐標系轉化為直角坐標系。

球坐標系是一種利用球坐標（r，θ，φ）表示一個點 P 在三維空間的位置的三維正交坐標系。如圖 2.2.1 所示，原點與點 P 之間的“徑向距離”為 r，原點到點 P 的連線與正 z 軸之間的“極角”為 θ，原點到點 P 的連線在 xy 平面的投影線與 x 軸之間的“方位角”為 φ。

球坐標系與直角坐標系之間的公式轉化如下：

在本設計中，以車啟動時的坐標為絕對坐標系的坐標原點，此后在每圈激光雷達的數據期間，以激光雷達為坐標原點建立一個個子坐標系，并記錄此刻激光雷達在初始絕對坐標系下的位移偏轉量。

xyz 三個坐標軸的方向與激光雷達坐標系的軸方向一致。借助電機的編碼器， 可測得激光雷達的水平平面移動速度和方向，即可得知激光雷達坐標系與絕對坐標之間的平移量；通過陀螺儀，可測得激光雷達的姿態角，以得知激光雷達坐標系與絕對坐標系之間的旋轉量。借助以上測得的兩個數值以及球坐標系與直角坐標系之間的轉換公式，即可將激光雷達坐標系中的點映射到大地絕對坐標系中。

2.2.4 點云配準（ICP 算法）

ICP 算法配準就是要將兩個不同坐標系下的點集，利用它們的幾何特性匹配起來。需要求解目標點集和參考點集之間的剛體變換矩陣和平移矩陣，利用剛體變換矩陣作用在目標點集，使兩個點集盡可能重合。對于目標點集 P 和參考點集 Q 來說，轉換公式為：

上式不總是成立的，因此我們要最小化目標函數

求解 R 和 T 常用的方法有：SVD 和非線性優化。本設計中使用了 SVD 的方法。

ICP 算法問題通常轉換為最小二乘最優解問題，將整個問題分為兩部分，第一部分是第二部分的基礎和輸入。第一部分稱為粗略配準或全局配準，粗略配準即通過計算兩點集間的位姿，得出點集間的大致重合結果，為下一步的精確配準提供合適初值。第二部分稱精確配準或局部配準，對兩個距離足夠接近的點集使用迭代優化策略以達到最終的配準結果。

第三部分 完成情況及性能參數

3.1 總述

本系統本設計完成了激光雷達的點云采集、 陀螺儀、編碼器的姿態信息采 集。PYNQ-Z2 開發板的控制芯片 ZYNQ 使用的 PS-PL 設計極大地提高了系 統設計的便捷性、可行性，降低了系統的設計難度。PS-PL 主從設計在保留系 統設計的簡易性同時提高了系統的運行速度和處理能力。PL 端 IP 核的設計極 大地加快了算法的計算速度。本設計的點云拼接部分通過 PL 端的 IP 核加速， 增強了拼接效果，成功地實現了實時三維重建的功能。

3.2 完成情況

在室內走廊中，我們擺放了一排桌子、一個消防滅火器，兩側分別為墻壁和 窗戶。

每采集 200 幀作為一組數據，存入 pcd 文件，得到每組數據生成的原始圖像如圖 3.2.2 所示，圖像右側為擺放桌子一側，可看到明顯桌面和桌腳的細節，左側較低側為擺放的消防滅火器。

每相鄰兩組點云進行 icp 配準，得到完整的小車駛過走廊的三維模型。

圖片右側離散的小點云為走廊中的窗戶投射到室外返回的激光，若走廊兩側都為墻面，則可返回得到完整的三維模型。

3.3 性能參數

激光雷達點云采集時距離越遠分辨率越低，在有效探測距離內對物體的寬度和深度的測量誤差取決于設備的穩定性（這里指激光雷達的抖動程度）。寬度測量誤差在 2cm 內，傾斜物體的測量誤差為 6cm，傾斜角度的誤差為 4°。由于測量時小車自身會有輕微抖動，所以結果有一定的誤差。

第四部分 總結

可擴展之處

（1）使用 python 編程。本次設計是 ZYNQ 上搭載了 Utunbu18.04，基于 C++ 語言的編程。使用過程中只能調用 C 語言基本的庫中的函數以及 PCL 庫函數， 顯示并不方便，對使用者并不友好。可將 PCL 移植到 Jupyter 平臺，利用 Python 更方便更快捷。

（2）全方位的立體掃描。對于搭載激光雷達的小車來說，只能掃描小車的 左、右和頂三面進行單向的掃描。想要掃描一個特定的物體，則無法了解其立體的信息，可以選擇更靈活的搭載物，例如無人機。

作品來源：Xilinx開源社區，已授權，作者：岳恒；李志遠；史巧雅。

編輯：jq