平衡運輸小車系統的設計

吳玉明 趙華林 李小華

摘要 本文研究了基于STM32微控制器的平衡運輸小車系統的設計問題，完成了基于STM32微控制器和MPU6050的控制系統的硬件設計;通過離散化PID算法軟件實現了對系統的閉環控制。同時利用了DMP濾波器提高了反饋信號的采集精度，提高了系統的穩定性。通過系統分析和實驗調試確定了系統的各項參數。實驗表明，在輸入擾動信號時，小車的承載托盤可以穩定在水平位置，其并且有較好的快速性和準確性。

【關鍵詞】平衡運輸小車 STM32 微控制器MPU6050 閉環控制 DMP濾波器

隨著現代社會自動化水平的提高，在許多現代化工作場所中，例如生產車間、快遞分揀站等，越來越多的智能平衡運輸小車和機器人替代了人工搬運，作為運輸易碎易損壞零件和易碎物品的運輸工具，提高了生產效率，使得智能平衡運輸機械成為了現代化工廠的重要標志。目前，對于平衡運輸小車的研究主要集中于托盤狀態的監測和對輕微振動的消除，所設計出的小車在實際控制中托盤的角度常常存在穩態誤差，信號的處理上也存在噪聲，使得物品容易掉落。所以，本文將針對平衡運輸小車中的控制算法的設計和信號的采集和濾波進行深入研究，以stm32微控制器為核心，使用包含MPU6050和DMP濾波器的采集系統作為反饋角度信號，通過離散化PID控制算法對小車托盤的角度進行閉環控制，解決了目前平衡運輸系統所存在的穩態誤差和噪聲問題，使系統滿足其控制要求。所設計出的平衡運輸小車具有結構簡單，動態性能良好，為平衡運輸系統的設計提供了低成本的可行性方案。

1 系統的建立

1.1 機械結構的搭建

1.1.1 運輸承載結構的搭建

小車的承載結構由三部分組成。

（1）托盤部分：托盤為20cm*20cm的正方形3D打印模型，分別在托盤的幾何中心、x軸負半軸距原點5cm處，y軸正半軸距原點5cm處作為萬向節的固定點，托盤的俯視圖如圖1所示。

（2）連接及傳動結構部分：在托盤的幾何中心處固定的萬向節用于連接托盤和小車平臺，在x軸和y軸處的萬向節用于連接標準舵機的力臂，使得標準舵機1和標準舵機2的力臂在轉動的同時可以分別帶動托盤繞x軸和y軸轉動，設轉動的角度分別為pitch和roll。

（3）標準舵機部分：用于承載托盤的標準舵機采用的是MG995標準金屬舵機，5.OV-6.OV、2A供電。標準舵機1的力臂擺動的平面應和托盤x軸所在直線在豎直方向上保持重合，如圖2所示。這樣的擺放位置使得力臂在擺動的過程中只會改變托盤繞x軸方向的角度pitch;標準舵機2同標準舵機1的原理相同，故不再贅述。

將上述三部分依次安裝好，得到承載結構的3D模型，如圖3所示。

1.1.2 小車驅動機構的選擇

小車使用四輪驅動，每個車輪都搭載一個扭矩為7.5KG/cm的直流圓周舵機作為車輪的驅動。

1.2 電源及驅動電路的選擇

1.2.1 電源部分

由于本系統中存在許多舵機，要求電源的放電電壓電流要足夠大，所以小車的電源采用的是3S航模電池組供電，電池組滿電電壓為12.6V，容量為11OOmAh 25C，最大放電電流為27.5A，滿足了本系統的供電要求。

1.2.2 降壓穩壓電路部分

該部分共分為兩部分。采用LM317可調降壓芯片構成，它可將由系統電源輸出的12.6V電壓降為SV輸出，為所有舵機供電;第二部分采用LM1117降壓芯片構成，它可將由LM317輸出的5V電壓降為3.3V電壓輸出，為微控制器及外圍器件供電。

1.3 微控制器及外圍器件的選擇

本系統的微控制器采用的是意法半導體公司生產的stm32f103c8t6微控制器，它的主頻為72MHz，處理速度快，對反饋的響應迅速，能夠滿足本系統的控制要求傳感器采用InvenSense公司生產的MPU6050，它是整合性6軸運動處理器件，內含3軸陀螺儀和3軸加速度傳感器，內置DMP濾波器，采用nc總線通信。通訊設備為HC-06型藍牙串口，使本系統可以通過手機上的上位機進行控制，顯示界面采用的是0.9寸OLED雙色液晶顯示屏，采用IIC總線通信。

