基于約束信息的目標(biāo)定位方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及目標(biāo)跟蹤技術(shù)領(lǐng)域，具體涉及一種基于約束信息的目標(biāo)定位方法。
【背景技術(shù)】
[0002] 機(jī)動目標(biāo)跟蹤是一門涉及多學(xué)科交叉的技術(shù)，目標(biāo)定位是指利用來自若干個(gè)傳感 器的目標(biāo)觀測數(shù)據(jù)集，對目標(biāo)的運(yùn)行狀態(tài)進(jìn)行估計(jì)和預(yù)測的過程。隨著科技的不斷進(jìn)步，機(jī) 動目標(biāo)的類型越來越多，運(yùn)行速度越來越快，而僅憑傳感器的輸出很難實(shí)現(xiàn)快速跟蹤與精 準(zhǔn)定位，因此必須充分利用一切有用信息以提高目標(biāo)定位的精度。
[0003] 事實(shí)上，通過分析目標(biāo)所處的環(huán)境及運(yùn)行的軌跡往往可挖掘出有用的先驗(yàn)信息， 將先驗(yàn)信息進(jìn)行合理利用可達(dá)到提高估計(jì)精度的目的。如，當(dāng)車輛沿直線運(yùn)動時(shí)，車輛的運(yùn) 行軌跡與道路的走向保持一致，若道路的走向已預(yù)先獲知，則可間接得到有關(guān)狀態(tài)的先驗(yàn) 信息；目標(biāo)在做勻速圓周運(yùn)動時(shí)，滿足速度與加速度點(diǎn)乘為零的約束關(guān)系，利用該約束關(guān)系 可以得到比直線運(yùn)動時(shí)更好的跟蹤結(jié)果；在組合導(dǎo)航定位系統(tǒng)中，通過在不同的位置分別 安裝GPS，慣性測量單元（IMU)和CCD相機(jī)等傳感器進(jìn)行距離測量，并根據(jù)各個(gè)傳感器之間 的相對位置以精確定位。以上這些均稱為約束條件，合理利用約束條件蘊(yùn)含的約束信息，并 結(jié)合傳感器的輸出，可有效的提高狀態(tài)的估計(jì)精度，實(shí)現(xiàn)對機(jī)動目標(biāo)的精準(zhǔn)定位。
[0004] 近年來，國內(nèi)外基于約束條件的狀態(tài)估計(jì)與目標(biāo)定位方法的研究有很多，且取得 了一些石開究成果。C.Rao等人在《Constrainedlinearstateestimation-amoving horizonapproach》（Automatica, 2001，37,ppl619 - 1628) -文中以恒速運(yùn)行的機(jī)動目標(biāo) 為對象，提取出目標(biāo)速度與加速度向量始終保持正交這一關(guān)系作為已知的先驗(yàn)信息，同時(shí) 結(jié)合卡爾曼濾波的計(jì)算過程，提出了更優(yōu)的濾波算法，并論證了濾波器的漸進(jìn)無偏收斂性， 然而該方法不具有遞歸功能，不適合對機(jī)動目標(biāo)的跟蹤定位。之后，D.Simon等人在《Kalman filteringwithstateequalityconstraints))(Trans.AES, 2002, 38,pp. 128-136) 一文中 提出了基于投影（PJ)的方法，通過將卡爾曼濾波結(jié)果與約束空間相結(jié)合得到新的約束估 計(jì)，具體表現(xiàn)為解一個(gè)拉格朗日方程實(shí)現(xiàn)目標(biāo)定位，然而該方法的定位精度仍然有待提高。 Ko等人在《Stateestimationforlinearsystemswithstateequalityconstraints》 (Automatica, 2007, 43:1363 - 1368) -文中對約束系統(tǒng)的一些參數(shù)做了假設(shè)，并在此基礎(chǔ) 上推導(dǎo)出一個(gè)約束預(yù)測器，驗(yàn)證了該預(yù)測器比投影方法具有更精確的估計(jì)結(jié)果，為艦船定 位和導(dǎo)航提供了可靠的輔助監(jiān)控?cái)?shù)據(jù)，然而該方法中所做的幾個(gè)假設(shè)條件限制了該方法的 應(yīng)用范圍° 之后，Hewett等人在《Arobustnullspacemethodforlinearequality constrainedstateestimation》（Trans.SP2010, 58, 3961 - 3971) -文中提出了基于零 空間分解（NS)的定位方法，認(rèn)為系統(tǒng)的狀態(tài)向量由固定部分和隨機(jī)部分共同構(gòu)成，首先分 別得到二者的估計(jì)值，接著通過重構(gòu)得到原約束系統(tǒng)狀態(tài)的估計(jì)值，然而該方法在降階的 過程中損失了部分信息，導(dǎo)致估計(jì)精度不高，無法實(shí)現(xiàn)目標(biāo)的精準(zhǔn)定位。

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

