基于STM32的5段式S型交流伺服電機(jī)控制方法*

夏侯命棟，張雄杰※，王 鮑，李 東，葉煜力，馮 麗

（1.東華理工大學(xué)核資源與環(huán)境國家重點(diǎn)實(shí)驗(yàn)室，南昌 330013；2.東華理工大學(xué)核技術(shù)應(yīng)用教育部工程研究中心，南昌 330013）

0 引言

在實(shí)際應(yīng)用中交流伺服電機(jī)主要用于精密測量臺(tái)、工業(yè)機(jī)器人、人形機(jī)器人等，其主要特點(diǎn)在于精密的位置控制[1]。為減少電機(jī)在啟停階段的抖動(dòng)和噪聲、速度不穩(wěn)定、定位不準(zhǔn)確，就必須采用專門的加減速控制算法設(shè)計(jì)一款控制器，使得該控制器能夠指定電機(jī)按照預(yù)設(shè)速度、預(yù)設(shè)位置進(jìn)行移動(dòng)。

加減速控制算法的使用已較為普遍，主要包括：指數(shù)型模式、線性模式、S 型模式，多項(xiàng)式加減速[2-3]。指數(shù)加減速算法實(shí)現(xiàn)啟停時(shí)速度突變按照指數(shù)規(guī)律上升或下降，肖立娜[4]將傳統(tǒng)的指數(shù)加減速算法進(jìn)行改進(jìn)，提出微小程序段間的速度平滑指數(shù)加減速算法，可實(shí)現(xiàn)微小程序段之間的連續(xù)加工，減少加減速控制次數(shù)，提高運(yùn)行效率。方佳偉[5]提出基于Sigmoid 函數(shù)S型加減速算法，此算法通過時(shí)間和速度數(shù)組的遍歷，減少了機(jī)械設(shè)備啟停的抖動(dòng)時(shí)間。黃艷[6]在2005 年提出一種7 段式的S型曲線加減速算法，該算法可以獲得平滑的速度和加速度，減少電機(jī)在運(yùn)行過程中產(chǎn)生不希望的振動(dòng)。鐘前進(jìn)[7]在2019 年提出了一種新型的S 型曲線加減速算法，該方法是利用三角函數(shù)在梯形速度軌跡上擬合一條S 型速度曲線，該算法簡單且能夠輸出連續(xù)的速度和加速度。徐川[8]提出一種多項(xiàng)式加減速控制方法，該多項(xiàng)式包含11個(gè)速度調(diào)整階段，使得系統(tǒng)具有更高的加減速柔性。

7 段式和11 段式的加減速算法，雖然可以實(shí)現(xiàn)較好地加減速，但分段公式多，運(yùn)算量較大。為降低計(jì)算復(fù)雜度，保證速度柔性[9]，本文提出5 段的S 型加減速算法。相比于指數(shù)型和線性模式，可以減小啟動(dòng)和停止過程中的噪聲，速度也趨于平滑。采用位移和速度數(shù)組遍歷查詢的方式進(jìn)行程序設(shè)計(jì)，可實(shí)現(xiàn)電機(jī)速度、加減速時(shí)間、位移可調(diào)的加速啟動(dòng)、勻速運(yùn)行、減速停止。將該算法運(yùn)用到STM32 單片機(jī)上實(shí)現(xiàn)對電機(jī)速度和位移進(jìn)行調(diào)整和驗(yàn)證。

1 5段式S型加減速控制算法1.1 5段式S型控制算法原理

5段的S型加減速是根據(jù)速度、時(shí)間、加速度、加加速度和位移等物理公式所構(gòu)造的分段函數(shù)。圖1和圖2分別為梯形加減速曲線和5段式S型加減速曲線。由于梯形加減速的加速度在運(yùn)行過程中存在突變，因此會(huì)使得伺服電機(jī)產(chǎn)生沖擊和噪聲，降低了運(yùn)行的平穩(wěn)性[10]。而在5段式的S型加減速中，電機(jī)的加速度是連續(xù)的，不存在突變，從而可以減少?zèng)_擊，提高運(yùn)行平穩(wěn)性。

