張?jiān)滦?，王立輝，湯新華

（東南大學(xué) 儀器科學(xué)與工程學(xué)院微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096）

車輛動(dòng)力學(xué)模型輔助的慣性導(dǎo)航系統(tǒng)

張?jiān)滦拢趿⑤x，湯新華

（東南大學(xué) 儀器科學(xué)與工程學(xué)院微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096）

為提高車輛導(dǎo)航系統(tǒng)的精確度和可靠性，提出一種車輛動(dòng)力學(xué)模型輔助慣性導(dǎo)航系統(tǒng)的方法。建立車輛非線性動(dòng)力學(xué)模型，利用四階龍格庫(kù)塔法實(shí)時(shí)解算速度信息。以慣導(dǎo)誤差方程為狀態(tài)方程，動(dòng)力學(xué)模型與慣性導(dǎo)航解算的速度差為觀測(cè)量，設(shè)計(jì)了容積卡爾曼濾波器，并用估計(jì)的狀態(tài)誤差對(duì)慣導(dǎo)進(jìn)行校正。仿真結(jié)果表明，所提出的利用車輛動(dòng)力學(xué)模型輔助慣導(dǎo)的方法能有效抑制慣導(dǎo)誤差的發(fā)散，位置精度和速度精度比純慣導(dǎo)系統(tǒng)提高了一個(gè)數(shù)量級(jí)，航向角精度提高了73%。

慣性導(dǎo)航系統(tǒng)；車輛動(dòng)力學(xué)模型；容積卡爾曼濾波；組合導(dǎo)航

慣性導(dǎo)航系統(tǒng)（Inertial navigation system, INS）具有信息量豐富、自主性等優(yōu)勢(shì)，但I(xiàn)NS導(dǎo)航誤差隨時(shí)間積累，無(wú)法長(zhǎng)時(shí)間單獨(dú)定位[1]。衛(wèi)星導(dǎo)航系統(tǒng)用于輔助 INS應(yīng)用最廣，但自主性差，在其失效階段，組合導(dǎo)航系統(tǒng)只能依賴INS工作，無(wú)法保證車輛導(dǎo)航系統(tǒng)長(zhǎng)時(shí)間工作的精度[2-3]。而車輛動(dòng)力學(xué)模型（Vehicle dynamic model, VDM）對(duì)載體狀態(tài)估計(jì)，不需依賴外部條件，具有較強(qiáng)的自主性和適用性[4]，可以利用VDM有效地輔助INS進(jìn)行導(dǎo)航。

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

針對(duì)第二類方法的不足，基于容積卡爾曼濾波器（cubature Kalman filter, 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)行反饋和輸出校正。該方法中所采用的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)的可靠性。

1 VDM輔助INS的組合導(dǎo)航系統(tǒng)模型

基于CKF的VDM輔助INS的組合導(dǎo)航系統(tǒng)如圖1所示，INS根據(jù)微慣性器件輸出的角增量和比力，利用姿態(tài)更新、速度更新和位置更新算法解算車輛的姿態(tài)、速度和位置；VDM 以方向盤(pán)轉(zhuǎn)角和縱向力為輸入量，利用四階龍格庫(kù)塔法實(shí)時(shí)解算載體坐標(biāo)系的速度，根據(jù)INS得到的姿態(tài)矩陣轉(zhuǎn)換為導(dǎo)航坐標(biāo)系下的速度；設(shè)計(jì)了CKF，以INS和VDM測(cè)量的速度之差作為觀測(cè)量，INS的誤差方程為狀態(tài)方程，估計(jì)INS狀態(tài)誤差，并用估計(jì)狀態(tài)誤差對(duì)INS進(jìn)行校正。

圖1 VDM輔助INS原理圖Fig.1 Schematic diagram of VDM aided INS

1.1 INS數(shù)值更新算法

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)生的位移角速度；為導(dǎo)航系轉(zhuǎn)換到載體系的姿態(tài)旋轉(zhuǎn)矩陣。

比力微分方程為

INS的位置（緯度L、經(jīng)度λ、高度h）微分方程式如下：

對(duì)于在路面上行駛的車輛，可以假設(shè)車輛不離開(kāi)地面，則車輛在載體坐標(biāo)系下的向上速度等于0，得到其非完整約束條件為

