本發(fā)明屬于慣性導(dǎo)航技術(shù)領(lǐng)域，涉及一種基于路徑匹配的慣導(dǎo)系統(tǒng)誤差修正方法。

背景技術(shù)：

目前定位定向系統(tǒng)，通常采用旋轉(zhuǎn)調(diào)制技術(shù)和里程計(jì)組合技術(shù)來提高系統(tǒng)精度，但不論是采取旋轉(zhuǎn)調(diào)制技術(shù)降低器件誤差對(duì)系統(tǒng)精度的影響，還是結(jié)合里程計(jì)進(jìn)行組合誤差修正，系統(tǒng)的定位誤差仍然會(huì)隨著車輛的行駛距離增大以及系統(tǒng)的工作時(shí)間加長(zhǎng)而逐漸變大。

對(duì)于上述問題，常用的解決方法是經(jīng)過一段時(shí)間后，利用衛(wèi)星信息對(duì)系統(tǒng)的位置誤差進(jìn)行修正以消除累計(jì)誤差。但衛(wèi)星信息不屬于自主導(dǎo)航信息來源，容易受到外界干擾而出現(xiàn)無法使用的情況。

技術(shù)實(shí)現(xiàn)要素：

針對(duì)上述現(xiàn)有技術(shù)，本發(fā)明的目的在于提供一種基于路徑匹配的慣導(dǎo)系統(tǒng)誤差修正方法，實(shí)現(xiàn)車輛在基準(zhǔn)路徑數(shù)據(jù)庫中的定位，從而用基準(zhǔn)路徑中的位置信息完成對(duì)慣導(dǎo)系統(tǒng)誤差的修正。

為了達(dá)到上述目的，本發(fā)明采用以下技術(shù)方案。

本發(fā)明的目的在于提供一種基于路徑匹配的慣導(dǎo)系統(tǒng)誤差修正方法，包括以下步驟：

步驟一、建立基準(zhǔn)路徑地圖數(shù)據(jù)庫；

步驟二、利用慣導(dǎo)系統(tǒng)的誤差大小，從步驟一所建立的基準(zhǔn)路徑地圖數(shù)據(jù)庫中選取路徑匹配的基準(zhǔn)路徑范圍；

步驟三、在步驟二中選取的基準(zhǔn)路徑范圍中，利用慣導(dǎo)系統(tǒng)運(yùn)動(dòng)路徑的變化特點(diǎn)，在所選取的基準(zhǔn)路徑范圍內(nèi)中尋找與慣導(dǎo)系統(tǒng)路徑最為匹配的一段路徑。

進(jìn)一步，步驟一所述的基準(zhǔn)路徑地圖數(shù)據(jù)庫的建立，參照車載導(dǎo)航領(lǐng)域常用的地理信息系統(tǒng)標(biāo)準(zhǔn)數(shù)據(jù)結(jié)構(gòu)建立，或者針對(duì)定位定向系統(tǒng)應(yīng)用特點(diǎn)建立基于標(biāo)準(zhǔn)定位點(diǎn)的路徑數(shù)據(jù)庫，以獲取路徑的經(jīng)緯度值；

進(jìn)一步，當(dāng)慣導(dǎo)系統(tǒng)在兩次匹配間隔內(nèi)的誤差發(fā)散大小在δS(δx,δy)以內(nèi)，則選取基準(zhǔn)路徑范圍為當(dāng)前慣導(dǎo)系統(tǒng)位置點(diǎn)相距誤差δS(δx,δy)內(nèi)的基準(zhǔn)路徑地圖數(shù)據(jù)庫中所有基準(zhǔn)路徑。

進(jìn)一步，首先，在慣導(dǎo)系統(tǒng)載體行駛過程中，每隔一定距離記錄慣導(dǎo)系統(tǒng)一位置點(diǎn)如D(x,y)，用所記錄的位置坐標(biāo)D_i(x_i,y_i)與行駛的這段路徑坐標(biāo)均值的差表示這段路徑的變化：

式中：x，y分別為慣導(dǎo)系統(tǒng)坐標(biāo)的經(jīng)緯度，單位rad；

所記錄的每一個(gè)位置點(diǎn)相對(duì)于行駛的這段路徑的變化(dx_i,dy_i)為：

其次，相應(yīng)的，在基準(zhǔn)路徑中取相同數(shù)量的位置點(diǎn)R_i(p_i,q_i)，位置點(diǎn)之間的間隔與上述慣導(dǎo)系統(tǒng)記錄的位置點(diǎn)間隔相同，基準(zhǔn)路徑中這段路徑坐標(biāo)均值為：

