冗余度機械臂零初始關(guān)節(jié)速度的運動規(guī)劃方案研究

徐劍琴，李克訥，楊 津，張 增

（1.廣西科技大學(xué)工程訓(xùn)練中心，廣西 柳州 545006；2.廣西科技大學(xué)電氣與信息工程學(xué)院，廣西 柳州 545006）

1 引言

冗余度機械臂的運動規(guī)劃和控制是機器人學(xué)研究中一個很有吸引力的領(lǐng)域[1-7]。機械臂的逆運動學(xué)求解問題是其中一個核心的研究內(nèi)容。對于冗余度機械臂，其逆運動學(xué)求解問題又稱冗余度解析問題[7]。而基于偽逆的運動規(guī)劃方案則是對該問題較為有效的解決方法之一?？紤]到偽逆法具有結(jié)構(gòu)簡單和易于實現(xiàn)的特點，許多研究學(xué)者提出了不同的基于偽逆的優(yōu)化方案來實現(xiàn)冗余度機械臂的運動規(guī)劃和控制[7]。在實際工程應(yīng)用中，機械臂的關(guān)節(jié)速度應(yīng)盡量減少突變。特別是在機械臂開始運動的瞬間，較大的關(guān)節(jié)初始速度意味著關(guān)節(jié)從零突變到較大的速度。這種情況會對關(guān)節(jié)電機形成機械沖擊和電流沖擊，甚至可能損壞機械臂。而且，電機的構(gòu)造和慣性作用也使得速度無法突變，勢必產(chǎn)生速度誤差，從而造成末端執(zhí)行器產(chǎn)生位置偏差，影響任務(wù)執(zhí)行精度，甚至可能導(dǎo)致整個任務(wù)失敗。因此，如何有效解決機械臂的非零初始關(guān)節(jié)速度是冗余度解析中值得研究的一個問題。但目前少有文獻報道該問題的研究情況。文獻[5]基于二次規(guī)劃方法提出了一種利用速度極限約束方法實現(xiàn)關(guān)節(jié)零初始速度運動，但主要針對機械臂的末端速度為零的自運動情形；文獻[7]提出了一種將最小速度范數(shù)方案和最小加速度范數(shù)方案進行加權(quán)組合的優(yōu)化方案，避免關(guān)節(jié)速度和關(guān)節(jié)加速度過高的現(xiàn)象，其重點是使得機械臂的關(guān)節(jié)速度在運動末態(tài)時接近零；文獻[8]提出了一種補償冗余度機械臂關(guān)節(jié)角從而達到補償機械臂位姿誤差的方法。但這些方法無法直接用以解決關(guān)節(jié)初始速度非零問題。在關(guān)節(jié)初始速度非零情況下，采用速度連續(xù)技術(shù)約束機械臂關(guān)節(jié)從零初始速度開始運動，并考慮末端執(zhí)行器期望軌跡與實時位置之間的誤差，求解出相應(yīng)的關(guān)節(jié)速度向量。為此，提出了一種基于偽逆的冗余度機械臂零初始關(guān)節(jié)速度的運動規(guī)劃方案，主要分以下三部分討論說明：首先，給出冗余度機械臂運動的數(shù)學(xué)描述和提出了一種基于偽逆的關(guān)節(jié)速度約束調(diào)整方法；然后針對末端執(zhí)行器執(zhí)行任務(wù)時產(chǎn)生的位置誤差，給出誤差的補償方法；最后，采用MATLAB對該方案進行仿真以驗證方案的可行性和有效性。

2 機械臂的運動規(guī)劃方案

2.1 關(guān)節(jié)速度的優(yōu)化

對于m

為了避免產(chǎn)生較大的非零初始關(guān)節(jié)速度對電機造成沖擊，可采用一種速度連續(xù)技術(shù)使機械臂關(guān)節(jié)速度能從零開始連續(xù)增大，即引入一個時變函數(shù)j（t）：

式中：τ＞0—控制電機升速過程的標(biāo)量系數(shù)，可根據(jù)電機性能和實際需要進行設(shè)定。

顯然，當(dāng)t≥0，函數(shù)值會由0開始隨時間t遞增，且迅速趨向于1，即j（t）∈［0，1）。因此，可通過連續(xù)函數(shù)j（t）對關(guān)節(jié)速度θ˙（t）進行約束以獲得實際可實現(xiàn)性的零初始關(guān)節(jié)速度。結(jié)合式（1）和式（2），并取向量z為零向量，設(shè)計關(guān)節(jié)速度為：

顯然，θ˙*（t）可隨時間t由0逐漸變化并收斂于（t）。

綜上所述，對于基于偽逆的冗余度解析方案，可用時變函數(shù)j（t）來約束關(guān)節(jié)速度，使之形成初值為0的連續(xù)過渡區(qū)，以更好地控制冗余機械臂，且具有簡單和數(shù)學(xué)易處理的特點。

