基于組合導(dǎo)航的汽車姿態(tài)數(shù)據(jù)采集系統(tǒng)設(shè)計(jì)

作者/ 王贇贇 李建民 侯文 中北大學(xué) 信息與通信工程學(xué)院(山西 太原 030051)

摘要：本文設(shè)計(jì)了一個(gè)基于組合導(dǎo)航的汽車姿態(tài)數(shù)據(jù)采集系統(tǒng)，該系統(tǒng)利用集加速度計(jì)和陀螺儀于一體的高精度高靈敏度的慣性測(cè)量芯片MPU6050，結(jié)合地磁傳感器形成的九軸傳感器，對(duì)汽車的線性加速度、角速度參數(shù)數(shù)據(jù)進(jìn)行直接采集，以互補(bǔ)濾波實(shí)現(xiàn)九軸傳感器的誤差補(bǔ)償，用四元數(shù)法對(duì)其姿態(tài)解算，得到汽車姿態(tài)角度。同時(shí)，利用卡爾曼濾波法將捷聯(lián)慣導(dǎo)和GPS結(jié)合，形成的組合導(dǎo)航系統(tǒng)實(shí)現(xiàn)汽車實(shí)時(shí)定位，提供汽車實(shí)時(shí)速度和位移。實(shí)驗(yàn)測(cè)試結(jié)果表明，該數(shù)據(jù)采集系統(tǒng)得到的數(shù)據(jù)準(zhǔn)確率高，實(shí)時(shí)定位準(zhǔn)，能夠可靠地為汽車安全策略研究提供數(shù)據(jù)，有著廣闊的使用價(jià)值。

引言

