[發明專利]一種基于輸出校正的容錯水平阻尼方法

有效

• 申請號: 202011368821.2
• 申請日: 2020-11-30
• 公開（公告）號: CN112129289B
• 公開（公告）日: 2021-02-05
• 發明（設計）人: 馮國虎;吳文啟;劉潺
• 申請（專利權）人: 中國人民解放軍國防科技大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C25/00
• 代理公司: 長沙國科天河知識產權代理有限公司 43225
• 代理人: 周達

1.一種基于輸出校正的容錯水平阻尼方法，其特征在于，包括：

第1步：裝訂初始參數，包括裝訂無阻尼的初始導航參數；

第2步：純慣導解算當前時刻無阻尼導航參數；

第3步：獲取當前時刻外參考速度、無阻尼速度與上一時刻的外參考速度、無阻尼速度，判斷是否符合判據并且，其中為預設的判據閾值；當前外參考速度不符合判據時，則將當前時刻的無阻尼導航參數賦值給當前時刻的阻尼后的導航參數并輸出，然后跳至第2步直至導航解算結束；當當前時刻的外參考速度符合判據時，而上一時刻的數據不符合判據，進行水平速度阻尼，記錄下當前時刻T_i為阻尼起始時間即T_D=T_i，得到當前時刻的阻尼后速度的方法是：

通過當前時刻的外參考速度、無阻尼速度，以及上兩個時刻的外參考速度、，無阻尼速度、和阻尼后速度、，采用公式

得到當前時刻的阻尼后速度，其中num(1)、num(2)、num(3)、dum(1)、dum(2)、dum(3)為基于輸出校正水平阻尼傳遞函數離散時間表達式系數，num(1)=1.0，num(2)=-1.99997367841154，num(3)=0.999975215058425，dum(1)=1.0，dum(2)=-1.998759231835443，dum(3)=0.998760768482328；

第4步：通過當前時刻的阻尼后速度采用位置增量方式解算當前時刻的阻尼后的位置；

第5步：通過當前時刻的阻尼后的位置解算阻尼后的姿態矩陣；

第6步：輸出當前時刻的阻尼后速度、位置和姿態矩陣，跳至第2步直至導航解算結束。

2.根據權利要求1所述的基于輸出校正的容錯水平阻尼方法，其特征在于：第1步中初始導航參數包含東北天三個方向的初始速度

，還包含經度、緯度的初始位置和初始姿態矩陣。

3.根據權利要求1所述的基于輸出校正的容錯水平阻尼方法，其特征在于：第2步中，根據IMU慣性測量單元實時提供的旋轉角速度、比力矢量和上一時刻的導航參數、、，按純慣導算法解算得到當前時刻的無阻尼導航參數、、。

4.根據權利要求1、2或3所述的基于輸出校正的容錯水平阻尼方法，其特征在于：第3步中，取值區間為。

5.根據權利要求4所述的基于輸出校正的容錯水平阻尼方法，其特征在于：第3步中，通過水速計獲取當前時刻外參考速度、無阻尼速度與上一時刻的外參考速度、無阻尼速度。

6.根據權利要求4所述的基于輸出校正的容錯水平阻尼方法，其特征在于：第4步中，判斷速度連續阻尼的時間是否超過小時，即如果，則；如果，則根據當前時刻無阻尼的位置、阻尼后的速度和外參考速度以及上一時刻無阻尼的位置，阻尼后的位置、速度和外參考速度，采用位置增量計算公式

得到當前時刻的阻尼后位置，其中R_N和R_E分別是子午面曲率半徑和橫向曲率半徑，h是海拔高度；、分別是時刻阻尼后的緯度、經度，、分別是時刻阻尼后的緯度、經度，、分別是時刻無阻尼的緯度、經度，，分別是時刻無阻尼的緯度、經度，、分別是時刻阻尼后慣導導航解算的北向、東向速度，、分別是時刻外參考速度在北向、東向上的分速度，、分別是時刻阻尼后慣導導航解算的北向、東向速度，、分別是時刻外參考速度在北向、東向上的分速度。

7.根據權利要求6所述的基于輸出校正的容錯水平阻尼方法，其特征在于：第4步中，取1.5。

8.根據權利要求1所述的基于輸出校正的容錯水平阻尼方法，其特征在于：第5步中，根據當前時刻無阻尼的位置計算出地球系至無阻尼導航系轉移矩陣，根據當前時刻的阻尼后位置計算出地球系至阻尼導航系轉移矩陣，阻尼后的姿態矩陣由公式計算得到。

9.根據權利要求8所述的基于輸出校正的容錯水平阻尼方法，其特征在于：第5步中，地球系至無阻尼導航系轉移矩陣為：

；

地球系至阻尼導航系轉移矩陣為：

。