本發(fā)明涉及雷達(dá)信號(hào)處理領(lǐng)域，尤其涉及一種基于群跟蹤算法的點(diǎn)云聚類和跟蹤方法。

背景技術(shù)：

1、近年來(lái)，毫米波雷達(dá)在多目標(biāo)跟蹤和感知方面得到了廣泛研究。這一切都要?dú)w功于雷達(dá)回波中攜帶的多普勒信息和微多普勒，微多普勒一般是指目標(biāo)各個(gè)部分運(yùn)動(dòng)而產(chǎn)生的附加多普勒信息。多普勒特性可以用于確定目標(biāo)的特征，以此來(lái)進(jìn)行目標(biāo)的跟蹤和感知，識(shí)別出的特征最終可轉(zhuǎn)化為目標(biāo)的運(yùn)動(dòng)特性，這歸功于毫米波雷達(dá)波長(zhǎng)極短而具有的高靈敏度。

2、通常，雷達(dá)工作在惡劣的信號(hào)傳播環(huán)境中，其生成的多維點(diǎn)云信息會(huì)受到諸如噪聲、目標(biāo)遮擋、多徑效應(yīng)等影響。這使得對(duì)多目標(biāo)跟蹤和感知任務(wù)變得相當(dāng)困難，例如噪聲會(huì)導(dǎo)致目標(biāo)跟蹤不穩(wěn)定、目標(biāo)遮擋會(huì)使得目標(biāo)跟蹤不連續(xù)、多徑效應(yīng)則會(huì)導(dǎo)致出現(xiàn)虛假目標(biāo)等。目前用于實(shí)現(xiàn)穩(wěn)健可靠的多目標(biāo)跟蹤和感知的研究和技術(shù)，特別是毫米波雷達(dá)，尚未有一個(gè)統(tǒng)一的實(shí)現(xiàn)框架。鑒于此，本發(fā)明提出一種群跟蹤算法，利用點(diǎn)云的多維度信息來(lái)解決上述難點(diǎn)，實(shí)現(xiàn)了一個(gè)點(diǎn)云處理流程。

3、現(xiàn)有技術(shù)方案

4、現(xiàn)有的多目標(biāo)跟蹤方案主要由兩部分組成，第一步先是聚類，第二步進(jìn)行跟蹤，由于毫米波雷達(dá)分辨率較高，會(huì)導(dǎo)致單一目標(biāo)具有多個(gè)點(diǎn)云信息，在這種情況下，原來(lái)的單一目標(biāo)就會(huì)演變成擴(kuò)展目標(biāo)，在對(duì)擴(kuò)展目標(biāo)進(jìn)行跟蹤前，需要先將雷達(dá)視場(chǎng)內(nèi)所有點(diǎn)云按照不同目標(biāo)劃分為不同的簇，每個(gè)簇對(duì)應(yīng)唯一的擴(kuò)展目標(biāo)，最后以簇為單位進(jìn)行目標(biāo)跟蹤。聚類可以視為進(jìn)行空間濾波，跟蹤可以視為進(jìn)行時(shí)間濾波，兩者都是作為單獨(dú)的模塊完成的。

5、1.現(xiàn)有的聚類算法有：

6、(1)k均值聚類：

7、k均值聚類可以說是最常見的聚類算法，由于簡(jiǎn)潔和高效使得它成為所有聚類算法中最廣泛使用的。算法的主要思想是首先給定一個(gè)數(shù)據(jù)點(diǎn)集合和需要聚類的數(shù)目k，k由用戶事先確定。然后隨機(jī)初始化k個(gè)中心點(diǎn)，每個(gè)數(shù)據(jù)點(diǎn)通過計(jì)算該點(diǎn)與每個(gè)中心點(diǎn)之間的距離來(lái)進(jìn)行分類，根據(jù)最小距離，將該點(diǎn)分類到對(duì)應(yīng)中心點(diǎn)的簇中，算法進(jìn)行多次迭代后就可以分成k組點(diǎn)簇。k均值算法的優(yōu)勢(shì)在于它的速度非?？欤?yàn)橹恍枰?jì)算點(diǎn)和簇中心之間的距離。

