本發(fā)明涉及一種多組多線激光雷達的自動標定算法。
背景技術：
：在基于多線激光雷達掃描的三維重建與SLAM應用中，多傳感器的數據融合往往能給算法帶來更好的魯棒性與精度，同時能抓取更多的環(huán)境三維細節(jié)信息，為進一步處理帶來更完善的空間數據。但是，多傳感器系統(tǒng)中，傳感器系統(tǒng)均存在自身的局部坐標，為了統(tǒng)一坐標系，我們必須要進行傳感器之間的坐標系標定，找到傳感器之間的三維坐標變換關系。為了解決此問題，人們提出了許多的方法和技術。最早采用的是手動測量以及用外部儀器測量等物理方法來尋找雷達傳感器之間的坐標關系，這些方法較為簡單，但是操作比較復雜且精度較低。為了提高精度，人們研究了采用特制的靶標來幫助獲取雷達之間的共視圖，從而進行更精確標定的相關技術。靶標標定法在一定程度上提高了標定的精度，但是需要特制帶有特定幾何特征的靶標，操作較為復雜，特別地，在比較稀疏的單幀雷達點云中，很難在雷達的共視數據中找到完全對應的靶標點。因此，如何精確地標定三維激光雷達之間的坐標成為了一個亟待解決的關鍵技術。技術實現要素：本發(fā)明的目的在于克服上述現有技術的不足，提供一種多組多線激光雷達的自動標定算法。為實現上述目的，本發(fā)明采用以下技術方案：一種多組多線激光雷達的自動標定算法，包括以下步驟：S1、基于激光雷達A與激光雷達B的位置關系，估計激光雷達A與激光雷達B的坐標變換關系初值Tguess；S2、利用地圖構建算法對激光雷達A所采集的點云數據進行局部子地圖M的構建；S3、獲取激光雷達A在地圖構建算法中時間0-N的運動軌跡激光雷達A在n時刻的點云數據以及激光雷達B在n時刻的點云數據基于數據同步和軌跡同步的假設，將點云數據通過與Tguess變換到時刻n在局部子地圖M中的位置，并通過最近鄰點云搜索算法找到其在局部子地圖M中的附近點，則有，式中，ApproxNearestNeighbors(*)為最近鄰點云搜索算法；S4、基于環(huán)境一致性約束，求解激光雷達A與激光雷達B的多組標定關系具體為，對0-N時刻中的每組點云分別求取與的距離最小值，則有，......則，優(yōu)選地，還包括步驟S5、根據隨機采樣一致性規(guī)則濾除異常樣本點,則有最后取均值求出Tcali。優(yōu)選地，步驟S1中激光雷達A與激光雷達B的坐標變換關系初值Tguess為手工測量獲得的粗略估計值。優(yōu)選地，步驟S2中構建局部子地圖M的地圖構建算法為基于Generalized-ICP的幀間匹配SLAM算法。采用上述技術方案后，本發(fā)明與
背景技術：
相比，具有如下優(yōu)點：基于激光雷達本身獲取的點云數據來產生一個概率三維模型，利用該模型本身的約束找到傳感器之間的坐標變換關系統(tǒng)計模型，并采用隨機采樣一致性規(guī)則消除配準過程中的歧義性問題，獲得精確的坐標變換，完成多組多線雷達的自動標定。附圖說明圖1為雙線雷達三維掃描系統(tǒng)；圖2為圖1中雷達A的坐標系表示；圖3為實施例1中公視點手動配準方法的雷達及靶標布置示意圖；圖4為實施例1中公視點手動配準方法獲取的點云數據示意圖圖5為實施例2中雷達A與雷達B的自動標定示意圖；圖6為實施例2中自動標定算法流程框圖；圖7為實施例2中第一組掃描數據的矩陣曲線；圖8為實施例2中第二組掃描數據的矩陣曲線。具體實施方式為了使本發(fā)明的目的、技術方案及優(yōu)點更加清楚明白，以下結合附圖及實施例，對本發(fā)明進行進一步詳細說明。應當理解，此處所描述的具體實施例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。如圖1所示的是一種獲取構建地圖所需點云數據的雙線雷達三維掃描系統(tǒng)，由于單顆雷達的視野較窄，因而通過兩顆雷達進行交叉安裝，來同步獲取更完整的三維數據。其中，雷達A置于水平上方，雷達B以45°角安裝于水平雷達的下方。如圖2所示的是圖1中雷達A的坐標系表示。在上述系統(tǒng)中，激光雷達A與激光雷達B均存在自身的局部坐標，為了統(tǒng)一坐標系，我們必須要進行傳感器之間的坐標系標定，找到傳感器之間的三維坐標變換關系。實施例1采用手動標定方式進行激光雷達A與激光雷達B的標定。如圖3所示，采用公視點手動配準方法，對雷達公視區(qū)域進行方形孔靶標放置，由于雷達公視區(qū)域較小，為了保證靶標采樣點數的密集，我們設置靶標孔大小為200x200mm，并將靶標A與B分別放置在系統(tǒng)左右邊大約一米位置。如圖4所示，得到兩組點云。由圖4可以看到，多線雷達的點云非常稀疏，難以在兩組點云公視的方塊孔靶標中找到對應點，所以我們采用RANSAC擬合(RANSAC擬合屬于現有技術，在此不做贅述)的方式，從中擬合出兩個方形，在方形角點上找到對應點進行坐標變換，最后計算的變換矩陣就是兩組點云的標定矩陣。使用靶標點，來對手動標定得到的估計標定矩陣進行誤差分析，(計算手動標定后的靶標對應點之間的平均位移距離及旋轉角度距離和總體最小均方誤差)能夠得到X,Y,Z，Roll，Yaw，Pitch的誤差及均方誤差，兩組實驗誤差如下：TABLE1SceneIDX_error(mm)Y_error(mm)Z_error(mm)Root_mean_error(mm)S115.89620.2717.37726.795S221.96023.38910.25733.682TABLE2SceneIDRoll_error(Radian)Yaw_error(Radian)Pitch_error(Radian)Root_mean_error(Radian)S10.0530.0600.0820.114S20.0660.0130.0960.116實施例2采用多組多線激光雷達自動標定算法對圖1所示的雙線雷達三維掃描系統(tǒng)進行自動標定。將系統(tǒng)放置在任意結構化場景中進行數據采集，本實施例使用走廊環(huán)境，兩組場景。每組場景1次行走，總共2組數據，每組數據分別采集550幀點云數據。如圖5所示，采用地圖構建算法對雷達A采集到的點云數據進行短時建圖，獲得子地圖M，雷達B的數據經過初值旋轉和同步位姿，變換到雷達A的地圖對應位置附近，而后進行自動標定與修正。具體步驟如圖6所示：S1、基于激光雷達A與激光雷達B的位置關系，估計激光雷達A與激光雷達B的坐標變換關系初值Tguess。坐標變換關系初值Tguess為手工測量或其它方式獲得的估計值。S2、利用地圖構建算法對激光雷達A所采集的點云數據進行局部子地圖M的構建。本實施例中采用的地圖構建算法為基于Generalized-ICP的幀間匹配SLAM算法。S3、獲取激光雷達A在地圖構建算法中時間0-N的運動軌跡激光雷達A在n時刻的點云數據以及激光雷達B在n時刻的點云數據基于數據同步(雙雷達的同步數據獲取中，外部的環(huán)境在雷達共視圖中的公共區(qū)域是同構的)及軌跡同步(雙雷達的同步數據獲取中，兩個雷達的運動軌跡在同一個時間內是相同的)的假設，將點云數據通過與Tguess變換到時刻n在局部子地圖M中的位置，并通過最近鄰點云搜索算法找到其在局部子地圖M中的附近點，則有，式中，ApproxNearestNeighbors(*)為最近鄰點云搜索算法；S4、基于環(huán)境一致性約束，求解激光雷達A與激光雷達B的多組標定關系具體為，對0-N時刻中的每組點云分別求取與的距離最小值，則有，......則，由于本實施例采用的是ICP算法，ICP算法在沒有明顯特征(比如走廊環(huán)境)的單一環(huán)境的時候容易陷入局部極小值，導致優(yōu)化后的解不是最優(yōu)解而是陷入局部極小值的次優(yōu)解，最終導致配準算法的失敗。因而，在步驟S4求解Tcali之后，還包括步驟S5。S5、根據隨機采樣一致性規(guī)則濾除異常樣本點，則有最終在濾除異常樣本點以后使用均值估計最終標定矩陣。如圖7與圖8所示的分別是2組數據矩陣曲線，其中，橫坐標為550幀點云數據，縱坐標為X、Y、Z、Roll、Pitch以及Yaw六組數據(分別對應圖中從上到下的六條曲線)，這六組數據整體均趨向于一個常數，圖中所示的波動點即ICP配準失敗的異常點。如此，濾除所述異常樣本點后，即得到了兩組估計矩陣，使用實施例1實驗環(huán)境中的靶標點來對自動標定得到的估計標定矩陣進行誤差分析(計算手動標定后的靶標對應點之間的平均位移距離及旋轉角度距離和總體最小均方誤差)能夠得到X,Y,Z，Roll，Yaw，Pitch的誤差及均方誤差，兩組實驗誤差如下：TABLE3SceneIDX_error(mm)Y_error(mm)Z_error(mm)Root_mean_error(mm)S16.3725.9433.1299.258S27.8775.2766.13411.291TABLE4SceneIDRoll_error(Radian)Yaw_error(Radian)Pitch_error(Radian)Root_mean_error(Radian)S10.0180.0220.0370.046S20.0290.0260.0240.045可以看出，實施例2所采用的自動標定算法，其位移標定精度與姿態(tài)標定精度均由于實施例1中的傳統(tǒng)手工靶標標定精度，顯示出了優(yōu)異的性能。實施例2中的自動標定算法只需要根據激光雷達傳感器的同步數據，就能自動標定雷達之間的坐標關系，高效快捷，并且本方法能推廣到多雷達系統(tǒng)的標定中。以上所述，僅為本發(fā)明較佳的具體實施方式，但本發(fā)明的保護范圍并不局限于此，任何熟悉本
技術領域：
的技術人員在本發(fā)明揭露的技術范圍內，可輕易想到的變化或替換，都應涵蓋在本發(fā)明的保護范圍之內。因此，本發(fā)明的保護范圍應該以權利要求的保護范圍為準。當前第1頁1 2 3