一種新型高階連續的點對點運動軌跡規劃算法

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

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

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

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

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

1 高階連續的點對點運動軌跡規劃

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

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

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

(1)

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

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

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

(2)

式中r2為減速調節因子，可調節減速階段的快慢程度，r2∈R+。

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

(3)

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

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

(4)

式中r為速度調節因子，可調節被控對象的運行速度，r∈R+。

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

(5)

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

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

2 高階連續的點對點運動軌跡性質證明

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

考慮到啟動加速度調節因子ε的數值很小，為使證明過程簡潔明晰，將其忽略不計。

構造如下函數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均為正數，因此式(6)≥0。

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

(7)

構造如下Lyapunov候選函數：

(8)

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

(9)

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

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

(10)

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

證明構造新函數：

(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]內，G(v1)為單調遞增函數，又因為G(0)=0，因此在[0,0.5/r1]內，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]內，G(v1)為單調遞減函數，又因為G(sd)=0，因此，在[(sd-0.5)/r2，sd/r2]內，G(v1)≥0。

綜上證明可得，在[0,sd/r2]內，

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

(16)

接下來構造一個新微分方程：

(17)

(18)

v1=c1eψ11t+c2eψ12t

(19)

(20)

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

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

c1=-c2

(21)

由v1≥0可知，c1為正數。

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

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

(22)

(23)

(24)

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

(25)

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

(26)

當t2≤t≤t3時，

(27)

(28)

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

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

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

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

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

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

(29)

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

3 可調參數對HCPPMT的影響分析

對不同被控對象，在加速、勻速、減速階段有不同要求。譬如：為提高效率，起重機械要求較快加速到最大運行速度后勻速運行，為保證安全，其最大運行速度也要根據環境風速進行調整，減速階段則要求緩慢地停靠在目標點，以實現精準定位。因此，有必要研究可調參數對HCPPMT各階段狀態和沖擊強度的影響規律，使其在不同的應用場合發揮最大效能。

為研究最大速度調節因子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越小，最大勻速運行速度越小，在作業距離一定時，耗時也越長。因此，作業距離越遠，應使r越大，以便盡可能提高作業效率，但r不能大于2；近距離作業時，則應使r較小，但太小會降低作業效率，因此以在sd/2鄰域的2N個計算步長內都以最大速度勻速運行為前提。對在室外環境中作業的起重機械，為保證安全，應根據風速等級，選擇合適的r值。

為研究加速調節因子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越小，軌跡初始階段越平緩，到達最大速度的時間變長，運行規定距離所耗費的時間也越多，隨著r1的增加，加速度變大，沖擊強度增加，到達最大速度的時間變短，勻速段和減速段提前，但最大速度大小和減速階段狀態不受影響。因此，對于需要慢啟動的精密機械，應取較小的r1值，以減小沖擊，而對于行程較長的起重機械，在不超過最大加速度的前提下，則宜使用較大r1值，以便提高效率。

圖1 參數r對HCPPMT軌跡的影響

圖2 參數r1對軌跡的影響

為研究減速調節因子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越小，進入減速段的時間越早，使勻速段的時間變短，減速段的時間變長，完成整個作業過程的時間增多，但變化趨勢越平緩，接近目標點時的速度也越慢，對于需要在目標點精確對位的精密裝備，如岸橋起重機，宜選擇較小的r2值。

圖3 參數r2對軌跡的影響

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

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

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

(30)

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

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

(31)

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

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

圖4 3種軌跡的運動狀態隨時間的變化曲線

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

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

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

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

(32)

圖5 三自由度機械臂的結構

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

j(t)=sign(D)·

(33)

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

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

表2 末端執行器對應的坐標和距離

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

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

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

從表3可以看出，為了在相同時間內到達目標點，采用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法的加速度、加加速度、加加加速度的增量越小，對執行器功率的要求越低，出現執行器飽和而不能完成指定任務的概率就越小；對系統的沖擊越小，系統振動幅度也就越低，系統的運動精度也就越高。

表3 末端執行器加速度、加加速度、加加加速度最大值

6 結 論

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

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

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