數(shù)學(xué)建模 飛行器空間坐標(biāo)修正

1、六校數(shù)學(xué)建模聯(lián)賽題目： 飛行器空間的坐標(biāo)修正【摘要】 隨著我國在航天事業(yè)上的不斷發(fā)展，飛行器的慣性導(dǎo)航系統(tǒng)的運(yùn)用起著越來越重要的作用。但由于慣性導(dǎo)航系統(tǒng)儀器的精度因素影響，會對飛行器的空間坐標(biāo)確定產(chǎn)生影響，且隨著時間的積累這種影響產(chǎn)生的誤差會逐漸增大，嚴(yán)重影響導(dǎo)航系統(tǒng)的精度，因此，對器的誤差修正非常必要。本文在分析了各物理量的關(guān)系的基礎(chǔ)上，經(jīng)過嚴(yán)密地推導(dǎo)，得出各個軸方向上的位移與其對應(yīng)速度的修正之后的函數(shù)關(guān)系，據(jù)此建立了相關(guān)的數(shù)學(xué)模型并通過評價說明了模型的合理性和科學(xué)性對于問題一：我們采用數(shù)據(jù)分析中的卡爾曼濾波法、迭代處理和線性擬合對誤差經(jīng)行剔除。對于問題二：我們用問題一修正過后的數(shù)據(jù)去修正飛

2、行器飛行速度所得的坐標(biāo)，避免了慣性導(dǎo)航系統(tǒng)中誤差隨著時間的推移而增大的效應(yīng)。對于問題三：對于大型客機(jī)，飛行速度穩(wěn)定高度基本不變，適合前面的處理方式。關(guān)鍵詞：卡耳曼濾波法 迭代法 線性擬合 相互支持度 加權(quán)融合一、問題重述1.1問題背景飛行器的導(dǎo)航精度問題一直是航空航天領(lǐng)域研究的重要課題，慣性導(dǎo)航系統(tǒng)是一種不依賴于任何外部信息的自主式導(dǎo)航系統(tǒng)，在航空航天領(lǐng)域起著越來越重要的作用。由于其系統(tǒng)結(jié)構(gòu)誤差、慣性測量部件誤差、標(biāo)度系數(shù)誤差等因素的影響，慣性導(dǎo)航系統(tǒng)的積累誤差隨著時間的推移而逐漸增大，這一問題嚴(yán)重影響到航空航天技術(shù)的發(fā)展。目前關(guān)于定位精度的研究成果主要是從物理技術(shù)(例如紅外測距)方面來提高定

3、位的精度，近年來，圍繞定位坐標(biāo)精度問題的相關(guān)研究也漸漸展開。因此進(jìn)一步研究飛行器空間坐標(biāo)修正方法有重要的理論意義和應(yīng)用價值。本題的目標(biāo)是利用數(shù)學(xué)的方法對飛行器的誤差進(jìn)行修正，并利用結(jié)果進(jìn)行飛行器的仿真。1.2問題提出某一觀測站測得飛行器空間位置(假設(shè)觀測站為坐標(biāo)原點(diǎn))X(x、y、z)，飛行器的飛行速度V(x軸、y軸、z軸),飛行器與觀測站之間的偏向角a，俯仰角q以及觀測數(shù)據(jù)的時間間隔t。所給的各項(xiàng)數(shù)據(jù)均含有一定的誤差，其中觀測站的坐標(biāo)(0,0,0)不含誤差，飛行器的坐標(biāo)（觀測值）可能含有較大誤差。請根據(jù)所給數(shù)據(jù)進(jìn)行如下工作：問題一：飛行器坐標(biāo)的數(shù)據(jù)為觀測值，由于電子儀器的精度和噪聲干擾等，含有

4、一定的誤差波動，建立數(shù)學(xué)模型對飛行器坐標(biāo)觀測值的隨機(jī)波動誤差進(jìn)行修正。問題二：由于觀測數(shù)據(jù)的儀器誤差，飛行器坐標(biāo)在長時間的飛行中，坐標(biāo)數(shù)據(jù)的觀測值由于誤差的累積發(fā)生漂移，建立數(shù)學(xué)模型，對飛行器的坐標(biāo)的這種誤差進(jìn)行修正。(提示：在短時間內(nèi)，可以視為飛行器坐標(biāo)含有一定的常量誤差，或者飛行器的這種誤差是線性變化的)。問題三：結(jié)合具體的飛行器給出誤差修正方案。二、模型假設(shè)假設(shè)一：.觀測站的坐標(biāo)與機(jī)體的的坐標(biāo)系不存在有相對運(yùn)動不考慮地球的曲率半徑的影響，不考慮地球公轉(zhuǎn)自轉(zhuǎn)。假設(shè)二：速度的測量值比坐標(biāo)的測量值精確度更高，對極其符合線性關(guān)系的數(shù)據(jù)，認(rèn)為線性擬合之后誤差已經(jīng)修正的很好了。假設(shè)三：假設(shè)噪聲的干擾

