本發(fā)明涉及距離傳感器的位置標定技術，更具體地，涉及距離傳感器外參數(shù)的標定方法。

背景技術：

在環(huán)境感知測量系統(tǒng)中，為了保證感知系統(tǒng)的魯棒性，采用多個傳感器獲取環(huán)境信息是未來發(fā)展趨勢。例如道路三維測量系統(tǒng)，自動駕駛系統(tǒng)，機器人環(huán)境感知系統(tǒng)中等，通常會采用多個距離傳感器如激光掃描傳感器來獲取環(huán)境信息。

多傳感器位姿標定是進行多傳感器信息融合的必要處理步驟。例如，在使用來自多個激光掃描傳感器的數(shù)據(jù)之前，需要對各個傳感器的相對位置和姿態(tài)進行標定，以便將各激光傳感器的數(shù)據(jù)統(tǒng)一到同一個坐標系下進行計算。

由于激光雷達獲取的是距離數(shù)據(jù)，不包含紋理特征，且激光雷達掃描時發(fā)出的激光為不可見光，因此很難獲取同名特征點以用于標定計算，使得多激光傳感器6dof(六自由度)外參數(shù)標定難以實現(xiàn)。

目前通常采用圓柱長桿和棋盤格標定板來實現(xiàn)多激光傳感器外參數(shù)標定。采用圓柱長桿標定只適用于同一平面內(nèi)的多激光傳感器的標定，即只能實現(xiàn)3dof(三自由度)外參數(shù)標定。采用棋盤格標定方法可以實現(xiàn)6dof參數(shù)標定，但是，一方面需要借助相機來輔助,同時也使得標定精度會受到相機自身標定精度的影響。另一方面，需要在不同位置擺放標定板并多次采集數(shù)據(jù)，使得標定過程費時費力。

技術實現(xiàn)要素：

本發(fā)明的目的是解決多個距離傳感器之間的外參數(shù)自動校準問題。傳感器的外參數(shù)包括位置和姿態(tài)(x,y,z,yaw,pitch,roll)等6個變量。為此，本發(fā)明中提出了一種新型的多個距離傳感器6dof外參數(shù)標定裝置和標定方法。

本發(fā)明的距離傳感器外參數(shù)標定方法包括：

s1，布置一個三腳架，三腳架位于待標定的距離傳感器的掃描范圍內(nèi)，三腳架包括s、a、b和c四個頂點，其中點a、b、c位于底面上；

s2，測量三腳架的三個斜邊sa、sb、sc長度及三個底邊ab、bc、ac的長度，建立三腳架坐標系，求解所述三腳架坐標系的各頂點坐標；

s3，根據(jù)各個距離傳感器的掃描數(shù)據(jù)，計算各個距離傳感器在所述三腳架坐標系下的位置參數(shù)；以及

s4，根據(jù)各個距離傳感器相對于三腳架坐標系下的旋轉矩陣和平移矩陣，選擇其中一個距離傳感器為參考坐標系，計算其他距離傳感器相對與該距離傳感器參考坐標系下的旋轉矩陣和平移矩陣。

本發(fā)明還提出一種距離傳感器外參數(shù)標定系統(tǒng)，包括：處理器，所述處理器被配置為執(zhí)行如下步驟：

s1，布置一個三腳架，并使三腳架位于待標定的距離傳感器的掃描范圍內(nèi)，三腳架包括s、a、b和c四個頂點，其中點a、b、c位于底面上；

s2，測量三腳架的三個斜邊sa、sb、sc長度及三個底邊ab、bc、ac的長度，建立三腳架坐標系，求解所述三腳架坐標系的各頂點坐標；

s3，根據(jù)各個距離傳感器的掃描數(shù)據(jù)，計算各個距離傳感器在所述三腳架坐標系下的位置參數(shù)；以及

s4，根據(jù)各個距離傳感器相對于三腳架坐標系下的旋轉矩陣和平移矩陣，選擇其中一個距離傳感器為參考坐標系，計算其他距離傳感器相對與該距離傳感器參考坐標系下的旋轉矩陣和平移矩陣。

本發(fā)明的技術效果包括：

1、本發(fā)明中提出了一種新型的標定裝置和標定算法，無需借助相機即可實現(xiàn)多激光傳感器的6dof位置參數(shù)配準。

2、由于標定中不需要使用相機,使得標定結果不受相機本身的標定精度影響,避免了標定過程中誤差的累積,標定結果精度高。