1.2 車輛動(dòng)力學(xué)模型

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è)自行車模型，如圖2所示。為導(dǎo)航坐標(biāo)系，軸向東，軸向北；為固定在車輛質(zhì)心上的載體坐標(biāo)系，軸與載體橫軸重合，向右為正，軸與載體縱軸重合，向前為正。

圖2 車輛動(dòng)力學(xué)模型示意圖Fig.2 Schematic diagram of vehicle dynamic model

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

車輛大部分行駛情況下前輪轉(zhuǎn)向角小于 10°，在極限情況下前輪轉(zhuǎn)向角在30°到40°之間，為應(yīng)用方便通?？杉俣ㄇ拜嗈D(zhuǎn)向角δf為小角度。將式(6)(7)代入式(5)，并考慮到δf是小角度（即且忽略二階及以上的高階微量），經(jīng)整理后可得：

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

2 VDM輔助INS濾波器設(shè)計(jì)

2.1 狀態(tài)方程和量測(cè)方程

采用組合導(dǎo)航間接濾波法，綜合考慮系統(tǒng)的精度和實(shí)時(shí)性，誤差狀態(tài)變量包括3個(gè)姿態(tài)誤差、2個(gè)速度誤差、2個(gè)位置誤差、3個(gè)陀螺常值漂移、2個(gè)加速度計(jì)常值零偏，即

卡爾曼濾波器的狀態(tài)方程為

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

將INS和VDM解算的東向速度之差、北向速度之差作為卡爾曼濾波器的觀測(cè)量，即觀測(cè)向量為則系統(tǒng)的量測(cè)方程為

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

2.2 容積卡爾曼濾波器

CKF采用一組等權(quán)值的容積點(diǎn)解決貝葉斯濾波的積分問(wèn)題，CKF算法具有很好的非線性逼近性能、數(shù)值精度以及濾波穩(wěn)定性[11]。其算法步驟如下：

① 時(shí)間更新

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

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

德國(guó)羅斯勒公司是生產(chǎn)和銷售研磨、拋丸設(shè)備的專業(yè)廠商，主要產(chǎn)品有研磨光整系統(tǒng)和拋丸清理系統(tǒng)，包括用于發(fā)動(dòng)機(jī)、起落架以及機(jī)翼等部件的光飾產(chǎn)品和拋丸設(shè)備，適于軸類、盤(pán)類和葉片的應(yīng)力強(qiáng)化與拋丸處理。羅斯勒公司創(chuàng)立于1933年，其表面處理技術(shù)居世界領(lǐng)先地位，目前已在英國(guó)、法國(guó)、西班牙、意大利、比利時(shí)、奧地利、新加坡、巴西、南非和美國(guó)等國(guó)家設(shè)有分公司，并計(jì)劃在中國(guó)投資建廠。

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

② 量測(cè)更新

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

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

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

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

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

6）卡爾曼增益矩陣：

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

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

3 仿真驗(yàn)證與分析

Carsim仿真中設(shè)置汽車的方向盤(pán)轉(zhuǎn)角δ按幅值 60°的正弦規(guī)律變化，汽車的縱向速度在不斷的加速、勻速和減速變化，方向盤(pán)轉(zhuǎn)角和縱向速度隨時(shí)間的變化過(guò)程如圖3所示，仿真時(shí)長(zhǎng)為120 s，采樣時(shí)間為0.01 s。在不考慮外界因素變化的情況下，對(duì)于確定的車輛，參數(shù)都是固定的，這些參數(shù)不會(huì)隨著車輛的行駛而變化。當(dāng)外界因素變化（如車輛的負(fù)載變化引起質(zhì)量m、Iz的變化）時(shí)，若將參數(shù)m、Iz當(dāng)作常量，會(huì)影響測(cè)量精度，此時(shí)可根據(jù)實(shí)際需求，增加測(cè)量m的傳感設(shè)備，通過(guò)四階龍格庫(kù)塔法求解微分方程(8)時(shí)利用實(shí)時(shí)更新的參數(shù)m、Iz，可以保證車輛導(dǎo)航的精確性。仿真實(shí)驗(yàn)車輛是一個(gè)前輪轉(zhuǎn)向的四輪車，不考慮外界因素的變化，其主要參數(shù)如下：