式中：p、q分別為基準(zhǔn)路徑坐標(biāo)的經(jīng)緯度，單位rad；

基準(zhǔn)路徑中每個(gè)位置點(diǎn)相對(duì)于此段路徑的變化(dp_i,dq_i)為：

最后，用r表示載體行駛路徑與基準(zhǔn)路徑變化規(guī)律的相近度：

則當(dāng)r最小時(shí)的基準(zhǔn)路徑為與載體路徑最為匹配的路徑。

進(jìn)一步，在路徑匹配過程中，對(duì)慣導(dǎo)系統(tǒng)路徑的變化幅度進(jìn)行判斷，計(jì)算各個(gè)位置點(diǎn)到路徑首尾連線的距離，當(dāng)距離之和小于一定的門限時(shí)，認(rèn)為這段路徑較直，變化較小，此時(shí)路徑匹配的錯(cuò)誤率可能變大，不進(jìn)行位置校正。

進(jìn)一步，所述慣導(dǎo)系統(tǒng)路徑的變化幅度判斷的方法為：

式中：d_i為第i個(gè)點(diǎn)距離路徑首尾連線的距離；

當(dāng)D小于門限T_D時(shí)，認(rèn)為路徑較直，此時(shí)不進(jìn)行匹配；

當(dāng)匹配成功時(shí)，以基準(zhǔn)路徑中的路徑端點(diǎn)為此時(shí)慣導(dǎo)系統(tǒng)的基準(zhǔn)位置信息，慣導(dǎo)系統(tǒng)位置對(duì)應(yīng)的基準(zhǔn)路徑的位置。

進(jìn)一步，該方法還包括以下步驟：

采用卡爾曼濾波的方法完成對(duì)慣導(dǎo)系統(tǒng)誤差的估計(jì)與補(bǔ)償，在慣導(dǎo)系統(tǒng)轉(zhuǎn)導(dǎo)航后，重啟卡爾曼濾波器，持續(xù)進(jìn)行時(shí)間更新；在路徑匹配成功時(shí)，開始量測(cè)更新，以基準(zhǔn)位置為參考信息進(jìn)行位置匹配，在路徑匹配未成功時(shí)仍然進(jìn)行時(shí)間更新。

進(jìn)一步，所述卡爾曼濾波的方法包括以下步驟：

(1)濾波狀態(tài)量：

采用15維的狀態(tài)量：

式中：ΔL、Δλ分別為慣導(dǎo)系統(tǒng)的緯度、經(jīng)度誤差，單位：rad；Δh為慣導(dǎo)系統(tǒng)的高度，單位：m；ΔV_n、ΔV_u、ΔV_e分別為慣導(dǎo)系統(tǒng)的北向、天向、東向速度誤差，單位m/s；Φ_n、Φ_u、Φ_e分別為慣導(dǎo)系統(tǒng)北向、天向、東向失準(zhǔn)角誤差，單位：rad；分別為慣導(dǎo)系統(tǒng)x、y、z軸加速度計(jì)零偏，單位：m/s²；ε_x、ε_y、ε_z分別為慣導(dǎo)系統(tǒng)x、y、z軸陀螺漂移，單位：rad/s；

(2)狀態(tài)方程：

式中：X為狀態(tài)量；F為系統(tǒng)矩陣；W是系統(tǒng)噪聲矩陣，依據(jù)系統(tǒng)的誤差特性確定；

系統(tǒng)矩陣F為：

式中，R_M為子午圈地球半徑，單位：m；R_N為卯酉圈地球半徑，單位：m；V_n，V_u，V_e分別為慣導(dǎo)系統(tǒng)北向、天向、東速向速度，單位：m/s；ω_ie為地球自轉(zhuǎn)角速率，單位rad/s；L為當(dāng)?shù)鼐暥龋瑔挝唬簉ad；h為當(dāng)?shù)馗叨?，單位：m，f_n，f_u，f_e分別為慣導(dǎo)系統(tǒng)北向、天向、東向加速度，單位：m/s²；為系統(tǒng)計(jì)算姿態(tài)矩陣；

(3)量測(cè)方程：

Z＝HX+V (9)

式中：Z為量測(cè)量；H為量測(cè)陣；X為狀態(tài)量；V為量測(cè)噪聲矩陣；

a)Z為量測(cè)量：

式中：l_ins、λ_ins分別為慣導(dǎo)系統(tǒng)的緯度、經(jīng)度，單位：rad；h_ins為慣導(dǎo)系統(tǒng)的高度，單位：m；l_b、λ_b分別為基準(zhǔn)的緯度、經(jīng)度，單位：rad；h_b為基準(zhǔn)的高度，單位：m；