3、本發(fā)明無需移動或者旋轉待標定激光雷達，并且可實現(xiàn)單次擺放標定裝置即可完成兩個或者兩個以上激光雷達之間的標定，提高標定效率。

4、使用單次測量數(shù)據(jù)即可完成標定工作,可大幅提高標定效率。

5、本發(fā)明中采用三腳架作為標定裝置，制作簡單，實用性好，易于應用推廣。

附圖說明

圖1為本發(fā)明的方法的流程圖；

圖2為本發(fā)明的方法的原理圖；

圖3為本發(fā)明的方法的實施示意圖；

圖4為本發(fā)明的三腳架坐標系的示意圖；

圖5為本發(fā)明的實驗結果的示意圖。

具體實施方式

下面參照附圖描述本發(fā)明的實施方式，其中相同的部件用相同的附圖標記表示。

本發(fā)明以激光掃描傳感器為例來進行描述，但是本發(fā)明不僅僅適用于激光掃描傳感器，其他距離傳感器也適用，比如kinect和rangesensor。

本發(fā)明的主要原理是，利用三腳架作為各個激光雷達的共同參照坐標系，標定時先計算各個激光雷達在該參照坐標系下的位置參數(shù)(6dof)，然后再計算各個激光雷達之間的相對位置參數(shù)，得到標定結果。無需像傳統(tǒng)方法一樣通過找到同名點的方式，實現(xiàn)傳感器之間的位姿標定。

圖1-2顯示了本發(fā)明的方法的流程圖。

在s1中，布置一個三腳架，各個激光雷達圍繞該三腳架放置，其中三腳架包括四個頂點為s、a、b和c，其中點a、b、c位于底面上，如圖3所示。三腳架放置在各個激光雷達的檢測重合范圍內(nèi)。

對于兩個激光雷達之間的標定，需要將三角架放在兩個激光雷達的檢測范圍之內(nèi)。

對于三個以上的激光雷達之間的標定，至少需要將三腳架放在兩個及以上的激光雷達的檢測重合范圍之內(nèi)。例如：如需標定三個激光雷達a、b、c之間的位置關系，第一次先將三腳架放置于激光雷達a,b的掃描范圍內(nèi)，標定a、b之間的位置關系；第二次將三角架放置于激光雷達b、c的掃描范圍內(nèi)，標定激光雷達b、c的位置關系；然后通過坐標轉換則可實現(xiàn)激光雷達a、c之間的標定。

若三腳架同時位于a、b、c三個激光雷達的掃描范圍內(nèi)，則放置一次三角架即可實現(xiàn)三個激光雷達之間的標定。

再次參考圖1，在s2中，測量三腳架的三個斜邊sa、sb、sc長度及三個底邊ab、bc、ac的長度，建立三腳架坐標系，求解所述三腳架坐標系的各頂點坐標。

具體為，以三腳架底面作為三腳架坐標系xy平面，以ac所在邊為x軸，以過b點且與ac邊垂直的直線為y軸，以與xy軸垂直且過xy軸交點的直線為z軸，建立以三腳架為參照系的直角坐標系(oxyz)。

如圖4所示，根據(jù)三腳架各斜邊長度，利用幾何關系建立方程，求解三腳架頂點s、a、b、c在所述三腳架參考坐標系下的坐標值(oa，ob，oc，os)：a點坐標為(-m，0，0)，b點坐標為(0，l，0)，c點坐標為(n，0，0)，s點坐標為(0，f，h)。

已知三斜邊邊長|ab|，|bc|，|bc|，求解l，m，n的值。根據(jù)幾何關系，可得如下方程：

令可得三角形面積為

m²+l²＝|ab|²

n²+l²＝|bc|²

已知四面體六邊邊長，根據(jù)海倫公式可求解h的值，已知h,|ob|，|sb|長，根據(jù)三角關系可求解f的值。

再次參考圖1，在s3中，根據(jù)各個激光雷達的掃描數(shù)據(jù)，計算各個激光雷達在所述三腳架坐標系下的位置參數(shù)，即標定各個激光雷達與所述三腳架的相對位置關系。步驟s3具體包括：

s3-1，對激光雷達獲取的三腳架的掃描點進行聚類運算，將激光掃描點分成三類，并計算各類的中心點a,b,c的坐標(pa，pb，pc)；

由于三腳架有三個斜邊，激光雷達掃描時候會分別掃描到三腳架的三邊。聚類運算就是提取激光掃描數(shù)據(jù)中對應于這三條斜邊的激光掃描點。各類中心點代表激光掃描平面與三角架的三斜邊的理論交點。

