Real-Time Temporal and Rotational Calibration of Heterogeneous Sensors Using Motion Correlation Analysis

論文作者：Kejie Qiu, Tong Qin, Jie Pan, Liu Siqi, and Shen Shaojie這是Shen Shaojie課題組的論文，方法雖不是特別創(chuàng)新，但做的深入且成系統(tǒng)。文章來源：IEEE TRANSACTIONS ON ROBOTICS，2021年4月，第37卷第2期論文鏈接：https://ieeexplore.ieee.org/document/9271875翻譯：chaochaoSEU摘要：準(zhǔn)確而魯棒的校準(zhǔn)對(duì)于多傳感器融合系統(tǒng)至關(guān)重要。由于傳感器獲取數(shù)據(jù)方式的巨大差異，異構(gòu)傳感器的校準(zhǔn)特別具有挑戰(zhàn)性。當(dāng)前，許多校準(zhǔn)方法都忽略了時(shí)間校準(zhǔn)，實(shí)際上時(shí)間校準(zhǔn)和空間校準(zhǔn)同樣重要。本文專注于異構(gòu)傳感器的時(shí)間校準(zhǔn)，且提供相應(yīng)的外部旋轉(zhuǎn)參數(shù)。大多數(shù)現(xiàn)有方法專用于特定的傳感器組合，例如IMU-相機(jī)或相機(jī)-激光系統(tǒng)。但是，異構(gòu)多傳感器融合是機(jī)器人領(lǐng)域的趨勢，因此需要一種統(tǒng)一的校準(zhǔn)方法。為此，我們利用3D旋轉(zhuǎn)運(yùn)動(dòng)特征進(jìn)行校準(zhǔn)，而不需要輔助標(biāo)定板，這是因?yàn)槭褂枚喾N里程計(jì)方法來獲取3D傳感器的運(yùn)動(dòng)信息。使用高頻IMU作為校準(zhǔn)參考，以IMU為中心的方案旨在實(shí)現(xiàn)一個(gè)統(tǒng)一的框架，該框架適用于可以獨(dú)立估計(jì)3D旋轉(zhuǎn)運(yùn)動(dòng)的各種目標(biāo)傳感器。通過結(jié)合獨(dú)立的以IMU為中心的校準(zhǔn)組合體，任意的傳感器也可以使用相同的參考IMU進(jìn)行校準(zhǔn)。由于采用了新穎的3D運(yùn)動(dòng)相關(guān)性量化和分析機(jī)制，因此可以實(shí)現(xiàn)實(shí)時(shí)估計(jì)時(shí)間偏移。給定時(shí)間對(duì)齊的傳感器運(yùn)動(dòng)，可以在相同的3D運(yùn)動(dòng)關(guān)聯(lián)機(jī)制中以解析解求出外部旋轉(zhuǎn)參數(shù)。通過與最新的校準(zhǔn)方法進(jìn)行比較，某些傳感器組合的實(shí)驗(yàn)結(jié)果表明了該方法的準(zhǔn)確性和魯棒性，異構(gòu)多傳感器集的校準(zhǔn)結(jié)果證明了該方法的可擴(kuò)展性和多功能性。I 引言多傳感器融合是各種精確環(huán)境感知機(jī)器人應(yīng)用的基礎(chǔ)，如同時(shí)定位和建圖（SLAM）[1]和動(dòng)態(tài)環(huán)境感知[2][3]。不同的傳感器可以互補(bǔ)，融合系統(tǒng)的整體感知能力將顯著提高。例如，IMU具有很高的更新率，但有噪聲和漂移；相機(jī)具有高分辨率的感知能力，但存在圖像模糊和尺度模糊，而激光雷達(dá)具有精確的距離感知能力，但水平分辨率和FOV有限。為了實(shí)現(xiàn)有效和魯棒的傳感器融合，不同傳感器的數(shù)據(jù)同步對(duì)融合系統(tǒng)至關(guān)重要。為了實(shí)現(xiàn)高精度的時(shí)間同步和空間校準(zhǔn)，利用高頻IMU，設(shè)計(jì)了一個(gè)以IMU為中心的校準(zhǔn)方案，如圖1所示。使用一個(gè)IMU作為公共校準(zhǔn)參考，所有的目標(biāo)傳感器都可以相對(duì)于中心IMU進(jìn)行校準(zhǔn)，并且使用相同參考IMU的任意兩個(gè)傳感器也可以相應(yīng)地進(jìn)行校準(zhǔn)。

