基于姿態(tài)檢測(cè)的兩輪自平衡車控制方法研究

王 文，鄧三鵬＊，張香玲，祁宇明

（1.天津職業(yè)技術(shù)師范大學(xué)機(jī)器人及智能裝備研究所，天津300222；2.天津博諾智創(chuàng)機(jī)器人技術(shù)有限公司，天津300353）

0 引言

兩輪平衡車作為一種綠色環(huán)保、靈活方便的交通方式受到歡迎，同一階倒立擺系統(tǒng)一樣，兩輪自平衡車具有欠驅(qū)動(dòng)、非線性、強(qiáng)耦合、多變量的特點(diǎn)[1]。兩輪自平衡車在平衡過(guò)程中需維持車體重心與輪子處于同一軸線，通過(guò)實(shí)時(shí)讀取陀螺儀傳感器和加速度傳感器測(cè)得的偏轉(zhuǎn)角度來(lái)改變狀態(tài)輸入值，控制電機(jī)的PWM輸出值產(chǎn)生的扭矩達(dá)到平衡目的，所以自平衡車實(shí)時(shí)監(jiān)測(cè)的偏轉(zhuǎn)角度大小成為關(guān)鍵，自平衡車姿態(tài)解算大致分為一階互補(bǔ)濾波、兩階互補(bǔ)濾波、小波分析、卡爾曼濾波算法、神經(jīng)網(wǎng)絡(luò)。小波分析、神經(jīng)網(wǎng)絡(luò)都存在數(shù)據(jù)計(jì)算量大、對(duì)處理器性能要求過(guò)高[2]；一階互補(bǔ)濾波、兩階互補(bǔ)濾波是從陀螺儀或加速度計(jì)進(jìn)行姿態(tài)角度信號(hào)濾波，優(yōu)點(diǎn)是濾波收斂速度快，但存在濾波效果不平滑現(xiàn)象造成的超調(diào)嚴(yán)重；卡爾曼濾波在收斂速度和平滑效果上都一般。比較之下，將卡爾曼和互補(bǔ)兩種濾波算法融合陀螺儀和加速度計(jì)獲取的位姿數(shù)據(jù)求解出最優(yōu)位姿參數(shù)[3]，采用PID算法控制兩輪自平衡車，控制可靠，運(yùn)算速度快。

1 建立兩輪自平衡車數(shù)學(xué)模型

兩輪自平衡車的力學(xué)模型不同于單級(jí)倒立擺，自平衡車系統(tǒng)是依靠底部電機(jī)扭矩輸出來(lái)達(dá)到整個(gè)系統(tǒng)平衡[4]。如圖1a所示，自平衡車輪子由底部電機(jī)驅(qū)動(dòng)，能夠產(chǎn)生轉(zhuǎn)矩，是主動(dòng)機(jī)構(gòu)；車體屬于是從動(dòng)機(jī)構(gòu)。為了能夠?qū)崿F(xiàn)系統(tǒng)的自平衡，必須實(shí)時(shí)支配驅(qū)動(dòng)電機(jī)進(jìn)行正反轉(zhuǎn)動(dòng)，以生成反傾覆的恰當(dāng)力矩，維持兩輪自平衡小車車體平衡。

圖1 兩輪自平衡車系統(tǒng)示意圖

如圖1b中，設(shè)自平衡車車體與y軸之間的夾角IA為偏轉(zhuǎn)角θ，小車車輪的水平位移u為X軸正方向，為小車車體繞輪子中心的轉(zhuǎn)動(dòng)慣量，L為小車車體重心到轉(zhuǎn)軸的距離，可以得出，其中為小車車輪組件在x軸方向給車體的力，為小車車輪組件在y軸方向給車體的力[5]。系統(tǒng)中車輪與電機(jī)位于同一軸線，可視為與車體一致，所以設(shè)車輪質(zhì)量為M；假設(shè)除車輪部分的為一均勻質(zhì)體，取其質(zhì)量為m；在理想條件下，設(shè)立水平和垂直運(yùn)動(dòng)狀態(tài)方程：

為了便于建模，在小角度的范圍內(nèi)[6]，取sinθ≈θ，cosθ≈1，由上式得系統(tǒng)運(yùn)動(dòng)方程：

2 兩輪自平衡車傳感器姿態(tài)檢測(cè)

