1. Toy模板网首页
  2. > Toy博客

机械臂正向与逆向运动学求解

10月前 作者：小博特 分类：Toy博客 阅读(38) 违法举报

这篇具有很好参考价值的文章主要介绍了机械臂正向与逆向运动学求解。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

        机械臂的正运动学求解即建立DH参数表，然后计算出各变换矩阵以及最终的变换矩阵。逆运动学求解，即求出机械臂各关节θ角与px,py,pz的关系，建立θ角与末端姿态之间的数学模型，在这里以IRB6700为例，对IRB6700进行正逆运动学求解和验证。

目录

正运动学求解

逆运动学求解

正逆运动学模型的验证

正运动学验证

逆运动学验证

总的Matlab代码，包含正逆运动学求解和验证

参考文献

正运动学求解

        首先使用DH法建立坐标系如下：

机械臂正向与逆向运动学求解

         查阅IRB6700的参数如下表

连杆i

/mm

 /° /mm /° 关节角范围/°
1 0 0 780 +170 ——  (-170)
2 320 -90 0 +85 —— (-65)
3 1125 0 0 +70 —— (-180)
4 200 -90 1142.5 +300 —— (-300)
5 0 90 0 +130 —— (-130)
6 0 -90 200 +360 —— (-360)

根据变换矩阵公式编写matlab代码计算得到正运动学的各变换矩阵

上式中的为,为，为

 计算变换矩阵的MATLAB代码如下

clear;clc
%%
%导入参数
%注，这里的a,afa的下标在实际使用时均需要减去1
syms a afa d theta [1 6]
a = [0 a2 a3 a4 0 0];
afa = [0 -90 0 -90 90 -90];
d = [d1 0 0 d4 0 d6];
theta = [theta1 theta2 theta3 theta4 theta5 theta6];
syms T [4 4 6]
%%
%正运动学模型求解
%计算各变换矩阵及总的变换矩阵
for i = 1:6
   T(:,:,i) =  trans_cal(  afa(i), a(i), d(i), theta(i)*180/pi );
          if(i == 1)
              trans_matrix = T(:,:,i);
          else
              trans_matrix = trans_matrix*T(:,:,i);
          end
end

 用到的funcion函数如下

function T = trans_cal(afa_ii,a_ii,d_i,theta_i)
%%
%计算变换矩阵函数T_{i-1,i}
%输入的参数为，afa_{i-1},a_{i-1},d_i,theta_i,与DH表达法的参数表对应
%ii 为i-1
%注意，这里输入的角度，均采用角度制，不采用弧度制

T = [cosd(theta_i)              -sind(theta_i)             0               a_ii
     sind(theta_i)*cosd(afa_ii)  cosd(theta_i)*cosd(afa_ii)  -sind(afa_ii)    -sind(afa_ii)*d_i
    sind(theta_i)*sind(afa_ii)   cosd(theta_i)*sind(afa_ii)  cosd(afa_ii)     cosd(afa_ii)*d_i
     0                         0                         0                1               ];
 
end

 各个变换矩阵存储在T中，其中即为变换矩阵,存储在trans_matrix中。

逆运动学求解

设总的变换矩阵设为下数：

有等式

根据上面的正运动学求解可以得到矩阵中的等各元素的值分别为什么

使用matlab的实时计算脚本可以较具体地看到

机械臂正向与逆向运动学求解

 然后通过封闭解法求逆，将上述等式中右边地变换矩阵乘到左边，然后观察等式两边矩阵中地元素，使等式两边的矩阵中的某个元素相等，不断分离变量，然后求解得到角

 首先求解，构造等式

 机械臂正向与逆向运动学求解

 观察等式，使等式两边矩阵的第三行第四列元素相等

机械臂正向与逆向运动学求解

上述等式只含有一个未知数，一个等式一个未知数，求解过程如下：

注：为了便于计算，均用代替，也用代替，即下列式子中的实际上为，其他的和亦然如此


机械臂正向与逆向运动学求解

 用同样的方式求解

机械臂正向与逆向运动学求解

 令等式左右两边的第一行第四列，和第三行第四列分别相等，求解过程如下

机械臂正向与逆向运动学求解

 机械臂正向与逆向运动学求解

