基于組合導航的汽車姿態數據采集系統設計

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

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

引言

　　交通事故的頻繁發生，使得很多人開始致力于駕駛行為檢測系統的研究。最初從醫學角度出發，例如腦電、皮電、心電^[1]、脈搏等，日本豐田公司設計了一塊手表似的裝置，通過檢測駕駛人的脈搏、心率等生理信號來達到檢測駕駛人的駕駛狀態^[2]，從人的形態變化出發，在出現一些異常駕駛行為時會表現出和正常時不一樣的形態，比如疲勞駕駛導致的眼睛閉合頻率加大[3]和視野方向^[4]，嘴巴打開角度偏大^[5]，手部動作^[6]，到后來應用光學、化學、物理、機械和電子學等實質性的檢測^[7]。然而，上述的這些研究都是以駕駛人為出發點，針對單一的不良駕駛行為進行的。吉林大學吳巖研究了一種基于握力特征量的駕駛員異常行為測控系統，駕駛人通過U盤把正常駕駛握力特征載入系統，通過方向盤上的傳感器實時感應來的數據與正常值作對比^[2]。這種方法綜合檢測了駕駛人的狀態，但是存在兩個問題，一是在存儲正常值時駕駛人的狀態不確定，導致所謂的正常值不正常;二是無法檢測駕駛人的一些不良習慣，如加減速不穩，愛急剎車、急轉彎等。基于此，以車輛為出發點，通過檢測車輛行駛過程中速度、加速度、角度等姿態數據來判斷駕駛人駕駛行為，不僅不影響駕駛人駕駛，還能從數據中分析出駕駛人是否符合駕駛要求，為公交公司、出租車公司，以及一些運營車輛的公司提供了司機測評依據，從根本上預防了交通事故的發生。文中設計了一種車輛姿態的數據采集系統，可以為相關數據分析機構提供比較準確的原始大數據，而且利用捷聯慣導和GPS形成的組合導航系統對車輛準確定位。

1 硬件設計

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

2 姿態分析及解算算法

2.1 姿態分析

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

2.2 基于四元數的姿態解算算法

　　通常用歐拉角法、方向余弦法、等效旋轉矢量法、四元數法、對偶四元數法四種方法來描述姿態變化。歐拉角法的方程奇異現象和三角函數計算問題，方向余弦法的多個微分方程，等效旋轉矢量法的計算量，對偶四元數的不成熟[8-10]，使得計算量小、無幾點誤差、相對成熟的四元數法成為本系統的姿態解算算法首選。

2.2.1 四元數姿態解算原理

　　四元數表示方法如下^[11]：

　　其中，w_x、w_y、w_z是陀螺儀輸出的數據。

　　根據二階龍格庫塔法有^[9]：

(7)

　　其中，T是姿態變換的時間間隔。根據初始化四元素值[6]，結合二階龍格庫塔法求得任一時刻的四元素值，進而得到姿態角。

2.2.2 數據采集系統的誤差補償

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

　　互補濾波的表達式如下^[11]：

2.3 基于卡爾曼濾波的組合導航系統解算算法

　　根據車的姿態來判斷駕駛人的行為，一定程度上會受到外界客觀環境的影響，比如路況。通過定位功能判斷車輛所處地段，結合地段的道路情況，很大程度上可以排除這一干擾，使得更準確地判斷駕駛人行為。目前GPS全球定位系統應用比較廣泛，具備省時、快速、高精度、高效率的優點，不存在累計誤差，但是在一些特殊的場合和地貌，GPS信號會變弱，甚至完全失去^[12]。由加速度計和陀螺儀形成的慣性導航系統是一種推算式導航系統，相對于GPS，受地貌、天氣等影響較小，但是正因為推算式使得在不斷積分過程中造成累積誤差，隨著時間的增加，漂移越大，最后使得導航信息發散失效。兩者結合正好互補性地達到準確定位的效果，文中利用卡爾曼濾波法將捷聯慣導和GPS進行松組合，將慣性導航與GPS得到的位置速度信息進行差運算得到觀測量，以慣性導航的誤差方程作為系統方程，采用卡爾曼濾波對誤差實現最優估計，對慣性導航進行數據修正^[13]。