本發(fā)明涉及激光雷達(dá)，尤其是涉及一種基于主輔式激光雷達(dá)和組合導(dǎo)航的道路三維重建方法。

背景技術(shù)：

1、激光雷達(dá)(light?detection?and?ranging，lidar)是一種先進(jìn)的三維實(shí)景復(fù)制技術(shù)，其生成的三維點(diǎn)云相比圖像視頻在表征不同復(fù)雜對(duì)象時(shí)具有更豐富的空間幾何信息。移動(dòng)激光掃描(mobile?laser?scanning，mls)系統(tǒng)是一種將激光雷達(dá)和其他傳感器固定在移動(dòng)載體(通常為車輛)上的裝置，通過(guò)其采集的三維點(diǎn)云數(shù)據(jù)近年來(lái)已被廣泛應(yīng)用于土木和交通工程領(lǐng)域，如三維重建、自動(dòng)駕駛和安全系統(tǒng)等。道路三維重建旨在最大程度還原真實(shí)的道路場(chǎng)景，并為城市三維建模、路面變形類病害巡查、道路線形幾何參數(shù)計(jì)算、道路資產(chǎn)清查以及交通基礎(chǔ)設(shè)施數(shù)字化等應(yīng)用提供高質(zhì)量的三維點(diǎn)云數(shù)據(jù)。

2、mls系統(tǒng)通常由lidar和組合導(dǎo)航(integrated?navigation?system，ins)構(gòu)成。然而，現(xiàn)有的商業(yè)mls系統(tǒng)大多由國(guó)外生產(chǎn)開(kāi)發(fā)，價(jià)格十分昂貴(通常在25萬(wàn)美元以上)，體型笨重且有時(shí)難以操作，這限制了全球用戶數(shù)量，也阻礙了mls系統(tǒng)快速普及和應(yīng)用。mls系統(tǒng)的制造屬于商業(yè)機(jī)密，各廠商均沒(méi)有公開(kāi)技術(shù)細(xì)節(jié)。鑒于此，本發(fā)明旨在自主研發(fā)一種基于主輔lidar和ins的低成本且輕量化的mls系統(tǒng)裝置(少于5,000美元)，并基于采集的mls點(diǎn)云數(shù)據(jù)提出準(zhǔn)確高效的道路三維重建方法。本發(fā)明為中小型企業(yè)和研究機(jī)構(gòu)等提供了一種低成本解決方案，用于替代昂貴的商用mls系統(tǒng)。

3、在大多數(shù)商用的mls系統(tǒng)中，通常只使用一臺(tái)機(jī)械式旋轉(zhuǎn)激光雷達(dá)進(jìn)行數(shù)據(jù)采集，但這類激光雷達(dá)通常價(jià)格昂貴，且由于機(jī)械式旋轉(zhuǎn)激光雷達(dá)是360度旋轉(zhuǎn)，導(dǎo)致采集到的點(diǎn)云數(shù)據(jù)中只有一小部分是道路，其余都是建筑物、車輛和行人等，這對(duì)道路三維重建任務(wù)是不利的。對(duì)此，本發(fā)明創(chuàng)新性地提出了主輔激光雷達(dá)協(xié)作的數(shù)據(jù)采集模式，且采用性價(jià)比更高的半固態(tài)激光雷達(dá)(其使用非重復(fù)掃描技術(shù)，隨著積分時(shí)間的增大，點(diǎn)云視場(chǎng)覆蓋率會(huì)持續(xù)增大，探測(cè)到視場(chǎng)中的更多細(xì)節(jié))。主激光雷達(dá)專注于路面點(diǎn)云數(shù)據(jù)的收集，而輔助激光雷達(dá)專注于捕捉路緣和部分路側(cè)的點(diǎn)云數(shù)據(jù)。這種主輔激光雷達(dá)協(xié)作采集數(shù)據(jù)的模式在增加道路點(diǎn)云數(shù)據(jù)的同時(shí)，也能保證覆蓋道路路緣以及部分路側(cè)點(diǎn)云數(shù)據(jù)，從而為道路三維重建提供更豐富且有效的細(xì)節(jié)信息，尤其是在道路邊緣附近，其往往是交通安全管控中的關(guān)鍵區(qū)域。

