估計算法、經(jīng)典Rodrigues參數(shù)奇異點及其處理方法，其特征在 于，具體描述如下： 步驟1，慣性系動基座對準過程 慣性系動基座對準算法以實時姿態(tài)陣的鏈式分解為基礎(chǔ)， Ctb - CtbCi^ ^ HS (1) 其中，nt系為實時導(dǎo)航坐標系，即載體時變位置東北天地理坐標系；1"為導(dǎo)航慣性系， 與動基座對準開始時刻的η系重合；b為載體坐標系；ib為載體慣性系，與對準開始時刻的 b系重合；式（1)中，是運動的nt系相對于導(dǎo)航慣性系i "的姿態(tài)陣，由GPS輸出位置信息 解析計算；能利用陀螺輸出進行姿態(tài)跟蹤，所以，慣性系動基座對準過程是對常值姿態(tài)陣 Ct的估計； 利用牛頓第二定律和哥氏定理，得到慣性系比力方程如下 V1" (〇 + ^ X V1'' (〇 - g1" (〇 = p (〇 ⑵ 其中，為t時刻載體對地速度在導(dǎo)航慣性系in內(nèi)的投影；為t時刻載體所在 位置重力加速度在導(dǎo)航慣性系1內(nèi)投影；/"(0為t時刻理想比力值； 對式（2)兩端分別進行積分，并記利用GPS輸出完成式（3)中F/"(X)的求解，進一步，在七^至t k更新周期內(nèi)，假設(shè)為常矢量，導(dǎo)航慣性系in內(nèi)對地速度，為線 性函數(shù)，即其中，t e [t^，tk] ;T = GPS量測更新周期； 將式（9)、（10)代入式（7)、⑶中，整理得利用捷聯(lián)慣導(dǎo)姿態(tài)、速度二子樣更新算法實現(xiàn)對式（4)中Fj(4)的求解；進一步，考慮 陀螺儀隨機常值漂移ε b和加速度計隨機常值零偏V6的影響，推導(dǎo)得其中，#為<^姿態(tài)誤差角；汾。仏)為加速度計慣性系比力積分誤差； 由式（2)、（3)、（4)和式（13)可得，求解常值姿態(tài)陣q的觀測方程為 V'm {h)~ (tk ) = Cfn Vy" {tk ) (邱 進一步，用經(jīng)典Rodrigues參數(shù)法來等價描述姿態(tài)陣，記對應(yīng)Rodrigues參數(shù)為1， 則二者滿足凱萊變換關(guān)系式，BP CI=(I + {1^(I-{1^) (17) 將式（17)代入式（16)，整理得，其中，. Wv包含慣性器件測量噪聲的積分和隨機擾動的積分，且有式（18)即是與姿態(tài)陣等價的Rodrigues參數(shù)1的觀測方程，若能估計出Rodrigues 參數(shù)1，則依據(jù)式（17)得到 綜上，慣性系動基座對準選取如下15維狀態(tài)由上述推導(dǎo)，系統(tǒng)方程及量測方程分別為，利用式（21)、式（22)設(shè)計濾波算法實現(xiàn)對Rodrigues參數(shù)1的估計，進而得到姿態(tài)陣 ，通過式（1)即實現(xiàn)動基座對準； 步驟2,二階非線性量測濾波估計算法 式（21)描述的系統(tǒng)方程為線性，式（22)描述的量測方程為非線性，但僅是狀態(tài)量的二 階非線性函數(shù)，能用有限階Taylor級數(shù)展開描述，即其中，Xktl為Taylor級數(shù)展開點；束t -XftO; ei是第i個分量為1，其余元素為0的 3維單位向量；Hk為非線性函數(shù)h的雅克比陣；D i為非線性函數(shù)h的二階偏導(dǎo)數(shù)陣；Tr為矩 陣求跡函數(shù)，且有其中，h = Qi1 h2 h3]T; 同時，對于二階非線性函數(shù)，其二階偏導(dǎo)數(shù)陣為常值矩陣，故由式（21)、（22)、（25)，知 DiS 15維常值對稱陣，其中非零元素僅有 Di(2,6) = 1，D2(1，6) = -1，D3(1，5) = 1， Di(3,5) = -1，D2(3,4) = 1，D3(2,4) = -I Di(6,2) = 1，D2(6，1) = -1，D3(5，1) = 1， 0八5,3) = -1，D2(4,3) = 1，D3(4,2) = -I (26) 步驟2.1，濾波時間更新算法 式（21)系統(tǒng)方程為線性，采用標準卡爾曼濾波算法完成時間更新，得到狀態(tài)量和估計 誤差方差陣的一步預(yù)測，即用狀態(tài)一步預(yù)測結(jié)果尤μ代替式（23)中Xktl，建立 起當前觀測量與狀態(tài)一步預(yù)測關(guān)系式，進而設(shè)計量測更新算法對一步預(yù)測結(jié)果毛/(^進行校 正，得到當前時刻的狀態(tài)最優(yōu)估計值+I下面推導(dǎo)基于式（23)二階泰勒級數(shù)量測方程的濾 波量測更新算法； 步驟2. 2,濾波量測更新算法 量測更新形式定義為與線性卡爾曼濾波量測更新一致，假設(shè)k時刻狀態(tài)估計結(jié)果為其中，Lk為引入的補償項，和最佳增益K k一樣均為待定值；L k和K k的確定原則分別為 使^^為無偏估計和使&的均方誤差陣Pk的跡最??； 定義狀態(tài)估計誤差其中，文嫩-1 = A . ， 由式（23)、式（24)、式（27)、式（28)整理得要使為無偏估計，即要求期望為零；假設(shè)時間更新為無偏估計，則對式（29) 右端取期望并令結(jié)果為零，得補償項Lk為其中，Ε[ ·]表示對括號內(nèi)變量求期望； 將式（30)代入式（29)中，整理得由于Ε[足]=0，E[wv] = 0,且^與A不相關(guān)，故由式（31)、式（32)得Pk為其中 Ak= E [AAT] (34) 式（32)中，A為3維列向量，從而Ak為3階方陣；利用式（32)，經(jīng)過推導(dǎo)得Λ ,第1行 第j列元素為式（33)中協(xié)方差陣更新方式與標準卡爾曼濾波形式一致，從而式（27)中最佳增益陣考察量測更新方程式（27)、（33)、（36)，基于二階泰勒級數(shù)的量測更新算法與標準卡爾 曼濾波在形式上完全一致，僅增加了對Lk、Ak的計算；而由式（30)、（35)知，L k、Ak的求解 簡單；考慮到DiS常值稀疏矩陣，將L k、Λ k描述為僅與P ,/η相關(guān)的形式，如此進一步減小 在線計算量； 步驟3,經(jīng)典Rodrigues參數(shù)奇異點及其處理方法 經(jīng)典Rodrigues參數(shù)是最少參數(shù)姿態(tài)描述方法之一，存在奇異點，一種等價表示方法 為(37) 其中，u為兩坐標系之間等效旋轉(zhuǎn)矢量的方向向量；φ為等效旋轉(zhuǎn)矢量轉(zhuǎn)過的角度； 當Φ取值為± π時，1值為無窮大，是經(jīng)典Rodrigues參數(shù)法進行姿態(tài)描述時的奇異 點，對ib系進行某種虛擬轉(zhuǎn)動得到i bl，使得ibl系相對于i n系的Rodrigues參數(shù)遠離奇異 點，然后采用前述濾波對準算法完成Cf對應(yīng)Rodrigues參數(shù)的估計，進而借助已知的虛擬 轉(zhuǎn)動，計算q。2.根據(jù)權(quán)利要求1所述的基于羅德里格參數(shù)和二階非線性量測的濾波對準算法，其特 征在于，所述步驟3中具體包括如下步驟： 步驟3.1，奇異點判別函數(shù)設(shè)計 當Φ取值為± π時，ib系至i n系姿態(tài)矩陣描述為此時，任意矢量R在兩坐標系內(nèi)投影滿足在式（39)兩端分別加上并利用單位向量恒等關(guān)系式I+(uX)2= uuT，則有(40) 也即是，此時任意矢量在兩坐標系內(nèi)的投影之和均共線，且與等效旋轉(zhuǎn)矢量平行； 因此，對當前濾波變量1是否接近奇異點的判斷等價于判斷式（19)中不同時刻的\是 否接近共線；為了縮短得出有效判斷所需時間，設(shè)定如下判別函數(shù)(41) 其中，&為\的方向向量；I · I表示取方陣行列式； 若用于計算gk的所有 '均共線，即當前對變量1的估計位于奇異點時，gk理論值為零； 步驟3. 2,虛擬轉(zhuǎn)動設(shè)計 虛擬轉(zhuǎn)動的設(shè)計依據(jù)：若ib系至in系等效旋轉(zhuǎn)矢量轉(zhuǎn)角大于η/2,將ib系繞著其某個 坐標軸轉(zhuǎn)動π角得到ibl系，使ibl系至in系等效旋轉(zhuǎn)矢量轉(zhuǎn)角小于π/2; 據(jù)此，記ib系繞其X軸轉(zhuǎn)動π角得到i bx系；i b系繞其y軸轉(zhuǎn)動π角得到i by系；i b系 繞其Z軸轉(zhuǎn)動JT角得到ibz系，則有 其中，、cZ、為已知常值對角陣，對角元為1或_l; 無論載體動基座對準開始時刻真實姿態(tài)為何值，姿態(tài)陣所對應(yīng)的 四個等效旋轉(zhuǎn)矢量中，至少有一個等效旋轉(zhuǎn)矢量的轉(zhuǎn)角小于JT/2,遠離奇異點； 步驟3. 3,奇異點規(guī)避的濾波算法 依據(jù)上面設(shè)計的判別函數(shù)和虛擬轉(zhuǎn)動，得到奇異點規(guī)避的濾波算法： (1)給定奇異點判別時間tk，在tk時刻以前，利用步驟1、步驟2推導(dǎo)的對準濾波算 法，同時處理四個獨立的濾波過程，分別對q；、?；、q；進行估計；其中，對c,t、 Ct進行估計時，式（21)、式（22)中相關(guān)參變量需要進行相應(yīng)的坐標變換；所有濾波 器狀態(tài)量初值均設(shè)為零，考慮式（27)及虛擬轉(zhuǎn)動分析結(jié)論，將1對應(yīng)的方差陣初值設(shè)定為 P〇0= 1，P 11= 1，P 22= 1 ; ⑵在tk時刻，分別計算姿態(tài)陣q；所對應(yīng)的判別函數(shù)值，gk、gkx、 gky、 gkz，僅選擇最大判別函數(shù)值所對應(yīng)的濾波過程完成后續(xù)濾波對準。
【專利摘要】本發(fā)明公開了一種基于羅德里格參數(shù)和二階非線性量測的濾波對準算法，包括由慣性系動基座對準過程、二階非線性量測濾波估計算法、經(jīng)典羅德里格參數(shù)奇異點及其處理方法，利用姿態(tài)陣分解，將動態(tài)姿態(tài)的估計問題轉(zhuǎn)化為一個常值姿態(tài)的估計，借助經(jīng)典Rodrigues參數(shù)和姿態(tài)陣之間的凱萊變換，建立了在慣性坐標系描述的系統(tǒng)方程線性，量測方程具有二階非線性的弱非線性誤差模型。同時對經(jīng)典羅德里格參數(shù)描述姿態(tài)存在奇異點的問題，也進行了詳細分析并給出了解決辦法。本發(fā)明在車輛啟動段典型機動條件下，能夠快速收斂，對準精度高，以與線性卡爾曼濾波相當?shù)挠嬎懔考坝嬎銖?fù)雜度實現(xiàn)了任意失準角非線性動基座對準。
【IPC分類】G01C25/00, G01C21/16
【公開號】CN104897170
【申請?zhí)枴緾N201510297409
【發(fā)明人】嚴恭敏, 梅春波, 付強文, 秦永元
【申請人】西北工業(yè)大學(xué)
【公開日】2015年9月9日
【申請日】2015年5月29日