摘要

????????定位和導航的需求不斷增長。目前，全球導航衛星系統（GNSS）被認為是全球定位服務的重要手段。然而，由于嚴重的信號衰減、反射和阻塞，其在城市地區的應用受到了限制。慣性導航系統（INS）可以在短時間內提供高精度的導航輸出，但其精度存在誤差累積的問題，特別是在配備低成本的2微納制造的（MEMS）慣性測量單元（IMUs）時。此外，光檢測和測距（LiDAR）在車輛中越來越普遍，它可以檢測環境中豐富的幾何信息，從而進行自我運動估計。針對利用這些車載技術的互補特點在城市環境中導航，提出了一種緊密耦合的多GNSS精確點定位（PPP）/INS/激光雷達融合系統。我們還開發了一種激光雷達滑動窗口平面特征跟蹤方法來進一步改進導航精度和計算效率。在不同的GNSS觀測條件下的車輛實驗中，評價了所提出的集成系統的性能。結果表明，我們提出的GNSS/INS/LiDAR集成可以在具有GNSS挑戰性的環境中保持亞米水平的水平定位精度，與傳統的GNSS/INS集成相比，其提高了（73.3%，59.7%，64.2%）。此外，該平面特征跟蹤方法在精度和效率方面都優于傳統的點對線和點對平面掃描匹配方法。

I. INTRODUCTION

????????在一些位置驅動的物聯網（IoT）應用程序中，高精度導航和連續導航解決方案已經變得至關重要，比如自動駕駛的[1]-[3]。然而，在復雜的場景[4]中，一個獨立的傳感器通常很難滿足高精度導航的需求。因此，多傳感器融合可以利用異構傳感器的互補特性，可能是保證城市環境[5]導航性能的可行解決方案。

????????目前，精確點定位（PPP）技術已經成為一種很有前途的提供全天候定位服務的方法，特別是在衛星軌道和時鐘[6]的精度顯著提高之后。不同于差分全球導航衛星系統（GNSS）技術，PPP可以有效、靈活地實現相當的定位精度。

????????隨著多GNSS星座的快速發展，采用GPS、BDS、格拉斯和伽利略觀測的多GNSS 的PPP比GPS的PPP [7]具有更高的精度和更快的再收斂速度。當使用來自不同系統的GNSS觀測時，存在系統間偏差（ISBs），應該被考慮在內[8]。然而，PPP在衛星可用性差或弱星座幾何[9]中容易退化。同時，慣性導航系統（INS）可以在不需要外部支持[10]的情況下提供高頻、連續的導航輸出。因此，一些研究將INS與GNSS相結合，以提高在露天天空和城市環境[11]-[14]中的定位性能。其中，GNSS和INS的松散耦合集成由于其簡單性而被廣泛應用，它融合了INS測量的位置[11]和GNSS測量的位置[11]。但是，當子系統無法工作[12]時，導航解決方案將是不連續的和脆弱的。在[13]中，緊密耦合的PPP/INS融合已被證明在準確性和魯棒性方面優于松散耦合的融合。

????????除了提高導航精度外，INS還可以幫助恢復GNSS數據間隙，并以緊密耦合的方式通過[14]縮短模糊性重新初始化的時間。然而，在GNSS中斷時，特別是在配備低成本微機電系統（MEMS）慣性測量單元（IMUs）時。因此，在具有稀疏GNSS觀測的城市環境中，從其他傳感器中進行的改進是非常可取的。