5、造成的誤差可以通過卡爾曼濾波器1或者迭代法消除。假設(shè)四：在使用速度擬合位移曲線的時候假設(shè)中間時間間隔做勻速運(yùn)動。 三、符號說明與解釋 X1,Y1,H1 由觀測站所得的飛行器空間位置坐標(biāo) X2,Y2,H2 由速度計算所得的坐標(biāo)vx,vy,vh 行器沿各個軸方向上的速度t 觀測數(shù)據(jù)的時間間隔alpha 飛行器與觀測站之間的偏向角.theta 飛行器與觀測站之間的俯仰角h2 h3 h4 迭代一次 迭代二次 迭代三次后h坐標(biāo)i,j,k 機(jī)體采用的坐標(biāo)系的三個基向量I,j,k 觀測站采用的坐標(biāo)系的三個基向量4、 問題分析問題一：慣性導(dǎo)航系統(tǒng)是自主式導(dǎo)航系統(tǒng)，飛行器的飛行速度Vx Vy Vz 及飛行器與觀

6、測站之間的仰角（theta）偏向角（alpha）可以由系統(tǒng)提供。為此，我們假設(shè)他符合迭代和卡爾曼濾波兩種模型。電子儀器精確度和噪音干擾：H1-t原始數(shù)據(jù)圖象我們發(fā)現(xiàn)在H坐標(biāo)上，數(shù)據(jù)存在很大波動，可以肯定噪聲干擾較大。所以要對H坐標(biāo)數(shù)據(jù)濾去噪聲帶來的數(shù)值。X1-t原始數(shù)據(jù)圖象Y1-t函數(shù)圖象根據(jù)上圖知道，飛行器坐標(biāo)的觀測值的隨機(jī)波動誤差的來源主要是h方向的誤差，x y方向如圖是線性關(guān)系，對飛行的空間坐標(biāo)影響不大，可以不用分析。根據(jù)圖知道，h方向的波動函數(shù)隨時間的增大圖象上下波動，影響較大，需要修正。對問題二的分析：短時間內(nèi)，飛行器含有一定的常量誤差，若飛行器k+1的時刻位置為X2(k+1)，k時

7、刻的位置為X2(k)，k時刻的速度為V(k)，則有：X2(k+1)=V(k)t+X2(k)+C(k) （C(k)為常量誤差）有上式知：X2(k+1)=SV(k)t+SC(k)由上面知，因常量誤差的積累會使誤差很大，即會使坐標(biāo)數(shù)據(jù)發(fā)生漂移，所以不能用這種方法計算。因此我們用問題一修正后的數(shù)據(jù)對飛行器速度所得的坐標(biāo)經(jīng)行修正，即：X2(k+1)=V(k)t+X1(k) (X1(k)為問題一修正后的坐標(biāo)位置），這樣就避免了因常量誤差的積累而使誤差很大影響數(shù)據(jù)的情況。然后在對X1 (k+1) 和 X2(k+1)兩個數(shù)據(jù)進(jìn)行融合，融合后的數(shù)據(jù)可以根據(jù)X=（s2）X(k+1)/(s2+s2)+（s2）X(k

8、+1)/(s2+s(s和s為X(k+1)和X(k+1)的方差）得出來的數(shù)據(jù)就很好的對誤差進(jìn)行了修正。此處定義一個距離相互支持度2：K1=10-（X2-X1）/10 (對于距離坐標(biāo)K表示在10m的精度內(nèi)，兩數(shù)據(jù)相似程度，)K2=-(1-2)/ (對于角度坐標(biāo)在的精度內(nèi)，兩角度數(shù)據(jù)相似程度)?？梢杂上嗷ブС侄扰袛嘈拚髷?shù)據(jù)有效性，一定程度上也決定加權(quán)數(shù)據(jù)融合的權(quán)因子。五、模型的建立與求解5.1針對問題一：5.1.1模型一、采用迭代原理。上面已經(jīng)說到導(dǎo)航系統(tǒng)觀測到的x y方向位置對飛行器的空間實(shí)際位置的影響不大，不需要修正，現(xiàn)在對h方向經(jīng)行修正，對數(shù)據(jù)迭代三次后所得的圖象為：迭代后的數(shù)值見附錄二。5

9、.1.2模型二、用卡爾曼模型，處理后的圖為圖二為卡爾曼濾波后的效應(yīng)放大圖。 由上面可以知道，采用迭代處理后的數(shù)據(jù)得到了很好的誤差修正，根據(jù)迭代三次的數(shù)據(jù)圖象來看，每一次的迭代都對數(shù)據(jù)經(jīng)行了一次修正，使數(shù)據(jù)趨于更加的穩(wěn)定，因此，迭代原理很好的的解決了原始數(shù)據(jù)誤差較大的情況，對誤差進(jìn)行了很好的修正。另外，卡爾曼濾波修正，由圖象可以知道，對數(shù)據(jù)也經(jīng)行了修正，只是，因?yàn)閿?shù)據(jù)的少，沒有很好顯示在圖片上，因此在這種情況下，我們一般采用迭代處理。5.2針對問題二：5.2.1問題一修正后的數(shù)據(jù)圖象：H1-t修正圖X1-t修正圖函數(shù)關(guān)系為 x=-65t+1360.25 Y1t修正圖，其函數(shù)關(guān)系為y=-70t+1

