本發(fā)明涉及車載導(dǎo)航技術(shù)領(lǐng)域，具體涉及一種導(dǎo)航定位方法及設(shè)備。

背景技術(shù)：

車載導(dǎo)航定位技術(shù)是智能交通系統(tǒng)的一個重要組成部分，也是關(guān)鍵技術(shù)之一。車載導(dǎo)航要求能夠連續(xù)、實時、準(zhǔn)確地確定車輛的位置，目前可用于車載導(dǎo)航技術(shù)的導(dǎo)航方式有：衛(wèi)星導(dǎo)航(如全球定位系統(tǒng)、北斗衛(wèi)星導(dǎo)航系統(tǒng))、里程計、微機(jī)電系統(tǒng)(Microelectro Mechanical Systems,MEMS)傳感器、磁傳感器、地圖匹配等等。衛(wèi)星導(dǎo)航定位精度較高、技術(shù)成熟、使用方便，但是它容易受到高樓、高架、隧道、地下車庫等的遮蔽而失效；而以里程計、MEMS傳感器進(jìn)行導(dǎo)航定位的航位推算算法(Dead-reckoning,DR)是一種低成本、自主的車輛導(dǎo)航定位方式，它的優(yōu)點(diǎn)是抗干擾能力強(qiáng)，能根據(jù)提供的傳感器數(shù)據(jù)，在短時間內(nèi)提供車輛較精確的導(dǎo)航參數(shù)，但是DR算法的誤差隨時間積累，不適合長時間的單獨(dú)導(dǎo)航；地圖匹配算法主要作為一種輔助導(dǎo)航手段，結(jié)合數(shù)字地圖中道路路線情況對導(dǎo)航結(jié)果進(jìn)行輔助修正，以提高定位精度，但存在一定的局限性。

目前，國內(nèi)外的車載導(dǎo)航系統(tǒng)，大多都采用衛(wèi)星導(dǎo)航和其他定位方法組合的方式來實現(xiàn)對車輛的導(dǎo)航和定位，主要有以下兩種組合方式：地圖匹配/衛(wèi)星組合導(dǎo)航系統(tǒng)以及DR/衛(wèi)星組合導(dǎo)航系統(tǒng)，但是在衛(wèi)星信號受到干擾甚至失效的情況下，這兩種組合導(dǎo)航系統(tǒng)的導(dǎo)航誤差將會顯著增大甚至不能使用。

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

本發(fā)明實施例公開了一種導(dǎo)航定位方法及設(shè)備，以期實現(xiàn)導(dǎo)航信號無效時的車輛定位導(dǎo)航，提高車載導(dǎo)航定位的準(zhǔn)確性。

第一方面，本發(fā)明實施例公開一種導(dǎo)航定位方法。方法包括：

當(dāng)檢測到衛(wèi)星信號失效時，以第k個計算周期的磁航向角為觀測量，通過所述第k個計算周期的航位推算算法(Dead-reckoning,DR)/磁航向組合導(dǎo)航算法模型得到所述第k個計算周期的第一狀態(tài)量組合，以所述第k個計算周期的橫向速度為觀測量，通過所述第k個計算周期的DR/車輛運(yùn)動組合導(dǎo)航算法模型得到所述第k個計算周期的第二狀態(tài)量組合，所述k為大于2的整數(shù)；

根據(jù)所述第k個計算周期的系統(tǒng)狀態(tài)方程，針對所述第k個計算周期的所述第一狀態(tài)量組合和所述第k個計算周期的所述第二狀態(tài)量組合進(jìn)行閉環(huán)卡爾曼組合濾波，得到所述第k個計算周期的第一誤差修正值；

根據(jù)所述第k個計算周期的所述第一誤差修正值修正所述第k個計算周期的初始航向角，得到所述第k個計算周期的目標(biāo)航向角，根據(jù)所述第k個計算周期的所述目標(biāo)航向角和所述第k個計算周期的DR模型，計算得到所述第k個計算周期的目標(biāo)速度和目標(biāo)位置。

由上可見，本發(fā)明實施例公開的導(dǎo)航定位方法，當(dāng)設(shè)備檢測到衛(wèi)星信號失效時，以第k個計算周期的磁航向角為觀測量，通過DR/磁航向組合導(dǎo)航算法模型得到第一狀態(tài)量組合，以第k個計算周期的橫向速度為觀測量，通過DR/車輛運(yùn)動組合導(dǎo)航算法模型得到第二狀態(tài)量組合；根據(jù)第k個計算周期的系統(tǒng)狀態(tài)方程，針對第一狀態(tài)量組合和第二狀態(tài)量組合進(jìn)行閉環(huán)卡爾曼組合濾波得到第一誤差修正值；根據(jù)第一誤差修正值修正第k個計算周期的初始航向角得到目標(biāo)航向角，根據(jù)目標(biāo)航向角和第k個計算周期的DR模型，計算得到目標(biāo)速度和目標(biāo)位置。可見，通過DR/磁航向組合導(dǎo)航算法模型和DR/車輛運(yùn)動組合導(dǎo)航算法模型在衛(wèi)星信號失效時對航向角進(jìn)行修正，進(jìn)而根據(jù)DR模型推算出目標(biāo)速度和目標(biāo)位置信息，實現(xiàn)了在衛(wèi)星信號無效時的車輛定位導(dǎo)航，提高了車載定位的準(zhǔn)確性。

在一個可能的設(shè)計中，所述第k個計算周期的DR/磁航向組合導(dǎo)航算法模型是根據(jù)所述第k個計算周期的初始航向角以及所述第k個計算周期的磁航向角而建立的，所述第k個計算周期的磁航向角是根據(jù)磁傳感器輸出數(shù)據(jù)計算得到的，所述第k個計算周期的初始航向角是根據(jù)第k-1個計算周期的目標(biāo)航向角和第k個計算周期獲取的角速度計算得到的，所述第k個計算周期的DR/車輛運(yùn)動組合導(dǎo)航算法模型是根據(jù)所述第k個計算周期的橫向速度而建立的，所述第k個計算周期的橫向速度是根據(jù)所述第k個計算周期的初始速度和所述第k個計算周期的初始航向角計算得到的，所述第k個計算周期的初始速度是根據(jù)所述第k個計算周期的初始航向角、第k-1個計算周期的目標(biāo)速度以及所述第k個計算周期的MEMS慣性傳感器加速度計輸出值計算得到；

