基于新型速度規劃的優化算法研究*

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

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

0 引言

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

1 新型曲線速度優化原理分析

鐘前進[10]提出的新型S加減速算法在對位移積分后，計算公式仍然與梯形加減速一樣，如此就大大簡化了軌跡規劃及速度前瞻時的運算量，從而大大簡化了算法，而且運算量相對來說比較小，非常適合嵌入式的運動控制，優化后的位移公式為：

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

由于梯形運動是直線加速度運動，滿足直線運動速度公式：

式中：Vc為當前實時速度；Va為起始速度；ta為加速運動的時間；Sa為加速運動的位移。

由速度公式可得加速度的關系式為：

根據當前運動過程中直線加速運動速度與位移的關系式可得：

式中：Vc為停止速度；Sd為減速運動位移。

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

運動過程中加速度值是一致的，即式（3）與式（5）相等，代入可得速度與時間、位移之間的映射關系為：

上式即為無勻速段運動下加減速值一致時瞬時速度對時間與位移的映射關系式，該關系式適用于加減速的全過程。而當運動過程中加速度與減速度不一致時，則根據運動一般公式可知加速過程與減速過程的瞬時速度映射關系式為：

而對于能夠達到勻速狀態的運動過程分析就比較簡單，在運動規劃過程中能夠求得勻速運動位移Su與勻速運動時間tu，從而根據位移與速度公式就能求得瞬時速度的映射公式：

2 例化優化算法實現

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

其中起點坐標是(xs，ys)，目標點坐標是(x，y)，為直線運動規劃時的插補位移求解公式，若為圓弧運動則插補公式為：

式中：r為圓弧運動的半徑；θ為圓弧運動的圓心角；flag為運算符號，由圓弧運動的順逆方向決定，逆時針時為正，順時針為負，核心共性都是對插補例化因子λ進行求解。

對插補例化因子的求解步驟如下。

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

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

（3）求加速時間ta、減速時間td、加速運動位移Sa、減速運動位移Sd，并獲得例化參數：

此時若加速時間ta=0，則表明運動過程為極端情況，此時沒有加速段，則在加速過程中瞬時速度例化為零，此時速度的例化參數為：

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

若減速時間td=0，此時沒有減速段，則在減速過程中瞬時速度例化為零，此時速度的例化參數為：

當起始速度與結束速度相等時，此時運動具有特殊性，但依據加減速運動段單獨分析方式，此時速度的例化參數為：

而當起止速度非對稱時，結合式（6）可得速度的例化參數為：

（4）由式（1）作簡單變換得到無勻速段梯形余弦計算位移公式，可得優化后的插補例化因子為：

式中：t為當前插補點的狀態時刻，且t=,i=1,2,3,…,N，

其中N代表總的插值點數。

通過對總的插值點數進行控制就能間接控制每個時刻的插補點的狀態時刻的運動狀態，從而實現細分化的插補路徑控制，其中插值點數的計算方式為：

式中：C為插值倍增系數，用來倍增插補點數；S為運動總位移或圓弧運動的圓心角；V為運動的線速度或圓弧運動的角速度。

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

式中：總位移為滿足Sp=Sa+Su+Sd的當前所有軸運動規劃的路徑總數。

在運動中若加速時間ta=0，則起始速度例化參數Vaf=0，否則起始速度例化參數；而若減速時間td=0，則結束速度例化參數Vef=Vcf，否則結束速度例化參數為Vef=。在求得起止速度例化參數后結合例化參數式（18）并代入式（1）可得插補例化因子為：

優化后的以上5個步驟流程能將實際情況所有條件下的規劃情況都涵蓋，在每一個插補點時刻i都會有唯一的插補例化因子與其對應，將例化因子代入式（9）中就能求得在每一個插補時刻點的坐標位置信息，并通過對當前的新型路徑因子計算式進行求導就能得到當前的狀態時刻的速度，結合對當前路徑輸出求出當前插補點坐標，細分到每個運動輸出上就能得到對當前速度規劃應該輸出的速度與脈沖個數。

3 實驗驗證分析

在仿真平臺MATLAB上根據以上步驟流程編寫算法與測試代碼，同時仿真S型規劃、梯形規劃與優化后的新型S型規劃，仿真數據通過驗證不同的運行距離，比較3種規劃算法的輸出曲線，為了讓三者能有明顯的對比，測試在設計時保證了3種規劃算法在插補時具有相同的插補數，這樣在每一個插補點的結果是在相同時刻的規劃輸出。

（1）驗證參數數據為Va=0 mm/s，Vc=60 mm/s，Ve=10 mm/s，Sp=100 mm，最大加速度為30 mm/s2。此時梯形速度規劃有兩個階段，S型規劃分為4段，對應的速度與加速度曲線如圖1所示。

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

（2）驗證參數數據為Va=0 mm/s，Vc=60 mm/s，Ve=10 mm/s，Sp=600 mm，最大加速度為30 mm/s2。此時梯形速度規劃有3個階段，S型規劃分為5段，對應的速度與加速度曲線如圖2所示。

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

（3）驗證參數數據為Va=0 mm/s，Vc=100 mm/s，Ve=10 mm/s，Sp=800 mm，最大加速度為30 mm/s2。此時梯形速度規劃有3個階段，S型規劃分為7段，對應的速度與加速度曲線如圖3所示。

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

由以上3組驗證數據的結果圖顯示，在非對稱的一般情況下，優化后的新型速度規劃算法都能根據約束條件靈活地計算輸出較好的結果，并且在同等約束條件下優化后的算法還能以更平滑的加速度進行更快地響應。

4 結束語

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