然后是

机械臂正向与逆向运动学求解

 令等式两边第一行第四列，第二行第四列相等

求解过程如下：

机械臂正向与逆向运动学求解

 再然后是的求解

机械臂正向与逆向运动学求解

令左右两边第三行第三列相等

求解过程如下

机械臂正向与逆向运动学求解

 最后是和的求解

对 构造机械臂正向与逆向运动学求解，使等式左右第一行第三列，和第三行第三列相等

对构造机械臂正向与逆向运动学求解，使等式左右第三行第一列，和第三行第二列相等

 求解过程如下

机械臂正向与逆向运动学求解

 至此完成了所有角的求解

 再次注名：上述推导过程中，为了便于计算，均用代替，也用代替，即上述式子中的实际上为，其他的和亦然如此

正逆运动学模型的验证

正运动学验证

随机选定一组关节角进行验证，

关节角
度数 22.12 -81.4 21.25 -84 19.14 275.13

使用之前求解的正运动学模型求解得到末端姿态矩阵如下

机械臂正向与逆向运动学求解

使用RobotStudio软件导入IRB6700机器人，输入选择的角

注意：由连杆参数可知，RobotStudio软件中的角度为原角度加上，为原角度减去，所以RobotStudio中输入的角度应如下：

关节角
度数 22.12 8.6 21.25 -84 19.14 95.13

 机械臂正向与逆向运动学求解

得到的TCP末端xyz坐标       

 机械臂正向与逆向运动学求解

对比RobotStudio软件中的末端姿态[x,y,z]值与正解求得的矩阵中末端姿态左边[px,py,pz]可得，两者相等，因此正运动学模型正确。

逆运动学验证

由上述的正运动学验证继续计算，上述正运动学得到的末端姿态矩阵如下：

机械臂正向与逆向运动学求解

根据此末端姿态矩阵对逆运动学进行求解，可得到八种不同的角组合，使末端姿态为[1635.672,594.531,1396.902]

求解的matlab代码较长，共一个主m文件，以及5个function文件，放在本文最后。

求解得到共八种组合的角结果，根据IRB6700机器人的各关节角工作范围减去4个不符合的解后，得到四个符合的角组合，然后保持RobotStudio软件中的角参数不变，调整RobotStudio软件中的轴配置参数，将逆运动学模型求解得到的结果与RobotStudio软件中的结果进行对比如下表

1 2 3 4 5 6
逆解1 -157.88 -41.51 -134.76 -30.62 -39.82 -144.08
逆解2 -157.88 -41.51 -134.76 149.38 39.82 35.92
逆解3 22.12 8.6 21.25 96 -19.14 -84.87
逆解4 22.12 8.6 21.25 -84 19.14 95.13

轴配置

（-2,-1,-2,-7）

 -157.88 -41.51 -134.76 -30.62 -39.82 -144.07

轴配置

（-2,1,0,6）

 -157.88 -41.51 -134.76 149.38 39.82 35.93

轴配置

（0,1,-1,1）

 22.12 8.6 21.25 96.01 -19.14 -84.87

轴配置

（0,-1,1,0）

 22.12 8.6 21.25 -84 19.14 95.13

由上表的结果对比可知，除了计算精度问题导致的，部分结果小数点后两位与RobotStudio存在一个单位的差异外，结果一致，因为可以验证逆运动学模型正确。

总的Matlab代码，包含正逆运动学求解和验证

clear;clc
%%
%导入参数
%注，这里的a,afa的下标在实际使用时均需要减去1
syms a afa d theta [1 6]
a = [0 a2 a3 a4 0 0];
afa = [0 -90 0 -90 90 -90];
d = [d1 0 0 d4 0 d6];
theta = [theta1 theta2 theta3 theta4 theta5 theta6];
syms T [4 4 6]
%%
%正运动学模型求解
%计算各变换矩阵及总的变换矩阵
for i = 1:6
   T(:,:,i) =  trans_cal(  afa(i), a(i), d(i), theta(i)*180/pi );
          if(i == 1)
              trans_matrix = T(:,:,i);
          else
              trans_matrix = trans_matrix*T(:,:,i);
          end
end