8、(2)dbscan算法：

9、dbscan算法是一種基于密度的聚類算法，算法從一個(gè)未被訪問的任意數(shù)據(jù)點(diǎn)開始，這個(gè)點(diǎn)的領(lǐng)域用距離定義。如果在該領(lǐng)域內(nèi)有足夠數(shù)量的點(diǎn)，則聚類過程開始，并且當(dāng)前數(shù)據(jù)點(diǎn)成為新簇中的第一個(gè)點(diǎn)。否則，這個(gè)點(diǎn)將標(biāo)記為噪聲，這兩種情況下，該點(diǎn)都會(huì)被標(biāo)記為“已訪問”。對(duì)于新簇中的第一個(gè)點(diǎn)，它的距離鄰域內(nèi)的點(diǎn)也會(huì)成為同簇的一部分，這個(gè)過程使得領(lǐng)域內(nèi)的所有點(diǎn)都屬于同一個(gè)簇，然后對(duì)新添加到簇中的所有新點(diǎn)重復(fù)上述過程直到所有點(diǎn)已經(jīng)被訪問完畢，每個(gè)點(diǎn)都被標(biāo)記為屬于一個(gè)簇或是噪聲。dbscan算法的優(yōu)勢(shì)在于它不需要預(yù)先確定簇的數(shù)量且能夠?qū)惓Ｖ底R(shí)別為噪聲。另外，它能夠很好地找到任意大小和任意形狀的簇。

10、2.現(xiàn)有的跟蹤算法

11、現(xiàn)有的跟蹤算法主要是使用卡爾曼濾波，它是一個(gè)理論完備應(yīng)用廣泛的算法，是一個(gè)通用技術(shù)，廣泛應(yīng)用于信號(hào)處理、機(jī)器人、目標(biāo)跟蹤等各種領(lǐng)域。算法的主要思想是以一種迭代的方式動(dòng)態(tài)估計(jì)下一個(gè)時(shí)刻的狀態(tài)，同時(shí)又考慮到了歷史狀態(tài)和觀測(cè)數(shù)據(jù)。針對(duì)目標(biāo)跟蹤，卡爾曼濾波對(duì)目標(biāo)的運(yùn)動(dòng)模型進(jìn)行建模，包括勻速度模型和勻加速度模型，可以用于估計(jì)目標(biāo)在下一幀的位置。根據(jù)應(yīng)用場(chǎng)景，卡爾曼濾波還可以分成擴(kuò)展卡爾曼濾波和無(wú)跡卡爾曼濾波。擴(kuò)展卡爾曼濾波主要用于分析非線性系統(tǒng)，當(dāng)系統(tǒng)的動(dòng)態(tài)或觀測(cè)模型不是線性時(shí)，擴(kuò)展卡爾曼濾波會(huì)對(duì)這些模型進(jìn)行線性化處理，然后應(yīng)用卡爾曼濾波步驟。無(wú)跡卡爾曼濾波提供了一種更準(zhǔn)確地處理非線性問題的方法，即使用無(wú)跡變換來(lái)處理均值和協(xié)方差的非線性傳遞問題，相比擴(kuò)展卡爾曼，無(wú)跡卡爾曼通常能得到更好的結(jié)果，因?yàn)樗褂昧烁唠A的統(tǒng)計(jì)信息。現(xiàn)有技術(shù)缺點(diǎn)：