　　交通事故的頻繁發(fā)生，使得很多人開始致力于駕駛行為檢測(cè)系統(tǒng)的研究。最初從醫(yī)學(xué)角度出發(fā)，例如腦電、皮電、心電^[1]、脈搏等，日本豐田公司設(shè)計(jì)了一塊手表似的裝置，通過檢測(cè)駕駛?cè)说拿}搏、心率等生理信號(hào)來達(dá)到檢測(cè)駕駛?cè)说鸟{駛狀態(tài)^[2]，從人的形態(tài)變化出發(fā)，在出現(xiàn)一些異常駕駛行為時(shí)會(huì)表現(xiàn)出和正常時(shí)不一樣的形態(tài)，比如疲勞駕駛導(dǎo)致的眼睛閉合頻率加大[3]和視野方向^[4]，嘴巴打開角度偏大^[5]，手部動(dòng)作^[6]，到后來應(yīng)用光學(xué)、化學(xué)、物理、機(jī)械和電子學(xué)等實(shí)質(zhì)性的檢測(cè)^[7]。然而，上述的這些研究都是以駕駛?cè)藶槌霭l(fā)點(diǎn)，針對(duì)單一的不良駕駛行為進(jìn)行的。吉林大學(xué)吳巖研究了一種基于握力特征量的駕駛員異常行為測(cè)控系統(tǒng)，駕駛?cè)送ㄟ^U盤把正常駕駛握力特征載入系統(tǒng)，通過方向盤上的傳感器實(shí)時(shí)感應(yīng)來的數(shù)據(jù)與正常值作對(duì)比^[2]。這種方法綜合檢測(cè)了駕駛?cè)说臓顟B(tài)，但是存在兩個(gè)問題，一是在存儲(chǔ)正常值時(shí)駕駛?cè)说臓顟B(tài)不確定，導(dǎo)致所謂的正常值不正常;二是無法檢測(cè)駕駛?cè)说囊恍┎涣剂?xí)慣，如加減速不穩(wěn)，愛急剎車、急轉(zhuǎn)彎等?；诖?，以車輛為出發(fā)點(diǎn)，通過檢測(cè)車輛行駛過程中速度、加速度、角度等姿態(tài)數(shù)據(jù)來判斷駕駛?cè)笋{駛行為，不僅不影響駕駛?cè)笋{駛，還能從數(shù)據(jù)中分析出駕駛?cè)耸欠穹像{駛要求，為公交公司、出租車公司，以及一些運(yùn)營(yíng)車輛的公司提供了司機(jī)測(cè)評(píng)依據(jù)，從根本上預(yù)防了交通事故的發(fā)生。文中設(shè)計(jì)了一種車輛姿態(tài)的數(shù)據(jù)采集系統(tǒng)，可以為相關(guān)數(shù)據(jù)分析機(jī)構(gòu)提供比較準(zhǔn)確的原始大數(shù)據(jù)，而且利用捷聯(lián)慣導(dǎo)和GPS形成的組合導(dǎo)航系統(tǒng)對(duì)車輛準(zhǔn)確定位。

1 硬件設(shè)計(jì)

　　系統(tǒng)由數(shù)據(jù)采集終端、無線數(shù)據(jù)傳輸和接收數(shù)據(jù)服務(wù)器三部分組成。以ST公司推出的性能較強(qiáng)的STM32F107V作為控制器，結(jié)合慣性測(cè)量芯片MPU6050和地磁傳感器形成的九軸傳感器和著名瑞士U-Blox公司生產(chǎn)的NEO-6系列NE0-6MGPSGPS定位模塊構(gòu)建穩(wěn)定的數(shù)據(jù)采集終端。借助終端采集的加速度值、角速度值和磁場(chǎng)強(qiáng)度，通過姿態(tài)解算算法得到姿態(tài)角信息和速度位移信息。采用內(nèi)嵌TCP/IP協(xié)議的芯片SIM5360作為無線通信模塊，借用移動(dòng)公司的GPRS網(wǎng)絡(luò)構(gòu)建無線數(shù)據(jù)傳輸平臺(tái)。以PC機(jī)作為服務(wù)器，接收采集終端采集和解算出的數(shù)據(jù)參數(shù)。系統(tǒng)整體框圖如圖1。

2 姿態(tài)分析及解算算法

2.1 姿態(tài)分析

　　在載體位置和運(yùn)動(dòng)規(guī)律的描述中，選取參考基準(zhǔn)的坐標(biāo)系是極其重要的，在這里，選取東北天的地理坐標(biāo)系(n系，也稱導(dǎo)航坐標(biāo)系)作為固定參考坐標(biāo)系。同時(shí)，固定傳感器于汽車裝飾臺(tái)中心，以此為原點(diǎn)，按傳感器方向建立動(dòng)坐標(biāo)系(b系，也稱載體坐標(biāo)系)。隨著汽車的運(yùn)動(dòng)，載體坐標(biāo)系會(huì)跟著變化，用載體坐標(biāo)系相對(duì)于地理坐標(biāo)系的變化角來描述汽車姿態(tài)變化，按載體坐標(biāo)系繞X、Y、Z三軸轉(zhuǎn)動(dòng)依次稱為俯仰角θ、翻滾角γ、偏航角，如圖2所示。

2.2 基于四元數(shù)的姿態(tài)解算算法

　　通常用歐拉角法、方向余弦法、等效旋轉(zhuǎn)矢量法、四元數(shù)法、對(duì)偶四元數(shù)法四種方法來描述姿態(tài)變化。歐拉角法的方程奇異現(xiàn)象和三角函數(shù)計(jì)算問題，方向余弦法的多個(gè)微分方程，等效旋轉(zhuǎn)矢量法的計(jì)算量，對(duì)偶四元數(shù)的不成熟[8-10]，使得計(jì)算量小、無幾點(diǎn)誤差、相對(duì)成熟的四元數(shù)法成為本系統(tǒng)的姿態(tài)解算算法首選。

2.2.1 四元數(shù)姿態(tài)解算原理

　　四元數(shù)表示方法如下^[11]：

　　其中，w_x、w_y、w_z是陀螺儀輸出的數(shù)據(jù)。

　　根據(jù)二階龍格庫塔法有^[9]：

(7)

　　其中，T是姿態(tài)變換的時(shí)間間隔。根據(jù)初始化四元素值[6]，結(jié)合二階龍格庫塔法求得任一時(shí)刻的四元素值，進(jìn)而得到姿態(tài)角。

2.2.2 數(shù)據(jù)采集系統(tǒng)的誤差補(bǔ)償

　　根據(jù)四元數(shù)姿態(tài)解算原理，姿態(tài)角的求解只要陀螺儀輸出的角速度，然而，只用陀螺儀得到的姿態(tài)角誤差很大，包括傳感器自身的測(cè)量誤差、陀螺儀抗振能力弱帶來的誤差、交叉耦合帶來的誤差，以及它的漂移誤差和隨機(jī)誤差等。在文獻(xiàn)[11]中介紹了用加速度計(jì)和磁力感應(yīng)組合采用歐拉角方式求得姿態(tài)角，同樣由于加速度干擾和磁干擾使得此組合得到的姿態(tài)角存在很大誤差。綜上，文中采用三種傳感器結(jié)合形成的九軸傳感器進(jìn)行數(shù)據(jù)采集，選用互補(bǔ)濾波法將加速度計(jì)和磁力傳感器得到的姿態(tài)數(shù)據(jù)和用陀螺儀得到的姿態(tài)數(shù)據(jù)的差作為陀螺儀輸出角速度的誤差補(bǔ)償，然后利用四元數(shù)法得到誤差小的姿態(tài)數(shù)據(jù)。

　　互補(bǔ)濾波的表達(dá)式如下^[11]：

2.3 基于卡爾曼濾波的組合導(dǎo)航系統(tǒng)解算算法

　　根據(jù)車的姿態(tài)來判斷駕駛?cè)说男袨?，一定程度上?huì)受到外界客觀環(huán)境的影響，比如路況。通過定位功能判斷車輛所處地段，結(jié)合地段的道路情況，很大程度上可以排除這一干擾，使得更準(zhǔn)確地判斷駕駛?cè)诵袨?。目前GPS全球定位系統(tǒng)應(yīng)用比較廣泛，具備省時(shí)、快速、高精度、高效率的優(yōu)點(diǎn)，不存在累計(jì)誤差，但是在一些特殊的場(chǎng)合和地貌，GPS信號(hào)會(huì)變?nèi)?，甚至完全失?sup>[12]。由加速度計(jì)和陀螺儀形成的慣性導(dǎo)航系統(tǒng)是一種推算式導(dǎo)航系統(tǒng)，相對(duì)于GPS，受地貌、天氣等影響較小，但是正因?yàn)橥扑闶绞沟迷诓粩喾e分過程中造成累積誤差，隨著時(shí)間的增加，漂移越大，最后使得導(dǎo)航信息發(fā)散失效。兩者結(jié)合正好互補(bǔ)性地達(dá)到準(zhǔn)確定位的效果，文中利用卡爾曼濾波法將捷聯(lián)慣導(dǎo)和GPS進(jìn)行松組合，將慣性導(dǎo)航與GPS得到的位置速度信息進(jìn)行差運(yùn)算得到觀測(cè)量，以慣性導(dǎo)航的誤差方程作為系統(tǒng)方程，采用卡爾曼濾波對(duì)誤差實(shí)現(xiàn)最優(yōu)估計(jì)，對(duì)慣性導(dǎo)航進(jìn)行數(shù)據(jù)修正^[13]。