基于信號融合的自平衡車姿態(tài)測量方法研究

吳學(xué)勤+許耀華+李娟娟+王建鋒+劉新雨

摘 要：針對自平衡車姿態(tài)角度測量問題，本文敘述了基于卡爾曼濾波和互補濾波的自平衡車MEMS IMU單軸融合算法的原理，分析了平衡車車身翻滾運動對融合算法計算結(jié)果的影響，最后分析了卡爾曼濾波法和互補濾波法的動態(tài)和靜態(tài)收斂速度。為了驗證各算法的效果，本文搭建了基于飛思卡爾K60單片機的信號采集平臺進行實驗。實驗表明在采用低成本MEMS IMU的自平衡車俯仰角計算中，兩種融合算法的效果接近，但是互補濾波法在靜態(tài)時的收斂速度較快，同時考慮計算時效性，采用基于互補濾波的單軸融合算法較為合適。

關(guān)鍵字：自平衡車；信號融合；卡爾曼濾波；互補濾波；姿態(tài)角

中圖分類號：TP273 文獻標(biāo)志碼：A

Research on Signal-fusion Based Attitude Angle Measurement of Self-balanced Car

WU Xue-qin1，XU Yao-hua1，LI Juan-juan1，WANG Jian-feng2，LIU Xin-yu1

（1.School of Automobile，Changan University，Xian，Shaanxi 710064；

2.Road Traffic Intelligence Detection and Engineering Technology Research Center of Shaanxi （Changan University），Xian，Shaanxi 710064，China）

Abstract：The principle of algorithms based on Kalman filtering and complementary filtering are presented to measure the attitude angle of a self-balance car.The impact of rolling motion of car is evaluated.The speed of convergence of Kalman filtering and complementary filtering is analyzed.A K60 microcontroller based experimental platform is developed to validate the efficiency of algorithms.Result of experiment shows that the performance of the two signal-fusion algorithms are approaching.But after considering the computational efficiency，we can conclude that the complementary filtering based algorithm is more suitable.

Key words：balance-car；signal fusion；Kalman filter；complementary filter；attitude angle

1 引 言

二輪直立式自平衡車具有體積、重量小，運動靈活的特點，在個人交通領(lǐng)域有重大的市場。二輪直立式自平衡車的結(jié)構(gòu)與倒立擺原理相似，其重心位于車輪支點上方。自平衡車本身無法實現(xiàn)平衡，必須依靠電機不斷為其提供修正力矩來保持車輛的直立。為了實現(xiàn)該目的，自平衡車上的控制單元必須實時地獲取車輛當(dāng)前的車輛姿態(tài)參數(shù)，包括車輛的俯仰角、俯仰角速度等信息[1]。

一般情況下，自平衡車依靠所安裝的IMU來測量車身的俯仰角和俯仰角速度。但是由于傳感器本身以及安裝位置等原因，由這些傳感器直接采集到的信號包含有噪聲。

目前有多種算法可以實現(xiàn)對陀螺儀和加速度計信號進行數(shù)據(jù)融合，從而濾除角度計算結(jié)果中的噪聲，比較常用的方法有互補濾波法和卡爾曼濾波法[2-3]。本文對這兩種測量方法進行對比，介紹各方法的原理，并用分析其在平衡車姿態(tài)角測量中的效果。

2 姿態(tài)角測量誤差分析

在工程測量中，傳感器的由于受到外界和自身干擾因素的影響，其輸出信號必然包含各種噪聲。

目前，自平衡車上普遍采用六軸MEMS IMU，包括以三個相互垂直的方向為敏感軸的三軸加速度計和對圍繞這三軸的旋轉(zhuǎn)運動敏感的三軸陀螺儀[4]。為了之后分析各數(shù)據(jù)融合方法的效果，這里先分析傳感器的數(shù)學(xué)模型。

平衡車的車身坐標(biāo)系及傾角示如圖1.1和圖1.2所示。

對于三軸加速度計信號，其數(shù)學(xué)描述為：