方向盤(pán)傳動(dòng)比為 20。設(shè)定車輛動(dòng)力學(xué)模型中 3個(gè)外輸入傳感器的量測(cè)噪聲為均值是0，方差分別為0.5°、20 N、20 N的高斯白噪聲。初始緯度為32.0586°，經(jīng)度為118.7869°，高度為0 m，初始速度和姿態(tài)為零，陀螺常值漂移為0.01 (°)/s，角度隨機(jī)游走為加速度計(jì)常值漂移為速度隨機(jī)游走為

圖3 Carsim仿真設(shè)置Fig.3 Simulation settings of Carsim

根據(jù)上述設(shè)置，對(duì)INS和INS/VDM組合導(dǎo)航進(jìn)行數(shù)值仿真研究，仿真結(jié)果如圖4～6所示。其中標(biāo)注為“真實(shí)值”的曲線表示真實(shí)結(jié)果，標(biāo)注為“INS”的曲線表示純慣性導(dǎo)航系統(tǒng)解算的結(jié)果，標(biāo)注為“INS/VDM”的曲線表示動(dòng)力學(xué)輔助導(dǎo)航系統(tǒng)修正解算的結(jié)果。圖4中分別是經(jīng)度、緯度和行駛軌跡的對(duì)比圖?？梢钥闯觯\(yùn)行30 s后INS測(cè)量的經(jīng)度偏離真實(shí)值增大，120 s時(shí)間內(nèi)經(jīng)度的最大偏差為0.43′，緯度最大偏差為0.15'。INS/VDM組合導(dǎo)航的運(yùn)行軌跡和真實(shí)軌跡吻合度高，經(jīng)度、緯度的最大偏差分別為0.006′和0.01′。圖5中分別為東向速度和北向速度的導(dǎo)航速度對(duì)比圖，INS測(cè)量的東向速度偏差較大，經(jīng)INS/VDM組合導(dǎo)航改善后，最大東向速度誤差由14.7 m/s降低到0.6 m/s，最大北向速度誤差由8.52 m/s降低到0.84 m/s。從圖6航向角對(duì)比圖可以看出，最大航向角誤差降低了78.4%。VDM輔助INS的組合導(dǎo)航方法在位置、速度和姿態(tài)上均有明顯的改善效果。表1給出了INS精度和INS/VDM組合導(dǎo)航系統(tǒng)精度的對(duì)比。

由表1可以看出，由于采用的慣性器件精度較低，純慣導(dǎo)系統(tǒng)誤差發(fā)散快，INS/VDM 組合導(dǎo)航系統(tǒng)估計(jì)的位置和速度精度比INS提高一個(gè)數(shù)量級(jí)，航向角估計(jì)精度提高了73%。VDM輔助INS對(duì)INS誤差的發(fā)散有很好地抑制效果。

表1 INS和INS/VDM精度對(duì)比Tab.1 Comparison on precisions of INS and INS/VDM

圖4 導(dǎo)航位置對(duì)比Fig.4 Comparison of navigation positions

圖5 導(dǎo)航速度對(duì)比Fig.5 Comparison of navigation velocities

4 結(jié) 論

針對(duì)INS導(dǎo)航誤差隨時(shí)間積累，無(wú)法長(zhǎng)時(shí)間保持導(dǎo)航精度的問(wèn)題，研究了一種基于CKF的VDM輔助INS的組合導(dǎo)航方法。VDM通過(guò)對(duì)車輛的力學(xué)分析建立車輛運(yùn)動(dòng)的微分方程組，采用四階龍格庫(kù)塔法對(duì)速度、角速率進(jìn)行解算，自主性和實(shí)時(shí)性好，無(wú)累積誤差。組合導(dǎo)航狀態(tài)模型的觀測(cè)量是VDM和INS解算的速度差，狀態(tài)方程是 INS的誤差方程，利用 CKF對(duì)狀態(tài)誤差進(jìn)行估計(jì)并修正 INS。仿真結(jié)果表明，INS/VDM 組合導(dǎo)航系統(tǒng)的位置精度和速度精度比INS提高了一個(gè)數(shù)量級(jí)，航向角精度提高了73%，有效地抑制了INS誤差發(fā)散，達(dá)到了提高車輛導(dǎo)航系統(tǒng)精確度和可靠性的目的。

（References）：

