本發(fā)明屬于激光定位與導(dǎo)航技術(shù)領(lǐng)域，特別地涉及一種基于二維激光掃描儀角點(diǎn)特征的閉環(huán)檢測(cè)方法。在閉環(huán)檢測(cè)方法中，涉及了二維點(diǎn)云的簽名生成方法和相似點(diǎn)云間的相對(duì)位姿計(jì)算方法。

背景技術(shù)：

在slam中，只是依靠scanmatch或者里程計(jì)來(lái)建立地圖的話，由于誤差累積問(wèn)題的存在，最終生成的地圖會(huì)與實(shí)際環(huán)境有很大的偏差，而閉環(huán)算法作為校正這一偏差的重要方法，具有非常大的研究?jī)r(jià)值。一般而言，閉環(huán)算法可以分為兩個(gè)步驟，首先需要進(jìn)行地點(diǎn)識(shí)別，然后需要計(jì)算相似兩幀之間的相對(duì)位姿進(jìn)而生成圖模型。地點(diǎn)識(shí)別的主要任務(wù)是當(dāng)移動(dòng)機(jī)器人從地點(diǎn)a出發(fā)后行進(jìn)一段時(shí)間再度回到地點(diǎn)a時(shí)，需要能夠識(shí)別出它之前到過(guò)地點(diǎn)a，即能檢測(cè)到當(dāng)前在a地掃到的數(shù)據(jù)幀f與出發(fā)時(shí)在a地掃到的數(shù)據(jù)幀f’具有相對(duì)較高的相似度。在地點(diǎn)識(shí)別完成之后，移動(dòng)機(jī)器人需要計(jì)算兩幀之間的相對(duì)位姿，同時(shí)根據(jù)得到的相對(duì)位姿也可以對(duì)兩幀之間的相似度進(jìn)行驗(yàn)證。在視覺領(lǐng)域，學(xué)者們已經(jīng)對(duì)閉環(huán)算法進(jìn)行了非常廣泛的研究。相比之下，基于2dlidar的slam閉環(huán)算法研究工作就顯得寥寥無(wú)幾。而在基于2dlidar的閉環(huán)算法本就不多的研究工作中，利用柵格地圖的研究又占了很大的比重。利用原始2d點(diǎn)云數(shù)據(jù)來(lái)進(jìn)行閉環(huán)檢測(cè)的工作不多的一個(gè)重要原因就是上一章中所提到的，即針對(duì)2dlidar進(jìn)行特征提取的研究工作不多。而特征點(diǎn)作為閉環(huán)檢測(cè)中地點(diǎn)識(shí)別環(huán)節(jié)的重要判定依據(jù)，與其相關(guān)的研究工作的缺乏直接導(dǎo)致了在原始2d點(diǎn)云數(shù)據(jù)上進(jìn)行閉環(huán)檢測(cè)工作的缺乏。從2010年以來(lái)，隨著flirt、falko等算法的提出，基于2dlidar原始數(shù)據(jù)的閉環(huán)檢測(cè)算法也打下了基礎(chǔ)。在之后的時(shí)期里，也出現(xiàn)了一些很有意義的基于2dlidar的閉環(huán)檢測(cè)工作，諸如gfp、glare、gsr以及glarot(glarerotation-invariant)等，其中前三者是基于flirt特征，glarot是基于falko。這些工作的一個(gè)共同點(diǎn)就是利用這些針對(duì)2dlidar的特征提取算法來(lái)將每幀數(shù)據(jù)壓縮為一張用提取出的特征點(diǎn)生成的簽名，這樣就可以通過(guò)計(jì)算兩張簽名之間的距離大小來(lái)判斷兩幀數(shù)據(jù)之間的相似性，從而完成地點(diǎn)識(shí)別的任務(wù)。簽名的生成方式可以分為兩大類，一種需要通過(guò)結(jié)合特征描述子，例如gfp用特征描述子順時(shí)針方向組合形成的短語(yǔ)來(lái)作為單幀數(shù)據(jù)的簽名，另一種則是通過(guò)對(duì)特征點(diǎn)之間的相對(duì)位置關(guān)系進(jìn)行離散化生成一個(gè)二維數(shù)組來(lái)作為單幀數(shù)據(jù)的簽名，其中的代表是glare。glare對(duì)單幀數(shù)據(jù)中每對(duì)特征點(diǎn)之間連成的線段的距離d和角度θ進(jìn)行離散化成nd和nθ，考慮到噪聲的影響，glare沒有采用one-hot的方式而是用一個(gè)以(nd,nθ)為中心的二元高斯分布來(lái)進(jìn)行建模，最后所有特征點(diǎn)對(duì)形成的二元高斯分布之和作為最后的簽名。gsr是在glare的基礎(chǔ)上進(jìn)行了改進(jìn)，它把特征點(diǎn)之間的角度定義為兩點(diǎn)法向量的角度之差，這使得glare具有了旋轉(zhuǎn)不變性，相應(yīng)的代價(jià)是計(jì)算量變大。glarot與glare和gsr不同之處在于它是基于falko特征。與gsr一樣，它也具有旋轉(zhuǎn)不變性，但它實(shí)現(xiàn)旋轉(zhuǎn)不變性的方式與gsr實(shí)現(xiàn)旋轉(zhuǎn)不變性的方式并不相同。gsr是在glare的基礎(chǔ)上，改變了簽名生成方式來(lái)獲得旋轉(zhuǎn)不變性，而glarot的簽名生成方式與glare相同，它通過(guò)改變計(jì)算兩張簽名之間相似度的方式來(lái)獲得旋轉(zhuǎn)不變性，這樣使得glarot的計(jì)算量相比gsr要減輕很多。檢索得到相似度最高的幀后，需要解算出兩幀之間的相對(duì)位姿。

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