2.2 位置誤差補償方法

因初始關(guān)節(jié)速度非零突變問題導(dǎo)致的速度誤差將傳導(dǎo)到末端速度，并形成末端位置誤差。如果沒有及時對該位置誤差進行消除或減小，那么它將存在于整個任務(wù)的執(zhí)行過程中，影響軌跡跟蹤精度，甚至?xí)?dǎo)致任務(wù)失敗。為了解決這一問題，可采用以下位置誤差補償方法[10]。

為了更快地減小誤差，使誤差函數(shù)（4）收斂于0，設(shè)計d（t）的時間導(dǎo)數(shù)如下[10]：

式中：λ0≥0—可任意設(shè)置的標(biāo)量系數(shù)。

將式（5）移項，得：

設(shè)末端執(zhí)行器在t=t1時的位置誤差為d（t1）=r（t1）-rd（t1）。把式（6）進行積分運算，并結(jié)合條件 t=t1時，d（t）=d（t1）進行整理，可求得：

可見當(dāng)t→∞時，d（t）→0，r（t）能隨時間按指數(shù)規(guī)律收斂到

把式（7）對時間求導(dǎo)，整理得：

即 t→∞ 時，d˙（t）亦收斂至 0。

綜上所述，機械臂末端執(zhí)行器速度可以綜合為：

3 仿真研究

在本節(jié)中，以平面四連桿機械臂分別跟蹤圓形和螺旋線曲線軌跡為例，對基于偽逆算法的零初始關(guān)節(jié)速度運動規(guī)劃方案進行MATLAB仿真，以討論和分析該方案的有效性。該機械臂相關(guān)參數(shù)如下：n=4，m=2，冗余度為 n-m=2，四個連桿的長度為 l=［l1，l2，l3，l4］T=［1，0.8，0.7，0.5］Tm。

3.1 圓形軌跡跟蹤

設(shè)機械臂末端執(zhí)行器需要跟蹤一個半徑r為1.0m的圓形軌跡，該圓形的參數(shù)方程為：

其中，ω=2π/T，任務(wù)執(zhí)行周期取T=20s，機械臂的初始關(guān)節(jié)角向量為［2π/3，-π/3，-π/3，0］Trad。

圖1 偽逆算法（1）求解所得的運動曲線（t=（0～5）s）Fig.1 Profile with Pseudoinverse Algorithm（1）（t=（0～5）s）

由偽逆算法（1）對圓形方程在速度層上進行反解仿真可知，機械臂在跟蹤期望軌跡過程中，X軸上的位置最大誤差值為ex=9.077×10-6m，Y軸上的位置最大誤差值為ey=3.626×10-6m，機械臂很好地完成軌跡跟蹤任務(wù)。此種情況下，實際軌跡和期望軌跡之間的實時誤差非常小。但關(guān)節(jié)速度的初始值并不為0。截取t=（0～5）s時段的θ˙曲線，如圖 1 所示。其中，θ˙（0）=［-0.047613，0.107128，0.138552，0.057730］Trad/s。由于電機在實際運動時，速度不能突變，因此，采用式（3）對關(guān)節(jié)速度θ˙進行約束。仿真實驗結(jié)果，如圖2所示。

圖2 加時變系數(shù)j（t）約束后的仿真情況Fig.2 Simulation Results with Time-Varying Coefficient j（t）

從仿真結(jié)果可見：采用時變系數(shù)j（t）進行約束后，關(guān)節(jié)速度θ˙*的初始值為 0，且在任務(wù)開始時的一小段時間內(nèi)||隨時間平滑增大。但從圖2（b）可看到，機械臂的實際跟蹤軌跡已明顯偏離期望軌跡，誤差絕對值增大較快，在t=0.3783s時誤差值ex=-0.0002456m，ey=0.0157m，即開始任務(wù)后的瞬間就產(chǎn)生較大的誤差，且持續(xù)存在到任務(wù)結(jié)束。該誤差主要是由于機械臂被約束從零初始速度而不是從較大的期望速度開始運動而引起的。值得注意的是，在實際情況下，機械臂控制電機的速度只能從零開始快速增大到控制值，如果直接采用偽逆算法（1）的反解結(jié)果也必然會導(dǎo)致末端執(zhí)行器出現(xiàn)較大位置誤差。為了減少該位置誤差的影響，把關(guān)節(jié)速度優(yōu)化方法（3）與位置誤差補償方法（8）相結(jié)合，即約束關(guān)節(jié)初始速度為零且考慮位置誤差補償?shù)那闆r，取λ0=2進行仿真驗證。結(jié)果，如圖3所示。由圖3（a）可見，實際跟蹤軌跡與期望的任務(wù)軌跡基本重合。位置誤差曲線，如圖3（b）所示，Y軸方向上的位置最大誤差值發(fā)生在t=0.1964s，ey=0.01492m，X軸方向上的位置最大誤差絕對值發(fā)生在t=0.281s，此時ex=-0.0002258m；隨后誤差絕對值迅速減小，在 t=2.337s時 ex=-1.287×10-6，ey=6.727×10-5m；t=3.087s時ex=-7.915×10-8m，ey=9.879×10-7m。結(jié)合圖和數(shù)據(jù)表明，實際跟蹤軌跡在很短時間內(nèi)快速且平滑地向期望的任務(wù)軌跡收斂，且位置誤差非常小。仿真結(jié)果證明了該方案的有效性。