4、在mls系統(tǒng)中，lidar設(shè)備通常安裝在移動(dòng)車輛上，用于連續(xù)采集三維點(diǎn)云數(shù)據(jù)。然而，車輛運(yùn)動(dòng)會(huì)導(dǎo)致lidar和ins的位置和姿態(tài)發(fā)生變化，相鄰的點(diǎn)云幀之間會(huì)存在位置和角度的偏差，因此需要通過(guò)點(diǎn)云配準(zhǔn)技術(shù)來(lái)校正這些偏差，以確保點(diǎn)云數(shù)據(jù)的空間連續(xù)性和準(zhǔn)確性。點(diǎn)云配準(zhǔn)是三維道路重建過(guò)程中最重要的步驟，其主要目的是將從多個(gè)不同視角或不同時(shí)間戳采集的點(diǎn)云數(shù)據(jù)融合到統(tǒng)一的坐標(biāo)系中。配準(zhǔn)結(jié)果將直接決定道路三維重建的精確度和可靠性，因此，有效的點(diǎn)云配準(zhǔn)技術(shù)是實(shí)現(xiàn)高質(zhì)量道路三維重建的關(guān)鍵。但是，目前的點(diǎn)云重建方法在處理復(fù)雜或動(dòng)態(tài)變化的道路環(huán)境時(shí)，尤其是在存在大量相似點(diǎn)云的場(chǎng)景中，往往無(wú)法提供高精度和高魯棒性，易受震動(dòng)等環(huán)境干擾，需要良好的初始姿態(tài)和多次迭代運(yùn)算，計(jì)算時(shí)間長(zhǎng)，難以滿足實(shí)時(shí)或近實(shí)時(shí)道路信息提取的需求。

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

1、本發(fā)明的目的就是為了提升道路三維重建的精確度和技術(shù)魯棒性，同時(shí)減少由于點(diǎn)云密度不一和環(huán)境干擾導(dǎo)致的誤差而提供的一種基于主輔式激光雷達(dá)和組合導(dǎo)航的道路三維重建方法。

2、本發(fā)明的目的可以通過(guò)以下技術(shù)方案來(lái)實(shí)現(xiàn)：

3、一種基于主輔式激光雷達(dá)和組合導(dǎo)航的道路三維重建方法，方法包括以下步驟：

4、步驟s1：基于主輔式激光雷達(dá)lidar和組合導(dǎo)航ins將車輛獲取的初始點(diǎn)云數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，得到ecef坐標(biāo)系下的點(diǎn)云進(jìn)行過(guò)濾，得到濾后點(diǎn)云數(shù)據(jù)，所述初始點(diǎn)云數(shù)據(jù)包括路面和道路兩側(cè)的點(diǎn)云數(shù)據(jù)；

5、步驟s2：對(duì)濾后點(diǎn)云數(shù)據(jù)的移動(dòng)幀進(jìn)行行進(jìn)方向配準(zhǔn)，得到配準(zhǔn)行進(jìn)方向后的點(diǎn)云數(shù)據(jù)；

6、步驟s3：對(duì)配準(zhǔn)行進(jìn)方向后的點(diǎn)云數(shù)據(jù)進(jìn)行路緣點(diǎn)云和路面點(diǎn)云的提取；

7、步驟s4：對(duì)路面點(diǎn)云進(jìn)行細(xì)化路面配準(zhǔn)，得到路面配準(zhǔn)后的路面點(diǎn)云；

8、步驟s5：對(duì)路緣點(diǎn)云進(jìn)行精準(zhǔn)路緣配準(zhǔn)，得到路緣配準(zhǔn)后的路緣點(diǎn)云，結(jié)合路面配準(zhǔn)后的路面點(diǎn)云得到重建點(diǎn)云數(shù)據(jù)。

