自動控制系統中的傳感器融合

傳感器融合的4個作用：

• 1、增加數據質量。比如減少噪聲；
• 2、增加可靠性。多傳感器互為備份；
• 3、估計預測狀態；
• 4、可增加被測范圍。相對于單個傳感器來說，多傳感器產生一個相關系統coherent system。

例1：測量加速度。將加速度傳感器放在桌上，因為存在噪聲，度數在9.8m/s2附近波動。多個傳感器取平均值可將噪聲降低。另外，多個傳感器還能增加可靠性。

例2：用兩種不同的傳感器，減少噪聲。手機中兩種用于測量方向的功能，磁體mag和帶有角速度傳感器陀螺儀的磁力計gyro。

磁體測量結果通過低通濾波器LPF可減少噪聲。兩個不同傳感器通過傳感器融合減少噪聲。

?例3：測量飛機速度。飛機上有兩種測量速度的傳感器方案，多個皮托管和GPS大氣風速模型估算空速。當一個皮托管不工作時依靠其他皮托管，所有皮托管都失效時，依靠GPS測速。

?例4：雷達測量船只速度。當有其他物體擋住被測船只時，可通過之前測量數據建立模型，估算船速和距離。

?例5：相機測距。一個相機無法測距，遠處的大物體和近處的小物體有相同的像素。

例6：利用慣性傳感器融合的方向估計，傳感器融合算法實例。將慣性測量單元mpu9250（包含三個傳感器，加速度計accelerometer、磁力計magnetometer、陀螺儀gyro），通過I2C總線連接到開源硬件arduion平臺，arduion通過usb連接到電腦matlab。

使用單個acc慣性傳感器存在的問題：

1、acc不止測量重力加速度，還對線性加速度敏感。對此問題的解決方法如圖，當線性加速度是系統已知動作時，可用系統模型估計方法消除偏差；

2、當acc沒有安裝在物體的重心處，還會受到離心力，影響數據。

使用單個mag傳感器存在的問題：

1、地球上不同地區的磁力線分布不同，磁針指示的位置不一定朝北；

2、mag會受到周圍磁體的干擾，分為硬磁體和軟磁體。硬磁體能夠產生磁場，如磁鐵和通電線圈，如圖藍色箭頭表示地球磁場方向、綠色箭頭表示硬磁體方向，他們的合成磁場為紅色箭頭，紅圓代表沒有硬磁體影響的磁場，隨著綠色箭頭的大小方向改變，實際測量結果軌跡為圓中心沿著綠色箭頭偏移的大小相同的圓；軟磁體是扭曲磁場分布的物體，比如任何金屬，當磁力計旋轉時，軟磁體對于測量結果的影響是讓正圓扭曲成了橢圓。

mag calibration校準系統的作用就是校準這些軟硬磁體的影響。綠圓表示完美系統磁場，紅圓表示在軟硬磁體共同影響下的實際測量磁場。測量實際磁場，再結合系統原磁場數據，通過矩陣反變換，得到矯正過的mag，這樣矯正過的mag測量的數據即為實際數據。

使用單個gyro慣性傳感器存在的問題：gyro工作原理如圖。

1、需要知道初始角度；2、誤差隨著積分不斷擴大。

兩種方向估計的方案：acc+mag 和 gyro，優缺點如圖

acc+mag：優點提供絕對測量值，缺點容易受到擾動；

gyro：優點提供相對測量值，缺點需要初始狀態和隨時間累計誤差。

兩種方案結合的算法：互補、卡爾曼濾波、其他濾波。原理都是，設置濾波器初始值，在過程中根據磁場和地球引力不斷糾正誤差漂移。

例7：融合GPS+IMU定位/測速/方向估計方案。matlab傳感器融合實例《pose estimation from asynchronous sensors》，比較不同傳感器組合測量下的位置估計。

IMU和GPS的傳感器融合算法使用EKF擴展卡爾曼濾波器。EKF適用于目標跟蹤。

例8：使用IMM濾波器跟蹤單個對象interacting multiple model filter。matlab傳感器融合實例《pose estimation from asynchronous sensors》。如圖是物體運動軌跡，分為3段共100s，200m/s直線勻速運動、10deg/s勻速轉彎、3m/s^2之線加速度運動。本例證明多（本例3個）傳感器濾波器IMM目標估計強于單個傳感器濾波器。

?濾波器性能評價指標：（3個方向的）歸一化距離normalized distance。多濾波器模型比單個濾波器噪聲更低（歸一化距離越接近零表示跟蹤性能越好）。

多濾波器模型原理，對不同的運動狀態用不同的濾波器。

單個濾波器的問題在于噪聲對于估計的影響。下圖是某單個濾波器（constant velocity）在不同噪聲設置下的歸一化距離，當噪聲設置低時，在勻速階段的誤差很小，在圓周和加速階段誤差大；當噪聲設置高時，在圓周和加速階段誤差減小，但勻速階段的誤差提高。這樣就失去了這個濾波器的特點（勻速時估計狀態準確），所以對于不同階段設置不同濾波器很有必要。

例9：如何一次跟蹤多個對象。實質是data association數據關聯問題和track maintenance跟蹤軌跡維護問題。困難：多個對象無法區分、對象活動重疊、對象數量一直變化。

