本發(fā)明涉及智能檢測(cè)領(lǐng)域，特別是涉及一種障礙物檢測(cè)方法及系統(tǒng)。

背景技術(shù)：

現(xiàn)代機(jī)器人技術(shù)及無(wú)人駕駛技術(shù)發(fā)展迅速，隨著技術(shù)的不斷成熟，可自主導(dǎo)航及路徑規(guī)劃的機(jī)器人出現(xiàn)在人們的生活中。在享受機(jī)器人技術(shù)及無(wú)人駕駛技術(shù)帶來(lái)的便利、提高生產(chǎn)效率的同時(shí)，機(jī)器人與機(jī)器人之間、機(jī)器人與人之間、機(jī)器人與建筑物之間等碰撞事故時(shí)有發(fā)生，如何避免機(jī)器人的碰撞事故成為了人們重點(diǎn)關(guān)注的問(wèn)題。因此，如何在機(jī)器人自主行動(dòng)過(guò)程中去對(duì)所處環(huán)境中的各類障礙物進(jìn)行檢測(cè)，成為解決碰撞事故的關(guān)鍵。常用障礙物檢測(cè)系統(tǒng)是基于單/雙目攝像頭、毫米波雷達(dá)等傳感器。

其中單目攝像頭檢測(cè)，是先通過(guò)圖像匹配進(jìn)行目標(biāo)識(shí)別(各種障礙等)，再通過(guò)目標(biāo)在圖像中的大小去估算目標(biāo)距離，需要建立并不斷維護(hù)一個(gè)龐大的樣本特征數(shù)據(jù)庫(kù)，保證這個(gè)數(shù)據(jù)庫(kù)包含待識(shí)別目標(biāo)的全部特征數(shù)據(jù)。而雙目攝像頭檢測(cè)，是通過(guò)對(duì)兩幅圖像視差的計(jì)算，直接對(duì)前方景物進(jìn)行距離測(cè)量。無(wú)論單/雙目攝像頭都受環(huán)境影響較大，如環(huán)境的光暗狀況、陰影、溫濕度等的干擾，圖像會(huì)存在不同程度的失真，另外計(jì)算量大，對(duì)計(jì)算單元的性能及算法要求較高。相比于單/雙目攝像頭，毫米波雷達(dá)具有更高的精度及分辨率，探測(cè)范圍也更加廣泛，但其探測(cè)距離受到頻段損耗的直接制約，也無(wú)法感知行人，并且對(duì)周邊所有障礙物無(wú)法進(jìn)行精準(zhǔn)的建模。目前也存在一些多傳感器融合的檢測(cè)系統(tǒng)，但其同樣面臨著計(jì)算量大、受環(huán)境制約等問(wèn)題。

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

本發(fā)明的目的是提供一種障礙物檢測(cè)方法及系統(tǒng)，以解決傳統(tǒng)檢測(cè)方法及系統(tǒng)中計(jì)算量大且受環(huán)境影響大的問(wèn)題。

為實(shí)現(xiàn)上述目的，本發(fā)明提供了如下方案：

一種障礙物檢測(cè)方法，所述方法包括：

獲取n線激光雷達(dá)掃描得到的點(diǎn)云數(shù)據(jù)，所述點(diǎn)云數(shù)據(jù)豎直方向坐標(biāo)不同、水平面上的投影為n條平行直線；

將所述點(diǎn)云數(shù)據(jù)劃分為q個(gè)區(qū)域，所述q個(gè)區(qū)域包括第1區(qū)域、第2區(qū)域至第q區(qū)域；

依次對(duì)所述第1區(qū)域的點(diǎn)云數(shù)據(jù)、所述第2區(qū)域的點(diǎn)云數(shù)據(jù)至所述第q區(qū)域的點(diǎn)云數(shù)據(jù)進(jìn)行平面擬合，對(duì)應(yīng)得到第1平面、第2平面至第q平面；

依次獲取所述第1區(qū)域的點(diǎn)云數(shù)據(jù)、所述第2區(qū)域的點(diǎn)云數(shù)據(jù)至所述第q區(qū)域的點(diǎn)云數(shù)據(jù)中的障礙物的坐標(biāo)；

對(duì)所述障礙物的坐標(biāo)數(shù)據(jù)進(jìn)行體素化處理，得到動(dòng)態(tài)障礙物的坐標(biāo)集合和靜態(tài)障礙物的坐標(biāo)集合。

可選的，所述將所述點(diǎn)云數(shù)據(jù)劃分為q個(gè)區(qū)域，具體包括：

將所述水平面上投影的n條直線中每連續(xù)m條直線對(duì)應(yīng)的點(diǎn)云數(shù)據(jù)劃分為同一區(qū)域，依次得到第1區(qū)域、第2區(qū)域至第q區(qū)域；所述第1區(qū)域的點(diǎn)云數(shù)據(jù)包括所述n條直線中第1至m條直線對(duì)應(yīng)的點(diǎn)云數(shù)據(jù)，所述第2區(qū)域的點(diǎn)云數(shù)據(jù)包括所述n條直線中第m至2m-1條直線對(duì)應(yīng)的點(diǎn)云數(shù)據(jù)，所述第q區(qū)域的點(diǎn)云數(shù)據(jù)包括所述n條直線中第1+(q-1)(m-1)至1+q(m-1)條直線對(duì)應(yīng)的點(diǎn)云數(shù)據(jù)。

