五连杆机械臂求正解逆解

最新推荐文章于 2022-06-01 13:50:59 发布

时光@印迹

最新推荐文章于 2022-06-01 13:50:59 发布

阅读量2.2k

点赞数 1

分类专栏：机械臂

本文链接：https://blog.csdn.net/weixin_42454034/article/details/109681661

版权

机械臂专栏收录该内容

2 篇文章 0 订阅

订阅专栏

本文详细介绍了五连杆机器人基于D-H参数的正解和逆解算法。正解通过D-H参数计算关节旋转角度与平移距离之间的关系，而逆解则从末端坐标和欧拉角出发，求解各关节角度。通过建立数学模型并利用矩阵变换，最终得出关节角度的解析解。这种方法对于理解机器人运动学和控制系统设计具有重要意义。

摘要由CSDN通过智能技术生成

D-H参数表
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|         0|         L1|     1.5708|          0|
|  2|         q2|          0|       L2|          0|          0|
|  3|         q3|          0|       L3|          0|          0|
|  4|         q4|          0|        L4|     1.5708|          0|
|  5|         q5|          0|        L5|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+

在这里插入图片描述

五连杆如图所示
正解用D-H参数相乘

L1=0.5; L2=0.7;L3=0.8;L4=0.7;L5=0.5;
p1 = [pi/4 0 L1  pi/2]; 
p2 = [pi/4 0 L2 0];
p3 = [pi/4 0 L3 0];
p4 = [0 0 L4  pi/2]; 
p5 = [pi/4 0 L5 0];

para = [p1;p2;p3;p4;p5];
pos = fk(para)

function T = fk(links)
    %正解
    %已知关节角求变换矩阵
    %  theta        kinematic: joint angle  绕Z轴旋转角度
    %  d            kinematic: link offset  绕Z轴平移距离
    %  a            kinematic: link length  绕X轴平移距离
    %  alpha        kinematic: link twist   绕X轴旋转角度

    T=diag([1 1 1 1]);   %单位阵
    si=size(links);  
    
    for i=1:si(1)   %行数
        T_link=T_para(links(i,:));
        T=T*T_link;
    end
end
function T = T_para(p)
%由连杆参数转化为转化矩阵
%theta：绕Z轴旋转角度； a:绕X轴平移距离; d:绕Z轴平移距离; alpha:绕X轴旋转角度
%返回4维矩阵
theta=p(1);d=p(2);a=p(3);alpha=p(4);
    T=[cos(theta),-sin(theta)*cos(alpha),sin(theta)*sin(alpha),a*cos(theta);
        sin(theta),cos(theta)*cos(alpha),-cos(theta)*sin(alpha),a*sin(theta);
        0,sin(alpha),cos(alpha),d;
        0,0,0,1];
end

逆解就解方程了

已知末端坐标及欧拉角和杆长 x y z R P Y L1 L2 L3 L4 L5  求五杆的转角 th1 th2 th3 th4 th5
 为了减少变量个数，alpha及 offset作为已知参数给出
由末端坐标及欧拉角求得末端的变换矩阵 T
[ cos(P)*cos(Y), cos(Y)*sin(P)*sin(R) - cos(R)*sin(Y), sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P), x]
[ cos(P)*sin(Y), cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y), cos(R)*sin(P)*sin(Y) - cos(Y)*sin(R), y]
[       -sin(P),                        cos(P)*sin(R),                        cos(P)*cos(R), z]
[             0,                                    0,                                    0, 1]

%连杆参数
p1 = [th1 0 L1 pi/2]; 
p2 = [th2 0 L2 0];
p3 = [th3 0 L3 0];
p4 = [th4 0 L4  pi/2]; 
p5 = [th5 0 L5 0];
%转化矩阵
T1 = T_para(p1);
T2 = T_para(p2);
T3 = T_para(p3);
T4 = T_para(p4);
T5 = T_para(p5);

%T1的逆设为T1_inv ,其余类比
T1_inv = inv(T1 )
T5_inv = inv(T5);

