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

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

（1.東華理工大學核資源與環境國家重點實驗室，南昌 330013；2.東華理工大學核技術應用教育部工程研究中心，南昌 330013）

0 引言

在實際應用中交流伺服電機主要用于精密測量臺、工業機器人、人形機器人等，其主要特點在于精密的位置控制[1]。為減少電機在啟停階段的抖動和噪聲、速度不穩定、定位不準確，就必須采用專門的加減速控制算法設計一款控制器，使得該控制器能夠指定電機按照預設速度、預設位置進行移動。

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

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

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

5段的S型加減速是根據速度、時間、加速度、加加速度和位移等物理公式所構造的分段函數。圖1和圖2分別為梯形加減速曲線和5段式S型加減速曲線。由于梯形加減速的加速度在運行過程中存在突變，因此會使得伺服電機產生沖擊和噪聲，降低了運行的平穩性[10]。而在5段式的S型加減速中，電機的加速度是連續的，不存在突變，從而可以減少沖擊，提高運行平穩性。

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

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

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

式中：a為電機運行的加速度；k為加速運動的加加速度；t為電機運行的當前時刻。

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

式中：T為電機加速過程中間點時間；t為總的加速過程時間。加加速運動與減加速運動的運行時間是總加速時間的一半。

式中：vm為電機運行的中間點速度；v0為初速度，vt為末速度。中間點速度是初速度與末速度的一半。

式中：V為電機運動的瞬時速度。

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

當瞬時速度V=Vm，且設置初速度V0=0時，將式（4）進行轉變得到式（5），進而得到恒定的k值。

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

1.2 控制算法的分段規劃

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

（1）加加速階段

由式（6）可知，若已知當前加加速段的位移S，即可求得當前加速的時刻值：

式中：i為當前加速次數對應位移；Ti為當前位移下的時刻。

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

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

（2）減加速階段

在減加速度階段中，電機運動的實際加加速度值為-k。因此相比于加加速階段，該速度表的生成類似于以初速度為Vt、加加速度為k的類減速運動。求取速度時本文將減加速階段的速度-時間進行等時離散化。

式中：Tn為類減速運動的當前加速次數時刻；temp為當前加速次數的位移；TimeDec為運行一小段位移的平均時間。

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

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

2 S型加減速的程序設計

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

圖3 整體實現框圖

2.1 速度表的生成

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

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

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

圖4 速度表的生成流程

2.2 速度曲線設置及速度表掃描

伺服電機在運動控制系統中伺服驅動器接收到控制器發送的一個脈沖就會轉動一定的角度，位移和速度均由脈沖信號確定。STM32F103ZET6 通過定時器發脈沖主要包括兩種方式，一種是PWM模式，另一種是輸出比較模式。PWM 模式輸出的脈沖信號頻率固定，占空比可變[16]。在實際控制伺服電機脈沖信號的頻率和占空比是連續變化的。PWM 模式對于控制伺服電機來說存在很大的困難和不便利，而輸出比較模式滿足頻率和占空比連續可調，便于控制伺服電機高效運轉。

在速度設置和掃描函數中，滿足速度的指標要求，首先必須保證速度表值是正確的，其次為保證運行過程中誤動作需要掃描電機當前的狀態，如若不是停止狀態則設置無效。電機的運轉方向可通過設置總脈沖的正負號來改變。為實現加速啟動、勻速、減速停止3個過程，則需要將電機速度切換點分成3 段，且加速啟動脈沖數與減速停止脈沖數相等且對稱。電機完整運行圖如圖5所示。

圖5 電機完整運行圖

電機從加速啟動到減速停止的過程中，當前脈沖值是逐一遞減的，為便于選取正確的運行狀態，減少中斷過程的計算量，應提前設定好速度切換點位置，比較當前脈沖值與速度切換點，從而判別當前所處的運行狀態。且由于運行過程中加速階段與減速階段是對稱的，則有：

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

控制流程如圖6所示。

圖6 電機狀態和速度表掃描流程

2.3 電機運行狀態切換

在狀態切換過程中，電機包含4 種狀態：加速、勻速、減速、停止，每一個狀態都對應一個脈沖值區間，判斷當前脈沖值所處的脈沖值區間，選擇不同的運行狀態。狀態之間的切換是由定時器中斷完成的，且中斷時間短，不宜加入過多的數學運算。因此需要事先將所有的速度表的值轉換成定時器計數值，并以數組的形式存放。

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

式中：Ft為單片機的時鐘頻率；V為速度表內的速度值，pulse/s；C為半周期計數值。

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

圖7 狀態切換流程

3 實驗及結果分析

為減少礦石在分選平臺加速啟動和減速停止時的撞擊，以不同速度較平穩地進入勻速區域進行礦石成分的分析，本文將設計的控制器與礦石分選平臺上的伺服電機驅動器連接，發送電機運行參數進行速度調整，以驗證5段S型算法和控制器調速的可行性，以及加減速階段和勻速階段速度平穩性的驗證。

3.1 實驗平臺

礦石分選平臺采用皮帶式線性導軌滑臺，全長4 m，電機旋轉一圈為12.5 cm，將滑臺與兩側的直線導軌固定，形成平面帶動負載。伺服電機安裝在導軌的尾部，平臺通過多個三角式的支架，使得平臺可以在穩固的前提下，節省占地空間。

3.2 速度表驗證

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

圖8 速度表擬合圖像

3.3 實際測試

在該平臺上進行控制器的實際測試，采集次數為1 000 次，單次采集周期為10 ms。圖像橫坐標單位為ms，縱坐標單位為r/min，速度反饋的速度值為正時，電機正轉，速度值為負時，電機反轉。

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

圖9 速度實測圖

4 結束語

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

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