本發(fā)明涉及自動(dòng)駕駛汽車目標(biāo)檢測(cè)，特別是涉及一種基于視覺(jué)傳感器與毫米波雷達(dá)融合的目標(biāo)檢測(cè)與跟蹤方法。

背景技術(shù)：

1、自動(dòng)駕駛汽車的核心在于其能夠在沒(méi)有人為干預(yù)的情況下自主完成駕駛?cè)蝿?wù)。為實(shí)現(xiàn)這一目標(biāo)，自動(dòng)駕駛系統(tǒng)需要對(duì)周圍環(huán)境進(jìn)行精確感知和理解，以便做出安全、有效的駕駛決策。傳感器是自動(dòng)駕駛系統(tǒng)的“感官”，其中視覺(jué)傳感器和毫米波雷達(dá)是最常用的兩種類型。盡管這兩種傳感器各有優(yōu)缺點(diǎn)，但它們?cè)谧詣?dòng)駕駛技術(shù)中都扮演著至關(guān)重要的角色。

2、視覺(jué)傳感器，特別是攝像頭，在自動(dòng)駕駛系統(tǒng)中具有廣泛應(yīng)用。它們能夠捕捉高分辨率的圖像和視頻，提供豐富的環(huán)境信息，如車道標(biāo)記、交通標(biāo)志、行人及其他車輛等。視覺(jué)傳感器的主要優(yōu)點(diǎn)包括高分辨率、豐富的顏色信息以及能夠檢測(cè)和識(shí)別復(fù)雜的物體和場(chǎng)景。然而，視覺(jué)傳感器也存在一些顯著的局限性。例如，在光線不足、強(qiáng)光干擾或天氣條件惡劣(如雨雪、霧天)時(shí)，攝像頭的性能會(huì)大大降低。此外，視覺(jué)傳感器處理圖像數(shù)據(jù)需要較高的計(jì)算資源，這對(duì)實(shí)時(shí)性要求高的自動(dòng)駕駛系統(tǒng)來(lái)說(shuō)是一個(gè)挑戰(zhàn)。

3、毫米波雷達(dá)是一種利用毫米波段電磁波進(jìn)行探測(cè)的傳感器，具有良好的穿透能力和高精度的測(cè)距、測(cè)速能力。毫米波雷達(dá)能夠在各種天氣條件下(如雨、雪、霧等)穩(wěn)定工作，不受光線變化的影響。因此，它在環(huán)境感知中具有獨(dú)特的優(yōu)勢(shì)，特別是在檢測(cè)車輛、行人和其他障礙物的距離和速度方面。然而，毫米波雷達(dá)也存在一些缺陷。首先，它的分辨率較低，難以精確識(shí)別物體的形狀和細(xì)節(jié)。其次，雷達(dá)信號(hào)容易受到多路徑效應(yīng)和干擾的影響，導(dǎo)致探測(cè)精度降低。此外，毫米波雷達(dá)的數(shù)據(jù)量較大，需要有效的信號(hào)處理算法來(lái)提取有用信息。

4、為了彌補(bǔ)單一傳感器的不足，提高環(huán)境感知的準(zhǔn)確性和可靠性，自動(dòng)駕駛系統(tǒng)通常采用多傳感器融合技術(shù)。傳感器融合技術(shù)分為前融合和后融合兩種。前融合是指在傳感器層面進(jìn)行數(shù)據(jù)融合，而后融合則是在特征提取或決策層面進(jìn)行數(shù)據(jù)融合。目前，大多數(shù)自動(dòng)駕駛系統(tǒng)采用前融合技術(shù)，將來(lái)自不同傳感器的數(shù)據(jù)在傳感器層面進(jìn)行初步融合。這種方法能夠利用多種傳感器的優(yōu)勢(shì)，但其缺點(diǎn)在于數(shù)據(jù)處理復(fù)雜，難以應(yīng)對(duì)各種環(huán)境變化。相比之下，后融合技術(shù)通過(guò)分別處理各傳感器的數(shù)據(jù)，再在特征或決策層面進(jìn)行融合，能夠更靈活地適應(yīng)環(huán)境變化，提高系統(tǒng)的魯棒性和精度。

