基于衛星導航/慣性單元松耦合的低速智能電動汽車航向角估計

熊 璐，陸逸適，夏 新，高樂天，余卓平

（1.同濟大學 汽車學院，上海 201804；2.同濟大學 新能源汽車工程中心，上海 201804）

近年來，低速智能電動汽車發展迅速，組合定位技術是支撐其發展的關鍵技術，航向角估計是組合定位算法中的重要組成部分［1］。準確的航向角也是車輛軌跡跟蹤控制的必要前提［2-3］。雙天線GNSS（global navigation satellite system）接收機可直接輸出車輛的航向角，然而，受制于成本，GNSS接收機輸出的航向角通常采樣頻率較低，并且伴隨有較大噪聲和時間延遲［4］，實時控制難以直接應用。智能汽車大多配備有高精度GNSS接收機、磁力計和慣性單元等傳感器，現有研究大多利用這些多源傳感器通過估計手段間接獲取航向角。這些研究主要分為兩類：GNSS/INS（inertial navigation system）組合方法，磁力計與IMU（inertial measurement unit）組合方法。

在GNSS/INS組合的方法中，航向角通常與俯仰角、側傾角、速度和位置等狀態一同在慣性導航系統INS中被解算，然后通過一個濾波器估計INS的誤差，從而對慣性導航的誤差進行修正［5］。當車輛正常行駛時，由GNSS/INS組合系統的誤差狀態方程可知，航向角誤差在任意時刻并不完全可觀［6-7］，航向角誤差只有在載體滿足一定水平加速度激勵的條件下才可以被準確估計，進而使用該誤差對慣性導航中航向角進行修正。但對低速智能電動汽車而言，車輛本身無法提供較大的加速度激勵，在常用的GNSS/INS組合架構下，航向角誤差的可觀性較差，從而導致航向角的精度難以保證。

也有學者通過利用磁力計對地磁進行測量，從而在進行姿態角解算時通過地磁對姿態角進行輔助估計。在無人機的應用中，方根在等學者對三軸磁力計進行了傾斜補償，然后使用拓展卡爾曼濾波算法融合加速度計、陀螺儀和磁力計信息對無人機的姿態進行了估計［8］。Yoon等學者設計了一個隨機濾波器，保證了在磁力計受干擾的條件下航向角估計器與磁力計的隔離［9］。然而，在電動汽車環境下，車輛磁場分布較為復雜，微弱的地磁很容易被車載電器周圍磁場干擾，從而導致較大的角度測量偏差，進而導致航向角誤差。

本文在低速智能電動汽車清掃車平臺下，利用智能汽車搭載的GNSS和IMU傳感器設計了航向角估計器，主要貢獻在于創新性地基于殘差自適應RAE（residual adaptive estimation）卡爾曼濾波器設計了GNSS/IMU松耦合（loosely coupled，LC）組合的航向角估計器：提出了基于IMU的航向角積分方法，推導了松耦合條件下誤差動態與測量模型；針對GNSS信號質量時變問題，基于本文提出的航向角誤差動態和測量模型，使用殘差自適應卡爾曼濾波算法對航向角誤差進行了估計。解決了IMU長時間積分累積誤差問題，GNSS航向角測量頻率低、噪聲時變和存在異常值的問題。最后，通過在不同GNSS信號條件下進行的多組實車試驗，驗證了本文提出的航向角估計算法的有效性。

1 基于GNSS/IMU組合的航向角估計

受GNSS/INS組合方式啟發，本文提出了GNSS/IMU LC組合的航向角估計方法。該方法架構如圖1所示，主要包含3個模塊：GNSS航向角接收模塊，基于IMU的航向角積分模塊，二者的誤差通過一個RAE卡爾曼濾波算法進行濾波處理，濾波后的航向誤差經過反饋作用于基于IMU的航向角積分模塊。圖1中，ψGNSS為GNSS接收機輸出航向，ψI為IMU積分所得航向角，eψ為航向角估計誤差，kψ為反饋系數。

圖1 航向角估計方法架構Fig.1 Structure of heading angle estimation method

1.1 基于IMU的航向積分方法

基于IMU的航向角積分方法由下式給出：