12、(1)k均值聚類最大的一個(gè)缺點(diǎn)就是必須預(yù)先確定簇的數(shù)量，然而由于毫米波雷達(dá)檢測(cè)到的目標(biāo)的數(shù)量是不確定的，一個(gè)目標(biāo)對(duì)應(yīng)的點(diǎn)云數(shù)量也是不確定的，所以很難去預(yù)先確定一個(gè)準(zhǔn)確的值。實(shí)際環(huán)境是復(fù)雜的，環(huán)境中可能有多種不同的目標(biāo)，如有行人、機(jī)動(dòng)車、非機(jī)動(dòng)車等。不同種類的目標(biāo)反射得到的點(diǎn)云數(shù)量和空間分布范圍是不同的，當(dāng)多目標(biāo)近鄰時(shí)由于k均值聚類的均值效應(yīng)和錯(cuò)誤收斂問題會(huì)導(dǎo)致聚類效果變差。除此之外，k均值聚類的另一個(gè)缺點(diǎn)是隨機(jī)選擇簇的中心點(diǎn)開始運(yùn)行，這導(dǎo)致對(duì)同樣的數(shù)據(jù)運(yùn)行多次該算法可能會(huì)產(chǎn)生不同的聚類結(jié)果。

13、(2)dbscan算法的主要缺點(diǎn)是當(dāng)數(shù)據(jù)點(diǎn)密度不均勻時(shí)，它的效果會(huì)比較差。這是因?yàn)楫?dāng)數(shù)據(jù)點(diǎn)密度變化時(shí)，用于識(shí)別鄰近點(diǎn)的距離閾值和最小點(diǎn)的閾值將隨著點(diǎn)密度而變化。在實(shí)際運(yùn)行環(huán)境中，數(shù)據(jù)點(diǎn)的密度會(huì)隨著環(huán)境和時(shí)間而變化，無(wú)法根據(jù)點(diǎn)簇的先驗(yàn)信息準(zhǔn)確估計(jì)上述閾值。此外，在多目標(biāo)場(chǎng)景下，因?yàn)槟繕?biāo)之間的距離太近導(dǎo)致點(diǎn)簇密度較大，從而使得不能區(qū)分距離較近的目標(biāo)。

14、(3)現(xiàn)有的多目標(biāo)跟蹤方法都有一個(gè)缺點(diǎn)，那就是把聚類和跟蹤割裂開來(lái)作為兩個(gè)單獨(dú)的模塊運(yùn)行，會(huì)對(duì)反饋的點(diǎn)云信息處理不及時(shí)導(dǎo)致對(duì)跟蹤的連續(xù)性產(chǎn)生影響。連續(xù)跟蹤是指僅在環(huán)境中的多個(gè)目標(biāo)位于雷達(dá)當(dāng)前視野范圍內(nèi)時(shí)對(duì)其進(jìn)行跟蹤，不連續(xù)跟蹤是連續(xù)跟蹤的擴(kuò)展，即在當(dāng)前視野范圍內(nèi)跟蹤目標(biāo)消失，假設(shè)目標(biāo)在雷達(dá)的未來(lái)視野中會(huì)重新出現(xiàn)，還可以將其與之前的軌跡相關(guān)聯(lián)。

15、另外，雷達(dá)信號(hào)和信息處理可以簡(jiǎn)單分為四個(gè)過程，對(duì)應(yīng)射頻前端、檢測(cè)、定位以及最后的分類。跟蹤算法位于定位過程中，它接收檢測(cè)得到的點(diǎn)云信息進(jìn)行處理后，向分類提供定位信息。雷達(dá)單目標(biāo)跟蹤的技術(shù)已經(jīng)比較成熟，常用的方法有卡爾曼濾波、常增益α～β濾波等。然而，隨著高分辨率雷達(dá)傳感器，特別是毫米波雷達(dá)的發(fā)展，檢測(cè)層能夠感知空間中一個(gè)或者多個(gè)目標(biāo)的很多個(gè)反射，從而得到更加豐富的點(diǎn)云信息，包括目標(biāo)的距離、方位、速度以及信噪比，通常一個(gè)目標(biāo)會(huì)由多個(gè)點(diǎn)云信息組成。這時(shí)候不能直接使用跟蹤算法，而是先要進(jìn)行點(diǎn)云聚類操作。

