基于SoCFPGA的BDS/GPS雙模定位算法研究

張文德，劉怡俊

（廣東工業(yè)大學(xué) 信息工程學(xué)院，廣東 廣州 510006）

0 引 言

目前，北斗衛(wèi)星導(dǎo)航系統(tǒng)（BDS）和GPS衛(wèi)星導(dǎo)航系統(tǒng)能夠?yàn)橛脩籼峁└呔?、全天候的定位服?wù)。但是，單個(gè)衛(wèi)星導(dǎo)航系統(tǒng)在觀測(cè)衛(wèi)星數(shù)目有限的情況下，定位能力較低。BDS和GPS在定位原理、系統(tǒng)設(shè)計(jì)方面存在共通性，能夠同時(shí)接收兩個(gè)衛(wèi)星導(dǎo)航系統(tǒng)的衛(wèi)星信號(hào)并組合定位，不僅解決了上述問題，而且提高了精度[1-2]。本文描述了BDS/GPS的組合定位算法，并在基于SoCFPGA的BDS/GPS接收機(jī)上使用嵌入式軟件實(shí)現(xiàn)該算法。

一般的定位解算方法會(huì)使用卡爾曼濾波器?？柭鼮V波的計(jì)算過程是遞推過程。遞推過程中會(huì)產(chǎn)生舍入誤差，SoCFPGA上的CPU也會(huì)產(chǎn)生計(jì)算誤差。這兩種誤差可能使估計(jì)的均方誤差陣失去非負(fù)定性或?qū)ΨQ性，使增益陣的計(jì)算逐漸喪失加權(quán)作用，導(dǎo)致卡爾曼濾波產(chǎn)生發(fā)散現(xiàn)象[3]。為了克服發(fā)散現(xiàn)象，本文使用平方根濾波算法優(yōu)化傳統(tǒng)卡爾曼濾波算法，并通過嵌入式軟件實(shí)現(xiàn)該算法，最終在接收機(jī)上運(yùn)行。

1 BDS/GPS雙模定位的卡爾曼濾波算法

計(jì)算機(jī)系統(tǒng)是離散的系統(tǒng)，本文描述了用于BDS/GPS雙模定位的離散卡爾曼濾波模型[4-5]。

系統(tǒng)狀態(tài)方程：

系統(tǒng)觀測(cè)方程：

其中，X(k)是系統(tǒng)k時(shí)刻的目標(biāo)狀態(tài)向量，Φ(k+1,k)是系統(tǒng)從k時(shí)刻到k+1時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣，G(k)是系統(tǒng)噪聲增益矩陣，Z(k+1)是系統(tǒng)k+1時(shí)刻的觀測(cè)向量，H(k+1)是觀測(cè)矩陣。卡爾曼濾波要求系統(tǒng)噪聲V(k)和觀測(cè)噪聲W(k+1)是相互獨(dú)立、均值為零的白噪聲，即：

定義誤差向量為：

定義預(yù)測(cè)誤差的相關(guān)矩陣為：

當(dāng)k+1時(shí)刻輸入Z(k+1)時(shí)，求得的新息向量為：

其中，Z(k+1)為新息向量。

定義新息向量Z(k+1)的相關(guān)矩陣為：

卡爾曼濾波的增益：

其中，Q(k)為系統(tǒng)噪聲矩陣，R(k+1)為觀測(cè)噪聲矩陣，要求它們分別是已知的非負(fù)定矩陣和正定矩陣。

2 平方根濾波優(yōu)化的卡爾曼濾波算法

本文基于SoCFPGA的接收機(jī)中使用卡爾曼濾波算法出現(xiàn)了發(fā)散現(xiàn)象。因此，本文引入平方根算法解決濾波過程中的發(fā)散問題，使接收機(jī)在定位方面更加穩(wěn)定?？柭鼮V波中，狀態(tài)均方差陣p(k)表示狀態(tài)估計(jì)誤差的平方。平方根濾波主要針對(duì)均方差更新過程而設(shè)計(jì)，采用橋萊斯基分解均方差陣更新平方根，減少計(jì)算誤差[5]。

2.1 均方差的觀測(cè)更新

將增益公式帶入卡爾曼濾波測(cè)量公式，得：

其中，γ(k+1)為待定系數(shù)。

2.2 均方差狀態(tài)更新

將Δ(k+1)、Δ(k)、Δ(k+1,k)代入卡爾曼濾波器的狀態(tài)公式，可以得到均方差陣的狀態(tài)更新為：

同時(shí)簡(jiǎn)記：

2.3 平方根濾波

若簡(jiǎn)記：

本節(jié)描述了避免估計(jì)的均方誤差陣失去非負(fù)定性或?qū)ΨQ性的方法。

3 基于SoCFPGA接收機(jī)的實(shí)驗(yàn)

本文的接收機(jī)式基于SoCFPGA平臺(tái)，如圖1所示，使用的FPGA類型為Altera的DE2-115。在FPGA上寫入數(shù)據(jù)采集、衛(wèi)星信號(hào)捕獲和跟蹤等模塊的硬件代碼，在FPGA上燒寫LEON3處理器，處理器中運(yùn)行裸機(jī)的嵌入式處理程序，實(shí)現(xiàn)射頻前端驅(qū)動(dòng)、捕獲和跟蹤環(huán)路的控制，位同步、幀同步處理以及定位解算等功能。本文使用平方根濾波算法優(yōu)化卡爾曼濾波算法也在LEON3處理器上運(yùn)行。

實(shí)驗(yàn)平臺(tái)的天線接到華力創(chuàng)通衛(wèi)星導(dǎo)航信號(hào)模擬器，接收機(jī)工作25 min后觀察定位結(jié)果，如圖2、圖3和圖4所示[6]。

圖1 實(shí)驗(yàn)平臺(tái)實(shí)物

圖2 接收機(jī)啟動(dòng)完成工作不到1 min的定位結(jié)果

圖3 接收機(jī)工作5 min后的定位結(jié)果

實(shí)驗(yàn)結(jié)果可以看出，接收機(jī)連續(xù)工作25 min后，定位精度在2 m以內(nèi)，驗(yàn)證了算法的有效性，能夠解決因計(jì)算機(jī)計(jì)算精度誤差和舍入精度誤差而引起的卡爾曼濾波發(fā)散問題。

圖4 接收機(jī)工作25 min后的定位結(jié)果

4 結(jié) 論

本文著重研究基于卡爾曼濾波算法的BDS/GPS雙模定位。由于接收機(jī)設(shè)備存在計(jì)算精度誤差、卡爾曼濾波迭代存在舍入誤差等原因，卡爾曼濾波算法會(huì)出現(xiàn)發(fā)散問題[7-8]。本文引入了平方根濾波算法優(yōu)化卡爾曼濾波算法，通過在接收機(jī)上進(jìn)行試驗(yàn)觀察，驗(yàn)證了算法的有效性和可行性，對(duì)實(shí)際工程應(yīng)用具有一定借鑒意義。