肖友剛，朱鋮臻，李 蔚，韓 錕

(1.中南大學 交通運輸工程學院，長沙 410075; 2.軌道交通安全關鍵技術(shù)國際合作聯(lián)合實驗室(中南大學)，長沙 410075)

點對點運動廣泛存在于機器人、數(shù)控裝備、起重機械等領域。利用目標位置與當前位置的誤差進行反饋控制，是很多點對點運動控制系統(tǒng)普遍采用的算法，這種方法存在著初始控制量大、過程不可控以及振動和沖擊較大等缺陷。為了解決這個問題，通常采用軌跡規(guī)劃法。合理的點對點軌跡規(guī)劃應該將最大速度、加速度、加加速度等進行合理約束，并能根據(jù)被控對象特點有效調(diào)整加、減速過程，且保證系統(tǒng)平穩(wěn)運行。目前，大多數(shù)軌跡規(guī)劃是根據(jù)點對點運動所經(jīng)歷的加速、勻速、減速過程構(gòu)造分段函數(shù)，然后利用各種方法對各段的銜接處進行平滑處理[1-6]。文獻[2]將取放路徑分解為兩個正交的坐標軸，基于三次B樣條曲線設計了高速取放并行機器人的四階連續(xù)平滑軌跡。文獻[7]建立了基于四元數(shù)的直線與圓弧運動的位姿模型，并采用弧長增量法插補技術(shù)實現(xiàn)了機械手空間軌跡規(guī)劃。文獻[8]將三角函數(shù)與直線連接組成加速度曲線，提出一種加加速度無突變且起止端連續(xù)的S型加減速規(guī)劃算法。文獻[9]采用五階S曲線加減速方法對電機啟動輸入信號進行規(guī)劃來抑制系統(tǒng)扭振，并運用粒子群優(yōu)化算法選取五階S曲線參數(shù)。文獻[10]以循環(huán)計算方式獲得加速段的運動距離與速度、最大速度的作用時間，提出了四階S型軌跡規(guī)劃算法。文獻[11]通過設計機器人末端位置和姿態(tài)的數(shù)學表達式，在過渡節(jié)點之間采用一種有限項的正弦級數(shù)進行曲線擬合，提出了一種工業(yè)機器人連續(xù)軌跡規(guī)劃過渡算法。文獻[12]將兩個周期不同的三角函數(shù)疊加，構(gòu)造了伺服電機加減速曲線，降低了伺服電機的功率要求。文獻[13]將速度和位移方程轉(zhuǎn)換為單一凸形函數(shù)，并根據(jù)加速度、速度約束條件進行修正，提出了一種高效的加減速時間規(guī)劃算法。文獻[14]針對加速度的不連續(xù)變化，研究了一種基于S型速度曲線的軌跡規(guī)劃方法。文獻[15]使用五次多項式修改生成的軌跡，給出了由三點確定一般平面曲線的條件，提出了一般曲線插值方法，在此基礎上設計了一種新軌跡。文獻[16]將取放路徑分解為兩個正交的坐標軸，基于三次B樣條曲線設計了高速取放并行機器人的四階連續(xù)平滑軌跡。根據(jù)標準邏輯函數(shù)構(gòu)造了分段S型加速度函數(shù)，得到了一條在速度、加速度、加加速度約束下四階可微的軌跡。文獻[17]將三次多項式和貝塞爾曲線相結(jié)合，設計了起、始點都光滑的軌跡。文獻[18]探討了最大速度曲線的特點，提出了一種最大或最小加速度的簡便計算方法。文獻[19]為減少高速運動階段的殘余振動，設計了一條加加速度約束下的非對稱運動軌跡。文獻[20]提出了一種基于相平面分析的軌跡規(guī)劃法，可將臺車速度、加速度以及負載最大擺幅始終約束在允許范圍內(nèi)。文獻[21]利用三角函數(shù)設計了S型速度曲線，并利用其對旋轉(zhuǎn)起重機兩級殘留擺角進行了控制。

