www.gusucode.com > matlab编程求解机器人的正逆解 采用遗传算法对机器人进行路径规划源码程序 > code/基本代码/solve.m
function theta = solve(Target) theta = zeros(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; 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(1) = pi/2; %给定theta1的关节角 %theta2求解 a = -(pz-d1-d7*sz);b=-((d7*sx-px)*cos(theta(1))+(d7*sy-py)*sin(theta(1))); Phi = atan(a/b); if a<0 Phi = Phi+pi; end phi = asin((d3+d4+d5)/sqrt(a^2+b^2)); theta(2) = phi-Phi; temp = sz*cos(theta(2))-(sx*cos(theta(1))+sy*sin(theta(1)))*sin(theta(2)); theta(6) = -acos(temp); temp2 = (nx*cos(theta(1))+ny*sin(theta(1)))*sin(theta(2))-nz*cos(theta(2))/sin(theta(6)); temp3 = (ax*cos(theta(1))+ay*sin(theta(1)))*sin(theta(2))-az*cos(theta(2))/sin(theta(6)); theta(7) = atan(temp3/temp2); tempA = px*sin(theta(1)) - py*cos(theta(1)) - d2 + d7*sy*cos(theta(1)) - d7*sx*sin(theta(1)) - d6*nx*sin(theta(1))*sin(theta(7)) - ay*d6*cos(theta(1))*cos(theta(7)) + ax*d6*cos(theta(7))*sin(theta(1)) + d6*ny*cos(theta(1))*sin(theta(7)); tempB = pz*sin(theta(2)) - d1*sin(theta(2)) - d7*sz*sin(theta(2)) + px*cos(theta(1))*cos(theta(2)) + py*cos(theta(2))*sin(theta(1)) - d6*nz*sin(theta(2))*sin(theta(7)) + az*d6*cos(theta(7))*sin(theta(2)) - d7*sx*cos(theta(1))*cos(theta(2)) - d7*sy*cos(theta(2))*sin(theta(1)) + ax*d6*cos(theta(1))*cos(theta(2))*cos(theta(7)) + ay*d6*cos(theta(2))*cos(theta(7))*sin(theta(1)) - d6*nx*cos(theta(1))*cos(theta(2))*sin(theta(7)) - d6*ny*cos(theta(2))*sin(theta(1))*sin(theta(7)); temp = tempA^2+tempB^2-a3^2-a4^2; theta(4) = acos(temp/2/a3/a4); up = tempA*(a3+a4*cos(theta(4)))-tempB*a4*sin(theta(4)); down = tempB*(a3+a4*cos(theta(4)))+tempA*a4*sin(theta(4)); theta(3) = atan(up/down); if down<0 theta(3) = theta(3)+pi; end s345 = sx*sin(theta(1))-sy*cos(theta(1))/sin(theta(6)); c345 = (ax*sin(theta(1))-ay*cos(theta(1)))*cos(theta(7))-(nx*sin(theta(1))-ny*cos(theta(1)))*sin(theta(7)); theta(5) = atan(s345/c345)-(-theta(3)+theta(4));