圖1 梯形加減速曲線示意圖

圖2 5段式S型加減速曲線示意圖

如圖2（a）中所示t1階段是加加速階段，t2階段為減加速階段，t3為勻速階段，t4為加減速階段，t5為減減速階段，Vm為中間點(diǎn)速度；圖2（b）中t階段為加速過程加速度曲線。

式中：a為電機(jī)運(yùn)行的加速度；k為加速運(yùn)動(dòng)的加加速度；t為電機(jī)運(yùn)行的當(dāng)前時(shí)刻。

式（1）中加加速度恒定為k，加速度是一條根據(jù)時(shí)間變化的直線。

式中：T為電機(jī)加速過程中間點(diǎn)時(shí)間；t為總的加速過程時(shí)間。加加速運(yùn)動(dòng)與減加速運(yùn)動(dòng)的運(yùn)行時(shí)間是總加速時(shí)間的一半。

式中：vm為電機(jī)運(yùn)行的中間點(diǎn)速度；v0為初速度，vt為末速度。中間點(diǎn)速度是初速度與末速度的一半。

式中：V為電機(jī)運(yùn)動(dòng)的瞬時(shí)速度。

式（4）對式（1）進(jìn)行積分求得某一時(shí)刻的瞬時(shí)速度，因此式（4）中速度的改變是由時(shí)間所確定的。

當(dāng)瞬時(shí)速度V=Vm，且設(shè)置初速度V0=0時(shí)，將式（4）進(jìn)行轉(zhuǎn)變得到式（5），進(jìn)而得到恒定的k值。

式中：S為總的位移，通過對瞬時(shí)速度進(jìn)行積分即可得到。因此運(yùn)行過程中總位移S的值是由加加速度k和運(yùn)行時(shí)間t共同決定。

1.2 控制算法的分段規(guī)劃

在5 段式S 型加減速中加速階段和減速階段是對稱的，加速階段速度表順序取值，減速階段速度表逆序取值，勻速階段則是以加速階段最大速度運(yùn)行，因此只需建立加速階段的速度表。

（1）加加速階段

由式（6）可知，若已知當(dāng)前加加速段的位移S，即可求得當(dāng)前加速的時(shí)刻值：

式中：i為當(dāng)前加速次數(shù)對應(yīng)位移；Ti為當(dāng)前位移下的時(shí)刻。

將Ti代入到式（4）則可求得速度的變化量dV：

存入數(shù)組的速度值即為：Speed[n]=V0+dV，0≤n＜加加速階段次數(shù)。

（2）減加速階段

在減加速度階段中，電機(jī)運(yùn)動(dòng)的實(shí)際加加速度值為-k。因此相比于加加速階段，該速度表的生成類似于以初速度為Vt、加加速度為k的類減速運(yùn)動(dòng)。求取速度時(shí)本文將減加速階段的速度-時(shí)間進(jìn)行等時(shí)離散化。

式中：Tn為類減速運(yùn)動(dòng)的當(dāng)前加速次數(shù)時(shí)刻；temp為當(dāng)前加速次數(shù)的位移；TimeDec為運(yùn)行一小段位移的平均時(shí)間。

將時(shí)間Tn代入式（4）則有速度變化量dV：

存入數(shù)組的速度值即為：Speed[n]=Vt-dV，加加速階段次數(shù)＜n＜最大加速次數(shù)。將加加速階段和減加速階段構(gòu)成的速度表從小到大依次排列和組合，因此可以得到整個(gè)加速階段速度表為Speed[n]。

2 S型加減速的程序設(shè)計(jì)

