机械|使用matlab建立机器人雅克比矩阵机械|matlab

雅克比矩阵是联系末端操作空间速度与空间关节速度的枢扭,推导过程如下：

文章图片

雅克比矩阵为m*n矩阵，其中m表示末端操作空间的自由度，一般为6个（即 x y z Wx Wy Wz）,n为关节空间的关节数，本例中为6旋转关节机器人，史陶比尔TX90
雅克比矩阵中各个元素的推导如图

文章图片

通过编写代码和机器人工具箱求解对比，发现结果一致，运行结果如下：

>> >> [J,T] = TX90_jacobian([0 0 0 0 00 ])//机器人工具箱的解J =-50.0000950.0000525.00000100.00000 50.00000.00000000 0-0.00000000 000000 01.00001.000001.00000 1.00000.00000.00001.00000.00001.0000T =1.00000050.0000 01.0000050.0000 001.0000950.0000 0001.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 =10050 01050 001950 0001 %自己的正运动学变换矩阵和雅克比矩阵与机器人工具箱求解的一致 %工具箱输入角度为[90 45 0 90 0 45]的求解结果 >> [J,T] = TX90_jacobian([90 45 0 90 0 45])J = -721.7514000 -100.00000 -50.0000671.7514371.2311-0.000000 -0.0000 -671.7514 -371.23110.0000-0.00000 0.0000-1.0000-1.00000.0000-0.00000.0000 -0.00000.00000.00000.7071-0.70710.7071 1.0000-0.0000-0.00000.70710.70710.7071T =-0.70710.70710.0000-50.0000 -0.5000-0.50000.7071721.7514 0.50000.50000.7071671.7514 0001.0000 % 自己创建的函数输入角度为[90 45 0 90 0 45]的求解结果 >> [ J ] = ykb( [90 45 0 90 0 45] )J =[ -721.75144, -4.1132913e-14, -2.2731346e-14,0,-100.0,0] [-50.0,671.75144,371.23106,0, -4.3297803e-15,0] [0,-671.75144,-371.23106,0,1.7934537e-15,0] [0,-1.0,-1.0, -1.7934537e-17,4.3297803e-17, -1.7934537e-17] [0,0,0,0.70710678,-0.70710678,0.70710678] [1.0,6.123234e-17,6.123234e-17,0.70710678,0.70710678,0.70710678]> [ T06] = tx90_ForwardKinematics( [90 45 0 90 0 45] )T06 =-0.70710.70710-50.0000 -0.5000-0.50000.7071721.7514 0.50000.50000.7071671.7514 0001.0000

以上代码表明该雅克比矩阵建立方法与机器人工具箱函数算出的结果一致，
运行速度方面，机器人工具箱函数更快
以下为代码：

% 机器人工具箱的函数 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; %thetadaalpha 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]; endfunction [ 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; 00 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]; endfunction [ 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); endfunction [ 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; endfunction [ 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建立机器人雅克比矩阵】总结：
通过编写机器人雅克比矩阵函数，对matlab的掌握更进一步，使用函数时，必须在文件夹里打开，否则容易报错，掌握如何对符号函数进行化简，求偏导最后再代值运算，比较实用。
参考教程：http://mp.weixin.qq.com/s?__biz=MzI1MTA3MjA2Nw==&mid=400013659&idx=1&sn=68abc24fff30e4a16601316a0fe91a46&chksm=7bdb82774cac0b61e3f6cbfd3bfa92973e9c033c816912b5c7439f880254a8571f66897e9229&mpshare=1&scene=23&srcid=0824XXjKUPzVQ1UjlVkokGjl#rd