基于擴(kuò)展Kalman濾波的無人駕駛車姿態(tài)測量研究

張健永，趙忠義，崔永志

基于擴(kuò)展Kalman濾波的無人駕駛車姿態(tài)測量研究

張健永，趙忠義，崔永志

（遼寧工業(yè)大學(xué) 機(jī)械工程與自動化學(xué)院，遼寧 錦州 121001）

針對一般傳感器精度低而高精度傳感器成本高的問題，提出了基于擴(kuò)展Kalman濾波的無人駕駛車姿態(tài)解算的測量算法，該算法產(chǎn)生了姿態(tài)測量的線性最小均方誤差估計，可以在線對無人駕駛車行駛過程中的噪聲進(jìn)行估計，使得誤差降低。Matlab仿真結(jié)果說明了該算法的有效性，為無人駕駛車的姿態(tài)參數(shù)測量提供了研究思路，具有一定的價值。

無人駕駛車；姿態(tài)解算；擴(kuò)展Kalman濾波

無人駕駛車行駛過程中姿態(tài)參數(shù)的準(zhǔn)確且實時地獲取是無人駕駛車安全行駛的有力保障，一般情況下利用陀螺儀和加速度計之類的傳感器進(jìn)行測量[1-3]?？墒怯捎趥鞲衅鞯木取⒊杀疽约盁o人駕駛車行駛在不同路面中的運(yùn)動學(xué)復(fù)雜度的緣故，無人駕駛車的姿態(tài)參數(shù)很多時候并不能直接測量。同時，高精度的傳感器成本過高，不利于大面積推廣應(yīng)用；低精度的傳感器有一定的漂移誤差。這些問題給無人駕駛車的研究帶來了很大的難題[4-6]。本文通過擴(kuò)展Kalman濾波算法對無人駕駛車姿態(tài)解算進(jìn)行仿真驗證，結(jié)果表明，擴(kuò)展Kalman濾波算法的姿態(tài)參數(shù)估計比較精確。

1 姿態(tài)解算

1.1 參考坐標(biāo)系

物體的姿態(tài)根據(jù)自由度參量來描述，空間中的每一個物體都有6個自由度。物體的運(yùn)動既是絕對的又是相對的，因此在研究運(yùn)動的物體姿態(tài)變化時，一般要選擇坐標(biāo)系作為參考。不同的情況下所使用的坐標(biāo)系并不相同，在無人車姿態(tài)測量中，選出3個坐標(biāo)系供使用[7]，如圖1所示。

圖1 姿態(tài)測量坐標(biāo)系

(1)地理坐標(biāo)系OXYZ：O為無人車車體質(zhì)心處，X軸指向北，Y軸指向東，Z軸垂直于當(dāng)?shù)厮矫妫此^的“東北天坐標(biāo)系”。

(2)車體坐標(biāo)系ObXbYbZb：在本文中，Xb軸為車體縱軸向前方向，Yb軸為無人車車體橫軸向右方向，Zb軸為垂直于車體向上的方向。

(3)導(dǎo)航坐標(biāo)系OpXpYpZp：本文選取地理坐標(biāo)系OXYZ為導(dǎo)航坐標(biāo)系。

1.2 無人車姿態(tài)積分算法

四元數(shù)與角速度之間的關(guān)系為：

矩陣形式如下：

根據(jù)上面3個公式，即可求得3個角度。

2 擴(kuò)展Kalman濾波算法

擴(kuò)展Kalman濾波(EKF)是一種適用于非線性系統(tǒng)的無偏、最小方差估計[12-13]。擴(kuò)展Kalman濾波的基本原理和經(jīng)典Kalman濾波基本一致[14]；不同的是，擴(kuò)展Kalman濾波算法是通過對非線性系統(tǒng)的函數(shù)進(jìn)行泰勒展開，舍去高階量，把非線性模型線性化[15]。對于一個隨機(jī)系統(tǒng)，具體的估計過程如下所示。

狀態(tài)方程與測量方程可描述為：

因此，擴(kuò)展Kalman濾波器基于時間變化的預(yù)測方程如下所示：

系統(tǒng)隨時間變化的狀態(tài)更新方程如下所示：

3 仿真研究

為了驗證該算法的有效性，設(shè)計了基于擴(kuò)展Kalman濾波的無人駕駛車姿態(tài)測量算法的仿真。以航向角30°為初始數(shù)據(jù)，進(jìn)行誤差分析。

圖2為應(yīng)用本文所述方法得到的航向角誤差，從圖2可以看到，在沒有使用擴(kuò)展卡爾曼濾波的情況下，角度所受到的干擾噪聲較大，誤差范圍達(dá)到3°左右；使用了擴(kuò)展卡爾曼濾波后，角度的誤差范圍控制在1°以內(nèi)。誤差精度提高了66%，且航向角趨向于真實值附近，驗證了該算法的優(yōu)點和可行性。

圖2 姿態(tài)角仿真圖

4 結(jié)束語

