[發明專利]一種INS/GPS組合定位定姿新方法

在審

• 申請號: 201310553102.1
• 申請日: 2013-11-08
• 公開（公告）號: CN104635251A
• 公開（公告）日: 2015-05-20
• 發明（設計）人: 鄭新奇;孫義博;賈宗仁;艾剛;王慶
• 申請（專利權）人: 中國地質大學（北京）
• 主分類號: G01S19/49
• 分類號: G01S19/49
• 代理公司: 北京市盛峰律師事務所 11337
• 代理人: 趙建剛

1.一種INS/GPS組合定位定姿新方法，其特征在于，包括以下步驟：

S1，采用線性卡爾曼濾波器對GPS原始測量數據進行濾波估計，輸出GPS導航最優估計值；其中，所述GPS導航最優估計值包括位置最優估計值和速度最優估計值；

S2，以所述位置最優估計值向INS提供初始位置信息，以所述速度最優估計值向INS提供初始速度信息，對INS原始測量數據進行解算，得到INS導航信息；其中，所述INS導航信息包括姿態、位置和速度信息；

S3，由于INS的數據采集頻率大于GPS的數據采集頻率，當沒有GPS原始測量數據時，所述INS導航信息即為最終輸出的數據；

當有GPS原始測量數據時，采用動態誤差模型建立9階擴展卡爾曼濾波器，融合INS導航信息與GPS導航最優估計值，反饋矯正同一時刻下的所述INS導航信息，輸出修正融合后的最佳位置數據和姿態數據。

2.根據權利要求1所述的INS/GPS組合定位定姿新方法，其特征在于，S1具體包括以下步驟：

S1.1，采用實時差分或事后差分的方式采集獲得所述GPS原始測量數據；

S1.2，從所述GPS原始測量數據中提取經度、緯度、高度、地速和航向信息，并將提取的球面坐標系下的經度和緯度數據轉換為UTM投影坐標系下的空間平面坐標(X，Y)；

S1.3，假設運動載體做勻速直線運動且不考慮載體自身的動力因素，以載體的空間平面坐標(X，Y)與平面速度(V_x，V_y)為狀態向量，以GPS的空間二維平面坐標和地速為觀測向量，建立所述線性卡爾曼濾波器，所述線性卡爾曼濾波器的狀態方程如式1，其觀測方程見式2：

X(k+1)＝ΦX(k)+Γu(k)????????????(1)

Z(k)＝HX(k)+v(k)?????????????????(2)

其中，k表示離散化時刻，X＝[X，Y，V_x，V_y]^T，Z＝[X，Y，V_x，V_y]^T，Φ=10T0010T00100001]]>為狀態轉移矩陣，Γ=0.5T20T000.5T20T]]>為控制向量矩陣，H=1000010000100001]]>為觀測矩陣，V_x＝V×|sin(h)|，V_y＝V×|cos(h)|。u，v為零均值的過程噪聲和觀測噪聲，T為采樣周期，V為GPS觀測的地速，h為以真北為參考基準的地面航向；

S1.4，采用GPS接收機差分定位精度作為過程噪聲協方程Q，以GPS測量精度為觀測噪聲協方差R，以S1.2轉換后的平面坐標(X，Y)以及高度、地速和航向信息作為線性卡爾曼濾波器初始輸入值，給定初始值X(0|0)及初始協方程P(0|0)后，對GPS原始測量數據進行卡爾曼濾波估計，待狀態量更新后，輸出位置及速度的最優估計值；其中，所述位置最優估計值包括轉換到地理坐標系下的經緯度值以及GPS高程值，即：經度μ、緯度λ和高程值h；所述速度最優估計值為載體地速V。