原理如圖：首先觀察observations物體各種屬性，然后assignment將觀察數據分配給某個或多個軌道，分配算法比如GNN全局最近鄰global?nearest?neighbor和JPDA聯合概率數據互聯Joint Probability Distribution Adaptation，GNN適用于稀疏分布對象sparely distributed objects，JPDA適用于雜散分布對象cluttered objects。然后track maintenance判斷所有軌道是否存在，增加或刪除軌道。然后運行濾波器，與例8相同對每一個軌道使用組合濾波器IMM，跟蹤目標。gating選通與軌道極不相關的數據拋棄。

舉例，兩個獨立物體分別移動，方向如圖從下往上沿直線移動，中間路程很接近，。GNN算法表現：在稀疏時表現良好，當兩物體接近時，GNN跟蹤的軌道2丟失，并創建到軌道8才確認軌道。JPDA算法表現：一直維持兩個獨立軌道，雖然在兩個物體接近時軌道有錯誤，且算法消耗大。

?

例10：軌道級融合track-level fusion。

例11：IMU在自動駕駛中的應用。

百度Apollo自動駕駛定位方案

例12：無人機目標跟蹤

tukfiui

《汽車傳感器融合與非線性濾波器》課程安排

《Sensor Fusion and Non-linear Filtering for Automotive Systems》

你會學到什么

貝葉斯統計和遞歸估計理論基礎

描述和建模常見傳感器及其測量值

比較用于定位的典型運動模型，以了解在實際問題中何時使用它們

描述卡爾曼濾波器（KF）的基本特性，并將其應用于線性狀態空間模型

在Matlab中實現關鍵的非線性濾波器，以解決非線性運動和/或傳感器模型的問題

通過分析應用程序中的特性和要求，選擇合適的過濾方法

教學大綱

第1節-統計學導論和入門

第2節-貝葉斯統計（第1周）

第3節-狀態空間模型和最佳過濾器（第1周）

第4節-卡爾曼濾波器及其特性（第2-3周）

第5節-運動和測量模型（第2-3周）

第6節-非線性濾波（第4周）

第7節-粒子過濾器（第5周）

傳感器信息融合方向

New material involves extensions into service-oriented networks and data mining to expand applicability to the dynamic growing field of information technology; automated detection fusion that adapts decision thresholds based on the variable performance of multiple sensors; an introduction of particle filtering, which provides a look into the background theories that gave rise to this important direction in target tracking; random set theory, which has been completely rewritten to provide an extensive development of this generalization of Bayesian and non-Bayesian approaches; situation and impact assessment theory and concepts, which have been significantly extended to build on research directions that have been previously glossed over; new techniques in visualization that have been developed; and finally, a new chapter in commercial off-the-shelf (COTS) software tools, which provides the reader with a wealth of fusion research tools and techniques.

面向服務的網絡和數據挖掘的擴展，以擴大信息技術動態增長領域的適用性；自動檢測融合，根據多個傳感器的可變性能調整決策閾值；粒子濾波提供了一個背景理論，提供目標跟蹤這一重要方向的研究；改寫的隨機集理論，以提供這種貝葉斯和非貝葉斯方法的推廣的廣泛發展；形勢和影響評估理論和概念大大擴展；新可視化技術；新的軟件為讀者提供了融合研究的工具。

傳感器信息融合三個等級

可用于多傳感器數據的三種基本方案：（1）傳感器數據的直接融合；（2） 通過特征向量表示傳感器數據，隨后融合特征向量；或（3）對每個傳感器進行處理，以實現隨后組合的高級推斷或決策。

如果多傳感器數據相稱（即，如果傳感器測量相同的物理現象，如兩個視覺圖像傳感器或兩個聲學傳感器），則可以直接組合原始傳感器數據。原始數據融合技術通常涉及經典的估計方法如卡爾曼濾波。該領域的挑戰涉及密集目標環境、快速機動目標或復雜信號傳播環境（例如，涉及多徑傳播、同信道干擾或雜波）的情況。然而，對于動態性能良好（即動態可預測）的目標，在良好的信噪比環境中進行單目標跟蹤是一個簡單、易于解決的問題。

特征級融合涉及從傳感器數據中提取具有代表性的特征。特征提取的一個例子是漫畫家使用關鍵面部特征來表示人臉。這種在政治諷刺作家中流行的技巧利用關鍵特征喚起對著名人物的認可。有證據證實，人類利用基于特征的認知功能來識別物體。在多傳感器特征級融合的情況下，從多個傳感器觀測值中提取特征，并將其組合成單個串聯特征向量，作為模式識別技術（如神經網絡、聚類算法或模板方法）的輸入。迄今為止，目標識別一直由基于特征的方法主導，其中特征向量（傳感器數據的表示）被映射到特征空間，希望基于特征向量相對于先驗確定的決策邊界的位置來識別目標。流行的模式識別技術包括神經網絡、統計分類器和向量機方法。盡管有許多技術可用，但這些方法的最終成功取決于良好特性的選擇。好的特征在特征空間中提供了極好的類可分性，而壞的特征則導致多類目標的特征空間區域嚴重重疊。需要在這一領域進行更多的研究，以指導特征的選擇，并結合關于目標類的顯式知識。