采用基于擴(kuò)展Kalman濾波的無人駕駛車姿態(tài)解算測量算法，利用四元數(shù)法進(jìn)行姿態(tài)解算，根據(jù)測得的數(shù)據(jù)融合擴(kuò)展卡爾曼濾波不斷估計和校正，從而降低了噪聲導(dǎo)致的誤差和漂移，提高了姿態(tài)測量的精度，實時有效地傳輸姿態(tài)參數(shù)，讓無人駕駛車的行駛更加安全可靠。對比無濾波的姿態(tài)測量值可以看出，該算法可以減少噪聲干擾，較為準(zhǔn)確地估算出無人駕駛車的姿態(tài)角，使整車運(yùn)行更加穩(wěn)定可靠。

[1] 王世峰, 戴祥, 徐寧,等. 無人駕駛汽車環(huán)境感知技術(shù)綜述[J]. 長春理工大學(xué)學(xué)報, 2017, 40(1): 1-6.

[2] 蘭京. 無人駕駛汽車發(fā)展現(xiàn)狀及關(guān)鍵技術(shù)分析[J]. 內(nèi)燃機(jī)與配件, 2019(15): 209-210.

[3] 姜允侃. 無人駕駛汽車的發(fā)展現(xiàn)狀剖析及發(fā)展趨勢研究[J]. 微型電腦應(yīng)用, 2019, 35(5): 60-64.

[4] 馬天力. 無人駕駛智能車的運(yùn)動穩(wěn)定性研究[D]. 西安: 西安工業(yè)大學(xué), 2013.

[5] Zhu Qiuming, Stuart L A, Tomas N R. Hierarchical collective agent network (HCAN) for efficient fusion and management of multiple networked sensors[J]. Information Fusion, 2007, 8(3): 266-280.

[6] 陳國崢, 孫建中. 艦載導(dǎo)彈發(fā)射裝置通用化技術(shù)研究[J]. 艦船科學(xué)技術(shù), 2003, 25(2): 34-38.

[7] 張小紅, 李征航, 喬俊軍, 等. 導(dǎo)航學(xué)[M]. 武漢: 武漢大學(xué)出版社, 2017.

[8] 王焱春, 張新英, 梁芬. 多傳感器信息融合的四旋翼姿態(tài)解算研究[J]. 單片機(jī)與嵌入式系統(tǒng)應(yīng)用, 2018, 18(8): 39-42.

[9] 姬占禮. 捷聯(lián)慣導(dǎo)/GPS組合導(dǎo)航在彈箭上的應(yīng)用研究[D]. 南京: 南京理工大學(xué), 2004.

[10] 宋麗君. 基于MEMS器件的航向姿態(tài)測量系統(tǒng)的研究[D]. 西安: 西北工業(yè)大學(xué), 2007.

[11] 鄧建鋒. 面向主動安全的汽車行駛姿態(tài)實時采集系統(tǒng)研究[D]. 桂林: 桂林電子科技大學(xué), 2012.

[12] Wang Shuliang, Bi Daping, Li Jianping, et al. Joint detection and tracking algorithm for cognitive radar based on parallel structure of EKF and particle filter[J]. IET Radar, Sonar and Navigation, 2019, 13(11): 1990-1997.

[13] Danijel Pavkovic, Matija Krznar, Ante Komljenovic, et al. Dual EKF-based state and parameter estimator for a LiFePO4 battery cell[J]. Journal of Power Electronics, 2017, 17(2): 398-410.

[14] Sudarshan Swain, Bidyadhar Subudhi. Iterated extended Kalman filter-based grid synchronisation control of a PV system[J]. IET Energy Systems Integration, 2019, 1(4): 219-228.

[15] 呂夢男, 孫濤, 李潔. 基于擴(kuò)展卡爾曼濾波的車輛狀態(tài)參數(shù)估計[J]. 農(nóng)業(yè)裝備與車輛工程, 2018, 56(5): 77-80.

Research on Attitude Measurement of Unmanned Vehicle Based on Extended Kalman Filter

ZHANG Jian-yong, ZHAO Zhong-yi, CUI Yong-zhi

（College of Mechanical Engineering and Automation, Liaoning University of Technology, Jinzhou 121001, China）

Aiming at the problem that the general sensor has low precision and high-precision sensor cost, this paper proposes a measurement algorithm based on extended Kalman filter for the attitude calculation of driverless vehicles. The algorithm generates linear minimum mean square error estimation of attitude measurement, which can be online. The noise during driving the driverless car is estimated, so that the error is reduced. The Matlab simulation results show the effectiveness of the algorithm provides the research on the attitude parameter measurement of the driverless car, which has certain value.

driverless car; attitude solution; extended Kalman filter

TP311

A

1674-3261(2020)04-0216-03

10.15916/j.issn1674-3261.2020.04.002

2019-11-12

張健永(1993-)，男，安徽蚌埠人，碩士生。

趙忠義(1971-)，男，遼寧錦州人，高級工程師，博士。

責(zé)任編校：孫 林