兩輪平衡車具有廣闊的應用前景， 使shi其qi成cheng為wei了le當dang前qian研yan究jiu的de熱re點dian。其qi中zhong，兩liang輪lun平ping衡heng車che的de姿zi態tai角jiao度du測ce量liang是shi研yan究jiu的de關guan鍵jian問wen題ti之zhi一yi。姿zi態tai角jiao度du測ce量liang是shi兩liang輪lun平ping衡heng車che運yun行xing和he控kong製zhi實shi現xian的de前qian提ti。姿zi態tai角jiao度du測ce量liang的de精jing度du和he速su度du
，將jiang直zhi接jie影ying響xiang兩liang輪lun平ping衡heng車che控kong製zhi算suan法fa的de穩wen定ding性xing和he可ke靠kao性xing。隨sui著zhe慣guan性xing測ce量liang元yuan件jian的de微wei型xing化hua與yu微wei處chu理li器qi運yun算suan能neng力li的de提ti高gao，兩liang輪lun平ping衡heng車che姿zi態tai測ce量liang普pu遍bian采cai用yong低di成cheng本ben的de慣guan性xing測ce量liang組zu合he元yuan件jian（Inertial Measurement Uint，IMU），結合微處理器數據處理算法實現高精度的姿態測量。IMU 主要由低成本的MEMS 陀螺儀和三軸加速度計組成。MEMS 陀螺儀有自主性好、功耗低、機電性能好易集成等優點。但是，MEMS 陀tuo螺luo儀yi具ju有you溫wen度du漂piao移yi特te性xing，其qi測ce量liang誤wu差cha會hui隨sui著zhe時shi間jian的de累lei加jia而er不bu斷duan的de累lei積ji，從cong而er影ying響xiang測ce量liang精jing度du。加jia速su度du計ji會hui受shou到dao平ping衡heng車che振zhen動dong的de影ying響xiang，混hun疊die額e外wai的de振zhen動dong量liang幹gan擾rao。所suo以yi單dan一yi的de傳chuan感gan器qi測ce量liang難nan以yi得de到dao精jing確que的de姿zi態tai角jiao度du。需xu采cai用yong多duo傳chuan感gan器qi信xin號hao融rong合he的de方fang法fa，來lai獲huo得de準zhun確que的de姿zi態tai角jiao度du量liang。

 

多傳感器數據的融合方法有神經網絡
、小波分析、卡爾曼濾波等姿態解算算法，但這些方法建立穩定可靠的更新方程通常具有較高的階數，且計算量大，不適合於低運算能力係統的實時計算。相比以上方法，互補濾波算法對處理器運算速度要求不高，且簡單可靠。本(ben)文(wen)基(ji)於(yu)互(hu)補(bu)濾(lv)波(bo)算(suan)法(fa)，設(she)計(ji)了(le)兩(liang)輪(lun)平(ping)衡(heng)車(che)姿(zi)態(tai)角(jiao)度(du)測(ce)量(liang)電(dian)路(lu)與(yu)數(shu)據(ju)處(chu)理(li)算(suan)法(fa)，設(she)計(ji)了(le)信(xin)號(hao)濾(lv)波(bo)預(yu)處(chu)理(li)，利(li)用(yong)互(hu)補(bu)濾(lv)波(bo)算(suan)法(fa)融(rong)合(he)兩(liang)種(zhong)傳(chuan)感(gan)器(qi)數(shu)據(ju)，分(fen)析(xi)了(le)互(hu)補(bu)濾(lv)波(bo)算(suan)法(fa)中(zhong)關(guan)鍵(jian)參(can)數(shu)的(de)計(ji)算(suan)方(fang)法(fa)。並(bing)將(jiang)此(ci)方(fang)法(fa)應(ying)用(yong)於(yu)兩(liang)輪(lun)平(ping)衡(heng)車(che)角(jiao)度(du)測(ce)量(liang)
，進(jin)行(xing)了(le)驗(yan)證(zheng)性(xing)試(shi)驗(yan)，給(gei)出(chu)了(le)實(shi)驗(yan)測(ce)試(shi)數(shu)據(ju)。

 

 