系統(tǒng)搭配數(shù)字陀螺儀MPU6050傳感元件，集成了3軸MEMS陀螺儀、3軸MEMS加速度計(jì)，通過(guò)I2C實(shí)現(xiàn)數(shù)據(jù)通信，輸出數(shù)值為角加速度值、角速度值，加速度計(jì)用來(lái)測(cè)量平衡車線性加速度，因平衡車往返震蕩頻繁致使加速度易敏感化，測(cè)得實(shí)時(shí)數(shù)值不準(zhǔn)確，計(jì)算傾斜角度產(chǎn)生大的誤差；通過(guò)對(duì)角速度進(jìn)行積分，陀螺儀可以測(cè)量瞬時(shí)動(dòng)態(tài)角度值的變化。小段時(shí)間內(nèi)測(cè)得的角度值精度高，不會(huì)受加速度的干擾。但是積分漂移和溫度會(huì)隨著積分時(shí)間的增長(zhǎng)而增長(zhǎng)，所以這兩種方法不適合單獨(dú)使用。為了準(zhǔn)確地測(cè)得自平衡車姿態(tài)信息，提出將陀螺儀測(cè)得數(shù)據(jù)信息和加速度計(jì)獲得的信息進(jìn)行融合處理。將卡爾曼和互補(bǔ)兩種濾波算法融合陀螺儀和加速度計(jì)獲取的位姿數(shù)據(jù)求解出最優(yōu)位姿參數(shù)。

2.1 卡爾曼濾波

卡爾曼濾波器的作用是估算線性離散時(shí)間在某時(shí)刻的過(guò)程狀態(tài)變量，系統(tǒng)狀態(tài)方程如下：

系統(tǒng)測(cè)量方程如下：

其中如表1所示。

表1 線性離散時(shí)間在K時(shí)刻的過(guò)程狀態(tài)

卡爾曼思想是要對(duì)k時(shí)刻的傾斜角度值預(yù)測(cè)，先要預(yù)測(cè)k時(shí)刻的傾斜角度值就需要依照k-1時(shí)刻的傾斜角度值，然后得到k時(shí)刻的高斯噪聲偏差，這里系統(tǒng)預(yù)測(cè)狀態(tài)：

2.2 互補(bǔ)濾波

兩輪自平衡車系統(tǒng)中，為防止由于加速度計(jì)產(chǎn)生的瞬時(shí)值計(jì)算傾角誤差大、陀螺儀測(cè)得角速度信號(hào)之間存在的微小誤差，經(jīng)積分漂移形成更大的誤差，采用了互補(bǔ)濾波算法，可獲得更優(yōu)的平衡姿態(tài)估計(jì)值?；パa(bǔ)濾波是通過(guò)識(shí)別被測(cè)量信號(hào)的不相同頻域來(lái)消除噪聲的，由低通濾波器提取高頻噪聲中測(cè)量信號(hào)的低頻信號(hào)，同樣的由高通濾波器提取低頻噪聲中的測(cè)量信號(hào)的高頻信號(hào)。在小段時(shí)間內(nèi)陀螺儀讀取的瞬時(shí)值為最準(zhǔn)的角度值，需要采取一種濾波算法對(duì)采樣加速度定期優(yōu)化，取角度平均值，這個(gè)過(guò)程中陀螺儀產(chǎn)生低頻信號(hào)，加速度計(jì)產(chǎn)生高頻信號(hào)，然后再將整個(gè)頻段的信號(hào)進(jìn)行拼接輸出，流程如圖2所示。

圖2 互補(bǔ)濾波

2.3 互補(bǔ)/卡爾曼濾波融合過(guò)程

首先運(yùn)用互補(bǔ)濾波算法將陀螺儀和加速度計(jì)所得數(shù)值進(jìn)行融合處理，計(jì)算得到陀螺儀姿態(tài)誤差。然后以互補(bǔ)陀螺儀的積分誤差作為觀測(cè)值，利用卡爾曼濾波對(duì)各部分誤差進(jìn)行補(bǔ)償，得到精確的姿態(tài)參數(shù)。

當(dāng)測(cè)量信息無(wú)效時(shí)，直接使用事先寫(xiě)好的設(shè)定值進(jìn)行補(bǔ)正陀螺儀角度值，輸出水平姿態(tài)角，融合流程如圖3所示。

圖3 卡爾曼/互補(bǔ)濾波融合流程

3 兩輪自平衡車控制算法

兩輪自平衡車分為直立、速度和轉(zhuǎn)向控制。