b)H為量測(cè)陣：

當(dāng)進(jìn)行量測(cè)更新時(shí)，

當(dāng)進(jìn)行時(shí)間更新時(shí)，

H＝[0_3×15] (12)

(4)卡爾曼濾波方程：

狀態(tài)一步預(yù)測(cè)：

狀態(tài)估計(jì)：

濾波增益矩陣：

一步預(yù)測(cè)誤差方差陣：

估計(jì)誤差方差陣：

P_k＝[I-K_kH_k]P_k,k-1 (17)

式中：為一步狀態(tài)預(yù)測(cè)值，為狀態(tài)估計(jì)矩陣，Φ_k,k-1為狀態(tài)一步轉(zhuǎn)移矩陣，為的轉(zhuǎn)置矩陣，Φ_k,k-1，H_k為量測(cè)矩陣，為H_k的轉(zhuǎn)置矩陣，Z_k為量測(cè)量，K_k為濾波增益矩陣，R_k為觀測(cè)噪聲陣，P_k,k-1為一步預(yù)測(cè)誤差方差陣，P_k為估計(jì)誤差方差陣，Γ_k,k-1為系統(tǒng)噪聲驅(qū)動(dòng)陣，為Γ_k,k-1的轉(zhuǎn)置矩陣，Q_k-1為系統(tǒng)噪聲陣。

本發(fā)明實(shí)施例提供的技術(shù)方案帶來的有益效果是：

本發(fā)明一種基于路徑匹配的慣導(dǎo)系統(tǒng)誤差修正方法，具有很高自主性的；首先建立路徑地圖數(shù)據(jù)庫，可參照目前車載導(dǎo)航領(lǐng)域常用的地理信息系統(tǒng)(GIS)標(biāo)準(zhǔn)數(shù)據(jù)結(jié)構(gòu)，也可針對(duì)定位定向系統(tǒng)應(yīng)用特點(diǎn)建立基于定位點(diǎn)的路徑數(shù)據(jù)庫；然后在載體行駛過程中選取慣導(dǎo)系統(tǒng)一定路程內(nèi)的行駛軌跡數(shù)據(jù)與慣導(dǎo)系統(tǒng)誤差范圍內(nèi)的所有基準(zhǔn)路徑數(shù)據(jù)進(jìn)行比對(duì)計(jì)算，基于路徑的變化特性完成路徑匹配，獲得當(dāng)前慣導(dǎo)系統(tǒng)所在位置的參考路徑，實(shí)現(xiàn)載體在基準(zhǔn)路徑數(shù)據(jù)庫中的定位，從而用基準(zhǔn)路徑中的位置信息完成對(duì)慣導(dǎo)系統(tǒng)誤差的修正。

本發(fā)明一種基于路徑匹配的慣導(dǎo)系統(tǒng)誤差修正方法，路徑匹配獲得的基準(zhǔn)位置信息是不連續(xù)的，載體行駛一段距離才能獲得一個(gè)基準(zhǔn)點(diǎn)，雖然可以直接用基準(zhǔn)位置信息對(duì)慣導(dǎo)位置進(jìn)行置換，但這樣不能對(duì)慣導(dǎo)的速度、姿態(tài)以及慣性器件誤差進(jìn)行修正；本發(fā)明進(jìn)一步在路徑匹配的基礎(chǔ)上，采用卡爾曼濾波的方法，利用這種不連續(xù)的、稀疏條件下的基準(zhǔn)位置信息實(shí)現(xiàn)對(duì)慣導(dǎo)系統(tǒng)誤差的估計(jì)與修正。

附圖說明

圖1是本發(fā)明路徑匹配的基本原理示意圖；

圖2是本發(fā)明路徑變化不明顯時(shí)的路徑匹配示意圖；

圖3是本發(fā)明路徑變化大小判斷示意圖；

具體實(shí)施方式

下面結(jié)合附圖和具體實(shí)施方式對(duì)本發(fā)明一種基于路徑匹配的慣導(dǎo)系統(tǒng)誤差修正方法作詳細(xì)說明。

本發(fā)明一種基于路徑匹配的慣導(dǎo)系統(tǒng)誤差修正方法，包括以下步驟：

步驟一、建立路徑地圖數(shù)據(jù)庫；