傳統(tǒng)的伺服控制器多以DSP 為主，對于一些成本低和體積小的就不在適用于DSP[11-12]。DSP 對電機(jī)控制有很大的優(yōu)化，但不利于伺服系統(tǒng)功能擴(kuò)展[13]。且DSP 多以現(xiàn)場進(jìn)行速度位移計(jì)算。本文的主控制芯片為STM32F103ZET6，通過發(fā)送有限脈沖和控制脈沖頻率以改變伺服電機(jī)的位移和速度。由于發(fā)送脈沖過程中的間隔時(shí)間短，為減少期間的計(jì)算，因此需要提前創(chuàng)建好速度表，同時(shí)減少定時(shí)精度造成的控制精度[14]。且由于單片機(jī)的內(nèi)存有限、若每個(gè)脈沖值都有對應(yīng)的速度值，就易出現(xiàn)單片機(jī)內(nèi)存不足或速度表數(shù)量龐大。為減少掃描時(shí)間和計(jì)算時(shí)間，本文提出以空間換速度的方式，設(shè)定加速次數(shù)以增大速度表取值時(shí)脈沖間隔，減少掃描次數(shù)，求取當(dāng)前加速次數(shù)的速度值來減少速度表的容量，提高運(yùn)行效率。整體實(shí)現(xiàn)框圖如圖3所示。

圖3 整體實(shí)現(xiàn)框圖

2.1 速度表的生成

交流伺服電機(jī)包括位置模式、速度模式和轉(zhuǎn)矩模式[15]。本文選用的是位置控制模式，該模式中速度是由單位時(shí)間內(nèi)發(fā)送的脈沖個(gè)數(shù)即脈沖頻率決定的，脈沖總量是電機(jī)位移量。為便于程序設(shè)計(jì)，控制器設(shè)置并寫入單圈脈沖數(shù)為6 400 pulse/rev。

根據(jù)5 段S 型算法原理和分段規(guī)劃，設(shè)置外部參數(shù):初末速度、加速時(shí)間、加速次數(shù)計(jì)算出加速段總脈沖數(shù)、加速脈沖時(shí)間間隔、單次加速脈沖個(gè)數(shù)。即可計(jì)算出速度表Speed[n]。

速度表的生成流程如圖4所示，通過圖4計(jì)算不同參數(shù)下的速度表，使得控制器可以掃描和取值進(jìn)行速度的切換和狀態(tài)的改變。

圖4 速度表的生成流程

2.2 速度曲線設(shè)置及速度表掃描

伺服電機(jī)在運(yùn)動(dòng)控制系統(tǒng)中伺服驅(qū)動(dòng)器接收到控制器發(fā)送的一個(gè)脈沖就會(huì)轉(zhuǎn)動(dòng)一定的角度，位移和速度均由脈沖信號確定。STM32F103ZET6 通過定時(shí)器發(fā)脈沖主要包括兩種方式，一種是PWM模式，另一種是輸出比較模式。PWM 模式輸出的脈沖信號頻率固定，占空比可變[16]。在實(shí)際控制伺服電機(jī)脈沖信號的頻率和占空比是連續(xù)變化的。PWM 模式對于控制伺服電機(jī)來說存在很大的困難和不便利，而輸出比較模式滿足頻率和占空比連續(xù)可調(diào)，便于控制伺服電機(jī)高效運(yùn)轉(zhuǎn)。

在速度設(shè)置和掃描函數(shù)中，滿足速度的指標(biāo)要求，首先必須保證速度表值是正確的，其次為保證運(yùn)行過程中誤動(dòng)作需要掃描電機(jī)當(dāng)前的狀態(tài)，如若不是停止?fàn)顟B(tài)則設(shè)置無效。電機(jī)的運(yùn)轉(zhuǎn)方向可通過設(shè)置總脈沖的正負(fù)號來改變。為實(shí)現(xiàn)加速啟動(dòng)、勻速、減速停止3個(gè)過程，則需要將電機(jī)速度切換點(diǎn)分成3 段，且加速啟動(dòng)脈沖數(shù)與減速停止脈沖數(shù)相等且對稱。電機(jī)完整運(yùn)行圖如圖5所示。