本發(fā)明針對(duì)構(gòu)建二維點(diǎn)云地圖累積誤差過(guò)大的情況，設(shè)計(jì)了一種基于二維激光掃描儀角點(diǎn)特征的閉環(huán)檢測(cè)方法，用于減小二維點(diǎn)云地圖創(chuàng)建過(guò)程中的累積誤差。本發(fā)明具體可以分為點(diǎn)云簽名生成以及相對(duì)位姿計(jì)算兩大部分；第一，提出了一種具有旋轉(zhuǎn)不變性的點(diǎn)云簽名生成方法；第二，提出了一種高精度的相似點(diǎn)云之間相對(duì)位姿的計(jì)算方法。

本發(fā)明的技術(shù)方案為：一種基于二維激光掃描儀角點(diǎn)特征的閉環(huán)檢測(cè)方法，利用從二維激光掃描儀獲得的二維點(diǎn)云數(shù)據(jù)中提取出的角點(diǎn)特征f，對(duì)二維點(diǎn)云數(shù)據(jù)c進(jìn)行以下步驟處理：

步驟1：利用提出得到的二維點(diǎn)云角點(diǎn)特征f，生成二維點(diǎn)云c的簽名，該簽名具有旋轉(zhuǎn)不變性的特征。根據(jù)兩幀點(diǎn)云簽名之間的歐氏距離來(lái)衡量?jī)蓭c(diǎn)云之間的相似性。

步驟2：利用點(diǎn)云角點(diǎn)特征之間的幾何結(jié)構(gòu)來(lái)計(jì)算相似兩幀點(diǎn)云之間的相對(duì)位姿，并用兩幀點(diǎn)云之間的相對(duì)位姿構(gòu)建圖模型，對(duì)圖模型進(jìn)行優(yōu)化后輸出優(yōu)化后的全局位姿。

進(jìn)一步地，點(diǎn)云類型為二維點(diǎn)云，方法旨在對(duì)二維點(diǎn)云數(shù)據(jù)進(jìn)行閉環(huán)檢測(cè)，減小二維點(diǎn)云地圖的累積誤差。

進(jìn)一步地，二維點(diǎn)云的簽名通過(guò)以下方法得到：

(1.1)對(duì)二維點(diǎn)云中任意兩個(gè)特征點(diǎn)之間形成的線段的距離和角度進(jìn)行離散化。

(1.2)對(duì)離散化后的距離和角度進(jìn)行高斯建模。

(1.3)綜合所有特征點(diǎn)對(duì)相應(yīng)的高斯模型，生成二維點(diǎn)云的歸一化后的簽名。

(1.4)將得到的簽名沿角度軸平移n個(gè)槽位，得到二維點(diǎn)云簽名對(duì)應(yīng)的n個(gè)平行簽名。