路徑地圖數(shù)據(jù)庫的建立，可參照目前車載導(dǎo)航領(lǐng)域常用的地理信息系統(tǒng)(GIS)標(biāo)準(zhǔn)數(shù)據(jù)結(jié)構(gòu)，也可針對(duì)定位定向系統(tǒng)應(yīng)用特點(diǎn)建立基于標(biāo)準(zhǔn)定位點(diǎn)的路徑數(shù)據(jù)庫；其主要目的是從上述現(xiàn)有數(shù)據(jù)庫中獲取路徑的經(jīng)緯度值；

步驟二、利用慣導(dǎo)系統(tǒng)的誤差大小，選取路徑匹配的基準(zhǔn)路徑范圍；目的是減少計(jì)算量，縮短路徑匹配時(shí)間；

例如，慣導(dǎo)系統(tǒng)在兩次匹配間隔內(nèi)的誤差發(fā)散大小在δS(δx,δy)以內(nèi)，則選取基準(zhǔn)路徑范圍為當(dāng)前慣導(dǎo)系統(tǒng)位置點(diǎn)相距誤差δS(δx,δy)內(nèi)的所有基準(zhǔn)路徑；

步驟三、在步驟二中選取的基準(zhǔn)路徑范圍中，尋找與慣導(dǎo)系統(tǒng)運(yùn)動(dòng)路徑最為匹配的路徑，其基本原理如圖1所示，利用慣導(dǎo)系統(tǒng)運(yùn)動(dòng)路徑的變化特點(diǎn)，在所選取的基準(zhǔn)路徑范圍內(nèi)中尋找與慣導(dǎo)系統(tǒng)路徑最為相近的一段路徑。

實(shí)施例1：

在載體行駛過程中，每隔一定距離記錄慣導(dǎo)系統(tǒng)一位置點(diǎn)如D(x,y)，用所記錄的位置坐標(biāo)D_i(x_i,y_i)與行駛的這段路徑坐標(biāo)均值的差表示路徑的變化，其中：

式中：x，y分別為慣導(dǎo)系統(tǒng)坐標(biāo)的經(jīng)緯度，單位rad；

所記錄的每一個(gè)位置點(diǎn)相對(duì)于行駛的這段路徑的變化(dx_i,dy_i)為：

相應(yīng)的，在基準(zhǔn)路徑中取相同數(shù)量的位置點(diǎn)R_i(p_i,q_i)，位置點(diǎn)之間的間隔與上述慣導(dǎo)系統(tǒng)記錄的位置點(diǎn)間隔相同，基準(zhǔn)路徑中這段路徑坐標(biāo)均值為：

式中：p、q分別為基準(zhǔn)路徑坐標(biāo)的經(jīng)緯度，單位rad；

基準(zhǔn)路徑中每個(gè)位置點(diǎn)相對(duì)于此段路徑的變化(dp_i,dq_i)為：

用r表示載體行駛路徑與基準(zhǔn)路徑變化規(guī)律的相近度：

則當(dāng)r最小時(shí)的基準(zhǔn)路徑為與載體路徑最為匹配的路徑；

實(shí)施例2：

但上述方法也存在一定的局限性，當(dāng)路徑變化越明顯時(shí)，路徑匹配的正確率越高。若較長(zhǎng)的一段路徑比較直，變化不明顯時(shí)，很可能出現(xiàn)匹配錯(cuò)誤的情況，如圖2所示的路徑。

為了避免上述路徑匹配錯(cuò)誤的情況出現(xiàn)，對(duì)慣導(dǎo)路徑的變化大小加以判斷，計(jì)算各個(gè)位置點(diǎn)到路徑首尾連線的距離，當(dāng)距離之和小于一定的門限時(shí)，認(rèn)為這段路徑較直，變化較小，此時(shí)路徑匹配的錯(cuò)誤率可能變大，不進(jìn)行位置校正。

例如圖3所示：

式中：d_i為第i個(gè)點(diǎn)距離路徑首尾連線的距離；

當(dāng)D小于門限T_D時(shí)，認(rèn)為路徑較直，此時(shí)不進(jìn)行匹配；

當(dāng)匹配成功時(shí)，以基準(zhǔn)路徑中的路徑端點(diǎn)為此時(shí)慣導(dǎo)系統(tǒng)的基準(zhǔn)位置信息，慣導(dǎo)位置對(duì)應(yīng)的基準(zhǔn)路徑的位置。

實(shí)施例3：

由于上述路徑匹配獲得的基準(zhǔn)位置信息是不連續(xù)的，車輛行駛一段距離才能獲得一個(gè)基準(zhǔn)點(diǎn)，雖然可以直接用基準(zhǔn)位置信息對(duì)慣導(dǎo)位置進(jìn)行置換，但這樣不能對(duì)慣導(dǎo)系統(tǒng)的速度、姿態(tài)以及慣性器件誤差進(jìn)行修正；

