亚洲免费av电影一区二区三区,日韩爱爱视频,51精品视频一区二区三区,91视频爱爱,日韩欧美在线播放视频,中文字幕少妇AV,亚洲电影中文字幕,久久久久亚洲av成人网址,久久综合视频网站,国产在线不卡免费播放

        ?

        基于ICP和NDT的激光點(diǎn)云匹配方法研究

        2022-04-28 14:10:14吳長(zhǎng)水
        關(guān)鍵詞:里程計(jì)關(guān)鍵幀立方體

        朱 鋒,吳長(zhǎng)水,茅 健

        (上海工程技術(shù)大學(xué) 機(jī)械與汽車(chē)工程學(xué)院,上海 201620)

        0 引 言

        自動(dòng)駕駛技術(shù)是當(dāng)前的研究熱點(diǎn),車(chē)輛定位技術(shù)是自動(dòng)駕駛不可或缺的部分,良好的定位可以幫助車(chē)輛實(shí)現(xiàn)自動(dòng)駕駛功能。同時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)是車(chē)輛定位的關(guān)鍵技術(shù)之一,指的是搭載特定傳感器的機(jī)器人或者車(chē)輛,在沒(méi)有先驗(yàn)信息的環(huán)境中,通過(guò)自身運(yùn)動(dòng)過(guò)程建立環(huán)境地圖,并估計(jì)自身的運(yùn)動(dòng)。根據(jù)傳感器的不同,SLAM技術(shù)又可分為視覺(jué)SLAM和激光SLAM兩大類(lèi)。近年來(lái),這2種SLAM技術(shù)取得了長(zhǎng)足的進(jìn)步,并逐漸在產(chǎn)品應(yīng)用上落地。尤其是在自動(dòng)駕駛領(lǐng)域,激光雷達(dá)的應(yīng)用逐漸普及起來(lái),三維激光SLAM技術(shù)將起到舉足輕重的作用。

        激光SLAM框架可以大致分為5個(gè)模塊:傳感器數(shù)據(jù)讀取、前端里程計(jì)、后端優(yōu)化、回環(huán)檢測(cè)、建圖。其中,前端里程計(jì)是激光SLAM的重要步驟,其功能是將激光雷達(dá)采集到的點(diǎn)云數(shù)據(jù)做相鄰幀之間的匹配,從而得出相鄰幀間位姿關(guān)系,再根據(jù)上一幀的位姿,來(lái)估計(jì)出當(dāng)前幀的位姿。三維激光前端里程計(jì)幀間匹配方法主要分為直接匹配和和特征匹配兩類(lèi)。直接匹配法又可細(xì)分為迭代最近點(diǎn)ICP和正態(tài)分布變換NDT。

        Chen等人提出的ICP算法,通過(guò)待匹配的2幀點(diǎn)云建立歐氏距離并使其最小化,不斷迭代直至滿足設(shè)定的終止條件,從而得到相對(duì)位姿變化。但激光點(diǎn)云數(shù)據(jù)往往較大,導(dǎo)致ICP算法耗時(shí)長(zhǎng);而且ICP算法需要提供一個(gè)較好的初值,否則最終迭代結(jié)果可能會(huì)陷入局部最優(yōu)。Censi提出了PL-ICP算法(point-to-line ICP),該法精度較高,適用于2D激光SLAM,缺點(diǎn)是對(duì)初值更敏感,容易陷入局部極值。Low提出了PP-ICP算法(point-toplane ICP),該算法精度高且適用于3D激光SLAM。

        Biber等人提出一種二維NDT的匹配方法,Magnusson等人在此基礎(chǔ)上將二維NDT推廣到三維匹配中,其核心思想是將點(diǎn)云數(shù)據(jù)投放到由小立方體組成的網(wǎng)格中,每個(gè)立方體中的點(diǎn)云轉(zhuǎn)換成一個(gè)概率密度函數(shù),然后求出點(diǎn)云之間的匹配關(guān)系。三維NDT算法初始化效果好,運(yùn)行速度快,魯棒性高,在3D激光SLAM使用較多。

        Zhang等人提出的特征匹配算法,其思想是先計(jì)算曲率,并按曲率提取線特征和面特征,再和上一幀做匹配,最后求出相對(duì)位姿。LOAM方案在純激光匹配算法中很具有代表性,長(zhǎng)期霸榜于KITTI數(shù)據(jù)集前三位。LeGO-LOAM在LOAM的基礎(chǔ)上對(duì)地面特征做了分割,減小了特征搜索范圍,從而節(jié)省了計(jì)算資源;并加入了回環(huán)修正,精度也有所提升。LIO-mapping提出了一種LiDAR與IMU緊耦合融合方法,在LOAM的基礎(chǔ)上加入IMU,基于滑動(dòng)窗口方法,把雷達(dá)線/面特征、IMU預(yù)積分等的約束放在一起進(jìn)行優(yōu)化,得到良好的建圖效果。LIO-SAM在LeGO-LOAM的基礎(chǔ)上,先通過(guò)點(diǎn)云特征計(jì)算出相對(duì)位姿,再利用相對(duì)位姿、IMU預(yù)積分和GPS做融合,分2步完成,相比于直接一步做緊耦合,大大提高了效率。上述列舉的方案中,前端激光里程計(jì)模塊都是基于特征匹配得到的。

        本文的研究對(duì)象是三維激光SLAM,針對(duì)前端激光里程計(jì),用2種不同的點(diǎn)云匹配方法:迭代最近點(diǎn)ICP和正態(tài)分布變換NDT,在KITTI數(shù)據(jù)集上分別進(jìn)行點(diǎn)云配準(zhǔn),同時(shí)也評(píng)估了這2種點(diǎn)云匹配方法的效率和精度。

        1 前端激光里程計(jì)匹配方法

        1.1 迭代最近點(diǎn)

        迭代最近點(diǎn)(ICP)算法的核心思想,即通過(guò)待匹配的2幀點(diǎn)云建立歐氏距離并使其最小化,不斷迭代直至滿足設(shè)定的終止條件,從而得到相對(duì)位姿變化。圖1為擬合示意圖。

        圖1 擬合示意圖Fig.1 Fitting schematic diagram

        然后構(gòu)建最小二乘問(wèn)題,求使得誤差平方和達(dá)到最?。?/p>

        觀察上式等號(hào)右側(cè)2項(xiàng),第一項(xiàng)只和有關(guān),第二項(xiàng)和都有關(guān)。只要求得旋轉(zhuǎn)矩陣,再令第二項(xiàng)為零即可求得平移。由2組點(diǎn)集的質(zhì)心,,令:

        計(jì)算有關(guān)的誤差項(xiàng),得:

        對(duì)3×3矩陣進(jìn)行SVD分解(奇異值分解),得:

        當(dāng)滿秩時(shí),解得

        1.2 正態(tài)分布變換

        正態(tài)分布變換(NDT)核心思想是將點(diǎn)云數(shù)據(jù)投放到由小立方體組成的網(wǎng)格中,每個(gè)立方體中的點(diǎn)云轉(zhuǎn)換成一個(gè)概率密度函數(shù),然后通過(guò)數(shù)學(xué)手段求出點(diǎn)云之間的匹配關(guān)系。圖2為通過(guò)概率刻畫(huà)點(diǎn)云,左邊為原始點(diǎn)云,右邊為每個(gè)方塊內(nèi)點(diǎn)的概率。

        圖2 概率刻畫(huà)點(diǎn)云Fig.2 Point cloud description with probability

        根據(jù)預(yù)測(cè)的位姿,對(duì)點(diǎn)進(jìn)行旋轉(zhuǎn)和平移:

        旋轉(zhuǎn)和平移后的點(diǎn)與目標(biāo)點(diǎn)集中的點(diǎn)在同一坐標(biāo)系下,此時(shí)可計(jì)算各點(diǎn)的聯(lián)合概率:

        所有點(diǎn)的聯(lián)合概率:

        取對(duì)數(shù),去除常數(shù)項(xiàng),簡(jiǎn)化問(wèn)題,則目標(biāo)函數(shù)變?yōu)椋?/p>

        按照高斯牛頓法的求解流程,計(jì)算殘差函數(shù)關(guān)于代求參數(shù)的雅可比,便可迭代優(yōu)化。

        2 基于數(shù)據(jù)集的實(shí)現(xiàn)

        本文是基于KITTI數(shù)據(jù)集實(shí)現(xiàn)的,使用的數(shù)據(jù)包為kitti_2011_10_03_drive_0027_synced.bag,對(duì)其中的激光雷達(dá)點(diǎn)云數(shù)據(jù),分別添加了前端里程計(jì)的算法ICP和NDT,算法流程見(jiàn)圖3和圖4,測(cè)試算法并得到了里程計(jì)的軌跡,最后和gnss軌跡做數(shù)據(jù)對(duì)比分析。

        圖3 ICP算法流程Fig.3 The flowchart of ICP algorithm

        圖4 NDT算法流程圖Fig.4 The flowchart of NDT algorithm

        里程計(jì)子模塊功能設(shè)計(jì)如下:

        (1)點(diǎn)云匹配。用pcl庫(kù)設(shè)置匹配參數(shù),從接收到一幀點(diǎn)云開(kāi)始,和地圖進(jìn)行匹配,如果是第一幀數(shù)據(jù),那么就認(rèn)為是地圖,供下一幀匹配使用。本文要把里程計(jì)軌跡和gnss軌跡做對(duì)比,所以把初始時(shí)刻gnss和imu給出的位姿作為里程計(jì)的初始位姿。

        (2)提取關(guān)鍵幀。如果把每一幀匹配好的點(diǎn)云都加入地圖,會(huì)使地圖太大,所以要提取關(guān)鍵幀,即每隔一段距離取一幀點(diǎn)云,用關(guān)鍵幀來(lái)拼接成地圖。

        (3)滑窗。關(guān)鍵幀會(huì)不斷增加,地圖會(huì)一直累加,那么會(huì)導(dǎo)致很多不必要的計(jì)算量,所以把時(shí)間靠前的關(guān)鍵幀給剔除,把和當(dāng)前幀一定距離范圍內(nèi)的關(guān)鍵幀找出來(lái)拼接即可。

        (4)點(diǎn)云濾波。在匹配之前需要濾波,對(duì)點(diǎn)云稀疏化。這里點(diǎn)云濾波是直接采用了pcl庫(kù)中的voxel_filter,其基本原理就是把三維空間劃分成等尺寸的立方體格子,在一個(gè)立方體格子內(nèi)最多只留一個(gè)點(diǎn)。濾波格子大小決定了匹配的效率和精度,格子越小,點(diǎn)越多,精度越高,但是速度越慢,反之速度加快,精度下降;可適當(dāng)調(diào)整參數(shù),對(duì)比效果。

        (5)位姿預(yù)測(cè)。點(diǎn)云匹配有一個(gè)特性,對(duì)位姿的預(yù)測(cè)值比較敏感,所以在載體運(yùn)動(dòng)時(shí),不能以其上一幀的位姿作為這一幀的預(yù)測(cè)值,可以使用IMU預(yù)測(cè),也可以使用運(yùn)動(dòng)模型預(yù)測(cè),此處采用運(yùn)動(dòng)模型來(lái)做位姿預(yù)測(cè)。假如當(dāng)前幀是第幀,那么用第2幀位姿和第1幀位姿就可以計(jì)算一個(gè)位姿變化量,在1幀位姿基礎(chǔ)上累加這個(gè)位姿變化量,就可認(rèn)為是第幀的預(yù)測(cè)值。

        3 實(shí)驗(yàn)結(jié)果分析

        本文中用到的計(jì)算機(jī)配置:Intel i7-9750H處理器,16 G內(nèi)存,系統(tǒng)環(huán)境為Ubuntu18.04和ROS Melodic1.14。使用精度評(píng)估工具evo,把對(duì)應(yīng)的gnss數(shù)據(jù)和激光里程計(jì)數(shù)據(jù)進(jìn)行分析處理。采用的評(píng)價(jià)指標(biāo)如下:

        (1)絕對(duì)位姿誤差(absolute pose error,)。用于比較估計(jì)軌跡和參考軌跡并計(jì)算整個(gè)軌跡的統(tǒng)計(jì)數(shù)據(jù),此處估計(jì)軌跡為激光里程計(jì),參考軌跡為gnss。

        (2)相對(duì)位姿誤差(relative pose error,)。相對(duì)位姿誤差比較運(yùn)動(dòng)過(guò)程中的姿態(tài)增量,可以給出局部精度,如slam系統(tǒng)每米的平移或者旋轉(zhuǎn)漂移量。

        將ICP算法和NDT算法分別在KITTI數(shù)據(jù)集上進(jìn)行仿真實(shí)驗(yàn),得出了的效果如圖5所示。圖5(a)、圖5(b)中的藍(lán)色軌跡是gnss軌跡,綠色軌跡為激光里程計(jì)。對(duì)比2種算法,可以發(fā)現(xiàn)激光里程計(jì)均出現(xiàn)了一定程度上的漂移。具體來(lái)說(shuō),圖5(a)中的漂移比較嚴(yán)重,圖5(b)中的漂移比較輕。下面將給出數(shù)據(jù)做定量分析。

        圖5 KITTI測(cè)試效果圖Fig.5 KITTI test effect diagram

        圖6和圖7分別展示了ICP和NDT的位姿誤差分析圖,將圖中數(shù)據(jù)整理到表1和表2中。在表1、表2中,max表示最大誤差,表示平均誤差,表示誤差中位數(shù),min表示最小誤差,表示均方根誤差,表示標(biāo)準(zhǔn)差。

        圖6 ICP方法的絕對(duì)位姿誤差和相對(duì)位姿誤差Fig.6 APE and RPE of ICP

        圖7 NDT方法的絕對(duì)位姿誤差和相對(duì)位姿誤差Fig.7 APE and RPE of NDT

        表1 絕對(duì)位姿誤差對(duì)比Tab.1 The comparison of APE

        表2 相對(duì)位姿誤差對(duì)比Tab.2 The comparison of RPE

        在絕對(duì)位姿誤差中,ICP比NDT的最大誤差和均方根誤差都要大很多;在相對(duì)位姿誤差對(duì)比中,ICP的誤差比NDT的誤差也大很多。根據(jù)圖5(a)和圖5(b),在運(yùn)動(dòng)開(kāi)始時(shí)沒(méi)有旋轉(zhuǎn),對(duì)于平移,ICP和NDT精度幾乎相當(dāng);在第三個(gè)轉(zhuǎn)彎處,ICP出現(xiàn)了較大的漂移誤差;在第七個(gè)轉(zhuǎn)彎處,ICP累積漂移更大,此處由于沒(méi)有較好的初值,最終迭代結(jié)果陷入了局部最優(yōu),因而軌跡出現(xiàn)了失真。由上可以得出:在角度變化時(shí),NDT的魯棒性明顯優(yōu)于ICP;且ICP需要提供一個(gè)較好的初值,否則最終迭代結(jié)果可能會(huì)陷入局部極值。

        4 結(jié)束語(yǔ)

        本文利用三維激光點(diǎn)云中2種匹配算法:迭代最近點(diǎn)(ICP)和正態(tài)分布變換(NDT)算法,實(shí)現(xiàn)了前端激光里程計(jì)的功能。為了比較ICP和NDT算法的效率和精度,在KITTI數(shù)據(jù)集上進(jìn)行了仿真實(shí)驗(yàn)。實(shí)驗(yàn)中,用gnss軌跡作為真值,激光里程計(jì)作為估計(jì)值,給出了2種算法的絕對(duì)位姿誤差和相對(duì)位姿誤差,結(jié)果表明NDT算法具有更好的效率和精度,具體表現(xiàn)為如下2點(diǎn):

        (1)對(duì)于前端激光里程計(jì),NDT的匹配精度優(yōu)于ICP。

        (2)由于ICP需要較好的初值且易受旋轉(zhuǎn)影響,ICP更適用于精匹配,NDT則更適用于粗匹配。

        本文實(shí)現(xiàn)了基本的激光里程計(jì)功能,但是對(duì)里程計(jì)來(lái)講,漂移是不可避免的。后續(xù)的工作,是增加其他約束來(lái)消除累計(jì)誤差,從而構(gòu)建高精地圖。

        猜你喜歡
        里程計(jì)關(guān)鍵幀立方體
        疊出一個(gè)立方體
        室內(nèi)退化場(chǎng)景下UWB雙基站輔助LiDAR里程計(jì)的定位方法
        基于單目直接法的水下視覺(jué)慣性里程計(jì)方法
        一種單目相機(jī)/三軸陀螺儀/里程計(jì)緊組合導(dǎo)航算法
        基于模板特征點(diǎn)提取的立體視覺(jué)里程計(jì)實(shí)現(xiàn)方法
        基于改進(jìn)關(guān)鍵幀選擇的RGB-D SLAM算法
        圖形前線
        立方體星交會(huì)對(duì)接和空間飛行演示
        太空探索(2016年9期)2016-07-12 09:59:53
        折紙
        基于相關(guān)系數(shù)的道路監(jiān)控視頻關(guān)鍵幀提取算法
        国产亚洲一区二区在线观看| www.日本一区| 亚洲春色视频在线观看| 国产精品国产三级野外国产| 亚洲国产成人av在线观看| 久久久久久久性潮| 国产欧美日韩图片一区二区| 日本免费精品一区二区三区视频| 国产情侣一区二区三区| 2021久久精品国产99国产精品| 91在线在线啪永久地址| 亚洲视频精品一区二区三区| 丰满人妻猛进入中文字幕| 日韩人妻无码精品久久久不卡| 国产成人久久综合热| 五月婷婷激情六月开心| 久久精品日本不卡91| 少妇无码av无码一区| 亚洲日韩精品欧美一区二区三区不卡| 国产精品三级国产精品高| 一级r片内射视频播放免费| 污污内射在线观看一区二区少妇| 亚洲乱码一区二区三区成人小说| 加勒比一区二区三区av| 国产乱码人妻一区二区三区| 中文人妻av久久人妻18| 久久精品中文字幕久久| 一区二区高清视频免费在线观看 | 国产av无码专区亚洲av极速版| 亚洲欧美日韩高清中文在线| 成人免费毛片立即播放| 成年女人a级毛片免费观看| 久久精品国产亚洲av高清色欲| 亚洲国产一区久久yourpan| 国产人妻熟女呻吟在线观看| 乌克兰少妇xxxx做受野外| 国产一级淫片免费播放电影| 国产一区二区黑丝美胸| 国产一区二区三区乱码| 国产99re在线观看只有精品| 国产女主播在线免费看|