所述第k個計算周期的系統(tǒng)狀態(tài)方程是根據(jù)所述第k-1個計算周期的狀態(tài)量而建立的；

所述第k個計算周期的DR模型是根據(jù)所述第k個計算周期的初始速度和初始位置而建立的，所述第k個計算周期的初始位置是根據(jù)所述第k個計算周期的初始速度和第k-1個計算周期的目標(biāo)位置計算得到的。

在一個可能的設(shè)計中，所述方法還包括：

當(dāng)檢測到衛(wèi)星信號有效時，以第k+1個計算周期的初始速度和初始位置為觀測量，通過所述第k+1個計算周期的DR/衛(wèi)星組合導(dǎo)航算法模型得到所述第k+1個計算周期的第三狀態(tài)量組合，所述第k+1個計算周期的DR/衛(wèi)星組合導(dǎo)航算法模型是根據(jù)所述第k+1個計算周期的所述初始速度和所述初始位置而建立的；

根據(jù)所述第k+1個計算周期的系統(tǒng)狀態(tài)方程，針對所述第k+1個計算周期的所述第一狀態(tài)量組合和所述第k+1個計算周期的所述第三狀態(tài)量組合進(jìn)行所述閉環(huán)卡爾曼組合濾波，得到所述第k+1個計算周期的第二誤差修正值；

根據(jù)所述第k+1個計算周期的所述第二誤差修正值修正所述第k+1個計算周期的初始航向角，得到所述第k+1個計算周期的目標(biāo)航向角，根據(jù)所述第k+1個計算周期的所述目標(biāo)航向角和所述第k+1個計算周期的DR模型，計算得到所述第k+1個計算周期的目標(biāo)速度和目標(biāo)位置。

由上可見，本可能的設(shè)計中公開的導(dǎo)航定位方法，除了實現(xiàn)了在衛(wèi)星信號無效時的車輛定位導(dǎo)航，提高了車載定位的準(zhǔn)確性之外，設(shè)備在城市峽谷、高架遮擋等衛(wèi)星信號受到干擾但非失效環(huán)境下，利用第二誤差修正值對航向角進(jìn)行修正，提高了衛(wèi)星信號受到干擾情況下的導(dǎo)航定位的精度性。

在一個可能的設(shè)計中，所述第k個計算周期的所述第一狀態(tài)量組合包括:所述第k個計算周期的磁航向角組合量測狀態(tài)Z_Mag，k，第一量測矩陣H_Mag，以及所述第k個計算周期的第一量測噪聲V_Mag，k；

基于如下公式計算所述Z_Mag，k

Z_Mag,k＝[Φ_DR,k-Φ_Mag,k]

基于如下公式計算所述H_Mag

H_Mag＝[1 0 0 0 0 0 0 0]

基于如下公式計算所述V_Mag，k

其中，Φ_DR,k為所述第k個計算周期初始航向角，Φ_Mag，k為所述第k個計算周期的磁航向角，為第k個計算周期得到的航向角誤差。

在一個可能的設(shè)計中，所述第k個計算周期的所述第二狀態(tài)量組合包括：所述第k個計算周期的車輛橫向速度量測狀態(tài)Z_Virt，k，所述第k個計算周期的第二量測矩陣H_Virt，k，以及所述第k個計算周期的第二量測噪聲V_Virt，k；

基于如下公式計算所述Z_Virt，k

基于如下公式計算所述H_Virt，k

H_Virt,k＝[-ν_eDR,ksinΦ_DR,k-ν_nDR,kcosΦ_DR,k cosΦ_DR,k-sinΦ_DR,k 0 0 0 0 0 0]

基于如下公式計算所述V_Virt，k

V_Virt,k＝[δv_x,k]

其中，分別為所述第k個計算周期得到的東向和北向速度誤差，v_eDR,k為所述第k個計算周期的初始東向速度；v_nDR,k為所述第k個計算周期的初始北向速度；δv_x,k為所述第k個計算周期車輛運(yùn)動時由于側(cè)滑所引起的附加橫向速度變化。

在一個可能的設(shè)計中，所述第k+1個計算周期的第三狀態(tài)量組合包括：所述第k+1個計算周期的經(jīng)度、緯度、東向速度和北向速度的組合量測狀態(tài)Z_Sat,k+1，所述第k+1個計算周期的第三量測矩陣H_Sat,k+1，以及所述第k+1個計算周期的第三量測噪聲V_Sat,k+1；

基于如下公式計算所述Z_Sat,k+1

基于如下公式計算所述H_Sat,k+1

基于如下公式計算所述V_Sat,k+1

其中，λ_DR,k+1和L_DR,k+1分別為所述第k+1個計算周期的經(jīng)度和緯度；λ_Sat,k+1、L_Sat,k+1、v_eSat,k+1和v_nSat,k+1分別為所述第k+1個計算周期的衛(wèi)星接收機(jī)定位的經(jīng)度、緯度、東向速度以及北向速度；R_m和R_n分別為子午圈半徑和卯酉圈半徑；N_eSat,k+1、N_nSat,k+1、M_eSat,k+1和M_nSat,k+1分別為所述第k+1個計算周期衛(wèi)星接收機(jī)計算出來的定位信息本身含有的經(jīng)度、緯度、東向速度和北向速度誤差。

由上可見，本可能的設(shè)計中公開的導(dǎo)航定位方法，除了實現(xiàn)了在衛(wèi)星信號無效時的車輛定位導(dǎo)航，提高了車載定位的準(zhǔn)確性之外，設(shè)備通過每個計算周期更新的初始速度和初始位置不斷地更新所述第一狀態(tài)量組合，所述第二狀態(tài)量組合，所述第三狀態(tài)量組合，提高了所述第一修正值，所述第二修正值的輸出精度，保持了每個計算周期導(dǎo)航信息的持續(xù)可靠輸出。

在一個可能的設(shè)計中，所述第k個計算周期的系統(tǒng)狀態(tài)方程為：