可選的，所述依次對(duì)所述第1區(qū)域的點(diǎn)云數(shù)據(jù)、所述第2區(qū)域的點(diǎn)云數(shù)據(jù)至所述第q區(qū)域的點(diǎn)云數(shù)據(jù)進(jìn)行平面擬合，對(duì)應(yīng)得到第1平面、第2平面至第q平面，具體包括：

利用四分位差法構(gòu)造門(mén)函數(shù)壓縮數(shù)據(jù)，確定第i區(qū)域點(diǎn)云數(shù)據(jù)的門(mén)上限和門(mén)下限；其中i＝1，2，……，q；

根據(jù)所述第i區(qū)域點(diǎn)云數(shù)據(jù)的門(mén)上限和所述門(mén)下限確定所述第i區(qū)域點(diǎn)云數(shù)據(jù)更新后的區(qū)域，所述第i區(qū)域點(diǎn)云數(shù)據(jù)更新后的區(qū)域?yàn)樗錾祥T(mén)限和所述下門(mén)限之間的點(diǎn)云數(shù)據(jù)區(qū)域；

根據(jù)所述第i區(qū)域點(diǎn)云數(shù)據(jù)更新后的區(qū)域，利用隨機(jī)抽樣一致性算法進(jìn)行平面擬合，得到第i區(qū)域的初始平面；

當(dāng)i＝1時(shí)，將所述第i區(qū)域的初始平面確定為第i平面；

當(dāng)i>1時(shí)，判斷第i區(qū)域的初始平面是否有效，得到第一判斷結(jié)果；

當(dāng)所述第一判斷結(jié)果表示是時(shí)，確定所述第i區(qū)域的初始平面為第i平面；

當(dāng)所述第一判斷結(jié)果表示否時(shí)，判斷第i+1區(qū)域的初始平面是否有效，得到第二判斷結(jié)果；

當(dāng)所述第二判斷結(jié)果表示是時(shí)，確定所述第i+1區(qū)域的初始平面為第i平面。

可選的，所述利用四分位差法構(gòu)造門(mén)函數(shù)壓縮數(shù)據(jù)，確定第i區(qū)域點(diǎn)云數(shù)據(jù)的門(mén)上限和門(mén)下限，具體包括：

確定所述第i區(qū)域點(diǎn)云數(shù)據(jù)的四分位差iqr＝q75％-q25％；其中iqr表示所述第i區(qū)域中豎直方向坐標(biāo)小于z75％且大于z25％的點(diǎn)云數(shù)據(jù)區(qū)域，其中豎直方向坐標(biāo)z75％＝z1+δz×75％，z25％＝z1+δz×25％，z1為所述第i區(qū)域中點(diǎn)云數(shù)據(jù)中豎直方向坐標(biāo)最小值，δz為所述第i區(qū)域中點(diǎn)云數(shù)據(jù)中數(shù)值方向坐標(biāo)最大值與最小值的差值；

確定所述第i區(qū)域點(diǎn)云數(shù)據(jù)的門(mén)上限為qmax＝q75％；

確定所述第i區(qū)域點(diǎn)云數(shù)據(jù)的門(mén)下限為qmin＝q25％-0.5×(iqr)。

可選的，所述根據(jù)所述第i區(qū)域點(diǎn)云數(shù)據(jù)更新后的區(qū)域，利用隨機(jī)抽樣一致性算法進(jìn)行平面擬合，得到第i區(qū)域的初始平面，具體包括：

在所述第i區(qū)域點(diǎn)云數(shù)據(jù)更新后的區(qū)域，利用三點(diǎn)的隨機(jī)抽樣一致性算法進(jìn)行平面擬合，得到初始擬合后的平面；

判斷所述初始擬合后的平面內(nèi)點(diǎn)數(shù)量是否大于設(shè)定閾值，得到第三判斷結(jié)果；

當(dāng)所述第三判斷結(jié)果表示是時(shí)，將所述初始擬合后的平面確定為第i區(qū)域的初始平面；

當(dāng)所述第三判斷結(jié)果表示否時(shí)，返回利用三點(diǎn)的隨機(jī)抽樣一致性算法進(jìn)行平面擬合的步驟。

可選的，所述當(dāng)i>1時(shí)，判斷第i區(qū)域的初始平面是否有效，得到第一判斷結(jié)果，具體包括：

利用公式計(jì)算所述第i區(qū)域的初始平面與第i-1區(qū)域的初始平面之間的角度差δψi，其中為所述第i區(qū)域的初始平面的單位法向量，為所述第i-1區(qū)域的初始平面的單位法向量；

計(jì)算所述第i區(qū)域的初始平面與第i-1區(qū)域的初始平面之間的高度差δzi；

判斷是否角度差δψi小于角度設(shè)定閾值且高度差δzi小于高度設(shè)定閾值，得到第四判斷結(jié)果；

當(dāng)所述第四判斷結(jié)果表示是時(shí)，確定所述第i區(qū)域的初始平面有效；