決策級融合在每個傳感器初步確定實體的位置、屬性和身份后，結合傳感器信息。決策級融合方法的示例包括加權決策方法（投票技術）、經典推理、貝葉斯推理和Dempster–Shafer方法。這些領域相對不成熟，有許多原型，但很少有穩健的操作系統。該領域的主要挑戰是建立一個可行的規則、框架、腳本或其他方法知識庫，以表示態勢評估或威脅評估方面的知識。不幸的是，只有原始的認知模型可以復制這些功能的人類表現。在開發用于自動態勢評估和威脅評估的可靠和大規模的基于知識的系統之前，需要進行大量的研究。提供希望的新方法是使用模糊邏輯和混合架構，將黑板系統的概念擴展到分層和多時間尺度方向。

對于單傳感器操作，運籌學和控制理論的技術已被應用于開發有效的系統，即使對于復雜的單傳感器，如相控陣雷達。相比之下，涉及多個傳感器、外部任務約束、動態觀測環境和多個目標的情況更具挑戰性。迄今為止，在嘗試建模和合并任務目標和約束以平衡優化性能與有限資源（如計算能力和通信帶寬（如傳感器和處理器之間））以及其他影響方面遇到了相當大的困難。效用理論的方法正被應用于制定系統性能和有效性的衡量標準。正在為基于上下文的近似推理開發基于知識的系統。智能、自校準傳感器的出現將帶來重大改進，這些傳感器可以準確、動態地評估自身的性能。分布式網絡中心環境的出現，其中傳感資源、通信能力和信息請求非常動態，給4級融合帶來了嚴重挑戰。在這樣的環境中很難（或可能不可能）優化資源利用率。在最近的一項研究中，Mullen等人5應用了基于市場的拍賣的概念來動態分配資源，將傳感器和通信系統視為服務供應商，將用戶和算法視為消費者，以快速評估如何分配系統資源以滿足信息消費者的需求

多目標跟蹤

跟蹤大量目標的任務異常困難，難度與物體數量的平方成正比；因此，10個對象需要100倍的努力，10000個對象會增加1億倍的難度。這種組合爆炸是解決多目標跟蹤問題的第一個障礙。

基于雷達系統等傳感器的一系列位置報告，可以跟蹤單個物體的運動。要重建對象的軌跡，按順序繪制連續位置，然后畫一條線穿過它們，延伸這條線可以預測物體的未來位置。假設同時跟蹤10個目標，定期收到10份新位置報告，但這些報告沒有指示其對應目標的標簽。繪制10個新位置時，原則上，每個報告可以與10條現有軌跡中的任何一條相關聯。需要考慮每個可能的報告和軌道組合，使得所有n個目標問題的難度成正比于或按n^2的順序，這被表示為O(n^2)。

多年來，人們一直試圖設計一種性能優于O(n^2)的多目標跟蹤算法。一些方案在特殊情況下或在多目標跟蹤問題的某些情況下提供了顯著的改進，但它們保留了O(n^2)最壞情況下的行為。然而，空間數據結構理論spatial data structures的最新結果使得一類新的算法成為可能，用于將位置報告與跟蹤算法關聯，在大多數現實環境中，這種算法的伸縮性優于二次伸縮。在退化情況下，所有目標都密集聚集，無法單獨解析，因此無法避免將每個報告與每個軌跡進行比較。當每個報告平均只能與恒定數量的軌跡相關聯時，可以實現次二次縮放。

即使是單目標跟蹤也存在與位置測量固有的不確定性相關的某些挑戰。第一個問題涉及決定如何表示這種不確定性。粗略的方法是定義位置估計周圍的誤差半徑。這種做法意味著發現目標的概率均勻分布在三維球體的整個體積內。不幸的是，這種簡單的方法遠遠不是最優的。與許多傳感器相關的誤差區域是高度非球形的；例如，雷達傾向于提供精確的距離信息，但徑向分辨率相對較差。此外，人們期望目標的實際位置平均更接近平均位置估計值，而不是誤差體積的周長，這反過來表明，靠近中心的概率密度應該更大。處理不確定性的第二個困難是確定如何從多個測量值中插值目標的實際軌跡，每個測量值都有自己的誤差余量。對于已知具有恒定速度的目標（例如以恒定速度沿直線移動），有一些方法可用于計算射彈直線路徑，通過某種測量，這些方法最適合過去位置序列。這種方法的一個可取特性是，隨著報告數量的增加，它應始終收斂于正確的路徑，估計速度和實際速度之間的差值應接近零。但是，保留目標的所有過去報告并在每次新報告到達時重新計算整個軌跡是不切實際的。這種方法最終將超過計算時間和存儲空間的所有限制。

1960年，Kalman提出了一種解決一大類跟蹤問題的近似最優方法。他的方法被稱為卡爾曼濾波，涉及到噪聲測量的遞歸融合，以產生對感興趣系統狀態的準確估計。卡爾曼濾波器的一個關鍵特征是其狀態估計的平均向量和誤差協方差矩陣表示，其中協方差矩陣提供了與平均估計相關的誤差分布二階矩的估計（通常是保守的高估）。估計協方差的平方根給出了標準偏差的估計值。如果測量誤差序列在統計上是獨立的，則卡爾曼濾波器產生一系列保守的融合估計值，誤差協方差減小。

