发六自由度机器手臂Inverse Kinematic 的Matlab代码

YFM

1. 首先由DH参数和各个关节角度q向量算出Transform Matrix  T(q) 并得到末端在世界坐标系中的坐标X(q)。

2. 给q加一个少量的扰动得到q_ist，算出此时的雅克布矩阵 J(q_ist)，Transform Matrix T(q_ist) 得到末端在世界坐标系中的坐标X(q_ist)。

3. 计算X(q)和X(q_ist)的差值 为err。 最后根据梯度下降发不断减小差值err使q_ist逐渐走向q。

Matlab的main函数片段：

1. %% q: Gelenkwinkel
   
2. %q=sym('q',[6,1]);
   
3. %assume(q, 'real');
   
4. 
   
5. clear;
   
6. q=[180.0;270.0;90.0;180.0;180.0;0.0];
   
7. %% Jacobian test
   
8. q3=[180.1;   269.9;   90.0;   180.0;   180.0;   0.0]; % Tn+1
   
9. q2=[179.9;   270.1;   90.0;   180.0;   180.0;   0.0]; % Tn
   
10. delta_q=[-0.2*pi/180;
    
11.     -0.2*pi/180; 0; 0; 0; 0]; %speed
    
12. %%offset generation
    
13. q_rand=4*rand(6,1)-2;
    
14. q_ist=q+q_rand;
    
15. %% Robot parameter
    
16. D = [0.2755; 0.41; 0.2073; 0.0741; 0.0741; 0.16];
    
17. e2 = 0.0098;
    
18. aa = deg2rad(30);
    
19. ca = cos(aa);
    
20. sa = sin(aa);
    
21. c2a = cos(2*aa);
    
22. s2a = sin(2*aa);
    
23. d4b = D(3)+sa/s2a*D(4);
    
24. d5b = sa/s2a*D(4)+sa/s2a*D(5);
    
25. d6b = sa/s2a*D(5)+D(6);
    
26. 
    
27. %% DH parameter: [theta d a alpha] . Winkel sind in RAD und Positionen in Meter
    
28. 
    
29. 
    
30. DH_Parameter = [ -q(1)*pi/180      D(1)  0    pi/2;...
    
31.                  (q(2)-90)*pi/180  0     D(2) pi;...
    
32.                  (q(3)+90)*pi/180  -e2   0    pi/2;...
    
33.                   q(4)*pi/180      -d4b  0    2*aa;...
    
34.                  (q(5)-180)*pi/180 -d5b  0    2*aa;...
    
35.                  (q(6)+90)*pi/180  -d6b  0    pi ];
    
36. 
    
37. %% Erzeuge Transformationsmatrizen
    
38. 
    
39. T1 = DH(DH_Parameter(1,:));
    
40. T2 = DH(DH_Parameter(2,:));
    
41. T3 = DH(DH_Parameter(3,:));
    
42. T4 = DH(DH_Parameter(4,:));
    
43. T5 = DH(DH_Parameter(5,:));
    
44. T6 = DH(DH_Parameter(6,:));
    
45. 
    
46. %debug=T6(3,4);
    
47. %% Transformation Hand-Roboter ( Tipp: Anschlie脽end mit matlabFunction in eine Funktion umwandeln f眉r Inverse Kinematik)
    
48. %T(q) = ...
    
49. T_q=T1*T2*T3*T4*T5*T6;
    
50. %% Jacbobi-Matrix
    
51. %J(q) = ...
    
52. %J=zeros(4,4,6);
    
53. %n=3;
    
54. %J(:,:,n)=T1;
    
55. A=zeros(4,4,6);
    
56. A(:,:,1)=T1;
    
57. A(:,:,2)=T2;
    
58. A(:,:,3)=T3;
    
59. A(:,:,4)=T4;
    
60. A(:,:,5)=T5;
    
61. A(:,:,6)=T6;
    
62. J_q=Jacobian_calu(A);
    
63. 
    
64. 
    
65. J_test=Jacobi_Matrix(q);
    
66. 
    
67. 
    
68. 
    
69. %% when velocity == diff(tx ty tz) Jacobian correct
    
70. diff=Transformations_Matrix(q3)-Transformations_Matrix(q2);
    
71. velocity= J_test*delta_q;
    
72. 
    
73. 
    
74. 
    
75. %matlabFunction(J,'File','Jacobi_Matrix.m');
    
76. %% Inverse Kinematik
    
77. 
    
78. % q_soll_Transp = IK_Transp(q_ist,[0.41;0.25951;0.084028]);
    
79. 
    
80. %q_soll_Pseudo = IK_Transp(q,[x y z]);
    
81. q_soll_Pseudo = IK_Pseudo(q_ist,[0.41;0.25951;0.084028]);
    
82. 
    
83. a=0;
    

复制代码

dory_m

学习，谢谢！！！

fuquan_dai

谢谢分享！！！！

dory_m

学习，谢谢！！！

Internationale

学习了，谢谢楼主

Garrys

这个看上去好复杂，谢谢分享

zf12862177

谢谢楼主分享