本發(fā)明涉及機(jī)器人及控制領(lǐng)域，尤其涉及一種無(wú)人駕駛汽車自主定位與地圖構(gòu)建的方法。

背景技術(shù)：

近年來(lái)，互聯(lián)網(wǎng)技術(shù)的迅速發(fā)展給汽車制造工業(yè)帶來(lái)了革命性變化的機(jī)會(huì)。與此同時(shí)，汽車智能化技術(shù)正逐步得到廣泛應(yīng)用，這項(xiàng)技術(shù)簡(jiǎn)化了汽車的駕駛操作并提高了行駛安全性。而其中最典型也是最熱門(mén)的未來(lái)應(yīng)用就是無(wú)人駕駛汽車。在人工智能技術(shù)的加持下，無(wú)人駕駛高速發(fā)展，正在改變?nèi)祟惖某鲂蟹绞?，進(jìn)而會(huì)大規(guī)模改變相關(guān)行業(yè)格局。

對(duì)于行駛在未知區(qū)域中的無(wú)人駕駛汽車而言，由于樓宇、樹(shù)木遮擋等原因，汽車常處于無(wú)信號(hào)或弱信號(hào)的狀態(tài)，無(wú)法提供有效定位；在環(huán)境惡劣的地方，因天氣等原因gps或北斗導(dǎo)航系統(tǒng)信號(hào)微弱，無(wú)法對(duì)無(wú)人駕駛汽車進(jìn)行有效的定位。為此，無(wú)人駕駛汽車必須具有自主定位與地圖構(gòu)建的能力。通過(guò)實(shí)時(shí)的自主定位與地圖構(gòu)建，獲取周圍環(huán)境信息，確定無(wú)人駕駛汽車所處的位置，為路徑規(guī)劃提供重要的依據(jù)。

在機(jī)器人領(lǐng)域，同時(shí)定位與地圖構(gòu)建(simultaneouslocalizationandmapping，slam)技術(shù)能夠?qū)C(jī)器人進(jìn)行實(shí)時(shí)定位與地圖構(gòu)建，是當(dāng)今的主要研究對(duì)象。

然而如今的無(wú)人駕駛汽車大多仍存在于輔助駕駛階段，缺乏自主定位與地圖構(gòu)建的能力。同時(shí)較少的無(wú)人駕駛汽車采用單一傳感器進(jìn)行自主定位與地圖構(gòu)建，定位與地圖構(gòu)建精度較低，不能有效的將多種傳感器進(jìn)行融合，或者能夠?qū)⒍喾N傳感器數(shù)據(jù)進(jìn)行融合，但是不能夠?qū)o(wú)人駕駛汽車進(jìn)行穩(wěn)定有效的自主定位與地圖構(gòu)建，并且不能大規(guī)模普及。

為此發(fā)明一種無(wú)人駕駛汽車自主定位與地圖構(gòu)建的方法，利用slam的技術(shù)，將多種傳感器數(shù)據(jù)進(jìn)行融合，采用新型的算法結(jié)構(gòu)，構(gòu)建穩(wěn)定有效的框架，對(duì)無(wú)人駕駛汽車進(jìn)行穩(wěn)定有效的自主定位與地圖構(gòu)建，應(yīng)用到無(wú)人駕駛汽車系統(tǒng)上，進(jìn)行大規(guī)模應(yīng)用。

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

發(fā)明目的：為了克服現(xiàn)有技術(shù)中存在的不足，本發(fā)明提供一種無(wú)人駕駛汽車自主定位與地圖構(gòu)建的方法，利用無(wú)人駕駛汽車車輪里程計(jì)、imu慣性測(cè)量單元、全景攝像頭、三維激光雷達(dá)的數(shù)據(jù)，通過(guò)數(shù)據(jù)融合及優(yōu)化，對(duì)無(wú)人駕駛汽車進(jìn)行自主定位與地圖構(gòu)建。

為實(shí)現(xiàn)上述目的，本發(fā)明采用的技術(shù)方案為：

一種無(wú)人駕駛汽車自主定位與地圖構(gòu)建的方法，包括以下步驟：

步驟一、初始化無(wú)人駕駛汽車的位姿為x0，無(wú)人駕駛汽車行駛的軌跡為x1:t＝{x1,x2...xt}，校正無(wú)人駕駛汽車車輪里程計(jì)、imu慣性測(cè)量單元、全景攝像頭、三維激光雷達(dá)的時(shí)間，使無(wú)人駕駛汽車車輪里程計(jì)、imu慣性測(cè)量單元、全景攝像頭、三維激光雷達(dá)的數(shù)據(jù)保持時(shí)間一致性；