16、面對(duì)多維度的點(diǎn)云信息，針對(duì)單一距離維度的傳統(tǒng)聚類算法不再適用。如k均值聚類算法(k-means?clustering?algorithm)中僅使用點(diǎn)的位置信息計(jì)算到簇中心之間的距離，且必須先確定簇的數(shù)量，這對(duì)我們希望使用一個(gè)聚類算法來(lái)幫助我們找出有多少簇來(lái)說是矛盾的。dbscan算法(density-based?spatial?clustering?of?applications?withnoise)是一種基于密度的聚類算法，與k均值算法相比它雖然不需要事先確定簇的數(shù)量，但是當(dāng)數(shù)據(jù)簇密度不均勻時(shí)，識(shí)別相近點(diǎn)的距離閾值和最小點(diǎn)數(shù)閾值的設(shè)置將隨著簇的變化而變化，在處理高維度的點(diǎn)云數(shù)據(jù)時(shí)這種現(xiàn)象尤其明顯。

17、除此之外，傳統(tǒng)的跟蹤算法使用的卡爾曼濾波只適用于線性系統(tǒng)模型，考慮到實(shí)際場(chǎng)景中存在許多非線性系統(tǒng)模型，這時(shí)候需要使用擴(kuò)展卡爾曼濾波ekf(extendedkalman?filters)對(duì)模型進(jìn)行線性化處理，然后再應(yīng)用卡爾曼濾波步驟。

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

1、本發(fā)明提出一種基于群跟蹤算法的點(diǎn)云聚類和跟蹤方法，在跟蹤的同時(shí)也進(jìn)行聚類，將傳統(tǒng)聚類、跟蹤這兩個(gè)單獨(dú)模塊統(tǒng)一、融通起來(lái)，能夠?qū)Χ嗑S點(diǎn)云信息進(jìn)行處理，輸出穩(wěn)定、可靠的目標(biāo)運(yùn)動(dòng)軌跡。同時(shí)利用多維點(diǎn)云信息解決毫米波雷達(dá)多目標(biāo)跟蹤過程中由于環(huán)境等因素導(dǎo)致的不穩(wěn)定、錯(cuò)誤的跟蹤結(jié)果。

2、本發(fā)明是一種基于群跟蹤算法的點(diǎn)云聚類和跟蹤方法，該方法包括如下步驟：

3、步驟1.確定運(yùn)動(dòng)模型；

4、步驟2.擴(kuò)展卡爾曼濾波線性化：使用擴(kuò)展卡爾曼濾波進(jìn)行狀態(tài)向量到測(cè)量向量的線性化；

5、步驟3.預(yù)測(cè)步驟：利用卡爾曼濾波操作預(yù)測(cè)下一時(shí)刻軌跡的位置；

6、步驟4.關(guān)聯(lián)步驟：關(guān)聯(lián)步驟主要將雷達(dá)測(cè)量值與一條現(xiàn)有的跟蹤軌跡關(guān)聯(lián)起來(lái)，圍繞每個(gè)預(yù)測(cè)的質(zhì)心構(gòu)建一個(gè)測(cè)量邊界，這個(gè)邊界的形狀由門限函數(shù)決定；

7、步驟5.分配步驟：對(duì)于未與任何軌跡相關(guān)聯(lián)的測(cè)量向量，即在門限函數(shù)外的點(diǎn)，將初始化并分配一個(gè)新的群組跟蹤器；

8、步驟6.更新步驟：預(yù)測(cè)質(zhì)心g1,apr(n)與測(cè)量平均值之間的差異稱為更新量，更新量衡量了預(yù)測(cè)值與觀察到的測(cè)量值之間的差異程度；利用更新量，在更新步驟中會(huì)將軌跡中心更新到g1(n)，由此反復(fù)，即可實(shí)現(xiàn)對(duì)點(diǎn)云軌跡進(jìn)行聚類和跟蹤；

9、步驟7.維持步驟：通過增加目標(biāo)軌跡的三個(gè)不同狀態(tài)并通過狀態(tài)轉(zhuǎn)移參數(shù)維護(hù)狀態(tài)之間的轉(zhuǎn)換；

