第72篇 NumPy进阶：用Python算机器人运动学，比Eigen还直观

上篇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_homogeneousT[: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, l11.0, l21.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, l11.0, l21.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*yz*z), 2*(x*y-z*w), 2*(x*zy*w)], [2*(x*yz*w), 1-2*(x*xz*z), 2*(y*z-x*w)], [2*(x*z-y*w), 2*(y*zx*w), 1-2*(x*xy*y)] ])实际项目中建议用scipy.spatial.transform.Rotation它封装了四元数、欧拉角、旋转矩阵之间的所有转换比自己写靠谱得多。在ROS2的Python节点里也经常用到它来处理姿态数据。但面试的时候能手写出来说明你理解原理。面试中常被追问的点实际项目中运动学计算经常需要在循环里反复调用。面试官可能会追问如果计算量很大你怎么优化这时候除了提到向量化还可以说说用缓存减少重复计算或者用SciPy的优化模块做逆运动学求解。另外四元数插值slerp在轨迹规划中也很常用面试时能提到这个会加分不少。补充一点实际项目中做运动学计算时数值稳定性也很重要。比如用atan2而不是atan来避免象限判断错误用归一化防止浮点误差累积。这些细节在面试中提出来能体现你对工程实践的深入理解。在机器人面试中能把NumPy和实际运动学场景结合起来讲是很好的加分项。给正在准备面试的你NumPy做运动学计算面试中偶尔会考。重点不在于你能不能写出来而在于你是否理解NumPy的向量化思维和切片操作。建议把旋转矩阵、齐次变换、正运动学这几个用NumPy实现一遍。下篇聊Matplotlib可视化——调试SLAM和规划算法的必备技能。算出来的数据要能直观地看到才能验证算法对不对。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第71篇 NumPy入门——机器人数据处理的Python标配下一篇预告第73篇 Matplotlib可视化——调试SLAM/规划算法的必备技能有任何问题欢迎评论区留言我会尽量回复。