當(dāng)所述第四判斷結(jié)果表示否時(shí)，確定所述第i區(qū)域的初始平面無(wú)效。

可選的，所述依次獲取所述第1區(qū)域點(diǎn)云數(shù)據(jù)、所述第2區(qū)域點(diǎn)云數(shù)據(jù)至所述第q區(qū)域點(diǎn)云數(shù)據(jù)中的障礙物的坐標(biāo)，具體包括：

利用公式遍歷計(jì)算第i區(qū)域內(nèi)第i平面外的任一點(diǎn)p到第i平面的距離d；其中p0為所述第i平面上任一點(diǎn)，為所述第i區(qū)域內(nèi)對(duì)應(yīng)第i平面的單位法向量；

判斷所述距離d是否大于距離設(shè)定閾值，得到第五判斷結(jié)果；

當(dāng)所述第五判斷結(jié)果表示是時(shí)，確定所述點(diǎn)p是障礙物點(diǎn)；

獲取所述點(diǎn)p的坐標(biāo)；

當(dāng)所述第五判斷結(jié)果表示否時(shí)，確定所述點(diǎn)p不是障礙物點(diǎn)。

可選的，所述對(duì)所述障礙物的坐標(biāo)數(shù)據(jù)進(jìn)行體素化處理，得到動(dòng)態(tài)障礙物的坐標(biāo)集合和靜態(tài)障礙物的坐標(biāo)集合，具體包括：

對(duì)第一時(shí)刻所述n線激光雷達(dá)掃描后得到的障礙物點(diǎn)，利用公式對(duì)障礙物點(diǎn)進(jìn)行第一體素化處理，其中，為向下取整函數(shù)，x1為任一障礙物點(diǎn)p1的坐標(biāo)，v為體素的大小，為所述障礙物點(diǎn)p1體素化后得到的對(duì)應(yīng)體素的位置坐標(biāo)，對(duì)所述障礙物點(diǎn)進(jìn)行第一體素化處理后，計(jì)算對(duì)應(yīng)體素內(nèi)包含的點(diǎn)個(gè)數(shù)，得到所述障礙物點(diǎn)p1的占用值；

對(duì)第二時(shí)刻所述n線激光雷達(dá)掃描后得到的障礙物點(diǎn)，利用公式對(duì)障礙物點(diǎn)進(jìn)行第二體素化處理，其中所第二時(shí)刻為所述第一時(shí)刻所述n線激光雷達(dá)掃描的后一次掃描時(shí)刻，x2為任一障礙物點(diǎn)p2的坐標(biāo)，為所述障礙物點(diǎn)p2體素化后得到的對(duì)應(yīng)體素的位置坐標(biāo)，對(duì)所述障礙物點(diǎn)進(jìn)行第二體素化處理后，計(jì)算對(duì)應(yīng)體素內(nèi)包含的點(diǎn)個(gè)數(shù)，得到所述障礙物點(diǎn)p2的占用值；

在所述第一體素化處理得到的體素和所述第二體素化處理得到的體素范圍內(nèi)，判斷以所述為中心、以設(shè)定體素閾值為半徑組成的球體范圍內(nèi)的體素的占用值是否與所述的占用值相等，得到第六判斷結(jié)果；

當(dāng)所述第六判斷結(jié)果表示是時(shí)，確定所述障礙物點(diǎn)p2為動(dòng)態(tài)障礙物；

當(dāng)所述第六判斷結(jié)果表示否時(shí)，確定所述障礙物點(diǎn)p2為靜態(tài)障礙物；

獲得動(dòng)態(tài)障礙物的坐標(biāo)集合和靜態(tài)障礙物的坐標(biāo)集合。

一種障礙物檢測(cè)系統(tǒng)，所述系統(tǒng)包括：

點(diǎn)云數(shù)據(jù)獲取模塊，用于獲取n線激光雷達(dá)掃描得到的點(diǎn)云數(shù)據(jù)，所述點(diǎn)云數(shù)據(jù)豎直方向坐標(biāo)不同、水平面上的投影為n條平行直線；

區(qū)域劃分模塊，用于將所述點(diǎn)云數(shù)據(jù)劃分為q個(gè)區(qū)域，所述q個(gè)區(qū)域包括第1區(qū)域、第2區(qū)域至第q區(qū)域；

平面擬合模塊，用于依次對(duì)所述第1區(qū)域的點(diǎn)云數(shù)據(jù)、所述第2區(qū)域的點(diǎn)云數(shù)據(jù)至所述第q區(qū)域的點(diǎn)云數(shù)據(jù)進(jìn)行平面擬合，對(duì)應(yīng)得到第1平面、第2平面至第q平面；

障礙物坐標(biāo)獲取模塊，用于依次獲取所述第1區(qū)域的點(diǎn)云數(shù)據(jù)、所述第2區(qū)域的點(diǎn)云數(shù)據(jù)至所述第q區(qū)域的點(diǎn)云數(shù)據(jù)中的障礙物的坐標(biāo)；

體素化處理模塊，用于對(duì)所述障礙物的坐標(biāo)數(shù)據(jù)進(jìn)行體素化處理，得到動(dòng)態(tài)障礙物的坐標(biāo)集合和靜態(tài)障礙物的坐標(biāo)集合。

