李克訥，張 增，王溫鑫

（廣西科技大學電氣與信息工程學院，廣西柳州545006）

（?通信作者電子郵箱likene@163.com）

0 引言

近年來，機器人技術在現(xiàn)代工業(yè)生產(chǎn)中廣泛應用，并帶來了實際生產(chǎn)效益，使其受到了越來越多的關注和重視［1-3］。其中，機械臂運動規(guī)劃是機器人技術中的一個重要組成部分［4-7］。在實際工業(yè)生產(chǎn)中，基座固定的工業(yè)機械臂由于臂長與自由度的原因，工作空間是有限的。然而，工作場所內常常擺放著一些在機械臂工作空間之外的裝配部件和需搬運的物品，因此可能會出現(xiàn)機械臂因末端不能到達物體所在位置而無法完成給定的抓取任務。導軌機械臂［2，8］將機械臂基座安裝在導軌上并實現(xiàn)一定距離的移動，擴大了機械臂的作業(yè)空間，使其更為有效地完成給定任務。因此，導軌機械臂逐漸被應用于工業(yè)生產(chǎn)之中［2，8-9］。

通常，機械臂實際應用場所的環(huán)境比較復雜，充滿了各種不確定的因素。特別是在環(huán)境的溫濕度變化較大、電磁輻射干擾、傳感器誤差、機械臂關節(jié)電機本身的反應能力差等各種因素的影響下，機械臂的關節(jié)速度值可能會偏離期望值［6，10-13］。在實際工程應用中，為了順利完成任務，導軌機械臂的基座和末端執(zhí)行器以及各個關節(jié)之間的運動需要相互協(xié)調。對于導軌機械臂而言，其基座在運動過程中也可能會對關節(jié)速度產(chǎn)生影響并使之偏離期望值。一旦關節(jié)速度偏離期望值，就會造成關節(jié)位移的偏差。該偏差會通過導軌機械臂的連桿結構逐級累加至末端執(zhí)行器，造成末端執(zhí)行器的運動軌跡偏離期望軌跡，從而無法將所需零部件運送到指定地點，進而導致任務的失敗。而且，若末端執(zhí)行器的運動軌跡與期望軌跡相差過大還會使導軌機械臂與環(huán)境障礙物發(fā)生碰撞，致使機械臂損壞，甚至危及操作人員及周圍物品的安全。由此可見，在任務執(zhí)行過程中，關節(jié)速度偏離期望值問題的解決是導軌機械臂安全和順利工作的重要保障，具有重要的實際意義。但目前有關該方面的研究較少。文獻［10］提出了一種基于退化雅可比矩陣的速度突變抑制方案，該方案有效抑制了機械臂因關節(jié)故障而引起的末端速度突變，使末端速度能收斂于任務執(zhí)行的期望解。文獻［14］利用三次多項式樣條插值的方法生成機械臂的期望軌跡曲線，并且將曲線進行離散化處理，保證了機械臂在運行過程中實際關節(jié)速度與期望速度的良好跟隨性。還有學者將冗余度機械臂的協(xié)作問題轉化成一個可完全分解的受約束二次規(guī)劃求解問題，并采用對偶遞歸神經(jīng)網(wǎng)絡求解該運動控制模型，以此對關節(jié)速度進行協(xié)調控制［15］。但是，這些方法都是在速度層上針對基座固定的機械臂進行運動規(guī)劃以保證關節(jié)或末端速度能跟隨規(guī)劃的期望曲線，無法直接應用于解決導軌機械臂關節(jié)速度偏離期望值的問題。

為此，本文設計了一種導軌機械臂關節(jié)速度糾偏運動規(guī)劃方案，旨在糾正導軌機械臂在任務執(zhí)行過程中的關節(jié)速度偏離現(xiàn)象；同時，還對末端執(zhí)行器隨基座一起運動所造成的實時軌跡和期望軌跡之間的位置誤差進行誤差修正。本文主要工作如下：1）根據(jù)正向運動學方程采用偽逆算法對導軌機械臂在速度層上進行求解；2）對設計的關節(jié)速度糾偏方法以及末端執(zhí)行器位置誤差修正方法給出詳細的說明及推導；3）采用Matlab軟件進行仿真以驗證所提方案的可行性和有效性。