10、步驟8.定義數(shù)據(jù)接口：雷達(dá)輸出的點(diǎn)云數(shù)據(jù)與群跟蹤算法輸入之間需要定義一個(gè)數(shù)據(jù)接口，以便算法能夠及時(shí)地對(duì)輸出的點(diǎn)云數(shù)據(jù)進(jìn)行處理。

11、進(jìn)一步地說，步驟1中的運(yùn)動(dòng)模型為勻加速度模型，對(duì)物體運(yùn)動(dòng)來(lái)說，這一假設(shè)在短時(shí)間內(nèi)基本都是成立的。

12、進(jìn)一步地說，步驟1具體為：對(duì)于運(yùn)動(dòng)物體，我們認(rèn)為其速度變化是合理的，故采用勻加速度模型；勻加速度模型假設(shè)目標(biāo)做勻加速運(yùn)動(dòng)，我們使用卡爾曼濾波器來(lái)重新定義目標(biāo)的位置估計(jì)，卡爾曼濾波器在n時(shí)刻的狀態(tài)被定義為：

13、s(n)＝fs(n-1)+γw(n)

14、這里的狀態(tài)向量s(n)被定義在笛卡爾坐標(biāo)系下：

15、

16、其中分別是n時(shí)刻x方向和y方向的速度分量，分別是n時(shí)刻x方向和y方向的加速度分量。在實(shí)際運(yùn)動(dòng)過程中，目標(biāo)加速度可能存在隨機(jī)擾動(dòng)，這種隨機(jī)擾動(dòng)稱為過程噪聲，用符號(hào)表示為w(n)；γ為噪聲增益，定義為：

17、

18、w(n)是定義在直角坐標(biāo)系下的過程噪聲向量，其協(xié)方差矩陣q(n)＝e[γw2(n)γt]為一個(gè)6x6矩陣，用于二維勻加速度模型；t為雷達(dá)幀時(shí)間；在卡爾曼濾波術(shù)語(yǔ)中，過程噪聲協(xié)方差矩陣q(n)代表系統(tǒng)實(shí)際狀態(tài)與運(yùn)動(dòng)模型的偏差；

19、f是狀態(tài)轉(zhuǎn)移矩陣，用于指定運(yùn)動(dòng)模型的動(dòng)態(tài)特性，二維勻加速度模型狀態(tài)轉(zhuǎn)移矩陣為：

20、

21、將s(n)展開可以得到：

22、

23、

24、

25、

26、

27、

28、由展開結(jié)果可知當(dāng)前位置的估計(jì)是上一位置的結(jié)果再加上上一位置的速度乘以時(shí)間t得到的微小偏移量最后加上噪聲；勻加速度模型增加了加速度分量，導(dǎo)致了增加了新的距離分量和速度分量；

29、輸入向量u(n)表示雷達(dá)獲取的第n個(gè)點(diǎn)云的測(cè)量值，包括第n個(gè)點(diǎn)云的徑向距離、方位角、和徑向速度：

30、

31、卡爾曼濾波器的狀態(tài)量s(n)和輸入向量u(n)的相對(duì)關(guān)系用下式表示：

32、u(n)＝h(s(n))+v(n)

33、其中，h(·)表示測(cè)量矩陣，用于把直角坐標(biāo)系下的點(diǎn)轉(zhuǎn)換到極坐標(biāo)下，v(n)是定義在極坐標(biāo)下的測(cè)量噪聲向量，其協(xié)方差矩陣r(n)為一個(gè)3x3矩陣。測(cè)量矩陣具體表達(dá)式為：

34、

35、進(jìn)一步地說，步驟2具體為：在上述公式中，輸入向量u(n)和狀態(tài)向量s(n)之間具有非線性關(guān)系，即：

36、

37、

38、

39、因此，使用擴(kuò)展卡爾曼濾波器來(lái)簡(jiǎn)化u(n)和s(n)之間的關(guān)系；具體操作是只保留h(·)的一階泰勒展開式，即形如f(x)＝f(x0)+f′(x0)(x-x0)的式子：

