一種約束條件下的機(jī)動(dòng)目標(biāo)跟蹤方法
【專利摘要】本發(fā)明公開了一種約束條件下的機(jī)動(dòng)目標(biāo)跟蹤方法，所述方法包括以下步驟：A、獲得機(jī)動(dòng)目標(biāo)的位置量測(cè)信息；B、將所述位置量測(cè)信息轉(zhuǎn)換為笛卡爾坐標(biāo)量測(cè)；C、在一維空間建立約束坐標(biāo)靜態(tài)模型,即建立機(jī)動(dòng)目標(biāo)在道路x，y方向上的位置關(guān)于里程數(shù)s的表達(dá)式；D、依據(jù)所述機(jī)動(dòng)目標(biāo)的運(yùn)動(dòng)特點(diǎn)設(shè)置運(yùn)動(dòng)模型,根據(jù)所建立的表達(dá)式建立狀態(tài)方程和量測(cè)方程；E、分別針對(duì)各所述運(yùn)動(dòng)模型，對(duì)轉(zhuǎn)換得到的笛卡爾坐標(biāo)量測(cè)進(jìn)行濾波，得到一維變量里程數(shù)s及其速度的濾波結(jié)果。根據(jù)本發(fā)明的實(shí)施方式，可以提高機(jī)動(dòng)目標(biāo)跟蹤的性能。
【專利說明】
一種約束條件下的機(jī)動(dòng)目標(biāo)跟蹤方法
技術(shù)領(lǐng)域
[0001 ]本發(fā)明涉及約束條件下的機(jī)動(dòng)目標(biāo)跟蹤，尤其涉及一種將二維約束轉(zhuǎn)到一維空間 下進(jìn)行處理的機(jī)動(dòng)目標(biāo)跟蹤方法。
【背景技術(shù)】
[0002] 在雷達(dá)目標(biāo)跟蹤領(lǐng)域，存在一些場(chǎng)景，目標(biāo)的運(yùn)動(dòng)軌跡發(fā)展不由其本身的運(yùn)動(dòng)速 度決定，而是受到外界環(huán)境的嚴(yán)重影響甚至嚴(yán)格約束。例如，在公路上行駛的車輛受到地形 和公路形狀的約束，離開公路的可能性極小，這就是典型的約束目標(biāo)跟蹤問題。
[0003] 然而，在處理約束目標(biāo)跟蹤問題的時(shí)候，常規(guī)目標(biāo)跟蹤方法存在明顯的缺陷：一、 約束軌跡形狀復(fù)雜，傳統(tǒng)運(yùn)動(dòng)模型失配嚴(yán)重;二、常規(guī)方法不考慮約束條件先驗(yàn)信息，造成 信息浪費(fèi);三、跟蹤結(jié)果難以滿足約束條件，存在性能損失。因此，如何合理利用先驗(yàn)約束信 息，建立準(zhǔn)確的約束目標(biāo)運(yùn)動(dòng)模型并提出對(duì)應(yīng)的跟蹤方法，對(duì)于提高約束目標(biāo)跟蹤精度具 有重要的意義。
[0004]目前比較常見的約束目標(biāo)跟蹤方法包括：
[0005] 一、模型降階法，如W.Wen and H.F.Durrant-Whyte，"Model-based multi-sensor data fusion /'Proceedings of IEEE International Conference on Robotics and Automation,Nice,France,pp. 1720-1726，1992 (基于模型的多傳感器數(shù)據(jù)融合），提取系 統(tǒng)模型中狀態(tài)分量之間的約束關(guān)系式，利用約束關(guān)系式將狀態(tài)分量互相表示來減少狀態(tài)向 量的維數(shù)；R.J.Hewett,M.T.Health，M.D.Butala and F.Kamalabadi，"A robust null space method for linear equality constrained state estimation，''IEEE Transactions on Signal Processing, vol .58,pp.3961 -3971,2010(針對(duì)線性等式約束 狀態(tài)估計(jì)的魯棒零空間方法），通過零空間分解獲得降階模型，這類方法簡(jiǎn)化了系統(tǒng)并獲得 了簡(jiǎn)單模型約束狀態(tài)的最優(yōu)估計(jì)，但缺點(diǎn)是模型降階后物理意義不清晰，且只適用于簡(jiǎn)單 問題難以推廣。
[0006] 二、完美量測(cè)法，如L? S.Wang，Y.T? Chiang and F? R.Chang，"Filtering method for nonlinear systems with constraints，''IEEE Proceedings of Control Theory and Applications，vol. 149(6)，pp. 525-531，2002(包含約束條件的非線性系統(tǒng)濾波方 法），即將等式約束作為沒有噪聲的偽量測(cè)引入，把約束估計(jì)問題便轉(zhuǎn)化為觀測(cè)擴(kuò)維的常規(guī) 估計(jì)問題來解決。但由于觀測(cè)噪聲協(xié)方差矩陣是奇異矩陣，因此在卡爾曼濾波求解時(shí)可能 引起數(shù)值計(jì)算問題。
[0007] 三、估計(jì)后投影法，如D? Simon and T? L? Chia，"Kalman filtering with state equality constraints，''IEEE Transactions on Aerospace and Electronic Systems, vol. 38( 1)，pp. 128-136，2002(狀態(tài)等式約束下的卡爾曼濾波），利用約束優(yōu)化技術(shù)將無約 束估計(jì)向約束子空間投影，尋找約束子空間中距離無約束估計(jì)最近的一點(diǎn)作為最終估計(jì)。 這樣得到的估計(jì)結(jié)果必定滿足約束條件，但不是約束狀態(tài)的最優(yōu)估計(jì)。
[0008] 四、模型修正法，主要有兩類方法，一種如S . Ko and R . Bi tmead，"State estimation for linear systems with state equality constraints，'，Automatica， vol.43(8)，pp. 1363-1368，2007(狀態(tài)等式約束下的線性系統(tǒng)狀態(tài)估計(jì)），通過左乘投影矩 陣實(shí)現(xiàn)無約束動(dòng)態(tài)模型和約束條件的融合。另一種如2.3.〇11 &113.1?.1^311(11.?.1?11， "Design and Analysis of Linear Equality Constrained Dynamic Systems," Proceedings of the 15th International Conference on Information Fusion, Singapore，pp. 2537-2544,2012(線性等式約束動(dòng)態(tài)系統(tǒng)的設(shè)計(jì)與分析），采用直接消除法 得到滿足約束條件的修正模型。這類方法仍然無法解決常規(guī)模型與復(fù)雜約束之間的失配問 題。
[0009] 上述幾種方法都是在常規(guī)無約束模型的基礎(chǔ)上研究約束跟蹤問題，并沒有系統(tǒng)的 解決模型失配等問題，且大多是基于目標(biāo)無機(jī)動(dòng)的假設(shè)開展研究。然而實(shí)際場(chǎng)景中，目標(biāo)可 能會(huì)有變速機(jī)動(dòng)。以道路場(chǎng)景為例，目標(biāo)可能在轉(zhuǎn)彎處減速，超車時(shí)加速等。因此，提出一種 更加貼近物理實(shí)際的機(jī)動(dòng)目標(biāo)運(yùn)動(dòng)建模和狀態(tài)估計(jì)方法具有重要的實(shí)踐意義。

【發(fā)明內(nèi)容】

[0010] 為了克服上述現(xiàn)有技術(shù)存在的一項(xiàng)或更多的缺陷，本發(fā)明提供一種基于一維約束 坐標(biāo)的機(jī)動(dòng)目標(biāo)跟蹤方法。
[0011]為實(shí)現(xiàn)上述目的，本發(fā)明提供了一種約束條件下的機(jī)動(dòng)目標(biāo)跟蹤方法，該方法包 括以下步驟:A、獲得機(jī)動(dòng)目標(biāo)的位置量測(cè)信息;B、將所述位置量測(cè)信息轉(zhuǎn)換為笛卡爾坐標(biāo) 量測(cè);C、在一維空間建立約束坐標(biāo)靜態(tài)模型，即建立機(jī)動(dòng)目標(biāo)在道路x，y方向上的位置關(guān)于 一維變量里程數(shù)s的表達(dá)式;D、依據(jù)所述機(jī)動(dòng)目標(biāo)的運(yùn)動(dòng)特點(diǎn)設(shè)置運(yùn)動(dòng)模型，根據(jù)所建立的 表達(dá)式建立狀態(tài)方程和量測(cè)方程;E、分別針對(duì)各所述運(yùn)動(dòng)模型，對(duì)轉(zhuǎn)換得到的笛卡爾坐標(biāo) 量測(cè)進(jìn)行濾波，得到一維變量里程數(shù)s及其速度的濾波結(jié)果。
[0012] 根據(jù)本發(fā)明的一種實(shí)施方式，所述機(jī)動(dòng)目標(biāo)為道路上行駛的車輛，所述機(jī)動(dòng)目標(biāo) 的位置量測(cè)信息為車輛相對(duì)觀測(cè)雷達(dá)坐標(biāo)系原點(diǎn)的距離和方位角，在所述步驟B中，利用去 偏量測(cè)轉(zhuǎn)換方法將所述位置量測(cè)信息轉(zhuǎn)換為笛卡爾坐標(biāo)量測(cè)。
[0013] 根據(jù)本發(fā)明的一種實(shí)施方式，在所述步驟C中，建立機(jī)動(dòng)目標(biāo)在道路x，y方向上的 位置關(guān)于一維變量里程數(shù)s的表達(dá)式：
[0014] C1:當(dāng)?shù)缆范螢橹本€段時(shí)，直線段上的任意點(diǎn)位置滿足表達(dá)式：
[0016]
，(X1，yi)為直線段起始點(diǎn)，（ X2，y2)為直線段終點(diǎn)；
[0017] C2:當(dāng)?shù)缆范螢閳A弧段時(shí)，圓弧段上的任意點(diǎn)位置滿足表達(dá)式：
[0019]其中，（xi，yi)為圓弧段起始點(diǎn)，（X2，y2)為圓弧段終點(diǎn)，（xojo)為圓心，圓弧起始角
[0020] 根據(jù)本發(fā)明的一種實(shí)施方式，所述步驟E包括計(jì)算各運(yùn)動(dòng)模型對(duì)應(yīng)的模型概率的 步驟，以及按模型概率針對(duì)各所述運(yùn)動(dòng)的濾波結(jié)果進(jìn)行加權(quán)融合的步驟。
[0021] 根據(jù)本發(fā)明的一種實(shí)施方式，所述步驟E濾波包括如下的步驟：
[0022] E1:將k-1時(shí)刻濾波結(jié)果相互作用初值i/°1M及其方差/^1M與轉(zhuǎn)換量測(cè)值4 一起 作為k時(shí)刻第j個(gè)模型的輸入值，利用公式進(jìn)行狀態(tài)一步預(yù)測(cè)，其中，為 狀態(tài)轉(zhuǎn)移矩陣，
[0023] E2、計(jì)算狀態(tài)一步預(yù)測(cè)協(xié)方差吃w :
[0025]其中Qk-丨過程噪聲協(xié)方差矩陣；
[0026] E3、計(jì)算無跡變換S采樣點(diǎn)狀態(tài)預(yù)測(cè)
[0028] 其中k是尺度參數(shù)
是("+幻巧丨,4均方根矩陣 的第1行或第1列，n是狀態(tài)向量的維數(shù)，對(duì)應(yīng)& =[^為『，《=2_;對(duì)應(yīng), ;?=3;
[0029] E4、計(jì)算S采樣點(diǎn)量測(cè)預(yù)測(cè)成_丨
[0030]
[0031 ]其中hk為量測(cè)函數(shù)，由所述一維坐標(biāo)約束建模給出；
[0032] E5、計(jì)算量測(cè)預(yù)測(cè)￡4^
[0034] 其中％?是無跡變換時(shí)對(duì)均值加權(quán)計(jì)算使用的權(quán)值
[0035] E6、計(jì)算新息指量測(cè)值與量測(cè)預(yù)測(cè)值的差
[0036]
[0037] E7、計(jì)算量測(cè)預(yù)測(cè)協(xié)方差，即新息協(xié)方差
[0039] 其中，是無跡變換時(shí)對(duì)均值加權(quán)計(jì)算使用的權(quán)值，iC為量測(cè)噪聲協(xié)方差矩陣
[0040] E8、計(jì)算量測(cè)和狀態(tài)向量的交互協(xié)方差
[0042] E9、確定濾波增益構(gòu)
[0043] iT/m，1
[0044] E10、進(jìn)行狀態(tài)更新，獲得k時(shí)刻j模型濾波結(jié)果i/|A
[0046] Ell、進(jìn)行協(xié)方差更新，獲得k時(shí)刻j模型濾波誤差協(xié)方差
[0048] E12、模型概率更新：
[0049]在得到各個(gè)模型k時(shí)刻的輸出和巧丨,后，更新該時(shí)刻各模型對(duì)應(yīng)的概率，計(jì)算 對(duì)應(yīng)模型j的可能性為
[0051 ]且對(duì)運(yùn)動(dòng)模型j的模型概率進(jìn)行更新：
[0053] E13、k時(shí)刻交互式多模型濾波的融合輸出為
[0056]根據(jù)本發(fā)明的一種實(shí)施方式，所述方法還包括基于k時(shí)刻針對(duì)各運(yùn)動(dòng)模型的濾波 結(jié)果外推下一時(shí)刻預(yù)測(cè)結(jié)果，并根據(jù)k時(shí)刻模型概率對(duì)針對(duì)各運(yùn)動(dòng)模型的預(yù)測(cè)結(jié)果加權(quán)融 合，得到k+1時(shí)刻融合預(yù)測(cè)結(jié)果的步驟。
[0057]本發(fā)明一些實(shí)施方式的一維坐標(biāo)機(jī)動(dòng)目標(biāo)建模方法保證了過程噪聲的自由度為 1，即只在沿道路方向上變化，比起傳統(tǒng)的在二維自由空間建模的方法更加貼近物理實(shí)際。 [0058]根據(jù)本發(fā)明的實(shí)施方式，能夠提尚跟蹤性能。
【附圖說明】
[0059] 結(jié)合附圖，可以更好地理解本發(fā)明，但是附圖僅僅是示例性的，不是對(duì)本發(fā)明的限 制。
[0060] 圖1示出了本發(fā)明的機(jī)動(dòng)目標(biāo)跟蹤方法的一種實(shí)施方式的流程圖；
[0061 ]圖2示出了本發(fā)明的機(jī)動(dòng)目標(biāo)跟蹤方法的濾波流程圖；
[0062]圖3示出了本發(fā)明的一個(gè)實(shí)施例中道路段參數(shù)示意圖；
[0063]圖4和圖5示出了本發(fā)明線性約束仿真實(shí)驗(yàn)的200次蒙特卡洛仿真結(jié)果；
[0064] 圖6示出了本發(fā)明非線性約束仿真實(shí)驗(yàn)的單次軌跡仿真結(jié)果；
[0065] 圖7和圖8示出了本發(fā)明非線性約束仿真實(shí)驗(yàn)的200次蒙特卡洛仿真結(jié)果。
【具體實(shí)施方式】
[0066] 下面結(jié)合附圖和實(shí)施例詳細(xì)說明本發(fā)明的實(shí)施方式。附圖和實(shí)施方式中的描述僅 僅是示例性的，不是對(duì)本發(fā)明的保護(hù)范圍的限制。
[0067] 圖1示出了依據(jù)本發(fā)明的一種實(shí)施方式的流程圖。如圖1所示，
[0068] 首先在步驟S210,從觀測(cè)雷達(dá)處獲得雷達(dá)位置量測(cè)信息。在一種實(shí)施方式中，雷達(dá) 位置量測(cè)信息為機(jī)動(dòng)目標(biāo)(該實(shí)施例以道路上行駛的車輛為例）相對(duì)觀測(cè)雷達(dá)坐標(biāo)系原點(diǎn) 的距離和方位角。至于雷達(dá)如何獲得這些信息、以及本發(fā)明實(shí)施方式的方法如何從雷達(dá)處 獲得這些信息，本領(lǐng)域技術(shù)人員可以采取各種方式來實(shí)現(xiàn)，無論采用哪種方法都在本發(fā)明 的保護(hù)范圍內(nèi)。
[0069]接著，在步驟S220，將來自觀測(cè)雷達(dá)的雷達(dá)位置量測(cè)信息（如上文所示，即機(jī)動(dòng)目 標(biāo)相對(duì)觀測(cè)雷達(dá)坐標(biāo)系原點(diǎn)的距離和方位角)轉(zhuǎn)換為笛卡爾坐標(biāo)量測(cè)。可以采用多種方法 進(jìn)行這種轉(zhuǎn)換，無論采用哪種方法都在本發(fā)明的范圍內(nèi)。在一種實(shí)施方式中，優(yōu)選地，采用 去偏量測(cè)轉(zhuǎn)換方法。
[0070]去偏量測(cè)轉(zhuǎn)換方法如下：
[0072]其中，代表轉(zhuǎn)換量測(cè)向量，Af代表轉(zhuǎn)換后x方向位置量測(cè)，代表轉(zhuǎn)換后y方向位 置量測(cè)，f和咬是從雷達(dá)獲取的距離和方位角量測(cè)，/C是基于目標(biāo)量測(cè)位置得到的平均真 實(shí)偏差。
[0074]對(duì)應(yīng)的協(xié)方差矩陣為
[0080]然后，在步驟S230,獲得車輛的運(yùn)動(dòng)軌跡的約束條件，即一維坐標(biāo)運(yùn)動(dòng)建模。這里 的一維坐標(biāo)是以目標(biāo)所在道路為坐標(biāo)軸，目標(biāo)在道路上行駛的里程數(shù)為坐標(biāo)。將道路近似 為直線段和曲線段來考慮，參考圖3。
[0081]對(duì)于直線段來說，必備的參數(shù)是起始點(diǎn)（X1，yi)和終點(diǎn)（X2，y 2)。有了這兩個(gè)點(diǎn)，道 路直線段的長(zhǎng)度S和方向角0可以表示為：