1 運動規(guī)劃方案描述

1.1 冗余度解析

定義導軌機械臂基座固定時的末端執(zhí)行器笛卡兒位置向量為x(t) ∈Rm（R 為實數(shù)空間，m為工作空間維數(shù)），根據(jù)正向運動學方程［16］可得：

式中：θ(t) ∈Rn為關節(jié)角向量（n為關節(jié)空間維數(shù)）；h(?) ∈Rm為由機械臂結構參數(shù)推導所得的非線性可微函數(shù)。

將式（1）對時間t求導可得：

式中：(t) ∈Rm為基座固定時的末端執(zhí)行器速度向量；(t) ∈Rn為關節(jié)速度向量；J為m×n維的雅可比矩陣。

根據(jù)任務執(zhí)行要求，可獲得執(zhí)行任務所需的期望位置向量rd(t) ∈Rm和期望速度向量(t) ∈Rm。當基座以某一規(guī)劃速度v(t) ∈Rm沿著指定軌跡s(t) ∈Rm移動時，定義基座移動時的末端執(zhí)行器笛卡兒位置向量為r(t) ∈Rm?？紤]到基座運動對機械臂末端的影響，為了順利執(zhí)行任務，需要滿足關系r(t)=rd(t)=x(t) +s(t)和(t)=(t)=(t) +v(t)。因此，對于導軌機械臂，可根據(jù)期望位置向量和基座運動狀態(tài)在執(zhí)行任務前將(t)的期望值(t)設定為：

結合式（2）和式（3）可得：

式中：(t) ∈Rn為期望關節(jié)速度向量。

當導軌機械臂的關節(jié)空間維數(shù)大于任務空間的維數(shù)時（n>m），式（4）可能存在多個或無窮個相應的期望關節(jié)速度解?？刹捎脗文嫠惴ǖ玫狡谕P節(jié)速度的解析解［17］，即：

式中：JΔ為雅可比矩陣J的廣義逆矩陣，JΔ=JT(JJT)-1∈Rn×m；I∈Rn×n為單位矩陣；z∈Rn為一個可以任意設置的向量，通常用來滿足第二任務的實現(xiàn)，如避障、關節(jié)奇異位形躲避等［18］。

值得指出的是，本文的目的在于糾正任務執(zhí)行過程中偏離期望值的關節(jié)速度，為了方便探討和研究，可將向量z設置為零向量。因此，式（5）可簡化為：

1.2 關節(jié)速度糾偏

為了糾正導軌機械臂在任務執(zhí)行過程中發(fā)生偏離的關節(jié)速度，本文對機械臂關節(jié)速度進行約束控制，使發(fā)生偏離的關節(jié)速度能收斂至期望值。

設計如下時變函數(shù)j(t)：

式中：μ（μ> 0）為控制電機變速過程快慢的標量系數(shù)，可根據(jù)電機性能和實際需要進行設定；tp為關節(jié)速度發(fā)生偏離的時刻。

顯然，當t≥tp時，時變函數(shù)值會由1 逐漸減少，最后衰減于0。本文仿真部分取μ=3.4。

設偏離期望值后的關節(jié)速度為(t)，定義其與期望關節(jié)速度之間的差異量為：

當關節(jié)速度偏離期望值時，結合式（7）和式（8）可設計關節(jié)速度(t)為：

顯然，當t≥tp時(t)可隨時間逐漸收斂于(t)。因此，可用時變函數(shù)j(t)控制導軌機械臂使偏離的關節(jié)速度回到期望的關節(jié)速度上。

綜上所述，結合式（6）和式（9）可將導軌機械臂的關節(jié)速度(t)規(guī)劃為：

式中：T為任務執(zhí)行的時間周期。

1.3 誤差修正