到目前為止，跟蹤過程中最重要的誤差來源是現有傳感器的分辨率有限。這一事實強化了人們的普遍看法，即有效跟蹤的主要障礙是傳感器報告的質量相對較差。大量目標的影響似乎是可控的，只需建造更大、更快的計算機。盡管研究界的許多人不這么認為

卡爾曼濾波

卡爾曼濾波（Kalman filtering）一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法。一句話概括卡爾曼濾波的思想：根據上一時刻的狀態，預測當前時刻的狀態，將預測的狀態與當前時刻的傳感器測量值（由于存在傳感器噪聲其值并不可信）進行加權更新，更新后的結果為最終的追蹤結果。簡單來說，卡爾曼濾波器是一個“optimal recursive data processing algorithm（最優化自回歸數據處理算法）”。對于解決很大部分的問題，它是最優，效率最高甚至是最有用的。Kalman濾波在測量方差已知的情況下能夠從一系列存在測量噪聲的數據中，估計動態系統的狀態。由于，它便于計算機編程實現，并能夠對現場采集的數據進行實時的更新和處理， Kalman濾波是目前應用最為廣泛的濾波方法，在通信、導航、制導與控制等多領域得到了較好的應用。

卡爾曼濾波是多源傳感數據融合應用的重要手段之一，為了扼要地介紹卡爾曼濾波的原理，此處形象地用毫米波雷達與視覺感知模塊融合目標位置的過程描述。舉一個簡單的例子，目前高級輔助駕駛系統（Advanced Driver Assistance System，ADAS）上，搭載有毫米波雷達和超聲波雷達模塊，兩者均能對障礙物車輛進行有效的位置估計判別。雷達利用主動傳感原理，發射毫米波，接收障礙物回波，根據波傳播時間計算角度距離。兩者均能識別出車輛位置，那么我們該如何融合信息，如何取舍，計算出具體的車輛位置呢？卡爾曼正是解決這個問題的方法之一。我們獲取的車輛位置在任何時刻都是有噪聲的，卡爾曼濾波利用目標的動態信息，設法去掉噪聲的影響，得到一個關于目標位置的好的估計。這個估計可以是對當前目標位置的估計(濾波)，也可以是對于將來位置的估計(預測)，還可以是對過去位置的估計(插值或平滑)。卡爾曼濾波就是這樣一個根據當前時刻目標的檢測狀態，預測估計目標下一時刻目標檢測狀態的一個動態迭代循環過程。

激光雷達可直接使用線性卡爾曼來進行障礙物跟蹤；而毫米波雷達則需要使用非線性卡爾曼來進行物體跟蹤。舉例：動手學無人駕駛（5）：多傳感器數據融合_10點43的博客-CSDN博客_無人駕駛多傳感器融合 ?https://blog.csdn.net/cg129054036/article/details/108038376

擴展的卡爾曼濾波

（1）在只有IMU數據時（此時GPS還未有輸出產生），IMU數據經過運動模型，得到預測狀態；然后預測狀態傳送回運動模型，繼續下一步預測；
（2）當有GPS數據產生時，上一時刻產生的預測狀態將會和接收到的GPS位置信息進行數據融合，得到修正后的狀態。然后再傳回運動模型，進行下一周期的運算。

舉例：動手學無人駕駛（6）：基于IMU和GPS數據融合的自車定位_10點43的博客-CSDN博客_gps和imu數據融合 ?https://blog.csdn.net/cg129054036/article/details/109964103

誤差卡爾曼濾波

誤差卡爾曼濾波數學基礎：四元數

論文：《Quaternion kinematics for the error-state Kalman filter》2017

使用誤差卡爾曼濾波的原因：

方向誤差狀態極小（和自由度有相同的參數數量），能夠避免過度參數化以及由此產生的協方差矩陣奇異的風險。

誤差狀態系統始終在接近原點的位置運行，因此遠離可能的參數奇異性，萬向節鎖定問題等，從而保證了線性化有效性始終不變。

誤差狀態量始終很小，這意味著所有二階項都可以忽略不計。 這使得Jacobian的計算非常容易和快速。

誤差狀態量動態變化緩慢，因為所有的 large-signal 動態變化已經被包含在名義狀態里了。因此，在卡爾曼濾波過程中，相較于預測過程，可以以更低的頻率來執行更新過程。

在文中作者對誤差狀態卡爾曼濾波還做了如下解釋：

在誤差狀態卡爾曼濾波中，會討論到三種狀態量：真值狀態，名義狀態和誤差狀態，真值狀態由名義狀態和誤差狀態組合表示（如線性組合，四元數乘積或矩陣乘積方式等）。其思想是將名義狀態視為 large-signal（以非線性方式可積分），將誤差狀態視為 small-signal（線性可積分并適用于線性高斯濾波）。

在使用IMU進行定位時，高頻IMU數據被積分到名義狀態中。名義狀態未考慮噪聲，結果它會積累誤差。誤差狀態通過誤差狀態卡爾曼濾波進行估計，包含了所有的噪聲和擾動。誤差狀態由小幅度的信號組成，其更新函數由線性動態系統定義，其動態、控制和測量矩陣均由名義狀態的值計算得出。誤差狀態卡爾曼修正是在其它傳感器信息（例如GPS，視覺等）到達時執行的。

