文/袁文玉

1 引言

兩輪自平衡車(chē)是一種環(huán)保安全、靈活便捷且能夠自動(dòng)調(diào)節(jié)姿態(tài)的新型輪式車(chē)，可應(yīng)用于較多的特殊場(chǎng)合。其基本原理類似倒立擺，是一種多變量、不穩(wěn)定、非線性及強(qiáng)耦合的系統(tǒng)。自平衡車(chē)系統(tǒng)的控制模塊大體分為速度控制模塊、方向控制模塊和姿態(tài)平衡控制模塊，本文主要討論自平衡車(chē)的姿態(tài)調(diào)整問(wèn)題，分析車(chē)體的受力情況及運(yùn)動(dòng)特性，并將其理想化建立數(shù)學(xué)模型。

在傳統(tǒng)的平衡車(chē)姿態(tài)調(diào)整方法中，常采用單一的姿態(tài)傳感器向控制器輸入車(chē)體的傾角信號(hào)，但由于傳感器本身存在的零漂和測(cè)量誤差使得在長(zhǎng)時(shí)間測(cè)量時(shí)，難以得到精確的車(chē)體傾角。故本文采用由陀螺儀和加速度計(jì)構(gòu)成的多傳感器姿態(tài)檢測(cè)模塊，提出一種基于卡爾曼濾波的自平衡車(chē)數(shù)據(jù)融合方法，設(shè)置固定的信任權(quán)重，將兩種傳感器的輸出數(shù)據(jù)進(jìn)行融合，同時(shí)濾除噪聲，以消除采用單個(gè)傳感器存在的誤差累積及提高車(chē)體角度測(cè)量的實(shí)時(shí)在線估計(jì)，得到最優(yōu)化的自平衡車(chē)姿態(tài)角度。

2 自平衡車(chē)運(yùn)動(dòng)特性模型分析

自平衡車(chē)以兩平行車(chē)輪為支撐，通過(guò)姿態(tài)檢測(cè)模塊測(cè)量車(chē)體傾角，經(jīng)控制器處理輸出控制信號(hào)，以此來(lái)驅(qū)動(dòng)兩輪電機(jī)維持車(chē)體平衡。在只考慮自平衡車(chē)靜止和直線行駛的情況下，其機(jī)械結(jié)構(gòu)類似于一級(jí)倒立擺模型,可以將其簡(jiǎn)化為倒立擺模型，在實(shí)際操作中車(chē)體姿態(tài)平衡時(shí)，其重心位置與豎直方向呈一定的傾角，但為簡(jiǎn)化模型分析假設(shè)車(chē)體平衡時(shí)重心位置位于垂直方向，自平衡車(chē)的結(jié)構(gòu)剖面圖和簡(jiǎn)化模型如圖1所示。

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

當(dāng)角度θ 較小時(shí)，正弦值與角度值基本相等，且因?yàn)殡姍C(jī)的驅(qū)動(dòng)力受車(chē)體傾角的影響，則平衡車(chē)的加速度與傾角存在一定的比例關(guān)系，假設(shè)系數(shù)為k,由此可將公式簡(jiǎn)化為：

圖1：自平衡車(chē)結(jié)構(gòu)剖面圖和簡(jiǎn)化模型

圖2：加速度敏感軸與重力加速度關(guān)系

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

自平衡車(chē)的姿態(tài)調(diào)整在保證測(cè)量角度精確的前提下，還需要確保恢復(fù)垂直狀態(tài)的迅速性，可添加有關(guān)角速度 的阻尼力：

由此可以看出，需要通過(guò)控制電機(jī)使車(chē)體的加速度維持在：

可見(jiàn)實(shí)時(shí)地進(jìn)行傾角測(cè)量，給予電機(jī)控制信號(hào)才能使車(chē)體維持平衡。

3 姿態(tài)檢測(cè)系統(tǒng)組成

3.1 加速度計(jì)測(cè)量原理與誤差分析

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

由上式可知，測(cè)得數(shù)據(jù)與傾角呈非線性正弦關(guān)系，可通過(guò)對(duì)輸出數(shù)據(jù)進(jìn)行反正弦變化得出車(chē)體的傾角值，但是當(dāng)平衡車(chē)運(yùn)動(dòng)時(shí)，車(chē)體所受加速度不再是單一的重力加速度還包括電機(jī)驅(qū)動(dòng)產(chǎn)生的加速度，同時(shí)存在著系統(tǒng)的振動(dòng)和機(jī)械噪聲，這些因素都會(huì)導(dǎo)致加速度計(jì)測(cè)量角度不精確，且加速度計(jì)動(dòng)態(tài)響應(yīng)較慢不適合跟蹤需要迅速應(yīng)變的動(dòng)態(tài)系統(tǒng)，所以需要與陀螺儀共同合作實(shí)現(xiàn)追蹤。

3.2 陀螺儀測(cè)量原理與誤差分析

陀螺儀主要用來(lái)測(cè)量車(chē)體的瞬間角速度，分x 軸、y 軸和z 軸分別可以用來(lái)檢測(cè)物體的俯仰角、翻滾角和偏航角。由于安裝在自平衡車(chē)上，陀螺儀的主要作用是測(cè)量車(chē)體的俯仰角變化的角速度，一般通過(guò)積分運(yùn)算得到車(chē)體的當(dāng)前角度值。在短時(shí)間內(nèi)，陀螺儀的測(cè)量精確度較高但是由于車(chē)體顛簸和機(jī)械振動(dòng)會(huì)造成陀螺儀測(cè)得的角速度信號(hào)出現(xiàn)微小漂移，使得積分出現(xiàn)誤差，最終形成誤差累積導(dǎo)致在長(zhǎng)時(shí)間測(cè)量時(shí)無(wú)法得到真實(shí)的車(chē)體傾角，這就需要加速計(jì)對(duì)其測(cè)量的角度進(jìn)行定期糾正。

