[發(fā)明專利]一種用于多傳感器融合的異步在線標定方法

有效

• 申請?zhí)枺?/td>: 201811265340.1
• 申請日: 2018-10-29
• 公開（公告）號: CN109544638B
• 公開（公告）日: 2021-08-03
• 發(fā)明（設計）人: 張劍華;王曾媛;吳佳鑫;馮宇婷;貴夢萍;甘雨;陳勝勇
• 申請（專利權(quán)）人: 浙江工業(yè)大學
• 主分類號: G06T7/80
• 分類號: G06T7/80;G01S7/497;G01C25/00
• 代理公司: 杭州斯可睿專利事務所有限公司 33241
• 代理人: 王利強

1.一種用于多傳感器融合的異步在線標定方法，其特征在于，所述方法包括以下步驟：

1)計算相機的旋轉(zhuǎn)

設點P為相機坐標系下的一個點，其坐標為[x y z]^T，設點p₁,p₂分別是點P在圖F_i,F_j中的投影，C_i,C_j分別是在時刻i和時刻j的相機坐標系，使用針孔相機模型，得到尺度無關的表達式：

p₁＝KP

p₂＝K(RP+t) (1.1)

其中K是相機的內(nèi)參矩陣，R和t是從C_i到C_j的旋轉(zhuǎn)矩陣和平移向量，進行轉(zhuǎn)換，得到對極約束：

其中表示向量t的斜對稱矩陣，t＝[a b c]^T,

定義本質(zhì)矩陣則式(1.2)寫作：

在已知內(nèi)參矩陣的情況下，給定一系列匹配的特征點，就估計出本質(zhì)矩陣E，對E進行分解，得到旋轉(zhuǎn)矩陣R和平移向量t；

2)計算慣性測量單元的旋轉(zhuǎn)

IMU具有高的采樣頻率，通過預積分集成設定時間段內(nèi)的IMU測量，以避免高頻率的狀態(tài)估計和大量的位姿狀態(tài)，慣性積分公式寫成下面的形式：

其中和分別是以t時刻IMU坐標系為參照坐標系的線性加速度和角速度，是向量的斜對稱矩陣，g^w是世界坐標系下的重力加速度，分別為IMU在時刻i在世界坐標系下的位置、速度和朝向；

IMU從時刻i到時刻j在i時刻IMU坐標系下的相對變化表示為：

3)計算激光雷達的旋轉(zhuǎn)

定義雷達完成一次掃描覆蓋為sweep，P_m表示在sweep m期間接受的點云集，假設一次sweep期間的角速度和線性加速度為常量，通過插值的方法獲得不同時刻的變換，設t為當前時間，t_m為sweep m的開始時間，為雷達在[t_m,t]期間的位姿變換，給定點p_i,p_i∈P_m,t_i是點p_i的時間戳，是雷達在[t_m,t_i]期間的位姿變換，通過線性插值估計：

定義Γ_m為從P_m中提取的邊緣點和平面的集合，將邊緣點和平面點投影到sweep m開始的時候，即t_m，得到點集構(gòu)造如下的幾何關系：

其中是點p_i的坐標，p_i∈Γ_m，是點p_i投影到t_m時刻的雷達坐標系下的坐標，在每次sweep結(jié)束的時刻，在該sweep期間接收到的點云都會被投影到該sweep結(jié)束的時刻，比如在sweep m-1期間接受到的點云P_m-1，被投影到t_m時刻的雷達坐標系下，得到點集點集是其中的邊緣點和平面點的集合，在集合和中尋找匹配點，得到匹配點之間距離的計算公式：

對于Γ_m中的所有點，構(gòu)造如下的非線性方程：

通過求解最小二乘解，得到的最優(yōu)解；

4)異步測量的對齊

基于短時間內(nèi)勻速運動的假設，求得相同時間段內(nèi)的不同傳感器的旋轉(zhuǎn)；

選擇相機作為基準傳感器，t_i和t_j是兩幀連續(xù)圖片的采集時間，在這段時間內(nèi)，慣性測量單元有多次采樣，t′_i是最接近且晚于或等于t_i的IMU采樣時間，t′_j是最接近且晚于或等于t_j的IMU采樣時間；首先用步驟2)中的方法對[t′_i，t′_j]間的IMU數(shù)據(jù)進行預積分，得到這段時間的IMU旋轉(zhuǎn)雷達在期間的旋轉(zhuǎn)由步驟3)中的方法計算得到，是最接近時間[t_i,t_j]的雷達sweep時間，定義分數(shù)γ表示相對旋轉(zhuǎn)需要被擴展或者縮小的程度：

其中t₀和t₁是根據(jù)傳感器測量直接計算得到的相對旋轉(zhuǎn)的起始時間和結(jié)束時間，t′₀和t′₁是期望得到的相對旋轉(zhuǎn)的起始時間和結(jié)束時間；

定義相對旋轉(zhuǎn)的插值如下：

其中Exp(·)是李代數(shù)和李群之間的指數(shù)映射，Log(·)是李群和李代數(shù)之間的對數(shù)映射；

5)計算傳感器之間的外參

對齊相同時間的不同傳感器的旋轉(zhuǎn)序列估計傳感器之間的外部朝向，通過步驟1),2)，3),4)得到[t_i,t_j]期間的相機旋轉(zhuǎn)IMU旋轉(zhuǎn)激光雷達的旋轉(zhuǎn)對于任何i和j，ij，下面的式子成立：

其中表示從IMU坐標系到相機坐標系的旋轉(zhuǎn)，表示從相機坐標系到雷達坐標系的旋轉(zhuǎn)，表示從雷達到IMU坐標系的旋轉(zhuǎn)；

線性系統(tǒng)的每一行都有相同的形式：

為了恢復旋轉(zhuǎn)矩陣使用單位四元數(shù)代替上式中的矩陣：

符號表示四元數(shù)的乘法，定義單位四元數(shù)為：

其中

將不斷到達的旋轉(zhuǎn)載入線性系統(tǒng)，得到超定線性方程組：

其中N是旋轉(zhuǎn)序列的大小，α是異常值處理的權(quán)重；隨著新的測量的增加，這個線性系統(tǒng)會進行重構(gòu)，因此使用之前計算的外部朝向來估計誤差，誤差被定義為：

其中‖·‖是向量的模，權(quán)重定義為：

其中s是放大殘差影響的因子，對于三個傳感器之間的標定，得到下式：

通過奇異值分解求解上面的超定線性方程，最小奇異值對應的奇異向量就是最小二乘解；

隨著新的數(shù)據(jù)不斷到來，標定的外參朝向會逐漸收斂到固定值，當局部時間段τ內(nèi)的外部旋轉(zhuǎn)的對應的歐拉角的方差分別小于閾值時，認為標定成功，表達式如下：

其中分別是四元數(shù)對應的偏航、俯仰和滾轉(zhuǎn)角在時間段τ內(nèi)的方差，η是根據(jù)經(jīng)驗設置的閾值，