[發明專利]一種定位定向設備速度補償方法

有效

• 申請號: 201611037069.7
• 申請日: 2016-11-23
• 公開（公告）號: CN108088443B
• 公開（公告）日: 2021-06-08
• 發明（設計）人: 馬濤;朱紅;李永峰;石志興;王根;郭元江
• 申請（專利權）人: 北京自動化控制設備研究所
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 核工業專利中心 11007
• 代理人: 張檑

1.一種定位定向設備速度補償方法，其特征在于：包括以下步驟：

步驟1、建立卡爾曼濾波模型

定義坐標系如下：

n系：導航坐標系ox_ny_nz_n，為東北天地理坐標系，x_n軸指向東，y_n軸指向北，z_n軸指向天；

b系：載體坐標系ox_by_bz_b，為右前上右坐標系，x_b軸指向載體的右方，y_b軸指向載體的前方，z_b軸指向載體的上方；

取狀態向量X為

其中，

[δV_e，δV_n，δV_u]為速度誤差矢量，δV_e為東向速度誤差、δV_n為北向速度誤差、δV_u為天向速度誤差；

[φ_e，φ_n，φ_u]為姿態誤差矢量，φ_e為東向姿態誤差角、φ_n為北向姿態誤差角、φ_u為天向姿態誤差角；

[δλ，δL，δh]為位置誤差矢量，δλ為經度誤差、δL為緯度誤差、δh為高度誤差；

為加速度計零偏矢量，為x_b軸加速度計零偏量、為y_b軸加速度計零偏量、為z_b軸加速度計零偏量；

[ε_x，ε_y，ε_z]為陀螺漂移矢量，ε_x為x_b軸陀螺漂移量、ε_y為y_b軸陀螺漂移量、ε_z為z_b軸陀螺漂移量；

[φ_ax，δK_D，φ_az]為里程計誤差矢量，φ_ax為x_b軸安裝誤差、δK_D為里程計刻度系數誤差、φ_az為z_b軸安裝誤差；

Δt：為慣導、里程計速度間的時間延遲；

[R_x，R_y，R_z]為桿臂矢量，R_x為x_b軸方向桿臂、R_y為y_b軸方向桿臂、R_z為z_b軸方向桿臂；

狀態方程為：

其中，G為系統噪聲矩陣，W為系統噪聲，A為系統狀態矩陣，

為從載體坐標系ox_by_bz_b到導航坐標系ox_ny_nz_n的坐標變換矩陣，通過導航解算得到；

ω_ie為地球自轉角速率，R_M和R_N分別為地球子午圈和卯酉圈半徑，L為緯度，V_u為天向速度，V_n為北向速度，V_e為東向速度，f_e、f_n和f_u分別為東北天向的等效加速度計測量值；

量測方程為：

Z＝HX+V

其中，Z為量測量，H為量測矩陣，V為量測噪聲；

分別為等效速度在東向、北向和天向的分量；

為坐標變換矩陣的第i行第j列元素，i＝1，2，3；j＝1，2，3；

V_D為里程計輸出等效速度；

為陀螺測量值向量；

分別為東北天向速度的一階導數；

步驟2、系統初始化

對慣導系統進行對準，并開始慣性導航解算，獲得姿態角[θ，γ，ψ]^T，速度和[λ，φ，h]^T，同時通過里程計獲得位移ΔS；

ΔS＝K_D·N_pluse為標量，K_D為里程計刻度系數，N_pluse為里程計輸出脈沖；

對準方法采用靜基座對準或動基座對準方法；

步驟3、天向和水平位移分量計算

根據步驟2導航解算得到的坐標變換矩陣計算里程計位移的天向分量和水平分量；

為3×3矩陣，則里程計輸出位移的天向分量為：

水平分量為：

其中為的第3行第2列的元素；

步驟4、東向和北向位移分量計算

根據步驟2導航解算得到的速度計算里程計位移的東向分量和北向分量；具體方法如下：

其中，為程計位移的東向分量，為程計位移的北向分量；

步驟5、等效速度計算，并更新卡爾曼濾波模型信息

計算濾波周期內的等效速度，具體公式如下：

其中，為等效速度，ΔSⁿ(t)為ΔSⁿ在時刻t的值，T_e為濾波周期，T_n為導航解算周期；

更新系統狀態矩陣A、量測矩陣H，計算量測量Z；

步驟6、對系統進行卡爾曼濾波計算；

步驟7、對慣導姿態陣、速度、位置誤差進行修正，對里程計刻度系數和安裝誤差進行修正；

其中，姿態誤差為φ＝[X(3) X(4) X(5)]^T，

則慣導姿態陣誤差修正公式為I為單位矩陣；

慣導速度誤差為δV＝[X(0) X(1) X(2)]^T，

則速度誤差修正方法為Vⁿ＝Vⁿ-δV；

慣導位置誤差修正方法為

λ＝λ-X (6)

L＝L-X (7)

h＝h-X (8)

里程計刻度系數誤差為δK_D＝X(15)，

則里程計刻度系數誤差修正方法為K_D＝K_D×(1+δK_D)；

各修正量使用完畢后相應的狀態量置為零。