綜上可以看出，絕大部分軌跡規(guī)劃都以梯形、S型速度或加速度曲線為基礎，生成的軌跡表達式都是在加速、勻速、減速段的銜接點存在斷點的分段函數(shù)。采用高次多項式、在銜接點采用正弦、余弦、樣條或者雙曲正切等光滑函數(shù)過渡，可以提升軌跡的平滑性和可微的階次，但是這種方法通常增加了曲線銜接點的個數(shù)，使曲線表達形式更加復雜，時間規(guī)劃更加困難，而且這些曲線參數(shù)眾多，難以根據(jù)被控對象要求對啟停進行針對性調(diào)整，使其效率或精度受到影響。

本文利用兩段雙曲正切函數(shù)構(gòu)造銜接點在勻速段的非對稱S型速度曲線，并將S型曲線與跟蹤微分器結(jié)合，生成一種高階連續(xù)的點對點運動軌跡(HCPPMT)，其具有結(jié)構(gòu)簡單、光滑、沖擊強度可控、各階段的快慢程度可調(diào)、適用范圍廣等優(yōu)點。與其他兩種軌跡的性能指標進行對比，說明了HCPPMT的優(yōu)越性。

1 高階連續(xù)的點對點運動軌跡規(guī)劃

點對點運動要求啟動平穩(wěn)、停車精準，而其所經(jīng)歷的加、減速階段勢必對其產(chǎn)生沖擊，緩慢運行可減少沖擊，但低下的效率使人難以接受。因此，需要根據(jù)被控對象特點，設計一條沖擊強度可調(diào)、在勻速段能以最大速度運行、終點恰好為目標點的高效運動軌跡。S型曲線包含加、減及勻速3個階段，符合點對點運動的特點，因此，先構(gòu)造各階段狀態(tài)都可按需調(diào)整的S型曲線。為了避免S型曲線在加速、勻速、減速段的銜接點出現(xiàn)斷點，構(gòu)造一個具有加速、勻速、減速特點，且能光滑過渡的函數(shù)就很關鍵。

以v1表示運動點的實時位置，sd表示目標位置，當v1∈[0,sd/2]，即運動點在起點和中點之間時，利用雙曲正切函數(shù)設計包含加速和部分勻速段、且變化率可調(diào)的光滑連續(xù)曲線S1：

f1(v1,sd)=-kd·tanh[(v1+ε)·r1]

(1)

式中：kd為額定速度；r1為加速調(diào)節(jié)因子，可調(diào)節(jié)加速階段的快慢程度，r1∈R+，r1越小，近似線性部分斜率越小，越平緩;ε為啟動加速度調(diào)節(jié)因子，用于調(diào)節(jié)啟動加速度。

當v1∈(sd/2,sd]，即運動點在中點和目標點之間時，設計包含部分勻速和減速段、且變化率可調(diào)的光滑連續(xù)曲線S2：

f2(v1,sd)=kd·tanh[(v1-sd)·r2]

(2)

式中r2為減速調(diào)節(jié)因子，可調(diào)節(jié)減速階段的快慢程度，r2∈R+。

將曲線S1和S2在中點進行銜接，形成包含加速、勻速及減速段的S型曲線：

(3)

該曲線中點附近處于勻速段，保障了整個S型曲線的連續(xù)性，利用雙曲正切函數(shù)的平滑特性實現(xiàn)了點對點運動中加速、勻速、減速階段的平滑過渡，有效避免了傳統(tǒng)點對點運動軌跡規(guī)劃中分別設計加速、勻速、減速段，然后采用平滑函數(shù)連接各段的繁瑣過程，有效克服了傳統(tǒng)設計中加速、勻速、減速銜接點處高階不可微、使系統(tǒng)沖擊和振動大的弊端。

被控對象啟動時相當于被施加了一階躍信號，該信號高階不可微，會對被控對象造成嚴重沖擊，階躍信號經(jīng)過跟蹤微分器后，其輸出曲線變?yōu)橐粭l平滑曲線，可有效消除初始點高階不可微的問題，因此對0點到v1-sd點用跟蹤微分器處理，得

(4)

式中r為速度調(diào)節(jié)因子，可調(diào)節(jié)被控對象的運行速度，r∈R+。

對式(3)、(4)進行綜合，得到速度、加速度和加加速度始終有界，可準確收斂至目標點，且高階連續(xù)的點對點光滑運動軌跡(HCPPMT)：

(5)

