基于PID算法和卡爾曼濾波的平衡車控制系統

楊俊偉

（廣東東軟學院 廣東省佛山市 528225）

1 引言

自平衡車控制系統是移動機器人領域的熱點問題，能夠將人工智能技術和自動控制理論有機結合的典型應用。隨著微電子技術的迅猛發展，自動控制與人工智能的融合會得到進一步提升，平衡車也是未來載具的重要方向之一[1]。本文以STM32 為主控制芯片，在數學模型分析基礎之上，采用PID 算法和卡爾曼濾波實現了兩輪自平衡車控制系統的設計與實現，由姿態傳感器模塊收集的傾角、角速度等數據，經過卡爾曼濾波數據融合分析，得到精確的平衡車姿態信息，通過PID 算法實現平衡車的直立控制、速度控制、轉向控制、手機藍牙遙控等，做到了平衡車的速度與穩定性的統一。

2 設計原理

如圖1 所示，平衡車的運動狀態與倒立的單擺相似，因此，利用可簡化的單擺模型進行數學建模,不考慮空氣阻力和摩擦力。設車體的質量為m，正常行駛時的加速度為a，平衡車與豎直方向之間的夾角為θ,當平衡車離開豎直方向平衡位置的時候，使重心恢復豎直方向所需拉力為：

當θ 很小時與sinθ 近似，車輪加速度a 與傾角成正比例關系，假設為k，因此可將公式近似為：

上述公式中，加入平衡車角速度ω 的阻尼力影響，可將公式修改如下：

因此，要將平衡車控制在豎直方向的加速度a 的控制公式為：

其中，θ 是傾角，ω 是角速度，k 和k1是比例系數，實際操作時，需要滿足k>g，k1>0，實時測量平衡車的傾角，通過卡爾曼濾波和PID 算法給予電機控制信號來控制平衡車的車體維持穩定。

3 平衡算法

3.1 PID算法

PID 算法是平衡車的核心算法，該算法結構簡單，參數易于調整，是自動控制系統中經常采用的控制算法。PID：比例單元（P）、積分單元（I）和微分單元（D）組成，PID 算法控制公式如下：

其中：

u(t)為控制器輸出的控制量；

e(t)為偏差信號，KP（對應參數 P）為比例系數，TI（對應參數I）為積分時間常數，TD(對應參數 D)為微分時間常數。

直立控制（PD 算法）：比例控制是引入了回復力，微分控制是引入了阻尼力，微分系數與轉動慣量有關。Kp 的值越大，系統的反應越快，但是平衡曲線的震蕩就較大，我們需要引入Kd 參數來減緩震蕩。在小車質量一定的情況下，重心位置增高，因為需要的回復力減小，所以比例系數下降；轉動慣量變大，所以微分控制系數增大。在小車重心位置一定的情況下，質量增大，因為需要的回復力增大，比例控制系數增大；轉動慣量變大，所以微分控制系數增大。

圖1：平衡車的簡化數學模型

圖2：PID 算法框圖

圖3：平衡車系統硬件結構圖

圖4：主控制芯片

圖5：姿態傳感器MPU6050

圖6：藍牙模塊

圖7：藍牙HC-05 原理圖

圖8：電機驅動器TB6612FNG 原理圖

速度控制（PI 算法），積分調節的作用體現為準確性，其作用在于消除系統偏差，KI 越大，系統越準確穩定，但響應時間會變慢。只要偏差存在，就不斷地對偏差進行積分（累加），并反應在調節力度上。

在PID 算法中，通過調節PID 這三個參數，就可以逐漸控制系統使其趨于穩定，達到平衡的效果。PID 算法框圖如圖2 所示。

PID 控制算法因時間離散化不同，通常分為增量式 PID 控制算法和位置式 PID 控制算法[3]。

PID 增量公式：

比例P:e(k)-e(k-1)當前誤差-上次誤差；

積分I:e(i)當前誤差；

微分D:e(k) - 2e(k-1)+e(k-2)當前誤差 - 2*上次誤差 + 上上次誤差

PID 位置公式：

e(k): 用戶設定的值（目標值）-控制對象的當前的狀態值

比例P:e(k)當前誤差

積分I:∑e(i)誤差的累加(包括e(k))

微分D:e(k) - e(k-1)當前誤差-上次誤差