許多傳感器融合方法首先假設(shè)不同傳感器的時(shí)間戳精確對(duì)齊[1]、[4]-[6]，這實(shí)際上只能通過嚴(yán)格的硬件同步來保證。但是對(duì)于大多數(shù)低成本和自搭建的傳感器集，硬件同步是不可用的。實(shí)際上，傳感器的時(shí)間戳?xí)艿讲煌瑫r(shí)鐘、觸發(fā)機(jī)制、傳輸延遲、數(shù)據(jù)阻塞、抖動(dòng)、傾斜的影響。正如文獻(xiàn)[7]所指出的，對(duì)上述因素的校正被稱為“時(shí)鐘同步”，而本文重點(diǎn)關(guān)注“時(shí)間校準(zhǔn)”，即確定測量實(shí)例和時(shí)間戳之間的恒定偏移的過程。因此，精確的時(shí)間校準(zhǔn)是傳感器融合的首要前提條件。大多數(shù)校準(zhǔn)方法都是專門為某種傳感器組合而設(shè)計(jì)的，如相機(jī)-IMU系統(tǒng)或相機(jī)-激光系統(tǒng)。然而，多源異構(gòu)傳感器融合在當(dāng)今的應(yīng)用場景中很常見，如自動(dòng)駕駛。且將來也會(huì)出現(xiàn)新傳感器。因此，需要一種統(tǒng)一、無目標(biāo)、實(shí)時(shí)、高精度的校準(zhǔn)方案。本文主要研究了基于3D運(yùn)動(dòng)相關(guān)分析的異構(gòu)傳感器的時(shí)間校準(zhǔn)。該方法使用相同的三維相關(guān)分析還可估計(jì)關(guān)鍵的外部旋轉(zhuǎn)參數(shù)。我們的方法適用于任何可以獨(dú)立估計(jì)三維旋轉(zhuǎn)的自運(yùn)動(dòng)（ego-motion自我運(yùn)動(dòng):在計(jì)算機(jī)視覺領(lǐng)域，自我運(yùn)動(dòng)是指估計(jì)相機(jī)相對(duì)于固定場景的運(yùn)動(dòng)。自我估計(jì)的一個(gè)例子是估計(jì)汽車相對(duì)于從汽車本身觀察到的道路或街道標(biāo)志線上的移動(dòng)位置。自我運(yùn)動(dòng)的估計(jì)在自主機(jī)器人導(dǎo)航應(yīng)用中很重要）?，F(xiàn)有的校準(zhǔn)算法大多屬于優(yōu)化方法的范疇：目標(biāo)是擬合/對(duì)齊不同傳感器的傳感器數(shù)據(jù)，校準(zhǔn)問題可以表述為狀態(tài)估計(jì)問題，可通過濾波或優(yōu)化的解決方法。還有另一類是利用運(yùn)動(dòng)相關(guān)分析來校準(zhǔn)類似于[8]和[9]中的傳感器。事實(shí)上，相關(guān)分析是估計(jì)兩個(gè)信號(hào)之間時(shí)移的最標(biāo)準(zhǔn)的方法[10]、[11]。但相關(guān)性方法的弱點(diǎn)是估計(jì)精度。例如，文獻(xiàn)[8]的結(jié)果需要通過優(yōu)化方法做進(jìn)一步優(yōu)化。受[8]和[9]的啟發(fā)，我們將一維相關(guān)分析擴(kuò)展到三維相關(guān)分析，充分利用三維運(yùn)動(dòng)，并設(shè)計(jì)了一個(gè)速率平衡濾波（a rate balance filter）來平衡中心IMU和目標(biāo)傳感器之間的更新率差異，從而實(shí)現(xiàn)更高的校準(zhǔn)精度和魯棒性，其可與基于優(yōu)化方法相媲美。眾所周知，每個(gè)外部感受性傳感器都可以根據(jù)特定的物理特征來感知部分環(huán)境，這可以是發(fā)光強(qiáng)度、紋理、邊緣、深度、光譜反射率等。即使兩個(gè)傳感器可以測量相同的量，測量的結(jié)果與另一個(gè)傳感器也會(huì)不同，特定的感測FOV和感測面積也可以不同。如果我們利用原始的傳感器數(shù)據(jù)進(jìn)行校準(zhǔn)，準(zhǔn)確的傳感器數(shù)據(jù)關(guān)聯(lián)是最大的問題。例如，傳統(tǒng)的雙目相機(jī)校準(zhǔn)方法依賴于重疊區(qū)域的極線約束或重投影誤差來求解外參，這意味著它不能處理兩個(gè)相機(jī)沒有重疊區(qū)域的情況。相反，自我運(yùn)動(dòng)由同一傳感器集中的所有傳感器共享，以進(jìn)行異構(gòu)傳感器校準(zhǔn)，從而實(shí)現(xiàn)一種統(tǒng)一的校準(zhǔn)風(fēng)格。如今，許多里程計(jì)算法和SLAM算法已經(jīng)被開發(fā)來獲取具有特定外部感受傳感器的三維自我運(yùn)動(dòng)，如用于單目相機(jī)的ORB-SLAM[13]、DSO[14]，用于激光的LOAM[15]和用于RGBD相機(jī)的RGBD-SLAM。而IMU作為載體傳感器可以直接測量具有角速度和線性加速度的三維自我運(yùn)動(dòng)特征。如果我們從不同的運(yùn)動(dòng)估計(jì)方法中提取運(yùn)動(dòng)特征作為獨(dú)立的信號(hào)，就不需要輔助標(biāo)定板，相應(yīng)的時(shí)間偏移可以首先通過精心設(shè)計(jì)的三維相關(guān)分析來估計(jì)，這對(duì)多重幾何變換是不變的。通過充分的運(yùn)動(dòng)激勵(lì)和精確的時(shí)間校準(zhǔn)，可以得到兩個(gè)傳感器觀察到的最大運(yùn)動(dòng)相關(guān)性。此后，外部旋轉(zhuǎn)參數(shù)的推導(dǎo)類似于一種幾何形狀的對(duì)準(zhǔn)方法，普氏分析[17][18]。給定時(shí)間對(duì)齊的運(yùn)動(dòng)數(shù)據(jù)，通過具有相同三維相關(guān)分析機(jī)制的特征方向分析，可以進(jìn)一步推導(dǎo)出外部旋轉(zhuǎn)參數(shù)。使用運(yùn)動(dòng)相關(guān)性的一個(gè)優(yōu)點(diǎn)是，在外部旋轉(zhuǎn)參數(shù)校準(zhǔn)之前，可以應(yīng)用時(shí)間校準(zhǔn)。因?yàn)闀r(shí)間偏移和外部旋轉(zhuǎn)是兩個(gè)不相關(guān)的量，如[8]指出的共同估計(jì)不相關(guān)的量將會(huì)損害估計(jì)結(jié)果。因?yàn)闀r(shí)間和空間的不對(duì)齊都引起擬合誤差，所以優(yōu)化算法同時(shí)聯(lián)合估計(jì)所有的狀態(tài)量。此外，與優(yōu)化方法相比，該方法具有更大的時(shí)間偏移估計(jì)范圍。另一個(gè)優(yōu)點(diǎn)是，基于時(shí)間校準(zhǔn)的三維相關(guān)分析，可以快速推導(dǎo)出外部旋轉(zhuǎn)參數(shù)，這意味著不需要外部旋轉(zhuǎn)參數(shù)的初始估計(jì)，而大多數(shù)優(yōu)化方法都依賴于初始猜測用于精確收斂。據(jù)作者所知，這項(xiàng)工作是第一個(gè)不需要初始猜測和額外標(biāo)定板輔助的統(tǒng)一解決異構(gòu)多傳感器的實(shí)時(shí)時(shí)間偏移和外部旋轉(zhuǎn)參數(shù)估計(jì)。貢獻(xiàn)如下：