可選的，所述體素化處理模塊具體包括：

第一體素化處理單元，用于對(duì)第一時(shí)刻所述n線激光雷達(dá)掃描后得到的障礙物點(diǎn)，利用公式對(duì)障礙物點(diǎn)進(jìn)行第一體素化處理，其中，為向下取整函數(shù)，x1為任一障礙物點(diǎn)p1的坐標(biāo)，v為體素的大小，為所述障礙物點(diǎn)p1體素化后得到的對(duì)應(yīng)體素的位置坐標(biāo)，對(duì)所述障礙物點(diǎn)進(jìn)行第一體素化處理后，計(jì)算對(duì)應(yīng)體素內(nèi)包含的點(diǎn)個(gè)數(shù)，得到所述障礙物點(diǎn)p1的占用值；

第二體素化處理單元，用于對(duì)第二時(shí)刻所述n線激光雷達(dá)掃描后得到的障礙物點(diǎn)，利用公式對(duì)障礙物點(diǎn)進(jìn)行第二體素化處理，其中所第二時(shí)刻為所述第一時(shí)刻所述n線激光雷達(dá)掃描的后一次掃描時(shí)刻，x2為任一障礙物點(diǎn)p2的坐標(biāo)，為所述障礙物點(diǎn)p2體素化后得到的對(duì)應(yīng)體素的位置坐標(biāo)，對(duì)所述障礙物點(diǎn)進(jìn)行第二體素化處理后，計(jì)算對(duì)應(yīng)體素內(nèi)包含的點(diǎn)個(gè)數(shù)，得到所述障礙物點(diǎn)p2的占用值；

占用值判斷單元，用于在所述第一體素化處理得到的體素和所述第二體素化處理得到的體素范圍內(nèi)，判斷以所述為中心、以設(shè)定體素閾值為半徑組成的球體范圍內(nèi)的體素的占用值是否與所述的占用值相等，得到第六判斷結(jié)果；

動(dòng)態(tài)障礙物確定單元，用于當(dāng)所述第六判斷結(jié)果表示是時(shí)，確定所述障礙物點(diǎn)p2為動(dòng)態(tài)障礙物；

靜態(tài)障礙物確定單元，用于當(dāng)所述第六判斷結(jié)果表示否時(shí)，確定所述障礙物點(diǎn)p2為靜態(tài)障礙物；

坐標(biāo)集合獲得單元，用于獲得動(dòng)態(tài)障礙物的坐標(biāo)集合和靜態(tài)障礙物的坐標(biāo)集合。

根據(jù)本發(fā)明提供的具體實(shí)施例，本發(fā)明公開(kāi)了以下技術(shù)效果：以三維激光雷達(dá)為傳感器，通過(guò)分段平面擬合、路面分離、點(diǎn)云體素化幾個(gè)步驟，實(shí)現(xiàn)了三維點(diǎn)云數(shù)據(jù)的大規(guī)模壓縮，大大降低了數(shù)據(jù)處理量，并且不受環(huán)境限制；不僅可以檢測(cè)出環(huán)境中的障礙物，還進(jìn)一步把環(huán)境劃分為三部分：路面區(qū)域(可行區(qū)域)、靜態(tài)障礙物區(qū)域、動(dòng)態(tài)障礙物區(qū)域，為機(jī)器人的有效行為決策提供了更好的環(huán)境信息。

附圖說(shuō)明

為了更清楚地說(shuō)明本發(fā)明實(shí)施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對(duì)實(shí)施例中所需要使用的附圖作簡(jiǎn)單地介紹，顯而易見(jiàn)地，下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例，對(duì)于本領(lǐng)域普通技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)性的前提下，還可以根據(jù)這些附圖獲得其他的附圖。

圖1為本發(fā)明障礙物檢測(cè)方法流程圖；

圖2為本發(fā)明障礙物檢測(cè)系統(tǒng)結(jié)構(gòu)圖；

圖3為本發(fā)明障礙物檢測(cè)方法中激光雷達(dá)掃描示意圖；

圖4為本發(fā)明障礙物檢測(cè)方法中擬合平面示意圖；

圖5為本發(fā)明障礙物檢測(cè)方法中k時(shí)刻障礙物體素化處理后示意圖；

圖6為本發(fā)明障礙物檢測(cè)方法中k+1時(shí)刻障礙物體素化處理后示意圖；

圖7為本發(fā)明障礙物檢測(cè)方法中k+1時(shí)刻障礙物體素化處理后與k時(shí)刻結(jié)合后的示意圖。

具體實(shí)施方式

