本技術(shù)涉及自動(dòng)駕駛，具體而言，涉及一種基于三維柵格地圖的無(wú)人清掃車(chē)主動(dòng)安全方法及系統(tǒng)。

背景技術(shù)：

1、隨著自動(dòng)駕駛技術(shù)的不斷普及，自動(dòng)駕駛的概念已經(jīng)深入各行各業(yè)，將自動(dòng)駕駛與傳統(tǒng)環(huán)衛(wèi)清掃車(chē)結(jié)合，替代傳統(tǒng)的人工駕駛，實(shí)現(xiàn)智能控制，節(jié)約了財(cái)力、物力和人力，并提高了清潔效率。其中，為實(shí)現(xiàn)清掃車(chē)的自主無(wú)人駕駛，首先需要保障清掃車(chē)和其他交通參與者的安全，研究和發(fā)展車(chē)輛的主動(dòng)安全技術(shù)具有重要意義。

2、目前，在現(xiàn)有的無(wú)人清掃車(chē)主動(dòng)安全技術(shù)中，所采用的主動(dòng)安全方法包括：使用超聲波雷達(dá)，利用超聲波雷達(dá)感知無(wú)人清掃車(chē)周?chē)系K物的距離，當(dāng)障礙物距離過(guò)近時(shí)，觸發(fā)清掃車(chē)的緊急制動(dòng)；直接使用激光雷達(dá)，利用激光雷達(dá)探測(cè)障礙物的點(diǎn)云位置信息，判斷與清掃車(chē)之間的距離，確認(rèn)是否進(jìn)行緊急制動(dòng)。其中，超聲波雷達(dá)信號(hào)傳輸頻率較低，在無(wú)人清掃車(chē)行駛過(guò)程中無(wú)法滿(mǎn)足實(shí)時(shí)檢測(cè)和接管的需求，并且超聲波反饋的一般只有和障礙物之間的距離信息，在主動(dòng)安全算法上的使用不具有靈活性。而激光雷達(dá)的感知周?chē)系K物信息的頻率雖然較高，但其數(shù)據(jù)量較大，若在車(chē)輛實(shí)時(shí)行駛過(guò)程中，直接使用激光雷達(dá)點(diǎn)云，則需要對(duì)數(shù)量龐大的點(diǎn)云信息進(jìn)行計(jì)算處理，篩選其中距離車(chē)輛較近的點(diǎn)，這對(duì)算法輸出結(jié)果的實(shí)時(shí)性提出了較大挑戰(zhàn)，且可能會(huì)出現(xiàn)閃現(xiàn)的點(diǎn)云噪點(diǎn)，若直接使用則極易造成車(chē)輛卡頓。

技術(shù)實(shí)現(xiàn)思路

1、本技術(shù)提供一種基于三維柵格地圖的無(wú)人清掃車(chē)主動(dòng)安全方法及系統(tǒng)，主要解決了無(wú)人清掃車(chē)行駛過(guò)程中的主動(dòng)安全問(wèn)題，在突然遇到障礙物時(shí)可以準(zhǔn)確識(shí)別、快速反應(yīng)、緊急制動(dòng)接管，并避免不必要的誤接管，解決了現(xiàn)有技術(shù)中使用超聲波雷達(dá)方法所存在的反應(yīng)速度慢、靈活性不足的問(wèn)題，還解決了現(xiàn)有技術(shù)中直接使用激光雷達(dá)點(diǎn)云方法的誤檢、時(shí)效性欠缺的問(wèn)題。

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

3、第一方面，本技術(shù)實(shí)施例提供了一種基于三維柵格地圖的無(wú)人清掃車(chē)主動(dòng)安全方法，所述無(wú)人清掃車(chē)主動(dòng)安全方法包括：

4、實(shí)時(shí)獲取激光雷達(dá)點(diǎn)云數(shù)據(jù)信息，并根據(jù)當(dāng)前幀的激光雷達(dá)點(diǎn)云數(shù)據(jù)信息構(gòu)建實(shí)時(shí)三維柵格地圖；

