基于新型速度規(guī)劃的優(yōu)化算法研究*

茍凌銘，唐樟春,2※，李征泰，高 偉

（1.電子科技大學(xué)機(jī)械與電氣工程學(xué)院，成都 611731；2.電子科技大學(xué)廣東電子信息工程研究院，廣東東莞 523808）

0 引言

運(yùn)動(dòng)控制的要求要減小運(yùn)動(dòng)控制系統(tǒng)在啟動(dòng)、停止、加速和減速運(yùn)動(dòng)階段的振動(dòng)與沖擊[1]，因此運(yùn)動(dòng)控制規(guī)劃中加減速的規(guī)劃設(shè)計(jì)與插補(bǔ)是運(yùn)動(dòng)控制系統(tǒng)性能的保證。本文基于一種優(yōu)化后的新型加減速規(guī)劃算法為計(jì)算理論，以時(shí)間分割思想為基礎(chǔ)，采用以路徑誤差為核心，細(xì)分路徑過(guò)程的方式來(lái)進(jìn)行插補(bǔ)運(yùn)算，從而實(shí)現(xiàn)規(guī)劃約束中速度與加速度的連續(xù)性與平滑性。Wu等[2-3]對(duì)梯形、S型和三角函數(shù)等加減速控制算法進(jìn)行理論研究。梯形速度規(guī)劃控制算法原理簡(jiǎn)單、比較易于實(shí)現(xiàn)[4]，但規(guī)劃計(jì)算結(jié)果速度不夠光滑，加速度突變，嚴(yán)重時(shí)會(huì)導(dǎo)致丟步[5]。S型規(guī)劃能解決加速度突變，為應(yīng)對(duì)特殊情況下S型曲線使用拉式逼近法來(lái)求解完整S型規(guī)劃[6]，但算法計(jì)算量較大，并不適合在一般嵌入式平臺(tái)使用。三角函數(shù)型加加速度軌跡規(guī)劃要具有更好的邊界值控制能力，并且插補(bǔ)曲線更加平滑[7-9]。而提出的梯形與余弦想結(jié)合的新型算法內(nèi)容，提供了一種新的計(jì)算思路[10]。在新思路下應(yīng)用到前瞻處理中，對(duì)運(yùn)動(dòng)拐點(diǎn)采用新運(yùn)動(dòng)公式來(lái)設(shè)計(jì)拐點(diǎn)處的加減速段[11]，但對(duì)于算法使用存在限制，在一些特殊條件限制下，規(guī)劃算法并不能輸出想要的結(jié)果。為此本文提出一種對(duì)新型速度規(guī)劃進(jìn)行優(yōu)化的方法，解決之前方法的局限性，使速度規(guī)劃算法能更靈活地運(yùn)用于實(shí)際條件，并提高響應(yīng)速度，對(duì)應(yīng)用該加減速運(yùn)動(dòng)規(guī)劃算法解決工業(yè)上要求快速平滑響應(yīng)的工程問(wèn)題具有重要的現(xiàn)實(shí)意義。

1 新型曲線速度優(yōu)化原理分析

鐘前進(jìn)[10]提出的新型S加減速算法在對(duì)位移積分后，計(jì)算公式仍然與梯形加減速一樣，如此就大大簡(jiǎn)化了軌跡規(guī)劃及速度前瞻時(shí)的運(yùn)算量，從而大大簡(jiǎn)化了算法，而且運(yùn)算量相對(duì)來(lái)說(shuō)比較小，非常適合嵌入式的運(yùn)動(dòng)控制，優(yōu)化后的位移公式為：

在提出的新型S型加減速規(guī)劃算法中是以梯形思想為基礎(chǔ)進(jìn)行分析與處理的，在梯形計(jì)算過(guò)程中核心思想在于根據(jù)約束的加減速度計(jì)算出運(yùn)動(dòng)的最大瞬時(shí)速度從而判定梯形規(guī)劃是兩段無(wú)勻速運(yùn)動(dòng)還是三段有勻速運(yùn)動(dòng)，結(jié)合實(shí)際多段連續(xù)短軌跡情況下也可能會(huì)存在一些極端加減速情況，比如只有加速段與勻速段、只有勻速段與減速段和只有勻速段的情況，甚至只有減速段的情況。在采用的基于時(shí)間與路徑例化插補(bǔ)算法中，最重要的就是在每個(gè)插補(bǔ)狀態(tài)對(duì)瞬時(shí)速度的例化，而例化公式是依據(jù)當(dāng)前瞬時(shí)速度對(duì)時(shí)間與位移的映射關(guān)系來(lái)確定的，因此本文就是對(duì)瞬時(shí)速度對(duì)時(shí)間與位移的映射關(guān)系進(jìn)行求解。對(duì)于極端情況下的處理思想采用判斷加速運(yùn)行時(shí)間與減速運(yùn)行時(shí)間是否是零即可確定當(dāng)前運(yùn)動(dòng)歸類，瞬時(shí)速度映射關(guān)系則可根據(jù)勻速時(shí)速度關(guān)系式得到。對(duì)于不能達(dá)到勻速狀態(tài)的情況則只能加速與減速，本文對(duì)不能達(dá)到勻速狀態(tài)的運(yùn)動(dòng)實(shí)時(shí)速度、位移與時(shí)間之間的映射關(guān)系進(jìn)行推導(dǎo)，并以此為依據(jù)實(shí)現(xiàn)在時(shí)間軸上的映射。