1. 針對(duì)多源異構(gòu)傳感器校準(zhǔn)，提出了一種使用魯棒的三維運(yùn)動(dòng)相關(guān)分析進(jìn)行統(tǒng)一、實(shí)時(shí)的時(shí)間偏移校準(zhǔn)方法。

2. 基于時(shí)間校準(zhǔn)結(jié)果，在相同的三維相關(guān)分析機(jī)制中得到了外部旋轉(zhuǎn)參數(shù)校準(zhǔn)的解析解。

3. 通過與參考值和最先進(jìn)的校準(zhǔn)方法的對(duì)比，展示了我們的方法的校準(zhǔn)精度和魯棒性。

本文的其余部分的結(jié)構(gòu)如下。Section II介紹了不同傳感器之間時(shí)空校準(zhǔn)的相關(guān)工作。Section III介紹了相關(guān)公式和原理。Section IV介紹了三維運(yùn)動(dòng)相關(guān)評(píng)價(jià)和關(guān)鍵方法。Section V展示了使用EuRoC數(shù)據(jù)集[19]的仿真結(jié)果。Section VI給出了實(shí)際的實(shí)驗(yàn)結(jié)果，并與其他校準(zhǔn)方法的結(jié)果進(jìn)行了比較。Section VII為總結(jié)。II 相關(guān)工作傳感器延遲是實(shí)時(shí)應(yīng)用中的一個(gè)常見問題，不同傳感器的延遲差異會(huì)導(dǎo)致時(shí)間偏移，如圖2所示。大多數(shù)時(shí)間校準(zhǔn)方法集中于估計(jì)時(shí)間偏移，而不是傳感器延遲，因?yàn)閭鞲衅鲾?shù)據(jù)的不對(duì)準(zhǔn)將極大地影響傳感器融合的質(zhì)量。一些傳感器融合工作首先假設(shè)，延遲是完全已知的[20]-[23]或時(shí)間偏移是大致已知的[24]、[25]。

