孫曉軍 宋代平 林敬周 韓偉航

1.重慶大學機械傳動國家重點實驗室,重慶,4000442.中國空氣動力學研究與發(fā)展中心超高速空氣動力研究所,綿陽，621000 3.遼寧石油化工大學信息與控制工程學院, 撫順，113001

0 引言

機械臂的目標軌跡往往先通過試教法確定，再通過軌跡規(guī)劃算法確定各個關節(jié)的速度曲線。傳統(tǒng)的軌跡規(guī)劃算法僅考慮了機械臂運動學模型的非線性及關節(jié)電機速度的限制，這使得規(guī)劃的軌跡不能充分發(fā)揮機械臂的工作效率。對目標軌跡進行二次規(guī)劃可提高機械臂的運動速度，縮短運動時間。與傳統(tǒng)的軌跡規(guī)劃算法相比，基于動力學模型的軌跡規(guī)劃算法能夠充分考慮機械臂運動學模型、動力學模型的非線性，以及關節(jié)速度、力/力矩的限制，規(guī)劃軌跡能夠充分發(fā)揮機械臂驅動能力，但該算法存在計算量大、計算效率低的問題[1]。

目前考慮機械臂動力學模型的軌跡規(guī)劃算法有3類。一是相平面法，通過引入路徑參數，對機械臂的運動學模型、動力學模型進行參數化變換，得到以路徑參數二階微分方程表示的路徑參數化動力學模型，再通過路徑參數化模型將關節(jié)速度、力/力矩等約束轉化為關于路徑參數的約束，最終將軌跡規(guī)劃問題轉化為關于路徑參數的二維優(yōu)化問題[2-4]。相平面法計算效率高，但通用性較差，約束增加困難，如考慮加加速度約束，相平面法解算將變得復雜[5]。二是直接優(yōu)化方法，通常將目標軌跡劃分為n段，將規(guī)劃問題轉化為具有n個不等式約束(動力學約束、運動學約束、關節(jié)力矩約束等)的優(yōu)化問題，然后采用如凸優(yōu)化法[6-10]等相對高效的優(yōu)化算法進行優(yōu)化處理。凸優(yōu)化法具有較高的通用性，約束添加簡單，但依賴于高效的優(yōu)化算法包，計算效率低于相平面法。三是采用最優(yōu)控制理論的時間最優(yōu)控制方法，通過求解最優(yōu)性必要條件的兩點邊值問題獲得最優(yōu)解[11]，該方法在理論上比前兩種算法速度快，但規(guī)劃的軌跡易引起運動學奇異的問題[5]。

由于相平面法具有較高的計算效率，因此本文采用相平面法進行速度曲線規(guī)劃，并對相平面法進行優(yōu)化改進以進一步提高算法的計算效率，對規(guī)劃的軌跡進行平滑處理以保證機械臂運動平穩(wěn)。相平面法優(yōu)化方法體現在以下三點：①通過模糊推理計算路徑點數目，減小計算規(guī)模，采用0值加速度曲線乘以比例系數替代關節(jié)力/力矩約束最大速度曲線，提高算法的計算效率；②通過速度特征點法提取多重約束最大速度曲線的速度特征點，再通過B樣條插值法對速度特征點插值，保證速度曲線連續(xù)光滑；③采用修型-射靶算法對速度曲線進行修形，得到滿足多重約束的最大速度曲線。

1 動力學模型路徑參數化

機械臂的動力學模型可以表示為

(1)

設機械臂在工作空間的目標軌跡為s(P)(?s∈R+)，P∈Rn×6為目標軌跡的路徑點向量。目標軌跡的時間參數化可表示為s(t):[0,T]→[ss,se]，T為機械臂跟蹤目標軌跡的運動時間，s為一維路徑參數，ss為起始路徑參數值，se為末端路徑參數值。

在關節(jié)空間，根據機械臂運動學模型可得機械臂末端的正運動學特征：

p=Z(q)

(2)

(3)

(4)

在參考坐標系，采用路徑s表示機械臂末端的正運動學特征：

p=f(s)

(5)

(6)

(7)

結合式(2)～式(4)可得關于路徑參數s的運動學模型：

q=Z-1(f(s))

(8)

(9)

(10)

將式(8)～式(10)代入式(1)，可得關于路徑參數s的動力學模型：

(11)

(12)

(13)

(14)

a4(s)=G(q(s))+JTτd(q(s))

(15)

2 多重約束下的最大速度曲線

2.1 目標軌跡路徑特征點的提取

