1. 机械臂运动学基础：刚体变换与齐次坐标
各位同学，欢迎来到《从零复现机械臂运动控制算法》的第一章。
做机器人这么多年，我见过太多人一上来就怼DH参数、怼逆解。结果呢？算出来的关节角度根本对不上，机械臂在空中乱画。问题出在哪？说白了，就是刚体变换没吃透。你连一个点在空间里怎么挪、怎么转都搞不清楚，后面的运动学、动力学全是空中楼阁。
所以这一章，咱们老老实实把地基打牢。我会带着你从旋转矩阵开始，一步步搭出齐次变换矩阵，最后让你亲手写出链式乘法的代码。嗯，开始吧。
1.1 刚体变换：空间里的“搬砖”与“转圈”
机械臂的每个连杆，都可以看作一个刚体。刚体在空间里能做什么？只有两件事：平移和旋转。你不能把它捏变形，也不能把它拉长——这就是“刚”的含义。
想象一下，你手里拿着一把螺丝刀。你可以把它从桌面A点平移到B点（平移），也可以把它绕着手腕转个角度（旋转）。这两种运动的组合，就能把螺丝刀送到任意位置、摆成任意姿态。
在数学上，我们用旋转矩阵描述“转了多少”，用平移向量描述“挪了多少”。
核心概念：刚体变换 = 旋转 + 平移。旋转不改变物体内部点的相对距离，平移不改变物体的朝向。两者独立，但组合使用。
1.2 旋转矩阵：别被9个数吓到
我第一次看到3x3的旋转矩阵时，心里也犯嘀咕：9个数，记都记不住。后来我发现，你根本不需要背。你只需要理解一件事：旋转矩阵的每一列，就是原坐标系的基向量在新坐标系下的坐标。
举个例子。假设世界坐标系是X-Y-Z，你把一个坐标系绕Z轴转了θ角。那么新坐标系的X'轴在世界系下的坐标就是[cosθ, sinθ, 0]，Y'轴是[-sinθ, cosθ, 0]，Z'轴不变，还是[0, 0, 1]。把这三个列向量拼起来，就是绕Z轴的旋转矩阵：
R_z(θ) =
[ cosθ -sinθ 0 ]
[ sinθ cosθ 0 ]
[ 0 0 1 ]绕X轴和绕Y轴的矩阵同理。我在项目中经常用这三个基本矩阵去组合任意姿态，比硬记欧拉角公式快得多。
我的习惯：写代码时，我从来不用角度制。所有旋转矩阵的输入都是弧度。你想想看，cos(30°)和cos(0.5236)，后者在计算机里才精确。我曾经见过一个同事把角度当弧度传进去，结果机械臂差点撞到天花板——嗯，从那以后我都在代码开头加一行 deg2rad 的检查。
1.3 平移向量：简单，但别搞反方向
平移比旋转简单。一个三维向量[x, y, z]就搞定了。但有个坑：平移向量表示的是“从原坐标系原点，到新坐标系原点的向量”。
什么意思？如果你把坐标系沿着X轴正方向移动了5个单位，那么平移向量就是[5, 0, 0]。但如果你问“点P在新坐标系下的坐标”，计算时用的是减法：P_new = P_old - t。很多人在这里搞反符号，导致整个运动学链全错。
我个人建议：永远用齐次坐标，把平移和旋转统一成4x4矩阵。这样加减号的问题就自动消失了。
1.4 4x4齐次变换矩阵：把旋转和平移装进一个盒子里
齐次变换矩阵长这样：
T =
[ R t ]
[ 0 1 ]其中R是3x3旋转矩阵，t是3x1平移向量，最后一行是[0 0 0 1]。
为什么非要加这一行？因为有了它，你才能用矩阵乘法把多个变换串起来。否则，旋转和平移得分开算，麻烦得要命。
一个点p = [x, y, z]，写成齐次形式就是[x, y, z, 1]。用T去变换它：
p' = T * p结果p'的前三个分量就是变换后的坐标。就这么简单。
注意：齐次坐标的最后一维必须是1。如果你不小心写成0，那表示的是一个“方向向量”，平移会被忽略——这在某些场合有用（比如法线变换），但初学者先统一用1，别给自己挖坑。
1.5 链式乘法：机械臂的“骨架”
机械臂由多个连杆串联而成。每个关节都有一个局部坐标系。从基座到末端执行器，要经过一连串的变换：
T_0n = T_01 * T_12 * T_23 * ... * T_(n-1)n这个T_0n就是末端在基座坐标系下的位姿。我当年第一次写出这个链式乘法时，看着控制台输出的坐标，心里特别踏实——机械臂的“骨架”就这么搭起来了。
链式乘法的顺序不能错。矩阵乘法不满足交换律，A * B和B * A天差地别。我习惯从基座开始，依次左乘下一个关节的变换矩阵。你可以用纸笔推一遍，感受一下。
1.6 本章知识体系
下面这张图把本章的核心逻辑串起来了。你可以把它当作一个“地图”，学完后再回来看，应该会更有感觉。
1.7 代码实战：从零构建齐次变换矩阵
光说不练假把式。下面我用Python写一个最小实现。你可以在自己的环境里跑一遍，改改参数，看看效果。
import numpy as np
def rot_z(theta):
"""绕Z轴旋转矩阵"""
c, s = np.cos(theta), np.sin(theta)
return np.array([
[c, -s, 0],
[s, c, 0],
[0, 0, 1]
])
def rot_y(theta):
"""绕Y轴旋转矩阵"""
c, s = np.cos(theta), np.sin(theta)
return np.array([
[ c, 0, s],
[ 0, 1, 0],
[-s, 0, c]
])
def rot_x(theta):
"""绕X轴旋转矩阵"""
c, s = np.cos(theta), np.sin(theta)
return np.array([
[1, 0, 0],
[0, c, -s],
[0, s, c]
])
def build_transform(R, t):
"""构建4x4齐次变换矩阵"""
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t
return T
# 示例：先绕Z转30度，再沿X平移5
theta = np.deg2rad(30)
R = rot_z(theta)
t = np.array([5.0, 0.0, 0.0])
T = build_transform(R, t)
print("齐次变换矩阵 T:")
print(T)
# 变换一个点
p = np.array([1.0, 0.0, 0.0, 1.0]) # 齐次坐标
p_transformed = T @ p
print(f"原始点: {p[:3]}")
print(f"变换后: {p_transformed[:3]}")
试试看：把上面的rot_z换成rot_y，或者把平移向量改成[0, 3, 2]，看看输出怎么变。我当年就是这么一点点摸索出直觉的。
1.8 避坑指南
- 矩阵乘法顺序：先旋转再平移，和先平移再旋转，结果完全不同。我的经验是：
T = Trans * Rot（先旋转后平移）更符合直觉，因为你先摆好姿态，再送到位置。 - 角度单位：永远用弧度。Python的
math.sin和np.sin都默认弧度。如果你非要用角度，记得转一下。 - 齐次坐标最后一维：点用1，向量用0。我曾经在算速度的时候忘了改，结果平移分量被错误地加到了速度上——查了整整一下午的bug。
- 浮点误差：旋转矩阵理论上是正交的（
R^T * R = I），但计算机算出来会有微小误差。长时间链式乘法后误差会累积。我习惯每隔几步做一次正交化修正。
1.9 小结
这一章我们干了三件事：理解了旋转矩阵和平移向量的物理意义，学会了构建4x4齐次变换矩阵，最后用链式乘法把多个变换串起来。这些都是机械臂运动学的“原子操作”。后面的DH参数、正运动学、逆运动学，全都建立在这个基础之上。
你可以在自己的电脑上把代码跑起来，多改几个参数试试。相信我，亲手算一遍比看十遍书都管用。