2 系統的初始化

初始化的順序為舵機的初始化，圓周舵機初始化，MPU6050的初始化。

2.1 標準舵機的初始化

第一步，初始化定時器3，根據stm32微控制器的定時器溢出頻率為：

設定定時器3的預裝載值ARR=799，分頻系數PSC=1799，則定時器3的溢出頻率為50Hz，周期為20ms，剛好滿足MG995舵機的PWM信號脈寬周期。第二步，通過改變定時器3輸出通道OC3和004的預裝載比較值CPR X=736和CPR Y=740，此時輸出的PWM信號正好可以使舵機的角度將托盤的姿態與水平面平行。

2.2 圓周舵機的初始化

圓周舵機的PWM脈寬特性與標準舵機基本一致，所以圓周舵機的初始化同標準舵機的初始化相同。設置預裝載比較值CPR= 750時，圓周舵機靜止，作為它的初始狀態。

2.3 MPU6050的初始化

MPU6050為IIC通信， 由 于stm32f103c8t6的硬件nc在傳輸數據時不穩定，所以通過軟件的方式將普通I/O模擬為nc總線，通過nc總線配置MPU6050的內部寄存器，配置完成后MPU6050就會向nc發送原始數據，具體過程為：初始化nc、MPU6050復位、設置數據量程、設置采樣頻率，激活MPU6050。

應注意的是，要先將控制托盤角度的舵機初始化，使托盤的pitch角和roll角的初值為O。，避免MPU6050在初始化的時選取錯誤的參照系。

3 系統的控制要求及控制方法的實現

3.1 系統的控制要求

本系統的要求為當小車的姿態發生偏移時，小車上方的托盤能夠迅速響應，調整托盤始終能夠與地面保持水平，并且盡量不產生振蕩，使得托盤里運送的物品不會掉落，這就要求對托盤pitch角和roll角的控制要求調節時間短，超調量小，并且無穩態誤差。3.2 控制方法的實現

3.2.1 閉環控制系統的建立

本系統的控制器為stm32f103c8t6微控制器，執行機構為標準舵機，輸出量為托盤的角度，MPU6050輸出的角度作為負反饋，根據系統的要求，輸入的給定恒為O，系統的擾動輸入為路面和地形的改變使得輸出角度發生改變，系統的結構圖如圖4。

3.2.2 控制算法的軟件實現

軟件編寫時，通過中斷的方式完成每次的控制，即當MPU6050的中斷信號發出后，開始進行本次的PID控制調節，過程如圖5所示。

（1）控制規律的建立：在中斷服務函數中，每次中斷都會調用PID算法對本次采集的偏差進行PID運算，具體為：

biasX=O-pitch

sumX=∑biasX

speedX=biasX-last_ biasX

BalanceX_oiasX*KPX+sumX*KIX+speedX*KDX

其中：BalanceX為X軸PID的輸出量;biasX是本次采集的pitch角偏差，其數值等于0 - pitch角;sumX是本次偏差和此前所有偏差的和，作為偏差的積分量;speedX是本次偏差和上一次偏差的差值，作為偏差的微分量;KPX是X軸PID的比例系數;KIX是X軸PID的積分系數KDX是X軸PID的微分系數。

Y軸的PID與X軸的相同，故不再列舉。

（2）數據的采集和濾波：由于MPU6050的原始數據通常會含有高頻噪聲，所以系統在進行PID運算之前，應該先對MPU6050采集到的原始數據進行濾波，本系統使用的是DMP濾波器，它可將原始數據濾波后直接轉換為四元數輸出，再由簡單的運算即可獲得系統中托盤的歐拉角，大大減少了微控制器的運算量，提高系統的控制精度。

在本系統中只需得到托盤的俯仰角（pitch）和橫滾角（roll），即托盤水平方向上的歐拉角。

（3）設置中斷：由于MPU6050可以產生中斷信號，所以本系統使用外部中斷，將中斷引腳與MPU6050的INT引腳連接，設置MPU6050為5ms通過INT引腳中斷一次，使微控制器先在中斷服務函數中讀取數據，再進行PID控制，這樣可以使數據的采集和數據的處理同步，使控制效果達到最佳。

