当前位置:   article > 正文

【机器人学】4-2.六自由度机器人动力学-牛顿欧拉递推式【附MATLAB代码】

【机器人学】4-2.六自由度机器人动力学-牛顿欧拉递推式【附MATLAB代码】

动力学问题,简单来看,主要由两个问题组成:

已知一个轨迹点theta、theta_d、theta_dd,希望求出期望的关节力矩tau(逆运动学),这个问题及相应公式对机械臂的控制问题非常有用;
已知一个力矩矢量tau,计算出机械臂各关节的瞬态运动变量theta、theta_d、theta_dd(正运动学),这个问题对机械臂的运动仿真及控制非常有用。
准备知识:

【机器人学】4-1.六自由度机器人动力学-开篇-基础知识

通过上一篇的知识,知道了速度在连杆间的传播,静力平衡,以及刚体的质量分布,有了质量,和线速度就可用牛顿定理求出力,有了连杆的惯性张量和角速度,就可用欧拉方程求得力矩。

牛顿欧拉递推式的方程主要是(4.21)和(4.22)。可以看到,可以通过迭代的方式编程实现。

        假设机械臂动力学参数如下:

关节1关节2关节3关节4关节5关节6
Ixx0-2.782-0.67410.49640.0893-0.2332
Ixy00. 086-0.3658-0.3109-0.02520. 0203
Ixz0-0.84130.03730.1658-0.0699-0.0601
Iyy000000
Iyz3.7033-0.13640.03770.21390.03250.0477
Izz02.47411.0102-0.12130. 1510.0763
lx03. 53491.8417-0.0475-0.02020.0006
ly00. 09390.07460.1624-0.08440.03
lz000000
m01010101010