40、u(n)≈h(sapr(n))+jh(sapr(n))[s(n)-sapr(n)]

41、其中，sapr(n)是狀態(tài)向量在時(shí)刻n基于時(shí)刻n-1的測(cè)量值得到的先驗(yàn)估計(jì)，是預(yù)測(cè)值，jh(s)為h(·)的雅可比矩陣：

42、

43、計(jì)算上述矩陣偏導(dǎo)數(shù)可得具體結(jié)果：

44、

45、由于測(cè)量矩陣h(·)中不含加速度變量，故雅可比矩陣中會(huì)出現(xiàn)兩列零向量。

46、進(jìn)一步地說，步驟3具體為：在時(shí)刻n-1處，兩個(gè)軌跡的質(zhì)心記為g1(n-1)和g2(n-1)，在預(yù)測(cè)步驟中，根據(jù)運(yùn)動(dòng)模型預(yù)測(cè)時(shí)刻n的軌跡質(zhì)心，預(yù)測(cè)的軌跡質(zhì)心標(biāo)記為g1,apr(n)、g2,apr(n)；

47、卡爾曼濾波預(yù)測(cè)步驟包括基于n-1時(shí)刻狀態(tài)向量s(n-1)和誤差協(xié)方差矩陣p(n-1)；誤差協(xié)方差矩陣定義為：p(n)＝cov(s(n)-sapr(n))；當(dāng)在n-1時(shí)刻得到輸入測(cè)量值時(shí)，先驗(yàn)狀態(tài)sapr(n)和先驗(yàn)誤差協(xié)方差估計(jì)papr(n)可以通過下式得到：

48、sapr(n)＝fs(n-1)

49、papr(n)＝fp(n-1)ft+q(n-1)

50、上述方程體現(xiàn)了卡爾曼濾波器的預(yù)測(cè)步驟。q(n)是上述步驟中提到的過程噪聲協(xié)方差矩陣；除此之外，在預(yù)測(cè)過程中我們還計(jì)算h(sapr(n))將預(yù)測(cè)的先驗(yàn)狀態(tài)從直角坐標(biāo)轉(zhuǎn)換到極坐標(biāo)下，該式子會(huì)在后續(xù)的更新步驟中被使用。

51、進(jìn)一步地說，步驟4具體為：在時(shí)刻n從檢測(cè)層獲得的一組測(cè)量值需要被關(guān)聯(lián)到唯一的一條軌跡當(dāng)中；那么就要計(jì)算測(cè)量值與預(yù)測(cè)軌跡質(zhì)心之間的距離度量，這個(gè)度量由打分函數(shù)決定，將測(cè)量值分配給距離最近的軌跡；最后對(duì)于每個(gè)軌跡，計(jì)算所有關(guān)聯(lián)測(cè)量值的平均值具體包括三個(gè)步驟，門限、打分和軌跡關(guān)聯(lián)分配。

52、進(jìn)一步地說，步驟5具體為：對(duì)于未與任何軌跡相關(guān)聯(lián)的測(cè)量向量，即在門限函數(shù)外的點(diǎn)，將初始化并分配一個(gè)新的群組跟蹤器；這是一個(gè)迭代過程，且只對(duì)剩下的點(diǎn)來(lái)進(jìn)行操作；

53、首先選擇一個(gè)任意點(diǎn)作為分配的起始點(diǎn)，并設(shè)置一個(gè)等于它的中心點(diǎn)；然后遍歷其它剩余點(diǎn)，判斷該剩余點(diǎn)與中心點(diǎn)是否滿足速度檢查和距離檢查；如果通過檢查，則剩余點(diǎn)將加入簇中并會(huì)重新計(jì)算中心點(diǎn)；完成后，會(huì)對(duì)新生成的簇進(jìn)行進(jìn)一步的檢查，包括簇內(nèi)的點(diǎn)云數(shù)量是否超過最少點(diǎn)云數(shù)量閾值、足夠強(qiáng)的信噪比以及質(zhì)心的最小動(dòng)態(tài)性；如果上述檢查都通過了，將會(huì)分配一個(gè)新的跟蹤對(duì)象，并使用關(guān)聯(lián)的點(diǎn)初始化離散矩陣；未通過的簇將會(huì)被忽略。