相機和激光雷達數據融合

激光雷達和攝像頭的數據融合，實際是激光雷達點云投影在攝像頭圖像平面形成的深度和圖像估計的深度進行結合，理論上可以將圖像估計的深度反投到3-D空間形成點云和激光雷達的點云融合，但很少人用。原因是，深度圖的誤差在3-D空間會放大，另外是3-D空間的點云分析手段不如圖像的深度圖成熟，畢竟2.5-D還是研究的歷史長，比如以前的RGB-D傳感器，Kinect或者RealSense。兩種傳感器優缺點：圖像傳感器成本低，分辨率高（可以輕松達到2K-4K）；激光雷達成本高，分辨率低，深度探測距離短。可是，激光雷達點云測距精確度非常高，測距遠遠大于那些Infrared/TOF depth sensor，對室外環境的抗干擾能力也強，同時圖像作為被動視覺系統的主要傳感器，深度估計精度差，更麻煩的是穩定性和魯棒性差。所以，把激光雷達的稀疏深度數據和致密的圖像深度數據結合形成互補。

稀疏的深度圖如何upsample變得致密，這也是一個已經進行的研究題目，類似image-based depth upsampling之類的工作。還有，激光雷達得到的點云投到攝像頭的圖像平面會發現，有一些不反射激光的物體表面造成“黑洞”，還有遠距離的街道或者天空區域基本上是沒有數據顯示，這樣就牽涉到另一個研究題目，image-based depth inpainting / completion。

解決這個問題的前提是，激光雷達和攝像頭的標定和同步是完成的，所以激光雷達的點云可以校準投影到攝像頭的圖像平面，形成相對稀疏的深度圖。

傳感器融合層次：數據層和任務層。數據層直接將雷達3d云轉換為2d圖像，做圖像融合；任務層根據任務（障礙物檢測、定位等）將不同傳感器的檢測結果融合。不同傳感器獲取的數據類型：

攝像頭是RGB圖像的像素陣列；

激光雷達是3-D點云距離信息（有可能帶反射值的灰度值）；

GPS-IMU給的是車身位置姿態信息；

雷達是2-D反射圖。

數據層融合思路

先是坐標對齊，將雷達坐標轉換到相機坐標。由相機捕獲的圖像數據由(U,V)表示，激光雷達捕獲的3維點陣云用(X,Y,Z)表示，建立一個轉化矩陣M，將3維點(x,y,z)映射到2維點(u,v)

然后將激光點往像平面投影，得到投影到像平面的點云。

借助圖像檢測的框圖（前面提到的YOLOv3檢測）對點云實現過濾聚類。

對聚類點后處理。

激光雷達和攝像頭的數據融合，實際是激光雷達點云投影在攝像頭圖像平面形成的深度和圖像估計的深度進行結合，理論上可以將圖像估計的深度反投到3-D空間形成點云和激光雷達的點云融合，但很少人用。原因是，深度圖的誤差在3-D空間會放大，另外是3-D空間的點云分析手段不如圖像的深度圖成熟，畢竟2.5-D還是研究的歷史長，比如以前的RGB-D傳感器，Kinect或者RealSense。

其他深度學習方法：

?“Propagating Confidences through CNNs for Sparse Data Regression“, 提出normalized convolution （NConv）layer的改進思路，訓練的時候NConv layer通過估計的confidence score最大化地融合 multi scale 的 feature map

ICRA的論文High-precision Depth Estimation with the 3D LiDAR and Stereo Fusion，只是在合并RGB image和depth map之前先通過幾個convolution layer提取feature map

法國INRIA的工作，“Sparse and Dense Data with CNNs: Depth Completion and Semantic Segmentation”作者發現CNN方法在早期層將RGB和深度圖直接合并輸入性能不如晚一些合并（這個和任務層的融合比還是early fusion），這也是它的第二個發現，這一點和上個論文觀點一致。

歸納深度學習的工作：都是從暴力訓練的模型開始，慢慢加入幾何約束，聯合訓練。拖延RGB和depth合并的時機是共識，分別訓練feature map。

傳統方法

把任務看成一個深度圖內插問題，方法類似SR和upsampling，只是需要RGB圖像的引導image-guided。實現這種圖像和深度之間的結合，需要的是圖像特征和深度圖特征之間的相關性，這個假設條件在激光雷達和攝像頭傳感器標定和校準的時候已經提到過，這里就是要把它應用在pixel（像素）/depel（深度素）/surfel（表面素）/voxel（體素）這個層次。基本上，技術上可以分成兩種途徑：局部法和全局法。這樣歸納，和其他幾個經典的計算機視覺問題，如光流計算，立體視覺匹配和圖像分割類似。圖像濾波的歷史：均值濾波-》高斯濾波-》中值濾波-》Anisotropic Diffusion -》Bilateral濾波（等價于前者）-》Non Local Means濾波-》BM3D，這些都是局部法。Joint Bilateral Filtering，還有著名的Guided image filtering，在這里都可以發揮作用。全局法就是MRF，CRF，TV（Total variation），dictionary learning 和 Sparse Coding之類。

激光雷達的深度圖是稀疏的