MATLAB仿真验证

  1. % 求齐次变换矩阵
  2. function T = DHTrans(alpha, a, d, theta)
  3. T= [cos(theta) -sin(theta) 0 a;
  4. sin(theta)*cos(alpha) cos(theta)*cos(alpha) -sin(alpha) -sin(alpha)*d;
  5. sin(theta)*sin(alpha) cos(theta)*sin(alpha) cos(alpha) cos(alpha)*d;
  6. 0 0 0 1];
  7. end
  1. % 六自由度机械臂运动的牛顿-欧拉递归逆动力学求解:
  2. % 参数:各关节运动角度, 速度, 加速度(全部6*1矩阵),刚体质量矩阵(1*6)
  3. % 返回值:各关节力矩(6*1矩阵)
  4. function tau = myNewtonEuler(theta, theta_d, theta_dd,m)
  5. % 初始化
  6. % 改进D-H参数
  7. th(1) = theta(1)*pi/180; d(1) = 0.1607; a(1) = 0; alp(1) = 0;
  8. th(2) = theta(2)*pi/180; d(2) = 0; a(2) = 0; alp(2) = pi/2;
  9. th(3) = theta(3)*pi/180; d(3) = 0; a(3) = 0.425; alp(3) = 0;
  10. th(4) = theta(4)*pi/180; d(4) = 0.1133; a(4) = 0.393; alp(4) = 0;
  11. th(5) = theta(5)*pi/180; d(5) = 0.099; a(5) = 0; alp(5) = -pi/2;
  12. th(6) = theta(6)*pi/180; d(6) = 0.0936; a(6) = 0; alp(6) = pi/2;
  13. %base_link的各项初始值
  14. w00 = [0; 0; 0]; v00 = [0; 0; 0]; w00d = [0; 0; 0]; v00d = [0; 0; 9.8];
  15. z = [0; 0; 1];
  16. % 各连杆质量
  17. m1 = m(1); m2 = m(2); m3 = m(3); m4 = m(4); m5 = m(5); m6 = m(6);
  18. % 惯性张量
  19. I1 = [0 0 0; 0 0 0; 0 0 3.7033];
  20. I2 = [-2.782 0.086 -0.8413; 0.086 0 -0.1364; -0.8413 -0.1364 2.4741];
  21. I3 = [-0.6741 -0.3658 0.0373; -0.3658 0 0.0377; 0.0373 0.0377 1.0102];
  22. I4 = [0.4964 -0.3109 0.1658; -0.3109 0 0.2139; 0.1658 0.2139 -0.1213];
  23. I5 = [0.0893 -0.0252 -0.0699; -0.0252 0 0.0325; -0.0699 0.0325 0.151];
  24. I6 = [-0.2332 0.0203 -0.0601; 0.0203 0 0.0477; -0.0601 0.0477 0.0763];
  25. T01 = DHTrans(alp(1), a(1), d(1), th(1));
  26. T12 = DHTrans(alp(2), a(2), d(2), th(2)+pi/2);
  27. T23 = DHTrans(alp(3), a(3), d(3), th(3));
  28. T34 = DHTrans(alp(4), a(4), d(4), th(4)-pi/2);
  29. T45 = DHTrans(alp(5), a(5), d(5), th(5));
  30. T56 = DHTrans(alp(6), a(6), d(6), th(6));
  31. % 各关节p及各link质心pc的距离(假设质心在几何中心)
  32. p10 = T01(1:3,4);p21 = T12(1:3,4); p32 = T23(1:3,4);
  33. p43 = T34(1:3,4); p54 = T45(1:3,4);
  34. p65 = T56(1:3,4); p76 = [0, 0, 0]';
  35. pc11 = [0;0;0];pc22 = [3.5349;0.0939;0]; pc33 = [1.8417; 0.0746; 0];
  36. pc44 = [-0.0475;0.1624;0]; pc55 = [-0.0202;-0.0844;0];
  37. pc66 = [0.0006; 0.03;0];
  38. % 旋转矩阵
  39. R01 = T01(1:3, 1:3); R12 = T12(1:3, 1:3); R23 = T23(1:3, 1:3);
  40. R34 = T34(1:3, 1:3); R45 = T45(1:3, 1:3); R56 = T56(1:3, 1:3);
  41. R10 = R01'; R21 = R12'; R32 = R23';
  42. R43 = R34'; R54 = R45'; R65 = R56';
  43. R67 = [1 0 0; 0 1 0; 0 0 1];
  44. %内推 i: 0->5
  45. % 连杆1到连杆6向外迭代
  46. % i = 0
  47. w11 = R10*w00 + theta_d(1)*z;
  48. w11d = R10*w00d + cross(R10*w00, z*theta_d(1)) + theta_dd(1)*z;
  49. v11d = R10*(cross(w00d, p10) + cross(w00, cross(w00, p10)) + v00d);
  50. vc11d = cross(w11d, pc11) + cross(w11, cross(w11, pc11)) + v11d;
  51. F11 = m1*vc11d;
  52. N11 = I1*w11d + cross(w11, I1*w11);
  53. % i = 1
  54. w22 = R21*w11 + theta_d(2)*z;
  55. w22d = R21*w11d + cross(R21*w11, z*theta_d(2)) + theta_dd(2)*z;
  56. v22d = R21*(cross(w11d, p21) + cross(w11, cross(w11, p21)) + v11d);
  57. vc22d = cross(w22d, pc22) + cross(w22, cross(w22, pc22)) + v22d;
  58. F22 = m2*vc22d;
  59. N22 = I2*w22d + cross(w22, I2*w22);
  60. % i = 2
  61. w33 = R32*w22 + theta_d(3)*z;
  62. w33d = R32*w22d + cross(R32*w22, z*theta_d(3)) + theta_dd(3)*z;
  63. v33d = R32*(cross(w22d, p32) + cross(w22, cross(w22, p32)) + v22d);
  64. vc33d = cross(w33d, pc33) + cross(w33, cross(w33, pc33)) + v33d;
  65. F33 = m3*vc33d;
  66. N33 = I3*w33d + cross(w33, I3*w33);
  67. % i= 3
  68. w44 = R43*w33 + theta_d(4)*z;
  69. w44d = R43*w33d + cross(R43*w33, z*theta_d(4)) + theta_dd(4)*z;
  70. v44d = R43*(cross(w33d, p43) + cross(w33, cross(w33, p43)) + v33d);
  71. vc44d = cross(w44d, pc44) + cross(w44, cross(w44, pc44)) + v44d;
  72. F44 = m4*vc44d;
  73. N44 = I4*w44d + cross(w44, I4*w44);
  74. % i = 4
  75. w55 = R54*w44 + theta_d(5)*z;
  76. w55d = R54*w44d + cross(R54*w44, z*theta_d(5)) + theta_dd(5)*z;
  77. v55d = R54*(cross(w44d, p54) + cross(w44, cross(w44, p54)) + v44d);
  78. vc55d = cross(w55d, pc55) + cross(w55, cross(w55, pc55)) + v55d;
  79. F55 = m5*vc55d;
  80. N55 = I5*w55d + cross(w55, I5*w55);
  81. % i = 5
  82. w66 = R65*w55 + theta_d(6)*z;
  83. w66d = R65*w55d + cross(R65*w55, z*theta_d(6)) + theta_dd(6)*z;
  84. v66d = R65*(cross(w55d, p65) + cross(w55, cross(w55, p65)) + v55d);
  85. vc66d = cross(w66d, pc66) + cross(w66, cross(w66, pc66)) + v66d;
  86. F66 = m6*vc66d;
  87. N66 = I6*w66d + cross(w66, I6*w66);
  88. % 外推: i: 6->1
  89. % 连杆6到连杆1向内迭代
  90. f77 = [0; 0; 0]; n77 = [0; 0; 0];
  91. % i = 6
  92. f66 = R67*f77 + F66;
  93. n66 = N66 + R67*n77 + cross(pc66, F66) + cross(p76, R67*f77);
  94. tau(6) = n66'*z;
  95. % i = 5
  96. f55 = R56*f66 + F55;
  97. n55 = N55 + R56*n66 + cross(pc55, F55) + cross(p65, R56*f66);
  98. tau(5) = n55'*z;
  99. % i = 4
  100. f44 = R45*f55 + F44;
  101. n44 = N44 + R45*n55 + cross(pc44, F44) + cross(p54, R45*f55);
  102. tau(4) = n44'*z;
  103. % i = 3
  104. f33 = R34*f44 + F33;
  105. n33 = N33 + R34*n44 + cross(pc33, F33) + cross(p43, R34*f44);
  106. tau(3) = n33'*z;
  107. % i = 2
  108. f22 = R23*f33 + F22;
  109. n22 = N22 + R23*n33 + cross(pc22, F22) + cross(p32, R23*f33);
  110. tau(2) = n22'*z;
  111. % i =1
  112. f11 = R12*f22 + F11;
  113. n11 = N11 + R12*n22 + cross(pc11, F11) + cross(p21, R12*f22);
  114. tau(1) = n11'*z;
  115. end