從HCPPMT的構(gòu)造過程可以看出，在中點處存在銜接點，處理不當會產(chǎn)生斷點，致使其高階不可微。為克服這一缺陷，應使式(3)在sd/2鄰域都處于勻速段。在目標點較遠時，式(3)可保證被控對象以最大速度勻速運行較長距離，不會出現(xiàn)斷點。而目標點很近時，若加、減速階段運行的總距離等于sd，則意味著加速到最大運行速度后就需要減速，這時就會出現(xiàn)拐點，使其高階不可微。因此，需調(diào)整速度調(diào)節(jié)因子r，使速度至少在sd/2鄰域的2N個計算步長內(nèi)都以最大速度勻速運行，N表示可微的階數(shù)。最大運行速度根據(jù)目標點距離sd，加速階段和減速階段的調(diào)節(jié)因子r1、r2，額定速度kd，速度調(diào)節(jié)因子r，用龍格庫塔法通過數(shù)值計算確定。

式(5)描述的高階連續(xù)點對點運動軌跡具有如下性質(zhì)：1)收斂至目標點sd處；2)速度始終有界，且r∈[0,2]時，0≤v2≤kd；3)加速度和加加速度始終有界。

2 高階連續(xù)的點對點運動軌跡性質(zhì)證明

定理1高階連續(xù)的點對點運動軌跡(HCPPMT)收斂至目標位置sd處。

考慮到啟動加速度調(diào)節(jié)因子ε的數(shù)值很小，為使證明過程簡潔明晰，將其忽略不計。

構(gòu)造如下函數(shù)h(v1):

(6)

式中Q=ln(cosh(r1sd/2))。當v1≤sd/2時，ln(cosh(r1v1))≤ln(cosh(r1sd/2))，又ln(cosh(r1v1))≥0，且kd、r、r1均為正數(shù)，因此式(6)≥0。

將式(6)對時間求導，可得

(7)

構(gòu)造如下Lyapunov候選函數(shù)：

(8)

根據(jù)式(6)可知，V為恒大于等于0的函數(shù)，將V對時間求導，可得

(9)

由式(9)≤0可知，HCPPMT是Lyapunov意義下穩(wěn)定的。

綜合式(3)、(5)可知，當t→∞時，f(v1,sd)=0，因此可得

(10)

定理2HCPPMT的期望軌跡速度始終大于0且有界，且r∈[0,2]時，0≤v2≤kd。

證明構(gòu)造新函數(shù)：

(11)

令G(v1)=g(v1)+f(v1,sd)，接下來證明G(v1) ≥0。

當0≤v1<1/(2r1)時，

G(v1)=kd(2r1v1-tanh(r1v1))

(12)

對式(12)針對v1求導，可得

(13)

因此，在[0,0.5/r1]內(nèi)，G(v1)為單調(diào)遞增函數(shù)，又因為G(0)=0，因此在[0,0.5/r1]內(nèi)，G(v1)≥0。

當1/(2r1)≤v1<(2sd-1)/(2r2)時，很顯然G(v1)≥0；當(sd-0.5)/r2≤v1

G(v1)=kdtanh(r2(v1-sd))-2kd(r2v1-sd)

(14)

對式(14)中的v1求導，得

(15)

因此，在[(sd-0.5)/r2，sd/r2]內(nèi)，G(v1)為單調(diào)遞減函數(shù)，又因為G(sd)=0，因此，在[(sd-0.5)/r2，sd/r2]內(nèi)，G(v1)≥0。

綜上證明可得，在[0,sd/r2]內(nèi)，

-f(v1,sd)≤g(v1)

(16)

接下來構(gòu)造一個新微分方程：

(17)

(18)

v1=c1eψ11t+c2eψ12t

(19)

(20)

其中：c1、c2為待定參數(shù)(根據(jù)初始狀態(tài)確定)，kd為額定速度，顯然式(20)是有界的。

由于t=0時，v1=0，將其代入式(19)，可得

c1=-c2

(21)

由v1≥0可知，c1為正數(shù)。

對式(19)求導，并結(jié)合式(21)，可得

v2=c1(ψ11eψ11t-ψ12eψ12t)

(22)

(23)

(24)

對式(24)求導，得速度v2：

(25)

對式(25)求導，并將t1代入，得

(26)

當t2≤t≤t3時，

(27)

(28)

