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 % %