本發(fā)明涉及一種激光雷達(dá)點(diǎn)云數(shù)據(jù)障礙檢測(cè)算法。

背景技術(shù)：

激光雷達(dá)作為一種主動(dòng)傳感器，具有對(duì)物體的感知信息來(lái)源于自身，受外界環(huán)境影響很小，在深度信息的獲取上，其可靠性和精確性要高于被動(dòng)傳感器的特點(diǎn)，因而其被廣泛應(yīng)用于環(huán)境感知系統(tǒng)。

無(wú)人車為高速移動(dòng)機(jī)器人,實(shí)時(shí)性要求極高。而三維激光雷達(dá)的原始數(shù)據(jù)量過(guò)于龐大,如直接在原始數(shù)據(jù)上進(jìn)行后續(xù)處理,難以達(dá)到實(shí)時(shí)性的要求。故需要一種高效快速的激光雷達(dá)點(diǎn)云數(shù)據(jù)處理算法。

基于柵格地圖的表示是目前最常用的一種三維數(shù)據(jù)的表示方法，這一類方法對(duì)于每個(gè)柵格都只是保存了部分?jǐn)?shù)據(jù)信息。使得需要處理的數(shù)據(jù)量變小。

柵格障礙檢測(cè)雖然處理數(shù)據(jù)的效率比較高，但主要缺點(diǎn)有:由于多個(gè)障礙物之間遮擋,斷裂以及遠(yuǎn)處柵格因缺失部分三維點(diǎn)云而出現(xiàn)漏檢的問(wèn)題。

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

針對(duì)以上現(xiàn)有技術(shù)問(wèn)題，一種新的激光雷達(dá)點(diǎn)云數(shù)據(jù)障礙檢測(cè)算法，保證障礙物檢測(cè)準(zhǔn)確性的同時(shí)實(shí)現(xiàn)高速處理。

具體技術(shù)方案如下：

一種激光雷達(dá)點(diǎn)云數(shù)據(jù)障礙檢測(cè)算法，其步驟為：

(1)獲取原始數(shù)據(jù)并解析；

(2)柵格地圖投射算法；

(3)一級(jí)柵格信息提??；

(4)二級(jí)柵格建立及信息提取；

(5)障礙物判定：采用分塊鄰域膨脹歷便算法；

(6)得出障礙物坐標(biāo)；

進(jìn)一步地：障礙物判定分塊領(lǐng)域膨脹歷便算法的具體步驟為：

(1)建立一級(jí)柵格；

(2)將一級(jí)柵格投射到二級(jí)柵格；

(3)二級(jí)柵格進(jìn)行八鄰域膨脹；

(4)計(jì)算膨脹后二級(jí)格柵的最小高度；

(5)遍及一級(jí)柵格，計(jì)算一級(jí)柵格與所屬膨脹二級(jí)柵格的高度差；

(6)超過(guò)高度閾值的一級(jí)柵格屬性設(shè)為障礙物；

(7)障礙物柵格計(jì)算直角坐標(biāo)系坐標(biāo)；

進(jìn)一步地，獲取原始數(shù)據(jù)并解析中：通過(guò)實(shí)時(shí)的數(shù)據(jù)包，獲得多維激光雷達(dá)原始數(shù)據(jù)，每個(gè)數(shù)據(jù)包都包含有每一激光束返回的距離信息和角度信息，定義激光雷達(dá)旋轉(zhuǎn)一周輸出的數(shù)據(jù)為一幀數(shù)據(jù)，經(jīng)過(guò)對(duì)原始數(shù)據(jù)的解析，每個(gè)激光雷達(dá)點(diǎn)數(shù)據(jù)包含三維空間位置坐標(biāo)、垂直角度、水平角度、強(qiáng)度、以及激光線束；

進(jìn)一步地，一級(jí)柵格信息提取包括：建立一級(jí)柵格地圖grid[(m+1),(n+1)]，柵格大小為g，將直角坐標(biāo)系下的三維點(diǎn)投射到(m,n)柵格平面上，每個(gè)柵格都只是保存了最大高度zmax，最小高度zmin，計(jì)算三維點(diǎn)的柵格坐標(biāo)(i，j)數(shù)據(jù)信息；

進(jìn)一步地，計(jì)算三維點(diǎn)的柵格坐標(biāo)(i，j)數(shù)據(jù)信息的示例性投射算法為：i＝x/g+sign(x)/2+m/2；j＝y(tǒng)/g+sign(y)/2+n/3，此算法表示裝載激光雷達(dá)的移動(dòng)載體處于一級(jí)柵格中的位置為x方向的中心，y方向的1/3處；

進(jìn)一步地，統(tǒng)計(jì)落在一級(jí)柵格內(nèi)的雷達(dá)三維點(diǎn),對(duì)于最小高度zmin超過(guò)2.5m的障礙點(diǎn),視為懸掛障礙物進(jìn)行剔除。

進(jìn)一步地，二級(jí)柵格建立及信息提取包括：根據(jù)一級(jí)柵格建立二級(jí)柵格地圖secgrid[m/3,n/3]，橫坐標(biāo)ii[0,m/3],縱坐標(biāo)jj[0,n/3]，設(shè)置高度閥值t1，遍歷一級(jí)柵格，對(duì)一級(jí)柵格所屬的二級(jí)柵格進(jìn)行八鄰域膨脹，計(jì)算膨脹后二級(jí)柵格中的最小高度；