5、實(shí)時(shí)獲取車(chē)輛底盤(pán)反饋的車(chē)輛擋位和前輪轉(zhuǎn)角數(shù)據(jù)信息，并對(duì)車(chē)輛推位預(yù)測(cè)速度、采樣時(shí)間及推位軌跡點(diǎn)數(shù)量進(jìn)行預(yù)設(shè)置，根據(jù)車(chē)輛運(yùn)動(dòng)學(xué)模型構(gòu)建車(chē)輛推位預(yù)測(cè)軌跡，并根據(jù)車(chē)輛三維模型和所述車(chē)輛推位預(yù)測(cè)軌跡，生成基于預(yù)測(cè)軌跡點(diǎn)的車(chē)輛三維預(yù)測(cè)位置框；其中，所述車(chē)輛推位預(yù)測(cè)軌跡包括每個(gè)推位軌跡點(diǎn)的位置姿態(tài)信息；

6、根據(jù)所述車(chē)輛三維預(yù)測(cè)位置框和所述實(shí)時(shí)三維柵格地圖，判斷所述車(chē)輛三維預(yù)測(cè)位置框中的每個(gè)柵格是否被占據(jù)；

7、若所述車(chē)輛三維預(yù)測(cè)位置框中的某個(gè)柵格被占據(jù)，則計(jì)算所述車(chē)輛三維預(yù)測(cè)位置框與車(chē)輛實(shí)際位置之間的距離，作為障礙物距離信息輸出，并根據(jù)輸出的所述障礙物距離信息進(jìn)行相對(duì)應(yīng)的主動(dòng)安全制動(dòng)。

8、在本技術(shù)的一些實(shí)施例中，所述根據(jù)當(dāng)前幀的激光雷達(dá)點(diǎn)云數(shù)據(jù)信息構(gòu)建實(shí)時(shí)三維柵格地圖，具體包括：

9、設(shè)置所述實(shí)時(shí)三維柵格地圖的長(zhǎng)寬高和分辨率，并將所述當(dāng)前幀的激光雷達(dá)點(diǎn)云數(shù)據(jù)信息轉(zhuǎn)換為所述實(shí)時(shí)三維柵格地圖的地圖數(shù)據(jù)。

10、在本技術(shù)的一些實(shí)施例中，所述根據(jù)車(chē)輛運(yùn)動(dòng)學(xué)模型構(gòu)建車(chē)輛推位預(yù)測(cè)軌跡，具體包括：

11、建立車(chē)輛運(yùn)動(dòng)學(xué)模型；所述車(chē)輛運(yùn)動(dòng)學(xué)模型為：

12、

13、其中，為車(chē)輛在x軸的分速度，為車(chē)輛在y軸的分速度，為航向角變化率，v為車(chē)輛速度，為車(chē)輛航向角，δ為前輪轉(zhuǎn)角，l為車(chē)輛軸距；

14、根據(jù)所述車(chē)輛運(yùn)動(dòng)學(xué)模型以及預(yù)設(shè)置的所述車(chē)輛推位預(yù)測(cè)速度、所述采樣時(shí)間、所述推位軌跡點(diǎn)數(shù)量，計(jì)算得到每個(gè)所述推位軌跡點(diǎn)的位置信息和航向角信息，計(jì)算公式為：

15、

16、其中，xk+1為第k+1個(gè)軌跡點(diǎn)車(chē)輛位置x坐標(biāo)，xk為第k個(gè)軌跡點(diǎn)車(chē)輛位置x坐標(biāo)，yk+1為第k+1個(gè)軌跡點(diǎn)車(chē)輛位置y坐標(biāo)，yk為第k個(gè)軌跡點(diǎn)車(chē)輛位置y坐標(biāo)，為第k+1個(gè)軌跡點(diǎn)車(chē)輛航向角，為第k個(gè)軌跡點(diǎn)車(chē)輛航向角，v'為車(chē)輛推位預(yù)測(cè)速度，t為采樣時(shí)間，為車(chē)輛航向角，δ為前輪轉(zhuǎn)角，l為車(chē)輛軸距，n為推位軌跡點(diǎn)數(shù)量；