圖5 電機(jī)完整運(yùn)行圖

電機(jī)從加速啟動(dòng)到減速停止的過程中，當(dāng)前脈沖值是逐一遞減的，為便于選取正確的運(yùn)行狀態(tài)，減少中斷過程的計(jì)算量，應(yīng)提前設(shè)定好速度切換點(diǎn)位置，比較當(dāng)前脈沖值與速度切換點(diǎn)，從而判別當(dāng)前所處的運(yùn)行狀態(tài)。且由于運(yùn)行過程中加速階段與減速階段是對稱的，則有：

圖5 和式（11）（12）中：TPulse為行程總脈沖；ATpulse為加速段總脈沖；UVpoint為勻速切換點(diǎn)；DPoint為減速切換點(diǎn)。

控制流程如圖6所示。

圖6 電機(jī)狀態(tài)和速度表掃描流程

2.3 電機(jī)運(yùn)行狀態(tài)切換

在狀態(tài)切換過程中，電機(jī)包含4 種狀態(tài)：加速、勻速、減速、停止，每一個(gè)狀態(tài)都對應(yīng)一個(gè)脈沖值區(qū)間，判斷當(dāng)前脈沖值所處的脈沖值區(qū)間，選擇不同的運(yùn)行狀態(tài)。狀態(tài)之間的切換是由定時(shí)器中斷完成的，且中斷時(shí)間短，不宜加入過多的數(shù)學(xué)運(yùn)算。因此需要事先將所有的速度表的值轉(zhuǎn)換成定時(shí)器計(jì)數(shù)值，并以數(shù)組的形式存放。

STM32F103ZET6單片機(jī)主頻為72 MHz，定時(shí)器采用高級定時(shí)器TIM1用于脈沖的產(chǎn)生，其模式為輸出比較下的翻轉(zhuǎn)模式，當(dāng)計(jì)數(shù)值達(dá)到比較值時(shí)對應(yīng)通道引腳會(huì)翻轉(zhuǎn)電平，且半周期翻轉(zhuǎn)一次。電機(jī)方向引腳選用PB13。為得到實(shí)際時(shí)鐘頻率2 MHz，預(yù)分頻系數(shù)為35，為方便計(jì)算將實(shí)際的時(shí)鐘頻率值擴(kuò)大10 倍。電機(jī)轉(zhuǎn)速單位為0.1 r/s，為實(shí)現(xiàn)實(shí)際轉(zhuǎn)速與定時(shí)器計(jì)數(shù)值匹配，則需將電機(jī)轉(zhuǎn)速與單圈脈沖數(shù)相乘。根據(jù)時(shí)鐘頻率與速度表的關(guān)系即可求得定時(shí)器的計(jì)數(shù)值。

式中：Ft為單片機(jī)的時(shí)鐘頻率；V為速度表內(nèi)的速度值，pulse/s；C為半周期計(jì)數(shù)值。

因此C的值最后都被按順序存放入數(shù)組中，此數(shù)組的值即為CCR 寄存器的值。通過獲取CNT 的值和重新設(shè)置CCR 寄存器的值，來改變占空比和頻率。狀態(tài)切換流程如圖7所示。

圖7 狀態(tài)切換流程

3 實(shí)驗(yàn)及結(jié)果分析

為減少礦石在分選平臺(tái)加速啟動(dòng)和減速停止時(shí)的撞擊，以不同速度較平穩(wěn)地進(jìn)入勻速區(qū)域進(jìn)行礦石成分的分析，本文將設(shè)計(jì)的控制器與礦石分選平臺(tái)上的伺服電機(jī)驅(qū)動(dòng)器連接，發(fā)送電機(jī)運(yùn)行參數(shù)進(jìn)行速度調(diào)整，以驗(yàn)證5段S型算法和控制器調(diào)速的可行性，以及加減速階段和勻速階段速度平穩(wěn)性的驗(yàn)證。

3.1 實(shí)驗(yàn)平臺(tái)

