dodo的部落格

機械手臂是未來無人化及智慧化工廠，及商店等不可或缺的一環，這篇文章就是要來談談，如何合使用matlab結合robotDK來控制機械手臂。

要操作機械手臂讓他能夠精確地到達目標地完成任務，不識一件容易的事，尤其當我們需要它進行極度精密的操作，像是將IC上細小的針腳接上電路板上、替鞋板的邊緣上膠，又或者是更複雜的多個手臂協作而不衝突。

這時我們需要線性代數這數學工具去規劃機械手臂的路徑(註:詳細關於用線性代數規畫路徑的方法會在另一篇說明，這裡只提這次實驗會用到的部分)

路徑規劃(目標)

1. 首先tool1機械手臂頂點的位置會下降到home點
2. Tool2(原本在home點)會順勢到Target1
3. 然後在tool1維持在home點的情況下tool2上半沿圓周移到Target2然後回到home
4. 再來到Target1再繞下半圓到Target2
5. 最後回home點完成動作

實驗步驟

首先在做這個專題的路徑規劃是使用那兩個tools點的xyz座標去規劃，對於tool2的圓周運動動，此處使用了極座標系統，所以x=Rsin(fi(t))、y=Rcos(fi(t))這是因為這裡的座標x軸是正向向下、而y軸正向是向左

再來計算速度分別將xy微分，記為dx = 150*cos(fi(t))、

dy = -150*sin(fi(t))

之後開始計算[速度(dxdydz);角速度(wx,wy,wz)]=J*q' 時，對應tool1和tool2的H6P矩陣，分別取用J陣列的前三raw。

再來J矩陣是線性相依的，不能取反矩陣，所以將tool1的dz速度拿掉因為圓周運動是發生在xy平面所以用不到z軸，由此算出來的dq為5*1向量，為了符合之後矩陣運算的格式，在後面補0，變成6*1向量。

最後用ode積分dq算出q也就是每個關節的角度。

把這些角度寫成script檔送給roboDK。

Matlab編程

按照上述步驟，會寫出以下演算法

%%

% Generate a Robolink object RDK. This object interfaces with RoboDK.

RDK = Robolink;

%%

% Get the library path

path = RDK.getParam('PATH_LIBRARY');

%%

% Display a list of all items

RDK.ItemList()

%%

% current project tree

robot = RDK.Item('UR5');

fprintf('Robot selected:\t%s\n', robot.Name());

robot.setVisible(1);

%%

% robot frame, hide it

Br = robot.Parent();

Br.setVisible(0);

Jrest = [135 -90 90 -90 -90 -90]';

robot.MoveJ(Jrest);

%%

% define a new rotated frame to be base frame

B0 = RDK.AddFrame('B0 frame',Br);

Hr0 = transl(0,0,-10)*rotz(-pi/4)*rotx(pi);%rotate誰?

B0.setPose(Hr0);

%%

% from now on, all motions are taken with respect to B0

robot.setFrame(B0);%顯示B0座標

%%

% Set tool point to 200m from robot frange in z (down) direction

global H6p1

H6p1 = transl(0,0,50);

%robot.setHtool(H6p1);%建立tool

%%

% Set tool point to 200m from robot frange in z (down) direction

global H6p2

H6p2 = transl(0,0,300);

robot.setHtool(H6p2);%建立tool

%%

home = RDK.AddTarget('home', B0);

home.setAsCartesianTarget();

htarrget =transl(300,0,-200);

home.setPose(htarrget);

%%

target1 = RDK.AddTarget('Target 1', B0);

target1.setAsCartesianTarget();

t1_pose = transl(300,150,0)*rotx(-pi/4.8649);%-pi/4.8649);

target1.setPose(t1_pose);

%%

target2 = RDK.AddTarget('Target 2', B0);

target2.setAsCartesianTarget();

t2_pose = transl(300,-150,0)*rotx(pi/4.8649);

target2.setPose(t2_pose);

%%

robot.MoveJ(target1);

q0 = robot.Joints*pi/180;

%X=[0;0;0;pi;0;0];

tspan = [0: 0.05 :5];

fi = linspace(0,5,100);

[TOUTY, YOUTY] = ode23(@invURK_fn, tspan, q0);

%%

%%

%儲存路徑(分上半圓與下半圓)

    fid = fopen('myfunfn1.script','w');

    fprintf(fid,'def myfunfn1():\n');

    for i=1:length(TOUTY)

        fprintf(fid,'\tmovej([% 8.6f, % 8.6f, % 8.6f, % 8.6f, % 8.6f,%8.6f],accel_radss,speed_ms,%4.2f,blend_radius_m)\n',YOUTY(i,:),0.05);

    end

    fprintf(fid,'end\n\nmyfunfn1()');

    fclose(fid);

%%

[TOUTY, YOUTY] = ode23(@invURK_fn1, tspan, q0);

fid = fopen('myfunfn2.script','w');

    fprintf(fid,'def myfunfn2():\n');

    for i=1:length(TOUTY)

        fprintf(fid,'\tmovej([% 8.6f, % 8.6f, % 8.6f, % 8.6f, % 8.6f,%8.6f],accel_radss,speed_ms,%4.2f,blend_radius_m)\n',YOUTY(i,:),0.05);

    end

    fprintf(fid,'end\n\nmyfunfn2()');

    fclose(fid);

%規劃路徑的副程式

%%

function [dq, dx, dy] = invURK_fn(fi, q)

global H6p1 H6p2;

    dx = 150*cos(pi/5*fi)*pi/5;

    dy = -150*sin(pi/5*fi)*pi/5;

    J1= JacUR5fn(q,H6p1);

    J2 = JacUR5fn(q,H6p2);

    J=[J1(1,[1:5]);J1(2,[1:5]);J2(1,[1:5]);J2(2,[1:5]);J2(3,[1:5])];

    dq = inv(J)*[0;0;dx;dy;0];

    dq = [dq;0];

end

看完上面那些算是演算法後 ， 會不會有種感覺 ， 當我們在類似於創客空間或以創客為主題所做出的那些娛樂用機械手臂 ， 真的就只是入門而已 ， 當你真的要進入機械手臂這一領域之後，會發現這一領域比想像的複雜得多，除了增加效率降低最短路徑規劃外，還要去考慮不同情況應要用幾軸的手臂，考慮機械手臂的負重對他的影像以及現實中會不會有其他干擾需要繞過等等 ，所以除了程式碼演算法法那些表面東西外 ，真正重要的是它背後的數學原理 ，那才能解決上述那些問題使機械手臂推廣到更多應用，真正做到把機械手臂用到極致 。

有問題或有錯的地方可以留言喔