ami=abi-gbi+vai（1）

式中，ami軸加速度計中沿X、Y或Z軸方向的加速度信號輸出，abi沿該軸方向的加速度分量，gbi力加速度在該軸的投影，vai噪聲。

對于三軸陀螺儀，其數(shù)學(xué)描述為：

ωmi=ωti+boi+bi+ωai（2）

式中，ωmi為陀螺儀所測角速率，ωti為角速率真值，boi為角速率常漂值，bi為時變漂移量，ωai為白噪聲。

假如單獨處理加速度計和陀螺儀的信號，并且小車只繞其Y軸轉(zhuǎn)動，可用以下公式分別計算車身當(dāng)前的傾角

θa（t）=arctanamxamz=arctanabx-gbx+vaxabz-gbz+vaz（3）

θg（t）=∫t0ωmy（t）dt+θg（0）=∫t0ωty（t）dt

+boyt+∫t0by（t）dt+θg（0）+e（4）

公式（3）表明，單獨依靠加速度計的信息計算傾角時，但是由于加速度計的測量結(jié)果不可避免地受到車體加速度和傳感器噪聲的影響，使得endprint

abx、abz、vax和vaz

不為零，因此其計算結(jié)果將偏離真值；

公式（4）表明，單獨依靠陀螺儀計算角度時，其計算結(jié)果將包含常漂量b0和時變漂移b的一次項，使得積分誤差將隨積分時間而累積，最終使得結(jié)果失真。值得注意的是，從頻域角度上看，陀螺儀的計算結(jié)果經(jīng)過積分后，相當(dāng)于進行了低通濾波，因此白噪聲ωai對角度測量誤差的影響較小，此時計算結(jié)果的誤差主要集中在低頻段。

3 測量方法原理介紹

以上分析表明，單獨依靠加速度計或陀螺儀信號測量自平衡車的姿態(tài)角是不可靠的。需要采用融合的方法進行處理，以得到較好的結(jié)果。

3.1 一階互補濾波

互補濾波通過對加速度計信號進行低通濾波，降低其高頻噪聲，對陀螺儀信號進行高通濾波，降低其低頻噪聲，最后加權(quán)融合的方法，實現(xiàn)兩種傳感器信息的融合[5]。一階互補濾波的傳感器信息融合原理如圖2所示。

圖2 互補濾波原理方框圖

將公式（3）和（4）計算結(jié)果的成分簡化為如下形式

θa（t）=θt（t）+na（t）（5）

θg（t）=θt（t）+ng（t）（6）

則根據(jù)圖2所示的原理，平衡車傾角估計值t的拉普拉斯變換結(jié)果為

t（s）=1τs+1θ（s）+τsτs+1θ（s）

+1τs+1Na（s）+τsτs+1Ng（s）（7）

式中，τ為濾波器時間常數(shù)，τ的取值取決于傳感器的噪聲分布特性。

忽略噪聲項，將公式（7）轉(zhuǎn)為微分方程，得到

（t）+τd（t）dt=θ（t）+τdθ（t）dt（8）

為了得到公式（8）的離散形式，便于在單片機中處理，令dθ（t）dt=θ（k）-θ（k-1）T，d（t）dt=（k）-（k-1）T，代入公式（8）得到一階互補濾波的離散形式

（k）=τT+τ（k-1）+TT+τθ（k）

+τT+τ[θ（k）-θ（k-1）]（9）

式中，T為傳感器信號更新的時間間隔。代入公式（3）和（4），用ωm（k）替換θ（k）-θ（k-1），用θa（k）替換θ（k），就能得到用于信息融合的計算公式。在實際計算中，τT+τ的值可通過實驗調(diào)整得到。將τT+τ替換為一個常系數(shù)k，然后對其進行修改，并觀察角度曲線的變化，以此確定最佳濾波參數(shù)。

3.2 卡爾曼濾波

卡爾曼濾波通過實時更新均值和協(xié)方差矩陣執(zhí)行濾波過程，是一種基于模型的線性濾波器[6]。本文所采用的卡爾曼濾波具體計算過程如下：

