2,Matlab仿真逆动力学画圈

注意

本文使用的方法并没有解决多解的问题，使用的是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