使用matlab建立機器人雅克比矩陣
阿新 • • 發佈：2019-01-12
雅克比矩陣是聯絡末端操作空間速度與空間關節速度的樞扭,推導過程如下：
雅克比矩陣為m*n矩陣，其中m表示末端操作空間的自由度，一般為6個（即 x y z Wx Wy Wz）,n為關節空間的關節數，本例中為6旋轉關節機器人，史陶比爾TX90
雅克比矩陣中各個元素的推導如圖
通過編寫程式碼和機器人工具箱求解對比，發現結果一致，執行結果如下：
>> >> [J,T] = TX90_jacobian([0 0 0 0 0 0 ])//機器人工具箱的解
J =
-50.0000 950.0000 525.0000 0 100.0000 0
50.0000 0.0000 0 0 0 0
0 -0.0000 0 0 0 0
0 0 0 0 0 0
0 1.0000 1.0000 0 1.0000 0
1.0000 0.0000 0.0000 1.0000 0.0000 1.0000
T =
1.0000 0 0 50.0000
0 1.0000 0 50.0000
0 0 1.0000 950.0000
0 0 0 1.0000
>> >> [ J ] = ykb( [0 0 0 0 0 0 ] )//自己寫的求雅克比函式
J =
[ -50.0, 950.0, 525.0, 0, 100.0, 0]
[ 50.0, 0, 0, 0, 0, 0]
[ 0, 0, 0, 0, 0, 0]
[ 0, 0, 0, 0, 0, 0]
[ 0, 1.0, 1.0, 0, 1.0, 0]
[ 1.0, 6.123234e-17, 6.123234e-17, 1.0, 6.123234e-17, 1.0]
>>>> [ T06 ] = tx90_ForwardKinematics( [0 0 0 0 0 0] )
T06 =
1 0 0 50
0 1 0 50
0 0 1 950
0 0 0 1
% 以上程式碼表明該雅克比矩陣建立方法與機器人工具箱函式算出的結果一致，
執行速度方面，機器人工具箱函式更快
以下為程式碼：
% 機器人工具箱的函式
function [J,T] = TX90_jacobian(theta)
%UNTITLED 此處顯示有關此函式的摘要
% 此處顯示詳細說明
%syms q1 q2 q3 q4 q5 q6
q1 = theta(1)/180*pi;
q2 = theta(2)/180*pi-pi/2;
q3 = theta(3)/180*pi+pi/2;
q4 = theta(4)/180*pi;
q5 = theta(5)/180*pi;
q6 = theta(6)/180*pi;
% theta d a alpha
L(1) = Link([q1, 0, 50, -pi/2]);
L(2) = Link([q2, 0, 425, 0]);
L(3) = Link([q3, 50, 0, pi/2]);
L(4) = Link([q4, 425, 0, -pi/2]);
L(5) = Link([q5, 0, 0, pi/2]);
L(6) = Link([q6, 100 ,0, 0]);
tx90 = SerialLink(L, 'name', '史陶比爾TX90');
J = tx90.jacob0([q1 q2 q3 q4 q5 q6]);
T = tx90.fkine([q1 q2 q3 q4 q5 q6]);
end
%自寫函式
function [ T ] = Trans( alpha, a, theta, d ) % 變換矩陣
T =[
cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);
sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
0, sin(alpha), cos(alpha), d;
0, 0, 0, 1];
end
function [ JT ] = test( theta )
%UNTITLED 此處顯示有關此函式的摘要
% 此處顯示詳細說明
T01 =Trans( -pi/2, 50, theta(1), 0 );%[alpha a theta d ] 長度統一為mm單位，角度統一為度單位
T12 =Trans( 0, 425, theta(2)-pi/2, 0 );
T23 =Trans( pi/2, 0, theta(3)+pi/2, 50 );
T34 =Trans( -pi/2, 0, theta(4), 425 );
T45 =Trans( pi/2, 0, theta(5), 0 );
T56 =Trans( 0, 0, theta(6), 100 );
T02 = T01*T12;
T03 = T01*T12*T23;
T04 = T01*T12*T23*T34;
T05 =T01*T12*T23*T34*T45;
T06=T01*T12*T23*T34*T45*T56;
T00 = [1 0 0 0;0 1 0 0; 0 0 1 0; 0 0 0 1]; %因為這是標準dh引數建立的轉換，設工具座標系與T06重合，與教程中改進的DH引數確定角速度方法不同
ox = T06(1,4); oy = T06(2,4);oz = T06(3,4);
w1 = T00(1:3,3);w2 = T01(1:3,3);w3 = T02(1:3,3);w4 = T03(1:3,3);w5 = T04(1:3,3);w6 = T05(1:3,3);
%Jw = [w1,w2,w3,w4,w5,w6];
J11 = diff(ox,theta(1));J12 = diff(ox,theta(2));J13 = diff(ox,theta(3));J14 = diff(ox,theta(4));J15 = diff(ox,theta(5));J16 = diff(ox,theta(6));
J21 = diff(oy,theta(1));J22 = diff(oy,theta(2));J23 = diff(oy,theta(3));J24 = diff(oy,theta(4));J25 = diff(oy,theta(5));J26 = diff(oy,theta(6));
J31 = diff(oz,theta(1));J32 = diff(oz,theta(2));J33 = diff(oz,theta(3));J34 = diff(oz,theta(4));J35 = diff(oz,theta(5));J36 = diff(oz,theta(6));
JT = [J11,J12,J13,J14,J15,J16;
J21,J22,J23,J24,J25,J26;
J31,J32,J33,J34,J35,J36;
w1, w2, w3, w4, w5, w6];
end
function [ J ] = ykb( jiao )
%UNTITLED 此處顯示有關此函式的摘要
% 此處顯示詳細說明
syms a1 a2 a3 a4 a5 a6;
theta =[a1 a2 a3 a4 a5 a6];
JT =test( theta );
q = jiao*pi/180;
JT1=subs(JT,a1,q(1));JT2=subs(JT1,a2,q(2));JT3=subs(JT2,a3,q(3));JT4=subs(JT3,a4,q(4));JT5=subs(JT4,a5,q(5));J=subs(JT5,a6,q(6));
old = digits;
digits(4)
J = vpa(J,8);
end
function [ T06 ] = tx90_ForwardKinematics( theta )
if nargin<2; end
d6=100;
T1 =Trans( -90, 50, theta(1), 0 );%[alpha a theta d ]
T2 =Trans( 0, 425, theta(2)-90, 0 );
T3 =Trans( 90, 0, theta(3)+90, 50 );
T4 =Trans( -90, 0, theta(4), 425 );
T5 =Trans( 90, 0, theta(5), 0 );
T6 =Trans( 0, 0, theta(6), d6 );
T06=T1*T2*T3*T4*T5*T6;
end
function [ T ] = Trans( alpha, a, theta, d )
T =[
cosd(theta), -sind(theta)*cosd(alpha), sind(theta)*sind(alpha), a*cosd(theta);
sind(theta), cosd(theta)*cosd(alpha), -cosd(theta)*sind(alpha), a*sind(theta);
0, sind(alpha), cosd(alpha), d;
0, 0, 0, 1];
end
總結：
通過編寫機器人雅克比矩陣函式，對matlab的掌握更進一步，使用函式時，必須在資料夾裡開啟，否則容易報錯，掌握如何對符號函式進行化簡，求偏導最後再代值運算，比較實用。
參考教程：http://mp.weixin.qq.com/s?__biz=MzI1MTA3MjA2Nw==&mid=400013659&idx=1&sn=68abc24fff30e4a16601316a0fe91a46&chksm=7bdb82774cac0b61e3f6cbfd3bfa92973e9c033c816912b5c7439f880254a8571f66897e9229&mpshare=1&scene=23&srcid=0824XXjKUPzVQ1UjlVkokGjl#rd