吳玉明 趙華林 李小華

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

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

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

1 系統(tǒng)的建立

1.1 機(jī)械結(jié)構(gòu)的搭建

1.1.1 運(yùn)輸承載結(jié)構(gòu)的搭建

小車的承載結(jié)構(gòu)由三部分組成。

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

（2）連接及傳動(dòng)結(jié)構(gòu)部分：在托盤的幾何中心處固定的萬(wàn)向節(jié)用于連接托盤和小車平臺(tái)，在x軸和y軸處的萬(wàn)向節(jié)用于連接標(biāo)準(zhǔn)舵機(jī)的力臂，使得標(biāo)準(zhǔn)舵機(jī)1和標(biāo)準(zhǔn)舵機(jī)2的力臂在轉(zhuǎn)動(dòng)的同時(shí)可以分別帶動(dòng)托盤繞x軸和y軸轉(zhuǎn)動(dòng)，設(shè)轉(zhuǎn)動(dòng)的角度分別為pitch和roll。

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

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

1.1.2 小車驅(qū)動(dòng)機(jī)構(gòu)的選擇

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

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

1.2.1 電源部分

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

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

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

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

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

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

初始化的順序?yàn)槎鏅C(jī)的初始化，圓周舵機(jī)初始化，MPU6050的初始化。

2.1 標(biāo)準(zhǔn)舵機(jī)的初始化

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

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

2.2 圓周舵機(jī)的初始化

圓周舵機(jī)的PWM脈寬特性與標(biāo)準(zhǔn)舵機(jī)基本一致，所以圓周舵機(jī)的初始化同標(biāo)準(zhǔn)舵機(jī)的初始化相同。設(shè)置預(yù)裝載比較值CPR= 750時(shí)，圓周舵機(jī)靜止，作為它的初始狀態(tài)。

2.3 MPU6050的初始化

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

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

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

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

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

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

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

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

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

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

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ù)通常會(huì)含有高頻噪聲，所以系統(tǒng)在進(jìn)行PID運(yùn)算之前，應(yīng)該先對(duì)MPU6050采集到的原始數(shù)據(jù)進(jìn)行濾波，本系統(tǒng)使用的是DMP濾波器，它可將原始數(shù)據(jù)濾波后直接轉(zhuǎn)換為四元數(shù)輸出，再由簡(jiǎn)單的運(yùn)算即可獲得系統(tǒng)中托盤的歐拉角，大大減少了微控制器的運(yùn)算量，提高系統(tǒng)的控制精度。

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

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

3.3

PID輸出量的處理

將本次輸出BalanceX作為一個(gè)增量，使本次標(biāo)準(zhǔn)舵機(jī)的輸出PWM為上一次的BalanceX與本次BalanceX的和?？蓪alanceX理解為本次舵機(jī)轉(zhuǎn)動(dòng)的速度，這個(gè)速度有方向，當(dāng)BalanceX=O時(shí)，表示舵機(jī)此時(shí)刻靜止。

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

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

4.1 平衡位置的確定

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

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

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

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

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

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

5 實(shí)際控制效果

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

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

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

6 結(jié)論

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

參考文獻(xiàn)

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

[3]紀(jì)浩，吳友強(qiáng)，鄒力涵.一種輪式移動(dòng)機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的實(shí)現(xiàn)[J]

.自動(dòng)化儀 表，2013，34 （11）： 26-29.

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

[5]馬保政，婁文忠，于秀麗等，基于微加速 度計(jì)的易碎物品運(yùn)輸監(jiān)測(cè)微系統(tǒng)設(shè)計(jì)[J].傳感技術(shù)學(xué)報(bào)，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].機(jī)械制造，2 009，47 （03）：35-36.