%%
%逆运动学计算求解
syms r [3 3]
syms PX_X PY_Y PZ_Z
T_60 = [r1_1 r1_2 r1_3 PX_X;
        r2_1 r2_2 r2_3 PY_Y;
        r3_1 r3_2 r3_3 PZ_Z;
        0    0    0    1];
    
left_1 = simplify( inv( T(:,:,2) ) * inv( T(:,:,1) ) * T_60 * inv( T(:,:,6) ) );
right_1 = simplify( T(:,:,3)*T(:,:,4)*T(:,:,5) );


left_2 =  simplify( inv( T(:,:,1) )* T_60 * inv( T(:,:,6) ) );
right_2 = simplify( T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5) );

left_3 =  simplify( inv(   T(:,:,1)*T(:,:,2)   ) * T_60 * inv( T(:,:,6) ) );
right_3 = simplify( T(:,:,3)*T(:,:,4)*T(:,:,5) );

left_4 =  simplify(  inv(   T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)   ) * T_60 );
right_4 = simplify(  T(:,:,6)  );

left_5 =  simplify(  inv(   T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4) ) * T_60 );
right_5 = simplify(  T(:,:,5)*T(:,:,6)  );

left_6 =  simplify(  inv(   T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)   ) * T_60 );
right_6 = simplify(  T(:,:,6)  );


%%
%逆运动学验算

%末端姿态
T_ni = [-0.5    0   0.866   1635.672;
        0       1   0       594.531;
        -0.866  0   -0.5    1396.902;
        0       0   0       1];
    
    
r1_1 = T_ni(1,1);r1_2 = T_ni(1,2);r1_3 = T_ni(1,3);PX_X   = T_ni(1,4);
r2_1 = T_ni(2,1);r2_2 = T_ni(2,2);r2_3 = T_ni(2,3);PY_Y   = T_ni(2,4);
r3_1 = T_ni(3,1);r3_2 = T_ni(3,2);r3_3 = T_ni(3,3);PZ_Z   = T_ni(3,4);

syms a d [1 6]
d1 = 780;d4 = 1142.5;d6 = 200;
a2 = 320;a3 = 1125  ;a4 = 200; 

syms A B C D E F [1 6]

%theta1的解
theta1_1 = ( atan2(0,1) - atan2( d6*r2_3-PY_Y , PX_X-d6*r1_3 ) )*180/pi %22.1230
theta1_2 = ( -atan2(0,-1) - atan2( d6*r2_3-PY_Y , PX_X-d6*r1_3 ) )*180/pi %-157.8770



%theta2的解
[theta2_1 , theta2_2] = theta2_calculate(theta1_1)  %theta1为22.123°时
[theta2_3 , theta2_4] = theta2_calculate(theta1_2)  %theta1为-157.8770°时


%theta3的解
theta3_11_21    =   theta3_calculate(theta1_1,theta2_1)  %theta1为22.123°theta2为-81.3955°时
theta3_11_22    =   theta3_calculate(theta1_1,theta2_2)  %theta1为22.123°theta2为22.0674°时
theta3_12_23    =   theta3_calculate(theta1_2,theta2_3)  %theta1为-157.8770°theta2为172.8834°时
theta3_12_24    =   theta3_calculate(theta1_2,theta2_4)  %theta1为-157.8770°theta2为228.4872°时



%theta4的解
[theta4_1_11_21,theta4_2_11_21] = theta4_calculate(theta1_1,theta2_1,theta3_11_21)  %theta1为22.123°theta2为-81.3955°时
[theta4_1_11_22,theta4_2_11_22] = theta4_calculate(theta1_1,theta2_2,theta3_11_22)  %theta1为22.123°theta2为22.0674°时
[theta4_1_12_23,theta4_2_12_23] = theta4_calculate(theta1_2,theta2_3,theta3_12_23)  %theta1为-157.8770°theta2为172.8834°时
[theta4_1_12_24,theta4_2_12_24] = theta4_calculate(theta1_2,theta2_4,theta3_12_24)  %theta1为-157.8770°theta2为228.4872°时