為解決上述技術(shù)問題，進(jìn)一步的采用卡爾曼濾波的方法，利用這種不連續(xù)的、稀疏條件下的基準(zhǔn)位置信息實(shí)現(xiàn)對(duì)慣導(dǎo)系統(tǒng)誤差的估計(jì)與修正。

具體方法為：在慣導(dǎo)系統(tǒng)轉(zhuǎn)導(dǎo)航后，重啟卡爾曼濾波器，持續(xù)進(jìn)行時(shí)間更新；在路徑匹配成功時(shí)，開始量測(cè)更新，以基準(zhǔn)位置為參考信息進(jìn)行位置匹配，在路徑匹配未成功時(shí)仍然進(jìn)行時(shí)間更新。

例如，一種濾波模型如下：

(1)濾波狀態(tài)量：

采用15維的狀態(tài)量：

式中：ΔL、Δλ分別為慣導(dǎo)系統(tǒng)的緯度、經(jīng)度誤差，單位：rad；Δh為慣導(dǎo)系統(tǒng)的高度，單位：m；ΔV_n、ΔV_u、ΔV_e分別為慣導(dǎo)系統(tǒng)的北向、天向、東向速度誤差，單位m/s；Φ_n、Φ_u、Φ_e分別為慣導(dǎo)系統(tǒng)北向、天向、東向失準(zhǔn)角誤差，單位：rad；分別為慣導(dǎo)系統(tǒng)x、y、z軸加速度計(jì)零偏，單位：m/s²；ε_x、ε_y、ε_z分別為慣導(dǎo)系統(tǒng)x、y、z軸陀螺漂移，單位：rad/s；

(2)狀態(tài)方程：

式中：X為狀態(tài)量；F為系統(tǒng)矩陣；W是系統(tǒng)噪聲矩陣，依據(jù)系統(tǒng)的誤差特性確定；

系統(tǒng)矩陣F為：

式中，R_M為子午圈地球半徑，單位：m；R_N為卯酉圈地球半徑，單位：m；V_n，V_u，V_e分別為慣導(dǎo)系統(tǒng)北向、天向、東速向速度，單位：m/s；ω_ie為地球自轉(zhuǎn)角速率，單位rad/s；L為當(dāng)?shù)鼐暥?，單位：rad；h為當(dāng)?shù)馗叨?，單位：m，f_n，f_u，f_e分別為慣導(dǎo)系統(tǒng)北向、天向、東向加速度，單位：m/s²；為系統(tǒng)計(jì)算姿態(tài)矩陣；

(3)量測(cè)方程：

Z＝HX+V (9)

式中：Z為量測(cè)量；H為量測(cè)陣；X為狀態(tài)量；V為量測(cè)噪聲矩陣；

a)Z為量測(cè)量：

式中：l_ins、λ_ins分別為慣導(dǎo)系統(tǒng)的緯度、經(jīng)度，單位：rad；h_ins為慣導(dǎo)系統(tǒng)的高度，單位：m；l_b、λ_b分別為基準(zhǔn)的緯度、經(jīng)度，單位：rad；h_b為基準(zhǔn)的高度，單位：m；

b)H為量測(cè)陣：

當(dāng)進(jìn)行量測(cè)更新時(shí)，

當(dāng)進(jìn)行時(shí)間更新時(shí)，

H＝[0_3×15] (12)

(4)卡爾曼濾波方程：

狀態(tài)一步預(yù)測(cè)：

狀態(tài)估計(jì)：

濾波增益矩陣：

一步預(yù)測(cè)誤差方差陣：

估計(jì)誤差方差陣：

P_k＝[I-K_kH_k]P_k,k-1 (17)

式中：為一步狀態(tài)預(yù)測(cè)值，為狀態(tài)估計(jì)矩陣，Φ_k,k-1為狀態(tài)一步轉(zhuǎn)移矩陣，為的轉(zhuǎn)置矩陣，Φ_k,k-1，H_k為量測(cè)矩陣，為H_k的轉(zhuǎn)置矩陣，Z_k為量測(cè)量，K_k為濾波增益矩陣，R_k為觀測(cè)噪聲陣，P_k,k-1為一步預(yù)測(cè)誤差方差陣，P_k為估計(jì)誤差方差陣，Γ_k,k-1為系統(tǒng)噪聲驅(qū)動(dòng)陣，為Γ_k,k-1的轉(zhuǎn)置矩陣，Q_k-1為系統(tǒng)噪聲陣。