第2章正运动学入门

第二章正运动学入门：DH参数法、连杆坐标系建立、变换矩阵推导

好，咱们正式开始啃正运动学这块硬骨头。

说实话，我刚入行那会儿，看到一堆坐标系和矩阵就头大。后来发现，只要把DH参数法搞明白，正运动学就是个套公式的活儿。你想想看，机器人每个关节动一下，末端到底在哪儿？这就是正运动学要回答的问题。

2.1 什么是正运动学？

正运动学，说白了就是：已知关节角度，求末端位姿。

比如你有个六轴机器人，你告诉每个关节转多少度，然后问「现在手爪在哪儿、朝哪个方向？」——这就是正运动学干的事。

我在做焊接机器人项目时，经常需要先算正运动学，才能判断焊枪能不能够到焊缝。嗯，这一步算错了，后面全白搭。

核心公式：

T = T₁ × T₂ × T₃ × … × Tₙ

其中 T 是末端相对于基座的变换矩阵，Tᵢ 是相邻连杆间的变换矩阵。

2.2 DH参数法——机器人界的「身份证」

DH参数法，全称Denavit-Hartenberg参数法。它用四个参数就能描述两个相邻关节之间的关系。我个人习惯把这四个参数记成「两转两移」：

参数	符号	含义	我记它的方式
连杆长度	aᵢ	沿Xᵢ轴，从Zᵢ₋₁到Zᵢ的距离	「长度」——两个关节轴之间的垂直距离
连杆扭角	αᵢ	绕Xᵢ轴，从Zᵢ₋₁到Zᵢ的转角	「扭角」——两个关节轴之间的夹角
连杆偏距	dᵢ	沿Zᵢ₋₁轴，从Xᵢ₋₁到Xᵢ的距离	「偏距」——沿着前一个关节轴的偏移
关节角	θᵢ	绕Zᵢ₋₁轴，从Xᵢ₋₁到Xᵢ的转角	「转角」——关节实际转动的角度

我的小窍门：

记参数顺序时，我脑子里有个口诀：「长扭偏角」——长度、扭角、偏距、关节角。对应到变换矩阵里，就是先沿X轴移动a，再绕X轴转α，然后沿Z轴移动d，最后绕Z轴转θ。

2.3 连杆坐标系建立——给机器人「画骨架」

建立坐标系是DH参数法里最容易出错的一步。我曾经因为坐标系方向搞反，调试了整整两天……后来总结了一套标准流程：

找关节轴：每个关节画一条Z轴，方向沿关节旋转轴（或移动轴）。
定原点：相邻两Z轴的公垂线与Zᵢ的交点，就是坐标系{i}的原点。
画X轴：沿公垂线方向，从Zᵢ₋₁指向Zᵢ。
定Y轴：右手定则，Y = Z × X。

你想想看，如果两个Z轴平行怎么办？嗯，这时候公垂线不唯一，我一般选经过前一个坐标系原点的那个。

⚠️ 避坑指南：

我曾经在建立SCARA机器人的坐标系时，把第二个关节的X轴方向搞反了。结果算出来的末端位置差了十万八千里。记住：X轴永远从Zᵢ₋₁指向Zᵢ，方向别搞反！

2.4 变换矩阵推导——从参数到矩阵的「翻译」

有了DH参数，变换矩阵就是机械式的计算。标准DH法的变换矩阵长这样：

Tᵢ = Rot(z, θᵢ) × Trans(z, dᵢ) × Trans(x, aᵢ) × Rot(x, αᵢ)

展开成4×4矩阵：

Tᵢ = [cosθᵢ   -sinθᵢcosαᵢ    sinθᵢsinαᵢ    aᵢcosθᵢ]
     [sinθᵢ    cosθᵢcosαᵢ   -cosθᵢsinαᵢ    aᵢsinθᵢ]
     [0        sinαᵢ         cosαᵢ         dᵢ     ]
     [0        0             0             1      ]

看着复杂？其实每个元素都有物理意义：

左上3×3是旋转矩阵，描述坐标系{i}相对于{i-1}的姿态
右上3×1是位置向量，描述坐标系{i}原点在{i-1}中的位置
最后一行是固定的[0 0 0 1]

实战经验：

我写代码时，从来不会手算这个矩阵。直接写个函数，输入a、α、d、θ四个参数，自动生成矩阵。这样既快又不容易出错。给你看个Python示例：

import numpy as np

def dh_transform(a, alpha, d, theta):
    """标准DH法变换矩阵"""
    ct = np.cos(theta)
    st = np.sin(theta)
    ca = np.cos(alpha)
    sa = np.sin(alpha)
    
    T = np.array([
        [ct, -st*ca,  st*sa,  a*ct],
        [st,  ct*ca, -ct*sa,  a*st],
        [0,   sa,     ca,     d   ],
        [0,   0,      0,      1   ]
    ])
    return T

2.5 知识体系总览

下面这张图，是我自己总结的正运动学核心逻辑。你看一遍，基本就清楚整个流程了：

2.6 一个完整的例子

拿一个两连杆平面机械臂来说。假设连杆1长L₁，连杆2长L₂，两个关节都是旋转关节。

第一步：建立坐标系

基坐标系{0}：Z₀垂直纸面向上，X₀水平向右
坐标系{1}：Z₁垂直纸面向上，X₁沿连杆1方向
坐标系{2}：Z₂垂直纸面向上，X₂沿连杆2方向

第二步：提取DH参数

连杆i	aᵢ	αᵢ	dᵢ	θᵢ
1	L₁	0	0	θ₁
2	L₂	0	0	θ₂

第三步：计算变换矩阵

T₁ = [cosθ₁  -sinθ₁  0  L₁cosθ₁]
     [sinθ₁   cosθ₁  0  L₁sinθ₁]
     [0       0       1  0      ]
     [0       0       0  1      ]

T₂ = [cosθ₂  -sinθ₂  0  L₂cosθ₂]
     [sinθ₂   cosθ₂  0  L₂sinθ₂]
     [0       0       1  0      ]
     [0       0       0  1      ]

T = T₁ × T₂

算出来的T矩阵右上角3×1，就是末端在基坐标系下的位置：

x = L₁cosθ₁ + L₂cos(θ₁+θ₂)
y = L₁sinθ₁ + L₂sin(θ₁+θ₂)

验证小技巧：

我每次算完，都会代入几个特殊角度验证。比如θ₁=0、θ₂=0时，末端应该在(L₁+L₂, 0)处。如果算出来不对，赶紧回头检查DH参数或者矩阵顺序。

嗯，正运动学就是这么回事。说白了就是：建坐标系→填DH参数→算矩阵→连乘。每一步都有章可循，多练几次就熟了。

公众号:蓝海资料掘金营，微信deep3321