%theta5的解
theta5_11_21_41 = theta5_calculate(theta1_1,theta2_1,theta3_11_21,theta4_1_11_21) %theta1为22.123°theta2为-81.3955°,theta4为第1种
theta5_11_21_42 = theta5_calculate(theta1_1,theta2_1,theta3_11_21,theta4_2_11_21) %theta1为22.123°theta2为-81.3955°,theta4为第2种
theta5_11_22_41 = theta5_calculate(theta1_1,theta2_2,theta3_11_22,theta4_1_11_22) %theta1为22.123°theta2为22.0674°,theta4为第1种
theta5_11_22_42 = theta5_calculate(theta1_1,theta2_2,theta3_11_22,theta4_2_11_22) %theta1为22.123°theta2为22.0674°,theta4为第2种

theta5_12_23_41 = theta5_calculate(theta1_2,theta2_3,theta3_12_23,theta4_1_12_23) %theta1为-157.8770°theta2为172.8834°,theta4为第1种
theta5_12_23_42 = theta5_calculate(theta1_2,theta2_3,theta3_12_23,theta4_2_12_23) %theta1为-157.8770°theta2为172.8834°,theta4为第2种
theta5_12_24_41 = theta5_calculate(theta1_2,theta2_4,theta3_12_24,theta4_1_12_24) %theta1为-157.8770°theta2为228.4872°,theta4为第1种
theta5_12_24_42 = theta5_calculate(theta1_2,theta2_4,theta3_12_24,theta4_2_12_24) %theta1为-157.8770°theta2为228.4872°,theta4为第2种


% theta6的解
theta6_11_21_41 = theta6_calculate(theta1_1,theta2_1,theta3_11_21,theta4_1_11_21) %theta1为22.123°theta2为-81.3955°,theta4为第1种
theta6_11_21_42 = theta6_calculate(theta1_1,theta2_1,theta3_11_21,theta4_2_11_21) %theta1为22.123°theta2为-81.3955°,theta4为第2种
theta6_11_22_41 = theta6_calculate(theta1_1,theta2_2,theta3_11_22,theta4_1_11_22) %theta1为22.123°theta2为22.0674°,theta4为第1种
theta6_11_22_42 = theta6_calculate(theta1_1,theta2_2,theta3_11_22,theta4_2_11_22) %theta1为22.123°theta2为22.0674°,theta4为第2种
theta6_12_23_41 = theta6_calculate(theta1_2,theta2_3,theta3_12_23,theta4_1_12_23) %theta1为-157.8770°theta2为172.8834°,theta4为第1种
theta6_12_23_42 = theta6_calculate(theta1_2,theta2_3,theta3_12_23,theta4_2_12_23) %theta1为-157.8770°theta2为172.8834°,theta4为第2种
theta6_12_24_41 = theta6_calculate(theta1_2,theta2_4,theta3_12_24,theta4_1_12_24) %theta1为-157.8770°theta2为228.4872°,theta4为第1种
theta6_12_24_42 = theta6_calculate(theta1_2,theta2_4,theta3_12_24,theta4_2_12_24) %theta1为-157.8770°theta2为228.4872°,theta4为第2种


function [theta2_1,theta2_2] = theta2_calculate(theta1)
%%
%导入参数
    syms r [3,3]
    syms a afa d
    syms PX_X PY_Y_Y PZ_Z
    T_ni = [-0.5    0   0.866   1635.672;
        0       1   0       594.531;
        -0.866  0   -0.5    1396.902;
        0       0   0       1];    
    r1_1 = T_ni(1,1);r1_2 = T_ni(1,2);r1_3 = T_ni(1,3);PX_X   = T_ni(1,4);
    r2_1 = T_ni(2,1);r2_2 = T_ni(2,2);r2_3 = T_ni(2,3);PY_Y   = T_ni(2,4);
    r3_1 = T_ni(3,1);r3_2 = T_ni(3,2);r3_3 = T_ni(3,3);PZ_Z   = T_ni(3,4);
    syms a d [1 6]
    d1 = 780;d4 = 1142.5;d6 = 200;
    a2 = 320;a3 = 1125  ;a4 = 200; 
    syms A B C D E F [1 6]
    theta1 = theta1/180*pi;

