上篇NumPy入门把基础打好了。今天用NumPy来做机器人运动学的计算——旋转矩阵、齐次变换、正运动学。

之前我们用Eigen做过同样的事。对比一下很有意思：Eigen是C++的静态类型世界，NumPy是Python的动态类型世界。两者做的事情一样，但使用体验完全不同。

先说个面试场景。

面试官问："你能用Python写一个二自由度机械臂的正运动学吗？给定两个关节角，算末端位置。"

这个题目不难，但能看出你对NumPy的熟练程度。

旋转矩阵

二维旋转矩阵用NumPy表示非常直观：

import numpy as np

def rotation_2d(theta):
    """二维旋转矩阵，theta为弧度"""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s], [s, c]])

R = rotation_2d(np.pi / 4)  # 旋转45度
point = np.array([1.0, 0.0])
rotated = R @ point

三维旋转稍微复杂一些，需要分别处理绕x、y、z轴的旋转：

def rot_x(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[1, 0, 0], [0, c, -s], [0, s, c]])

def rot_y(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])

def rot_z(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])

# ZYX欧拉角转旋转矩阵
R = rot_z(yaw) @ rot_y(pitch) @ rot_x(roll)

NumPy的矩阵乘法用@运算符，链式调用非常清晰。在C++里用Eigen写同样的代码，需要R = Rz * Ry * Rx，看起来差不多，但Eigen需要编译，NumPy可以在Python交互式环境里即时看结果。做算法原型的时候，这个差异很重要。

齐次变换矩阵

齐次变换把旋转和平移统一到一个4x4矩阵里：

def homogeneous_transform(R, t):
    """构造齐次变换矩阵"""
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

# 用法
R = rot_z(np.pi / 6)
t = np.array([1.0, 0.5, 0.0])
T = homogeneous_transform(R, t)

# 变换一个点
point_homogeneous = np.array([0.1, 0.2, 0.0, 1.0])
transformed = T @ point_homogeneous

T[:3, :3] = R这种切片赋值是NumPy的强项。在C++里你需要手动循环赋值或者用Eigen的block操作。

变换矩阵的逆也很方便

T_inv = np.linalg.inv(T)
# 或者利用变换矩阵的特殊结构，更高效：
R_inv = T[:3, :3].T  # 旋转矩阵的逆等于转置
t_inv = -R_inv @ T[:3, 3]
T_inv_fast = homogeneous_transform(R_inv, t_inv)

正运动学实战

来写一个二自由度平面机械臂的正运动学：

def fk_2dof(theta1, theta2, l1=1.0, l2=1.0):
    """二自由度平面机械臂正运动学
    theta1, theta2: 关节角（弧度）
    l1, l2: 连杆长度
    返回: 末端位置 [x, y]
    """
    T1 = homogeneous_transform(rot_z(theta1), np.zeros(3))
    T2 = homogeneous_transform(rot_z(theta2), np.array([l1, 0, 0]))
    
    # 总变换
    T_total = T1 @ T2
    
    # 末端位置（在第二连杆末端）
    end_local = np.array([l2, 0, 0, 1.0])
    end_world = T_total @ end_local
    
    return end_world[:2]

# 测试
pos = fk_2dof(np.pi/4, np.pi/6)
print(f"末端位置: x={pos[0]:.3f}, y={pos[1]:.3f}")

如果要批量计算多个关节角度对应的末端位置，NumPy的向量化就派上用场了：

def fk_batch(thetas, l1=1.0, l2=1.0):
    """批量正运动学
    thetas: shape (N, 2)，N组关节角
    返回: shape (N, 2)，N组末端位置
    """
    theta1 = thetas[:, 0]
    theta2 = thetas[:, 1]
    
    x = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
    y = l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2)
    
    return np.column_stack([x, y])

# 一次算1000组
thetas = np.random.uniform(-np.pi, np.pi, (1000, 2))
positions = fk_batch(thetas)

1000组正运动学，用for循环可能要几百毫秒，向量化版本只要不到1毫秒。这就是NumPy的威力。在实际项目中，批量计算的能力非常重要——比如蒙特卡洛定位需要对上千个粒子做运动学更新，向量化是保证实时性的关键。

四元数运算

四元数在机器人姿态表示中非常重要。NumPy也可以处理：

def quat_multiply(q1, q2):
    """四元数乘法 q1 * q2
    四元数格式: [w, x, y, z]
    """
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    return np.array([
        w1*w2 - x1*x2 - y1*y2 - z1*z2,
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2
    ])

def quat_to_rot_matrix(q):
    """四元数转旋转矩阵"""
    w, x, y, z = q / np.linalg.norm(q)  # 归一化
    return np.array([
        [1-2*(y*y+z*z), 2*(x*y-z*w),   2*(x*z+y*w)],
        [2*(x*y+z*w),   1-2*(x*x+z*z), 2*(y*z-x*w)],
        [2*(x*z-y*w),   2*(y*z+x*w),   1-2*(x*x+y*y)]
    ])

实际项目中建议用scipy.spatial.transform.Rotation，它封装了四元数、欧拉角、旋转矩阵之间的所有转换，比自己写靠谱得多。在ROS2的Python节点里也经常用到它来处理姿态数据。但面试的时候，能手写出来说明你理解原理。

面试中常被追问的点

实际项目中，运动学计算经常需要在循环里反复调用。面试官可能会追问：如果计算量很大，你怎么优化？这时候除了提到向量化，还可以说说用缓存减少重复计算，或者用SciPy的优化模块做逆运动学求解。另外，四元数插值（slerp）在轨迹规划中也很常用，面试时能提到这个会加分不少。

补充一点，实际项目中做运动学计算时，数值稳定性也很重要。比如用atan2而不是atan来避免象限判断错误，用归一化防止浮点误差累积。这些细节在面试中提出来，能体现你对工程实践的深入理解。

在机器人面试中，能把NumPy和实际运动学场景结合起来讲，是很好的加分项。

给正在准备面试的你

NumPy做运动学计算，面试中偶尔会考。重点不在于你能不能写出来，而在于你是否理解NumPy的向量化思维和切片操作。建议把旋转矩阵、齐次变换、正运动学这几个用NumPy实现一遍。

下篇聊Matplotlib可视化——调试SLAM和规划算法的必备技能。算出来的数据要能直观地看到，才能验证算法对不对。

---

如果这篇文章对你有帮助，欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。

「机器人软件开发面试·从入门到精通」连载系列 上一篇：第71篇 NumPy入门——机器人数据处理的Python标配 下一篇预告：第73篇 Matplotlib可视化——调试SLAM/规划算法的必备技能

有任何问题欢迎评论区留言，我会尽量回复。