5、將視覺(jué)傳感器和毫米波雷達(dá)的后融合應(yīng)用于自動(dòng)駕駛系統(tǒng)，能夠充分利用兩種傳感器的優(yōu)勢(shì)，彌補(bǔ)各自的不足。具體來(lái)說(shuō)，視覺(jué)傳感器可以提供高分辨率的圖像信息，有助于識(shí)別和分類物體；毫米波雷達(dá)則能夠精確測(cè)量物體的距離和速度，且不受天氣和光線條件的影響。通過(guò)后融合技術(shù)，可以在決策層面綜合兩種傳感器的數(shù)據(jù)，形成更為準(zhǔn)確和可靠的環(huán)境感知結(jié)果。例如，在城市道路場(chǎng)景中，視覺(jué)傳感器可以識(shí)別和分類路上的行人、車輛和交通標(biāo)志，而毫米波雷達(dá)可以提供這些物體的距離和速度信息。通過(guò)后融合算法，將視覺(jué)傳感器的分類結(jié)果與毫米波雷達(dá)的測(cè)距信息結(jié)合起來(lái)，可以更準(zhǔn)確地感知周圍環(huán)境，提高自動(dòng)駕駛系統(tǒng)的安全性和可靠性。

6、此外，后融合技術(shù)還具有較高的靈活性和可擴(kuò)展性。它可以根據(jù)不同的應(yīng)用場(chǎng)景和需求，自適應(yīng)地調(diào)整融合算法和參數(shù)，適應(yīng)各種復(fù)雜的駕駛環(huán)境。例如，在高速公路上，毫米波雷達(dá)的作用可能更為重要，而在城市街道上，視覺(jué)傳感器的作用則更加顯著。通過(guò)靈活調(diào)整后融合算法，可以實(shí)現(xiàn)對(duì)不同駕駛環(huán)境的優(yōu)化感知。

7、盡管視覺(jué)與毫米波雷達(dá)的后融合技術(shù)具有顯著的優(yōu)勢(shì)，但其實(shí)現(xiàn)過(guò)程中仍面臨一些挑戰(zhàn)。首先是數(shù)據(jù)同步和校準(zhǔn)問(wèn)題。由于視覺(jué)傳感器和毫米波雷達(dá)的工作原理不同，數(shù)據(jù)采集頻率和延遲也不同，如何實(shí)現(xiàn)兩者數(shù)據(jù)的準(zhǔn)確同步和空間校準(zhǔn)是一個(gè)難點(diǎn)。其次，后融合算法的設(shè)計(jì)和實(shí)現(xiàn)需要考慮多種因素，如數(shù)據(jù)處理的實(shí)時(shí)性、算法的復(fù)雜性和計(jì)算資源的需求等。此外，還需要進(jìn)行大量的實(shí)驗(yàn)和測(cè)試，驗(yàn)證算法的有效性和可靠性。

8、為了克服上述問(wèn)題，本發(fā)明提出了一種視覺(jué)傳感器和毫米波雷達(dá)信息融合的目標(biāo)檢測(cè)與跟蹤技術(shù)，各傳感器獨(dú)立進(jìn)行目標(biāo)檢測(cè)與目標(biāo)跟蹤，數(shù)據(jù)融合方法采用決策級(jí)融合，有效克服了單一傳感器在目標(biāo)感知方面存在的精度不高且受環(huán)境影響較大的問(wèn)題。

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

1、本發(fā)明的目的是針對(duì)現(xiàn)有技術(shù)中存在的單一傳感器檢測(cè)準(zhǔn)確率不夠高且受到外界環(huán)境變化干擾比較大的問(wèn)題，而提供一種基于視覺(jué)傳感器與毫米波雷達(dá)融合的目標(biāo)檢測(cè)與跟蹤方法。

2、為實(shí)現(xiàn)本發(fā)明的目的所采用的技術(shù)方案是：

3、一種基于視覺(jué)傳感器與毫米波雷達(dá)融合的目標(biāo)檢測(cè)與跟蹤方法，包括以下步驟：

4、步驟1，對(duì)視覺(jué)傳感器拍攝的圖像中的被檢測(cè)目標(biāo)進(jìn)行實(shí)時(shí)檢測(cè)，得到目標(biāo)先驗(yàn)框；

5、步驟2，對(duì)步驟1得到所述目標(biāo)先驗(yàn)框中的目標(biāo)特征點(diǎn)進(jìn)行投影變換，得到被檢測(cè)目標(biāo)在雷達(dá)坐標(biāo)系下的坐標(biāo)；

6、步驟3，對(duì)毫米波雷達(dá)的檢測(cè)信號(hào)進(jìn)行濾波處理，去除噪聲，得到有效信號(hào)；

7、步驟4，利用交互式多模型imm算法對(duì)步驟3得到的有效信號(hào)進(jìn)行連續(xù)追蹤，從而得到被檢測(cè)目標(biāo)的橫縱向距離和橫縱向速度，進(jìn)而得到毫米波雷達(dá)信號(hào)目標(biāo)在雷達(dá)坐標(biāo)系中的坐標(biāo)；

