專利名稱：：車載導(dǎo)航系統(tǒng)誤差協(xié)方差陣部分參數(shù)自適應(yīng)調(diào)節(jié)的方法
技術(shù)領(lǐng)域：
：本發(fā)明屬于車載導(dǎo)航
技術(shù)領(lǐng)域：
,具體涉及一種車載衛(wèi)星/慣性組合導(dǎo)航系統(tǒng)中卡爾曼濾波器誤差協(xié)方差陣Q，R部分參數(shù)自適應(yīng)調(diào)節(jié)的方法，用于提高車載導(dǎo)航定位精度。
背景技術(shù)：
：卡爾曼(Kalman)濾波是現(xiàn)代組合導(dǎo)航系統(tǒng)中應(yīng)用十分廣泛的數(shù)據(jù)融合技術(shù)，在準(zhǔn)確獲得不同測量系統(tǒng)誤差與系統(tǒng)狀態(tài)估計(jì)誤差的信息的基礎(chǔ)上，依據(jù)各種物理量之間的狀態(tài)轉(zhuǎn)移關(guān)系，合理利用不同測量系統(tǒng)的測量信息，能夠獲得對待估計(jì)狀態(tài)的最優(yōu)估計(jì)。在Kalman濾波中，狀態(tài)誤差協(xié)方差陣Q、觀測誤差協(xié)方差陣R的準(zhǔn)確程度對估計(jì)結(jié)果精度的影響很大，是Kalman濾波的重要參數(shù)。實(shí)際應(yīng)用中Q-狀態(tài)誤差協(xié)方差陣、R-觀測誤差協(xié)方差陣參數(shù)的選擇多采用經(jīng)驗(yàn)性的試湊方法以及自適應(yīng)濾波法。經(jīng)驗(yàn)性試湊法依據(jù)測量設(shè)備的測量特性、組合濾波能力確定Q、R初步范圍，并通過調(diào)整Q、R的仿真分析給出較為合適的協(xié)方差陣，工作量較大且不能適應(yīng)觀測精度變化的需要；現(xiàn)有的自適應(yīng)濾波方法雖然具有Q、R的自適應(yīng)調(diào)節(jié)能力，但其調(diào)節(jié)思想是從系統(tǒng)濾波方程結(jié)構(gòu)關(guān)系出發(fā)，沒有充分考慮組合濾波系統(tǒng)中不同測量手段的測量特性進(jìn)行協(xié)方差陣的直接估計(jì)，影響了kalman濾波的精度。
發(fā)明內(nèi)容本發(fā)明的目的是為了解決導(dǎo)航系統(tǒng)卡爾曼(kalman)濾波方法中對誤差協(xié)方差陣參數(shù)進(jìn)行自適應(yīng)調(diào)節(jié)的問題，提供了一種車載導(dǎo)航系統(tǒng)誤差協(xié)方差陣部分參數(shù)自適應(yīng)調(diào)節(jié)的方法，該方法的主要思想是充分考慮GPS、里程儀的測量與誤差性質(zhì)，構(gòu)造了一種可以有效反映GPS測量誤差大小的估計(jì)方法，利用短時測量精度較高的里程儀信息對相對測量精度較低的GPS誤差進(jìn)行'估計(jì)，實(shí)現(xiàn)對GPS測量誤差、濾波后車輛位置誤差自適應(yīng)估計(jì)，并在此^上合理自適應(yīng)修正Q、R，從而達(dá)到自適應(yīng)濾波的目的。本發(fā)明提供的自適應(yīng)調(diào)節(jié)方法是建立在kalman濾波的基礎(chǔ)上進(jìn)行的，在不考慮GPS系統(tǒng)誤差的情況下僅對隨機(jī)誤差進(jìn)行估計(jì)。利用GPS直接測量信息，依據(jù)相鄰時刻的位置差可以獲得每秒內(nèi)車輛行駛的位移；里程儀測量的信息也可以獲得每秒內(nèi)車輛行駛的位移，即航位推算(DR);因此依據(jù)這兩種測量方法構(gòu)造相同時間段內(nèi)每秒車輛行駛的位移序列具有很強(qiáng)的可比性?？紤]到車輛的機(jī)動性，一般情況下，短時間內(nèi)車輛行駛的路線大都為直線，并且航位推算(DR)的相對定位精度較高，這樣可以利用DR的位置信息作為參考信息，使GPS每秒的位移量與DR每秒的位移量作差可獲得GPS位置信息的觀測誤差，構(gòu)造一段時6間內(nèi)的觀測誤差序列，建立觀測i吳差與誤差協(xié)方差陣之間的關(guān)系，從而可以實(shí)現(xiàn)對誤差協(xié)方差陣進(jìn)行估計(jì)調(diào)節(jié)。觀測誤差序列的構(gòu)造利用的是軌跡的相對位置，因此不需要考慮車輛行駛的初始航向。本發(fā)明方法主要采用了基于一段時間內(nèi)軌跡的均值和方差的統(tǒng)計(jì)特性構(gòu)建了GPS軌跡的光滑程度簡稱光滑度，用于判斷是否需要調(diào)節(jié)誤差協(xié)方差陣，利用GPS每秒的位移量與DR每秒的位移量構(gòu)造觀測噪聲序列，建立了此噪聲序列與誤差協(xié)方差陣之間的關(guān)系，使得誤差協(xié)方差陣在濾波系統(tǒng)中跟隨噪聲的變化而動態(tài)調(diào)節(jié)，提高了車載定位精度，與選取Q，R為經(jīng)驗(yàn)固定值相比定位精度提高了7%以上。主要的技術(shù)方案分為四個模塊，一是對車載導(dǎo)航系統(tǒng)進(jìn)行數(shù)學(xué)建模，采用的是常用的kalman濾波的方法；二是對系統(tǒng)濾波，獲得狀態(tài)信息；三是對GPS輸出的軌跡進(jìn)行光滑度的判定，根據(jù)短時間內(nèi)GPS的定位誤差均值和方差的統(tǒng)計(jì)特性來判定軌跡的光滑程度；四是依據(jù)第三個模塊得出的結(jié)果，構(gòu)建GPS光滑度與kalman濾波誤差協(xié)方差陣之間的關(guān)系，對Q，R誤差協(xié)方差陣進(jìn)行自適應(yīng)調(diào)節(jié)，達(dá)到實(shí)時更新的效果。具體過程以及相應(yīng)的步驟如下所示步驟一、建立車載導(dǎo)航系統(tǒng)的數(shù)學(xué)模型根據(jù)車輛導(dǎo)航系統(tǒng)的實(shí)際數(shù)據(jù)情況，選取GPS北向距離iV"GPS東向距離A、GPS航向y、GPS速度v、里程儀的刻度因子S,陀螺角速率^^)作為狀態(tài)變量，構(gòu)建Kalman濾波模型狀態(tài)方程，具體的數(shù)學(xué)模型如下狀態(tài)向量Z:<formula>formulaseeoriginaldocumentpage7</formula>狀態(tài)方程為<formula>formulaseeoriginaldocumentpage7</formula>狀態(tài)誤差向量為<formula>formulaseeoriginaldocumentpage7</formula>GPS/DR導(dǎo)航系統(tǒng)的狀態(tài)方程組如下<formula>formulaseeoriginaldocumentpage7</formula>l表示當(dāng)前時刻，A+l為下一時刻，W，W2,W3，W4，W5,W6分別表示各個狀態(tài)變量的狀態(tài)誤差，7為周期，本發(fā)明方法取為ls。狀態(tài)轉(zhuǎn)移矩陣經(jīng)過泰勒展開得<formula>formulaseeoriginaldocumentpage8</formula>w表示為狀態(tài)變量估計(jì)值的狀態(tài)誤差向量。選取GPS北向距離A^、GPS東向距離五^、GPS航向y^、GPS速度v，d，陀螺角速率^^作為觀測變量，構(gòu)建Kalman濾波模型觀測方程，具體的數(shù)學(xué)模型如下觀測向量/::Z-[AT^￡薩^爐v，rf^,。]觀測方程為+1)=//(QxO:)+觀測誤差向量為eT("=[eie2e3e4e5]GPS/DR導(dǎo)航系統(tǒng)的觀測方程組如下K*)=，)+e2e,，e2，e3,e4，&分別表示各個觀測變量的觀測誤差，附(A)表示的是當(dāng)前周期內(nèi)里程儀累計(jì)的脈沖數(shù)。觀測矩陣為.<formula>formulaseeoriginaldocumentpage8</formula>觀測誤差協(xié)方差陣:<formula>formulaseeoriginaldocumentpage9</formula><table>tableseeoriginaldocumentpage9</column></row><table>依據(jù)上述建立的數(shù)學(xué)豐莫型進(jìn)入步驟二進(jìn)行系統(tǒng)濾波，系統(tǒng)濾波的輸出值即為狀態(tài)變量。針對此車載導(dǎo)航系統(tǒng)分析，本發(fā)明僅對狀態(tài)誤差協(xié)方差陣<和觀測誤差協(xié)方差陣/中的《、《22、&、"22進(jìn)行自適應(yīng)調(diào)節(jié)。步驟二、系統(tǒng)濾波。依據(jù)步驟一建立的數(shù)學(xué)模型對車輛導(dǎo)航系統(tǒng)進(jìn)行系統(tǒng)濾波，濾波是一個循環(huán)的過程，以周期T(本發(fā)明方法取為lS)為單位，根據(jù)上一時刻對當(dāng)前時刻的估計(jì)以及當(dāng)前時刻的觀測變量對當(dāng)前時刻的狀態(tài)變量進(jìn)行濾波，獲得當(dāng)前時刻更為精確的狀態(tài)變量，即為系統(tǒng)濾波的輸出值。根據(jù)所獲得的觀測變量和狀態(tài)變量對仏,、&2、&、^進(jìn)行調(diào)節(jié)。步驟三、GPS光滑度判定。光滑度是指輸出軌跡的光滑程度。以短時間內(nèi)航位推算(DR)軌跡作為理想軌跡對GPS軌跡光滑度進(jìn)行估計(jì)判定。具體的判定步驟以及判定的方法如下1)設(shè)定軌跡的時間長度。車輛的運(yùn)動軌跡隨著時間的變化而變化，把運(yùn)動軌跡根據(jù)相等的時間長度分成很多不同的軌跡段，設(shè)定時間長度H，一般取15s20s,并且不同軌跡段之間不可有時間重疊。時間長度可以根據(jù)車輛行駛的路況進(jìn)行調(diào)整。2)DR相對運(yùn)動序列A的構(gòu)造。依據(jù)步驟l)中設(shè)定的時間長度H,利用DR解箅公式獲得每段軌跡內(nèi)相鄰兩點(diǎn)之間的距離丄丄=*一1)+/2S(A)表示里程儀的刻度因素，w(A:-l)表示前一周期內(nèi)里程儀累計(jì)的脈沖數(shù)，w(Q表示的是當(dāng)前周期內(nèi)里程儀累計(jì)的脈沖數(shù)；根據(jù)獲得的相鄰兩點(diǎn)之間的距離丄即步驟1)中所設(shè)時間長度H內(nèi)每秒的移動直線距離，構(gòu)造序列A。3)GPS相對運(yùn)動序列B的構(gòu)造。依據(jù)步驟l)中設(shè)定的時間長度H,利用GPS相鄰時刻輸出的位置信息做差，求出每一秒的位移，得出相鄰兩點(diǎn)間距離，構(gòu)造序列B。4)構(gòu)造距離差序列C。將序列A和序列B相減得到一個距離差序列C，C中各元素為相鄰兩秒中GPS測量位移與DR測量位移之差，距離差序列C中的元素有正負(fù)之分。5)誤差統(tǒng)計(jì)。距離差序列C進(jìn)行統(tǒng)計(jì)，獲得一定時間段內(nèi)如15s內(nèi)由步驟4)獲得的距離差序列C內(nèi)元素的均值與方差。6)設(shè)定比較的閾值判斷光滑度。設(shè)定均值閾值為510，方差閾值為1520,如果一定時間段H內(nèi)距離差序列C中元素的均值和方差均小于設(shè)定的均值閾值和方差閾值，則判定這段時間H內(nèi)的運(yùn)動軌跡是光滑的，則不需要調(diào)節(jié)&、&2、&、"22四個參數(shù)，否則認(rèn)為是不光滑的，進(jìn)入步驟四。步驟四、依據(jù)GPS光滑度調(diào)節(jié)協(xié)方差陣。距離差序列C的均值代表了GPS定位誤差的均值、距離差序列C的方差代表了GPS定位誤差的方差，而在濾波系統(tǒng)中，觀測誤差協(xié)方差陣R和狀態(tài)誤差方差協(xié)陣Q也反映的是誤差的信息，因此可以根據(jù)獲得的距離差統(tǒng)計(jì)特性對Kalman濾波中的協(xié)方差陣進(jìn)行調(diào)節(jié)更新。濾波中的協(xié)方差陣也包括了航向、速度等信息，由于光滑度的判定只是就距離而言的，因此光滑度的判定僅調(diào)節(jié)協(xié)方差陣中的四個參數(shù)，東向距離狀態(tài)誤差&、東向距離觀測誤差&、北向距離狀態(tài)誤差《22、北向距離觀測誤差&。所述調(diào)節(jié)協(xié)方差陣的步驟如下1)統(tǒng)計(jì)特性的確定。一個距離差序列C的均值與方差數(shù)值不能作為統(tǒng)計(jì)特性看待，而應(yīng)對連續(xù)多個序列的統(tǒng)計(jì)特性綜合考慮后才能夠給出近似的統(tǒng)計(jì)特性，因此通常情況(水平坐標(biāo)精度因子-HDOP數(shù)值變化較小的情況下，根據(jù)GPS的測量性質(zhì)在HDOP波動幅度大于0.5的情況下)下需要3次序列統(tǒng)計(jì)后結(jié)果都比較近似才可以認(rèn)為具有一定的統(tǒng)計(jì)意義。*根據(jù)GPS的測量性質(zhì)在HDOP波動幅度大于0.5的情況下POS加旭加w=(POS鵬加—n+POS咖加—n+1+POS加咖—n+2)/3POS咖，=(POS孤n+POS—+POSvarn+2)/3POS^"表示序列差的均值，POSmeann、POSmemn+1、pos^n+2表示連續(xù)三個距離差序列c的均值。POS皿n。w表示序列差的方差，POSvarn、POSwn+1、POSv"+2表示連續(xù)三個距離差序列c的方差。*根據(jù)GPS的測量性質(zhì)在HDOP波動幅度小于0.5的情況下在保證一次序列都是針對同一級別HDOP統(tǒng)計(jì)的結(jié)果情況下，可以不進(jìn)行3次序列的平均，而使用當(dāng)前序列的方差或者均值數(shù)據(jù)，從而使得系統(tǒng)濾波性能可以快速跟蹤噪聲的變化。2)協(xié)方差陣元素的更新。根據(jù)GPS定位誤差情況，需要更新的參數(shù)主要有9、&2、r、r22，由于在步驟三的光滑度判定部分獲得是均值和方差數(shù)據(jù)，并沒有將其分解到東向、北向分別考慮，因此依據(jù)光滑度判定對協(xié)方差陣中元素進(jìn)行調(diào)整時，將東向、北向誤差簡化認(rèn)為是具有相同的統(tǒng)計(jì)特性。在kalman濾波系統(tǒng)中，協(xié)方差陣的調(diào)節(jié)主要用于對濾波的最優(yōu)增益矩陣K,估計(jì)誤差方差陣P，以及狀態(tài)向量的估計(jì)，濾波方程的解如下雞+llit+l)"Ot+ll)t)+綠+l)[:KA:+l)-釋+l錄+l網(wǎng)11A:)—("1，I"+1)=+11(A+(A:++11A)i/r+1)+及(A+l)]一1P("11"=維+1,A:)尸(JtIA)/+1,/k)+鵬単+11it+1)=綠+11ifc)-綠++1)，+11"i^+l^+l)表示第k+l時刻對x("l)的估計(jì)；i(A:+llA:)表示第k時刻對:c(^:+l)的估計(jì)值；《("l)表示第k+1時刻的最優(yōu)增益矩陣;y("l)表示第k+1時刻的觀測向量；//("l)表示k+l時刻的觀測矩陣；-("l，"表示第k時刻向第k+l時刻轉(zhuǎn)換的狀態(tài)轉(zhuǎn)移矩陣；尸(A+1I"表示估計(jì)誤差方差陣，A:+l^表示第k時刻對第k+l時刻的估計(jì)，A:^表示第k時刻對第k時刻的估計(jì)；i(A+l)表示的是第k+l時刻觀測誤差協(xié)方差陣，Q("表示的11是第k時刻的狀態(tài)誤差協(xié)方差陣。本發(fā)明方法即是對不同時刻的g(W和及("進(jìn)行自適應(yīng)調(diào)節(jié)，從而實(shí)時調(diào)節(jié)誤差估計(jì)方差陣以及最優(yōu)增益矩陣，如公式所示，進(jìn)而達(dá)到對狀態(tài)進(jìn)行精確估計(jì)的效果。本發(fā)明方法采用的兩個序列的差測出的誤差信息可以描述及(Q的大小，及W物理含義上表示的觀測誤差信息，通過對POS，n。w的統(tǒng)計(jì)，可以獲得實(shí)際觀測誤差的值。本發(fā)明方法采用了GPS位置的位置差信息，對于GPS位置系統(tǒng)誤差不能做出適當(dāng)?shù)墓烙?jì)；在實(shí)際的導(dǎo)航系統(tǒng)中，由于構(gòu)建模型的不準(zhǔn)確，也會帶來誤差，根據(jù)衰減記憶濾波思想，可以通過增大觀測誤差的方式抑制模型誤差的影響；同時由于統(tǒng)計(jì)樣本數(shù)量少，觀測誤差不能完全保證誤差的統(tǒng)計(jì)信息；基于以上三種考慮，可知獲得的觀測誤差信息比實(shí)際的誤差要小，在Kalman濾波過程中需要對觀測噪聲方差進(jìn)行必要的放大，對于R中的&，^給予以下設(shè)定a的取值范圍對精度的影響存在著一定的變化趨勢，"的取值為0.5~3時逐漸變化時，定位精度的提高程度是從小逐漸變大，再逐漸變小的一個過程，a存在著一個真值，對《的取值偏離真值越多，誤差越大，針對本發(fā)明所采用的Kalman濾波模型，當(dāng)"取為12時能夠獲得相比其他值時更髙的定位精度。對于不同的模型也存在這樣一個變化的趨勢，只是依據(jù)不同的模型，"取值的大小有差異。通過大量的系統(tǒng)實(shí)際運(yùn)行驗(yàn)證，也獲得了相同的結(jié)論。Kalman濾波方程中容易推導(dǎo)出五(岸IA:-,-1》=//(A;)単IA:-r+単)在濾波估計(jì)較準(zhǔn)確的情況下，E(i^l"l)y(A:l"l))可以近似為i("。在濾波估計(jì)中位置信息即是狀態(tài)變量，也是觀測變量，在濾波模型準(zhǔn)確時，對狀態(tài)變量的估計(jì)也準(zhǔn)確，此時P陣較小。在kalman濾波的解算公式中可以得知，+11"=雞+1,A)岸I(A:+1,"+洲可知估計(jì)誤差協(xié)方差陣P與Q存在以上的數(shù)學(xué)關(guān)系，而根據(jù)￡CKAI"I)/"IA—1))=//("尸(A:I(A)+i("同樣可以知道Q與iLI)/(AI&-1))也存在一定的數(shù)學(xué)關(guān)系，IH)/(A:IA:-l))可以近似為及("，本發(fā)明方法的及W根據(jù)POS"。w的統(tǒng)計(jì)信息可以獲得，因此本發(fā)明對于Q中的仏p《22給予以下的設(shè)定Q反映的是狀態(tài)誤差信息，在模型準(zhǔn)確的情況下，值較小，針對本發(fā)明所釆用的濾波模'型，對"進(jìn)行了反復(fù)的調(diào)試，得出當(dāng)y8取為10%40%時，均能提高系統(tǒng)的定位精度，相比Q，R取經(jīng)驗(yàn)值時的定位精度高，自適應(yīng)調(diào)節(jié)能力強(qiáng)。對于不同的濾波模型，仏尸也不同，/的取值范圍也因似情況而定。步驟l)統(tǒng)計(jì)特性的確定可以得到實(shí)時的戶OSvM,的值，運(yùn)用以上的兩個公式從而實(shí)現(xiàn)對于協(xié)方差陣Q、R中的參數(shù)&、《22、^、^的自適應(yīng)調(diào)節(jié)。本發(fā)明的優(yōu)點(diǎn)在于(1)本發(fā)明方法實(shí)現(xiàn)簡單，易于操作；(2)該方法可以對誤差協(xié)方差陣Q,R參數(shù)進(jìn)行自適應(yīng)調(diào)節(jié)；(3)減小試湊法的工作量；(4)提高導(dǎo)航系統(tǒng)的定位精度，與直接濾波相比提高7%。圖1是本發(fā)明自適應(yīng)調(diào)節(jié)方法的流程圖；圖2是本發(fā)明的GPS光滑度判定示意圖；圖3a是本發(fā)明的仿真實(shí)驗(yàn)軌跡1結(jié)果圖；圖3b是本發(fā)明的仿真實(shí)驗(yàn)軌跡2結(jié)果圖；圖3c是本發(fā)明的仿真實(shí)驗(yàn)軌跡3結(jié)果圖；圖3d仿真實(shí)驗(yàn)位置誤差結(jié)果圖。具體實(shí)施例方式下面將結(jié)合附圖和實(shí)施例對本發(fā)明作進(jìn)一步的詳細(xì)說明。本發(fā)明的車載導(dǎo)航系統(tǒng)誤差協(xié)方差陣參數(shù)自適應(yīng)調(diào)節(jié)的方法的流程如附圖1所示，具體實(shí)現(xiàn)過程如下步驟一、建立車載導(dǎo)航系統(tǒng)的數(shù)學(xué)模型。、根據(jù)車輛導(dǎo)航系統(tǒng)的的實(shí)際數(shù)據(jù)情況，選取GPS北向距離A^、GPS東向距離A、GPS航向y、GPS速度"里程儀的刻度因子S,陀螺角速率^XA:)作為狀態(tài)變量，構(gòu)建Kalman濾波模型狀態(tài)方程，具體的數(shù)學(xué)模型如下狀態(tài)向量Z:Z-[iV,wvS一a]狀態(tài)方程為x(A:+l)-0(^+l，&)x(A;)+wO)狀態(tài)誤差向量為m^(A:)-[m^w2w3w4w5w6〗GPS/DR導(dǎo)航系統(tǒng)的狀態(tài)方程組如下~(it+1)=乂(it)+v,cos,))+&(A:+1)=五,(A:)+v,sin,))+w2y(A:+l)-V^)+7y炒(A:)+W3v(&+l)=v(*)+w4A表示當(dāng)前時刻，A+l為下一時刻，v^，w2,w3，w4，w5，w分別表示各個狀態(tài)變量的狀態(tài)誤差，r為周期，本發(fā)明方法取為ls。狀態(tài)轉(zhuǎn)移矩陣經(jīng)過泰勒展開得<formula>formulaseeoriginaldocumentpage14</formula>狀態(tài)誤差協(xié)方差陣:<formula>formulaseeoriginaldocumentpage14</formula>w表示為狀態(tài)變量估計(jì)值的狀態(tài)誤差向量。選取GPS北向距離A^、GPS東向距離E^、GPS航向y^、GPS速度v^，陀螺角速率^^作為觀測變量，構(gòu)建Kalman濾波模型觀測方程，具體的數(shù)學(xué)模型如下觀測向量/:W諷v,—觀測方程為+1)=//(A)x(A:)+觀測誤差向量為^(^=[《e2e3e4e5]GPS/DR導(dǎo)航系統(tǒng)的觀測方程組如下KA0=，+e2A,e2，e3，e4,6分別表示各個觀測變暈的觀測誤差，m(W表示的是當(dāng)前周期內(nèi)里程儀累計(jì)的脈沖數(shù)。經(jīng)過泰勒展開10000001000000100000000000001觀測誤差協(xié)方差陣:0000000000&00000,4400000尸55e表示為觀測變量估計(jì)值的觀測誤差。根據(jù)狀態(tài)與觀測向量的定義，狀態(tài)協(xié)方差陣的說明如下表1,表2所示依據(jù)上述建立的數(shù)學(xué)模型進(jìn)入步驟二進(jìn)行系統(tǒng)濾波，系統(tǒng)濾波的輸出值即為狀態(tài)變量。針對此車載導(dǎo)航系統(tǒng)分析，本發(fā)明僅對狀態(tài)誤差協(xié)方差陣0和觀測誤差協(xié)方差陣i中的&、《22、&、^進(jìn)行自適應(yīng)調(diào)節(jié)。步驟二、依據(jù)步驟一建立的數(shù)學(xué)模型進(jìn)行系統(tǒng)濾波。依據(jù)步驟一建立的數(shù)學(xué)模型對車輛導(dǎo)航系統(tǒng)進(jìn)行系統(tǒng)濾波，濾波是一個循環(huán)的過程，以周期T(本發(fā)明方法取為ls)為單位，根據(jù)上一時刻對當(dāng)前時刻的估計(jì)以及當(dāng)前時刻的觀測變量對當(dāng)前時刻的狀態(tài)變量進(jìn)行濾波，獲得當(dāng)前時刻更為精確的狀態(tài)變量，作為系統(tǒng)濾波的輸出值。根據(jù)所獲得的觀測變量和狀態(tài)變量對。,/進(jìn)行調(diào)節(jié)。步驟三、GPS光滑度判定。光滑度是指輸出軌跡的光滑程度。以短時間內(nèi)航位推算(DR)軌跡作為理想軌跡對GPS軌跡光滑度進(jìn)行估計(jì)判定。具體的判定步驟以及判定的方法如下1)設(shè)定軌跡的時間長度。車輛的運(yùn)動軌跡隨著時間的變化而變化，把運(yùn)動軌跡根據(jù)相等的時間長度分成很多不同的軌跡段，設(shè)定時間長度H,—般取15s20s,并且不同軌跡段之間不可有時間重疊。時間長度可以根據(jù)車輛行駛的路況進(jìn)行調(diào)整。2)DR相對運(yùn)動序列A的構(gòu)造。依據(jù)步驟l)中設(shè)定的時間長度，利用DR解算公式獲得每段軌跡內(nèi)相鄰兩點(diǎn)之間的距離丄丄=柳*—1)+w(A:))/215S("表示里程儀的刻度因素，w(^:-l)表示前一時刻里程儀的脈沖數(shù)，w("表示當(dāng)前周期內(nèi)里程儀累計(jì)的脈沖數(shù)，依據(jù)以上公式構(gòu)造序列A，A中的每個元素表示一個周期Is內(nèi)車輛行駛的位移量。3)GPS相對運(yùn)動序列B的構(gòu)造。依據(jù)步驟l)中設(shè)定的時間長度，利用GPS相鄰時刻輸出的位置信息做差，求出每一秒的位移，得出相鄰兩點(diǎn)間距離，構(gòu)造序列B。4)構(gòu)造距離差序列C。將序列A和序列B相減得到一個距離差序列C，獲得相鄰兩秒中GPS測量位移與DR測量位移之差，距離差序列C中的元素有正負(fù)之分。5)誤差統(tǒng)計(jì)。對距離差序列C進(jìn)行統(tǒng)計(jì)，獲得一定時間段內(nèi)如15s內(nèi)由步驟4)獲得的距離差序列C內(nèi)元素的均值與方差。6)設(shè)定比較的閾值判斷光滑度。設(shè)定均值閾值為510，方差閾值為1520，如果一定時間段H內(nèi)如15s內(nèi)的距離差序列C中元素的均值和方差均小于設(shè)定的均值閾值和方差閾值,則判定這段時間段H內(nèi)的運(yùn)動軌跡是光滑的，則不需要調(diào)節(jié)《、《22、〃、^四個參數(shù)，否則認(rèn)為是不光滑的，進(jìn)入步驟四。示意說明圖如圖2所示，標(biāo)志為T的點(diǎn)代表的是陀螺、里程儀獲得的位置信息，標(biāo)志為G的點(diǎn)代表的是GPS的定位信息，箭頭表示車輛行駛的方向。從示意圖中可以看出，由陀螺和里程儀獲得的位置信息較為平穩(wěn)，位置的波動較小，輸出的軌跡與GPS定位的軌跡相比較為光滑。這樣短時間內(nèi)可以以陀螺和里程儀獲得的軌跡作為參照，而GPS獲得的軌跡與之進(jìn)行比較，得出GPS定位的光滑度。步驟四、依據(jù)GPS光滑度調(diào)節(jié)協(xié)方差陣。在不考慮GPS定位系統(tǒng)誤差的情況下，距離差序列C的均值代表了GPS定位誤差的均值、距離差序列C的方差代表了GPS定位誤差的方差，而在濾波系統(tǒng)中，觀測誤差協(xié)方差陣R和狀態(tài)誤差協(xié)方差陣Q也反映的是誤差的信息，因此可以根據(jù)獲得的距離差統(tǒng)計(jì)特性對Kalman濾波中的協(xié)方差陣進(jìn)行調(diào)節(jié)更新。濾波中的協(xié)方差陣也包括了航向，速度等信息，由于光滑度的判定只是就距離而言的，因此光滑度的判定僅調(diào)節(jié)協(xié)方差陣中的四個參數(shù)，東向距離狀態(tài)誤差&、東向距離觀測誤差&、北向距離狀態(tài)誤差《22、北向距離觀測誤差&。所述調(diào)節(jié)協(xié)方差陣的步驟如下1)統(tǒng)計(jì)特性的確定。一個距離差序列C的均值與方差數(shù)值不能作為統(tǒng)計(jì)特性看待，而應(yīng)對連續(xù)多個序列的統(tǒng)計(jì)特性綜合考慮后才能夠給出近似的統(tǒng)計(jì)特性，因此通常情況(水平坐標(biāo)精度因子-HDOP數(shù)值變化較小的情況下，根據(jù)GPS的測量性質(zhì)在HDOP波動幅度大于0.5的情況下)下需要3次序列統(tǒng)計(jì)后結(jié)果都比較近似才可以認(rèn)為具有一定的統(tǒng)計(jì)意義。*根據(jù)GPS的測量性質(zhì)在HDOP波動幅度大于0.5的情況下POS—w=(POSmeann+POS，n+1十POS，n+2)/3POS=(POS做一n+POS—POS腿"表示序列差的均值POS^』、POSmeann+1、POSm咖n+2表示連續(xù)三個距離差序列c的均值。POS，n。w表示序列差的方差，POS，n、POSwn+1、POS^n+2表示連續(xù)三個距離差序列c的方差。*根據(jù)GPS的測量性質(zhì)在HDOP波動幅度小于0.5的情況下在保證一次序列都是針對同一級別HDOP統(tǒng)計(jì)的結(jié)果情況下，可以不進(jìn)行3次序列的平均，而使用當(dāng)前序列的方差或者均值數(shù)據(jù)，從而使得系統(tǒng)濾波性能可以快速跟蹤噪聲的變化。2)協(xié)方差陣元素的更新。根據(jù)GPS定位誤差情況需要更新的參數(shù)主要有^、《22、&、^,'由于在步驟三的光滑度判定部分獲得是均值和方差數(shù)據(jù)，并沒有將其分解到東向、北向分別考慮，因此依據(jù)光滑度判定對協(xié)方差陣中元素進(jìn)行調(diào)整時，將東向、北向誤差簡化認(rèn)為是具有相同的統(tǒng)計(jì)特性。在kalman濾波系統(tǒng)中，協(xié)方差陣的調(diào)節(jié)主要用于對濾波的最優(yōu)增益矩陣K,估計(jì)誤差方差陣P,以及狀態(tài)向量的估計(jì)，濾波方程的解如下単+11A:+1)=雄+11A:)+単++1)-//(A:+l)単+1網(wǎng)+1)=+11yt)/fr(A:+1)[//(A:+l)戶(it+11+1)++1)]-1単+11A:)=維+1，I，r(A:+1,A)+,+11A+1)=戶(A+11"—+1)//(A+l)P(矢+11A:)i(A:+llA:+l)表示第k+l時刻對+1)的估計(jì)；i(A+11A)表示第k時刻對1)的估計(jì)值；《(A:+1)表示第k+1時刻的最優(yōu)增益矩陣；y(A+l)表示第k+1時刻的觀測向量；H("l)表示k+1時刻的觀測矩陣；-(A:+U)表示第k時刻向第k+l時刻轉(zhuǎn)換的狀態(tài)轉(zhuǎn)移矩陣；P(A:+11A:)表示估計(jì)誤差方差陣，A+ll&表示第k時刻對第k+1時刻的估計(jì),1|&表示第k時刻對第k時刻的估計(jì)；及("l)表示的是第k+l時刻觀測誤差協(xié)方差陣，2(A)表示的是第k時刻的狀態(tài)誤差協(xié)方差陣。本發(fā)明方法即是對不同時刻的。(A)和及("進(jìn)行自適應(yīng)調(diào)節(jié)，從而實(shí)時調(diào)節(jié)i吳差估計(jì)方差陣以及最優(yōu)增益矩陣，如公式所示，進(jìn)而達(dá)到對狀態(tài)進(jìn)行精確估計(jì)的效果。本發(fā)明方法釆用的兩個序列的差測出的誤差信息可以描述及(先)的大小，及("物理含義上表示的觀測誤差信息，通過對POS^^的統(tǒng)計(jì)，可以獲得實(shí)際觀測誤差的值。本發(fā)明方法17釆用了GPS位置的位置差信息，對于GPS位置系統(tǒng)誤差不能做出適當(dāng)?shù)墓烙?jì)；在實(shí)際的導(dǎo)航系統(tǒng)中，由于構(gòu)建模型的不準(zhǔn)確，也會帶來誤差，根據(jù)衰減記憶濾波思想，可以通過增大觀測誤差的方式抑制模型誤差的影響；同時由于統(tǒng)計(jì)樣本數(shù)量少，觀測誤差不能完全保證誤差的統(tǒng)計(jì)信息；基于以上三種考慮，可知獲得的觀測誤差信息比實(shí)際的誤差要小，在Kalman濾波過程中需要對觀測噪聲方差進(jìn)行必要的放大，對于R中的&，^給予以下設(shè)定"的取值范圍對精度的影響存在著一定的變化趨勢，a的取值為0.5~3時逐漸變化時，定位精度的提高程度是從小逐漸變大，再逐漸變小的一個過程，"存在著一個真值，對a的取值偏離真值越多，誤差越大，針對本發(fā)明所采用的Kalman濾波模型，當(dāng)"取為12時能夠獲得相比其他值時更高的定位精度。對于不同的模型也存在這樣一個變化的趨勢，只是依據(jù)不同的模型，"取值的大小有差異。通過大量的系統(tǒng)實(shí)際運(yùn)行驗(yàn)證，也獲得了相同的結(jié)論。Kalman濾波方程中容易推導(dǎo)出IA—1)/(A;IA;-1))=//(A:)戶(A;jA:—r(Jt)+在濾波估計(jì)較準(zhǔn)確的情況下，五(KW"l)y(WA:-l))可以近似為及(A:)。在濾波估計(jì)中位置信息即是狀態(tài)變量，也是觀測變量，在濾波模型準(zhǔn)確時，對狀態(tài)變量的估計(jì)也準(zhǔn)確，此時P陣較小。在kalman濾波的解算公式中可以得知，+11A:)=+1,"単I，r(A:+1，it)+卿可知估計(jì)誤差協(xié)方差陣P與Q存在以上的數(shù)學(xué)關(guān)系，而根據(jù)I*-1)^W"l))=//("戶(A:I"+及W同樣可以知道Q與IA:-1)/IA:二l))也存在一定的數(shù)學(xué)關(guān)系，|A:—1)/(A|A-1))可以近似為，本發(fā)明方法的及(A:)根據(jù)POS柳，的統(tǒng)計(jì)信息可以獲得，因此本發(fā)明對于Q中的&，《22給予以下的設(shè)定H2,織鄉(xiāng)—睛Q反映的是狀態(tài)誤差信息，在模型準(zhǔn)確的情況下，Q值較小，針對本發(fā)明所采用的濾波模型，對^進(jìn)行了反復(fù)的調(diào)試，得出當(dāng)yS取為10%40%時，均能提高系統(tǒng)的定位精度，相比Q,R取經(jīng)驗(yàn)值時的定位精度高，自適應(yīng)調(diào)節(jié)能力強(qiáng)。對于不同的濾波模型，//、P也不同，"的取值范圍也因此情況而定。步驟l)統(tǒng)計(jì)特性的確定可以得到實(shí)時的POS^,的值，運(yùn)用以上的兩個公式從而實(shí)現(xiàn)對與協(xié)方差陣Q、R中參數(shù)&、《22、&、^的自適應(yīng)調(diào)節(jié)。本發(fā)明方法通過真實(shí)的數(shù)據(jù)進(jìn)行了仿真實(shí)驗(yàn)，依據(jù)以上的步驟對《u、《22、&、^四個參數(shù)值進(jìn)行了自適應(yīng)調(diào)節(jié)，相比不調(diào)節(jié)誤差協(xié)方差陣的定位精度提高了7%。實(shí)驗(yàn)的結(jié)果如圖3a、圖3b、圖3c所示，截取了不同時間段的軌跡輸出，圖3d為仿真實(shí)驗(yàn)位置誤差結(jié)果圖。圖中橫坐標(biāo)表示的是車輛行駛的東向位置信息，縱坐標(biāo)表示的是北向位置信息。同時圖中的短點(diǎn)虛線表示的是無噪聲時GPS輸出的位置信息，長點(diǎn)虛線是加噪聲后濾波系統(tǒng)輸出的GPS的位置信息，點(diǎn)線交叉虛線表示的是采用協(xié)方差陣取經(jīng)驗(yàn)值時GPS的位置信息，實(shí)線表示的是釆用了本發(fā)明方法的GPS的位置信息。從圖中可以看出實(shí)線比長點(diǎn)線明顯的接近無噪聲時的位置信息，相比取去經(jīng)驗(yàn)值時也更接近沒有誤差的位置信息，表明采用本發(fā)明的方法可以提高GPS的定位精度。圖3d為仿真實(shí)驗(yàn)位置誤差結(jié)果圖，橫坐標(biāo)表示時刻，縱坐標(biāo)表示位置誤差，點(diǎn)線交叉虛線表示Q,R取經(jīng)驗(yàn)值時的定位i吳差，實(shí)線表示Q，R采用本發(fā)明方法自適應(yīng)調(diào)節(jié)的定位誤差，選取了兩個不同的軌跡段，圖中可以看到實(shí)線表示的定位誤差明顯較小，提高了定位精度o仿真過程中，不同軌跡段的噪聲的大小也不一樣，本發(fā)明方法可以適應(yīng)噪聲的變化，達(dá)到實(shí)時動態(tài)調(diào)節(jié)的效果。同時表3統(tǒng)計(jì)了不同軌跡段內(nèi)的位置誤差的均值，均值l表示的是Q中的&、《22取16,R中的&、&取36經(jīng)驗(yàn)值時的情況，均值2表示Q,R采用本發(fā)明方法"取2，-取O.l自適應(yīng)調(diào)節(jié)時的情況。表4統(tǒng)計(jì)了不同軌跡段內(nèi)的位置誤差的方差，同理方差1表示的是&、《22取16,&、"22取36時的情況，方差2表示a取2，戶取O.l時的情況。從表中可以看出，均值2和方差2分別小于均值1和方差1，表明本發(fā)明方法相比誤差協(xié)方差陣取經(jīng)驗(yàn)值時可以減小定位誤差，提高定位精度。表3不同軌跡段位置誤差均值<table>tableseeoriginaldocumentpage19</column></row><table>權(quán)利要求1、車載導(dǎo)航系統(tǒng)誤差協(xié)方差陣參數(shù)自適應(yīng)調(diào)節(jié)的方法，包括如下步驟步驟一、建立車載導(dǎo)航系統(tǒng)的數(shù)學(xué)模型；步驟二、系統(tǒng)濾波；依據(jù)步驟一建立的數(shù)學(xué)模型對車輛導(dǎo)航系統(tǒng)進(jìn)行系統(tǒng)濾波，濾波是一個循環(huán)的過程，以周期T為單位，根據(jù)上一時刻對當(dāng)前時刻的估計(jì)以及當(dāng)前時刻的觀測變量對當(dāng)前時刻的狀態(tài)變量進(jìn)行濾波估計(jì)，獲得當(dāng)前時刻更為精確的狀態(tài)變量，做為系統(tǒng)濾波的輸出值；其特征在于還包括如下步驟步驟三、GPS光滑度判定；1)設(shè)定軌跡的時間長度；把車輛的運(yùn)動軌跡根據(jù)相等時間長度分成很多不同的軌跡段，設(shè)定時間長度H取15s～20s，并且不同軌跡段之間不可有時間重疊；2)DR相對運(yùn)動序列A的構(gòu)造；依據(jù)步驟1)中設(shè)定的時間長度，利用DR解算公式獲得每段軌跡內(nèi)相鄰兩點(diǎn)之間的距離LL＝S(k)*(m(k-1)+m(k))/2S(k)表示里程儀的刻度因素，m(k-1)表示前一周期內(nèi)里程儀累計(jì)的脈沖數(shù)，m(k)表示當(dāng)前周期內(nèi)累計(jì)的脈沖數(shù)，依據(jù)以上公式構(gòu)造序列A，A中的每個元素表示一個周期1s內(nèi)車輛行駛的位移量；3)GPS相對運(yùn)動序列B的構(gòu)造；依據(jù)步驟1)中設(shè)定的時間長度，利用GPS相鄰時刻輸出的位置信息做差，求出每一秒的位移，得出相鄰兩點(diǎn)間距離，構(gòu)造序列B；4)構(gòu)造距離差序列；將兩序列A和序列B相減得到一個距離差序列C，獲得相鄰兩秒中GPS測量位移與DR測量位移之差，距離差序列C中的元素有正負(fù)之分；5)誤差統(tǒng)計(jì)；對距離差序列C進(jìn)行統(tǒng)計(jì)，獲得一定時間段內(nèi)如15s內(nèi)由步驟4)獲得的距離差序列C內(nèi)元素的均值與方差；6)設(shè)定比較的閾值判斷光滑度；設(shè)定均值閾值為5～10，方差閾值為15～20，如果一定時間長度H內(nèi)距離差序列C中元素的均值和方差均小于設(shè)定的均值閾值和方差閾值，則判定這段時間H內(nèi)的運(yùn)動軌跡是光滑的，則不需要調(diào)節(jié)q11、q22、r11、r22四個參數(shù)，否則認(rèn)為是不光滑的，進(jìn)入步驟四；步驟四、依據(jù)GPS光滑度調(diào)節(jié)協(xié)方差陣；所述調(diào)節(jié)協(xié)方差陣的步驟如下1)統(tǒng)計(jì)特性的確定●根據(jù)GPS的測量性質(zhì)在HDOP波動幅度大于0.5的情況下POSmean_now＝(POSmean_n+POSmean_n+1+POSmean_n+2)/3POSvar_now＝(POSvar_n+POSvar_n+1+POSvar_n+2)/3POSmean_now表示序列差的均值POSmean_n、POSmean_n+1、POSmean_n+2表示連續(xù)三個距離差序列C的均值；POSvar_now表示序列差的方差，POSvar_n、POSvar_n+1、POSvar_n+2表示連續(xù)三個距離差序列C的方差；·根據(jù)GPS的測量性質(zhì)在HDOP波動幅度小于0.5的情況下在保證一次序列都是針對同一級別HDOP統(tǒng)計(jì)的結(jié)果情況下，使用當(dāng)前序列的方差或者均值數(shù)據(jù)對協(xié)方差陣元素進(jìn)行更新，從而使得系統(tǒng)濾波性能可以快速跟蹤噪聲的變化；2)協(xié)方差陣元素的更新r11＝r22＝α*POSvar_now其中α為1～2；q11＝q22＝β*POSvar_now其中β為10％～40％。2、根據(jù)權(quán)利要求l所述的車載導(dǎo)航系統(tǒng)誤差協(xié)方差陣參數(shù)自適應(yīng)調(diào)節(jié)的方法，其特征在于步驟一、建立車載導(dǎo)航系統(tǒng)的數(shù)學(xué)模型；根據(jù)車輛導(dǎo)航系統(tǒng)的的實(shí)際數(shù)據(jù)情況，選取GPS北向距離i^、GPS東向距離A、GPS航向W、GPS速度v、里程儀的刻度因子S，陀螺角速率V^(A:)作為狀態(tài)變量，選取GPS北向距離iV^、GPS東向距離五^、GPS航向y^、GPS速度v^^,陀螺角速率v^。作為觀測變量，構(gòu)建Kalman濾波模型，包括狀態(tài)方程與觀測方程，具體的數(shù)學(xué)模型如下狀態(tài)向量Z:xr=[i\^&yvS一歐]狀態(tài)方程為JC(A:+l)=^(A:+l(A:)+w(A:)狀態(tài)誤差向量為v/(A:)-0^w2w3w4w5w6]GPS/DR導(dǎo)航系統(tǒng)的狀態(tài)方程組如下，+i)=，)+v("rcos,))+Wl五,(it+1)=五,(A:)+v("rsin,))+w21)=++w3v(A:+l)=v(A)+vi;4l表示當(dāng)前時刻，^+l為下一時刻，w,，w2，w3,>v4，w5，、分別表示各個狀態(tài)變量的狀態(tài)誤差，r為周期，取為Is,狀態(tài)轉(zhuǎn)移矩陣經(jīng)過泰勒展開得<formula>formulaseeoriginaldocumentpage4</formula>w表示為狀態(tài)變量估計(jì)值的狀態(tài)誤差向量；e5]觀測向量/:V觀測方程為+1)=//(A)x(A:)+e④觀測誤差向量為Z("=[e,e2e3e4GPS/DR導(dǎo)航系統(tǒng)的觀測方程組如下A,e2，e3,e4,&分別表示各個觀測變量的觀測誤差，m(A;)表示的是當(dāng)前周期內(nèi)里程儀累計(jì)的脈沖數(shù)，觀測矩陣為<formula>formulaseeoriginaldocumentpage4</formula>觀測誤差協(xié)方差陣:<formula>formulaseeoriginaldocumentpage5</formula>e表示為觀測變量估計(jì)值的觀測誤差向量；&、r22、r33、r44,"55分別為北向位置觀測噪聲方差、東向位置觀測噪聲方差、GPS航向觀測噪聲方差和GPS速度觀測噪聲方差，陀螺角速率觀測誤差。全文摘要本發(fā)明公開了一種車載導(dǎo)航系統(tǒng)誤差協(xié)方差陣部分參數(shù)自適應(yīng)調(diào)節(jié)的方法，該方法需要建立車載導(dǎo)航系統(tǒng)的數(shù)學(xué)模型，根據(jù)模型系統(tǒng)能夠進(jìn)行濾波，采用了基于一段時間內(nèi)軌跡的均值和方差的統(tǒng)計(jì)特性構(gòu)建了GPS軌跡的光滑程度，用于判斷是否需要調(diào)節(jié)誤差協(xié)方差陣，利用GPS每秒的位移量與DR每秒的位移量構(gòu)造觀測噪聲序列，建立了此噪聲序列與誤差協(xié)方差陣之間的關(guān)系，使得誤差協(xié)方差陣在濾波系統(tǒng)中跟隨噪聲的變化而動態(tài)調(diào)節(jié)。本發(fā)明方法實(shí)現(xiàn)簡單，易于操作；可以對誤差協(xié)方差陣Q，R參數(shù)進(jìn)行自適應(yīng)調(diào)節(jié)；減小試湊法的工作量；提高導(dǎo)航系統(tǒng)的定位精度，與取經(jīng)驗(yàn)值時直接濾波相比提高7％。文檔編號G01C21/28GK101592492SQ20091008812公開日2009年12月2日申請日期2009年7月6日優(yōu)先權(quán)日2009年7月6日發(fā)明者倩劉,周艷麗,海張,張曉鷗,沈曉蓉,范耀祖,高婷婷申請人:北京航空航天大學(xué)