[1]李博文, 姚丹亞.低成本車載 MEMS慣導(dǎo)導(dǎo)航定位方法[J].中國(guó)慣性技術(shù)學(xué)報(bào), 2014, 22(6): 719-723.Li B W, Yao D Y.Low-cost MEMS IMU navigation positioning method for land vehicle[J].Journal of Chinese Inertial Technology, 2014, 22(6): 719-723.

[2]Gao N, Zhao L.An integrated land vehicle navigation system based on context awareness[J].GPS Solutions,2016, 20(3): 509-524.

[3]Quinchia A G, Falco G, Falletti E, et al.A comparison between different error modeling of MEMS applied to GPS/INS integrated systems[J].Sensors, 2013, 13(8): 9549.

[4]Skog I, Ndel P.In-car positioning and navigation technologies: a survey[J].IEEE Transactions on Intelligent Transportation Systems, 2009, 10(1): 4-21.

[5]Li X, Song X, Chan C.Reliable vehicle sideslip angle fusion estimation using low-cost sensors[J].Measurement,2014, 51(1): 241-258.

[6]Miao Z, Zhang H, Zhang J.A robust method of vehicle stability accurate measurement using GPS and INS[J].Measurement Science Review, 2015, 15(6): 294-303.

[7]Nam K, Fujimoto H, Hori Y.Lateral stability control of in-wheel-motor-driven electric vehicles based on sideslip angle estimation using lateral tire force sensors[J].IEEE Transactions on Vehicular Technology, 2012, 61(5):1972-1985.

[8]Lian Y F, Zhao Y, Hu L L, et al.Cornering stiffness and sideslip angle estimation based on simplified lateral dynamic models for four-in-wheel-motor-driven electric vehicles with lateral tire force information[J].International Journal of Automotive Technology, 2015, 16(4): 669-683.

[9]Kanghyun N.Application of novel lateral tire force sensors to vehicle parameter estimation of electric vehicles[J].Sensors, 2015, 15(11): 28385-28401.

[10]王麗芬, 楊功流, 單友東, 等.車載定位定向系統(tǒng)誤差在線補(bǔ)償方法[J].中國(guó)慣性技術(shù)學(xué)報(bào), 2015, 23(2): 145-149.Wang L F, Yang G L, Shan Y D, et al.On-line errorcompensation of vehicular position and orientation determination system[J].Journal of Chinese Inertial Technology, 2015, 23(2):145-149.

[11]Arasaratnam I, Haykin S, Hurd T R.Cubature Kalman filtering for continuous-discrete systems: theory and simulations[J].IEEE Transactions on Signal Processing,2010, 58(10): 4977-4993.

Vehicle dynamic model aided inertial navigation system

ZHANG Yue-xin, WANG Li-hui, TANG Xin-hua
(Key laboratory of micro-inertial instrument and advanced navigation technology, Ministry of education,School of instrument science and engineering, Southeast University, Nanjing 210096, China)

To improve the accuracy and reliability of the vehicle navigation system, an inertial navigation system aided by vehicle dynamic model is proposed.A nonlinear dynamic model of the vehicle is established and a fourth-order RungeKutta algorithm is used to calculate the velocity in real time.The error equation of the inertial navigation system is the state equation, and the calculated velocity difference between the vehicle dynamic model and inertial navigation is the observation vector.Then a cubature Kalman filter is designed,and the estimated state error is used to compensate the inertial navigation system.Simulation results show that the proposed method can effectively suppress the divergence of the inertial navigation error.The accuracies of the estimated position and velocity are both one order higher than those of the pure inertial navigation system, and the accuracy of the estimated yaw angle is improved by about 73%.

inertial navigation system; vehicle dynamic model; cubature Kalman filter; integrated navigation system

V249.32

A

1005-6734(2017)05-0576-05

10.13695/j.cnki.12-1222/o3.2017.05.004

2017-05-24；

2017-08-29

國(guó)家重點(diǎn)研發(fā)計(jì)劃（2016YFD0702000）；國(guó)家自然科學(xué)基金（61773113，51477028，41704025）；江蘇省自然科學(xué)基金（BK20160668）

張?jiān)滦拢?990—），女，博士研究生，從事組合導(dǎo)航研究。E-mail: smileyuexin@163.com

聯(lián) 系 人：王立輝（1979—），男，博士生導(dǎo)師，教授，從事導(dǎo)航、精密儀器等方面的應(yīng)用研究。E-mail: wlhseu@163.com