【机器人原理与实践（三）】六轴机械臂正逆解控制

本章针对机械臂的运动学进行建模分析。机械臂运动学模型反映的是个关节的角度与执行器末端位姿之间的映射关系。在工业过程中需要考虑的根据期望位姿推导关节角度（机械臂逆解），以及根据现有的关节角度推导末端执行器位姿（机械臂正解）。

3.1 空间转换矩阵的理解

3.1.1平移变换

3.1.2旋转变换

除了位置之外，还需要对刚体的指向，即姿态进行描述。而这也是旋转矩阵最本质的来源，即来源于 坐标系的旋转，这一部分理解不好或是理解不透彻，会导致此后面对各种形式的旋转时出现混乱，所以公式将进行详细的表述。

描述姿态的方式就是为刚体建立一个固连于刚体的标准正交坐标系，并由 其相对于参考坐标系的 单位向量 在参考坐标系中的描述 来表示。如上图所示，刚体的局部固连参考系为o-xyz，其局部固连参考系坐标系的单位向量为（x,y,z），而这局部坐标系的单位向量在参考坐标系 O−xyz 中的表示为：

可以进行进一步的整理，即上式等价于：

最后一个矩阵就是两个坐标系基之间的关系，即两个坐标系之间的转换关系，将这个矩阵定义为旋转矩阵

结合上面两种变换可以得到三维空间变换的一般形式：

用这个4X4的矩阵就可以描述四个机械臂末端相对于机械臂位置的变换，而当你将机械臂的起始坐标设置为（0,0,0）你就可以直接通过上述矩阵确定机械臂的末端位姿。(px,py,pz)将直接反应机械臂末端所直接对应的坐标。而n,o,a将直接反应机械臂末端坐标系相对于机械臂基座坐标系所发生的的旋转。

3.2 D-H参数法

D-H参数法是Denavit和Hartenberg在他们1955年提出的一种机器人的建模方法。该方法看似简单，但是通用性很强，虽然年代久远，但仍然是推导机器人运动学方程的标准方法。
机械臂通常有多个关节组成。D-H参数法在这些关节处以一定的规则建立参考，然后找到两两相邻的关节坐标系之间的转换关系，就是说在建立了坐标系的约束下，有限次的平移的旋转运动可以使用一个齐次矩阵表示这种转换关系。只要将相应的转换矩阵相乘，就可以得到任意两个关节坐标系的位置关系。在实际应用中，仅仅需要第一关节与最后一个关节的转换关系，就可在首关节确定的情况下确定末端执行器位姿。

以上图为例从关节n+1到关节n+2：
绕z轴转θ_(n+1)，使x_n轴和x_(n+1)轴相互平行。
延z轴平移d_(n+1)，使x_n轴和x_(n+1)轴共线。
延x轴平移a_(n+1)，使x_n轴和x_(n+1)轴原点重合。
z_n轴绕x_(n+1)轴旋转旋转α_(n+1)角度，使z_n z_(n+1)重合。
相邻坐标系之间的变换都可以遵循以上步骤：
绕z轴转θ_(n+1)：

延z轴平移d_(n+1)：

延x轴平移a_(n+1)：

z_n轴绕x_(n+1)轴旋转旋转α_(n+1)角度：

总变换矩阵为以上矩阵按顺序相乘

这就是任意相邻坐标系之间转换矩阵的通式。根据上述描述可以很容易的发现，使用D-H参数法建立机械臂运动学方程，关键在于要根据具体的机械臂建立合适点的关节坐标系。建立这样的一个关节坐标系需要四个关键参数，即θ,d,a,α, 这些参数就是机械臂的D-H参数，我们也可以根据这些参数使用Matlab进行相关的仿真。

3.3 建立机械臂模型

3.3.1 机械臂模型介绍

这是一个简单的六轴机械臂

图3.5 简化图片

经过测量以后得到机械臂的D-H参数表：

表格 3.1 D-H参数表

序号	θ	d	a	α
0-1	θ1	58	0	pi/2
1-2	θ2+pi/2	0	55	0
2-3	θ3	0	55	0
3-4	θ4	0	55	0

使用Matlab 机器人工具箱构建仿真
L(1) = Link([ 0, d(1), aa(1), alpha(1),0,offset(1) ]);
L(2) = Link([ 0, d(2), aa(2), alpha(2) ,0,offset(2) ]);
L(3) = Link([ 0, d(3), aa(3), alpha(3),0,offset(3) ]);
L(4) = Link([ 0, d(4), aa(4), alpha(4),0,offset(4) ]);

结合表3.1与公式2.4结合Matlab得到A1-A6所有的转换矩阵，与总的转换矩阵：

那么该机械臂的正运动学方程即为以上4个矩阵按顺序乘积，由于结构太过于复杂这里不直接列出了A04=A01A12A23*A34;

在得到旋转矩阵之后，根据以下公式就可求出末端执行器的位姿

这里有四个旋转角，在D-H参数已知的情况下，该运动学方程的计算结果就是机械臂末端相对机械臂底端坐标系的位姿矩阵。

3.3.2 使用Matlab进行示教仿真

3.4 机器人运动学

3.4.1 机器人正运动学

关于机械臂的正运动学问题，可以调用Robotics工具箱中的fkine函数进行求解。函数调用的一般形式为T=R.fkine{Q}，其中R表示机械臂模型，T为前向运动的正解，Q为机械臂四个关节的角度值。
取Q1=[pi/5,-pi/3.pi/2.-pi/4],带入fkine函数，或者直接带入公式1计算的矩阵的到一个齐次变换矩阵如下：
T =

	-1         0      	0    	-130
     0         0        -1         0
     0        -1         0       113
     0         0         0         1