機械臂目標軌跡的路徑點向量P(P=(P1,P2,…,PK))一般提前設定，通過運動學逆解求解其對應的關節(jié)向量q(q=(q1，q2，…，qK))。為了使機械臂平穩(wěn)通過路徑點，采用B樣條等曲線對路徑點向量P進行插值得到連續(xù)的目標軌跡。軌跡二次規(guī)劃需要將目標軌跡進行離散化處理，進而將軌跡規(guī)劃問題轉化為稀疏離散化優(yōu)化問題。若離散的位姿向量序列過大將導致優(yōu)化規(guī)模過大，降低軌跡規(guī)劃算法的效率；若位姿向量序列過小將丟失目標軌跡的特征，甚至失真。在此采用目標軌跡的路徑復雜程度與動力學模型非線性程度作為提取目標軌跡路徑特征點的依據，使用模糊推理算法計算路徑特征點的間隔，等間距提取路徑特征點。

(16)

結合式(1)、式(8)、式(9)可得向心加速度/科氏加速度單位向量

(17)

結合式(1)、式(8)可得重力加速度單位向量

γg=a4=[JT]-1G(Z-1(f(s)))

(18)

路徑特征點算法流程圖見圖1，算法步驟如下。

圖1 路徑特征點算法流程圖

(1)根據下式計算路徑復雜度：

(19)

式中，Δs為迭代步長；Δqx、Δqy、Δqz為機器人末端在參考坐標系下x、y、z方向的微分變化量；Δqα、Δqβ、Δqγ為機器人末端在參考坐標系下繞x、y、z軸旋轉的角度微分變量；sum(·)為求和函數。

(2)計算動力學模型非線性程度。令迭代步長為Δs，計算路徑點si=ss的動力學特征向量γm,i、γc,i、γg,i，再計算路徑點sj=si+Δs的動力學特征向量γm,j、γc,j、γg，j，計算si與sj特征向量的夾角θn，刷新si=sj、sj=si+Δs，若sj=si+Δs>se，則θmax=max(θn)為動力學模型變化程度。

(3)采用模糊推理計算路徑特征點間隔ds。利用路徑點間隔ds等間距提取路徑特征點序列，令得到的路徑特征點向量P′=(P1，P2，…，PN),N≥K，對應的關節(jié)向量q′=(q1，q2，…，qN)。

2.2 關節(jié)速度約束下的最大速度曲線

(20)

將式(9)代入式(20)可得關節(jié)i速度邊界限制的最大速度：

(21)

通過式(21)依次求解機械臂各個自由度速度邊界限制的最大速度曲線，然后通過下式求解所有關節(jié)速度邊界限制得到的最大速度ζv：

(22)

依次求解路徑特征點的最大速度ζv，得到關節(jié)速度邊界限制的最大速度曲線V1：

V1={ζv(s),s∈[0,1]}

(23)

2.3 關節(jié)加速度/力矩約束下的最大速度曲線

關節(jié)力矩約束表示為

Tmin≤τ(s)≤Tmax

(24)

其中，Tmin∈R6×1、Tmax∈R6×1，為機械臂關節(jié)力矩邊界。將式(11)代入式(24)可得

(25)

為了表述方便，式(25)可表示為

(26)

分析式(26)有以下三種情況：

(1)a1,i(s)>0時，有

定義加速度的上邊界β為

(2)a1,i(s)<0時，有

定義加速度的下邊界α為

(3)a1,i(s)=0時為零慣性點，不予考慮。

依次求取各個關節(jié)力矩限制的加速度，可得路徑加速度邊界：

(27)

i=0,1,…，6

至此得關節(jié)力/力矩約束轉化為對路徑加速度的約束：

(28)

2.4 啟動/停止最大速度曲線

機械臂在啟動時希望機械臂以最大的加速度沿著目標路徑軌跡運動，采用數值積分[12]方法求解啟動速度，計算公式如下：

(29)

(30)

啟動/停止最大速度曲線的獲取步驟如下：

(2)類似地，在停止運動時以最小的加速度減速運動，采用逆向積分的方法求解停止最大速度曲線V5。

2.5 多重約束下最大速度曲線的生成

圖2 多重約束最大速度曲線的生成

3 最大速度曲線的優(yōu)化

由于2.5節(jié)求得的最大速度曲線Ve可能存在加速度不連續(xù)等問題，故需對Ve進行優(yōu)化。最大速度曲線Ve的優(yōu)化分為速度特征點的獲取、速度特征點B樣條插值[13](或其他平滑插值算法)和修型-打靶算法修正速度特征點幅值三步。

最大速度曲線Ve速度特征點的提取方法為：提取最大速度曲線Ve的首尾速度Ve,0、Ve,K；令i←3，若Ve,i-1≥Ve,i≤Ve,i+1，則Ve,i為特征速度點，否則令i←i+1，依此再進行判斷，掃描所有最大速度曲線Ve離散點；若速度特征點數目小于4，對速度特征點數進行等間距插值。

圖3 B樣條插值速度特征點

修型-打靶算法流程圖見圖4，算法的流程如下：