8、步驟5，根據(jù)步驟4得到的毫米波雷達(dá)信號(hào)目標(biāo)在雷達(dá)坐標(biāo)系中的坐標(biāo)和步驟2得到的視覺(jué)識(shí)別到的目標(biāo)在雷達(dá)坐標(biāo)系中的映射點(diǎn)的坐標(biāo)，計(jì)算每個(gè)雷達(dá)探測(cè)目標(biāo)和視覺(jué)識(shí)別目標(biāo)的匹配代價(jià)dijk，根據(jù)匹配代價(jià)計(jì)算結(jié)果，判斷輸出融合檢測(cè)結(jié)果。

9、在上述技術(shù)方案中，所述步驟1中，利用基于卷積神經(jīng)網(wǎng)絡(luò)的yolov5s算法進(jìn)行檢測(cè)，得到目標(biāo)先驗(yàn)框。

10、在上述技術(shù)方案中，所述步驟2中，所述目標(biāo)特征點(diǎn)為目標(biāo)先驗(yàn)框的底邊中點(diǎn)。

11、在上述技術(shù)方案中，所述步驟2中，所述投影變換的步驟為：

12、將所述目標(biāo)特征點(diǎn)由原始的像素坐標(biāo)系轉(zhuǎn)換到圖像坐標(biāo)系，再將目標(biāo)特征點(diǎn)進(jìn)行投影變換，將其轉(zhuǎn)換到相機(jī)坐標(biāo)系，之后通過(guò)旋轉(zhuǎn)平移變換轉(zhuǎn)換到雷達(dá)坐標(biāo)系中。

13、在上述技術(shù)方案中，視覺(jué)得到的目標(biāo)特征點(diǎn)的像素坐標(biāo)為(u，v)，其對(duì)應(yīng)的雷達(dá)坐標(biāo)系坐標(biāo)為(x，y，z)，在相機(jī)坐標(biāo)系下的坐標(biāo)為(xc，yc，zc)，dx和dy是像素的物理尺寸，f為焦距，(u0，v0)為相機(jī)的光心在圖像坐標(biāo)系下的坐標(biāo)，r和t分別為雷達(dá)坐標(biāo)系和相機(jī)坐標(biāo)系之間的旋轉(zhuǎn)矩陣和平移向量，則像素坐標(biāo)系與雷達(dá)坐標(biāo)系之間的變換公式為：

14、

15、其中，zc是目標(biāo)特征點(diǎn)在相機(jī)坐標(biāo)系下的z值，k是視覺(jué)傳感器的內(nèi)參矩陣，k的表達(dá)式為：

16、

17、由視覺(jué)得到的目標(biāo)特征點(diǎn)在雷達(dá)坐標(biāo)系下對(duì)應(yīng)坐標(biāo)為：

18、

19、設(shè)[a1?a2?a3]t表示r-1k-1，其中ai表示矩陣r-1k-1矩陣的第i行，[b1?b2?b3]t表示r-1t，bi表示矩陣r-1t的第i行。則公式(3)可以表示為：

20、

21、要求出像素坐標(biāo)系下的點(diǎn)在雷達(dá)坐標(biāo)系下的坐標(biāo)，還需要求出未知數(shù)求zc，假設(shè)來(lái)自圖像目標(biāo)先驗(yàn)框的底邊在地平面上，雷達(dá)的安裝高度為h，則圖像中目標(biāo)先驗(yàn)框的底邊中點(diǎn)在雷達(dá)坐標(biāo)系下的坐標(biāo)可以表示為：

22、

23、由公式(5)，可以得到：

24、

25、將公式(6)代入公式(4)，像素坐標(biāo)系與雷達(dá)坐標(biāo)系之間的投影關(guān)系可以表示為：

26、

27、經(jīng)過(guò)公式(7)，在測(cè)得雷達(dá)安裝高度h后，可以計(jì)算視覺(jué)檢測(cè)目標(biāo)特征點(diǎn)在雷達(dá)坐標(biāo)系中的坐標(biāo)。

28、在上述技術(shù)方案中，所述步驟3中，根據(jù)雷達(dá)檢測(cè)目標(biāo)的位置、速度、反射率和生命周期等對(duì)無(wú)效目標(biāo)、幽靈目標(biāo)進(jìn)行濾除，無(wú)效目標(biāo)分為空目標(biāo)、假目標(biāo)和無(wú)威脅目標(biāo)，并通過(guò)不同的方式進(jìn)行過(guò)濾。具體包括以下步驟：