%%
%theta2计算
    A2 = ( PX_X - d6*r1_3 )*cos(theta1) + ( PY_Y - d6*r2_3 )*sin(theta1) - a2;
    B2 = d1 + d6*r3_3 - PZ_Z;
    C2 = 2*A2*a3;
    D2 = 2*B2*a3;
    E2 = A2^2 + B2^2 + a3^2 - a4^2 - d4^2;
    F2 = sqrt( C2^2 + D2^2 );
    
    theta2_1 = ( -atan2(C2,D2)  + atan2(  E2/F2 ,  sqrt( 1- (E2/F2)^2 ) ) )*180/pi  ;
    theta2_2 = ( -atan2(C2,D2)  + atan2(  E2/F2 , -sqrt( 1- (E2/F2)^2 ) ) )*180/pi  ;

end
function theta3 = theta3_calculate(theta1,theta2)
%%
%导入参数
    syms r [3,3]
    syms a afa d
    syms PX_X PY_Y_Y PZ_Z
    T_ni = [-0.5    0   0.866   1635.672;
        0       1   0       594.531;
        -0.866  0   -0.5    1396.902;
        0       0   0       1];    
    r1_1 = T_ni(1,1);r1_2 = T_ni(1,2);r1_3 = T_ni(1,3);PX_X   = T_ni(1,4);
    r2_1 = T_ni(2,1);r2_2 = T_ni(2,2);r2_3 = T_ni(2,3);PY_Y   = T_ni(2,4);
    r3_1 = T_ni(3,1);r3_2 = T_ni(3,2);r3_3 = T_ni(3,3);PZ_Z   = T_ni(3,4);
    syms a d [1 6]
    d1 = 780;d4 = 1142.5;d6 = 200;
    a2 = 320;a3 = 1125  ;a4 = 200; 
    syms A B C D E F [1 6]
    theta1 = theta1/180*pi;
    theta2 = theta2/180*pi;

%%
%theta3计算
    A3 = d1*sin(theta2) - a2*cos(theta2) - PZ_Z*sin(theta2) + d6*r3_3*sin(theta2) + PX_X*cos(theta1)*cos(theta2) ...
    + PY_Y*cos(theta2)*sin(theta1) - d6*r1_3*cos(theta1)*cos(theta2) - d6*r2_3*cos(theta2)*sin(theta1) - a3;

    B3 = d1*cos(theta2) - PZ_Z*cos(theta2) + a2*sin(theta2) + d6*r3_3*cos(theta2) - PX_X*cos(theta1)*sin(theta2) ...
    - PY_Y*sin(theta1)*sin(theta2) + d6*r2_3*sin(theta1)*sin(theta2) + d6*r1_3*cos(theta1)*sin(theta2);

    C3 = ( a4*B3-d4*A3 ) / ( a4^2 + d4^2 );
    
    D3 = ( a4*A3+d4*B3 ) / ( a4^2 + d4^2 );
    
    theta3 = atan2( C3,D3 )*180/pi;


end
function [theta4_1,theta4_2] = theta4_calculate(theta1,theta2,theta3)
%%
%导入参数
    syms r [3,3]
    syms a afa d
    syms PX_X PY_Y_Y PZ_Z
    T_ni = [-0.5    0   0.866   1635.672;
        0       1   0       594.531;
        -0.866  0   -0.5    1396.902;
        0       0   0       1];    
    r1_1 = T_ni(1,1);r1_2 = T_ni(1,2);r1_3 = T_ni(1,3);PX_X   = T_ni(1,4);
    r2_1 = T_ni(2,1);r2_2 = T_ni(2,2);r2_3 = T_ni(2,3);PY_Y   = T_ni(2,4);
    r3_1 = T_ni(3,1);r3_2 = T_ni(3,2);r3_3 = T_ni(3,3);PZ_Z   = T_ni(3,4);
    syms a d [1 6]
    d1 = 780;d4 = 1142.5;d6 = 200;
    a2 = 320;a3 = 1125  ;a4 = 200; 
    syms A B C D E F [1 6]
    theta1 = theta1/180*pi;
    theta2 = theta2/180*pi;
    theta3 = theta3/180*pi;

%%
%theta4计算