X_k＝A_k,k-1X_k-1+G_k-1W_k-1

其中，A_k,k-1為第所述k個計算周期的狀態(tài)轉(zhuǎn)移矩陣，X_k為所述第k個計算周期的系統(tǒng)狀態(tài)量，G_k-1為所述第k-1個計算周期的系統(tǒng)噪聲矩陣，W_k-1為所述第k-1個計算周期的白噪聲隨機(jī)誤差矢量，和分別為所述第k個計算周期得到的精度和緯度誤差，ε_r為陀螺的一階馬爾科夫誤差，ε_b隨機(jī)常數(shù)誤差，和分別為所述第k個計算周期的加速度計右向和前向一階馬爾科夫誤差。

在一個可能的設(shè)計中，所述根據(jù)所述第k個計算周期的系統(tǒng)狀態(tài)方程，針對所述第k個計算周期的所述第一狀態(tài)量組合和所述第k個計算周期的所述第二狀態(tài)量組合進(jìn)行閉環(huán)卡爾曼組合濾波得到所述第k個計算周期的第一誤差修正，包括：

通過如下公式計算得到所述第k個計算周期的第一誤差修正值

其中，為所述第k個計算周期的第一誤差修正值，

在一個可能的設(shè)計中，根據(jù)所述第k+1個計算周期的系統(tǒng)狀態(tài)方程，針對所述第k+1個計算周期的所述第一狀態(tài)量組合和所述第k+1個計算周期的所述第三狀態(tài)量組合進(jìn)行所述閉環(huán)卡爾曼組合濾波得到所述第k+1個計算周期的第二誤差修正值，包括：

通過如下公式計算得到所述第k+1個計算周期的第二誤差修正值

其中，為所述第k+1個計算周期的第二誤差修正值，

在一個可能的設(shè)計中，所述根據(jù)所述第k個計算周期的所述第一誤差修正值修正所述第k個計算周期的初始航向角以得到所述第k個計算周期的目標(biāo)航向角，包括：

通過如下公式計算得到所述第k個計算周期的目標(biāo)航向角Φ′_k，

其中，所述Φ′_k為所述第k個計算周期的目標(biāo)航向角。

由上可見，本可能的設(shè)計中公開的導(dǎo)航定位方法，除了實現(xiàn)了在衛(wèi)星信號無效時的車輛定位導(dǎo)航，提高了車載定位的準(zhǔn)確性之外，設(shè)備通過閉環(huán)組合卡爾曼濾波算法，對航向角進(jìn)行修正，進(jìn)一步提高了導(dǎo)航定位信息的輸出精度。

在一個可能的設(shè)計中，所述根據(jù)所述第k個計算周期的所述目標(biāo)航向角和所述第k個計算周期的DR模型，計算得到所述第k個計算周期的目標(biāo)速度和目標(biāo)位置，包括：

通過如下公式計算得到所述第k個計算周期的目標(biāo)東向速度v_e，k，所述第k個計算周期目標(biāo)北向速度v_n，k，所述第k個計算周期的目標(biāo)經(jīng)度λ_k和所述第k個計算周期的目標(biāo)緯度L_k，

f_e,k＝f_x′_,kcosΦ′_k+f′_y,ksinΦ′_k

f_n,k＝-f′_x,ksinΦ′_k+f′_y,kcosΦ′_k

v_e,k＝v_e,k-1+Tf_e,k

v_n,k＝v_n,k-1+Tf_n,k

L_k＝L_k-1+Tv_n,k/R_m

λ_k＝λ_k-1+Tv_e,k/(R_ncosL_k)

其中，f′_x，k和f′_y，k分別為所述第k個計算周期MEMS慣性傳感器中加速度計輸出的右向和前向加速度值的修正值，v_e，k為所述第k個計算周期的目標(biāo)東向速度，v_n，k為所述第k個計算周期的目標(biāo)北向速度；f_e,k和f_n,k分別為所述第k個計算周期的東向和北向的加速度投影；T為傳感器采樣時間間隔；L_k為所述第k個計算周期的目標(biāo)緯度，λ_k為所述第k個計算周期的目標(biāo)經(jīng)度。

在這個可能的設(shè)計中，所述第k個計算周期MEMS慣性傳感器中加速度計輸出的右向和前向加速度值的修正值f′_x，k和f′_y，k分別由所述MEMS慣性傳感器安裝過程中存在的俯仰角偏差和橫滾角偏差修正所述第k個計算周期MEMS慣性傳感器中加速度計輸出的右向和前向加速度值所得。

由上可見，本可能的設(shè)計中公開的導(dǎo)航定位方法，除了實現(xiàn)了在衛(wèi)星信號無效時的車輛定位導(dǎo)航，提高了車載定位的準(zhǔn)確性之外，設(shè)備通過對低精度的MEMS慣性傳感器安裝過程中存在的俯仰角偏差和橫滾角偏差進(jìn)行修正，提高了MEMS慣性傳感器水平加速度計的輸出精度，進(jìn)而高了導(dǎo)航定位的目標(biāo)速度和目標(biāo)位置的輸出精度。

第二方面，本發(fā)明實施例公開一種設(shè)備，該設(shè)備具有實現(xiàn)上述方法設(shè)計中設(shè)備的行為的功能。所述功能可以通過硬件實現(xiàn)，也可以通過硬件執(zhí)行相應(yīng)的軟件實現(xiàn)。所述硬件或軟件包括一個或多個與上述功能相對應(yīng)的模塊。

在一個可能的設(shè)計中，設(shè)備包括處理器，所述處理器被配置為支持設(shè)備執(zhí)行上述方法中相應(yīng)的功能。進(jìn)一步的，設(shè)備還可以包括接收器和發(fā)射器，所述接收器和發(fā)射器用于設(shè)備與其他設(shè)備如導(dǎo)航衛(wèi)星之間的通信。進(jìn)一步的，設(shè)備還可以包括存儲器，所述存儲器用于與處理器耦合，其保存設(shè)備必要的程序指令和數(shù)據(jù)。

