基于 Matlab 的捷聯慣導算法設計及仿真1doc

基于 Matlab 的捷聯慣導算法設計及仿真1

嚴恭敏 西北工業大學航海學院，西安 (710072) E-mail：yangongmin@163.com

摘要：根據圓錐誤差補償算法和劃船誤差補償算法的研究成果，考慮到實際捷聯慣導算法 仿真程序編寫的方便性，總結了一些與捷聯慣導更新算法有關的函數的計算公式。對圓錐誤

差補償算法和捷聯慣導算法進行了仿真，仿真結果和理論分析結論吻合。在附錄中給出了

Matlab 的 m 文件源程序代碼，具有一定的參考價值。 關鍵詞：捷聯慣導；四元數；等效旋轉矢量；Matlab；算法；仿真 中圖分類號：V249.3

1. 引言

在捷聯慣導系統中采用數學平臺，姿態更新解算是捷聯慣導系統算法的核心部分，由于 四元數法算的優良特性，它在工程實際中經常被采用。為了減小姿態計算的不可交換性誤差， 前人研究并建立了等效旋轉矢量方程，高精度姿態更新解算的研究主要集中在等效旋轉矢量 方程的求解上，在圓錐運動環境下，許多研究者提出并完善了圓錐誤差補償算法?；趫A錐 誤差補償算法和劃船誤差補償算法的等效原理，可將圓錐誤差補償算法移植到劃船誤差補償 算法中去，從而減少了劃船誤差推導的繁瑣過程。上述研究都已經比較成熟[1-6]，本文根據 這些研究結果，并考慮到實際仿真程序編寫的方便性，總結了一些與捷聯慣導算法有關的函 數的計算公式或步驟，其中更詳細的推導過程可見參考文獻[7,8]。最后，對圓錐誤差補償算 法和捷聯慣導算法進行了仿真。附錄中給出了Matlab的m文件源程序代碼具有一定的參考價 值。

2. 捷聯慣導算法

文中選取東-北-天(E-N-U)地理坐標系為導航坐標系，記為 n 系；捷聯慣組坐標系記 為 b 系。

2.1 相關函數

(1)四元數的共軛與乘積。四元數 q 可表示為 q = q0 + qv = q0 + q1i + q2 j + q3 k 。用 q

表示 q 的共軛四元數； q = q1q2 表示四元數 q 是四元數 q1 與 q2 的乘積，四元數相乘是不可

交換的。

(2)四元數與向量相乘。一般情況下利用矩陣進行坐標變換，如關系式 v n = C n v b ，

同樣利用四元數也可以表示坐標變換。設變換四元數 q n 與變換矩陣 C n 相對應，則定義變換

bb

四元數 q n 和向量 v b 的乘法，記為 v n = q n v b ，它由以下規則實現：首先，將向量擴展成四

bb

元數，令 qb = 0 + v b ；其次，做四元數乘法，令 q n = q n qb q n ? ；最后，從四元數 q n 中提取

bb

向量，有 v n = q n 。

(3)等效旋轉矢量與四元數之間的轉換。等效旋轉矢量 v 轉化為四元數 q 的公式為

1 本課題得到水下信息處理與控制國家級重點實驗室基金(9140C230206070C2306)資助。

q = Fv→q

(v) = cos( v / 2) + v sin( v / 2)

v

(1)

其中 Fv→q (?) 表示從等效旋轉矢量轉換到四元數的函數，容易看出有 q

= Fv→q (?v) 成立。

假設 v 是小量，則從 q 中可以求出 v ，首先令 vn 2 = v / 2 = arccos(q0 ) ，再求得

v = Fq→v

(q) =

2 ? vn 2 q

sin(vn 2 )

(2)

其中 Fq→v (?) 表示從四元數轉換到等效旋轉矢量的函數。

(4)姿態向量 A 與姿態四元數 q n 之間的轉換。記由俯仰角θ 、橫滾角γ 和航向角ψ 組

成的向量 A = [θ

γψ ]T 為姿態向量，通過姿態矩陣 C n 作為過渡，容易實現 A 與 q n 之間

bb

的轉換，此處不作詳細分析。

2.2 地球模型

通常給出的地球橢球模型參數為長半軸 Re 和扁率 f ，由橢球幾何學容易得到其它參數，

在慣性導航算法中經常用到，它們是偏心率 e =

2 f ? f 2 、短半軸 R

= (1 ? f )Re 、第二

偏心率 ep =

2 + R 2 / R 、子午圈半徑 R

= Re (1 ? e

) /(1 ? e2

sin 2

L)3 / 2

、卯酉圈半徑

RN = Re

/(1 ? e2 sin 2 L)1/ 2 。另外，重力加速度 g 和地球自轉角速率ω 也是慣性導航解算

的必備參數，在 GRS80 橢球模型中，正常重力公式為

g = g

0 (1 + 5.27094 × 10

2

sin 2 L

+ 2.32718 × 10?5

sin

4 L) ? 3.086 × 10

-5

?6

總結

以上是生活随笔為你收集整理的基于matlab的捷联惯导算法设计及仿真,基于 Matlab 的捷联惯导算法设计及仿真1doc.doc...的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。