淺析基于ZYNQ的激光雷達(dá)三維建模

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

1.1 設(shè)計(jì)目的

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

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

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

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

1.3 主要技術(shù)特點(diǎn)

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

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

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

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

1.5 主要創(chuàng)新點(diǎn)

（1） 可通過移動 app 對小車進(jìn)行遠(yuǎn)程控制；

（2） 算法基于 PCL 點(diǎn)云庫；

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

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

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

2.1 整體介紹

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

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

2.2 各模塊介紹

2.2.1 R-Fans-16 激光雷達(dá)

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

2.2.2 基于 STM32 的麥輪小車

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

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

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

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

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

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

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

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

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

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

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

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

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

3.1 總述

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

3.2 完成情況

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

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

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

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

3.3 性能參數(shù)

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

第四部分 總結(jié)

可擴(kuò)展之處

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

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

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

編輯：jq