A4 = r3_3*cos(theta2)*sin(theta3) + r3_3*cos(theta3)*sin(theta2) - r1_3*cos(theta1)*cos(theta2)*cos(theta3) ...
    - r2_3*cos(theta2)*cos(theta3)*sin(theta1) + r1_3*cos(theta1)*sin(theta2)*sin(theta3) ...
    + r2_3*sin(theta1)* sin(theta2)*sin(theta3);  
B4 = r2_3*cos(theta1) - r1_3*sin(theta1);
theta4_1 = ( atan2( 0,1 ) + atan2( B4,A4 ) )*180/pi;
theta4_2 = ( atan2( 0,-1 ) + atan2( B4,A4 ) )*180/pi;

end
function theta5 = theta5_calculate(theta1,theta2,theta3,theta4)
%%
%导入参数
    syms r [3,3]
    syms a afa d
    syms PX_X PY_Y_Y PZ_Z
    T_ni = [-0.5    0   0.866   1635.672;
        0       1   0       594.531;
        -0.866  0   -0.5    1396.902;
        0       0   0       1];    
    r1_1 = T_ni(1,1);r1_2 = T_ni(1,2);r1_3 = T_ni(1,3);PX_X   = T_ni(1,4);
    r2_1 = T_ni(2,1);r2_2 = T_ni(2,2);r2_3 = T_ni(2,3);PY_Y   = T_ni(2,4);
    r3_1 = T_ni(3,1);r3_2 = T_ni(3,2);r3_3 = T_ni(3,3);PZ_Z   = T_ni(3,4);
    syms a d [1 6]
    d1 = 780;d4 = 1142.5;d6 = 200;
    a2 = 320;a3 = 1125  ;a4 = 200; 
    syms A B C D E F [1 6]
    theta1 = theta1/180*pi;
    theta2 = theta2/180*pi;
    theta3 = theta3/180*pi;
    theta4 = theta4/180*pi;

%%
%theta4计算

A5 = r1_3*sin(theta1)*sin(theta4) - r2_3*cos(theta1)*sin(theta4) - r3_3*cos(theta2)*cos(theta4)*sin(theta3) ...
    - r3_3*cos(theta3)*cos(theta4)*sin(theta2) + r1_3*cos(theta1)*cos(theta2)*cos(theta3)*cos(theta4) + ...
    r2_3*cos(theta2)*cos(theta3)*cos(theta4)*sin(theta1) - r1_3*cos(theta1)*cos(theta4)*sin(theta2)*sin(theta3) ...
    - r2_3*cos(theta4)*sin(theta1)*sin(theta2)*sin(theta3);
B5 = r3_3*sin(theta2)*sin(theta3) - r3_3*cos(theta2)*cos(theta3) - r1_3*cos(theta1)*cos(theta2)*sin(theta3) ... 
    - r1_3*cos(theta1)*cos(theta3)*sin(theta2) - r2_3*cos(theta2)*sin(theta1)*sin(theta3) - ... 
    r2_3*cos(theta3)*sin(theta1)*sin(theta2);
theta5 = atan2(-A5,B5)*180/pi;

end
function theta6 = theta6_calculate(theta1,theta2,theta3,theta4)
%%
%导入参数
    syms r [3,3]
    syms a afa d
    syms PX_X PY_Y_Y PZ_Z
    T_ni = [-0.5    0   0.866   1635.672;
        0       1   0       594.531;
        -0.866  0   -0.5    1396.902;
        0       0   0       1];    
    r1_1 = T_ni(1,1);r1_2 = T_ni(1,2);r1_3 = T_ni(1,3);PX_X   = T_ni(1,4);
    r2_1 = T_ni(2,1);r2_2 = T_ni(2,2);r2_3 = T_ni(2,3);PY_Y   = T_ni(2,4);
    r3_1 = T_ni(3,1);r3_2 = T_ni(3,2);r3_3 = T_ni(3,3);PZ_Z   = T_ni(3,4);
    syms a d [1 6]
    d1 = 780;d4 = 1142.5;d6 = 200;
    a2 = 320;a3 = 1125  ;a4 = 200; 
    syms A B C D E F [1 6]
    theta1 = theta1/180*pi;
    theta2 = theta2/180*pi;
    theta3 = theta3/180*pi;
    theta4 = theta4/180*pi;

