我正在尝试解决以下优化问题:
我正在计算每个在30度之间离散的phi值的θ1,θ2和θ3。到150度
function thetas = inverse_kinematics_1(l1,l2,l3,phi)
x = 100;
y = 0;
x1 = x - (l3*cos(phi));
y1 = y - (l3*sin(phi));
a = sqrt(x1^2 + y1^2);
y2 = -y1/a;
x2 = -x1/a;
gamma = atan2(y2,x2);
c = (- x1^2 - y1^2 - l1^2 + l2^2)/(2*l1*a);
d = acos(c);
theta1 = gamma + d;
if theta1 < 0
theta1 = theta1 + 2*pi;
end
e = (y1 - l1*sin(theta1))/l2;
f = (x1 - l1*cos(theta1))/l2;
theta2 = atan2(e,f) - theta1;
if theta2 < 0
theta2 = theta2 + 2*pi;
end
theta3 = (phi)- (theta1 + theta2);
if theta3 < 0
theta3 = theta3 + 2*pi;
end
thetas = [theta1,theta2,theta3].*180/pi;
end
如何在这种情况下编写约束?