一種新型GPS/DR組合導航系統

尹路明+張志恒+張小朋

摘 要： 利用三個陀螺儀和一個里程計提出一種新型GPS/DR組合導航系統。首先推導了這種組合導航系統的狀態方程，然后給出了松組合方式的濾波方程。最后用實測算例對這種新型組合導航系統進行了驗證，結果表明，相比于SINS系統、傳統的DR系統，這種DR系統在車載系統應用中具有更好的優勢；而且這種組合導航系統能夠很好地提高單一導航系統的導航精度。

關鍵詞： GPS； DR； GPS/DR 組合導航；Kalman濾波

中圖分類號： TN966?34 文獻標識碼： A 文章編號： 1004?373X（2014）13?0136?03

A new GPS/DR integrated navigation system

YIN Lu?ming， ZHANG Zhi?heng， ZHANG Xiao?peng

（Beijing Global Information Center of Application and Exploitation， Beijing 100094， China）

Abstract： In this paper， a new GPS/DR integrated navigation system consisting of three gyros and one odometer is presen?ted. The state equation of this integrated navigation system is deduced. The filtering equation of the loose integration mode of this new GPS/DR system is given. The new GPS/DR integrated navigation system was verified by actual calculation. The result shows that the new DR system has more superior than SINS system and traditional DR system， and this new GPS/DR integrated navigation system can improve the navigation accuracy the simplex navigation system.

Keywords： GPS； DR； GPS/DR integrated navigation； Kalman filtering

航位推算（Dead?Reckoning，DR）和全球定位系統（GPS）是目前在陸地車輛導航系統中最常用的兩種定位技術。但低成本DR系統定位誤差隨時間累積，不能單獨、長時間地使用[1?3]。而當GPS衛星信號受到遮擋時，又無法給出定位解或者定位精度很差。由此可見，單獨的DR、GPS系統均不能提供車輛導航所需的精確、連續、可靠的定位導航信息。通過組合導航技術，可以將上述兩種具有很強互補性的導航系統有機結合起來，構成GPS/DR組合導航系統[4?6]。以往的DR系統可以是由一個陀螺儀和一個里程計或者一個加速度計組成的一種二維系統，也可以是SINS系統。本文提出一種新型DR系統，由三個低成本陀螺儀和一個里程計組成，它既不同于傳統的DR系統也不同于慣性導航系統（SINS）。傳統的DR系統缺少高程信息，因此導航精度不高；而SINS需要安裝三個加速度計來測量比力，增加了設備的成本。

1 新型GPS/DR系統狀態方程

以地固坐標系為導航坐標系，系統狀態參數13維，分別為載體的位置（3維）、速度（3維）、姿態（3維）、陀螺漂移（3維）以及里程計刻度因子誤差（1維）[4，7]。則系統狀態方程為[7]：

[Xk=Fk，k-1Xk+GkWk] （1）

其中：[Xk=（δxδyδzδxδyδzφxφyφzεxεyεzk）T；][Fk，k-1]為連續系統的狀態轉移矩陣：

[Fk，k-1=03×3I3×3a13×3a23×303×3a33×303×3a43×301×1a53×303×303×3-ΩeieReb01×103×303×303×3Tg03×303×303×303×303×3Tk，][I3×3]為單位陣，[Ωeie]為[00ωeT]的反對稱陣，[Ωeie=0-ωe0ωe00000]，[ωe]是地球旋轉角速度，[Reb]是載體坐標系向地固坐標系的旋轉矩陣，[Tg]，[Tk]分別為陀螺儀和里程計誤差的相關事件。從[Fk，k-1]的表達式可以看出，與GPS/INS的狀態方程類似，但因為加速度計換成里程計，與速度有關的系數不同，這里暫時用[a]代替，下面再做討論。[Gk]為系統的動態噪聲矩陣；[Wk]為系統的過程白噪聲矢量。

將上式離散化簡寫可得：

[Xk=Φk，k-1Xk-1+wk] （2）

式中：[Xk，][Xk-1]分別為[k]和[k-1]歷元的狀態向量；[wk]為動力學模型噪聲向量，其協方差陣為[Σwk；][Φk，k-1]為離散后的狀態轉移矩陣。

一般來說，GPS/INS組合導航的時間間隔比較短，狀態轉換矩陣可依據下式得到：

[Φk，k-1=exp（Fk，k-1Δt）≈1+Fk，k-1Δt] （3）

