龔林強(qiáng)，楊萍，申濤，鄭海霞，張淑珍

（蘭州理工大學(xué)機(jī)電工程學(xué)院，甘肅蘭州 730050）

兩輪自平衡智能車直立行走研究

龔林強(qiáng)，楊萍，申濤，鄭海霞，張淑珍

（蘭州理工大學(xué)機(jī)電工程學(xué)院，甘肅蘭州 730050）

兩輪智能車直立行走及直立行走的穩(wěn)定性一直是兩輪自平衡智能車制作過程中的關(guān)鍵問題，針對(duì)建立的兩輪自平衡智能車動(dòng)力學(xué)模型，分析了兩輪自平衡智能車自身無法維持直立狀態(tài)的原因，并得出兩輪自平衡智能車直立控制所需要的關(guān)鍵參數(shù)為智能車車身的姿態(tài)傾角，通過互補(bǔ)濾波將陀螺儀加速度計(jì)采集到的數(shù)據(jù)進(jìn)行融合，得到準(zhǔn)確的姿態(tài)傾角值，并以實(shí)驗(yàn)對(duì)比的方式得到了陀螺儀加速度計(jì)在兩輪自平衡智能車上的最優(yōu)安裝位置，最終實(shí)現(xiàn)兩輪自平衡智能車直立行走。

兩輪自平衡智能車；平衡控制；陀螺儀；加速度計(jì)；安裝位置

兩輪自平衡智能車是一種輪式機(jī)器人，屬于動(dòng)態(tài)自平衡機(jī)器人的一種，是一種多變量、強(qiáng)耦合、參數(shù)不確定的二輪系統(tǒng)[1]，但其具有體積小、轉(zhuǎn)彎半徑為零、運(yùn)動(dòng)靈活等優(yōu)點(diǎn)，所以近些年以來，二輪自平衡智能車的研究也成為了國內(nèi)外研究的熱點(diǎn)之一[2]。兩輪自平衡智能車型屬于倒立擺模型，在沒有施加外力的情況下，智能車在前進(jìn)過程中車身無法直立。

文中從首先從動(dòng)力學(xué)的角度分析了兩輪自平衡智能車自身無法直立的原因，以及兩輪自平衡智能車直立控制中需要用到的關(guān)鍵參數(shù)，隨后就陀螺儀加速度計(jì)安裝位置的不同，對(duì)智能車自平衡控制造成的影響進(jìn)行實(shí)驗(yàn)分析，最終而實(shí)現(xiàn)兩輪自平衡智能車直立行走。

1 兩輪自平衡智能車的動(dòng)力學(xué)模型

1.1 倒立擺模型分析

本課題自主制作的兩輪自平衡智能車如圖1所示。

圖1 智能車實(shí)體

文中以智能車車身與地面角夾角接近90°且能夠平穩(wěn)前進(jìn)的狀態(tài)作為智能車平衡狀態(tài)，在運(yùn)動(dòng)狀態(tài)下，將智能車力學(xué)模型簡化為倒立擺模型，如圖2（a）所示。

假設(shè)智能車向左運(yùn)動(dòng)，其質(zhì)量為m，質(zhì)心高度為L，加速度大小為a，由于慣性力F與重力G的存在，智能車車身會(huì)向右傾斜，車身受力如圖2（a）所示，沿車身法線及切線方向?qū)嚿硭芰M(jìn)行分解，如圖2（b）所示。

圖2 智能車車身受力圖

經(jīng)過計(jì)算，圖2（b）中各分力的大小如下式：

上式中，F(xiàn)τ和Fn為慣性力沿智能車車身切線與法線方向的分力，Gτ和Gn為重力沿智能車車身切線與法線方向的分力，此時(shí)智能車受到的回復(fù)力為F1=mgsinθ+macosθ,方向與位移方向相反，導(dǎo)致智能車在無外力情況下無法自動(dòng)恢復(fù)到平衡位置。

1.2 兩輪平衡車系統(tǒng)動(dòng)力學(xué)方程

根據(jù)對(duì)倒立擺的分析可得，為了使智能車能夠在平衡位置穩(wěn)定下來，必須施加一個(gè)與位移方向相同的外力T，假設(shè)外力T沿車身切線方向分力大小為:Tτ=mgsinθ+macosθ;當(dāng)θ接近于0的時(shí)候，可近似認(rèn)為：Tτ=m(g+k1)θ;其中k1=a/θ;此時(shí)，智能車能能夠在偏離平衡位置處穩(wěn)定下來。若Tτ＞F1，即k1θ＞a，k1＞a/θ，此時(shí)智能車車身切線方向所受合力的方向與位移方向相同，智能車車身在外力作用下可回到平衡位置。此外，增加阻尼力可以減少系統(tǒng)恢復(fù)至平衡狀態(tài)所需時(shí)間，假設(shè)增加的阻尼力與車身偏離平衡位置的角度成正比，方向與Tτ所產(chǎn)生的加速度方向相同，上式可改寫為：

