[發明專利]一種角速度輔助的Kalman濾波定位方法

有效

• 申請號: 201610905124.3
• 申請日: 2016-10-18
• 公開（公告）號: CN107918139B
• 公開（公告）日: 2021-05-11
• 發明（設計）人: 賈小波;王倩倩;鄭丹旸;吳淑琴;鄒世合;李軍華
• 申請（專利權）人: 鄭州威科姆科技股份有限公司
• 主分類號: G01S19/42
• 分類號: G01S19/42
• 代理公司: 鄭州中原專利事務所有限公司 41109
• 代理人: 霍彥偉;李想

1.一種角速度輔助的Kalman濾波定位方法，其特征在于：包括如下步驟：

步驟1、運動載體的接收機捕獲并跟蹤衛星信號，解調衛星導航電文，剔除故障衛星和異常衛星；

步驟2、利用接收到的前幾個歷元的衛星信息解算運動載體的初始狀態變量X₀、方位角S₀、角速度Vr₀、方差P₀；

步驟3、確定采用的Kalman濾波方程模型，假設沒有系統控制輸入，根據不同載體類型和不同的接收機類型確定以下參數：過程激勵噪聲協方差矩陣為Q，觀測噪聲協方差矩陣為R，轉換矩陣Φ，測量矩陣H，馬爾可夫過程在三個坐標軸方向的相關時間常數τ_x、τ_y、τ_z；

取狀態變量為X＝[x v_x a_x ε_x y v_y a_y ε_y z v_z a_z ε_z]^T，觀測變量Z＝[L_x L_vx L_y L_vy L_zL_vz]^T；

其中，分別為X、Y、Z軸上的加速度方差矩陣，分別為X、Y、Z軸上的定位誤差方差矩陣；其中(x v_x a_x)、(y v_y a_y)、(z v_z a_z)3組狀態變量分別為xyz三個坐標軸上的位置、速度、加速度分量，ε_x、ε_y、ε_z分別為各種誤差源在三個坐標軸方向造成的總位置誤差，觀測變量(L_x L_vx)、(L_y L_vy)、(L_z L_vz)分別為接收機解算出的xyz三個坐標軸上的位置、速度分量；

R＝diag[R_x R_y R_z]，其中R_x、R_y、R_z分別為X、Y、Z軸上的觀測噪聲協方差分量；

步驟4、根據現時刻接收衛星信息初步解算位置、速度、載體運行方位角，然后根據前一時刻的位置、速度、加速度，利用卡爾曼濾波方程解算現時刻的位置、速度和加速度；

具體包括如下步驟：

步驟401、檢測接收衛星信息是否良好以解算當前載體運行方位角，若是，利用如下卡爾曼濾波方程解算當前時刻的狀態變量返回退出；若否，轉到步驟402；

K_k＝P_(k，k-1)H^T(HP_(k，k-1)H^T+R)^-1

P_(k，k-1)＝ΦP_k-1Φ^T+Q

P_k＝(I-K_kH)P_(k，k-1)

式中，是利用K-1時刻狀態變量預測的K時刻狀態變量預測結果，是K-1時刻的狀態變量最優結果，是K時刻的狀態變量最優估算值，K_k為卡爾曼增益；P_k為后驗估計誤差的協方差矩陣，P_(k，k-1)為先驗估計誤差的協方差矩陣，Z_k是K時刻觀測變量；

步驟402、將k-1時刻狀態變量X_k-1＝[x，v_x，a_x，ε_x，y，v_y，a_y，ε_y，z，v_z，a_z，ε_z]^T由XYZ坐標系轉換為NEU坐標系，設NEU坐標系下的速度變量為Vn0、Ve0、Vh0：

步驟403、計算添加角速度輔助后的速度變量：

Vn＝(Vn0*Vn0+Ve0*Ve0)^1/2*cos(S_k-1)

Ve＝(Vn0*Vn0+Ve0*Ve0)^1/2*sin(S_k-1)

Vh＝Vh0

步驟404、令Vr_k-1＝Vr_k-2，Vr_k-1、Vr_k-2分別是K-1、K-2時刻的角速度；根據輔助方程角速度：Vr_k-1＝(S_k-S_k-1)/T計算S_k＝S_k-1+Vr_k-1*T，S_k是K時刻的載體運行方位角，T為觀測時間間隔；步驟405、將NEU坐標系下的速度變量Vn、Ve、Vh轉換為XYZ坐標系，設此時XYZ坐標系下的速度變量相應為v_x1、v_y1、v_z1，令

步驟406、聯合Kalman濾波方程，兩者進行等權計算現時刻狀態變量預測結果：返回退出。