式中：下標k表示k時刻，k-1表示k-1時刻；ψk-1表示航向值；dψk表示k時刻橫擺角速度增量，從IMU直接獲取；eψk由航向角誤差估計模塊估計；bψk表示角速度零偏，由靜止時IMU輸出的角速度獲取；Δt為IMU采樣時間。

1.2 航向誤差估計方法

1.2.1 航向誤差估計測量模型

假設IMU積分得到的航向角和GNSS接收機輸出的航向角滿足如下關系：

式中：ψr表示真實航向角；wI和wGNSS分別表示積分所得航向和GNSS接收機輸出航向的高斯白噪聲。

將式（2）和（3）作差可得航向角誤差的測量方程

鏈路聚合技術是一種高效、高可靠性的技術，能夠低成本的解決企業網絡交換設備之間級聯造成的帶寬瓶頸，同時也提供了多線冗余的功能，現在已成為企業交換網絡中主流的應用。

式中：η=wI-wGNSS表示高斯白噪聲。

1.2.2 航向誤差估計過程模型

航向角誤差估計的動態由下式給出：

式中：上標點表示導數；w1和w2分別表示航向角誤差動態和零偏的白噪聲。進一步將連續方程改寫為離散方程可得

基于式（4）和式（6）所描述的系統，即可通過卡爾曼濾波算法對航向角誤差進行估計。

1.3 本文自適應卡爾曼濾波算法

卡爾曼濾波算法結構如圖2所示。圖中，Gk表示卡爾曼濾波增益，Pk-1|k-1為k-1時刻協方差矩陣，Pk|k為k時刻協方差矩陣，Ck|k-1為測量矩陣，Ak-1表示k-1時刻系統矩陣，Pk|k-1為k-1時刻對k時刻預測的狀態誤差協方差矩陣，Rk為測量噪聲協方差矩陣，I為單位陣，為k時刻最優狀態，為k-1時刻最優狀態，為k-1時刻預測k時刻的狀態，zk為k時刻測量量，dt為時間步長，Γk-1為過程噪聲的輸入矩陣，Qk-1過程噪聲協方差矩陣。

圖2 卡爾曼濾波算法Fig.2 Kalman filter

因為GNSS信號易受干擾，在實際使用卡爾曼濾波時，測量噪聲的協方差矩陣需要進行在線自適應估計，以保證濾波效果的穩健性。而系統噪聲主要由IMU的噪聲決定，一般變化較小，不再進行自適應。

k時刻航向角誤差測量的新息dk和殘差εk分別定義為

為了使算法對當前GNSS接收機輸出的速度和位置統計特性具有更好的動態性能，使用漸消因子對過去時刻的測量值進行部分遺忘，削弱過去測量信息對當前估計的影響，漸消因子b為（0，1）之間的實數，根據漸消因子得到漸消系數αk為

由于式（13）中包含(εkεkT+CkPk|kCkT)，需要迭代運算，為了保證計算的合理性，避免引起濾波異常，因此需要對的各個元素的大小進行限制。這里需要對各個測量值依次進行約束，對于第i個量測有

2 航向角誤差反饋策略

前面已提到，GNSS信號易受外界環境干擾，如遮擋、多路徑效應等，造成航向角測量中出現異常值，經過異常值進行量測更新后，即使測量噪聲協方差矩陣已經進行了自適應處理，估計所得的航向角誤差也不應再反饋至基于IMU的積分算法中，需要對這種異常情況設計策略對航向角誤差反饋加以約束，如圖3所示。當標志位Fψ=1時，反饋的航向角誤差為0。

圖3 航向角誤差反饋策略Fig.3 Feedback strategy of heading angle error

圖3中，下標GNSS表示對應GNSS接收機的變量，變量下標T表示相應變量的閾值，E()和Var()分別表示變量的期望和方差，下標E和V分別表示期望和方差，RRMS_Lati和RRMS_Longi分別表示GNSS接收機輸出的緯度和經度的均方差誤差，PPDOP表示幾何精度因子。

3 試驗結果與分析

為了對本文提出的算法進行驗證，進行了離線和在線試驗驗證。離線試驗是通過數據采集設備采集實車試驗數據，然后使用Matlab/Simulink運算平臺對算法驗證。在線試驗是通過嵌入式處理器在線接收IMU和GNSS信息，在處理器中對算法在線實時驗證。