9、進(jìn)一步地，步驟s1具體為：

10、步驟s1.1：通過(guò)剛體變換，將主輔式激光雷達(dá)lidar坐標(biāo)系下的初始點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到慣性測(cè)量單元imu坐標(biāo)系；

11、步驟s1.2：將慣性測(cè)量單元imu坐標(biāo)系下的點(diǎn)云轉(zhuǎn)化到東北天enu坐標(biāo)系；

12、步驟s1.3：將東北天enu坐標(biāo)系下的點(diǎn)云轉(zhuǎn)化到地心地固直角ecef坐標(biāo)系，得到ecef坐標(biāo)系下的點(diǎn)云；

13、步驟s1.4：選取時(shí)刻t1、t2下組合導(dǎo)航ins輸出的位置和姿態(tài)數(shù)據(jù)，分別計(jì)算主輔式激光雷達(dá)lidar在t1、t2時(shí)刻的原點(diǎn)和慣性測(cè)量單元imu的原點(diǎn)，并分別采用三個(gè)原點(diǎn)在ecef坐標(biāo)系下的的坐標(biāo)作為參考點(diǎn)r1、r2、r3，定義分割平面κi，其中i＝1，2，3，4，將ecef坐標(biāo)系下的點(diǎn)云與分割平面比較，保留與分割平面相交或在分割平面內(nèi)部的點(diǎn)云作為濾后點(diǎn)云數(shù)據(jù)。

14、進(jìn)一步地，ecef坐標(biāo)系下的點(diǎn)云的每個(gè)點(diǎn)為：

15、pecef＝meemei(milplidar+pil)+pee

16、其中，pecef為ecef坐標(biāo)系中的點(diǎn)，mee為從enu坐標(biāo)系到ecef坐標(biāo)系旋轉(zhuǎn)矩陣，pee為第一平移向量，mei為從imu坐標(biāo)系到enu坐標(biāo)系的旋轉(zhuǎn)矩陣，mil為從lidar坐標(biāo)系到imu坐標(biāo)系旋轉(zhuǎn)矩陣，pil為第二平移向量。

17、進(jìn)一步地，步驟s2具體為：

18、步驟s2.1：從濾后點(diǎn)云數(shù)據(jù)中提取樹(shù)干點(diǎn)云并投影到與路面平行的平面上，然后采用dbsacn算法對(duì)投影點(diǎn)進(jìn)行聚類，分別得到點(diǎn)云中固定幀和移動(dòng)幀的多個(gè)投影點(diǎn)簇；

19、步驟s2.2：設(shè)點(diǎn)云中v＝{v1,v2,…,vn}為固定點(diǎn)，u＝{u1,u2,…,un}為移動(dòng)點(diǎn)，所述固定點(diǎn)形成固定幀，移動(dòng)點(diǎn)形成移動(dòng)幀，設(shè)t*為從u到v的最優(yōu)平移向量，定義最小二乘意義上的誤差函數(shù)e(t)并得出從移動(dòng)點(diǎn)質(zhì)心到固定點(diǎn)質(zhì)心的最優(yōu)平移向量t*，所述最優(yōu)平移向量t*具體為：

20、

21、步驟s2.3：設(shè)為固定幀的投影點(diǎn)簇質(zhì)心，為移動(dòng)幀的投影點(diǎn)簇質(zhì)心，在投影點(diǎn)簇中查找pc和qc之間的匹配投影點(diǎn)簇對(duì)；

22、步驟s2.4：根據(jù)最優(yōu)平移向量和匹配投影點(diǎn)簇對(duì)的數(shù)量對(duì)點(diǎn)云的移動(dòng)幀進(jìn)行配準(zhǔn)，得到配準(zhǔn)后的點(diǎn)云移動(dòng)幀，得到配準(zhǔn)行進(jìn)方向后的點(diǎn)云數(shù)據(jù)，配準(zhǔn)的過(guò)程為：

