一、前言
如果机器人工程师缺乏机器人学理论的支撑和足够的认识，那么随着机器人项目的深入推进，可能会越走越艰难，所谓万丈高楼平地起，必须得打好比万丈高楼还要夯实得地基，大厦才能牢固。本篇对六轴机械臂的基础建模知识进行了较为彻底的梳理，参考了较多的网上资料，发现，很多文章或者教程并没有把DH建模这个事说得特别清楚，这里对一些注意点或者所谓的坑做了一些着重说明，仅供参考。本篇花了笔者1周的时间整理和思考，全篇的思路如下：
二、三维空间的坐标系变换
我们知道，一个直角坐标系遵照右手定则，沿着某个轴逆时针旋转θi角度的旋转矩阵为：
我们知道，任何一个坐标系f0，可以通过三个以上基础旋转，唯一变换到另一个同原点的坐标系fn，两个关系可以用按照变换的先后顺序左乘表示，如上图右侧。
我们知道，三维空间里，如果有两个坐标系F0，F6，如果想知道P在F6系中的向量（或者点坐标）在F0系中是多少，可以通过如下的两种形式表示：
如上式2所示，旋转和平移可以组成一个齐次变换矩阵T，用在机械臂上面，可以表示一个机械臂的末端位姿：
如上图，机械臂的末端位姿有两部分组成：
可以理解为一个是末端原点的坐标t0，一个是旋转量R06（或者可以用欧拉角序列、翻滚⾓-俯仰⾓-偏航⾓序列等表示），在图中对应的红点就是机械臂的末端的原点，其坐标其实就是t0，也就是式2中的T中的最后一列前三个（dx0，dy0，dz0）。末端的姿态还涉及到旋转，这个信息是含在了旋转矩阵R06（见式1）中。
在机械臂中，可以控制的只有各个关键的电机角度，六轴就是六个电机的角度[q1,q2,q3,q4,q5,q6]，通过设定六个电机角度，可以把机械臂的末端送到目标的位置。这个过程，数学上相当于机械臂基座的坐标系，通过一些列的旋转平移变换到了末端，由如下的这种机制：
变换的矩阵T可以分解连乘得到：
T60表示的是末端F0到F6的运动，注意到，由于变换的顺序是左乘原则，就是越左边的运动越晚，这与我们直观的感觉想违背，我们直观理解是从左往右依次进行（右乘原则），这个可以根据变换矩阵的性质，给转变过来：
根据以上性质，可以得到右乘形式：
机械臂的建模就是基于类似这样的机制，有学者经过研究简化后，提出了机器人的DH建模理论。
于是，DH建模所用到的基础旋转矩阵就变为：
三、MDH建模要点
DH建模有两种，一种是标准DH建模，SDH，一种是改进DH，MDH。为了后续建模方便，建议使用MDH规则建模。其主要的原理是这样的：
（1）先确定z轴（各关节的电机转轴）：
（2）逐步从固定支座base坐标系F0，确定x轴的方向，原则是x轴的要是本坐标系z轴和下一个坐标系z轴的公垂线。如上图，X0轴应该垂直于Z0，和Z1，那么可以这么定：
如上图，X0垂直于轴Z0和Z1，X1轴垂直于Z1和Z2，X2应该垂直于Z2和Z3…
（3）根据右乘原理，MDH中规定当前关节i的参数先后的顺序是：
以上面的坐标系F1为例，四个参数代表是坐标F0系，先以X0轴旋转Alpha，再沿着x0轴方向移动a（负方向移动为负数），再以Z0为轴旋转Offset，最后沿着Z0方向移动d（负方向移动为负数），最后得到F1坐标系。此处，alpha1=0，X0不需要旋转，a=0，沿着X0没有平移，offset=pi/2，沿着Z0逆时针旋转90度，X0轴转到了X1轴的位置，然后d=170.56，沿着Z0正方向移动了170.56，这样经过连续的旋转+平移后，就完成了关键1的参数建模。以此类推，逐步确定其余5组参数。
所有的参数确定，如下图示例所示：
四、MDH的变换矩阵推导
根据以上的分析，MDH模型中，从一个坐标系Fi-1到Fi的变换矩阵其实可以这些推导：用python表示：
import sympy as sy
#MDH
alpha=[0,np.pi/2,0,np.pi,-np.pi/2,np.pi/2]
a=[0,0,136.35,100,0,0]
offset=[np.pi/2,np.pi/2,0,np.pi/2,0,0]
d=[170.46,80,0,0,-85,-62.4]
"""推导转换矩阵"""
#z轴旋转角度
theta1=sy.Symbol("theta1")
#z轴平移
d1=sy.Symbol("d1")
#x轴旋转角度
al1=sy.Symbol("al1")
#x轴平移
a1=sy.Symbol("a1")
def MTi(alpha,a,d,theta,DHtype="MDH"):
#x轴旋转变化阵，右手系，逆时针为正
Rx=sy.Matrix([
[1, 0, 0, 0],
[0, sy.cos(alpha), -sy.sin(alpha), 0],
[0, sy.sin(alpha), sy.cos(alpha), 0],
[0, 0, 0, 1]
])
#沿着x轴平移，有方向
Tx=sy.Matrix([
[1, 0, 0, a],
[0, 1, 0, 0],
[0, 0,1, 0],
[0, 0,0, 1]
])
#z轴旋转阵，右手系，逆时针为正
Rz=sy.Matrix([
[sy.cos(theta), -sy.sin(theta), 0, 0],
[sy.sin(theta), sy.cos(theta), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
#沿着z轴平移，有方向
Tz=sy.Matrix([
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0,1, d],
[0, 0,0, 1]
])
print("MDH——TR",Rx@Tx@Rz@Tz)
#右乘原则，先旋转X轴alpha，再沿X轴平移a，再旋转Z轴theta（这里为offset值），最后沿着Z轴平移D
return Rx@Tx@Rz@Tz
#MDH的转换矩阵如下：
MTi(al1,a1,d1,theta1)
可得到MDH的基本变换矩阵为：
简写如下：
五、机械臂MDH的python模型
根据右乘原则，整个机械臂的变换矩阵，可以由6个基本变换矩阵右先后右乘获得：
A06=A1A2A2A4A5*A6
用python推导计算如下：
#MDH方法
import sympy as sy
import numpy as np
import math
from math import atan2
#MDH参数
alpha=[0,np.pi/2,0,np.pi,-np.pi/2,np.pi/2]
a=[0,0,136.35,100,0,0]
offset=[np.pi/2,np.pi/2,0,np.pi/2,0,0]
d=[170.46,80,0,0,-85,-62.4]
def MTi(alpha,a,d,theta,DHtype="MDH"):
#x轴变化阵，右手系，顺针旋负方向
Rx=sy.Matrix([
[1, 0, 0, 0],
[0, sy.cos(alpha), -sy.sin(alpha), 0],
[0, sy.sin(alpha), sy.cos(alpha), 0],
[0, 0, 0, 1]
])
#沿着x轴平移，有方向
Tx=sy.Matrix([
[1, 0, 0, a],
[0, 1, 0, 0],
[0, 0,1, 0],
[0, 0,0, 1]
])
#z轴旋转阵，右手系，顺针旋负方向
Rz=sy.Matrix([
[sy.cos(theta), -sy.sin(theta), 0, 0],
[sy.sin(theta), sy.cos(theta), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
#沿着z轴平移，有方向
Tz=sy.Matrix([
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0,1, d],
[0, 0,0, 1]
])
print("MDH——TR",Rx@Tx@Rz@Tz)
return Rx@Tx@Rz@Tz
def Aiv(alpha,a,d,offset,theta=[0,0,0,0,0,0],Tnum=6):
"""
依次右乘，推导出机械臂位姿矩阵
"""
for i in range(Tnum):
Ti=MTi(alpha[i],a[i],d[i],offset[i])
if i==0:
Ai=Ti
else:
Ai=Ai@Ti
return Ai
#机械臂位姿矩阵如下
A06= Aiv(alpha,a,d,offset)
print("位姿矩阵：",A06)
可以计算得到机械臂的位姿矩阵：
至此根据建立的MDH参数，就可以计算出末端的位姿矩阵了，为了画出机械臂的样子，我们可以通过连续变换矩阵，分别计算出各轴的位姿，并用matplot画出来：
六、python源码
可复制直接运行。
python源码资源地址