3D慣導(dǎo)Lidar仿真

LiDAR-Inertial 3D Plane Simulator

摘要

提出了最近點(diǎn)平面表示的形式化方法，并分析了其在三維室內(nèi)同步定位與映射中的應(yīng)用。提出了一個(gè)利用最近點(diǎn)平面表示的無奇異平面因子，并在基于圖的優(yōu)化框架中證明了它與慣性預(yù)積測(cè)量的融合。所得到的LiDAR慣性三維平面SLAM（LIPS）系統(tǒng)在定制的LiDAR模擬器和實(shí)際實(shí)驗(yàn)中都得到了驗(yàn)證。

I.介紹

準(zhǔn)確、魯棒的室內(nèi)定位和映射是自動(dòng)機(jī)器人許多應(yīng)用的基本要求。室內(nèi)環(huán)境通常是豐富的指令信息，如直線和平面，應(yīng)加以利用，以實(shí)現(xiàn)高精度的同時(shí)定位和制圖（SLAM）。盡管在室內(nèi)環(huán)境中使用GPS進(jìn)行定位，慣性導(dǎo)航系統(tǒng)（INS）與外部處理傳感器（如攝像機(jī)[1，2]、光探測(cè)和測(cè)距（LiDAR）傳感器[3，4]甚至聲納[5]）的輔助已經(jīng)被證明是有效。

最近激光雷達(dá)傳感技術(shù)在重量和尺寸上都有所減少，允許便攜式和手持使用，每秒在周圍環(huán)境中提供220萬個(gè)數(shù)據(jù)點(diǎn)。激光雷達(dá)已被證明是有效的紋理少和低光環(huán)境，典型的辦公室和建筑內(nèi)部，提供高信噪比（SNR）的測(cè)量。基本上，激光雷達(dá)傳感器不依賴于其他傳感器所需的照明或紋理特性。激光雷達(dá)傳感器最具挑戰(zhàn)性的一個(gè)方面是如何處理大量無序的三維點(diǎn)數(shù)據(jù)估計(jì)。傳統(tǒng)的方法是使用迭代閉合點(diǎn)（ICP）解算器來確定兩個(gè)點(diǎn)之間的相對(duì)變換。使用ICP只恢復(fù)相對(duì)姿態(tài)，防止在狀態(tài)中包含信息量大的環(huán)境元素，如平面估計(jì)使用平面基元進(jìn)行估計(jì)的第一個(gè)挑戰(zhàn)是它們的參數(shù)化[6]。最常見的表示是平面的法向矢量和距離標(biāo)量，稱為黑森形式。自黑森形式是一個(gè)過度參數(shù)化，它將遭受一個(gè)奇異的信息矩陣在最小平方上他的工作是由特拉華大學(xué)（UD）工程學(xué)院部分支持，UD網(wǎng)絡(luò)安全倡議、NSF（IIS-1566129）和DTRA（HDTRA1-16-1-0039）。為了避免這種過度參數(shù)化，通常使用包含兩個(gè)角（水平角和垂直角）和一個(gè)距離標(biāo)量的球面坐標(biāo)作為其誤差狀態(tài)表示。雖然這是最小的，但當(dāng)垂直角等于±π2時(shí)，它會(huì)有歧義。最近，Kaess[6]提出使用單位四元數(shù)及其無奇異性的3自由度乘性誤差狀態(tài)，該狀態(tài)與平面的幾何聯(lián)系不清楚，其數(shù)值穩(wěn)定性可能不是最優(yōu)的。相比之下，在這項(xiàng)工作中，本文使用最接近的點(diǎn)（CP）來表示，這是由平面上最接近的點(diǎn)定義的，以確定給定的參考系。

本文的主要貢獻(xiàn)如下：

?最近點(diǎn)（CP）平面表示法的制定、奇點(diǎn)分析及其作為平面表示法和三維平面SLAM誤差狀態(tài)的使用。

?設(shè)計(jì)一種新的激光雷達(dá)慣性三維平面SLAM（LIPS）系統(tǒng)，該系統(tǒng)具有基于圖形優(yōu)化的魯棒相對(duì)平面錨定因子，有效地克服了CP表示的奇異性問題。

?開發(fā)一個(gè)通用的激光雷達(dá)模擬器，用于評(píng)估激光雷達(dá)輔助定位算法，該模擬器是開源的，可以更好地造福社區(qū)。

?通過蒙特卡羅模擬和實(shí)際實(shí)驗(yàn)驗(yàn)證提議的LIPS系統(tǒng)。

1. 基于圖的優(yōu)化

建立優(yōu)化問題并迭代求解：

2.LIPS系統(tǒng)

在LIPS系統(tǒng)中，定義每個(gè)時(shí)刻機(jī)器人的的狀態(tài)為16*1維向量：

LIPS系統(tǒng)的最終的cost函數(shù)可以描述為：

3. 連續(xù)IMU預(yù)積分因子

最終的測(cè)量殘差如下：

4.最近點(diǎn)平面表示

“最近點(diǎn)（CP）”表示可以被描述為駐留在平面上并且最接近當(dāng)前幀的原點(diǎn)的3D點(diǎn)。 這種表示的好處在于它已經(jīng)處于其最小的表示中，并且如果特別注意選擇它所定義的參考系，則它是單一的，無奇點(diǎn)。 通過將平面表示為單個(gè)3D點(diǎn)，在優(yōu)化期間更新參數(shù)時(shí)也有一個(gè)簡單的加性誤差模型。 可以使用Hesse法向量和距離標(biāo)量來描述該CP表示：

圖1：平面上最近點(diǎn)的直觀表示

5.錨面因子

