平衡運輸小車系統(tǒng)的設計

吳玉明 趙華林 李小華

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

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

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

1 系統(tǒng)的建立

1.1 機械結構的搭建

1.1.1 運輸承載結構的搭建

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

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

（2）連接及傳動結構部分：在托盤的幾何中心處固定的萬向節(jié)用于連接托盤和小車平臺，在x軸和y軸處的萬向節(jié)用于連接標準舵機的力臂，使得標準舵機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 小車驅(qū)動機構的選擇

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

1.2 電源及驅(qū)動電路的選擇

1.2.1 電源部分

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

1.2.2 降壓穩(wěn)壓電路部分

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

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

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

2 系統(tǒng)的初始化

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

2.1 標準舵機的初始化

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

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

2.2 圓周舵機的初始化

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

2.3 MPU6050的初始化

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

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

3 系統(tǒng)的控制要求及控制方法的實現(xiàn)

3.1 系統(tǒng)的控制要求

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

3.2.1 閉環(huán)控制系統(tǒng)的建立

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

3.2.2 控制算法的軟件實現(xiàn)

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

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

biasX=O-pitch

sumX=∑biasX

speedX=biasX-last_ biasX

BalanceX_oiasX*KPX+sumX*KIX+speedX*KDX

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

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

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

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

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

3.3

PID輸出量的處理

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

4 PID參數(shù)的調(diào)試

由于系統(tǒng)的X軸和Y軸PID參數(shù)完全相同，所以以X軸為例說明PID參數(shù)的調(diào)試過程。

4.1 平衡位置的確定

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

4.2 PID參數(shù)極性的確定

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

4.3 KPX，KDX，KIX參數(shù)大小的確定

在KPX，KDX，KIX參數(shù)的極性確定之后，經(jīng)過多次的試驗，對PID參數(shù)的大小進行了確定，調(diào)試的主要過程如表1。

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

5 實際控制效果

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

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

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

6 結論

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

參考文獻

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

[3]紀浩，吳友強，鄒力涵.一種輪式移動機器人運動控制系統(tǒng)的實現(xiàn)[J]

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

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

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

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

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