目的是從輸入的稀疏深度圖生成密集的深度圖。深度完整（depth completion）的目標是從稀疏、不規則、映射到2D平面的點云生成密集的深度預測。通常，卷積神經網絡（CNN）可在規則網格上處理數據，即普通相機數據。設計用于稀疏和不規則間隔輸入激光雷達數據的CNN仍然是一個開放的研究問題。

“HMS-Net: Hierarchical Multi-scale Sparsity-invariant Network for Sparse Depth Completion ”2018

常規做法稀疏不變卷積的CNN只能逐漸對特征采樣后失去大量分辨率；提出的稀疏不變編碼器-解碼器網絡可以有效地融合來自不同層的多尺度特征做深度完整。有效利用多尺度特征，提出3種稀疏性不變（sparsity-invariant）操作。可以合并其他RGB特征，進一步提高深度完整系統的性能。

“Sparse and noisy LiDAR completion with RGB guidance and uncertainty”2019

提出新方法，可以精確地完整化RGB圖像引導的稀疏激光雷達深度圖。一方面，單目深度預測方法無法生成絕對且精確的深度圖。另一方面，基于激光雷達的方法仍然明顯優于立體視覺方法。

提出框架，同時提取全局和局部信息生成適當的深度圖。簡單的深度完整并不需要深度網絡。但是，該文提出一種融合方法，由單目相機提供RGB指導，利用目標信息并糾正稀疏輸入數據的錯誤，這樣大大提高了準確性。此外，利用置信度掩碼考慮來自每種模態深度預測的不確定性。

“3D LiDAR and Stereo Fusion using Stereo Matching Network with Conditional Cost Volume Normalization”2019

主動和被動深度測量技術的互補特性促使激光雷達傳感器和立體雙目相機融合，以改善深度感知。作者不直接融合激光雷達和立體視覺模塊來估計深度，而是利用帶兩種增強技術的立體匹配網絡：激光雷達信息的輸入融合和條件成本容積歸一化（Conditional Cost Volume Normalization，CCVNorm）。 所提出的框架是通用的，并且緊密地與立體匹配神經網絡中成本容積組件集成。

《Scalable Primitives for Generalized Sensor Fusion in Autonomous Vehicles》

在自動駕駛領域，深度神經網絡在感知、預測和規劃任務中得到應用。隨著自動駕駛汽車（AVs）越來越接近生產水平，多模式傳感器輸入和不同傳感器平臺得異構車隊在行業中變得越來越普遍。然而，神經網絡結構通常針對特定的傳感器平臺，對輸入的變化不具有魯棒性，這使得規模化和模型部署問題特別困難。此外，大多數玩家仍然將優化軟件和硬件的問題視為完全獨立的問題。

這里作者提出了一種端到端架構，即廣義傳感器融合（GSF），其設計方式是傳感器輸入和目標任務均是模塊化和可修改。這使AV系統設計人員能夠輕松試驗不同的傳感器配置和方法，并把大型工程組織共享的相同模型在異構車隊上部署。其實驗結果展示了昂貴的高密度（HD）激光雷達傳感器與廉價的低密度（LD）激光雷達加攝像機裝置在3D目標檢測任務中的近似等價性。這為業界共同設計硬件和軟件架構以及建立異構配置的大型車隊鋪平了道路。

多傳感器標定

另外在自動駕駛研發中，GPS/IMU和攝像頭或者激光雷達的標定，雷達和攝像頭之間的標定也是常見的。不同傳感器之間標定最大的問題是如何衡量最佳，因為獲取的數據類型不一樣。這樣的話，實現標定誤差最小化的目標函數會因為不同傳感器配對而不同。

另外，標定方法分targetless和target兩種，前者在自然環境中進行，約束條件少，不需要用專門的target；后者則需要專門的控制場，有ground truth的target，比如典型的棋盤格平面板。

targetless方法的若干算法：

1、手-眼標定（Hand-Eye Calibration）。

這是一個被標定方法普遍研究的，一定約束條件下的問題：可以廣義的理解，一個“手”（比如GPS/IMU）和一個“眼”（激光雷達/攝像頭）都固定在一個機器上，那么當機器運動之后，“手”和“眼”發生的姿態變化一定滿足一定的約束關系，這樣求解一個方程就可以得到“手”-“眼”之間的坐標轉換關系，一般是AX=XB形式的方程。手眼系統分兩種：eye in hand和eye to hand，自動駕駛是前者，即手-眼都在動的情況。手眼標定分兩步法和單步法，后者最有名的論文是“hand eye calibration using dual quaternion"。一般認為，單步法精度高于兩步法，前者估計旋轉之后再估計平移。

東京大學的論文《LiDAR and Camera Calibration using Motion Estimated by Sensor Fusion Odometry》來看看激光雷達和攝像頭的標定算法。求解激光雷達的姿態變化采用ICP，而攝像頭的運動采用特征匹配。后者有一個單目SFM的scale問題。SFM（Structure From Motion）從運動攝像機拍攝的多幅二維圖像中估計攝像機運動和重建場景結構，是計算機視覺領域中的一個核心問題，被稱為從運動恢復結構問題。論文提出了一個基于傳感器融合的解法：初始估計來自于無尺度的攝像頭運動和有尺度的激光雷達運動；之后有scale的攝像頭運動會在加上激光雷達點云數據被重新估計。最后二者的外參數就能通過手-眼標定得到。這個方法無法在室外自然環境中使用，因為點云投影的圖像點很難確定。

