UWB/LiDAR-SLAM 緊耦合定位系統(tǒng)

超寬帶(UWB)定位技術是近年來得到廣泛應用的無線定位技術，由于其系統(tǒng)復雜度低、時間分辨率高、抗多徑效應強、信號穿透性強等優(yōu)點，被應用于室內(nèi)定位技術研究。UWB定位精度由UWB的脈沖寬度和響應頻率決定。UWB脈沖寬度僅為納秒或亞納秒級，具有超高帶寬特性，從而保證發(fā)射出超低的信號功率譜密度，理論上可以達到厘米級。但在復雜的室內(nèi)環(huán)境中，障礙物(行人、車輛、墻柱等)會引起UWB脈沖信號在傳播過程中發(fā)生反射、折射和穿透，導致信號有一定的衰減、時間延遲和較大的傳播距離，這些因素又會造成基于定位錨點與移動標簽之間信號強度測量或時間測量的距離估計有偏差，這種偏差稱為非視距(NLOS)誤差。

NLOS誤差的干擾會顯著降低UWB系統(tǒng)的定位精度和穩(wěn)定性，嚴重時甚至導致無法測距定位，這與UWB應用于室內(nèi)高精度定位的期望嚴重不符。因此，如何在NLOS環(huán)境下有效識別和抑制NLOS誤差，以提高定位算法的精度和穩(wěn)定性，成為室內(nèi)高精度定位的關鍵。

對于存在NLOS誤差的室內(nèi)定位場景，傳統(tǒng)的NLOS識別技術大致分為三類，即基于距離估計的方法，基于信道統(tǒng)計的方法和基于位置估計的方法。

基于距離估計的方法通過將距離估計的方差與預定義閾值進行比較或使用概率密度函數(shù)（ PDF ）來區(qū)分視距（LOS）和NLOS誤差。然而，這種方法不可避免地增加了由于距離采集而導致的時間延遲，也受到先驗分布函數(shù)的要求和選擇合適閾值的困難的限制。

基于信道統(tǒng)計的方法通常使用假設檢驗或分類理論，利用從信道脈沖響應（CIR）獲得的幅度（如接收信號強度（RSS）、接收信號的最 大幅度、功率差和功率比）和時間（如到達時間（TOA）、均方根延遲擴展、峰值至前置延遲、上升時間、平均額外延遲和最 大額外延遲）統(tǒng)計數(shù)據(jù)來區(qū)分 LOS 和 NLOS 誤差，并提取信道特征參數(shù)，如偏度、峰度和核函數(shù)以進行識別。然而，這些方法需要對多個統(tǒng)計數(shù)據(jù)的分布進行建模，因此需要創(chuàng)建和頻繁更新大型訓練數(shù)據(jù)庫，而 NLOS 數(shù)據(jù)的收集通常非常繁瑣和密集。

基于位置估計的方法可在定位過程中識別 NLOS 誤差。在有效測量冗余的情況下，可以通過比較不同距離值子集的位置估計結果來識別 NLOS 誤差。然而，當有多個測量值存在NLOS誤差或冗余測量值不足（二維定位至少需要3個測量值）時，該方法無法可靠地識別NLOS測量值，因此不適用于室內(nèi)錨點放置稀疏的復雜定位場景。

如果事先知道用戶的位置信息，也可以結合環(huán)境數(shù)據(jù)（衰減因子和幾何形狀）來識別NLOS誤差。Jo等人利用二維地圖進行信號射線追蹤，從而準確掌握無線信號傳播過程，識別NLOS信號并估計NLOS引起的誤差。然而，由于計算復雜度高，且需要提前了解環(huán)境中物體的介電常數(shù)、邊界條件等物理屬性，通常無法實現(xiàn)實時射線追蹤。

Suski等和Zhu等通過建立室內(nèi)靜態(tài)環(huán)境的誤差指紋數(shù)據(jù)庫，提高了UWB定位精度。王等采用手工繪制的二維地圖匹配線段信息進行NLOS識別，并根據(jù)遮擋情況調(diào)整UWB測距值的權重，有效消除了NLOS誤差的影響。但生成環(huán)境地圖所需的初步測量對于大型建筑物來說可能是一項艱巨的任務，并且如果空間發(fā)生任何變化，包括人或物體的移動，相應的定位誤差地圖或數(shù)字地圖就不再有效。因此，這些方法由于缺乏適用于所有環(huán)境的附加信息而不實用，并且動態(tài)環(huán)境阻礙了它們的實時實施。

n  UWB和 LiDAR-SLAM融合

利用LiDAR點云的NI方法可以高效準確地識別UWB NLOS誤差，從而提高實際場景中UWB測距定位的性能。結合NI和REKF的TC集成方法比其他對比方法獲得了更好的定位效果和魯棒性

通過 LiDAR-SLAM 生成的 LiDAR 點云地圖與來自 UWB 錨點的位置信息相結合，通過實時障礙物檢測和 NLOS 識別 (NI) 區(qū)分視距 (LOS) 和 NLOS 測量。此外，為緩解長期SLAM中定位誤差的積累，利用UWB LOS測量和LiDAR-SLAM定位信息，構建了改進的UWB/LiDAR-SLAM TC定位模型。采用魯棒擴展卡爾曼濾波器（REKF）進行參數(shù)求解，抑制UWB粗誤差的影響，提高了集成系統(tǒng)的魯棒性和定位性能。

