專(zhuān)利名稱(chēng)：基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤ukf定位濾波算法的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及衛(wèi)星導(dǎo)航接收機(jī)位置參數(shù)解算，尤其是涉及一種基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤UKF定位濾波算法。
背景技術(shù)：
衛(wèi)星導(dǎo)航技術(shù)的飛速發(fā)展已逐漸取代了無(wú)線電導(dǎo)航、天文導(dǎo)航等傳統(tǒng)導(dǎo)航技術(shù)， 而成為一種普遍采用的導(dǎo)航定位技術(shù)，并在精度、實(shí)時(shí)性、全天候等方面取得了長(zhǎng)足進(jìn)步。 目前，世界上多個(gè)國(guó)家和地區(qū)已經(jīng)建立或者正在建設(shè)全球衛(wèi)星定位導(dǎo)航系統(tǒng)，具體包括美國(guó)建立的GPS系統(tǒng)、俄羅斯建立的GL0NASS系統(tǒng)、歐盟正在建設(shè)的“伽利略”系統(tǒng)和中國(guó)正在建設(shè)的“北斗二代”系統(tǒng)。導(dǎo)航系統(tǒng)中的定位解算模塊是根據(jù)衛(wèi)星信號(hào)的量測(cè)信息采用相應(yīng)算法對(duì)用戶(hù)接收機(jī)位置、速度和時(shí)鐘參數(shù)進(jìn)行解算，是衛(wèi)星導(dǎo)航系統(tǒng)實(shí)現(xiàn)的關(guān)鍵技術(shù)之一。在衛(wèi)星導(dǎo)航接收機(jī)中，導(dǎo)航定位解算是用戶(hù)接收機(jī)定位最為關(guān)鍵的一個(gè)環(huán)節(jié)， 采用定位算法的優(yōu)劣直接導(dǎo)致用戶(hù)定位精度的好壞。傳統(tǒng)接收機(jī)用戶(hù)定位的算法選擇為最小二乘，具體實(shí)現(xiàn)步驟(1)獲取跟蹤環(huán)路輸出偽距信息P = [P1, P2LPN]T，其中N代表跟蹤通道輸出偽距數(shù)目；(2)用戶(hù)接收機(jī)初始位置估計(jì)Xtl = [x0, y0, ζ0]τ ；3)量測(cè)方程線性化，即量測(cè)方程Pi =機(jī)-X)2+ (Yi-Y)2 + (Zi-Zf + CVtu在初始用戶(hù)位置估計(jì)Xtl = [X。，yQ, zQ]T 處線性化得到[Δ P P Δ P2LA pm]T = Η[Δχ, Ay, Δ z, Ctu]T ; (4)由量測(cè)方程得到狀態(tài)量的最小二乘估計(jì)=(丑； (5)更新用戶(hù)接收機(jī)的位置即 [XJ,zl =[XJ,Z]Tki+[Ax,Ay,Azfklki ； (6)讀取觀測(cè)數(shù)據(jù)，轉(zhuǎn)至步驟⑶繼續(xù)執(zhí)行，直至觀測(cè)數(shù)據(jù)結(jié)束。最小二乘算法應(yīng)用于運(yùn)動(dòng)狀態(tài)估計(jì)中，其算法簡(jiǎn)單，使用與對(duì)常值向量或隨機(jī)向量的估計(jì)，并且在對(duì)被估計(jì)量和量測(cè)誤差缺乏了解的情況下仍能使用，但是由于缺乏對(duì)系統(tǒng)動(dòng)力學(xué)狀態(tài)的考慮，估計(jì)精度并不很高，特別是對(duì)于非靜止運(yùn)動(dòng)狀態(tài)，存在較大的估計(jì)誤差。因此，有些研究學(xué)者提出了卡爾曼濾波算法，用以加強(qiáng)系統(tǒng)動(dòng)力學(xué)的估計(jì)，以提高狀態(tài)量的估計(jì)精度。用戶(hù)接收機(jī)通常采用最小二乘算法(參見(jiàn)Fundamentals of Global Positioning System Receivers-A Software Approach，SENOND EDIRI0N，Canada，James B and Yen T， 2005.)和卡爾曼 Kalman 濾波算法(參見(jiàn) Jwo D J and Wang S H，Adaptive Fuzzy Strong Tracking Extended Kalman Filtering for GPS Navigation, IEEE SENSORS JOURNAL. VOL. 7, NO. 5, MAY 2007.和 Liu J and Lu M Q，An Adaptive UKF Filtering Algorithm for GPS Position Estimation， IEEE XPLORE Wireless Communications, Networking and Mobile Computing. 2009. WiCom 09.5th International Conference on Issue Data 24-26Sept. 2009.)進(jìn)行用戶(hù)接收機(jī)位置的解算。最小二乘算法雖然算法簡(jiǎn)單，但由于缺乏對(duì)系統(tǒng)動(dòng)力學(xué)狀態(tài)的考慮，估計(jì)精度并不很高，特別是對(duì)于非靜止運(yùn)動(dòng)狀態(tài)，存在較大的估計(jì)誤差。而Kalman濾波算法雖能夠解決定位解算中的非線性問(wèn)題，但是對(duì)于先驗(yàn)信息要求極為嚴(yán)格，同時(shí)定位精度受到載體動(dòng)態(tài)性的限制。

發(fā)明內(nèi)容
本發(fā)明為克服現(xiàn)有技術(shù)的不足，提出一種基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤UKF定位濾波算法，該方法在傳統(tǒng)UKF算法基礎(chǔ)上，將自適應(yīng)強(qiáng)跟蹤算法和模糊邏輯系統(tǒng)結(jié)合，大大地提高了衛(wèi)星導(dǎo)航系統(tǒng)定位算法的性能。根據(jù)本發(fā)明的一個(gè)方面，提供一種基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤UKF定位濾波算法，包括以下步驟(I)建立濾波模型；(2)設(shè)定濾波器初始參數(shù)； (3)運(yùn)用UKF濾波算法對(duì)狀態(tài)量進(jìn)行濾波；(4)運(yùn)用模糊邏輯系統(tǒng)求解強(qiáng)跟蹤自適應(yīng)算法中的軟化因子；(5)強(qiáng)跟蹤自適應(yīng)算法中自適應(yīng)因子求解；(6)歷元時(shí)刻遞加1，讀取下一時(shí)刻觀測(cè)，返回步驟(4)，直至結(jié)束。進(jìn)一步地，步驟(I)中所述濾波模型包括針對(duì)低動(dòng)態(tài)環(huán)境，根據(jù)接收機(jī)的位置、速度、時(shí)鐘鐘差和頻差建立的常速CV模型； 針對(duì)高動(dòng)態(tài)環(huán)境，根據(jù)接收機(jī)的位置、速度、加速度、時(shí)鐘鐘差和頻差建立的常加速度CA模型。進(jìn)一步地，步驟(2)中所述設(shè)定濾波器初始參數(shù)包括設(shè)置系統(tǒng)噪聲矩陣Q (t)和觀測(cè)噪聲矩陣R ；設(shè)置狀態(tài)量初始估計(jì)Xtl和初始狀態(tài)估計(jì)誤差協(xié)方差P。;濾波時(shí)刻k清零，將自適應(yīng)因子賦值1，即自適應(yīng)因子X(jué)i,,= I。進(jìn)一步地，步驟(3)中所述運(yùn)用UKF濾波算法對(duì)狀態(tài)量X進(jìn)行濾波的步驟包括初始狀態(tài)采樣初始狀態(tài)量采樣,得到初始狀態(tài)sigma樣本點(diǎn)及對(duì)應(yīng)權(quán)重因子；一步預(yù)測(cè)狀態(tài)采樣根據(jù)所獲得的初始狀態(tài)sigma樣本點(diǎn)，對(duì)一步預(yù)測(cè)狀態(tài)和一步預(yù)測(cè)狀態(tài)協(xié)方差矩陣進(jìn)行估計(jì)；一步預(yù)測(cè)狀態(tài)采樣二次采樣對(duì)獲得的一步預(yù)測(cè)狀態(tài)及一步預(yù)測(cè)狀態(tài)協(xié)方差矩陣進(jìn)行二次采樣，得到二次采樣的一步預(yù)測(cè)狀態(tài)sigma樣本點(diǎn)及對(duì)應(yīng)權(quán)重因子；輸出一步預(yù)測(cè)根據(jù)一步預(yù)測(cè)狀態(tài)的二次采樣輸出觀測(cè)量一步預(yù)測(cè)，之后進(jìn)行觀測(cè)量自協(xié)方差矩陣、觀測(cè)量和狀態(tài)量之間互協(xié)方差矩陣估計(jì)；卡爾曼增益求解通過(guò)獲得的觀測(cè)量自協(xié)方差矩陣和觀測(cè)量與狀態(tài)量的互協(xié)方差矩陣，求解卡爾曼增益矩陣，實(shí)現(xiàn)狀態(tài)量估計(jì)更新和狀態(tài)量估計(jì)協(xié)方差更新。進(jìn)一步地，步驟(4)中所述運(yùn)用模糊邏輯系統(tǒng)求解強(qiáng)跟蹤自適應(yīng)算法中的軟化因子的步驟包括獲得濾波信息向量Vk ;模糊邏輯系統(tǒng)輸入通過(guò)濾波信息向量vk，求解模糊邏輯系統(tǒng)輸入A，其中模糊邏輯系統(tǒng)輸入^為濾波信息向量Vk單步協(xié)方差矩陣的跡與單步理論信息方差陣的跡之比；模糊邏輯系統(tǒng)求解根據(jù)模糊邏輯系統(tǒng)輸入^的變化規(guī)律，進(jìn)行模糊邏輯系統(tǒng)的模糊化過(guò)程，設(shè)定模糊規(guī)則，進(jìn)行模糊推理，去模糊化過(guò)程，實(shí)現(xiàn)軟化因子輸出。所述模糊化過(guò)程中輸入?yún)⒘侩`屬度函數(shù)包括三角形函數(shù)，所述模型系統(tǒng)規(guī)則包括一階T-S模型、IF-THEN形式。進(jìn)一步地，步驟(5)中所述強(qiáng)跟蹤自適應(yīng)算法中自適應(yīng)因子λ “求解的步驟包括由濾波信息向量Vk對(duì)濾波信息真實(shí)協(xié)方差Vk進(jìn)行估計(jì)求解；求解包含濾波信息協(xié)方差Vk和觀測(cè)噪聲估計(jì)的自適應(yīng)矩陣Nk ；由自適應(yīng)矩陣Nk和信息理論協(xié)方差矩陣Mk求解自適應(yīng)因子中間參數(shù)Ci,,；通過(guò)對(duì)中間參數(shù)Ci,,進(jìn)行判定來(lái)對(duì)自適應(yīng)因子X(jué)i,k賦值。本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于(I)與傳統(tǒng)的UKF算法相比，對(duì)一步預(yù)測(cè)狀態(tài)進(jìn)行了二次采樣過(guò)程，減小了對(duì)于隨機(jī)變量通過(guò)非線性系統(tǒng)后輸出變量統(tǒng)計(jì)特性的估計(jì)誤差，在一定程度上提高了狀態(tài)量估計(jì)的精度。(2)將強(qiáng)跟蹤算法與UKF算法相結(jié)合，克服了傳統(tǒng)UKF濾波算法容易受初始值和模型誤差的缺點(diǎn)。(3)強(qiáng)跟蹤算法中的軟化因子估計(jì)采用了模糊邏輯推理系統(tǒng)進(jìn)行估計(jì)，用單次濾波信息協(xié)方差的跡與理論信息協(xié)方差的跡之比作為模糊邏輯系統(tǒng)的輸入，時(shí)刻監(jiān)測(cè)濾波器的工作狀況，實(shí)時(shí)調(diào)整軟化因子。(4)自適應(yīng)參數(shù)求解過(guò)程中，對(duì)于信息實(shí)際協(xié)方差矩陣估計(jì)時(shí)，采用了一種新的遞推估計(jì)算法，更加直接的反應(yīng)器載體實(shí)際的運(yùn)動(dòng)狀態(tài)，同時(shí)也減小了計(jì)算量。


圖I是現(xiàn)行GPS定位最小二乘算法的流程圖；圖2是現(xiàn)行UKF濾波算法的流程圖；圖3是本發(fā)明實(shí)施例提供的自適應(yīng)強(qiáng)跟蹤UKF定位濾波算法的流程圖；圖4是本發(fā)明實(shí)施例提供的強(qiáng)跟蹤算法中軟化因子的求解流程圖；圖5是本發(fā)明實(shí)施例提供的強(qiáng)跟蹤算法中自適應(yīng)因子的求解流程圖。
具體實(shí)施例方式為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖，對(duì)本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說(shuō)明。應(yīng)當(dāng)理解，此處所描述的具體實(shí)施例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。本發(fā)明中處理的數(shù)據(jù)是衛(wèi)星導(dǎo)航系統(tǒng)跟蹤模塊輸出的測(cè)量偽距信息，同時(shí)還包括對(duì)應(yīng)跟蹤時(shí)刻衛(wèi)星的位置和速度參數(shù)。用戶(hù)接收機(jī)的定位是在地心地固ECEF坐標(biāo)系下進(jìn)行的，首先根據(jù)接收機(jī)通道衛(wèi)星信號(hào)跟蹤測(cè)量得到的偽距信息，對(duì)用戶(hù)接收機(jī)的位置參數(shù)和時(shí)鐘鐘差參數(shù)進(jìn)行解算。當(dāng)通道跟蹤有效衛(wèi)星超過(guò)4顆，可用于接收機(jī)的實(shí)際定位解算。下面結(jié)合附圖，對(duì)本發(fā)明詳細(xì)說(shuō)明如下參見(jiàn)圖3，該圖示出了本發(fā)明實(shí)施例提供的自適應(yīng)強(qiáng)跟蹤UKF定位濾波算法的詳細(xì)流程；
SI :建立濾波模型在建立濾波模型之前，本發(fā)明須要獲取衛(wèi)星測(cè)量偽距信息，以及對(duì)應(yīng)跟蹤時(shí)刻衛(wèi)星在地心地固ECEF坐標(biāo)系下的位置和速度參數(shù)；首先，由衛(wèi)星信號(hào)模擬器或者軟件接收機(jī)處理實(shí)際數(shù)據(jù)得到衛(wèi)星的測(cè)量偽距信息，一般情況下偽距信息獲得頻率為1Hz，同時(shí)得到對(duì)應(yīng)跟蹤時(shí)刻衛(wèi)星在地心地固ECEF坐標(biāo)系下的位置和速度參數(shù)。當(dāng)采用衛(wèi)星信號(hào)模擬器生成衛(wèi)星的測(cè)量偽距信息時(shí)(通常采用Renix文件還原偽距生成的方式)，須要首先確定使用歷書(shū)的周數(shù)，設(shè)置仿真間隔和仿真總時(shí)間(其中可以包含相應(yīng)選星算法，保證定位精度)。由于衛(wèi)星信號(hào)模擬器生成數(shù)據(jù)時(shí)，只模擬了時(shí)鐘誤差在生成偽距中，因此在生成的偽距中添加適量的白噪聲，視為偽距中的環(huán)境噪聲。如果處理的數(shù)據(jù)是實(shí)際數(shù)據(jù)，經(jīng)過(guò)軟件接收機(jī)處理后，即可得到相應(yīng)通道衛(wèi)星的偽距信息和衛(wèi)星的位置、速度參數(shù)信息。然后，根據(jù)用戶(hù)接收機(jī)(載體)的實(shí)際運(yùn)動(dòng)規(guī)律選取狀態(tài)估計(jì)量和系統(tǒng)量測(cè)量，建立濾波模型；建立濾波模型包括建立以下模型建立CV模型對(duì)于載體運(yùn)動(dòng)的低動(dòng)態(tài)環(huán)境，使用接收機(jī)的位置、速度和時(shí)鐘參數(shù)作為狀態(tài)量X
權(quán)利要求
1.一種基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤UKF定位濾波算法，其特征在于，包括(1)建立定位濾波模型；(2)設(shè)定濾波器初始參數(shù)；(3)運(yùn)用UKF濾波算法對(duì)狀態(tài)量進(jìn)行濾波；(4)運(yùn)用模糊邏輯系統(tǒng)求解強(qiáng)跟蹤自適應(yīng)算法中的軟化因子；(5)強(qiáng)跟蹤自適應(yīng)算法中自適應(yīng)因子求解；(6)歷元時(shí)刻遞加1，讀取下一時(shí)刻觀測(cè)，返回步驟(4)，直至結(jié)束。
2.根據(jù)權(quán)利要求I所述的基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤UKF定位濾波算法，其特征在于，所述定位濾波模型包括針對(duì)低動(dòng)態(tài)環(huán)境，根據(jù)接收機(jī)的位置、速度、時(shí)鐘鐘差和頻差建立的常速CV模型；針對(duì)高動(dòng)態(tài)環(huán)境，根據(jù)接收機(jī)的位置、速度、加速度、時(shí)鐘鐘差和頻差建立的常加速度CA模型。
3.根據(jù)權(quán)利要求I所述的基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤UKF定位濾波算法，其特征在于，所述設(shè)定濾波器初始參數(shù)包括設(shè)置系統(tǒng)噪聲矩陣β⑩和觀測(cè)噪聲矩陣Λ ;設(shè)置狀態(tài)量初始估計(jì)石和初始狀態(tài)估計(jì)誤差協(xié)方差石；濾波時(shí)刻k清零，將自適應(yīng)因子賦值1，即自適應(yīng)因子為j = I。
4.根據(jù)權(quán)利要求I所述的基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤UKF定位濾波算法，其特征在于，所述運(yùn)用UKF濾波算法對(duì)狀態(tài)量濾波的步驟包括初始狀態(tài)采樣初始狀態(tài)量采樣，得到初始狀態(tài)sigma樣本點(diǎn)及對(duì)應(yīng)權(quán)重因子；一步預(yù)測(cè)狀態(tài)采樣根據(jù)所獲得的初始狀態(tài)sigma樣本點(diǎn)，對(duì)一步預(yù)測(cè)狀態(tài)和一步預(yù)測(cè)狀態(tài)協(xié)方差矩陣進(jìn)行估計(jì)；一步預(yù)測(cè)狀態(tài)二次采樣對(duì)獲得的一步預(yù)測(cè)狀態(tài)及一步預(yù)測(cè)狀態(tài)協(xié)方差矩陣進(jìn)行二次采樣，得到二次采樣的一步預(yù)測(cè)狀態(tài)sigma樣本點(diǎn)及對(duì)應(yīng)權(quán)重因子；輸出一步預(yù)測(cè)根據(jù)一步預(yù)測(cè)狀態(tài)的二次采樣輸出觀測(cè)量一步預(yù)測(cè)，之后進(jìn)行觀測(cè)量自協(xié)方差矩陣、觀測(cè)量和狀態(tài)量之間互協(xié)方差矩陣估計(jì)；卡爾曼增益求解通過(guò)獲得的觀測(cè)量自協(xié)方差矩陣和觀測(cè)量與狀態(tài)量的互協(xié)方差矩陣，求解卡爾曼增益矩陣，實(shí)現(xiàn)狀態(tài)量估計(jì)更新和狀態(tài)量估計(jì)協(xié)方差更新。
5.根據(jù)權(quán)利要求I所述的基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤UKF定位濾波算法，其特征在于，所述運(yùn)用模糊邏輯系統(tǒng)求解強(qiáng)跟蹤自適應(yīng)算法中的軟化因子的步驟包括獲得濾波信息向量vJt ;模糊邏輯系統(tǒng)輸入通過(guò)濾波信息向量A，求解模糊邏輯系統(tǒng)輸入其中模糊邏輯系統(tǒng)輸入1為濾波信息向量巧單步協(xié)方差矩陣的跡與單步理論信息方差陣的跡之比；模糊邏輯系統(tǒng)求解根據(jù)模糊邏輯系統(tǒng)輸入％的變化規(guī)律，進(jìn)行模糊邏輯系統(tǒng)的模糊化過(guò)程，設(shè)定模糊規(guī)則，進(jìn)行模糊推理，去模糊化過(guò)程，實(shí)現(xiàn)軟化因子輸出。
6.根據(jù)權(quán)利要求5所述的基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤UKF定位濾波算法，其特征在于，所述模糊化過(guò)程中輸入?yún)⒘侩`屬度函數(shù)包括三角形函數(shù)，所述模型規(guī)則包括一階T-S模型、IF-THEN形式。
7.根據(jù)權(quán)利要求I所述的基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤UKF定位濾波算法，其特征在于，所述強(qiáng)跟蹤自適應(yīng)算法中自適應(yīng)因子毛t求解的步驟包括由濾波信息向量七對(duì)濾波信息真實(shí)協(xié)方差^進(jìn)行估計(jì)求解；求解包含濾波信息協(xié)方差G和觀測(cè)噪聲估計(jì)的自定應(yīng)矩陣隊(duì);由自定應(yīng)矩陣AZa和信息理論協(xié)方差矩陣Λ4求解自適應(yīng)因子中間參數(shù)(^ ；通過(guò)對(duì)中間參數(shù)Cu進(jìn)行判定來(lái)對(duì)自適應(yīng)因子賦值。
全文摘要
本發(fā)明公開(kāi)了一種基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤UKF定位濾波算法，包括(1)建立定位濾波模型；(2)設(shè)定濾波器初始參數(shù)；(3)運(yùn)用UKF濾波算法對(duì)狀態(tài)量進(jìn)行濾波；(4)運(yùn)用模糊邏輯系統(tǒng)求解強(qiáng)跟蹤自適應(yīng)算法中的軟化因子；(5)強(qiáng)跟蹤自適應(yīng)算法中自適應(yīng)因子求解；(6)歷元時(shí)刻遞加1，讀取下一時(shí)刻觀測(cè)，返回步驟(4)，直至結(jié)束。本發(fā)明通過(guò)在UKF濾波算法基礎(chǔ)上引入強(qiáng)跟蹤自適應(yīng)算法，并在強(qiáng)跟蹤自適應(yīng)算法中采用新遞推算法估計(jì)信息協(xié)方差矩陣，對(duì)強(qiáng)跟蹤算法中軟化因子采用模糊邏輯推理系統(tǒng)進(jìn)行求解，根據(jù)歷元時(shí)刻濾波器的工作狀況對(duì)軟化因子進(jìn)行實(shí)時(shí)估計(jì)。在衛(wèi)星導(dǎo)航用戶(hù)接收機(jī)位置估計(jì)中，可大大地提高載體適應(yīng)動(dòng)態(tài)性的能力和定位性能。
文檔編號(hào)G01S19/37GK102608631SQ201110333619
公開(kāi)日2012年7月25日 申請(qǐng)日期2011年10月28日 優(yōu)先權(quán)日2011年10月28日
發(fā)明者衛(wèi)奇, 王玉寶, 金天 申請(qǐng)人:北京航空航天大學(xué)