关于机械臂逆运动学 jacobian矩阵

机器人逆运动学 

机器人的逆解问题比较复杂，为了说明问题，下面先以2自由度的机器人为例。

如图所示，已知机器人末端的坐标值（x,y） ，试利用x,y  表示    

根据图中的几何关系可知： 

联立求解上述两方程，可分别求出           的表达式。

因此可进一步得到：

从机器人的手爪末端位置姿态出发，可以求出机器人对应的各关节的角度。该例的机器人是属于平面多关节机器人，对于一般的机械手来讲，其求解过程比较复杂，往往其解不是唯一的。

雅可比矩阵的定义 
前面讨论了机器人的指尖位置和方向与各关节的变化位置之间的关系。在本节将进一步讨论指尖的速度与各关节的速度（转动或平移）之间的关系。
考虑机械手的手爪位置    和关节变量     的关系用正运动学方程表示如下：

面以2自由度机械手为例，如前面图3－2所示的2自由度机械手的雅可比矩阵。前面已推导过，该机器人的指尖位置可以表示为

则与这个机器人的平移速度相关的雅可比矩阵，可以下列形式给出：

现在，我们来讨论一下     的各列向量的几何学意义，即在                 时，考虑       ，   的几何学意义。根据式（3－63），   是在时         ，也就是第2关节固定时，仅在第1关节转动的情况下，指尖平移速度在基准坐标系上表示出的向量。
同样，     是第1关节固定时，仅在第2关节转动的情况下，指尖平移速度在基准坐标系上表示出的向量。  

代码如下：

close all;
clear all;

%机械臂末端规划沿z轴上升螺旋
R=2;%螺旋半径
V=4;%上升速度

%初始位置
 x0 = 3;
 y0 =4;
 z0= 9;



%二自由度机械臂 两臂长度
L1=8;
L2=13.5;
L3=16;

%几何法求解初始角度
seta1=atan(y0/x0);
a=acos((L2*L2+L3*L3-(x0/cos(seta1))^2-(z0-L1)*(z0-L1))/(2*L2*L3));
seta3=pi-a;
seta2=atan((z0-L1)/(x0/cos(seta1)))-atan(L3*sin(seta3)/(L2+L3*cos(seta3)));

%上一位置
x1=x0;
y1=y0;
z1=z0;
i=1;
for t=0.01:0.01:1
    %末端运动方程
    x =3+3*t;
	y =4+4*t;
    z =9+2*t;
    
    %坐标变换
    delta_x=x-x1;
    delta_y=y-y1;
    delta_z=z-z1;
    
    %更新数据
    x1=x;
    y1=y;
    z1=z;
    
    %jacobian
    J=[ -sin(seta1)*(L3*cos(seta2 + seta3) + L2*cos(seta2)), -cos(seta1)*(L3*sin(seta2 + seta3) + L2*sin(seta2)), -L3*sin(seta2 + seta3)*cos(seta1);
         cos(seta1)*(L3*cos(seta2 + seta3) + L2*cos(seta2)), -sin(seta1)*(L3*sin(seta2 + seta3) + L2*sin(seta2)), -L3*sin(seta2 + seta3)*sin(seta1);
                                                          0,               L3*cos(seta2 + seta3) + L2*cos(seta2),             L3*cos(seta2 + seta3)];
    q=(J^-1)*[delta_x;delta_y;delta_z];

    %角度变换
    delta_j1(i)=q(1,1);
    delta_j2(i)=q(2,1);
    delta_j3(i)=q(3,1);
    
    
    seta1=atan(y/x);
    a=acos((L2*L2+L3*L3-(x/cos(seta1))^2-(z-L1)*(z-L1))/(2*L2*L3));
    seta3=pi-a;
    seta2=atan((z-L1)/(x/cos(seta1)))-atan(L3*sin(seta3)/(L2+L3*cos(seta3)));
    
    %弧度检验
    seta_1(i)=seta1;
    seta_2(i)=seta2;
    seta_3(i)=seta3;
    i=i+1;
end



iii = 0:0.01:1;
%seta1角度
figure(1);
plot(iii(1:100),delta_j1(1:100),'k');
hold on
set(gca,'ylim',[-1,1]);
xlabel('t / s'),ylabel('delta_seta1 / rad');

%seta2角度
figure(2);
plot(iii(1:100),delta_j2(1:100),'k');
hold on
set(gca,'ylim',[-1,1]);
xlabel('t / s'),ylabel('delta_seta2 / rad');

%seta3角度
figure(3);
plot(iii(1:100),delta_j3(1:100),'k');
hold on
set(gca,'ylim',[-1,1]);
xlabel('t / s'),ylabel('delta_seta3 / rad');

%seta1角度
figure(4);
plot(iii(1:100),seta_1(1:100),'k');
hold on
set(gca,'ylim',[-2*pi,2*pi]);
xlabel('t / s'),ylabel('seta1 / rad');

%seta2角度
figure(5);
plot(iii(1:100),seta_2(1:100),'k');
hold on
set(gca,'ylim',[-2*pi,2*pi]);
xlabel('t / s'),ylabel('seta2 / rad');

%seta3角度
figure(6);
plot(iii(1:100),seta_3(1:100),'k');
hold on
set(gca,'ylim',[-2*pi,2*pi]);
xlabel('t / s'),ylabel('seta3 / rad');