由于三維（3-D）傳感器技術的快速發(fā)展，大型建筑物的環(huán)境地圖可以通過傳感器快速準確地獲取，例如光檢測和測距（LiDAR）、紅綠藍深度（RGB-D）相機和立體相機[。與相機相比，基于LiDAR的同步定位和地圖構建（SLAM）可以直接、準確地提供環(huán)境中結構信息的密集三維點，并且受光線和天氣的影響較小，并且由于其檢測范圍遠、精度高和魯棒性強而被廣泛應用于三維建筑物的室內(nèi)地圖構建 。

通常，LiDAR-SLAM通過配準連續(xù)幀的點云來估計幀間姿態(tài)變換矩陣，然后獲得軌跡并構建環(huán)境地圖。因此，環(huán)境地圖構建質量的關鍵在于LiDAR六自由度（6-DOF）姿態(tài)估計的準確性。

傳統(tǒng)的LiDAR-SLAM多采用基于迭代最近點（ICP）和正態(tài)分布變換（NDT）的算法或其變體進行幀間姿態(tài)估計。ICP通過迭代匹配相鄰幀的最近點并最小化對應特征點之間的距離來估計兩幀點云之間的姿態(tài)變換矩陣。但該類算法對初值敏感，初始化不正確會導致收斂到真實解的吸引盆之外的局部極小值，并且收斂速度慢。作為ICP的替代，NDT算法不根據(jù)對應點的特征進行匹配。該算法將底層掃描表示為一組高斯分布，將表面掃描局部建模為PDF，并通過優(yōu)化方法獲得最 大化概率密度和的最優(yōu)變換參數(shù)。與ICP算法相比，該算法的運算速度和魯棒性有較大提高。然而，NDT 將原始點云離散化為單元，而單元大小的定義會顯著影響最終的地圖質量 。

LiDAR 里程計和地圖繪制 (LOAM) 是當前最 先進的 LiDAR-SLAM 解決方案，在 KITTI 視覺基準排名中，其旋轉和平移誤差得分較高 ,LOAM是一種基于線面特征的全局地圖跟蹤方法，通過提取線和面特征將連續(xù)幀與全局地圖的一致性關聯(lián)起來，通過執(zhí)行高頻里程計線程和低頻建圖線程，可以解決復雜的SLAM問題，這些并行線程大大提高了SLAM技術的實時性。LOAM能夠在室內(nèi)外大范圍環(huán)境中以可接受的精度進行高效建圖，但由于缺乏閉環(huán)檢測，誤差積累不可避免。輕量級地面優(yōu)化的LOAM（LeGO-LOAM)采用地面分割和點云聚類，使提取的特征點更加有效，同時在LOAM中添加優(yōu)化的后端，并使用貝葉斯樹（iSAM2）閉環(huán)模塊進行增量平滑和建圖，抑制長期漂移，提高了LOAM的計算效率和精度。然而，上述兩種方法在判別性結構較差的場景中容易受到運動退化的影響，性能較差。

考慮到實際場景中單個傳感器的脆弱性，以及UWB與LiDAR傳感器的互補性（UWB在開闊場景中表現(xiàn)更佳，而LiDAR-SLAM更適合于UWB表現(xiàn)不佳的復雜場景），將UWB與LiDAR -SLAM結合起來，應用信息融合的方法，充分利用和結合兩種定位系統(tǒng)的優(yōu)勢，實現(xiàn)高精度、高普適性、強魯棒性、連續(xù)工作的室內(nèi)定位系統(tǒng)。一方面，LiDAR-SLAM可以提高UWB定位系統(tǒng)在復雜場景下的可靠性和可用性。另一方面，作為一種遞歸導航方案，LiDAR-SLAM導航誤差隨著移動距離的增加逐漸發(fā)散。LiDAR-SLAM與UWB的融合有助于抑制SLAM的誤差積累。

在大型建筑或時變環(huán)境等復雜環(huán)境下，UWB信號受NLOS誤差影響較大，豐富的幾何特征使LiDAR-SLAM能夠提供準確、魯棒的位姿估計與建圖結果。在先進的LiDAR-SLAM算法LeGO-LOAM的基礎上，提出并實現(xiàn)了基于LiDAR點云算法的UWB NLOS識別。該方法將UWB錨點的位置信息與LeGO-LOAM生成的環(huán)境地圖相結合，通過高效、精準地對錨點視線方向進行障礙物檢測與NLOS識別，實時區(qū)分LOS與NLOS測量，提高UWB數(shù)據(jù)質量。該方法不需要前期繁瑣的數(shù)據(jù)收集工作與訓練階段，具有良好的普適性，能很好地應對環(huán)境中動態(tài)障礙物的干擾。實驗結果表明，該NLOS識別算法在不增加大量計算量的情況下，是合理有效的。

為了抑制LiDAR-SLAM的誤差積累，同時獲得世界坐標系下的定位結果，提出一種改進的UWB/LiDAR-SLAM緊耦合定位系統(tǒng)，以LiDAR點云識別的UWB LOS測量值和LeGO-LOAM的定位結果作為集成系統(tǒng)的輸入。考慮到UWB測量值除受NLOS傳播影響外，還受信號多徑效應、強度衰減等因素影響，容易產(chǎn)生較大的粗略誤差，采用魯棒擴展卡爾曼濾波器（REKF）進行參數(shù)解算，通過降低異常值權重，有效抑制異常測量值對濾波結果的影響。動態(tài)定位實驗證明了所提NLOS識別與REKF相結合的緊耦合集成方法的準確性和魯棒性。