基于偽逆的導(dǎo)軌機械臂關(guān)節(jié)速度糾偏運動規(guī)劃方案

李克訥，張 增，王溫鑫

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

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

0 引言

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

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

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

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

1.1 冗余度解析

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

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

將式（1）對時間t求導(dǎo)可得：

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

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

結(jié)合式（2）和式（3）可得：

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

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

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

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

1.2 關(guān)節(jié)速度糾偏

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

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

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

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

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

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

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

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

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

1.3 誤差修正

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

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

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

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

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

式中κ0為可任意設(shè)置的正標(biāo)量系數(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求導(dǎo)可得：

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

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

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

2 實驗仿真與結(jié)果分析

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

2.1 弧形軌跡跟蹤

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

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

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

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

圖3 關(guān)節(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 內(nèi)，X軸方向的誤差值由增大到，Y軸方向的誤差值由增大到?？梢?，在速度發(fā)生偏離后的很短時間內(nèi)機械臂末端執(zhí)行器產(chǎn)生了較大的位置誤差，且誤差持續(xù)存在于整個任務(wù)執(zhí)行過程中。該誤差的產(chǎn)生主要是因為在圖3（d）中的關(guān)節(jié)速度在t=2.524 1 s 時刻偏離了圖2（d）所示的期望關(guān)節(jié)速度曲線，這不利于機械臂的安全運行及任務(wù)的執(zhí)行。

為了糾正任務(wù)執(zhí)行過程中偏離的關(guān)節(jié)速度，可采用時變函數(shù)j(t)對關(guān)節(jié)速度進行約束調(diào)整。而且，為了及時消除末端執(zhí)行器在任務(wù)執(zhí)行過程中產(chǎn)生的位置誤差，可考慮將關(guān)節(jié)速度糾偏方法（式（10））與誤差修正方法（式（14））相結(jié)合得到優(yōu)化方案（式（22）），仿真結(jié)果如圖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）可見，當(dāng)關(guān)節(jié)速度在t=2.524 1 s 時刻發(fā)生偏離后能自動收斂于圖2（d）中所示的期望關(guān)節(jié)速度曲線，且關(guān)節(jié)速度在任務(wù)執(zhí)行過程中隨時間連續(xù)且平滑地變化。因此，仿真結(jié)果驗證了本文所提方案的有效性。

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

2.2 直線軌跡跟蹤

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

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

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

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

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

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

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

為了糾正機械臂在任務(wù)執(zhí)行過程中產(chǎn)生偏離的關(guān)節(jié)速度，可采用時變函數(shù)j(t)對關(guān)節(jié)速度進行約束控制；并且，為了及時消除末端執(zhí)行器出現(xiàn)的位置誤差，將關(guān)節(jié)速度糾偏方法與誤差修正方法相結(jié)合得到優(yōu)化方案，仿真結(jié)果如圖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）可以看到，當(dāng)關(guān)節(jié)速度在t=4.783 6 s 時刻發(fā)生偏離后能自動收斂于圖5（d）中顯示的期望關(guān)節(jié)速度曲線，且在任務(wù)執(zhí)行過程中隨時間連續(xù)且平滑地變化。由仿真結(jié)果可知，當(dāng)機械臂的基座運動軌跡以及期望跟蹤軌跡發(fā)生改變時，本文所提的糾偏方案仍然可行。

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

3 結(jié)語

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