上式中，c為阻尼系數(shù)，為方便計(jì)算，另c=mk2，根據(jù)上式可得此時(shí)外力Tτ產(chǎn)生的加速度大小a1與角度之間的關(guān)系為：a1=kθ+k2θ′，其中k=g+k1，假設(shè)此時(shí)外力干擾引起智能車產(chǎn)生的角加速度為α(t)，根據(jù)Lagrange方程[3]建立智能車系統(tǒng)的運(yùn)動(dòng)微分方程：

由于α(t)≠0，當(dāng)θ趨近于 0的時(shí)候?qū)⒐溅?=kθ+k2θ′代入系統(tǒng)運(yùn)動(dòng)微分方程，并化簡可得：

此時(shí)，系統(tǒng)輸入輸出之間的傳遞函數(shù)為：

由此得出倒立擺模型不能自我恢復(fù)穩(wěn)定的原因以及兩輪平衡車維持平衡狀態(tài)的條件為：

1）兩輪自平衡智能車在沒有外力作用的情況下，在偏離平衡位置處，受到的恢復(fù)力與運(yùn)動(dòng)方向相反；

2）兩輪車要維持平衡狀態(tài)，必須要測(cè)得精確的姿態(tài)傾角大小以及當(dāng)前角加速度，將其作為反饋值并通過PI控制，輸出相應(yīng)的PWM方波作用于電機(jī)控制產(chǎn)生一個(gè)與加速度方向相同的外力，以此維持兩輪自平衡車的動(dòng)態(tài)平衡。

2 陀螺儀加速度計(jì)的安裝位置對(duì)智能車平衡的影響

2.1 螺儀加速度計(jì)

為了測(cè)得準(zhǔn)確智能車的姿態(tài)傾角與角加速度，必須在智能車上安裝陀螺儀與加速度計(jì)，其中，但由于陀螺儀測(cè)得值是一個(gè)與角速度有關(guān)的瞬時(shí)量，需進(jìn)行積分處理之后可以得到角度變化量，積分時(shí)間越短，輸出角度越準(zhǔn)確，但由于積分時(shí)間不可能無限小，故積分的誤差會(huì)隨著時(shí)間的推移而逐漸積累，導(dǎo)致測(cè)得的角度與實(shí)際角度不相符。而加速度計(jì)測(cè)量的是重力方向，其參考基準(zhǔn)為豎直向下的重力，因此輸出角度不會(huì)有誤差，當(dāng)智能車處于穩(wěn)定勻速范圍內(nèi)前進(jìn)的時(shí)候，加速度計(jì)的測(cè)量值在長時(shí)間內(nèi)是準(zhǔn)確的，但加速度計(jì)不能區(qū)分重力加速度與外力加速度，當(dāng)智能車做變速運(yùn)動(dòng)時(shí)，它的輸出角度與實(shí)際角度明顯不相符。所以陀螺儀、加速度計(jì)當(dāng)中的某一種是無法保證測(cè)量結(jié)果的準(zhǔn)確性的，必須結(jié)合使用。

2.2 互補(bǔ)濾波

陀螺儀在低頻段測(cè)量誤差較大，而加速度計(jì)測(cè)量誤差主要集中在高頻段，在智能車直立方案中，選擇將加速度計(jì)和陀螺儀測(cè)得的兩種角度信號(hào)，通過互補(bǔ)濾波的方法進(jìn)行融合，得到準(zhǔn)確的角度信號(hào)，保證智能車的平衡性能，互補(bǔ)濾波原理圖如圖3所示。

圖3 互補(bǔ)濾波原理圖

互補(bǔ)濾波程序如下：

上述程序中Acc_jiaodu為加速度計(jì)偏移角度，1 155為智能車處于平衡位置時(shí)刻，加速度計(jì)測(cè)得的角度；Gyr_jiaosudu為陀螺儀便宜角度，1 177為智能車處于平衡位置時(shí)刻，加速度計(jì)測(cè)得的角度；real_angle為通過互補(bǔ)濾波方法得到的實(shí)際偏移角度。

2.3 加速度計(jì)安裝位置對(duì)智能車平衡性能的影響