3.3

PID輸出量的處理

將本次輸出BalanceX作為一個增量，使本次標準舵機的輸出PWM為上一次的BalanceX與本次BalanceX的和。可將BalanceX理解為本次舵機轉動的速度，這個速度有方向，當BalanceX=O時，表示舵機此時刻靜止。

4 PID參數的調試

由于系統的X軸和Y軸PID參數完全相同，所以以X軸為例說明PID參數的調試過程。

4.1 平衡位置的確定

在標準舵機初始化部分已經得知，當預裝載輸出比較值CPR X=736時，pitch=O°，X軸方向處于平衡狀態。

4.2 PID參數極性的確定

在調試過程中，按照KPX，KDX，KIX的順序依次確定滿足系統控制要求的PID參數極性，經過多次試驗，若滿足系統的控制要求，則PID參數應滿足KPX>O，KDXo，任何一個參數的極性設置錯誤都會使系統在擾動信號輸入時無法穩定，使響應曲線振蕩。

4.3 KPX，KDX，KIX參數大小的確定

在KPX，KDX，KIX參數的極性確定之后，經過多次的試驗，對PID參數的大小進行了確定，調試的主要過程如表1。

由此得到KPX=1，KDX= -0.6，KIX= O.l，根據工程經驗，將結果乘以0 6，最終確定X軸方向的PID參數：KPX= 0.6，KDX =-0.36，KIX= 0.06.Y軸的PID參數同X軸的相同，在PID算法的控制下，系統的控制效果達到最佳。

5 實際控制效果

經過上述的PID參數調試，當系統經過以KPX= 0.6， KDX =-0.36， KIX= 0.06; KPY=0 6，KDY=-0.36，KIY= 0.06為參數的PID算法控制后，利用小車從水平路面駛入斜面模擬擾動信號的輸入，通過上位機繪制pitch角的實時波形觀察實際控制效果，曲線的橫坐標表示時間，縱坐標表示此時pitch的大小與方向。為了和MPU6050采集的pitch角數據保持一致，規定當托盤的Y軸正半軸部分低于水平面時，pitchO。。

以pirch角為例，在受到外界擾動輸入時系統pitch角的響應曲線如圖6所示。

在圖6中，橫坐標一格表示0.5s，縱坐標一格表示20。從圖中可以觀察到，在橫坐標t= 97.46s附近和t=98.96s附近分別輸入+27。和_270的擾動信號（即駛入駛出斜面）時，系統均能很快地調節pitch角穩定在O。附近，并且系統的動態性能指標良好，調節時間短，幾乎沒有穩態誤差和超調量，滿足了系統的所有控制要求系統的動態性能指標良好。

6 結論

本文設計了一種結構簡單易于實現的平衡運輸小車系統。運用了離散化PID算法和DMP濾波器使該系統可以滿足所有的控制要求，同時還解決了此前平衡運輸系統中存在穩態誤差的問題。在上述的試驗分析中證明，本系統具有良好的快速性和穩定性，系統中所使用的PID算法將輸出量設置為PWM信號的增量相較于直接將輸出量作為PWM信號的來說，作為增量的算法的實際控制效果更好，更符合控制要求。

參考文獻

[1]閆娜，自動化物流配送中心系統設計研究 [J].自動化與儀器儀表，2014 （12）： 22-24.[2]楊豐，周廣田，現代物流配送中心信息系統設計分析[J].信息技術，2008 （11）：11-13.

[3]紀浩，吳友強，鄒力涵.一種輪式移動機器人運動控制系統的實現[J]

.自動化儀 表，2013，34 （11）： 26-29.

[4]楊莘，劉海濤.基于STM32的兩輪自平衡遙控小車[J].數字技術與應用，2014 （05）：151-15 2.

[5]馬保政，婁文忠，于秀麗等，基于微加速 度計的易碎物品運輸監測微系統設計[J].傳感技術學報，2008，21（03）： 539-542.

[6]傅忠云，朱海霞，孫金秋等，基于慣性傳感器MPU6050的濾波算法研究[J].壓電 與聲光，201 5，37 （05）： 821-825.

[7]王君榮，陳名松，數字PID控制算法的研究和仿真[J]，大眾科技，2008 （04）： 27-2.[8]鄭成.PID參數整定方法的研究[J].機械制造，2 009，47 （03）：35-36.