[發(fā)明專利]一種基于UFIR濾波器的GNSS定位漂移處理方法

在審

• 申請?zhí)枺?/td>: 202110066718.0
• 申請日: 2021-01-19
• 公開（公告）號: CN112859126A
• 公開（公告）日: 2021-05-28
• 發(fā)明（設(shè)計(jì)）人: 劉甫;張輝;馮敏健
• 申請（專利權(quán)）人: 上海智駕汽車科技有限公司
• 主分類號: G01S19/40
• 分類號: G01S19/40;G01S19/37;G01S19/21
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于UFIR濾波器的GNSS定位漂移處理方法，其特征在于：具體包括以下步驟，

步驟一、建立車輛行駛過程中的運(yùn)動(dòng)學(xué)模型，確定相關(guān)矩陣中具體參數(shù)值；

步驟二、確定UFIR濾波器無偏估計(jì)下的最優(yōu)滑動(dòng)窗口長度，保證估測結(jié)果的可靠性；

步驟三、根據(jù)車輛運(yùn)動(dòng)學(xué)模型參數(shù)，確定UFIR濾波器迭代形式下的各矩陣參數(shù)，讀取并存儲最優(yōu)滑動(dòng)窗口長度相對應(yīng)的GNSS歷史定位信息數(shù)據(jù)；

步驟四、設(shè)定GNSS定位信息置信范圍，通過閾值對當(dāng)前GNSS定位信息進(jìn)行判斷，當(dāng)獲取的定位信息超過閾值時(shí)，判斷定位出現(xiàn)漂移現(xiàn)象，啟動(dòng)UFIR濾波器，利用累積的歷史定位信息數(shù)據(jù)對當(dāng)前位置信息進(jìn)行估測，得到此時(shí)較為精確的定位信息。

2.根據(jù)權(quán)利要求1所述的一種基于UFIR濾波器的GNSS定位漂移處理方法，其特征在于：所述步驟一中的運(yùn)動(dòng)學(xué)模型為：

其中，X_k∈R⁴為車輛運(yùn)動(dòng)的真實(shí)狀態(tài)；A∈R^4×4為狀態(tài)轉(zhuǎn)移矩陣；W_k∈R²為過程噪聲；B∈R^4×2為過程噪聲的系數(shù)矩陣；Z_k∈R²為GNSS觀測得到的車輛定位信息；C∈R^2×4為觀測轉(zhuǎn)移矩陣；v_k∈R²為測量噪聲；

所述車輛運(yùn)動(dòng)的真實(shí)狀態(tài)X_k、過程噪聲Z_k和GNSS觀測得到的車輛定位信息W_k對應(yīng)的矩陣分別為：

其中，x(k)和y(k)分別表示k時(shí)刻時(shí)，車輛在GNSS系統(tǒng)所定義測量坐標(biāo)系中的x軸與y軸方向上的坐標(biāo)值；v_x(k)和v_y(k)分別表示k時(shí)刻時(shí)，車輛在GNSS系統(tǒng)所定義測量坐標(biāo)系中的x軸與y軸方向上的速度值；x_z(k)和y_z(k)分別表示GNSS所測得的車輛軸x與y軸方向上的坐標(biāo)值；假設(shè)車輛的加速度為隨機(jī)變量，即a_x(k)與a_y(k)分別表示車輛在x軸與y軸方向上的加速度值；

所述狀態(tài)轉(zhuǎn)移矩陣A、過程噪聲的系數(shù)矩陣B、觀測轉(zhuǎn)移矩陣C具體為：

其中，T為GNSS定位采樣周期；

同時(shí)，過程噪聲與測量噪聲假設(shè)其均滿足均值為零的條件：

3.根據(jù)權(quán)利要求2所述的一種基于UFIR濾波器的GNSS定位漂移處理方法，其特征在于：所述步驟二中的UFIR濾波器無偏估計(jì)下的最優(yōu)滑動(dòng)窗口長度為：

其中V_k的表達(dá)形式如下：

其中Z_k為k時(shí)刻的觀測值，為k時(shí)刻的先驗(yàn)狀態(tài)變量，C為系統(tǒng)的觀測轉(zhuǎn)移矩陣。

4.根據(jù)權(quán)利要求3所述的一種基于UFIR濾波器的GNSS定位漂移處理方法，其特征在于：所述步驟三中的確定UFIR濾波器迭代形式下的各矩陣參數(shù)具體內(nèi)容包括：

確定迭代參數(shù)矩陣H_m,k，其表達(dá)形式如下：

式中，F(xiàn)_m,k表示從m時(shí)刻到k時(shí)刻的總累積狀態(tài)轉(zhuǎn)移矩陣；C_m,k從m時(shí)刻到k時(shí)刻的總累積觀測轉(zhuǎn)移矩陣；h_m,k表示從m時(shí)刻到k時(shí)刻的迭代系數(shù)矩陣；H_m,k表示從m時(shí)刻到k時(shí)刻的迭代參數(shù)矩陣；

具體地，其中為m+1時(shí)刻到k時(shí)刻單累積狀態(tài)轉(zhuǎn)移矩陣，其廣泛定義為：

所述步驟三中的存儲對應(yīng)最優(yōu)滑動(dòng)窗口長度的歷史定位信息數(shù)據(jù)的存儲表示形式如下：

Z_m,k＝[Z_m^TZ_m+1^T...Z_k^T]^T

式中Z_m,k為從m時(shí)刻到k時(shí)刻的總累積觀測矩陣，Z_m為m時(shí)刻所觀測得到的定義信息數(shù)據(jù)。

5.根據(jù)權(quán)利要求4所述的一種基于UFIR濾波器的GNSS定位漂移處理方法，其特征在于：所述步驟四中的設(shè)定GNSS定位信息置信范圍，通過閾值對當(dāng)前GNSS定位信息進(jìn)行判斷的具體方法為：

針對車輛行駛環(huán)境中對GNSS定位信息精確度影響的主要因素，構(gòu)造了如下所示的自適應(yīng)調(diào)整的置信區(qū)間確定公式：

σ＝f₁M+f₂Q+f₃P+R

式中，M為接收衛(wèi)星顆數(shù)所對應(yīng)的參數(shù)值；Q為電磁干擾強(qiáng)度所對應(yīng)的參數(shù)值；P為定位信號強(qiáng)弱評判值；R為調(diào)整系數(shù)，由車輛的行駛狀態(tài)確定；f₁、f₂與f₃分別為對應(yīng)參數(shù)的重要因子；

通過當(dāng)前時(shí)刻的位置信息與上一時(shí)刻的位置信息進(jìn)行絕對距離作差，將得到的計(jì)算值與置信區(qū)間比對：

J＝F(Z_k,Z_k-1)-σ

若J＞0，即距離差值小于置信區(qū)間值，則判定獲得的當(dāng)前位置信息準(zhǔn)確；若J＜0，即距離差值大于置信區(qū)間值，則認(rèn)定獲取的定位信息受到外界干擾，出現(xiàn)了漂移現(xiàn)象，啟動(dòng)UFIR濾波器，利用歷史積累定位數(shù)據(jù)估測當(dāng)前位置信息，得到較為準(zhǔn)確的數(shù)據(jù)，保證車輛自動(dòng)駕駛功能的正常運(yùn)行。