Matlab 由终位置坐标推算二自由度机械臂运动轨迹

偷偷拿来记录一下萌新的cs路——day8 Matlab仿真numerical solution for point to point control（综合性太强了，研究两个小时还没有完全弄明白，先记录下来代码

背景和参数

对终位置坐标进行泰勒展开

 

% numerical solution for point to point control

clear

% robot link lengths
l1 = 2; l2 = 1.5;

% input initial position
x0 = 1; y0 = 1;

% input final position
xf = 2; yf = 2;

% solve the initial joint angles (closed form) 简单计算 见原理图
q2 = acos((x0^2+y0^2-l1^2-l2^2)/(2*l1*l2));
theta = atan2(y0,x0); % 四象限反正切 若theta = tan(x0,y0)则无正负仅输出角度绝对值
phi = asin(l2*sin(pi-q2)/sqrt(x0^2+y0^2));
%phi = acos((x0^2+y0^2+l1^2-l2^2)/(2*l1*sqrt(x0^2+y0^2)));
%q1 = theta-phi; % solution 1
q1 = theta+phi; % solution 2
q2 = -q2;
q10 = q1; q20 = q2;

% plot
pp(1,:) = [0,0]; 
pp(2,:) = [l1*cos(q1),l1*sin(q1)]; 
pp(3,:) = [l2*cos(q1+q2),l2*sin(q1+q2)]; 
pp(3,:) = pp(2,:) + pp(3,:); 
plot(pp(:,1),pp(:,2),'ob-'); % 画两条黑线
axis([-1,3,-1,3]);
grid on % 网格线
hold on

% define initial angles
q = [q10,q20]'; % 定义矩阵-初始角度

% define initial tip error
p0 = [x0,y0]'; % 【】‘：转置矩阵
pf = [xf,yf]';
delp = pf-p0;

% define threshold 越高，越精确
%er = 10e-1; 
%er = 10e-2;
er = 10e-3;

% counter for iteration 计数器
k = 0;

% solve the final joint angles
while norm(delp,2) > er % 返回delp最大奇异值
    % calculate Jacobian
    ja(1,1) = -l1*sin(q(1))-l2*sin(q(1)+q(2));
    ja(1,2) = -l2*sin(q(1)+q(2));
    ja(2,1) = l1*cos(q(1))+l2*cos(q(1)+q(2));
    ja(2,2) = l2*cos(q(1)+q(2));
    delq = inv(ja)*delp; % inv：求Jacobian逆矩阵
    q = q+delq; % 将Jacobian加到q
    % tip position
    x = l1*cos(q(1))+l2*cos(q(2)+q(1));
    y = l1*sin(q(1))+l2*sin(q(2)+q(1));
    p = [x,y]';
    delp = pf-p;
    k = k+1; %计数器加一
end

%slove the true final joint angles(closed form)
q2 = acos((xf^2+yf^2-l1^2-l2^2)/(2*l1*l2));
theta = atan2(yf,xf);
%phi = asin((l2*sin(pi-q2)/sqrt(xf^2+yf^2)));
phi = acos((xf^2+yf^2+l1^2-l2^2)/(2*l1*sqrt(xf^2+yf^2)));
%q1 = theta-phi; % solution 1
q2 = theta+phi; % solution 2
q2 = -q2;

pp(1,:) = [0,0];
pp(2,:) = [l1*cos(q1),l1*sin(q1)];
pp(3,:) = [l2*cos(q1+q2),l2*sin(q1+q2)];
pp(3,:) = pp(2,:) + pp(3,:);
plot(pp(:,1),pp(:,2),'ok-');
axis([-1,3,-1,3]);

% get the numerical final angles
q1f = q(1); q2f = q(2);

% intermediate points
m = 10; % increments
dq1 = (q1f-q10)/m; % differential
dq2 = (q2f-q20)/m;

for j=1:m % j从1取到m
    q1 = q10+j*dq1;
    q2 = q20+j*dq2;
    pp(1,:) = [0,0];
    pp(2,:) = [l1*cos(q1),l1*sin(q1)];
    pp(3,:) = [l2*cos(q1+q2),l2*sin(q1+q2)];
    pp(3,:) = pp(2,:)+pp(3,:);
    pause(0.1)
    
    plot(pp(:,1),pp(:,2),'or-')
    axis([-1,3,-1,3]);
end

dd = num2str(norm(delp)); % num2str：数字转化为字符数组
kk = num2str(norm(k));
text(1.7,2.5,'Tip error = '); % text(x,y,txt)坐标系中定位文本
text(2.3,2.5,dd);
text(1.7,2.3,'Iteration = ');
text(2.3,2.3,kk);

运行结果：（er=10e-3情况）

（er=10e-1情况）

有一个问题至今没有解决，为什么末端坐标不在（2，2）呢？恳请大佬们帮忙解答

以后学了更多知识还会回来更新的！有误之处恳请大佬们指正，提前感谢！