????????光檢測和測距（激光雷達）作為一種外感受傳感器，可以檢測到詳細的結構信息，用于自我運動估計。大量的研究工作已經致力于解決激光雷達掃描之間的相對姿態，這是基于激光雷達的導航解決方案的基本概念。迭代最近點（ICP）算法[15]及其變體[16]是最常用的估計相對姿態變化的方法。然而，這些方法依賴于初始值，在具有垂直稀疏和圓形結構的環境中可能會失效。為了解決這一問題，一些研究將原始激光雷達點云轉換為更有效的表示方法，如線段[17]、車道標記[18]和曲率函數[19]。以至于在建設方面殘差、點對點距離和點對點平面距離由于其有效性和簡單性，在以往的實現中得到了廣泛的應用。在眾所周知的激光雷達里程計和建圖（LOAM）框架[20]中，提取低曲率或高曲率的邊緣和平面特征來構造兩次掃描的約束（點到平面和點到線距離）。這些幾何約束是有效的，但當它們跨多個連續的激光雷達幀執行時，它們將非常耗時。受視覺特征跟蹤的啟發，[21 Bundle Adjustment for Lidar Mapping]和[22]的作者在多次掃描中提取和跟蹤平面特征，這可以克服典型的幀對幀跟蹤方法的次優性。此外，在[23]中提出了一種新的自適應體素化方法來有效地搜索平面特征對應關系。近年來，深度學習方法[24]在點云處理中也引起了廣泛的關注。但這些數據驅動的方法是否足夠有效和通用，仍然值得探索。

????????基于三維激光雷達測量的成功處理，GNSS、INS和激光雷達的融合成為可能，并迅速增殖[25]-[31]。正如[25]和[26]所研究的那樣，高頻GNSS/INS姿態估計分別被反饋到激光雷達模塊中，作為精確掃描匹配和森林項建圖的先驗值。基于擴展卡爾曼濾波器（EKF），[27 INS/GPS/LiDAR Integrated Navigation System for Urban and Indoor Environments Using Hybrid Scan Matching Algorithm]采用了一種創新的混合掃描匹配算法，進一步提高了松散耦合GPS/INS/LiDAR集成的精度和穩定性。在兩個相鄰時期之間的GPS位置和激光雷達姿態估計提供了周期性的修正來抑制INS的發散。此外，[28]提出了一種基于優化的平滑映射激光雷達慣性里程計（LIO-SAM），該方法通過融合IMU預積分因子、激光雷達測程因子和可選GPS位置因子得到最優估計。

????????為了避免松散耦合融合[29]中的信息丟失，提出了一種緊密耦合RTK/INS/LiDAR集成系統，用于GNSS觀測條件[30]下的導航。利用差分GNSS技術對激光雷達特征的全局坐標進行了精確的預先登記，并在GNSS退化環境中提供了有效的位置約束。此外，[31“GIL: a tightly? coupled GNSS PPP/INS/LiDAR method for precise vehicle navigation]最近的一項工作將[20 LOAM]中的掃描匹配模塊擴展到多幀，并開發了一個基于EKF的GNSS/INS/LiDAR估計器，可以在城市環境中保持米級定位精度。在滑動窗口中每兩個激光雷達幀之間的掃描匹配將導致沉重的計算負擔。此外，幀間間隔越大，導致點云之間的相似性較低，難以建立準確的數據關聯。

????????以往關于GNSS、IMU和LiDAR融合的工作主要使用GNSS位置或集中于松散耦合積分。在這篇文章中，我們提出了一種緊密耦合的多GNSS PPP/INS/激光雷達融合方法，以實現在城市環境中精確和連續的導航。原始測量從GNSS載波相位和偽距觀測，MEMS-IMU和提取的激光雷達平面特征通過一個集中的EKF在觀測水平上直接融合。

????????此外，受[23]和[32]的啟發，開發了一種改進的平面特征提取、平面特征合并和數據關聯等改進的平面特征跟蹤方法，以進一步提高計算效率和導航精度。在GNSS部分阻塞和GNSS困難環境下進行了車輛實驗，以評估該系統的有效性。

本文的主要貢獻可以總結如下：

●據我們所知，這是第一次原始GNSS測量（即偽距和載波相位測量），MEMS-IMU數據和激光雷達平面特征緊密融合在一個一致的基于濾波器的框架，這可以有效地克服獨立傳感器的缺點，避免信息損失松散耦合方案。

●提出了一種具有多種檢查機制的激光雷達滑動窗平面特征跟蹤方法，并將其引入到該集成系統中。與傳統的使用點對點和點對點平面測量的掃描匹配方法相比，該方法在效率和精度方面都表現得更好，這對實時傳感器融合至關重要。