下面討論[Φk，k-1]陣中與速度有關的系數。

載體坐標系中，由里程計可以得到載體的瞬時速度[Vb=（0v0）T，]轉換成地固坐標系的速度為：

[Ve=RebVb] （4）

對上式微分：

[δVe=δRebVb+RebδVb=ΩRebVb+RebδVb=ΩVe+RebδVb=-Ωeε+R12vR22vR32v?k] （5）

式中[Ωe]是[Ve]的反對稱陣，[Ωe=0-vezveyvez0-vex-veyvex0]。

2 新型GPS/DR系統觀測方程

三個陀螺儀和一個里程計組成的DR系統力學編排與INS系統類似，計算坐標轉換矩陣[Reb]的過程完全一致，這里就不詳細介紹，見參考文獻[7]。這里主要介紹由載體速度計算載體在地固坐標系中的位置和速度：

[VDR，k=RebVbk=R12vR22vR32v] （6）

[rDR，k=rDR，k-1+VeΔt] （7）

式中：[v=ffactor×pulse，][ffactor]是尺度系數，[pulse]是脈沖數，[rDR，k-1，][rDR，k]分別為DR推算的[k-1，][k]時刻的位置，[Δt]為采樣間隔。

采用松組合方式，取GPS和DR輸出的位置和速度之差作為觀測量，構造量測方程。設GPS、DR在地固坐標系中的位置和速度輸出分別為[rGPS，][VGPS]和[rDR，][VDR，]則令：

[Lk=rGPS-rDRVGPS-VDR] （8）

誤差方程為：

[Vk=AkXk-Lk] （9）

式中：[Ak]為量測矩陣，[Ak=I3×303×303×3I3×303×303×303×303×301×101×1，][Lk]為觀測向量，其協方差陣為[Σk，][Vk]為殘差向量，[Xk]為狀態參數向量。

3 Kalman濾波

結合狀態方程（2）和觀測方程（8），可得濾波解為：

[Xk=Xk+Kk（Zk-HkXk）] （10）

濾波增益矩陣為：

[Kk=ΣXkHTk（HkΣXkHTk+Σk）-1] （11）

預測向量的協方差陣為：

[ΣXk=Φk，k-1ΣXk-1ΦTk-1+Σwk-1] （12）

狀態向量的協方差陣為：

[ΣXk=（HTkΣ-1kHk+Σ-1Xk）-1] （13）

4 計算與比較

本文算例所用數據采集于2007年，GPS/DR組合系統安裝在汽車上，試驗路線如圖1所示。DR采樣頻率為100 Hz，GPS數據采樣周期為0.1 s，組合周期為0.1 s。算例中，下列參數由經驗確定：陀螺漂移和里程計刻度因子誤差相關時間分別為600 s，100 s，初始方差分別取1.0 deg/h和0.001；初始位置誤差為5 m，5 m，7 m；初始速度誤差為0.1 m/s；初始平臺失準角誤差分別為100.0 s，100.0 s和500.0 s。參考“真值”取高精度的載波相位雙差計算結果。

圖1 行車軌跡

首先，為了比較這種方法單獨導航的效果，GPS不參與導航，只是每10 s進行一次修正。下面給出SINS、一個陀螺儀一個里程計的DR系統（DR1）、三個陀螺儀一個里程計的DR系統（DR3）在維度方向的導航誤差，分別如圖2～圖4所示。

從圖2～圖4可以看出，SINS由于加速度計的誤差較大、二次積分的誤差以及車輪的碰撞、剎車等造成單獨導航誤差較大；而DR1系統由于是二維系統，缺少觀測俯仰角和翻滾角，所以單獨到航的誤差也較大；DR3系統結合了SINS系統和DR1系統的優點，充分考慮地勢不平坦的因素，采用三個陀螺儀測量載體的三個姿態角，利用車載系統只有向前運動的特點，采用里程計避免利用加速度計，從而達到較高的導航精度。

圖2 SINS緯度方向誤差

圖3 DR1緯度方向誤差

圖4 DR3緯度方向誤差

圖5～圖7是DR3系統與GPS系統組合導航的誤差圖，GPS單獨到航的精度15 m左右。

圖5 GPS/DR3 x方向誤差

圖6 GPS/DR3 y方向誤差

圖7 GPS/DR3 z方向誤差

從圖5～圖7可以看出，GPS/DR組合提高了導航的精度，發揮出GPS或者DR單獨導航所無法發揮的優勢。

