六轴UR机械臂正逆运动学求解_MATLAB代码(标准DH参数表)

机器人 专栏收录该内容
1 篇文章 0 订阅

 

代码逻辑参考: https://blog.csdn.net/fengyu19930920/article/details/81144042#comments

 

2020/7/22补充:我把所有文件和机器人工具箱打包了,新来的朋友可以不往下看了,直接下载用吧。

链接:https://pan.baidu.com/s/

1n7uyCeUrw1zO1b7UGNDSnw 

%%%%%%%%%%%%%%%%%%%%%自行把上面链接放到一行,进行搜索%%%%%%%%%%%%%%%%%%%%%
提取码:csdn

2020/7/22 补充:用机器人工具箱迭代求逆解有时报错,但niyundongxue.m求逆解没问题。所以有两种方法解决:1. 注释掉工具箱求逆解过程:

%  AA=ikine(robot_UR5,A);

2. 更换下theta角度值,比如更换为:

theta=[1,1/2,1/3,1,1,1];%没有什么含义,随便取的

此博客目的在于写个能求正逆解的MATLAB程序,便于其他功能开发,比较速成,被验证可行。

2019/5/30 新编辑:已经有两个小伙伴证明了:我这个程序在我的MATLAB2017b上运行没问题,但在2018版本上就会报错,我也不知道是什么原因!!!

2019/4/25 新编辑:改正了zhengyundongxue和niyundongxue中的DH表(之前写错了,汗。。。)

背景介绍: 

本人想用    MATLAB+V-REP  仿真机器人,对机器人的正逆运动学简单写了下代码实现部分。

 首先是正运动学求解代码(所用DH参数表为UR5)

function T = zhengyundongxue(theta)
    %已知关节角求变换矩阵
    a=[0,-0.42500,-0.39225,0,0,0];
    d=[0.089159,0,0,0.10915,0.09465,0.08230];
    alpha=[pi/2,0,0,pi/2,-pi/2,0];

    
    T01=T_para(theta(1),d(1),a(1),alpha(1));
    T12=T_para(theta(2),d(2),a(2),alpha(2));
    T23=T_para(theta(3),d(3),a(3),alpha(3));
    T34=T_para(theta(4),d(4),a(4),alpha(4));
    T45=T_para(theta(5),d(5),a(5),alpha(5));
    T56=T_para(theta(6),d(6),a(6),alpha(6));
    
    T=T01*T12*T23*T34*T45*T56;
    
end

 

function T = T_para(theta,d,a,alpha)
    T=[ccc(theta),-sss(theta)*ccc(alpha),sss(theta)*sss(alpha),a*ccc(theta);
        sss(theta),ccc(theta)*ccc(alpha),-ccc(theta)*sss(alpha),a*sss(theta);
        0,sss(alpha),ccc(alpha),d;
        0,0,0,1];
end

好了,问题来了,这个ccc和sss函数就是cos和sin,我承认我SB了。。。其实我的目的是,在角度和弧度之间切换。

function sss=sss(a)
%     sss=sin(a/180*pi);
    sss=sin(a);
end
function ccc=ccc(a)
%     ccc=cos(a/180*pi);
     ccc=cos(a);
end

        至此,正运动学代码部分贴完了。
  

        其次,是逆运动学代码部分

