本發(fā)明屬于車輛組合導(dǎo)航技術(shù)領(lǐng)域，特別涉及一種基于ckf濾波的車輛動(dòng)力學(xué)模型輔助慣導(dǎo)組合導(dǎo)航方法。

背景技術(shù)：

隨著微慣性器件的迅速發(fā)展，慣性導(dǎo)航系統(tǒng)(inertialnavigationsystem，ins)可滿足車輛導(dǎo)航系統(tǒng)低成本、微型化的需求。但ins會(huì)產(chǎn)生隨時(shí)間積累的誤差，無(wú)法長(zhǎng)時(shí)間保持導(dǎo)航精度，故需要引入輔助傳感器測(cè)量的信息與ins組成組合導(dǎo)航系統(tǒng)進(jìn)行工作。其中最常見的是慣導(dǎo)/衛(wèi)星組合導(dǎo)航系統(tǒng)。但在實(shí)際應(yīng)用中，衛(wèi)星導(dǎo)航系統(tǒng)(globalnavigationsatellitesystem,gnss)自主性差，易受干擾，gnss信號(hào)容易受到阻隔，在gnss失效階段導(dǎo)航系統(tǒng)仍然相當(dāng)于ins單獨(dú)工作。這種情況下，如果保持長(zhǎng)時(shí)間的導(dǎo)航精度，需要其它狀態(tài)信息輔助慣導(dǎo)系統(tǒng)。而車輛動(dòng)力學(xué)模型(vehicledynamicmodel,vdm)對(duì)載體狀態(tài)估計(jì)，不需依賴外部條件，具有較強(qiáng)的自主性和適用性，可以利用vdm有效地輔助ins進(jìn)行導(dǎo)航。

目前，基于vdm的車輛狀態(tài)估計(jì)主要有兩類，一類是以速度為已知量，利用非線性vdm對(duì)側(cè)偏角等狀態(tài)進(jìn)行估計(jì)，該類方法對(duì)速度的精度要求較高。第二類是以輪胎力為已知量，基于擴(kuò)展卡爾曼濾波器、一階斯梯林插值濾波器、遞推最小二乘法濾波器，估計(jì)車速、橫擺角速度、質(zhì)心側(cè)偏角等狀態(tài)。但第二類方法中將非線性模型簡(jiǎn)化為線性時(shí)會(huì)造成一定的誤差，降低測(cè)量精度，并且vdm估計(jì)的狀態(tài)信息沒有用于輔助其他導(dǎo)航系統(tǒng)，具有一定的局限性。

基于以上分析，現(xiàn)有的慣性導(dǎo)航技術(shù)精度較低，有待改進(jìn)，本案由此產(chǎn)生。

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

本發(fā)明的目的，在于提供一種基于ckf濾波的車輛動(dòng)力學(xué)模型輔助慣導(dǎo)組合導(dǎo)航方法，其針對(duì)慣導(dǎo)誤差隨時(shí)間積累，無(wú)法長(zhǎng)時(shí)間保持導(dǎo)航精度的問題，可提高車輛導(dǎo)航系統(tǒng)的精確度和可靠性。

為了達(dá)成上述目的，本發(fā)明的解決方案是：

一種基于ckf濾波的車輛動(dòng)力學(xué)模型輔助慣導(dǎo)組合導(dǎo)航方法，包括如下步驟：

步驟1，根據(jù)微慣性器件輸出的角增量和比力，利用慣導(dǎo)數(shù)值更新算法解算車輛的姿態(tài)、速度和位置；

步驟2，建立三自由度車輛動(dòng)力學(xué)模型，以方向盤轉(zhuǎn)角和縱向力為控制輸入量，利用四階龍格庫(kù)塔法實(shí)時(shí)解算載體速度；

步驟3，以慣導(dǎo)誤差方程為狀態(tài)方程，動(dòng)力學(xué)模型和慣導(dǎo)解算的速度差為觀測(cè)量，設(shè)計(jì)ckf濾波器對(duì)組合導(dǎo)航系統(tǒng)進(jìn)行狀態(tài)估計(jì)；

步驟4，ckf估計(jì)得到的位置、速度和姿態(tài)誤差對(duì)捷聯(lián)慣導(dǎo)解算結(jié)果進(jìn)行輸出校正，陀螺和加表誤差對(duì)慣導(dǎo)進(jìn)行反饋校正。

上述步驟1的詳細(xì)內(nèi)容是：利用六軸加速度計(jì)和陀螺儀測(cè)量載體的角增量和比力，解算出載體當(dāng)前時(shí)刻的導(dǎo)航參數(shù)，導(dǎo)航坐標(biāo)系n采用東北天地理坐標(biāo)系，載體坐標(biāo)系b采用右前上坐標(biāo)系；慣導(dǎo)數(shù)值更新算法包含姿態(tài)微分方程、比力微分方程和位置微分方程。

