基于偽逆的導軌機械臂關節速度糾偏運動規劃方案

李克訥，張 增，王溫鑫

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

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

0 引言

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

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

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

1 運動規劃方案描述

1.1 冗余度解析

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

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

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

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

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

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

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

當導軌機械臂的關節空間維數大于任務空間的維數時（n>m），式（4）可能存在多個或無窮個相應的期望關節速度解。可采用偽逆算法得到期望關節速度的解析解［17］，即：

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

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

1.2 關節速度糾偏

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

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

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

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

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

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

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

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

式中：T為任務執行的時間周期。

1.3 誤差修正

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

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

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

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

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

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

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

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

進而可得：

式中C為待定系數。

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

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

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

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

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

2 實驗仿真與結果分析

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

2.1 弧形軌跡跟蹤

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

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

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

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

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

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

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

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

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

2.2 直線軌跡跟蹤

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

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

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

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

圖6（a）顯示了機械臂的運動過程。從圖6（b）可見，末端執行器實際運動軌跡明顯偏離了期望軌跡。從圖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。可見，在速度偏離后的很短時間內機械臂末端執行器產生了較大的位置誤差，且誤差持續存在于整個任務的執行過程中。該誤差的產生主要是因為圖6（d）中的關節速度曲線在t=4.783 6 s 時刻偏離了圖5（d）所示的期望關節速度曲線，這不利于機械臂的安全運行及任務的執行。

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

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

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

圖7（a）顯示了機械臂的運動過程。在圖7（b）中，末端執行器的實際運動軌跡與期望軌跡基本重合，表明優化后的機械臂能順利跟蹤期望軌跡。從圖7（c）可以看到，X軸方向的位置最大誤差值出現在t=5.260 4 s 時刻，|ex|max=0.005 7 m；Y軸方向的位置最大誤差值出現在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）可以看到，當關節速度在t=4.783 6 s 時刻發生偏離后能自動收斂于圖5（d）中顯示的期望關節速度曲線，且在任務執行過程中隨時間連續且平滑地變化。由仿真結果可知，當機械臂的基座運動軌跡以及期望跟蹤軌跡發生改變時，本文所提的糾偏方案仍然可行。

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

3 結語

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