基于優化理論的GPS定位解算算法

趙 鑫 秦紅磊 郎榮玲

(北京航空航天大學 電子信息工程學院，北京 100191)

GPS(Global Positioning System)定位解算是根據偽距、偽距率等測量值，計算接收機的位置P、速度V和時間T等信息的過程.在現有的接收機中一般采用最小二乘方法和EKF(Extended Kalman Filter)作為定位解算的方法.最小二乘方法簡單易于實現且計算速度快，但是定位精度低且魯棒性差.EKF算法是近似的非線性濾波方法，其精度高于最小二乘法，但EKF只是對非線性的系統及觀測模型進行局部的線性化，并未完全解決系統的非線性濾波問題［1－5］.

針對以上傳統定位解算方法存在的問題，本文提出了一種新的定位解算算法——基于優化理論的最大后驗估計算法.可以在擁有較高精度的同時采用一種非線性的估計方法進行定位解算.該方法用優化理論的思路求解系統狀態量的最大后驗概率估計值.它是從系統狀態量、觀測量的聯合概率密度函數出發，將估計問題轉化成優化問題，用優化問題的解法對系統的狀態進行估計.

本文的主要內容是:首先介紹GPS接收機中定位解算的基本原理;在此基礎上，介紹了GPS接收機基于優化理論的最大后驗估計定位解算算法的推導過程，以及推導出的優化問題的求解方法.最后，本文采用基于模擬器產生的數據進行了實驗驗證，有效證明了新提出的GPS定位解算方法能完全解決定位解算中的非線性問題，并擁有較高的定位精度.

1 GPS接收機定位解算原理

空中某點的位置可以通過測量它到空中一些已知位置的距離來獲得.在空間三維情況下，由于測量得到的接收機到衛星之間的距離中存在著未知的固定偏差，所以定位解算至少需要4顆衛星和4個距離才能確定用戶的位置.式(1)、式(2)即為由偽距、偽距率確定用戶位置的基本方程［6－7］.

通過GPS接收機對m(m≥4)顆衛星信號傳輸時間的測量可得到用戶與m顆相應衛星的偽距，列方程得

通過GPS接收機對m(m≥4)顆衛星信號多普勒頻移的測量可得到用戶與m顆相應衛星的偽距率，對式(1)求時間的導數，可得偽距變化率方程:

式中，ρi為第 i顆衛星測得的偽距離;(xi，yi，zi)為已知衛星位置;Δtu為待求用戶鐘差;為第i顆衛星測得的偽距離變化率;)為已知衛星速度;(x，y，z)為待求用戶位置，由位置解算得到為待求的用戶三維速度;為待求用戶鐘差變化率.

在下面推導的定位解算新方法中，將式(1)、式(2)作為定位解算系統建模中的量測方程.

2 基于優化理論的定位解算算法

定位解算過程的數學模型屬于量測模型非線性，觀測噪聲和驅動噪聲是高斯白噪聲的非線性高斯系統.

下面將具體講述定位解算新方法——基于優化理論的最大后驗概率估計方法的推導.

2.1 狀態模型

GPS導航系統狀態模型可取為

其中

觀測矩陣F可取為

式中，(x，y，z)為用戶在大地坐標系中的三維坐標;(vx，vy，vz)為用戶在大地坐標系中的三維速度;Δt，Δf為用戶接收機的時差與頻差;wVX，wVY，wVZ為高斯白噪聲.

2.2 算法的數學推導

在給定的系統狀態方程、量測方程模型下，用優化算法對給定的系統狀態量確定一種估計，這種估計使系統狀態量的后驗概率最大［8－9］.

假設定位解算系統的模型可以簡寫為

這里，h 關于 X 連續可微，Xk∈Rm，Wk∈Rm，Zk∈Rr并且 Vk∈Rr，r為可見衛星顆數的兩倍，m為狀態量的個數，這里取8.在模型中序列{Wk}，{Vk}，X0的分布滿足以下概率密度函數:

利用概率密度函數變換規則，由模型式(4)可以得到條件概率密度函數:

將式(8)、式(9)代入式(10)可得到:

其中α為常量.

通過上述步驟就把在非線性高斯系統假設下的最大后驗估計轉化成了一個優化問題.問題的的形式如下:

其中

此優化問題將采用動態規劃方法進行求解.動態規劃是解決多級決策過程最優化的一種數學方法，是根據一類多級決策問題的特點，把多級決策問題變換為一系列互相聯系的單級決策問題，然后逐個加以解決.

下面將用正向動態規劃算法求解非線性系統模型下狀態量的最優估計［10］.

由式(12)可知，非線性高斯系統假設下0～k時刻的總代價函數為

其中

定義0時刻的部分代價函數為

用動態規劃方法可得1時刻的部分代價函數的表達式:

對于k大于等于1的時刻，部分代價函數的表達式可表示為

這樣，就把多階段決策問題轉化為一系列單階段決策問題，利用各階段之間的關系，逐個求解就可得到系統各個時刻狀態量的最大后驗估計值.