17、根據(jù)每個(gè)所述推位軌跡點(diǎn)的所述位置信息和所述航向角信息，構(gòu)建所述車(chē)輛推位預(yù)測(cè)軌跡。

18、在本技術(shù)的一些實(shí)施例中，所述車(chē)輛三維模型的構(gòu)建方法包括：

19、根據(jù)車(chē)輛各局部實(shí)際形狀和傳感器布局，分別選擇相對(duì)應(yīng)形狀的三維模型作為初始局部模型；

20、以車(chē)輛后軸中心為三維坐標(biāo)系原點(diǎn)，根據(jù)所述車(chē)輛整體實(shí)際形狀，對(duì)所有所述初始局部模型進(jìn)行組合拼接，合成所述車(chē)輛三維模型。

21、在本技術(shù)的一些實(shí)施例中，所述根據(jù)車(chē)輛三維模型和所述車(chē)輛推位預(yù)測(cè)軌跡，生成基于預(yù)測(cè)軌跡點(diǎn)的車(chē)輛三維預(yù)測(cè)位置框，具體包括：

22、獲取每個(gè)所述推位軌跡點(diǎn)的所述位置姿態(tài)信息；所述位置姿態(tài)信息包括位置信息和航向角信息；

23、根據(jù)每個(gè)所述推位軌跡點(diǎn)的所述位置姿態(tài)信息和所述車(chē)輛三維模型，得到所述車(chē)輛三維模型的每個(gè)初始局部模型的頂點(diǎn)坐標(biāo)，并根據(jù)所述車(chē)輛三維模型的所有所述初始局部模型的頂點(diǎn)坐標(biāo)生成所述車(chē)輛三維預(yù)測(cè)位置框。

24、在本技術(shù)的一些實(shí)施例中，所述根據(jù)所述車(chē)輛三維預(yù)測(cè)位置框和所述實(shí)時(shí)三維柵格地圖，判斷所述車(chē)輛三維預(yù)測(cè)位置框中的每個(gè)柵格是否被占據(jù)，具體包括：

25、從第一個(gè)所述推位軌跡點(diǎn)開(kāi)始，執(zhí)行柵格遍歷步驟；其中，所述柵格遍歷步驟包括：將與第一個(gè)所述推位軌跡點(diǎn)相對(duì)應(yīng)的所述車(chē)輛三維預(yù)測(cè)位置框的每個(gè)所述初始局部模型的頂點(diǎn)坐標(biāo)分別轉(zhuǎn)換為所述實(shí)時(shí)三維柵格地圖的柵格坐標(biāo)，記為頂點(diǎn)柵格坐標(biāo)；遍歷所述車(chē)輛三維預(yù)測(cè)位置框的所有頂點(diǎn)柵格坐標(biāo)所圍區(qū)域內(nèi)的所有柵格值，判斷是否存在大于預(yù)設(shè)閾值的柵格；

26、若存在，則判定第一個(gè)所述推位軌跡點(diǎn)相對(duì)應(yīng)的所述車(chē)輛三維預(yù)測(cè)位置框中的柵格被占據(jù)，停止遍歷，否則對(duì)第二個(gè)所述推位軌跡點(diǎn)執(zhí)行柵格遍歷步驟，直至判定柵格被占據(jù)或者遍歷完所有所述推位軌跡點(diǎn)。

27、第二方面，本技術(shù)實(shí)施例提供了一種基于三維柵格地圖的無(wú)人清掃車(chē)主動(dòng)安全系統(tǒng)，所述無(wú)人清掃車(chē)主動(dòng)安全系統(tǒng)包括：

28、地圖構(gòu)建模塊，用于實(shí)時(shí)獲取激光雷達(dá)點(diǎn)云數(shù)據(jù)信息，并根據(jù)當(dāng)前幀的激光雷達(dá)點(diǎn)云數(shù)據(jù)信息構(gòu)建實(shí)時(shí)三維柵格地圖；