(1.5)對(duì)任意兩幀點(diǎn)云的簽名，其距離定義為一幀點(diǎn)云的簽名與另一幀點(diǎn)云m個(gè)平行簽名之間最小的歐式距離。

進(jìn)一步地，兩個(gè)特征點(diǎn)之間形成的線段的距離和角度離散化通過(guò)以下方法得到：

(1.1.1)對(duì)其中任意兩個(gè)特征點(diǎn)fi和fj，其對(duì)應(yīng)的坐標(biāo)為(xi,yi)和(xj,yj)，則兩個(gè)特征點(diǎn)之間形成的線段的距離和角度為：

對(duì)dij和進(jìn)行等距離離散化，離散化后的就對(duì)應(yīng)著一個(gè)大小為nd行，nψ列的二維直方圖中的一個(gè)槽：

進(jìn)一步地，對(duì)離散化后的距離和角度進(jìn)行高斯建模通過(guò)以下方法得到：所建立的高斯分布模型gij的中心在nij＝(nd，nψ)，方差為ωij。

進(jìn)一步地，平行簽名通過(guò)以下方法得到：簽名的角度軸槽的個(gè)數(shù)為nψ，對(duì)得到的簽名沿著角度軸循環(huán)平移nψ個(gè)槽位，得到nψ個(gè)平行簽名。

進(jìn)一步地，兩幀點(diǎn)云簽名之間的距離通過(guò)以下方法得到：選出兩幀簽名signature1、signature2的峰值，其所在的角度槽值分別為nψ1、nψ2，兩者之間差值為δψ＝nψ1-nψ2，選取一個(gè)合適的范圍[δψ-δ,δψ+δ]，從signature2對(duì)應(yīng)的nψ個(gè)平行簽名中選取平移量在上述范圍內(nèi)的共計(jì)2δ個(gè)平行簽名，并從中選取與signature1之間最小的歐式距離作為兩幀簽名之間的最終距離。

進(jìn)一步地，相似兩幀點(diǎn)云之間的相對(duì)位姿通過(guò)以下方法得到：

2.1)兩幀簽名對(duì)應(yīng)的數(shù)據(jù)幀中的角點(diǎn)分別構(gòu)成了一個(gè)無(wú)向圖，無(wú)向圖中的節(jié)點(diǎn)對(duì)應(yīng)數(shù)據(jù)幀中的角點(diǎn)，無(wú)向圖中兩個(gè)節(jié)點(diǎn)之間的邊為對(duì)應(yīng)數(shù)據(jù)幀中兩角點(diǎn)之間的歐式距離，得到的兩個(gè)無(wú)向圖分別為g1(v,e)和g2(v,e)。

2.2)對(duì)于g1(v,e)中每一條邊edge1i，從g2(v,e)中選取與其長(zhǎng)度最接近的邊為edge2i，若兩條邊之間長(zhǎng)度的差值在閾值以內(nèi)，就認(rèn)為兩條邊匹配上，反之，認(rèn)為edge1i在g2(v,e)中不存在與其匹配的邊。

2.3)對(duì)于匹配上的兩條邊edge1i和edge2i，分別對(duì)應(yīng)著兩個(gè)節(jié)點(diǎn)為和由此產(chǎn)生個(gè)配對(duì)，對(duì)這4個(gè)節(jié)點(diǎn)對(duì)分別投一票。

2.4)遍歷完g1(v,e)中每一條邊后，得到對(duì)所有點(diǎn)對(duì)的投票結(jié)果，對(duì)g1(v,e)中的每個(gè)節(jié)點(diǎn)，選取與其關(guān)聯(lián)的配對(duì)得票數(shù)最高的節(jié)點(diǎn)作為g2(v,e)中與之配對(duì)的節(jié)點(diǎn)，若最高的得票數(shù)為0，則認(rèn)為該節(jié)點(diǎn)在g2(v,e)中沒有與之匹配的節(jié)點(diǎn)。

2.5)計(jì)算所有配對(duì)上的兩個(gè)節(jié)點(diǎn)之間的平均相對(duì)位移作為兩幀點(diǎn)云之間的相對(duì)位姿。

本發(fā)明的有益效果是：

(1)本發(fā)明設(shè)計(jì)了一種二維點(diǎn)云簽名生成方法，生成的簽名具有旋轉(zhuǎn)不變性，可以根據(jù)兩幀簽名之間的距離來(lái)衡量?jī)蓭c(diǎn)云之間的相似性。