%%
%theta4计算

A6 = r2_1*cos(theta1)*cos(theta4) - r1_1*cos(theta4)*sin(theta1) - r3_1*cos(theta2)*sin(theta3)*sin(theta4) ...
    - r3_1*cos(theta3)*sin(theta2)*sin(theta4) + r1_1*cos(theta1)*cos(theta2)*cos(theta3)*sin(theta4) + ...
    r2_1*cos(theta2)*cos(theta3)*sin(theta1)*sin(theta4) - r1_1*cos(theta1)*sin(theta2)*sin(theta3)*sin(theta4) ...
    - r2_1*sin(theta1)*sin(theta2)*sin(theta3)*sin(theta4);
B6 = r2_2*cos(theta1)*cos(theta4) - r1_2*cos(theta4)*sin(theta1) - r3_2*cos(theta2)*sin(theta3)*sin(theta4) ... 
    - r3_2*cos(theta3)*sin(theta2)*sin(theta4) + r1_2*cos(theta1)*cos(theta2)*cos(theta3)*sin(theta4)  ...
    + r2_2*cos(theta2)*cos(theta3)*sin(theta1)*sin(theta4) - r1_2*cos(theta1)*sin(theta2)*sin(theta3)*sin(theta4) ...
    - r2_2*sin(theta1)*sin(theta2)*sin(theta3)*sin(theta4);

theta6 = atan2(-A6,-B6)*180/pi;

end
function T = trans_cal(afa_ii,a_ii,d_i,theta_i)
%%
%计算变换矩阵函数T_{i-1,i}
%输入的参数为，afa_{i-1},a_{i-1},d_i,theta_i,与DH表达法的参数表对应
%ii 为i-1
%注意，这里输入的角度，均采用角度制，不采用弧度制

T = [cosd(theta_i)              -sind(theta_i)             0               a_ii
     sind(theta_i)*cosd(afa_ii)  cosd(theta_i)*cosd(afa_ii)  -sind(afa_ii)    -sind(afa_ii)*d_i
    sind(theta_i)*sind(afa_ii)   cosd(theta_i)*sind(afa_ii)  cosd(afa_ii)     cosd(afa_ii)*d_i
     0                         0                         0                1               ];
 
end

参考文献

[1] 尹绪伟. 打磨机器人不同位姿下的刚度特性研究[D]. 武汉: 武汉理工大学, 2019.

[2] 王宇迪. 基于加工姿态最优的发动机飞轮壳机器人铣削修边空间路径规划[D]. 武汉: 武汉理工大学, 2022.文章来源地址https://www.toymoban.com/news/detail-409431.html

到了这里，关于机械臂正向与逆向运动学求解的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处： 如若内容造成侵权/违法违规/事实不符，请点击违法举报进行投诉反馈，一经查实，立即删除！

分享到：
领支付宝红包 赞助服务器费用