這是一個典型的二階系統(tǒng)，當r2≤1/kd時，其階躍響應可以無超調(diào)地跟蹤設定值sd，因而其位移是有界的，當然速度也是有界的。又由于無超調(diào)的階躍響是單調(diào)上升的，因此當t2≤t≤t3，r2≤1/kd時，v2>0且有界。

由于所規(guī)劃軌跡的最大速度在勻速段，而勻速段所在區(qū)間為[t1,t2]，該區(qū)間內(nèi)的最大速度為rkd/2，因而整個區(qū)間的速度都小于rkd/2。

綜合以上分析，可得期望軌跡速度始終大于0且有界，且r∈[0,2]時，0≤v2≤kd。

定理3HCPPMT的期望軌跡加速度和加加速度分別有界。

根據(jù)式(23)、(26)、(27)，顯然加速度有界。

將式(5)對時間求導，得

(29)

由于加速度有界，且tanh函數(shù)為有界函數(shù)，因此式(29)有界，即加加速度有界。

3 可調(diào)參數(shù)對HCPPMT的影響分析

對不同被控對象，在加速、勻速、減速階段有不同要求。譬如：為提高效率，起重機械要求較快加速到最大運行速度后勻速運行，為保證安全，其最大運行速度也要根據(jù)環(huán)境風速進行調(diào)整，減速階段則要求緩慢地?？吭谀繕它c，以實現(xiàn)精準定位。因此，有必要研究可調(diào)參數(shù)對HCPPMT各階段狀態(tài)和沖擊強度的影響規(guī)律，使其在不同的應用場合發(fā)揮最大效能。

為研究最大速度調(diào)節(jié)因子r對HCPPMT的影響，令sd=30 m、kd=2 m/s、ε=0.01、r=2、r2=0.4，當r分別取2、1.61和1.24時，位移、速度、加速度隨時間的變化如圖1所示，從圖中可以看出，當r等于2時，軌跡的最大勻速運行速度等于額定速度。r越小，最大勻速運行速度越小，在作業(yè)距離一定時，耗時也越長。因此，作業(yè)距離越遠，應使r越大，以便盡可能提高作業(yè)效率，但r不能大于2；近距離作業(yè)時，則應使r較小，但太小會降低作業(yè)效率，因此以在sd/2鄰域的2N個計算步長內(nèi)都以最大速度勻速運行為前提。對在室外環(huán)境中作業(yè)的起重機械，為保證安全，應根據(jù)風速等級，選擇合適的r值。

為研究加速調(diào)節(jié)因子r1對HCPPMT的影響，令sd=30 m、kd=2 m/s、ε=0.01、r=2、r2=0.4，當r1分別取0.65、1和1.8時，HCPPMT的位移、速度、加速度隨時間的變化如圖2所示，從圖2中可以看出，r1越小，軌跡初始階段越平緩，到達最大速度的時間變長，運行規(guī)定距離所耗費的時間也越多，隨著r1的增加，加速度變大，沖擊強度增加，到達最大速度的時間變短，勻速段和減速段提前，但最大速度大小和減速階段狀態(tài)不受影響。因此，對于需要慢啟動的精密機械，應取較小的r1值，以減小沖擊，而對于行程較長的起重機械，在不超過最大加速度的前提下，則宜使用較大r1值，以便提高效率。

圖1 參數(shù)r對HCPPMT軌跡的影響

圖2 參數(shù)r1對軌跡的影響

為研究減速調(diào)節(jié)因子r2對HCPPMT的影響，令sd=30 m、kd=2 m/s、ε=0.01、r=1.24、r1=1， 當r2分別取0.2、0.28和0.36時，HCPPMT的位移、速度、加速度隨時間的變化如圖3所示，從圖中可以看出，在進入減速段前，整個軌跡曲線保持不變，但r2越小，進入減速段的時間越早，使勻速段的時間變短，減速段的時間變長，完成整個作業(yè)過程的時間增多，但變化趨勢越平緩，接近目標點時的速度也越慢，對于需要在目標點精確對位的精密裝備，如岸橋起重機，宜選擇較小的r2值。

圖3 參數(shù)r2對軌跡的影響

4 3種軌跡生成算法的對比分析