5 結 論

由三個陀螺儀和一個里程計組成的GPS/DR組合導航系統，在GPS無法工作時，能夠充分考慮地勢不平坦的因素，采用三個陀螺儀測量載體的三個姿態角，利用車載系統只有向前運動的特點，采用里程計避免利用加速度計，從而達到較高的導航精度；而在GPS能夠正常工作時，能夠相互輔助，提高了了整體導航的精度，發揮出GPS或者DR單獨導航所無法發揮的優勢。

參考文獻

[1] 吳秋平，萬德鈞，徐曉蘇，等.車載GPS/DR組合導航系統的研究及其濾波算法[J].東南大學學報，1997，27（2）：55?59.

[2] 房建成，申功勛，萬德鈞，等.GPS/DR組合導航系統自適應擴展卡爾曼濾波模型的建立[J].控制理論與應用，1998，15（3）：385?389.

[3] 房建成，申功勛，萬德鈞.一種自適應聯合卡爾曼濾波器及其在車載GPSDR組合導航系統中的應用研究[J].中國慣性技術學報，1998，6（4）：1?6.

[4] 寇艷紅，張其善，李先亮.車載GPSDR組合導航系統的數據融合算法[J].北京航空航天大學學報，2003，29（3）：264?268.

[5] 寇艷紅，張其善，李先亮.車載GPSDR組合導航系統的信息融合新方案[J].遙測遙控，2002，23（1）：7?12.

[6] 柴艷菊，歐吉坤.GPS/DR組合導航中一種新的數據融合算法[J].武漢大學學報：信息科學版，2005，30（12）：1048?1051.

[7] 董緒榮，張守信，華仲春.GPS/INS組合導航定位及其應用[M].長沙：國防科技大學出版社，1998.

式中[Ωe]是[Ve]的反對稱陣，[Ωe=0-vezveyvez0-vex-veyvex0]。

2 新型GPS/DR系統觀測方程

三個陀螺儀和一個里程計組成的DR系統力學編排與INS系統類似，計算坐標轉換矩陣[Reb]的過程完全一致，這里就不詳細介紹，見參考文獻[7]。這里主要介紹由載體速度計算載體在地固坐標系中的位置和速度：

[VDR，k=RebVbk=R12vR22vR32v] （6）

[rDR，k=rDR，k-1+VeΔt] （7）

式中：[v=ffactor×pulse，][ffactor]是尺度系數，[pulse]是脈沖數，[rDR，k-1，][rDR，k]分別為DR推算的[k-1，][k]時刻的位置，[Δt]為采樣間隔。

采用松組合方式，取GPS和DR輸出的位置和速度之差作為觀測量，構造量測方程。設GPS、DR在地固坐標系中的位置和速度輸出分別為[rGPS，][VGPS]和[rDR，][VDR，]則令：

[Lk=rGPS-rDRVGPS-VDR] （8）

誤差方程為：

[Vk=AkXk-Lk] （9）

式中：[Ak]為量測矩陣，[Ak=I3×303×303×3I3×303×303×303×303×301×101×1，][Lk]為觀測向量，其協方差陣為[Σk，][Vk]為殘差向量，[Xk]為狀態參數向量。

3 Kalman濾波

結合狀態方程（2）和觀測方程（8），可得濾波解為：

[Xk=Xk+Kk（Zk-HkXk）] （10）

濾波增益矩陣為：

[Kk=ΣXkHTk（HkΣXkHTk+Σk）-1] （11）

預測向量的協方差陣為：

[ΣXk=Φk，k-1ΣXk-1ΦTk-1+Σwk-1] （12）

狀態向量的協方差陣為：

[ΣXk=（HTkΣ-1kHk+Σ-1Xk）-1] （13）

4 計算與比較

本文算例所用數據采集于2007年，GPS/DR組合系統安裝在汽車上，試驗路線如圖1所示。DR采樣頻率為100 Hz，GPS數據采樣周期為0.1 s，組合周期為0.1 s。算例中，下列參數由經驗確定：陀螺漂移和里程計刻度因子誤差相關時間分別為600 s，100 s，初始方差分別取1.0 deg/h和0.001；初始位置誤差為5 m，5 m，7 m；初始速度誤差為0.1 m/s；初始平臺失準角誤差分別為100.0 s，100.0 s和500.0 s。參考“真值”取高精度的載波相位雙差計算結果。

圖1 行車軌跡

