调机械臂的时候最让人抓狂的往往不是电机不转而是位置到了、姿态却歪了十几度。我第一次被位置与姿态描述狠狠教育是在一次抓取实验里末端执行器停在了目标上方偏左大约三厘米的位置姿态还整体绕自身轴多转了小半圈夹爪对着空气开合。复盘下来问题不在控制器参数而在我对姿态的理解——我把它当成了三个可以像位置一样做加减的数。位置与姿态描述看上去只是机器人学导论里的第一章内容实际上它是后面运动学、轨迹规划、手眼标定、力控这些东西的公共地基地基歪一厘米上层就歪一米。这篇内容适合三类人看刚上机器人学导论、被旋转矩阵和欧拉角绕晕的学生手头要把机械臂、云台、无人机、移动平台的状态量对接起来的工程实现者以及想把书本符号落成能跑代码的实践者。我会按为什么难、怎么表示、怎么算、怎么排错的顺序把位置与姿态描述的几套主流表示法讲清楚中间给出可以直接抄下来跑的 Python 片段也会说说我踩过的那些坑。1. 位置好写姿态难缠一次抓取失败拆出来的差别1.1 位置是标准向量三个数可以放心加减位置这件事之所以简单是因为它天然活在三维欧氏空间里。一个点 P 在参考系 A 下的位置就是三个实数组成的有序三元组写成列向量形式 p^A [x, y, z]^T。这个上标 A 不是什么装饰它表示这组数字是在 A 坐标系里量出来的。同一个点在 A 系里是 [1, 0, 0]把参考系整体转 90 度之后数字可能变成 [0, 1, 0]但点是同一个点。很多初学者的混乱就是从忽略这个上标开始的——手里拿着三个数却说不清它们是相对谁的。位置向量满足我们熟悉的全部代数性质p1 p2 有意义位移叠加k·p 有意义缩放p1 - p2 表示从 p2 指向 p1 的位移向量。这意味着位置可以用线性代数那一整套工具处理求平均、做最小二乘、插值都不会出问题。这也是为什么位置的代码通常写起来很顺手三行数组加减乘除一目了然。我在项目里有一个硬性习惯任何一个位置变量命名里必须带参考系信息比如p_base_tcp、p_cam_target。这不是洁癖。做手眼标定时相机系下的目标点和基座系下的目标点数值上可能只差一个变换矩阵但一旦命名混了你就会拿着相机系的 z 去和基座系的 z 做比较然后花半天时间怀疑标定结果。位置的坑基本都在忘了它是相对谁这一件事上。1.2 姿态没有减法因为旋转不满足交换律姿态的麻烦在于它不是向量空间里的元素而是三维旋转群 SO(3) 里的元素。SO(3) 是一个群有乘法旋转的复合和单位元不转但它不是线性空间——两个旋转相加没有意义你也不能把一个旋转乘以 2 表示转两倍那么多。想转两倍得用轴角形式把角度翻倍而不是把旋转矩阵乘二。更反直觉的是旋转的复合不可交换。你可以做个小实验拿一本书放在桌上先绕世界坐标系的 Z 轴转 90 度再绕世界坐标系的 X 轴转 90 度记下书的最终朝向。然后把书复位把顺序反过来先绕 X 转 90 度再绕 Z 转 90 度。两次结果完全不同误差可能到 90 度量级。这就是为什么姿态描述里先转哪个后转哪个是必须写进文档的信息而不能靠默认。这里有个特别容易被混淆的点角速度是可以相加的有限转动不行。原因在于角速度是瞬时量它活在姿态流形的切空间里切空间是线性空间所以能加起来而有限转动是流形上的点两点之间没有加法。我见过不少人在多传感器融合里把 IMU 的三个角速度积分出来的角度直接相加然后奇怪为什么姿态会漂。实际上那三个角速度积分出来的是一个旋转得按旋转的复合规则乘起来而不是向量加。1.3 开工之前先把符号约定钉死机器人学的符号约定是个重灾区不同教材的同一个符号含义可能正好相反。我建议在动手写第一行代码之前先在自己的项目里写一份一页纸的约定文档把下面这些事定死坐标系一律用右手系位置和旋转都用列向量相乘的约定也就是 p^A R_AB · p^B下标写成目标系_参考系还是参考系_目标系要统一上标表示被描述量所在或所表达于的坐标系。以 R_AB 为例我采用的是把 B 系中的坐标转换到 A 系这个语义也就是它同时是 B 系的三个基轴在 A 系中的排列。这个约定下 R_BA R_AB^T链式关系是 R_AC R_AB · R_BC非常好记。但如果你看的教材采用相反约定所有公式都得跟着翻抄公式时不能只抄一半。另一个必须提前定的是角度单位。ROS 体系里很多接口用弧度但示教器上显示的是度配置文件里写的是度代码里忘了转末端姿态就会以 57.3 倍的错误比例歪掉。我的做法是变量名带单位后缀theta_rad、theta_deg转换只在输入输出边界做一次中间层永远只认弧度。这条规则帮我省掉的调试时间比任何一个算法优化都多。2. 旋转矩阵九个数字里的六条硬约束2.1 方向余弦把 B 的三根轴搬到 A 里量一遍旋转矩阵最朴素的理解方式就是把 B 坐标系的三根单位轴拿到 A 坐标系里逐个描述一遍然后按列拼起来。B 系的 X 轴在 A 系里是一个单位向量写成三分量Y 轴、Z 轴同理。把这三个列向量并排放成一个 3×3 矩阵得到的就正好是 R_AB。这样构造出来的矩阵每个元素都是两个单位向量夹角的余弦所以叫方向余弦矩阵。它有一个非常直观的语义某一列就是对应坐标轴在参考系里的方向。调试的时候我经常直接打印旋转矩阵然后看一眼第三列——它就是末端 Z 轴通常对应工具朝向在世界系里的方向如果这一列和期望的接近垂直方向差别很大那问题基本就在姿态上而不是位置上。这个按列读的习惯在排查问题时效费比极高。比如机械臂末端本应该竖直向下对着桌面那么工具 Z 轴在世界系里应该接近 [0, 0, -1]取决于你工具系的 Z 指向内还是外。你只要看 R 的第三列是不是接近这个值就知道姿态对不对不需要去反解欧拉角。2.2 正交与行列式九个数字只有三个自由度旋转矩阵必须满足两个条件R^T R I且 det(R) 1。第一条是正交性保证三根轴互相垂直且长度为一第二条排除了镜像反射那种变换行列式是 -1它把左手系变成右手系不是刚体旋转。九个元素加上六个独立约束正交性给出六个独立方程剩下来正好三个自由度和姿态有三个自由度这个基本事实吻合。这也解释了为什么用旋转矩阵做优化时特别难受你要么把九个数当自由变量然后每步做正交化投影要么干脆换参数化。工程上后者更常见——用四元数或轴角去做优化和插值只在需要和硬件接口对接时才转成旋转矩阵。数值上还有一件事必须留意旋转矩阵连乘多了以后会漂移。浮点误差累积几千步R^T R 与单位阵的偏差可能到 1e-6 量级甚至更大行列式也会从 1 慢慢偏出去。这时候如果拿去求逆误差会被放大。我的处理方式是在关键节点做一次正交化最简单的是 SVD 法import numpy as np def orthonormalize(R): U, _, Vt np.linalg.svd(R) Rn U Vt if np.linalg.det(Rn) 0: U[:, -1] * -1 Rn U Vt return Rn如果只是在做视觉显示或者低频计算这一步可以省但如果是几百赫兹的迭代估计环路建议每步或者每几十步做一次成本很低稳定性提升明显。2.3 复合顺序左乘和右乘差的是一整个坐标系旋转的复合是姿态描述里最容易出错的地方。R_AC R_AB · R_BC 这个式子要读成先把 C 系里的点转到 B 系再转到 A 系。矩阵乘法从右往左作用在向量上这个顺序不能乱。左乘和右乘的差别用一句话概括左乘相当于绕参考系固定系旋转右乘相当于绕当前坐标系动系旋转。举个例子一个物体当前姿态是 R你想让它绕自身 Z 轴转 30 度写 R · Rz(30°)想让它绕世界 Z 轴转 30 度写 Rz(30°) · R。这两个结果一般不同只有在这两种 Z 轴恰好平行时才相等。我见过的最典型的翻车场景是云台控制。操作手想让云台先俯仰再偏航代码里按偏航矩阵乘俯仰矩阵的顺序写了结果每次俯仰之后偏航的方向都跟着变了操作手感极其别扭。改成左乘固定轴顺序之后立刻就对了。所以当有人问你姿态控制不听话的时候第一个该问的问题往往不是算法而是你矩阵乘的顺序是什么。def rot_x(t): c, s np.cos(t), np.sin(t) return np.array([[1, 0, 0], [0, c, -s], [0, s, c]]) def rot_y(t): c, s np.cos(t), np.sin(t) return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]]) def rot_z(t): c, s np.cos(t), np.sin(t) return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) # 绕世界 Z 转 90°再绕世界 X 转 90° R1 rot_x(np.pi/2) rot_z(np.pi/2) # 交换顺序 R2 rot_z(np.pi/2) rot_x(np.pi/2) print(np.round(R1, 3)) print(np.round(R2, 3))跑一下这两行你会看到两个完全不同的矩阵这是理解复合顺序最快的办法比看十页推导都管用。3. 欧拉角与 RPY说人话的代价是死锁3.1 十二种约定与内旋外旋这组绕人的词欧拉角的核心思想是把一个任意姿态拆成三次绕坐标轴的旋转每次一个角度。因为每次可以选择 X、Y、Z 三根轴中的任意一根且相邻两次不能选同一根组合起来一共有 12 种约定。这个数字本身就说明了问题的复杂度如果你只写欧拉角是 (30°, 45°, 60°)这句话信息量约等于零因为不知道约定就无法还原姿态。这 12 种约定又被分成两大类。第一类是绕动轴旋转也叫内旋每次旋转都绕着上一次旋转之后的新坐标轴第二类是绕定轴旋转也叫外旋每次旋转都绕原始参考系的固定轴。有个非常实用的等价关系绕动轴按 X→Y→Z 顺序转结果等于绕定轴按 Z→Y→X 顺序转。记住这条看到不同教材的描述就能互相翻译。我的建议是在代码注释和接口文档里永远把约定写全格式写成内旋 ZYZ或者定轴 XYZ等价内旋 ZYX不要只写欧拉角。团队协作里因为省略约定造成的返工我见过的次数两只手数不过来。3.2 ZYZ 和 RPY 各自的地盘机器人领域最常见的两种约定一种用在机械臂腕部一种用在飞行器和移动平台。机械臂的球腕三个旋转轴交于一点的那种手腕几乎都用 ZYZ 欧拉角。原因是它的三个角有明确的物理含义最外面那个角对应整条臂的方位中间那个角对应腕部的倾斜通常限制在 0 到 180 度之间最里面那个角是末端绕自身轴的滚转。这种参数化的好处是中间角接近 0 或 180 度时腕部会出现奇异而这个奇异恰好和机械臂运动学奇异对应得上分析起来很整齐。飞行器、移动机器人、视觉标定里则普遍用 RPY也就是绕固定轴依次 Xroll、Ypitch、Zyaw数学上写成 R Rz(γ) · Ry(β) · Rx(α)注意乘法顺序是从右往左对应 X、Y、Z写的时候别把顺序弄反。它的好处是三个角有很强的直觉roll 是左右倾斜pitch 是抬头低头yaw 是左右转向。IMU 输出的姿态角基本都是这个格式。从旋转矩阵反解 RPY 的公式值得记一下因为实际项目里几乎天天用def R_to_rpy(R): # R Rz(yaw) Ry(pitch) Rx(roll) sy -R[2, 0] sy np.clip(sy, -1.0, 1.0) pitch np.arcsin(sy) if abs(abs(sy) - 1.0) 1e-8: # 奇异位形roll 与 yaw 不独立约定 roll 0 roll 0.0 yaw np.arctan2(-R[0, 1], R[1, 1]) else: roll np.arctan2(R[2, 1], R[2, 2]) yaw np.arctan2(R[1, 0], R[0, 0]) return roll, pitch, yaw注意那个np.clip。如果旋转矩阵带一点数值误差-R[2,0]可能变成 1.0000000002arcsin会直接返回 nan然后整个姿态估计链路就崩了。这个小防护我在每个反解函数里都加。3.3 万向节死锁几何图像加上数值验证死锁这个词被讲得特别玄其实几何图像很清楚。以 RPY 为例roll 绕 X 转pitch 绕 Y 转yaw 绕 Z 转。当 pitch 等于正负 90 度时Y 轴和 Z 轴发生了关系——第一次 roll 旋转会把 Z 轴转到原来的 X 位置附近结果就是 roll 和 yaw 变成了绕同一根物理轴旋转两个角度不再独立。三个自由度退化成了两个你无法单独调整 roll 和 yaw。注意死锁不是数值问题也不是实现 bug它是这套参数化方法固有的拓扑缺陷。欧拉角本质上是用三个数去覆盖一个拓扑上不是环面、也不是球面的流形任何最小参数化都必然存在奇异只是位置不同。所以换一种欧拉角约定就能避开死锁这种想法是不成立的只能把奇点挪到你不常用的区域去。数值上怎么验证自己的代码有没有正确处理死锁我的做法很土但很有效构造一个 pitch 恰好等于 90 度的矩阵转成 RPY 再转回矩阵对比两者。如果回来的是 nan 或者完全不对说明缺了上面的分支处理。def check_gimbal_lock(): R rot_z(0.7) rot_y(np.pi/2) rot_x(0.3) r, p, y R_to_rpy(R) R2 rot_z(y) rot_y(p) rot_x(r) print(pitch , p, 重建误差 , np.max(np.abs(R - R2)))在死锁位形上重建出来的矩阵和原始矩阵应该完全一致差异在 1e-15 量级但反解出的 roll 和 yaw 可能和输入值不一样——因为不唯一。如果你的单元测试断言反解出的角度必须等于输入角度它在死锁点必然会失败。这个测试用例的写法本身就是个坑我建议改成断言重建矩阵一致鲁棒得多。3.4 反解分支与角度连续性除了死锁欧拉角还有两个实际使用中的麻烦。第一个是多解。同一个旋转矩阵通常有两组欧拉角解比如 RPY 里 (roll, pitch, yaw) 和 (roll180°, 180°-pitch, yaw180°) 都是合法解。反解函数默认返回哪一组取决于 atan2 的分支和 pitch 的取值范围。做轨迹规划时如果相邻两个路点的解不连续中间就会插出一条莫名其妙的翻转路径机械臂会突然抡半圈。解决办法是在反解之后做一次分支选择把候选解都算出来选和上一个时刻角度差最小的那一组。第二个是 ±180 度附近的跳变。角度从 179 度走到 -179 度数值上跳了 358 度但物理上只转了 2 度。对角度做求导、滤波、PID 控制时如果直接处理原始数值就会在跳变点产生巨大的冲击。标准做法是做角度归一化把差值折到 (-180°, 180°] 区间def wrap_pi(a): return (a np.pi) % (2 * np.pi) - np.pi这个函数短到不值得称为算法但我在几乎所有角度相关的代码里都会定义它而且只在计算两个角度的差时使用。越过一次 180 度导致的控制器震荡排查起来非常费时间因为现象是偶发的数据看起来也没错。从工程实践角度看我更倾向的结论是欧拉角和 RPY 适合做人的接口和局部表示不适合做内部的连续状态量。内部状态一律用旋转矩阵或四元数只在显示、日志、给操作员看的时候转成角度。这个原则帮我避免了大量和分支、跳变、死锁相关的杂事。4. 轴角、四元数与旋转的最小表示4.1 罗德里格斯公式从一个轴和一个角度长出整张旋转矩阵欧拉定理告诉我们任何一个三维旋转都可以表示成绕某一根固定轴转某一个角度。于是姿态可以用四元数之外的另一种方式描述一个单位向量 k 加上一个角度 θ总共四个数、一个单位长度的约束正好三个自由度。这个表示叫轴角物理意义极其清晰做视觉伺服和手眼标定时特别直观——误差就是绕某个方向转多少度。从轴角到旋转矩阵的转换由罗德里格斯公式给出R I sinθ · [k]× (1 - cosθ) · [k]ײ其中 [k]× 是由向量 k 构成的反对称矩阵也就是叉乘的矩阵形式。这个公式值得抄下来存着因为在力控和阻抗控制里力矩与姿态误差的关系直接和这个公式相关。它的一个直观解释是第一项是原始方向第二项控制小角度时的线性响应第三项补足旋转的非线性部分。def skew(k): return np.array([[0, -k[2], k[1]], [k[2], 0, -k[0]], [-k[1], k[0], 0]]) def axis_angle_to_R(k, theta): k np.asarray(k, dtypefloat) k k / np.linalg.norm(k) K skew(k) return np.eye(3) np.sin(theta) * K (1 - np.cos(theta)) * (K K) def R_to_axis_angle(R): theta np.arccos(np.clip((np.trace(R) - 1) / 2, -1.0, 1.0)) if theta 1e-8: return np.array([1.0, 0.0, 0.0]), 0.0 if abs(theta - np.pi) 1e-6: # 180 度附近反对称部分退化改用对角线求解 A (R np.eye(3)) / 2 k np.sqrt(np.clip(np.diag(A), 0, None)) idx int(np.argmax(k)) if k[idx] 1e-8: k A[:, idx] / k[idx] return k / np.linalg.norm(k), theta k np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]) k k / (2 * np.sin(theta)) return k, theta需要重点提醒的是 180 度附近的分支。当 θ 接近 π 时反对称部分 R - R^T 趋近于零矩阵用 k (R - R^T) / (2 sinθ) 求出来的轴会被噪声彻底淹没。这是我在做半个圆周翻转的关节标定时踩过的坑最后的做法是切到对角线的平方根方法就是上面代码里那一段。这类在某一点附近特殊处理的分支每个姿态表示法里都有写通用函数时必须考虑。4.2 四元数的半角和它的双覆盖四元数用四个数表示旋转q [w, x, y, z] 或者写成标量加向量的形式。单位四元数对应旋转表示方式是 q [cos(θ/2), sin(θ/2) · k]其中 k 和 θ 就是轴角表示里的轴和角度。注意那个二分之一角这是四元数最关键也最容易被忽略的细节。为什么是半角因为四元数是通过旋转的双覆盖来作用的单位四元数构成的空间 S³ 到 SO(3) 是一个二对一的映射q 和 -q 表示同一个旋转。这个性质带来两个直接后果。第一两个姿态之间的角度距离在四元数空间里是实际转角的一半做误差计算时不要忘了乘 2。第二四元数插值走的路程是实际旋转的一半弧长所以天然的插值结果比线性插值平滑得多。四元数的乘法在工程代码里天天用公式不难但容易写错某一项的符号。我习惯直接用固定写法def qmul(q1, q2): 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, ])写完一定要做回归测试用 qmul 把同一个旋转连续乘两次结果应该等于角度翻倍的同一根轴的旋转。这个小测试能在一分钟内抓出符号错误比对着公式检查半天快得多。另外从四元数转旋转矩阵的时候如果 q 没有归一化比如多次乘法累积了浮点误差转出来的矩阵会带一个整体缩放把位置一起放大现象是机械臂好像变长了一点。我一般在 qmul 之后立刻归一化代价极小。4.3 slerp为什么姿态插值必须走这条路两个姿态之间怎么平滑过渡是最能体现表示法差异的地方。对欧拉角做线性插值看起来简单实际会出大问题当两个姿态的欧拉角相差较大或者经过死锁区域时插值出来的路径会和最短旋转路径偏离很远甚至中途出现奇怪的翻转。原因还是那个老问题——欧拉角是三个独立的数线性插值等于在三个坐标轴上分别走直线这和旋转流形上的测地线不是一回事。四元数的球面线性插值slerp解决的正是这个问题。它的思路是在单位球面上沿着两个四元数之间的最短大圆弧走而不是在四维空间里走直线。公式不复杂工程实现上要注意两个细节。def slerp(q0, q1, t): q0 q0 / np.linalg.norm(q0) q1 q1 / np.linalg.norm(q1) d float(np.dot(q0, q1)) if d 0.0: q0 -q0 d -d if d 0.9995: q q0 t * (q1 - q0) return q / np.linalg.norm(q) th0 np.arccos(np.clip(d, -1.0, 1.0)) th th0 * t q2 q1 - q0 * d q2 q2 / np.linalg.norm(q2) return q0 * np.cos(th) q2 * np.sin(th)第一个细节是if d 0: q0 -q0。因为 q 和 -q 表示同一个旋转如果不做这一步插值可能沿着绕远路的方向走实际效果是末端突然转一个大圈。第二个细节是d 0.9995时切到线性插值因为此时 sinθ 接近零除法会数值不稳定。这两个分支我在每一份姿态插值代码里都写属于标准防护。还有一个常被忽略的问题slerp 给出的是姿态插值但通常我们还要同时插值位置两者用同一个 t 参数。这在大部分场景没问题但如果是抓取这类高精度任务建议对位置用较高次的多项式或者样条对姿态用 slerp然后把两者在时间上对齐而不是简单地都用线性。4.4 四种表示法到底该怎么选我把常用的几种姿态表示放在一起做个对照这张表我在项目里给新人讲过很多次。表示法参数个数冗余优点缺点典型用途旋转矩阵96 个约束复合简单、作用直观冗余大、迭代会漂移数值计算中间量、链式变换欧拉角 / RPY3无人易读、直观有死锁、多解、跳变显示、日志、人机接口轴角41 个约束物理意义清晰0 度和 180 度需分支误差定义、力控四元数41 个约束插值好、复合高效不直观、双覆盖内部状态量、姿态估计选型的基本逻辑是内部计算用旋转矩阵和四元数对外接口用人能看懂的角度。具体一点运动学链式求变换用 4×4 齐次矩阵最方便姿态估计滤波器的状态量用四元数姿态误差和期望力矩用轴角示教器显示和配置文件用 RPY。中间每一步转换都要写单元测试因为这些转换函数一旦写错错误会以看起来对了但就是不精确的形式潜伏很久。5. 齐次变换把位置和姿态打包进一次乘法5.1 4×4 分块和齐次坐标存在的理由有了旋转矩阵和位置向量一个坐标系相对另一个坐标系的完整描述就是一个 4×4 矩阵T_AB [[R_AB, p_AB], [0, 0, 0, 1]]左上 3×3 是姿态右上 3×1 是 B 系原点在 A 系中的位置最后一行固定是 [0, 0, 0, 1]。这个结构叫齐次变换矩阵它把旋转加平移这两件事统一成了一次矩阵乘法。齐次坐标的引入乍看像是为了凑维度实际上解决了两个根本问题。第一平移在三维里是加法旋转是乘法两者本来不能统一成一次线性运算扩到四维之后都能写成乘法。第二它顺便表达了点和向量的区别——点的第四分量是 1向量的第四分量是 0。这个区别非常实用一个向量经过变换矩阵时不应该被平移而一个点应该被平移。我强烈建议在代码里显式区分这两类数据。比如定义一个函数transform_point和一个transform_vector内部用不同的齐次分量不要图省事都用 1。把方向向量当点来变换是所有变换库里最隐蔽的错误之一因为它在平移量很小的时候几乎看不出来。5.2 链式相乘和求逆的解析式齐次变换最大的价值在于链式关系T_AC T_AB · T_BC。机械臂的正运动学就是把从基座到末端的每一段变换乘起来手眼标定就是把相机到末端的变换和基座到相机的变换串起来。整个过程是纯矩阵乘法写起来干净利落这也是为什么几乎所有机器人库都用齐次变换作为标准接口。更值一提的是求逆。数值求 4×4 矩阵的逆是通用操作但齐次变换的逆有解析形式而且比数值求逆快得多、稳得多T_BA T_AB^(-1) [[R_AB^T, -R_AB^T · p_AB], [0, 0, 0, 1]]这个公式的推导很简单逆变换的姿态就是转置位置则是把原点变换回去。它的实现只要几次矩阵乘法而且天然保持正交性不会像数值求逆那样引入误差。def inv_transform(T): R T[:3, :3] p T[:3, 3] Ti np.eye(4) Ti[:3, :3] R.T Ti[:3, 3] -R.T p return Ti这段代码我在每个项目里都会写一遍。用np.linalg.inv(T)也能跑但在几百赫兹的环路里解析形式的性能优势和数值稳定性优势都很实在。而且如果 T 因为浮点误差而不再是严格的齐次变换通用求逆会把这个误差原封不动放大解析式则不会。5.3 三个高频混用的错误第一个错误是变换顺序反了。T_AB · T_BC 和 T_BC · T_AB 完全不是一回事。判断方法很简单看下标能不能约掉能约掉的就是正确顺序就像单位换算一样。这个下标相消的检查法我在写多级变换链的时候每次都会用只要下标接不上基本就能断定顺序错了。第二个错误是齐次矩阵没有保持结构。比如做插值时对四个块独立插值插完之后左上角不再是正交矩阵导致缩放变形。正确做法是把旋转和平移分开插值旋转用 slerp插完再拼回齐次矩阵。第三个错误是混用坐标系。手眼标定里最容易出现因为涉及基座、末端、相机、标定板四个坐标系。我见过一个案例工程师把标定板的位姿从相机系直接当成了基座系下的目标位姿误差看起来很小因为相机离标定板近但机械臂怎么调都差几毫米。最后是在每个变换上强制加命名用静态检查工具禁止无命名的变换相乘才解决的。6. DH 参数让描述变成机械臂真能到的坐标6.1 四个参数各自管什么前面讲的都是两个坐标系之间怎么描述而机械臂正运动学的任务是给一组关节角末端在哪。中间需要的是一座桥这座桥就是 Denavit-Hartenberg 参数。标准 DH 约定下相邻两个连杆坐标系之间的变换由四个参数决定连杆长度 a_i沿前一关节轴方向的偏移、连杆扭转角 alpha_i两根相邻关节轴的夹角、连杆偏距 d_i沿关节轴方向的偏移、关节角 theta_i绕关节轴的转角。对于旋转关节theta_i 是变量其他三个是常值对于移动关节d_i 是变量。这个分工很干净所以 DH 参数一直是教科书的标配。单个连杆的变换可以写成四个基本变换的连乘顺序固定A_i Rz(theta_i) · Tz(d_i) · Tx(a_i) · Rx(alpha_i)展开之后就是那个非常标准的 4×4 矩阵def dh_transform(a, alpha, d, theta): ct, st np.cos(theta), np.sin(theta) ca, sa np.cos(alpha), np.sin(alpha) return np.array([ [ct, -st*ca, st*sa, a*ct], [st, ct*ca, -ct*sa, a*st], [0.0, sa, ca, d], [0.0, 0.0, 0.0, 1.0], ])这段代码短到可以背下来但它的正确性完全依赖于前面的约定。一旦换到别的约定矩阵形式就变了。6.2 标准 DH 与改进 DH 的账要算清这里必须说清楚一件事DH 参数有两套主流约定标准 DH 和改进 DH也叫 Craig 约定它们把坐标系固连的位置不一样。标准 DH 把坐标系固连在连杆的远端靠近下一个关节改进 DH 固连在近端靠近上一个关节。结果是同一个机械臂两套约定给出的参数表完全不同变换矩阵的乘法顺序也不同。改进 DH 的顺序是A_i Tx(a_{i-1}) · Rx(alpha_{i-1}) · Rz(theta_i) · Tz(d_i)。def mdh_transform(a_prev, alpha_prev, d, theta): ct, st np.cos(theta), np.sin(theta) ca, sa np.cos(alpha_prev), np.sin(alpha_prev) return np.array([ [ct, -st, 0.0, a_prev], [st*ca, ct*ca, -sa, -d*sa], [st*sa, ct*sa, ca, d*ca], [0.0, 0.0, 0.0, 1.0], ])我踩过的坑是从厂商手册抄参数表手册用的是改进 DH代码里写的是标准 DH 的变换函数结果末端位置差了十几厘米。查了半天坐标变换最后发现是两套约定混用。从那以后我定了一条规矩任何一个 DH 相关的文件第一行注释必须写明用的是哪套约定且不允许在一个项目里同时出现两套除非有明确的转换说明。6.3 手写正运动学并做闭环自检把参数表和变换函数凑在一起正运动学就是一行循环def forward_kinematics(dh_params, joint_angles): T np.eye(4) for (a, alpha, d, theta_offset), q in zip(dh_params, joint_angles): T T dh_transform(a, alpha, d, q theta_offset) return T跑通之后最重要的一件事是自检。我常用的自检方法有三种。第一种是零位检查。把所有关节角置零看末端位姿是否和机械臂手册上标注的零位一致。这一步能抓出大部分符号错误和偏置遗漏因为零位是已知的。第二种是单轴旋转检查。只让第一个关节转 90 度观察末端位置的变化是否符合几何直觉。比如基座关节转 90 度末端应该绕基座 Z 轴画弧高度不变。如果高度变了说明有轴的对应关系搞错了。第三种是数值微分检查。用有限差分算末端的线速度和角速度和雅可比矩阵的解析结果对比。这两者在不奇异的位置应该吻合到 1e-6 量级。这个方法稍复杂但它能抓出那种位置看着对、姿态旋向反了的隐蔽错误我建议在正式使用前至少做一次。提示正运动学自检最好写成固定的测试脚本每次改完参数就重跑一遍。我见过太多因为改了 DH 表里一个小数点导致整台设备行为异常的情况而这种问题靠肉眼翻参数表基本不可能发现。7. 姿态出错时的排查链路7.1 先查约定再查数据姿态问题排查有个原则不要在还不确定约定是否统一的时候去分析数值。所以第一步永远是核对约定链条。我会按顺序问自己四个问题输入数据的姿态用的是什么表示法它的旋转顺序是内旋还是外旋角度单位是度还是弧度它描述的是坐标系到坐标系还是物体到世界这四个问题里任何一个不清楚后面所有的计算都建立在不牢靠的假设上。实际项目里最省事的做法是找一组已知答案的数据做端到端验证。比如把相机正对一块标定板此时标定板相对相机应该有一个接近单位阵的旋转矩阵。如果算出来不是问题就在链条中的某个环节而不是在某个公式上。第二步才是查数值。查数值也有顺序先看旋转矩阵是否正交把 R^T R 打印出来非对角元应该在 1e-6 以内再看行列式是否为 1再看是否有明显的数量级异常比如某一行全是零那多半是数据没读进来。这三步一分钟就能做完却能筛掉一大半低级问题。7.2 可视化别用肉眼看九宫格九个数一屏人眼是分辨不出对错的。姿态调试一定要可视化。我常用的三种手段成本从低到高。第一种是打印三根轴的方向向量。旋转矩阵的每一列就是一根轴直接看第三列是否指向期望方向比看整张矩阵快得多。如果工具 Z 轴应该朝下而第三列接近 [0, 0, 1]那就是差了 180 度问题可能出在轴的定义上。第二种是在三维里画坐标轴。用一个简单的坐标系可视化工具把每个关键坐标系的三根轴画出来箭头方向一目了然。多关节情况下配合滑块调角度很快就能定位到是哪一段变换错了。这个手段在我定位手眼标定问题时效率最高。第三种是把姿态角随时间的变化画成曲线。如果在某个时刻曲线上出现尖刺那基本就是 ±180 度跳变或者分支选择出问题。这类问题只看单帧数据永远发现不了。我更倾向的做法是姿态相关的代码一旦改过先把曲线画出来看一遍再上真机。哪怕只是改了一个符号也值得多花两分钟。7.3 一张高频坑位对照表现象最可能的原因快速验证方式末端位置对、姿态整体差 180 度轴的定义相反例如 Z 指向内/外检查旋转矩阵第三列符号姿态在小角度对大角度偏欧拉角顺序或内旋外旋搞反用 90 度级别的组合旋转验证姿态角曲线出现尖刺±180 度跳变未归一化打印连续两帧的角度差反解出现 nanarcsin/arccos 输入越界加 clip检查死锁位形姿态缓慢漂移迭代连乘未做正交化打印 R^T R - I 的范数插值过程突然翻大圈四元数符号未统一检查相邻帧点积是否为负末端位置整体偏一点向量被当成点做了平移检查齐次分量用的是 0 还是 1这张表里的每一条我都在真实项目里遇到过其中姿态缓慢漂移和插值翻大圈这两条花的时间最长因为它们的现象不具备偶发性之外的特征数据看起来都没问题。8. 我自己用下来比较稳的几个习惯最后说说几个长期用下来觉得值得坚持的做法。第一个是边界转换原则整个系统内部只保留一种姿态表示。我的默认组合是齐次矩阵做链式变换、四元数做状态量、RPY 只用于显示。所有转换函数集中在同一个模块里每个函数都配单元测试。这样一来当出现问题时可怀疑的范围就压缩到了少数几个函数上而不是散落各处。第二个是数据自带坐标系原则。任何位置和姿态变量要么名字里带坐标系要么封装成带参考系标签的结构体。这条规则在只有一个人的项目里显得啰嗦但只要超过两个人协作回报立刻显现。我现在的习惯是连日志打印都带坐标系名回看几十条日志时能省下大量猜测。第三个是用重建法验证反解。任何一个从姿态反解角度的函数都要配一个反解再重建的测试断言重建出的旋转矩阵与原矩阵一致而不是断言角度相等。这个小改动让我的测试用例在死锁位形和 180 度附近都能稳定通过不再需要为特殊情况写例外。第四个是保留一组已知答案的基准数据。我手上有一份从实际设备采出来的姿态序列包含小角度、大角度、接近死锁、跨越 ±180 度这几类。每次改动姿态相关的代码先拿这份数据跑一遍看输出是否和上次一致。这比重新设计测试用例省事得多也更容易发现改了一个地方、影响了另一个地方的回归问题。如果后面还要往下扩展我建议的下一步是看看姿态误差在优化问题里怎么定义。因为一旦涉及轨迹优化或者标定求解误差函数写成 R1 - R2 的范数是不对的正确的写法是 log(R1^T R2) 这类基于李代数的方式。那个话题比姿态描述本身要绕一些但理解了今天这些表示法的来龙去脉之后再看它就不会觉得突兀了。