需要注意的是，根據(jù)正向運動學方程，x(t)和(t)都與相應的關節(jié)速度(t)有著密切的關系。如果關節(jié)速度偏離冗余度解析求解得到的期望關節(jié)速度，即使采用糾偏方法使其回到期望值，仍然會導致x(t)和(t)出現(xiàn)誤差。對于導軌機械臂，基座運動時的末端執(zhí)行器位置向量r(t)、速度向量(t)分別滿足關系：r(t)=x(t) +s(t)，(t)=(t) +v(t)。因此，如果沒能及時對該誤差進行消除或減少，它將導致r(t)、(t)分別偏離任務期望位置向量rd(t)、任務期望速度向量(t)，從而影響任務執(zhí)行的精度，甚至造成任務失敗。為了解決這一問題，可采用以下設計的位置誤差修正方法。

定義x(t)與期望值xd(t)之間的實時誤差σ(t)為：

為了減少該誤差，設計σ(t)對時間的導數(shù)(t)如下：

式中κ為可任意設置的非負標量系數(shù)。

進一步地，可將κ設計為時變系數(shù)：

式中κ0為可任意設置的正標量系數(shù)。本文仿真取κ0=1。

將式（13）代入式（12），可得：

以下證明若在t=t1時產(chǎn)生了誤差σ(t1)=x(t1)-xd(t1)，x(t)與(t)在式（14）的條件下，會隨時間按指數(shù)規(guī)律分別收斂到xd(t)、d(t)。

進而可得：

式中C為待定系數(shù)。

將式（18）代入式（17）可得：

將式（20）對時間t求導可得：

由 式（20） 和式（21） 可知當t→+∞ 時，|x(t)-xd(t)|→0，|(t)-(t)|→0?？梢妜(t)、(t)能隨時間按指數(shù)規(guī)律分別收斂到xd(t)、(t)，從而保證了當基座移動時，若關節(jié)速度發(fā)生了偏離，末端執(zhí)行器仍能順利完成給定任務。證畢。

綜上所述，導軌機械臂的關節(jié)速度糾偏運動規(guī)劃方案控制框圖如圖1所示。

圖1 導軌機械臂關節(jié)速度糾偏運動規(guī)劃方案控制框圖Fig.1 Control block diagram of motion planning scheme for deviation correction of rail manipulator joint velocity

2 實驗仿真與結果分析

由于本文的目的僅在于糾正導軌機械臂偏離期望值的關節(jié)速度以及由此引起的末端執(zhí)行器偏離期望軌跡的現(xiàn)象，且不包括其他優(yōu)化指標。因此，在本章中，以基座直線移動和圓形移動的四連桿冗余度機械臂為例，對導軌機械臂關節(jié)速度糾偏運動規(guī)劃方案進行了仿真實驗，以討論和分析該方案的有效性。機械臂相關參數(shù)如下：關節(jié)空間維數(shù)n=4，工作空間維數(shù)m=2，四個連桿長度分別為[1，0.8，0.7，0.5]T（單位為m）。

2.1 弧形軌跡跟蹤

設基座直線移動機械臂的末端執(zhí)行器從指定起始點去跟蹤一段弧形軌跡。機械臂的實際關節(jié)角向量qa和期望關節(jié)角向量qd的初始值（單位為rad）?；⌒诬壽E的參數(shù)方程為：

圖2 基于偽逆算法的弧形軌跡跟蹤仿真Fig.2 Simulation of circular trajectory tracking based on pseudoinverse algorithm

圖2（a）給出的是機械臂的運動過程。圖2（b）表示當基座運動時末端執(zhí)行器跟蹤給定弧形軌跡的執(zhí)行狀況。從圖2（b）可見，機械臂的實際運動軌跡與期望的軌跡基本重合。圖2（c）給出的是末端執(zhí)行器分別在X軸和Y軸方向隨時間變化的位置誤差曲線。由誤差曲線可知，X軸方向和Y軸方向誤差值均小于1×10-5m，可見偽逆算法能保證機械臂有較好的軌跡跟蹤精度；而且，根據(jù)圖2（d）給出的關節(jié)速度的變化曲線，可知關節(jié)速度在機械臂運行過程中保持連續(xù)光滑的狀態(tài)，有利于關節(jié)電機的運行及任務的執(zhí)行。