圖3 取時變系數(shù)j（t）、λ0=2時的仿真情況Fig.3 Simulation Results with Time-Varying Coefficient j（t）and λ0=2

綜上所述，當(dāng)冗余度機械臂末端執(zhí)行器跟蹤給定的圓形軌跡時，計算機仿真結(jié)果表明，采用基于偽逆的冗余度機械臂零初始關(guān)節(jié)速度的運動規(guī)劃方案，選擇合適的j（t）、λ0，方案能起到有效的且較好的跟蹤性能。其中，j（t）使得關(guān)節(jié)速度獲得零初始值，而λ0是在軌跡跟蹤過程中減小位置誤差快慢的調(diào)節(jié)因子。值得一提的是，時變系數(shù)j（t）表達式里的參數(shù)是影響軌跡跟蹤初期的關(guān)節(jié)速度和位置誤差的重要因素，在實踐中可以根據(jù)硬件來設(shè)計適當(dāng)小的參數(shù)。

3.2 螺旋線軌跡跟蹤

在該節(jié)中，期望末端執(zhí)行器跟蹤一螺旋線軌跡，該螺旋線的參數(shù)方程是：

機械臂的關(guān)節(jié)角向量初始值為［π/2，-π/4，-π/3，0］Trad，任務(wù)執(zhí)行周期為T=20s，軌跡參數(shù)ω=2π/T，r=1.4m。對螺旋線軌跡跟蹤的仿真情況討論如下。

圖4 優(yōu)化后末端執(zhí)行器在X軸和Y軸的位置誤差Fig.4 Position Error of the End Effector in X and Y Axis After Optimized

設(shè)置λ0=2，優(yōu)化后的關(guān)節(jié)角θ和關(guān)節(jié)速度θ˙*是平滑的、連續(xù)變化的，并且θ˙*的初始值已約束為0。從圖4的位置誤差曲線可以看出，由于關(guān)節(jié)速度θ˙*被約束為從0開始連續(xù)變化，使得在末端執(zhí)行器跟蹤期望軌跡的初期，跟蹤軌跡出現(xiàn)了較大的誤差。位置最大誤差絕對值發(fā)生在t=0.21s，此時有ex=-0.08517m，ey=0.05165m。隨后誤差絕對值均迅速減小，在t=2.756s時ex=-4.645×10-6m，ey=2.752×10-5m；t=3.367s時 ex=-5.667×10-8m，ey=6.916×10-7m。顯然，機械臂能自動調(diào)節(jié)，使末端執(zhí)行器的運動軌跡能在較短時間內(nèi)收斂于期望軌跡。綜上所述，從跟蹤圓形和螺旋線軌跡這兩個實例可以看出，所提出機械臂零初始速度的運動規(guī)劃方案能夠提供一種靈活、有效的方法來使得關(guān)節(jié)從零初始速度開始平穩(wěn)地執(zhí)行任務(wù)，降低機械沖擊，減小對電機的損害；并且在任務(wù)期間，若跟蹤軌跡發(fā)生偏離，也能迅速地自動調(diào)整，使機械臂盡快收斂到期望軌跡。值得指出的是，將關(guān)節(jié)初始速度不為零的情況進行仿真實驗，以供說明和討論，而實際中，初始速度為零或不為零的情況都有，仿真實驗驗證運動規(guī)劃方案對這些情況均能有效、準(zhǔn)確和靈活地進行調(diào)整。

4 結(jié)論

針對冗余度機械臂在執(zhí)行軌跡跟蹤任務(wù)時出現(xiàn)的初始關(guān)節(jié)速度不為零這一問題，提出了一種基于偽逆的冗余度機械臂的運動規(guī)劃方案。該方案約束機械臂關(guān)節(jié)從零速度開始跟蹤期望軌跡，并能及時減小執(zhí)行任務(wù)期間產(chǎn)生的末端執(zhí)行器位置誤差，保證機械臂成功跟蹤期望軌跡。利用平面四連桿機械臂進行MATLAB仿真，在設(shè)置參數(shù)λ0=2時，末端執(zhí)行器位置誤差在t≈3s時達到10-7m精度，仿真結(jié)果表明了所提方案的可行性和有效性。