可以得到仿真图像：

根据得到的图像结合实际可以确定，我们做出的正向运动学仿真大致是正确的。

3.4.2机器人逆运动学

[4]我们已经看到了前向运动学问题。逆运动学问题更有趣，其解决方案更有用。在位置层面，问题表述为：“考虑到机器人手部的理想位置，所有机器人关节的角度必须是什么？”。人类一直在解决这个问题，甚至没有想到它。当你早上吃麦片时，你只需伸手拿起勺子。你不会想，“我的肩膀需要这样做，我的肘部需要这样做，等等。” 下面我们将看看大多数机器人如何解决这个问题。我们将从一个非常简单的例子开始：
将机械臂的末端指向（-130,0,113）
对于机器人逆运动学的问题同样可以使用工具箱中的ikine函数解决这个问题。
可以在指定转化函数p的情况下输入,得到所有关节的角度向量：
q=bot.ikine(p,‘mask’,[1 1 1 1 0 0])
figure(2);
bot.plot(q)

p =

    -1         0         0      -130
     0         0        -1         0
     0        -1         0       113
     0         0         0         1

q =
0.0000 0.1111 1.0963 0.6225

当然只用MATLAB是没有用的我们必须使用可以在开发板上运行的语言重新实现这样的效果。
这样的逆解问题本质上其实是解方程的问题，我们可以将上面的问题转换为：
我们需要达到的效果是已知x,y,z(机械臂末端坐标)，我们需要计算出th1,th2,th3,th4
通过三个算式四个未知数显然是不太可能的，所以可以指定出th4也就是末端角度以后解算下面的方程。

eq1=75*sin(th4)*(cos(th1)*sin(th2)*sin(th3)-cos(th1)*cos(th2)*cos(th3)) -75*cos(th4)*(cos(th1)*cos(th2)*sin(th3) + cos(th1)*cos(th3)*sin(th2)) - 55*cos(th1)*sin(th2) - 55*cos(th1)*cos(th2)*sin(th3) - 55*cos(th1)*cos(th3)*sin(th2)==x;

eq2= 75*sin(th4)*(sin(th1)*sin(th2)*sin(th3) - cos(th2)*cos(th3)*sin(th1)) - 75*cos(th4)*(cos(th2)*sin(th1)*sin(th3) + cos(th3)*sin(th1)*sin(th2)) - 55*sin(th1)*sin(th2) - 55*cos(th2)*sin(th1)*sin(th3) - 55*cos(th3)*sin(th1)*sin(th2)==y;

eq3= 55*cos(th2) + 55*cos(th2)*cos(th3) - 55*sin(th2)*sin(th3) + 75*cos(th4)*(cos(th2)*cos(th3) - sin(th2)*sin(th3)) - 75*sin(th4)*(cos(th2)*sin(th3) + cos(th3)*sin(th2)) + 58==z;

[th1,th2,th3] = solve(eq1,eq2,eq3,th1,th2,th3);

基于这样的方程解算通过python实现：

def changeto(x,y,z,th4,pwm):
    th4=th4*3.14/180
    def solve_function(unsolved_value):
        th1,th2,th3=unsolved_value[0],unsolved_value[1],unsolved_value[2]
        return [
            75*sin(th4)*(cos(th1)*sin(th2)*sin(th3) - cos(th1)*cos(th2)*cos(th3)) - 75*cos(th4)*(cos(th1)*cos(th2)*sin(th3) + cos(th1)*cos(th3)*sin(th2)) - 55*cos(th1)*sin(th2) - 55*cos(th1)*cos(th2)*sin(th3) - 55*cos(th1)*cos(th3)*sin(th2)-x,
            75*sin(th4)*(sin(th1)*sin(th2)*sin(th3) - cos(th2)*cos(th3)*sin(th1)) - 75*cos(th4)*(cos(th2)*sin(th1)*sin(th3) + cos(th3)*sin(th1)*sin(th2)) - 55*sin(th1)*sin(th2) - 55*cos(th2)*sin(th1)*sin(th3) - 55*cos(th3)*sin(th1)*sin(th2)-y,
            55*cos(th2) + 55*cos(th2)*cos(th3) - 55*sin(th2)*sin(th3) + 75*cos(th4)*(cos(th2)*cos(th3) - sin(th2)*sin(th3)) - 75*sin(th4)*(cos(th2)*sin(th3) + cos(th3)*sin(th2)) + 58-z,
        ]
    solved=fsolve(solve_function,[0, 0, 0])
    solved=solved*180/3.14
print(solved)

3.5 机械臂运动过程分析

机械臂的运动过程，也就是舵机驱动关节转动的过程。过程如下：

（1）通过程序设置初始化各个脱机状态。
（2）各个舵机转动一定角度，使得长臂、断臂和机械臂末端都处于水平位置，整个过程中 号舵机旋转一定角度控制机械爪处于打开状态。
（3）2号舵机旋转一定角度，使长臂处于垂直状态，同时机械爪关闭。
（4）3号关节旋转一定角度，使断臂穿衣水平状态，通过机械爪打开。
（5）关闭机械爪。
（6）使机械臂回到初始状态

3.6 本章小结

本章首先进行连杆坐标，使用并分析了 D-H 模型，并使用 MatLab 软件进行了正反解的验证，分析了机械臂运动学正逆解，并给出了仿真图。通过Matlab计算出的算式直接应用到python中，给出了在python中的正逆解算法。