机械臂速成小指南（九）：正运动学分析

指南目录：

机械臂速成小指南（零）：指南主要内容及分析方法

机械臂速成小指南（一）：机械臂发展概况

机械臂速成小指南（二）：机械臂的应用

机械臂速成小指南（三）：机械臂的机械结构

机械臂速成小指南（四）：机械臂关键部件之减速机

机械臂速成小指南（五）：末端执行器

机械臂速成小指南（六）：步进电机驱动器

机械臂速成小指南（七）：机械臂位姿的描述方法

机械臂速成小指南（八）：运动学建模（标准DH法）

机械臂速成小指南（九）：正运动学分析​​​​​​​

        在第八章中，我们已经得到了所使用机械臂的DH参数，如下表所示：

DH参数及关节变量的范围

连杆编号	​	​	​	​	关节变量范围
1	64.2	-90	169.77	θ1	(-170,+170)
2	305	0	0	θ2	(-132,0)
3	0	90	0	θ3	(1,141)
4	0	-90	-222.63	θ4	(-165,+ 165)
5	0	90	0	θ5	(-105,+105)
6	0	0	-36.25	θ6	(-155,+155)

        在确定机械臂的DH参数后，使用齐次变换矩阵描述各坐标系之间的旋转、平移关系，并利用齐次变换矩阵构建机械臂的正运动学方程，以此对六自由度机械臂展开正运动学分析。

一、构建机械臂的正运动学方程

        从坐标系i到坐标系i-1的齐次变换矩阵 是一个仅与关节变量有关的函数，本文记为 。下面列出推导 的过程，以坐标系​​​​​​​i与坐标系i-1为例，坐标变换坐标系的旋转、平移过程如下图所示。

        首先，设一坐标系k与坐标系i-1排列一致，然后将坐标系k沿轴zi-1 平移di 并绕轴zi-1 旋转θi。此时坐标系k已与坐标系 排列一致，该过程可用齐次变换矩阵描述为

式中 表示cos(n) ， 表示sin(n)，计算时需考虑到两个坐标系的x 轴之间初始的夹角θ。

        接着，将坐标系k沿轴xi' 平移 并绕其旋转，这使得坐标系与坐标系i排列一致，该过程可用齐次变换矩阵描述为

        从坐标系i 到坐标系i-1 的齐次变换矩阵可通过右乘单一变换矩阵 及 得出，为

        根据上式可得出表示各坐标系间相对位置和方向的齐次变换矩阵，将其看作各关节的齐次变换矩阵，计算结果如下表所示。

关节编号	齐次变换矩阵
1
2
3
4
5
6

        由于坐标系0与基坐标系重合，又由于坐标系6即为工具坐标系，所以有

        则用于表示工具坐标系相对于基坐标系的正运动学方程可以由上表中6个齐次变换矩阵右乘得出，为

        ​​​​前三个关节组成拟人臂，则其正运动学方程表示为

其中， 及 分别表示sin⁡(qj+…+qk)和cos⁡(qj+…+qk) 。

        后三个关节组成球形腕，同理可得，球型腕的正运动学方程可表示为

        进而可以得出机械臂的正运动学方程为：

        其中，为关节变量组成的向量，向量q的空间称为关节空间。

        经计算，末端执行器的位置表示为

        同理，可得到末端执行器的姿态表示为

最终，末端执行器的RPY角可表示为关于关节变量的超越函数，即

        至此，机械臂的正运动学公式求解完毕，当我们将关节变量代入公式即可得到末端执行器的位姿。

二、正运动学方程求解的编程实现

        由于正运动学方程运算量较大，为提高运算速度，我们选择使用上位机完成运算。使用Python编写机械臂正运动方程求解函数CalcFwdKin()，输入参数为6个关节变量，输出为末端执行器的位置与姿态。

        机械臂的正运动学方程是关节变量的超越函数，求解过程中存在大量的三角函数计算及矩阵运算，因此我们引入Python自带的math标准库，该模块提供了许多对浮点数的数学运算函数。

        在求解正运动学方程的过程中使用math.sin()、math.cos()及math.atan2()完成三角函数的计算；使用math.sqrt()求取平方根；通过math.degrees()及math.radians()完成弧度与角度之间的转换。

        要使用 math 函数必须先导入：