圖4 修型-打靶算法流程圖

(1)對最大速度曲線Ve進行分段采樣得到檢測序列S，采用差分方法計算檢測序列S對應的加速度as。

(2)根據式(27)計算得到檢測序列S對應的最大加速度序列amax、最小加速度序列amin。

(3)依次檢查加速度as是否滿足約束要求，將不滿足amin、amax的加速度保存在Sc,i=[Sc1,Sc2]，其中Sc,i表示第i個相鄰速度特征點分段，Sc1表示小于amin序列所對應的加速度，Sc2表示大于amax序列所對應的加速度。

(4)若Sc1非空，則降低Sc1序列所對應的速度特征點分段的起始速度幅值，增大終止速度幅值；若Sc2非空，則降低Sc2序列所對應的速度特征點分段終止速度幅值，增大起始速度幅值。

(5)若Sc非空，則重復步驟(2)～步驟(4)修正速度幅值，直到Sc為空，若檢測序列S所對應的速度非負，則認為修形后的最大速度曲線滿足關節(jié)速度、關節(jié)力矩/加速度要求，若檢測序列S所對應的速度為負，則規(guī)劃算法失效。

4 實驗

為驗證速度曲線規(guī)劃算法的有效性，以六自由度上攻角機械臂為例驗證軌跡規(guī)劃算法的有效性及計算效率。上攻角機械臂的結構簡圖見圖5，包括軸向X、法向Y、側向Z、俯仰α、偏航β、滾轉γ六個自由度，飛行器模型掛載在滾轉機構末端，各個自由度的力矩/電流約束、速度約束如表1所示。

表1 關節(jié)力矩/電流約束、速度約束表

圖5 上攻角機械臂結構簡圖

軌跡規(guī)劃算法編寫在監(jiān)控PC(CPU 主頻為2.3 GHz、內存為 7.9 GB)的MATLAB 2016軟件上，監(jiān)控PC上的人機交互界面軟件通過MATLAB接口調用軌跡規(guī)劃算法，并將規(guī)劃結果通過ADS通信傳輸給工控機。工控機運行機械臂的運動控制軟件，完成機械臂的運動控制及各個關節(jié)位置、速度、電流等數據的采集。

4.1 算法有效性實驗

4.1.1簡單軌跡規(guī)劃實驗

為了驗證速度曲線規(guī)劃算法的有效性，進行特定軌跡規(guī)劃實驗。定義簡單目標軌跡的起始位姿(-500 mm,-100 mm,0,-8°,0°,0°)T與終點位姿(0,600 mm,0,0°,0°,0°)T，起始位姿與終點位姿采用B樣條插值得到連續(xù)的軌跡。通過動力學特征向量法提取有限數目路徑特征點，路徑特征點在各個自由度的分布如圖6所示。

圖6 X、Y、α軸的路徑特征點的分布

比例系數Kv=1.2，通過MATLAB計算得到關節(jié)速度約束、關節(jié)力矩約束、啟動/停止約束等約束的最大速度曲線，對上述約束曲線取向下交集可得到多重約束下的最大速度曲線MV1，如圖7所示。不難看出最大速度曲線MV1存在突變，易引起上攻角機械臂振蕩而影響系統(tǒng)精度，因此需要對最大速度曲線MV1進行平滑處理。

圖7 簡單目標軌跡相平面

提取最大速度曲線MV1的特征速度點，然后采用B樣條插值[14]得到連續(xù)光滑的最大速度曲線MV2。B樣條插值可能導致最大速度曲線MV2在P1、P2處超過最大速度曲線邊界(圖8)，超出最大速度曲線邊界的速度段不滿足關節(jié)速度或關節(jié)力矩/加速度的約束限制，為此采用修型-打靶算法矯正速度特征值，進而實現對最大速度曲線MV2的修形，使得最終的速度曲線滿足關節(jié)速度、關節(jié)力矩/加速度等約束條件，如圖8所示。

圖8 修型-打靶算法修正速度特征點

通過數值計算可得到速度曲線MV2、加速度曲線MVA2在時間軸的表示，如圖9、圖10所示。不難看出，速度曲線變化連續(xù)平滑，不存在突變線性，加速度曲線除了在啟動與停止時刻為滿足加速、減速特性加速度不為零外，其他時刻都連續(xù)平滑。綜上所述,規(guī)劃得到的速度曲線滿足對速度、加速度軌跡連續(xù)光滑的要求。

圖9 簡單軌跡相平面

圖10 簡單軌跡相平面

圖11 關節(jié)位置曲線

圖12 關節(jié)速度曲線

圖13 X軸關節(jié)電流曲線

圖14 Y、α軸關節(jié)電流曲線

4.1.2復雜軌跡規(guī)劃實驗