10、309.55.2.2、由飛行器速度所計算的所得的坐標(biāo)修正圖：Y2-t速度坐標(biāo)修正后數(shù)值圖象X2-t速度坐標(biāo)修正后的數(shù)值圖象紫色圖線為修正值，紅色部分為x2與x1的差值。H2-t速度坐標(biāo)修正圖三個坐標(biāo)方向數(shù)據(jù)融合后的圖象： 由上圖可以知道，x y h方向通過導(dǎo)航系統(tǒng)直接得到的數(shù)據(jù)經(jīng)過線性擬合后的修正圖象和由飛行器的速度計算所得的函數(shù)圖象十分相識，誤差很小。說明X(k+1)=V(k)t+X(k)此函數(shù)關(guān)系成立，對常量誤差的積累起到很好的修正。由融合后的數(shù)據(jù)圖象顯示知，呈現(xiàn)出很好的線性關(guān)系，說明融合后的數(shù)據(jù)對誤差其到很好的修正。5.3針對問題三我們通過仿真數(shù)據(jù)結(jié)合問題一和問題二的方法對數(shù)據(jù)進(jìn)行修正。

11、X1坐標(biāo)與x2速度坐標(biāo)修正圖：Y1坐標(biāo)與y2速度坐標(biāo)修正圖H1坐標(biāo)與h2速度坐標(biāo)修正圖 由上面三個圖象可以知，仿真數(shù)據(jù)的坐標(biāo)修正和速度坐標(biāo)修正的圖象基本一致，說明誤差很小，即誤差被修正的很好。因?yàn)閮蓚€數(shù)據(jù)的相互支持度很高，所以數(shù)據(jù)的融合可以算他們的平均值，融合后的數(shù)據(jù)圖像為：X數(shù)據(jù)融合圖：Y數(shù)據(jù)融合后的圖象h數(shù)據(jù)融合后的圖形 由仿真數(shù)據(jù)融合后的圖形可知，通過導(dǎo)航系統(tǒng)直接給出的空間位置數(shù)據(jù)與飛行器速度積分演算出來的空間位置數(shù)據(jù)項(xiàng)融合后，可以很好的對誤差進(jìn)行修正，這一方案在融合圖象上很好驗(yàn)證了可行性，且誤差修正的精確度高！六、模型評價 對于問題一的修正，我們假設(shè)了兩種模型，分別采用了均值迭代法和卡

12、爾曼濾波法來處理電子儀器的精度和噪聲干擾，由數(shù)據(jù)函數(shù)圖象知，迭代后的數(shù)值隨時間的增加波動明顯減小，而且迭代的次數(shù)越多，波動越小，即修正的效果越好，在很大的程度上消除了噪聲的干擾。另外，為了更進(jìn)一步的對誤差修正，我們還應(yīng)用了卡爾曼濾波法，由圖顯示，卡爾曼濾波雖然也顯示出誤差修正的效果，但應(yīng)為數(shù)據(jù)的少，在很短的時間內(nèi)對誤差修正，由卡爾曼放大圖上可知，這一波動修正也有一定的效應(yīng)。 對于問題二，我們通過上面修正后的數(shù)據(jù)與通過速度坐標(biāo)修正后的數(shù)據(jù)進(jìn)行融合，應(yīng)為兩個的數(shù)據(jù)的相互支持度很高，所以融合是采用取兩數(shù)據(jù)的平均值，觀測圖知，這種融合很巧妙的將飛行器的空間坐標(biāo)誤差進(jìn)行修正，而且效果非常的好，很好的避開

