2014年2月22日 星期六

利用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 servotheta190-Matlabtheta1之值

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,可完成規畫的運動。


11 則留言:

  1. 我看了你第一篇 就跑去print一隻來玩 ,接完arduino 和matlab 才發現你後面還有這幾篇,可以請教你matlab 怎麼控是伺馬達的嗎? 我的信箱
    henryyou2@gmail.com

    回覆刪除
  2. 大大你好,我對於運動學多軸機器人的matlab蠻有興趣的!用DH矩陣做轉換以及後續的量測角度以及匯出!
    小弟功力不強~可以請大大求解或是討論嗎?
    我的信箱lonelywind21@gmai.com

    回覆刪除
  3. 大大你好,對matlab for arduino有興趣,不知是否可以跟您討論程式的寫法與參考想知道程式方面是怎麼去寫的,我的信箱dd7er99@gmail.com

    回覆刪除
  4. 謝謝您的寶貴分享,我最近一直研究matlab for arduino來控制機械手臂,目前已經有機械手臂了,不過控制方面一直沒有突破,因為本人並沒有學過matlab,摸了很久還是不太懂,想跟您討論,我的信箱A1101001d@gmail.com

    回覆刪除
  5. 有關matlab的部分,可以直接寫信給黃淵堯先生,此專案matlab部分由他執行,他可以提出比我還專業的建議。email如下:r02522636@ntu.edu.tw

    回覆刪除
  6. 您好~我對matlab for arduino與Robotic tool box方面有興趣,不知是否可以跟您討論參考程式撰寫的方法,我的信箱n700ck101@gmail.com

    回覆刪除
  7. 你好想請教一些有關Robotic Toolbox的東西,
    你在表上Link2 thera 為"q-pi/2"但Matlab顯示卻只有q請問你是怎去修改thera參數,最近剛好也遇到相同問題懇請幫忙答覆

    回覆刪除
  8. 寫太好了 感謝 煩請指教
    hanstage582@hotmail.com.tw

    回覆刪除
  9. hi,您好,我也是對手臂超有興趣的,可以請教您一些問題嗎 ,感謝

    我的信箱:xyzflook@yahoo.com.tw

    回覆刪除
  10. 您好 最近做專題有再涉略機械手臂的部分,想請教您說這個toolbox能否算五軸的機械手臂。
    信箱:construct102@gmail.com

    回覆刪除
  11. 您好 最近在研究機械手臂的角度模擬,想請教一些問題
    信箱:carry0937830108@gmail.com

    回覆刪除