下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例?；诒景l(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒(méi)有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。

為使本發(fā)明的上述目的、特征和優(yōu)點(diǎn)能夠更加明顯易懂，下面結(jié)合附圖和具體實(shí)施方式對(duì)本發(fā)明作進(jìn)一步詳細(xì)的說(shuō)明。

圖1為本發(fā)明障礙物檢測(cè)方法流程圖。如圖1所示，所述方法包括：

步驟101：獲取點(diǎn)云數(shù)據(jù)。獲取n線激光雷達(dá)掃描得到的點(diǎn)云數(shù)據(jù)，所述點(diǎn)云數(shù)據(jù)豎直方向坐標(biāo)不同、水平面上的投影為n條平行直線。具體可以采用十六線激光雷達(dá)，也可以用三十二線激光雷達(dá)，激光雷達(dá)掃描示意圖具體參見(jiàn)圖3。

步驟102：劃分點(diǎn)云數(shù)據(jù)。將所述點(diǎn)云數(shù)據(jù)劃分為q個(gè)區(qū)域，所述q個(gè)區(qū)域包括第1區(qū)域、第2區(qū)域至第q區(qū)域。具體的，將所述水平面上投影的n條直線中每連續(xù)m條直線對(duì)應(yīng)的點(diǎn)云數(shù)據(jù)劃分為同一區(qū)域，依次得到第1區(qū)域、第2區(qū)域至第q區(qū)域；所述第1區(qū)域的點(diǎn)云數(shù)據(jù)包括所述n條直線中第1至m條直線對(duì)應(yīng)的點(diǎn)云數(shù)據(jù)，所述第2區(qū)域的點(diǎn)云數(shù)據(jù)包括所述n條直線中第m至2m-1條直線對(duì)應(yīng)的點(diǎn)云數(shù)據(jù)，所述第q區(qū)域的點(diǎn)云數(shù)據(jù)包括所述n條直線中第1+(q-1)(m-1)至1+q(m-1)條直線對(duì)應(yīng)的點(diǎn)云數(shù)據(jù)。例如，采用十六線激光雷達(dá)掃描得到的點(diǎn)云數(shù)據(jù)，則獲得16條掃描線點(diǎn)云數(shù)據(jù)，可以將每3條掃描線的點(diǎn)云數(shù)據(jù)劃為一個(gè)區(qū)域，1-4條線的點(diǎn)云數(shù)據(jù)為參考區(qū)域，4-6條線的點(diǎn)云數(shù)據(jù)為第一區(qū)域，6-8條線的點(diǎn)云數(shù)據(jù)為第二區(qū)域，8-10條線的點(diǎn)云數(shù)據(jù)為第三區(qū)域……依次構(gòu)成7個(gè)區(qū)域。

步驟103：進(jìn)行平面擬合。依次對(duì)所述第1區(qū)域的點(diǎn)云數(shù)據(jù)、所述第2區(qū)域的點(diǎn)云數(shù)據(jù)至所述第q區(qū)域的點(diǎn)云數(shù)據(jù)進(jìn)行平面擬合，對(duì)應(yīng)得到第1平面、第2平面至第q平面。對(duì)于每一區(qū)域的點(diǎn)云數(shù)據(jù)進(jìn)行平面擬合具體步驟為：

(1)使用四分位差法構(gòu)造門(mén)函數(shù)壓縮數(shù)據(jù)并更新區(qū)域；

確定所述第i區(qū)域點(diǎn)云數(shù)據(jù)的四分位差iqr＝q75％-q25％；其中iqr表示所述第i區(qū)域中豎直方向坐標(biāo)小于z75％且大于z25％的點(diǎn)云數(shù)據(jù)區(qū)域，其中豎直方向坐標(biāo)z75％＝z1+δz×75％，z25％＝z1+δz×25％，z1為所述第i區(qū)域中點(diǎn)云數(shù)據(jù)中豎直方向坐標(biāo)最小值，δz為所述第i區(qū)域中點(diǎn)云數(shù)據(jù)中數(shù)值方向坐標(biāo)最大值與最小值的差值；

確定所述第i區(qū)域點(diǎn)云數(shù)據(jù)的門(mén)上限為qmax＝q75％；

確定所述第i區(qū)域點(diǎn)云數(shù)據(jù)的門(mén)下限為qmin＝q25％-0.5×(iqr)；

保留z∈(qmin,qmax)的點(diǎn)為更新后的區(qū)域。

(2)針對(duì)更新后的區(qū)域使用隨機(jī)抽樣一致算法從區(qū)域內(nèi)隨機(jī)抽取三個(gè)點(diǎn)構(gòu)造初始平面，當(dāng)初始平面內(nèi)點(diǎn)數(shù)量足夠多的時(shí)候(即初始平面內(nèi)點(diǎn)數(shù)量大于設(shè)定閾值時(shí))，初始平面擬合成功并表示為：

aix+biy+ciz+di＝0。

(3)依次構(gòu)造所有區(qū)域內(nèi)的平面，并驗(yàn)證其有效性。

驗(yàn)證構(gòu)造的初始平面是否有效的方法為：計(jì)算相鄰平面的角度差及高度差，即利用公式計(jì)算第i區(qū)域的初始平面與第i-1區(qū)域的初始平面之間的角度差δψi，其中為所述第i區(qū)域的初始平面的單位法向量，為所述第i-1區(qū)域的初始平面的單位法向量；

利用公式δzi＝|zi-zi-1|計(jì)算第i區(qū)域的初始平面與第i-1區(qū)域的初始平面之間的高度差δzi；其中

當(dāng)兩者均滿足閾值要求，則該平面有效；否則該平面無(wú)效，將其舍棄。例如將第2區(qū)域的初始平面舍棄后，第3區(qū)域擬合的初始平面的相鄰平面則為第1區(qū)域的初始平面。通過(guò)平面擬合獲得每個(gè)區(qū)域的代表平面，作為參考路面。

步驟104：獲得障礙物坐標(biāo)。依次獲取所述第1區(qū)域的點(diǎn)云數(shù)據(jù)、所述第2區(qū)域的點(diǎn)云數(shù)據(jù)至所述第q區(qū)域的點(diǎn)云數(shù)據(jù)中的障礙物的坐標(biāo)。采用計(jì)算各區(qū)域中平面外的點(diǎn)到平面的距離，將遠(yuǎn)離平面的點(diǎn)確定為障礙物點(diǎn)，將接近平面的點(diǎn)確定為路面點(diǎn)。具體的，利用公式遍歷計(jì)算第i區(qū)域內(nèi)第i平面外的任一點(diǎn)p到第i平面的距離d；其中p0為所述第i平面上任一點(diǎn)，為所述第i區(qū)域內(nèi)對(duì)應(yīng)第i平面的單位法向量；然后判斷所述距離d是否大于距離設(shè)定閾值，如果是，確定所述點(diǎn)p是障礙物點(diǎn)，并獲取所述點(diǎn)p的坐標(biāo)；

步驟105：對(duì)障礙物坐標(biāo)進(jìn)行體素化處理。將所有非路面點(diǎn)即障礙物點(diǎn)進(jìn)行體素化并根據(jù)每個(gè)體素內(nèi)包含點(diǎn)的個(gè)數(shù)計(jì)算各自的占用值，將這一時(shí)刻數(shù)據(jù)作為模板儲(chǔ)存在數(shù)據(jù)存儲(chǔ)模塊內(nèi)。使用同樣的方法處理下一時(shí)刻數(shù)據(jù)，得到若干個(gè)體素及占用值，將其映射到上一時(shí)刻得到的模板數(shù)據(jù)中進(jìn)行對(duì)比處理。由于在任何時(shí)刻，同一障礙在體素內(nèi)呈現(xiàn)相同的占用值，結(jié)合兩組數(shù)據(jù)遍歷每一個(gè)體素進(jìn)行判斷，若在該體素的相鄰體素中具有與其相等的占用值，則該體素代表動(dòng)態(tài)障礙物；若在該體素的相鄰體素中沒(méi)有與其相等的占用值，則該體素代表靜態(tài)障礙物。具體體素化示意圖參見(jiàn)圖5。體素化處理主要作用于非路面點(diǎn)即障礙物點(diǎn)，使其以體素的形式表現(xiàn)出來(lái)，計(jì)算體素內(nèi)所含點(diǎn)的個(gè)數(shù)，記作體素的占用值。對(duì)比相鄰體素之間的占用值變化，從而檢測(cè)動(dòng)、靜態(tài)障礙物得到動(dòng)態(tài)障礙物的坐標(biāo)集合和靜態(tài)障礙物的坐標(biāo)集合。具體流程如下：

對(duì)第一時(shí)刻所述n線激光雷達(dá)掃描后得到的障礙物點(diǎn)，利用公式對(duì)障礙物點(diǎn)進(jìn)行第一體素化處理，其中，為向下取整函數(shù)，x1為任一障礙物點(diǎn)p1的坐標(biāo)，v為體素的大小，為所述障礙物點(diǎn)p1體素化后得到的對(duì)應(yīng)體素的位置坐標(biāo)，對(duì)所有障礙物點(diǎn)進(jìn)行第一體素化處理后，計(jì)算各體素內(nèi)包含的點(diǎn)個(gè)數(shù)，記作其占用值；

對(duì)第二時(shí)刻所述n線激光雷達(dá)掃描后得到的障礙物點(diǎn)，利用公式對(duì)障礙物點(diǎn)進(jìn)行第二體素化處理，其中所第二時(shí)刻為所述第一時(shí)刻所述n線激光雷達(dá)掃描的后一次掃描時(shí)刻，x2為任一障礙物點(diǎn)p2的坐標(biāo)，為所述障礙物點(diǎn)p2體素化后得到的對(duì)應(yīng)體素的位置坐標(biāo)，對(duì)所有障礙物點(diǎn)進(jìn)行第二體素化處理后，計(jì)算各體素內(nèi)包含的點(diǎn)個(gè)數(shù)，記作其占用值；

在所述第一體素化處理得到的體素和所述第二體素化處理得到的體素范圍內(nèi)，判斷以體素為中心、以設(shè)定體素閾值為半徑組成的球體范圍內(nèi)的體素的占用值是否與所述體素的占用值相等；

如果是，確定所述障礙物點(diǎn)p2為動(dòng)態(tài)障礙物；

如果否，確定所述障礙物點(diǎn)p2為靜態(tài)障礙物。

步驟106：得到動(dòng)態(tài)障礙物坐標(biāo)集合和靜態(tài)障礙物坐標(biāo)集合。根據(jù)步驟105確定的動(dòng)態(tài)障礙物和靜態(tài)障礙物，依次獲得相應(yīng)的坐標(biāo)，構(gòu)成動(dòng)態(tài)障礙物坐標(biāo)集合和靜態(tài)障礙物坐標(biāo)集合，并結(jié)合步驟103獲得的平面，指示機(jī)器人合理規(guī)劃路線，避免碰撞。

圖2為本發(fā)明障礙物檢測(cè)系統(tǒng)結(jié)構(gòu)圖。如圖2所示，所述系統(tǒng)包括：

點(diǎn)云數(shù)據(jù)獲取模塊201，用于獲取n線激光雷達(dá)掃描得到的點(diǎn)云數(shù)據(jù)，所述點(diǎn)云數(shù)據(jù)豎直方向坐標(biāo)不同、水平面上的投影為n條平行直線；

區(qū)域劃分模塊202，用于將所述點(diǎn)云數(shù)據(jù)劃分為q個(gè)區(qū)域，所述q個(gè)區(qū)域包括第1區(qū)域、第2區(qū)域至第q區(qū)域；

平面擬合模塊203，用于依次對(duì)所述第1區(qū)域的點(diǎn)云數(shù)據(jù)、所述第2區(qū)域的點(diǎn)云數(shù)據(jù)至所述第q區(qū)域的點(diǎn)云數(shù)據(jù)進(jìn)行平面擬合，對(duì)應(yīng)得到第1平面、第2平面至第q平面；

障礙物坐標(biāo)獲取模塊204，用于依次獲取所述第1區(qū)域的點(diǎn)云數(shù)據(jù)、所述第2區(qū)域的點(diǎn)云數(shù)據(jù)至所述第q區(qū)域的點(diǎn)云數(shù)據(jù)中的障礙物的坐標(biāo)；

體素化處理模塊205，用于對(duì)所述障礙物的坐標(biāo)數(shù)據(jù)進(jìn)行體素化處理，得到動(dòng)態(tài)障礙物的坐標(biāo)集合和靜態(tài)障礙物的坐標(biāo)集合。

所述系統(tǒng)的各模塊具體工作過(guò)程參見(jiàn)圖1中相應(yīng)各步驟的說(shuō)明。

圖3為本發(fā)明障礙物檢測(cè)方法中激光雷達(dá)掃描示意圖。以十六線激光雷達(dá)為例，將十六線激光雷達(dá)與處理計(jì)算機(jī)連接，各子系統(tǒng)經(jīng)過(guò)初始化開(kāi)始正常運(yùn)行，十六線激光雷達(dá)通過(guò)dp83848網(wǎng)絡(luò)模塊以u(píng)dp為傳輸協(xié)議，向處理計(jì)算機(jī)傳輸掃描得到的稠密點(diǎn)云數(shù)據(jù)。

如圖3所示，十六線三維激光雷達(dá)，共有十六個(gè)掃描平面，每?jī)蓚€(gè)平面掃描之間的夾角δα為2°，在0到λ0的范圍內(nèi)，雷達(dá)數(shù)據(jù)密集且精確度高，該區(qū)域作為參考區(qū)域。雷達(dá)被固定在距離地面高h(yuǎn)處，有：