為了驗(yàn)證陀螺儀加速度計(jì)安裝位置的不同對(duì)濾波效果以及之后智能車直立平衡性能的影響，分別將陀螺儀加速度計(jì)沿著車身中軸線與地面成垂直角度的位置分別安裝在智能車的3個(gè)不同位置進(jìn)行單獨(dú)測(cè)試并對(duì)比，如圖4所示。

圖4 陀螺儀加速度計(jì)位安裝置選擇示意圖

圖5 不同位置陀螺儀濾波效果

如圖5所示，其中縱軸表示互補(bǔ)濾波得出的擬合曲線的角度值，橫軸代表時(shí)間變量，在調(diào)節(jié)PI參數(shù)確保濾波曲線跟隨性一致的情況下，就陀螺儀加速度計(jì)分別處于圖4所示3個(gè)位置時(shí)候測(cè)得的角度曲線以及智能車直立效果進(jìn)行對(duì)比：

1）當(dāng)陀螺儀加速度計(jì)處于圖4所示位置1處時(shí)，濾波效果如圖5（a）所示，智能車在回到平衡位置附近時(shí)加速度計(jì)得出的數(shù)據(jù)過沖很明顯，智能車在恢復(fù)直立過程中再平衡位置兩側(cè)有較為劇烈的擺動(dòng)；

2）當(dāng)陀螺儀加速度計(jì)處于圖4所示位置2處的時(shí)候，濾波效果如圖5（b）所示，當(dāng)智能車在回到平衡位置附近時(shí)，加速度計(jì)得出的角度曲線雖然過沖程度較位置1時(shí)有所減小，但智能車在平衡位置附近的擺動(dòng)仍然比較嚴(yán)重；

3）當(dāng)我們將陀螺儀位置選擇在圖4所示位置3的時(shí)候，讓陀螺儀加速度計(jì)盡量與車輪轉(zhuǎn)動(dòng)軸平行，此時(shí)加速度計(jì)在智能車回到平衡位置附近時(shí)，得出的波形曲線的過沖問題有明顯的減弱，如圖5（c）所示；而在實(shí)際調(diào)試時(shí)候，智能車在平衡位置前后的擺動(dòng)也大為減輕[4-17]。

3 兩輪自平衡智能車直立行走控制

兩輪自平衡智能車直立行走控制主要有以下3個(gè)部分：直立控制、速度控制、差速控制，直立控制可以保證智能車穩(wěn)定在平衡位置處，程序設(shè)計(jì)如下：

上述程序中KP、KD為直立控制中的PD參數(shù)，real_angle為通過互補(bǔ)濾波方法得到的實(shí)際車身傾角，PWMOUT_Angle為輸出值，用于電機(jī)轉(zhuǎn)速控制。

另外，速度控制保證智能車能夠穩(wěn)定向前移動(dòng)，差速控制能夠保證智能車根據(jù)道路情況及時(shí)減速或轉(zhuǎn)彎，智能車直立行走程序設(shè)計(jì)如下：

上述程序中，PWMOUT_Speed為設(shè)定的智能車前進(jìn)速度，PWMOUT_Dir為根據(jù)智能車左右兩側(cè)傳感器所測(cè)數(shù)據(jù)得出的差速值。

4 結(jié)束語

文中基于以MC9S12XS128處理器為核心的兩輪自平衡智能車為研究對(duì)象，通過建立數(shù)學(xué)模型，得出了兩輪自平衡智能車在沒有外力的情況下無法維持直立行走的原因，再基于所建立動(dòng)力學(xué)模型之上，通過理論計(jì)算的方法得出了智能車直立控制中需要用到的關(guān)鍵參數(shù)為智能車當(dāng)前的姿態(tài)傾角。使用陀螺儀與加速度計(jì)測(cè)量智能車實(shí)際姿態(tài)傾角，通過互補(bǔ)濾波的方法對(duì)陀螺儀、加速度計(jì)測(cè)得數(shù)據(jù)進(jìn)行融合從而得到智能車當(dāng)前姿態(tài)傾角的精確值，最后通過實(shí)驗(yàn)對(duì)比的方法就陀螺儀與加速度計(jì)不同的安裝位置對(duì)互補(bǔ)濾波效果以及智能車直立控制的效果進(jìn)行分析，從而總結(jié)出陀螺儀加速度計(jì)的最優(yōu)安裝位置，最終實(shí)現(xiàn)了兩輪自平衡智能車的直立行走。

[1]阮曉鋼，孫榮毅.兩輪自平衡機(jī)器人運(yùn)動(dòng)平衡控制研究[J].科學(xué)技術(shù)與工程，2014，14（24）:84-88.