為了模擬關節(jié)速度在執(zhí)行任務過程中偏離期望值的情形，本節(jié)仿真在t=2.524 1 s 處給關節(jié)速度加擾動使其偏離期望值，具體仿真結果如圖3所示。

圖3 關節(jié)速度受擾的弧形軌跡跟蹤仿真Fig.3 Simulation of circular trajectory tracking suffered from joint velocity disturbance

圖3（a）給出了機械臂的運動過程。圖3（b）給出了基座運動時末端執(zhí)行器跟蹤給定弧形軌跡的執(zhí)行狀況。從圖3（b）可見，末端執(zhí)行器實際運動軌跡明顯偏離了期望軌跡。而且，從圖3（c）的誤差曲線可以看到，在時段t=2.524 1～4.658 5 s 內，X軸方向的誤差值由增大到，Y軸方向的誤差值由增大到?？梢姡谒俣劝l(fā)生偏離后的很短時間內機械臂末端執(zhí)行器產(chǎn)生了較大的位置誤差，且誤差持續(xù)存在于整個任務執(zhí)行過程中。該誤差的產(chǎn)生主要是因為在圖3（d）中的關節(jié)速度在t=2.524 1 s 時刻偏離了圖2（d）所示的期望關節(jié)速度曲線，這不利于機械臂的安全運行及任務的執(zhí)行。

為了糾正任務執(zhí)行過程中偏離的關節(jié)速度，可采用時變函數(shù)j(t)對關節(jié)速度進行約束調整。而且，為了及時消除末端執(zhí)行器在任務執(zhí)行過程中產(chǎn)生的位置誤差，可考慮將關節(jié)速度糾偏方法（式（10））與誤差修正方法（式（14））相結合得到優(yōu)化方案（式（22）），仿真結果如圖4所示。

圖4（a）顯示了機械臂的運動過程。在圖4（b）中，末端實際運動軌跡與期望軌跡基本重合，表明優(yōu)化后的機械臂能順利跟蹤期望軌跡。從圖4（c）可以看到，X軸方向的位置最大誤差值出現(xiàn)在t=3.125 1 s 時刻,；Y軸方向的位置最大誤差值出現(xiàn)在t=3.080 6 s 時刻，。兩方向的位置誤差到最大值后都迅速減少，在t=5.090 4 s 時刻誤差分別減小到|ex|=5.140 6×10-5m，|ey|=1.452 4×10-5m。從圖4（d）可見，當關節(jié)速度在t=2.524 1 s 時刻發(fā)生偏離后能自動收斂于圖2（d）中所示的期望關節(jié)速度曲線，且關節(jié)速度在任務執(zhí)行過程中隨時間連續(xù)且平滑地變化。因此，仿真結果驗證了本文所提方案的有效性。

圖4 采用優(yōu)化方案的弧形軌跡跟蹤仿真Fig.4 Simulation of circular trajectory tracking by using optimized scheme

2.2 直線軌跡跟蹤

為了討論當機械臂的基座運動軌跡以及期望跟蹤軌跡發(fā)生改變時本文所提的糾偏方案仍然可行，本節(jié)討論末端執(zhí)行器跟蹤直線軌跡的仿真情況。機械臂的實際關節(jié)角向量qa和期望關節(jié)角向量qd的初始值。該直線軌跡的起點為(1.742，2.225)，終點為(-0.804 2，2.544)。執(zhí)行器的起始點坐標和直線起點坐標一致，任務執(zhí)行周期T=10 s?；\動的圓形軌跡參數(shù)方程為：

式中：參數(shù)λ=0.3，角頻率。為方便進行比較，首先在正常情況（關節(jié)速度無偏離）下用偽逆算法對機械臂跟蹤直線軌跡進行求解仿真，仿真結果如圖5所示。

