一種ins/gps組合定位定姿新方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于導(dǎo)航及攝影測(cè)量技術(shù)領(lǐng)域，具體涉及一種INS/GPS組合定位定姿新方 法。
【背景技術(shù)】
[0002] 組合導(dǎo)航系統(tǒng)是以計(jì)算機(jī)為基礎(chǔ)，以最佳統(tǒng)計(jì)理論為方法，將載體上各種單一導(dǎo) 航設(shè)備采集的導(dǎo)航信息輸入計(jì)算機(jī)，經(jīng)過綜合處理，相互取長(zhǎng)補(bǔ)短，得到載體最佳的導(dǎo)航參 數(shù)，從而形成一個(gè)統(tǒng)一的整體。組合導(dǎo)航技術(shù)克服了單一導(dǎo)航系統(tǒng)中存在的各項(xiàng)不足，是 目前導(dǎo)航領(lǐng)域發(fā)展的主要方向。在基于移動(dòng)載體的攝影測(cè)量方面，主要采用全球定位系統(tǒng) (GPS)與慣性導(dǎo)航系統(tǒng)（INS)組合的導(dǎo)航方式直接獲取移動(dòng)載體的外方位元素，大大提高 了數(shù)據(jù)的處理速度及數(shù)據(jù)精度。
[0003] 其中，GPS是當(dāng)前應(yīng)用最為廣泛的衛(wèi)星導(dǎo)航定位系統(tǒng)，能夠提供全球性、全天候的 導(dǎo)航定位授時(shí)服務(wù)，具有定位精度高和誤差不隨時(shí)間積累的優(yōu)點(diǎn)，并且差分模式下定位精 度可達(dá)厘米計(jì)，但是，GPS信號(hào)具有易受干擾、信號(hào)遮擋、動(dòng)態(tài)環(huán)境中可靠性差、數(shù)據(jù)更新率 低、沒有姿態(tài)信息輸出等不足；而INS (Inertial Navigation System，慣性導(dǎo)航系統(tǒng)）是利 用固定在載體上的慣性測(cè)量裝置感受載體的運(yùn)動(dòng)，完全自主的輸出包括姿態(tài)在內(nèi)的全部導(dǎo) 航參數(shù)，還具有數(shù)據(jù)更新率高的優(yōu)點(diǎn)，但是存在導(dǎo)航誤差隨著導(dǎo)航時(shí)間的延長(zhǎng)而迅速積累 的問題，不能單獨(dú)長(zhǎng)時(shí)間工作?？梢奍NS與GPS具有非常好的互補(bǔ)特性，采用INS與GPS組 合的導(dǎo)航系統(tǒng)，一方面可以使用GPS定位誤差不隨時(shí)間積累的定位結(jié)果修正INS的導(dǎo)航結(jié) 果；另一方面，短時(shí)間內(nèi)高精度、高穩(wěn)定的INS導(dǎo)航結(jié)果可以很好的解決GPS信號(hào)失鎖的問 題，大大的提高了系統(tǒng)的魯棒性，是導(dǎo)航及攝影測(cè)量領(lǐng)域?qū)崟r(shí)獲取載體位置和姿態(tài)數(shù)據(jù)的 比較理想的組合方式。
[0004] INS/GPS組合導(dǎo)航的實(shí)質(zhì)是多傳感器的數(shù)據(jù)融合技術(shù)，數(shù)據(jù)濾波技術(shù)是其中的關(guān) 鍵技術(shù)之一。目前主要采用的數(shù)據(jù)濾波方法為卡爾曼濾波，卡爾曼濾波是一種線性最小方 差估計(jì)方法，采用狀態(tài)空間方法描述系統(tǒng)，算法采用遞推形式，便于在計(jì)算機(jī)上實(shí)現(xiàn)，可處 理時(shí)變系統(tǒng)、非平穩(wěn)信號(hào)、多維信號(hào)濾波等問題。INS/GPS組合導(dǎo)航屬于非線性系統(tǒng)，需采用 擴(kuò)展卡爾曼濾波方法將非線性系統(tǒng)轉(zhuǎn)為一個(gè)近似線性化的系統(tǒng)方程。但是，目前算法中大 多直接以系統(tǒng)的位置、速度以及姿態(tài)等值直接作為狀態(tài)量，沒有充分考慮系統(tǒng)中各項(xiàng)誤差 傳播情況，導(dǎo)致線性化誤差增大、雅閣比矩陣計(jì)算繁瑣、各項(xiàng)矩陣數(shù)值較大引起計(jì)算消耗極 大內(nèi)存空間，執(zhí)行時(shí)間冗長(zhǎng)、效率低等問題，極容易使系統(tǒng)出現(xiàn)誤差增大甚至結(jié)果發(fā)散的情 況，所以設(shè)計(jì)一種高效的數(shù)據(jù)濾波方法是非常有必要的。