●在不同GNSS觀測條件的城市環境下評估了該系統的導航性能，并利用GNSS和點對點測量[31]與GNSS/INS集成和GNSS/INS/LiDAR集成進行了仔細比較。此外，我們還對平面特征跟蹤方法進行了詳細的實時效率分析。

II. LIDAR PLANE-FEATURE TRACKING 雷達的平面特征跟蹤

?????????高魯棒性和高效性對于處理原始激光雷達點云至關重要。然而，原始激光雷達點云的數量通常是巨大的，在實際應用中單獨處理每個點是不必要的和計算上不可行的。同時，單個點所包含的信息非常有限，很難在多個激光雷達幀之間建立穩定的數據關聯。為了應對這些挑戰，我們開發了一種改進的平面特征跟蹤算法，具有多種檢查機制，以在滑動窗口中的多個激光雷達幀上有效地跟蹤三維環境平面特征。根據[32]中的基本思想，該平面特征跟蹤方法可分為三個步驟：特征提取、特征合并和數據關聯，這將在下一節中詳細介紹。根據[33]，每個平面特征都可以由法向量和中心點組成的6自由度的（6-DOF）向量表示。為方便起見，以下公式中的平面特征簡稱為PF。

A. Plane feature extraction

????????正如[20]中眾所周知的LOAM在[20]中所做的那樣，我們首先從原始點云中提取具有低局部平滑度的點作為候選點。然后，對于每個候選點，搜索并選擇有限距離內的周圍點，在圖1左側用藍色標記。?

?一旦所選點可用，就可以通過[34]來計算中心點(c)：

?根據[34]，在平面上采樣的單個點滿足以下約束：

Ax+By+Cz +1 =? 0

n=[A B C]即為平面的法向量，為平面上的矢量

通過將所有點疊加在同一平面上，可以構造一個最小二乘問題來求解法向量為(3)：

為了在提取中剔除可能的病態平面，我們計算了從所有點到該平面的距離之和，并排除了那些大于閾值的距離。

?

?該閾值的經驗值設置為0.4 m [35]。雖然也有一些其他的方法通過計算平面[22]的不確定性來過濾非條件平面，但我們的方法同時考慮了效率和穩定性。在(1)和(3)的基礎上，我們可以得到經過初步篩選的所有平面特征的法向量和中心點。然后，提取出的平面特征將形成一個新的平面點云，這將成為后續平面特征合并的基本操作對象。請注意，我們建議kd-tree在所有搜索過程中進行快速索引。

?

B. Plane feature merging

?????????在以往的平面特征提取中，平面由候選點和一定數量的周圍點決定。這種方法可能會導致以下一些問題： 1)重復性和冗余性的幾何信息。特別是當激光雷達掃描結構環境時，不同的平面特征通常代表相同的表面，如墻壁和廣告牌。2）可靠性和穩定性較低。為了節省計算成本，選擇的周圍點的數量通常小于20個，這可能導致脆弱和不良的平面。3)計算復雜度和內存需求較高。提取操作相對耗時，提取后的平面特征數量較大。如果它們直接在測量更新中使用，計算負擔仍然很大。

????????因此，迫切需要一種能夠減少冗余度和提高提取平面穩定性的合并操作。對于中選擇的每一個，kd樹被用來搜索它的鄰近樹。然后，我們可以構造以下殘差來確定這兩個平面是否可以被視為同一平面，我們選擇平面特征, 其相鄰特征為

?????????其中(5)中的第一項表示兩個平面之間的并行性，第二項和第三項為雙向距離檢查，分別表示從第一個中心點到第二個平面特征的距離和到第二個特征點到第一個平面的距離。如果殘差小于經驗閾值（法向差為0.2rad，距離差為0.4m），則將兩個平面特征合并，如圖1右側所示。此外，相應的中心點和法向量也將被重新計算在(1)和(3)合并之后。此過程將重復多次，以確保合并所有相同的平面特征。最后，只保留了具有較高局部穩定性和幀間重合的平面特征。

算法1：

輸入：給定一個傳入的激光雷達掃描

輸出：在滑動窗口的平面特征對應激光雷達幀

●使用系統預測姿態后進行去畸變[33]

●提取平面特征從原始激光雷達點云

