本發(fā)明涉及多傳感器信息融合技術(shù)領(lǐng)域,尤其涉及一種適用于雜波環(huán)境的多目標(biāo)跟蹤方法及跟蹤系統(tǒng)。
背景技術(shù):
貝葉斯濾波技術(shù)能夠提供一種強(qiáng)大的統(tǒng)計(jì)方法工具,用于協(xié)助解決雜波環(huán)境下以及測(cè)量數(shù)據(jù)具有不確定性情況下的多傳感器信息的融合與處理。在現(xiàn)有技術(shù)中,用于雜波環(huán)境的多目標(biāo)跟蹤有效方法主要有:基于高斯混合概率假設(shè)密度濾波器的目標(biāo)跟蹤方法和傳遞邊緣分布的測(cè)量驅(qū)動(dòng)目標(biāo)跟蹤方法。這兩種目標(biāo)跟蹤方法的主要問(wèn)題是計(jì)算量較大,并且在新目標(biāo)出現(xiàn)后的最初幾個(gè)時(shí)間步中不能提供其狀態(tài)估計(jì),如何有效提供新目標(biāo)在其出現(xiàn)后的最初幾個(gè)時(shí)間步的狀態(tài)估計(jì),同時(shí)減少計(jì)算量是多目標(biāo)貝葉斯濾波方法中需要探索和解決的一個(gè)關(guān)鍵技術(shù)問(wèn)題。
技術(shù)實(shí)現(xiàn)要素:
有鑒于此,本發(fā)明的目的在于提供一種適用于雜波環(huán)境的多目標(biāo)跟蹤方法及其系統(tǒng),旨在解決現(xiàn)有技術(shù)中在新目標(biāo)出現(xiàn)后的最初幾個(gè)時(shí)間步中不能提供其狀態(tài)估計(jì)以及計(jì)算量大的問(wèn)題。
本發(fā)明提出一種適用于雜波環(huán)境的多目標(biāo)跟蹤方法,主要包括:
預(yù)測(cè)步驟、根據(jù)前一時(shí)刻各個(gè)目標(biāo)的邊緣分布和存在概率,以及當(dāng)前時(shí)刻與前一時(shí)刻的時(shí)間差,預(yù)測(cè)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的邊緣分布和存在概率;
其中,以k-1表示前一時(shí)刻,k表示當(dāng)前時(shí)刻,tk-1表示前一時(shí)刻的時(shí)間,tk表示當(dāng)前時(shí)刻的時(shí)間,k-1時(shí)刻目標(biāo)i的邊緣分布和存在概率分別表示為N(xi,k-1;mi,k-1,Pi,k-1)和ρi,k-1,其中N表示高斯分布,i=1,2,…Nk-1,xi,k-1為k-1時(shí)刻目標(biāo)i的狀態(tài)向量,mi,k-1和Pi,k-1分別表示k-1時(shí)刻目標(biāo)i的狀態(tài)均值和協(xié)方差,Nk-1為前一時(shí)刻目標(biāo)的總數(shù);
由k-1時(shí)刻目標(biāo)i的邊緣分布N(xi,k-1;mi,k-1,Pi,k-1)和存在概率ρi,k-1,預(yù)測(cè)k-1時(shí)刻的目標(biāo)i在k時(shí)刻的邊緣分布和存在概率分別為N(xi,k;mi,k|k-1,Pi,k|k-1)和ρi,k|k-1,其中mi,k|k-1=Fi,k|k-1mi,k-1,Pi,k|k-1=Qi,k-1+Fi,k|k-1Pi,k-1(Fi,k|k-1)T,ρi,k|k-1=pS,k(Δtk)ρi,k-1,F(xiàn)i,k|k-1為狀態(tài)轉(zhuǎn)移矩陣,上標(biāo)T表示矩陣或向量的轉(zhuǎn)置,Δtk=tk-tk-1為k時(shí)刻與k-1時(shí)刻的時(shí)間差,Qi,k-1為k-1時(shí)刻目標(biāo)i的過(guò)程噪聲協(xié)方差矩陣,pS,k(Δtk)為目標(biāo)的幸存概率,且T為采樣周期,δ為給定的常數(shù),i=1,2,…Nk-1;
分類(lèi)步驟、根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的預(yù)測(cè)邊緣分布和預(yù)測(cè)存在概率,以及當(dāng)前時(shí)刻的測(cè)量集,確定出測(cè)量集中的每個(gè)測(cè)量是否源于前一時(shí)刻已存在的目標(biāo),并分別進(jìn)行歸類(lèi);
更新步驟、根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的預(yù)測(cè)邊緣分布和預(yù)測(cè)存在概率,以及當(dāng)前時(shí)刻源于已存在目標(biāo)的測(cè)量,利用貝葉斯規(guī)則確定前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的更新邊緣分布和更新存在概率;
裁減與提取步驟、根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的更新邊緣分布和更新存在概率,將存在概率小于第一閾值的目標(biāo)裁減掉,同時(shí)提取存在概率大于第二閾值的目標(biāo)的邊緣分布作為當(dāng)前時(shí)刻的輸出;
生成步驟、利用當(dāng)前時(shí)刻的其它測(cè)量和其前兩時(shí)刻的其它測(cè)量產(chǎn)生新目標(biāo),并利用最小二乘法估計(jì)新目標(biāo)在當(dāng)前時(shí)刻的狀態(tài)均值、協(xié)方差和邊緣分布;
補(bǔ)充步驟、提取新目標(biāo)在當(dāng)前時(shí)刻的邊緣分布對(duì)當(dāng)前時(shí)刻的輸出進(jìn)行補(bǔ)充,并提取新目標(biāo)在前兩個(gè)時(shí)刻的狀態(tài)估計(jì)分別對(duì)前兩個(gè)時(shí)刻的輸出進(jìn)行補(bǔ)充;
合并步驟、將在所述裁減與提取步驟中裁減后余下目標(biāo)的邊緣分布和存在概率,分別與在所述生成步驟中生成的新目標(biāo)在當(dāng)前時(shí)刻的邊緣分布和存在概率進(jìn)行合并,形成當(dāng)前時(shí)刻各個(gè)目標(biāo)的邊緣分布和存在概率,并作為下一次遞歸的輸入。
優(yōu)選的,所述分類(lèi)步驟具體包括:根據(jù)k-1時(shí)刻已存在的目標(biāo)i在k時(shí)刻的預(yù)測(cè)邊緣分布N(xi,k;mi,k|k-1,Pi,k|k-1)和預(yù)測(cè)存在概率ρi,k|k-1,以及k時(shí)刻的測(cè)量集合中的第j個(gè)測(cè)量yj,k,確定測(cè)量yj,k是否源于已存在目標(biāo),并分別進(jìn)行歸類(lèi);
其中,所述確定測(cè)量yj,k是否源于已存在目標(biāo),并分別進(jìn)行歸類(lèi)的步驟包括:
子步驟A、求取概率其中,Hk為觀測(cè)矩陣,Rk為觀測(cè)噪聲方差矩陣,pD,k為目標(biāo)的檢測(cè)概率,λc,k為雜波密度;
子步驟B、若將測(cè)量yj,k歸入其它測(cè)量類(lèi);若將測(cè)量yj,k歸入源于已存在目標(biāo)的測(cè)量,在測(cè)量集合中的每個(gè)測(cè)量處理后,測(cè)量集合中yk的測(cè)量被分為兩類(lèi),一類(lèi)是源于已存在目標(biāo)的測(cè)量,表示為另一類(lèi)是其它測(cè)量,表示為其中M1,k和M2,k分別源于已存在目標(biāo)測(cè)量的數(shù)目和其它測(cè)量的數(shù)目,且M1,k+M2,k=Mk。
優(yōu)選的,所述更新步驟具體包括:根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的預(yù)測(cè)邊緣分布N(xi,k;mi,k|k-1,Pi,k|k-1)和預(yù)測(cè)存在概率ρi,k|k-1,以及當(dāng)前時(shí)刻源于已存在目標(biāo)的測(cè)量集合利用貝葉斯規(guī)則確定當(dāng)前時(shí)刻各個(gè)已存在目標(biāo)的更新邊緣分布和存在概率;
其中,所述利用貝葉斯規(guī)則確定當(dāng)前時(shí)刻各個(gè)已存在目標(biāo)的更新邊緣分布和存在概率的步驟包括:
子步驟C、利用貝葉斯規(guī)則對(duì)測(cè)量處理,得到目標(biāo)i對(duì)應(yīng)于測(cè)量的存在概率均值向量和協(xié)方差矩陣其中在所有的M1,k個(gè)測(cè)量處理后,各個(gè)目標(biāo)對(duì)應(yīng)于各測(cè)量的更新邊緣分布和存在概率分別為和其中i=1,…,Nk-1,j=1,…,M1,k;
子步驟D、設(shè)其中則k時(shí)刻目標(biāo)i的更新邊緣分布取為相應(yīng)的存在概率取為其中i=1,…,Nk-1,當(dāng)q=M1,k+1時(shí)有
優(yōu)選的,所述生成步驟具體包括:利用k時(shí)刻的其它測(cè)量k-1時(shí)刻的其它測(cè)量和k-2時(shí)刻的其它測(cè)量產(chǎn)生新目標(biāo),并利用最小二乘法估計(jì)新目標(biāo)在當(dāng)前時(shí)刻的狀態(tài)均值、協(xié)方差和邊緣分布;
其中,所述利用k時(shí)刻的其它測(cè)量k-1時(shí)刻的其它測(cè)量和k-2時(shí)刻的其它測(cè)量產(chǎn)生新目標(biāo)的步驟包括:
子步驟E、從中取測(cè)量從中取測(cè)量從中取測(cè)量計(jì)算得到其中e=1,…,M2,k-2,f=1,…,M2,k-1,g=1,…,M2,k,||·||2表示向量的2范數(shù),|·|表示取絕對(duì)值,(·,·)表示兩向量的內(nèi)積;
子步驟F、判斷條件vmin≤vf,e≤vmax、vmin≤vg,f≤vmax、ag,f,e≤amax和cg,f,e≥cmin是否滿(mǎn)足,其中vmin、vmax、amax和cmin為4個(gè)給定的參數(shù),分別表示最小速度、最大速度、最大加速度和夾角余弦的最小值;若4個(gè)條件同時(shí)滿(mǎn)足,利用測(cè)量和測(cè)量由最小二乘法得到一個(gè)新目標(biāo)的在k時(shí)刻的狀態(tài)均值協(xié)方差和邊緣分布其中σw為測(cè)量噪聲的標(biāo)準(zhǔn)差;同時(shí),指定新目標(biāo)的存在概率取為新目標(biāo)在k-1時(shí)刻的狀態(tài)估計(jì)為其中新目標(biāo)在k-2時(shí)刻的狀態(tài)估計(jì)為其中
另一方面,本發(fā)明還提供一種適用于雜波環(huán)境的多目標(biāo)跟蹤系統(tǒng),所述系統(tǒng)包括:
預(yù)測(cè)模塊,用于根據(jù)前一時(shí)刻各個(gè)目標(biāo)的邊緣分布和存在概率,以及當(dāng)前時(shí)刻與前一時(shí)刻的時(shí)間差,預(yù)測(cè)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的邊緣分布和存在概率;
其中,以k-1表示前一時(shí)刻,k表示當(dāng)前時(shí)刻,tk-1表示前一時(shí)刻的時(shí)間,tk表示當(dāng)前時(shí)刻的時(shí)間,k-1時(shí)刻目標(biāo)i的邊緣分布和存在概率分別表示為N(xi,k-1;mi,k-1,Pi,k-1)和ρi,k-1,其中N表示高斯分布,i=1,2,…Nk-1,xi,k-1為k-1時(shí)刻目標(biāo)i的狀態(tài)向量,mi,k-1和Pi,k-1分別表示k-1時(shí)刻目標(biāo)i的狀態(tài)均值和協(xié)方差,Nk-1為前一時(shí)刻目標(biāo)的總數(shù);
由k-1時(shí)刻目標(biāo)i的邊緣分布N(xi,k-1;mi,k-1,Pi,k-1)和存在概率ρi,k-1,預(yù)測(cè)k-1時(shí)刻的目標(biāo)i在k時(shí)刻的邊緣分布和存在概率分別為N(xi,k;mi,k|k-1,Pi,k|k-1)和ρi,k|k-1,其中mi,k|k-1=Fi,k|k-1mi,k-1,Pi,k|k-1=Qi,k-1+Fi,k|k-1Pi,k-1(Fi,k|k-1)T,ρi,k|k-1=pS,k(Δtk)ρi,k-1,F(xiàn)i,k|k-1為狀態(tài)轉(zhuǎn)移矩陣,上標(biāo)T表示矩陣或向量的轉(zhuǎn)置,Δtk=tk-tk-1為k時(shí)刻與k-1時(shí)刻的時(shí)間差,Qi,k-1為k-1時(shí)刻目標(biāo)i的過(guò)程噪聲協(xié)方差矩陣,pS,k(Δtk)為目標(biāo)的幸存概率,且T為采樣周期,δ為給定的常數(shù),i=1,2,…Nk-1;
分類(lèi)模塊,用于根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的預(yù)測(cè)邊緣分布和預(yù)測(cè)存在概率,以及當(dāng)前時(shí)刻的測(cè)量集,確定出測(cè)量集中的每個(gè)測(cè)量是否源于前一時(shí)刻已存在的目標(biāo),并分別進(jìn)行歸類(lèi);
更新模塊,用于根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的預(yù)測(cè)邊緣分布和預(yù)測(cè)存在概率,以及當(dāng)前時(shí)刻源于已存在目標(biāo)的測(cè)量,利用貝葉斯規(guī)則確定前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的更新邊緣分布和更新存在概率;
裁減與提取模塊,用于根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的更新邊緣分布和更新存在概率,將存在概率小于第一閾值的目標(biāo)裁減掉,同時(shí)提取存在概率大于第二閾值的目標(biāo)的邊緣分布作為當(dāng)前時(shí)刻的輸出;
生成模塊,用于利用當(dāng)前時(shí)刻的其它測(cè)量和其前兩時(shí)刻的其它測(cè)量產(chǎn)生新目標(biāo),并利用最小二乘法估計(jì)新目標(biāo)在當(dāng)前時(shí)刻的狀態(tài)均值、協(xié)方差和邊緣分布;
補(bǔ)充模塊,用于提取新目標(biāo)在當(dāng)前時(shí)刻的邊緣分布對(duì)當(dāng)前時(shí)刻的輸出進(jìn)行補(bǔ)充,并提取新目標(biāo)在前兩個(gè)時(shí)刻的狀態(tài)估計(jì)分別對(duì)前兩個(gè)時(shí)刻的輸出進(jìn)行補(bǔ)充;
合并模塊,用于將在所述裁減與提取步驟中裁減后余下目標(biāo)的邊緣分布和存在概率,分別與在所述生成步驟中生成的新目標(biāo)在當(dāng)前時(shí)刻的邊緣分布和存在概率進(jìn)行合并,形成當(dāng)前時(shí)刻各個(gè)目標(biāo)的邊緣分布和存在概率,并作為下一次遞歸的輸入。
優(yōu)選的,所述分類(lèi)模塊具體用于:根據(jù)k-1時(shí)刻已存在的目標(biāo)i在k時(shí)刻的預(yù)測(cè)邊緣分布N(xi,k;mi,k|k-1,Pi,k|k-1)和預(yù)測(cè)存在概率ρi,k|k-1,以及k時(shí)刻的測(cè)量集合中的第j個(gè)測(cè)量yj,k,確定測(cè)量yj,k是否源于已存在目標(biāo),并分別進(jìn)行歸類(lèi);
其中,所述分類(lèi)模塊包括:
第一子模塊,用于求取概率其中,Hk為觀測(cè)矩陣,Rk為觀測(cè)噪聲方差矩陣,pD,k為目標(biāo)的檢測(cè)概率,λc,k為雜波密度;
第二子模塊,用于若將測(cè)量yj,k歸入其它測(cè)量類(lèi);若將測(cè)量yj,k歸入源于已存在目標(biāo)的測(cè)量,在測(cè)量集合中的每個(gè)測(cè)量處理后,測(cè)量集合中yk的測(cè)量被分為兩類(lèi),一類(lèi)是源于已存在目標(biāo)的測(cè)量,表示為另一類(lèi)是其它測(cè)量,表示為其中M1,k和M2,k分別源于已存在目標(biāo)測(cè)量的數(shù)目和其它測(cè)量的數(shù)目,且M1,k+M2,k=Mk。
優(yōu)選的,所述更新模塊具體用于:根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的預(yù)測(cè)邊緣分布N(xi,k;mi,k|k-1,Pi,k|k-1)和預(yù)測(cè)存在概率ρi,k|k-1,以及當(dāng)前時(shí)刻源于已存在目標(biāo)的測(cè)量集合利用貝葉斯規(guī)則確定當(dāng)前時(shí)刻各個(gè)已存在目標(biāo)的更新邊緣分布和存在概率;
其中,所述更新模塊包括:
第三子模塊,用于利用貝葉斯規(guī)則對(duì)測(cè)量處理,得到目標(biāo)i對(duì)應(yīng)于測(cè)量的存在概率均值向量和協(xié)方差矩陣其中在所有的M1,k個(gè)測(cè)量處理后,各個(gè)目標(biāo)對(duì)應(yīng)于各測(cè)量的更新邊緣分布和存在概率分別為和其中i=1,…,Nk-1,j=1,…,M1,k;
第四子模塊,用于設(shè)其中則k時(shí)刻目標(biāo)i的更新邊緣分布取為相應(yīng)的存在概率取為其中i=1,…,Nk-1,當(dāng)q=M1,k+1時(shí)有
優(yōu)選的,所述生成模塊具體用于:利用k時(shí)刻的其它測(cè)量k-1時(shí)刻的其它測(cè)量和k-2時(shí)刻的其它測(cè)量產(chǎn)生新目標(biāo),并利用最小二乘法估計(jì)新目標(biāo)在當(dāng)前時(shí)刻的狀態(tài)均值、協(xié)方差和邊緣分布;
其中,所述生成模塊包括:
第五子模塊,用于從中取測(cè)量從中取測(cè)量從中取測(cè)量計(jì)算得到其中e=1,…,M2,k-2,f=1,…,M2,k-1,g=1,…,M2,k,||·||2表示向量的2范數(shù),|·|表示取絕對(duì)值,(·,·)表示兩向量的內(nèi)積;
第六子模塊,用于判斷條件vmin≤vf,e≤vmax、vmin≤vg,f≤vmax、ag,f,e≤amax和cg,f,e≥cmin是否滿(mǎn)足,其中vmin、vmax、amax和cmin為4個(gè)給定的參數(shù),分別表示最小速度、最大速度、最大加速度和夾角余弦的最小值;若4個(gè)條件同時(shí)滿(mǎn)足,利用測(cè)量和測(cè)量由最小二乘法得到一個(gè)新目標(biāo)的在k時(shí)刻的狀態(tài)均值協(xié)方差和邊緣分布其中σw為測(cè)量噪聲的標(biāo)準(zhǔn)差;同時(shí),指定新目標(biāo)的存在概率取為新目標(biāo)在k-1時(shí)刻的狀態(tài)估計(jì)為其中新目標(biāo)在k-2時(shí)刻的狀態(tài)估計(jì)為其中
本發(fā)明提供的技術(shù)方案,通過(guò)預(yù)測(cè)、分類(lèi)、更新、裁減與提取、生成、補(bǔ)充、合并這些步驟,利用最小二乘法估計(jì)新目標(biāo)在其出現(xiàn)后的最初3個(gè)時(shí)間步的狀態(tài)估計(jì),有效地解決了現(xiàn)有方法在新目標(biāo)出現(xiàn)后的前幾個(gè)時(shí)間步不能提供新目標(biāo)狀態(tài)估計(jì)的問(wèn)題,具有處理速度快的特點(diǎn),且其計(jì)算量明顯小于現(xiàn)有方法,具有很強(qiáng)的實(shí)用性。
附圖說(shuō)明
圖1為本發(fā)明一實(shí)施方式中適用于雜波環(huán)境的多目標(biāo)跟蹤方法流程圖;
圖2為本發(fā)明一實(shí)施方式中適用于雜波環(huán)境的多目標(biāo)跟蹤系統(tǒng)的內(nèi)部結(jié)構(gòu)示意圖;
圖3為本發(fā)明一實(shí)施方式中利用本發(fā)明實(shí)施例提供的傳感器在50個(gè)掃描周期的測(cè)量數(shù)據(jù)圖;
圖4為本發(fā)明一實(shí)施方式中根據(jù)本發(fā)明適用于雜波環(huán)境下的多目標(biāo)跟蹤方法與高斯混合概率假設(shè)密度濾波方法經(jīng)過(guò)一次實(shí)驗(yàn)得到的OSPA距離示意圖;
圖5為本發(fā)明一實(shí)施方式中根據(jù)本發(fā)明用于適用于雜波環(huán)境下的多目標(biāo)跟蹤方法與高斯混合概率假設(shè)密度濾波方法在經(jīng)過(guò)100次實(shí)驗(yàn)得到的平均OSPA距離示意圖。
具體實(shí)施方式
為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白,以下結(jié)合附圖及實(shí)施例,對(duì)本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說(shuō)明。應(yīng)當(dāng)理解,此處所描述的具體實(shí)施例僅僅用以解釋本發(fā)明,并不用于限定本發(fā)明。
以下將對(duì)本發(fā)明所提供的一種適用于雜波環(huán)境的多目標(biāo)跟蹤方法進(jìn)行詳細(xì)說(shuō)明。
請(qǐng)參閱圖1,為本發(fā)明一實(shí)施方式中適用于雜波環(huán)境的多目標(biāo)跟蹤方法流程圖。
在步驟S1中,預(yù)測(cè)步驟、根據(jù)前一時(shí)刻各個(gè)目標(biāo)的邊緣分布和存在概率,以及當(dāng)前時(shí)刻與前一時(shí)刻的時(shí)間差,預(yù)測(cè)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的邊緣分布和存在概率。
在本實(shí)施方式中,所述預(yù)測(cè)步驟S1具體包括:
以k-1表示前一時(shí)刻,k表示當(dāng)前時(shí)刻,tk-1表示前一時(shí)刻的時(shí)間,tk表示當(dāng)前時(shí)刻的時(shí)間,k-1時(shí)刻目標(biāo)i的邊緣分布和存在概率分別表示為N(xi,k-1;mi,k-1,Pi,k-1)和ρi,k-1,其中N表示高斯分布,i=1,2,…Nk-1,xi,k-1為k-1時(shí)刻目標(biāo)i的狀態(tài)向量,mi,k-1和Pi,k-1分別表示k-1時(shí)刻目標(biāo)i的狀態(tài)均值和協(xié)方差,Nk-1為前一時(shí)刻目標(biāo)的總數(shù);
由k-1時(shí)刻目標(biāo)i的邊緣分布N(xi,k-1;mi,k-1,Pi,k-1)和存在概率ρi,k-1,預(yù)測(cè)k-1時(shí)刻的目標(biāo)i在k時(shí)刻的邊緣分布和存在概率分別為N(xi,k;mi,k|k-1,Pi,k|k-1)和ρi,k|k-1,其中mi,k|k-1=Fi,k|k-1mi,k-1,Pi,k|k-1=Qi,k-1+Fi,k|k-1Pi,k-1(Fi,k|k-1)T,ρi,k|k-1=pS,k(Δtk)ρi,k-1,F(xiàn)i,k|k-1為狀態(tài)轉(zhuǎn)移矩陣,上標(biāo)T表示矩陣或向量的轉(zhuǎn)置,Δtk=tk-tk-1為k時(shí)刻與k-1時(shí)刻的時(shí)間差,Qi,k-1為k-1時(shí)刻目標(biāo)i的過(guò)程噪聲協(xié)方差矩陣,pS,k(Δtk)為目標(biāo)的幸存概率,且T為采樣周期,δ為給定的常數(shù),i=1,2,…Nk-1。
在步驟S2中,分類(lèi)步驟、根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的預(yù)測(cè)邊緣分布和預(yù)測(cè)存在概率,以及當(dāng)前時(shí)刻的測(cè)量集,確定出測(cè)量集中的每個(gè)測(cè)量是否源于前一時(shí)刻已存在的目標(biāo),并分別進(jìn)行歸類(lèi)。
在本實(shí)施方式中,所述分類(lèi)步驟S2具體包括:根據(jù)k-1時(shí)刻已存在的目標(biāo)i在k時(shí)刻的預(yù)測(cè)邊緣分布N(xi,k;mi,k|k-1,Pi,k|k-1)和預(yù)測(cè)存在概率ρi,k|k-1,以及k時(shí)刻的測(cè)量集合中的第j個(gè)測(cè)量yj,k,確定測(cè)量yj,k是否源于已存在目標(biāo),并分別進(jìn)行歸類(lèi);
其中,所述確定測(cè)量yj,k是否源于已存在目標(biāo),并分別進(jìn)行歸類(lèi)的步驟包括:
子步驟A、求取概率其中,Hk為觀測(cè)矩陣,Rk為觀測(cè)噪聲方差矩陣,pD,k為目標(biāo)的檢測(cè)概率,λc,k為雜波密度;
子步驟B、若將測(cè)量yj,k歸入其它測(cè)量類(lèi);若將測(cè)量yj,k歸入源于已存在目標(biāo)的測(cè)量,在測(cè)量集合中的每個(gè)測(cè)量處理后,測(cè)量集合中yk的測(cè)量被分為兩類(lèi),一類(lèi)是源于已存在目標(biāo)的測(cè)量,表示為另一類(lèi)是其它測(cè)量,表示為其中M1,k和M2,k分別源于已存在目標(biāo)測(cè)量的數(shù)目和其它測(cè)量的數(shù)目,且M1,k+M2,k=Mk。
在步驟S3中,更新步驟、根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的預(yù)測(cè)邊緣分布和預(yù)測(cè)存在概率,以及當(dāng)前時(shí)刻源于已存在目標(biāo)的測(cè)量,利用貝葉斯規(guī)則確定前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的更新邊緣分布和更新存在概率。
在本實(shí)施方式中,所述更新步驟S3具體包括:根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的預(yù)測(cè)邊緣分布N(xi,k;mi,k|k-1,Pi,k|k-1)和預(yù)測(cè)存在概率ρi,k|k-1,以及當(dāng)前時(shí)刻源于已存在目標(biāo)的測(cè)量集合利用貝葉斯規(guī)則確定當(dāng)前時(shí)刻各個(gè)已存在目標(biāo)的更新邊緣分布和存在概率;
其中,所述利用貝葉斯規(guī)則確定當(dāng)前時(shí)刻各個(gè)已存在目標(biāo)的更新邊緣分布和存在概率的步驟包括:
子步驟C、利用貝葉斯規(guī)則對(duì)測(cè)量處理,得到目標(biāo)i對(duì)應(yīng)于測(cè)量的存在概率均值向量和協(xié)方差矩陣其中在所有的M1,k個(gè)測(cè)量處理后,各個(gè)目標(biāo)對(duì)應(yīng)于各測(cè)量的更新邊緣分布和存在概率分別為和其中i=1,…,Nk-1,j=1,…,M1,k;
子步驟D、設(shè)其中則k時(shí)刻目標(biāo)i的更新邊緣分布取為相應(yīng)的存在概率取為其中i=1,…,Nk-1,當(dāng)q=M1,k+1時(shí)有
在步驟S4中,裁減與提取步驟、根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的更新邊緣分布和更新存在概率,將存在概率小于第一閾值的目標(biāo)裁減掉,同時(shí)提取存在概率大于第二閾值的目標(biāo)的邊緣分布作為當(dāng)前時(shí)刻的輸出。
在步驟S5中,生成步驟、利用當(dāng)前時(shí)刻的其它測(cè)量和其前兩時(shí)刻的其它測(cè)量產(chǎn)生新目標(biāo),并利用最小二乘法估計(jì)新目標(biāo)在當(dāng)前時(shí)刻的狀態(tài)均值、協(xié)方差和邊緣分布。
在本實(shí)施方式中,所述生成步驟S5具體包括:利用k時(shí)刻的其它測(cè)量k-1時(shí)刻的其它測(cè)量和k-2時(shí)刻的其它測(cè)量產(chǎn)生新目標(biāo),并利用最小二乘法估計(jì)新目標(biāo)在當(dāng)前時(shí)刻的狀態(tài)均值、協(xié)方差和邊緣分布;
其中,所述利用k時(shí)刻的其它測(cè)量k-1時(shí)刻的其它測(cè)量和k-2時(shí)刻的其它測(cè)量產(chǎn)生新目標(biāo)的步驟包括:
子步驟E、從中取測(cè)量從中取測(cè)量從中取測(cè)量計(jì)算得到其中e=1,…,M2,k-2,f=1,…,M2,k-1,g=1,…,M2,k,||·||2表示向量的2范數(shù),|·|表示取絕對(duì)值,(·,·)表示兩向量的內(nèi)積;
子步驟F、判斷條件vmin≤vf,e≤vmax、vmin≤vg,f≤vmax、ag,f,e≤amax和cg,f,e≥cmin是否滿(mǎn)足,其中vmin、vmax、amax和cmin為4個(gè)給定的參數(shù),分別表示最小速度、最大速度、最大加速度和夾角余弦的最小值;若4個(gè)條件同時(shí)滿(mǎn)足,利用測(cè)量和測(cè)量由最小二乘法得到一個(gè)新目標(biāo)的在k時(shí)刻的狀態(tài)均值協(xié)方差和邊緣分布其中σw為測(cè)量噪聲的標(biāo)準(zhǔn)差;同時(shí),指定新目標(biāo)的存在概率取為新目標(biāo)在k-1時(shí)刻的狀態(tài)估計(jì)為其中新目標(biāo)在k-2時(shí)刻的狀態(tài)估計(jì)為其中
在步驟S6中,補(bǔ)充步驟、提取新目標(biāo)在當(dāng)前時(shí)刻的邊緣分布對(duì)當(dāng)前時(shí)刻的輸出進(jìn)行補(bǔ)充,并提取新目標(biāo)在前兩個(gè)時(shí)刻的狀態(tài)估計(jì)分別對(duì)前兩個(gè)時(shí)刻的輸出進(jìn)行補(bǔ)充。
在步驟S7中,合并步驟、將在所述裁減與提取步驟中裁減后余下目標(biāo)的邊緣分布和存在概率,分別與在所述生成步驟中生成的新目標(biāo)在當(dāng)前時(shí)刻的邊緣分布和存在概率進(jìn)行合并,形成當(dāng)前時(shí)刻各個(gè)目標(biāo)的邊緣分布和存在概率,并作為下一次遞歸的輸入。
本發(fā)明提供的一種適用于雜波環(huán)境的多目標(biāo)跟蹤方法,通過(guò)預(yù)測(cè)、分類(lèi)、更新、裁減與提取、生成、補(bǔ)充、合并這些步驟,利用最小二乘法估計(jì)新目標(biāo)在其出現(xiàn)后的最初3個(gè)時(shí)間步的狀態(tài)估計(jì),有效地解決了現(xiàn)有方法在新目標(biāo)出現(xiàn)后的前幾個(gè)時(shí)間步不能提供新目標(biāo)狀態(tài)估計(jì)的問(wèn)題,具有處理速度快的特點(diǎn),且其計(jì)算量明顯小于現(xiàn)有方法,具有很強(qiáng)的實(shí)用性。
請(qǐng)參閱圖2,所示為本發(fā)明一實(shí)施方式中適用于雜波環(huán)境的多目標(biāo)跟蹤系統(tǒng)10的結(jié)構(gòu)示意圖。
在本實(shí)施方式中,適用于雜波環(huán)境的多目標(biāo)跟蹤系統(tǒng)10,主要包括預(yù)測(cè)模塊11、分類(lèi)模塊12、更新模塊13、裁減與提取模塊14、生成模塊15、補(bǔ)充模塊16以及合并模塊17。
預(yù)測(cè)模塊11,用于根據(jù)前一時(shí)刻各個(gè)目標(biāo)的邊緣分布和存在概率,以及當(dāng)前時(shí)刻與前一時(shí)刻的時(shí)間差,預(yù)測(cè)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的邊緣分布和存在概率。
在本實(shí)施方式中,所述預(yù)測(cè)模塊11具體用于:
以k-1表示前一時(shí)刻,k表示當(dāng)前時(shí)刻,tk-1表示前一時(shí)刻的時(shí)間,tk表示當(dāng)前時(shí)刻的時(shí)間,k-1時(shí)刻目標(biāo)i的邊緣分布和存在概率分別表示為N(xi,k-1;mi,k-1,Pi,k-1)和ρi,k-1,其中N表示高斯分布,i=1,2,…Nk-1,xi,k-1為k-1時(shí)刻目標(biāo)i的狀態(tài)向量,mi,k-1和Pi,k-1分別表示k-1時(shí)刻目標(biāo)i的狀態(tài)均值和協(xié)方差,Nk-1為前一時(shí)刻目標(biāo)的總數(shù);
由k-1時(shí)刻目標(biāo)i的邊緣分布N(xi,k-1;mi,k-1,Pi,k-1)和存在概率ρi,k-1,預(yù)測(cè)k-1時(shí)刻的目標(biāo)i在k時(shí)刻的邊緣分布和存在概率分別為N(xi,k;mi,k|k-1,Pi,k|k-1)和ρi,k|k-1,其中mi,k|k-1=Fi,k|k-1mi,k-1,Pi,k|k-1=Qi,k-1+Fi,k|k-1Pi,k-1(Fi,k|k-1)T,ρi,k|k-1=pS,k(Δtk)ρi,k-1,F(xiàn)i,k|k-1為狀態(tài)轉(zhuǎn)移矩陣,上標(biāo)T表示矩陣或向量的轉(zhuǎn)置,Δtk=tk-tk-1為k時(shí)刻與k-1時(shí)刻的時(shí)間差,Qi,k-1為k-1時(shí)刻目標(biāo)i的過(guò)程噪聲協(xié)方差矩陣,pS,k(Δtk)為目標(biāo)的幸存概率,且T為采樣周期,δ為給定的常數(shù),i=1,2,…Nk-1。
分類(lèi)模塊12,用于根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的預(yù)測(cè)邊緣分布和預(yù)測(cè)存在概率,以及當(dāng)前時(shí)刻的測(cè)量集,確定出測(cè)量集中的每個(gè)測(cè)量是否源于前一時(shí)刻已存在的目標(biāo),并分別進(jìn)行歸類(lèi)。
在本實(shí)施方式中,所述分類(lèi)模塊12具體用于:根據(jù)k-1時(shí)刻已存在的目標(biāo)i在k時(shí)刻的預(yù)測(cè)邊緣分布N(xi,k;mi,k|k-1,Pi,k|k-1)和預(yù)測(cè)存在概率ρi,k|k-1,以及k時(shí)刻的測(cè)量集合中的第j個(gè)測(cè)量yj,k,確定測(cè)量yj,k是否源于已存在目標(biāo),并分別進(jìn)行歸類(lèi);
其中,所述分類(lèi)模塊12包括:第一子模塊和第二子模塊。
第一子模塊,用于求取概率其中,Hk為觀測(cè)矩陣,Rk為觀測(cè)噪聲方差矩陣,pD,k為目標(biāo)的檢測(cè)概率,λc,k為雜波密度。
第二子模塊,用于若將測(cè)量yj,k歸入其它測(cè)量類(lèi);若將測(cè)量yj,k歸入源于已存在目標(biāo)的測(cè)量,在測(cè)量集合中的每個(gè)測(cè)量處理后,測(cè)量集合中yk的測(cè)量被分為兩類(lèi),一類(lèi)是源于已存在目標(biāo)的測(cè)量,表示為另一類(lèi)是其它測(cè)量,表示為其中M1,k和M2,k分別源于已存在目標(biāo)測(cè)量的數(shù)目和其它測(cè)量的數(shù)目,且M1,k+M2,k=Mk。
更新模塊13,用于根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的預(yù)測(cè)邊緣分布和預(yù)測(cè)存在概率,以及當(dāng)前時(shí)刻源于已存在目標(biāo)的測(cè)量,利用貝葉斯規(guī)則確定前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的更新邊緣分布和更新存在概率。
在本實(shí)施方式中,所述更新模塊13具體用于:根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的預(yù)測(cè)邊緣分布N(xi,k;mi,k|k-1,Pi,k|k-1)和預(yù)測(cè)存在概率ρi,k|k-1,以及當(dāng)前時(shí)刻源于已存在目標(biāo)的測(cè)量集合利用貝葉斯規(guī)則確定當(dāng)前時(shí)刻各個(gè)已存在目標(biāo)的更新邊緣分布和存在概率;
其中,所述更新模塊13包括:第三子模塊和第四子模塊。
第三子模塊,用于利用貝葉斯規(guī)則對(duì)測(cè)量處理,得到目標(biāo)i對(duì)應(yīng)于測(cè)量的存在概率均值向量和協(xié)方差矩陣其中在所有的M1,k個(gè)測(cè)量處理后,各個(gè)目標(biāo)對(duì)應(yīng)于各測(cè)量的更新邊緣分布和存在概率分別為和其中i=1,…,Nk-1,j=1,…,M1,k。
第四子模塊,用于設(shè)其中則k時(shí)刻目標(biāo)i的更新邊緣分布取為相應(yīng)的存在概率取為其中i=1,…,Nk-1,當(dāng)q=M1,k+1時(shí)有
裁減與提取模塊14,用于根據(jù)前一時(shí)刻已存在的各個(gè)目標(biāo)在當(dāng)前時(shí)刻的更新邊緣分布和更新存在概率,將存在概率小于第一閾值的目標(biāo)裁減掉,同時(shí)提取存在概率大于第二閾值的目標(biāo)的邊緣分布作為當(dāng)前時(shí)刻的輸出。
生成模塊15,用于利用當(dāng)前時(shí)刻的其它測(cè)量和其前兩時(shí)刻的其它測(cè)量產(chǎn)生新目標(biāo),并利用最小二乘法估計(jì)新目標(biāo)在當(dāng)前時(shí)刻的狀態(tài)均值、協(xié)方差和邊緣分布。
在本實(shí)施方式中,所述生成模塊15具體用于:利用k時(shí)刻的其它測(cè)量k-1時(shí)刻的其它測(cè)量和k-2時(shí)刻的其它測(cè)量產(chǎn)生新目標(biāo),并利用最小二乘法估計(jì)新目標(biāo)在當(dāng)前時(shí)刻的狀態(tài)均值、協(xié)方差和邊緣分布;
其中,所述生成模塊15包括:第五子模塊和第六子模塊。
第五子模塊,用于從中取測(cè)量從中取測(cè)量從中取測(cè)量計(jì)算得到其中e=1,…,M2,k-2,f=1,…,M2,k-1,g=1,…,M2,k,||·||2表示向量的2范數(shù),|·|表示取絕對(duì)值,(·,·)表示兩向量的內(nèi)積。
第六子模塊,用于判斷條件vmin≤vf,e≤vmax、vmin≤vg,f≤vmax、ag,f,e≤amax和cg,f,e≥cmin是否滿(mǎn)足,其中vmin、vmax、amax和cmin為4個(gè)給定的參數(shù),分別表示最小速度、最大速度、最大加速度和夾角余弦的最小值;若4個(gè)條件同時(shí)滿(mǎn)足,利用測(cè)量和測(cè)量由最小二乘法得到一個(gè)新目標(biāo)的在k時(shí)刻的狀態(tài)均值協(xié)方差和邊緣分布其中σw為測(cè)量噪聲的標(biāo)準(zhǔn)差;同時(shí),指定新目標(biāo)的存在概率取為新目標(biāo)在k-1時(shí)刻的狀態(tài)估計(jì)為其中新目標(biāo)在k-2時(shí)刻的狀態(tài)估計(jì)為其中
補(bǔ)充模塊16,用于提取新目標(biāo)在當(dāng)前時(shí)刻的邊緣分布對(duì)當(dāng)前時(shí)刻的輸出進(jìn)行補(bǔ)充,并提取新目標(biāo)在前兩個(gè)時(shí)刻的狀態(tài)估計(jì)分別對(duì)前兩個(gè)時(shí)刻的輸出進(jìn)行補(bǔ)充。
合并模塊17,用于將在所述裁減與提取步驟中裁減后余下目標(biāo)的邊緣分布和存在概率,分別與在所述生成步驟中生成的新目標(biāo)在當(dāng)前時(shí)刻的邊緣分布和存在概率進(jìn)行合并,形成當(dāng)前時(shí)刻各個(gè)目標(biāo)的邊緣分布和存在概率,并作為下一次遞歸的輸入。
本發(fā)明提供的一種適用于雜波環(huán)境的多目標(biāo)跟蹤系統(tǒng)10,通過(guò)預(yù)測(cè)模塊11、分類(lèi)模塊12、更新模塊13、裁減與提取模塊14、生成模塊15、補(bǔ)充模塊16以及合并模塊17這些模塊,利用最小二乘法估計(jì)新目標(biāo)在其出現(xiàn)后的最初3個(gè)時(shí)間步的狀態(tài)估計(jì),有效地解決了現(xiàn)有方法在新目標(biāo)出現(xiàn)后的前幾個(gè)時(shí)間步不能提供新目標(biāo)狀態(tài)估計(jì)的問(wèn)題,具有處理速度快的特點(diǎn),且其計(jì)算量明顯小于現(xiàn)有方法,具有很強(qiáng)的實(shí)用性。
以下通過(guò)將本發(fā)明的適用于雜波環(huán)境的多目標(biāo)跟蹤系統(tǒng)10與現(xiàn)有的高斯混合概率假設(shè)密度濾波器進(jìn)行對(duì)比來(lái)說(shuō)明本發(fā)明的有益效果。
作為本發(fā)明的一個(gè)實(shí)例,考慮二維空間[-1000m,1000m]×[-1000m,1000m]中運(yùn)動(dòng)的目標(biāo),目標(biāo)的狀態(tài)由位置和速度構(gòu)成,表示為其中x和y分別表示位置分量,和分別表示速度分量,上標(biāo)T表示向量的轉(zhuǎn)置;過(guò)程噪聲方差矩陣為其中,Δtk=tk-tk-1為當(dāng)前時(shí)刻與前一時(shí)刻的時(shí)間差,σv為過(guò)程噪聲標(biāo)準(zhǔn)差;觀測(cè)噪聲方差矩陣σw為觀測(cè)噪聲的標(biāo)準(zhǔn)差;參數(shù)δ取為δ=2.5,最小速度vmin、最大速度vmax、最大加速度amax和夾角余弦的最小值cmin分別取為vmin=30ms-1、vmax=80ms-1、amax=10ms-2和cmin=0.94。
為了產(chǎn)生仿真數(shù)據(jù),取幸存概率pS,k=1.0、檢測(cè)概率pD,k=0.95、雜波密度λc,k=2.5×10-6m-2、過(guò)程噪聲的標(biāo)準(zhǔn)差σv=1ms-2、觀測(cè)噪聲的標(biāo)準(zhǔn)差σw=2m和傳感器的掃描周期T=1s。一次實(shí)驗(yàn)中傳感器在50個(gè)掃描周期的仿真觀測(cè)數(shù)據(jù)如圖3所示。
為了處理仿真數(shù)據(jù),將本發(fā)明與高斯混合概率假設(shè)密度濾波器的相關(guān)參數(shù)設(shè)置為pS,k=1.0、pD,k=0.95、λc,k=2.5×10-6m-2、σw=2m、σv=1ms-2、第一閾值為10-3、第二閾值為0.5、高斯混合概率假設(shè)密度濾波器新目標(biāo)的權(quán)重為wγ=0.1,新目標(biāo)的協(xié)方差為圖4為用現(xiàn)有的高斯混合概率假設(shè)密度濾波器與本發(fā)明對(duì)圖3中的數(shù)據(jù)處理得到的最優(yōu)亞模式分配(Optimal Subpattern Assignment,OSPA)距離。圖5為用現(xiàn)有的高斯混合概率假設(shè)密度濾波器與本發(fā)明分別進(jìn)行50次Monte Carlo實(shí)驗(yàn)得到的平均OSPA距離。
現(xiàn)有的高斯混合概率假設(shè)密度濾波器與本發(fā)明的實(shí)驗(yàn)結(jié)果比較表明,本發(fā)明的方法可以獲得更為精確和可靠的目標(biāo)狀態(tài)估計(jì)、其OSPA距離比現(xiàn)有的這種方法得到的OSPA距離要小,特別在多目標(biāo)出現(xiàn)的最初時(shí)刻(t=1s至t=16s),OSPA距離減小更為明顯。
表1
表1顯示了現(xiàn)有的高斯混合概率假設(shè)密度濾波器與本發(fā)明在50次實(shí)驗(yàn)中得到的一次實(shí)驗(yàn)的平均執(zhí)行時(shí)間,結(jié)果表明本發(fā)明的平均執(zhí)行時(shí)間明顯小于現(xiàn)有的高斯混合概率假設(shè)密度濾波器。
本發(fā)明提供的技術(shù)方案,通過(guò)預(yù)測(cè)、分類(lèi)、更新、裁減與提取、生成、補(bǔ)充、合并這些步驟,利用最小二乘法估計(jì)新目標(biāo)在其出現(xiàn)后的最初3個(gè)時(shí)間步的狀態(tài)估計(jì),有效地解決了現(xiàn)有方法在新目標(biāo)出現(xiàn)后的前幾個(gè)時(shí)間步不能提供新目標(biāo)狀態(tài)估計(jì)的問(wèn)題,具有處理速度快的特點(diǎn),且其計(jì)算量明顯小于現(xiàn)有方法,具有很強(qiáng)的實(shí)用性。
值得注意的是,上述實(shí)施例中,所包括的各個(gè)單元只是按照功能邏輯進(jìn)行劃分的,但并不局限于上述的劃分,只要能夠?qū)崿F(xiàn)相應(yīng)的功能即可;另外,各功能單元的具體名稱(chēng)也只是為了便于相互區(qū)分,并不用于限制本發(fā)明的保護(hù)范圍。
另外,本領(lǐng)域普通技術(shù)人員可以理解實(shí)現(xiàn)上述各實(shí)施例方法中的全部或部分步驟是可以通過(guò)程序來(lái)指令相關(guān)的硬件來(lái)完成,相應(yīng)的程序可以存儲(chǔ)于一計(jì)算機(jī)可讀取存儲(chǔ)介質(zhì)中,所述的存儲(chǔ)介質(zhì),如ROM/RAM、磁盤(pán)或光盤(pán)等。
以上所述僅為本發(fā)明的較佳實(shí)施例而已,并不用以限制本發(fā)明,凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等,均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。