由于梯形運(yùn)動(dòng)是直線加速度運(yùn)動(dòng)，滿足直線運(yùn)動(dòng)速度公式：

式中：Vc為當(dāng)前實(shí)時(shí)速度；Va為起始速度；ta為加速運(yùn)動(dòng)的時(shí)間；Sa為加速運(yùn)動(dòng)的位移。

由速度公式可得加速度的關(guān)系式為：

根據(jù)當(dāng)前運(yùn)動(dòng)過(guò)程中直線加速運(yùn)動(dòng)速度與位移的關(guān)系式可得：

式中：Vc為停止速度；Sd為減速運(yùn)動(dòng)位移。

由Sa+Sd=Sp（其中Sp是總位移）和式（2）結(jié)合代入公式（4）化簡(jiǎn)可得加速度關(guān)系式：

運(yùn)動(dòng)過(guò)程中加速度值是一致的，即式（3）與式（5）相等，代入可得速度與時(shí)間、位移之間的映射關(guān)系為：

上式即為無(wú)勻速段運(yùn)動(dòng)下加減速值一致時(shí)瞬時(shí)速度對(duì)時(shí)間與位移的映射關(guān)系式，該關(guān)系式適用于加減速的全過(guò)程。而當(dāng)運(yùn)動(dòng)過(guò)程中加速度與減速度不一致時(shí)，則根據(jù)運(yùn)動(dòng)一般公式可知加速過(guò)程與減速過(guò)程的瞬時(shí)速度映射關(guān)系式為：

而對(duì)于能夠達(dá)到勻速狀態(tài)的運(yùn)動(dòng)過(guò)程分析就比較簡(jiǎn)單，在運(yùn)動(dòng)規(guī)劃過(guò)程中能夠求得勻速運(yùn)動(dòng)位移Su與勻速運(yùn)動(dòng)時(shí)間tu，從而根據(jù)位移與速度公式就能求得瞬時(shí)速度的映射公式：

2 例化優(yōu)化算法實(shí)現(xiàn)

對(duì)于梯形速度規(guī)劃，對(duì)時(shí)間進(jìn)行時(shí)間軸映射時(shí)默認(rèn)加減速時(shí)間是一致的即對(duì)稱運(yùn)動(dòng)，非對(duì)稱運(yùn)動(dòng)對(duì)時(shí)間軸例化是不能單一而談的，在運(yùn)動(dòng)規(guī)劃的一般條件下微小段距離運(yùn)動(dòng)規(guī)劃會(huì)比較多，而在實(shí)際微小段運(yùn)動(dòng)中經(jīng)過(guò)前瞻處理后速度的約束條件會(huì)存在比較多的復(fù)雜情況，而對(duì)于復(fù)雜情況下的速度規(guī)劃不能單一簡(jiǎn)化，本文提出的思路則是對(duì)加速階段與減速階段進(jìn)行單獨(dú)分析，規(guī)劃結(jié)果以規(guī)劃能達(dá)到的最大速度為連接點(diǎn)與著手點(diǎn)，對(duì)加速階段的瞬時(shí)速度進(jìn)行時(shí)間與位移例化，并對(duì)減速階段的瞬時(shí)速度也進(jìn)行時(shí)間與位移的例化。本文基于一般非對(duì)稱運(yùn)動(dòng)為核心，給出非對(duì)稱時(shí)間軸例化一般式，如此既能滿足對(duì)稱速度規(guī)劃也能滿足非對(duì)稱速度規(guī)劃，在插補(bǔ)過(guò)程中，計(jì)算的核心就在于求解插補(bǔ)的例化因子λ：