為了進一步驗證速度曲線規(guī)劃算法對復雜軌跡規(guī)劃的有效性，圖15、圖16給出了復雜目標軌跡對應的上攻角機械臂關節(jié)曲線。

圖15 X、Y、Z自由度激勵軌跡曲線

圖16 α、β、γ自由度激勵軌跡曲線

圖17 復雜目標軌跡相平面

圖18 修正后復雜目標軌跡相平面 phase plan of complex trajectory after correction

4.2 算法計算效率分析

4.2.1路徑特征點數目對算法計算效率的影響

合適數目的路徑特征點序列將影響軌跡規(guī)劃算法的計算效率與有效性。當不采用路徑特征點算法時路徑特征點序列為依次經過的路徑點，與路徑復雜程度、機械臂非線性程度無關。路徑特征點算法以路徑的復雜程度、機械臂非線性程度為評價變量，模糊計算路徑特征點間距ds∈(0,1)，再等間距提取路徑特征點，與目標軌跡路徑點數目無關。圖19所示為采用路徑特征點算法與不采用路徑特征點算法對復雜目標軌跡進行規(guī)劃的計算耗時，從中不難看出，路徑點數目越多，算法越耗時，當路徑點數目過多時，采用路徑特征點算法將大幅提高軌跡規(guī)劃算法的計算效率。

圖19 路徑離散點數目對算法效率的影響

速度曲線規(guī)劃算法可分為如下幾步：連續(xù)路徑軌跡的生成及路徑特征點的獲取T1、獲取多重約束最大速度曲線T2、最大速度曲線的優(yōu)化T3。表2所示為軌跡規(guī)劃算法優(yōu)化前后，對簡單目標軌跡與復雜目標軌跡進行規(guī)劃各步驟所占解算時間的比例，不難看出，優(yōu)化前獲取多重約束最大速度曲線T2為軌跡規(guī)劃算法的主要計算時間，并且它在復雜目標軌跡規(guī)劃中所占用的比例大于簡單目標軌跡，優(yōu)化后，獲取多重約束最大速度曲線T2的占比大幅度縮小。因此采用加速度恒為0的速度曲線替代最大速度曲線V2，可極大地縮短軌跡規(guī)劃的耗時。

表2 軌跡規(guī)劃算法各步驟計算時間占比

4.2.2不同軌跡規(guī)劃算法/優(yōu)化算法的計算效率

在實際應用中，采用軌跡規(guī)劃算法/優(yōu)化算法規(guī)劃目標路徑軌跡的速度曲線所花費的時間是衡量軌跡規(guī)劃算法計算效率的重要標準。為了研究本文軌跡規(guī)劃算法相對于其他軌跡規(guī)劃算法/優(yōu)化算法的性能，對比分析速度曲線規(guī)劃算法、遺傳算法[7-8]、凸優(yōu)化算法[3,9]對簡單軌跡、復雜軌跡的計算效率，結果如表3所示。不難看出，本文軌跡規(guī)劃算法相對于其他兩種路徑規(guī)劃算法計算耗時最少，遺傳算法耗時最多，此外復雜軌跡的計算耗時長于簡單軌跡的計算耗時。

表3 優(yōu)化算法計算效率對比表

綜上所述，速度曲線規(guī)劃算法相對于凸優(yōu)化算法、遺傳算法在計算效率上具有一定的優(yōu)勢，路徑的復雜程度影響軌跡規(guī)劃算法的計算效率，路徑越復雜，相應規(guī)劃用時也越長；此外路徑點的數目對規(guī)劃算法影響最大，過多的路徑點將降低軌跡規(guī)劃的效率，但本文采用的路徑特征點法與速度特征點法可在一定程度上降低路徑特征點數目對速度曲線規(guī)劃算法計算效率的影響。

5 結論

本文的軌跡規(guī)劃算法以相平面法為基礎，并對相平面法進行優(yōu)化，以提高算法的計算效率及保證規(guī)劃的軌跡連續(xù)光滑。

(1)算法計算效率的提高。通過模糊推理計算路徑點數目來減小計算規(guī)模；采用加速度恒為0的速度曲線替代最大速度曲線V2，簡化最大速度曲線V2的求解，縮短計算時間。

(2)速度曲線連續(xù)光滑處理。通過速度特征點法提取多重約束最大速度曲線的速度特征點，再通過B樣條插值法對速度特征點進行插值，保證速度曲線連續(xù)光滑；采用修型-射靶算法對速度曲線進行修形，使得到的速度曲線滿足約束要求。

最后通過實驗驗證了本文軌跡規(guī)劃算法對不同復雜程度目標軌跡的有效性，目標軌跡的復雜程度越大，其計算時間越長，此外驗證了其計算效率與路徑特征點數目成正相關。通過對比驗證了本文軌跡規(guī)劃算法相對于凸優(yōu)化算法、遺傳算法具有更高的計算效率。