α0＝arctan(λ0/h)

掃描區(qū)域被分割成λk-1個(gè)，每個(gè)區(qū)域的長(zhǎng)度λk由下式給出：

λk＝h·tan(α0+k·η·δα),(k:1,2,......,n),{k:1,…,n}

其中，η取3，表示每三個(gè)掃描平面構(gòu)成一個(gè)區(qū)域。

圖4為本發(fā)明障礙物檢測(cè)方法中擬合平面示意圖。如圖4所示，利用四分位差法構(gòu)造門(mén)函數(shù)壓縮數(shù)據(jù)，方法為：

四分位差iqr＝q75％-q25％；其中iqr為第i區(qū)域中豎直方向坐標(biāo)小于z75％且大于z25％的點(diǎn)云數(shù)據(jù)區(qū)域，其中豎直方向坐標(biāo)z75％＝z1+δz×75％，z25％＝z1+δz×25％，z1為所述第i區(qū)域中點(diǎn)云數(shù)據(jù)中豎直方向坐標(biāo)最小值，δz為所述第i區(qū)域中點(diǎn)云數(shù)據(jù)中數(shù)值方向坐標(biāo)最大值與最小值的差值；

確定第i區(qū)域點(diǎn)云數(shù)據(jù)的門(mén)上限為qmax＝q75％；

