一種改進(jìn)gnss/ins實(shí)時(shí)緊組合導(dǎo)航實(shí)時(shí)性能的方法
【專利摘要】本發(fā)明公開了一種改進(jìn)GNSS/INS實(shí)時(shí)緊組合導(dǎo)航實(shí)時(shí)性能的方法，包括(1)保存GNSS采樣時(shí)刻k的預(yù)測(cè)誤差協(xié)方差矩陣和機(jī)械編排解算的導(dǎo)航狀態(tài)；(2)在時(shí)刻k時(shí)GNSS觀測(cè)數(shù)據(jù)的接收時(shí)刻，采用Kalma濾波法進(jìn)行組合更新解算，得時(shí)刻k的狀態(tài)參數(shù)估計(jì)量以及狀態(tài)參數(shù)協(xié)方差估計(jì)量(3)根據(jù)和估計(jì)組合更新解算完成時(shí)刻j的狀態(tài)參數(shù)估計(jì)量以及狀態(tài)參數(shù)協(xié)方差估計(jì)量(4)采用以及修正時(shí)刻j的慣導(dǎo)誤差。本發(fā)明可降低GNSS數(shù)據(jù)延遲和組合解算耗時(shí)對(duì)實(shí)時(shí)導(dǎo)航輸出的影響，可有效改善實(shí)時(shí)組合導(dǎo)航的實(shí)時(shí)性能，對(duì)組合導(dǎo)航算法在運(yùn)算能力較低的處理器上的實(shí)現(xiàn)以及對(duì)實(shí)時(shí)性要求嚴(yán)格的應(yīng)用場(chǎng)合有格外重要的意義。
【專利說明】
-種改進(jìn)GNSS/1 NS實(shí)時(shí)緊組合導(dǎo)航實(shí)時(shí)性能的方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于實(shí)時(shí)組合導(dǎo)航系統(tǒng)技術(shù)領(lǐng)域，尤其設(shè)及一種改進(jìn)GNSS/INS實(shí)時(shí)緊組合 導(dǎo)航實(shí)時(shí)性能的方法。
【背景技術(shù)】
[0002] 多種手段組合導(dǎo)航是當(dāng)前導(dǎo)航技術(shù)的發(fā)展趨勢(shì)，基于全球衛(wèi)星導(dǎo)航系統(tǒng)(Global 化vigation Satellite System,GNSS)和慣性導(dǎo)航系統(tǒng)（Ine;rtial 化vigation System, INS)的組合是目前最具有應(yīng)用價(jià)值的組合模式之一，特別是基于GNSS原始觀測(cè)量和INS數(shù) 據(jù)的緊組合。該組合模式下，即使在可見衛(wèi)星不足W單獨(dú)進(jìn)行GNSS解算時(shí)，仍可有效利用有 限的GNSS觀測(cè)數(shù)據(jù)和INS數(shù)據(jù)通過Kalman濾波進(jìn)行緊組合解算，得到可靠的導(dǎo)航信息。因 此，GNSS/INS緊組合在工程中得到了越來越廣泛的應(yīng)用。
[0003] 然而，GNSS/INS緊組合實(shí)時(shí)應(yīng)用中的GNSS觀測(cè)數(shù)據(jù)時(shí)間延遲和組合解算時(shí)間延遲 (運(yùn)里統(tǒng)稱為時(shí)間延遲）問題，會(huì)分別導(dǎo)致當(dāng)前時(shí)刻觀測(cè)數(shù)據(jù)無法實(shí)時(shí)得到W及觀測(cè)數(shù)據(jù)無 法及時(shí)處理的問題，直接影響GNSS/INS緊組合系統(tǒng)的實(shí)時(shí)性能。目前，時(shí)間延遲的解決策略 主要采用增廣狀態(tài)方法和基于新息重組理論建立最優(yōu)濾波器。其中，增廣狀態(tài)方法會(huì)增加 狀態(tài)維度，使得計(jì)算量增大;而新息重組的方法，需要建立多個(gè)觀測(cè)方程，如果有多步延遲， 則不利于對(duì)Kalman濾波器的靈活拓展與應(yīng)用。而且運(yùn)些方法只能解決觀測(cè)數(shù)據(jù)傳輸延遲， 對(duì)于更新計(jì)算延遲則無能為力。GNSS/INS實(shí)時(shí)緊組合法主要包括"機(jī)械編排、Kalman預(yù)測(cè)" W及"Kalman觀測(cè)更新"兩部分，前者一般可W在IMUQnedial Measurement化it)采樣間 隔內(nèi)完成計(jì)算;后者則比較耗時(shí)，普通嵌入式處理器中無法在足夠短的時(shí)間內(nèi)（例如下一個(gè) 慣導(dǎo)數(shù)據(jù)到來前)及時(shí)完成，再加上GNSS觀測(cè)數(shù)據(jù)大都是從商業(yè)接收機(jī)板卡中獲取，觀測(cè)數(shù) 據(jù)傳輸會(huì)有一定的延遲，運(yùn)就導(dǎo)致Kalman濾波的觀測(cè)更新計(jì)算不能及時(shí)完成。運(yùn)種情況下， 如果使用標(biāo)準(zhǔn)Kalman濾波算法，就需要將后續(xù)IM識(shí)見測(cè)數(shù)據(jù)緩存起來不處理，等待觀測(cè)更新 完成才能進(jìn)行下一步計(jì)算，運(yùn)就會(huì)造成組合導(dǎo)航結(jié)果輸出的阻塞和延遲，影響系統(tǒng)的實(shí)時(shí) 性能。開發(fā)GNSS/INS實(shí)時(shí)緊組合系統(tǒng)時(shí)，就需要一種方法解決在存在時(shí)間延遲的情況下，及 時(shí)完成導(dǎo)航解算。

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

