1. 人体手臂运动链七自由度的生物学设计图1.1 肩、肘、腕的自由度分配在真正上手七自由度机械臂之前我一直把它当成比六轴多一个轴的机器。直到有一次做末端姿态恒定的直线轨迹现场老师傅随口说了句让胳膊肘像人端水一样绕过去我才意识到这台机器在设计之初到底模仿了什么——人体手臂本身就是一条典型的七自由度运动链。从解剖学简化来看人体手臂从胸廓到指尖的自由度分配大致是这样肩关节近似一个球窝关节提供三个旋转自由度前屈后伸、外展内收、内外旋肘关节只有一个屈伸自由度前臂的旋前旋后贡献一个腕关节提供两个掌屈背伸、尺偏桡偏。加起来正好是 3 1 1 2 7 个有效自由度。这七个自由度里最耐人寻味的是手掌的位置和姿态只占六个约束。也就是说当你要求手掌稳稳地端住一杯水位置三个约束、姿态三个约束时手臂还剩下一个自由度没有用掉。这时你转动肘部让手肘在空间里画出一个圆杯子纹丝不动。这个多出来的自由度在机器人学里叫冗余自由度它直接对应了人类手臂运动学上的一个核心特征末端约束不唯一解。1.2 冗余带来的灵巧与工程映射人类在日常生活中几乎无时无刻不在利用这个冗余自由度。你从桌面拿杯子递到嘴边杯子始终水平姿态约束但肘部会在肩—腕连线附近走一条弧线避开水杯和身体之间的障碍你伸手去够一个高处的开关前臂会自然内旋或外旋让手腕处于最舒服的角度——这些都是末端位姿不变、肘部位置自由调整的实例。用机器人术语讲这些运动都发生在**自运动流形self-motion manifold**上。工程上的动机和生物上的动机完全一致冗余自由度可以拿来避障、躲开关节限位、提高灵巧度可操作性、降低某几个关节的运动负担甚至在任务中途灵活切换构型而不打断末端的轨迹。六自由度机械臂做不到这些因为它的逆解是若干组离散解没有连续调整的自由度。很多七轴协作臂宣传类人运动柔软避障本质都是在利用这个冗余自由度做文章。不过引入冗余自由度也带来了控制上的难题如何从无穷多个逆解中选出一个合理的数值迭代法对所有冗余机械臂都通用但速度慢、结果不稳定如果想要在高速控制周期内反复求解最稳妥的方式就是找到这个构型的解析解closed-form solution。下面就从人体仿生学的结构出发看看什么样的构型最适合推解析解以及解析解到底是怎么一步步算出来的。2. S-R-S构型三个球心如何把求解降维2.1 S-R-S是什么S-R-S 是 Spherical-Revolute-Spherical 的缩写翻译过来就是球—转—球构型从基座开始前三个关节的旋转轴交于一点构成一个等效的球形关节这一点叫肩中心 S中间一个关节是单纯的旋转关节构成肘中心 E后三个关节的旋转轴同样交于一点构成另一个球形关节这一点叫腕中心 W。整体看起来就是肩部球形关节 一个肘部转动关节 腕部球形关节自由度分配为 3 1 3 7。市面上不少七轴机械臂都在采用或近似采用这种构型KUKA LBR iiwa 系列、典型的七轴协作臂设计以及大量实验室自研的七轴臂核心骨架都可以纳入 S-R-S 的分析框架。不同厂家在连杆偏置、关节零位、轴方向上各有差异但肩部三轴交于一点 腕部三轴交于一点这个几何特征是通用的也是能推解析解的关键。Pieper 准则在六自由度机器人里广为人知只要存在三个相邻关节轴交于一点机械臂就存在闭式逆解。S-R-S 构型把这一准则用到了极致——不仅肩部三轴交于一点腕部三轴也交于一点于是整个逆解天然被拆成肩部决定肘点位置、腕部决定末端姿态两个子问题每个子问题都能用几何关系直接算出来。2.2 三点骨架 S-E-W一旦末端位姿给定腕中心 W 的位置就是确定的后面会详细讲怎么算。这时整个机械臂的骨架可以抽象成三个点肩中心 S、肘中心 E、腕中心 W。这条简化很重要中间那条完整的运动链根本不重要所有连杆、法兰、外壳都可以抛到脑后问题退化为一个纯几何问题——已知三角形 SEW 的两个边长肩长 L1、肘长 L2和第三边 SW记为 d求第三个顶点 E 的位置。E (肘点) /|\ / | \ L1 / | \ L2 / |h \ / | \ S-----F-----W c_u d-c_uSW 的长度 d 由末端位姿唯一确定。三角形三边 L1、L2、d 都是已知量三角形自然就固定了。但由于机械臂有冗余自由度当 S 和 W 固定时E 并不是唯一确定的——E 可以绕 SW 轴线旋转旋转一周形成的轨迹就是一个圆。这个圆正是整个解析解的核心几何对象。2.3 为什么球—转—球能大幅简化计算对比一般的七自由度臂如果肩部和腕部不是球关节肘点位置和腕部姿态会耦合在一起解耦就要靠数值方法处理。而 S-R-S 构型有两个球心做天然边界腕点位置只依赖前三关节因为腕中心就是后三关节轴的交点它在前三关节坐标系里的位置完全由前三关节决定后三关节只能改变腕部转动不改变腕点坐标。腕点一旦确定肘部关节角只依赖 d肘关节角 q4 只由三角形边 L1、L2、d 决定和臂角无关。这两个特性让整个逆解变成一条清晰的流水线先求腕点再求肘点再反推肩部三关节角最后反推腕部三关节角。每一步都是基础的三维几何运算没有任何迭代。下面逐步展开。3. 手臂平面和臂角ψ把冗余自由度显式化3.1 参考平面与臂平面的定义要显式处理那个多出来的自由度最优雅的办法是引入臂角arm angle记为 ψ。它的物理含义非常直观手臂平面绕肩—腕连线转过的角度。先定义两个平面参考平面过 SW 直线且包含世界坐标系竖直轴通常取基座坐标系的重力反方向记为向量 g的竖直平面。如果 SW 本身恰好竖直可以自定义一个备用方向。臂平面由 S、E、W 三个点张成的平面。臂角 ψ 的定义是臂平面绕 SW 轴相对参考平面旋转的角度。ψ 0 时臂平面与参考平面重合ψ 增大肘点 E 就沿圆形轨迹绕着 SW 轴移动。固定末端的六维位姿后给一个 ψ整个机械臂的所有七个关节角都被唯一确定。这样一来原来的7 自由度逆解、6 个任务约束、无穷多个解就变成了6 个任务约束 1 个显式参数 ψ一一对应——方程个数和未知数个数正好匹配。3.2 构造旋转坐标系为了把 ψ 变成可以写进公式的数字需要在肩中心 S 处建立一个随 SW 方向变化的局部坐标系。设u 是 SW 方向的单位向量u (p_w - p_s) / dv 是参考平面内垂直于 u 的单位向量由世界竖直向量 g 和 u 的叉积归一化得到v normalize(g × u)w 是第三个正交方向w u × v这样 U-V-W 构成一个右手正交坐标系u 指向腕点v 在参考平面内水平方向w 是参考平面内的另一个方向也可以理解为竖直平面内的斜向。在这个坐标系下肘点 E 的位置可以写成E(ψ) p_s c_u · u r · (cos ψ · v sin ψ · w)这个式子的几何意义非常清楚肘点先从 S 沿 u 方向前进一段距离 c_u到达垂足 F然后在垂直于 u 的平面由 v、w 张成内以半径 r 绕 SW 轴转 ψ 角。当 ψ 从 0 扫到 2πE 就画出一个完整的圆。3.3 几个关键的几何量用三角形 SEW 的余弦定理和勾股定理可以直接算出上式里的 c_u 和 r以及肘关节角 q4在三角形 SEW 中设垂足 F 到 S 的距离为 xF 到 E 的距离为 h则x² h² L1² (d - x)² h² L2²两式相减x² - (d - x)² L1² - L2²展开整理2dx - d² L1² - L2²c_u x (L1² - L2² d²) / (2d)于是r h sqrt(L1² - c_u²)肘关节角 q4 直接由余弦定理给出q4 arccos((L1² L2² - d²) / (2 · L1 · L2))注意 q4 只和 d 有关和 ψ 无关。这意味着冗余运动改变肘点在圆上的位置时肘关节本身的弯曲角度不受影响——这一点在后面的奇异分析和轨迹规划里很重要。这里有一个非常实际的小坑d 的取值必须在 |L1 - L2| 到 L1 L2 之间三角形才成立。一旦末端位姿让 SW 距离超出这个区间解析解直接失效机械臂在物理上也不可能到达该位姿。做好这个判断可以提前拦截大量无效目标点。4. 从末端位姿到全部七个关节角闭式解四步走4.1 第一步由末端位姿求腕点位置逆解输入通常是一个四阶齐次变换矩阵或者位置加旋转矩阵p_e, R_e。这里 p_e 是末端坐标系原点或者说末端工具点在基座标系下的位置R_e 是末端坐标系相对基座标系的姿态。腕中心 W 并不一定和末端坐标系原点重合。如果腕部中心相对于末端坐标系有一个固定的偏移向量 a_w那么腕点在基座标系下的位置是p_w p_e - R_e · a_w这个式子很容易理解把末端坐标系里的偏移量 a_w 通过 R_e 旋转到基座标系再从末端位置里减掉就得到腕点。很多机械臂的腕中心到末端法兰之间存在一个沿着末端 Z 轴方向的偏置a_w 就是 (0, 0, d_tool) 这样的量所以这一步不能省略。实际工程里还有个常见问题末端工具比如吸盘、夹爪会让 a_w 变得很大。如果你在计算时只拿到法兰盘位姿却忘了加上工具偏移逆解算出来的腕点会整体偏掉一个固定量机械臂末端实际走的轨迹就会整体偏移。我在集成视觉抓取时就因为这个偏移量漏加导致装配孔位差了 1 厘米。先把工具坐标系标定准确再进逆解流程这是最稳妥的顺序。4.2 第二步求肘点坐标和肘关节角腕点位置已知后算出 SW 距离d ||p_w - p_s||其中 p_s 是肩中心在基座标系下的固定位置。把这些量代入第三节的公式c_u (L1² - L2² d²) / (2d) r sqrt(L1² - c_u²)然后构造 u、v、w 三个单位向量肘点坐标就是E(ψ) p_s c_u · u r · (cos ψ · v sin ψ · w)同时肘关节角q4 arccos((L1² L2² - d²) / (2 · L1 · L2))这里要说明一点q4 的正方向取决于肘关节的转动正方向定义机械臂实际装配时可能存在镜像或反向需要根据具体机型的 DH 表对最终值做取负或取补处理。通用算法求出的是三角形内角工程上再加一个符号映射就好。关于臂角方向有个经验大多数情况下机械臂肘关节允许的活动范围远小于 180 度所以 q4 取 arccos 的默认主值区间 [0, π] 基本就够用。只有在少数特殊设计肘部能过伸的机型才需要考虑 q4 的另一支解。4.3 第三步肩部三个关节角反解拿到肘点 E(ψ) 后肩部的三个关节角 q1、q2、q3 就可以从肘点在肩坐标系下的坐标反推出来。具体做法是先把 E(ψ) 从基座标系变换到肩部第一关节的零位坐标系下。假设肩部的固定旋转矩阵为 R_s0由机械臂 DH 模型中基座到肩关节的固定连杆决定则肘点在肩坐标系的坐标是e_s R_s0^T · (E(ψ) - p_s)肩部三个旋转轴交于一点所以 e_s 的三个分量 (x_s, y_s, z_s) 和肩部三关节角之间存在直接对应关系。至于具体的公式取决于肩部三个轴的排列方式。最常见的构型是三个轴依次呈 Z-Y-Z 或 Z-Y-X 欧拉角排列。以 Z-Y-Z 为例肘点在肩坐标系的坐标经过三次旋转后其分量可以表示为e_s [sin(q2)·cos(q1)·sin(q3) cos(q1)·cos(q2)·cos(q3)?] 或者多项组合式。实际工程中我不会手推这种展开式而是直接利用旋转矩阵的解析反解把肩部的旋转矩阵 R_shoulder 拆成三个基本旋转的乘积R_shoulder Rz(q1) · Ry(q2) · Rz(q3)根据矩阵元素的 atan2 关系反解三个角。更通用的做法是调用一个通用的旋转矩阵到欧拉角函数把 R_shoulder 映射到约定的欧拉角。重要的是理解这一步的几何本质肘点在以肩中心为球心的球面上肩部三个关节角不过是在定位这个球面点两个角决定经纬度再调整一个绕自身轴的旋转第三个角。球面点坐标和姿态旋转一旦确定三个角度就确定了。4.4 第四步腕部三个关节角反解肩部三关节角和肘关节角都已知后腕部旋转矩阵可以从姿态链里解出来。整个末端姿态可以写成R_e R_shoulder(q1, q2, q3) · R_elbow(q4) · R_wrist(q5, q6, q7)注意 R_elbow 不仅包含肘关节转动 q4 本身还包括经过肘关节的两段连杆之间固定存在的旋转比如 DH 模型里的连杆扭转和连杆偏置带来的常值旋转。所以实际计算时通常是先用机械臂正运动学的中间结果把前三关节 肘关节这一段的旋转矩阵 R_0_4 算出来然后R_wrist (R_0_4)^T · R_e得到腕部的 3×3 旋转矩阵后再按腕部三个轴的排列方式反解 q5、q6、q7。这一步和第三步本质一样都是三维旋转矩阵到欧拉角的反解这里不再重复展开。需要特别留心的是腕部结构偶尔会引入附加旋转有些机型为了走线方便在腕部球心附近加了一个固定偏置旋转导致腕部三轴的坐标系方向和理想模型有夹角。这种情况下直接用 (R_0_4)^T 乘出来得到的 R_wrist 里会混入偏置项。稳妥的处理方式是把整个多出来的常值旋转矩阵并入 R_0_4保持前段矩阵 腕部三轴的干净分解。4.5 全流程伪代码把上面四步串起来的算法流程可以写成下面这段伪代码def ik_srs(p_e, R_e, psi, params): # params: p_s, L1, L2, a_w, R_s0, 关节轴定义等 # Step 1: 计算腕点位置 p_w p_e - R_e params.a_w # Step 2: 计算 SW 距离和几何量 sw_vec p_w - params.p_s d norm(sw_vec) # 几何可用性检查 if d abs(L1 - L2) or d L1 L2: return None # 目标超出工作空间 u sw_vec / d c_u (L1*L1 - L2*L2 d*d) / (2*d) r sqrt(max(L1*L1 - c_u*c_u, 0.0)) # Step 3: 构造正交基 v, w g np.array([0, 0, 1.0]) if norm(np.cross(g, u)) eps: g np.array([1.0, 0, 0]) # SW竖直时的备用参考轴 v normalize(np.cross(g, u)) w np.cross(u, v) # Step 4: 肘点位置由 psi 决定 e p_s c_u * u r * (cos(psi) * v sin(psi) * w) # Step 5: 肘关节角 q4 acos((L1*L1 L2*L2 - d*d) / (2*L1*L2)) # Step 6: 肩部三角反解 e_s R_s0.T (e - p_s) q1, q2, q3 euler_from_shoulder_frame(e_s, R_s0, ...) # Step 7: 腕部三角度反解 R_0_4 fk_first_four_joints(q1, q2, q3, q4, params) R_wrist R_0_4.T R_e q5, q6, q7 euler_from_wrist_matrix(R_wrist, ...) return [q1, q2, q3, q4, q5, q6, q7]这段伪代码就是整个解析解算法的主干。只要把每个机型的 DH 参数、固定旋转矩阵、轴方向约定替换成实际值就能直接嵌进控制程序。整个流程不涉及任何迭代现代 CPU 上单次求解在微秒级完全满足高频控制需求。5. 边界条件和奇异解析解最容易翻车的地方5.1 手臂完全伸展/完全折叠的退化当 SW 距离 d 趋近 L1 L2 时手臂完全伸直三角形 SEW 变成一条线段垂足 F 无限接近 Wr 趋近 0。这时肘部圆的半径为零肘点被钉死在 SW 直线上臂角 ψ 失去物理意义——无论 ψ 取什么值肘点都是同一个点。反过来当 d 趋近 |L1 - L2| 时手臂完全折叠同样发生三角形退化。在这些退化位置附近解析解虽然在数值上还能算出一个结果但 ψ 对关节角的映射非常敏感ψ 微小变化会导致肘点几乎不动肩部关节角出现剧烈跳变。运行路径规划时应该把这两条边界作为奇异禁区处理或者在规划阶段就对 d 的最大最小值做硬约束。5.2 参考平面退化SW 竖直时怎么办构造 v normalize(g × u) 的前提是 g 和 u 不平行。当 SW 方向恰好竖直u 平行于 g时叉积结果是零向量v 无法定义参考平面本身就变得不确定。这时再谈ψ 0 对应竖直参考平面就没有意义了。我的处理方法是定义一个备用参考轴if norm(np.cross(g, u)) eps: g np.array([1.0, 0, 0]) # 改用基座标系 X 轴但要小心切换参考轴会让 ψ 的定义发生偏移同一个物理构型在不同参考轴下的 ψ 值不同。如果运动过程中反复跨越竖直区间ψ 曲线会出现跳变。解决手段是记录切换前后的肘点位置把新的 ψ 值对齐到旧 ψ 值上保证轨迹连续性。具体做法是切换后先用旧 ψ 算一个肘点位置再把这个肘点在新基下对应的 ψ 求出来用这个值继续。5.3 腕部姿态反解的万向节锁腕部三个旋转轴交于一点本质上就是一个球关节用欧拉角表示姿态时不可避免会遇到万向节锁当第二个旋转角接近 0 或 π 时第一个和第三个旋转轴的旋转方向变得平行两个角无法被唯一确定数值上表现为 atan2 的输入接近零结果极不稳定。这在 S-R-S 解析解里很常见因为腕部姿态反解本质上就是一次欧拉角反解。工程上有两种应对思路在姿态反解时改用旋转向量或四元数差值先计算期望姿态和当前姿态之间的误差再用误差旋转向量分解到三个腕部关节轴方向在选臂角阶段加入腕部奇异度约束把接近奇异的构型直接排除掉。前者偏实时控制后者偏规划实际系统往往两者配合。5.4 臂角ψ与关节限位的区间映射七自由度解析解给出的是给定 ψ 的一个唯一解但机械臂每个关节都有自己的限位并不是所有 ψ 都可行。把每个关节的限位映射回 ψ 空间可以得到一组可行区间q_i(ψ) ∈ [q_i_min, q_i_max] → ψ ∈ [ψ_i_low, ψ_i_high]对所有关节求交集得到总的可行 ψ 区间集合。这一步在轨迹规划里特别有用它把关节限位约束从七个关节的维度折叠成一维的 ψ 区间后续避障和优化只需要在一维空间里处理计算量小很多。这里有个很隐蔽的坑q_i(ψ) 往往不是单调函数可能在 [0, 2π) 内有多个交替的可行段。直接对 ψ 做均匀采样再逐个检查关节限位反而是最简单可靠的方式——反正单次逆解只要微秒级扫一遍 400 个点也不到 10 毫秒。追求极致的场合再用符号计算求出 q_i(ψ) 的解析表达式做区间精确求解。6. 选臂角从一维扫描到真实控制周期6.1 目标函数怎么定给定末端位姿ψ 决定整个构型。选 ψ 就是在自运动流形上选一个点目标函数可以根据任务需求自由组合关节限位可行性硬约束优先保证所有关节在限位内避障肘部是机械臂最大的突出物最容易撞障碍物可以定义肘点到最近障碍物的距离让 ψ 远离危险区间操作性度量最大化 det(J·J^T) 或最小化条件数让机械臂在给定姿态下保留最大的运动灵活度平滑性让当前关节角尽量接近上一控制周期的关节角减小运动突变。实际项目中我习惯分三层第一层是限位硬约束第二层是避障硬约束第三层是平滑性和操作性等软约束加权求和。软约束之间再根据场景调权重比如装配场景更看重平滑性搬运场景更看重避障。6.2 扫描加精修的两阶段策略虽然解析解是闭式解但目标函数往往是非凸的直接做梯度优化容易陷进局部最优。我的做法是两阶段第一阶段在 [0, 2π) 上均匀采样 N 个 ψ。N 取 100 到 500 之间每个样本点跑一次完整解析解评估目标函数保留最优的几个候选区间。第二步在最优候选区间内用黄金分割搜索或拟牛顿法做局部精修把 ψ 的精度提到理想水平。这套策略在实测中的性能数据很有参考价值单次解析逆解在我的测试机上平均耗时约 15 微秒采样 400 个点加局部精修总耗时约 8 毫秒。相比 1 ms 到 2 ms 的六自由度数值迭代这个速度看起来慢但它换来的是确定性和可重复性——同样的输入永远得到同样的输出不会像数值法那样受初值影响。而 8 毫秒在 100 Hz 到 1000 Hz 的控制周期里完全够用尤其是用于离线轨迹规划和在线路径重规划非常稳定。6.3 相邻控制周期的连续性处理如果目标函数没有剧烈变化最优 ψ 在相邻控制周期里应该连续变化。每次控制循环都从头做全局扫描其实没必要可以先以上一周期的最优 ψ 为初值在一个小邻域比如 ±20 度内做局部搜索只有当局部搜索结果明显劣于全局扫描结果时才触发一次全局扫描。实测中这个策略可以把平均单周期计算时间压缩到 1 毫秒以内同时保持 ψ 轨迹连续无跳变。还有一个细节ψ 是周期变量0 和 2π 是同一个值。做差值、滤波、平滑时必须先把 ψ 映射到连续空间处理不能用普通线性运算否则在 2π 附近会出现假跳变。一般做法是维护一个累加变量把每次的 ψ 增量做角度回绕处理。6.4 一次实测案例绕过障碍的 L 型轨迹最后分享一个完整的实测片段末端执行器从 A 点直线运动到 B 点中间有一个立柱障碍物正好挡在肘部活动空间里。第一版控制直接用局部避障算法结果机械臂在通过障碍时肘部离立柱最近只有 3 厘米非常惊险后来改成在轨迹规划阶段为每个路径点预先选好 ψ让肘部沿着一条与立柱保持 15 厘米以上距离的弧形曲线运动。具体做法是均匀取路径上的 50 个目标点对每个点采样 360 个 ψ计算每个样本的肘点位置和障碍物距离筛选出可行区间再在相邻路径点之间做 ψ 的连续性约束限制 ψ 最大变化率。最终运行时ψ 从 20 度平滑变化到 135 度全程无跳变肘部最小离障碍距离约 13 厘米关节角速度无超限。整个规划过程在离线阶段完成耗时不到 150 毫秒在线阶段固定 ψ 曲线做解析逆解每个周期 50 微秒以内。这个案例里最值得体会的不是逆解本身而是把问题转换到 ψ 空间的威力三维空间的避障问题被折叠成一维参数曲线上的一段可行区间物理意义清晰计算量又小。这比在关节空间里做数值逆解再试探构型要直观得多。P.S. 一个实操小技巧如果你把 ψ 的可行区间和对应的关节角打印出来观察会发现很多机械臂在大多数目标点下可行 ψ 区间都相当宽裕。这时刻意选择偏向自然的 ψ比如让肘部自然下垂的构型往往比强行优化某个指标更好用——机械臂动作会意外地顺眼而且不容易吓到旁边的人。