由 T = T1*T2*T3*T4*T5
T1_inv  * T = T2*T3*T4*T5
T1_inv  * T  = 
[  cos(P)*cos(Y - th1),   sin(th1)*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y)) - cos(th1)*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R)), cos(th1)*(sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P)) - sin(th1)*(cos(Y)*sin(R) - cos(R)*sin(P)*sin(Y)), x*cos(th1) - L1 + y*sin(th1)]
[              -sin(P),                                                                                       cos(P)*sin(R),                                                                                     cos(P)*cos(R),                            z]
[ -cos(P)*sin(Y - th1), - cos(th1)*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y)) - sin(th1)*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R)), cos(th1)*(cos(Y)*sin(R) - cos(R)*sin(P)*sin(Y)) + sin(th1)*(sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P)),      x*sin(th1) - y*cos(th1)]
[                    0,                                                                                                   0,                                                                                                 0,                            1]
 
 T2*T3*T4*T5=
[ cos(th2 + th3 + th4)*cos(th5), -cos(th2 + th3 + th4)*sin(th5),  sin(th2 + th3 + th4), L3*cos(th2 + th3) + L2*cos(th2) + L4*cos(th2 + th3)*cos(th4) - L4*sin(th2 + th3)*sin(th4) - L5*cos(th5)*(sin(th2 + th3)*sin(th4) - cos(th2 + th3)*cos(th4))]
[ sin(th2 + th3 + th4)*cos(th5), -sin(th2 + th3 + th4)*sin(th5), -cos(th2 + th3 + th4), L3*sin(th2 + th3) + L2*sin(th2) + L4*cos(th2 + th3)*sin(th4) + L4*sin(th2 + th3)*cos(th4) + L5*cos(th5)*(cos(th2 + th3)*sin(th4) + sin(th2 + th3)*cos(th4))]
[                      sin(th5),                       cos(th5),                     0,                                                                                                                                                 L5*sin(th5)]
[                             0,                              0,                     0,                                                                                                                                                           1]

 
 由两者三行四列相等：
   L5*sin(th5) =  x*sin(th1) - y*cos(th1)
求得th1与th5关系

 由两者1行1列与2行1列 得到
 sin(th2 + th3 + th4) =    -sin(P)/cos(th5)
 cos(th2 + th3 + th4) =  cos(P)*cos(Y - th1) / cos(th5);
由于 sin(th2 + th3 + th4)与 cos(th2 + th3 + th4) 有多种表示方式，在此取不包含R的表达式

又有   T1_inv  * T* T5_inv  = T2*T3*T4
T1_inv  * T* T5_inv = 
[ sin(th5)*(cos(th1)*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R)) - sin(th1)*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y))) + cos(P)*cos(th5)*cos(Y - th1),   cos(P)*sin(th5)*cos(Y - th1) - cos(th5)*(cos(th1)*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R)) - sin(th1)*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y))), cos(th1)*(sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P)) - sin(th1)*(cos(Y)*sin(R) - cos(R)*sin(P)*sin(Y)), x*cos(th1) - L5*(cos(P)*cos(Y)*cos(th1) + cos(P)*sin(Y)*sin(th1)) - L1 + y*sin(th1)]
[                                                                                                  - sin(P)*cos(th5) - cos(P)*sin(R)*sin(th5),                                                                                                      cos(P)*sin(R)*cos(th5) - sin(P)*sin(th5),                                                                                     cos(P)*cos(R),                                                                       z + L5*sin(P)]
[ sin(th5)*(cos(th1)*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y)) + sin(th1)*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R))) - cos(P)*cos(th5)*sin(Y - th1), - cos(th5)*(cos(th1)*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y)) + sin(th1)*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R))) - cos(P)*sin(th5)*sin(Y - th1), cos(th1)*(cos(Y)*sin(R) - cos(R)*sin(P)*sin(Y)) + sin(th1)*(sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P)),      x*sin(th1) - y*cos(th1) - L5*(cos(P)*cos(Y)*sin(th1) - cos(P)*sin(Y)*cos(th1))]
[                                                                                                                                           0,                                                                                                                                             0,                                                                                                 0,                                                                                   1]
 
 T2*T3*T4 = 
[ cos(th2 + th3 + th4), 0,  sin(th2 + th3 + th4), L3*cos(th2 + th3) + L2*cos(th2) + L4*cos(th2 + th3 + th4)]
[ sin(th2 + th3 + th4), 0, -cos(th2 + th3 + th4), L3*sin(th2 + th3) + L2*sin(th2) + L4*sin(th2 + th3 + th4)]
[                    0, 1,                     0,                                                         0]
[                    0, 0,                     0,                                                         1]