其中起點(diǎn)坐標(biāo)是(xs，ys)，目標(biāo)點(diǎn)坐標(biāo)是(x，y)，為直線運(yùn)動(dòng)規(guī)劃時(shí)的插補(bǔ)位移求解公式，若為圓弧運(yùn)動(dòng)則插補(bǔ)公式為：

式中：r為圓弧運(yùn)動(dòng)的半徑；θ為圓弧運(yùn)動(dòng)的圓心角；flag為運(yùn)算符號(hào)，由圓弧運(yùn)動(dòng)的順逆方向決定，逆時(shí)針時(shí)為正，順時(shí)針為負(fù)，核心共性都是對(duì)插補(bǔ)例化因子λ進(jìn)行求解。

對(duì)插補(bǔ)例化因子的求解步驟如下。

（1）由設(shè)定的最大速度求解加速度段位移與減速度段位移以及兩段位移之和。

（2）若總位移比步驟（1）求解位移和大則到步驟（5），若總位移等于求解的兩段位移和，則說(shuō)明是剛好達(dá)到最大速度，否則需要計(jì)算能達(dá)到的最大速度。

（3）求加速時(shí)間ta、減速時(shí)間td、加速運(yùn)動(dòng)位移Sa、減速運(yùn)動(dòng)位移Sd，并獲得例化參數(shù)：

此時(shí)若加速時(shí)間ta=0，則表明運(yùn)動(dòng)過(guò)程為極端情況，此時(shí)沒(méi)有加速段，則在加速過(guò)程中瞬時(shí)速度例化為零，此時(shí)速度的例化參數(shù)為：

式中：Vcaf、Vcdf、Vaf、Vef分別為加速段速度、減速段速度、起始速度、結(jié)束速度的例化參數(shù)。

若減速時(shí)間td=0，此時(shí)沒(méi)有減速段，則在減速過(guò)程中瞬時(shí)速度例化為零，此時(shí)速度的例化參數(shù)為：

當(dāng)起始速度與結(jié)束速度相等時(shí)，此時(shí)運(yùn)動(dòng)具有特殊性，但依據(jù)加減速運(yùn)動(dòng)段單獨(dú)分析方式，此時(shí)速度的例化參數(shù)為：

而當(dāng)起止速度非對(duì)稱時(shí)，結(jié)合式（6）可得速度的例化參數(shù)為：

（4）由式（1）作簡(jiǎn)單變換得到無(wú)勻速段梯形余弦計(jì)算位移公式，可得優(yōu)化后的插補(bǔ)例化因子為：

式中：t為當(dāng)前插補(bǔ)點(diǎn)的狀態(tài)時(shí)刻，且t=,i=1,2,3,…,N，

其中N代表總的插值點(diǎn)數(shù)。

通過(guò)對(duì)總的插值點(diǎn)數(shù)進(jìn)行控制就能間接控制每個(gè)時(shí)刻的插補(bǔ)點(diǎn)的狀態(tài)時(shí)刻的運(yùn)動(dòng)狀態(tài)，從而實(shí)現(xiàn)細(xì)分化的插補(bǔ)路徑控制，其中插值點(diǎn)數(shù)的計(jì)算方式為：

式中：C為插值倍增系數(shù)，用來(lái)倍增插補(bǔ)點(diǎn)數(shù)；S為運(yùn)動(dòng)總位移或圓弧運(yùn)動(dòng)的圓心角；V為運(yùn)動(dòng)的線速度或圓弧運(yùn)動(dòng)的角速度。

（5）同步驟（3），先求加速時(shí)間ta、勻速時(shí)間tu、減速時(shí)間td、加速位移Sa、減速位移Sd，然后可求得例化參數(shù)為：

式中：總位移為滿足Sp=Sa+Su+Sd的當(dāng)前所有軸運(yùn)動(dòng)規(guī)劃的路徑總數(shù)。

在運(yùn)動(dòng)中若加速時(shí)間ta=0，則起始速度例化參數(shù)Vaf=0，否則起始速度例化參數(shù)；而若減速時(shí)間td=0，則結(jié)束速度例化參數(shù)Vef=Vcf，否則結(jié)束速度例化參數(shù)為Vef=。在求得起止速度例化參數(shù)后結(jié)合例化參數(shù)式（18）并代入式（1）可得插補(bǔ)例化因子為：