第三方面，本發(fā)明實施例公開一種計算機(jī)可讀存儲介質(zhì)，其中，所述計算機(jī)可讀存儲介質(zhì)存儲有用于計算機(jī)設(shè)備執(zhí)行的程序代碼，該程序代碼具體包括執(zhí)行指令，所述執(zhí)行指令用于執(zhí)行本發(fā)明實施例第一方面任一方法中所描述的部分或全部步驟。

由上可見，本發(fā)明實施例公開的導(dǎo)航定位方法，當(dāng)設(shè)備檢測到衛(wèi)星信號失效時，以第k個計算周期的磁航向角為觀測量，通過DR/磁航向組合導(dǎo)航算法模型得到第一狀態(tài)量組合，以第k個計算周期的橫向速度為觀測量，通過DR/車輛運(yùn)動組合導(dǎo)航算法模型得到第二狀態(tài)量組合；根據(jù)第k個計算周期的系統(tǒng)狀態(tài)方程，針對第一狀態(tài)量組合和第二狀態(tài)量組合進(jìn)行閉環(huán)卡爾曼組合濾波得到第一誤差修正值；根據(jù)第一誤差修正值修正第k個計算周期的初始航向角得到目標(biāo)航向角，根據(jù)目標(biāo)航向角和第k個計算周期的DR模型，計算得到目標(biāo)速度和目標(biāo)位置?？梢?，通過DR/磁航向組合導(dǎo)航算法模型和DR/車輛運(yùn)動組合導(dǎo)航算法模型在衛(wèi)星信號失效時對航向角進(jìn)行修正，進(jìn)而根據(jù)DR模型推算出目標(biāo)速度和目標(biāo)位置信息，實現(xiàn)了在衛(wèi)星信號無效時的車輛定位導(dǎo)航，提高了車載定位的準(zhǔn)確性。

附圖說明

為了更清楚地說明本發(fā)明實施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對實施例或現(xiàn)有技術(shù)描述中所需要使用的附圖作簡單地介紹，顯而易見地，下面描述中的附圖僅僅是本發(fā)明的一些實施例，對于本領(lǐng)域普通技術(shù)人員來講，在不付出創(chuàng)造性勞動的前提下，還可以根據(jù)這些附圖獲得其他的附圖。

圖1是本發(fā)明實施例公開的一種導(dǎo)航定位方法的流程示意圖；

圖2A是本發(fā)明實施例公開的一種導(dǎo)航控制設(shè)備的功能單元框圖；

圖2B是本發(fā)明實施例公開的一種導(dǎo)航控制設(shè)備的結(jié)構(gòu)示意圖。

具體實施方式

下面結(jié)合本發(fā)明實施例中的附圖，對本發(fā)明實施例中的技術(shù)方案進(jìn)行描述。

請參閱圖1，圖1是本發(fā)明實施例公開了一種導(dǎo)航定位方法的流程示意圖，該方法應(yīng)用于導(dǎo)航定位系統(tǒng)，該導(dǎo)航定位系統(tǒng)可以是由安裝于車輛中的導(dǎo)航顯示屏、導(dǎo)航控制設(shè)備、衛(wèi)星接收機(jī)、傳感器(如磁傳感器、慣性傳感器(加速度計和陀螺儀)等)組成，如圖所示，本導(dǎo)航定位方法包括：

S101，當(dāng)導(dǎo)航控制設(shè)備檢測到衛(wèi)星信號失效時，以第k個計算周期的磁航向角為觀測量，通過所述第k個計算周期的航位推算算法(Dead-reckoning,DR)/磁航向組合導(dǎo)航算法模型得到所述第k個計算周期的第一狀態(tài)量組合，以所述第k個計算周期的橫向速度為觀測量，通過所述第k個計算周期的DR/車輛運(yùn)動組合導(dǎo)航算法模型得到所述第k個計算周期的第二狀態(tài)量組合；

其中，所述k為大于2的整數(shù)，所述第k個計算周期的DR/磁航向組合導(dǎo)航算法模型是根據(jù)所述第k個計算周期的初始航向角以及所述第k個計算周期的磁航向角而建立的，所述第k個計算周期的磁航向角是根據(jù)磁傳感器輸出數(shù)據(jù)計算得到的，所述第k個計算周期的初始航向角是根據(jù)第k-1個計算周期的目標(biāo)航向角和第k個計算周期獲取的角速度計算得到的，所述第k個計算周期的DR/車輛運(yùn)動組合導(dǎo)航算法模型是根據(jù)所述第k個計算周期的橫向速度而建立的，所述第k個計算周期的橫向速度是根據(jù)所述第k個計算周期的初始速度和所述第k個計算周期的初始航向角計算得到的，所述第k個計算周期的初始速度是根據(jù)所述第k個計算周期的初始航向角、第k-1個計算周期的目標(biāo)速度以及所述第k個計算周期的MEMS慣性傳感器加速度計輸出值計算得到。

具體的，所述第k個計算周期的DR/磁航向組合導(dǎo)航算法模型是根據(jù)所述第k個計算周期的初始航向角以及所述第k個計算周期的磁航向角而建立的，包括：

首先，所述導(dǎo)航控制設(shè)備根據(jù)第k個計算周期水平方向磁傳感器輸出數(shù)據(jù)，按下式計算第k個計算周期的磁航向角Φ_Mag，k：

式中，mag_x，k和mag_y，k分別為第k個計算周期磁傳感器X軸和Y軸的輸出數(shù)據(jù)。

同時，第k個計算周期的初始航向角以及第k個計算周期的磁傳感器輸出的航向角信息可分別表示為：

Φ_Mag,k＝Φ_k-E_Mag,k

式中，Φ_DR,k為第k個計算周期的初始航向角，為第k個計算周期得到的航向角誤差，E_Mag，k為第k個計算周期得到的磁航向角誤差，Φ_k表示第k個計算周期的航向角的真實值。

其中，第k個計算周期的初始航向角的遞推公式為：

Φ_DR,k＝Φ′_k-1-Tω_z,k

式中，第一個計算周期的初始航向角的初始值與第一個計算周期的磁航向角近似相等，所述第一個計算周期的磁航向角是通過所述導(dǎo)航定位系統(tǒng)的磁傳感器獲取的，Φ′_k-1為第k-1個計算周期的目標(biāo)航向角，ω_z,k為第k個計算周期的豎直方向的所述MEMS慣性傳感器中的陀螺儀輸出的角速度。

