利用Matlab做逆向運動學以控制6軸Robotic Arm

相關閱讀

(1) Project：利用3D printer製作6軸機械手臂 (1)

(2) Project：利用3D printer製作6軸機械手臂 (2)

(3) Project：利用3D printer製作6軸機械手臂 (3) 最終回的測試

(4) 利用LabVIEW做人機介面，再透過Arduino來控制機器手臂

(5) 利用Matlab做逆向運動學以控制6軸Robotic Arm

-------------------------------------------------------------------------------

台大機械碩士生 黃淵堯 蔡佳勳

關鍵字：Matlab, Arduino, RC Servo, Robotic Arm

夾取運動示範的影片:

----------------------------------------------------------------------------------------------------------

上個學期修習黃漢邦、林沛群老師的 "機器人簡介"，剛好有一個專題作業，是把課程的知識應用在機械手臂的控制程式上。為此我們向大明助教商借已經組裝完的六軸機器手臂。機器手臂的構造，在之前的文章裡面有提過，這邊就不再贅述了。

以下直接進行我們控制部分規劃的介紹。

我們使用Matlab進行正逆向運動學計算，然後將各關節的角度輸出到Arduino，由Arduino來發送脈衝訊號來驅動RC伺服馬達來旋轉到特定角度。MATLAB的部分，我們使用兩個函式庫來進行運算，Arduino IO函式庫可負責Matlab和Arduino資料傳輸的工作 (使用方式可參考: http://goo.gl/TMp9Qx)。而Robotic Toolbox可以進行機器手臂運動學的計算以及軌跡的規劃 (使用方式可參考: http://goo.gl/eTyJKR)


首先將手臂的各項參數定義至Toolbox中，我們使用Standard D-H Parameter方式進行定義運動學各項參數：

依CAD上的量測結果
L1=0.094m
L2=0.118m
L3=0.128m
L4=0.09m
使用下列Robotic Toolbox函式

L(n) = Link([θ,d,a,α]);

依次將參數輸入
然後使用

SerialLink(L, 'name', 'robot_arm');

替手臂取名，這邊以robot_arm為例。
之後只要鍵入手臂名稱robot_arm就可觀看參數。

以D-H方法建立手臂參數後就可輸入各關節角度，以進行正向運動學計算。

在此可輸入六個關節的角度

Robot_arm.fkine(θ1,θ2,θ3,θ4,θ5,θ6)

來進行正向運動學運算，傳回一4*4空間位置矩陣，也可使用

Robot_arm.plot(θ1,θ2,θ3,θ4,θ5,θ6);

看模擬手臂在空間中的位置。

對於有Closed-Form Solution的手臂，可在函式庫中直接使用以下指令求解逆向運動學

Robot_arm.ikine6s(T,’ru’)

T為空間矩陣
後方參數可選擇手臂Configuration
left or right handed 'l', 'r'
elbow up or down 'u', 'd'
wrist flipped or not flipped 'f', 'n'

輸入後回傳手臂位置的六個角度，以下為使用不同參數來求得Upper Arm跟Lower Arm的各軸角度，並模擬姿勢如下圖

另外，對於無Closed-Form Solution 之手臂型式也可支援使用數值方法求解。
完成正逆向運動學的建立，現在可以進行軌跡規劃的部份，我們將起始點及終點位置點做逆向運動學，得知各關節之角度，然後使用

Robot_arm.jtraj( T1,T2,t)

指令建立Joint Space軌跡規劃，建立的方法為將起點及終點位置進行逆向運動學計算，得各關節角度，並內插Cubic Polynomial函數，以下以兩位置做示範，可得一圓弧形軌跡如下。

因Joint Space軌跡規劃較無逆向運動學問題，故我們進行一長距離長時間軌跡規劃進行示範，給定一抓取物體至定點之動作並做軌跡規劃，可畫出時間對角度之關係圖如下：

在計算逆向運動學的時候，各角度皆使用弧度(rad)來表示，但RC Servo只能讀取角度0~180間的整數值，並且RC Servo裝配的位置也和模型建立有所差異，故經由實驗建立Matlab運算和實際RC Servo間的相位關係：

qs=qm;

qs(:,1)=(pi/2)-qm(:,1);% RC servo上theta1為90度-Matlab上theta1之值

qs(:,2)=pi-qm(:,2);

qs(:,3)=qm(:,3);

qs(:,4)=(pi/2)-qm(:,4);

qs(:,5)=qm(:,5)+pi/2;

qs(:,6)=(pi/2)-qm(:,6);

qs=round(qs)%取整數

經由轉換之後可得：

有了軌跡之後，
我們可以使用Arduino IO函式庫來建立Matlab及Arduino間的連結，使用

a=arduino('COM5')

輸入Arduino目前與電腦連接的腳位後可進行連接，之後可以使用

a.      servoWrite(1,45)

來輸入馬達目標角度給Arduino，這邊以第一腳位，Servo轉角45度為例。將以上軌跡規劃，經過Arduino IO函式庫傳入Arduino後輸入RC servo，可完成規畫的運動。