(2)給出了一種相似兩幀點(diǎn)云之間的評(píng)價(jià)函數(shù)，可以精確地計(jì)算出兩幀點(diǎn)云之間的相對(duì)位姿。

附圖說(shuō)明

圖1為本發(fā)明基于二維激光掃描儀角點(diǎn)特征的閉環(huán)檢測(cè)方法的步驟流程圖；

圖2為本發(fā)明二維點(diǎn)云簽名生成示例；

具體實(shí)施方式

為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖及實(shí)施例，對(duì)本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說(shuō)明。應(yīng)該理解，此處所描述的具體實(shí)施例僅用以解釋本發(fā)明，并不用于限定本發(fā)明。

相反，本發(fā)明涵蓋任何由權(quán)利要求定義的在本發(fā)明的精髓和范圍上做的替代、修改、等效方法以及方案。進(jìn)一步，為了使公眾對(duì)本發(fā)明有更好的了解，在下文對(duì)本發(fā)明的細(xì)節(jié)描述中，詳盡描述了一些特定的細(xì)節(jié)部分。對(duì)本領(lǐng)域技術(shù)人員來(lái)說(shuō)沒有這些細(xì)節(jié)部分的描述可以完全理解本發(fā)明。

基于二維激光掃描儀角點(diǎn)特征的閉環(huán)檢測(cè)方法，利用從二維激光掃描儀獲得的二維點(diǎn)云數(shù)據(jù)中提取出的角點(diǎn)特征f，對(duì)二維點(diǎn)云數(shù)據(jù)c進(jìn)行以下步驟處理：

步驟1：利用提出得到的二維點(diǎn)云角點(diǎn)特征f，生成二維點(diǎn)云c的簽名，該簽名具有旋轉(zhuǎn)不變性的特征；根據(jù)兩幀點(diǎn)云簽名之間的歐氏距離來(lái)衡量?jī)蓭c(diǎn)云之間的相似性；

二維點(diǎn)云的簽名通過(guò)以下方法得到：

1.1)對(duì)二維點(diǎn)云中任意兩個(gè)特征點(diǎn)之間形成的線段的距離和角度進(jìn)行離散化；

兩個(gè)特征點(diǎn)之間形成的線段的距離和角度離散化通過(guò)以下方法得到：

對(duì)其中任意兩個(gè)特征點(diǎn)fi和fj，其對(duì)應(yīng)的坐標(biāo)分別為(xi,yi)和(xj,yj)，則兩個(gè)特征點(diǎn)之間形成的線段的距離dij和角度為：

對(duì)dij和進(jìn)行等距離離散化，離散化后的對(duì)應(yīng)一個(gè)大小為nd行，nψ列的二維直方圖中的一個(gè)槽bin(nd，nψ)：

1.2)對(duì)離散化后的距離和角度進(jìn)行高斯建模；所建立的高斯分布模型gij的中心在nij＝(nd，nψ)，方差為ωij。

1.3)綜合所有特征點(diǎn)對(duì)相應(yīng)的高斯模型，生成二維點(diǎn)云的歸一化后的簽名；

1.4)將得到的簽名沿角度軸平移n個(gè)槽位，得到二維點(diǎn)云簽名對(duì)應(yīng)的n個(gè)平行簽名；平行簽名通過(guò)以下方法得到：簽名的角度軸槽的個(gè)數(shù)為nψ，對(duì)得到的簽名沿著角度軸循環(huán)平移nψ個(gè)槽位，得到nψ個(gè)平行簽名。

1.5)對(duì)任意兩幀點(diǎn)云的簽名，其距離定義為一幀點(diǎn)云的簽名與另一幀點(diǎn)云m個(gè)平行簽名之間最小的歐式距離；

兩幀點(diǎn)云簽名之間的距離通過(guò)以下方法得到：選出兩幀簽名signature1、signature2的峰值，其所在的角度槽值分別為nψ1、nψ2，兩者之間差值為δψ＝nψ1-nψ2，選取一個(gè)范圍[δψ-δ,δψ+δ]，從signature2對(duì)應(yīng)的nψ個(gè)平行簽名中選取平移量在上述范圍內(nèi)的共計(jì)2δ個(gè)平行簽名，并從中選取與signature1之間最小的歐式距離作為兩幀簽名之間的最終距離。

