修正逆解文章——六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）

我不是女神ヾ 2023-01-01 15:58 359阅读 0赞

> 如下参考链接1的作者大大实现了UR5机械臂的正运动学和逆运动学的Matlab代码。但逆解部分在不同版本的Matlab中运行有错误。
> 
> 本篇文章是MatlabR2016a下完成的，并说明一下原代码错误的原因。

**目录**

一、参考链接说明

二、UR5机械臂运动学正解

三、UR5机械臂运动学逆解

四、测试代码

五、参考链接1的逆解代码错误原因

--------------------

参考链接：[六轴UR机械臂正逆运动学求解\_MATLAB代码（标准DH参数表）][UR_MATLAB_DH]

[UR机械臂正逆运动学求解][UR]

## 一、参考链接说明 ##

参考链接2的作者实现了UR5机械臂的正运动学和逆运动学的理论推导，给出了求解公式；

参考链接1的作者在参考链接2的基础上，写出了Matlab代码，但作者给出的代码只能在**特定的Matlab中运行，如2017b。**

## 二、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
    
    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

## 三、UR5机械臂运动学逆解 ##

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);
        theta6(1,1:2)=atan2(mm,nn)-atan2(sin(theta5(1,1:2)),0);
        theta6(2,1:2)=atan2(mm,nn)-atan2(sin(theta5(2,1:2)),0);
        
        %求解关节角3
        mmm(1,1:2)=d(5)*(sin(theta6(1,1:2)).*(nx*cos(theta1)+ny*sin(theta1))+cos(theta6(1,1:2)).*(ox*cos(theta1)+oy*sin(theta1))) ...
            -d(6)*(ax*cos(theta1)+ay*sin(theta1))+px*cos(theta1)+py*sin(theta1);
        nnn(1,1:2)=pz-d(1)-az*d(6)+d(5)*(oz*cos(theta6(1,1:2))+nz*sin(theta6(1,1:2)));
        mmm(2,1:2)=d(5)*(sin(theta6(2,1:2)).*(nx*cos(theta1)+ny*sin(theta1))+cos(theta6(2,1:2)).*(ox*cos(theta1)+oy*sin(theta1))) ...
            -d(6)*(ax*cos(theta1)+ay*sin(theta1))+px*cos(theta1)+py*sin(theta1);
        nnn(2,1:2)=pz-d(1)-az*d(6)+d(5)*(oz*cos(theta6(2,1:2))+nz*sin(theta6(2,1:2)));
        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

## 四、测试代码 ##

clc;clear
    
    %% 初始化关节角度
    %theta=[1,1/2,1/3,1,1,1];%没有什么含义，随便取的
    theta=[1,1/2,1,1,1,1];%没有什么含义，随便取的
    %theta=[1,1,1,1,1,1];%没有什么含义，随便取的
    
    %% 求运动学正解
    TT=zhengyundongxue(theta)
    
    %% 求运动学逆解
    niyundongxue(TT)

## 五、参考链接1的逆解代码错误原因 ##

按照求解过程，在求解到不同关节角时，得到的解的个数如下：

![20210103215107408.png][]

![20210103215244713.png][]

同样的，

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70][]

改了这两个地方就好啦。这样，大家勉强能入门了，但如果能知其所以然，推导到其他标准DH系参数，就更好了！！！

[UR_MATLAB_DH]: https://blog.csdn.net/weixin_40534238/article/details/89378744?utm_medium=distribute.pc_relevant_download.none-task-blog-BlogCommendFromBaidu-2.nonecase&depth_1-utm_source=distribute.pc_relevant_download.none-task-blog-BlogCommendFromBaidu-2.nonecas
[UR]: https://blog.csdn.net/fengyu19930920/article/details/81144042#comments
[20210103215107408.png]: /images/20221120/8a3fffcebf784bbf8f1e3ea05c27d383.png
[20210103215244713.png]: /images/20221120/778d53d123cf47839432260f6d6dee8a.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70]: /images/20221120/d97fd0e71010446a89c6fe6129618cac.png