沿平衡車3 個機體軸即直立時正前、正右、正上方向定義為x、y
、z 三軸參考坐標係。所受的3 軸重力加速度分量定義為gx、gy
、gz。假設兩輪平衡車處於靜止或勻速運 行的狀態。得到重力加速度與平衡車姿態角度的關係如式1所示：

其中，Cbn為慣性坐標係到載體坐標係的變換矩陣
；θ 為俯仰角
；φ為橫滾角；g 為重力加速度； 可以通過測量重力加速度分量gx、gy
、gz，計算出平衡車俯仰角θ1和橫滾角φ1估計值

若使用陀螺儀來測量平衡車姿態角度，設陀螺儀測量載體相對慣性坐標係的x
、y
、z 三軸旋轉角速度分別為ωx、ωy
、 ωz。並定義0 時刻平衡車直立靜止。可得到俯仰角θ2和橫滾角φ2估計值與ωx、ωy之間的關係如式3 所示:

在實際應用中，由於平衡車機體運行時存在運動加速度、測量噪聲， 以及陀螺儀本身存在漂移等因素的影響， 式（2）、（3）姿態角度測量方法失效，為了準確的獲得姿態角度。可將以上的2 種姿態角度測量得到的姿態角度信息相融合。

 

 

該係統中慣性組合測量電路如圖1 所示
， 由加速度計MMA7361、陀螺儀ENC-03 及放大電路組成。實現對加速度計和陀螺儀測得信號進行放大。加速度計和陀螺儀信號經放大

，分別由angle 引腳和gyro 引腳輸出後
，信號通過AD 采樣轉換為數字信號，傳遞到微控製器中，再利用互補濾波算法，得到姿態角度。

圖1 慣性組合角度測量電路圖

 

MEMS 陀螺儀的漂移誤差由常值漂移
、隨機漂移組成，漂移信號不符合平穩

、正態、零均值的時間序列特征。圖2 為陀螺儀采集到的一段數據
， 對其積分後得到的俯仰角θ2，表現為誤差不斷累加，逐漸增大。通過式（4）所示算法進行均值扣除。

圖2 角速度與角度的曲線

 

去掉陀螺儀信號的均值（即常值分量）後，這樣陀螺儀的漂移隻含有隨機漂移，此時的陀螺儀信號將作為互補濾波融合的輸入。

 

加速度計受機體振動的影響
，對其采用滑動均值濾波的方法對加速度傳感器原始數據進行處理，濾波原理如式（5）所示：

圖3 濾波前後加速度計數據圖

 

陀螺儀動態響應特性優良，解算姿態角時，由於陀螺儀低頻漂移的影響，積分後低頻擾動會產生較大誤差


；加速度計解算的姿態角會受到平衡車運行中機體高頻振動的影響， 輸shu出chu角jiao度du中zhong攜xie帶dai較jiao大da分fen量liang的de高gao頻pin幹gan擾rao。二er者zhe在zai頻pin域yu上shang具ju有you互hu補bu特te性xing，采cai用yong互hu補bu濾lv波bo器qi對dui這zhe兩liang種zhong傳chuan感gan器qi數shu據ju融rong合he
，可ke提ti高gao姿zi態tai角jiao度du測ce量liang的de精jing度du和he動dong態tai響xiang應ying的de性xing能neng。

 

互補濾波器的基本原理圖如圖4 所示。

圖4 互補濾波器的原理圖

 