相关文章

  • 机器人学基础（1）-位置运动学-正运动学、逆运动学方程建立及其求解

    机器人学基础（1）-位置运动学-正运动学、逆运动学方程建立及其求解

    机器人学基础学了个知识框架入门，以此来写一下总结笔记，便于以后要用到相关知识点进行翻阅。 本次机器人学基础笔记主要分为几个章节： 位置运动学、微分运动和速度、动力学分析和力、轨迹规划。 后续其他的知识点等学到了再进行补充。 本人也是刚入门学习，还有

    2023年04月21日
    浏览(37)

  • 机械臂运动学逆解（牛顿法）

      常用的工业6轴机械臂采用6轴串联结构，虽然其运动学正解比较容易，但是其运动学逆解非常复杂，其逆解的方程组高度非线性，且难以化简。   由于计算机技术的发展，依靠其强大的算力，可以通过数值解的方式对机械臂的运动学逆解方程组进行求解。以下将使用牛

    2024年01月22日
    浏览(50)
  • 5_机械臂运动学基础_矩阵

    5_机械臂运动学基础_矩阵

    上次说的向量空间是为矩阵服务的。 1、学科回顾   从科技实践中来的数学问题无非分为两类：一类是线性问题，一类是非线性问题。线性问题是研究最久、理论最完善的；而非线性问题则可以在一定基础上转化为线性问题求解。 线性变换： 数域 F 上线性空间V中的变换T若满

    2024年01月25日
    浏览(40)
  • 机械臂速成小指南（九）：正运动学分析

    机械臂速成小指南（九）：正运动学分析

    👨‍🏫🥰🥳需要机械臂相关资源的同学可以在我的CSDN主页中寻找哦🤖😽🦄 指南目录📖： 🎉🎉机械臂速成小指南（零点五）：机械臂相关资源🎉🎉 机械臂速成小指南（零）：指南主要内容及分析方法 机械臂速成小指南（一）：机械臂发展概况 机械臂速成小指南（二

    2023年04月18日
    浏览(40)
  • matlab机械臂建模运动学仿真+轨迹规划

    matlab机械臂建模运动学仿真+轨迹规划

    1.内容参考B站视频：https://www.bilibili.com/video/BV1q44y1x7WC?spm_id_from=333.337.search-card.all.click 2.工具箱10.4版下载 https://download.csdn.net/download/yohnyang/86099523 3.关于机器人控制相关算法的C++实现可以详见我的专栏 ：https://blog.csdn.net/yohnyang/category_12119720.html Link类使用介绍：https://blog.csdn.

    2024年02月01日
    浏览(44)
  • （6）六轴机械臂的运动学正、逆解

    （6）六轴机械臂的运动学正、逆解

    下面在前面的ur5机械臂的DH参数基础是对其正逆解进行求解，为了后面能在MATLAB中利用stl文件进行实际显示，这里以标准DH参数为例进行讲解。（ 修正DH参数在用plot3d函数是显示失败，不知道是不是这个函数只能显示标准dh参数的机械臂模型，有知道的网友可以在评论里告知一

    2023年04月09日
    浏览(52)
  • MATLAB - 使用运动学 DH 参数构建机械臂

    MATLAB - 使用运动学 DH 参数构建机械臂

           使用 Puma560® 机械手机器人的 Denavit-Hartenberg (DH) 参数，逐步建立刚体树形机器人模型。在连接每个关节时，指定其相对 DH 参数。可视化机器人坐标系，并与最终模型进行交互。 DH 参数定义了每个刚体通过关节与其父体连接的几何形状。这些参数遵循四种变换惯例：

    2024年01月16日
    浏览(48)
  • 三轴机械臂逆运动学解算（附代码）

    三轴机械臂逆运动学解算（附代码）

    机械臂运动位姿的求解有两种方式 通过控制已知的连轴（舵机或电机）的旋转角度，求出机械臂终端的空间坐标 通过已知的抓取点的空间坐标，求解出三个舵机所需要转动的角度，这里主要讲解逆运动学解法 ​​ 此处θ1 ，θ2， θ3是三个舵机所需转动的角度 ，γ是杆3相对于

    2024年02月04日
    浏览(74)
  • 机械臂速成小指南（二十二）：机械臂逆运动学的数值解方法

    机械臂速成小指南（二十二）：机械臂逆运动学的数值解方法

    👨‍🏫🥰🥳需要机械臂相关资源的同学可以在我的CSDN主页中寻找哦🤖😽🦄   指南目录📖： 🎉🎉机械臂速成小指南（零点五）：机械臂相关资源🎉🎉 机械臂速成小指南（零）：指南主要内容及分析方法 机械臂速成小指南（一）：机械臂发展概况 机械臂速成小指南（

    2024年02月12日
    浏览(46)
  • 六自由度机器人（机械臂）运动学建模及运动规划系列（一）——简介

    六自由度机器人（机械臂）运动学建模及运动规划系列（一）——简介

    毕业设计做了六轴机器人相关的课题，做完之后学到很多，在这里分享一下。本篇首先对六轴机器人及其研究内容进行简单的介绍。 六轴机器人中的六轴指个六自由度，由关节和连杆组成。常见的六轴机器人为 串联型旋转关节机器人 。这里以一款川崎机器人为例，展示一下

    2024年02月02日
    浏览(47)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包，优惠每天领

二维码1

领取红包

二维码2

领红包