[0084]被估計(jì)的狀態(tài)變量是弧長(zhǎng)或者稱其為路程，包括它對(duì)應(yīng)的速度和加速度，用s來表 示。那么直線段上的任意點(diǎn)可以表示為：
[0086]對(duì)于圓弧段來說，除了起始點(diǎn)（X1，yi)和終點(diǎn)(X2，y 2)這兩個(gè)先驗(yàn)信息外，我們還需 要圓心(xo，yo)來確定圓弧起始角0〇和曲率半徑R，具體如下：
[0089] 應(yīng)該注意，在該式中，兩個(gè)等號(hào)分別對(duì)應(yīng)利用圓弧段起點(diǎn)和終點(diǎn)計(jì)算曲率半徑的 形式，不是遞進(jìn)關(guān)系而是并列關(guān)系。
[0090] 同樣以s作為變量，圓弧段上的任意點(diǎn)可以表示為：
[0092] 由此得到目標(biāo)x，y方向位置關(guān)于里程數(shù)s的表達(dá)式，該表達(dá)式可以用于量測(cè)方程構(gòu) 建和濾波結(jié)果轉(zhuǎn)換中。
[0093] 接著，在步驟S240,根據(jù)目標(biāo)運(yùn)動(dòng)特點(diǎn)設(shè)置運(yùn)動(dòng)模型。針對(duì)不同應(yīng)用場(chǎng)景采用不同 運(yùn)動(dòng)模型來描述目標(biāo)運(yùn)動(dòng)過程，運(yùn)動(dòng)模型主要由目標(biāo)運(yùn)動(dòng)特性決定，影響狀態(tài)方程的建立。 在本發(fā)明的一些實(shí)施方式中，道路上行駛的車輛主要做近勻速或近勻加速運(yùn)動(dòng)，因此，運(yùn)動(dòng) 模型分為近勻速模型NCV和近勻加速模型NCA。
[0094]之后，在步驟S250,進(jìn)行模型濾波器初始化。
[0095] 為進(jìn)行濾波，需要建立狀態(tài)方程和系統(tǒng)觀測(cè)方程。狀態(tài)方程和系統(tǒng)觀測(cè)方程應(yīng)該 在濾波器初始化前進(jìn)行，即建立方程后再初始化，然后進(jìn)行濾波。
[0096] 狀態(tài)方程是目標(biāo)運(yùn)動(dòng)規(guī)律的假設(shè)，根據(jù)選定的運(yùn)動(dòng)模型寫出目標(biāo)狀態(tài)的遞推表達(dá) 式115=(1 )10；1{-1+「1^1^1，其中(1)1<是狀態(tài)轉(zhuǎn)移矩陣，「1 {是過程噪聲分布矩陣，與運(yùn)動(dòng)模型有關(guān)。
[0099] 分別對(duì)應(yīng)近勻速NCV模型和近勻加速NCA模型，狀態(tài)變量分別為巧和 采用NCA模型時(shí)需要先將狀態(tài)擴(kuò)維，引入加速度分量，濾波后再降維，T為雷 達(dá)掃描周期，Vk-i為零均值高斯白噪聲，其協(xié)方差為0L,二cov(v/fJ =
[0100] 其中qs是沿道路方向的過程噪聲標(biāo)準(zhǔn)差；
[0101] 系統(tǒng)觀測(cè)方程為
[0103] 其中hk(Xk)由所述S1中推導(dǎo)出的里程s與x，y坐標(biāo)間關(guān)系給出，概為去偏轉(zhuǎn)換測(cè)量 噪聲。
[0104] 盡管為了描述的方便，將模型濾波器的初始化設(shè)為單獨(dú)的一個(gè)步驟，但是根據(jù)上 下文，其也可以認(rèn)為是后面濾波步驟的一部分。
[0105] 盡管濾波初始化可以通過一維觀測(cè)獨(dú)立完成，但這樣會(huì)造成信息浪費(fèi)，初始化精 度也不理想。根據(jù)本發(fā)明的一種實(shí)施方式，采用最小均方誤差融合的方法將多維觀測(cè)融合 進(jìn)行濾波初始化，既避免了信息浪費(fèi)又提高了初始化精度。具體過程如下：
[0106] 對(duì)于隨機(jī)變量s = f(x，y)，其方差與x，y方差之間的關(guān)系為：
[0108] 其中 <和<是隨機(jī)變量x，y的方差。
[0109] 若s可分別由x，y單獨(dú)表示，即s = fi(x)，s = f2(y)，則可分別求出s對(duì)x，y的方差：
[0111]此時(shí)估計(jì)的融合公式為：

[0113]其中是分別是采用x，y-維觀測(cè)進(jìn)行初始化的結(jié)果，一維觀測(cè)初始化方法 隨軌跡形狀不同而不同，以直線段道路軌跡為例，一維觀測(cè)初始化方法為 為融合后的方差：
[0115] 之后，在步驟S260進(jìn)行濾波。
[0116] 執(zhí)行交互式多模型濾波，此時(shí)已知上一時(shí)刻各模型濾波器狀態(tài)估計(jì)結(jié)果ihp，狀 態(tài)估計(jì)誤差協(xié)方差，上一時(shí)刻模型概率上一時(shí)刻濾波結(jié)果），本時(shí)刻轉(zhuǎn)換量測(cè)< (雷達(dá)得到），過程噪聲協(xié)方差矩陣Qk-i，量測(cè)噪聲協(xié)方差矩陣霉，狀態(tài)轉(zhuǎn)移矩陣Oh，量測(cè) 函數(shù)hk(先驗(yàn)已知）。
[0117] 首先是進(jìn)行狀態(tài)估計(jì)值之間的相互作用，目的是得到對(duì)應(yīng)j模型濾波器的混合初 值夫及其協(xié)方差^tv
[0118] 子濾波器模型Mj在k_l時(shí)刻的混合狀態(tài)初值由模型Mi的輸出、相應(yīng)的模型概 率以及過渡概率PU來計(jì)算，即
[0121 ]在k_l時(shí)刻，子濾波器模型Mj的狀態(tài)誤差協(xié)方差陣為
[0123] 其中，充4為k-1時(shí)刻第i個(gè)子濾波器的狀態(tài)估計(jì)，（^為相應(yīng)的狀態(tài)協(xié)方差矩 陣，N為模型數(shù)目，
[0124] 將狀態(tài)向量起DliM&其方差巧^_4與轉(zhuǎn)換量測(cè)值％ -起作為k時(shí)刻第j個(gè)模型的輸 入值，由于狀態(tài)變量與量測(cè)間存在非線性關(guān)系，選用無跡卡爾曼濾波器(UKF)進(jìn)行濾波。
[0125] 各模型濾波器單獨(dú)濾波。由于狀態(tài)向量和量測(cè)向量之間是非線性關(guān)系，需要非線 性濾波方法來處理?？梢圆捎枚喾N非線性濾波方法，無論采用哪種方法都在本發(fā)明的范圍 內(nèi)。在一種實(shí)施方式中，優(yōu)選地，采用無跡卡爾曼濾波方法。該方法通過一系列精確選擇的S 采樣點(diǎn)經(jīng)過非線性模型的映射來傳遞隨機(jī)量的統(tǒng)計(jì)特性，是一種有效的非線性濾波方法。
[0126] 單獨(dú)濾波目的是得到對(duì)應(yīng)j模型濾波器的濾波4結(jié)果及其協(xié)方差％，濾波過程參 考圖2。
[0127] 計(jì)算狀態(tài)一步預(yù)測(cè)
[0129] 其中，Oh為狀態(tài)轉(zhuǎn)移矩陣，為相互作用得到的混合初值。
[0130] 計(jì)算協(xié)方差一步預(yù)測(cè)
[0132] 其中，Qk-i為過程噪聲協(xié)方差矩陣
[0133] 計(jì)算無跡變換S采樣點(diǎn)狀態(tài)預(yù)測(cè)
[0135]其中K是尺度參數(shù)，可以為任何數(shù)值
是+ 均方根矩陣的第1行或第1列，n是狀態(tài)向量的維數(shù)，對(duì)應(yīng)x( = ，n = 2 ;對(duì)應(yīng)
[0136]計(jì)算5采樣點(diǎn)量測(cè)預(yù)測(cè)
[0138] 其中hk為量測(cè)函數(shù)，由一維坐標(biāo)約束建模給出
[0139] 計(jì)算量測(cè)預(yù)測(cè)鴦M
[0141] 其中兩?是無跡變換時(shí)對(duì)均值加權(quán)計(jì)算使用的權(quán)值
[0142] 計(jì)算新息 <，指量測(cè)值與量測(cè)預(yù)測(cè)值的差
[0143] vJk = H
[0144] 計(jì)算量測(cè)預(yù)測(cè)協(xié)方差，即新息協(xié)方差
[0146] 其中是無跡變換時(shí)對(duì)均值加權(quán)計(jì)算使用的權(quán)值，％為量測(cè)噪聲協(xié)方差矩陣
[0147] 量測(cè)和狀態(tài)向量的交互協(xié)方差
[0149] 確定濾波增益瓦/
[0150]
[0151] 進(jìn)行狀態(tài)更新，獲得k時(shí)刻j模型濾波結(jié)果
[0152] &H +
[0153] 進(jìn)行協(xié)方差更新，獲得k時(shí)刻j模型濾波誤差協(xié)方差
[0155]其中wr和壞f分別是對(duì)均值和協(xié)方差加權(quán)計(jì)算時(shí)使用的權(quán)值
[0157] 同樣其中k是尺度參數(shù)，可以為任何數(shù)值，只要(n+K)辛〇;n是狀態(tài)向量的維數(shù)，對(duì) .，對(duì)應(yīng), :/?=3;a決定采樣點(diǎn)在均值周圍的散布，優(yōu)選 設(shè)定為比較小的正值，0用于引入變量的先驗(yàn)分布信息，優(yōu)選高斯分布下設(shè)定0 = 2。
[0158] 模型概率更新，目的是得到j(luò)模型對(duì)應(yīng)的模型概率<
[0159]在得到各個(gè)模型k時(shí)刻的輸出力泰和^后，更新該時(shí)刻各模型對(duì)應(yīng)的概率 <，計(jì)算 對(duì)應(yīng)模型j的可能性為
[0161]模型j的模型更新為
[0163] 狀態(tài)估計(jì)融合，目的是得到融合濾波結(jié)果及其協(xié)方差Pk|k
[0164] k時(shí)刻交互式多模型濾波的融合輸出為
[0167] 于是便得到了對(duì)量測(cè)值的濾波結(jié)果及其協(xié)方差，接著對(duì)目標(biāo)狀態(tài)(包括位置和速 度)進(jìn)行外推預(yù)測(cè)。
[0168] 最后，在步驟S270,將濾波結(jié)果轉(zhuǎn)換到笛卡爾坐標(biāo)系下
[0169] 利用步驟S230中推導(dǎo)出的里程s與x，y坐標(biāo)間關(guān)系將交互式多模型濾波結(jié)果 % = ['，4u-T轉(zhuǎn)換到笛卡爾坐標(biāo)系下，得到目標(biāo)位置和速度濾波結(jié)果A = 然后轉(zhuǎn)第一步等待下一時(shí)刻k+i時(shí)刻雷達(dá)觀測(cè)數(shù)據(jù)。
[0170] 本發(fā)明所述的公路坐標(biāo)運(yùn)動(dòng)建模方法和跟蹤方法為約束目標(biāo)跟蹤提供了一條新 途徑，合理利用等式約束條件的同時(shí)有效避免了信息浪費(fèi)。本發(fā)明的一些實(shí)施方式的公路 坐標(biāo)運(yùn)動(dòng)建模方法的一個(gè)優(yōu)點(diǎn)在于保證了過程噪聲的自由度為1，即只在沿道路方向上變 化，比起傳統(tǒng)的在二維自由空間建模的方法更加貼近物理實(shí)際。這種建模方法還使得目標(biāo) 動(dòng)態(tài)模型與其運(yùn)動(dòng)軌跡獨(dú)立，在保證濾波結(jié)果是滿足約束條件的同時(shí)大大降低了復(fù)雜道路 約束對(duì)濾波精度的影響，有效提高濾波性能。在跟蹤方法中引入交互式多模型方法，提高了 應(yīng)對(duì)目標(biāo)可能存在的變速機(jī)動(dòng)的能力，多個(gè)模型同時(shí)作用有效匹配目標(biāo)實(shí)際運(yùn)動(dòng)模型，減 少模型失配導(dǎo)致的性能惡化。
[0171]為展示本發(fā)明的實(shí)施方式的效果，設(shè)置兩組仿真場(chǎng)景分別對(duì)應(yīng)線性等式約束和非 線性等式約束。首先在線性等式約束場(chǎng)景中驗(yàn)證方法性能，跟蹤一個(gè)在道路直線段上勻速 行駛的目標(biāo)。將本發(fā)明提出的約束坐標(biāo)卡爾曼濾波方法(CCKF)與幾種經(jīng)典的約束估計(jì)算法 進(jìn)行比較，包括完美量測(cè)法(PMKF)，估計(jì)后投影法(EPKF)，線性等式約束卡爾曼濾波方法 (LECKF)以及模型降維法(MRKF)。此外還選取一種經(jīng)典的無約束算法，轉(zhuǎn)換量測(cè)法(CMKF)作 為對(duì)比。選取均方根誤差(RMSE)作為性能的量度，進(jìn)行200次蒙特卡洛仿真。
[0172]仿真結(jié)果參見圖4、5所示，可以明顯看出CCKF方法成功將約束條件引入跟蹤系統(tǒng) 中，性能明顯優(yōu)于非約束的CMKF方法。與其他約束估計(jì)算法相比，CCKF也展現(xiàn)出一定的性能 優(yōu)勢(shì)。這是由于其采用了不同的目標(biāo)運(yùn)動(dòng)建模方法，運(yùn)動(dòng)模型更加貼近物理實(shí)際。
[0173]接著在非線性場(chǎng)景中驗(yàn)證交互式多模型約束坐標(biāo)卡爾曼濾波方法（MM-CCKF)跟 蹤機(jī)動(dòng)目標(biāo)的能力，跟蹤一個(gè)在近似田徑跑道形狀道路段上變速運(yùn)動(dòng)的目標(biāo)。將頂M-CCKF 與包含勻速(CV)、勻加速(CA)和坐標(biāo)轉(zhuǎn)彎(CT)模型的交互式多模型算法(IMM-CVCACT、mM-CVCA)對(duì)比。同樣選取均方根誤差(RMSE)作為性能的量度。單次軌跡仿真結(jié)果如圖6所示，可 以看出，濾波結(jié)果是滿足約束條件的，證明MM-CCKF可以有效引入約束條件并保證濾波結(jié) 果滿足約束條件。
[0174] 200次蒙特卡洛仿真結(jié)果如圖7、8所示，可以明顯看出頂M-CCKF算法性能整體優(yōu)于 其他兩種交互式多模型算法，尤其是在模型轉(zhuǎn)換的時(shí)候，其他兩種算法均出現(xiàn)了明顯的濾 波發(fā)散現(xiàn)象，而MM-CCKF的曲線波動(dòng)相對(duì)較為平緩，基本保持在相近的水平上。這是由于 頂M-CCKF算法運(yùn)動(dòng)模型更加貼近物理實(shí)際，同時(shí)目標(biāo)的動(dòng)態(tài)模型與其運(yùn)動(dòng)軌跡獨(dú)立，非線 性約束對(duì)濾波過程的影響很小，因此得到了比較好的結(jié)果。
[0175] 因而，本發(fā)明的一些實(shí)施方式將目標(biāo)所在道路作為一維坐標(biāo)系，于是道路上目標(biāo) 的運(yùn)動(dòng)便可以由其行駛里程數(shù)和速度隨時(shí)間的變化來描述。再將整個(gè)公路分成多個(gè)道路段 分別用直線段和圓弧段來近似，建立目標(biāo)位置和行駛里程數(shù)之間的關(guān)系，在一維空間內(nèi)對(duì) 行駛里程數(shù)進(jìn)行濾波。濾波跟蹤過程利用無跡卡爾曼濾波(UKF)來處理一維狀態(tài)變量和笛 卡爾坐標(biāo)量測(cè)之間存在的非線性關(guān)系，并結(jié)合交互式多模型（MM)方法，引入近勻速(NCV) 和近勻加速(NCA)模型應(yīng)對(duì)目標(biāo)存在機(jī)動(dòng)的情況，得到一維濾波結(jié)果，最后將一維濾波結(jié)果 轉(zhuǎn)換到笛卡爾坐標(biāo)系，得到目標(biāo)位置的濾波結(jié)果。
[0176] 另外，本發(fā)明所述的建模方法使得目標(biāo)動(dòng)態(tài)模型與其運(yùn)動(dòng)軌跡獨(dú)立，在保證濾波 結(jié)果是滿足約束條件的同時(shí)大大降低了復(fù)雜道路約束對(duì)濾波精度的影響，有效提高濾波性 能。
[0177] 本發(fā)明所述的公路坐標(biāo)機(jī)動(dòng)目標(biāo)跟蹤方法中引入交互式多模型方法，提高了應(yīng)對(duì) 目標(biāo)可能存在的變速機(jī)動(dòng)的能力，多個(gè)模型同時(shí)作用有效匹配目標(biāo)實(shí)際運(yùn)動(dòng)模型，減少模 型失配導(dǎo)致的性能惡化。
[0178] 應(yīng)該注意，以上僅為本發(fā)明的較佳實(shí)施例而已，并非用于限定本發(fā)明的保護(hù)范圍。 凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的 保護(hù)范圍之內(nèi)。
【主權(quán)項(xiàng)】
1. 一種約束條件下的機(jī)動(dòng)目標(biāo)跟蹤方法，其特征在于，所述方法包括以下步驟： A、 獲得機(jī)動(dòng)目標(biāo)的位置量測(cè)信息； B、 將所述位置量測(cè)信息轉(zhuǎn)換為笛卡爾坐標(biāo)量測(cè)； C、 在一維空間建立約束坐標(biāo)靜態(tài)模型，即建立機(jī)動(dòng)目標(biāo)在道路x，y方向上的位置關(guān)于 一維變量里程數(shù)s的表達(dá)式； D、 依據(jù)所述機(jī)動(dòng)目標(biāo)的運(yùn)動(dòng)特點(diǎn)設(shè)置運(yùn)動(dòng)模型，根據(jù)所建立的表達(dá)式建立狀態(tài)方程和 量測(cè)方程； E、 分別針對(duì)各所述運(yùn)動(dòng)模型，對(duì)轉(zhuǎn)換得到的笛卡爾坐標(biāo)量測(cè)進(jìn)行濾波，得到一維變量 里程數(shù)s及其速度的濾波結(jié)果。2. 根據(jù)權(quán)利要求1所述的方法，其特征在于，所述機(jī)動(dòng)目標(biāo)為道路上行駛的車輛，所述 機(jī)動(dòng)目標(biāo)的位置量測(cè)信息為車輛相對(duì)觀測(cè)雷達(dá)坐標(biāo)系原點(diǎn)的距離和方位角，在所述步驟B 中，利用去偏量測(cè)轉(zhuǎn)換方法將所述位置量測(cè)信息轉(zhuǎn)換為笛卡爾坐標(biāo)量測(cè)。3. 根據(jù)權(quán)利要求1所述的方法，其特征在于，在所述步驟C中，如下地建立機(jī)動(dòng)目標(biāo)在道 路x，y方向上的位置關(guān)于里程數(shù)s的表達(dá)式： C1:當(dāng)?shù)缆范螢橹本€段時(shí)，直線段上的任意點(diǎn)位置滿足表達(dá)式：，_(義1，71)為直線段起始點(diǎn)，（12,72)為直線段終點(diǎn)； C2:當(dāng)?shù)缆范螢閳A弧段時(shí)，圓弧段上的任意點(diǎn)位置滿足表達(dá)式：其中，（XI，yi)為圓弧段起始點(diǎn)，（X2，y2)為圓弧段終點(diǎn)，（XQ，y〇)為圓心，圓弧起始角4. 根據(jù)權(quán)利要求3所述的方法，其特征在于，所述步驟E包括計(jì)算各運(yùn)動(dòng)模型對(duì)應(yīng)的模 型概率的步驟，以及按模型概率針對(duì)各所述運(yùn)動(dòng)的濾波結(jié)果進(jìn)行加權(quán)融合的步驟。5. 根據(jù)權(quán)利要求4所述的方法，其特征在于，所述步驟E的濾波包括如下的步驟： E1:將k-1時(shí)刻濾波結(jié)果相互作用初值及其方差/^^與轉(zhuǎn)換量測(cè)值拉一起作為k 時(shí)刻第j個(gè)模型的輸入值，利用公式每進(jìn)行狀態(tài)一步預(yù)測(cè)，其中為狀態(tài) 轉(zhuǎn)移矩陣， E2、計(jì)算狀態(tài)一步預(yù)測(cè)協(xié)方差:其中Qk-i過程噪聲協(xié)方差矩陣； E3、計(jì)算無跡變換S采樣點(diǎn)狀態(tài)預(yù)測(cè)其中k是尺度參數(shù)，其數(shù)值滿足(n+K)辛〇,是均方根矩陣的第 1行或第1列，n是狀態(tài)向量的維數(shù)，對(duì)應(yīng)& = .，n = 2;對(duì)應(yīng)& = [^,.4.，為= 3; E4、計(jì)算S采樣點(diǎn)量測(cè)預(yù)測(cè) 其中hk為量測(cè)函數(shù)，由所述一維坐標(biāo)約束建模給出； E5、計(jì)算量測(cè)預(yù)測(cè)私《其中是無跡變換時(shí)對(duì)均值加權(quán)計(jì)算使用的權(quán)值 E6、計(jì)算新息 <，指量測(cè)值與量測(cè)預(yù)測(cè)值的差 d毛V-丨 E7、計(jì)算量測(cè)預(yù)測(cè)協(xié)方差，即新息協(xié)方差其中，》r是無跡變換時(shí)對(duì)均值加權(quán)計(jì)算使用的權(quán)值，為量測(cè)噪聲協(xié)方差矩陣 E8、計(jì)算量測(cè)和狀態(tài)向量的交互協(xié)方差E9、確定濾波增益g my E10、進(jìn)行狀態(tài)更新，獲得k時(shí)刻j模型濾波結(jié)果i4： % = +Kivi E11、進(jìn)行協(xié)方差更新，獲得k時(shí)刻j模型濾波誤差協(xié)方差 P^PUiPUKif E12、模型概率更新： 在得到各個(gè)模型k時(shí)刻的輸出毛^和2^后，更新該時(shí)刻各模型對(duì)應(yīng)的概率，計(jì)算對(duì)應(yīng) 模型j的可能性為且對(duì)運(yùn)動(dòng)模型j的模型概率進(jìn)行更新：E13、k時(shí)刻交互式多模型濾波的融合輸出為6.根據(jù)權(quán)利要求5所述的方法，其特征在于，還包括基于k時(shí)刻針對(duì)各運(yùn)動(dòng)模型的濾波 結(jié)果外推下一時(shí)刻預(yù)測(cè)結(jié)果，并根據(jù)k時(shí)刻模型概率對(duì)針對(duì)各運(yùn)動(dòng)模型的預(yù)測(cè)結(jié)果加權(quán)融 合，得到k+1時(shí)刻融合預(yù)測(cè)結(jié)果的步驟。
【文檔編號(hào)】G01S13/72GK106054170SQ201610339347
【公開日】2016年10月26日
【申請(qǐng)日】2016年5月19日
【發(fā)明人】周共健, 李可毅, 陳曦
【申請(qǐng)人】哈爾濱工業(yè)大學(xué)