進(jìn)一步地，計(jì)算一級(jí)柵格的最大高度zmax^i，j及所屬膨脹二級(jí)柵格最小高度templow之間高度差，若zmax^i，j－templow≥t1，則一級(jí)柵格屬性設(shè)為障礙物；

進(jìn)一步地，柵格屬性為障礙物的柵格轉(zhuǎn)化為直角坐標(biāo)系坐標(biāo)的算法為：

floatx＝(i-m/2)*g–sign(i-m/2)*g/2

floaty＝(j-n/3)*g–sign(j-n/3)*g/2。

本發(fā)明與現(xiàn)有技術(shù)相比，具有如下優(yōu)點(diǎn)和效果：

通過(guò)獲取激光點(diǎn)云數(shù)據(jù)，建立兩級(jí)柵格地圖，首先對(duì)懸掛物進(jìn)行剔除再通過(guò)對(duì)二級(jí)柵格進(jìn)行八鄰域膨脹，遍歷一級(jí)柵格，分區(qū)計(jì)算高度差的算法對(duì)障礙物進(jìn)行判定。本發(fā)明既保留了柵格法快速穩(wěn)定的特點(diǎn)，又解決了多個(gè)障礙物之間遮擋,斷裂以及遠(yuǎn)處柵格因缺失部分三維點(diǎn)云而出現(xiàn)漏檢的問(wèn)題。

附圖說(shuō)明

圖1為本發(fā)明處理流程示意圖

圖2為分塊鄰域膨脹遍歷算法處理流程示意圖

圖3為八鄰域膨脹模板

圖4為普通柵格算法得出的障礙物示意圖(白色1為障礙物屬性)

圖5為本發(fā)明算法得出的障礙物示意圖(白色1為障礙物屬性)

具體實(shí)施方式

以下結(jié)合實(shí)施例，對(duì)本發(fā)明進(jìn)行進(jìn)一步的詳細(xì)說(shuō)明。

通過(guò)實(shí)時(shí)的udp數(shù)據(jù)包，獲得多維激光雷達(dá)原始數(shù)據(jù)。每個(gè)數(shù)據(jù)包都包含有每一激光束返回的距離信息和角度信息。我們定義激光雷達(dá)旋轉(zhuǎn)一周輸出的數(shù)據(jù)為一幀數(shù)據(jù)。

經(jīng)過(guò)對(duì)原始數(shù)據(jù)的解析，每個(gè)激光雷達(dá)點(diǎn)數(shù)據(jù)包含三維空間位置坐標(biāo)(x,y,z)，垂直角度verangle，水平角度horangle，強(qiáng)度intensity，以及激光線束id。

建立一級(jí)柵格地圖grid[(m+1),(n+1)]，柵格大小為g。將直角坐標(biāo)系下的三維點(diǎn)投射到(m,n)柵格平面上。每個(gè)柵格都只是保存了最大高度zmax，最小高度zmin。計(jì)算三維點(diǎn)的柵格坐標(biāo)(i，j)(0≤i≤m；0≤j≤n)數(shù)據(jù)信息，使得需要處理的數(shù)據(jù)量變小。示例性投射算法：i＝x/g+sign(x)/2+m/2；j＝y(tǒng)/g+sign(y)/2+n/3。

此算法表示裝載激光雷達(dá)的移動(dòng)載體處于一級(jí)柵格中的位置為x方向的中心，y方向的1/3處。

統(tǒng)計(jì)落在一級(jí)柵格內(nèi)的雷達(dá)三維點(diǎn)(x,y,z),對(duì)于最小高度zmin超過(guò)2.5m的障礙點(diǎn),視為懸掛障礙物進(jìn)行剔除。

根據(jù)一級(jí)柵格建立二級(jí)柵格地圖secgrid[m/3,n/3]，橫坐標(biāo)ii[0,m/3],縱坐標(biāo)jj[0,n/3]。

設(shè)置高度閥值t1，遍歷一級(jí)柵格，對(duì)一級(jí)柵格所屬的二級(jí)柵格進(jìn)行八鄰域膨脹，計(jì)算膨脹后二級(jí)柵格中的最小高度。計(jì)算一級(jí)柵格的最大高度zmaxi，j及所屬膨脹二級(jí)柵格最小高度templow之間高度差。若zmaxi，j-templow≥t1，則一級(jí)柵格屬性設(shè)為障礙物。

分塊鄰域膨脹遍歷算法偽代碼為

把柵格屬性為障礙物的柵格轉(zhuǎn)化為直角坐標(biāo)系坐標(biāo)(floatx，floaty)。算法為：

floatx＝(i-m/2)*g–sign(i-m/2)*g/2

floaty＝(j-n/3)*g–sign(j-n/3)*g/2

上面結(jié)合附圖對(duì)本發(fā)明進(jìn)行了示例性描述，顯然本發(fā)明具體實(shí)現(xiàn)并不受上述方式的限制，只要采用了本發(fā)明的方法構(gòu)思和技術(shù)方案進(jìn)行的各種改進(jìn)，或未經(jīng)改進(jìn)直接應(yīng)用于其它場(chǎng)合的，均在本發(fā)明的保護(hù)范圍之內(nèi)。