上述姿態(tài)微分方程的表達(dá)式為：

其中，q為姿態(tài)四元數(shù)，為載體系相對(duì)于導(dǎo)航系的角速度，為陀螺輸出的載體系相對(duì)于慣性系的角速度，為地球自轉(zhuǎn)引起的導(dǎo)航系旋轉(zhuǎn)角速度，為載體運(yùn)動(dòng)產(chǎn)生的位移角速度，為姿態(tài)旋轉(zhuǎn)矩陣。

上述比力微分方程的表達(dá)式為：

其中，vi＝[vievinviu]^t為慣導(dǎo)解算的東向、北向和天向速度，為加速度計(jì)測(cè)量的比力，為由載體運(yùn)動(dòng)和地球自轉(zhuǎn)引起的哥氏加速度，為由載體運(yùn)動(dòng)引起的對(duì)地向心加速度，gⁿ為重力加速度，為由載體系轉(zhuǎn)換為導(dǎo)航系的轉(zhuǎn)換矩陣。

上述位置微分方程的表達(dá)式為：

其中，rm和rn分別為子午圈和卯酉圈主曲率半徑，l、λ和h分別為慣導(dǎo)解算的緯度、經(jīng)度和高度，vie、vin和viu分別為慣導(dǎo)解算的東向速度、北向速度和天向速度。

上述步驟2的詳細(xì)內(nèi)容是：建立橫向、縱向、橫擺三自由度非線性車輛動(dòng)力學(xué)模型，忽略左右輪差異，模型等效簡(jiǎn)化為前、后車輪分別集中在車輛前、后軸中點(diǎn)而構(gòu)成的一個(gè)自行車模型，xnoyn為導(dǎo)航坐標(biāo)系，xn軸向東，yn軸向北；xboyb為固定在車輛質(zhì)心上的載體坐標(biāo)系，xb軸與載體橫軸重合，向右為正，yb軸與載體縱軸重合，向前為正；

根據(jù)牛頓力學(xué)，車輛的動(dòng)力學(xué)模型為：

其中，和分別為車輛的橫向速度、縱向速度和橫擺角速度；m和iz分別為車輛的質(zhì)量、繞豎軸的轉(zhuǎn)動(dòng)慣量；a、b分別為汽車前輪和后輪輪軸中心到質(zhì)心的距離；δf為前輪轉(zhuǎn)向角，由方向盤轉(zhuǎn)角傳感器測(cè)得的方向盤轉(zhuǎn)角δ除以從方向盤到前輪的轉(zhuǎn)向傳動(dòng)比來(lái)確定；cd為空氣阻力系數(shù)；af為車輛前向面積；ρa(bǔ)為空氣密度；ftf、ftr分別為作用在單個(gè)前輪和后輪上的縱向力；fsf、fsr分別為作用在單個(gè)前輪和后輪上的橫向力；對(duì)于行駛在一般道路交通環(huán)境下的車輛，將作用在各輪上的側(cè)向力表示為：

fsf＝cαfαf

fsr＝cαrαr

式中，cαf、cαr分別為前、后輪胎的側(cè)偏剛度；αf、αr分別為前、后輪胎的側(cè)偏角，近似表示為：

將側(cè)向力表達(dá)式、側(cè)偏角表達(dá)式代入動(dòng)力學(xué)模型表達(dá)式，并考慮到δf通常是小角度，經(jīng)整理后得：

利用方向盤轉(zhuǎn)角傳感器和車輪力傳感器信息得到車輛動(dòng)力學(xué)模型的控制輸入向量u＝[δfftfftr]^t，通過(guò)四階龍格庫(kù)塔法求解上式得載體坐標(biāo)系下車輛的橫向速度縱向速度和橫擺角速度根據(jù)慣導(dǎo)解算的姿態(tài)矩陣，得到導(dǎo)航坐標(biāo)系下由動(dòng)力學(xué)模型計(jì)算的速度：

上述步驟3中，ckf濾波器的誤差狀態(tài)變量包括3個(gè)姿態(tài)誤差、2個(gè)速度誤差、2個(gè)位置誤差、3個(gè)陀螺常值漂移、2個(gè)加速度計(jì)常值零偏，即

ckf濾波器的狀態(tài)方程為

式中，a為相應(yīng)的狀態(tài)轉(zhuǎn)移矩陣，由ins誤差方程構(gòu)成；w為過(guò)程噪聲；

