六自由度串聯機械臂最優軌跡規劃

劉豪進,趙炎康,周振興

(中南林業科技大學，湖南 長沙 410007)

機械手的規劃軌跡精度取決于機械臂的運動學模型的建立，機械臂的各關節空間的路徑軌跡規劃以及機械臂的插值算法補償[1]。本文從ABB IRB1660工業型機械臂參數設計入手，建立機械臂仿真模型，在正逆運動學仿真計算的基礎上對機械臂進行關節空間軌跡規劃，模擬仿真機械臂末端執行其在空間任意兩點間的運動過程，分析其運動學參數，使用 Robotics Toolbox[2]機器人工具箱，對連桿進行軌跡規劃，針對機械手臂運動特性入手，完成仿真分析。

1 機械手臂模型建立

1.1 D-H坐標系表述

為了具體分析機械臂運動詳情，建立運動學方程[3]是必要的，而現在大多數是多自由度的機械臂，具有多根連桿和關節，也就需要針對每一根連桿和關節來建立坐標系，并建立起各個坐標系的關聯。為了論述鄰近兩連桿之間空間位置關系，引入了D-H參數法。D-H參數法為關節鏈中的每一根連桿桿件建立了對應坐標系，如圖1所示。

圖1 連桿坐標簡化模型

相鄰兩連桿之間的參數：①兩根連桿之間(兩公垂線ai軸與ai-1軸之間)的距離di；②笛卡爾空間坐標xi軸線、yi軸線和zi軸線；③連桿轉角θi為兩根公垂線 ai與ai-1之間的夾角，即xi軸與xi-1軸之間的夾角，繞zi-1軸從xi-1旋轉到xi軸，符合右手規則時為正；④連桿扭角ai為兩關節軸線之間的夾角，即zi軸與 zi-1軸之間的夾角，繞x軸從zi-1軸旋轉到zi軸；⑤第i根連桿就用L來表示[4]。

而對于轉動關節， d是常數；對于移動關節， di是變量。對于轉動關節，θi為變量；對于移動關節， θi為常數。當兩關節軸線平行時， ai=0；當兩關節軸線垂直時， ai=90°。

通過測量得到ABB IRB1660型機械臂D-H參數為下表1所示。

表1 ABB IRB1660連桿參數

注：ai表示第i根公垂線，αi表示第i根連桿扭角，di表示公垂線之間的軸線距離，θi表示第i根連桿轉角。

2 機械臂軌跡規劃仿真

機械臂軌跡仿真需要利用MATLAB自帶畫圖程序來實現，用到拆分窗口函數，以及畫圖函數。將預先規劃好的時間 、關節旋轉角度、角速度及其加速度等參數輸入進函數，即可繪圖模擬[5]。用分析函數進行曲線擬合分析。

2.1 建立初始化機械臂連桿模型

本文主要使用的是Robotics Toolbox 為機械臂的仿真，創建相應的機械臂對象模型。

通過語句來使機械臂狀態初始化,代入表1數據：>>L1=link([0,0,pi/2,0,0],'standard')

……

用SerialLink函數建立機械臂模型，并用畫圖teach函數畫出，如圖2。

>>robot=SerialLink([L1,L2,L3,L4,L5,L6]);

>>robot.name='Robotic6R';

>>q0=[0 0 0 0 0 0];>>robot.plot(q0);

圖2 機械手臂仿真圖

然后調節teach函數來驅使機械手臂進行動作，通過調節相應滑條來驅使連桿關節發生旋轉，模擬實際操作機械手臂的全過程。

針對機械臂的運動學特性，進行分析仿真。工具箱中fkine、ikine函數可以求運動學正、逆問題[6-8]。首先設置六個關節扭角分別為：-pi/8、-pi/8、-pi/4、0、pi/8、0，用q向量來代替，計算末端位置T。編程如下:

>> q=[-pi/8 -pi/8 -pi/4 0 pi/8 0];

q =

-0.3927 -0.3927 -0.7854 0 0.3927 0

>> T=fkine(Robotic6R,q);

T =

0.1913 -0.8446 0.5000 1.2597

-0.7325 -0.4619 -0.5000 0.0202

0.6533 -0.2706 -0.7071 0.5904

0 0 0 1.0000

T就是機械臂在扭角為q時的末端位置。

借助ikine函數用已知的空間位置T來反求各個關節的轉動角度，編程如下:

>> qi=ikine(Robotic6R,T);

qi =

-0.3927 -0.3927 -0.7854 0.0000 0.3927 -0.0000

qi為已知ABB機械臂末端執行器位置為T時各關節的轉角。由上述仿真計算結果得qi=q。綜上所述，通過求解運動學正問題，反求運動學逆問題得到初始角度值，證明Matlab中的機械臂物理建模完全正確。

2.2 仿真軌跡規劃

關節空間中機械臂的每一條軌跡運動通過拆解為每個關節轉動一定角度所形成，反映各關節轉角與機器人末端執行器的位置或姿態一一對應。對機械臂仿真，進行軌跡規劃時，用到工具箱中的jtraj函數和ctraj函數。Jtraj函數是已知初始和終止的關節角度，利用五次多項式公式來進行關節空間軌跡分析； ctraj函數是已知初始和終止的末端關節姿態，利用勻速加速度和勻速減速度來規劃笛卡爾空間坐標軌跡規劃[9]。

2.2.1 關節空間軌跡規劃

設置空間中任意兩點的轉角向量qz=[0 0 0 0 0 0]和q=[-pi/8 -pi/8 -pi/4 0 pi/8 0],采樣總時長為5秒，采樣間隔為0.02秒。設置部分程序代碼如下：

