基于卡爾曼濾波的自平衡車數據融合應用

文/袁文玉

1 引言

兩輪自平衡車是一種環保安全、靈活便捷且能夠自動調節姿態的新型輪式車，可應用于較多的特殊場合。其基本原理類似倒立擺，是一種多變量、不穩定、非線性及強耦合的系統。自平衡車系統的控制模塊大體分為速度控制模塊、方向控制模塊和姿態平衡控制模塊，本文主要討論自平衡車的姿態調整問題，分析車體的受力情況及運動特性，并將其理想化建立數學模型。

在傳統的平衡車姿態調整方法中，常采用單一的姿態傳感器向控制器輸入車體的傾角信號，但由于傳感器本身存在的零漂和測量誤差使得在長時間測量時，難以得到精確的車體傾角。故本文采用由陀螺儀和加速度計構成的多傳感器姿態檢測模塊，提出一種基于卡爾曼濾波的自平衡車數據融合方法，設置固定的信任權重，將兩種傳感器的輸出數據進行融合，同時濾除噪聲，以消除采用單個傳感器存在的誤差累積及提高車體角度測量的實時在線估計，得到最優化的自平衡車姿態角度。

2 自平衡車運動特性模型分析

自平衡車以兩平行車輪為支撐，通過姿態檢測模塊測量車體傾角，經控制器處理輸出控制信號，以此來驅動兩輪電機維持車體平衡。在只考慮自平衡車靜止和直線行駛的情況下，其機械結構類似于一級倒立擺模型,可以將其簡化為倒立擺模型，在實際操作中車體姿態平衡時，其重心位置與豎直方向呈一定的傾角，但為簡化模型分析假設車體平衡時重心位置位于垂直方向，自平衡車的結構剖面圖和簡化模型如圖1所示。

由于自平衡車的車輪與電機通過車軸連接，陀螺儀、加速度計、單片機和運算放大器PCB 板靠近電機安裝使重心偏下，可將其視為一個整體作為兩車輪的質量和m；忽略輪胎與地面的摩擦力以及行駛時的空氣阻力；忽略電機和編碼器齒輪之間的摩擦力。假設車體正常行駛時的加速度為a，車體與豎直方向之間的夾角為θ，使重心恢復豎直方向所需拉力為：

當角度θ 較小時，正弦值與角度值基本相等，且因為電機的驅動力受車體傾角的影響，則平衡車的加速度與傾角存在一定的比例關系，假設系數為k,由此可將公式簡化為：

圖1：自平衡車結構剖面圖和簡化模型

圖2：加速度敏感軸與重力加速度關系

圖3：卡爾曼濾波器仿真模型

自平衡車的姿態調整在保證測量角度精確的前提下，還需要確保恢復垂直狀態的迅速性，可添加有關角速度 的阻尼力：

由此可以看出，需要通過控制電機使車體的加速度維持在：

可見實時地進行傾角測量，給予電機控制信號才能使車體維持平衡。

3 姿態檢測系統組成

3.1 加速度計測量原理與誤差分析

本文采用加速度計與陀螺儀組成姿態檢測模塊，經K60 核心控制器進行A/D 轉換后由卡爾曼濾波器進行數據融合與濾波，得出精確的角度信號。應用MPU6050 中的3 軸加速度計模塊，安裝時使z 軸與車體傾倒方向一致，以重力加速度為輸入，靜止時測得的數據為重力加速度在各軸上的分量，當車體的傾角發生變化時，加速度計三個敏感軸的方向隨之發生變化由此引起加速度分量變化，由圖2可知，α、β、θ 分別為x 軸、z 軸、y 軸與重力加速度的夾角，α1、β1、θ1分別為各軸與水平方向的夾角，各軸傾角與加速度分量的關系為：

由上式可知，測得數據與傾角呈非線性正弦關系，可通過對輸出數據進行反正弦變化得出車體的傾角值，但是當平衡車運動時，車體所受加速度不再是單一的重力加速度還包括電機驅動產生的加速度，同時存在著系統的振動和機械噪聲，這些因素都會導致加速度計測量角度不精確，且加速度計動態響應較慢不適合跟蹤需要迅速應變的動態系統，所以需要與陀螺儀共同合作實現追蹤。

3.2 陀螺儀測量原理與誤差分析

陀螺儀主要用來測量車體的瞬間角速度，分x 軸、y 軸和z 軸分別可以用來檢測物體的俯仰角、翻滾角和偏航角。由于安裝在自平衡車上，陀螺儀的主要作用是測量車體的俯仰角變化的角速度，一般通過積分運算得到車體的當前角度值。在短時間內，陀螺儀的測量精確度較高但是由于車體顛簸和機械振動會造成陀螺儀測得的角速度信號出現微小漂移，使得積分出現誤差，最終形成誤差累積導致在長時間測量時無法得到真實的車體傾角，這就需要加速計對其測量的角度進行定期糾正。