將慣導(dǎo)和車輛動(dòng)力學(xué)模型解算的東向速度之差、北向速度之差作為ckf濾波器的觀測(cè)量，即觀測(cè)向量為z＝[vie-vmevin-vmn]^t，則系統(tǒng)的量測(cè)方程為：

z＝hx+v

式中，h＝[02×3i2×202×7]為系統(tǒng)量測(cè)矩陣，v為量測(cè)噪聲；

對(duì)前述系統(tǒng)狀態(tài)方程和量測(cè)方程進(jìn)行離散化，得到k時(shí)刻的離散化系統(tǒng)狀態(tài)模型為：

xk＝fxk-1+wk-1

zk＝hxk+vk

式中，f＝exp(a×δt)為離散化的系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，δt為采樣時(shí)間；wk-1為均值為0，方差為qk-1的高斯白噪聲，vk為均值為0，方差為rk的高斯白噪聲且wk-1和vk相互獨(dú)立。

上述步驟3中，ckf濾波器的狀態(tài)估計(jì)包括時(shí)間更新和量測(cè)更新兩步：

(1)時(shí)間更新

構(gòu)造容積點(diǎn)

式中，pk-1k-1、為k-1時(shí)刻協(xié)方差和狀態(tài)估計(jì)值；m＝2n，n是狀態(tài)向量x維數(shù)；[1]i是點(diǎn)集[1]的第i列；

計(jì)算經(jīng)狀態(tài)方程傳播容積點(diǎn)：

一步預(yù)測(cè)狀態(tài)和誤差協(xié)方差陣：

(2)量測(cè)更新

構(gòu)造容積點(diǎn)：

計(jì)算經(jīng)量測(cè)方程傳播容積點(diǎn)：

zi,k/k-1＝hxi,k/k-1

計(jì)算觀測(cè)量預(yù)測(cè)值：

估計(jì)新息自協(xié)方差矩陣：

估計(jì)互協(xié)方差矩陣：

ckf濾波器增益矩陣：

估計(jì)k時(shí)刻的狀態(tài)量：

估計(jì)k時(shí)刻狀態(tài)誤差協(xié)方差陣：

上述步驟4的詳細(xì)內(nèi)容是；對(duì)微慣性器件輸出的角增量比力進(jìn)行反饋校正得：

對(duì)ins解算的緯度l、經(jīng)度λ進(jìn)行輸出校正得組合導(dǎo)航位置：

lout＝l-δl

λout＝λ-δλ

對(duì)慣導(dǎo)解算的姿態(tài)進(jìn)行輸出校正得組合導(dǎo)航姿態(tài)：

θout＝θ-δθ

γout＝γ-δγ

對(duì)慣導(dǎo)解算的東向速度vie、北向速度vin進(jìn)行輸出校正得組合導(dǎo)航速度：

veout＝vie-δvx

vnout＝vin-δvy

其中，θ表示俯仰角，γ表示橫滾角，表示航向角。

采用上述方案后，本發(fā)明提出基于ckf濾波的vdm輔助ins組合導(dǎo)航方法，建立車輛三自由度非線性動(dòng)力學(xué)模型，利用四階龍格庫(kù)塔法實(shí)時(shí)解算速度信息。以ins誤差方程為狀態(tài)方程，vdm與ins解算的速度差為觀測(cè)量，設(shè)計(jì)了ckf，并用估計(jì)的狀態(tài)誤差對(duì)ins進(jìn)行校正。本發(fā)明中所采用的ckf算法無(wú)需對(duì)狀態(tài)模型線性化，直接通過(guò)非線性系統(tǒng)方程對(duì)容積點(diǎn)進(jìn)行傳播，估計(jì)精度高。vdm輔助ins的組合導(dǎo)航方法可有效抑制ins導(dǎo)航誤差發(fā)散，改善車輛導(dǎo)航系統(tǒng)的可靠性。本發(fā)明能夠有效抑制純慣導(dǎo)誤差隨時(shí)間累積問題，提高車輛的導(dǎo)航精度和可靠性，具有實(shí)時(shí)性好、自主性強(qiáng)等特點(diǎn)。

附圖說(shuō)明

圖1是本發(fā)明的原理圖；

圖2是車輛動(dòng)力學(xué)模型示意圖。

具體實(shí)施方式

以下將結(jié)合附圖，對(duì)本發(fā)明的技術(shù)方案進(jìn)行詳細(xì)說(shuō)明。

如圖1所示，本發(fā)明提供一種基于ckf濾波的車輛動(dòng)力學(xué)模型輔助慣導(dǎo)組合導(dǎo)航方法，包括如下步驟：