1)計算中心點公式(1)

2)計算法向量公式(3)

3)殘差檢查如公式(4)

●合并平面特性在局部的特征平面

?1)為每個平面特性，找到最近的對應平面特征

2)構造殘差向量(5)

3)確定兩個平面特征是否代表相同的地標通過檢查并行性和雙向點對點距離

4)如果應該是相同的地標，合并兩個平面特征和重新計算范數向量和中心點第二-A

5)重復多次以確保所有同一平面合并

●構建平面數據關聯跨多個幀滑動窗口

1)在最古老的每個平面特征幀，找到最相鄰的平面特征

2）將殘差向量構造為(6)

3)如果殘差大于閾值，則剔除相關特征

4)執行RANSAC以進一步去除離群值

C. Plane data association

????????在滑動窗口中尋找多個激光雷達幀之間的平面特征對應關系。假設一個平面特征在最古老的激光雷達幀m中被i索引。它將被投影到另一個激光雷達幀，以找到其最近的平面特征的索引。根據[32]，我們可以通過檢查殘差來確定兩個平面是否代表相同的位置，可以用以下描述：?

?如果殘差大于經驗閾值（法向差為0.35rad，距離差為0.5m），我們將拒絕將平面特征與平面特征聯系起來。在獲得激光雷達幀之間的所有特征對應后，我們建議RANSAC[36]進一步去除時間跟蹤中的離群值。在算法1中簡要介紹了平面特征跟蹤方法。

III. TIGHTLY COUPLED GNSS/INS/LIDAR INTEGRATED SYSTEM

????????為了實現這個融合系統，我們選擇了一個EKF來融合來自GNSS、INS和LiDAR的原始測量數據。在本節中，我們首先描述緊密耦合的GNSS/INS/LiDAR集成的系統結構。然后，按照標準的EKF例程，進行濾波器描述、狀態預測、多GNSS PPP測量更新和激光雷達測量更新均按順序表示。

?A. System overview

????????圖2所示為所提出的GNSS/INS/LiDAR融合系統的系統結構。自給式慣性傳感器因其采樣率高、故障率低而被認為是核心傳感器。經過參數初始化和初始動態對齊[37]后，INS機械化可用于狀態預測。利用歐洲軌道確定中心（CODE）提供的精確軌道和時鐘產品，原始的GNSS載流子相位和偽距觀測將進行周期滑移檢測[38]和異常值檢測[39]。對于激光雷達部分，將處理原始的激光雷達掃描點，包括平面特征提取、平面特征合并和數據關聯，詳見第二節。如果有預處理過的GNSS或激光雷達觀測數據，則將依次執行相應的測量更新。最后，將反饋估計的IMU偏差，以糾正下一個IMU數據。?

B. State description

?完整狀態向量由INS狀態、PPP狀態和激光雷達狀態組成，可表示為：

?其中上標和e分別為body坐標系（b幀）和ECEF坐標系（e系），符號分別表示e幀的三維姿態、速度和位置誤差；在(8)中，在線估計加速度計偏置ba和陀螺儀偏置bg，并分別建模為白噪聲na和ng驅動的隨機行走過程[40]。

在PPP狀態參數化方面，我們遵循標準的多GNSS PPP處理[7]，并應用無電離層（IF）組合，以消除一階電離層延遲。對應的狀態向量可以寫成：

上式(9)第一項為GPS接收機時鐘，?

則分別代表北斗和Galileo相對于GPS系統的ISB。

ZWD表示天頂對流層延遲的濕分量的誤差，

?

?是三個衛星系統的中頻模糊度校正向量。關于PPP誤差狀態構建的更多細節請參考[40]。

????????利用多狀態約束卡爾曼濾波（MSCKF）[42]的基本思想，在過去的狀態向量中保持一個克隆的滑動窗口，用于構造激光雷達約束。請注意，提取的激光雷達平面特征將被邊緣化，并在[43]后從狀態向量中移除。因此，激光雷達的狀態可以寫成：

?

其中，l符號表示激光雷達坐標系（l-系），遵循右上移順序；分別表示激光雷達姿態和位置誤差；k為激光雷達姿態的預定義滑動窗口大小。（多對位置和姿態組成的）