其次，所述導(dǎo)航控制設(shè)備建立第k個計算周期磁航向角組合量測狀態(tài)方程，包括第一狀態(tài)量組合Z_Mag，k、H_Mag、V_Mag，k；

其中，第k個計算周期磁航向角組合量測狀態(tài)方程為：

其中，第一量測矩陣H_Mag為：

H_Mag＝[1 0 0 0 0 0 0 0]

第k個計算周期的第一量測噪聲V_Mag，k為：

具體的，所述第k個計算周期的DR/車輛運(yùn)動組合導(dǎo)航算法模型是根據(jù)所述第k個計算周期的橫向速度而建立的，包括：

首先，所述導(dǎo)航控制設(shè)備建立第k個計算周期的車輛運(yùn)動速度的量測狀態(tài)方程模型：

根據(jù)第k個計算周期的初始東向速度v_eDR，k和初始北向速度v_nDR，k，計算出第k個計算周期的車輛橫向速度參數(shù)為，

其中，v_eDR，k，v_nDR，k分別為所述第k個計算周期得到的東向和北向速度誤差。

其次，所述導(dǎo)航控制設(shè)備將所述橫向速度進(jìn)行最小量化展開，獲得線性化的量測狀態(tài)方程：

再次，在實際車輛運(yùn)動情況下，考慮到路面不平整以及轉(zhuǎn)彎側(cè)滑等因素，則第k個計算周期的橫向速度約束方程為：

v_x,CAR,k＝v_x,k-δv_x,k＝(v_eDR,kcosΦ_DR,k-v_nDR,ksinΦ_DR,k)-δv_x,k

式中，v_x,k為第k個計算周期的橫側(cè)向運(yùn)動速度真實值，δv_x,k表示第k個計算周期車輛運(yùn)動時由于側(cè)滑所引起的附加橫向速度變化，建模為白噪聲，其取值大小和車輛的具體運(yùn)動特性有關(guān)，當(dāng)車輛運(yùn)行速度大時，側(cè)滑速度大。

最后，所述導(dǎo)航控制設(shè)備根據(jù)所述線性化橫向速度量測狀態(tài)方程和所述橫向速度約束方程，可以構(gòu)造第k個計算周期的橫向速度虛擬量測方程為：

從而獲得第k個計算周期的車輛橫向速度量測狀態(tài)方程，包括第二狀態(tài)量組合Z_Virt，k、H_Virt，k、V_Virt，k。

所述第k個計算周期的車輛橫向速度量測狀態(tài)方程為：

Z_Virt,k＝H_Virt,kX_k+V_Virt,k

其中，第k個計算周期的第二量測矩陣H_Virt,k為：

H_Virt,k＝[-ν_eDR,ksinΦ_DR,k-ν_nDR,kcosΦ_DR,k cosΦ_DR,k-sinΦ_DR,k 0 0 0 0 0 0]

第k個計算周期的第二量測噪聲V_Virt,k為：

V_Virt,k＝[δv_x,k]

S102，所述導(dǎo)航控制設(shè)備根據(jù)所述第k個計算周期的系統(tǒng)狀態(tài)方程，針對所述第k個計算周期的所述第一狀態(tài)量組合和所述第k個計算周期的所述第二狀態(tài)量組合進(jìn)行閉環(huán)卡爾曼組合濾波，得到所述第k個計算周期的第一誤差修正值；

其中，所述系統(tǒng)狀態(tài)方程為：

式中，A為狀態(tài)轉(zhuǎn)移矩陣，G為系統(tǒng)噪聲矩陣，W白噪聲隨機(jī)誤差矢量，其中：

W＝[ω_gz ω_rz ω_ax ω_ay]

式中，f_x和f_y分別為靜止情況下DR模型中獲取的MEMS慣性傳感器的加速度計輸出的右向和前向加速度值；R_m和R_n分別為子午圈半徑和卯酉圈半徑；L_DR為初始緯度；v_eDR為初始東向速度；ω_gz為陀螺白噪聲，ω_rz為陀螺一階馬爾科夫驅(qū)動白噪聲，ω_ax和ω_ay為加速度計一階馬爾科夫驅(qū)動白噪聲；T_ε和T_a分別為MEMS慣性傳感器中陀螺儀和加速度計的一階馬爾科夫相關(guān)時間。

其中，所述系統(tǒng)狀態(tài)方程以航向角誤差水平速度誤差和位置誤差和陀螺的一階馬爾科夫誤差ε_r和隨機(jī)常數(shù)誤差ε_b以及加速度計一階馬爾科夫誤差和作為系統(tǒng)狀態(tài)量，建立系統(tǒng)狀態(tài)方程，所述系統(tǒng)狀態(tài)量為：

將所述系統(tǒng)狀態(tài)方程離散化處理得第k個計算周期的系統(tǒng)狀態(tài)方程：

X_k＝A_k,k-1X_k-1+G_k-1W_k-1

其中，

具體的，在衛(wèi)星信號失效時，所述導(dǎo)航控制設(shè)備根據(jù)所述第k個計算周期的車輛系統(tǒng)狀態(tài)方程，針對所述第k個計算周期的所述第一狀態(tài)量組合和所述第k個計算周期的所述第二狀態(tài)量組合進(jìn)行閉環(huán)卡爾曼組合濾波，得到所述第k個計算周期的第一誤差修正值包括：

式中，

其中：

P,Q的初始值與MEMS傳感器的誤差參數(shù)有關(guān)。

S103，所述導(dǎo)航控制設(shè)備根據(jù)所述第k個計算周期的所述第一誤差修正值修正所述第k個計算周期的初始航向角，得到所述第k個計算周期的目標(biāo)航向角，根據(jù)所述第k個計算周期的所述目標(biāo)航向角和所述第k個計算周期的DR模型，計算得到所述第k個計算周期的目標(biāo)速度和目標(biāo)位置。

其中，所述第k個計算周期的DR模型是根據(jù)所述第k個計算周期的初始速度和初始位置而建立的，所述第k個計算周期的初始位置是根據(jù)所述第k個計算周期的初始速度和第k-1個計算周期的目標(biāo)位置計算得到的。

