机械臂雅可比矩阵的矢量积理解和matlab实现

雅可比矩阵的第 J i J_i Ji列:
J i = [ v j ω j ] J_i=\left[\begin{array}{c} \boldsymbol{ v}_{j} \\ \boldsymbol{ \omega}_{j} \end{array}\right] Ji=[vjωj]

关于一些基本概念可以参考博客,部分细节如下:

  • 每个移动关节, J i J_i Ji计算如下:

    J i = [ Z i 0 ] J_i=\left[\begin{array}{c} \boldsymbol{ Z}_{i} \\ 0 \end{array}\right] Ji=[Zi0]
    式中, Z j Z_j Zj 0 i T _0^iT 0iT第3列的前3行。

  • 每个旋转关节, J i J_i Ji计算如下:
    J i = [ Z i × 0 i R i n P Z i ] J_i=\left[\begin{array}{c} \boldsymbol{ Z}_{i}×_0^iR _i^nP \\ \boldsymbol{ Z}_{i} \end{array}\right] Ji=[Zi×0iRinPZi]
    式中, 0 i R _0^iR 0iR 0 i T _0^iT 0iT对应的旋转矩阵, i n P _i^nP inP i n T _i^nT inT对应的位移矩阵, i n T = i i + 1 T i + 1 i + 2 T . . . n − 1 n T _i^nT=_i^{i+1}T_{i+1}^{i+2}T..._{n-1}^nT inT=ii+1Ti+1i+2T...n1nT

SDH代码程序参考博客,如下:

function [ J ] = Jacob_cross_SDH( q )
%JACOB_CROSS_SDH 函数摘要
%   输入q0为逼近角,单位为弧度,矩阵大小1*6;
%   输出J为速度雅各比矩阵,矩阵大小6*6;
%   说明:利用向量积的方法求解系统的雅各比矩阵,方法1和方法2任选一种
%   说明:此求解方法基于SDH参数建模,若MDH方法建模,需进行一定的下标改动

d=[105.03,0,0,75.66,80.09,44.36];
a=[0,-174.42,-174.42,0,0,0];
alp=[pi/2,0,0,pi/2,-pi/2,0];
offset=[0,-pi/2,0,-pi/2,0,0];
thd=q+offset;

% 求各个关节间的变换矩阵
T0=trotz(0)*transl(0,0,0)*trotx(0)*transl(0,0,0);
T1=trotz(thd(1))*transl(0,0,d(1))*trotx(alp(1))*transl(a(1),0,0);
T2=trotz(thd(2))*transl(0,0,d(2))*trotx(alp(2))*transl(a(2),0,0);
T3=trotz(thd(3))*transl(0,0,d(3))*trotx(alp(3))*transl(a(3),0,0);
T4=trotz(thd(4))*transl(0,0,d(4))*trotx(alp(4))*transl(a(4),0,0);
T5=trotz(thd(5))*transl(0,0,d(5))*trotx(alp(5))*transl(a(5),0,0);
T6=trotz(thd(6))*transl(0,0,d(6))*trotx(alp(6))*transl(a(6),0,0);

% 求各个关节相对于惯性坐标系的变换矩阵
T00 = T0;
T01 = T1;
T02 = T1*T2;
T03 = T1*T2*T3;
T04 = T1*T2*T3*T4;
T05 = T1*T2*T3*T4*T5;
T06 = T1*T2*T3*T4*T5*T6;

% 求各个关节相对于末端坐标系的变换矩阵
T06 = T1*T2*T3*T4*T5*T6;
T16 = T2*T3*T4*T5*T6;
T26 = T3*T4*T5*T6;
T36 = T4*T5*T6;
T46 = T5*T6;
T56 = T6;

% 提取各变换矩阵的旋转矩阵
R00 = t2r(T00);
R01 = t2r(T01);
R02 = t2r(T02);
R03 = t2r(T03);
R04 = t2r(T04);
R05 = t2r(T05);
R06 = t2r(T06);

% 取旋转矩阵第3列,即Z轴方向分量
Z0 = R00(: , 3);
Z1 = R01(: , 3);
Z2 = R02(: , 3);
Z3 = R03(: , 3);
Z4 = R04(: , 3);
Z5 = R05(: , 3);
Z6 = R06(: , 3);

%% Method.1
% 求末端关节坐标系相对于前面各个坐标系的位置,即齐次变换矩阵的第四列
% pi6为坐标系i和末端坐标系的相对位置在坐标系i下的表示
P06 = T06(1:3, 4);
P16 = T16(1:3, 4);
P26 = T26(1:3, 4);
P36 = T36(1:3, 4);
P46 = T46(1:3, 4);
P56 = T56(1:3, 4);
P66 = [0; 0; 0];

% 使用向量积求出雅可比矩阵
% R0i为坐标系0到坐标系i的旋转矩阵
% R0i*Pi6指坐标系i和末端坐标系的相对位置在0坐标系下的表示
J1 = [cross(Z0, R00*P06); Z0];
J2 = [cross(Z1, R01*P16); Z1];
J3 = [cross(Z2, R02*P26); Z2];
J4 = [cross(Z3, R03*P36); Z3];
J5 = [cross(Z4, R04*P46); Z4];
J6 = [cross(Z5, R05*P56); Z5];

%% Method.2

% % pi为坐标系i与世界坐标系0的相对位置
% p0=transl(T00);
% p1=transl(T01);
% p2=transl(T02);
% p3=transl(T03);
% p4=transl(T04);
% p5=transl(T05);
% p6=transl(T06);
% 
% % p6-pi为i坐标系指向末端坐标系的向量
% % p6-pi即为末端坐标系与i坐标系相对位置在世界坐标系中的表示
% % Ji=[Jv;Jw]    对应六自由度的速度分量和旋转分量
% J1 = [cross(Z0, p6-p0); Z0];
% J2 = [cross(Z1, p6-p1); Z1];
% J3 = [cross(Z2, p6-p2); Z2];
% J4 = [cross(Z3, p6-p3); Z3];
% J5 = [cross(Z4, p6-p4); Z4];
% J6 = [cross(Z5, p6-p5); Z5];


J = [J1, J2, J3, J4, J5, J6];

end

  • 关于平移矩阵的理解,只有平移没有旋转时,旋转矩阵为单位矩阵,如果全零会造成矩阵相乘的结果错误。这一点可以通过旋转与平移全零矩阵运算得出。
T0=trotz(0)*transl(0,0,0)*trotx(0)*transl(0,0,0);

计算结果如下:

T0 =

     1     0     0     0
     0     1     0     0
     0     0     1     0
     0     0     0     1
  • 11
    点赞
  • 19
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值