首先，為了比較這種方法單獨導航的效果，GPS不參與導航，只是每10 s進行一次修正。下面給出SINS、一個陀螺儀一個里程計的DR系統（DR1）、三個陀螺儀一個里程計的DR系統（DR3）在維度方向的導航誤差，分別如圖2～圖4所示。

從圖2～圖4可以看出，SINS由于加速度計的誤差較大、二次積分的誤差以及車輪的碰撞、剎車等造成單獨導航誤差較大；而DR1系統由于是二維系統，缺少觀測俯仰角和翻滾角，所以單獨到航的誤差也較大；DR3系統結合了SINS系統和DR1系統的優點，充分考慮地勢不平坦的因素，采用三個陀螺儀測量載體的三個姿態角，利用車載系統只有向前運動的特點，采用里程計避免利用加速度計，從而達到較高的導航精度。

圖2 SINS緯度方向誤差

圖3 DR1緯度方向誤差

圖4 DR3緯度方向誤差

圖5～圖7是DR3系統與GPS系統組合導航的誤差圖，GPS單獨到航的精度15 m左右。

圖5 GPS/DR3 x方向誤差

圖6 GPS/DR3 y方向誤差

圖7 GPS/DR3 z方向誤差

從圖5～圖7可以看出，GPS/DR組合提高了導航的精度，發揮出GPS或者DR單獨導航所無法發揮的優勢。

5 結 論

由三個陀螺儀和一個里程計組成的GPS/DR組合導航系統，在GPS無法工作時，能夠充分考慮地勢不平坦的因素，采用三個陀螺儀測量載體的三個姿態角，利用車載系統只有向前運動的特點，采用里程計避免利用加速度計，從而達到較高的導航精度；而在GPS能夠正常工作時，能夠相互輔助，提高了了整體導航的精度，發揮出GPS或者DR單獨導航所無法發揮的優勢。

參考文獻

[1] 吳秋平，萬德鈞，徐曉蘇，等.車載GPS/DR組合導航系統的研究及其濾波算法[J].東南大學學報，1997，27（2）：55?59.

[2] 房建成，申功勛，萬德鈞，等.GPS/DR組合導航系統自適應擴展卡爾曼濾波模型的建立[J].控制理論與應用，1998，15（3）：385?389.

[3] 房建成，申功勛，萬德鈞.一種自適應聯合卡爾曼濾波器及其在車載GPSDR組合導航系統中的應用研究[J].中國慣性技術學報，1998，6（4）：1?6.

[4] 寇艷紅，張其善，李先亮.車載GPSDR組合導航系統的數據融合算法[J].北京航空航天大學學報，2003，29（3）：264?268.

[5] 寇艷紅，張其善，李先亮.車載GPSDR組合導航系統的信息融合新方案[J].遙測遙控，2002，23（1）：7?12.

[6] 柴艷菊，歐吉坤.GPS/DR組合導航中一種新的數據融合算法[J].武漢大學學報：信息科學版，2005，30（12）：1048?1051.

[7] 董緒榮，張守信，華仲春.GPS/INS組合導航定位及其應用[M].長沙：國防科技大學出版社，1998.

式中[Ωe]是[Ve]的反對稱陣，[Ωe=0-vezveyvez0-vex-veyvex0]。

2 新型GPS/DR系統觀測方程

三個陀螺儀和一個里程計組成的DR系統力學編排與INS系統類似，計算坐標轉換矩陣[Reb]的過程完全一致，這里就不詳細介紹，見參考文獻[7]。這里主要介紹由載體速度計算載體在地固坐標系中的位置和速度：

[VDR，k=RebVbk=R12vR22vR32v] （6）

[rDR，k=rDR，k-1+VeΔt] （7）

式中：[v=ffactor×pulse，][ffactor]是尺度系數，[pulse]是脈沖數，[rDR，k-1，][rDR，k]分別為DR推算的[k-1，][k]時刻的位置，[Δt]為采樣間隔。

采用松組合方式，取GPS和DR輸出的位置和速度之差作為觀測量，構造量測方程。設GPS、DR在地固坐標系中的位置和速度輸出分別為[rGPS，][VGPS]和[rDR，][VDR，]則令：

[Lk=rGPS-rDRVGPS-VDR] （8）

誤差方程為：

[Vk=AkXk-Lk] （9）

式中：[Ak]為量測矩陣，[Ak=I3×303×303×3I3×303×303×303×303×301×101×1，][Lk]為觀測向量，其協方差陣為[Σk，][Vk]為殘差向量，[Xk]為狀態參數向量。