3.1 試驗平臺介紹

試驗平臺車為一臺智能電動清掃車，如圖4所示。該車作業最大車速為5 km·h-1。用于離線試驗測試平臺如圖5所示。華測公司的P2組合導航產品測得的航向角用于作為參考值，Trimble BD982 GNSS接收機用于提供測量航向角，ADIS16490提供橫擺角速度增量和橫擺角速度信息，三者的串口信息通過STM32F103單片機轉為CAN（controller area network）信號通過一臺NI CompactRIO 9082數據采集設備采集，數據采集程序基于Labview 2013搭建，實車試驗采集的數據在Matlab/Simulink中經過本文提出的算法處理對航向角進行估計。

圖4 試驗平臺車：低速智能電動清掃車Fig.4 Test vehicle:Low speed autonomous electric sweeper

圖5 離線測試試驗平臺Fig.5 Test platform for offline validation

實車在線試驗驗證平臺如圖6所示。與圖5不同的是，這里使用了一塊內核為DSP28335的工業級嵌入式控制器作為算法運算平臺，控制器運行周期為10 ms。由于控制器在線數據采集變量個數受限于控制器資源，故只展示主要結果，更多算法細節在離線試驗結果中展示。

圖6 在線試驗驗證平臺Fig.6 Test platform for online validation

3.2 討論與分析

圖7和圖8分別給出了離線和在線試驗結果。由于離線仿真軟件設置的計算精度與在線控制器相同，兩種試驗結果也基本相同，在離線試驗中額外給出了測量協方差自適應結果。

圖7 離線試驗結果Fig.7 Results of offline test

試驗過程中，車輛行駛速度在1.5 m·s-1以下。Trimble BD982 GNSS接收機直接輸出的航向角噪聲較大，由圖7b、圖7c、圖8b和圖8c可以看到，航向角誤差經過自適應卡爾曼濾波后噪聲明顯可以被削弱。由圖7f可以看出，誤差經過反饋至積分所得航向中對積分累積誤差進行修正，由航向角局部放大圖可以看到，修正后航向角相比于BD982更加平滑。圖7b和圖8b中圈出的GNSS測量航向異常值也可經過算法濾除。圖7g和圖7h中也可看到，自適應卡爾曼濾波中的協方差矩陣可隨著測量噪聲變化而改變，以減弱時變噪聲對算法的影響。在實際應用中，將估計所得航向應用于車輛組合定位中，通過航向將車速分解至導航坐標系，進一步積分得到位置，如圖7d和圖8d。當GNSS接收機輸出位置產生異常點時，如圖7e和圖8e，位置反饋被切斷，位置由積分算法推算得到。可以看到使用估計所得航向進行推算得到的位置能夠避免跳點，平滑度過GNSS信號異常區域，與華測P2推算所得位置較為接近。

圖8 在線試驗結果Fig.8 Results of online test

總體上，由圖7與圖8以及表1和表2可以看出，航向角平均誤差小于0.2°，航向角誤差均方差小于0.6°，最大誤差小于3°。誤差較大的點出現在GNSS信號異常區域，由于華測P2中所使用的GNSS接收機為Novatel 718D板卡，而本研究中所使用的是BD982，二者差異可能也會導致兩者航向產生誤差，筆者團隊后面將更換GNSS接收機對本文航向算法進一步驗證。

表1 離線試驗誤差表格Tab.1 Estimation error of offline test （°）

表2 在線試驗誤差表格Tab.2 Estimation error of online test （°）

4 結論

基于低速智能電動汽車，本文研究了基于GNSS/IMU松耦合組合的航向角估計算法。得出以下結論：

（1）提出了GNSS/IMU松耦合條件下的航向角估計方法，設計了基于IMU的航向角積分方法，推導了松耦合條件下誤差動態與測量模型，并基于此模型設計了航向角誤差估計方法，從而對航向角間接估計。

（2）針對GNSS信號質量時變問題，使用殘差自適應卡爾曼濾波算法對航向角誤差進行了估計，為了減小計算量，基于漸消因子通過遞推方式對測量噪聲協方差矩陣進行了自適應。針對GNSS信號質量設計了航向角誤差反饋修正策略。在不同GNSS信號條件下進行的多組實車試驗驗證了本文提出的航向角估計算法的有效性。