孫泰祎，王子蘭，王 彥，朱延正

(山東科技大學 電氣與自動化工程學院,山東 青島 266590)

互補濾波技術在自平衡機器人中的應用

孫泰祎，王子蘭，王 彥，朱延正

(山東科技大學 電氣與自動化工程學院,山東 青島 266590)

首先在理論上建立了自平衡機器人的動力學模型?；赑D控制算法，開發(fā)保持自平衡機器人直立的控制算法，并設計相應的控制器。利用模擬量輸出的陀螺儀ENC03和加速度計MMA7361兩個傳感器分別測量角速度和角加速度，再采用互補濾波技術實時獲得自平衡機器人的角度。在實驗部分，自平衡機器人的機械結構使用普通小功率直流電機，驅動芯片使用BTS7960大電流半橋芯片，以ColdFrie MCF52255芯片作為控制器，采用藍牙模塊進行數(shù)據(jù)傳輸。將編寫好的程序燒寫到32位飛思卡爾單片機中，實現(xiàn)對自平衡機器人的控制，通過實時采集數(shù)據(jù)驗證所開發(fā)的程序和算法的合理性。實驗結果驗證了所設計自平衡機器人的機械結構與硬件電路的合理性。

自平衡機器人；PD控制算法；互補濾波

在現(xiàn)代化生產(chǎn)過程中，機器人的作用越來越顯著，機器人發(fā)展水平的提升甚至可以代表一個國家綜合國力的提升。隨著科技的進步，移動機器人技術也逐步發(fā)展起來。利用反饋的觀點建立了兩輪自平衡機器人的動力學模型，國內外目前已經(jīng)有大量的研究成果[1-5]。文獻[1]利用可控性和可線性化得出該系統(tǒng)的強可檢測性的條件和最大自由度，并且根據(jù)推導方程結果設計了兩種特殊的控制器。仿真結果證實了設計兩種控制器的有效性。文獻[2]利用卡爾曼可控性的判別條件，設計線性化的機器人動力學模型，利用該模型設計一個兩輪移動機器人，并且能夠控制該機器人沿著預定軌道行走，該機器人有成本低、重量輕、效率高的優(yōu)點。文獻[3]采用Skinner操作條件反射理論實現(xiàn)對行為決策的優(yōu)化，解決兩輪機器人的運動平衡控制問題，該操作條件反射學習機制具有較快的自主平衡控制技能和較好的魯棒性能。本研究采用牛頓力學的分析方法構建兩輪自平衡機器人的動力學模型，設計控制算法，利用互補濾波技術獲得了小車實時姿態(tài)，以飛思卡爾公司生產(chǎn)的32位冷火系列芯片MCF52255作為核心控制芯片，然后利用PD控制算法控制小車實現(xiàn)自平衡，最后利用陀螺儀和加速度計等模塊搭建自平衡機器人的機械結構，實驗驗證了模型和算法的正確性。

1 自平衡機器人的動力學模型

自平衡機器人的動力學模型類似于一個底部可以運動的一級倒立擺模型[1-5]。該倒立擺模型具有低成本、高效率、強靈活性、非線性、高階次、強耦合、多變量、受環(huán)境影響大等特點。自平衡機器人在無外力作用下不能保持直立狀態(tài)，若要保持直立狀態(tài)，必須使用雙輪差速直流電機，在機器人預運動方向施加相同方向的加速度，如圖1所示。

圖1 自平衡機器人保持平衡原理圖Fig.1 Schematic diagram of keeping balance of the self-balancing robot

圖2 自平衡機器人受力分析圖Fig.2 Stress analysis of the self-balancing robot

下面對自平衡機器人進行數(shù)學建模。假設該倒立擺模型的重力為mg(其中g為重力加速度)，車模重心的高度為L,干擾加速度為u(t)，如圖2所示進行受力分析。倒立擺受到外力干擾而出現(xiàn)傾斜，當沒有產(chǎn)生加速運動時,即a(t)時的運動方程為

(1)

當θ很小的時候cosθ≈1，sinθ≈θ。可以得到簡化的運動方程

(2)

當產(chǎn)生加速運動時，可以得到倒立擺傾角θ(t)、倒立擺角速度ω(t)、倒立擺底部運動加速度a(t)和干擾加速度u(t)之間的運動方程

(3)