function theta=niyundongxue(T)
    %变换矩阵T已知
    %SDH:标准DH参数表求逆解(解析解)
    %部分DH参数表如下,需要求解theta信息
    
    a=[0,-0.42500,-0.39225,0,0,0];
    d=[0.089159,0,0,0.10915,0.09465,0.08230];
    
    alpha=[pi/2,0,0,pi/2,-pi/2,0];% alpha没有用到,故此逆解程序只适合alpha=[pi/2,0,0,pi/2,-pi/2,0]的情况!
    
    nx=T(1,1);ny=T(2,1);nz=T(3,1);
    ox=T(1,2);oy=T(2,2);oz=T(3,2);
    ax=T(1,3);ay=T(2,3);az=T(3,3);
    px=T(1,4);py=T(2,4);pz=T(3,4);
    
    %求解关节角1
    m=d(6)*ay-py;  n=ax*d(6)-px; 
    theta1(1,1)=atan2(m,n)-atan2(d(4),sqrt(m^2+n^2-(d(4))^2));
    theta1(1,2)=atan2(m,n)-atan2(d(4),-sqrt(m^2+n^2-(d(4))^2));
  
    %求解关节角5
    theta5(1,1:2)=acos(ax*sin(theta1)-ay*cos(theta1));
    theta5(2,1:2)=-acos(ax*sin(theta1)-ay*cos(theta1));      
    
    %求解关节角6
    mm=nx*sin(theta1)-ny*cos(theta1); nn=ox*sin(theta1)-oy*cos(theta1);
    theta6=atan2(mm,nn)-atan2(sin(theta5),0);
    
    %求解关节角3
    mmm=d(5)*(sin(theta6).*(nx*cos(theta1)+ny*sin(theta1))+cos(theta6).*(ox*cos(theta1)+oy*sin(theta1))) ...
        -d(6)*(ax*cos(theta1)+ay*sin(theta1))+px*cos(theta1)+py*sin(theta1);
    nnn=pz-d(1)-az*d(6)+d(5)*(oz*cos(theta6)+nz*sin(theta6));
    theta3(1:2,:)=acos((mmm.^2+nnn.^2-(a(2))^2-(a(3))^2)/(2*a(2)*a(3)));
    theta3(3:4,:)=-acos((mmm.^2+nnn.^2-(a(2))^2-(a(3))^2)/(2*a(2)*a(3)));
    
    %求解关节角2
    mmm_s2(1:2,:)=mmm;
    mmm_s2(3:4,:)=mmm;
    nnn_s2(1:2,:)=nnn;
    nnn_s2(3:4,:)=nnn;
    s2=((a(3)*cos(theta3)+a(2)).*nnn_s2-a(3)*sin(theta3).*mmm_s2)./ ...
        ((a(2))^2+(a(3))^2+2*a(2)*a(3)*cos(theta3));
    c2=(mmm_s2+a(3)*sin(theta3).*s2)./(a(3)*cos(theta3)+a(2));
    theta2=atan2(s2,c2);   
    
    %整理关节角1 5 6 3 2
    theta(1:4,1)=theta1(1,1);theta(5:8,1)=theta1(1,2);
    theta(:,2)=[theta2(1,1),theta2(3,1),theta2(2,1),theta2(4,1),theta2(1,2),theta2(3,2),theta2(2,2),theta2(4,2)]';
    theta(:,3)=[theta3(1,1),theta3(3,1),theta3(2,1),theta3(4,1),theta3(1,2),theta3(3,2),theta3(2,2),theta3(4,2)]';
    theta(1:2,5)=theta5(1,1);theta(3:4,5)=theta5(2,1);
    theta(5:6,5)=theta5(1,2);theta(7:8,5)=theta5(2,2);
    theta(1:2,6)=theta6(1,1);theta(3:4,6)=theta6(2,1);
    theta(5:6,6)=theta6(1,2);theta(7:8,6)=theta6(2,2); 
    
    %求解关节角4
    theta(:,4)=atan2(-sin(theta(:,6)).*(nx*cos(theta(:,1))+ny*sin(theta(:,1)))-cos(theta(:,6)).* ...
        (ox*cos(theta(:,1))+oy*sin(theta(:,1))),oz*cos(theta(:,6))+nz*sin(theta(:,6)))-theta(:,2)-theta(:,3);  
    
end

       代码测试部分:


L1 = Link('d', 0.089159, 'a', 0,        'alpha', pi/2 ,'standard' );
L2 = Link('d', 0,        'a', -0.42500,   'alpha',   0  ,'standard' );
L3 = Link('d', 0,        'a', -0.39225, 'alpha',   0  ,'standard' );
L4 = Link('d', 0.10915,  'a', 0,        'alpha', pi/2 ,'standard' );
L5 = Link('d', 0.09465,  'a', 0,        'alpha', -pi/2 ,'standard');
L6 = Link('d', 0.08230,  'a', 0,        'alpha',   0   ,'standard');

robot_UR5=SerialLink([L1,L2,L3,L4,L5,L6],'name','UR5');   %SerialLink 类函数
robot_UR5.display();  %Link 类函数
robot_UR5.teach(); %可以自由拖动的关节角度

theta=[1,1/2,1,1,1,1];%没有什么含义,随便取的

% robot_UR5.plot(theta);   %SerialLink 类函数

%利用机器人工具箱求正、逆运动学
A=fkine(robot_UR5,theta);
AA=ikine(robot_UR5,A);

%利用刚写的函数求正、逆运动学
B=zhengyundongxue(theta);
BB=niyundongxue(B);

结果部分:

经再次运行正运动学验证,我所求的8组逆解都是正确的。(不知道为什么机器人工具箱没有求出逆解???

 

          Tip:

                  1. 编写代码时,不要参考原博主https://blog.csdn.net/fengyu19930920/article/details/81144042#comments的结论部分,有错误,比如第一个m和n的定义与前述不同。

                   2. 关于奇异位置,未作说明。

                   3. 代码有没有硬核?名字无所谓,瞎起的,代码的逻辑不知道别人能不能看懂,能用就完事了。

                   4. 改进DH参数表求逆运动学,请参考https://blog.csdn.net/jldemanman/article/details/80785075,本人未验证她的代码是否可用(明天周五了,想了想这周进度,又是深深的难过。。。)

相关推荐
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值