机器人工具箱验证

  1. clear;
  2. clc;
  3. %各关节质量
  4. m=[0 10 10 10 10 10];
  5. % DH parameters th d a alpha sigma
  6. L1=Link([ 0 0.1607 0 0 ],'modified');
  7. L2=Link([ 0 0 0 pi/2 ],'modified');L2.offset = pi/2;
  8. L3=Link([ 0 0 0.425 0 ],'modified');
  9. L4=Link([ 0 0.1133 0.393 0 ],'modified');L4.offset = -pi/2;
  10. L5=Link([ 0 0.099 0 -pi/2 ],'modified');
  11. L6=Link([ 0 0.0936 0 pi/2 ],'modified');
  12. thetaOffset=[0, pi/2, 0, -pi/2, 0, 0];
  13. L1.Jm = 0; L2.Jm = 0; L3.Jm = 0; L4.Jm = 0; L5.Jm = 0; L6.Jm = 0;
  14. L1.m = m(1); L2.m = m(2); L3.m = m(3);
  15. L4.m = m(4); L5.m = m(5); L6.m = m(6);
  16. % L1.r =
  17. L1.I = [0 0 0; 0 0 0; 0 0 3.7033];
  18. L2.I = [-2.782 0.086 -0.8413; 0.086 0 -0.1364; -0.8413 -0.1364 2.4741];
  19. L3.I = [-0.6741 -0.3658 0.0373; -0.3658 0 0.0377; 0.0373 0.0377 1.0102];
  20. L4.I = [0.4964 -0.3109 0.1658; -0.3109 0 0.2139; 0.1658 0.2139 -0.1213];
  21. L5.I = [0.0893 -0.0252 -0.0699; -0.0252 0 0.0325; -0.0699 0.0325 0.151];
  22. L6.I = [-0.2332 0.0203 -0.0601; 0.0203 0 0.0477; -0.0601 0.0477 0.0763];
  23. % 质心位置
  24. p10 = [0;0;0];p21 = [3.5349;0.0939;0]; p32 = [1.8417; 0.0746; 0];
  25. p43 = [-0.0475;0.1624;0]; p54 = [-0.0202;-0.0844;0];
  26. p65 = [0.0006; 0.03;0];
  27. L1.r = p10; L2.r = p21; L3.r = p32;
  28. L4.r = p43; L5.r = p54; L6.r = p65;
  29. robot = SerialLink([L1, L2, L3, L4, L5, L6]);
  30. robot.name='My_Robot';
  31. theta = [pi, 0, pi, 0, 0, pi]*pi/180;
  32. qd = [1 1 pi 1 1 1]; qdd = [1 1 1 pi 1 1];
  33. tau_N = myNewtonEuler(theta*180/pi, qd', qdd',m);
  34. tau = robot.rne(theta, qd, qdd, [0 0 9.8]);
  35. [tau_N;tau]

MATLAB计算结果

当theta = [pi, 0, pi, 0, 0, pi]*pi/180; qd = [1 1 pi 1 1 1]; qdd = [1 1 1 pi 1 1];时结果如下,第一行为自己的牛顿欧拉递推式的结果,第二行为机器人工具箱的结果。

当theta = [pi, 0, pi/2, 0, pi/2, pi]*pi/180; qd = [1 1 pi 1 pi/2 1]; qdd = [1 pi/2 1 pi 1 1];时结果如下。

总结

牛顿欧拉递推式公式相对简单,代码实现相对比较简单。实现时注意递推的初始状态以及各参数的物理意义。

跟新记录:        

        2024-07-19 感谢评论区的小伙伴指出的错误,由于博主的不严谨,外推公式的F与N有误现已修改。

  下一章: 4-3.六自由度机器人动力学-拉格朗日方程【附MATLAB代码】

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/从前慢现在也慢/article/detail/935142
推荐阅读
相关标签
  

闽ICP备14008679号