自平衡車直立控制采用PD控制算法，輸入變量是根據(jù)互補(bǔ)濾波融合卡爾曼濾波融得出更精確的傾角θ，NI myRIO控制器根據(jù)這個(gè)輸入變量來(lái)改變電機(jī)的運(yùn)動(dòng)方向和速度，使車輪產(chǎn)生合適的加速度，平衡車就能產(chǎn)生與傾倒趨勢(shì)相反的力矩，通過(guò)動(dòng)態(tài)實(shí)時(shí)調(diào)整重心誤差補(bǔ)償，從而維持自平衡小車穩(wěn)定直立平衡。平衡PWM輸出公式表示為：

KP為比例系數(shù)，θ為傾斜角度，KD為微分系數(shù)，φ為角速度。

速度控制采用PI控制算法，依據(jù)自平衡小車底部?jī)呻姍C(jī)自帶的霍爾編碼器獲取的信息加入速度PI控制，實(shí)現(xiàn)系統(tǒng)速度調(diào)節(jié)。速度PWM輸出公式表示為：

將PD控制運(yùn)用到轉(zhuǎn)向控制，通過(guò)陀螺儀Z軸角度輔助控制實(shí)現(xiàn)小車轉(zhuǎn)向，轉(zhuǎn)向PWM表示為：

綜合三種：

4 實(shí)驗(yàn)與分析

兩輪自平衡系統(tǒng)主要由NI myRIO控制器、陀螺儀傳感器、帶編碼器直流減速電機(jī)、驅(qū)動(dòng)控制板和電源組成。NI myRIO控制器內(nèi)嵌Xilinx Zynq芯片，直流供電范圍為6 V～16 V，易于上手使用、編程開(kāi)發(fā)簡(jiǎn)單；姿態(tài)傳感器為數(shù)字陀螺儀MPU6050六軸傳感元件，集成了3軸MEMS陀螺儀、3軸MEMS加速度計(jì)。實(shí)體樣機(jī)平衡效果如圖4所示。

圖4 兩輪自平衡車平衡效果展示

實(shí)驗(yàn)條件：（1）平整的平面；（2）中途介入人為外力干擾；（3）試驗(yàn)方案：自由平衡——介入外力干擾——自由平衡；（4）一次試驗(yàn)15 min，不同平面條件下各測(cè)十次。

根據(jù)上述系統(tǒng)運(yùn)動(dòng)狀態(tài)方程，結(jié)合位姿檢測(cè)策略得出的機(jī)器人傾角、角速度輸出控制量。每次試驗(yàn)動(dòng)作超過(guò)1000次，然后停止試驗(yàn)并重新測(cè)試驗(yàn)證其穩(wěn)定性。

機(jī)器人機(jī)體傾斜角度、機(jī)器人角速度收斂分別如圖5a和圖6a所示，人為外力干擾后機(jī)器人機(jī)體傾斜角度、機(jī)器人角速度收斂分別如圖5b和圖6b所示。

圖5 兩輪自平衡車穩(wěn)定直立角度和受沖擊角度波形圖

圖6 兩輪自平衡車穩(wěn)定直立角速度和受沖擊角速度波形圖

由實(shí)驗(yàn)結(jié)果分析可以得出，采用融合卡爾曼濾波和互補(bǔ)濾波算法對(duì)陀螺儀傳感器和加速度傳感器獲取的位姿數(shù)據(jù)作處理后，系統(tǒng)收斂速度更快，誤差更小，系統(tǒng)更穩(wěn)定。控制界面如圖7所示。

圖7 兩輪自平衡車labview界面顯示

5 結(jié)論

針對(duì)平衡車姿態(tài)檢測(cè)手段單一，長(zhǎng)時(shí)間運(yùn)行存在控制不穩(wěn)定造成人身財(cái)產(chǎn)損失的問(wèn)題，改進(jìn)了姿態(tài)檢測(cè)手段，將卡爾曼濾波和互補(bǔ)濾波兩種算法融合陀螺儀傳感器和加速度傳感器獲取的位姿數(shù)據(jù)求解出最優(yōu)姿態(tài)參數(shù)，在平衡運(yùn)動(dòng)過(guò)程中所采集的姿態(tài)實(shí)時(shí)數(shù)據(jù)更準(zhǔn)確，不存在由于長(zhǎng)時(shí)間使用累積的誤差；采用PID控制算法建立控制運(yùn)動(dòng)方程，控制靈活。最終在搭建的自平衡車進(jìn)行測(cè)試，穩(wěn)定能力更好，且在外力干擾條件下能迅速回正，響應(yīng)速度更快。