當θ很小時，cosθ≈1，sinθ≈θ?？梢缘玫胶喕牡沽[的運動方程

(4)

利用式(2)可以得到以系統(tǒng)角度θ作為輸出，干擾加速度u(t)作為輸入的傳遞函數(shù)

(5)

(6)

(7)

(8)

由上述公式和條件kd>0，kp>g可以得出該系統(tǒng)是可控的：

(9)

2 自平衡機器人中的互補濾波技術

在實際運行過程中，倒立擺本身的擺動所產(chǎn)生的加速度會產(chǎn)生很強的干擾信號，疊加之后的信號無法反映倒立擺的傾角。相反，陀螺儀傳感器輸出機器人的角速度信號，并且該信號的噪聲比較小，一般不會受車體運動的影響。角速度積分，可以獲得機器人的角度信號，所以陀螺儀傳感器可以進一步平滑角度信號。另一方面，角速度信號經(jīng)過積分運算，會產(chǎn)生微小的偏差和位移，并且會形成積累誤差，積累誤差會導致電路飽和，要想消除積累誤差，就必須通過加速度傳感器進行校正。

因此，要想獲得倒立擺模型的實時角度θ(t)的精確值，就需要利用互補濾波技術來融合陀螺儀和加速度計的輸出，將加速度產(chǎn)生的干擾信號濾除。

圖3 互補濾波算法結構圖Fig.3 Structure chart of the complementary filtering algorithm

圖3是互補濾波算法的算法框圖。首先利用陀螺儀傳感器和加速度計傳感器分別檢測自平衡機器人的角速度和角度，陀螺儀測得的角速度信號Ugyro與加速度計測得的角度信號Uacce和電壓輸出信號成比例關系。電壓輸出信號連接到單片機的AD(anglog digit)采集模塊，經(jīng)過AD采集，將自平衡機器人的角速度信號和角度信號轉化成數(shù)字量的角速度信號Cargyro和角度信號Caracce，便于控制器處理和控制。

經(jīng)過上述操作，得到了自平衡機器人實時的角速度信號Cargyro和角度信號Caracce。這兩個信號再減去陀螺儀的零點Zgyro和加速度計的零點Zacce，即可得到角速度偏移量 ΔCargyro和角度偏移量 ΔCaracce。兩個信號經(jīng)過兩個比例系數(shù)Pgyro和Pacce的放大,可以得到實時的角速度信號ω和角度信號θ′。

加速度計得到的角度值θ′(k)與上一時刻的θ(k-1)進行比較，得到的偏差經(jīng)過比例系數(shù)1/Tz的放大，再與陀螺儀輸出的角速度信號疊加之后進行積分，可以得到當前時刻的θ(k)。根據(jù)圖3的互補濾波算法，利用陀螺儀傳感器信號和加速度計傳感器信號的融合波形，可以得到倒立擺模型實時的角度輸出信號，經(jīng)過比例微分控制，以及死區(qū)處理和飽和處理可以得到驅動直流電機的PWM(pulse-width modulation)信號。再經(jīng)過電機驅動模塊，驅動左右兩個直流電機，完成自平衡機器人的姿態(tài)調整。

圖4 系統(tǒng)結構示意圖Fig.4 Schematic diagram of system structure

3 自平衡機器人的硬件設計

為了保證機器人能夠穩(wěn)定地保持自平衡，需要精心設計該系統(tǒng)的硬件部分，主要包括：控制模塊、模擬陀螺儀加速度計模塊、電源模塊、雙電機驅動模塊、藍牙數(shù)據(jù)傳輸模塊和小功率直流電機模塊等。機械結構設計方面，首先在自平衡機器人背面添加兩塊海綿保護，一旦發(fā)生事故可以盡最大的可能保護小車各個模塊不因撞擊而損壞。由于驅動板比較薄，而且小車后邊有兩塊海綿墊，所以將驅動模塊放到小車的后邊。安裝穩(wěn)壓模塊時，用兩個人工鉆孔固定穩(wěn)壓模塊。由于主控芯片需要不斷接各個引腳，因此，小車的主控芯片暫時懸空。為了降低小車的重心，將電池盡量往下放。圖4是機器人系統(tǒng)結構示意圖。

3.1 控制模塊

控制模塊的選擇是飛思卡爾公司生產(chǎn)的冷火系列芯片MCF522255。MCF52255 和MCF52259這兩個單片機型號都采用 Coldfire V2 內核。MCF52259 比 MCF52255 多了一個外部總線接口，這就多了44個引腳。外部總線接口是用來擴展外部存儲器(RAM或ROM)或外設的，由于 MCF5225X系列單片機片內已經(jīng)集成了足夠的存儲器模塊和豐富強大的外設模塊，很少會使用總線擴展外設，對于完成平衡任務的機器人來說就更不用擴展了。因此，選擇MCF522255作為本次實驗設計的主控芯片。 MCF522255芯片具有ColdFrie V2 32位內核， 80 MHz主頻，4通道16位脈沖調制模塊(PWM)，3個異步串口模塊(UART)，片內512 K Flash ROM和64 K SRAM，2個16位中斷定時器(PIT)，8通道12位模擬數(shù)字轉換(ADC)， 57個外設中斷源和10引腳的BWM調試方式?；谝陨现饕匦?，可以完成控制任務。

3.2 模擬陀螺儀加速度計模塊

該模塊采用日本村田公司生產(chǎn)的ENC-03陀螺儀作為單軸角速度傳感器，利用3軸加速度傳感器MMA7361來檢測Z軸的加速度。使用該模塊時，需注意陀螺儀的水平安裝。

3.3 電源模塊

電源模塊包括電池、保險絲、開關和穩(wěn)壓模塊。首先使用飛思卡爾智能車比賽的專用電池，供電上限是8.2 V。穩(wěn)壓芯片選擇的是AMS1117和LM2941S，該穩(wěn)壓芯片輸出3.3 V，供給主控芯片、電機驅動模塊和陀螺儀加速度計模塊。采用LM2941S經(jīng)過可調電阻器穩(wěn)壓輸出5 V電壓，主要供給藍牙模塊。這兩款芯片電路設計簡單，輸出紋波小。

3.4 雙電機驅動模塊

電機驅動芯片選用的是BTN7960芯片，是一款針對電機驅動應用的完全集成的大電流半橋芯片。該芯片能將電磁干擾減至最小，并且與微控制器的連接簡單。驅動集成電路還具有電流檢測診斷、轉換率調整、死區(qū)時間生成以及過熱、過壓、欠壓、過流和短路保護等功能。由于直立車的兩個電機都需正反向控制，所以使用4片BTN7960芯片，兩兩組成一個H橋，分別驅動左右電機。此芯片開關頻率可以達到25 kHz，可以很好地解決電機噪聲大和發(fā)熱的問題，同時驅動能力有了明顯的提高，響應速度快。電機驅動模塊原理如圖5所示。

3.5 藍牙模塊

雖然無線串口已經(jīng)流行了很多年，但仍存在許多問題，比如不能全雙工通信、數(shù)據(jù)量大的時候容易丟數(shù)據(jù)，最高波特率僅115 200 bps等等，并且許多無線串口價格不菲。藍牙串口和無線串口實現(xiàn)的效果是一樣的，都可以實現(xiàn)串口數(shù)據(jù)的無線傳輸，而且藍牙串口有很多優(yōu)點，比如最高波特率可以到達 1 382 400 bps，是115 200 bps 的10倍多，保證了電腦發(fā)送數(shù)據(jù)時不會丟數(shù)據(jù)。其次，藍牙串口是全雙工的，MCU (microprogrammed control unit)高速發(fā)送數(shù)據(jù)到電腦時，電腦依然可以正常發(fā)送數(shù)據(jù)到 MCU。另外，藍牙串口無需像 MX232、PL2303 之類的電平轉換模塊。最后，藍牙串口的成本低?；谝陨蟽?yōu)點，使用藍牙串口通信模塊，來傳輸陀螺儀和加速度計的波形，以便進行波形擬合。