其中θ 為實際的角度值
，ω 為陀螺儀測量的角速度
， 互補濾波算法後估計的角度值為互補濾波算法後估計的角度值
， 加速度計測量中引入的高頻噪聲n1
，陀螺儀測量中引入的低頻噪聲n2
，用低通濾波器G1（s）消除加速度計中的高頻噪聲n1，用高通濾波器G2（s）消除陀螺儀中的低頻噪聲n2。兩個濾波器的傳遞函數， 被設計為（6）、（7）所示
，圖4 結構可化簡為圖5 結構。

圖5 互補濾波器頻域圖

 

選用的濾波傳遞函數需滿足G1（s）+G2（s）=1，由加速度計得到θ1經低通濾波器和陀螺儀得到θ2經高通濾波器後的數據融合為角度估計值互補濾波算法後估計的角度值，適當的選取權重因子K 值，可以使係統中高、低通濾波器具有合適的截止率。得到穩定的姿態角度。

由（8）式可看出，在小於截止頻率的低頻段，加速度計對姿態解算結果起主要作用
；在大於截止頻率的高頻段
，陀螺儀對姿態解算結果起主要作用。通過調整時間常數，改變濾波器的截止頻率τ，實現對陀螺儀和加速度計權重的調整。

 

為了驗證上述設計方案的可行性

， 利用直立兩輪小車為實驗驗證平台。本實驗使用慣性測量組合元件（IMU）中的慣性傳感器分別選用了ENC-03（測量範圍：±300（deg/s））陀螺儀，采樣頻率為1.25 kHz 和MMA7361 加速度計（測量範圍：±1.5 g）。角度更新頻率為1.25 kHz。互補濾波器截止頻率為138 Hz。以俯仰角（θ）為例，進行了測試。

 

 

把陀螺儀測得的角速度數據和加速度計測得的角度數據通過（10）式進行融合後的波形如圖6 所示，從圖6 中可以看出， 經互補濾波算法融合後得到角度消除了陀螺儀的漂移和加速度計的高頻擾動
，可得以下結論：

 

1）單從陀螺儀獲取的角速度積分後得到的角度是不正確的， 要把加速度計測得的角度值和陀螺儀測得角速度積分後的角度進行互補濾波算法融合，提高角度精度。

 

2）經互補濾波後陀螺儀的隨機漂移得到較為明顯的抑製，表現出了此互補濾波算法的有效性和優越性。

圖6 角速度與角度的融合曲線圖

 

 

經互補濾波處理後的角度與直接由加速度計測得的角度時域比較，對其互補濾波處理前後的信號進行FFT 變換其頻譜圖如圖7 所示， 通過互補濾波算法可以降低隨機噪聲的幹擾
，可以使測得角度的波形更加的平滑。

圖7 融合前後角度比較圖

 

 

文wen中zhong分fen析xi了le兩liang輪lun平ping衡heng車che姿zi態tai角jiao度du解jie算suan時shi陀tuo螺luo儀yi漂piao移yi和he加jia速su度du計ji高gao頻pin擾rao動dong的de影ying響xiang，針zhen對dui陀tuo螺luo儀yi漂piao移yi和he加jia速su度du計ji高gao頻pin擾rao動dong采cai用yong互hu補bu濾lv波bo融rong合he加jia速su度du計ji和he陀tuo螺luo儀yi信xin號hao。互hu補bu濾lv波bo能neng有you效xiao消xiao除chu陀tuo螺luo儀yi的de漂piao移yi，抑yi製zhi加jia速su度du計ji的de高gao頻pin擾rao動dong，減jian少shao輸shu出chu姿zi態tai角jiao的de動dong態tai誤wu差cha，提ti高gao了le角jiao度du測ce量liang精jing度du，能neng夠gou滿man足zu兩liang輪lun平ping衡heng車che的de姿zi態tai控kong製zhi需xu要yao。實shi驗yan結jie果guo表biao明ming了le該gai方fang法fa的de有you效xiao性xing，可ke推tui廣guang應ying用yong於yu車che載zai導dao航hang、兩輪平衡車、微小型機器人的姿態角度測量係統。