29、步驟s1，濾除空目標(biāo)：雷達(dá)反射截面積rcs為0時(shí)，將其濾除；

30、步驟s2，濾除非威脅目標(biāo)：根據(jù)經(jīng)驗(yàn)，設(shè)置水平距離閾值為x，置垂直距離閾值為y，將目標(biāo)的水平坐標(biāo)x與閾值x進(jìn)行比較，將目標(biāo)的垂直坐標(biāo)y與閾值y進(jìn)行比較，如果|x|>x或|y|>y時(shí)，則將其過(guò)濾掉；

31、步驟s3，濾除假目標(biāo)：創(chuàng)建一個(gè)線程組合連續(xù)雷達(dá)幀的結(jié)果，并設(shè)置一個(gè)閾值ε，當(dāng)xk-xk-1的絕對(duì)值大于ε時(shí)，則將其過(guò)濾掉，xk為目標(biāo)在k時(shí)刻的狀態(tài)向量，xk-1為目標(biāo)在k-1時(shí)刻的狀態(tài)向量；

32、步驟s4，濾除幽靈目標(biāo)：有效的目標(biāo)生命周期方法分為“生成”、“持久”和“消失”三個(gè)階段，“生成”的狀態(tài)參數(shù)為detectcount+1and?lostcount＝0，參數(shù)含義為“檢測(cè)到前方有目標(biāo)，目標(biāo)檢測(cè)次數(shù)+1，且丟失次數(shù)清0”，“持久”的狀態(tài)參數(shù)為detectcount≥cd?andlostcount≤cl，參數(shù)含義為“當(dāng)目標(biāo)檢測(cè)次數(shù)大于閾值cd且丟失次數(shù)小于閾值cl時(shí)，目標(biāo)為真目標(biāo)”，“消失”的狀態(tài)參數(shù)為detectcount<cd?or?lostcount>cl，參數(shù)含義為“當(dāng)目標(biāo)檢測(cè)次數(shù)小于閾值cd或丟失次數(shù)大于閾值cl時(shí)，目標(biāo)為假目標(biāo)”，狀態(tài)參數(shù)detectcount為目標(biāo)被檢測(cè)到的次數(shù)，狀態(tài)參數(shù)lostcount為目標(biāo)連續(xù)丟失的次數(shù)，當(dāng)detectcount≥cd?andlostcount≤c時(shí)，輸出有效目標(biāo)，否則將其濾除。

33、在上述技術(shù)方案中，所述步驟4中，利用交互式多模型卡爾曼濾波對(duì)毫米波雷達(dá)探測(cè)到的有效信號(hào)進(jìn)行目標(biāo)追蹤，對(duì)下一周期的目標(biāo)狀態(tài)進(jìn)行預(yù)測(cè)，讀取下一幀毫米波雷達(dá)實(shí)際測(cè)量數(shù)據(jù)，將預(yù)測(cè)狀態(tài)與實(shí)測(cè)狀態(tài)進(jìn)行匹配，具體包括以下步驟：

34、步驟a，根據(jù)毫米波雷達(dá)探測(cè)到的初始目標(biāo)信息，設(shè)定初始狀態(tài)向量和協(xié)方差矩陣。選擇恒速模型cv和恒加速模型ca，為每個(gè)模型初始化狀態(tài)向量、協(xié)方差矩陣和轉(zhuǎn)移概率矩陣；

35、步驟b、對(duì)每個(gè)模型mi(cv或ca)狀態(tài)預(yù)測(cè)步驟如下：

36、

37、其中，表示k-1時(shí)刻濾波器i的狀態(tài)估計(jì)，表示交互后的混合狀態(tài)估計(jì)，所述狀態(tài)包括橫縱向距離和橫縱向速度變量，i＝1，2，3……，為狀態(tài)轉(zhuǎn)移矩陣，表示相應(yīng)的狀態(tài)協(xié)方差矩陣，表示交互后的混合狀態(tài)協(xié)方差矩陣，是過(guò)程噪聲協(xié)方差矩陣；

38、步驟c、對(duì)每個(gè)模型mi，卡爾曼濾波更新步驟如下：

39、

40、其中，是k時(shí)刻模型mi的卡爾曼濾波增益，表示在狀態(tài)時(shí)的雅可比矩陣，zk是目標(biāo)在k時(shí)刻的觀測(cè)值，是殘差協(xié)方差矩陣，是測(cè)量噪聲協(xié)方差矩陣；

41、步驟d、每個(gè)模型的概率更新步驟如下：

42、

43、其中，是測(cè)量值高斯似然函數(shù)，是模型mi在k時(shí)刻的概率，ci是模型mi的初始概率；