近年來，公開的各種傳感器校準(zhǔn)方法：從單時(shí)間/空間校準(zhǔn)[26]到聯(lián)合校準(zhǔn)[8]、[27]，從離線校準(zhǔn)[28]到實(shí)時(shí)校準(zhǔn)[27]，從解析解方案[26]到迭代優(yōu)化方案[29]。然而，解析解方案不能用于時(shí)間校準(zhǔn)。顯然，具有無目標(biāo)的實(shí)時(shí)校準(zhǔn)方法由于具有很強(qiáng)的適應(yīng)性和靈活性，因此更有吸引力?；诜蔷€性代價(jià)函數(shù)迭代最小化的優(yōu)化方案更精確，但計(jì)算成本更高，它們需要良好的初始估計(jì)來精確收斂。Tungadi and Kleema[30]提出通過計(jì)算周期運(yùn)動(dòng)的相移來估計(jì)移動(dòng)機(jī)器人的激光雷達(dá)和車輪里程計(jì)之間的時(shí)間偏移，但這種方法需要已知外部旋轉(zhuǎn)參數(shù)。Fleps等[29]提出將傳感器軌跡建模為b樣條，并聯(lián)合優(yōu)化b樣條的控制點(diǎn)和空間配準(zhǔn)參數(shù)。由Furgale[28]提出的工具箱Kalibr，可同時(shí)估計(jì)時(shí)間偏移和相機(jī)運(yùn)動(dòng)，以及利用連續(xù)批量優(yōu)化估計(jì)的外部旋轉(zhuǎn)和平移，并給出了估計(jì)偏移的不確定性。然后，它被擴(kuò)展到多傳感器系統(tǒng)的一般時(shí)空校準(zhǔn)，如激光雷達(dá)-相機(jī)校準(zhǔn)，并解釋了適用于硬件同步之外的同步方案的問題[7]。此外，Rehder等 [9]將Kalibr擴(kuò)展到多IMU的外參校準(zhǔn)。這兩種都使用b樣條參數(shù)化傳感器運(yùn)動(dòng)以實(shí)現(xiàn)平滑的角加速度。估計(jì)兩個(gè)信號(hào)之間的時(shí)移的最標(biāo)準(zhǔn)的方法是檢測它們之間的互相關(guān)的峰值 [10][11]。例如，Mair等 [8]一種IMU和相機(jī)之間時(shí)間和空間配準(zhǔn)的初始化方法。它首先使用相互相關(guān)或相位一致性來估計(jì)時(shí)間偏移，這與空間對(duì)齊無關(guān)。給定估計(jì)的時(shí)間偏移，使用改進(jìn)的手眼校準(zhǔn)方法估計(jì)IMU和相機(jī)之間的解析解旋轉(zhuǎn)參數(shù)。然而，只提取絕對(duì)轉(zhuǎn)速（1D）進(jìn)行交叉相關(guān)和相位分析。使用一維互相關(guān)的估計(jì)精度有限，使用相位一致無法獲得噪聲數(shù)據(jù)的精確時(shí)間對(duì)齊。因此，它只被用作濾波或優(yōu)化方法的初始化方法。我們充分利用三維運(yùn)動(dòng)，并評(píng)估與三維相關(guān)性的運(yùn)動(dòng)相關(guān)性，以更魯棒和準(zhǔn)確的校準(zhǔn)，實(shí)現(xiàn)與優(yōu)化方法相當(dāng)?shù)木?。事?shí)上，三維動(dòng)態(tài)相關(guān)分析已用于動(dòng)態(tài)對(duì)象跟蹤[31]、[32]中的運(yùn)動(dòng)分解。最近，一些視覺-慣性緊耦合的實(shí)時(shí)運(yùn)動(dòng)估計(jì)方法將時(shí)間偏移作為估計(jì)附加狀態(tài)[27][33]。它可以處理時(shí)變偏移，估計(jì)偏移的不確定性也可以用這種方法來建模。[33]的工作是一種濾波方法，[27]的工作是一種優(yōu)化方法。與之前的框架相比，這兩種方法都只稍微增加了計(jì)算復(fù)雜度，而沒有估計(jì)時(shí)間偏移。為了使殘差對(duì)時(shí)間偏移可微，Li和Mourikis[33]對(duì)傳感器集相對(duì)于估計(jì)時(shí)間偏移的位置和方向應(yīng)用一階近似，而Qin和Shen[27]假設(shè)每個(gè)特征點(diǎn)在圖像平面的短時(shí)間間隔內(nèi)以恒定的速度移動(dòng)。實(shí)際上，一階近似[33]是傳感器運(yùn)動(dòng)本身在短時(shí)間間隔內(nèi)的另一個(gè)恒定速度假設(shè)。然而，它們是專門為IMU-相機(jī)系統(tǒng)設(shè)計(jì)的。此外，由于恒定速度假設(shè)，兩種方法的收斂范圍受到有限，換句話說，與其他迭代解一樣，它們需要對(duì)精確收斂有很好的初始猜測。對(duì)于外參的估計(jì)，這個(gè)問題可以從統(tǒng)計(jì)形狀分析的角度抽象為點(diǎn)集配準(zhǔn)。它可以用典型的普氏對(duì)準(zhǔn)方法[17]、[18]來求解，除剛性變換外，還能找到尺度和反射變換。利用奇異值分解(SVD可以有效地計(jì)算外參。然而，普氏對(duì)齊假定數(shù)據(jù)點(diǎn)關(guān)聯(lián)良好，或者換句話說，傳感器的運(yùn)動(dòng)數(shù)據(jù)可以很好地同步。[33]和[27]在運(yùn)動(dòng)跟蹤過程中都一直在估計(jì)平移和旋轉(zhuǎn)參數(shù)。Li和Liang[34]提出了一個(gè)使用無跡卡爾曼濾波的時(shí)空配準(zhǔn)模型來進(jìn)行多個(gè)不同的傳感器融合。Zhang專注于使用兩個(gè)解析解[26]來校準(zhǔn)IMU和磁力計(jì)之間的外參，從而消除傳統(tǒng)方法中的迭代過程。然而，Zhang和Song[26]假設(shè)IMU的尺度因子和偏差被預(yù)先校準(zhǔn)，傳感器是同步的。Kelly和Sukhatme[35]提出了時(shí)延迭代最近點(diǎn)來估計(jì)本體感受傳感器和外部感受傳感器之間的時(shí)間偏移和外在旋轉(zhuǎn)參數(shù)。該算法通過在三維方向空間中調(diào)整曲線來迭代計(jì)算時(shí)空變換。最近提出了各種針對(duì)激光-相機(jī)校準(zhǔn)的外參估計(jì)方法。Scaramuzza等[38]介紹了一種三維激光雷達(dá)和全向相機(jī)的外參校準(zhǔn)技術(shù)，采用相機(jī)和激光雷達(dá)之間的手動(dòng)校準(zhǔn)技術(shù)。文獻(xiàn)[39]提出了一種單目相機(jī)和平2D面激光雷達(dá)之間的統(tǒng)一時(shí)空校準(zhǔn)方法。[7]中的激光相機(jī)校準(zhǔn)案例也使用二維激光雷達(dá)，并要求環(huán)境至少部分由平面組成。Faraz等[40]使用人工標(biāo)定板估計(jì)了激光-相機(jī)的外參，其中也估計(jì)了激光雷達(dá)的內(nèi)參，而Pande等[41]提出了一種無目標(biāo)的外參校準(zhǔn)方法，利用Barzilai–Borwein steepest gradient ascent algorithm將激光雷達(dá)反射率和光學(xué)圖像最大化交互信息?；跇?biāo)準(zhǔn)化的交互信息的工作也提出了[42]，可以應(yīng)用于更一般的環(huán)境，因?yàn)樗粌H是基于激光雷達(dá)的反射率，而是使用粒子群優(yōu)化，這并不局限于凸問題，所以單掃描校準(zhǔn)是可能的。然而，所有這些方法都是為純粹的外參校準(zhǔn)而設(shè)計(jì)的，并假設(shè)傳感器數(shù)據(jù)具有較好的同步。III 概況IMU模型：