步驟二、采集無(wú)人駕駛汽車車輪里程計(jì)數(shù)據(jù)u1:t＝{u1,u2...ut}，imu慣性測(cè)量單元的數(shù)據(jù)i1:t＝{i1,i2...it}，全景攝像頭的數(shù)據(jù)c1:t＝{c1,c2...ct}，三維激光雷達(dá)的數(shù)據(jù)r1:t＝{r1,r2...rt}，汽車大腦通過(guò)全景攝像頭和三維激光雷達(dá)的數(shù)據(jù)計(jì)算出無(wú)人駕駛汽車周圍所有環(huán)境路標(biāo)為l1:t＝{l1,l2...lt}；

步驟三、利用評(píng)價(jià)函數(shù)f評(píng)價(jià)全景攝像頭采集數(shù)據(jù)c1:t的復(fù)雜程度，根據(jù)評(píng)價(jià)指標(biāo)選擇fast、orb、surf、sift特征點(diǎn)的一種，進(jìn)一步利用pcl庫(kù)將數(shù)據(jù)c1:t轉(zhuǎn)換成點(diǎn)云p1:t；

步驟四、利用粒子濾波器去除三維激光雷達(dá)的數(shù)據(jù)r1:t異常值，與步驟三生成的點(diǎn)云數(shù)據(jù)p1:t、無(wú)人駕駛汽車車輪里程計(jì)數(shù)據(jù)u1:t＝{u1,u2...ut}，imu慣性測(cè)量單元的數(shù)據(jù)i1:t＝{i1,i2...it}進(jìn)行數(shù)據(jù)關(guān)聯(lián)；

步驟五、估計(jì)無(wú)人駕駛汽車軌跡和周圍環(huán)境三維地圖的后驗(yàn)概率p(x1:t，l1:t|c1:t，r1:t，u1:t，i1:t，x0)，進(jìn)一步將函數(shù)轉(zhuǎn)化成最小二乘法問(wèn)題進(jìn)行求解，得出無(wú)人駕駛汽車實(shí)時(shí)位姿xt’；

步驟六、根據(jù)步驟五中的無(wú)人駕駛汽車實(shí)時(shí)位姿xt’，利用圖優(yōu)化的方法將步驟三中的點(diǎn)云和步驟四優(yōu)化的三維激光雷達(dá)的數(shù)據(jù)r1:t實(shí)時(shí)構(gòu)建無(wú)人駕駛汽車周圍環(huán)境三維地圖；

步驟七、利用詞袋模型進(jìn)行閉環(huán)檢測(cè)，進(jìn)一步提高人駕駛汽車周圍環(huán)境三維地圖的精度。

優(yōu)選的是，在步驟三中的環(huán)境復(fù)雜評(píng)價(jià)函數(shù)，f＝g+h，g為環(huán)境的亮度函數(shù)，h為sift特征點(diǎn)數(shù)量函數(shù)，

優(yōu)選的是，在步驟四中，將三維激光雷達(dá)的數(shù)據(jù)的坐標(biāo)系均轉(zhuǎn)換為世界坐標(biāo)系下的數(shù)據(jù)，根據(jù)時(shí)間戳進(jìn)行數(shù)據(jù)關(guān)聯(lián)；

數(shù)據(jù)關(guān)聯(lián)函數(shù):

優(yōu)選的是，在步驟五中的最小二乘法問(wèn)題中，具體采用g2o模塊中l(wèi)evenberg-marqudart方法進(jìn)行求解。

優(yōu)選的是，在步驟五中，轉(zhuǎn)化后的公式為

其中hi,j(qi-lj)為關(guān)聯(lián)數(shù)據(jù)在路標(biāo)平面圖像上投影位置的函數(shù)。

優(yōu)選的是，在步驟六的無(wú)人駕駛汽車周圍環(huán)境三維地圖構(gòu)建中，針對(duì)稀疏矩陣采用cholesky分解進(jìn)行求解，提高運(yùn)算速度。

優(yōu)選的是，在步驟七的的閉環(huán)檢測(cè)中，將步驟四優(yōu)化的三維激光雷達(dá)的數(shù)據(jù)r1:t轉(zhuǎn)換成視覺(jué)模型，同樣利用詞袋模型進(jìn)行閉環(huán)檢測(cè)。