13、了因常量誤差的積累導(dǎo)致飛行器坐標(biāo)數(shù)據(jù)發(fā)生漂移的現(xiàn)象。 對于問題三，因前面我們通過數(shù)據(jù)融合后的數(shù)據(jù)有很好的修正誤差的效果，所以對于具體的飛行器，我們依然采用的是數(shù)據(jù)融合來修正誤差！數(shù)據(jù)融合及卡耳曼可以很好的對電子儀器中的噪聲干擾進(jìn)行修正，但對于電子儀器的精度干擾不能很好的修正七模型改進(jìn)。本個模型對誤差的修正并沒有達(dá)到真正最優(yōu)的效果，因?yàn)閿?shù)據(jù)的組數(shù)較少，加之我們對一些程序設(shè)計沒有很好的掌握，未能在卡爾曼濾波器及均值迭代法消除噪聲的數(shù)值中做太多分析，所以卡爾曼的最優(yōu)濾波迭代處理的效果未能很好展現(xiàn)出來，如果能在問題一采用的卡爾曼濾波和均值迭代處理中通過程序設(shè)計消除噪聲影響，加上后面的數(shù)據(jù)融合進(jìn)行修正效

14、果會更好些。參考文獻(xiàn)1. 胡 旅 滿 ，劉先省。一種實(shí)用的數(shù)據(jù)融合算法2004.1222. 趙希人.卡爾曼濾波器在船用慣性導(dǎo)航系統(tǒng)中的應(yīng)用.1985.713. 高等數(shù)學(xué).江西出版社.2011.8附錄一均值迭代處理的mat lab代碼：functionh2= nihe (h)%UNTITLED4Summaryofthisfunctiongoeshere%DetailedexplanationgoeshereN=41;temp=(h-mean(h)./abs(h-mean(h);aver=(sum(abs(h-mean(h)/N;h2=h-temp.*aver;End卡爾曼濾波mat lab代碼：

15、clearclc;N=101;%采樣點(diǎn)的個數(shù)CON=602.737800000000610.741200000000618.744600000000626.748000000000634.751400000000642.754800000000650.758200000000658.761600000000666.765000000000674.768400000000682.771800000000690.775200000000698.778600000000706.782000000000714.785400000000722.788800000000730.79220000000073

16、8.795600000000746.799000000000754.802400000000762.805800000000770.809200000000778.812600000000786.816000000000794.819400000000802.822800000000810.826200000000818.829600000000826.833000000000834.836400000000842.839800000000850.843200000000858.846600000000866.850000000000874.853400000000882.8568000000

17、00890.860200000000898.863600000000906.867000000000914.870400000000922.873800000000930.877200000000938.880600000000946.884000000000954.887400000000962.890800000000970.894200000000978.897600000000986.901000000000994.9044000000001002.907800000001010.911200000001018.914600000001026.918000000001034.92140

18、0000001042.924800000001050.928200000001058.931600000001066.935000000001074.938400000001082.941800000001090.945200000001098.948600000001106.952000000001114.955400000001122.958800000001130.962200000001138.965600000001146.969000000001154.972400000001162.975800000001170.979200000001178.982600000001186.9

19、86000000001194.989400000001202.992800000001210.996200000001218.999600000001227.003000000001235.006400000001243.009800000001251.013200000001259.016600000001267.020000000001275.023400000001283.026800000001291.030200000001299.033600000001307.037000000001315.040400000001323.043800000001331.0472000000013

20、39.050600000001347.054000000001355.057400000001363.060800000001371.064200000001379.067600000001387.071000000001395.074400000001403.07780000000;%飛行器坐標(biāo)的理論值x=zeros(1,N);%用來記錄坐標(biāo)的最優(yōu)化估算值y=599.8058765607.7992796613.8542847623.2107999633.2484116638.9026154648.8974241656.1049206665.4691306670.0803864679.7515

21、546686.768601698.9148593704.8106257713.3940629718.9618163727.4848863735.7186891744.9918199751.7793962760.6700869765.8879912775.6334528783.1049811790.4219001800.4799414808.390873816.0904032822.5414777833.1495586840.2802725847.6881812856.03111863.7753913870.2361353879.793164887.1445866895.0371693902.8

22、768351911.4707023918.0414119928.9803901936.4808528943.8897035952.0581586959.1716099969.0238533975.8949409983.2911497993.3061495999.75184321007.4047261015.7801941023.1090331030.9191051042.5414311049.6438051056.2546871062.7286751071.1301971079.7499961088.8010251094.6268811101.6654211110.5677131120.288

23、2781128.3769411136.4691831143.7779221152.2354881159.645071168.9099921174.6225171184.4764611191.0994911199.7590061208.5998191217.0784581222.8385681233.2766561240.6322281247.9165631255.7762781263.7311071271.6514551280.0125511287.9663471296.8564431305.5210871312.5018721319.8037691328.6499051336.1090711342.9192191352.9268721360.3125451368.1916121376.5007481384.3244841391.0822851399.896368;%坐標(biāo)的觀測值，其中疊加了噪聲x(1)=599.8058765;%為x(k)賦初值p(1)=1;%x(1)對應(yīng)的協(xié)方差Q=cov(randn