3 實驗驗證

為了驗證算法的有效性，取EKF的濾波結果作為比較.仿真分析將分別以模擬產生的GPS測量偽距值、測量偽距率值以及實際環境中軟件接收機產生的實際數據作為觀測量.分別采用EKF、基于優化理論的定位解算算法進行濾波.

首先驗證基于優化理論的定位解算算法的有效性，接收機運動狀態具體描述如下:

GPS測量偽距、偽距率模擬器初始位置為東經116.34°，北緯39.98°，高度102.97m，以20m/s向東勻速行進.數據輸出頻率1Hz.觀測噪聲為高斯噪聲，偽距量測噪聲方差為3.0×3.0m2，偽距率量測噪聲為0.1 ×0.1(m/s)2.

基于優化理論的最大后驗估計算法對數據處理后的誤差波形圖如圖1～圖5所示.

圖1 x軸方向定位誤差

圖2 y軸方向定位誤差

圖3 z軸方向定位誤差

下面分別利用EKF、基于優化理論的定位解算算法對數據進行濾波估計以實現定位，并對定位結果進行比較分析.

此項實驗的軟件接收機運動狀態具體描述如下:

圖4 鐘差估計誤差

圖5 頻差估計誤差

GPS軟件接收機初始位置為東經116.34°，北緯 39.98°，高度 73.21m，接收機狀態靜止.數據輸出頻率1Hz.觀測噪聲為高斯噪聲.實驗數據為實際環境中軟件接收機接收衛星信號后經過數據處理測得的偽距值、偽距率值.

EKF和基于優化理論的最大后驗估計算法對數據處理后的誤差波形圖如圖6～圖8所示.

圖6 x軸方向定位曲線

圖7 y軸方向定位曲線

圖8 z軸方向定位曲線

兩種濾波方法在高斯噪聲下的統計特性如表1所示.

表1 兩種濾波方式定位結果誤差對比

由上面的實驗結果可以看出，基于優化理論的最大后驗估計算法在x，y軸方向定位精度要略低于EKF，在z軸方向上的定位精度要略高于EKF.

4 結束語

本文基于優化理論的思想，提出了一種新的定位解算算法.該方法采用優化理論的思路求解系統狀態量的最大后驗估計值.仿真實驗的結果表明:該方法在擁有較高精度的同時采用一種非線性的估計方法進行定位解算，能完全解決定位解算中的非線性問題.

References)

［1］ Zhang Haitao，Zhao Yujiao.The performance comparison and analysis of extended Kalman filters for GPS/DR navigation［J］.OPTIK，2011，122(9):777 －781

［2］ Hong Deng，Liu Guangbin，Chen Haoming，et al.Application of EKF for missile attitude estimation based on"SINS/CNS"integrated guidance system［C］//Systems and Control in Aeronautics and Astronautics(ISSCAA).Harbin:IEEE Press，2010:1101 －1104

［3］ Christiand C，Lee Yu-Cheol，Yu Wonpil，et al.Augmented EKF localization for mobile robots in urban environments［C］//IEEE International Conference on Robotics and Biomimetics.Tianjin:IEEE Computer Society，2010:986 －991

［4］ Karlekar J，Zhou S Z，Lu Weiquan，et al.Positioning，tracking and mapping for outdoor augmentation［C］//IEEE International Symposium on Mixed and Augmented Reality(ISMAR).Seoul:IEEE Press，2010:175 －184

［5］ Steven M Kay.統計信號處理基礎:估計與檢測理論［M］.北京:電子工業出版社，2003:182－226 Steven M Kay.Fundamentals of statistical signal processing，volume I:estimation theory and volume П:detection theory［M］.Beijing:Electronics Industry Press，2003:182 － 226(in Chinese)

［6］ Pratap Misra，Per Enge.全球定位系統:信號、測量與性能［M］.2版.北京:電子工業出版社，2008:112－173 Pratap Misra，Per Enge.Global positioning system:signals，measurements，and performance［M］.2nd ed.Beijing:Electronics Industry Press，2008:112 －173(in Chinese)

［7］ Jame Bao-Yen Tsui.全球定位系統軟件接收機基礎［M］.2版，北京:電子工業出版社，2007:6－10 James Bao-Yen Tsui.Fundamentals of global positioning system receivers a software approach［M］.2nd ed.Beijing:Electronics Industry Press，2007:6 － 10(in Chinese)

［8］荊海英.最優控制理論與方法［M］.2版，沈陽:東北大學出版社，2002:73－102 Jing Haiying.Optimal control theory and methods［M］.2nd ed.Shenyang:Northeastern University Press，2002:73 －102(in Chinese)

［9］ Graham C Goodwin，María M Seron，José A De Doná.Constrained control and estimation［M］.London:Springer Press，2001:187－215

［10］李國勇.最優控制理論及參數優化［M］.北京:國防工業出版社，2006:118－137 Li Guoyong.Optimal control theory and parameter optimization［M］.Beijing:National Defence Industry Press，2006:118 －137(in Chinese)