首先，所述導(dǎo)航控制設(shè)備根據(jù)所述第k個計算周期的所述第一誤差修正值修正所述第k個計算周期的初始航向角，得到所述第k個計算周期的目標(biāo)航向角，包括：

通過所述第k個計算周期的所述第一誤差修正值中的如下所述公式計算得到所述第k個計算周期的目標(biāo)航向角Φ′_k，

其中，所述Φ′_k為所述第k個計算周期的目標(biāo)航向角。

其次，所述導(dǎo)航控制設(shè)備根據(jù)所述第k個計算周期的所述目標(biāo)航向角和所述第k個計算周期的DR模型，計算得到所述第k個計算周期的目標(biāo)速度和目標(biāo)位置，包括：

根據(jù)所述第k個計算周期的初始速度和初始位置建立所述第k個計算周期的DR模型；

具體的，所述第k個計算周期的DR模型的導(dǎo)航坐標(biāo)系選擇東北天坐標(biāo)系，載體坐標(biāo)系選為右前上坐標(biāo)系；

通過如下第k個計算周期的DR模型公式計算得到所述第k個計算周期的目標(biāo)東向速度v_e，k，所述第k個計算周期目標(biāo)北向速度v_n,k，所述第k個計算周期的目標(biāo)經(jīng)度λ_k和所述第k個計算周期的目標(biāo)緯度L_k，

f_e,k＝f′_x,kcosΦ′_k+f′_y,ksinΦ′_k

f_n,k＝-f′_x,ksinΦ′_k+f′_y,kcosΦ′_k

v_e,k＝v_e,k-1+Tf_e,k

v_n,k＝v_n,k-1+Tf_n,k

L_k＝L_k-1+Tv_n,k/R_m

λ_k＝λ_k-1+Tv_e,k/(R_ncosL_k)

其中，f′_x,k和f′_y,k分別為所述第k個計算周期MEMS慣性傳感器中加速度計輸出的右向和前向加速度值的修正值，v_e,k為所述第k個計算周期的目標(biāo)東向速度，v_n,k為所述第k個計算周期的目標(biāo)北向速度；f_e,k和f_n,k分別為所述第k個計算周期的東向和北向的加速度投影；T為傳感器采樣時間間隔；L_k為所述第k個計算周期的目標(biāo)緯度，λ_k為所述第k個計算周期的目標(biāo)經(jīng)度。

其中，所述第k個計算周期微機(jī)電系統(tǒng)(Microelectro Mechanical Systems,MEMS)慣性傳感器中加速度計輸出的右向和前向加速度值的修正值f′_x,k和f′_y,k分別由所述MEMS慣性傳感器安裝過程中存在的俯仰角偏差和橫滾角偏差修正所述第k個計算周期MEMS慣性傳感器中加速度計輸出的右向和前向加速度值所得，包括：

通過如下所述公式獲得修正后第k個計算周期的加速度計輸出f′_x,k和f′_y,k為：

f'＝(C₁C₂)^-1f

其中：

f′＝[f′_x,k f′_y,k f′_z,k]^T

f＝[f_x,k f_y,k f_z,k]^T

式中，Δθ為俯仰角偏差；Δγ為橫滾角偏差；f_x,k，f_y,k和f_z,k分別為修正前的右向，前向和上向的加速度計輸出值。

其中，所述f_x,k，f_y,k和f_z,k為靜止情況下第k個計算周期獲得的加速度計輸出值，根據(jù)加速度投影關(guān)系，所述Δθ和Δγ可以計算為：

sinΔθ＝f_y,k/g

tanΔγ＝-f_x,k/f_z

式中，g為重力加速度。

進(jìn)一步的，所述第k個計算周期的第一誤差修正值還用于修正下列信息：

ω_z,k+1＝ω_z,k-ε_b-ε_r

具體的，通過第k個計算周期的第一誤差修正值中的ε_b,ε_r，的數(shù)據(jù)對第k個計算周期的Φ_DR,k，v_eDR,k，v_nDR,k，λ_DR,k，L_DR,k，ω_z,k，f_x,k，f_y,k進(jìn)行修正，得到第k+1個計算周期的Φ_DR,k+1，v_eDR,k+1，v_nDR,k+1，λ_DR,k+1，L_DR,k+1，ω_z,k+1，f_x,k+1，f_y,k+1值，用于第k+1個計算周期的導(dǎo)航定位方法中。

本發(fā)明實施例公開的導(dǎo)航定位方法，當(dāng)導(dǎo)航控制設(shè)備檢測到衛(wèi)星信號失效時，以第k個計算周期的磁航向角為觀測量，通過DR/磁航向組合導(dǎo)航算法模型得到第一狀態(tài)量組合，以第k個計算周期的橫向速度為觀測量，通過DR/車輛運(yùn)動組合導(dǎo)航算法模型得到第二狀態(tài)量組合；根據(jù)第k個計算周期的系統(tǒng)狀態(tài)方程，針對第一狀態(tài)量組合和第二狀態(tài)量組合進(jìn)行閉環(huán)卡爾曼組合濾波得到第一誤差修正值；根據(jù)第一誤差修正值修正第k個計算周期的初始航向角得到目標(biāo)航向角，根據(jù)目標(biāo)航向角和第k個計算周期的DR模型，計算得到目標(biāo)速度和目標(biāo)位置?？梢姡ㄟ^DR/磁航向組合導(dǎo)航算法模型和DR/車輛運(yùn)動組合導(dǎo)航算法模型在衛(wèi)星信號失效時對航向角進(jìn)行修正，進(jìn)而根據(jù)DR模型推算出目標(biāo)速度和目標(biāo)位置信息，實現(xiàn)了在衛(wèi)星信號無效時的車輛定位導(dǎo)航，提高了車載定位的準(zhǔn)確性。

在一個示例中，當(dāng)所述導(dǎo)航控制設(shè)備檢測到衛(wèi)星信號有效時，以第k+1個計算周期的初始速度和初始位置為觀測量，通過所述第k+1個計算周期的DR/衛(wèi)星組合導(dǎo)航算法模型得到所述第k+1個計算周期的第三狀態(tài)量組合，所述第k+1個計算周期的DR/衛(wèi)星組合導(dǎo)航算法模型是根據(jù)所述第k+1個計算周期的所述初始速度和所述初始位置而建立的；

