基于姿態檢測的兩輪自平衡車控制方法研究

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

（1.天津職業技術師范大學機器人及智能裝備研究所，天津300222；2.天津博諾智創機器人技術有限公司，天津300353）

0 引言

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

1 建立兩輪自平衡車數學模型

兩輪自平衡車的力學模型不同于單級倒立擺，自平衡車系統是依靠底部電機扭矩輸出來達到整個系統平衡[4]。如圖1a所示，自平衡車輪子由底部電機驅動，能夠產生轉矩，是主動機構；車體屬于是從動機構。為了能夠實現系統的自平衡，必須實時支配驅動電機進行正反轉動，以生成反傾覆的恰當力矩，維持兩輪自平衡小車車體平衡。

圖1 兩輪自平衡車系統示意圖

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

為了便于建模，在小角度的范圍內[6]，取sinθ≈θ，cosθ≈1，由上式得系統運動方程：

2 兩輪自平衡車傳感器姿態檢測

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

2.1 卡爾曼濾波

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

系統測量方程如下：

其中如表1所示。

表1 線性離散時間在K時刻的過程狀態

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

2.2 互補濾波

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

圖2 互補濾波

2.3 互補/卡爾曼濾波融合過程

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

當測量信息無效時，直接使用事先寫好的設定值進行補正陀螺儀角度值，輸出水平姿態角，融合流程如圖3所示。

圖3 卡爾曼/互補濾波融合流程

3 兩輪自平衡車控制算法

兩輪自平衡車分為直立、速度和轉向控制。

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

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

速度控制采用PI控制算法，依據自平衡小車底部兩電機自帶的霍爾編碼器獲取的信息加入速度PI控制，實現系統速度調節。速度PWM輸出公式表示為：

將PD控制運用到轉向控制，通過陀螺儀Z軸角度輔助控制實現小車轉向，轉向PWM表示為：

綜合三種：

4 實驗與分析

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

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

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

根據上述系統運動狀態方程，結合位姿檢測策略得出的機器人傾角、角速度輸出控制量。每次試驗動作超過1000次，然后停止試驗并重新測試驗證其穩定性。

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

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

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

由實驗結果分析可以得出，采用融合卡爾曼濾波和互補濾波算法對陀螺儀傳感器和加速度傳感器獲取的位姿數據作處理后，系統收斂速度更快，誤差更小，系統更穩定。控制界面如圖7所示。

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

5 結論

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