[0004] 針對(duì)現(xiàn)有GNSS/INS實(shí)時(shí)緊組合導(dǎo)航中GNSS觀測(cè)數(shù)據(jù)傳輸延遲和更新計(jì)算耗時(shí)較 長所帶來的時(shí)間延遲問題，本發(fā)明提出了一種改進(jìn)GNSS/INS實(shí)時(shí)緊組合導(dǎo)航實(shí)時(shí)性能的方 法。
[0005] 本發(fā)明采用如下技術(shù)方案：
[0006] -種改進(jìn)GNSS^NS實(shí)時(shí)緊組合導(dǎo)航實(shí)時(shí)性能的方法，包括：
[0007] 將GNSS/INS緊組合分解成機(jī)械編排、Kalman預(yù)測(cè)任務(wù)和Kalman觀測(cè)更新任務(wù)，下 述步驟實(shí)施過程中保持機(jī)械編排、Kalman預(yù)測(cè)任務(wù)優(yōu)先級(jí)高于Kalman觀測(cè)更新任務(wù)：
[000引（1)將GNSS采樣時(shí)刻記為時(shí)刻k，保存時(shí)刻k的預(yù)測(cè)誤差協(xié)方差矩陣巧,W和機(jī)械編 排解算的導(dǎo)航狀態(tài)；
[0009] (2)在時(shí)亥化時(shí)GNSS觀測(cè)數(shù)據(jù)的接收時(shí)刻，基于時(shí)刻k的GNSS觀測(cè)數(shù)據(jù)、私-1和根據(jù) 時(shí)刻k的機(jī)械編排解算的導(dǎo)航狀態(tài)形成的狀態(tài)向量Zk,采用Kalman濾波法進(jìn)行組合更新解 算，得時(shí)刻k的狀態(tài)參數(shù)估計(jì)量駕W及狀態(tài)參數(shù)協(xié)方差估計(jì)量P/;
[0010] (3)將組合更新解算完成時(shí)刻記為時(shí)刻j，基于公J
估計(jì) 時(shí)刻j的狀態(tài)參數(shù)估計(jì)量及狀態(tài)參數(shù)協(xié)方差估計(jì)量Pm; 表示時(shí)刻k到時(shí)刻j的累 積狀態(tài)轉(zhuǎn)移矩陣;為時(shí)刻k+l到時(shí)刻j的累計(jì)狀態(tài)噪聲矩陣
[00川 （4)采用i;,山及Pm修正時(shí)刻j的慣導(dǎo)誤差。
[0012] 和現(xiàn)有技術(shù)相比，本發(fā)明具有如下優(yōu)點(diǎn)和有益效果：
[0013] 1、本發(fā)明針對(duì)GNSS/INS組合導(dǎo)航系統(tǒng)，基于Kalman濾波法，將滯后的狀態(tài)參數(shù)估 計(jì)量W及狀態(tài)參數(shù)協(xié)方差估計(jì)量轉(zhuǎn)移到當(dāng)前時(shí)刻，從而降低GNSS數(shù)據(jù)延遲和組合解算耗時(shí) 對(duì)實(shí)時(shí)導(dǎo)航輸出的影響，可有效改善實(shí)時(shí)組合導(dǎo)航的實(shí)時(shí)性能。
[0014] 2、本發(fā)明對(duì)組合導(dǎo)航算法在運(yùn)算能力較低的處理器上的實(shí)現(xiàn)W及對(duì)實(shí)時(shí)性要求 嚴(yán)格的應(yīng)用場(chǎng)合有格外重要的意義。
[0015] 3、在DSP硬件平臺(tái)上實(shí)現(xiàn)和驗(yàn)證本發(fā)明，車載實(shí)測(cè)結(jié)果表明：相對(duì)于標(biāo)準(zhǔn)組合導(dǎo)航 解算法，本發(fā)明能夠在保障組合導(dǎo)航系統(tǒng)精度的前提下，最大限度地減小輸出導(dǎo)航結(jié)果的 時(shí)間延遲，保障組合導(dǎo)航結(jié)果的實(shí)時(shí)性。
【附圖說明】
[0016] 圖1為GNSS^NS實(shí)時(shí)緊組合導(dǎo)航系統(tǒng)結(jié)構(gòu)圖；
[0017] 圖2為時(shí)間延遲的處理時(shí)序示意圖；
[001引圖3為實(shí)施例中車載巧聯(lián)軌跡；
[0019] 圖4為GNSS數(shù)據(jù)延遲的統(tǒng)計(jì)直方圖，其中，橫坐標(biāo)表示GNSS數(shù)據(jù)延遲，單位為ms;
[0020] 圖5為200Hz中斷計(jì)算耗時(shí)的統(tǒng)計(jì)直方圖，其中，橫坐標(biāo)表示200Hz中斷計(jì)算耗時(shí)， 單位為ms;
[0021] 圖6為組合更新計(jì)算耗時(shí)的統(tǒng)計(jì)直方圖，其中，橫坐標(biāo)表示組合更新計(jì)算耗時(shí)，單 位為ms;
[0022] 圖7為本發(fā)明導(dǎo)航解算延遲示意圖；
[0023] 圖8為實(shí)施例的導(dǎo)航誤差曲線。
【具體實(shí)施方式】
[0024] GNSS/INS緊組合可分解成具有優(yōu)先級(jí)別的兩個(gè)任務(wù)"機(jī)械編排、Kalman預(yù)測(cè)"和 "Kalman觀測(cè)更新"獨(dú)立進(jìn)行，其中，任務(wù)"機(jī)械編排、Kalman預(yù)測(cè)"的優(yōu)先級(jí)高于任務(wù) 乂a Iman觀測(cè)更親開'。
[0025] 本發(fā)明具體步驟如下：
[00%] (I)將GNSS采樣時(shí)刻記為時(shí)刻k，保存時(shí)刻k的預(yù)測(cè)誤差協(xié)方差矩陣巧;W和機(jī)械編 排解算的導(dǎo)航狀態(tài)。
[0027] (2)見圖2,將時(shí)刻k時(shí)GNSS觀測(cè)數(shù)據(jù)的實(shí)際接收時(shí)刻記為時(shí)刻ts，在時(shí)刻ts采用 Kalman濾波法進(jìn)行組合更新解算(見式（12)~（13))，獲得時(shí)刻k的狀態(tài)參數(shù)估計(jì)量轉(zhuǎn)W及 狀態(tài)參數(shù)協(xié)方差估計(jì)量C。
[0028] 組合更新解算完成后，還需要將滯后計(jì)算出來的反映時(shí)刻k的狀態(tài)參數(shù)估計(jì)量W 及狀態(tài)參數(shù)協(xié)方差估計(jì)量轉(zhuǎn)移到組合更新完成時(shí)刻j，W修正時(shí)刻j的慣導(dǎo)誤差，即反饋修 正。本發(fā)明利用狀態(tài)轉(zhuǎn)移模型達(dá)到該目的，所述的狀態(tài)轉(zhuǎn)移即根據(jù)時(shí)刻k的狀態(tài)參數(shù)估計(jì)時(shí) 亥Ijj的狀態(tài)參數(shù)。
[0029] 根據(jù)k個(gè)時(shí)刻的觀測(cè)向量zi、Z2……Zk對(duì)時(shí)刻j的狀態(tài)參數(shù)Xj作線性最小方差估計(jì)
金從：
[0030] (1)
[0031] 式（1)中，zi、Z2……Zk分別表示時(shí)刻1、2、……k的觀測(cè)向量，E(.)表示數(shù)學(xué)期望。
[0032] GNSS/INS緊組合導(dǎo)航系統(tǒng)中，狀態(tài)參數(shù)X由INS誤差狀態(tài)和GNSS接收機(jī)時(shí)鐘誤差共 同組成，可W表示為：
[0033]
(2)
[0034] 式（2)中：Srn、Svn、4分別表示位置誤差、速度誤差、姿態(tài)角誤差;bg、sg分別表示巧 螺的零偏和比例因子;ba、Sa分別表示加速度計(jì)的零偏和比例因子；表示接收機(jī)鐘差誤 差;SfR表示接收機(jī)鐘漂誤差。
[003引虹n、Svn、(6、bg、Sg、ba、Sa為INS誤差狀態(tài)，孫,，、SfR為GNSS接收機(jī)時(shí)鐘誤差。
[0036] 考慮如下線性離散系統(tǒng)：
[0037] Xk=巫 k/k-ixk-1+Gk/k-iwk-i (3)
[003引 Zk =化 xk+Uk (4)
[0039] 式(3)~(4)中：
[0040] a、祉-1分別表示時(shí)刻k、k-l的狀態(tài)參數(shù)；
[0041] Ok/k-i為時(shí)刻k-巧Ij時(shí)刻k的狀態(tài)轉(zhuǎn)移矩陣，其值由狀態(tài)參數(shù)的動(dòng)力學(xué)模型決定；
[0042] Gk/k-i為時(shí)刻k-1到時(shí)刻k的系統(tǒng)噪聲驅(qū)動(dòng)矩陣；
[0043] Wk-I為時(shí)刻k-1的系統(tǒng)噪聲；
[0044] Zk為時(shí)刻k的觀測(cè)向量，其由INS推算的GNSS偽距、多普勒值與GNSS接收機(jī)觀測(cè)的 偽距、多普勒觀測(cè)值作差得到；
[0045] 化為時(shí)刻k的設(shè)計(jì)矩陣；
[0046] Uk為時(shí)刻k的觀測(cè)噪聲矩陣。
[0047] 根據(jù)式（3)，并且考慮到巫k+1/k-i=巫k+1/k巫k/k-i，從時(shí)刻k的狀態(tài)參數(shù)Xk轉(zhuǎn)移時(shí)刻占' 的狀態(tài)參數(shù)Xi,如下：
[0048]
(5)
[0049] 上述Ok+1/k-i表不時(shí)刻k-1到時(shí)刻k+1的狀態(tài)轉(zhuǎn)移矩陣，Ok+1/k、巫j/k分別表不時(shí)刻k 到時(shí)刻k+1、時(shí)刻j的狀態(tài)轉(zhuǎn)移矩陣，康示時(shí)刻巧IJ時(shí)刻j的狀態(tài)轉(zhuǎn)移矩陣;Gi/1-康示時(shí)刻 i-1到時(shí)刻i的系統(tǒng)噪聲驅(qū)動(dòng)矩陣;Wi-I表示時(shí)刻i-1的系統(tǒng)噪聲。
[(K)加]式(5)代入式（1)得到：
[0化1](6) =ZC千 1
[00對(duì)由式（3)可知，Wk-I只影響到祉，所Wwk-i與zi、Z2......Zk不相關(guān)，且E(Wk-I) =0，E( ?) 表示數(shù)學(xué)期望，因此有：
[0053]由式（3)可知，Wk-I 只影響到Xk,所 W Wi-I 與 Zi、Z2......Zk不相關(guān)，且E(Wi-I) = 0, i>k+ 1且i《j，因此有：
[0054]
(7)
[0化5]式(7)中，緝表示Xk的更新估計(jì)量。
[0化6] 的誤差為：
(8)
[0化7]
[005引對(duì)于預(yù)測(cè)誤差協(xié)方差矩陣Pm =巧i,,,兩J]，T表示矩陣的轉(zhuǎn)置：
[0060] 式中
衰示Xk的誤差;IVj為X在時(shí)刻j的預(yù)測(cè)誤差協(xié)方差矩陣，K為X在時(shí)刻k的協(xié)方差
矩陣;Qi-I為Wi-I的協(xié)方差矩陣。GNSS/INS緊組合中Gi/i-i為正定矩陣，因此， 「 n (9)
[0化9]
[0061] 4
表示時(shí)刻k+l到時(shí)刻j的累計(jì)狀態(tài)噪聲矩陣。具體 實(shí)施時(shí)采用公式(10)累積狀態(tài)噪聲矩陣：
[0062]
(10)
[0063] 式（10)中，Qm、Qk分別表不wm、wk的協(xié)方差矩陣，機(jī)i、wk分別表不時(shí)刻m、k時(shí)的系統(tǒng)噪 聲，m為大于k的時(shí)刻；表示時(shí)刻m到時(shí)刻m+1的轉(zhuǎn)移矩陣;Mm,k+1表示時(shí)刻k+巧Ij時(shí)亥Ijm的 累計(jì)狀態(tài)噪聲矩陣，Mk+i,k+i表示時(shí)刻k+1的累計(jì)狀態(tài)噪聲矩陣。
[0064]綜上，可獲得狀態(tài)轉(zhuǎn)移公式，如下：
[00化]
(11)
[0066] 由式（11)可W看出，狀態(tài)轉(zhuǎn)移可W由累積狀態(tài)轉(zhuǎn)移矩陣及累計(jì)狀態(tài)噪聲矩陣 Mj,W得到，為時(shí)刻k到時(shí)刻j時(shí)間段內(nèi)相鄰時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣的連乘，M川+1由式（10) 獲得。把GNSS采樣時(shí)刻看作時(shí)刻k，組合更新解算完成時(shí)刻看作時(shí)刻j，利用式（11)把時(shí)刻k 的狀態(tài)參數(shù)估計(jì)量起W及狀態(tài)參數(shù)協(xié)方差估計(jì)量Pr轉(zhuǎn)移到時(shí)刻j，進(jìn)而對(duì)時(shí)刻j的慣導(dǎo)誤差 進(jìn)行反饋修正。
[0067] 本發(fā)明組合更新解算采用標(biāo)準(zhǔn)Kalman濾波法，如下：
[006引 Cl 2)
[0069] (13)
[0070] 式中乂 W表示根據(jù)時(shí)刻l、2-'k-l的觀測(cè)向量估計(jì)的時(shí)刻k的狀態(tài)參數(shù);Kk為卡爾 曼濾波增益矩陣;巧為時(shí)刻k的狀態(tài)參數(shù)協(xié)方差估計(jì)量;巧為步驟(1)中保存的時(shí)刻k的預(yù) 測(cè)誤差協(xié)方差矩陣；I表示單位矩陣;化為觀測(cè)信息先驗(yàn)方差矩陣，按衛(wèi)星高度角確定觀測(cè) 值先驗(yàn)方差。
[0071] 在步驟（1)~(2)處理過程中保持優(yōu)先進(jìn)行INS機(jī)械編排和Kalman預(yù)測(cè)。運(yùn)樣，只要 能保證"慣性機(jī)械編排、Kalman預(yù)測(cè)"在IMU采樣間隔內(nèi)完成，并保證"Kalman觀測(cè)更新"計(jì)算 在GNSS采樣間隔內(nèi)完成，就能夠保證組合導(dǎo)航解算的實(shí)時(shí)性。
[0072] 實(shí)施例
[0073] 本實(shí)施例基于DSP(Digital Signal Processor)嵌入式平臺(tái)實(shí)現(xiàn)并驗(yàn)證本發(fā)明方 法。
[0074] 本實(shí)施例采用基于TMS320C6747開發(fā)的GNSS/INS實(shí)時(shí)緊組合系統(tǒng)，IMU采樣率為 200Hz，主要性能參數(shù)見表1 ,GNSS采樣率為IHz，導(dǎo)航結(jié)果輸出率為200Hz。
[00對(duì)表1 IMU主要性能參數(shù)
[0076]
[0077] GNSS/INS實(shí)時(shí)緊組合導(dǎo)航系統(tǒng)結(jié)構(gòu)見圖1，主要包括有2個(gè)處理器MCU (Microcontroller Unit)和DSP(Digi1:al Si即al Processor) ,MCU負(fù)責(zé)從巧螺儀及加速度 計(jì)采集原始數(shù)據(jù)并利用GNSS板卡提供的lPPS(Pulse Per Sond)把IMU數(shù)據(jù)標(biāo)記上GPS時(shí)間 標(biāo)志，通過串口向外部和DSP發(fā)送帶有時(shí)間標(biāo)志的IMU數(shù)據(jù)，DSP負(fù)責(zé)實(shí)時(shí)組合導(dǎo)航的計(jì)算。 為了實(shí)現(xiàn)多任務(wù)模式的解算，在DSP中進(jìn)行了中斷優(yōu)先級(jí)和中斷嵌套設(shè)計(jì)，DSP分為4k化的 中斷、200化中斷W及主函數(shù)S部分，4曲Z中斷優(yōu)先級(jí)高于200化中斷，負(fù)責(zé)把GNSS、IMU原始 數(shù)據(jù)采集到原始數(shù)據(jù)環(huán)形緩存中W及把導(dǎo)航結(jié)果環(huán)形緩存中的數(shù)據(jù)通過串口發(fā)送到外部； 200化的中斷從IMU原始數(shù)據(jù)環(huán)形緩存中獲取IM識(shí)見測(cè)數(shù)據(jù)進(jìn)行"機(jī)械編排、Kalman預(yù)測(cè)"W 及從主函數(shù)獲取組合更新量進(jìn)行反饋，再把導(dǎo)航結(jié)果存入導(dǎo)航結(jié)果環(huán)形緩存中；主函數(shù)負(fù) 責(zé)從GNSS原始數(shù)據(jù)環(huán)形緩存中獲取GNSS觀測(cè)數(shù)據(jù)進(jìn)行"Kalman觀測(cè)更新"，并將組合更新量 傳入200化中斷。
[0078] 對(duì)時(shí)間延遲的具體處理時(shí)序見圖2 ,WGNSS采樣時(shí)刻to的處理為例，具體步驟如 下：
[0079] 步驟l:200Hz中斷進(jìn)行"機(jī)械編排、Kalman預(yù)測(cè)"的同時(shí)，利用IMU數(shù)據(jù)的時(shí)間判斷 當(dāng)前時(shí)刻是否為GNSS采樣時(shí)刻，若是GNSS采樣時(shí)刻，保存當(dāng)前時(shí)刻的預(yù)測(cè)誤差協(xié)方差矩陣 W及機(jī)械編排解算的導(dǎo)航狀態(tài)，并且開始累計(jì)計(jì)算狀態(tài)轉(zhuǎn)移矩陣W及狀態(tài)噪聲矩陣。
[0080] 步驟2:由于GNSS觀測(cè)數(shù)據(jù)會(huì)滯后到來，主函數(shù)在該GNSS采樣時(shí)刻的數(shù)據(jù)接收完成 之后，利用之前保存的預(yù)測(cè)誤差協(xié)方差矩陣W及導(dǎo)航狀態(tài)，再加上接收到的GNSS觀測(cè)信息 形成Kalman觀測(cè)更新方程進(jìn)行乂alman觀測(cè)更新"，得到GNSS采樣時(shí)刻的組合更新量。
[0081] 步驟3:200Hz中斷判斷該GNSS采樣時(shí)刻的組合更新是否完成，如果完成，則利用累 計(jì)到此刻的狀態(tài)轉(zhuǎn)移矩陣W及狀態(tài)噪聲矩陣，使用狀態(tài)轉(zhuǎn)移的方法將主函數(shù)傳遞過來的該 GNSS采樣時(shí)刻的組合更新量變換到當(dāng)前時(shí)刻，使用轉(zhuǎn)移后的組合更新量進(jìn)行反饋，W修正 當(dāng)前時(shí)刻的慣導(dǎo)誤差，輸出最優(yōu)的組合結(jié)果。最后清除累計(jì)的狀態(tài)轉(zhuǎn)移矩陣W及狀態(tài)噪聲 矩陣，為下一輪計(jì)算做準(zhǔn)備。
[0082] 為驗(yàn)證GNSS^NS實(shí)時(shí)緊組合導(dǎo)航系統(tǒng)的性能，使用DSP片內(nèi)計(jì)時(shí)器統(tǒng)計(jì)200化中斷 內(nèi)從IMU原始數(shù)據(jù)獲取到輸出導(dǎo)航結(jié)果W及主函數(shù)中組合更新的運(yùn)行時(shí)間，并在導(dǎo)航結(jié)果 中記錄輸出，作為評(píng)價(jià)運(yùn)行耗時(shí)的依據(jù)。
[0083] 測(cè)試驗(yàn)證
[0084] 圖3為2015年9年29武漢市文化大道附近的實(shí)際車載測(cè)試軌跡，測(cè)試時(shí)長40多分 鐘，平均速率為20km/h，最大速率達(dá)38km/h，測(cè)試環(huán)境為開闊天空。本次測(cè)試，使用化imble Net R9作為靜止參考站，基線長度約16虹1，在測(cè)試車中搭載GNSS^NS實(shí)時(shí)緊組合導(dǎo)航系統(tǒng)， 同時(shí)采集GNSS原始觀測(cè)數(shù)據(jù)、IMU原始觀測(cè)數(shù)據(jù)W及實(shí)時(shí)組合導(dǎo)航解算結(jié)果。采用武漢邁普 時(shí)空導(dǎo)航公司開發(fā)的GINS Vl. 5軟件提供的PPK(Post Processed Kinematic VINS松組合 平滑結(jié)果作為位置、速度和姿態(tài)的參考真值(基線小于20km的情況下，位置精度為0.05m，速 度精度為0.0 lm/s，航向精度為0.02~0.05deg，橫滾俯仰精度為0.0 l~0.02deg)。
[0085] 1)實(shí)時(shí)性驗(yàn)證
[0086] 根據(jù)實(shí)時(shí)記錄的計(jì)算耗時(shí)，繪制GNSS數(shù)據(jù)延遲、200化中斷計(jì)算耗時(shí)W及組合更新 計(jì)算耗時(shí)的統(tǒng)計(jì)直方圖，見圖4~6。從圖4可W看出，56ms、59msW及48ms左右獲取的GNSS數(shù) 據(jù)有明顯的延遲，時(shí)間延遲的差異可能是觀測(cè)衛(wèi)星數(shù)目不同W及導(dǎo)航星歷消息數(shù)據(jù)更新引 起的傳輸數(shù)據(jù)大小不同導(dǎo)致的。從圖5可W看出，200化中斷函數(shù)的執(zhí)行時(shí)間95%左右的都 大約是0.75ms，2 %左右的大約是Ims，少部分大約在2.8ms，沒有大于3ms的部分，可W認(rèn)為 中斷函數(shù)可W在200化中斷間隔中執(zhí)行完畢。從圖6可W看出，組合更新執(zhí)行時(shí)間大部分在 45ms~70ms左右，沒有大于90ms的部分，結(jié)合圖4,可W看出GNSS延遲更新時(shí)間大約為 150ms，完全可W在一個(gè)GNSS采樣間隔內(nèi)完成組合更新計(jì)算。
[0087] 圖7為本發(fā)明導(dǎo)航解算延遲示意圖，從圖7看出，本發(fā)明導(dǎo)航解算延遲都在5ms之 內(nèi)，在IMU采樣間隔內(nèi)即可完成導(dǎo)航解算，延遲時(shí)間大致可W分為2部分，延遲較小的部分完 成的是"機(jī)械編排、Kalman預(yù)測(cè)"，延遲較大的部分要多處理組合更新量的狀態(tài)轉(zhuǎn)移W及反 饋。由此看來，本發(fā)明方法可有效減少時(shí)間延遲，確保系統(tǒng)的實(shí)時(shí)有效性能。
[0088] 2)實(shí)測(cè)精度驗(yàn)證
[0089] 將測(cè)試采集的實(shí)時(shí)導(dǎo)航結(jié)果與參考真值做比較，導(dǎo)航誤差見圖8和表2,從圖8和表 2可W看出，北向、東向、垂向的位置誤差Std分別為0.408m、0.363m、0.366m，北向、東向、垂 向的速度誤差Std分別為0.034m/s、0.034m/s、0.014m/s，橫滾、俯仰、航向的姿態(tài)誤差Std分 別為0.017deg、0.021deg、0.184deg，導(dǎo)航誤差均在偽距緊組合理論精度范圍內(nèi)，可W認(rèn)為 運(yùn)用本發(fā)明方法實(shí)現(xiàn)的GNSS^NS實(shí)時(shí)緊組合系統(tǒng)計(jì)算精度滿足設(shè)計(jì)要求。
[0090] 表2實(shí)時(shí)導(dǎo)航誤差統(tǒng)計(jì)
[00911
【主權(quán)項(xiàng)】
1. 一種改進(jìn)GNSS/INS實(shí)時(shí)緊組合導(dǎo)航實(shí)時(shí)性能的方法，其特征是： 將GNSS/INS緊組合分解成機(jī)械編排、Kalman預(yù)測(cè)任務(wù)和Kalman觀測(cè)更新任務(wù)，下述步 驟實(shí)施過程中保持機(jī)械編排、Kalman預(yù)測(cè)任務(wù)優(yōu)先級(jí)高于Kalman觀測(cè)更新任務(wù)： (1) 將GNSS采樣時(shí)刻記為時(shí)刻k，保存時(shí)刻k的預(yù)測(cè)誤差協(xié)方差矩陣1和機(jī)械編排解 算的導(dǎo)航狀態(tài)； (2) 在時(shí)刻k時(shí)GNSS觀測(cè)數(shù)據(jù)的接收時(shí)刻，基于時(shí)刻k的GNSS觀測(cè)數(shù)據(jù)、和根據(jù)時(shí)刻 k的機(jī)械編排解算的導(dǎo)航狀態(tài)形成的狀態(tài)向量zk，采用Kalman濾波法進(jìn)行組合更新解算，得 時(shí)刻k的狀態(tài)參數(shù)估計(jì)量劣以及狀態(tài)參數(shù)協(xié)方差估計(jì)量Pf ; (3) 將組合更新解算完成時(shí)刻記為時(shí)刻j，基于公式古計(jì)時(shí)刻 j的狀態(tài)參數(shù)估計(jì)量％』以及狀態(tài)參數(shù)協(xié)方差估計(jì)量巧；Φ i/k表示時(shí)刻k到時(shí)刻j的累積狀 態(tài)轉(zhuǎn)移矩陣;Mj,k+1為時(shí)刻k+Ι到時(shí)刻j的累計(jì)狀態(tài)噪聲矩陣(4) 采用以及修正時(shí)刻j的慣導(dǎo)誤差。
【文檔編號(hào)】G01C21/16GK105954783SQ201610262477
【公開日】2016年9月21日
【申請(qǐng)日】2016年4月26日
【發(fā)明人】常樂, 牛小驥, 章紅平, 張全
【申請(qǐng)人】武漢大學(xué)