C. State prediction

在此步驟中，IMU數據被用于狀態預測。INS連續動力學模型可以表示為：?

?

?上式坐標的項分別代表了姿態的導數、速度的導數、位置的導數。R為余弦矩陣，從base到地球e系得余弦矩陣。為地球的自轉角速度，g代表在地球e系得重力誤差，為陀螺儀的誤差，f為角速度的誤差值。

基于式（11），融合系統的完整過程模型可寫為：

?對于激光雷達部分，激光雷達在滑動窗口中的歷史姿態被視為沒有過程噪聲的常數。然后，可以根據（12）進行協方差傳播。每當接收到一個新的激光雷達掃描時，當前的激光雷達姿態將被增強到狀態矢量中。其初始姿態估計可以通過當前INS預測的姿態來計算：

?其中，符號（^）表示估計值。是IMU和激光雷達之間預先校準的外部參數，在處理[44]過程中被認為是恒定的。相應的增廣協方差可以表示為：

D. Multi-GNSS PPP measurement update?

在PPP處理中，IF組合被廣泛用于消除一階電離層延遲，它可以表示為：

?上式的P和L分別代表了偽距和載波相位觀測，為接收機(r)到衛星(s)的幾何距離；分別表示中頻組合中的波長和模糊度；分別為偽距和載波相位的測量噪聲；與頻率無關的對流層延遲(T)由干、濕分量組成，兩者都可以用各自的天頂延遲(ZHD，ZWD)和對應的映射函數()干濕[8]來表示。對于GPS、BDS和Galileo的多星座情況，偽距觀測的IF組合可以表示為：

?載波相位的值可以表示為：

?

?在構造IF組合之前，我們利用[37]中提出的動態DIA方法來拒絕總誤差。根據（18）和（19），多GNSS的PPP測量值的殘余向量可以寫成：

其中，下標INS和GNSS分別表示INS預測的GNSS測量值和原始GNSS測量值；為相應的測量噪聲；，為INS誤差狀態和PPP誤差狀態的雅可比矩陣，可參考[40]。

E. LiDAR measurement update

在第二節中，我們獲得了滑動窗口中多個激光雷達幀的相關平面特征。雖然該表示可以顯著加速平面特征跟蹤，但不能直接用于測量更新。其主要原因是平面特征的6-DOF表示比具有3-DOF的真實平面更冗余，這可能會導致額外的計算資源和數值不穩定性。因此，與[32]類似，平面特征的表示通過最近點來代替來重新參數化平面。最近點和平面特征和之間的相對關系可以表示為：

?此外，最近點CP可以用其法向量和距離標量d表示為：

以激光雷達幀m中的第i個平面特征和激光雷達幀n中對應的第j個平面特征為例，測量值 可以表示為：

?

?上式中R為平面特征的法向量由n幀特征的法向量和m幀特征之間的相對旋轉；

?

為平面特征的法向量和距離標量得到的距離標量以及幀與幀之間的相對平移。將當前估計的測量模型線性化，測量函數的殘差可以近似為：

?

?其中iH和HLiDAR分別為對應的第個平面特征的雅可比矩陣和涉及的激光雷達狀態。它們的具體形式可以參考附錄b。通過堆疊同一平面特征的所有關聯，我們有：

?受[43]的啟發，將平面特征誤差ψ投影到其左Null空間，使其與激光雷達狀態去相關。然后，（25）可以重寫為：

?為了去除在第II-C節中獲得的相關平面特征中的異常值，在計算出最終的rLiDAR和HLiDAR后，執行卡方檢驗[45?LIC-Fusion: LiDARInertial-Camera Odometry]來檢查先驗殘差：

?其中Q和Qw為ronghe 系統和測量噪聲的協方差矩陣。如果馬氏氏距離小于卡方分布的95%，則可以進行相應的激光雷達測量更新。殘差向量本質上是點-面距離，其噪聲經驗值設置為0.05m。

總結

以上是生活随笔為你收集整理的武汉大学《GNSS、INS和激光雷达的紧密耦合预积分，用于城市环境中的车辆导航》的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。