使用matlab建立机器人雅克比矩阵
程序员文章站
2022-05-21 09:30:23
...
雅克比矩阵是联系末端操作空间速度与空间关节速度的枢扭,推导过程如下：
雅克比矩阵为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
%自己的正运动学变换矩阵和雅克比矩阵与机器人工具箱求解的一致
%工具箱输入角度为[90 45 0 90 0 45]的求解结果
>> [J,T] = TX90_jacobian([90 45 0 90 0 45])
J =
-721.7514 0 0 0 -100.0000 0
-50.0000 671.7514 371.2311 -0.0000 0 0
-0.0000 -671.7514 -371.2311 0.0000 -0.0000 0
0.0000 -1.0000 -1.0000 0.0000 -0.0000 0.0000
-0.0000 0.0000 0.0000 0.7071 -0.7071 0.7071
1.0000 -0.0000 -0.0000 0.7071 0.7071 0.7071
T =
-0.7071 0.7071 0.0000 -50.0000
-0.5000 -0.5000 0.7071 721.7514
0.5000 0.5000 0.7071 671.7514
0 0 0 1.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.7071 0.7071 0 -50.0000
-0.5000 -0.5000 0.7071 721.7514
0.5000 0.5000 0.7071 671.7514
0 0 0 1.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;
% 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
上一篇： MATLAB简单机器人视觉控制（仿真2）
下一篇： MATLAB机器人可视化