根據(jù)目標(biāo)傳感器，將不同IMU中心組合兩類：IMU-IMU和IMU-一般傳感器校準(zhǔn)。在不失去一般性的情況下，我們選擇了中心IMU和一個(gè)目標(biāo)傳感器進(jìn)行標(biāo)準(zhǔn)說明，使用相同參考IMU的任意兩個(gè)傳感器可以通過組合相應(yīng)的中心IMU標(biāo)準(zhǔn)對(duì)進(jìn)行校準(zhǔn)。

在本文中，我們將關(guān)鍵的時(shí)間偏移td視為觀測持續(xù)時(shí)間內(nèi)一個(gè)恒定的未知值，并將我們之前的工作[27]中的IMU-相機(jī)時(shí)間偏移定義擴(kuò)展到一般的目標(biāo)傳感器:

時(shí)間偏移是移動(dòng)目標(biāo)傳感器時(shí)間戳的時(shí)間，使目標(biāo)傳感器和中心IMU數(shù)據(jù)流保持時(shí)間一致的時(shí)間。它可以是一個(gè)正值或負(fù)值；如果目標(biāo)傳感器序列的延遲比IMU序列更長，則td為一個(gè)負(fù)值，反之亦然。IV 數(shù)學(xué)模型相關(guān)性分析是一種廣泛使用的相似度測量技術(shù)，特別適用于分析時(shí)移序列。我們沒有對(duì)提取的絕對(duì)角速度[8]等一維運(yùn)動(dòng)特征進(jìn)行三維相關(guān)性評(píng)估（相關(guān)性系數(shù)），而是直接測量原始三維運(yùn)動(dòng)的三維相關(guān)性，從而實(shí)現(xiàn)高精度的時(shí)間偏移估計(jì)和基于解析解的外部旋轉(zhuǎn)參數(shù)。兩個(gè)多變量隨機(jī)過程之間的穩(wěn)健相關(guān)性度量是軌跡相關(guān) [43]。它也用于動(dòng)態(tài)對(duì)象[32]的運(yùn)動(dòng)分解和尺度估計(jì)，其中動(dòng)態(tài)對(duì)象的度量尺度優(yōu)化通過最小化的相關(guān)對(duì)象運(yùn)動(dòng)和主體的運(yùn)動(dòng)，而在本文中，我們提出通過最大化相應(yīng)的運(yùn)動(dòng)相關(guān)性來校準(zhǔn)兩個(gè)傳感器的時(shí)間偏移和外部旋轉(zhuǎn)參數(shù)。整個(gè)算法流程：

