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

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

0 引言

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

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

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

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

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

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

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

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

2.1 卡爾曼濾波

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

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

其中如表1所示。

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

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

2.2 互補(bǔ)濾波

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

圖2 互補(bǔ)濾波

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

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

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

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

3 兩輪自平衡車控制算法

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

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

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

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

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

綜合三種：

4 實驗與分析

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

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

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

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

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

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

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

由實驗結(jié)果分析可以得出，采用融合卡爾曼濾波和互補(bǔ)濾波算法對陀螺儀傳感器和加速度傳感器獲取的位姿數(shù)據(jù)作處理后，系統(tǒng)收斂速度更快，誤差更小，系統(tǒng)更穩(wěn)定?？刂平缑嫒鐖D7所示。

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

5 結(jié)論

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