浅析机器人（正 ）运动学D-H变换原理及算法

经常给客户培训，期间会有善于思考的朋友们提出一些很有价值的问题！比如，有人会问，机器人是怎么运动的？这种问题往往让我不知从何下手！多数情况下我都是讲一下机器人的各种坐标系用法，大家也都把这些坐标系的用法当成机器人最基础的知识来使用了！买来的机器人简单应用这也足够用了！对于复杂的非标应用，就很难搞定了，有些复杂功能，你得了解机器人最底层的逻辑，才能写出客户想要的功能！例如和多个外部轴同步运动共用坐标系；例如开发自动校准机器人零点的非标功能；或者你们自己设计一个三轴或更多轴的运动平台，需要坐标系的运用；等等！

今天我用最简单的语言给大家浅析下机器人最底层的算法，运动学正解；意在让大家了解其原理，懂其原理，大家遇到问题可以有个思路，思路对了，具体的程序就容易了！

什么是运动学正解？

本文都用如下图六轴机械手举例！有实体图和简化成只有六个电机的连杆结构，主要用连杆电机图做计算逻辑的解析！

机器人实体图

机器人电机轴位置图

什么是运动学（正）解；

机器人得有坐标数据，大家才能知道机器人现在在哪里，然后写出它去哪里的程序！

机器人的坐标怎么理解呢？比如六轴焊接机器人，我使用它就是为了焊接，我只需要它的焊枪头的位置数据！焊枪头在哪里，就可以说机器人在哪里！

那么机器人焊枪头的坐标数据是怎么来的呢？说到坐标，就得有一个原点或者基准，这个原点和基准是不会变化的！这个基准就是在机器人的底座上，永远不会变化；正运动学就是求这个焊枪头在基准坐标里的位置；

焊接机器人

怎么求焊枪头位置数据（TCP)呢？

以下面两幅图为例讲解，分别是机器人正视图和其连杆机构，机器人右视图和其连杆机构！

机器人正视图和其连杆机构

机器人右视图和其连杆机构

假定机器人这个姿势六个电机都在零点，如下两图所示，首先我们要给连杆简图建立坐标系，坐标系怎么建立呢？分六步完成：

第一步：找电机轴，标出轴线，如图Z1-Z6为轴线，对应六个关节轴，正视图和右视图配合看着比较清晰;

正视图轴线

右视图轴线

第二步：找出关节轴i和i+1之间的公垂线(就是和两条相邻的轴线都垂直的线），如果两条轴线相交则没有公垂线，那么就取其交点；以公垂线与关节轴i的交点或相交轴线的交点为坐标系{i}的原点（例如Z1和Z2的公垂线为L1，L1与Z1的交点就是坐标原点O1）；如下图红点和红圈都是坐标系原点：

坐标系原点正视图

坐标系原点右视图

第三步：确定Z轴（图中已经标出），和轴线是同一条线；（没有规定Z轴方向，所以可以有两个方向，我们就使用图示的方向，所以每家机器人规定的方向可能会不同！）

第四步：规定X轴沿着公垂线的指向，或者轴线相交的情况下规定X垂直于相交直线的平面；（垂直于相交平面的直线又有两个方向，我们只用图示的方向，所以每家机器人的X方向也可能有所不同！）如下图，红色箭头代表X轴：

确定X轴，正视图

确定X轴，右视图

第五步：右手定则确定Y轴方向；如上图，绿色箭头为Y轴方向；

第六步：尽量使基坐标O0和坐标系O1重合，图中就是重合状态，也有的机器人基坐标在O1下方或者角度有偏移，都无所谓，就是参数有点变化，下面讲到参数！

坐标系定义好了，要转换坐标系了（例如坐标系O1转换到 坐标系O2），正常情况下，把一个坐标系转换到另一个坐标系需要6个参数，（X/Y/Z/A/B/C）,三个坐标值和三个坐标旋转数据，每个电机定义好坐标系用这六个参数一样可以完成坐标系转换；但是国外有两个真专家（Denavit-Hartenberg），研究出来用四个参数就可以求解的方法，后人就一直用他们的方法简化运算，我也就讲他们的方法D-H变换了，上面规定设定坐标系的步奏也是为了找到这四个参数。

既然是规定好的，我们就按照规定来，规定参数为⍺、a、d、θ；

ai=沿Xi轴，从Zi移动到Zi+1的距离；

⍺i=绕Xi轴，从Zi旋转到Zi+1的角度；

di=沿Zi轴，从Xi-1移动到Xi的距离；

θi=绕Zi轴，从Xi-1旋转到Xi的角度；

i代表坐标系，如下图右下角列出的DH变换表格，对应的数据在图中更可以找到，有兴趣的朋友可以自己推理下！例如坐标系O1需要ai-1,⍺i-1,di,θi这四个参数，因为定义基坐标系O0和坐标系O1重合，所以四个参数都是0，如果设定基座标O0和坐标O1有Z轴旋转角度90度，那么⍺0=90；其实d、a和⍺参数就是电机机械安装位置的相对数据， θ是对于电机旋转角度，只有这一个是变量！

DH参数 正视图

DH参数 右视图

怎么用设定的坐标系和四个参数算出TCP呢？

上图中只建立坐标系到O5，后面可以在加一个坐标系O6，这个O6就是TCP了，图中O6和O5我设定为重合，所以表格中第六行的四个参数都为0，有的机器人机械结构会把O6沿着六轴往外移动一点，这个改d参数就可以了。

下面这个变换矩阵，就是计算从坐标系Oi-1变换到坐标系Oi的运算。c代表cos，s代表sin三角函数，参数带入公式计算也很简单吧！

变换矩阵

例如从坐标系O0变换到O1，再变换到O2，再变换到O3，如下图计算；最终得出的坐标是O3的坐标；同理想得出O6（TCP）的坐标，则再从O3变换到O4，O4变换到O5，O5变换到O6！六个矩阵相乘，得到TCP相对于基座标O0的变换矩阵，基坐标系O0乘以这个变换矩阵（6矩阵相乘结果），得出TCP坐标值！

变换到O3的三个矩阵

O3-O4

O4-O5

O5-O6

到此就结束了，D-H变换的原理大概讲完了，这就是机器人运动学正解，可以移动各个关节，但是TCP是相对6轴不动的！如果配合机器人运动学逆解，你可以自己搭建一个简易机器人玩了！不知道各位朋友是否耐心看完了？

下一次讲讲逆解，逆解复杂很多了！