礦石分選平臺(tái)采用皮帶式線性導(dǎo)軌滑臺(tái)，全長4 m，電機(jī)旋轉(zhuǎn)一圈為12.5 cm，將滑臺(tái)與兩側(cè)的直線導(dǎo)軌固定，形成平面帶動(dòng)負(fù)載。伺服電機(jī)安裝在導(dǎo)軌的尾部，平臺(tái)通過多個(gè)三角式的支架，使得平臺(tái)可以在穩(wěn)固的前提下，節(jié)省占地空間。

3.2 速度表驗(yàn)證

為驗(yàn)證速度表的正確性，將串口打印的速度值進(jìn)行整理，并將速度點(diǎn)進(jìn)行擬合得出相應(yīng)的速度表。與設(shè)定值進(jìn)行誤差分析。圖8 所示為部分速度表的速度圖像。圖中橫坐標(biāo)為加速次數(shù)，縱坐標(biāo)為速度，初速度均為0，加速總次數(shù)為250 次。由于5段式的S型加減速度是將加速過程分成兩段，時(shí)間與加速次數(shù)轉(zhuǎn)換過程中存在細(xì)小的時(shí)間差、因此存在較小不平滑的速度現(xiàn)象。經(jīng)過計(jì)算圖（a）（b）（c）（d）速度表內(nèi)最大速度分別為99.97×0.1 r/s，199.99×0.1 r/s，249.99×0.1 r/s，299.99×0.1 r/s，與實(shí)際設(shè)置的速度，誤差在0.1%以下。因此速度表構(gòu)建成立。

圖8 速度表擬合圖像

3.3 實(shí)際測試

在該平臺(tái)上進(jìn)行控制器的實(shí)際測試，采集次數(shù)為1 000 次，單次采集周期為10 ms。圖像橫坐標(biāo)單位為ms，縱坐標(biāo)單位為r/min，速度反饋的速度值為正時(shí)，電機(jī)正轉(zhuǎn)，速度值為負(fù)時(shí)，電機(jī)反轉(zhuǎn)。

圖9 為兩種不同速度下的采集圖像。經(jīng)計(jì)算圖中實(shí)際運(yùn)行速度下的速度波動(dòng)在15 r/min 上下范圍內(nèi)，小范圍內(nèi)波動(dòng)平穩(wěn)，因此噪聲和抖動(dòng)較小。加減速時(shí)間較短，速度反饋的圖像近似于梯形，設(shè)定位移為30 rad，由梯形的面積公式求得總位移，整個(gè)運(yùn)行過程定位誤差在0～2 rad 左右的范圍內(nèi)。勻速區(qū)域是礦石分選平臺(tái)的探測區(qū)域，因此只需在探測區(qū)域前達(dá)到指定速度，且加速時(shí)間極短，使得定位誤差幾乎可以忽略。因而該控制方法適用于礦石分選平臺(tái)的控制。

圖9 速度實(shí)測圖

4 結(jié)束語

本文設(shè)計(jì)了一款5段式S型加減速的交流伺服電機(jī)控制器。將5 段式S 型加減速算法改進(jìn)后運(yùn)用到STM32 上，通過設(shè)置加速次數(shù)的方式代替速度表內(nèi)呈現(xiàn)每一個(gè)脈沖值的速度。減少單片機(jī)的掃描時(shí)間、速度表計(jì)算的時(shí)間、中斷過程處理的時(shí)間。采用位移與速度遍歷查詢的方式進(jìn)行程序設(shè)計(jì)，并將該控制器運(yùn)用到礦石分選平臺(tái)上進(jìn)行實(shí)際控制。

實(shí)驗(yàn)結(jié)果表明：本文改進(jìn)后的S 型加減速控制算法，對基于STM32F103 這種低成本、小型化、易擴(kuò)展而設(shè)計(jì)的控制器具有適用性，為以后嵌入其他算法而設(shè)計(jì)的單片機(jī)-伺服控制器提供了參考依據(jù)。