圖5（a）給出了機械臂的運動過程。從圖5（b）可見，機械臂末端的實際運動軌跡與期望的軌跡基本重合。由圖5（c）可知，末端執(zhí)行器在X軸方向和Y軸方向的誤差值均保持在1×10-5m 之內?？梢?，偽逆算法能保證機械臂有較好的軌跡跟蹤精度。根據(jù)圖5（d）顯示的關節(jié)速度變化曲線，可知關節(jié)速度在任務執(zhí)行過程中保持連續(xù)光滑的狀態(tài)，有利于關節(jié)電機的運行。

為了模擬關節(jié)速度在執(zhí)行任務過程中偏離期望值的情形，在t=4.783 6 s 處給關節(jié)速度加擾動使其偏離期望值，具體仿真結果如圖6所示。

圖6（a）顯示了機械臂的運動過程。從圖6（b）可見，末端執(zhí)行器實際運動軌跡明顯偏離了期望軌跡。從圖6（c）可以看到，在t=4.783 6～6.639 8 s 時段內，X軸方向的誤差值由|ex|=2.274 3×10-6m增大到|ex|=0.457 0 m；Y軸方向的誤差值由|ey|=1.201 5×10-6m 增大到|ey|=0.285 1 m?？梢?，在速度偏離后的很短時間內機械臂末端執(zhí)行器產(chǎn)生了較大的位置誤差，且誤差持續(xù)存在于整個任務的執(zhí)行過程中。該誤差的產(chǎn)生主要是因為圖6（d）中的關節(jié)速度曲線在t=4.783 6 s 時刻偏離了圖5（d）所示的期望關節(jié)速度曲線，這不利于機械臂的安全運行及任務的執(zhí)行。

圖5 基于偽逆算法的直線軌跡跟蹤仿真Fig.5 Simulation of linear path trajectory tracking based on pseudoinverse algorithm

圖6 關節(jié)速度受擾的直線軌跡跟蹤仿真Fig.6 Simulation of linear path trajectory tracking suffered from joint velocity disturbance

為了糾正機械臂在任務執(zhí)行過程中產(chǎn)生偏離的關節(jié)速度，可采用時變函數(shù)j(t)對關節(jié)速度進行約束控制；并且，為了及時消除末端執(zhí)行器出現(xiàn)的位置誤差，將關節(jié)速度糾偏方法與誤差修正方法相結合得到優(yōu)化方案，仿真結果如圖7所示。

圖7（a）顯示了機械臂的運動過程。在圖7（b）中，末端執(zhí)行器的實際運動軌跡與期望軌跡基本重合，表明優(yōu)化后的機械臂能順利跟蹤期望軌跡。從圖7（c）可以看到，X軸方向的位置最大誤差值出現(xiàn)在t=5.260 4 s 時刻，|ex|max=0.005 7 m；Y軸方向的位置最大誤差值出現(xiàn)在t=5.343 9 s 時刻，|ey|max=0.001 6 m。兩方向的位置誤差達到最大值后均迅速降低，在t=7.486 6 s 時刻分別下降到|ex|=1.662 7×10-5m，|ey|=1.035 6×10-5m。從圖7（d）可以看到，當關節(jié)速度在t=4.783 6 s 時刻發(fā)生偏離后能自動收斂于圖5（d）中顯示的期望關節(jié)速度曲線，且在任務執(zhí)行過程中隨時間連續(xù)且平滑地變化。由仿真結果可知，當機械臂的基座運動軌跡以及期望跟蹤軌跡發(fā)生改變時，本文所提的糾偏方案仍然可行。

圖7 采用優(yōu)化方案的直線軌跡跟蹤仿真Fig.7 Simulation of linear path trajectory tracking by using optimized scheme

3 結語

本文提出了基于偽逆的導軌機械臂關節(jié)速度糾偏運動規(guī)劃方案，從對關節(jié)速度約束控制的角度入手，解決了導軌機械臂執(zhí)行軌跡跟蹤任務時關節(jié)速度偏離期望值的問題。同時，考慮了末端執(zhí)行器在任務執(zhí)行過程中產(chǎn)生位置誤差的不利影響，通過誤差修正方法，保證機械臂順利完成軌跡跟蹤任務，大大提升了方案的可行性。仿真結果表明了所提糾偏方案是可行且有效的。