當激光雷達掃描到其中的某一條斜邊時候，可能得到多個激光雷達掃描點，對激光點進行聚類并計算該類的中心，目的是為了更準確地計算該邊與激光掃描平面的交點。其原理是把多個掃描點的平均值(即中心點)當做交點，減少單個激光掃描點的測量誤差帶來的影響。

s3-2，依據(jù)所述中心點a,b,c的坐標，計算三個中心點a,b,c之間的距離(lab，lbc，lac)。

s3-3，依據(jù)幾何關系，求解三個中心點a,b,c在所述三腳架坐標系下的坐標值(oα，ob，oc)。計算方法如下：

假設激光雷達l掃描平面與三腳架的交點為a，b，c。其在激光雷達坐標系下的坐標(x，y)可記為pa，pb，pc。點a，b，c在三腳架坐標系下的三維坐標(x，y，z)可記作oa，ob，oc，該坐標值為未知量。根據(jù)幾何關系，可以得到如下方程。

lsa＝||os-oa||(1)

laa＝||oa-oa||(2)

lsa＝||os-oa||(3)

lsb＝||os-ob||(4)

lbb＝||ob-ob||(5)

lsb＝||os-ob||(6)

lsc＝||os-oc||(7)

lcc＝||oc-oc||(8)

lsc＝||os-oc||(9)

lsa＝lsa+laa(10)

lsb＝lsb+lbb(11)

lsc＝lsc+lcc(12)

lab＝||oa-ob||＝||pa-pb||(13)

lac＝||oa-oc||＝||pa-pc||(14)

lbc＝||ob-oc||＝||pb-pc||(15)

記三腳架斜邊sa與sb的夾角為α，三腳架斜邊sa與sc的夾角為β，三腳架斜邊sb與sc的夾角為γ，根據(jù)三角關系，可得如下方程：

lsa²+lsb²-2lsalsbcosα＝lab²(16)

lsa²+lsc²-2lsalsccosβ＝lac²(17)

lsb²+lsc²-2lsb-lsccosγ＝lbc²(18)

聯(lián)立方程(1)～(18)，可計算出a，b，c三點在三腳架坐標系下的坐標oa,ob,oc。

s3-4，根據(jù)三個中心點a,b,c在三腳架坐標系下的坐標和在激光雷達坐標系下的坐標，計算旋轉矩陣r和平移矩陣t。其計算方法如下：

從該激光雷達傳感器坐標系到三腳架坐標系的坐標變換中，記旋轉矩陣為r,平移矩陣為t。則有：

oa＝rpa+t(19)

ob＝rpb+t(20)

oc＝rpc+t(21)

聯(lián)立方程(19)～(21)，可計算出旋轉矩陣r和平移矩陣t。

s3-5，按步驟s3-3和s3-4依次計算各激光雷達相對于三腳架坐標系下的旋轉矩陣ri和平移矩陣ti。

再次參考圖1，在s4中，根據(jù)各個激光雷達相對于三腳架坐標系下的旋轉矩陣和平移矩陣，選擇其中一個激光雷達為參考坐標系，計算其他激光雷達相對與該激光雷達參考坐標系下的旋轉矩陣和平移矩陣即得到兩激光雷達之間的相對位置關系，完成標定。

具體地，對三腳架坐標系下任意一點(x,y,z),根據(jù)上述結果可得如下方程：

聯(lián)立方程(22)～(23)，可得到兩激光雷達之間的相對位置關系，完成校準。

如圖5所示，本發(fā)明已經(jīng)通過實驗和模擬實驗兩種方法進行了實驗驗證，證明了本發(fā)明的方法可行，可以實現(xiàn)多個距離傳感器的6dof外參數(shù)標定。在上述實驗中，通過本發(fā)明所提出的標定方法，計算出了三個激光雷達的相互位置關系，并在2維和3維空間中將標定后的激光雷達的相互位置關系可視化。通過可視化，使得不可見的激光掃描平面及激光與三腳架的交點等變?yōu)榭梢?，可用于輔助確認各傳感器位置和姿態(tài)標定的結果。

本發(fā)明還對應提出一種距離傳感器外參數(shù)標定系統(tǒng)，其包括處理器，所述處理器裝載有程序，所述程序運行時，實現(xiàn)如上所述的各方法步驟。

以上所述的實施例，只是本發(fā)明較優(yōu)選的具體實施方式，本領域的技術人員在本發(fā)明技術方案范圍內(nèi)進行的通常變化和替換都應包含在本發(fā)明的保護范圍內(nèi)。