注意
本文使用的方法并没有解决多解的问题,使用的是fsolve函数,使得结果只有一个解
代码
定义连杆参数和圆
使用俩连杆作为“画笔”来画圆
l1 = 1.2; l2 = 1;
phi = linspace(0,2*pi,100);
xRef = 1 + 0.6*cos(phi); yRef = 1 + 0.6*sin(phi);
求出对应末端执行点的关节变量
%%%% 猜测解的位置 %%%%
theta = [0.1 0.3];
thetaAll = zeros(length(phi),2);
%%%% 计算每个点对应的theta向量 %%%%
for i = 1:length(phi)
para = [l1 l2 xRef(i) yRef(i)];
options = optimoptions('fsolve','Display','off','MaxIterations',100,'MaxFunctionEvaluations',300);
[result] = fsolve(@fun,theta,options,para);
thetaAll(i,1) = result(1);
thetaAll(i,2) = result(2);
function F=fun(theta,para)
l1 = para(1); l2 = para(2);
xRef = para(3); yRef = para(4);
theta1 = theta(1); theta2 = theta(2);
qx = l1*cos(theta1) + l2*cos(theta2+theta1);
qy = l1*sin(theta1) + l2*sin(theta2+theta1);
F = [qx-xRef;qy-yRef];
end
画图仿真
for i = 1:length(phi)
c1 = cos(thetaAll(i,1)); s1 = sin(thetaAll(i,1));
c2 = cos(thetaAll(i,2)); s2 = sin(thetaAll(i,2));
R01 = [c1 -s1;s1 c1]; R12 = [c2 -s2;s2 c2];
L01 = [0;0]; L12 = [l1;0];
H01 = [R01 L01;0 0 1];
H12 = [R12 L12;0 0 1];
po1 = [l1;0;1];po2 = [l2;0;1];
p1Base = H01*po1; p2Base = H01*H12*po2;
p1Base = p1Base(1:2);
p2Base = p2Base(1:2);
plot(p2Base(1),p2Base(2),'ko','MarkerSize',5,'MarkerEdgeColor','k','MarkerFaceColor','k');
hold on;
h1 = line([0 p1Base(1)],[0 p1Base(2)],'LineWidth',5,'Color','r');
h2 = line([p1Base(1) p2Base(1)],[p1Base(2) p2Base(2)],'LineWidth',5,'Color','b');
axis('equal');
xlabel('x');ylabel('y');
axis([-2 2 -2 2]);
pause(0.1);
delete(h1);
delete(h2);
end