基于優(yōu)化理論的GPS定位解算算法

趙 鑫 秦紅磊 郎榮玲

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

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

針對(duì)以上傳統(tǒng)定位解算方法存在的問題，本文提出了一種新的定位解算算法——基于優(yōu)化理論的最大后驗(yàn)估計(jì)算法.可以在擁有較高精度的同時(shí)采用一種非線性的估計(jì)方法進(jìn)行定位解算.該方法用優(yōu)化理論的思路求解系統(tǒng)狀態(tài)量的最大后驗(yàn)概率估計(jì)值.它是從系統(tǒng)狀態(tài)量、觀測(cè)量的聯(lián)合概率密度函數(shù)出發(fā)，將估計(jì)問題轉(zhuǎn)化成優(yōu)化問題，用優(yōu)化問題的解法對(duì)系統(tǒng)的狀態(tài)進(jìn)行估計(jì).

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

1 GPS接收機(jī)定位解算原理

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

通過GPS接收機(jī)對(duì)m(m≥4)顆衛(wèi)星信號(hào)傳輸時(shí)間的測(cè)量可得到用戶與m顆相應(yīng)衛(wèi)星的偽距，列方程得

通過GPS接收機(jī)對(duì)m(m≥4)顆衛(wèi)星信號(hào)多普勒頻移的測(cè)量可得到用戶與m顆相應(yīng)衛(wèi)星的偽距率，對(duì)式(1)求時(shí)間的導(dǎo)數(shù)，可得偽距變化率方程:

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

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

2 基于優(yōu)化理論的定位解算算法

定位解算過程的數(shù)學(xué)模型屬于量測(cè)模型非線性，觀測(cè)噪聲和驅(qū)動(dòng)噪聲是高斯白噪聲的非線性高斯系統(tǒng).

下面將具體講述定位解算新方法——基于優(yōu)化理論的最大后驗(yàn)概率估計(jì)方法的推導(dǎo).

2.1 狀態(tài)模型

GPS導(dǎo)航系統(tǒng)狀態(tài)模型可取為

其中

觀測(cè)矩陣F可取為

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

2.2 算法的數(shù)學(xué)推導(dǎo)

在給定的系統(tǒng)狀態(tài)方程、量測(cè)方程模型下，用優(yōu)化算法對(duì)給定的系統(tǒng)狀態(tài)量確定一種估計(jì)，這種估計(jì)使系統(tǒng)狀態(tài)量的后驗(yàn)概率最大［8－9］.

假設(shè)定位解算系統(tǒng)的模型可以簡寫為

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

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

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

其中α為常量.

通過上述步驟就把在非線性高斯系統(tǒng)假設(shè)下的最大后驗(yàn)估計(jì)轉(zhuǎn)化成了一個(gè)優(yōu)化問題.問題的的形式如下:

其中

此優(yōu)化問題將采用動(dòng)態(tài)規(guī)劃方法進(jìn)行求解.動(dòng)態(tài)規(guī)劃是解決多級(jí)決策過程最優(yōu)化的一種數(shù)學(xué)方法，是根據(jù)一類多級(jí)決策問題的特點(diǎn)，把多級(jí)決策問題變換為一系列互相聯(lián)系的單級(jí)決策問題，然后逐個(gè)加以解決.

下面將用正向動(dòng)態(tài)規(guī)劃算法求解非線性系統(tǒng)模型下狀態(tài)量的最優(yōu)估計(jì)［10］.

由式(12)可知，非線性高斯系統(tǒng)假設(shè)下0～k時(shí)刻的總代價(jià)函數(shù)為

其中

定義0時(shí)刻的部分代價(jià)函數(shù)為

用動(dòng)態(tài)規(guī)劃方法可得1時(shí)刻的部分代價(jià)函數(shù)的表達(dá)式:

對(duì)于k大于等于1的時(shí)刻，部分代價(jià)函數(shù)的表達(dá)式可表示為

