一種約束條件下的機動目標跟蹤方法
【專利摘要】本發明公開了一種約束條件下的機動目標跟蹤方法，所述方法包括以下步驟：A、獲得機動目標的位置量測信息；B、將所述位置量測信息轉換為笛卡爾坐標量測；C、在一維空間建立約束坐標靜態模型,即建立機動目標在道路x，y方向上的位置關于里程數s的表達式；D、依據所述機動目標的運動特點設置運動模型,根據所建立的表達式建立狀態方程和量測方程；E、分別針對各所述運動模型，對轉換得到的笛卡爾坐標量測進行濾波，得到一維變量里程數s及其速度的濾波結果。根據本發明的實施方式，可以提高機動目標跟蹤的性能。
【專利說明】
一種約束條件下的機動目標跟蹤方法
技術領域
[0001 ]本發明涉及約束條件下的機動目標跟蹤，尤其涉及一種將二維約束轉到一維空間 下進行處理的機動目標跟蹤方法。
【背景技術】
[0002] 在雷達目標跟蹤領域，存在一些場景，目標的運動軌跡發展不由其本身的運動速 度決定，而是受到外界環境的嚴重影響甚至嚴格約束。例如，在公路上行駛的車輛受到地形 和公路形狀的約束，離開公路的可能性極小，這就是典型的約束目標跟蹤問題。
[0003] 然而，在處理約束目標跟蹤問題的時候，常規目標跟蹤方法存在明顯的缺陷：一、 約束軌跡形狀復雜，傳統運動模型失配嚴重;二、常規方法不考慮約束條件先驗信息，造成 信息浪費;三、跟蹤結果難以滿足約束條件，存在性能損失。因此，如何合理利用先驗約束信 息，建立準確的約束目標運動模型并提出對應的跟蹤方法，對于提高約束目標跟蹤精度具 有重要的意義。
[0004]目前比較常見的約束目標跟蹤方法包括：
[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 (基于模型的多傳感器數據融合），提取系 統模型中狀態分量之間的約束關系式，利用約束關系式將狀態分量互相表示來減少狀態向 量的維數；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(針對線性等式約束 狀態估計的魯棒零空間方法），通過零空間分解獲得降階模型，這類方法簡化了系統并獲得 了簡單模型約束狀態的最優估計，但缺點是模型降階后物理意義不清晰，且只適用于簡單 問題難以推廣。
[0006] 二、完美量測法，如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(包含約束條件的非線性系統濾波方 法），即將等式約束作為沒有噪聲的偽量測引入，把約束估計問題便轉化為觀測擴維的常規 估計問題來解決。但由于觀測噪聲協方差矩陣是奇異矩陣，因此在卡爾曼濾波求解時可能 引起數值計算問題。
[0007] 三、估計后投影法，如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(狀態等式約束下的卡爾曼濾波），利用約束優化技術將無約 束估計向約束子空間投影，尋找約束子空間中距離無約束估計最近的一點作為最終估計。 這樣得到的估計結果必定滿足約束條件，但不是約束狀態的最優估計。
[0008] 四、模型修正法，主要有兩類方法，一種如S . Ko and R . Bi tmead，"State estimation for linear systems with state equality constraints，'，Automatica， vol.43(8)，pp. 1363-1368，2007(狀態等式約束下的線性系統狀態估計），通過左乘投影矩 陣實現無約束動態模型和約束條件的融合。另一種如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(線性等式約束動態系統的設計與分析），采用直接消除法 得到滿足約束條件的修正模型。這類方法仍然無法解決常規模型與復雜約束之間的失配問 題。
[0009] 上述幾種方法都是在常規無約束模型的基礎上研究約束跟蹤問題，并沒有系統的 解決模型失配等問題，且大多是基于目標無機動的假設開展研究。然而實際場景中，目標可 能會有變速機動。以道路場景為例，目標可能在轉彎處減速，超車時加速等。因此，提出一種 更加貼近物理實際的機動目標運動建模和狀態估計方法具有重要的實踐意義。
【發明內容】
[0010] 為了克服上述現有技術存在的一項或更多的缺陷，本發明提供一種基于一維約束 坐標的機動目標跟蹤方法。
[0011]為實現上述目的，本發明提供了一種約束條件下的機動目標跟蹤方法，該方法包 括以下步驟:A、獲得機動目標的位置量測信息;B、將所述位置量測信息轉換為笛卡爾坐標 量測;C、在一維空間建立約束坐標靜態模型，即建立機動目標在道路x，y方向上的位置關于 一維變量里程數s的表達式;D、依據所述機動目標的運動特點設置運動模型，根據所建立的 表達式建立狀態方程和量測方程;E、分別針對各所述運動模型，對轉換得到的笛卡爾坐標 量測進行濾波，得到一維變量里程數s及其速度的濾波結果。
[0012] 根據本發明的一種實施方式，所述機動目標為道路上行駛的車輛，所述機動目標 的位置量測信息為車輛相對觀測雷達坐標系原點的距離和方位角，在所述步驟B中，利用去 偏量測轉換方法將所述位置量測信息轉換為笛卡爾坐標量測。
[0013] 根據本發明的一種實施方式，在所述步驟C中，建立機動目標在道路x，y方向上的 位置關于一維變量里程數s的表達式：
[0014] C1:當道路段為直線段時，直線段上的任意點位置滿足表達式：
[0016]
，(X1，yi)為直線段起始點，（ X2，y2)為直線段終點；
[0017] C2:當道路段為圓弧段時，圓弧段上的任意點位置滿足表達式：
[0019]其中，（xi，yi)為圓弧段起始點，（X2，y2)為圓弧段終點，（xojo)為圓心，圓弧起始角
[0020] 根據本發明的一種實施方式，所述步驟E包括計算各運動模型對應的模型概率的 步驟，以及按模型概率針對各所述運動的濾波結果進行加權融合的步驟。
[0021] 根據本發明的一種實施方式，所述步驟E濾波包括如下的步驟：
[0022] E1:將k-1時刻濾波結果相互作用初值i/°1M及其方差/^1M與轉換量測值4 一起 作為k時刻第j個模型的輸入值，利用公式進行狀態一步預測，其中，為 狀態轉移矩陣，
[0023] E2、計算狀態一步預測協方差吃w :
[0025]其中Qk-丨過程噪聲協方差矩陣；
[0026] E3、計算無跡變換S采樣點狀態預測
[0028] 其中k是尺度參數
是("+幻巧丨,4均方根矩陣 的第1行或第1列，n是狀態向量的維數，對應& =[^為『，《=2_;對應, ;?=3;
[0029] E4、計算S采樣點量測預測成_丨
[0030]
[0031 ]其中hk為量測函數，由所述一維坐標約束建模給出；
[0032] E5、計算量測預測￡4^
[0034] 其中％?是無跡變換時對均值加權計算使用的權值
[0035] E6、計算新息指量測值與量測預測值的差
[0036]
[0037] E7、計算量測預測協方差，即新息協方差
[0039] 其中，是無跡變換時對均值加權計算使用的權值，iC為量測噪聲協方差矩陣
[0040] E8、計算量測和狀態向量的交互協方差
[0042] E9、確定濾波增益構
[0043] iT/m，1
[0044] E10、進行狀態更新，獲得k時刻j模型濾波結果i/|A
[0046] Ell、進行協方差更新，獲得k時刻j模型濾波誤差協方差
[0048] E12、模型概率更新：
[0049]在得到各個模型k時刻的輸出和巧丨,后，更新該時刻各模型對應的概率，計算 對應模型j的可能性為
[0051 ]且對運動模型j的模型概率進行更新：
[0053] E13、k時刻交互式多模型濾波的融合輸出為
[0056]根據本發明的一種實施方式，所述方法還包括基于k時刻針對各運動模型的濾波 結果外推下一時刻預測結果，并根據k時刻模型概率對針對各運動模型的預測結果加權融 合，得到k+1時刻融合預測結果的步驟。
[0057]本發明一些實施方式的一維坐標機動目標建模方法保證了過程噪聲的自由度為 1，即只在沿道路方向上變化，比起傳統的在二維自由空間建模的方法更加貼近物理實際。 [0058]根據本發明的實施方式，能夠提尚跟蹤性能。
【附圖說明】
[0059] 結合附圖，可以更好地理解本發明，但是附圖僅僅是示例性的，不是對本發明的限 制。
[0060] 圖1示出了本發明的機動目標跟蹤方法的一種實施方式的流程圖；
[0061 ]圖2示出了本發明的機動目標跟蹤方法的濾波流程圖；
[0062]圖3示出了本發明的一個實施例中道路段參數示意圖；
[0063]圖4和圖5示出了本發明線性約束仿真實驗的200次蒙特卡洛仿真結果；
[0064] 圖6示出了本發明非線性約束仿真實驗的單次軌跡仿真結果；
[0065] 圖7和圖8示出了本發明非線性約束仿真實驗的200次蒙特卡洛仿真結果。
【具體實施方式】
[0066] 下面結合附圖和實施例詳細說明本發明的實施方式。附圖和實施方式中的描述僅 僅是示例性的，不是對本發明的保護范圍的限制。
[0067] 圖1示出了依據本發明的一種實施方式的流程圖。如圖1所示，
[0068] 首先在步驟S210,從觀測雷達處獲得雷達位置量測信息。在一種實施方式中，雷達 位置量測信息為機動目標(該實施例以道路上行駛的車輛為例）相對觀測雷達坐標系原點 的距離和方位角。至于雷達如何獲得這些信息、以及本發明實施方式的方法如何從雷達處 獲得這些信息，本領域技術人員可以采取各種方式來實現，無論采用哪種方法都在本發明 的保護范圍內。
[0069]接著，在步驟S220，將來自觀測雷達的雷達位置量測信息（如上文所示，即機動目 標相對觀測雷達坐標系原點的距離和方位角)轉換為笛卡爾坐標量測。可以采用多種方法 進行這種轉換，無論采用哪種方法都在本發明的范圍內。在一種實施方式中，優選地，采用 去偏量測轉換方法。
[0070]去偏量測轉換方法如下：
[0072]其中，代表轉換量測向量，Af代表轉換后x方向位置量測，代表轉換后y方向位 置量測，f和咬是從雷達獲取的距離和方位角量測，/C是基于目標量測位置得到的平均真 實偏差。
[0074]對應的協方差矩陣為
[0080]然后，在步驟S230,獲得車輛的運動軌跡的約束條件，即一維坐標運動建模。這里 的一維坐標是以目標所在道路為坐標軸，目標在道路上行駛的里程數為坐標。將道路近似 為直線段和曲線段來考慮，參考圖3。
[0081]對于直線段來說，必備的參數是起始點（X1，yi)和終點（X2，y 2)。有了這兩個點，道 路直線段的長度S和方向角0可以表示為：
[0084]被估計的狀態變量是弧長或者稱其為路程，包括它對應的速度和加速度，用s來表 示。那么直線段上的任意點可以表示為：
[0086]對于圓弧段來說，除了起始點（X1，yi)和終點(X2，y 2)這兩個先驗信息外，我們還需 要圓心(xo，yo)來確定圓弧起始角0〇和曲率半徑R，具體如下：
[0089] 應該注意，在該式中，兩個等號分別對應利用圓弧段起點和終點計算曲率半徑的 形式，不是遞進關系而是并列關系。
[0090] 同樣以s作為變量，圓弧段上的任意點可以表示為：
[0092] 由此得到目標x，y方向位置關于里程數s的表達式，該表達式可以用于量測方程構 建和濾波結果轉換中。
[0093] 接著，在步驟S240,根據目標運動特點設置運動模型。針對不同應用場景采用不同 運動模型來描述目標運動過程，運動模型主要由目標運動特性決定，影響狀態方程的建立。 在本發明的一些實施方式中，道路上行駛的車輛主要做近勻速或近勻加速運動，因此，運動 模型分為近勻速模型NCV和近勻加速模型NCA。
[0094]之后，在步驟S250,進行模型濾波器初始化。
[0095] 為進行濾波，需要建立狀態方程和系統觀測方程。狀態方程和系統觀測方程應該 在濾波器初始化前進行，即建立方程后再初始化，然后進行濾波。
[0096] 狀態方程是目標運動規律的假設，根據選定的運動模型寫出目標狀態的遞推表達 式115=(1 )10；1{-1+「1^1^1，其中(1)1<是狀態轉移矩陣，「1 {是過程噪聲分布矩陣，與運動模型有關。
[0099] 分別對應近勻速NCV模型和近勻加速NCA模型，狀態變量分別為巧和 采用NCA模型時需要先將狀態擴維，引入加速度分量，濾波后再降維，T為雷 達掃描周期，Vk-i為零均值高斯白噪聲，其協方差為0L,二cov(v/fJ =
[0100] 其中qs是沿道路方向的過程噪聲標準差；
[0101] 系統觀測方程為
[0103] 其中hk(Xk)由所述S1中推導出的里程s與x，y坐標間關系給出，概為去偏轉換測量 噪聲。
[0104] 盡管為了描述的方便，將模型濾波器的初始化設為單獨的一個步驟，但是根據上 下文，其也可以認為是后面濾波步驟的一部分。
[0105] 盡管濾波初始化可以通過一維觀測獨立完成，但這樣會造成信息浪費，初始化精 度也不理想。根據本發明的一種實施方式，采用最小均方誤差融合的方法將多維觀測融合 進行濾波初始化，既避免了信息浪費又提高了初始化精度。具體過程如下：
[0106] 對于隨機變量s = f(x，y)，其方差與x，y方差之間的關系為：
[0108] 其中 <和<是隨機變量x，y的方差。
[0109] 若s可分別由x，y單獨表示，即s = fi(x)，s = f2(y)，則可分別求出s對x，y的方差：
[0111]此時估計的融合公式為：
[0113]其中是分別是采用x，y-維觀測進行初始化的結果，一維觀測初始化方法 隨軌跡形狀不同而不同，以直線段道路軌跡為例，一維觀測初始化方法為 為融合后的方差：
[0115] 之后，在步驟S260進行濾波。
[0116] 執行交互式多模型濾波，此時已知上一時刻各模型濾波器狀態估計結果ihp，狀 態估計誤差協方差，上一時刻模型概率上一時刻濾波結果），本時刻轉換量測< (雷達得到），過程噪聲協方差矩陣Qk-i，量測噪聲協方差矩陣霉，狀態轉移矩陣Oh，量測 函數hk(先驗已知）。
[0117] 首先是進行狀態估計值之間的相互作用，目的是得到對應j模型濾波器的混合初 值夫及其協方差^tv
[0118] 子濾波器模型Mj在k_l時刻的混合狀態初值由模型Mi的輸出、相應的模型概 率以及過渡概率PU來計算，即
[0121 ]在k_l時刻，子濾波器模型Mj的狀態誤差協方差陣為
[0123] 其中，充4為k-1時刻第i個子濾波器的狀態估計，（^為相應的狀態協方差矩 陣，N為模型數目，
[0124] 將狀態向量起DliM&其方差巧^_4與轉換量測值％ -起作為k時刻第j個模型的輸 入值，由于狀態變量與量測間存在非線性關系，選用無跡卡爾曼濾波器(UKF)進行濾波。
[0125] 各模型濾波器單獨濾波。由于狀態向量和量測向量之間是非線性關系，需要非線 性濾波方法來處理。可以采用多種非線性濾波方法，無論采用哪種方法都在本發明的范圍 內。在一種實施方式中，優選地，采用無跡卡爾曼濾波方法。該方法通過一系列精確選擇的S 采樣點經過非線性模型的映射來傳遞隨機量的統計特性，是一種有效的非線性濾波方法。
[0126] 單獨濾波目的是得到對應j模型濾波器的濾波4結果及其協方差％，濾波過程參 考圖2。
[0127] 計算狀態一步預測
[0129] 其中，Oh為狀態轉移矩陣，為相互作用得到的混合初值。
[0130] 計算協方差一步預測
[0132] 其中，Qk-i為過程噪聲協方差矩陣
[0133] 計算無跡變換S采樣點狀態預測
[0135]其中K是尺度參數，可以為任何數值
是+ 均方根矩陣的第1行或第1列，n是狀態向量的維數，對應x( = ，n = 2 ;對應
[0136]計算5采樣點量測預測
[0138] 其中hk為量測函數，由一維坐標約束建模給出
[0139] 計算量測預測鴦M
[0141] 其中兩?是無跡變換時對均值加權計算使用的權值
[0142] 計算新息 <，指量測值與量測預測值的差
[0143] vJk = H
[0144] 計算量測預測協方差，即新息協方差
[0146] 其中是無跡變換時對均值加權計算使用的權值，％為量測噪聲協方差矩陣
[0147] 量測和狀態向量的交互協方差
[0149] 確定濾波增益瓦/
[0150]
[0151] 進行狀態更新，獲得k時刻j模型濾波結果
[0152] &H +
[0153] 進行協方差更新，獲得k時刻j模型濾波誤差協方差
[0155]其中wr和壞f分別是對均值和協方差加權計算時使用的權值
[0157] 同樣其中k是尺度參數，可以為任何數值，只要(n+K)辛〇;n是狀態向量的維數，對 .，對應, :/?=3;a決定采樣點在均值周圍的散布，優選 設定為比較小的正值，0用于引入變量的先驗分布信息，優選高斯分布下設定0 = 2。
[0158] 模型概率更新，目的是得到j模型對應的模型概率<
[0159]在得到各個模型k時刻的輸出力泰和^后，更新該時刻各模型對應的概率 <，計算 對應模型j的可能性為
[0161]模型j的模型更新為
[0163] 狀態估計融合，目的是得到融合濾波結果及其協方差Pk|k
[0164] k時刻交互式多模型濾波的融合輸出為
[0167] 于是便得到了對量測值的濾波結果及其協方差，接著對目標狀態(包括位置和速 度)進行外推預測。
[0168] 最后，在步驟S270,將濾波結果轉換到笛卡爾坐標系下
[0169] 利用步驟S230中推導出的里程s與x，y坐標間關系將交互式多模型濾波結果 % = ['，4u-T轉換到笛卡爾坐標系下，得到目標位置和速度濾波結果A = 然后轉第一步等待下一時刻k+i時刻雷達觀測數據。
[0170] 本發明所述的公路坐標運動建模方法和跟蹤方法為約束目標跟蹤提供了一條新 途徑，合理利用等式約束條件的同時有效避免了信息浪費。本發明的一些實施方式的公路 坐標運動建模方法的一個優點在于保證了過程噪聲的自由度為1，即只在沿道路方向上變 化，比起傳統的在二維自由空間建模的方法更加貼近物理實際。這種建模方法還使得目標 動態模型與其運動軌跡獨立，在保證濾波結果是滿足約束條件的同時大大降低了復雜道路 約束對濾波精度的影響，有效提高濾波性能。在跟蹤方法中引入交互式多模型方法，提高了 應對目標可能存在的變速機動的能力，多個模型同時作用有效匹配目標實際運動模型，減 少模型失配導致的性能惡化。
[0171]為展示本發明的實施方式的效果，設置兩組仿真場景分別對應線性等式約束和非 線性等式約束。首先在線性等式約束場景中驗證方法性能，跟蹤一個在道路直線段上勻速 行駛的目標。將本發明提出的約束坐標卡爾曼濾波方法(CCKF)與幾種經典的約束估計算法 進行比較，包括完美量測法(PMKF)，估計后投影法(EPKF)，線性等式約束卡爾曼濾波方法 (LECKF)以及模型降維法(MRKF)。此外還選取一種經典的無約束算法，轉換量測法(CMKF)作 為對比。選取均方根誤差(RMSE)作為性能的量度，進行200次蒙特卡洛仿真。
[0172]仿真結果參見圖4、5所示，可以明顯看出CCKF方法成功將約束條件引入跟蹤系統 中，性能明顯優于非約束的CMKF方法。與其他約束估計算法相比，CCKF也展現出一定的性能 優勢。這是由于其采用了不同的目標運動建模方法，運動模型更加貼近物理實際。
[0173]接著在非線性場景中驗證交互式多模型約束坐標卡爾曼濾波方法（MM-CCKF)跟 蹤機動目標的能力，跟蹤一個在近似田徑跑道形狀道路段上變速運動的目標。將頂M-CCKF 與包含勻速(CV)、勻加速(CA)和坐標轉彎(CT)模型的交互式多模型算法(IMM-CVCACT、mM-CVCA)對比。同樣選取均方根誤差(RMSE)作為性能的量度。單次軌跡仿真結果如圖6所示，可 以看出，濾波結果是滿足約束條件的，證明MM-CCKF可以有效引入約束條件并保證濾波結 果滿足約束條件。
[0174] 200次蒙特卡洛仿真結果如圖7、8所示，可以明顯看出頂M-CCKF算法性能整體優于 其他兩種交互式多模型算法，尤其是在模型轉換的時候，其他兩種算法均出現了明顯的濾 波發散現象，而MM-CCKF的曲線波動相對較為平緩，基本保持在相近的水平上。這是由于 頂M-CCKF算法運動模型更加貼近物理實際，同時目標的動態模型與其運動軌跡獨立，非線 性約束對濾波過程的影響很小，因此得到了比較好的結果。
[0175] 因而，本發明的一些實施方式將目標所在道路作為一維坐標系，于是道路上目標 的運動便可以由其行駛里程數和速度隨時間的變化來描述。再將整個公路分成多個道路段 分別用直線段和圓弧段來近似，建立目標位置和行駛里程數之間的關系，在一維空間內對 行駛里程數進行濾波。濾波跟蹤過程利用無跡卡爾曼濾波(UKF)來處理一維狀態變量和笛 卡爾坐標量測之間存在的非線性關系，并結合交互式多模型（MM)方法，引入近勻速(NCV) 和近勻加速(NCA)模型應對目標存在機動的情況，得到一維濾波結果，最后將一維濾波結果 轉換到笛卡爾坐標系，得到目標位置的濾波結果。
[0176] 另外，本發明所述的建模方法使得目標動態模型與其運動軌跡獨立，在保證濾波 結果是滿足約束條件的同時大大降低了復雜道路約束對濾波精度的影響，有效提高濾波性 能。
[0177] 本發明所述的公路坐標機動目標跟蹤方法中引入交互式多模型方法，提高了應對 目標可能存在的變速機動的能力，多個模型同時作用有效匹配目標實際運動模型，減少模 型失配導致的性能惡化。
[0178] 應該注意，以上僅為本發明的較佳實施例而已，并非用于限定本發明的保護范圍。 凡在本發明的精神和原則之內，所作的任何修改、等同替換、改進等，均應包含在本發明的 保護范圍之內。
【主權項】
1. 一種約束條件下的機動目標跟蹤方法，其特征在于，所述方法包括以下步驟： A、 獲得機動目標的位置量測信息； B、 將所述位置量測信息轉換為笛卡爾坐標量測； C、 在一維空間建立約束坐標靜態模型，即建立機動目標在道路x，y方向上的位置關于 一維變量里程數s的表達式； D、 依據所述機動目標的運動特點設置運動模型，根據所建立的表達式建立狀態方程和 量測方程； E、 分別針對各所述運動模型，對轉換得到的笛卡爾坐標量測進行濾波，得到一維變量 里程數s及其速度的濾波結果。2. 根據權利要求1所述的方法，其特征在于，所述機動目標為道路上行駛的車輛，所述 機動目標的位置量測信息為車輛相對觀測雷達坐標系原點的距離和方位角，在所述步驟B 中，利用去偏量測轉換方法將所述位置量測信息轉換為笛卡爾坐標量測。3. 根據權利要求1所述的方法，其特征在于，在所述步驟C中，如下地建立機動目標在道 路x，y方向上的位置關于里程數s的表達式： C1:當道路段為直線段時，直線段上的任意點位置滿足表達式：，_(義1，71)為直線段起始點，（12,72)為直線段終點； C2:當道路段為圓弧段時，圓弧段上的任意點位置滿足表達式：其中，（XI，yi)為圓弧段起始點，（X2，y2)為圓弧段終點，（XQ，y〇)為圓心，圓弧起始角4. 根據權利要求3所述的方法，其特征在于，所述步驟E包括計算各運動模型對應的模 型概率的步驟，以及按模型概率針對各所述運動的濾波結果進行加權融合的步驟。5. 根據權利要求4所述的方法，其特征在于，所述步驟E的濾波包括如下的步驟： E1:將k-1時刻濾波結果相互作用初值及其方差/^^與轉換量測值拉一起作為k 時刻第j個模型的輸入值，利用公式每進行狀態一步預測，其中為狀態 轉移矩陣， E2、計算狀態一步預測協方差:其中Qk-i過程噪聲協方差矩陣； E3、計算無跡變換S采樣點狀態預測其中k是尺度參數，其數值滿足(n+K)辛〇,是均方根矩陣的第 1行或第1列，n是狀態向量的維數，對應& = .，n = 2;對應& = [^,.4.，為= 3; E4、計算S采樣點量測預測 其中hk為量測函數，由所述一維坐標約束建模給出； E5、計算量測預測私《其中是無跡變換時對均值加權計算使用的權值 E6、計算新息 <，指量測值與量測預測值的差 d毛V-丨 E7、計算量測預測協方差，即新息協方差其中，》r是無跡變換時對均值加權計算使用的權值，為量測噪聲協方差矩陣 E8、計算量測和狀態向量的交互協方差E9、確定濾波增益g my E10、進行狀態更新，獲得k時刻j模型濾波結果i4： % = +Kivi E11、進行協方差更新，獲得k時刻j模型濾波誤差協方差 P^PUiPUKif E12、模型概率更新： 在得到各個模型k時刻的輸出毛^和2^后，更新該時刻各模型對應的概率，計算對應 模型j的可能性為且對運動模型j的模型概率進行更新：E13、k時刻交互式多模型濾波的融合輸出為6.根據權利要求5所述的方法，其特征在于，還包括基于k時刻針對各運動模型的濾波 結果外推下一時刻預測結果，并根據k時刻模型概率對針對各運動模型的預測結果加權融 合，得到k+1時刻融合預測結果的步驟。
【文檔編號】G01S13/72GK106054170SQ201610339347
【公開日】2016年10月26日
【申請日】2016年5月19日
【發明人】周共健, 李可毅, 陳曦
【申請人】哈爾濱工業大學