【發(fā)明內(nèi)容】

[0005] 針對(duì)現(xiàn)有技術(shù)存在的缺陷，本發(fā)明提供一種INS/GPS組合定位定姿新方法，以位 置、速度、姿態(tài)的測(cè)量誤差作為狀態(tài)向量，以GPS與INS輸出的位置與速度之差作為觀測(cè)向 量，結(jié)合誤差協(xié)方差可以得到融合后的高精度位置和姿態(tài)數(shù)據(jù)，具有精度高、數(shù)據(jù)處理速度 快、硬件要求低等特點(diǎn)，可適用于低成本的INS結(jié)合GPS的定位定姿組合方案。
[0006] 本發(fā)明采用的技術(shù)方案如下：
[0007] 本發(fā)明提供一種INS/GPS組合定位定姿新方法，包括以下步驟：
[0008] S1，采用線性卡爾曼濾波器對(duì)GPS原始測(cè)量數(shù)據(jù)進(jìn)行濾波估計(jì)，輸出GPS導(dǎo)航最優(yōu) 估計(jì)值；其中，所述GPS導(dǎo)航最優(yōu)估計(jì)值包括位置最優(yōu)估計(jì)值和速度最優(yōu)估計(jì)值；
[0009] S2,以所述位置最優(yōu)估計(jì)值向INS提供初始位置信息，以所述速度最優(yōu)估計(jì)值向 INS提供初始速度信息，對(duì)INS原始測(cè)量數(shù)據(jù)進(jìn)行解算，得到INS導(dǎo)航信息；其中，所述INS 導(dǎo)航信息包括姿態(tài)、位置和速度信息；
[0010] S3,由于INS的數(shù)據(jù)采集頻率大于GPS的數(shù)據(jù)采集頻率，當(dāng)沒有GPS原始測(cè)量數(shù)據(jù) 時(shí)，所述INS導(dǎo)航信息即為最終輸出的數(shù)據(jù)；
[0011] 當(dāng)有GPS原始測(cè)量數(shù)據(jù)時(shí)，采用動(dòng)態(tài)誤差模型建立9階擴(kuò)展卡爾曼濾波器，融合 INS導(dǎo)航信息與GPS導(dǎo)航最優(yōu)估計(jì)值，反饋矯正同一時(shí)刻下的所述INS導(dǎo)航信息，輸出修正 融合后的最佳位置數(shù)據(jù)和姿態(tài)數(shù)據(jù)。
[0012] 優(yōu)選的，Sl具體包括以下步驟：
[0013] SI. 1，采用實(shí)時(shí)差分或事后差分的方式采集獲得所述GPS原始測(cè)量數(shù)據(jù)；
[0014] SI. 2,從所述GPS原始測(cè)量數(shù)據(jù)中提取經(jīng)度、緯度、高度、地速和航向信息，并將提 取的球面坐標(biāo)系下的經(jīng)度和緯度數(shù)據(jù)轉(zhuǎn)換為UTM投影坐標(biāo)系下的空間平面坐標(biāo)（X，Y);
[0015] SI. 3,假設(shè)運(yùn)動(dòng)載體做勻速直線運(yùn)動(dòng)且不考慮載體自身的動(dòng)力因素，以載體的空 間平面坐標(biāo)（X，Y)與平面速度（Vx，V y)為狀態(tài)向量，以GPS的空間二維平面坐標(biāo)和地速為 觀測(cè)向量，建立所述線性卡爾曼濾波器，所述線性卡爾曼濾波器的狀態(tài)方程如式1，其觀測(cè) 方程見式2 :
[0016] X (k+Ι) = ΦΧ (k) + Γ u (k) (1)
[0017] Z (k) = HX (k) +v (k) (2)
【主權(quán)項(xiàng)】
1. 一種INS/GPS組合定位定姿新方法，其特征在于，包括以下步驟： Sl，采用線性卡爾曼濾波器對(duì)GPS原始測(cè)量數(shù)據(jù)進(jìn)行濾波估計(jì)，輸出GPS導(dǎo)航最優(yōu)估計(jì) 值；其中，所述GPS導(dǎo)航最優(yōu)估計(jì)值包括位置最優(yōu)估計(jì)值和速度最優(yōu)估計(jì)值； 52, 以所述位置最優(yōu)估計(jì)值向INS提供初始位置信息，以所述速度最優(yōu)估計(jì)值向INS提 供初始速度信息，對(duì)INS原始測(cè)量數(shù)據(jù)進(jìn)行解算，得到INS導(dǎo)航信息；其中，所述INS導(dǎo)航信 息包括姿態(tài)、位置和速度信息； 53, 由于INS的數(shù)據(jù)采集頻率大于GPS的數(shù)據(jù)采集頻率，當(dāng)沒有GPS原始測(cè)量數(shù)據(jù)時(shí)， 所述INS導(dǎo)航信息即為最終輸出的數(shù)據(jù)； 當(dāng)有GPS原始測(cè)量數(shù)據(jù)時(shí)，采用動(dòng)態(tài)誤差模型建立9階擴(kuò)展卡爾曼濾波器，融合INS導(dǎo) 航信息與GPS導(dǎo)航最優(yōu)估計(jì)值，反饋矯正同一時(shí)刻下的所述INS導(dǎo)航信息，輸出修正融合后 的最佳位置數(shù)據(jù)和姿態(tài)數(shù)據(jù)。
2. 根據(jù)權(quán)利要求1所述的INS/GPS組合定位定姿新方法，其特征在于，Sl具體包括以 下步驟： SI. 1，采用實(shí)時(shí)差分或事后差分的方式采集獲得所述GPS原始測(cè)量數(shù)據(jù)； SI. 2,從所述GPS原始測(cè)量數(shù)據(jù)中提取經(jīng)度、緯度、高度、地速和航向信息，并將提取的 球面坐標(biāo)系下的經(jīng)度和緯度數(shù)據(jù)轉(zhuǎn)換為UTM投影坐標(biāo)系下的空間平面坐標(biāo)（X，Y); SI. 3,假設(shè)運(yùn)動(dòng)載體做勻速直線運(yùn)動(dòng)且不考慮載體自身的動(dòng)力因素，以載體的空間平 面坐標(biāo)（X，Y)與平面速度（Vx，V y)為狀態(tài)向量，以GPS的空間二維平面坐標(biāo)和地速為觀測(cè) 向量，建立所述線性卡爾曼濾波器，所述線性卡爾曼濾波器的狀態(tài)方程如式1，其觀測(cè)方程 見式2 : X(k+1) = ΦΧ〇〇 + Γιι〇〇 (I)
Z (k) = HX (k) +v (k) (2) 其中，k 表示離散化時(shí)刻，X = [X，Y，Vx，Vy]T，Z = [X，Y，Vx，ν』τ，Φ 為狀態(tài)轉(zhuǎn)移矩陣，
為控制向量矩陣
1觀測(cè)矩陣，Vx = VX |sin (h) I，Vy = VX |cos (h) I。u，V為零均值的過程噪聲和觀測(cè)噪聲，T為采樣周期，V 為GPS觀測(cè)的地速，h為以真北為參考基準(zhǔn)的地面航向； 51. 4,采用GPS接收機(jī)差分定位精度作為過程噪聲協(xié)方程Q，以GPS測(cè)量精度為觀測(cè)噪 聲協(xié)方差R，以SI. 2轉(zhuǎn)換后的平