關于如何優化激光雷達-攝像頭標定的論文，不是通過3-D點云和圖像點的匹配誤差來估計標定參數，而是直接計算點云在圖像平面形成的深度圖，其和攝像頭獲取的圖像存在全局匹配的測度。不過這些方法，需要大量迭代，最好的做法是根據手眼標定產生初始值為好。

2、IMU-攝像頭標定

德國Fraunhofer論文“INS-Camera Calibration without Ground Control Points“。雖然是給無人機的標定，對車輛也適合。IMU-攝像頭標定和激光雷達-攝像頭標定都是類似的，先解決一個手眼標定，然后優化結果。只是IMU沒有反饋信息可用，只有姿態數據，所以就做pose graph optimization。其中攝像頭還是用SFM估計姿態。

3、激光雷達系統標定

牛津大學論文“Automatic self-calibration of a full field-of-view 3D n-laser scanner".本文定義點云的“crispness” 作為質量測度，通過一個熵函數Re?nyi Quadratic Entropy (RQE）最小化作為在線標定激光雷達的優化目標。作者還討論了激光雷達的時鐘偏差問題解決方案。

4、基于深度學習的多傳感器標定

以深度學習的方法訓練CNN模型去回歸坐標系轉換的參數。主要是兩個CNN模型：RegNet和CalibNet。

RegNet應該是第一個深度卷積神經網絡（CNN）推斷多傳感器的6自由度（DOF）外參數標定，即激光雷達（LiDAR）和單目攝像頭。RegNet將標定的三個步驟（特征提取、特征匹配和全局回歸）映射到單個實時CNN模型中。在訓練期間，隨機對系統進行重新調整，以便訓練RegNet推斷出激光雷達投影到攝像頭的深度測量與RGB圖像之間的對應關系，并最終回歸標定外參數。此外，通過迭代執行多個CNN，在不同程度失標定數據上進行訓練。

CalibNet是一個自監督的深度網絡，能夠實時自動估計激光雷達和2D攝像頭之間的6-自由度剛體轉換關系。在訓練時，不進行直接監督（例如不直接回歸標定參數）；相反，可以訓練網絡去預測標定參數，以最大化輸入圖像和點云的幾何和光度一致性。CalibNet的流程：（a）來自標定攝像頭的RGB圖像；（b）原始激光雷達點云作為輸入，并輸出最佳對齊兩個輸入的6-自由度剛體變換T；（c）顯示錯誤標定設置的彩色點云輸出；（d）顯示使用CalibNet網絡標定后的輸出。

融合算法PointPainting: Sequential Fusion for 3D Object Detection

數據級融合。在融合網絡結構設計上并非end-to-end訓練方式，分成了2個階段

• 第一階段，對camera數據做語義分割
• 第二階段，將激光雷達點云與語義信息相結合做3D目標檢測。通過特征投影將兩個階段聯系起來，也就是圖中的第二步：Point Painting。

這個投影過程也分為兩步，首先在源數據上建立點云與圖像像素間的對應關系，這個對應關系可以通過多傳感器的標定參數計算得到：P（camera）=K*T*P（lidar），其中K為相機的內參，T為lidar到camera的標定參數，如下式，由于不同傳感器獲得數據是有一些時間差，通過自車的定位信息做出補償即可。第二步則在此對應關系的基礎上，將語義分割網絡的輸出與點云特征concatenate，得到3D目標檢測網絡的輸入。

融合算法LaserNet++：Sensor Fusion for Joint 3D Object Detection and Semantic Segmentation

數據級融合。LaserNet++設計了一個end-to-end的融合網絡，分別用兩個網絡來提取camera數據與lidar點云的特征，然后通過特征投影，將兩種特征通過相加的方式組合成新的特征，再使用laserne完成3D目標檢測。

投影過程也是分為兩步，首先在源數據上建立camera/range view圖像與圖像像素的對應關系，然后再利用對應關系得到兩種傳感器相對應的特征。Lasernet++在做點云特征提取時，使用了camera/rang view的表達方式，因此，建立點云與圖像像素對應關系也分為兩步：

• 計算點云與range view圖像的對應關系
• 計算點云與圖像像素的對應關系，如下式，其中（u，v）為圖像坐標，p為點云坐標，K為相機內參，R和t為從lidar坐標到camera坐標變換的旋轉矩陣和平移矩陣。

融合算法ContFusion：Deep Continuous Fusion for Multi-Sensor 3D Object Detection

是一個end-to-end網絡，分成了4個部分：camera特征提取、特征融合、lidar特征提取、檢測輸出。這種設計的好處就是可以dense的融合兩種信息，但是問題也很明顯，效率不怎么高。

特征投影關系的建立整體上來說分成2大步：建立源數據投影關系、提取融合特征。分成了5步：

