淺析基于ZYNQ的激光雷達三維建模

第一部分 設(shè)計概述

1.1 設(shè)計目的

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

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

1.2 應(yīng)用領(lǐng)域

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

1.3 主要技術(shù)特點

首先，本系統(tǒng)采用 pcl 點云庫中的 icp 算法，可將激光雷達返回的數(shù)據(jù)進行多次迭代計算，實現(xiàn)精確配準(zhǔn)。

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

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

1.4 關(guān)鍵性能指標(biāo)

1.5 主要創(chuàng)新點

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

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

（3） 通過 Icp 算法可達到實時數(shù)據(jù)配準(zhǔn)；

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

第二部分 系統(tǒng)組成及功能說明

2.1 整體介紹

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

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

2.2 各模塊介紹

2.2.1 R-Fans-16 激光雷達

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

2.2.2 基于 STM32 的麥輪小車

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

2.2.3 坐標(biāo)系轉(zhuǎn)換

本設(shè)計中采用的是 R-Fans-16 導(dǎo)航型雷達，它采集的數(shù)據(jù)是建立在自身的坐標(biāo)系之中的，三維重建的本質(zhì)是將激光雷達坐標(biāo)系中的數(shù)據(jù)轉(zhuǎn)換為大地絕對坐標(biāo)系，即球坐標(biāo)系轉(zhuǎn)化為直角坐標(biāo)系。

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

球坐標(biāo)系與直角坐標(biāo)系之間的公式轉(zhuǎn)化如下：

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

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

2.2.4 點云配準(zhǔn)（ICP 算法）

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

上式不總是成立的，因此我們要最小化目標(biāo)函數(shù)

求解 R 和 T 常用的方法有：SVD 和非線性優(yōu)化。本設(shè)計中使用了 SVD 的方法。

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

第三部分 完成情況及性能參數(shù)

3.1 總述

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

3.2 完成情況

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

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

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

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

3.3 性能參數(shù)

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

第四部分 總結(jié)

可擴展之處

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

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

作品來源：Xilinx開源社區(qū)，已授權(quán)，作者：岳恒；李志遠；史巧雅。

編輯：jq