44、步驟e、模型交互步驟如下：

45、

46、j表示第j個(gè)濾波器，表示模型mi和模型moj之間的差異，模型moj是轉(zhuǎn)換后的狀態(tài)；

47、步驟f、最終狀態(tài)估計(jì)和預(yù)測(cè)協(xié)方差矩陣更新步驟如下：

48、

49、在上述技術(shù)方案中，所述步驟5的決策層融合包括以下步驟：

50、步驟5a、對(duì)同一時(shí)刻，根據(jù)步驟4得到的毫米波雷達(dá)信號(hào)目標(biāo)在雷達(dá)坐標(biāo)系中的坐標(biāo)和步驟2視覺(jué)識(shí)別到的目標(biāo)在雷達(dá)坐標(biāo)系中的映射點(diǎn)的坐標(biāo)，兩兩計(jì)算歐式距離d，作為匹配代價(jià)，記dijk為k時(shí)刻視覺(jué)識(shí)別到的第i個(gè)目標(biāo)與毫米波雷達(dá)探測(cè)到的第j個(gè)目標(biāo)的匹配代價(jià)，保留各視覺(jué)識(shí)別到的目標(biāo)與毫米波雷達(dá)各目標(biāo)最小的匹配代價(jià)。

51、步驟5b、對(duì)匹配代價(jià)計(jì)算結(jié)果進(jìn)行劃分，判斷兩傳感器檢測(cè)結(jié)果是否為同一目標(biāo)，并輸出融合檢測(cè)結(jié)果，完成毫米波雷達(dá)與視覺(jué)融合的目標(biāo)檢測(cè)，具體包含以下情況：

52、若毫米波雷達(dá)與視覺(jué)均識(shí)別到目標(biāo)，且匹配代價(jià)dijk<dmax時(shí)，則兩傳感器檢測(cè)為同一目標(biāo)，通過(guò)視覺(jué)識(shí)別確定目標(biāo)類別，通過(guò)毫米波雷達(dá)檢測(cè)結(jié)果確定目標(biāo)的運(yùn)動(dòng)狀態(tài)信息，并輸出目標(biāo)類別和運(yùn)動(dòng)狀態(tài)信息；

53、若毫米波雷達(dá)探測(cè)到目標(biāo)，但視覺(jué)未識(shí)別，或者dijk大于dmax，則出現(xiàn)極端天氣或因遮擋視覺(jué)檢測(cè)精度下降，此時(shí)只輸出毫米波雷達(dá)檢測(cè)的目標(biāo)運(yùn)動(dòng)狀態(tài)信息；

54、若視覺(jué)識(shí)別到目標(biāo)，但毫米波雷達(dá)未探測(cè)到，則毫米波雷達(dá)出現(xiàn)目標(biāo)丟失，此時(shí)輸出視覺(jué)識(shí)別到的目標(biāo)的類別信息及其特征點(diǎn)在雷達(dá)坐標(biāo)系下的投影坐標(biāo)。

55、與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果是：

56、1.針對(duì)現(xiàn)有的通過(guò)單目攝像頭進(jìn)行目標(biāo)定位需要假設(shè)目標(biāo)的先驗(yàn)尺寸信息，且定位穩(wěn)定性較差問(wèn)題，提出了利用目標(biāo)與地平面的交點(diǎn)和雷達(dá)安裝高度計(jì)算中間未知量zc的方法，提高了單目視覺(jué)目標(biāo)定位的精度。

57、2.針對(duì)毫米波雷達(dá)虛假目標(biāo)較多，且隨車輛運(yùn)動(dòng)過(guò)程中容易出現(xiàn)目標(biāo)丟失的問(wèn)題，提出了基于目標(biāo)的反射率、位置、速度、存在次數(shù)等性質(zhì)的虛假目標(biāo)濾除算法，并使用交互式多模型卡爾曼濾波對(duì)處理后的毫米波雷達(dá)信號(hào)進(jìn)行目標(biāo)跟蹤，提高了毫米波雷達(dá)探測(cè)的精準(zhǔn)度。

58、3.對(duì)毫米波雷達(dá)和攝像頭的融合是決策級(jí)融合，兩種傳感器首先單獨(dú)檢測(cè)目標(biāo)并進(jìn)行處理，隨后一起輸入融合邏輯模塊，計(jì)算匹配代價(jià)。根據(jù)匹配代價(jià)計(jì)算結(jié)果做出相應(yīng)的判斷，打破傳統(tǒng)主輔融合對(duì)主傳感器的依賴，提高了車輛行駛過(guò)程中目標(biāo)識(shí)別的穩(wěn)定性。