A典型相關(guān)分析（CCA）和跡相關(guān)給定兩個(gè)隨機(jī)向量x=[x1,x2,x3]T和y=[y1,y2,y3]T可以用x和y的樣本數(shù)據(jù)估計(jì)互協(xié)方差和自協(xié)方差：

CCA[44]的目標(biāo)是找到a和b的線性組合向量對(duì)，從而使相關(guān)系數(shù)Corr(aTx, bTy)最大。

事實(shí)上，通過矩陣的特征值分解，可以以解析解的形式計(jì)算出三對(duì)標(biāo)準(zhǔn)變量：

x和y之間的跡相關(guān)被定義為正則相關(guān)系數(shù)的平方的均方根：

與兩個(gè)隨機(jī)變量之間的歸一化相關(guān)系數(shù)相似，跡相關(guān)是兩個(gè)隨機(jī)向量之間的另一個(gè)歸一化測量，不受絕對(duì)值或尺度的影響。由于跡相關(guān)計(jì)算的特征值分解操作和協(xié)方差計(jì)算的零均值歸一化操作，跡相關(guān)的另一個(gè)特性是它對(duì)縮放、旋轉(zhuǎn)、平移變換是不變的，這將顯著簡化不同參考系中表示的兩個(gè)傳感器運(yùn)動(dòng)之間的相關(guān)測量，使時(shí)間偏移估計(jì)與未知的標(biāo)度、旋轉(zhuǎn)和平移無關(guān)。換句話說，時(shí)間校準(zhǔn)與其他狀態(tài)估計(jì)被解耦。然而，對(duì)于優(yōu)化方法，它們都需要聯(lián)合估計(jì)。我們將跡相關(guān)的多重幾何變換不變性總結(jié)為：

