[發明專利]一種不受歷史數據影響快速收斂的地面靜態對準方法

有效

• 申請號: 201811553380.6
• 申請日: 2018-12-18
• 公開（公告）號: CN109682397B
• 公開（公告）日: 2021-01-29
• 發明（設計）人: 王獻忠;劉赟;張國柱;張麗敏;桑小沖;董晉芳
• 申請（專利權）人: 上海航天控制技術研究所
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 上海元好知識產權代理有限公司 31323
• 代理人: 包姝晴;徐雯瓊

1.一種不受歷史數據影響快速收斂的地面靜態對準方法，其特征在于，包括如下步驟：

步驟1，將慣組陀螺角速率和加表比力轉換到準北東地坐標系；

預處理：在準北東地坐標系對角速率和比力濾波；

基于準北東地坐標系地向比力，判別是否重賦初始對準角；

步驟2，基于準北東地坐標系北向和東向比力估計水平失準角；

分段處理：對水平失準角進行限幅處理和平滑處理；

步驟3，基于準北東地坐標系東向角速率估計方位失準角；

分段處理：對方位失準角進行限幅處理和平滑處理；

步驟4，基于水平失準角和方位失準角，利用四元數乘法計算精對準四元數；

基于精對準四元數，按3-2-1轉序計算精對準角度；

其中，所述步驟1包括如下步驟：

步驟1.1，慣組陀螺角速率和加表比力轉換到準北東地坐標系：

q_n′b,0＝[1 0 0 0]^T

A_n′b,0＝I_3×3

f^n′＝A_n′b,k-1·f^b

ω^n′＝A_n′b,k-1·ω^b

其中，q_n′b,0為地理系相對于本體系姿態四元數的初始值；A_n′b,0為本體系到地理系的轉換矩陣的初始值；A_n′b,k-1為k-1時刻本體系到地理系的轉換矩陣；f^b為加速度計的測量值；ω^b為陀螺的測量值；為地理系加速度；為地理系角速度；

步驟1.2，在準北東地坐標系對角速率和比力濾波：

k_lv＝0.02

其中，為k時刻北向加速度濾波值；為k時刻東向加速度濾波值；為k-1時刻北向加速度濾波值；為k-1時刻東向加速度濾波值；為k時刻東向角速度濾波值；

步驟1.3，基于準北東地坐標系地向比力，判別是否重賦初始對準角：

如果f_z^n′＞g_n/2，則重賦初始對準角：

dθ＝0°、dψ＝0°

其中，為北向失準角，dθ為東向失準角，dψ為方位失準角。

2.根據權利要求1所述不受歷史數據影響快速收斂的地面靜態對準方法，其特征在于，所述步驟2包括如下步驟：

步驟2.1，基于準北東地坐標系北向和東向比力估計水平失準角：

k_a＝0.01°/(m/s²)

步驟2.2，對水平失準角進行限幅處理和平滑處理：

將水平失準角限幅到[-10°，10°]，若水平失準角小于0.1°，將比例作用減弱50％。

3.根據權利要求2所述不受歷史數據影響快速收斂的地面靜態對準方法，其特征在于，所述步驟3包括如下步驟：

步驟3.1，基于準北東地坐標系東向角速率估計方位失準角：

k_ω＝1600°/(°/s)

步驟3.2，對方位失準角進行限幅處理和平滑處理：

方位失準角進行10°限幅；方位失準角小于0.1°，比例作用減弱50％。

4.根據權利要求3所述不受歷史數據影響快速收斂的地面靜態對準方法，其特征在于，所述步驟4包括如下步驟：

步驟4.1，基于水平失準角和方位失準角，利用四元數乘法計算精對準四元數：

失準角轉換成弧度，求誤差四元數：

其中，q_n′b,k為k時刻地理系相對于本體系的姿態四元數；q_n′b,k-1為k-1地理系相對于本體系的姿態四元數；q_bn′,k為k時刻本體系相對于地理系的姿態四元數；q_bn′,k-1為k-1時刻本體系相對于地理系的姿態四元數；

步驟4.2，基于精對準四元數，按3-2-1轉序計算精對準角度：

由q_bn,k＝q_bn′,k＝[q₀ q₁ q₂ q₃]^T

求A_bn,k：

其中，A_bn,k為地理系到本體系的轉換矩陣；

A_bn,k按3-2-1轉序求取三軸姿態角；

姿態按3-2-1轉序表示的姿態轉換矩陣如下：

姿態四元數按3-2-1轉序求三軸姿態角，如果|a₁₃|≤0.99999，則：

sinθ＝-a₁₃，θ＝asin(-a₁₃)

否則：

θ＝asin(-a₁₃)

其中，為橫滾角，θ為俯仰角，ψ為方位角。