所述導(dǎo)航控制設(shè)備根據(jù)所述第k+1個計算周期的車輛系統(tǒng)狀態(tài)方程，針對所述第k+1個計算周期的所述第一狀態(tài)量組合和所述第k+1個計算周期的所述第三狀態(tài)量組合進(jìn)行所述閉環(huán)卡爾曼組合濾波，得到所述第k+1個計算周期的第二誤差修正值；

所述導(dǎo)航控制設(shè)備根據(jù)所述第k+1個計算周期的所述第二誤差修正值修正所述第k+1個計算周期的初始航向角，得到所述第k+1個計算周期的目標(biāo)航向角，根據(jù)所述第k+1個計算周期的所述目標(biāo)航向角和所述第k+1個計算周期的DR模型，計算得到所述第k+1個計算周期的目標(biāo)速度和目標(biāo)位置。

其中，所述第k+1個計算周期的DR/衛(wèi)星組合導(dǎo)航算法模型是根據(jù)所述第k+1個計算周期的所述初始速度和所述初始位置而建立的，包括：

以第k+1個計算周期的初始經(jīng)度、第k+1個計算周期的初始緯度、第k+1個計算周期的初始東向速度和第k+1個計算周期的初始北向速度作為觀測量，建立第k+1個計算周期的量測方程，包括第三狀態(tài)量組合Z_Sat,k+1、H_Sat,k+1、V_Sat,k+1；

所述第k+1個計算周期的量測方程為：

Z_Sat,k＝H_Sat,kX_k+V_Sat,k

其中，

第k+1個計算周期的第三量測矩陣H_Sat,k+1為：

第k+1個計算周期的第三量測噪聲V_Sat,k+1為：

式中，λ_DR,k+1和L_DR,k+1分別為所述第k+1個計算周期的經(jīng)度和緯度；λ_Sat,k+1、L_Sat,k+1、v_eSat,k+1和v_nSat,k+1分別為所述第k+1個計算周期的衛(wèi)星接收機(jī)定位的經(jīng)度、緯度、東向速度以及北向速度；N_eSat,k+1、N_nSat,k+1、M_eSat,k+1和M_nSat,k+1分別為所述第k+1個計算周期衛(wèi)星接收機(jī)計算出來的定位信息本身含有的經(jīng)度、緯度、東向速度和北向速度誤差。

具體的，根據(jù)所述第k+1個計算周期的車輛系統(tǒng)狀態(tài)方程，針對所述第k+1個計算周期的所述第一狀態(tài)量組合和所述第k+1個計算周期的所述第三狀態(tài)量組合進(jìn)行所述閉環(huán)卡爾曼組合濾波，得到所述第k+1個計算周期的第二誤差修正值包括：

通過如下公式計算得到所述第k+1個計算周期的第二誤差修正值

式中，

其中：

具體的，根據(jù)所述第k+1個計算周期的所述第二誤差修正值修正所述第k+1個計算周期的初始航向角，得到所述第k+1個計算周期的目標(biāo)航向角，根據(jù)所述第k+1個計算周期的所述目標(biāo)航向角和所述第k+1個計算周期的DR模型，計算得到所述第k+1個計算周期的目標(biāo)速度和目標(biāo)位置如S103所描述的方法。

由上可見，本示例中提供的導(dǎo)航定位方法，實現(xiàn)了在衛(wèi)星信號無效時的車輛定位導(dǎo)航，提高了車載定位的準(zhǔn)確性之外，設(shè)備在城市峽谷、高架遮擋等衛(wèi)星信號受到干擾但非失效環(huán)境下，利用第二誤差修正值對初始航向角進(jìn)行修正，提高了衛(wèi)星信號受到干擾情況下的導(dǎo)航定位的精度性。

上述主要從設(shè)備執(zhí)行過程的角度對本發(fā)明實施例的方案進(jìn)行了介紹?？梢岳斫獾氖?，設(shè)備為了實現(xiàn)上述功能，其包含了執(zhí)行各個功能相應(yīng)的硬件結(jié)構(gòu)和/或軟件模塊。本領(lǐng)域技術(shù)人員應(yīng)該很容易意識到，結(jié)合本文中所公開的實施例描述的各示例的單元及算法步驟，本發(fā)明能夠以硬件或硬件和計算機(jī)軟件的結(jié)合形式來實現(xiàn)。某個功能究竟以硬件還是計算機(jī)軟件驅(qū)動硬件的方式來執(zhí)行，取決于技術(shù)方案的特定應(yīng)用和設(shè)計約束條件。專業(yè)技術(shù)人員可以對每個特定的應(yīng)用使用不同方法來實現(xiàn)所描述的功能，但是這種實現(xiàn)不應(yīng)認(rèn)為超出本發(fā)明的范圍。

本發(fā)明實施例可以根據(jù)上述方法示例對設(shè)備進(jìn)行功能單元的劃分，例如，可以對應(yīng)各個功能劃分各個功能單元，也可以將兩個或兩個以上的功能集成在一個處理單元中。上述集成的單元既可以采用硬件的形式實現(xiàn)，也可以采用軟件功能單元的形式實現(xiàn)。需要說明的是，本發(fā)明實施例中對單元的劃分是示意性的，僅僅為一種邏輯功能劃分，實際實現(xiàn)時可以有另外的劃分方式。

在采用集成的單元的情況下，圖2A示出了上述實施例中所涉及的設(shè)備的一種可能的導(dǎo)航控制設(shè)備的結(jié)構(gòu)示意圖。導(dǎo)航控制設(shè)備200包括：處理單元202和通信單元203。處理單元202用于對導(dǎo)航控制設(shè)備的動作進(jìn)行控制管理，例如，處理單元202用于支持設(shè)備執(zhí)行圖1中的步驟S101至104和/或用于本文所描述的技術(shù)的其它過程。通信單元203用于支持移動終端與其他設(shè)備如導(dǎo)航衛(wèi)星之間的通信。導(dǎo)航控制設(shè)備還可以包括存儲單元201，用于存儲設(shè)備的程序代碼和數(shù)據(jù)。

