原文：https://blog.csdn.net/lingchen2348/article/details/78561906
一、建系的原则
1）确定Z轴：找出关节轴线及关节转向采用右手定则确定Z；
2）确定原点：如果两相邻轴线Zi与Zi+1不相交，则公垂线与轴线i的交点为原点，注意平行时原点的选择应使偏置为零；如果相交则交点为原点，注意:如果重合则原点应使偏置为零；
3）确定X轴：两轴线不相交时，X与公垂线重合，指向从i到i+1；若两轴线相交，则X是两轴线所成平面的法线；
注意:如果两轴线重合，则X轴与轴线垂直且使其他连杆参数为零；
4）按右手定则确定Y ；
5）当第一个关节变量为零时，规定{0}与{1}重合，对于末端坐标系{n}，原点与X任选，希望坐标系{n}使杆参数尽量为零。
二、先建立中间坐标系{i}，后两端坐标系{0}、{n}
坐标系{0}和{n}的规定，Z0轴沿关节轴1的方向，关节变量1为零时， 坐标系{0}与{1}重合
关节1是旋转关节时， d0=0,
关节1是移动关节时， θ0=0
Zn轴沿关节轴n-1的方向，关节变量n-1为零时， 坐标系{n-1}与{n}重合
关节n-1是旋转关节时， dn=0,
三、控制思路
原文：https://blog.csdn.net/weixin_42018112/article/details/82382692
1、只求起点和终点对应的关节位置，直接对这两处关节位置进行线性插值求得关节运动轨迹——这样虽然省了很多计算量，但end effector不大可能沿着直线走。
2、把这条直线插入很多很多中间点，每一个点都求出对应的关节位置，然后控制每一个关节按着这一系列的关节位置走（就是我们前面说的解析/优化解法）。
3、还是把这条直线插入很多很多中间点，但如果点与点的间隔足够小、运动时间足够短，我们就可以在每一点用雅可比矩阵求逆来求得当前关节位置的变化——换个角度讲，我们也可以设定end effector沿这条直线的运动速度，用雅可比矩阵求逆求得关节速度，直接控制关节的运动速度而不是位置。