由两者三行四列相等：
x*sin(th1) - y*cos(th1) - L5*(cos(P)*cos(Y)*sin(th1) - cos(P)*sin(Y)*cos(th1)) = 0
则 th1 = atan( (y-L5*cos(P)*sin(Y))/(x-L5*cos(P)*cos(Y)) ) 
所以  th5=asin( (x*sin(th1)-y*cos(th1)) / L5);

因为五杆只有五个转动副，理应只能控制五个未知量，所以一个末端坐标应该由其它表示
由两者二行二列相等
 cos(P)*sin(R)*cos(th5) - sin(P)*sin(th5) = 0
 得到 R =asin( sin(P)*sin(th5)/cos(P)/cos(th5) );
在此可以得到R，但由于sinR已知时 cosR有双解 所以不使用存在cosR的等式

 由二者1行4列与2行4列 联立解方程
 L3*cos(th2 + th3) + L2*cos(th2) + L4*cos(th2 + th3 + th4) =   x*cos(th1) - L5*(cos(P)*cos(Y)*cos(th1) + cos(P)*sin(Y)*sin(th1)) + y*sin(th1)
  L3*sin(th2 + th3) + L2*sin(th2) + L4*sin(th2 + th3 + th4) =   z - L1 + L5*sin(P)
令
m1 = z - L1  + L5*sin(P) - L4* sin(th2 + th3 + th4) ;
n1=x*cos(th1) - L5*(cos(P)*cos(Y)*cos(th1) + cos(P)*sin(Y)*sin(th1)) + y*sin(th1) - L4*cos(th2 + th3 + th4);

方程简化为
m1 - L2*sin(th2) = L3*sin(th2 + th3)
n1 -  L2*cos(th2) = L3*cos(th2 + th3)
两边平方相加,得
2*m1*L2*sin(th2) + 2*n1*L2*cos(th2) =m1*m1+n1*n1+L2*L2 - L3*L3
令：
m2=2*m1*L2;
n2=2*n1*L2;
k=m1^2+n1^2+L2^2-L3^2;
解这个方程，可以通过平方消去sin或者cos,如果消去的是sin,则使用acos可以求得th2,得到
th2 =acos(( 2*n2*k-sqrt(4*n2^2*k^2-4*(k^2-m2^2)*(m2^2+n2^2)) ) / ( 2*(m2^2+n2^2) ));
或者 th2 =acos(( 2*n2*k + sqrt(4*n2^2*k^2-4*(k^2-m2^2)*(m2^2+n2^2)) ) / ( 2*(m2^2+n2^2) ));
求解th3与th4时同样使用acos，这样使用求解角度范围在0~pi是唯一的
再代入方程，得
 th3 =acos((n1 - L2*cos(th2))/L3) -th2;
 th4=acos(cos234)-th2-th3; %cos234为 cos(th2 + th3 + th4) 
求解完毕

则可以由 x y z  P Y 求五杆的转角 th1 th2 th3 th4 th5
有多解 ，而此处没有讨论角度范围，所以该方法适合求解角度范围在0~pi

如果消去的是cos,则使用asin可以求得th2,
det =4*m2^2*k^2-4*(k^2-n2^2)*(m2^2+n2^2);
th2(1) =asin(( 2*m2*k-sqrt(det) ) / ( 2*(m2^2+n2^2) ));
th2(2) =asin(( 2*m2*k+sqrt(det) ) / ( 2*(m2^2+n2^2) ));
th3 =asin((m1 -L2*sin(th2))/L3) -th2;
th4=asin(sin234)-th2-th3;
c= [th1,th2(1),th3(1),th4(1),th5];
该方法适合求解角度范围在-pi/2~pi/2

时光@印迹

关注

1
点赞
踩
21

收藏

觉得还不错? 一键收藏
0
评论
五连杆机械臂求正解逆解

D-H参数表+---+-----------+-----------+-----------+-----------+-----------+| j | theta | d | a | alpha | offset |+---+-----------+-----------+-----------+-----------+-----------+| 1| q1| L1| 0| 1.5708|
复制链接

扫一扫

专栏目录