1.利用工具箱建模，供验证用
选用二轴平面机械臂，两个关节角分别是theta1和theta2. view(3); Lnk1 = Link([ 0 0 100 0 0]);%theta,d,a,alpha Lnk2 = Link([0 0 200 0 0]); Robot = SerialLink([Lnk1 Lnk2]); Robot.teach(); 运行结果如图：
2.使用DH法求正运动学
此处是传统正运动学的求法，利用变化矩阵相乘，得到T02，即可根据两个转角，求得机械臂末端位姿。
syms theta1 alpha1 a1 d1 theta2 alpha2 a2 d2;
A1=[cos(theta1),-sin(theta1)*cos(alpha1),sin(theta1)*sin(alpha1),a1*cos(theta1);...
sin(theta1),cos(theta1)*cos(alpha1),-cos(theta1)*sin(alpha1),a1*sin(theta1);...
0,sin(alpha1),cos(alpha1),d1;...
0,0,0,1];
A2=[cos(theta2),-sin(theta2)*cos(alpha2),sin(theta2)*sin(alpha2),a2*cos(theta2);...
sin(theta2),cos(theta2)*cos(alpha2),-cos(theta2)*sin(alpha2),a2*sin(theta2);...
0,sin(alpha2),cos(alpha2),d2;...
0,0,0,1];
a1=sym(100);
a2=sym(200);
d1=sym(0);
d2=sym(0);
alpha1=sym(0);
alpha2=sym(0);
T01 = (simplify(eval(A1)));
T12 = (simplify(eval(A2)));
T02 = ((simplify(eval(A1*A2))));
结果如图
T02即为最后所得的矩阵。后面利用雅可比矩阵求真逆运动学都是使用此处的结果来验证。
3.求雅可比正逆矩阵
雅可比矩阵的具体定义请大家参考网上，此处只给出具体实现 雅可比真逆运动学矩阵如下：
J = [diff(T02(1,4),theta1),diff(T02(1,4),theta2);diff(T02(2,4),theta1),diff(T02(2,4),theta2)];%雅可比矩阵，diff表示偏导数
nJ = inv(J);%逆雅可比矩阵
结果如下图：
4.利用雅可比矩阵求正运动学
稍微介绍下原理：假如要求theta1旋转3.14rad，theta2旋转1.57rad时候的机械臂末端位姿，那么，我们将这两个角度314等分，每次轴1转0.01rad，轴2转0.005rad，通过累加，得到机械臂的末端位姿。程序如下：
%%%正运动学
x=0;
y=0;
AAA = [300;0];%机械臂初始位置
for i = 1:314
TTT = [- 200*sin(x + y) - 100*sin(x), -200*sin(x + y);200*cos(x + y) + 100*cos(x), 200*cos(x + y)];
AAA =AAA + TTT*[0.01;0.005];%位置累加
x = x + 0.01;%
y = y + 0.005;
end
AAA; %机械臂最后位置
AAA就是最后的机械臂所在位置
5.雅可比逆矩阵求逆运动学（迭代解法）
此处就是上述正运动学推导的反演，但有个需要注意的问题是，此处的变量是末端速度，这个速度需要随着机械臂当前位姿的实时反馈而改变，而正运动学的变量是轴的速度，这个是可控的，可以不考虑他的误差。但逆运动学这恶一定要用实时反馈的数据做成闭环。
%%%逆运动学
x=roundn(0.00001,-15);%0的话是奇异位置
y=roundn(0.00001,-15);
qqq = [x;y];%机械臂关节初始位置
200*cos(qqq(1) + qqq(1)) + 100*cos(qqq(1))
200*sin(qqq(1) + qqq(2)) + 100*sin(qqq(1))
pos = [280, 20];%目标位置
posn = [200*cos(qqq(1) + qqq(1)) + 100*cos(qqq(1)),200*sin(qqq(1) + qqq(2)) + 100*sin(qqq(1))];
flag = 0;
while flag == 0
TTT = [(-cos(x + y)/(100*(cos(x + y)*sin(x) - sin(x + y)*cos(x)))),(-sin(x + y)/(100*(cos(x + y)*sin(x) - sin(x + y)*cos(x))));((2*cos(x + y) + cos(x))/(200*(cos(x + y)*sin(x) - sin(x + y)*cos(x)))), ((2*sin(x + y) + sin(x))/(200*(cos(x + y)*sin(x) - sin(x + y)*cos(x))))];
TTT
qqq = qqq + TTT * [(pos(1)-posn(1))/10000;(pos(2)-posn(2))/10000];
posn = [200*cos(qqq(1) + qqq(1)) + 100*cos(qqq(1)),200*sin(qqq(1) + qqq(2)) + 100*sin(qqq(1))];
qqq
x = qqq(1);
y = qqq(2);
200*cos(qqq(1) + qqq(1)) + 100*cos(qqq(1))
200*sin(qqq(1) + qqq(2)) + 100*sin(qqq(1))
if(abs(200*cos(qqq(1) + qqq(1)) + 100*cos(qqq(1))-pos(1))<1&&abs(200*sin(qqq(1) + qqq(2)) + 100*sin(qqq(1))-pos(2))<1)
flag = 1;
end
end
qqq;
我设置的误差是1，即机械臂末端位置距离希望位置小于1就结束迭代。 好了，以上就是用matlab来实现的雅可比矩阵求运动学。上述代码是可以转化为C或者C++的，因为没有借助其他库，当然，如果要移植到实际可用的地方，需要把雅可比矩阵和雅可比逆矩阵算出来然后再写。 （这里的雅可比矩阵没有考虑姿态噢，后面我准备系统研究下六轴串联机械臂利用雅可比矩阵求逆运动学迭代解，准备用C++写，matlab验证）