優(yōu)化后的以上5個(gè)步驟流程能將實(shí)際情況所有條件下的規(guī)劃情況都涵蓋，在每一個(gè)插補(bǔ)點(diǎn)時(shí)刻i都會(huì)有唯一的插補(bǔ)例化因子與其對(duì)應(yīng)，將例化因子代入式（9）中就能求得在每一個(gè)插補(bǔ)時(shí)刻點(diǎn)的坐標(biāo)位置信息，并通過(guò)對(duì)當(dāng)前的新型路徑因子計(jì)算式進(jìn)行求導(dǎo)就能得到當(dāng)前的狀態(tài)時(shí)刻的速度，結(jié)合對(duì)當(dāng)前路徑輸出求出當(dāng)前插補(bǔ)點(diǎn)坐標(biāo)，細(xì)分到每個(gè)運(yùn)動(dòng)輸出上就能得到對(duì)當(dāng)前速度規(guī)劃應(yīng)該輸出的速度與脈沖個(gè)數(shù)。

3 實(shí)驗(yàn)驗(yàn)證分析

在仿真平臺(tái)MATLAB上根據(jù)以上步驟流程編寫(xiě)算法與測(cè)試代碼，同時(shí)仿真S型規(guī)劃、梯形規(guī)劃與優(yōu)化后的新型S型規(guī)劃，仿真數(shù)據(jù)通過(guò)驗(yàn)證不同的運(yùn)行距離，比較3種規(guī)劃算法的輸出曲線，為了讓三者能有明顯的對(duì)比，測(cè)試在設(shè)計(jì)時(shí)保證了3種規(guī)劃算法在插補(bǔ)時(shí)具有相同的插補(bǔ)數(shù)，這樣在每一個(gè)插補(bǔ)點(diǎn)的結(jié)果是在相同時(shí)刻的規(guī)劃輸出。

（1）驗(yàn)證參數(shù)數(shù)據(jù)為Va=0 mm/s，Vc=60 mm/s，Ve=10 mm/s，Sp=100 mm，最大加速度為30 mm/s2。此時(shí)梯形速度規(guī)劃有兩個(gè)階段，S型規(guī)劃分為4段，對(duì)應(yīng)的速度與加速度曲線如圖1所示。

圖1 條件（1）速度及加速度曲線

（2）驗(yàn)證參數(shù)數(shù)據(jù)為Va=0 mm/s，Vc=60 mm/s，Ve=10 mm/s，Sp=600 mm，最大加速度為30 mm/s2。此時(shí)梯形速度規(guī)劃有3個(gè)階段，S型規(guī)劃分為5段，對(duì)應(yīng)的速度與加速度曲線如圖2所示。

圖2 條件（2）速度及加速度曲線

（3）驗(yàn)證參數(shù)數(shù)據(jù)為Va=0 mm/s，Vc=100 mm/s，Ve=10 mm/s，Sp=800 mm，最大加速度為30 mm/s2。此時(shí)梯形速度規(guī)劃有3個(gè)階段，S型規(guī)劃分為7段，對(duì)應(yīng)的速度與加速度曲線如圖3所示。

圖3 條件（3）速度及加速度曲線

由以上3組驗(yàn)證數(shù)據(jù)的結(jié)果圖顯示，在非對(duì)稱的一般情況下，優(yōu)化后的新型速度規(guī)劃算法都能根據(jù)約束條件靈活地計(jì)算輸出較好的結(jié)果，并且在同等約束條件下優(yōu)化后的算法還能以更平滑的加速度進(jìn)行更快地響應(yīng)。

4 結(jié)束語(yǔ)

在以速度規(guī)劃為核心的控制中，被控對(duì)象的運(yùn)動(dòng)狀態(tài)要求盡可能地平穩(wěn)，也就是驅(qū)動(dòng)被控對(duì)象的電機(jī)輸出速度曲線要足夠的光滑，運(yùn)動(dòng)加減速過(guò)程要平滑，不能有突變。為適應(yīng)可移植運(yùn)算在嵌入式平臺(tái)復(fù)雜度的運(yùn)算，并適用于所有運(yùn)動(dòng)條件下的運(yùn)動(dòng)規(guī)劃，本文對(duì)提出的一種可移植到嵌入式平臺(tái)的新型S曲線加減速算法進(jìn)行了優(yōu)化改進(jìn)，提升了原算法計(jì)算的靈活性，能夠適配所有的運(yùn)動(dòng)條件進(jìn)行速度規(guī)劃，在驗(yàn)證的算法設(shè)計(jì)結(jié)果中，數(shù)據(jù)表明優(yōu)化后的新型S曲線加減速算法不僅能輸出平滑的速度，還能更快地響應(yīng)達(dá)到最大速度，從而使受控對(duì)象能夠更好地快速平穩(wěn)運(yùn)動(dòng)到期望位置。