二连杆及三连杆的正逆运动学及雅可比
二连杆和三连杆是进行机器人控制中常用的模型，虽然机械臂大多是六轴，但是为了减轻求解复杂度，方便对算法的验证，仍会将六轴机械臂简化为两轴或者三轴的形式。
二连杆正逆运动学及雅可比
正运动学
function [x,y] = kin2(q1,q2,A)
a = A(1);
b = A(2);
x = acos(q1)+bcos(q1+q2);
y = asin(q1)+bsin(q1+q2);
end
微分运动学（雅可比）
function [J2] = dKin2(q1,q2,A)
a = A(1);
b = A(2);
J2 = zeros(2)
J2(1,1) = -asin(q1)-bsin(q1+q2);
J2(1,2) = -bsin(q1+q2);
J2(2,1) = acos(q1)+bcos(q1+q2);
J2(2,2) = bcos(q1+q2);
end
逆运动学
如图，由第二连杆末端向x轴做垂线，并连接第二连杆和原点，形成一个边长为 的直角三角形，记该直角三角形左下角为 ,有
求解q1
取平方并相加
对x和y进行替换并移项，得到
应用三角公式得到
得到
公式1
求解q2
求解q1+q2的过程和求解q1是对称的，可以得到
得到
公式2
但是这里并不是有四组解。由于
因此，公式1和公式2中的±号始终是相反的，仅有两组解。下面代码仅考虑q2>0
function [q1,q2] = iKin2(x,y,A)
a = A(1);
b = A(2);
q0 = atan2(y./x);
a0 = (x.2+y.2).^0.5;
temp = (x.2+y.2+a.2-b.2)./(2*a0.*a);
q1 = -acos(temp)+q0;
temp = (x.2+y.2-a.2+b.2)./(2*a0.*b);
q2 = acos(temp)+q0-q1;
end
三连杆正逆运动学及雅可比
正运动学
function [x,y,theta] = kin3(q1,q2,q3,A)
a = A(1);
b = A(2);
c = A(3);
x = acos(q1)+bcos(q1+q2)+ccos(q1+q2+q3);
y = asin(q1)+bsin(q1+q2)+csin(q1+q2+q3);
theta = q1+q2+q3;
end
微分运动学
function [J3] = dKin3(q1,q2,q3,A)
a = A(1);
b = A(2);
c = A(3);
J3 = zeros(3)
J3(1,1) = -asin(q1)-bsin(q1+q2)-csin(q1+q2+q3);
J3(1,2) = -bsin(q1+q2)-csin(q1+q2+q3);
J3(1,3) = -csin(q1+q2+q3);
J3(2,1) = acos(q1)+bcos(q1+q2)+ccos(q1+q2+q3);
J3(2,2) = bcos(q1+q2)+ccos(q1+q2+q3);
J3(2,3) = ccos(q1+q2+q3);
J3(3,3) = 1;
J3(3,3) = 1;
J3(3,3) = 1;
end
逆运动学
三连杆的逆运动学可以化为二连杆的逆运动学进行求解。根据正运动学得到
转变为一个二连杆运动学进行求解即可。仅考虑q2>0
function [q1,q2,q3] = iKin3(x,y,theta,A)
a = A(1);
b = A(2);
c = A(3);
xx = x-ccos(theta);
yy = y-csin(theta);
AA = [a,b];
[q1,q2] = iKin2(xx,yy,AA);
q3 = theta - q1 -q2;
end