• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

机械臂通过雅可比矩阵实现正运动学及逆运动学迭代解(工具:matlab)

人工智能 搁浅 1131次浏览 0个评论

1.利用工具箱建模,供验证用

选用二轴平面机械臂,两个关节角分别是theta1和theta2. view(3); Lnk1 = Link([ 0 0 100 0 0]);%theta,d,a,alpha Lnk2 = Link([0 0 200 0 0]); Robot = SerialLink([Lnk1 Lnk2]); Robot.teach(); 运行结果如图:  
机械臂通过雅可比矩阵实现正运动学及逆运动学迭代解(工具:matlab)

2.使用DH法求正运动学

此处是传统正运动学的求法,利用变化矩阵相乘,得到T02,即可根据两个转角,求得机械臂末端位姿。  

syms theta1 alpha1 a1 d1 theta2 alpha2 a2 d2;
A1=[cos(theta1),-sin(theta1)*cos(alpha1),sin(theta1)*sin(alpha1),a1*cos(theta1);...
sin(theta1),cos(theta1)*cos(alpha1),-cos(theta1)*sin(alpha1),a1*sin(theta1);...
0,sin(alpha1),cos(alpha1),d1;...
0,0,0,1];
A2=[cos(theta2),-sin(theta2)*cos(alpha2),sin(theta2)*sin(alpha2),a2*cos(theta2);...
sin(theta2),cos(theta2)*cos(alpha2),-cos(theta2)*sin(alpha2),a2*sin(theta2);...
0,sin(alpha2),cos(alpha2),d2;...
0,0,0,1];
a1=sym(100);
a2=sym(200);
d1=sym(0);
d2=sym(0);
alpha1=sym(0);
alpha2=sym(0);
T01 = (simplify(eval(A1)));
T12 = (simplify(eval(A2)));
T02 = ((simplify(eval(A1*A2))));

  结果如图
机械臂通过雅可比矩阵实现正运动学及逆运动学迭代解(工具:matlab) T02即为最后所得的矩阵。后面利用雅可比矩阵求真逆运动学都是使用此处的结果来验证。

3.求雅可比正逆矩阵

雅可比矩阵的具体定义请大家参考网上,此处只给出具体实现 雅可比真逆运动学矩阵如下:  

J = [diff(T02(1,4),theta1),diff(T02(1,4),theta2);diff(T02(2,4),theta1),diff(T02(2,4),theta2)];%雅可比矩阵,diff表示偏导数
nJ = inv(J);%逆雅可比矩阵

  结果如下图:
机械臂通过雅可比矩阵实现正运动学及逆运动学迭代解(工具:matlab)
机械臂通过雅可比矩阵实现正运动学及逆运动学迭代解(工具:matlab)

4.利用雅可比矩阵求正运动学

稍微介绍下原理:假如要求theta1旋转3.14rad,theta2旋转1.57rad时候的机械臂末端位姿,那么,我们将这两个角度314等分,每次轴1转0.01rad,轴2转0.005rad,通过累加,得到机械臂的末端位姿。程序如下:  

%%%正运动学
x=0;
y=0;
AAA = [300;0];%机械臂初始位置
for i = 1:314
     TTT = [- 200*sin(x + y) - 100*sin(x), -200*sin(x + y);200*cos(x + y) + 100*cos(x),  200*cos(x + y)];
     AAA =AAA + TTT*[0.01;0.005];%位置累加
     x = x + 0.01;%
     y = y + 0.005;
end
AAA; %机械臂最后位置

  AAA就是最后的机械臂所在位置

5.雅可比逆矩阵求逆运动学(迭代解法)

此处就是上述正运动学推导的反演,但有个需要注意的问题是,此处的变量是末端速度,这个速度需要随着机械臂当前位姿的实时反馈而改变,而正运动学的变量是轴的速度,这个是可控的,可以不考虑他的误差。但逆运动学这恶一定要用实时反馈的数据做成闭环。  

%%%逆运动学
x=roundn(0.00001,-15);%0的话是奇异位置
y=roundn(0.00001,-15);
qqq = [x;y];%机械臂关节初始位置
200*cos(qqq(1) + qqq(1)) + 100*cos(qqq(1))
200*sin(qqq(1) + qqq(2)) + 100*sin(qqq(1))
pos = [280, 20];%目标位置
posn = [200*cos(qqq(1) + qqq(1)) + 100*cos(qqq(1)),200*sin(qqq(1) + qqq(2)) + 100*sin(qqq(1))];
flag = 0;
while flag == 0
   TTT = [(-cos(x + y)/(100*(cos(x + y)*sin(x) - sin(x + y)*cos(x)))),(-sin(x + y)/(100*(cos(x + y)*sin(x) - sin(x + y)*cos(x))));((2*cos(x + y) + cos(x))/(200*(cos(x + y)*sin(x) - sin(x + y)*cos(x)))), ((2*sin(x + y) + sin(x))/(200*(cos(x + y)*sin(x) - sin(x + y)*cos(x))))];
   TTT
   qqq = qqq + TTT * [(pos(1)-posn(1))/10000;(pos(2)-posn(2))/10000];
   posn = [200*cos(qqq(1) + qqq(1)) + 100*cos(qqq(1)),200*sin(qqq(1) + qqq(2)) + 100*sin(qqq(1))];
   qqq
   x = qqq(1);
   y = qqq(2);
   200*cos(qqq(1) + qqq(1)) + 100*cos(qqq(1))
   200*sin(qqq(1) + qqq(2)) + 100*sin(qqq(1))
   if(abs(200*cos(qqq(1) + qqq(1)) + 100*cos(qqq(1))-pos(1))<1&&abs(200*sin(qqq(1) + qqq(2)) + 100*sin(qqq(1))-pos(2))<1)
       flag = 1;
   end
end
qqq;

  我设置的误差是1,即机械臂末端位置距离希望位置小于1就结束迭代。   好了,以上就是用matlab来实现的雅可比矩阵求运动学。上述代码是可以转化为C或者C++的,因为没有借助其他库,当然,如果要移植到实际可用的地方,需要把雅可比矩阵和雅可比逆矩阵算出来然后再写。   (这里的雅可比矩阵没有考虑姿态噢,后面我准备系统研究下六轴串联机械臂利用雅可比矩阵求逆运动学迭代解,准备用C++写,matlab验证)


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明机械臂通过雅可比矩阵实现正运动学及逆运动学迭代解(工具:matlab)
喜欢 (0)

您必须 登录 才能发表评论!

加载中……