29、推位生成模塊，用于實(shí)時(shí)獲取車(chē)輛底盤(pán)反饋的車(chē)輛擋位和前輪轉(zhuǎn)角數(shù)據(jù)信息，并對(duì)車(chē)輛推位預(yù)測(cè)速度、采樣時(shí)間及推位軌跡點(diǎn)數(shù)量進(jìn)行預(yù)設(shè)置，根據(jù)車(chē)輛運(yùn)動(dòng)學(xué)模型構(gòu)建車(chē)輛推位預(yù)測(cè)軌跡，并根據(jù)車(chē)輛三維模型和所述車(chē)輛推位預(yù)測(cè)軌跡，生成基于預(yù)測(cè)軌跡點(diǎn)的車(chē)輛三維預(yù)測(cè)位置框；其中，所述車(chē)輛推位預(yù)測(cè)軌跡包括每個(gè)推位軌跡點(diǎn)的位置姿態(tài)信息；

30、柵格占據(jù)判斷模塊，用于根據(jù)所述車(chē)輛三維預(yù)測(cè)位置框和所述實(shí)時(shí)三維柵格地圖，判斷所述車(chē)輛三維預(yù)測(cè)位置框中的每個(gè)柵格是否被占據(jù)，并用于若所述車(chē)輛三維預(yù)測(cè)位置框中的某個(gè)柵格被占據(jù)，則計(jì)算所述車(chē)輛三維預(yù)測(cè)位置框與車(chē)輛實(shí)際位置之間的距離，作為障礙物距離信息輸出，并根據(jù)輸出的所述障礙物距離信息判斷是否進(jìn)行主動(dòng)安全制動(dòng)。

31、第三方面，本技術(shù)實(shí)施例提供了一種基于三維柵格地圖的無(wú)人清掃車(chē)主動(dòng)安全裝置，包括處理器、存儲(chǔ)器以及存儲(chǔ)在所述存儲(chǔ)器中的計(jì)算機(jī)程序，所述處理器執(zhí)行所述計(jì)算機(jī)程序時(shí)，執(zhí)行如第一方面所述的基于三維柵格地圖的無(wú)人清掃車(chē)主動(dòng)安全方法。

32、第四方面，本技術(shù)實(shí)施例提供了一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，其上存儲(chǔ)有計(jì)算機(jī)程序，所述計(jì)算機(jī)程序被處理器執(zhí)行時(shí)實(shí)現(xiàn)如第一方面所述的基于三維柵格地圖的無(wú)人清掃車(chē)主動(dòng)安全方法。

33、第五方面，本技術(shù)實(shí)施例提供了一種計(jì)算機(jī)程序產(chǎn)品，所述計(jì)算機(jī)程序產(chǎn)品中包含有指令，當(dāng)指令在計(jì)算機(jī)或處理器上運(yùn)行時(shí)，使得計(jì)算機(jī)或處理器執(zhí)行如第一方面所述的基于三維柵格地圖的無(wú)人清掃車(chē)主動(dòng)安全方法。

34、本技術(shù)實(shí)施例的有益效果如下：

35、該無(wú)人清掃車(chē)主動(dòng)安全方法采用將實(shí)時(shí)激光雷達(dá)點(diǎn)云轉(zhuǎn)換為實(shí)時(shí)三維柵格地圖的方式，并根據(jù)車(chē)輛實(shí)時(shí)反饋的狀態(tài)信息預(yù)測(cè)車(chē)輛行駛軌跡，判斷基于預(yù)測(cè)軌跡的車(chē)輛三維框體的柵格是否被占據(jù)，以確認(rèn)車(chē)輛是否會(huì)發(fā)生碰撞，并得到車(chē)輛和障礙物之間的距離信息，根據(jù)該距離信息可靈活調(diào)整車(chē)輛進(jìn)行緊急制動(dòng)的時(shí)機(jī)，有效保證了無(wú)人清掃車(chē)行駛過(guò)程中的實(shí)時(shí)安全，使無(wú)人清掃車(chē)安全可靠行駛，并保障與其他交通參與者之間的安全性，解決了現(xiàn)有技術(shù)使用超聲波雷達(dá)急停和直接使用激光雷達(dá)點(diǎn)云的方式中所存在的實(shí)時(shí)性、靈活性較差，以及誤檢導(dǎo)致車(chē)輛卡頓，或車(chē)輛模型導(dǎo)致誤接管的問(wèn)題。