對于E（k）的說明：E（k）這個變量的意義是系統的誤差。從公式本身來分析：兩個公式都是對系統誤差進行的數學運算，增量式公式中與系統前一次的誤差E（k-1）和前兩次的誤差E（k-2）有關。而且位置公式應用時控制的不是輸出量，而是輸出量的增量，與積分環節無關。適用于步進電機的調節，尤其適用于沒有積分環節的控制系統。位置式公式中與系統前一次的誤差E（k-1）和誤差的累加值ΣE（k）有關，其輸出與整個系統的過去狀態有關，是對系統每一次不同狀態的分析和調整，對系統本身有著較大的影響[4]。

3.2 卡爾曼濾波

由于加速度計靜態性能優良但是動態跟蹤能力不行，而陀螺儀測量傾角精確但是存在積分誤差的累積，因此系統對加速度計和陀螺儀測得的數據通過卡爾曼濾波器實現數據融合[5]。解決了長時間使用陀螺儀而造成的漂移誤差，同時也補償了長時間使用加速度計而造成的動態誤差[6]。融合兩個傳感器的輸出數據，根據數據特征建立狀態預測方程，對平衡車的姿態角進行估算，得出最優化的車體傾角和角速度，大大確保了數據的準確性，減小了數據的波動。

卡爾曼濾波，也稱為線性二次估計，卡爾曼濾波會根據各測量在不同時間下的值，考慮各時間下的聯合分布，產生比僅基于單個測量值更精確的未知變量的估計，k 時刻自平衡車系統的預測方程[7]：

A 為系統的預測矩陣，B 為控制矩陣

X(k|k-1):是應用 k-1 時刻的最優解對k 時刻的估計；

X(k-1|k-1):為k-1 時刻的最優解。

經由傳感器測量數據修正的狀態方程為：

上式中，Kh為卡爾曼增益，其表示方程如下：

X(k|k)和P(k|k)是最新的最優估計，根據卡爾曼的迭代思想 X(k|k) 和 P(k|k) 將會作為新的 X(k-1|k-1) 和 P(k-1|k-1) 進行 k+1 時刻的最優運算。

4 系統硬件設計

系統以Stm32F407VGTx 為主控制芯片，硬件模塊主要有：電姿態傳感器模塊、電機模塊、藍牙通訊模塊等。如圖3 所示。

4.1 主控制器

如圖4 所示，平衡車系統主控芯片采用Stm32F407VGTx，STM32F407 系列MCU 具有高集成度、高性能、嵌入式存儲器和外設的醫療、工業與消費類應用。具有512KB~1MB Flash 和192KB SRAM，采用尺寸小至10×10 mm 的100~176 引腳封裝。提供了工作頻率為168 MHz 的Cortex ?-M4 內核（具有浮點單元）的性能。

4.2 姿態傳感器

姿態控制模塊采用MPU6050 傳感器，是InvenSense 公司推出的一款六軸運動處理芯片，集成了三軸陀螺儀及三軸加速度計傳感器，內置兩組I2C 接口，分別用于通信和連接外部磁力傳感器。MPU6050 采用自帶的數位運動處理器DMP（Digital Motion Processor），通過主I2C 接口，以單一數據流的形式直接讀取完整的9 軸融合演算數據并向應用端輸出。本系統以I/O 口模擬I2C 與MPU6050 通信獲取數據，再利用卡爾曼濾波算法解算出當前平衡車的傾角和角速度。如圖5 所示。

4.3 藍牙模塊

藍牙模塊如圖6 所示。

如圖7 所示，HC-05 是一款高性能主從一體藍牙串口模塊，具有成本低、體積小、功耗低、收發靈敏性高等優點。HC-05 具有兩種工作模式：命令響應工作模式和自動連接工作模式。當模塊處于命令響應工作模式時能才能執行AT 命令，可向模塊發送各種 AT指令，為模塊設定控制參數或發布控制命令。

4.4 電機模塊

電機模塊主要是由直流電機、電機驅動器和電機測速器組成，電機選擇供電電壓為12V、減速比為1:30 的直流減速電機，型號為JGB37-520。電機驅動模塊采用東芝半導體公司生產的一款直流電機驅動器件TB6612FNG，具有大電流MOSFET-H 橋結構，雙通道電路輸出，可同時驅動兩個電機，分別用來控制平衡車的左右輪。電機轉速檢測采用增量式磁性編碼器，霍爾編碼器是電機測速中常用的電子元件，通過輸出兩個通道正交相位90°的方波把速度數據傳輸給單片機。如圖8 所示。

5 總結

本文提出基于PID 算法和卡爾曼濾波數據融合的平衡車控制系統設計與實現，本系統以PID 算法為核心，由卡爾曼濾波對加速度計和陀螺儀傳感器數據融合實現系統的最優估算。根據實測，系統具有調節速度快、精度較高、抗干擾能力強等優點，具有一定的實際應用參考價值。