www.gusucode.com > matlab编程求解机器人的正逆解 采用遗传算法对机器人进行路径规划源码程序 > code/基本代码/Reverse_Solve.m

    
%逆向运动学的求解

Theta = [90,90,90,0,90,-90,0];          %目标输入关节角
CD_trans = cell(1,7);
Target = eye(4);                        %目标预期位姿
Theta_solve = cell(1,7);               %待求的实际关节角
d1 = 0.25;d2 = 0.25;d3 = 0.25;d4 = 0.25;
d5 = 0.25;d6 = 0.25;d7 = 0.25;
a3 = 2.5;a4 = 2.5;
k=1;

%旋转矩阵的求解
CD_trans{1} = CD_Trans1(0,0.25,90,90);
CD_trans{2} = CD_Trans1(0,0.25,80,90);
CD_trans{3} = CD_Trans1(2.5,0.25,90,0);
CD_trans{4} = CD_Trans1(2.5,0.25,0,0);
CD_trans{5} = CD_Trans1(0,0.25,90,90);
CD_trans{6} = CD_Trans1(0,0.25,-90,90);
CD_trans{7} = CD_Trans1(0,0.25,0,90);

for i =1:7
    Target = Target*CD_trans{i};
end



nx = Target(1,1);ny = Target(2,1);nz = Target(3,1);
sx = Target(1,2);sy = Target(2,2);sz = Target(3,2);
ax = Target(1,3);ay = Target(2,3);az = Target(3,3);
px = Target(1,4);py = Target(2,4);pz = Target(3,4);

% Theta_solve{1} = 90*pi/180;            %给定theta1的关节角
% 
% %theta2求解
% 
% a = pz-d1-d7*sz;b=(d7*sx-px)*cos(Theta_solve{1})+(d7*sy-py)*sin(Theta_solve{1});
% Phi = atan(a/b);phi = asin(-(d3+d4+d5)/sqrt(a^2+b^2));
% Theta_solve{2}(1) = -phi+Phi;
% Theta_solve{2}(2) = -pi+phi+Phi;
% 
% %theta6,7求解
% for i=1:2
%         theta1 = Theta_solve{1};
%         theta2 = Theta_solve{2}(i);
%         temp = sz*cos(theta2)-(sx*cos(theta1)+sy*sin(theta1))*sin(theta2);
%         Theta_solve{6}(2*i-1) = acos(temp);
%         Theta_solve{6}(2*i)=-acos(temp);
%         temp2 = (nx*cos(theta1)+ny*sin(theta1))*sin(theta2)-nz*cos(theta2);
%         temp3 = (ax*cos(theta1)+ay*sin(theta1))*sin(theta2)-az*cos(theta2);
%         Theta_solve{7}(i) = atan(temp3/temp2);
% end
% 
% %theta4求解
% for i=1:2
%     for j=1:2
%         
%     t1 = Theta_solve{1};
%     t2 = Theta_solve{2}(i);
%     t6 = Theta_solve{6}(j);
%     t7 = Theta_solve{7}(j);
%     tempA = d2 - d1 + py - d7*sy - d6*ny*sin(theta(7)) + ay*d6*cos(theta(7));
%     tempB = px*cos(theta(1) - t2) - pz*sin(theta(1) - t2) + (ax*d6*cos(theta(1) - t2 - t7))/2 + (d6*nz*cos(theta(1) - t2 - t7))/2 - (az*d6*sin(theta(1) - t2 - t7))/2 + (d6*nx*sin(theta(1) - t2 - t7))/2 - d7*sx*cos(theta(1) - t2) + d7*sz*sin(theta(1) - t2) + (ax*d6*cos(theta(1) - t2 + t7))/2 - (d6*nz*cos(theta(1) - t2 + t7))/2 - (az*d6*sin(theta(1) - t2 + t7))/2 - (d6*nx*sin(theta(1) - t2 + t7))/2;
%     temp = tempA^2+tempB^2-a3^2-a4^2;
%     Theta_solve{4}(k) = acos(temp/2/a3/a4);
%     t4=Theta_solve{4}(k);
%     
%     up = tempA*(a3+a4*cos(t4))-tempB*a4*sin(t4);
%     down = tempB*(a3+a4*cos(t4))+tempA*a4*sin(t4);
%     Theta_solve{3}(k) = atan(up/down);
%     t3 = Theta_solve{3}(k);
%     
%     s345 = sx*sin(t1)-sy*cos(t1)/sin(t6);
%     c345 = (ax*sin(t1)-ay*cos(t1))*cos(t7)-(nx*sin(t1)-ny*cos(t1))*sin(t7);
%     Theta_solve{5}(k) = atan(s345/c345)-(t3+t4);
%     
%     k=k+1;
%     
%     Theta_solve{4}(k) = -acos(temp/2/a3/a4);
%     t4=Theta_solve{4}(k);
%     
%     up = tempA*(a3+a4*cos(t4))-tempB*a4*sin(t4);
%     down = tempB*(a3+a4*cos(t4))+tempA*a4*sin(t4);
%     Theta_solve{3}(k) = atan(up/down);
%     t3 = Theta_solve{3}(k);
%     
%     Theta_solve{5}(k) = atan(s345/c345)-(t3+t4);
%     k=k+1;
%     end
% end
% 
%