一種微慣性航姿參考系統(tǒng)的姿態(tài)獲取方法
【技術領域】
[0001] 本發(fā)明屬于導航技術領域，更具體地，涉及一種微慣性航姿參考系統(tǒng)的姿態(tài)獲取 方法。
【背景技術】
[0002] 在慣性導航系統(tǒng)中，姿態(tài)是反映載體運動的一個重要參數(shù)，通常姿態(tài)解算是根據(jù) 陀螺的數(shù)據(jù)和載體的速度、位置信息實時計算載體姿態(tài)矩陣，實現(xiàn)導航坐標系對地理坐標 系的實時跟隨以及載體姿態(tài)的解算（俯仰角、橫滾角和航向角）。而姿態(tài)精度對載體的速 度、位置計算、識別跟蹤、運動軌跡、性能分析等有重要影響。但是由于陀螺和加速度計的精 度有限，并且隨著時間的增加誤差會不斷的積累增大，尤其是低成本的MEMS陀螺漂移誤差 更大，使系統(tǒng)的姿態(tài)解算精度越來越低，不能夠滿足工程技術要求。
【發(fā)明內容】
[0003] 針對現(xiàn)有技術的缺陷，本發(fā)明的目的在于提供一種微慣性航姿參考系統(tǒng)的姿態(tài)獲 取方法，旨在解決現(xiàn)有技術中由于MEMS陀螺精度低、漂移誤差大導致航姿參考系統(tǒng)姿態(tài)解 算精度較低的技術問題。
[0004] 本發(fā)明提供了一種微慣性航姿參考系統(tǒng)的姿態(tài)獲取方法，包括下述步驟：
[0005] (1)根據(jù)傳感器采集的載體加速度信息和三維磁場強度信息獲得載體的姿態(tài)角初 值；并根據(jù)所述姿態(tài)角初值獲得當前時刻的系統(tǒng)姿態(tài)矩陣；
[0006] (2)根據(jù)所述系統(tǒng)姿態(tài)矩陣獲得陀螺漂移誤差補償量，并根據(jù)陀螺漂移誤差調節(jié) 模型對陀螺輸出的數(shù)據(jù)進行補償；
[0007] (3)根據(jù)補償后的陀螺輸出的數(shù)據(jù)對下一個時刻的系統(tǒng)姿態(tài)矩陣進行更新，并根 據(jù)更新后的姿態(tài)矩陣獲得載體的姿態(tài)角。
[0008] 更進一步地，步驟（1)具體包括：
[0009] (1. 1)獲得載體坐標系下的載體加速度信1 和三維磁場強度 信息
[0010] (1. 2)根據(jù)所述載體加速度信息計算載體的俯仰角初值和橫滾角初值；并根據(jù)所 述三維磁場強度信息計算載體的航向角初值；
[0011] (1.3)根據(jù)載體的俯仰角初值、橫滾角初值和航向角初值并結合公式
[0013] 獲得載體當前時刻的姿態(tài)矩陣^!;
[0014] 其中，hx hy hj別為X軸、Y軸、Z軸的磁場強度，a x ay \分別為載體在X軸、Y 軸、Ζ軸的加速度；載體的俯仰角
載體的橫滾角
g為載體在地 理坐標系的重力加速度，載體的航向角
.,Hny= hx*cos γ+hy*sin Θ *sin γ -hz*cos Θ *sin γ ;Hnx= hy*cos Θ +hz*sin γ ;導航坐標系為東北天地理坐標系，中n表 示導航坐標系，b表示載體坐標系。
[0015] 更進一步地，，步驟（2)中，陀螺漂移誤差調節(jié)模型為ω (t) = c〇gyrci(t) + c〇c(t);
[0016] 其中，載體坐標系下陀螺的角速度信息
ω；1 ω：： 分別為三個陀螺的輸出信息，〇e(t)為陀螺漂移誤差補償量，c〇(t)為陀螺 補償以后的輸出
[0017] 由于陀螺誤差會隨著時間的增加不斷累積，導致獲取的姿態(tài)信息含有很大的累積 誤差。加速度計測量信息是俯仰角、橫滾角的觀測量，磁強計信息是航向角的觀測量，本方 法利用加速度計測得信息和磁強計測得的信息對陀螺誤差進行補償，消除陀螺的累積誤 差，提高姿態(tài)獲取精度。
[0018] 更進一步地，步驟（3)具體包括：
[0019] (3. 1)根據(jù)補償后的陀螺輸出數(shù)據(jù)ω⑴獲得陀螺角增量反對稱矩陣
[0021] (3. 2)根據(jù)公式
對下一個時刻的姿態(tài)矩陣G中四元數(shù)進 行更新；
[0022] (3. 3)根據(jù)更新后的四元數(shù)對下一個時刻的姿態(tài)矩陣q進行更新；
[0023] (3. 4)根據(jù)更新后的姿態(tài)矩陣計算下一個時刻載體的姿態(tài)角；
[0024] 其中，ωχ coy ωζ分別為補償后三個陀螺的輸出數(shù)據(jù)；At為系統(tǒng)采樣周期；qA 下一個時刻的四元數(shù)，qk i為當前時刻的四元數(shù)，Μ為陀螺角增量的反對稱矩陣；更新后的 系統(tǒng)姿態(tài)矩陣
[0026] q。，q2, q3為四元數(shù)；下一個時刻載體的姿態(tài)角包括俯仰角θ = arcsin(T32)、橫 滾角，
[0027] 在本發(fā)明實施例中，載體的姿態(tài)信息可以用于系統(tǒng)控制和航姿參考。
[0028] 本發(fā)明提供的微慣性航姿參考系統(tǒng)的姿態(tài)獲取方法，以陀螺、加速度計和磁強計 為系統(tǒng)基本傳感器，并利用加速度計和磁強計的信息對陀螺的漂移進行有效補償，降低陀 螺的漂移誤差，根據(jù)補償后的陀螺輸出數(shù)據(jù)對姿態(tài)矩陣進行更新，從而提高航姿參考系統(tǒng) 的姿態(tài)解算精度，對慣性導航領域的姿態(tài)解算有較大貢獻，本發(fā)明簡單巧妙，符合實際需求 進步顯著且實用性強，能夠大規(guī)模推廣使用。
【附圖說明】
[0029] 圖1是本發(fā)明實施例提供的一種微慣性航姿參考系統(tǒng)的姿態(tài)獲取方法實現(xiàn)流程 圖。
【具體實施方式】
[0030] 為了使本發(fā)明的目的、技術方案及優(yōu)點更加清楚明白，以下結合附圖及實施例，對 本發(fā)明進行進一步詳細說明。應當理解，此處所描述的具體實施例僅僅用以解釋本發(fā)明，并 不用于限定本發(fā)明。
[0031] 本發(fā)明提供的微慣性航姿參考系統(tǒng)的姿態(tài)獲取方法，解決了由MEMS陀螺精度低、 漂移誤差大所導致的航姿參考系統(tǒng)姿態(tài)解算精度較低，不能滿足工程化應用的問題。
[0032] 如圖1所示，本發(fā)明實施例提供的微慣性航姿參考系統(tǒng)的姿態(tài)獲取方法，包括以 下幾個步驟：
[0033] 步驟1 :系統(tǒng)姿態(tài)矩陣初始化處理；
[0034] 步驟2 :讀取加速度計數(shù)據(jù)和磁強計數(shù)據(jù)，并對陀螺輸出數(shù)據(jù)進行誤差補償；
[0035] 步驟3 :根據(jù)補償后的陀螺輸出數(shù)據(jù)對系統(tǒng)姿態(tài)矩陣進行更新并根據(jù)更新后的姿 態(tài)矩陣求取系統(tǒng)姿態(tài)。
[0036] 作為本發(fā)明的一個實施例，步驟1中根據(jù)讀取的加速度計和磁強計信息計算系統(tǒng) 初始姿態(tài)矩陣，具體通過以下步驟實現(xiàn)：
[0037] (la)選擇東北天地理坐標系作為導航坐標系并從加速度計獲得載體加速度信息
其中ax ay az分別為載體在X軸、Y軸、Z軸的加速度；以及從磁強計 獲取三維磁場強度信息
：;其中hx hy hz分別為X軸、Y軸、Z軸的磁場強 度；其中，東北天地理坐標糸的三個方向分別對應載體的XYZ三個坐標軸，在導航領域是約 定成熟的定義，不再詳細定義。