[2]孟鵬，章 政.基于陀螺儀與加速度計(jì)的二輪自平衡控制系統(tǒng)設(shè)計(jì)[J].傳感器與微系統(tǒng)，2014，33（10）:61-64.

[3]Guo Qi，Zhou Zhao-ying，Xiong Wei.Design of a barometricaltimeter based on a micro pressure sens-or[J].Nano Technologyand Precision Engineering，2012，10（4）:337-341.

[4]馮智勇，曾瀚，張力.基于陀螺儀及加速度計(jì)信號(hào)融合的姿態(tài)角度測(cè)量[J].西南師范大學(xué)學(xué)報(bào)：自然科學(xué)版，2011，36（4）:137-140.

[5]蒙祖祈，劉海剛，項(xiàng)華珍，等.小型兩輪自平衡電動(dòng)車系統(tǒng)的設(shè)計(jì)與研究[J].工業(yè)控制計(jì)算機(jī)，2015，28（3）:64-67.

[6]梁光勝，杜夢(mèng)楠，周子豪，等.基于互補(bǔ)濾波的兩輪自平衡車姿態(tài)控制[J].測(cè)控技術(shù)，2015，34（5）:72-74.

[7]卓晴，黃開勝，邵貝貝.學(xué)做智能車-挑戰(zhàn)“飛思卡爾”杯[M].北京：北京航空航天出版社，2007.

[8]隋金雪，楊莉，張巖.“飛思卡爾”杯智能汽車設(shè)計(jì)與實(shí)現(xiàn)實(shí)例教程[M].北京：電子工業(yè)出版社，2014.

[9]李有堂.機(jī)械振動(dòng)理論與應(yīng)用[M].北京：科學(xué)出版社，2012.

[10]周牡丹，康愷，蔡普郎，等.兩輪平衡車控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[J].自動(dòng)化技術(shù)與應(yīng)用，2014，33（10）:4-8.

[11]蘭建軍，李佳璐，劉穎，等.基于自適應(yīng)互補(bǔ)濾波算法的輪式倒立擺控制[J].組合機(jī)床與自動(dòng)化加工技術(shù)，2014，6（6）:66-68.

[12]張曉兵.獨(dú)輪機(jī)器人姿態(tài)檢測(cè)信息融合算法的研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2015.

[13]SHEN Xiao-wei，YAO Min-li，JIA Wei-ming，et al.Adaptive complementary filter using fuzzy logic and si- multaneous perturbation stochastic approximation algorithm [J].Measurement，2012（45）:1257-1265.

[14]范竹青，張正華.輪式機(jī)器人直立控制研究及其實(shí)現(xiàn)[J].信息技術(shù)，2013（12）:141-144.

[15]張團(tuán)善，李文真.單輪自平衡機(jī)器車的系統(tǒng)建模與最優(yōu)控制[J].西安工程大學(xué)學(xué)報(bào)，2014，28（1）:77-83.

[16]梁華，李曉虹.楊光祥兩輪自平衡機(jī)器人動(dòng)力學(xué)模型分析及PID控制方法研究[J].重慶師范大學(xué)學(xué)報(bào):自然科學(xué)版，2016（1）:163-167.

[17]高曉娟，徐光輝，張歡，等.基于加速度特征的人體跌倒檢測(cè)算法[J].西安工程大學(xué)學(xué)報(bào)，2015，29（1）:90-94

[18]賈立，邵定國，沈飛天.現(xiàn)代控制理論[M].上海：上海大學(xué)出版社，2013.

Research on balance control of two?wheeled self?balancing vehicle

GONG Lin?qiang，YANG Ping，SHEN Tao，ZHENG Hai?xia，ZHANG Shu?zhen
（College of Mechano?Electronic Engineering，Lanzhou Univ.of Tech.，Lanzhou730050，China）

The perpendicularity and stability of the models’perpendicularity are main problems in the process of producing a two?wheeled self?balancing vehicle，based on the two self balancing intelligent vehicle dynamics model，we analyzed the reason for why two self balancing intelligent vehicle can’t maintain upright by itself，and we realized the key parameters for two self standing balance control is attitude angle，we get accurate attitude angle value by fusing the data collected by accelerometer in method of complementary filtering and obtained optimal installation position of the gyro accelerometer by the experimental comparison，finally we realized the upright walking of two?wheeled smart car.

two wheeled self balancing vehicle；balancing control；installation position of gyroscope and accelerometer

TP242.6

A

1674-6236（2017）22-0145-04

2016-09-13稿件編號(hào)：201609132

龔林強(qiáng)（1990—），男，甘肅岷縣人，碩士研究生。研究方向:特殊環(huán)境下的機(jī)器人開發(fā)。