3 Kalman濾波

結合狀態方程（2）和觀測方程（8），可得濾波解為：

[Xk=Xk+Kk（Zk-HkXk）] （10）

濾波增益矩陣為：

[Kk=ΣXkHTk（HkΣXkHTk+Σk）-1] （11）

預測向量的協方差陣為：

[ΣXk=Φk，k-1ΣXk-1ΦTk-1+Σwk-1] （12）

狀態向量的協方差陣為：

[ΣXk=（HTkΣ-1kHk+Σ-1Xk）-1] （13）

4 計算與比較

本文算例所用數據采集于2007年，GPS/DR組合系統安裝在汽車上，試驗路線如圖1所示。DR采樣頻率為100 Hz，GPS數據采樣周期為0.1 s，組合周期為0.1 s。算例中，下列參數由經驗確定：陀螺漂移和里程計刻度因子誤差相關時間分別為600 s，100 s，初始方差分別取1.0 deg/h和0.001；初始位置誤差為5 m，5 m，7 m；初始速度誤差為0.1 m/s；初始平臺失準角誤差分別為100.0 s，100.0 s和500.0 s。參考“真值”取高精度的載波相位雙差計算結果。

圖1 行車軌跡

首先，為了比較這種方法單獨導航的效果，GPS不參與導航，只是每10 s進行一次修正。下面給出SINS、一個陀螺儀一個里程計的DR系統（DR1）、三個陀螺儀一個里程計的DR系統（DR3）在維度方向的導航誤差，分別如圖2～圖4所示。

從圖2～圖4可以看出，SINS由于加速度計的誤差較大、二次積分的誤差以及車輪的碰撞、剎車等造成單獨導航誤差較大；而DR1系統由于是二維系統，缺少觀測俯仰角和翻滾角，所以單獨到航的誤差也較大；DR3系統結合了SINS系統和DR1系統的優點，充分考慮地勢不平坦的因素，采用三個陀螺儀測量載體的三個姿態角，利用車載系統只有向前運動的特點，采用里程計避免利用加速度計，從而達到較高的導航精度。

圖2 SINS緯度方向誤差

圖3 DR1緯度方向誤差

圖4 DR3緯度方向誤差

圖5～圖7是DR3系統與GPS系統組合導航的誤差圖，GPS單獨到航的精度15 m左右。

圖5 GPS/DR3 x方向誤差

圖6 GPS/DR3 y方向誤差

圖7 GPS/DR3 z方向誤差

從圖5～圖7可以看出，GPS/DR組合提高了導航的精度，發揮出GPS或者DR單獨導航所無法發揮的優勢。

5 結 論

由三個陀螺儀和一個里程計組成的GPS/DR組合導航系統，在GPS無法工作時，能夠充分考慮地勢不平坦的因素，采用三個陀螺儀測量載體的三個姿態角，利用車載系統只有向前運動的特點，采用里程計避免利用加速度計，從而達到較高的導航精度；而在GPS能夠正常工作時，能夠相互輔助，提高了了整體導航的精度，發揮出GPS或者DR單獨導航所無法發揮的優勢。

參考文獻

[1] 吳秋平，萬德鈞，徐曉蘇，等.車載GPS/DR組合導航系統的研究及其濾波算法[J].東南大學學報，1997，27（2）：55?59.

[2] 房建成，申功勛，萬德鈞，等.GPS/DR組合導航系統自適應擴展卡爾曼濾波模型的建立[J].控制理論與應用，1998，15（3）：385?389.

[3] 房建成，申功勛，萬德鈞.一種自適應聯合卡爾曼濾波器及其在車載GPSDR組合導航系統中的應用研究[J].中國慣性技術學報，1998，6（4）：1?6.

[4] 寇艷紅，張其善，李先亮.車載GPSDR組合導航系統的數據融合算法[J].北京航空航天大學學報，2003，29（3）：264?268.

[5] 寇艷紅，張其善，李先亮.車載GPSDR組合導航系統的信息融合新方案[J].遙測遙控，2002，23（1）：7?12.

[6] 柴艷菊，歐吉坤.GPS/DR組合導航中一種新的數據融合算法[J].武漢大學學報：信息科學版，2005，30（12）：1048?1051.

[7] 董緒榮，張守信，華仲春.GPS/INS組合導航定位及其應用[M].長沙：國防科技大學出版社，1998.