一種無(wú)人駕駛汽車自主定位與地圖構(gòu)建的系統(tǒng)，該系統(tǒng)包括汽車大腦、車輪里程計(jì)、全景攝像頭、imu慣性測(cè)量單元和三維激光雷達(dá)；所述汽車大腦作為整個(gè)系統(tǒng)的核心部件，分別與車輪里程計(jì)、全景攝像頭、imu慣性測(cè)量單元和三維激光雷達(dá)相連；所述車輪里程計(jì)采集汽車行程數(shù)據(jù)；所述全景攝像頭觀測(cè)汽車周圍視覺(jué)環(huán)境；所述imu慣性測(cè)量單元采集測(cè)量汽車三軸姿態(tài)角以及加速度；所述三維激光雷達(dá)采集汽車周圍環(huán)境的點(diǎn)云信息。

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

本發(fā)明提供的一種無(wú)人駕駛汽車自主定位與地圖構(gòu)建的方法，利用slam的技術(shù)，將多種傳感器數(shù)據(jù)進(jìn)行融合，采用新型的算法結(jié)構(gòu)，構(gòu)建穩(wěn)定有效的框架，對(duì)無(wú)人駕駛汽車進(jìn)行穩(wěn)定有效的自主定位與地圖構(gòu)建，應(yīng)用到無(wú)人駕駛汽車系統(tǒng)上，進(jìn)行大規(guī)模應(yīng)用。

附圖說(shuō)明

圖1為本發(fā)明的無(wú)人駕駛汽車自主定位與地圖構(gòu)建的方法示意圖；

圖2為本發(fā)明的無(wú)人駕駛汽車自主定位與地圖構(gòu)建的系統(tǒng)框圖；

圖3為本發(fā)明實(shí)現(xiàn)自主定位的圖示；

圖4、圖5分別為本發(fā)明實(shí)現(xiàn)地圖構(gòu)建的圖示。

具體實(shí)施方式

下面結(jié)合附圖對(duì)本發(fā)明作更進(jìn)一步的說(shuō)明。

如圖1，一種無(wú)人駕駛汽車自主定位與地圖構(gòu)建的方法，包括以下步驟：

步驟一、初始化無(wú)人駕駛汽車的位姿為x0，無(wú)人駕駛汽車行駛的軌跡為x1:t＝{x1,x2...xt}，校正無(wú)人駕駛汽車車輪里程計(jì)、imu慣性測(cè)量單元、全景攝像頭、三維激光雷達(dá)的時(shí)間，使無(wú)人駕駛汽車車輪里程計(jì)、imu慣性測(cè)量單元、全景攝像頭、三維激光雷達(dá)的數(shù)據(jù)保持時(shí)間一致性；

步驟二、采集無(wú)人駕駛汽車車輪里程計(jì)數(shù)據(jù)u1:t＝{u1,u2...ut}，imu慣性測(cè)量單元的數(shù)據(jù)i1:t＝{i1,i2...it}，全景攝像頭的數(shù)據(jù)c1:t＝{c1,c2...ct}，三維激光雷達(dá)的數(shù)據(jù)r1:t＝{r1,r2...rt}，汽車大腦通過(guò)全景攝像頭和三維激光雷達(dá)的數(shù)據(jù)計(jì)算出無(wú)人駕駛汽車周圍所有環(huán)境路標(biāo)為l1:t＝{l1,l2...lt}；

步驟三、利用評(píng)價(jià)函數(shù)f評(píng)價(jià)全景攝像頭采集數(shù)據(jù)c1:t的復(fù)雜程度，根據(jù)評(píng)價(jià)指標(biāo)選擇fast、orb、surf、sift特征點(diǎn)的一種，進(jìn)一步利用pcl庫(kù)將數(shù)據(jù)c1:t轉(zhuǎn)換成點(diǎn)云p1:t；plc庫(kù)是一個(gè)依賴的開(kāi)源庫(kù)，可以當(dāng)成是一種軟件。

在步驟三中的環(huán)境復(fù)雜評(píng)價(jià)函數(shù)，f＝g+h，g為環(huán)境的亮度函數(shù)，h為sift特征點(diǎn)數(shù)量函數(shù)，

步驟四、利用粒子濾波器去除三維激光雷達(dá)的數(shù)據(jù)r1:t異常值,與步驟三生成的點(diǎn)云數(shù)據(jù)p1:t、無(wú)人駕駛汽車車輪里程計(jì)數(shù)據(jù)u1:t＝{u1,u2...ut}，imu慣性測(cè)量單元的數(shù)據(jù)i1:t＝{i1,i2...it}進(jìn)行數(shù)據(jù)關(guān)聯(lián)；

將三維激光雷達(dá)、的數(shù)據(jù)的坐標(biāo)系均轉(zhuǎn)換為世界坐標(biāo)系下的數(shù)據(jù)，根據(jù)時(shí)間戳進(jìn)行數(shù)據(jù)關(guān)聯(lián)。