54、進(jìn)一步地說，步驟6具體為：更新過程如下所示，當(dāng)有足夠多和軌跡相關(guān)聯(lián)的動(dòng)態(tài)點(diǎn)時(shí)才執(zhí)行更新步驟；

55、(1)對(duì)于每一個(gè)軌跡，首先計(jì)算所有關(guān)聯(lián)測(cè)量值的平均值如果需要，這個(gè)平均值也可以是一個(gè)信噪比加權(quán)平均值；

56、(2)使用(1)得到的平均值計(jì)算跟新量，也叫測(cè)量誤差，h(sapr(n))已經(jīng)在步驟3中計(jì)算得到；

57、

58、(3)計(jì)算群組測(cè)量殘差協(xié)方差矩陣

59、

60、rc是質(zhì)心點(diǎn)的測(cè)量誤差協(xié)方差矩陣，它用于卡爾曼更新，定義如下：

61、

62、是群離散矩陣，rm是測(cè)量協(xié)方差矩陣，na是與軌跡相關(guān)聯(lián)的測(cè)量點(diǎn)數(shù)，是給定軌跡中點(diǎn)的估計(jì)數(shù)量；

63、

64、注意到當(dāng)是沒有出現(xiàn)錯(cuò)誤檢測(cè)的情況。當(dāng)所有點(diǎn)都被檢測(cè)到；當(dāng)只有單個(gè)關(guān)聯(lián)的測(cè)量值；

65、點(diǎn)的估計(jì)數(shù)量通過下式遞歸得到:

66、

67、群離散矩陣在時(shí)刻n的估計(jì)描述了此時(shí)目標(biāo)點(diǎn)簇的離散程度，對(duì)于測(cè)量向量離散矩陣d定義如下：

68、

69、上述矩陣中，分別從三方面計(jì)算目標(biāo)點(diǎn)簇中所有測(cè)量值與質(zhì)心間的協(xié)方差；n表示目標(biāo)點(diǎn)簇中的測(cè)量值總數(shù)；是給定一組測(cè)量值的質(zhì)心，則群離散矩陣dn與離散矩陣d按下式更新：

70、

71、為權(quán)重因子，描述了當(dāng)前幀計(jì)算得到的目標(biāo)離散度在目標(biāo)積累的多幀離散度中所占的權(quán)重，當(dāng)na與接近時(shí)，會(huì)給予d更高的權(quán)重；

72、(4)利用預(yù)測(cè)誤差協(xié)方差papr(n)和群組測(cè)量殘差協(xié)方差c(n)計(jì)算卡爾曼增益：

73、

74、(5)根據(jù)更新量y(n)和卡爾曼增益k(n)計(jì)算后驗(yàn)狀態(tài)向量s(n)：

75、s(n)＝sapr(n)+k(n)y(n)

76、(6)更新后驗(yàn)誤差協(xié)方差矩陣p(n):

77、p(n)＝papr(n)-k(n)jh(sapr(n))papr(n)。

78、進(jìn)一步地說，根據(jù)權(quán)利要求1所述的基于群跟蹤算法的點(diǎn)云聚類和跟蹤方法，其特征在于，步驟7中：跟蹤的目標(biāo)在以下三個(gè)狀態(tài)中轉(zhuǎn)換，三個(gè)狀態(tài)指檢測(cè)、活動(dòng)和空閑；

79、當(dāng)一個(gè)軌跡被分配時(shí)，跟蹤的目標(biāo)就處在檢測(cè)狀態(tài)；當(dāng)一個(gè)軌跡處在活動(dòng)狀態(tài)時(shí)，意味著它通過了所有檢查成為了一個(gè)活動(dòng)的目標(biāo)；當(dāng)一個(gè)軌跡處在空閑狀態(tài)時(shí)，意味著該目標(biāo)已經(jīng)消失。

