UR5e机器人动力学

机器人动力学研究力与运动之间的关系，核心目标是建立关节力矩与关节位置、速度、加速度的数学关系。动力学模型通常分为：

• 正向动力学：已知关节力矩，计算末端执行器的运动（加速度）。

• 逆向动力学：已知期望的运动（位置、速度、加速度），计算所需的关节力矩。

https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/

正向动力学方程

function dS = robot_dynamics(t,q,T)q1 = q(1); q2 = q(2); q3 = q(3);q4 = q(4); q5 = q(5); q6 = q(6);dq1 = q(7); dq2 = q(8); dq3 = q(9);dq4 = q(10); dq5 = q(11); dq6 = q(12);dS = zeros(12,1);dq = [dq1, dq2, dq3, dq4, dq5, dq6]';M = M_fun(q1,q2,q3,q4,q5,q6);C = C_fun(q1,q2,q3,q4,q5,q6,dq1,dq2,dq3,dq4,dq5,dq6);G = G_fun(q1,q2,q3,q4,q5,q6);
%%dS(1) = dq1;dS(2) = dq2;dS(3) = dq3;dS(4) = dq4;dS(5) = dq5;dS(6) = dq6;J = Je_fun(q1,q2,q3,q4,q5,q6);Fx = 0; Fy = 0; Fz = 0;tx = 0; ty = 0; tz = 0;Fe = [Fx; Fy; Fz; tx; ty; tz];Te = J' * Fe;ddq = inv(M) * (-C*dq - G' + T + Te);dS(7) = ddq(1);dS(8) = ddq(2);dS(9) = ddq(3);dS(10) = ddq(4);dS(11) = ddq(5);dS(12) = ddq(6);
end

M矩阵

function M_mat = M_fun(q1,q2,q3,q4,q5,q6)
%M_FUN
%    M_MAT = M_FUN(Q1,Q2,Q3,Q4,Q5,Q6)%    This function was generated by the Symbolic Math Toolbox version 8.7.
%    01-Oct-2022 21:26:48t2 = cos(q3);
t3 = cos(q4);
t4 = cos(q5);
t5 = sin(q2);
t6 = sin(q3);
t7 = sin(q4);
t8 = sin(q5);
t9 = q2+q5;
t10 = q3+q4;
t11 = q2.*2.0;
t12 = q3.*2.0;
t13 = q4.*2.0;
t14 = q5.*2.0;
t20 = -q5;
t40 = atan(3.818188629788691e-2);
t42 = atan(7.656026541520085e+2);
t44 = 9.934139466657452e+7;
t47 = 1.077033123737933e+9;
t15 = t4.^2;
t16 = q2+t10;
t17 = q3+t9;
t18 = sin(t9);
t19 = sin(t10);
t21 = -t14;
t23 = t9+t10;
t27 = q2+t20;
t37 = t4.*3.38e-2;
t41 = t6.*4.0271725e-2;
t45 = -t42;
t46 = t5.*3.53883322e-1;
t48 = t2.*1.054733773125;
t50 = t2.*t7.*6.92336305e-2;
t51 = t3.*t6.*6.92336305e-2;
t55 = t7.*1.2779713677e-1;
t56 = t7.*6.3898568385e-2;
t60 = t7.*7.747599173958887e+27;
t64 = t6.*t7.*t8.*1.45620788325e-2;
t65 = t4.*3.462224718e-3;
t66 = t2.*t3.*t8.*1.45620788325e-2;
t71 = t3.*t8.*1.3439942169525e-2;
t72 = t3.*t8.*2.687988433905e-2;
t22 = cos(t16);
t24 = sin(t16);
t25 = sin(t17);
t26 = cos(t23);
t28 = q3+t27;
t29 = sin(t27);
t30 = q5+t23;
t31 = t16+t20;
t32 = t16+t21;
t43 = -t41;
t49 = q2+q3+t45;
t52 = -t50;
t53 = -t51;
t57 = -t55;
t58 = -t56;
t61 = t19.*8.394466918884709e+27;
t62 = t18.*7.28103941625e-3;
t67 = -t64;
t68 = -t65;
t70 = t60-1.9958366655817