日本无码免费高清在线|成人日本在线观看高清|A级片免费视频操逼欧美|全裸美女搞黄色大片网站|免费成人a片视频|久久无码福利成人激情久久|国产视频一二国产在线v|av女主播在线观看|五月激情影音先锋|亚洲一区天堂av

  • 手機(jī)站
  • 小程序

    汽車測試網(wǎng)

  • 公眾號

    • 汽車測試網(wǎng)

    • 在線課堂

    • 電車測試

FAST-LIO2: 快速直接的激光雷達(dá)-慣性里程計(jì)

2021-09-12 21:53:49·  來源：同濟(jì)智能汽車研究所  
　
k表示激光雷達(dá)掃描的索引，
j=1,... ,m} 表示在掃描結(jié)束時(shí)間時(shí)，在局部激光雷達(dá)坐標(biāo)系L中第k次掃描采到的點(diǎn)。由于激光雷達(dá)測量噪聲，每個(gè)測量點(diǎn)通常都會(huì)受到由測距和波束定向噪聲組成的噪聲
的污染。去除此噪聲可得到在局部激光雷達(dá)坐標(biāo)系中的真實(shí)點(diǎn)位置
 
 
 
在使用相應(yīng)的激光雷達(dá)姿態(tài)
和外參
將真實(shí)點(diǎn)投影到全局坐標(biāo)系之后，這個(gè)真實(shí)點(diǎn)應(yīng)該正好位于地圖中的一個(gè)局部小平面塊上，即：
 
 
其中
是對應(yīng)平面的法向量，
是平面上的一個(gè)點(diǎn)（見圖2）。需要注意的是，
和
都包含在狀態(tài)向量xk中。因此，第 j個(gè)點(diǎn)測量值
所貢獻(xiàn)的測量值可以從（4）總結(jié)為更緊湊的形式，如下所示
 
 
這定義了狀態(tài)向量的隱式測量模型.
 
 
圖2 測量模型
B.迭代卡爾曼濾波
基于在流形
上制定的狀態(tài)模型(2)和測量模型(5)，我們采用迭代卡爾曼濾波器，按照[55]和[22]中的過程直接在流形M上操作。它由兩個(gè)關(guān)鍵步驟組成：每此IMU測量值的傳播和每此激光雷達(dá)掃描的迭代更新，這兩個(gè)步驟都自然地估計(jì)流形上的狀態(tài)，從而避免任何重整化。由于IMU測量的頻率通常高于激光雷達(dá)掃描的頻率（例如，IMU測量為200Hz，激光雷達(dá)掃描為10Hz~100Hz），因此濾波器在更新之前通常會(huì)執(zhí)行多個(gè)傳播步驟。
1）傳播：假設(shè)在融合最后一次（即第k-1次）激光雷達(dá)掃描后的最佳狀態(tài)估計(jì)是為
，協(xié)方差矩陣為
。前向傳播是在IMU測量值到達(dá)時(shí)執(zhí)行的。更具體地說，通過將過程噪聲wi設(shè)置為零，狀態(tài)和協(xié)方差按照(2)進(jìn)行傳播：
 
 
其中Qi是噪聲wi的協(xié)方差，矩陣
和
的計(jì)算如下（參見[55]中更抽象的推導(dǎo)和[22]中更具體的推導(dǎo)）：
 
 
前向傳播一直持續(xù)到新的（即第k次）掃描結(jié)束，新掃描中的傳播狀態(tài)和協(xié)方差表示為
。
2）殘差計(jì)算：假設(shè)在當(dāng)前迭代更新時(shí)狀態(tài)xk的估計(jì)值（見第IV-B3節(jié)）是
，根據(jù)（6）中的傳播預(yù)測出的狀態(tài)，當(dāng)k=0時(shí)（即在第一次迭代前），
。然后，我們將每個(gè)測量的激光雷達(dá)點(diǎn)投影到全局坐標(biāo)系
，并在由ikd-Tree表示的地圖中搜索其最近的5個(gè)點(diǎn)（參見第V-A節(jié)）。然后使用找到的最近相鄰點(diǎn)來擬合局部小平面塊，該小平面塊具有測量模型中使用的法向量
和質(zhì)心
分享到：
 
反對 0 舉報(bào) 0 收藏 0 評論 0
滬ICP備11026917號-25