確定第i區(qū)域點(diǎn)云數(shù)據(jù)的門(mén)下限為qmin＝q25％-0.5×(iqr)

根據(jù)所述第i區(qū)域點(diǎn)云數(shù)據(jù)的門(mén)上限和所述門(mén)下限，保留z∈(qmin,qmax)的點(diǎn)為第i區(qū)域點(diǎn)云數(shù)據(jù)更新后的區(qū)域；

根據(jù)所述第i區(qū)域點(diǎn)云數(shù)據(jù)更新后的區(qū)域，利用三點(diǎn)的隨機(jī)抽樣一致性算法進(jìn)行平面擬合，得到第i區(qū)域的初始平面aix+biy+ciz+di＝0；

為驗(yàn)證構(gòu)造的初始平面是否有效，計(jì)算相鄰平面的角度差及高度差，即利用公式計(jì)算第i區(qū)域的初始平面與第i-1區(qū)域的初始平面之間的角度差δψi，其中為所述第i區(qū)域的初始平面的單位法向量，為所述第i-1區(qū)域的初始平面的單位法向量；

利用公式δzi＝|zi-zi-1|計(jì)算第i區(qū)域的初始平面與第i-1區(qū)域的初始平面之間的高度差δzi，

當(dāng)兩者均滿足閾值要求，則該平面有效；否則該平面無(wú)效，將其舍棄。