步驟1，根據(jù)微慣性器件輸出的角增量和比力，利用慣導(dǎo)(ins)數(shù)值更新算法解算車輛的姿態(tài)、速度和位置；

其中，ins數(shù)值更新算法利用六軸加速度計(jì)和陀螺儀測(cè)量載體的比力和角增量，根據(jù)已知的初始條件，解算出載體當(dāng)前時(shí)刻的導(dǎo)航參數(shù)。導(dǎo)航坐標(biāo)系n采用東北天地理坐標(biāo)系，載體坐標(biāo)系b采用右前上坐標(biāo)系。ins基本方程包含姿態(tài)微分方程、比力微分方程和位置微分方程。其中，姿態(tài)四元數(shù)微分表達(dá)式為

其中，q為姿態(tài)四元數(shù)，為載體系相對(duì)于導(dǎo)航系的角速度，為陀螺輸出的載體系相對(duì)于慣性系的角速度，為地球自轉(zhuǎn)引起的導(dǎo)航系旋轉(zhuǎn)角速度，為載體運(yùn)動(dòng)產(chǎn)生的位移角速度，為姿態(tài)旋轉(zhuǎn)矩陣。

比力微分方程為

其中，vi＝[vievinviu]^t為慣導(dǎo)解算的東向、北向和天向速度，為加速度計(jì)測(cè)量的比力，為由載體運(yùn)動(dòng)和地球自轉(zhuǎn)引起的哥氏加速度，為由載體運(yùn)動(dòng)引起的對(duì)地向心加速度，gⁿ為重力加速度，為由載體系轉(zhuǎn)換為導(dǎo)航系的轉(zhuǎn)換矩陣。

ins的位置(緯度、經(jīng)度和高度)微分方程式如下

其中，rm和rn分別為子午圈和卯酉圈主曲率半徑，l、λ和h分別為慣導(dǎo)解算的緯度、經(jīng)度和高度，vie、vin和viu分別為慣導(dǎo)解算的東向速度、北向速度和天向速度。

步驟2，建立三自由度車輛動(dòng)力學(xué)模型，以方向盤轉(zhuǎn)角和縱向力為控制輸入量，利用四階龍格庫(kù)塔法實(shí)時(shí)解算載體速度；

vdm利用載體自身的輸入量，解算出載體的部分運(yùn)動(dòng)參數(shù)，如速度、角速率等。vdm解算結(jié)果輔助校正ins的輸出，從而構(gòu)成ins/vdm的組合導(dǎo)航系統(tǒng)，提高車輛導(dǎo)航精度。：建立橫向、縱向、橫擺三自由度非線性車輛動(dòng)力學(xué)模型，忽略左右輪差異，模型等效簡(jiǎn)化為前、后車輪分別集中在車輛前、后軸中點(diǎn)而構(gòu)成的一個(gè)自行車模型，xnoyn為導(dǎo)航坐標(biāo)系，xn軸向東，yn軸向北；xboyb為固定在車輛質(zhì)心上的載體坐標(biāo)系，xb軸與載體橫軸重合，向右為正，yb軸與載體縱軸重合，向前為正。

據(jù)牛頓力學(xué)，車輛的動(dòng)力學(xué)模型為：

其中，和分別為車輛的橫向速度、縱向速度和橫擺角速度；m和iz分別為車輛的質(zhì)量、繞豎軸的轉(zhuǎn)動(dòng)慣量；a、b分別為汽車前輪和后輪輪軸中心到質(zhì)心的距離；δf為前輪轉(zhuǎn)向角，由方向盤轉(zhuǎn)角傳感器測(cè)得的方向盤轉(zhuǎn)角δ除以從方向盤到前輪的轉(zhuǎn)向傳動(dòng)比來(lái)確定；cd為空氣阻力系數(shù)；af為車輛前向面積；ρa(bǔ)為空氣密度；ftf、ftr分別為作用在單個(gè)前輪和后輪上的縱向力；fsf、fsr分別為作用在單個(gè)前輪和后輪上的橫向力；對(duì)于行駛在一般道路交通環(huán)境下的車輛，將作用在各輪上的側(cè)向力表示為：

式中，cαf、cαr分別為前、后輪胎的側(cè)偏剛度；αf、αr分別為前、后輪胎的側(cè)偏角，近似表示為：

將式(5)、(6)代入式(4)，并考慮到，并考慮到δf通常是小角度，經(jīng)整理后得：