23、

24、其中，(vr,vt)和(uh,uk)為一組匹配點(diǎn)對(duì)，sopt表示最優(yōu)平移向量，κ表示匹配的聚類簇對(duì)的數(shù)量，2κ表示匹配點(diǎn)對(duì)的數(shù)量，pmoving表示移動(dòng)幀中的任意點(diǎn)。

25、進(jìn)一步地，所述匹配投影點(diǎn)簇對(duì)滿足：

26、

27、其中，i為任意移動(dòng)點(diǎn)u匹配的聚類簇的指標(biāo)，ρ為給定執(zhí)行的歐式距離閾值，dij為固定幀中的點(diǎn)pj與移動(dòng)幀中的點(diǎn)qi之間的歐氏距離，dj為以dij為元素的距離向量集。

28、進(jìn)一步地，步驟s3具體為：

29、步驟s3.1：根據(jù)初始平面w，得到雙平面窗口w1-w2，在配準(zhǔn)行進(jìn)方向后的點(diǎn)云數(shù)據(jù)的道路點(diǎn)云上以給定步長(zhǎng)向兩側(cè)滑動(dòng)雙平面窗口w1-w2；

30、步驟s3.2：在窗口滑動(dòng)過(guò)程中，攔截位于當(dāng)前雙平面窗口w1和w2中的點(diǎn)云，記為a；

31、步驟s3.3：對(duì)攔截的點(diǎn)云進(jìn)行平面擬合，如果擬合平面與初始平面w之間的夾角大于給定的閾值，則確定a中滿足條件的點(diǎn)為路緣點(diǎn)云；

32、步驟s3.4：找到所有路緣點(diǎn)云后，兩側(cè)路緣點(diǎn)云之間的點(diǎn)云為路面點(diǎn)云。

33、進(jìn)一步地，步驟s4具體為：

34、步驟s4.1：設(shè)uf和um分別為固定幀和移動(dòng)幀的路面點(diǎn)云的擬合平面，記rv為羅德里格斯旋轉(zhuǎn)矩陣，p0代表旋轉(zhuǎn)軸上的一個(gè)點(diǎn)，計(jì)算旋轉(zhuǎn)矩陣rp；

35、步驟s4.2：設(shè)p1為平面um上的一個(gè)點(diǎn)，求解um到uf的第一齊次平移變換矩陣，使兩個(gè)擬合平面um到uf相互重合，所述第一齊次平移變換矩陣為：

36、

37、步驟s4.3：基于knn算法思想提取移動(dòng)幀和固定幀之間的重疊點(diǎn)云，然后采用icp算法對(duì)重疊點(diǎn)云進(jìn)行細(xì)化路面配準(zhǔn)，得到路面配準(zhǔn)后的點(diǎn)云[pmoving:]←micpt1rp[pmoving:]，其中，pmoving:表示移動(dòng)幀中路面點(diǎn)云的任意一點(diǎn)。[u:]表示u的齊次坐標(biāo)形式。

38、進(jìn)一步地，所述旋轉(zhuǎn)矩陣rp為：

39、

40、其中，rv為羅德里格斯旋轉(zhuǎn)矩陣，p0代表旋轉(zhuǎn)軸上的一個(gè)點(diǎn)。

41、進(jìn)一步地，步驟s5具體為：

42、步驟s5.1：首先對(duì)路緣點(diǎn)云進(jìn)行平面擬合，以確定路緣的幾何位置，然后將路緣點(diǎn)云沿其法線方向滑動(dòng)，截取與路緣相鄰的路面點(diǎn)云，將相鄰的路面點(diǎn)云再次平面擬合，基于擬合的平面的交線確定路緣和路面的交線；

