[發明專利]一種定常系數陸標圖像導航濾波方法

有效

• 申請號: 202010350607.8
• 申請日: 2020-04-28
• 公開（公告）號: CN111637894B
• 公開（公告）日: 2022-04-12
• 發明（設計）人: 李驥;張曉文;張洪華;關軼峰
• 申請（專利權）人: 北京控制工程研究所
• 主分類號: G01C21/24
• 分類號: G01C21/24;G01C21/16
• 代理公司: 中國航天科技專利中心 11009
• 代理人: 劉秀祥

1.一種定常系數陸標圖像導航濾波方法，其特征在于，包括如下步驟：

S1、根據上一個周期的探測器本體角速度測量、探測器本體非引力加速度測量、修正后的探測器位置、修正后的探測器速度、探測器姿態四元數；外推計算本周期的探測器估計位置、探測器估計速度、探測器姿態四元數；

S2、在探測器本體坐標系下，根據陸標視線方向測量、探測器內已知的陸標慣性位置、S1中的探測器姿態四元數，確定探測器位置；

S3、以S2中探測器位置為測量，以S1中探測器估計位置、探測器估計速度為狀態，利用常系數增益矩陣對探測器估計位置、探測器估計速度進行修正，獲得修正后的探測器位置、修正后的探測器速度；然后重復S1~S3；

S2中，首先建立陸標相對位置方程，然后通過非求逆方法確定探測器位置，具體為：將所有陸標視線方向轉到慣性系下，建立探測器的慣性位置方程；利用探測器的慣性位置方程，獲得陸標之間的相對位置方程，并聯立成方程組，然后使用最小二乘方法求解出探測器相對所有陸標的斜距；根據所述斜距的計算結果，以及探測器的慣性位置方程，針對每個陸標獲得一個探測器慣性位置的計算值，再取均值就得到探測器位置。

2.根據權利要求1所述的一種定常系數陸標圖像導航濾波方法，其特征在于，S1中，利用慣性測量單元獲得上一個周期的探測器本體角速度測量、探測器本體非引力加速度。

3.根據權利要求1所述的一種定常系數陸標圖像導航濾波方法，其特征在于，S2中，利用圖像導航敏感器獲得陸標視線方向測量。

4.一種定常系數陸標圖像導航濾波方法，其特征在于，包括如下步驟：

S10、根據上一個周期的探測器本體角速度測量、探測器本體非引力加速度測量，利用慣性導航進行外推，獲得本周期的探測器估計位置、探測器估計速度、探測器姿態四元數；

S20、根據陸標視線方向測量、已知的陸標慣性位置、探測器姿態四元數，確定探測器位置；

S30、利用常系數增益矩陣，對S10中慣性導航外推的位置、速度進行濾波修正；然后轉入S10；

S20中，首先建立陸標相對位置方程，然后通過非求逆方法確定探測器位置，具體為：將所有陸標視線方向轉到慣性系下，建立探測器的慣性位置方程；利用探測器的慣性位置方程，獲得陸標之間的相對位置方程，并聯立成方程組，然后使用最小二乘方法求解出探測器相對所有陸標的斜距；根據所述斜距的計算結果，以及探測器的慣性位置方程，針對每個陸標獲得一個探測器慣性位置的計算值，再取均值就得到探測器位置。

5.根據權利要求4所述的一種定常系數陸標圖像導航濾波方法，其特征在于，S10中，利用慣性測量單元獲得上一個周期的探測器本體角速度測量、探測器本體非引力加速度。

6.根據權利要求4所述的一種定常系數陸標圖像導航濾波方法，其特征在于，S20中，利用圖像導航敏感器獲得陸標視線方向測量。