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

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

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

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

中圖分類號(hào)：TP273 文獻(xiàn)標(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 引 言

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

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

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

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

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

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

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

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

ami=abi-gbi+vai（1）

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

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

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

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

假如單獨(dú)處理加速度計(jì)和陀螺儀的信號(hào)，并且小車只繞其Y軸轉(zhuǎn)動(dòng)，可用以下公式分別計(jì)算車身當(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）表明，單獨(dú)依靠加速度計(jì)的信息計(jì)算傾角時(shí)，但是由于加速度計(jì)的測量結(jié)果不可避免地受到車體加速度和傳感器噪聲的影響，使得endprint

abx、abz、vax和vaz

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

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

3 測量方法原理介紹

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

3.1 一階互補(bǔ)濾波

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

圖2 互補(bǔ)濾波原理方框圖

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

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

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

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

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

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

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

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

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

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

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

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

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

3.2 卡爾曼濾波

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

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

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

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

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

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

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

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

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

yk=Hkxk+vk

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

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ù)號(hào)分別表示后驗(yàn)估計(jì)和先驗(yàn)估計(jì)。通過重復(fù)公式（11）所示的遞推計(jì)算過程，即可實(shí)現(xiàn)對(duì)狀態(tài)參數(shù)的實(shí)時(shí)估計(jì)。

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

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

由于IMU傳感器固定在平衡車車體上，因此需要分析重力在車體坐標(biāo)系三個(gè)軸向上投影分量的變化。這里用歐拉角法對(duì)車體坐標(biāo)系相對(duì)世界坐標(biāo)系的轉(zhuǎn)動(dòng)進(jìn)行描述。其中，所述的世界坐標(biāo)系原點(diǎn)與車體坐標(biāo)系原點(diǎn)重合，其垂直軸Zw垂直于水平面，其縱軸Xw正方向?yàn)檐圀w坐標(biāo)系X在水平面上投影，Yw軸所在方向Xw與Zw方向向量的叉乘。用歐拉角描述的車系坐標(biāo)系相對(duì)世界坐標(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)系中的表達(dá)為[gx gy gz]T=ag。

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

將與公式（13）代入公式（3）中，并且令車體運(yùn)動(dòng)加速度和噪聲項(xiàng)為零，則可以判斷出車身的翻滾運(yùn)動(dòng)對(duì)角度的估計(jì)不會(huì)造成影響。

4 實(shí)驗(yàn)驗(yàn)證

4.1 實(shí)驗(yàn)平臺(tái)搭建

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