• <tr id="yyy80"></tr>
  • <sup id="yyy80"></sup>
  • <tfoot id="yyy80"><noscript id="yyy80"></noscript></tfoot>
  • 99热精品在线国产_美女午夜性视频免费_国产精品国产高清国产av_av欧美777_自拍偷自拍亚洲精品老妇_亚洲熟女精品中文字幕_www日本黄色视频网_国产精品野战在线观看 ?

    基于自適應(yīng)強(qiáng)跟蹤濾波的捷聯(lián)慣導(dǎo)/里程計(jì)組合導(dǎo)航方法

    2018-05-31 03:10:26李士心黃鳳榮邱時(shí)前范超男
    關(guān)鍵詞:里程計(jì)標(biāo)度協(xié)方差

    李士心,黃鳳榮,邱時(shí)前,范超男

    (1. 天津職業(yè)技術(shù)師范大學(xué) 電子工程學(xué)院,天津 300222;2. 河北工業(yè)大學(xué) 機(jī)械工程學(xué)院,天津 300401;3. 北京鐳航世紀(jì)科技有限公司,北京 100081)

    車(chē)載定位定向系統(tǒng)是為陸基武器平臺(tái)提供陣地坐標(biāo)和方位指向的基準(zhǔn)設(shè)備,一般采用捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)與里程計(jì)(OD)組合模式進(jìn)行不依賴(lài)外界信息的自主導(dǎo)航,以避免戰(zhàn)時(shí)受外界干擾。對(duì)于大型輪式車(chē)而言,由于輪胎與地面接觸面積大,會(huì)導(dǎo)致里程計(jì)標(biāo)度由于輪胎胎壓變化、路況變化等原因發(fā)生較大變化,無(wú)法滿(mǎn)足車(chē)載定位定向系統(tǒng)給里程計(jì)分配的誤差要求,從而直接影響系統(tǒng)定位精度。

    非線(xiàn)性系統(tǒng)狀態(tài)估計(jì)在組合導(dǎo)航、初始對(duì)準(zhǔn)等領(lǐng)域具有廣泛的應(yīng)用[1-5],發(fā)展了擴(kuò)展卡爾曼濾波(EKF)、無(wú)跡卡爾曼濾波(Unsented Kalman Filter, UKF)、中心差分卡爾曼濾波(Central Difference Kalman Filter),帶漸消因子的強(qiáng)跟蹤濾波(STF)、粒子濾波(Particle Filter, PF)、容積卡爾曼濾波(CKF)等多種非線(xiàn)性濾波方法[6-8]。其中,STF提出漸消因子的概念,有效解決了 EKF在模型不確定時(shí)的濾波問(wèn)題;CKF采用球面徑向容積準(zhǔn)則進(jìn)行相應(yīng)的數(shù)值積分運(yùn)算[9],不僅克服了EKF和UKF在強(qiáng)非線(xiàn)性系統(tǒng)中的應(yīng)用局限性,而且濾波精度高于CDKF和PF。

    為解決里程計(jì)標(biāo)度誤差在大型輪式車(chē)行駛過(guò)程會(huì)發(fā)生變化導(dǎo)致定位精度變差的問(wèn)題,本文以 CKF為算法框架,引入STF漸消因子的基本理論,提出了SINS/OD組合導(dǎo)航的自適應(yīng)ST-CKF算法,并通過(guò)仿真分析和跑車(chē)試驗(yàn)驗(yàn)證方法的正確性。

    1 強(qiáng)跟蹤濾波器

    假設(shè)有如下形式的非線(xiàn)性離散系統(tǒng):

    其中:fk和hk+1分別為狀態(tài)函數(shù)和量測(cè)函數(shù);與分別為系統(tǒng)狀態(tài)向量和觀測(cè)向量;系統(tǒng)過(guò)程噪聲wk和量測(cè)噪聲vk互不相關(guān)且

    STF在EKF的基礎(chǔ)上引入漸消因子,實(shí)時(shí)調(diào)整預(yù)測(cè)誤差協(xié)方差陣,使濾波殘差序列保持相互正交,從而保持對(duì)系統(tǒng)狀態(tài)的跟蹤。STF的遞推公式為:

    其中,λk+1為漸消因子,In×n為n階單位陣,

    采用次優(yōu)算法計(jì)算漸消因子:

    其中,tr[·]為矩陣求跡的算子,為未引入漸消因子的狀態(tài)預(yù)測(cè)協(xié)方差陣。

    為實(shí)際輸出殘差序列的協(xié)方差陣,通常是未知的,由下式進(jìn)行估算:

    其中,為遺忘因子。

    式(13)和式(14)利用了測(cè)量函數(shù)的泰勒級(jí)數(shù)展開(kāi)式計(jì)算漸消因子,存在求解雅克比矩陣的問(wèn)題。根據(jù)文獻(xiàn)[4],為避免測(cè)量函數(shù)的雅克比矩陣數(shù)值計(jì)算,假設(shè)引入漸消因子之前的狀態(tài)預(yù)測(cè)誤差協(xié)方差陣為新息協(xié)方差陣為互協(xié)方差陣為則式(13)(14)分別有如下的等價(jià)表達(dá)式:

    2 容積卡爾曼濾波器

    根據(jù)文獻(xiàn)[6],CKF算法步驟如下:

    1)時(shí)間更新

    假設(shè)k時(shí)刻的狀態(tài)xk的狀態(tài)誤差協(xié)方差為Pk,對(duì)做Cholesky分解,有:

    選擇容積點(diǎn)為:

    系統(tǒng)狀態(tài)方程傳播后容積點(diǎn)為:

    其中,m=2n,且有是點(diǎn)集[1]的第i列。

    計(jì)算狀態(tài)預(yù)測(cè)值:

    計(jì)算誤差協(xié)方差陣:

    2)量測(cè)更新

    對(duì)Pk+1|k做Cholesky分解:

    計(jì)算容積點(diǎn):

    經(jīng)量測(cè)方程傳播后的容積點(diǎn)為:

    計(jì)算觀測(cè)預(yù)測(cè)值:

    計(jì)算量測(cè)誤差協(xié)方差陣:

    計(jì)算一步預(yù)測(cè)互相關(guān)協(xié)方差陣:

    計(jì)算濾波增益:

    狀態(tài)估計(jì)值:

    求取狀態(tài)誤差協(xié)方差估計(jì)值:

    3 自適應(yīng)強(qiáng)跟蹤容積卡爾曼濾波器

    STF算法在工程上有以下三點(diǎn)缺陷:1)STF算法基于EKF算法框架,將非線(xiàn)性函數(shù)截?cái)嘟凭€(xiàn)性化,對(duì)于強(qiáng)非線(xiàn)性系統(tǒng),計(jì)算間隔要設(shè)的非常小以減小截?cái)嗾`差;2)需要用數(shù)值計(jì)算方法計(jì)算非線(xiàn)性函數(shù)的雅可比矩陣,運(yùn)算量比較大;3)對(duì)于某些不可微非線(xiàn)性函數(shù)或時(shí)刻點(diǎn),無(wú)法計(jì)算雅可比矩陣。

    因此,STF數(shù)值計(jì)算穩(wěn)定性不佳,甚至對(duì)于強(qiáng)非線(xiàn)性系統(tǒng),容易濾波發(fā)散。STF的缺陷都是基于EKF算法框架下的,尋求一種可以代替EKF的算法框架,既能保持STF漸消因子強(qiáng)迫正交的理論優(yōu)勢(shì),又能避免計(jì)算雅可比矩陣的繁瑣,還能進(jìn)一步提高非線(xiàn)性函數(shù)線(xiàn)性化的計(jì)算精度,具有比較強(qiáng)的現(xiàn)實(shí)意義。CKF算法過(guò)程中不需要計(jì)算導(dǎo)數(shù),且具有泰勒展開(kāi)的三階精度,故基于CKF算法框架設(shè)計(jì)強(qiáng)跟蹤C(jī)KF (ST-CKF)算法,能避免STF的工程應(yīng)用缺陷,融合STF魯棒性強(qiáng)和CKF計(jì)算精度高的優(yōu)點(diǎn),實(shí)現(xiàn)工程應(yīng)用的目的。

    ST-CKF的算法步驟如下:

    1)時(shí)間更新

    由式(18)~(22)計(jì)算沒(méi)有引入漸消因子的狀態(tài)預(yù)測(cè)協(xié)方差陣進(jìn)行Cholesky分解:

    2)計(jì)算漸消因子

    由式(23)~(28)計(jì)算沒(méi)有引入漸消因子的量測(cè)

    誤差協(xié)方差陣和一步預(yù)測(cè)互相關(guān)協(xié)方差陣

    然后由式(32)~(34)、式(11)~(17)計(jì)算漸消因子λk+1。

    3)量測(cè)更新

    根據(jù)加入漸消因子λk+1的狀態(tài)xk+1的統(tǒng)計(jì)特性由式(24)求取相應(yīng)加入漸消因子的容積點(diǎn)以此重復(fù)式(25)~(31)的流程求得狀態(tài)估計(jì)和Pk+1。

    4 SINS/OD組合導(dǎo)航卡爾曼濾波模型

    本文選取SINS的姿態(tài)角誤差φE、φN、φU,水平速度誤差δvE、δvN,經(jīng)緯度誤差δλS、δLS,陀螺常值漂移εx、εy、εz,加速度計(jì)零偏?x、?y、?z,里程計(jì)推算經(jīng)緯度誤差δλD、δLD,里程計(jì)標(biāo)度因數(shù)誤差δKD,里程計(jì)方位安裝偏角φα作為狀態(tài)變量,建立17維的SINS/OD組合導(dǎo)航狀態(tài)方程;選取 SINS解算經(jīng)緯度與里程計(jì)推算經(jīng)緯度之差作為量測(cè)構(gòu)建量測(cè)方程。誤差模型的建立與推導(dǎo)過(guò)程參看文獻(xiàn)[10]。

    SINS/OD組合導(dǎo)航的狀態(tài)向量為:

    式中:W為3只陀螺漂移和3只加速度計(jì)零偏測(cè)量噪聲,F(xiàn)為系統(tǒng)狀態(tài)矩陣,見(jiàn)式(37)。的第i行第j列元素,vD為里程計(jì)輸出速度。

    量測(cè)方程為:

    其中,V為里程計(jì)推算緯度和經(jīng)度的量測(cè)噪聲,

    5 仿真分析和試驗(yàn)驗(yàn)證

    分別采用EKF、STF、ST-CKF進(jìn)行SINS/OD組合導(dǎo)航的仿真分析,濾波性能的好壞可以通過(guò) SINS/OD組合導(dǎo)航的定位精度、里程計(jì)安裝偏角和標(biāo)度誤差的跟蹤性能來(lái)衡量。

    假設(shè):3個(gè)陀螺的常值漂移均為0.01 (°)/h,隨機(jī)白噪聲標(biāo)準(zhǔn)差為0.01 (°)/h;3個(gè)加速度計(jì)的零偏均為0.05 mg,隨機(jī)白噪聲標(biāo)準(zhǔn)差為0.05 mg;初始水平姿態(tài)角誤差 0.01°,初始航向誤差 0.1°,初始水平速度誤差和初始經(jīng)緯度誤差均為0;載車(chē)初始位置為北緯40°,東經(jīng) 118°,初始時(shí)刻方位角為 45°,俯仰角為 0°,橫滾角為0°,以20 m/s的速度勻速行駛,行駛時(shí)間為 900 s;里程計(jì)標(biāo)度誤差設(shè)為0.01,方位安裝偏角設(shè)為 0.05°??紤]大型輪式車(chē)的實(shí)際應(yīng)用背景進(jìn)行以下兩種情況仿真分析:

    1)假設(shè)里程計(jì)標(biāo)度誤差δKD受路況變化影響發(fā)生了突變,在300s時(shí)由0.01階躍到0.02;

    2)假設(shè)里程計(jì)標(biāo)度誤差δKD受胎壓變化影響發(fā)生了漸變,在300s時(shí)開(kāi)始到600s結(jié)束,由0.01斜坡變化到0.02。

    以上兩種情況分別采用EKF、STF、ST-CKF對(duì)δKD進(jìn)行估計(jì),里程計(jì)標(biāo)度誤差階躍變化和斜坡變化的跟蹤曲線(xiàn)如圖1和圖2所示。

    圖1 EKF、STF和ST-CKF算法對(duì)δDK 階躍變化的跟蹤曲線(xiàn)Fig.1 Step change tracking curve ofδDK using EKF, STF and ST-CKF algorithms

    圖2 EKF、STF和ST-CKF算法對(duì) δKD線(xiàn)斜坡變化的跟蹤曲Fig.2 Ramp change tracking curve ofδDK using EKF, STF and ST-CKF algorithms

    由圖1~2中曲線(xiàn)不難看出,EKF在運(yùn)行一段時(shí)間后可以有效地估計(jì)出δKD的常值量,然而在隨后誤差發(fā)生幅度較大的突變或漸變時(shí),EKF失去了跟蹤能力。因此對(duì)于 SINS/OD組合導(dǎo)航系統(tǒng)而言,EKF可以在δKD為常值或變化不大的情況下使用,作為一種在線(xiàn)自主標(biāo)定方法是有效的,而STF和ST-CKF無(wú)論對(duì)誤差的突變還是漸變都具有很強(qiáng)的跟蹤能力。

    為驗(yàn)證ST-CKF的工程實(shí)用性,進(jìn)行了大型輪式車(chē)的跑車(chē)試驗(yàn)。試驗(yàn)采用的激光陀螺定位定向系統(tǒng)慣性元件精度為:陀螺零偏穩(wěn)定性<0.015 (°)/h,陀螺零偏重復(fù)性<0.005 (°)/h,加速度計(jì)零偏穩(wěn)定性和重復(fù)性<0.05 mg。試驗(yàn)設(shè)計(jì)的行車(chē)路線(xiàn)如圖3所示(僅示意變化量),行車(chē)?yán)锍?20 km,包括去程和回程共2個(gè)航程。試驗(yàn)前靜止對(duì)準(zhǔn)3 min,試驗(yàn)過(guò)程中以GPS位置信息作為參考基準(zhǔn),GPS定位精度<2 m。分別采用EKF、STF和ST-CKF對(duì)跑車(chē)過(guò)程中錄取的激光陀螺定位定向系統(tǒng)和里程計(jì)原始數(shù)據(jù)進(jìn)行離線(xiàn)仿真。

    圖3 行車(chē)軌跡Fig.3 Trajectory of vehicle

    圖4 第1航程EKF算法的位置誤差曲線(xiàn)Fig.4 Position error curve of test-1 using EKF algorithm

    圖5 第2航程EKF算法的位置誤差曲線(xiàn)Fig.5 Position error curve of test-2 using EKF algorithm

    兩個(gè)航程的EKF位置誤差試驗(yàn)結(jié)果如圖4和圖5所示,STF和ST-CKF位置誤差試驗(yàn)結(jié)果如圖6和圖7所示,圖中毛刺為GPS跳數(shù)所致。由圖4和圖5可以看出,由于EKF無(wú)法實(shí)時(shí)跟蹤里程計(jì)標(biāo)度的變化,去程定位誤差最大達(dá)到了 1617 m,回程最大誤差為619 m,定位誤差>7‰D(其中,D為里程),根本沒(méi)有發(fā)揮出慣性元件的使用精度。由圖6和圖7可以看出,STF和ST-CKF均達(dá)到了<1.5‰D的定位精度,對(duì)比可以看出ST-CKF對(duì)STF的改進(jìn),去程定位誤差最大由282 m提升到128 m,回程定位誤差最大由180 m提升到111 m,使得SINS/OD組合導(dǎo)航系統(tǒng)的定位精度由1.3‰D提高到0.6‰D,ST-CKF比STF更進(jìn)一步消除了里程計(jì)誤差的影響,達(dá)到了慣性元件的理論精度。

    圖6 第1航程STF和ST-CKF算法的位置誤差曲線(xiàn)Fig.6 Position error curve of test-1 using STF and ST-CKF algorithms

    圖7 第2航程STF和ST-CKF算法的位置誤差曲線(xiàn)Fig.7 Position error curve of test-2 using STF and ST-CKF algorithms

    去程和回程的里程計(jì)標(biāo)度誤差和方位安裝偏角ST-CKF估計(jì)曲線(xiàn)如圖8和圖9所示。由圖8可以看出,里程計(jì)標(biāo)度誤差在試驗(yàn)過(guò)程中近似斜坡緩慢變化了大約 0.015;由圖9可以看出,里程計(jì)標(biāo)度誤差在190 km處由于路況(高速轉(zhuǎn)入市內(nèi)公路)改變而發(fā)生了明顯突變。里程計(jì)標(biāo)度的實(shí)際變化包括了仿真分析的兩種情況,說(shuō)明了仿真分析的合理性。由圖8分析,如果不能對(duì)里程計(jì)誤差進(jìn)行實(shí)時(shí)估計(jì)補(bǔ)償?shù)脑?huà),會(huì)造成約220 000×0.015÷2=1650 m的位置誤差,與EKF去程定位誤差最大1617 m相符,這也反映了里程計(jì)標(biāo)度誤差自適應(yīng)補(bǔ)償?shù)谋匾浴?/p>

    圖8 第1航程ST-CKF算法里程計(jì)誤差估計(jì)曲線(xiàn)Fig.8 OD’s error estimated curve of test-1 using ST-CKF algorithm

    圖9 第2航程ST-CKF算法里程計(jì)誤差估計(jì)曲線(xiàn)Fig.9 OD’s error estimated curve of test-2 using ST-CKF algorithm

    6 結(jié) 論

    本文針對(duì)EKF在SINS/OD組合導(dǎo)航誤差模型中對(duì)里程計(jì)誤差模型描述不準(zhǔn)確時(shí)難以實(shí)時(shí)跟蹤誤差變化以及STF在工程應(yīng)用上的局限性,用CKF替代EKF的算法框架,引入STF漸消因子的基本理論,提出了SINS/OD組合導(dǎo)航的自適應(yīng)ST-CKF算法,并進(jìn)行了仿真分析和跑車(chē)試驗(yàn)。仿真證明STF和ST-CKF具有實(shí)時(shí)跟蹤里程計(jì)誤差變化的能力,驗(yàn)證了算法有效性。跑車(chē)試驗(yàn)結(jié)果表明,基于自適應(yīng)STF的組合導(dǎo)航方法能有效估計(jì)速度輔助傳感器的時(shí)變誤差,所提出的ST-CKF算法進(jìn)一步提高了誤差估計(jì)精度,有效解決了里程計(jì)標(biāo)度誤差在大型輪式車(chē)行駛過(guò)程發(fā)生變化導(dǎo)致定位精度變差的問(wèn)題。另外,本文方法不僅適用于陸用定位定向?qū)Ш?,還可以推廣到SINS/DVL組合的航海導(dǎo)航領(lǐng)域[11-12],具有良好的應(yīng)用前景。

    參考文獻(xiàn)(References):

    [1] Zhang L, Yang C, Chen Q W, et al. Robust H-infinity CKF/KF hybrid filtering method for SINS alignment[J].IET Science, Measurement & Technology, 2016, 10(8):916-925

    [2] Cui B B, Chen X Y, Xu Y, et al. Performance analysis of improved iterated cubature Kalman filter and its application to GNSS/INS[J]. ISA Transactions, 2017, 66: 460-468.

    [3] Zhou W D, Hou J X, Liu L, et al. Design and simulation of the integrated navigation system based on extended Kalman filter[J]. Open Physics, 2017, 15(1): 182-187.

    [4] 王小旭, 趙琳, 薛紅香. 強(qiáng)跟蹤C(jī)DKF及其在組合導(dǎo)航中的應(yīng)用[J]. 控制與決策, 2010, 25(12): 1837-1842.

    Wang X X, Zhao L, Xue H X. Strong tracking CDKF and application for integrated navigation[J]. Control and Decision, 2010, 25(12): 1837-1842.

    [5] 趙小明, 趙帥, 郭永剛, 等. 基于強(qiáng)跟蹤濾波的車(chē)載行進(jìn)間對(duì)準(zhǔn)[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2015, 23(2): 141-144.Zhao X M, Zhao S, Guo Y G, et al. In-motion alignment based on strong tracking filter[J]. Journal of Chinese Inertial Technology, 2015, 23(2): 141-144.

    [6] Arasaratnam I, Haykin S. Cubature Kalman filters[J].IEEE Transactions on Automatic Control, 2009, 54(6):1254-2169.

    [7] Pasha S A, Hoang D T, Vo B N. Nonlinear baysian filtering using the unscented linear fractional transformation model[J]. IEEE Transactions on Signal Processing,2013, 58(2): 477-489.

    [8] Gustafsson F. Particle filter theory and practice with positioning applications[J]. IEEE Aerospace and Electronic Systems Magazine, 2015, 25(7): 53-82.

    [9] 劉萬(wàn)利, 張秋昭. 基于 Cubature卡爾曼濾波的強(qiáng)跟蹤濾波算法[J]. 系統(tǒng)仿真學(xué)報(bào), 2014, 26(5): 1102-1107

    Liu W L, Zhang Q Z. New strong tracking filter based on cubature Kalman filter[J]. Journal of System Simulation,2014, 26(5): 1102-1107

    [10] 趙洪松, 繆玲娟, 沈軍. 捷聯(lián)慣導(dǎo)/里程計(jì)高精度組合導(dǎo)航算法[J]. 兵工學(xué)報(bào), 2014, 35(4): 433-440.

    Zhao H S, Miao L J, Shen J. High accuracy algorithm for SINS/Odometer integrated navigation system[J]. Acta Armamentarii, 2014, 35(4): 433-440.

    [11] Gao W, Li J C. Adaptive Kalman filtering for the integrated SINS/DVL system[J]. Journal of Computational Information Systems. 2013, 16(9): 6443-6450.

    [12] Wang L, Cheng X H, Li S X, et al. Adaptive interacting multiple model filter for AUV integrated navigation[J]. Journal of Chinese Inertial Technology, 2016, 24(4): 511-515.

    猜你喜歡
    里程計(jì)標(biāo)度協(xié)方差
    層次分析法中兩種標(biāo)度的對(duì)比分析
    室內(nèi)退化場(chǎng)景下UWB雙基站輔助LiDAR里程計(jì)的定位方法
    一種單目相機(jī)/三軸陀螺儀/里程計(jì)緊組合導(dǎo)航算法
    基于模板特征點(diǎn)提取的立體視覺(jué)里程計(jì)實(shí)現(xiàn)方法
    不確定系統(tǒng)改進(jìn)的魯棒協(xié)方差交叉融合穩(wěn)態(tài)Kalman預(yù)報(bào)器
    加權(quán)無(wú)標(biāo)度網(wǎng)絡(luò)上SIRS 類(lèi)傳播模型研究
    大角度斜置激光慣組與里程計(jì)組合導(dǎo)航方法
    一種基于廣義協(xié)方差矩陣的欠定盲辨識(shí)方法
    創(chuàng)新孵化網(wǎng)絡(luò)演化無(wú)標(biāo)度特征仿真分析
    縱向數(shù)據(jù)分析中使用滑動(dòng)平均Cholesky分解對(duì)回歸均值和協(xié)方差矩陣進(jìn)行同時(shí)半?yún)?shù)建模
    项城市| 南川市| 斗六市| 文昌市| 邵东县| 若羌县| 东港市| 疏勒县| 济阳县| 蚌埠市| 清水县| 行唐县| 东方市| 新密市| 永宁县| 盐津县| 汉沽区| 天全县| 循化| 高青县| 康保县| 芜湖县| 互助| 庆城县| 山阴县| 正定县| 浦县| 光山县| 新民市| 扶沟县| 营口市| 江陵县| 米易县| 呼伦贝尔市| 什邡市| 毕节市| 沛县| 栾城县| 万州区| 仁布县| 云龙县|