圖5 電機驅動原理圖Fig.5 Schematic diagram of motor drive

4 利用互補濾波技術調試機器人

根據(jù)圖3的互補濾波算法結構圖可知，需要整定的參數(shù)有Zacce、Zgyro、TZ、Tσ、Pacce、Pgyro、Kp、Kd。經(jīng)過參數(shù)調節(jié)，最終確定參數(shù)各為Zacce=1 400、Zgyro=1 820、TZ=1、Tσ=0.007 5、Pacce=0.75、Pgyro=1。整定后可以得到如圖6所示的曲線2，該曲線是實際自平衡機器人經(jīng)過參數(shù)整定之后得到的角度跟蹤曲線。由圖6可知，經(jīng)過互補濾波之后，角度跟蹤情況比由單個加速度計獲得的波形 (圖5中曲線3)平滑了很多。

圖6 互補濾波波形圖Fig.6 Waveform graph of the complementary filtering

經(jīng)過初步參數(shù)調節(jié)，確定了互補濾波的6個參數(shù)，為了實現(xiàn)機器人自平衡,還需要進一步調節(jié)Kp和Kd來實現(xiàn)角度控制，整定這兩個參數(shù)需要從理論上分析這兩個參數(shù)的作用。比例參數(shù)Kp相當于自平衡機器人的恢復力，滿足Kp>g才能實現(xiàn)小車的初步直立。但是Kp不能無限制的增大，否則會使倒立擺來回擺動。微分參數(shù)Kd相當于倒立擺的阻尼力，該阻尼力會消除倒立擺的擺動，但是該參數(shù)過大會出現(xiàn)震顫現(xiàn)象。調節(jié)這兩個參數(shù)應該采取先比例后微分的順序。最終，確定出兩個參數(shù)的值分別為：Kp= 0.591 25、Kd=0.27。經(jīng)過參數(shù)整定，自平衡機器人能夠檢測實時的角度變化信息，并且進行決策，輸出PWM波，改變電機的轉速，使其在試驗平臺上保持直立。實驗結果驗證了所設計的自平衡機器人的機械結構與硬件電路的合理性。