為克服CP表示的奇點(diǎn)問題，在第一觀察幀中對(duì)平面進(jìn)行參數(shù)化，保證到平面的距離將是非零的（從此處向后，這將表示為“錨”幀/狀態(tài)）。如圖2所示，從一幀到另一幀的平面表示變換不是直接的3D點(diǎn)變換，而是需要在新幀中計(jì)算CP。 使用Hesse平面表示，可以將錨坐標(biāo)系{A}中表示的平面映射到局部坐標(biāo)系{L}中，如下所示：

圖2：在局部{L}坐標(biāo)系中看到的最近點(diǎn)平面表示的圖示視圖

6.點(diǎn)到平面的壓縮

為了找到對(duì)應(yīng)于平面的無序點(diǎn)云的子集，可以使用RANSAC或其他平面分割方法。

首先將提取的點(diǎn)子集壓縮為本地CP并匹配可用于優(yōu)化的協(xié)方差。可以從制定加權(quán)最小二乘優(yōu)化問題開始，尋求最小化提取點(diǎn)與本地CP測(cè)量值Lп之間的點(diǎn)到平面距離：

計(jì)算最終局部最近點(diǎn)的協(xié)方差矩陣：

主要結(jié)果

A. LiDAR慣性模擬器

為了評(píng)估所提出系統(tǒng)的可行性，開發(fā)了定制的LiDAR
IMU模擬器。創(chuàng)建了2D平面圖并垂直拉伸以創(chuàng)建曼哈頓世界環(huán)境（為清楚起見，注意到CP表示可以處理任意平面方向）。一組有序的3D控制點(diǎn)用于通過環(huán)境創(chuàng)建3D樣條軌跡。使用分析樣條分化，可以在軌跡上的任何時(shí)間獲得真實(shí)的IMU測(cè)量。在給定的LiDAR感測(cè)頻率下，使用由角分辨率和垂直天頂角定義的固有LiDAR傳感器模型生成光線。然后，生成的光線與環(huán)境中的所有平面相交，并找到所有光線平面交叉點(diǎn)。最后一步通過強(qiáng)制執(zhí)行每條光線只應(yīng)擊中最接近LiDAR幀的平面來執(zhí)行因遮擋而不應(yīng)生成的交叉點(diǎn)的無效。

表1：模擬中使用的現(xiàn)實(shí)參數(shù)

B.蒙特卡洛仿真

在不同的LiDAR噪聲值下進(jìn)行了80次LIPS系統(tǒng)的Monte-Carlo模擬，其結(jié)果如圖4和表2所示。 所提出的CP表示和錨平面因子能夠在不同水平的LiDAR傳感器噪聲下以高精度定位在平面環(huán)境中。 模擬是在實(shí)時(shí)完成的，平面對(duì)應(yīng)已知并使用iSAM2解決。 朝向開始的大的非零定向誤差是由于傳感器在初始化之后保持靜止一段時(shí)間，僅有少量遠(yuǎn)離平面約束定向。

圖3：使用3D點(diǎn)作為輸入的競(jìng)爭基線的平面分割精度。地面真值深度圖或推斷深度圖(由基于DNN的系統(tǒng))作為它們的輸入。PlaneNet優(yōu)于使用推斷深度圖的所有其他方法，PlaneNet甚至比其他使用地面圖的方法還要好。

表2：在不同的LiDAR噪聲值下，平均RMSE超過80次蒙特卡羅模擬

由于進(jìn)出房間，模擬軌跡的開始具有有限量的環(huán)閉合，導(dǎo)致估計(jì)誤差增加，如通常在測(cè)距系統(tǒng)中看到的那樣（參見圖4）。 在300秒之后，軌跡重新進(jìn)入長走廊并返回到起始位置，重新觀察先前看到的走廊部分。 如圖4所示，具有先前平面的循環(huán)閉合迅速將估計(jì)誤差減小到零。

圖4：通過3D環(huán)境生成180米長的模擬軌跡。 原始的二維平面圖（底部）已被擠壓，并且樣條曲線適合控制點(diǎn)以生成完整的軌跡。 軌跡從左上角開始，在距離地板不同高度的房間內(nèi)進(jìn)出，然后最終重新進(jìn)入走廊并返回起始位置。

C.平面表示比較

CP表示在其四元數(shù)對(duì)應(yīng)物上產(chǎn)生了改進(jìn)的準(zhǔn)確度。雖然此處顯示的結(jié)果是使用iSAM2求解器生成的，但發(fā)現(xiàn)在完整批次優(yōu)化期間，與CP相比，四元數(shù)表示收斂速度較慢。對(duì)這些結(jié)果的猜想是，基于CP的測(cè)量模型具有比四元數(shù)參數(shù)化更好的線性高斯近似，因此提供了更好的數(shù)值性能。

D.真實(shí)世界實(shí)驗(yàn)

在這個(gè)測(cè)試中，平面物體放置在LiDAR傳感器周圍，以便于RANSAC提取，避免退化運(yùn)動(dòng)[38]，并確保LiDAR在所有自由度上完全受約束。 使用以10Hz工作的八通道Quanergy
M8 LiDAR，其中Microstrain
3DM-GX3-25 IMU連接到以500Hz工作的LiDAR底部。 手動(dòng)估算了LiDAR到IMU的外部轉(zhuǎn)換，但這可以很容易地添加到因子圖中進(jìn)行在線估算。 為了評(píng)估估計(jì)漂移，傳感器單元在平面表面前移動(dòng)并返回到相同的起始位置。在30米軌跡距離之后，起始和終止姿勢(shì)之間的差異為1.5厘米，對(duì)應(yīng)于軌跡長度上的0.05％誤差。

圖5：所提出的定位操作的實(shí)驗(yàn)環(huán)境（左）和平面的重建深度圖（右）

總結(jié)

以上是生活随笔為你收集整理的3D惯导Lidar仿真的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。