利用方向盤轉(zhuǎn)角傳感器和車輪力傳感器信息得到車輛動(dòng)力學(xué)模型的控制輸入向量u＝[δfftfftr]^t，通過(guò)四階龍格庫(kù)塔法求解上式得載體坐標(biāo)系下車輛的橫向速度縱向速度和橫擺角速度根據(jù)慣導(dǎo)解算的姿態(tài)矩陣，得到導(dǎo)航坐標(biāo)系下由動(dòng)力學(xué)模型計(jì)算的速度：

步驟3，以慣導(dǎo)誤差方程為狀態(tài)方程，動(dòng)力學(xué)模型和慣導(dǎo)解算的速度差為觀測(cè)量，設(shè)計(jì)ckf濾波器對(duì)組合導(dǎo)航系統(tǒng)進(jìn)行狀態(tài)估計(jì)；

ckf濾波器的誤差狀態(tài)變量包括3個(gè)姿態(tài)誤差、2個(gè)速度誤差、2個(gè)位置誤差、3個(gè)陀螺常值漂移、2個(gè)加速度計(jì)常值零偏，即

即

ckf濾波器的狀態(tài)方程為

式中，a為相應(yīng)的狀態(tài)轉(zhuǎn)移矩陣，由ins誤差方程構(gòu)成；w為過(guò)程噪聲；

將慣導(dǎo)和車輛動(dòng)力學(xué)模型解算的東向速度之差、北向速度之差作為ckf濾波器的觀測(cè)量，即觀測(cè)向量為z＝[vie-vmevin-vmn]^t，則系統(tǒng)的量測(cè)方程為：

z＝hx+v

式中，h＝[02×3i2×202×7]為系統(tǒng)量測(cè)矩陣，v為量測(cè)噪聲；

對(duì)前述系統(tǒng)狀態(tài)方程和量測(cè)方程進(jìn)行離散化，得到k時(shí)刻的離散化系統(tǒng)狀態(tài)模型為：

式中，f＝exp(a×δt)為離散化的系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，δt為采樣時(shí)間；wk-1為均值為0，方差為qk-1的高斯白噪聲，vk為均值為0，方差為rk的高斯白噪聲且wk-1和vk相互獨(dú)立。

ckf濾波器的狀態(tài)估計(jì)包括時(shí)間更新和量測(cè)更新兩步：

(1)時(shí)間更新

構(gòu)造容積點(diǎn)

式中，pk-1/k-1、為k-1時(shí)刻協(xié)方差和狀態(tài)估計(jì)值；m＝2n，n是狀態(tài)向量x維數(shù)；[1]i是點(diǎn)集[1]的第i列；

計(jì)算經(jīng)狀態(tài)方程傳播容積點(diǎn)：

一步預(yù)測(cè)狀態(tài)和誤差協(xié)方差陣：

(2)量測(cè)更新

構(gòu)造容積點(diǎn)：

計(jì)算經(jīng)量測(cè)方程傳播容積點(diǎn)：

zi,k/k-1＝hxi,k/k-1

計(jì)算觀測(cè)量預(yù)測(cè)值：

估計(jì)新息自協(xié)方差矩陣：

估計(jì)互協(xié)方差矩陣：

ckf濾波器增益矩陣：

估計(jì)k時(shí)刻的狀態(tài)量：

估計(jì)k時(shí)刻狀態(tài)誤差協(xié)方差陣：

步驟4，ckf估計(jì)得到的位置、速度和姿態(tài)誤差對(duì)捷聯(lián)慣導(dǎo)解算結(jié)果進(jìn)行輸出校正，陀螺和加表誤差對(duì)慣導(dǎo)進(jìn)行反饋校正；

對(duì)微慣性器件輸出的角增量比力進(jìn)行反饋校正得：

對(duì)ins解算的緯度l、經(jīng)度λ進(jìn)行輸出校正得組合導(dǎo)航位置：

lout＝l-δl

λout＝λ-δλ

對(duì)慣導(dǎo)解算的姿態(tài)進(jìn)行輸出校正得組合導(dǎo)航姿態(tài)：

θout＝θ-δθ

γout＝γ-δγ

對(duì)慣導(dǎo)解算的東向速度vie、北向速度vin進(jìn)行輸出校正得組合導(dǎo)航速度：

veout＝vie-δvx

vnout＝vin-δvy

其中，θ表示俯仰角，γ表示橫滾角，表示航向角。

以上實(shí)施例僅為說(shuō)明本發(fā)明的技術(shù)思想，不能以此限定本發(fā)明的保護(hù)范圍，凡是按照本發(fā)明提出的技術(shù)思想，在技術(shù)方案基礎(chǔ)上所做的任何改動(dòng)，均落入本發(fā)明保護(hù)范圍之內(nèi)。