基于余弦函數(shù)運動軌跡規(guī)劃的算法研究

周鵬 李朋 陳勝華

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

引言

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

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

1 傳統(tǒng)加減速算法

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

1.1 梯形加減速算法

梯形加減速算法是一種常用的運動規(guī)劃算法，用于實現(xiàn)平穩(wěn)的加速和減速過程，以達(dá)到穩(wěn)定運動的目的。系統(tǒng)在加減速過程中，速度沿著固定的斜率變化，其加速度保持不變，如圖1所示。

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

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

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

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

經(jīng)過分析加速開始和結(jié)束時刻，即上圖中的O點和A點，加速度從0變?yōu)閍和a變?yōu)?，即這一時刻的加加速度J為式（3）：

以加速度50mm/s，插值周期為0.002s為例，此刻的加加速度為25000mm/s2。這會導(dǎo)致在實際系統(tǒng)中出現(xiàn)電機(jī)的各種異響和振動，導(dǎo)致系統(tǒng)運行不夠平穩(wěn)。

1.2 余弦函數(shù)加減速算法

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

二者結(jié)合后的速度規(guī)劃示意圖如圖2所示。

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

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

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

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

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

對處于不同階段的位置函數(shù)求導(dǎo)可得到對應(yīng)階段的速度函數(shù)，如式（6）所示：

同理，對不同階段的速度函數(shù)求導(dǎo)得到對應(yīng)階段的加速度函數(shù)，如式（7）所示：

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

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

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

2 算法的實現(xiàn)

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

圖3 算法實現(xiàn)流程圖

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

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

表1 軌跡參數(shù)計算關(guān)鍵參數(shù)

圖4 軌跡參數(shù)計算功能塊

圖5 外部位置給定功能塊

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

3 算法的物理驗證

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

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

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

圖6 實例1運動軌跡

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

圖7 實際運行結(jié)果波形

4 結(jié)論

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