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));