[0005] 本發(fā)明為了克服以上不足，提供了一種不僅能實(shí)現(xiàn)機(jī)動目標(biāo)實(shí)時(shí)跟蹤定位，且可 以有效提_定位精度的基于約束彳目息的目標(biāo)定位方法。
[0006] 為了解決上述技術(shù)問題，本發(fā)明的技術(shù)方案是：一種基于約束信息的目標(biāo)定位方 法，用于對機(jī)動目標(biāo)實(shí)時(shí)跟蹤定位，包括以下步驟：
[0007]S1:約束模型建模，分別建立狀態(tài)值xk、測量值zk以及約束方程的數(shù)學(xué)表達(dá)式；
[0008]S2:系統(tǒng)降維，根據(jù)約束方程，尋找一個(gè)新變量用以表示其他變量，得到關(guān)于新變 量的降維方程，對狀態(tài)值xk的表達(dá)式進(jìn)行降維；
[0009]S3:關(guān)聯(lián)測量值zk與新變量，建立測量值zk與新變量的表達(dá)關(guān)系；
[0010]S4 :獲取測量信息，通過傳感器實(shí)時(shí)獲取機(jī)動目標(biāo)的狀態(tài)信息的測量值zk;
[0011]S5:設(shè)計(jì)遞歸濾波器，根據(jù)新變量和誤差協(xié)方差上一時(shí)刻的估計(jì)值以及測量值zk 的當(dāng)前值，設(shè)計(jì)遞歸濾波器，求出新變量和誤差協(xié)方差的當(dāng)前估計(jì)值；
[0012]S6:重構(gòu)狀態(tài)值xk和誤差協(xié)方差的當(dāng)前估計(jì)值，根據(jù)新變量的當(dāng)前值求出原始狀 態(tài)值xk和誤差協(xié)方差的當(dāng)前估計(jì)值；
[0013]S7:重復(fù)步驟S2-S6,對機(jī)動目標(biāo)進(jìn)行實(shí)時(shí)跟蹤定位。
[0014] 進(jìn)一步的，所述步驟S1中，狀態(tài)值xk和測量值zk滿足數(shù)學(xué)表達(dá)式：
[0015] xk=A.XH+rkUH+Wk-1
[0016] zk=Hkxk+vk
[0017]約束方程：Dkxk=dk
[0018] 其中，k為采樣時(shí)刻值；狀態(tài)值
【主權(quán)項(xiàng)】
1. 一種基于約束信息的目標(biāo)定位方法，用于對機(jī)動目標(biāo)實(shí)時(shí)跟蹤定位，其特征在于，包 括以下步驟： 51 :約束模型建模，分別建立狀態(tài)值xk、測量值zk以及約束方程的數(shù)學(xué)表達(dá)式； 52 :系統(tǒng)降維，根據(jù)約束方程，尋找一個(gè)新變量用以表示其他變量，得到關(guān)于新變量的 降維方程，對狀態(tài)值xk的表達(dá)式進(jìn)行降維； 53 :關(guān)聯(lián)測量值zk與新變量，建立測量值zk與新變量的表達(dá)關(guān)系； 54 :獲取測量信息，通過傳感器實(shí)時(shí)獲取機(jī)動目標(biāo)的狀態(tài)信息的測量值zk; 55 :設(shè)計(jì)遞歸濾波器，根據(jù)新變量和誤差協(xié)方差上一時(shí)刻的估計(jì)值以及測量值zk的當(dāng) 前值，設(shè)計(jì)遞歸濾波器，求出新變量和誤差協(xié)方差的當(dāng)前估計(jì)值； 56 :重構(gòu)狀態(tài)值xk和誤差協(xié)方差的當(dāng)前估計(jì)值，根據(jù)新變量的當(dāng)前值求出原始狀態(tài)值 xk和誤差協(xié)方差的當(dāng)前估計(jì)值； 57 :重復(fù)步驟S2-S6,對機(jī)動目標(biāo)進(jìn)行實(shí)時(shí)跟蹤定位。
2. 根據(jù)權(quán)利要求1所述的基于約束信息的目標(biāo)定位方法，其特征在于，所述步驟S1中， 狀態(tài)值xk和測量值zk滿足數(shù)學(xué)表達(dá)式： Xk - ^ kXk-l+ ^ kUk-l+Wk-l Zk一H kXk+Vk 約束方程：Dkxk=dk 其中，k為采樣時(shí)刻值；狀態(tài)值xfWf，包含四個(gè)狀態(tài)元素；Uh為已知 輸入項(xiàng)；過程噪聲Wk和vk為均值為零的高斯白噪聲；wk和vk的協(xié)方差分別為Uk和Rk;Ak是nXn的狀態(tài)轉(zhuǎn)移矩陣，rk是nXp的輸入矩陣，Hk是mXn的測量矩陣，Dk是sXn的行滿秩 約束矩陣，dk是sX1的約束向量，m、n、p、s均為正整數(shù)，且s〈n。
3. 根據(jù)權(quán)利要求2所述的基于約束信息的目標(biāo)定位方法，其特征在于，所述步驟S2具 體包括以下步驟： 521 :將約束矩陣Dk進(jìn)行分解，表示成巧Zfl耳T1，其中Ek為可逆矩陣，D;=吆(_,&)為￡^(_人)].為由Dk的其中s列構(gòu)成的滿秩方陣，Dkhkj)表示矩陣 Dk的第k』列，j= 1，2,…，s;代=[A. (?,A. (?, ,2…(?人)]為Dk余下的n-s列； 522 :尋找新變量y2，k，對狀態(tài)值xk的數(shù)學(xué)表達(dá)式進(jìn)行降維，令八=￡^心，對應(yīng)Dk的分塊 將yk進(jìn)行分塊表示成八，并根據(jù)約束方程用y2，k表示yu，得到關(guān)于新變 量y2，k的降維方程。
4. 根據(jù)權(quán)利要求3所述的基于約束信息的目標(biāo)定位方法，其特征在于，所述降維方程 為：
其中，4=Hr，
5.根據(jù)權(quán)利要求4所述的基于約束信息的目標(biāo)定位方法，其特征在于，所述步驟S3中， 測量值zk與新變量y2，k的表達(dá)關(guān)系為：
6.根據(jù)權(quán)利要求5所述的基于約束信息的目標(biāo)定位方法，其特征在于，所述步驟S5具 體包括以下步驟： 551 :獲得k-1時(shí)刻，即上一時(shí)刻y2, ^的估計(jì)值九與誤差協(xié)方差的估計(jì)值 ?2, k-l|k-l； 552 :根據(jù)最小二乘算法獲得y2,kk時(shí)刻，即當(dāng)前時(shí)刻的預(yù)測值)與誤差協(xié)方差當(dāng)前 時(shí)刻的預(yù)測值P2，kM; 553 :結(jié)合當(dāng)前時(shí)刻的測量值zk，得到y(tǒng)2,k的當(dāng)前估計(jì)值和誤差協(xié)方差的當(dāng)前估計(jì) 值P2，k|k。
7.根據(jù)權(quán)利要求6所述的基于約束信息的目標(biāo)定位方法，其特征在于，所述步驟S52中 根據(jù)最小二乘算法獲得的P2，k|lrl分別為：
8.根據(jù)權(quán)利要求7所述的基于約束信息的目標(biāo)定位方法，其特征在于，所述步驟S53中 兔鄧和P2，k|k分別為：
其中，
9. 根據(jù)權(quán)利要求7所述的基于約束信息的目標(biāo)定位方法，其特征在于，所述步驟S6具 體包括： 561 :重構(gòu)得到y(tǒng)k的當(dāng)前估計(jì)值jV和誤差協(xié)方差的當(dāng)前估計(jì)值Of; 562 :根據(jù)凡和'=盡墦>拉得到原始狀態(tài)值xk的當(dāng)前估計(jì)值毛f和原始誤差 協(xié)方差的當(dāng)前估計(jì)值Pk|k。
10. 根據(jù)權(quán)利要求9所述的基于約束信息的目標(biāo)定位方法，其特征在于，所述步驟S61 中夕#和分別為：
A
【專利摘要】本發(fā)明公開了一種基于約束信息的目標(biāo)定位方法，用于對機(jī)動目標(biāo)實(shí)時(shí)跟蹤定位，包括以下步驟：首先進(jìn)行約束模型建模；接著進(jìn)行系統(tǒng)降維和關(guān)聯(lián)測量值與新變量；其次，獲取測量信息，并設(shè)計(jì)遞歸濾波器；然后重構(gòu)狀態(tài)值和誤差協(xié)方差的當(dāng)前估計(jì)值；重復(fù)上述步驟對機(jī)動目標(biāo)進(jìn)行實(shí)時(shí)跟蹤定位。本發(fā)明通過系統(tǒng)降維和關(guān)聯(lián)測量值與新變量，簡化了原始數(shù)學(xué)模型和計(jì)算復(fù)雜度，降低計(jì)算量，利于得到更優(yōu)的估計(jì)效果，提高了定位精度；通過設(shè)計(jì)遞歸濾波器，根據(jù)新變量和誤差協(xié)方差上一時(shí)刻的估計(jì)值以及測量值的當(dāng)前值，求出新變量和誤差協(xié)方差的當(dāng)前估計(jì)值，有效實(shí)現(xiàn)對機(jī)動目標(biāo)的實(shí)時(shí)跟蹤定位，且無需保留更前面的測量值與約束信息，節(jié)省了存儲空間。
【IPC分類】G01S19-48
【公開號】CN104614751
【申請?zhí)枴緾N201510049788
【發(fā)明人】文傳博
【申請人】上海電機(jī)學(xué)院
【公開日】2015年5月13日
【申請日】2015年1月30日