>>qz=[0 0 0 0 0 0];

>>q=[-pi/8 -pi/8 -pi/4 0 pi/8 0];

>>t=[0:.02:5];

>>[qf,qd,qdd]=jtraj(q,qz,t);

>>subplot(3,2,1);

>>plot(t,qf(:,1));

>>xlabel('時間/s');

>>ylabel('角1旋轉角度/rad');

……其中qf是關節旋轉角度，qd是關節角速度，qdd是在關節角加速度。將程序中qf換成qd或qdd得到六個關節角的角速度和角加速度關于時間的函數圖像。得到關節旋轉角度與時間的關系函數如圖3所示，關節角速度與時間的關系函數如圖4所示，關節角加速度與時間的關系函數如圖5所示。

圖3 各關節角旋轉角度仿真曲線

圖4 各關節角速度仿真曲線

機械臂從初始位置qz到達目標位置q，以關節1為參考對象：圖3中關節1從-0.4(/rad)角度經過5秒到達0(/rad)角度。圖4中，關節1的角速度從第0秒開始上升，在第2.5秒左右達到峰值，在第5秒完全回落為0(/rad)。關節1角速度有一個先快后慢的過程，在第2.5秒速度最大。圖5中，關節1的角加速度在第1秒上升到頂峰，第2.5秒下降為0(/rad)，在第4秒下降為最低值，在5秒上升為0(/rad)。關節1角加速度有一個先上升后下降再上升的過程，在第1秒加速最快，第4秒加速最慢，在第2.5秒和第5秒不加速。

圖5 各關節角加速度仿真曲線

2.2.2 軌跡曲線擬合

在現實生活中，機械臂接收信號頻率過疏，則會造成機械臂的反應遲鈍，不能很好地擬合理想化的規劃軌跡。但接收信號過密，則會造成機械臂為了更好地擬合理想化的規劃軌跡產生過平滑，而不停抖動或偏離規劃軌跡。因此在軌跡規劃時要選擇更高效實惠的軌跡規劃，兼顧軌跡平滑與機械臂動作平滑。

為了驗證仿真中更符合實際需求的軌跡規劃，先設置橫向比較，再進行縱向排除，得到最優軌跡規劃。

選擇有效的關節角函數圖像，挑選出第一關節角的旋轉角度、角速度和角加速度作為參考基準圖像。

首先對第一關節角的旋轉角度分別用三次方多項式(如圖6)、三次多項式三次樣條插值(如圖6)、四次多項式三次樣條插值(如圖7)與五次多項式三次樣條插值(圖7)進行函數曲線擬合，得到殘差值和函數表達式。

圖6 旋轉角度三次多項式與其三次樣條插值擬合與殘差模對比

圖7 旋轉角度四次多項式與五次多項式三次樣條插值擬合與殘差模對比

經過對比，在一定時間內第一關節旋轉角度環境下，三次多項式三次樣條插值擬合得到的曲線與四次多項式三次樣條插值擬合得到的曲線基本一致。而利用五次多項式三次樣條插值來進行擬合得到的殘差模最小。并且由于三次多項式和三次多項式三次樣條插值的方程和曲線一致，因為在下列對第一關節的角速度和角加速度的函數擬合中，將二者合并為一個比較項。

對第一關節角速度進行三次多項式三次樣條插值(如圖8)、四次多項式三次樣條插值(如圖8)與五次多項式三次樣條插值(圖9)進行函數曲線擬合，得到殘差值和函數表達式。

圖8 角速度三次多項式與四次多項式三次樣條插值擬合與殘差模對比

圖9 角速度五次多項式三次樣條插值擬合與殘差模

經過對比，在角速度環境下，利用五次多項式三次樣條插值來進行擬合得到的殘差模雖然最小，但是利用四次多項式三次樣條插值來進行擬合得到的殘差模與前者非常接近，而且從曲線表現形式上來看，四次多項式三次樣條插值的擬合曲線更平滑，而不會為了擬合曲線而頻率過高地小幅度劇烈抖動，所以在角速度的環境下，利用四次多項式三次樣條插值來擬合曲線最合適。

對第一關節角加速度進行三次多項式三次樣條插值(如圖10)、四次多項式三次樣條插值(如圖10)與五次多項式三次樣條插值(圖11)進行函數曲線擬合，得到殘差值和函數表達式。

圖10 角加速度三次多項式與四次多項式三次樣條插值擬合與殘差模對比

經過對比，在角加速度環境下，利用四次和五次多項式三次樣條插值來進行擬合得到的殘差模雖然最小，但是利用三次多項式三次樣條插值來進行擬合得到的殘差模與前二者非常接近，而且從曲線表現形式上來看，三次多項式三次樣條插值的擬合曲線更平滑，而不會為了擬合曲線而頻率過高地小幅度劇烈抖動，所以在角加速度的環境下，利用三次多項式三次樣條插值來擬合曲線最合適。

綜上所述，在一定時間內旋轉角度的環境下，選擇五次多項式三次樣條插值來進行擬合軌跡效率最佳。在第一關節角速度環境下，選擇四次多項式三次樣條插值來進行擬合軌跡效率最佳。在第一關節角加速度環境下，選擇三次多項式三次樣條插值來進行擬合軌跡效率最佳。

3 結論

本文利用D-H參數法對ABB IRB1660型號機械臂進行了參數采集和物理建模；基于Matlab對機械臂進行初始化，通過正解反求逆解驗證建模的合理性；將軌跡拆解為每一個關節角的旋轉動作并分析最優解擬合軌跡。本文重在探索機械臂的仿真模擬特性，有極強的經濟性，并有助于減少實際操作中出現的問題。