圖5為本發(fā)明障礙物檢測(cè)方法中k時(shí)刻體素化處理示意圖。如圖5所示，在動(dòng)、靜態(tài)障礙物檢測(cè)的過(guò)程中，認(rèn)為隨著時(shí)間的改變及數(shù)據(jù)的積累，動(dòng)態(tài)障礙會(huì)占據(jù)不同的體素并具有相同的占用值，靜態(tài)障礙物占據(jù)相同的體素。因此構(gòu)造模板匹配機(jī)制。

對(duì)第k時(shí)刻激光雷達(dá)掃描后得到的障礙物點(diǎn)，利用公式對(duì)障礙物點(diǎn)進(jìn)行體素化處理，其中，為向下取整函數(shù)，x1為任一障礙物點(diǎn)p1的坐標(biāo)，v為體素的大小，v＝0.1，為所述障礙物點(diǎn)p1體素化后得到的對(duì)應(yīng)體素的位置坐標(biāo)，對(duì)所有障礙物點(diǎn)進(jìn)行體素化處理后，；計(jì)算障礙物所占體素的占用值，假設(shè)探測(cè)到三個(gè)障礙物o1、o2、o3，占據(jù)了三個(gè)體素并具有不同的占用值，如圖5所示，將其作為模板。

圖6為本發(fā)明障礙物檢測(cè)方法中k+1時(shí)刻障礙物體素化處理后示意圖。如圖6所示，對(duì)第k+1時(shí)刻激光雷達(dá)掃描后得到的障礙物點(diǎn)，利用公式對(duì)障礙物點(diǎn)進(jìn)行體素化處理，其中所第k+1時(shí)刻為所述第k時(shí)刻所述n線激光雷達(dá)掃描的后一次掃描時(shí)刻，x2為任一障礙物點(diǎn)p2的坐標(biāo)，為所述障礙物點(diǎn)p2體素化后得到的對(duì)應(yīng)體素的位置坐標(biāo)，對(duì)所有障礙物點(diǎn)進(jìn)行體素化處理后；計(jì)算障礙物所占體素的占用值，假設(shè)探測(cè)到三個(gè)障礙物o1’、o2、o3，占據(jù)了三個(gè)體素并具有不同的占用值，如圖6所示.

圖7為本發(fā)明障礙物檢測(cè)方法中k+1時(shí)刻障礙物體素化處理后與k時(shí)刻結(jié)合后的示意圖。將k+1時(shí)刻體素化后數(shù)據(jù)與k時(shí)刻數(shù)據(jù)結(jié)合，假設(shè)o1發(fā)生了運(yùn)動(dòng)，到達(dá)了o1’所在體素但其占用值不變，o2、o3未發(fā)生運(yùn)動(dòng)，還占據(jù)著相同的體素，如圖7所示。在第k時(shí)刻中，分別以o1’、o2、o3為中心體素，與其相鄰體素(設(shè)定閾值范圍內(nèi)的體素)占用值作對(duì)比，若具有相同的占用值則該障礙為動(dòng)態(tài)障礙并從模板中剔除，所有對(duì)比完成后更新模板，依次往下迭代。即：在第k時(shí)刻，與o1’相鄰并具有占用值的體素為o1及o3，o1’與o1具有相同的占用值，與o3相比具有不同的占用值，故o1為運(yùn)動(dòng)障礙物，將其從模板中剔除。更新后的模板即為圖6。

本說(shuō)明書(shū)中各個(gè)實(shí)施例采用遞進(jìn)的方式描述，每個(gè)實(shí)施例重點(diǎn)說(shuō)明的都是與其他實(shí)施例的不同之處，各個(gè)實(shí)施例之間相同相似部分互相參見(jiàn)即可。對(duì)于實(shí)施例公開(kāi)的系統(tǒng)而言，由于其與實(shí)施例公開(kāi)的方法相對(duì)應(yīng)，所以描述的比較簡(jiǎn)單，相關(guān)之處參見(jiàn)方法部分說(shuō)明即可。

本文中應(yīng)用了具體個(gè)例對(duì)本發(fā)明的原理及實(shí)施方式進(jìn)行了闡述，以上實(shí)施例的說(shuō)明只是用于幫助理解本發(fā)明的方法及其核心思想；同時(shí)，對(duì)于本領(lǐng)域的一般技術(shù)人員，依據(jù)本發(fā)明的思想，在具體實(shí)施方式及應(yīng)用范圍上均會(huì)有改變之處。綜上所述，本說(shuō)明書(shū)內(nèi)容不應(yīng)理解為對(duì)本發(fā)明的限制。