• 作者在lidar特征提取時使用了bev表達，我們知道bev上的像素點與點云并不是一一對應的，bev圖是很稀疏的，因此作者使用了KNN的方式，獲得bev圖上每一個像素對應的K個點云
• 將bev圖下得到的k個最近鄰的點反投影回點云坐標
• 建立點云點與圖像像素的對應關系，建立方式和單階段融合的方式相同
• 由于lidar點投影到圖像素坐標為浮點數，因此需要通過插值得到投影點的特征向量
• 通過mlp得到圖像特征與幾何特征的融合特征，計算方式如下式所示，其中fj為j點的圖像特征，x為點云3D坐標，xj-xi為j點與目標點i點距離差。最后hi與lidar點云特征相加得到最終的融合特征。

ContFusion融合方式中KNN的參數選取很關鍵，k值和d值對最終結果影響較大，k和d都不易太大，太大會融合遠處點云點的信息，干擾當前點的特征表達，模型效果變差。

融合技術中的兩個核心問題

以上針對融合階段這種分類方式對多傳感器融合技術做了簡單的分析，重點分析了多傳感器融合技術中的兩個核心問題：數據/特征投影，融合網絡設計。

• 投影方式決定了融合算法能否end-to-end訓練，而end-to-end訓練能使模型得到更好的結果，如在PointPainting方法中，作者認為由于PointPainting沒有使用end-to-end訓練，會造成該方法無法得到最好的結果。
• 對于融合網絡設計，主要考慮融合特征的提取，或融合方式的設計，是數據/單層特征融合，還是多層特征融合，還是目標級特征融合？正如MV3D中討論的early fusion、deep fusion、late fusion：deep fusion相對另外兩種融合方式在精度上可以提高0.5個百分點，但是在實際應用時，還需要考慮speed vs accuracy平衡的問題。

參考文獻：

1、［Matlab官方］了解傳感器融合和跟蹤_嗶哩嗶哩_bilibili ?https://www.bilibili.com/video/BV1fi4y1F7rU/?spm_id_from=333.788.recommend_more_video.0

2、傳感器融合的數學基礎 [完結] Sensor Fusion 2020 (挪威科技大學)_嗶哩嗶哩_bilibili ?https://www.bilibili.com/video/BV1JA411J7XS?p=15

3、P14小邁步之人工智能(四)：實戰傳感器融合與目標限蹤?https://www.bilibili.com/medialist/play/ml1238052723/BV1xf4y1i73L?s_eid=PEP_webinarfromILM

4、Sensor Fusion and Non-linear Filtering for Automotive Systems | edX ?https://www.edx.org/course/sensor-fusion-and-non-linear-filtering-for-automot?index=product&queryID=1dd0f6fa07927ff922a70bef647ce15b&position=1

5、《Handbook of multisensor data fusion _ theory and practice》

6、多傳感器信息融合(標定, 數據融合, 任務融合)_ChenGuiGan的博客-CSDN博客_多傳感器信息融合 ?https://blog.csdn.net/ChenGuiGan/article/details/104960658?spm=1001.2101.3001.6650.2&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7Edefault-2.no_search_link&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7Edefault-2.no_search_link

7、無人駕駛傳感器融合系列（十）—— 目標追蹤之相機與激光雷達數據融合_默_存的博客-CSDN博客 ?https://blog.csdn.net/weixin_40215443/article/details/96456408

8、多傳感器信息融合(標定, 數據融合, 任務融合)_ChenGuiGan的博客-CSDN博客_多傳感器信息融合 ?https://blog.csdn.net/ChenGuiGan/article/details/104960658?spm=1001.2101.3001.6650.2&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7Edefault-2.no_search_link&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7Edefault-2.no_search_link

9、自動駕駛系統的傳感器標定方法 - 知乎 ?https://zhuanlan.zhihu.com/p/57028341

10、重讀經典《Quaternion kinematics for the error-state Kalman filter》_10點43的博客-CSDN博客 ?https://blog.csdn.net/cg129054036/article/details/119703694

11、用于自動駕駛中廣義傳感器融合的可規模化設計 - https://zhuanlan.zhihu.com/p/443153723

12、自動駕駛系統的傳感器標定方法 - 知乎 ?https://zhuanlan.zhihu.com/p/57028341

13、基于深度學習的多傳感器標定 - 知乎 ?https://zhuanlan.zhihu.com/p/73073753

14、頭條 | 自動駕駛多傳感器融合技術淺析 ?https://mp.weixin.qq.com/s?__biz=MzAwNTMzODc4OA==&mid=2456142764&idx=1&sn=8ebba7511bb63dc0b227f976629e07c7&chksm=8c8f4067bbf8c9712238ff3f53bfdc421380dadea18ade13e40c3d9a9e53c924271616d998c8&token=1575724551&lang=zh_CN#rd

15、多傳感器數據深度圖的融合：最近基于深度學習的方法 - 知乎 ?https://zhuanlan.zhihu.com/p/90773462

16、傳感器融合-數據篇 - 知乎 ?https://zhuanlan.zhihu.com/p/109895639

17、傳感器融合-任務篇 - 知乎 ?https://zhuanlan.zhihu.com/p/109900137

18、幾個攝像頭和雷達融合的目標檢測方法 - 知乎 ?https://zhuanlan.zhihu.com/p/371258127

19、

20、

總結

以上是生活随笔為你收集整理的传感器融合sensor fusion的全部內容，希望文章能夠幫你解決所遇到的問題。

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