机器人学，机器视觉与控制学习笔记——第三章、机械臂运动学
绪 论：机器人库的安装：https://blog.csdn.net/yukinoai/article/details/78946500
第二章：位置与姿态描述：https://blog.csdn.net/yukinoai/article/details/78955761
运动学是力学的一个分支，他在不考虑外力和质量的前提下研究一个物体。机械臂是由一组成为连杆的刚体组成的，连杆之间由关节连接。
分析机械臂运动学的第一步是根据机械臂的构造建立连杆坐标系，连杆坐标系最常用的方法就是D-H法。
D-H法
详细请参考https://blog.csdn.net/yukinoai/article/details/90573008
首先根据下图确定每个关节坐标系的方向，对于转动关节，zi-1轴在转轴上，xi-1轴在z-1i轴和zi轴的公垂线上，方向由zi-1指向zi，Y轴方向自然确定。
- ai-1是zi-1到zi沿xi-1的距离
- αi-1是zi-1到zi绕xi-1的角度
- di是xi-1到xi沿zi的距离
- θi是xi-1到xi绕zi的角度
所以从关节i-1到关节i的坐标系变换可以看成，坐标系{i-1}绕xi-1旋转αi-1，再沿沿xi-1移动ai-1，再绕zi旋转θi，最后沿zi移动di变换为坐标系{i}。
把每个关节都如上处理就可以列出D-H表
把表中数据带入连杆变换矩阵公式中就可以得到每个连杆的变换矩阵
最后把每个关节的变换矩阵相乘就能得到总变换矩阵
例如下面这个机械臂，他由5个关节，从下面开始分别为转动，移动，移动，转动，转动。
连杆坐标系如下
根据连杆坐标系可列出D-H表
| 构建i |
αi-1 |
ai-1 |
di |
θi |
变量范围 |
| 1 |
0° |
0 |
0 |
θ1 |
（0~360°） |
| 2 |
0° |
0 |
d2 |
180° |
（30~270mm） |
| 3 |
-90° |
a2（70mm） |
d3 |
90° |
（190~340mm） |
| 4 |
-90° |
a3（68mm） |
0 |
θ4(-90°) |
（0~-180°） |
| 5 |
90° |
0 |
d5（105mm） |
θ5(90°) |
（0~360°） |
本书中的工具箱提供了很好的可视化工具
其中几个常用的函数有
L(1) = Link([0 0 1 0]);%根据D-H法定义一个连杆
L(2) = Link([0 0 1 0]);
two_link = SerialLink(L, 'name', 'two link');%这个函数可以把定义好的连杆组成一个机械臂
%所以输入这个值会出现他的D-H表
>> two_link
two_link =
two link (2 axis, RR, stdDH)
+---+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha |
+---+-----------+-----------+-----------+-----------+
| 1| q1| 0| 1| 0|
| 2| q2| 0| 1| 0|
+---+-----------+-----------+-----------+-----------+工具箱中有已建好的PUMA560机械臂
mdl_puma560%调用机械臂参数
>> p560%出现D-H表
p560 =
Puma 560 (6 axis, RRRRRR, stdDH)
Unimation; viscous friction; params of 8/95;
+---+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha |
+---+-----------+-----------+-----------+-----------+
| 1| q1| 0| 0| 1.571|
| 2| q2| 0| 0.4318| 0|
| 3| q3| 0.15| 0.0203| -1.571|
| 4| q4| 0.4318| 0| 1.571|
| 5| q5| 0| 0| -1.571|
| 6| q6| 0| 0| 0|
+---+-----------+-----------+-----------+-----------+
内部已定义了四个特殊位姿
qz (0, 0, 0, 0, 0, 0)
qr (0, π /2, −π/2, 0, 0, 0)
qs (0, 0, −π /2, 0, 0, 0)
qn (0, π/4 , −π, 0,π/4 , 0)
p560.plot(qz)%对应的输入这个函数就能画出机械臂的构型对应构型如下
T = p560.fkine(qn)%可以求出这个构型的变换矩阵
qi = p560.ikine6s(T)%运动学反解
从影子可以明显发现，末端的位姿一致，但机械臂的构型不同，说明同一位姿对应两个构型，运动学反解不是唯一的。
如果能给出末端的轨迹，可以通过工具箱观察PUMA560的运动过程
>> T1 = transl(0.4, 0.2, 0) * trotx(pi);%初始变换矩阵
>> T2 = transl(0.4, -0.2, 0) * trotx(pi/2);%最终变换矩阵
>> q1 = p560.ikine6s(T1);%求广义变量
>> q2 = p560.ikine6s(T2);
>> t = [0:0.05:2]';%两秒内40步完成
>> q = jtraj(q1, q2, t);%通过插值函数得到轨迹
p560.plot(q)%生成动画
qplot(t, q);%给出每个关节的运动曲线