系統(tǒng)所對應(yīng)的狀態(tài)方程和觀測方程為

xk=Fxk-1+Guk-1+wk-1（10）

公式（10）中，ωb為陀螺儀角速率的漂移量，對應(yīng)于公式（2）中的b0、b兩項。由于b項為時變項，因此ωb亦為時變項。通過添加一定的狀態(tài)方程噪聲，可以使算法跟蹤=該參數(shù)的變化 [7]。

系統(tǒng)所對應(yīng)的狀態(tài)方程為

wR=[n 0 a]=[θa]k=[1 0]θωbk+vk（11）

在公式（11）中，系統(tǒng)以加速度傳感器對應(yīng)的車身傾角計算值θa作為觀測量。

將公式（10）和（11）寫為矩陣代數(shù)形式

xk=Fxk-1+Guk-1+wk-1

yk=Hkxk+vk

則離散卡爾曼濾波器的遞推過程每一步的計算公式為：

P-k=Fk-1P+k-1FTk-1+Qk-1（11a）

Kk=P-KHTk（HkP-kHTk+Rk）-1（11b）

=Fk-1+k-1+Gk-1uk-1（11c）

+k=-k+Kk（yk-Hk-k）（11d）

P+k=（1-KkHk）P+k（I-KkHk）T+KkRkKTk（11f）

式中，Qk=E（wkwTk），Rk=E（vkvTk），Pk為狀態(tài)向量的協(xié)方差矩陣，上標(biāo)的正負號分別表示后驗估計和先驗估計。通過重復(fù)公式（11）所示的遞推計算過程，即可實現(xiàn)對狀態(tài)參數(shù)的實時估計。

3.3 IMU單軸融合算法可靠性分析

自平衡車載地面上行駛時，由于地面不可能完全水平，因此平衡車的車身難免存在繞X軸的翻滾運動。這種翻滾會照成重力加速度在三軸投影分量的變化，造成在公式（3）的計算結(jié)果中包含這一擾動量。以下對該運動對IMU單軸融合算法計算結(jié)果的影響進行分析。

由于IMU傳感器固定在平衡車車體上，因此需要分析重力在車體坐標(biāo)系三個軸向上投影分量的變化。這里用歐拉角法對車體坐標(biāo)系相對世界坐標(biāo)系的轉(zhuǎn)動進行描述。其中，所述的世界坐標(biāo)系原點與車體坐標(biāo)系原點重合，其垂直軸Zw垂直于水平面，其縱軸Xw正方向為車體坐標(biāo)系X在水平面上投影，Yw軸所在方向Xw與Zw方向向量的叉乘。用歐拉角描述的車系坐標(biāo)系相對世界坐標(biāo)系的旋轉(zhuǎn)矩陣為[8]

在該矩陣中，向量n、o和a分別表示導(dǎo)航坐標(biāo)系Xw、Yw、Zw軸的方向向量在車體坐標(biāo)系中的投影[8]，為翻滾角，θ為俯仰角，ψ為偏航角。由于重力加速度在導(dǎo)航坐標(biāo)系中的向量與導(dǎo)航坐標(biāo)系的Zn軸重合，因此重力加速度在車體坐標(biāo)系中的表達為[gx gy gz]T=ag。

令偏航角ψ=0，得到重力加速度在車體坐標(biāo)系X軸和Z軸向上的投影為

將與公式（13）代入公式（3）中，并且令車體運動加速度和噪聲項為零，則可以判斷出車身的翻滾運動對角度的估計不會造成影響。

4 實驗驗證

4.1 實驗平臺搭建

本文采用的IMU傳感器型號為MPU6050，由IvenSense公司推出，上面集成了三軸角速度機、三軸陀螺儀、溫度傳感器和一個數(shù)字運動處理模塊（Digital Motion Processor，DMP），并通過I2C總線直接輸出傳感器的數(shù)字信號。本文所搭建的實驗平臺結(jié)構(gòu)框圖如圖3所示。endprint