5 結論

建立了自平衡機器人的動力學模型，將互補濾波算法應用到自平衡機器人中，設計了硬件電路并進行調試。實驗結果表明，通過調節(jié)參數(shù)，能夠實現(xiàn)機器人的自平衡。但是本算法僅僅完成了自平衡機器人的直立，如果再加一個攝像頭和一個豎直方向的陀螺儀，改進一下算法，還能使機器人在保持直立的狀態(tài)下躲避障礙物，完成轉向運動。

[1]PATHAK K,FRANCH J,AGRAWAL S K.Velocity and position control of a wheeled inverted pendulum by partial feedback linearization[J].IEEE Transactions on Robotics,2005,21(3):505-513.

[2]SALERNO A,ANGELES J.On the nonlinear controllability of a quasiholonomic mobile robot[C]//IEEE International Conference on Robotics and Automation,2003：3379-3384.

[3]任紅格,阮曉鋼.基于Skinner操作條件反射的兩輪機器人自平衡控制[J].控制理論與應用,2010,27(10):1423-1428. REN Hongge,RUAN Xiaogang.Self-balance control of two-wheeled robot based on Skinner’s operant conditioned reflex[J].Control Theory and Applications,2010,27(10):1423-1428.

[4]HAY S.Trajectory tracking control for navigation of the inverse pendulum type self-contained mobile robot[J].Robotics and Autonomous Systems，1996,17(1):65-80.

[5]KIM Y,KIM S H,KWAK Y K.Dynamic analysis of a nonholonomic two-wheeled inverted pendulum robot[J].Journal of Intelligent and Robotic Systems,2005,44(1):25-46.

[6]BLANKESPOOR A,ROBERT R.Experimental verification of the dynamic model for a quarter size self-balancing wheelchair[C]//American Control Conference,Proceedings of the IEEE,2004:488-492.

[7]GRASSER F,DARRIGO A,COLOMBI S.JOE:A mobile,inverted pendulum[J].IEEE Transactions on Industrial Electronics,2002,19(1):107-114.

[8]SALERNO A,OSTROVSKAYA S,ANGELES J.The development of quasiholonomic wheeled robots[C]//IEEE International Conference on Robotics and Automation,2002,4:3514-3520.

[9]AMBROSE,ROBERT O.Mobile manipulation using NASA's Robonaut[C]//IEEE International Conference on Robotics and Automation,2004,2:2104-2110.

(責任編輯：呂海亮)

Application of Complementary Filtering Technique in Self-balancing Robot

SUN Taiyi,WANG Zilan,WANG Yan,ZHU Yanzheng

(College of Electrical Engineering and Automation,Shandong University of Scienceand Technology,Qingdao,Shandong 266590,China)

A dynamic model of the self-balancing robot was first established. Based on PD(proportional differential) control algorithm, a control algorithm was developed to keep the self-balancing robot erect and the corresponding controller of the self-balancing robot was designed. The gyrostat sensor ENC03 and the accelerometer sensor MMA7261 were employed to measure the angular speed and angular acceleration respectively. Then the complementary filtering technique was used to calculate the angle of the self-balancing robot. In the experiment, miniwatt DC(direct current) motor was used in the mechanical structure of the self-balancing robot, large current half-bridge chip BTS7960 as the driver chip, ColdFire MCF52255 as the control chip, and bluetooth module for data transmission. The program was downloaded to the 32-bit control chip to control the self-balancing robot. The validness of the developed algorithm/program was verified by the collected real time data. The experiment results show that the proposed mechanical structure and hardware circuit for the self-balancing robot is reasonable and valid.

self-balancing robot;PD control algorithm;complementary filtering

2017-03-31

孫泰祎(1993—)，男，山東膠州人，碩士研究生,主要從事故障診斷與容錯控制研究. 朱延正(1986—)，男，山東臨清人，博士后，主要從事切換系統(tǒng)控制及其應用研究，本文通信作者. E-mail:yanzhengzhu@sdust.edu.cn

TP242

A

1672-3767(2017)04-0096-05

10.16452/j.cnki.sdkjzk.2017.04.014