為了檢驗HCPPMT的效果，取sd=20 m、kd=5 m/s、ε=0.01、r=1.28、r1=0.5、r2=0.2，計算步長取0.05進行仿真，得仿真結(jié)果T1。同時，在相同目標位置和計算步長下，將S型速度曲線的仿真結(jié)果記為T2，S型位移曲線的仿真結(jié)果記為T3。

S型速度曲線采用文獻[22]中的形式，為便于對比，將角度轉(zhuǎn)換成位移：

(30)

其中：yc為該軌跡的最大速度，t1為加速階段時間，t2為勻速階段時間，T為總時間，具體參數(shù)取值分別為yc=3.065 m/s，t1=2.345 s，t2=12 s、T=18.69 s。

S型位移曲線采用文獻[23]中的表達式：

(31)

其中：ve為最大速度，ae為最大加速度，κ為初始加速度調(diào)節(jié)因子，κ=1.4，ve=3.056 m/s，ae=2.2 m/s2。

將仿真時間步長設置為0.01 s，仿真結(jié)果如圖4及表1所示。

圖4 3種軌跡的運動狀態(tài)隨時間的變化曲線

表1 3種軌跡各項指標最大值對比

圖4中自上至下分別為T1、T2、T3的位移、加速度，加加速度，加加加速度隨時間t的變化曲線，從圖中4(a)可以看出：3種軌跡幾乎同時到達指定位置，也就是在合適條件下它們的運行效率基本相同。從圖4(b)及表1可以看出：軌跡T3的加速度在初始階段出現(xiàn)了突變，說明利用軌跡T3進行跟蹤控制時，在起動時需要很大驅(qū)動力，這勢必要求提高執(zhí)行器的功率，而且會對系統(tǒng)產(chǎn)生嚴重沖擊，T1、T2的加速度都是連續(xù)的，但軌跡T1的最大加速度更小，說明所需的最大驅(qū)動力更小，對執(zhí)行器的最大功率要求也就更低，有利于降低成本，而且T2在各段(加速、勻速、減速)銜接處出現(xiàn)了明顯轉(zhuǎn)折，而軌跡T1的加速度在整個過程中都比較平滑，因而利用T1進行軌跡跟蹤控制，系統(tǒng)運行更平穩(wěn)。從圖4(c)及表1可以看出：T3初始階段的加加速度也出現(xiàn)了突變，會對系統(tǒng)產(chǎn)生很大的柔性沖擊，使其大幅振動，T2則在各段銜接處出現(xiàn)了最大值為2.73 m/s3的躍變，會對系統(tǒng)產(chǎn)生明顯的沖擊和振動，而T1加加速度的最大值為1.24 m/s3，遠小于T2、T3的加加速度最小值，并且T2的加加速度平滑且連續(xù)，不會對系統(tǒng)產(chǎn)生大沖擊和振動。從圖4(d)可以看出，軌跡T2、T3的加加加速度在各段銜接處都出現(xiàn)了突變，而軌跡T1的加加加速度光滑且連續(xù)，且最大值只有2.75 m/s4，即使進行更高階微分，都將是平滑且連續(xù)的，因此特別適用于需要進行高階微分的場合(圖中↑表示在該時刻，縱坐標值為無窮大值)。

5 HCPPMT在三自由度機械臂軌跡規(guī)劃中的應用分析

三自由度機械臂運動軌跡曲線對機械臂末端運動有著重要影響，如果軌跡的加速度或者加加速度過大，會使機械臂在運行過程中出現(xiàn)振蕩、使運動關節(jié)經(jīng)受大沖擊，太大的加速度還將使機械臂執(zhí)行器飽和，使機械臂無法完成既定任務。三自由度機械臂的結(jié)構(gòu)如圖5所示，其中，l1、l2、l3分別表示機械臂1、2、3的長度，分別為0.85、0.95、0.65 m；θ1、θ2、θ3分別表示機械臂1、2、3的轉(zhuǎn)動角度，末端執(zhí)行器坐標為(px、py、pz)，機械臂軌跡規(guī)劃是通過對機械臂末端執(zhí)行器空間坐標的x軸、y軸、z軸進行點對點規(guī)劃，再利用末端執(zhí)行器坐標和機械臂的轉(zhuǎn)動角度關系式，將設計好的x軸、y軸、z軸的軌跡轉(zhuǎn)化為關節(jié)的轉(zhuǎn)動，實現(xiàn)末端執(zhí)行器的空間運動。末端執(zhí)行器坐標和機械臂的轉(zhuǎn)動角度關系式可用式(32)描述[16]：