4 基于卡爾曼濾波的數據融合方法設計

由于加速度計靜態性能優良但動態跟蹤能力不行，陀螺儀測量傾角精確簡便但是存在積分誤差累積，所以需要融合兩傳感器的輸出數據，得出最優化的車體傾角，可通過卡爾曼濾波器實現數據融合過程。首先，在考慮到卡爾曼濾波器主要用于離散時間系統的前提下，由于車體傾角的角度可由角速度積分得出，可將車體的實際傾斜角度θ 與用加速度計觀測的陀螺儀測量角速度的常數偏差t 作為系統的狀態向量，可得如下狀態與測量方程：

式中，X(k)為k 時刻自平衡車的狀態（陀螺儀測得的角度及其常數誤差），X(k-1)為k-1時刻自平衡車的狀態，U(k-1)為k-1 時刻陀螺儀測得的角速度，為k 時刻的陀螺儀測量過程中存在的噪聲，Z(k)為k 時刻加速度計經反正弦計算后輸出的角度，為k 時刻的加速度計的測量過程中存在的干擾。

簡化狀態方程與測量方程為：

其中，A 為系統的預測矩陣，B 為控制矩陣，H 為傳感器測量矩陣。

根據以上公式可知，每一個狀態變量更新到一個新的狀態時，僅需要上一個時刻參與估算且其仍服從高斯分布，此外，為完成對卡爾曼濾波器的校正得到最優的角度值，需要求取k 時刻的協方差矩陣P(k)，而系統的過程噪聲協方差矩陣Q(k)和測量誤差協方差矩陣R(k)為P(k)的組成部分需進行合理調整，如下所示：

k 時刻自平衡車系統的預測方程：

由預測狀態方程得出的系統協方差為：

經傳感器測量數據修正的狀態方程：

上式中，Kh為卡爾曼增益，其表示方程如下：

式中，HT為H 的轉置矩陣。

k 時刻 的協方差：

由此，X(k|k)和P(k|k)就是最新的最優估計，根據卡爾曼的迭代思想X(k|k)和P(k|k)將作為新的X(k-1|k-1)和P(k-1|k-1)進行k+1 時刻的最優化運算。

4 效果分析

4.1 仿真分析

利用MATLAB 建立本文設計的卡爾曼濾波器smiulink 仿真模型，以對比濾波前后波形驗證濾波效果，仿真模型如圖3所示。

通過對陀螺儀和加速度計信任權重的分配，以及測量噪聲協方差系數的調整，以達到理想的濾波效果。濾波前后波形對比如圖4所示。

4.2 實測應用分析

將基于卡爾曼濾波的數據融合的方法應用于以K60 為核心控制器的自平衡車上，以檢測其有效性。由上式（16）、（18）、（19） 可知，自平衡車的姿態主要由卡爾曼濾波器分配給陀螺儀和加速度計信任權重q_acce 、q_gyro 以及測量矩陣的協方差r_acce 決定。信任權重的分配需要根據傳感器的測量精度，因陀螺儀的測量精度高于加速度計，所以q_gyro應略小于q_acce，由此設置q_gyro=0.001，q_acce=0.003，r_acce=0.001，通過UART 無線通信將波形顯示到上位機進行觀察。加速度計與數據融合輸出波形對比如圖5所示。

上位機中顯示出兩條波形，棕色波形表示加速計z 軸加速度經反正弦變化得出的數據波形，綠色波形是經過卡爾曼濾波器進行數據融合后得出的波形，可以看出加速度計的波形具有較多毛刺，這是因為加速度計能比較準確地反映車體傾角的變化，而陀螺儀測得的角速度積分波形較平滑，但是存在一定的積分誤差，經卡爾曼數據融合后的波形既具有較好的平滑性又能夠準確跟蹤加速度的波形，且無超調和滯后，此時自平衡車姿態良好，從而驗證該方法的正確性和有效性。

5 總結

圖4：濾波前后波形對比圖

圖5：加速度計與卡爾曼濾波輸出波形對比

本文針對加速度計和陀螺儀作為單一傳感器進行角度測量的缺陷分析，采用多種傳感器進行角度測量，利用卡爾曼濾波算法將輸出數據進行融合濾波以得出最優化的角度值，最后通過對實測數據波形分析實現對自平衡車姿態的理想調整，能夠使車體維持在一定傾角且前后偏移較小，說明只要選擇合理的控制參數，就可以實現自平衡車體姿態的穩定性。