其中，處理單元202可以是處理器或控制器，例如可以是中央處理器(Central Processing Unit，CPU)，通用處理器，數(shù)字信號處理器(Digital Signal Processor，DSP)，專用集成電路(Application-Specific Integrated Circuit，ASIC)，現(xiàn)場可編程門陣列(Field Programmable Gate Array，F(xiàn)PGA)或者其他可編程邏輯器件、晶體管邏輯器件、硬件部件或者其任意組合。其可以實現(xiàn)或執(zhí)行結(jié)合本發(fā)明公開內(nèi)容所描述的各種示例性的邏輯方框，模塊和電路。所述處理器也可以是實現(xiàn)計算功能的組合，例如包含一個或多個微處理器組合，DSP和微處理器的組合等等。通信單元203可以是通信接口、收發(fā)器、收發(fā)電路等，其中，通信接口是統(tǒng)稱，可以包括一個或多個接口。存儲單元201可以是存儲器。

當(dāng)處理單元202為處理器，通信單元203為通信接口，存儲單元401為存儲器時，本發(fā)明實施例所涉及的設(shè)備可以為圖2B所示的導(dǎo)航控制設(shè)備。

參閱圖2B所示，該導(dǎo)航控制設(shè)備210包括：處理器212、通信接口213、存儲器211?？蛇x的，導(dǎo)航控制設(shè)備210還可以包括總線214。其中，通信接口213、處理器212以及存儲器211可以通過總線214相互連接；總線214可以是外設(shè)部件互連標(biāo)準(zhǔn)(Peripheral Component Interconnect，簡稱PCI)總線或擴(kuò)展工業(yè)標(biāo)準(zhǔn)結(jié)構(gòu)(Extended Industry Standard Architecture，簡稱EISA)總線等。所述總線214可以分為地址總線、數(shù)據(jù)總線、控制總線等。為便于表示，圖2B中僅用一條粗線表示，但并不表示僅有一根總線或一種類型的總線。

上述圖2A或圖2B所示的導(dǎo)航控制設(shè)備也可以理解為一種用于導(dǎo)航控制設(shè)備的裝置，本發(fā)明實施例不限定。

本發(fā)明實施例還公開一種計算機(jī)存儲介質(zhì)，其中，該計算機(jī)存儲介質(zhì)可存儲有程序，該程序執(zhí)行時包括上述方法實施例中記載的任何一種導(dǎo)航定位方法的部分或全部步驟。

需要說明的是，對于前述的各方法實施例，為了簡單描述，故將其都表述為一系列的動作組合，但是本領(lǐng)域技術(shù)人員應(yīng)該知悉，本發(fā)明并不受所描述的動作順序的限制，因為依據(jù)本發(fā)明，某些步驟可以采用其他順序或者同時進(jìn)行。其次，本領(lǐng)域技術(shù)人員也應(yīng)該知悉，說明書中所描述的實施例均屬于優(yōu)選實施例，所涉及的動作和模塊并不一定是本發(fā)明所必須的。

在上述實施例中，對各個實施例的描述都各有側(cè)重，某個實施例中沒有詳述的部分，可以參見其他實施例的相關(guān)描述。

在本發(fā)明所公開的幾個實施例中，應(yīng)該理解到，所揭露的裝置，可通過其它的方式實現(xiàn)。例如，以上所描述的裝置實施例僅僅是示意性的，例如所述單元的劃分，僅僅為一種邏輯功能劃分，實際實現(xiàn)時可以有另外的劃分方式，例如多個單元或組件可以結(jié)合或者可以集成到另一個系統(tǒng)，或一些特征可以忽略，或不執(zhí)行。另一點(diǎn)，所顯示或討論的相互之間的耦合或直接耦合或通信連接可以是通過一些接口，裝置或單元的間接耦合或通信連接，可以是電性或其它的形式。

所述作為分離部件說明的單元可以是或者也可以不是物理上分開的，作為單元顯示的部件可以是或者也可以不是物理單元，即可以位于一個地方，或者也可以分布到多個網(wǎng)絡(luò)單元上?？梢愿鶕?jù)實際的需要選擇其中的部分或者全部單元來實現(xiàn)本實施例方案的目的。

另外，在本發(fā)明各個實施例中的各功能單元可以集成在一個處理單元中，也可以是各個單元單獨(dú)物理存在，也可以兩個或兩個以上單元集成在一個單元中。上述集成的單元既可以采用硬件的形式實現(xiàn)，也可以采用軟件功能單元的形式實現(xiàn)。

所述集成的單元如果以軟件功能單元的形式實現(xiàn)并作為獨(dú)立的產(chǎn)品銷售或使用時，可以存儲在一個計算機(jī)可讀取存儲器中?；谶@樣的理解，本發(fā)明的技術(shù)方案本質(zhì)上或者說對現(xiàn)有技術(shù)做出貢獻(xiàn)的部分或者該技術(shù)方案的全部或部分可以以軟件產(chǎn)品的形式體現(xiàn)出來，該計算機(jī)軟件產(chǎn)品存儲在一個存儲器中，包括若干指令用以使得一臺計算機(jī)設(shè)備(可為個人計算機(jī)、服務(wù)器或者網(wǎng)絡(luò)設(shè)備等)執(zhí)行本發(fā)明各個實施例所述方法的全部或部分步驟。而前述的存儲器包括：U盤、只讀存儲器(ROM，Read-Only Memory)、隨機(jī)存取存儲器(RAM，Random Access Memory)、移動硬盤、磁碟或者光盤等各種可以存儲程序代碼的介質(zhì)。

以上對本發(fā)明實施例進(jìn)行了詳細(xì)介紹，本文中應(yīng)用了具體個例對本發(fā)明的原理及實施方式進(jìn)行了闡述，以上實施例的說明只是用于幫助理解本發(fā)明的方法及其核心思想；同時，對于本領(lǐng)域的一般技術(shù)人員，依據(jù)本發(fā)明的思想，在具體實施方式及應(yīng)用范圍上均會有改變之處，綜上所述，本說明書內(nèi)容不應(yīng)理解為對本發(fā)明的限制。