(32)

圖5 三自由度機械臂的結(jié)構(gòu)

為了驗證HCPPMT的有效性，在同樣條件下將HCPPMT和文獻[16]設計的光滑7段式三自由度機械臂末端執(zhí)行器運動軌跡(SJM)進行對比，SJM的表達式為

j(t)=sign(D)·

(33)

式中：D為定位距離，Jmax為最大加加速度。

與文獻[16]一樣，根據(jù)表2中機械臂末端執(zhí)行器起始點、目標點坐標和路徑距離用HCPPMT算法進行仿真，并與SJM進行對比，得到的結(jié)果如圖6、圖7、圖8和表2所示。從這些圖中可以看出，在整個過程中，這兩種方法的加速度、加加速度、加加加速度都光滑連續(xù)，但HCPPMT法在這3個指標上均比較有優(yōu)勢，其最大值都小于SJM法的最大值，然而，HCPPMT法到達目標點的時間稍長，這是由于加入跟蹤微分器后雖然進一步保證了HCPPMT的高階可微性，但是使啟停過程變得比較平緩，因此需要探尋既能使兩段雙曲S型軌跡高階可微，又不影響啟?？炻暮瘮?shù)與其結(jié)合，這是后續(xù)將深入研究的課題。

表2 末端執(zhí)行器對應的坐標和距離

圖6 末端執(zhí)行器x軸方向的運動軌跡

圖7 末端執(zhí)行器y軸方向的運動軌跡

圖8 末端執(zhí)行器z軸方向的運動軌跡

從表3可以看出，為了在相同時間內(nèi)到達目標點，采用SJM法，y軸方向的最大加速度比z軸方向的最大加速度大1.18 m/s2，比x軸方向的最大加速度大1.69 m/s2；y軸方向的最大加加速度比z軸方向的最大加加速度大3.63 m/s3，比x軸方向的最大加加速度大5.22 m/s3；y軸方向的最大加加加速度比z軸方向的最大加加加速度大32.07 m/s4，比x軸方向的最大加加加速度大46.12 m/s4；而采用HCPPMT法，y軸方向的最大加速度比z軸方向的最大加速度大0.70 m/s2，比x軸方向的最大加速度大1.18 m/s2；y軸方向的最大加加速度比z軸方向的最大加加速度大3.29 m/s3，比x軸方向的最大加加速度大4.56 m/s3；y軸方向的最大加加加速度比z軸方向的最大加加加速度大26.32 m/s4，比x軸方向的最大加加加速度大35.16 m/s4。因此，定位距離越大，采用HCPPMT法的加速度、加加速度、加加加速度的增量越小，對執(zhí)行器功率的要求越低，出現(xiàn)執(zhí)行器飽和而不能完成指定任務的概率就越??；對系統(tǒng)的沖擊越小，系統(tǒng)振動幅度也就越低，系統(tǒng)的運動精度也就越高。

表3 末端執(zhí)行器加速度、加加速度、加加加速度最大值

6 結(jié) 論

1)通過利用兩段雙曲正切函數(shù)構(gòu)造銜接點在勻速段的S型速度曲線，并將其與跟蹤微分器結(jié)合，生成了結(jié)構(gòu)簡單、高階連續(xù)的點對點運動軌跡，實現(xiàn)了點對點運動加速、勻速、減速過程的平滑過渡，減少了加速、勻速、減速段切換過程中的沖擊，增強了運動的平穩(wěn)性。

2)通過調(diào)整HCPPMT的相關參數(shù)，可調(diào)節(jié)加、減速階段的快慢程度、勻速階段的最大速度、作業(yè)時間，使HCPPMT的平滑程度、沖擊強度可控，各階段狀態(tài)可調(diào)，且HCPPMT的收斂性及物理性質(zhì)都通過了嚴格的數(shù)學證明，適用于不同應用場合。后續(xù)將利用HCPPMT對數(shù)控機床、起重機等進行軌跡跟蹤控制研究。

3)通過和其他兩種軌跡對比，以及在三自由度機械臂末端執(zhí)行器軌跡規(guī)劃上的應用，說明了HCPPMT的優(yōu)越性。