80、狀態(tài)之間的轉(zhuǎn)換由狀態(tài)轉(zhuǎn)移參數(shù)來(lái)維護(hù)；

81、當(dāng)計(jì)數(shù)器的數(shù)值超過相應(yīng)閾值后，就會(huì)發(fā)生如下四種狀態(tài)轉(zhuǎn)換；空閑到檢測(cè)、檢測(cè)到活動(dòng)、檢測(cè)到空閑和活動(dòng)到空閑。

82、本發(fā)明具有的有益效果是：

83、1.本發(fā)明提出一種基于群跟蹤算法的點(diǎn)云聚類和跟蹤方法，在聚類的同時(shí)也進(jìn)行跟蹤。同時(shí)利用毫米波雷達(dá)的多維點(diǎn)云信息減少噪聲、目標(biāo)遮擋、多徑效應(yīng)等影響，輸出穩(wěn)定可靠的跟蹤軌跡。

84、2.本發(fā)明通過增加目標(biāo)軌跡的狀態(tài)并通過狀態(tài)轉(zhuǎn)移參數(shù)維護(hù)狀態(tài)之間的轉(zhuǎn)換，能夠提高連續(xù)跟蹤和不連續(xù)跟蹤的性能。

85、3.本發(fā)明可移植性強(qiáng)，只需要適配數(shù)據(jù)接口，適用于多種毫米波雷達(dá)輸出的點(diǎn)云數(shù)據(jù)。參數(shù)配置靈活，適用于短中長(zhǎng)距離段以及多種類型目標(biāo)的跟蹤。本方案利用了勻加速度模型，合理地描述了一般物體的運(yùn)動(dòng)狀態(tài)。根據(jù)勻加速度模型是非線性系統(tǒng)，不適用于卡爾曼濾波的情形，使用了擴(kuò)展卡爾曼濾波進(jìn)行狀態(tài)向量到測(cè)量向量的線性化，方便后續(xù)的卡爾曼濾波操作。預(yù)測(cè)步驟使用了經(jīng)典的卡爾曼濾波操作預(yù)測(cè)下一時(shí)刻軌跡的位置。

86、4.關(guān)聯(lián)中的門限函數(shù)即定義的群體誤差協(xié)方差矩陣考慮到了目標(biāo)的機(jī)動(dòng)性、群組的離散程度以及測(cè)量噪聲，限制了關(guān)聯(lián)的范圍。打分函數(shù)是針對(duì)一個(gè)測(cè)量點(diǎn)落在多個(gè)軌跡門限內(nèi)時(shí)分配到哪個(gè)軌跡的判別依據(jù)，測(cè)量點(diǎn)將被分配到得分最高的軌跡，使得關(guān)聯(lián)過程在點(diǎn)云密集的時(shí)候也能夠區(qū)分不同的目標(biāo)。

87、5.分配過程使用了點(diǎn)云的距離、速度、信噪比等多維點(diǎn)云信息，將未被關(guān)聯(lián)的點(diǎn)以合理的方式分配一個(gè)新的軌跡。只有當(dāng)有足夠多的動(dòng)態(tài)點(diǎn)與軌跡相關(guān)聯(lián)時(shí)才進(jìn)行更新操作，更新步驟中通過計(jì)算群體誤差協(xié)方差矩陣、群離散矩陣、軌跡中點(diǎn)的估計(jì)數(shù)量等參數(shù)計(jì)算卡爾曼增益，最后更新后驗(yàn)狀態(tài)向量以及后驗(yàn)誤差協(xié)方差矩陣。

88、6.維持過程中通過增加目標(biāo)軌跡的三個(gè)不同狀態(tài)并通過狀態(tài)轉(zhuǎn)移參數(shù)維護(hù)狀態(tài)之間的轉(zhuǎn)換，能夠提高連續(xù)跟蹤和不連續(xù)跟蹤的性能。