這樣，就把多階段決策問題轉(zhuǎn)化為一系列單階段決策問題，利用各階段之間的關(guān)系，逐個(gè)求解就可得到系統(tǒng)各個(gè)時(shí)刻狀態(tài)量的最大后驗(yàn)估計(jì)值.

3 實(shí)驗(yàn)驗(yàn)證

為了驗(yàn)證算法的有效性，取EKF的濾波結(jié)果作為比較.仿真分析將分別以模擬產(chǎn)生的GPS測(cè)量偽距值、測(cè)量偽距率值以及實(shí)際環(huán)境中軟件接收機(jī)產(chǎn)生的實(shí)際數(shù)據(jù)作為觀測(cè)量.分別采用EKF、基于優(yōu)化理論的定位解算算法進(jìn)行濾波.

首先驗(yàn)證基于優(yōu)化理論的定位解算算法的有效性，接收機(jī)運(yùn)動(dòng)狀態(tài)具體描述如下:

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

基于優(yōu)化理論的最大后驗(yàn)估計(jì)算法對(duì)數(shù)據(jù)處理后的誤差波形圖如圖1～圖5所示.

圖1 x軸方向定位誤差

圖2 y軸方向定位誤差

圖3 z軸方向定位誤差

下面分別利用EKF、基于優(yōu)化理論的定位解算算法對(duì)數(shù)據(jù)進(jìn)行濾波估計(jì)以實(shí)現(xiàn)定位，并對(duì)定位結(jié)果進(jìn)行比較分析.

此項(xiàng)實(shí)驗(yàn)的軟件接收機(jī)運(yùn)動(dòng)狀態(tài)具體描述如下:

圖4 鐘差估計(jì)誤差

圖5 頻差估計(jì)誤差

GPS軟件接收機(jī)初始位置為東經(jīng)116.34°，北緯 39.98°，高度 73.21m，接收機(jī)狀態(tài)靜止.數(shù)據(jù)輸出頻率1Hz.觀測(cè)噪聲為高斯噪聲.實(shí)驗(yàn)數(shù)據(jù)為實(shí)際環(huán)境中軟件接收機(jī)接收衛(wèi)星信號(hào)后經(jīng)過數(shù)據(jù)處理測(cè)得的偽距值、偽距率值.

EKF和基于優(yōu)化理論的最大后驗(yàn)估計(jì)算法對(duì)數(shù)據(jù)處理后的誤差波形圖如圖6～圖8所示.

圖6 x軸方向定位曲線

圖7 y軸方向定位曲線

圖8 z軸方向定位曲線

兩種濾波方法在高斯噪聲下的統(tǒng)計(jì)特性如表1所示.

表1 兩種濾波方式定位結(jié)果誤差對(duì)比

由上面的實(shí)驗(yàn)結(jié)果可以看出，基于優(yōu)化理論的最大后驗(yàn)估計(jì)算法在x，y軸方向定位精度要略低于EKF，在z軸方向上的定位精度要略高于EKF.

4 結(jié)束語

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

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.統(tǒng)計(jì)信號(hào)處理基礎(chǔ):估計(jì)與檢測(cè)理論［M］.北京:電子工業(yè)出版社，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.全球定位系統(tǒng):信號(hào)、測(cè)量與性能［M］.2版.北京:電子工業(yè)出版社，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.全球定位系統(tǒng)軟件接收機(jī)基礎(chǔ)［M］.2版，北京:電子工業(yè)出版社，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］荊海英.最優(yōu)控制理論與方法［M］.2版，沈陽:東北大學(xué)出版社，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］李國勇.最優(yōu)控制理論及參數(shù)優(yōu)化［M］.北京:國防工業(yè)出版社，2006:118－137 Li Guoyong.Optimal control theory and parameter optimization［M］.Beijing:National Defence Industry Press，2006:118 －137(in Chinese)