发六自由度机器手臂Inverse Kinematic 的Matlab代码
1. 首先由DH参数和各个关节角度q向量算出Transform MatrixT(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函数片段：
%% q: Gelenkwinkel
%q=sym('q',);
%assume(q, 'real');
clear;
q=;
%% Jacobian test
q3=; % Tn+1
q2=; % Tn
delta_q=[-0.2*pi/180;
-0.2*pi/180; 0; 0; 0; 0]; %speed
%%offset generation
q_rand=4*rand(6,1)-2;
q_ist=q+q_rand;
%% Robot parameter
D = ;
e2 = 0.0098;
aa = deg2rad(30);
ca = cos(aa);
sa = sin(aa);
c2a = cos(2*aa);
s2a = sin(2*aa);
d4b = D(3)+sa/s2a*D(4);
d5b = sa/s2a*D(4)+sa/s2a*D(5);
d6b = sa/s2a*D(5)+D(6);
%% DH parameter: . Winkel sind in RAD und Positionen in Meter
DH_Parameter = [ -q(1)*pi/180 D(1)0 pi/2;...
(q(2)-90)*pi/1800 D(2) pi;...
(q(3)+90)*pi/180-e2 0 pi/2;...
q(4)*pi/180 -d4b0 2*aa;...
(q(5)-180)*pi/180 -d5b0 2*aa;...
(q(6)+90)*pi/180-d6b0 pi ];
%% Erzeuge Transformationsmatrizen
T1 = DH(DH_Parameter(1,:));
T2 = DH(DH_Parameter(2,:));
T3 = DH(DH_Parameter(3,:));
T4 = DH(DH_Parameter(4,:));
T5 = DH(DH_Parameter(5,:));
T6 = DH(DH_Parameter(6,:));
%debug=T6(3,4);
%% Transformation Hand-Roboter ( Tipp: Anschlie脽end mit matlabFunction in eine Funktion umwandeln f眉r Inverse Kinematik)
%T(q) = ...
T_q=T1*T2*T3*T4*T5*T6;
%% Jacbobi-Matrix
%J(q) = ...
%J=zeros(4,4,6);
%n=3;
%J(:,:,n)=T1;
A=zeros(4,4,6);
A(:,:,1)=T1;
A(:,:,2)=T2;
A(:,:,3)=T3;
A(:,:,4)=T4;
A(:,:,5)=T5;
A(:,:,6)=T6;
J_q=Jacobian_calu(A);
J_test=Jacobi_Matrix(q);
%% when velocity == diff(tx ty tz) Jacobian correct
diff=Transformations_Matrix(q3)-Transformations_Matrix(q2);
velocity= J_test*delta_q;
%matlabFunction(J,'File','Jacobi_Matrix.m');
%% Inverse Kinematik
% q_soll_Transp = IK_Transp(q_ist,);
%q_soll_Pseudo = IK_Transp(q,);
q_soll_Pseudo = IK_Pseudo(q_ist,);
a=0;
学习，谢谢！！！ 谢谢分享！！！！ 学习，谢谢！！！ 学习了，谢谢楼主 这个看上去好复杂，谢谢分享 谢谢楼主分享
页:
[1]