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

背景技術(shù)：

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

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

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

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

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

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

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

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

s2，測量三腳架的三個(gè)斜邊sa、sb、sc長度及三個(gè)底邊ab、bc、ac的長度，建立三腳架坐標(biāo)系，求解所述三腳架坐標(biāo)系的各頂點(diǎn)坐標(biāo)；

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

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

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

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

s2，測量三腳架的三個(gè)斜邊sa、sb、sc長度及三個(gè)底邊ab、bc、ac的長度，建立三腳架坐標(biāo)系，求解所述三腳架坐標(biāo)系的各頂點(diǎn)坐標(biāo)；

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

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

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

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

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

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

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

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

附圖說明

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

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

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

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

圖5為本發(fā)明的實(shí)驗(yàn)結(jié)果的示意圖。

具體實(shí)施方式

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

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

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

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

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

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

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

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

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

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

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

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

令可得三角形面積為

m²+l²＝|ab|²

n²+l²＝|bc|²

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

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

s3-1，對激光雷達(dá)獲取的三腳架的掃描點(diǎn)進(jìn)行聚類運(yùn)算，將激光掃描點(diǎn)分成三類，并計(jì)算各類的中心點(diǎn)a,b,c的坐標(biāo)(pa，pb，pc)；

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

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

s3-2，依據(jù)所述中心點(diǎn)a,b,c的坐標(biāo)，計(jì)算三個(gè)中心點(diǎn)a,b,c之間的距離(lab，lbc，lac)。

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

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

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ù)三角關(guān)系，可得如下方程：

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

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

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

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

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

從該激光雷達(dá)傳感器坐標(biāo)系到三腳架坐標(biāo)系的坐標(biāo)變換中，記旋轉(zhuǎn)矩陣為r,平移矩陣為t。則有：

oa＝rpa+t(19)

ob＝rpb+t(20)

oc＝rpc+t(21)

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

s3-5，按步驟s3-3和s3-4依次計(jì)算各激光雷達(dá)相對于三腳架坐標(biāo)系下的旋轉(zhuǎn)矩陣ri和平移矩陣ti。

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

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

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

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

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

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