步驟2：利用點(diǎn)云角點(diǎn)特征之間的幾何結(jié)構(gòu)來(lái)計(jì)算相似兩幀點(diǎn)云之間的相對(duì)位姿，并用兩幀點(diǎn)云之間的相對(duì)位姿構(gòu)建圖模型，相似兩幀點(diǎn)云之間的相對(duì)位姿通過(guò)以下方法得到：

2.1)兩幀簽名對(duì)應(yīng)的數(shù)據(jù)幀中的角點(diǎn)分別構(gòu)成了一個(gè)無(wú)向圖，無(wú)向圖中的節(jié)點(diǎn)對(duì)應(yīng)數(shù)據(jù)幀中的角點(diǎn)，無(wú)向圖中兩個(gè)節(jié)點(diǎn)之間的邊為對(duì)應(yīng)數(shù)據(jù)幀中兩角點(diǎn)之間的歐式距離，得到的兩個(gè)無(wú)向圖分別為g1(v,e)和g2(v,e)；

2.2)對(duì)于g1(v,e)中每一條邊edge1i，從g2(v,e)中選取與其長(zhǎng)度最接近的邊為edge2i，若兩條邊之間長(zhǎng)度的差值在閾值以內(nèi)，就認(rèn)為兩條邊匹配上，反之，認(rèn)為edge1i在g2(v,e)中不存在與其匹配的邊；

2.3)對(duì)于匹配上的兩條邊edge1i和edge2i，分別對(duì)應(yīng)兩個(gè)節(jié)點(diǎn)為和由此產(chǎn)生個(gè)配對(duì)，對(duì)這4個(gè)節(jié)點(diǎn)對(duì)分別投一票；

2.4)遍歷完g1(v,e)中每一條邊后，得到對(duì)所有點(diǎn)對(duì)的投票結(jié)果，對(duì)g1(v,e)中的每個(gè)節(jié)點(diǎn)，選取與其關(guān)聯(lián)的配對(duì)得票數(shù)最高的節(jié)點(diǎn)作為g2(v,e)中與之配對(duì)的節(jié)點(diǎn)，若最高的得票數(shù)為0，則認(rèn)為該節(jié)點(diǎn)在g2(v,e)中沒有與之匹配的節(jié)點(diǎn)；

2.5)計(jì)算所有配對(duì)上的兩個(gè)節(jié)點(diǎn)之間的平均相對(duì)位移作為兩幀點(diǎn)云之間的相對(duì)位姿。

最后，對(duì)圖模型進(jìn)行優(yōu)化后輸出優(yōu)化后的全局位姿。

實(shí)施例1

參考圖1，所示為本發(fā)明實(shí)施例的針對(duì)二維激光掃描儀的角點(diǎn)特征提取方法的步驟流程圖，其具體步驟如前所述，不再贅述，僅給出各步驟的參數(shù)。

通過(guò)分辨率為π/180的二維激光掃描儀獲得二維點(diǎn)云數(shù)據(jù)，進(jìn)行以下步驟處理：

1.利用二維激光掃描儀獲得二維點(diǎn)云數(shù)據(jù)，提取點(diǎn)云中的角點(diǎn)，對(duì)于任意兩個(gè)角點(diǎn)之間的線段，其端點(diǎn)坐標(biāo)為(0,0)，(1,1)，線段的長(zhǎng)度為1.414m，角度為π/4rad。

2.對(duì)線段長(zhǎng)度以0.5m為間隔，角度以π/18rad為間隔進(jìn)行離散化，離散化后線段得到的離散量為(1.414m，π/4rad)∈bin(3，5)。

3.以bin(3，5)為中心進(jìn)行高斯建模，對(duì)應(yīng)的高斯模型為n(nij-bin(3，5),ωij)。

4.對(duì)所有角點(diǎn)重復(fù)步驟1-3，累加得到的高斯模型，并對(duì)最終得到的簽名進(jìn)行歸一化，簽名最終的結(jié)果如圖2。

5.計(jì)算相似兩幀點(diǎn)云之間的相對(duì)位姿，并在圖模型中對(duì)應(yīng)的節(jié)點(diǎn)之間添加一條邊。

6.對(duì)得到的圖模型進(jìn)行優(yōu)化，輸出優(yōu)化后的位姿，完成閉環(huán)檢測(cè)。

以上所述僅為本發(fā)明的較佳實(shí)施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。