B運(yùn)動(dòng)特征我們用于運(yùn)動(dòng)相關(guān)分析的關(guān)鍵運(yùn)動(dòng)特征是三維載體的角速度，它可以用三維隨機(jī)過程來表示。首先，我們直接利用中心IMU的原始數(shù)據(jù)作為關(guān)鍵校準(zhǔn)參考。對(duì)于不均勻分布的IMU樣本，采用了基于雙線性插值的數(shù)據(jù)均值化預(yù)處理步驟。對(duì)于以IMU為中心的傳感器同步，根據(jù)IMU測量模型，我們有以下連續(xù)時(shí)間關(guān)系

C速率平衡濾波在實(shí)際實(shí)踐中，測深方法的估計(jì)方向在頻率fG中離散分布，比IMU采樣頻率遠(yuǎn)小于fI。如果我們直接使用估計(jì)的轉(zhuǎn)速和原始的轉(zhuǎn)速來進(jìn)行數(shù)據(jù)關(guān)聯(lián)，就會(huì)出現(xiàn)嚴(yán)重的不一致問題。因?yàn)槟繕?biāo)傳感器的估計(jì)轉(zhuǎn)速實(shí)際上是△t（從tk到tk+1的1/fG秒）內(nèi)的平均測量值，該時(shí)間長于IMU采樣時(shí)間δt（1/fI秒）。目標(biāo)傳感器的平均體角速度可以估算為

為了解決這種不一致，一種解決方案是使用廣泛使用的b樣條[28]、[45]或其他插值方法來擬合目標(biāo)傳感器的運(yùn)動(dòng)，并且可以從較短的時(shí)間窗口δt來估計(jì)旋轉(zhuǎn)速度。但使用插值方法的精確表示在很大程度上依賴于主題模型的選擇和擬合參數(shù)的調(diào)整，這也引入了一個(gè)巨大的計(jì)算負(fù)擔(dān)，如相關(guān)方法所示。因此，我們沒有插值目標(biāo)傳感器的低頻數(shù)據(jù)，而是設(shè)計(jì)了一個(gè)速率平衡濾波，它對(duì)IMU的高頻數(shù)據(jù)應(yīng)用類似的平均效應(yīng)，以實(shí)現(xiàn)一致的數(shù)據(jù)關(guān)聯(lián)，并避免了復(fù)雜的計(jì)算。這樣，用于數(shù)據(jù)關(guān)聯(lián)的運(yùn)動(dòng)特征實(shí)際上是在1/fG秒內(nèi)的平均角速度。這兩種解決方案的詳細(xì)比較如圖4所示。