import math

        效果如下图所示：

  ############## DH TABLE ################
  ############## DH 参数表 ###############
  C13 = C4
  C14 = C5
  C15 = C6
  C16 = C7
  C17 = C8
  C18 = C9
  D13 = math.radians(DHr1)
  D14 = math.radians(DHr2)
  D15 = math.radians(DHr3)
  D16 = math.radians(DHr4)
  D17 = math.radians(DHr5)
  D18 = math.radians(DHr6)
  E13 = DHd1
  E14 = DHd2
  E15 = DHd3
  E16 = DHd4
  E17 = DHd5
  E18 = DHd6
  F13 = DHa1
  F14 = DHa2
  F15 = DHa3
  F16 = DHa4
  F17 = DHa5
  F18 = DHa6
  ## WORK FRAME INPUT
  H13 = float(UFxEntryField.get()) 
  H14 = float(UFyEntryField.get())  
  H15 = float(UFzEntryField.get())
  H16 = float(UFrxEntryField.get()) 
  H17 = float(UFryEntryField.get()) 
  H18 = float(UFrzEntryField.get()) 
  ## TOOL FRAME INPUT
  J13 = float(TFxEntryField.get()) 
  J14 = float(TFyEntryField.get())  
  J15 = float(TFzEntryField.get())
  J16 = float(TFrxEntryField.get()) 
  J17 = float(TFryEntryField.get()) 
  J18 = float(TFrzEntryField.get())
  ## WORK FRAME TABLE
  B21 = math.cos(math.radians(H18))*math.cos(math.radians(H17))
  B22 = math.sin(math.radians(H18))*math.cos(math.radians(H17))
  B23 = -math.sin(math.radians(H18))
  B24 = 0
  C21 = -math.sin(math.radians(H18))*math.cos(math.radians(H16))+math.cos(math.radians(H18))*math.sin(math.radians(H17))*math.sin(math.radians(H16))
  C22 = math.cos(math.radians(H18))*math.cos(math.radians(H16))+math.sin(math.radians(H18))*math.sin(math.radians(H17))*math.sin(math.radians(H16))
  C23 = math.cos(math.radians(H17))*math.sin(math.radians(H16))
  C24 = 0
  D21 = math.sin(math.radians(H18))*math.sin(math.radians(H16))+math.cos(math.radians(H18))*math.sin(math.radians(H17))*math.cos(math.radians(H16))
  D22 = -math.cos(math.radians(H18))*math.sin(math.radians(H16))+math.sin(math.radians(H18))*math.sin(math.radians(H17))*math.cos(math.radians(H16))
  D23 = math.cos(math.radians(H17))*math.cos(math.radians(H16))
  D24 = 0
  E21 = H13
  E22 = H14
  E23 = H15
  E24 = 1 
  ## J1 FRAME
  B27 = math.cos(C13)
  B28 = math.sin(C13)
  B29 = 0
  B30 = 0
  C27 = -math.sin(C13)*math.cos(D13)
  C28 = math.cos(C13)*math.cos(D13)
  C29 = math.sin(D13)
  C30 = 0  
  D27 = math.sin(C13)*math.sin(D13)
  D28 = -math.cos(C13)*math.sin(D13)
  D29 = math.cos(D13)
  D30 = 0 
  E27 = F13*math.cos(C13)
  E28 = F13*math.sin(C13)
  E29 = E13
  E30 = 1
  ## J2 FRAME
  B33 = math.cos(C14)
  B34 = math.sin(C14)
  B35 = 0
  B36 = 0
  C33 = -math.sin(C14)*math.cos(D14)
  C34 = math.cos(C14)*math.cos(D14)
  C35 = math.sin(D14)
  C36 = 0
  D33 = math.sin(C14)*math.sin(D14)
  D34 = -math.cos(C14)*math.sin(D14)
  D35 = math.cos(D14)
  D36 = 0
  E33 = F14*math.cos(C14)
  E34 = F14*math.sin(C14)
  E35 = E14
  E36 = 1
  ## J3 FRAME 
  B39 = math.cos(C15)
  B40 = math.sin(C15)
  B41 = 0
  B42 = 0
  C39 = -math.sin(C15)*math.cos(D15)
  C40 = math.cos(C15)*math.cos(D15)
  C41 = math.sin(D15)
  C42 = 0
  D39 = math.sin(C15)*math.sin(D15)
  D40 = -math.cos(C15)*math.sin(D15)
  D41 = math.cos(D15)
  D42 = 0
  E39 = F15*math.cos(C15)
  E40 = F15*math.sin(C15)
  E41 = 0
  E42 = 1
  ## J4 FRAME 
  B45 = math.cos(C16)
  B46 = math.sin(C16)
  B47 = 0
  B48 = 0
  C45 = -math.sin(C16)*math.cos(D16)
  C46 = math.cos(C16)*math.cos(D16)
  C47 = math.sin(D16)
  C48 = 0
  D45 = math.sin(C16)*math.sin(D16)
  D46 = -math.cos(C16)*math.sin(D16)
  D47 = math.cos(D16)
  D48 = 0
  E45 = F16*math.cos(C16)
  E46 = F16*math.sin(C16)
  E47 = E16
  E48 = 1
  ## J5 FRAME 
  B51 = math.cos(C17)
  B52 = math.sin(C17)
  B53 = 0
  B54 = 0
  C51 = -math.sin(C17)*math.cos(D17)
  C52 = math.cos(C17)*math.cos(D17)
  C53 = math.sin(D17)
  C54 = 0 
  D51 = math.sin(C17)*math.sin(D17)
  D52 = -math.cos(C17)*math.sin(D17)
  D53 = math.cos(D17)
  D54 = 0
  E51 = F17*math.cos(C17)
  E52 = F17*math.sin(C17)
  E53 = E17
  E54 = 1
  ## J6 FRAME
  B57 = math.cos(C18)
  B58 = math.sin(C18)
  B59 = 0
  B60 = 0
  C57 = -math.sin(C18)*math.cos(D18)
  C58 = math.cos(C18)*math.cos(D18)
  C59 = math.sin(D18)
  C60 = 0
  D57 = math.sin(C18)*math.sin(D18)
  D58 = -math.cos(C18)*math.sin(D18)
  D59 = math.cos(D18)
  D60 = 0
  E57 = F18*math.cos(C18)
  E58 = F18*math.sin(C18)
  E59 = E18
  E60 = 1
  ###################### TOOL FRAME ###########################
  ###################### 工具坐标系 ############################
  B63 = math.cos(math.radians(J18))*math.cos(math.radians(J17))
  B64 = math.sin(math.radians(J18))*math.cos(math.radians(J17))
  B65 = -math.sin(math.radians(J18))
  B66 = 0
  C63 = -math.sin(math.radians(J18))*math.cos(math.radians(J16))+math.cos(math.radians(J18))*math.sin(math.radians(J17))*math.sin(math.radians(J16))
  C64 = math.cos(math.radians(J18))*math.cos(math.radians(J16))+math.sin(math.radians(J18))*math.sin(math.radians(J17))*math.sin(math.radians(J16))
  C65 = math.cos(math.radians(J17))*math.sin(math.radians(J16))
  C66 = 0
  D63 = math.sin(math.radians(J18))*math.sin(math.radians(J16))+math.cos(math.radians(J18))*math.sin(math.radians(J17))*math.cos(math.radians(J16))
  D64 = -math.cos(math.radians(J18))*math.sin(math.radians(J16))+math.sin(math.radians(J18))*math.sin(math.radians(J17))*math.cos(math.radians(J16))
  D65 = math.cos(math.radians(J17))*math.cos(math.radians(J16))
  D66 = 0
  E63 = J13
  E64 = J14
  E65 = J15
  E66 = 1
  ## WF*J1
  G24 = (B21*B27)+(C21*B28)+(D21*B29)+(E21*B30)
  G25 = (B22*B27)+(C22*B28)+(D22*B29)+(E22*B30)
  G26 = (B23*B27)+(C23*B28)+(D23*B29)+(E23*B30)
  G27 = (B24*B27)+(C24*B28)+(D24*B29)+(E24*B30)
  H24 = (B21*C27)+(C21*C28)+(D21*C29)+(E21*C30)
  H25 = (B22*C27)+(C22*C28)+(D22*C29)+(E22*C30)
  H26 = (B23*C27)+(C23*C28)+(D23*C29)+(E23*C30)
  H27 = (B24*C27)+(C24*C28)+(D24*C29)+(E24*C30)
  I24 = (B21*D27)+(C21*D28)+(D21*D29)+(E21*D30)
  I25 = (B22*D27)+(C22*D28)+(D22*D29)+(E22*D30)
  I26 = (B23*D27)+(C23*D28)+(D23*D29)+(E23*D30)
  I27 = (B24*D27)+(C24*D28)+(D24*D29)+(E24*D30)
  J24 = (B21*E27)+(C21*E28)+(D21*E29)+(E21*E30)
  J25 = (B22*E27)+(C22*E28)+(D22*E29)+(E22*E30)
  J26 = (B23*E27)+(C23*E28)+(D23*E29)+(E23*E30)
  J27 = (B24*E27)+(C24*E28)+(D24*E29)+(E24*E30)
  ## (WF*J1)*J2
  G30 = (G24*B33)+(H24*B34)+(I24*B35)+(J24*B36)
  G31 = (G25*B33)+(H25*B34)+(I25*B35)+(J25*B36)
  G32 = (G26*B33)+(H26*B34)+(I26*B35)+(J26*B36)
  G33 = (G27*B33)+(H27*B34)+(I27*B35)+(J27*B36)
  H30 = (G24*C33)+(H24*C34)+(I24*C35)+(J24*C36)
  H31 = (G25*C33)+(H25*C34)+(I25*C35)+(J25*C36)
  H32 = (G26*C33)+(H26*C34)+(I26*C35)+(J26*C36)
  H33 = (G27*C33)+(H27*C34)+(I27*C35)+(J27*C36)
  I30 = (G24*D33)+(H24*D34)+(I24*D35)+(J24*D36)
  I31 = (G25*D33)+(H25*D34)+(I25*D35)+(J25*D36)
  I32 = (G26*D33)+(H26*D34)+(I26*D35)+(J26*D36)
  I33 = (G27*D33)+(H27*D34)+(I27*D35)+(J27*D36)
  J30 = (G24*E33)+(H24*E34)+(I24*E35)+(J24*E36)
  J31 = (G25*E33)+(H25*E34)+(I25*E35)+(J25*E36)
  J32 = (G26*E33)+(H26*E34)+(I26*E35)+(J26*E36)
  J33 = (G27*E33)+(H27*E34)+(I27*E35)+(J27*E36)
  ## (WF*J1*J2)*J3
  G36 = (G30*B39)+(H30*B40)+(I30*B41)+(J30*B42) 
  G37 = (G31*B39)+(H31*B40)+(I31*B41)+(J31*B42)  
  G38 = (G32*B39)+(H32*B40)+(I32*B41)+(J32*B42)  
  G39 = (G33*B39)+(H33*B40)+(I33*B41)+(J33*B42)  
  H36 = (G30*C39)+(H30*C40)+(I30*C41)+(J30*C42)  
  H37 = (G31*C39)+(H31*C40)+(I31*C41)+(J31*C42)  
  H38 = (G32*C39)+(H32*C40)+(I32*C41)+(J32*C42)  
  H39 = (G33*C39)+(H33*C40)+(I33*C41)+(J33*C42)  
  I36 = (G30*D39)+(H30*D40)+(I30*D41)+(J30*D42)  
  I37 = (G31*D39)+(H31*D40)+(I31*D41)+(J31*D42)  
  I38 = (G32*D39)+(H32*D40)+(I32*D41)+(J32*D42)  
  I39 = (G33*D39)+(H33*D40)+(I33*D41)+(J33*D42)  
  J36 = (G30*E39)+(H30*E40)+(I30*E41)+(J30*E42)  
  J37 = (G31*E39)+(H31*E40)+(I31*E41)+(J31*E42)  
  J38 = (G32*E39)+(H32*E40)+(I32*E41)+(J32*E42)  
  J39 = (G33*E39)+(H33*E40)+(I33*E41)+(J33*E42)
  ## (WF*J1*J2*J3)*J4
  G42 = (G36*B45)+(H36*B46)+(I36*B47)+(J36*B48)  
  G43 = (G37*B45)+(H37*B46)+(I37*B47)+(J37*B48) 
  G44 = (G38*B45)+(H38*B46)+(I38*B47)+(J38*B48) 
  G45 = (G39*B45)+(H39*B46)+(I39*B47)+(J39*B48) 
  H42 = (G36*C45)+(H36*C46)+(I36*C47)+(J36*C48)  
  H43 = (G37*C45)+(H37*C46)+(I37*C47)+(J37*C48) 
  H44 = (G38*C45)+(H38*C46)+(I38*C47)+(J38*C48) 
  H45 = (G39*C45)+(H39*C46)+(I39*C47)+(J39*C48) 
  I42 = (G36*D45)+(H36*D46)+(I36*D47)+(J36*D48)  
  I43 = (G37*D45)+(H37*D46)+(I37*D47)+(J37*D48) 
  I44 = (G38*D45)+(H38*D46)+(I38*D47)+(J38*D48) 
  I45 = (G39*D45)+(H39*D46)+(I39*D47)+(J39*D48) 
  J42 = (G36*E45)+(H36*E46)+(I36*E47)+(J36*E48)  
  J43 = (G37*E45)+(H37*E46)+(I37*E47)+(J37*E48) 
  J44 = (G38*E45)+(H38*E46)+(I38*E47)+(J38*E48) 
  J45 = (G39*E45)+(H39*E46)+(I39*E47)+(J39*E48)
  ## (WF*J1*J2*J3*J4)*J5
  G48 = (G42*B51)+(H42*B52)+(I42*B53)+(J42*B54)
  G49 = (G43*B51)+(H43*B52)+(I43*B53)+(J43*B54)
  G50 = (G44*B51)+(H44*B52)+(I44*B53)+(J44*B54)
  G51 = (G45*B51)+(H45*B52)+(I45*B53)+(J45*B54)
  H48 = (G42*C51)+(H42*C52)+(I42*C53)+(J42*C54)
  H49 = (G43*C51)+(H43*C52)+(I43*C53)+(J43*C54)
  H50 = (G44*C51)+(H44*C52)+(I44*C53)+(J44*C54)
  H51 = (G45*C51)+(H45*C52)+(I45*C53)+(J45*C54)
  I48 = (G42*D51)+(H42*D52)+(I42*D53)+(J42*D54)
  I49 = (G43*D51)+(H43*D52)+(I43*D53)+(J43*D54)
  I50 = (G44*D51)+(H44*D52)+(I44*D53)+(J44*D54)
  I51 = (G45*D51)+(H45*D52)+(I45*D53)+(J45*D54)
  J48 = (G42*E51)+(H42*E52)+(I42*E53)+(J42*E54)
  J49 = (G43*E51)+(H43*E52)+(I43*E53)+(J43*E54)
  J50 = (G44*E51)+(H44*E52)+(I44*E53)+(J44*E54)
  J51 = (G45*E51)+(H45*E52)+(I45*E53)+(J45*E54)
  ## (WF*J1*J2*J3*J4*J5)*J6 
  G54 = (G48*B57)+(H48*B58)+(I48*B59)+(J48*B60)
  G55 = (G49*B57)+(H49*B58)+(I49*B59)+(J49*B60)
  G56 = (G50*B57)+(H50*B58)+(I50*B59)+(J50*B60)
  G57 = (G51*B57)+(H51*B58)+(I51*B59)+(J51*B60)
  H54 = (G48*C57)+(H48*C58)+(I48*C59)+(J48*C60)
  H55 = (G49*C57)+(H49*C58)+(I49*C59)+(J49*C60)
  H56 = (G50*C57)+(H50*C58)+(I50*C59)+(J50*C60)
  H57 = (G51*C57)+(H51*C58)+(I51*C59)+(J51*C60)
  I54 = (G48*D57)+(H48*D58)+(I48*D59)+(J48*D60)
  I55 = (G49*D57)+(H49*D58)+(I49*D59)+(J49*D60)
  I56 = (G50*D57)+(H50*D58)+(I50*D59)+(J50*D60)
  I57 = (G51*D57)+(H51*D58)+(I51*D59)+(J51*D60)
  J54 = (G48*E57)+(H48*E58)+(I48*E59)+(J48*E60)
  J55 = (G49*E57)+(H49*E58)+(I49*E59)+(J49*E60)
  J56 = (G50*E57)+(H50*E58)+(I50*E59)+(J50*E60)
  J57 = (G51*E57)+(H51*E58)+(I51*E59)+(J51*E60)
  ## (WF*J1*J2*J3*J4*J5*J6)*TF
  G60 = (G54*B63)+(H54*B64)+(I54*B65)+(J54*B66)
  G61 = (G55*B63)+(H55*B64)+(I55*B65)+(J55*B66)
  G62 = (G56*B63)+(H56*B64)+(I56*B65)+(J56*B66)
  G63 = (G57*B63)+(H57*B64)+(I57*B65)+(J57*B66)
  H60 = (G54*C63)+(H54*C64)+(I54*C65)+(J54*C66)
  H61 = (G55*C63)+(H55*C64)+(I55*C65)+(J55*C66)
  H62 = (G56*C63)+(H56*C64)+(I56*C65)+(J56*C66)
  H63 = (G57*C63)+(H57*C64)+(I57*C65)+(J57*C66)
  I60 = (G54*D63)+(H54*D64)+(I54*D65)+(J54*D66)
  I61 = (G55*D63)+(H55*D64)+(I55*D65)+(J55*D66)
  I62 = (G56*D63)+(H56*D64)+(I56*D65)+(J56*D66)
  I63 = (G57*D63)+(H57*D64)+(I57*D65)+(J57*D66)
  J60 = (G54*E63)+(H54*E64)+(I54*E65)+(J54*E66)
  J61 = (G55*E63)+(H55*E64)+(I55*E65)+(J55*E66)
  J62 = (G56*E63)+(H56*E64)+(I56*E65)+(J56*E66)
  J63 = (G57*E63)+(H57*E64)+(I57*E65)+(J57*E66)