简介面向机器人控制、工业自动化方向的开发者与学生这份材料围绕六轴机械臂位置规划展开采用简化几何解法处理机械臂在竖直与水平方向的平面运动并涉及七段S曲线速度规划帮助读者理解路径平滑、工作范围约束与运动精度之间的取舍。压缩包共5个文件约3KB以3个MATLAB脚本和2个数据文件为主脚本承载几何解算与运动规划的实现框架数据文件保存规划所需的数值及水平运动样本便于对照运行、修改参数并复现结果。已有1329人学习下载反映该主题在轨迹规划入门与课程实践中具有一定关注度。材料体量轻巧适合作为六轴机械臂位置规划的代码骨架与实验起点读者可据此梳理关节运动分解、理解数据与算法的对应关系并针对不同负载、速度或作业场景开展二次开发与验证。1. 反直觉的结论机械臂真正难的不是转多少度而是该转到哪做机械臂硬件的人和使用机械臂的控制代码是两件不同的事。前者关心减速比和编码器分辨率后者关心的往往是另一件事末端要走到 (x, y, z)六个关节各自该转多少度。这个从笛卡尔空间回推到关节空间的过程就是位置规划里最硬的一关。哪怕轨迹规划把每一个插补点都排得漂漂亮亮位置规划解不出来整条轨迹也只是躺在文件里的一串数字而已。几何解法是这条路上最老、也最不依赖算力的一种走法——它不迭代用一组三角公式一次性算出全部候选解代价是它对机械臂的几何结构有近乎苛刻的前提。值得搞懂的原因很朴素当某个节拍要求 1 ms 的项目里你发现数值迭代根本来不及能救场的往往就是这一组公式。而它翻车的方式也恰恰藏在“简化”这两个字里。2. 解耦的前提六轴臂凭什么能被拆成“位置三轴 姿态三轴”2.1 Pieper 准则划出的那条线不是所有六轴都有解析解六轴串联机械臂的逆运动学本质是一个六元非线性超越方程组。一般形式的六自由度机构这个方程组没有闭式解——它的解对应一个高阶多项式次数可以高到 16 次工程上只能靠数值迭代去逼近。这不是偷懒是数学上就没给这条路。绕开它的办法有两条一条是用雅可比矩阵做迭代另一条是干脆把机械结构设计成“有解”的形状。工业六轴绝大多数选了后者依据就是 Pieper 准则当相邻三个关节轴交于一点或者三个关节轴互相平行时逆运动学存在解析解。前一种做法最普遍就是把第 4、5、6 轴做成共点的球形腕。这个结构决定的直接后果是解耦。腕心第 4、5、6 轴的交点的位置只由第 1、2、3 轴决定腕部三个轴不管怎么转腕心纹丝不动它们只负责让末端相对腕心摆出一个姿态。一个六维问题被切成三维位置加三维姿态几何解法才有机会成立。反过来说如果一台机械臂的腕部三轴不共点几何解法就直接失效了这一点在选型阶段就该确认清楚。提示球形腕是几何解法的门票不是可选项。拿到一台新机器第一件事是翻手册确认腕部三轴是否共点而不是先动手写公式。2.2 DH 参数不是给仿真看的是几何解法的输入清单很多人把 DH 参数表当成建模用的元数据做完正运动学就丢在一边。几何解法里这张表是真正的输入。下面是一类典型六轴臂的改进 DH 参数用来说明哪些参数会被用到。关节 iα(i-1)a(i-1)d(i)θ(i)在几何解法中的角色10°0d1θ1底座高度进入平面二连杆的竖直偏移2-90°a10θ2肩部水平偏置多数构型里为 030°a20θ3大臂长度余弦定理的主角4-90°a3d4θ4小臂长度与肩部横向偏置590°00θ5只影响姿态不影响腕心位置6-90°0d6θ6法兰到腕心的距离解耦时先减掉看这张表会发现一件事几何解法真正吃进去的只有四个数——大臂长 a2、小臂长 a3、肩部偏置 d4、底座高 d1外加一个法兰到腕心的距离 d6。其余参数在推导过程中被结构本身消掉了。这正是“简化”二字的来源。它不去解六个未知数的方程组而是解三个一元或二元的三角方程每个方程都能用反三角函数直接写出闭式表达。计算量从上万次浮点迭代掉到几十次三角函数调用这才是几何解法能在高节拍场景里站住脚的根本原因。也要说清楚代价这四个数一旦测不准误差会直接、线性地映射到关节角上没有任何迭代过程帮你吸收。标定这件事在几何解法里没有后悔药可吃。2.3 解耦的第一步把 d6 从末端位姿里减掉末端位姿 T06 给的是法兰坐标系的位姿而几何解法需要的是腕心。这两者之间只差一个沿着末端 z 轴的长度 d6但符号和方向经常被写反。import numpy as np def wrist_center(T06, d6): 从末端法兰位姿反推腕心在基座坐标系下的位置。 T06 : 4x4 齐次变换矩阵末端法兰相对基座的位姿 d6 : 法兰坐标系原点到腕心的距离沿末端 z 轴方向为正 R T06[:3, :3] p T06[:3, 3] z_axis R[:, 2] # 末端 z 轴在基座系下的单位方向向量 return p - d6 * z_axis # 往回退而不是往前走这段代码只有三行但里面有两个容易出错的点。第一个是方向腕心在法兰的“后方”所以要减不要加写成加号会让整个求解结果稳定地偏出去两个 d6 的距离而且看起来还挺像那么回事非常难查。第二个是坐标系z_axis 必须是末端坐标系在基座系下的 z 轴也就是旋转矩阵的第三列不是第三行。行列写反在数值上不会报错只会让姿态稍微对不上属于典型的玄学 bug。还有一层容易漏的如果末端还挂了工具T06 里已经包含了工具坐标系得先把 TCP 偏移减掉回到法兰再减 d6。顺序错一步整条链就都歪了。3. 三段式推导底座角、平面二连杆、腕部欧拉角3.1 底座回转角 θ1一个 atan2 之外还有两个候选最朴素的写法是 θ1 atan2(py, px)也就是腕心在基座 XY 平面上的投影角。这个式子在肩部没有横向偏置时是对的一旦 d4 不为零就不成立了。原因是腕心并不落在底座回转轴上它被 d4 推开了一段距离。把这个几何关系画在水平面里底座到腕心的水平距离 r hypot(px, py)腕心相对回转中心的横向偏移是 d4两者构成一个直角三角形。于是回转角要先减掉一个修正角θ1 atan2(py, px) − atan2(d4, √(r² − d4²))这里的 atan2(d4, ρ) 就是那个修正角ρ √(r² − d4²) 是腕心在“手臂伸展方向”上的有效径向距离。这个式子里藏着一个硬约束r 必须大于等于 |d4|。如果算出来 r |d4|说明腕心落进了底座附近的一个圆柱区域几何上根本够不着直接判无解比硬算出一个 NaN 要体面得多。θ1 一般有两个候选解。第二个来自“肩部翻转”——手臂从底座另一侧绕过来够同一个点θ1 atan2(−py, −px) atan2(d4, ρ)这两个解在数学上都成立但在实际机械臂上通常只有一个能过关节限位。所以在实现里不要提前砍掉任何一个统一交给后面的筛选环节处理否则很容易出现“某些姿态下突然无解”的怪现象。3.2 投影降维把三维问题压成一个平面二连杆θ1 定下来之后三维空间里的事情就塌缩成一个平面问题了。在这个以 θ1 为法向的竖直平面里整条手臂退化成两根连杆底座到肘部是一根长为 a2 的杆肘部到腕心是一根长为 a3 的杆。腕心在这个平面里的坐标也很容易写出来。水平方向是带符号的径向距离 ρ上面那个竖直方向是 z pz − d1把底座高度减掉因为二连杆的起点在肩部而不是地面。这里要注意 ρ 的符号。选第一个 θ1 解时 ρ 取正选第二个解时 ρ 要取负因为腕心确实落在了伸展方向的反侧。负的 ρ 会让后面的 atan2 自动落到正确的象限这比手动加 π 要干净得多。很多实现里把 ρ 强行取绝对值结果就是肩部翻转的那一半解全部算错表现出来是“某些位置能解、某些位置解出来完全不对”。降维这一步是整个几何解法的核心动作。它之所以能成立靠的是 θ1 已经把手臂平面确定下来了——第 2、3 轴的运动完全被约束在这个平面内不会跑出去。如果机械臂的第 2 轴有倾斜角或者肩部有 y 方向的偏置这个平面假设就要重新审视公式也得跟着改。3.3 θ2 与 θ3余弦定理、肘部构型和那个必须夹紧的反余弦平面二连杆的逆解是高中内容但工程上出问题的地方一点不少。给定连杆长度 a2、a3 和末端相对肩部的位置 (ρ, z)肘部夹角由余弦定理给出cos θ3 (ρ² z² − a2² − a3²) / (2·a2·a3)这个式子有三个必须处理的地方。第一右边可能超出 [−1, 1]。原因通常是目标点在工作空间边界附近浮点误差把 0.9999999 推成了 1.0000001acos 直接返回 NaN。正确做法是先判断是否超过 1 加一个小容差超了就当作无解跳过在容差内则夹紧到 ±1 再算。夹紧不是掩盖错误边界点本来就该判为奇异构型强行算出一个值反而更危险。第二θ3 有两个解acos(c3) 和 −acos(c3)对应肘部向上和肘部向下两种构型。这两个解在物理上都是合法的手臂姿态走的是完全不同的空间路径。一个常见误区是只保留一个然后发现某些目标点解不出来——其实是另一个解能解只是被提前扔了。第三θ2 不能直接写成 atan2(z, ρ)。因为 θ2 的零位定义在连杆 a2 的方向上而腕心相对肩部的连线并不沿着 a2。要减掉一个由 θ3 决定的偏置角θ2 atan2(z, ρ) − atan2(a3·sin θ3, a2 a3·cos θ3)这个减号后的第二项本质上是“从肩部到腕心的连线”和“大臂方向”之间的夹角。写错这一项的表现很有迷惑性末端位置在部分区域是准的换个区域就整体偏移看起来像是标定问题实际是公式漏了一项。注意θ2 的零位约定和 DH 参数的写法强相关。换一套 DH 定义θ2 里通常要加减一个固定偏置常数。直接套用别人的公式而不核对零位是几何解法最常见的翻车来源之一。3.4 腕部反解ZYZ 分解与那个绕不开的死锁前三个关节解出来腕心位置就定了。剩下的三个关节只负责姿态思路是把末端旋转矩阵“剥掉”前三轴的贡献得到腕部自身的旋转R36 R03ᵀ · R06球形腕的三轴按 Z-Y-Z 顺序排列时R36 就是一个标准的 ZYZ 欧拉角旋转矩阵。分解过程很直接θ5 atan2(√(R36[0,2]² R36[1,2]²), R36[2,2])θ5 落在 [0, π]对应的 sin θ5 恒非负。只要 sin θ5 不接近零剩下两个角就能唯一确定θ4 atan2(R36[1,2], R36[0,2]) θ6 atan2(R36[2,1], −R36[2,0])再加上一组翻转解 (θ4 π, −θ5, θ6 π)腕部同样给出两个候选。前三个关节的 2×2 组解乘上腕部的 2 组一共 8 组候选这就是六轴臂“最多 8 组逆解”这个说法的由来。真正麻烦的是 sin θ5 ≈ 0 的退化情形也就是常说的万向节死锁。此时第 4 轴和第 6 轴的旋转轴重合姿态只约束了 θ4 θ6 的和θ5 0 时或差θ5 π 时单独两个角无法确定。工程上的处理是固定 θ4 0把全部旋转量分给 θ6这样解仍然连续可用代价是在这个位形附近关节角的变化率会突然变大。如果轨迹规划阶段要经过这个点最好提前在笛卡尔空间绕开而不是指望求解器在死锁点上表现优雅。4. 落地实现把公式写成能跑、能验证的求解器4.1 求解器主干一次遍历产出全部候选解把前面三段拼起来主干的形状就很清晰了先解耦出腕心再按 θ1 的两个分支分别投影每个分支里按 θ3 的两个符号各算一次最后对每组前三个角补上腕部的两组解。import math import numpy as np class SixAxisGeometricIK: def __init__(self, a2, a3, d1, d4, d6): self.a2, self.a3 a2, a3 # 大臂、小臂长度 self.d1, self.d4, self.d6 d1, d4, d6 def inverse(self, T06, eps1e-9): 返回所有候选关节角每组为长度 6 的 list。 R06, p06 T06[:3, :3], T06[:3, 3] px, py, pz p06 - self.d6 * R06[:, 2] # 解耦出腕心 r math.hypot(px, py) if r abs(self.d4) - eps: return [] # 腕心落进底座圆柱无解 rho math.sqrt(max(r * r - self.d4 * self.d4, 0.0)) phi math.atan2(py, px) delta math.atan2(self.d4, rho) if rho eps else math.copysign(math.pi / 2, self.d4) z pz - self.d1 # 去掉底座高度 sols [] # 两个肩部构型rad 取正/负分别对应腕心在伸展方向的两侧 for q1, rad in ((phi - delta, rho), (phi - math.pi delta, -rho)): c3 (rad * rad z * z - self.a2 ** 2 - self.a3 ** 2) / (2 * self.a2 * self.a3) if abs(c3) 1.0 1e-6: continue # 超出可达距离跳过 c3 max(-1.0, min(1.0, c3)) for q3 in (math.acos(c3), -math.acos(c3)): # 肘上 / 肘下 k1 self.a2 self.a3 * math.cos(q3) k2 self.a3 * math.sin(q3) q2 math.atan2(z, rad) - math.atan2(k2, k1) sols.extend(self._solve_wrist([q1, q2, q3], R06)) return [self._wrap(q) for q in sols] def _solve_wrist(self, q123, R06): R03 self._rot_prefix(q123) # 只用前三个关节算旋转 R36 R03.T R06 s5 math.hypot(R36[0, 2], R36[1, 2]) if s5 1e-6: q5 math.atan2(s5, R36[2, 2]) # 恒落在 [0, pi] q4 math.atan2(R36[1, 2], R36[0, 2]) q6 math.atan2(R36[2, 1], -R36[2, 0]) return [q123 [q4, q5, q6], q123 [q4 math.pi, -q5, q6 math.pi]] # 腕部翻转解 # 死锁第 4、6 轴共线固定 q4 0全部旋转交给 q6 q5 0.0 if R36[2, 2] 0 else math.pi q6 math.atan2(R36[1, 0], R36[0, 0]) return [q123 [0.0, q5, q6]] def _wrap(self, q): return [(x math.pi) % (2 * math.pi) - math.pi for x in q]几个参数值得单独说明。eps控制的是各类奇异的判断阈值设得太大正常位形会被误判成奇异解会莫名其妙变少设得太小浮点误差会漏过去acos 和除法都会失稳一般取 1e-9 到 1e-6 之间。_wrap把角度归一化到 [−π, π)这一步看着可有可无但它是后面连续性筛选能成立的前提——不然 179° 和 −179° 会被算成差了 358°。_solve_wrist里那个 1e-6 的阈值则专门用来识别死锁选得比它更小会让数值噪声被当成正常位形解出来的 θ4、θ6 会在零附近剧烈抖动。_rot_prefix只需要累乘前三个关节的旋转部分不用算完整变换这也是几何解法省时间的细节之一。整条路径里没有一次迭代所有开销都是固定次数的三角函数和矩阵乘法。4.2 多解筛选为什么不能只挑“第一个能用的”八组候选解摆在面前选哪一个直接决定运动质量。常见的做法是给每组解算一个代价取最小的。最基础的代价是行程对每个关节算当前解和上一周期解的差值先过 _wrap 归一化取绝对值加权求和。权重按关节的物理特性给——大关节转动代价高腕部关节便宜有的场合还会按惯量或功率加权。别小看这个权重全设 1 和按惯量设实际运动轨迹的平滑度差很多。第二层是关节限位过滤。这一步必须在代价计算之前做否则可能挑出一个合法但物理上转不到的解。限位还分软限位和硬限位轨迹规划阶段用软限位留出余量避免贴着限位运行时触发急停。第三层是避障和自碰撞属于可选但强烈建议的检查。几何解法本身对环境一无所知它只认数学。一个数学上完美、物理上会撞到工作台的解交给下游就是事故。def pick(self, sols, q_prev, limits, weights): best, best_cost None, float(inf) for q in sols: if any(not (lo qi hi) for qi, (lo, hi) in zip(q, limits)): continue # 越限直接淘汰 cost sum(w * abs((a - b math.pi) % (2 * math.pi) - math.pi) for a, b, w in zip(q, q_prev, weights)) if cost best_cost: best, best_cost q, cost return best这个函数里有个容易忽略的细节如果没有解通过筛选best会是 None。调用方必须处理这种情况比如保持上一周期的关节角、减速停止或者向上层报一条可恢复的错误。直接对 None 做下标访问程序在第一个奇异点就崩了。连续性跟踪还有一个变种做法不只看上一周期而是看未来几个插补点用一小段窗口做平滑。这在高速轨迹里效果明显代价是引入了前瞻延迟。要不要上取决于节拍和上位机的算力余量。4.3 闭环校验用正运动学打自己的脸几何解法写完最该做的一件事不是立刻接轨迹而是拿正运动学回头验一遍。做法很笨但极有效随机采样一堆关节角正运动学算出末端位姿把这个位姿喂给逆解检查原始关节角是否在返回的候选解集合里且每个候选解反算回去的末端位置误差都在 1e-9 量级。def self_check(ik, fk, n2000, tol1e-8): worst 0.0 for _ in range(n): q_true np.random.uniform(-math.pi, math.pi, 6) T fk(q_true) sols ik.inverse(T) if not sols: raise RuntimeError(f无解: {q_true}) # 工作空间内不该出现 errs [np.linalg.norm(fk(q)[:3, 3] - T[:3, 3]) for q in sols] worst max(worst, min(errs)) if min(errs) tol: raise RuntimeError(f残差过大: {q_true}, {min(errs)}) return worst这段校验能抓住绝大多数公式错误θ2 漏了偏置项、ρ 忘了带符号、d6 方向反了都会在这里暴露成“残差过大”或者“明明在工作空间里却报无解”。它抓不住的是标定误差和物理装配偏差——那些只能靠实测。区分这两类问题很重要前者改代码后者改参数表混在一起查会非常痛苦。5. 现场排查几何解法翻车的五种典型现场5.1 关节角在 ±π 处来回跳变现象机械臂走到某个方位时第 1 轴或第 4 轴的角度在两个值之间剧烈来回切换看起来像在抽搐但末端位置其实是同一个点。原因逆解返回的角度没有做归一化atan2 的输出刚好跨过 ±π 边界179.9° 和 −179.9° 在数学上只差 0.2°但在数值上差了将近 360°。如果下游的控制器按角度绝对值做插值就会要求电机从正方向一路绕回去。解决在求解器出口统一做一次角度归一化到 [−π, π)并且在计算行程代价时用归一化差值而不是原始差值。如果关节本身支持多圈连续旋转还要额外维护一个圈数偏移量把归一化后的角度还原成连续值再下发给伺服。5.2 某些目标点突然报无解但手动示教明明够得着现象工作空间边缘附近的点逆解返回空列表可手动拖着机械臂能走到那个位置。原因两个可能。一是余弦定理里的 c3 因为浮点误差超出了 [−1, 1]acos 直接返回 NaN被判定为无解。二是 r |d4| 的判据用了严格小于而且没留容差边界点在数值上刚好跨过去。解决给所有反三角函数的输入加夹紧给所有空间判据留 1e-6 量级的容差。同时把“无解”和“在边界上”区分成两种状态上报——前者是目标不可达后者是可以到达但处于奇异构型附近运动学上要减速通过。这两者混为一谈排查时会浪费大量时间。5.3 位置解对了姿态整体偏了 90° 或 180°现象末端位置精确落在目标点上但工具朝向差了整整一个直角或者半圈。原因腕部旋转矩阵 R36 R03ᵀ·R06 里R03 只累乘了前三个关节的旋转如果 DH 参数里的 α 角符号写反或者第 2、3 轴的旋转方向定义和公式不一致R03 就会差一个固定旋转。位置不受影响是因为腕心位置只和前三个关节的角度值有关和旋转矩阵的具体形式无关。解决把正运动学的旋转矩阵和求解器内部累乘出来的 R03 逐元素比对差异会立刻定位到是哪一轴的 α 或旋转方向出了问题。另一个高发点是欧拉角分解的顺序——ZYZ 和 ZYX 写反了解出来的 θ4、θ6 会互换表现出来也是姿态整体偏。5.4 换了一台同型号的机器同样的代码结果差几毫米现象代码一字没改参数表看起来也一样但新机器上末端位置稳定偏差 2~5 mm重复定位精度倒是不错。原因几何解法对标定误差没有吸收能力。a2、a3、d4、d6 这四个数哪怕只差 0.5 mm误差也会线性地映射到关节角和末端位置。同型号机器之间的装配公差、连杆加工误差、减速机回差都会让实际几何参数偏离名义值。解决做一次简单的几何参数辨识——让机械臂走若干组姿态用外部测量设备记录末端实际位置反解出真实的连杆长度和偏置更新参数表。哪怕只辨识 a2、a3、d4 这三个数末端精度通常也能改善一个量级。指望靠改代码补偿这类误差方向就错了。5.5 轨迹经过某个姿态时关节速度突然爆表现象笛卡尔空间里末端走得很匀速但第 4 或第 6 轴在某一段突然加速到限值甚至触发过流保护。原因轨迹穿过了腕部奇异位形sin θ5 接近零。此时 θ4 和 θ6 的可行解空间退化数值上的微小扰动会被放大成巨大的角度变化这就是死锁点附近关节速度发散的本质。它是几何结构决定的不是求解器的 bug。解决在轨迹规划阶段做奇异检测用雅可比矩阵的最小奇异值或者腕部角 θ5 的绝对值作为判据接近阈值时降低末端线速度或者在路径上绕开。另一个办法是在笛卡尔空间插补时改用带阻尼的逆解过渡牺牲一点末端精度换取速度连续性。硬扛着穿过去迟早会撞限位。6. 设计取舍闭式解、迭代解和你的 1 ms 节拍几何解法不是唯一选项也不是永远的最优选项。把三条常见路线摆在一起看取舍会清楚很多。方案单次求解开销能否枚举全部解对结构的依赖奇异点表现典型适用场景几何/解析解几十次三角函数调用微秒级能最多 8 组强要求球形腕等结构条件死锁处退化需单独处理高节拍、确定性要求强的场合雅可比伪逆迭代每次迭代一次矩阵求逆通常 5~20 次不能只收敛到初值附近的一组弱任意构型可用雅可比奇异需阻尼处理构型不规整、离线计算阻尼最小二乘与伪逆同量级多一次阻尼项计算不能弱阻尼项天然平滑不发散接近奇异、要求运动连续几何解法的优势是确定性和可枚举性。给定一个位姿它一定在固定次数的运算内给出全部解最坏情况的开销可以预估这对实时控制来说很重要。可枚举这一点在避障和构型切换里价值极大——你得先知道有哪些选择才能谈选哪个。它的劣势同样明确结构条件不满足就直接失效标定误差无法吸收奇异点要单独打补丁。而且“简化”本身就是一把双刃剑——每简化一步就有一个物理假设被固化进代码日后换机型、加工具、改装配都可能要重推一遍公式。我的建议是把它当成一个可以信赖但不该盲信的工具。前提条件先验清楚标定做扎实然后用正运动学闭环校验和实测两条腿走路。从那以后我每次把一套新的参数塞进求解器之前都会先跑一遍随机采样的闭环校验再拿实机在几个边界点上对比末端位置两个都过了才敢接轨迹。吃过一次亏某次 d4 的符号写反位置误差在两三毫米跑起来一切正常直到某台机器在特定姿态下撞了工装才发现。希望帮到你。这份资料里的推导和代码最合适的用法是对着自己机器人的参数表走一遍——公式是死的参数和结构才是活的。本文还有配套的精品资源点击获取