43、步驟s5.2：設(shè)路緣和路面的交線在固定幀和移動(dòng)幀中的方向向量分別為lf和lm，pf和pm分別為lf和lm上的任意一點(diǎn)，計(jì)算lm到lf的旋轉(zhuǎn)角β、旋轉(zhuǎn)軸向量k和旋轉(zhuǎn)軸上的一點(diǎn)p0，將計(jì)算得到的參數(shù)代入旋轉(zhuǎn)矩陣rp的公式，得到從移動(dòng)幀到固定幀的齊次旋轉(zhuǎn)矩陣rp1；

44、步驟s5.3：求解使得兩條擬合線lf和lm重合的第二齊次平移矩陣t，得到路緣配準(zhǔn)后的路緣點(diǎn)云[pmoving:]←micptrp1[pmoving:]，其中pmoving:表示移動(dòng)幀的路緣點(diǎn)云中任一點(diǎn)。

45、進(jìn)一步地，所述lm到lf的旋轉(zhuǎn)角β、旋轉(zhuǎn)軸向量k和旋轉(zhuǎn)軸上的一點(diǎn)p0為：

46、

47、其中，p1表示路緣點(diǎn)云的平均值；

48、第二齊次平移矩陣t為：

49、

50、其中，p0表示旋轉(zhuǎn)軸上的一點(diǎn)，即點(diǎn)p1到lm的投影。

51、與現(xiàn)有技術(shù)相比，本發(fā)明具有以下有益效果：

52、(1)本發(fā)明采用基于投影點(diǎn)簇的新型關(guān)鍵點(diǎn)匹配方法，能夠有效處理具有多相似形狀或點(diǎn)的復(fù)雜道路場(chǎng)景，顯著減少了沿行駛方向(y軸)的位移偏差以及錯(cuò)誤匹配。此外，結(jié)合雙平面滑動(dòng)窗口算法，本發(fā)明不僅精確地提取出路面及路緣的點(diǎn)云，而且在多變道路條件下優(yōu)化了數(shù)據(jù)處理流程，極大提升了三維重建的精細(xì)度和整體質(zhì)量，且本發(fā)明引入了平面對(duì)平面的配準(zhǔn)方法和基于knn算法的重疊點(diǎn)云提取技術(shù)，實(shí)現(xiàn)了對(duì)路面點(diǎn)云由粗到精的校準(zhǔn)，有效矯正了路面點(diǎn)云的垂直方向(z軸)和路緣點(diǎn)云的x軸方向上的誤差，提高了配準(zhǔn)結(jié)果的準(zhǔn)確性和可靠性；同時(shí)，還提出了基于相交線的配準(zhǔn)方法，用于精確校準(zhǔn)路緣點(diǎn)云，有效矯正了水平方向(x軸)上的誤差，滿足實(shí)時(shí)或近實(shí)時(shí)道路信息提取的需求。通過(guò)強(qiáng)調(diào)沿x、y和z三個(gè)方向的精確控制，有效降低了由于點(diǎn)云密度不一和環(huán)境干擾導(dǎo)致的誤差，優(yōu)化了整個(gè)道路三維重建過(guò)程。

53、(2)本發(fā)明提出了主激光雷達(dá)和輔助激光雷達(dá)協(xié)作采集方式，并對(duì)激光雷達(dá)(lidar)與組合導(dǎo)航(ins)數(shù)據(jù)進(jìn)行融合，融合后的點(diǎn)云數(shù)據(jù)完整還原了道路三維場(chǎng)景。通過(guò)這種創(chuàng)新的主輔式雷達(dá)數(shù)據(jù)采集與融合方法，顯著增強(qiáng)了數(shù)據(jù)的覆蓋范圍和質(zhì)量，提高了三維重建的細(xì)節(jié)與準(zhǔn)確性，特別是在復(fù)雜的道路環(huán)境或多變的道路邊緣條件下，本系統(tǒng)能夠有效捕獲到完整的道路信息，增強(qiáng)了系統(tǒng)的適用性和可靠性，可實(shí)現(xiàn)道路路面及路緣的精準(zhǔn)三維重建。