數(shù)據(jù)關(guān)聯(lián)函數(shù)

步驟五、估計(jì)無(wú)人駕駛汽車軌跡和周圍環(huán)境三維地圖的后驗(yàn)概率函數(shù)進(jìn)一步將該函數(shù)轉(zhuǎn)化成最小二乘法問(wèn)題進(jìn)行求解，得出無(wú)人駕駛汽車實(shí)時(shí)位姿xt’；

轉(zhuǎn)化后的公式為

其中hi,j(qi-lj)為關(guān)聯(lián)數(shù)據(jù)在路標(biāo)平面圖像上投影位置的函數(shù)

在步驟五中的最小二乘法問(wèn)題中，具體采用g2o模塊中l(wèi)evenberg-marqudart方法進(jìn)行求解；

步驟六、根據(jù)步驟五中的無(wú)人駕駛汽車實(shí)時(shí)位姿xt’，根據(jù)同一時(shí)間t下三維激光雷達(dá)數(shù)據(jù)和xt’在中間坐標(biāo)中位置，進(jìn)行有序堆積，利用圖優(yōu)化的方法將步驟三中的點(diǎn)云和步驟四優(yōu)化的三維激光雷達(dá)的數(shù)據(jù)實(shí)時(shí)構(gòu)建無(wú)人駕駛汽車周圍環(huán)境三維地圖；

在步驟六的無(wú)人駕駛汽車周圍環(huán)境三維地圖構(gòu)建中，針對(duì)稀疏矩陣采用cholesky分解進(jìn)行求解，將一個(gè)矩陣分解為若干個(gè)矩陣的乘積可以大大降低存儲(chǔ)空間，提高運(yùn)算速度；

步驟七、利用詞袋模型進(jìn)行閉環(huán)檢測(cè)，當(dāng)遇到重復(fù)或已知地域時(shí)，能夠?qū)⑦\(yùn)行軌跡進(jìn)行回環(huán)處理，進(jìn)一步提高人駕駛汽車周圍環(huán)境三維地圖的精度；

在步驟七的的閉環(huán)檢測(cè)中，將步驟四優(yōu)化的三維激光雷達(dá)的數(shù)據(jù)r1:t轉(zhuǎn)換成視覺(jué)模型，同樣利用詞袋模型進(jìn)行閉環(huán)檢測(cè)。

如圖2所示，一種無(wú)人駕駛汽車自主定位與地圖構(gòu)建的系統(tǒng)，該系統(tǒng)包括汽車大腦1、車輪里程計(jì)2、全景攝像頭3、imu慣性測(cè)量單元4和三維激光雷達(dá)5；

所述汽車大腦1作為整個(gè)系統(tǒng)的核心部件，分別與車輪里程計(jì)2、全景攝像頭3、imu慣性測(cè)量單元4和三維激光雷達(dá)5相連；所述車輪里程計(jì)2采集汽車行程數(shù)據(jù)；所述全景攝像頭3觀測(cè)汽車周圍視覺(jué)環(huán)境；所述imu慣性測(cè)量單元4采集測(cè)量汽車三軸姿態(tài)角以及加速度；所述三維激光雷達(dá)5采集汽車周圍環(huán)境的點(diǎn)云信息。其中汽車大腦1配備高性能的cpu和gpu.

如圖3所示，給出了本發(fā)明實(shí)現(xiàn)自主定位的圖示，圖中的直線為汽車定位的位姿。

圖4、圖5分別為本發(fā)明實(shí)現(xiàn)地圖構(gòu)建的圖示。本發(fā)明提供的一種無(wú)人駕駛汽車自主定位與地圖構(gòu)建的方法，利用slam的技術(shù)，將多種傳感器數(shù)據(jù)進(jìn)行融合，采用新型的算法結(jié)構(gòu)，構(gòu)建穩(wěn)定有效的框架，對(duì)無(wú)人駕駛汽車進(jìn)行穩(wěn)定有效的自主定位與地圖構(gòu)建，應(yīng)用到無(wú)人駕駛汽車系統(tǒng)上，進(jìn)行大規(guī)模應(yīng)用。

以上僅是本發(fā)明的優(yōu)選實(shí)施方式，應(yīng)當(dāng)指出：對(duì)于本技術(shù)領(lǐng)域的普通技術(shù)人員來(lái)說(shuō)，在不脫離本發(fā)明原理的前提下，還可以做出若干改進(jìn)和潤(rùn)飾，這些改進(jìn)和潤(rùn)飾也應(yīng)視為本發(fā)明的保護(hù)范圍。