基于余弦函數運動軌跡規劃的算法研究

周鵬 李朋 陳勝華

寧波潤華全芯微電子設備有限公司 浙江 寧波 315400

引言

在自動化設備運行過程中，快速、平穩、可靠、無碰撞是對控制策略的基本要求，控制策略的優劣對設備性能的發揮至關重要，它能體現整個設備的可靠程度和工業自動化水平的高低[1]。為了保證電機控制設備在啟停過程中不產生沖擊、超調、振蕩等異常情況，在規劃電機運行速度的時候需要加入專門的加減速控制策略算法來保證加減速過程中平穩精準的控制，這也是一個控制系統能否平穩運行，精準定位的前提保證。

傳統的運動控制軌跡規劃策略中常用的加減速控制算法包括梯形加減速算法、分段加減速算法、指數型加減速算法等，這些加減速方式在系統啟動、停止和加減速開始和結束階段存在加速度突變，加加速度會很大因而會產生沖擊和異響，因此這些加減速方式不適合高速高精度的控制系統[2]。而常用的7段S型控制方法，雖然能夠提供平滑的速度和加速度，但是其步驟較為煩瑣、計算量比較大、對控制器的CPU性能要求比較高。本文提出了一種集成簡易的“S”形加減速算法，其軌跡規劃集成了簡單的梯形加減速算法和余弦加減速算法，因三角函數在時間域內具有連續可導性，所以它具備了連續的加速度和加加速度，從而避免了其突變帶來的沖擊和振蕩[3-4]。

1 傳統加減速算法

在一些要求不高的場合一般使用梯形加減速或者指數型加減速算法，其中以步進電機控制最為常見。這兩種算法的優點是運算量較小，算法實現簡單，缺點是在加減速開始和結束、機器啟動和停止階段存在明顯的機械沖擊和振動。而余弦加減速算法相較而言比較復雜。

1.1 梯形加減速算法

梯形加減速算法是一種常用的運動規劃算法，用于實現平穩的加速和減速過程，以達到穩定運動的目的。系統在加減速過程中，速度沿著固定的斜率變化，其加速度保持不變，如圖1所示。

圖1 梯形加減速速度變化示意圖

由上圖可知在定位過程中，速度變化分為加速、勻速、減速3個階段。

在加速階段，速度以固定的加速度上升，其速度如表達式（1）：

同理在減速階段，速度以固定的加速度下降，其速度如表達式（2）：

經過分析加速開始和結束時刻，即上圖中的O點和A點，加速度從0變為a和a變為0，即這一時刻的加加速度J為式（3）：

以加速度50mm/s，插值周期為0.002s為例，此刻的加加速度為25000mm/s2。這會導致在實際系統中出現電機的各種異響和振動，導致系統運行不夠平穩。

1.2 余弦函數加減速算法

考慮到梯形加減速的缺點，以及余弦函數加減速的優點，本文考慮將兩者結合，即在軌跡規劃時候仍然使用梯形加減速規劃的3個階段，即加速階段，勻速階段、減速階段[5-6]。在加速階段、減速階段的過程中引入余弦加減速算法。

二者結合后的速度規劃示意圖如圖2所示。

圖2 余弦加減速速度變化示意圖

由圖2可知，整體運動過程分為3個階段，它們的耗時分別用t1、t2、t3表示。v0為規劃的初始速度，vc為勻速運行的速度，ve為結束運行的速度。一般來說，定位時初始速度和結束速度都會設置為0。

根據上圖描述，不同階段的耗時、位移可以表述如式（4）所示：

其中，Sa、Sd、Sc分別表示加速階段、減速階段和勻速階段的位移。

計算出3個階段的執行時間和運行位移后，以時間t為自變量帶入到余弦函數中，計算出3個不同階段位移與時間的函數關系。

對處于不同階段的位置函數求導可得到對應階段的速度函數，如式（6）所示：

同理，對不同階段的速度函數求導得到對應階段的加速度函數，如式（7）所示：

由上式可知，不同階段的加速度隨著時間的變化而變化，并且在加速起始階段、減速起始階段，即t為0、t1、t3時刻，加速度是連續的[7]。正因為如此，采用余弦加減速在啟停階段不會存在加加速度突變的情況，對應的機械運動也不會存在沖擊或異響。

該算法非常適合精確的定位控制，通過對不同階段的位置函數進行積分運算可得到不同階段的位移：

由（8）式可知，對位移積分后不同階段的位移與梯形加減速完全一致，這樣就大大簡化了算法的運算量以實現目標控制。

2 算法的實現

結合上述的位置規劃公式，在倍福TwinCAT的軟件平臺上實現該算法[8]。該算法主要分為兩部分。算法流程圖如圖3所示。

圖3 算法實現流程圖

第一部分是根據設定的加速度、啟停速度（默認為0）、運行位移（相對）來計算加速時間、減速時間、勻速時間（如果存在）、加減速位移、勻速位移等參數。第二部分是以時間為主線，隨著時間的推移，設置不同運行階段的給定目標位置[9]。

根據設定的加速度、位移等運動參數計算各個階段的運行時間、位移是算法實現的重要組成部分。根據式（4）編寫了該功能塊，如圖4所示：功能塊中主要參數如表1所示：

表1 軌跡參數計算關鍵參數

圖4 軌跡參數計算功能塊

圖5 外部位置給定功能塊

其中，是否有勻速段的判斷至關重要。它將算法分為不同的兩支，位置在不同階段的給定也是不同的。在不同的階段，根據式（5）可以得到不同時間階段的位置給定。

3 算法的物理驗證

在倍福的開發軟件TwinCAT中編寫的代碼中，采用外部位置給定的功能來實現該功能[10]。

該功能塊只需要配置對應的電機和實時（每個計算周期）給定規劃位置即可控制電機按照規劃的路徑運行。設置ACC和DEC為50mm/s，Vs和Ve均設置為0。

實例1，設置勻速速度Vc為60mm/s，運行位移lr Distance為100mm。經過計算，加減速時間均為1.2s，加減速距離為36mm，且包含了勻速階段。運動過程如圖6所示。

圖6 實例1運動軌跡

實例2，設置勻速速度Vc為60mm/s，運行位移lr Distance為40mm。經過計算，加減速時間均為0.894s，加減速距離為20mm，并不包含了勻速階段,最高速度為44.7mm/s。運動過程如圖7所示。

圖7 實際運行結果波形

4 結論

本為提出了結合梯形加減速和余弦加減速的算法，能夠在運動控制系統中按照運動要求，保證系統在加減速具有高度的柔性和穩定性。同時該算法折中考慮了運行的穩定性和算法的復雜程度，將該種算法在倍福的TwinCAT平臺上做了驗證，經過實驗驗證表明這一種加減速算法能夠克服傳統的算法的缺點來得到較為理想的速度和加速度。