現(xiàn)在我們可以把這種關(guān)系總結(jié)為：

D 時(shí)間偏移量估計(jì)給定數(shù)據(jù)對(duì)齊的測量值，下一步是建立時(shí)間偏移的連接和相應(yīng)的相關(guān)評(píng)估。由于我們只有離散運(yùn)動(dòng)測量而不應(yīng)用連續(xù)時(shí)間插值，因此我們使用時(shí)間偏移枚舉來解決優(yōu)化問題。如圖5所示。枚舉的ωkG的ωkI將會(huì)以IMU采樣間隔作為枚舉步驟進(jìn)行采樣，平均角速度計(jì)算的開始時(shí)間和結(jié)束時(shí)間將偏移td：

通過最大化跡相關(guān)，可以估計(jì)最優(yōu)的時(shí)間偏移：

為了進(jìn)一步提高估計(jì)精度，我們應(yīng)用對(duì)前三個(gè)最大跡相關(guān)采樣點(diǎn)的二次擬合進(jìn)行估計(jì)細(xì)化，如圖6所示。

E外部旋轉(zhuǎn)參數(shù)估計(jì)給定兩個(gè)傳感器之間精確的時(shí)間校準(zhǔn)，可以在相同的三維運(yùn)動(dòng)相關(guān)評(píng)估框架下推導(dǎo)出相對(duì)旋轉(zhuǎn)。假設(shè)di，i∈{1,2,3}是IMU坐標(biāo)空間中的三個(gè)正交方向。顯然，投射在ωI軸di上的運(yùn)動(dòng)分量和投影在ωG軸RIGdi上的運(yùn)動(dòng)分量是嚴(yán)格相關(guān)的，這意味著：

因此，相對(duì)旋轉(zhuǎn)RIG在以下條件下只對(duì)應(yīng)于矩陣：

在實(shí)踐中，我們將在一定程度上軟化必要的條件，以獲得更高的估計(jì)率，即

F可觀測性條件外部旋轉(zhuǎn)的一個(gè)致命的簡并運(yùn)動(dòng)情況是對(duì)稱運(yùn)動(dòng)，其中投射在兩個(gè)非線性軸上的運(yùn)動(dòng)分量是嚴(yán)格的正相關(guān)或負(fù)相關(guān)的。當(dāng)對(duì)稱運(yùn)動(dòng)發(fā)生時(shí)，就存在模糊的坐標(biāo)解。例如，在圖7中，如果目標(biāo)傳感器在藍(lán)軸、綠軸上的運(yùn)動(dòng)投影嚴(yán)格正相關(guān)或嚴(yán)格正相關(guān)，則表示目標(biāo)傳感器的真坐標(biāo)為實(shí)體坐標(biāo)，且該坐標(biāo)至少有三個(gè)模糊解，如虛線所示。為了消除對(duì)稱運(yùn)動(dòng)可能引起的模糊解，我們進(jìn)一步檢查了以下可觀測性條件：

在滿足必要條件和可觀測條件的情況下，可以在RRT=I和det(R)=1的約束下相應(yīng)地估計(jì)外部旋轉(zhuǎn)矩陣（SVD）：

外部旋轉(zhuǎn)參數(shù)是時(shí)間偏移估計(jì)的副產(chǎn)品，因?yàn)樗梢栽诮o定時(shí)間對(duì)齊運(yùn)動(dòng)數(shù)據(jù)的解析解計(jì)算，只需要一個(gè)SVD操作和可觀測性檢查。換句話說，在我們的校準(zhǔn)框架中，外部旋轉(zhuǎn)參數(shù)估計(jì)所增加的計(jì)算復(fù)雜度幾乎為零，這進(jìn)一步確保了在多傳感器系統(tǒng)內(nèi)的實(shí)時(shí)性能。

后半部分請(qǐng)移步公眾號(hào)原文閱讀：https://mp.weixin.qq.com/s/2Qb1TbaiBpM7HscmKNcAAA