4 基于卡爾曼濾波的數(shù)據(jù)融合方法設(shè)計(jì)

由于加速度計(jì)靜態(tài)性能優(yōu)良但動(dòng)態(tài)跟蹤能力不行，陀螺儀測(cè)量?jī)A角精確簡(jiǎn)便但是存在積分誤差累積，所以需要融合兩傳感器的輸出數(shù)據(jù)，得出最優(yōu)化的車(chē)體傾角，可通過(guò)卡爾曼濾波器實(shí)現(xiàn)數(shù)據(jù)融合過(guò)程。首先，在考慮到卡爾曼濾波器主要用于離散時(shí)間系統(tǒng)的前提下，由于車(chē)體傾角的角度可由角速度積分得出，可將車(chē)體的實(shí)際傾斜角度θ 與用加速度計(jì)觀測(cè)的陀螺儀測(cè)量角速度的常數(shù)偏差t 作為系統(tǒng)的狀態(tài)向量，可得如下?tīng)顟B(tài)與測(cè)量方程：

式中，X(k)為k 時(shí)刻自平衡車(chē)的狀態(tài)（陀螺儀測(cè)得的角度及其常數(shù)誤差），X(k-1)為k-1時(shí)刻自平衡車(chē)的狀態(tài)，U(k-1)為k-1 時(shí)刻陀螺儀測(cè)得的角速度，為k 時(shí)刻的陀螺儀測(cè)量過(guò)程中存在的噪聲，Z(k)為k 時(shí)刻加速度計(jì)經(jīng)反正弦計(jì)算后輸出的角度，為k 時(shí)刻的加速度計(jì)的測(cè)量過(guò)程中存在的干擾。

簡(jiǎn)化狀態(tài)方程與測(cè)量方程為：

其中，A 為系統(tǒng)的預(yù)測(cè)矩陣，B 為控制矩陣，H 為傳感器測(cè)量矩陣。

根據(jù)以上公式可知，每一個(gè)狀態(tài)變量更新到一個(gè)新的狀態(tài)時(shí)，僅需要上一個(gè)時(shí)刻參與估算且其仍服從高斯分布，此外，為完成對(duì)卡爾曼濾波器的校正得到最優(yōu)的角度值，需要求取k 時(shí)刻的協(xié)方差矩陣P(k)，而系統(tǒng)的過(guò)程噪聲協(xié)方差矩陣Q(k)和測(cè)量誤差協(xié)方差矩陣R(k)為P(k)的組成部分需進(jìn)行合理調(diào)整，如下所示：

k 時(shí)刻自平衡車(chē)系統(tǒng)的預(yù)測(cè)方程：

由預(yù)測(cè)狀態(tài)方程得出的系統(tǒng)協(xié)方差為：

經(jīng)傳感器測(cè)量數(shù)據(jù)修正的狀態(tài)方程：

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

式中，HT為H 的轉(zhuǎn)置矩陣。

k 時(shí)刻 的協(xié)方差：

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

4 效果分析

4.1 仿真分析

利用MATLAB 建立本文設(shè)計(jì)的卡爾曼濾波器smiulink 仿真模型，以對(duì)比濾波前后波形驗(yàn)證濾波效果，仿真模型如圖3所示。

通過(guò)對(duì)陀螺儀和加速度計(jì)信任權(quán)重的分配，以及測(cè)量噪聲協(xié)方差系數(shù)的調(diào)整，以達(dá)到理想的濾波效果。濾波前后波形對(duì)比如圖4所示。

4.2 實(shí)測(cè)應(yīng)用分析

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

上位機(jī)中顯示出兩條波形，棕色波形表示加速計(jì)z 軸加速度經(jīng)反正弦變化得出的數(shù)據(jù)波形，綠色波形是經(jīng)過(guò)卡爾曼濾波器進(jìn)行數(shù)據(jù)融合后得出的波形，可以看出加速度計(jì)的波形具有較多毛刺，這是因?yàn)榧铀俣扔?jì)能比較準(zhǔn)確地反映車(chē)體傾角的變化，而陀螺儀測(cè)得的角速度積分波形較平滑，但是存在一定的積分誤差，經(jīng)卡爾曼數(shù)據(jù)融合后的波形既具有較好的平滑性又能夠準(zhǔn)確跟蹤加速度的波形，且無(wú)超調(diào)和滯后，此時(shí)自平衡車(chē)姿態(tài)良好，從而驗(yàn)證該方法的正確性和有效性。

5 總結(jié)

圖4：濾波前后波形對(duì)比圖

圖5：加速度計(jì)與卡爾曼濾波輸出波形對(duì)比

本文針對(duì)加速度計(jì)和陀螺儀作為單一傳感器進(jìn)行角度測(cè)量的缺陷分析，采用多種傳感器進(jìn)行角度測(cè)量，利用卡爾曼濾波算法將輸出數(shù)據(jù)進(jìn)行融合濾波以得出最優(yōu)化的角度值，最后通過(guò)對(duì)實(shí)測(cè)數(shù)據(jù)波形分析實(shí)現(xiàn)對(duì)自平衡車(chē)姿態(tài)的理想調(diào)整，能夠使車(chē)體維持在一定傾角且前后偏移較小，說(shuō)明只要選擇合理的控制參數(shù)，就可以實(shí)現(xiàn)自平衡車(chē)體姿態(tài)的穩(wěn)定性。