手里那台四轴桌面臂去年折腾了我整整两个周末。现象很典型示教好的抓取点跑十几遍之后末端总往一个方向偏五六毫米换一组姿态又好了。我一开始怀疑舵机虚位、怀疑结构刚度拆了装装了拆最后发现问题出在逆解上——4自由度机械臂正逆解公式推导与代码实现这件事看着是课本里的老题目真落到一台带舵机、带装配误差的机器上坑全在推导之外的细节里。这篇东西我打算按自己实际推一遍、写一遍、调一遍的顺序来组织。核心是那套四轴平面构型一个绕竖直轴回转的基座加上三个互相平行、都垂直于基座回转轴的水平关节。这套构型的正解能写成非常干净的形式逆解也能完全闭式求解不需要迭代。适合正在做毕业设计、做桌面级机械臂、或者要给舵机臂写运动控制的人参考。有线性代数和一点三角函数基础就能跟上我把每一步为什么这么建坐标系、为什么这么化简都写出来。1. 四轴机械臂的解为什么比六轴更拧巴很多人第一次接触四轴会下意识把六轴那套位置和姿态各三自由度、腕部三轴交于一点的经验直接搬过来然后发现公式怎么推都不对。问题不在推导能力在于任务维度和驱动维度对不上。先把这件事捋清楚后面的公式才写得下去。1.1 三加一四轴真正能干的事末端在空间里的完整描述是六维的三个位置分量 x、y、z三个姿态分量通常用欧拉角或者旋转矩阵来描述。六轴机械臂有六个驱动刚好一对一匹配所以任意一个可达位姿都有解。四轴只有四个驱动从维度上就是欠约束的你不可能同时把六个分量都指定死。那四轴靠什么工作两条路。第一条是把姿态要求交给结构本身去满足比如末端装一个始终朝下的平行夹爪或者末端是吸盘、是喷头它天然只关心位置和一个朝向角。第二条是显式降维只指定三个位置分量加一个平面内的姿态角加起来四维正好对上四个驱动。我这里用的是第二条路。所谓平面内姿态角指的是末端连杆在基座回转后的那个竖直平面里相对于水平方向的夹角。这个角度是可以自由指定的也是四轴最有用的那个额外自由度——它决定了你的夹爪是垂直向下扎还是斜着伸进去。提示如果你手上的四轴任务必须控制末端绕竖直轴的偏航角那基本做不到。要么加一个第五轴要么在末端加一个独立的舵机做工具旋转别硬在运动学里绕。1.2 三种常见的四轴构型解法难度差很多四轴机械臂的物理形态差别很大选错构型会让逆解从一页纸变成必须迭代。我按可解性从易到难排一下。第一种是平面关节型也就是本文主推的构型J1 绕竖直轴回转J2、J3、J4 三条轴线互相平行且垂直于 J1 轴线整个臂在 J1 转过之后的那个竖直平面里运动。这种构型位置只由 J1、J2、J3 决定姿态由 J4 补偿可以做到完全闭式求解没有迭代、没有初值依赖。第二种是全回转型四个轴都平行于竖直方向类似 SCARA 的思路。这种构型末端只能在一个水平面附近工作Z 方向通常靠一根丝杠或者一个直线模组实现严格说不能叫四自由度串联。它的平面逆解就是经典的两连杆问题非常简单但工作空间很扁。第三种是带平行四边形的码垛构型。为了把驱动电机放到基座附近减小惯量厂家会用一套平行四边形连杆把 J3 的动力引过去。这种机构在运动学上等效于平面三连杆但连杆参数和关节角的映射需要额外处理而且平行四边形会引入耦合标定的时候要多一组参数。还有一种常见的是偏置腕构型末端最后一个关节的轴线与前两个不共面好处是姿态能力更强坏处是腕部不再解耦闭式解会变得很难看。入门阶段我不建议碰。构型位置解姿态能力闭式解难度平面关节型J1J2J3平面内一角低纯几何全回转型平面两连杆几乎无极低平行四边形码垛型等效三连杆平面内一角中需处理耦合偏置腕型三连杆稍强高常需数值解1.3 腕部解耦是能不能拿到闭式解的分水岭闭式解能不能推出来关键看一点能不能把位置问题和姿态问题拆开。在六轴里这个条件是腕部三个轴交于一点满足了就能把末端位置减去腕心偏移先算腕心位置再反推腕部三个角。四轴这边条件更宽松一些因为姿态只要求一个角。只要末端工具那段连杆的长度已知、方向由最后一个关节角唯一决定就可以先按目标姿态把这段连杆扣掉把问题退化成两连杆的位置逆解。我上面说的平面构型天然满足这个条件L3 的方向完全由 q2q3q4 决定扣掉之后腕心就落在一个两连杆可达的平面上。反过来说如果你的机构里末端工具和最后一个关节之间还夹着一个偏置量比如夹爪安装面偏离了关节轴线那扣掉的时候就要做一次旋转和平移的复合公式会长一截但思路不变。我建议在三维建模阶段就把这个偏置尽量做成零能省掉后面一大堆调试。2. 从坐标系到末端位姿四轴正解的完整推导正解是整个运动学的地基。逆解的每一个公式都是从正解里反推出来的正解推错了逆解调一周也调不出来。这一节我按建系—写变换—连乘—化简四步走每一步都把容易搞反的地方标出来。2.1 连杆坐标系按四条规则建别凭感觉我用的是标准 DH 约定。每个连杆固定一个坐标系四条规则Z 轴沿关节轴线方向指向任意但建议统一朝下一个连杆。X 轴沿相邻两根 Z 轴的公垂线从 Z(i-1) 指向 Zi。原点取在 X 轴与 Zi 轴的交点。Y 轴由右手定则补出来不用单独定义。四条规则里最容易翻车的是第二条X 轴的方向有正负两种选法选错会导致 a 和 α 都取反最后正解出来的位置是镜像的。我的经验是统一让 Z 轴指向远离基座的方向这样 X 轴的方向就唯一确定了不会有歧义。还有一个坑当两根 Z 轴平行的时候公垂线不唯一X 轴可以在整个平面内平移。这时候不要慌把原点放在关节轴线的交点上即可a 取连杆实际长度α 取 0。这正是我们这套构型里 J2 到 J4 的情况三根轴线全平行所以 α 全是 0非常规整。建完系之后四个关节的 DH 参数如下表。其中 q1 到 q4 是关节变量L1、L2、L3 是大臂、小臂、末端段长度h 是肩关节相对基座底面的高度。连杆 iθidiaiαi1q1h090°2q20L103q30L204q40L30这里 α1 取 90° 而不是 −90°是为了让第二个坐标系的 Y 轴朝上指向基座竖直方向。这个符号选择会让平面内的 y 坐标直接对应世界系的高度增量后面公式能省掉一堆负号。如果你取 −90°所有涉及高度的公式都要多一个负号很容易在写代码时漏掉我在第一次实现时就栽在这上面正解是对的逆解写出来 z 一直是反的。2.2 DH 变换矩阵的每一项到底代表什么相邻两个坐标系之间的变换是四步绕 Z 转 θ、沿 Z 移 d、沿 X 移 a、绕 X 转 α。写成矩阵就是T_i Rz(θi) · Tz(di) · Tx(ai) · Rx(αi)展开成 4x4 齐次矩阵T_i [ cθ -sθcα sθsα a·cθ sθ cθcα -cθsα a·sθ 0 sα cα d 0 0 0 1 ]这个矩阵不用死记理解成两块就行。左上角 3x3 是旋转部分描述第 i 个坐标系相对于第 i−1 个坐标系的姿态右上角 3x1 是平移部分描述原点的位置。平移部分之所以是[a·cθ, a·sθ, d]而不是简单的[a, 0, d]是因为沿 X 移 a这个动作是在已经转过 θ 的坐标系里做的所以要先转 θ 再平移。代码实现import numpy as np def dh_transform(theta, d, a, alpha): 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], ])有个细节值得提np.cos和np.sin接受的是弧度如果你的关节角输入是角度制先转弧度。我在接口层统一规定内部一律用弧度只在最外层跟用户交互时转角度这样可以避免在十几个函数里反复写np.deg2rad。2.3 四个矩阵连乘为什么结果能化简得这么干净把四个矩阵按顺序乘起来得到从基座到末端的变换 T04。手动乘确实麻烦但因为我们这套构型的 α2、α3、α4 全是 0中间三个矩阵的旋转部分退化成纯 Z 轴旋转平移部分只有 X 方向分量。这让连乘的结果有非常强的规律。具体展开T12、T23、T34 的左上角都是Rz(qi)平移是[Li·cos(qi), Li·sin(qi), 0]的齐次形式。把它们按顺序作用于原点就得到末端在第二坐标系平面内的坐标。再乘上 T01平面内的径向分量投影到世界 X 和 Y高度分量加上 h。最后的结果写成世界坐标p_x x_l · cos(q1) p_y x_l · sin(q1) p_z h y_l其中x_l L1·cos(q2) L2·cos(q2q3) L3·cos(q2q3q4) y_l L1·sin(q2) L2·sin(q2q3) L3·sin(q2q3q4)这个形式的物理意义非常直观x_l 是从基座回转轴到末端的水平距离也就是极坐标里的半径y_l 是末端相对肩关节的高度。q1 负责把这个半径甩到正确的方位角上其余三个角全在平面内解决。很多人把这个结构叫柱坐标加平面臂名字很贴切。再看末端姿态。末端连杆在平面内的方向角 φ 就是三个角之和φ q2 q3 q4这个 φ 就是前面说的第四维。它决定了夹爪在平面内的朝向比如 φ −90° 表示末端连杆竖直向下此时夹爪是垂直扎下去的这是最常见的抓取姿态。2.4 用一组数据把正解验一遍推导对不对跑一组数就知道。取 L1 0.20 mL2 0.18 mL3 0.08 mh 0.15 m关节角 q [30°, 45°, −60°, 15°]弧度制分别是 0.5236、0.7854、−1.0472、0.2618。手算平面坐标q2 45°L1 分量(0.20·cos45°, 0.20·sin45°) (0.1414, 0.1414)q2q3 −15°L2 分量(0.18·cos(−15°), 0.18·sin(−15°)) (0.1739, −0.0466)q2q3q4 0°L3 分量(0.08, 0)所以 x_l 0.1414 0.1739 0.08 0.3953y_l 0.1414 − 0.0466 0 0.0948。世界坐标p_x 0.3953·cos30° 0.3423p_y 0.3953·sin30° 0.1977p_z 0.15 0.0948 0.2448。用代码跑一遍对比def fk(q, L, h): T np.eye(4) params [ (q[0], h, 0.0, np.pi / 2), (q[1], 0.0, L[0], 0.0), (q[2], 0.0, L[1], 0.0), (q[3], 0.0, L[2], 0.0), ] for theta, d, a, alpha in params: T T dh_transform(theta, d, a, alpha) return T q np.deg2rad([30, 45, -60, 15]) T fk(q, [0.20, 0.18, 0.08], 0.15) print(T[:3, 3]) # 期望 [0.3423, 0.1977, 0.2448]两个结果一致说明建系和参数表都没问题。这一步千万别跳我见过太多人直接往下推逆解结果正解里 α1 的符号就错了逆解怎么调都差一个镜像。3. 逆解推导把六维问题拆成转台角度加平面三连杆逆解是整个事情的核心。给定末端目标位置 (px, py, pz) 和目标姿态角 φ求四个关节角。我的思路是分三步剥洋葱先定 q1再定平面内的两连杆最后用姿态角把 q4 补出来。3.1 第一步基座回转角由水平投影直接锁定从正解看p_x 和 p_y 只跟 q1 和 x_l 有关p_x x_l·cos(q1), p_y x_l·sin(q1)两边平方相加得 x_l² p_x² p_y²所以x_l sqrt(p_x² p_y²) 取正根这里有个选择x_l 取正还是取负取正表示机械臂永远朝外伸取负表示可以从基座轴的另一侧够过去。工程上一般取正因为躯干在中间会干涉而且取负会让 q1 的解变成两族相差 180°。我在实现时直接固定 x_l ≥ 0把 q1 唯一化这样后面的多解筛选只集中在肘部那两个分支上逻辑清爽很多。那么 q1 就是q1 atan2(p_y, p_x)用 atan2 而不是 atan是因为它能正确处理四个象限还在 p_x p_y 0 时返回 0不会抛除零错误。目标正好落在基座轴的正上方时x_l 0q1 变成不确定的这时候机械臂理论上可以绕圈实际处理方式是保持上一次的 q1 不变只动平面关节。平面内的目标高度是y_l p_z − h到这里三维问题已经变成平面问题了已知平面内目标点 (x_l, y_l) 和目标姿态角 φ求 q2、q3、q4。3.2 第二步用姿态角把末端连杆扣掉退化成两连杆这一步是整个逆解里最漂亮的技巧。既然末端连杆的方向角 φ 已经指定那么这段连杆的贡献是完全已知的末端连杆从腕心指向末端向量是 (L3·cos φ, L3·sin φ)。反过来说腕心坐标就是目标点减去这段向量x_w x_l − L3·cos(φ) y_w y_l − L3·sin(φ)腕心wrist center是个重要概念它在这里指的是第三关节轴线上的那个点。扣掉末端连杆之后剩下的就是一个标准的两连杆机构从肩关节出发经过 L1、L2 到达腕心。注意如果末端工具不是装在第四个关节的轴线上而是有个安装偏置那么扣掉时要先把这个偏置旋转到当前姿态下再扣公式变成x_w x_l − L3·cos(φ) − b·cos(φβ)这种形式。我的建议是结构设计阶段就让偏置为零。现在目标变成已知 (x_w, y_w)用长度 L1 和 L2 的两连杆去够它。这是一个从高中就开始接触的问题用余弦定理。3.3 第三步余弦定理求肘部两个根对应两种构型两连杆能构成三角形的前提是目标点到原点的距离 D 满足 |L1 − L2| ≤ D ≤ L1 L2D² x_w² y_w² cos(q3) (D² − L1² − L2²) / (2·L1·L2)cos(q3) 在 [−1, 1] 之外就说明目标不可达要么太远够不着要么太近缩不回来。数值实现里必须做 clamp否则 acos 会返回 NaN一路传下去会让舵机指令变成垃圾值。这是我踩过的坑某次目标点算出来出界 0.3 mmacos 直接 NaN整条轨迹卡死排查了半天才发现是边界问题。在定义域内q3 ±acos( cos(q3) )两个符号对应两种肘部构型。取正号是肘部向上相对腕心方向逆时针偏取负号是肘部向下。物理上手肘可以朝上也可以朝下两种都能到达同一个点这也是四轴逆解多解的主要来源。有了 q3q2 用角度差来算q2 atan2(y_w, x_w) − atan2( L2·sin(q3), L1 L2·cos(q3) )这个公式的推导思路是第一段连杆的方向角 目标方向角 − 三角形内角。前半段atan2(y_w, x_w)是肩到腕心的方向后半段是利用三角形内角关系算出来的修正量。用 atan2 的差形式比用 asin 或 acos 稳健得多因为它不会在分母趋零时炸掉。最后q4 φ − q2 − q3把姿态角减掉前两个角剩下的就是第四个关节该补的角度。注意 q4 不需要再做归一化处理的话实际下发前要把它绕到 [−π, π] 区间因为舵机是有行程限制的多转一圈会撞限位。3.4 多解筛选限位优先连续性次之加权距离兜底前面一算肘部就有两个解加上 x_l 取负的话还有更多。实际下发只能选一个。我的筛选顺序是这样的。第一步硬限位过滤。每个关节都有实际能到的角度范围比如 q3 只能在 [−140°, 5°] 之间因为你不可能让手肘往后折断。把不满足限位的分支直接扔掉。这一步通常就能砍掉一半的解。第二步连续性优先。如果机械臂正在运动优先选和上一时刻关节角最接近的解。具体做法是计算每个候选解与上一时刻解的欧氏距离可以加权比如基座关节的位移权重小一点因为它转一点点末端就动很多选距离最小的。这能避免机械臂在运动过程中突然翻肘那种动作既吓人又容易撞到东西。第三步工作空间偏好。如果两个解都满足限位且距离相近我一般优先选肘部向上的那个因为这种构型下末端离基座回转轴更远夹爪不容易碰到自己的大臂。这是经验性的具体看你的结构。def select_solution(candidates, q_prev, limits, weights(1.0, 1.0, 1.2, 0.8)): best, best_cost None, np.inf for q in candidates: if not np.all(q limits[:, 0]) or not np.all(q limits[:, 1]): continue dq np.array(q) - np.array(q_prev) cost np.sum(np.abs(dq) * np.array(weights)) if cost best_cost: best, best_cost q, cost return best这段代码有个细节值得说权重不要全取 1。第三个关节肘部对末端位置的杠杆最长它的变化最容易造成末端大幅移动所以权重稍微给大一点能有效抑制肘部反复翻转。4. 推导不成立的时候奇异性、不可达与数值兜底闭式解再漂亮也有它算不出来的地方。工作空间边界、奇异位形、目标点漂移出界这些在实机上天天发生。我的原则是闭式解优先出问题立刻切数值解兜底同时给出一个最近可达点作为降级方案绝不能因为解不出来就卡死整个控制循环。4.1 奇异位形的判定与处理平面两连杆的奇异位形有两类。第一类是边界奇异对应 cos(q3) ±1也就是 q3 0 或者 q3 ±180°。此时两段连杆共线机械臂完全伸直或者完全折叠。这时候 D L1 L2 或 |L1 − L2|雅可比矩阵的秩从 2 掉到 1q2 和 q3 的具体分配变得不确定——你可以让 q2 转 10°、q3 转 0°也可以让 q2 转 0°、q3 转 10°末端位置完全一样。处理办法很直接检测到 |cos(q3)| 0.999 的时候固定 q3 为一个预设的小角度比如 5°让 q2 单独去补位置。这样虽然牺牲了一点精度但姿态是确定的不会出现关节乱跳。第二类是腕心过原点也就是 x_w y_w 0。这时候atan2(0, 0)无意义q2 完全不确定。物理上腕心正好落在肩关节轴线上两段连杆的贡献互相抵消。处理方法同样是固定一个角让另一个去补。第三类在四轴上还存在是基座奇异x_l 0 时 q1 不确定。处理方式是保持上一时刻的 q1 不变只调整平面关节。4.2 出界时不要硬算投影到最近可达点目标点超出工作空间是很常见的尤其是做示教的时候手一抖多拖了几厘米。硬算会得到 NaN直接下发会让舵机的目标位置变成一个巨大的非法值。正确的做法是把目标点投影到工作空间边界上。对于这套构型投影可以按径向和高度分别处理。先算腕心的模长 D 和目标方向如果 D L1 L2就把腕心按方向缩放到 L1 L2 的长度上如果 D |L1 − L2|就缩放到 |L1 − L2|。高度方向同理如果超过基座顶部的极限高度截断到极限。def project_into_workspace(x_w, y_w, L1, L2): D np.hypot(x_w, y_w) D_min, D_max abs(L1 - L2), L1 L2 if D 1e-9: return D_min, 0.0 # 退化到肩轴线附近 D_c min(max(D, D_min), D_max) scale D_c / D return x_w * scale, y_w * scale这样即使目标不可达机械臂也会朝着目标方向尽可能地伸过去而不是原地不动或者乱动。在做拖拽示教的时候这个行为体验好很多。4.3 数值迭代雅可比加阻尼最小二乘当闭式解因为各种原因失效或者你想做纯位置的逆解不指定 φ只有三个坐标约束就需要数值方法了。四轴的雅可比矩阵是 3x4 的J [ ∂p_x/∂q1 ∂p_x/∂q2 ∂p_x/∂q3 ∂p_x/∂q4 ∂p_y/∂q1 ∂p_y/∂q2 ∂p_y/∂q3 ∂p_y/∂q4 ∂p_z/∂q1 ∂p_z/∂q2 ∂p_z/∂q3 ∂p_z/∂q4 ]把正解对四个角分别求偏导结果是∂p/∂q1 [ −x_l·sin(q1), x_l·cos(q1), 0 ] ∂p/∂q2 [ cos(q1)·dx2, sin(q1)·dx2, dy2 ] ∂p/∂q3 [ cos(q1)·dx3, sin(q1)·dx3, dy3 ] ∂p/∂q4 [ cos(q1)·dx4, sin(q1)·dx4, dy4 ]其中dx2 −L1·sin(q2) − L2·sin(q2q3) − L3·sin(q2q3q4)dy2 L1·cos(q2) L2·cos(q2q3) L3·cos(q2q3q4)dx3少掉第一项dx4只剩−L3·sin(q2q3q4)。逐项对着正解求导就能得到写代码的时候建议用符号计算工具自动生成手推容易漏项。注意 J 是 3x4 的不是方阵不能直接求逆。最朴素的做法是伪逆Δq J⁺ · e其中 e 是当前末端位置和目标位置的误差。伪逆的问题在于靠近奇异位形时 J 的最小奇异值趋近于零伪逆会给出巨大的关节增量机械臂会突然抽搐。解决办法是阻尼最小二乘Δq Jᵀ · ( J·Jᵀ λ²·I )⁻¹ · eλ 是阻尼系数取值在 0.01 到 0.1 之间比较常用。λ 越大越稳定但收敛越慢越小越快但在奇异附近越容易爆。我的做法是根据最小奇异值动态调 λ正常情况取 0.01检测到接近奇异时提到 0.1。def dls_step(J, e, lam0.05): A J J.T (lam ** 2) * np.eye(3) return J.T np.linalg.solve(A, e)迭代过程就是不断算误差、更新 Δq、限幅、再算误差直到误差小于阈值比如 0.1 mm或者迭代超过 50 次。限幅非常重要每次迭代最多让关节动 3° 到 5°防止一步跨到奇异区。4.4 两种解法的分工实际代码里我是这么安排的每次收到目标位姿先用闭式解快速算一遍。闭式解成功且所有候选都满足限位直接用耗时在微秒级。闭式解失败不可达、退化、限位全不满足切数值解同时把目标投影到工作空间内。数值解迭代次数也给它限死超时就直接保持当前位置不动并在日志里打一条警告。这样做的理由是闭式解是确定性的同一个目标永远给同一个结果这对重复精度很关键数值解依赖初值稍微变一下初值结果就差一点不适合做主路径。但它作为兜底非常可靠尤其在闭式解因为浮点误差在边界上抖动的时候。5. 代码工程化从单个函数到能跑的抓取动作公式推完了落到工程上还有一堆事。我习惯把运动学相关的代码拆成几个独立模块这样调试的时候能单独测每一层不会一锅粥。5.1 模块划分和数据流我的目录结构大概是这样的arm_kin/ config.py 连杆参数、关节限位、舵机标定表 transform.py dh_transform 和基本位姿工具 kinematics.py fk / ik 闭式解 / jacobian solver.py 多解筛选、工作空间投影、数值兜底 trajectory.py 关节空间与笛卡尔空间插值 driver.py 角度到舵机脉冲的映射与限速数据流是单向的上位机给一个目标位姿solver 出关节角序列trajectory 把序列插成密集的点driver 把每个点转成舵机指令。每一层都只依赖上一层的输出不反向调用。这样如果末端不动我可以直接打印关节角看是谁的锅如果是舵机响应慢那问题一定在 driver 层。5.2 用正解回代做验证别只看眼睛写完逆解第一件事不是接舵机是做回代验证。随机在关节空间里采一千组样本跑正解得到末端位姿再把这个位姿丢给逆解比较两次的关节角差和位置差。errs [] rng np.random.default_rng(42) for _ in range(1000): q rng.uniform(limits[:, 0], limits[:, 1]) T fk(q, L, h) p, phi T[:3, 3], np.arctan2(T[1, 2], T[0, 2]) # 从旋转矩阵提取平面姿态角 q_sol ik(p[0], p[1], p[2], phi, L, h) if q_sol is None: continue T_back fk(q_sol, L, h) errs.append(np.linalg.norm(T_back[:3, 3] - p)) print(np.max(errs), np.mean(errs))正常情况下最大误差应该在 1e-9 量级均值在 1e-12 量级。如果最大误差到 1e-3 以上说明某处有 bug通常是 atan2 的参数顺序反了或者 q4 归一化时把角度卷错了。我建议把这个测试做成单元测试每次改运动学代码都跑一遍比在实机上试快一百倍。还有一个容易被忽略的点从旋转矩阵里提取 φ 的时候要确认你提取的是正确的那个分量。对于这套构型末端连杆在平面内φ 可以通过atan2(T[1,2], T[0,2])或者取 T04 的前两列关系得到。第一次写的时候我提取错了列导致回代误差一直在 0.3 rad 附近查了半天才反应过来是提取方式不对。5.3 关节角到舵机脉冲零位、方向和限速运动学算出来的关节角是纯数学量跟真实舵机之间隔着一层标定。这层标定有三个参数。第一个是零位偏移。机械臂装配完成后关节角的零点几乎不可能正好对上数学模型的零点。比如数学上 q2 0 表示大臂水平实际装出来可能是水平再往上 3°。这个偏移要单独测量并加到映射里。映射公式是pulse pulse_center (q_math q_offset) * dir * pulse_per_rad其中pulse_center是舵机中位脉冲典型值 1500 us但不同舵机差别很大pulse_per_rad是每弧度对应多少微秒由舵机行程决定比如 270° 行程对应 2000 us 的话就是 2000 / (270/180×π) ≈ 424 us/rad。dir取 ±1用来修正装配方向。第二个是方向符号。不同关节的装配方向可能相反正向关节角对应舵机顺时针还是逆时针必须实测确认。我的做法是给一个小的正向角度指令看末端实际往哪走走反了就翻符号。第三个是限速与限加速。舵机尤其是总线舵机不能瞬间从 0° 跳到 90°那样会拉低母线电压、触发过流保护甚至打齿。我的做法是在 driver 层做梯形速度规划把关节角序列按最大角速度比如 60°/s和最大角加速度重采样。提示总线舵机通常会回报当前位置可以用这个反馈做闭环修正。但不要设太高的增益舵机的内部 PID 已经调过了你在外面再套一层容易震荡。5.4 关节空间插值和笛卡尔空间插值怎么选插值方式的选择取决于任务。如果是点位抓取比如从料盘拿一个东西放到另一个位置两个点之间怎么走不重要那用关节空间插值最省事。把起止两组关节角做五次多项式插值速度加速度都连续机械臂走起来很柔和。五次多项式的系数可以通过起止位置、速度、加速度六个边界条件唯一确定。如果是喷涂、涂胶、画线这类需要末端走直线的任务就必须用笛卡尔空间插值。做法是在起止位姿之间做直线插补每隔 1 mm 取一个点每个点都跑一次逆解。这里有个坑直线路径可能穿过奇异位形或者工作空间外的区域。我的做法是提前采样路径上的点做可达性检查发现不可达就把路径分段绕行而不是等走到一半再报错。还有一点笛卡尔插值得到的关节角序列相邻点之间的关节增量可能不均匀有些地方大有些地方小。我会再检查一遍最大关节增量如果超过安全值就在那个位置加密采样点。6. 装到机器上之后才会暴露的问题偏差、标定与补偿代码跑通只是开始。真实机械臂上会有一堆模型之外的误差这一节讲我实际遇到的处理方法。6.1 末端偏差的排查顺序末端重复定位不准原因可能有一堆。我的排查顺序是从大到小、从粗到精。先看重复性。让机械臂走同一个点二十次看末端标记点在多大范围内散。如果散度在 1 mm 以内说明重复性好问题出在绝对精度上也就是标定不准。如果散度超过 3 mm那先查机械结构关节虚位、连杆松动、同步带松紧、舵机齿轮间隙。再看绝对误差。重复性好但绝对精度差通常是三个原因连杆实际长度和模型不一致、零位偏移量测错、装配导致关节轴线不平行。这三个里零位偏移的影响最大因为它是角度误差会随连杆长度放大。基座零位差 1°末端在 400 mm 处就偏 7 mm。最后看区域相关性。如果误差在某个工作区域特别大别的地方都还好那很可能是坐标系建立的问题或者那个区域接近奇异位形误差被雅可比放大了。现象可能原因排查手段散度大3mm虚位、间隙、松动手动晃动每个关节看旷量散度小但整体偏零位偏移、连杆长度测量实际连杆重标零位特定区域偏大邻近奇异、坐标系错检查该区域雅可比条件数偏的方向随姿态变化关节轴线不平行建模时引入轴线夹角参数6.2 用几个已知点拟合参数是最省事的标定方法完整的运动学标定是个大工程涉及二十多个参数。对桌面级四轴来说没必要走那么远。我的做法是只标四个参数三个连杆长度和基座零位。具体操作在机械臂工作空间里选 6 到 8 个点用外部测量手段可以用一个带刻度的工作台或者装个相机看标记点记录末端的实际坐标。然后构造一个最小二乘问题让模型在这些点上的正解与实际测量值的误差平方和最小优化变量就是四个参数。用 scipy 几行就能写from scipy.optimize import least_squares def residual(params, q_samples, p_measured): L1, L2, L3, dq1 params preds [] for q in q_samples: q_adj q.copy() q_adj[0] dq1 T fk(q_adj, [L1, L2, L3], h) preds.append(T[:3, 3]) return (np.array(preds) - p_measured).ravel() res least_squares(residual, x0[0.20, 0.18, 0.08, 0.0], args(q_samples, p_measured))优化完通常能把误差从几毫米压到零点几毫米。注意不要用太多的参数去拟合太少的点那会过拟合在测量点上是准的别的地方反而更差。经验上参数数量的三倍是点的最低数量。6.3 重力矩对跟随误差的影响这是四轴舵机臂上最容易被忽视的一点。大臂水平伸出的时候肩关节承担的重力矩最大舵机要输出更大的电流才能保持位置。如果在重载姿态下要求舵机快速运动它可能跟不上指令产生明显的跟随滞后。表现出来就是末端轨迹在负载大的那段鼓出去一块。解决办法是加前馈的重力矩补偿。理论上肩关节需要补偿的力矩等于各段连杆的重力乘以对应的力臂τ m1·g·r1·cos(q2) m2·g·r2·cos(q2q3) m3·g·r3·cos(q2q3q4)其中 mi 是各段质量ri 是重心到关节的距离。这些参数可以用称重和估算得到不需要很精确能补偿到 70% 就能显著改善跟随误差。具体怎么用这个力矩如果是电流控制的舵机直接叠加到输出电流上如果是位置控制的舵机只能通过限制运动速度来间接缓解——在负载大的姿态区间把速度调低。我用的是后者在轨迹规划阶段根据当前姿态估算重力矩动态调整该段的角速度上限。提示重力矩补偿只在前馈层做别想着用大增益反馈去修正舵机内部已经在做这件事了外面再加一层会让系统变硬遇到扰动更容易震荡。6.4 手眼关系核对走一个十字最快如果机械臂上装了相机做抓取相机和末端之间的变换关系手眼关系必须标定。完整标定需要棋盘格和一堆求解但日常核对有没有明显错误有个土办法让末端走一个标准的十字轨迹用相机看末端标记点的轨迹是不是十字。如果相机看到的轨迹是十字但比例不对说明尺度有问题通常是相机内参或者安装高度不对。如果轨迹是个平行四边形或者歪掉的十字说明手眼旋转矩阵有偏差。如果轨迹整体平移了说明平移量不对。这个方法不能精确标定但能在五分钟内判断出问题出在哪个环节非常实用。手眼标定的精度直接影响抓取成功率。我的经验是如果抓取精度要求是 2 mm手眼标定的残差应该控制在 0.5 mm 以内留出足够余量给运动学误差和机械误差。这套东西我从第一版跑通到稳定前前后后改了六七轮。早期版本我只用数值解图省事结果每次断电重启、初值一变同一个点的关节角就不一样重复精度惨不忍睹。改成闭式解为主、数值解兜底之后同一个目标点每次给的结果完全一致重复精度才真正稳定下来。后来我又加了一个小改进把上一次成功求解的关节角存下来作为下一次的初值和解筛选的参考。这个简单的状态保持让机械臂在连续抓取多个相邻点位时动作连贯了很多不再出现两个点之间肩膀突然扭一下的情况。如果你也在这条路上折腾我建议优先把正解和逆解的回代验证做扎实传感器和舵机的问题可以慢慢查运动学公式错了后面全是白费功夫。
四轴机械臂正逆解推导与代码实现：闭式解、多解筛选与工程调参
手里那台四轴桌面臂去年折腾了我整整两个周末。现象很典型示教好的抓取点跑十几遍之后末端总往一个方向偏五六毫米换一组姿态又好了。我一开始怀疑舵机虚位、怀疑结构刚度拆了装装了拆最后发现问题出在逆解上——4自由度机械臂正逆解公式推导与代码实现这件事看着是课本里的老题目真落到一台带舵机、带装配误差的机器上坑全在推导之外的细节里。这篇东西我打算按自己实际推一遍、写一遍、调一遍的顺序来组织。核心是那套四轴平面构型一个绕竖直轴回转的基座加上三个互相平行、都垂直于基座回转轴的水平关节。这套构型的正解能写成非常干净的形式逆解也能完全闭式求解不需要迭代。适合正在做毕业设计、做桌面级机械臂、或者要给舵机臂写运动控制的人参考。有线性代数和一点三角函数基础就能跟上我把每一步为什么这么建坐标系、为什么这么化简都写出来。1. 四轴机械臂的解为什么比六轴更拧巴很多人第一次接触四轴会下意识把六轴那套位置和姿态各三自由度、腕部三轴交于一点的经验直接搬过来然后发现公式怎么推都不对。问题不在推导能力在于任务维度和驱动维度对不上。先把这件事捋清楚后面的公式才写得下去。1.1 三加一四轴真正能干的事末端在空间里的完整描述是六维的三个位置分量 x、y、z三个姿态分量通常用欧拉角或者旋转矩阵来描述。六轴机械臂有六个驱动刚好一对一匹配所以任意一个可达位姿都有解。四轴只有四个驱动从维度上就是欠约束的你不可能同时把六个分量都指定死。那四轴靠什么工作两条路。第一条是把姿态要求交给结构本身去满足比如末端装一个始终朝下的平行夹爪或者末端是吸盘、是喷头它天然只关心位置和一个朝向角。第二条是显式降维只指定三个位置分量加一个平面内的姿态角加起来四维正好对上四个驱动。我这里用的是第二条路。所谓平面内姿态角指的是末端连杆在基座回转后的那个竖直平面里相对于水平方向的夹角。这个角度是可以自由指定的也是四轴最有用的那个额外自由度——它决定了你的夹爪是垂直向下扎还是斜着伸进去。提示如果你手上的四轴任务必须控制末端绕竖直轴的偏航角那基本做不到。要么加一个第五轴要么在末端加一个独立的舵机做工具旋转别硬在运动学里绕。1.2 三种常见的四轴构型解法难度差很多四轴机械臂的物理形态差别很大选错构型会让逆解从一页纸变成必须迭代。我按可解性从易到难排一下。第一种是平面关节型也就是本文主推的构型J1 绕竖直轴回转J2、J3、J4 三条轴线互相平行且垂直于 J1 轴线整个臂在 J1 转过之后的那个竖直平面里运动。这种构型位置只由 J1、J2、J3 决定姿态由 J4 补偿可以做到完全闭式求解没有迭代、没有初值依赖。第二种是全回转型四个轴都平行于竖直方向类似 SCARA 的思路。这种构型末端只能在一个水平面附近工作Z 方向通常靠一根丝杠或者一个直线模组实现严格说不能叫四自由度串联。它的平面逆解就是经典的两连杆问题非常简单但工作空间很扁。第三种是带平行四边形的码垛构型。为了把驱动电机放到基座附近减小惯量厂家会用一套平行四边形连杆把 J3 的动力引过去。这种机构在运动学上等效于平面三连杆但连杆参数和关节角的映射需要额外处理而且平行四边形会引入耦合标定的时候要多一组参数。还有一种常见的是偏置腕构型末端最后一个关节的轴线与前两个不共面好处是姿态能力更强坏处是腕部不再解耦闭式解会变得很难看。入门阶段我不建议碰。构型位置解姿态能力闭式解难度平面关节型J1J2J3平面内一角低纯几何全回转型平面两连杆几乎无极低平行四边形码垛型等效三连杆平面内一角中需处理耦合偏置腕型三连杆稍强高常需数值解1.3 腕部解耦是能不能拿到闭式解的分水岭闭式解能不能推出来关键看一点能不能把位置问题和姿态问题拆开。在六轴里这个条件是腕部三个轴交于一点满足了就能把末端位置减去腕心偏移先算腕心位置再反推腕部三个角。四轴这边条件更宽松一些因为姿态只要求一个角。只要末端工具那段连杆的长度已知、方向由最后一个关节角唯一决定就可以先按目标姿态把这段连杆扣掉把问题退化成两连杆的位置逆解。我上面说的平面构型天然满足这个条件L3 的方向完全由 q2q3q4 决定扣掉之后腕心就落在一个两连杆可达的平面上。反过来说如果你的机构里末端工具和最后一个关节之间还夹着一个偏置量比如夹爪安装面偏离了关节轴线那扣掉的时候就要做一次旋转和平移的复合公式会长一截但思路不变。我建议在三维建模阶段就把这个偏置尽量做成零能省掉后面一大堆调试。2. 从坐标系到末端位姿四轴正解的完整推导正解是整个运动学的地基。逆解的每一个公式都是从正解里反推出来的正解推错了逆解调一周也调不出来。这一节我按建系—写变换—连乘—化简四步走每一步都把容易搞反的地方标出来。2.1 连杆坐标系按四条规则建别凭感觉我用的是标准 DH 约定。每个连杆固定一个坐标系四条规则Z 轴沿关节轴线方向指向任意但建议统一朝下一个连杆。X 轴沿相邻两根 Z 轴的公垂线从 Z(i-1) 指向 Zi。原点取在 X 轴与 Zi 轴的交点。Y 轴由右手定则补出来不用单独定义。四条规则里最容易翻车的是第二条X 轴的方向有正负两种选法选错会导致 a 和 α 都取反最后正解出来的位置是镜像的。我的经验是统一让 Z 轴指向远离基座的方向这样 X 轴的方向就唯一确定了不会有歧义。还有一个坑当两根 Z 轴平行的时候公垂线不唯一X 轴可以在整个平面内平移。这时候不要慌把原点放在关节轴线的交点上即可a 取连杆实际长度α 取 0。这正是我们这套构型里 J2 到 J4 的情况三根轴线全平行所以 α 全是 0非常规整。建完系之后四个关节的 DH 参数如下表。其中 q1 到 q4 是关节变量L1、L2、L3 是大臂、小臂、末端段长度h 是肩关节相对基座底面的高度。连杆 iθidiaiαi1q1h090°2q20L103q30L204q40L30这里 α1 取 90° 而不是 −90°是为了让第二个坐标系的 Y 轴朝上指向基座竖直方向。这个符号选择会让平面内的 y 坐标直接对应世界系的高度增量后面公式能省掉一堆负号。如果你取 −90°所有涉及高度的公式都要多一个负号很容易在写代码时漏掉我在第一次实现时就栽在这上面正解是对的逆解写出来 z 一直是反的。2.2 DH 变换矩阵的每一项到底代表什么相邻两个坐标系之间的变换是四步绕 Z 转 θ、沿 Z 移 d、沿 X 移 a、绕 X 转 α。写成矩阵就是T_i Rz(θi) · Tz(di) · Tx(ai) · Rx(αi)展开成 4x4 齐次矩阵T_i [ cθ -sθcα sθsα a·cθ sθ cθcα -cθsα a·sθ 0 sα cα d 0 0 0 1 ]这个矩阵不用死记理解成两块就行。左上角 3x3 是旋转部分描述第 i 个坐标系相对于第 i−1 个坐标系的姿态右上角 3x1 是平移部分描述原点的位置。平移部分之所以是[a·cθ, a·sθ, d]而不是简单的[a, 0, d]是因为沿 X 移 a这个动作是在已经转过 θ 的坐标系里做的所以要先转 θ 再平移。代码实现import numpy as np def dh_transform(theta, d, a, alpha): 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], ])有个细节值得提np.cos和np.sin接受的是弧度如果你的关节角输入是角度制先转弧度。我在接口层统一规定内部一律用弧度只在最外层跟用户交互时转角度这样可以避免在十几个函数里反复写np.deg2rad。2.3 四个矩阵连乘为什么结果能化简得这么干净把四个矩阵按顺序乘起来得到从基座到末端的变换 T04。手动乘确实麻烦但因为我们这套构型的 α2、α3、α4 全是 0中间三个矩阵的旋转部分退化成纯 Z 轴旋转平移部分只有 X 方向分量。这让连乘的结果有非常强的规律。具体展开T12、T23、T34 的左上角都是Rz(qi)平移是[Li·cos(qi), Li·sin(qi), 0]的齐次形式。把它们按顺序作用于原点就得到末端在第二坐标系平面内的坐标。再乘上 T01平面内的径向分量投影到世界 X 和 Y高度分量加上 h。最后的结果写成世界坐标p_x x_l · cos(q1) p_y x_l · sin(q1) p_z h y_l其中x_l L1·cos(q2) L2·cos(q2q3) L3·cos(q2q3q4) y_l L1·sin(q2) L2·sin(q2q3) L3·sin(q2q3q4)这个形式的物理意义非常直观x_l 是从基座回转轴到末端的水平距离也就是极坐标里的半径y_l 是末端相对肩关节的高度。q1 负责把这个半径甩到正确的方位角上其余三个角全在平面内解决。很多人把这个结构叫柱坐标加平面臂名字很贴切。再看末端姿态。末端连杆在平面内的方向角 φ 就是三个角之和φ q2 q3 q4这个 φ 就是前面说的第四维。它决定了夹爪在平面内的朝向比如 φ −90° 表示末端连杆竖直向下此时夹爪是垂直扎下去的这是最常见的抓取姿态。2.4 用一组数据把正解验一遍推导对不对跑一组数就知道。取 L1 0.20 mL2 0.18 mL3 0.08 mh 0.15 m关节角 q [30°, 45°, −60°, 15°]弧度制分别是 0.5236、0.7854、−1.0472、0.2618。手算平面坐标q2 45°L1 分量(0.20·cos45°, 0.20·sin45°) (0.1414, 0.1414)q2q3 −15°L2 分量(0.18·cos(−15°), 0.18·sin(−15°)) (0.1739, −0.0466)q2q3q4 0°L3 分量(0.08, 0)所以 x_l 0.1414 0.1739 0.08 0.3953y_l 0.1414 − 0.0466 0 0.0948。世界坐标p_x 0.3953·cos30° 0.3423p_y 0.3953·sin30° 0.1977p_z 0.15 0.0948 0.2448。用代码跑一遍对比def fk(q, L, h): T np.eye(4) params [ (q[0], h, 0.0, np.pi / 2), (q[1], 0.0, L[0], 0.0), (q[2], 0.0, L[1], 0.0), (q[3], 0.0, L[2], 0.0), ] for theta, d, a, alpha in params: T T dh_transform(theta, d, a, alpha) return T q np.deg2rad([30, 45, -60, 15]) T fk(q, [0.20, 0.18, 0.08], 0.15) print(T[:3, 3]) # 期望 [0.3423, 0.1977, 0.2448]两个结果一致说明建系和参数表都没问题。这一步千万别跳我见过太多人直接往下推逆解结果正解里 α1 的符号就错了逆解怎么调都差一个镜像。3. 逆解推导把六维问题拆成转台角度加平面三连杆逆解是整个事情的核心。给定末端目标位置 (px, py, pz) 和目标姿态角 φ求四个关节角。我的思路是分三步剥洋葱先定 q1再定平面内的两连杆最后用姿态角把 q4 补出来。3.1 第一步基座回转角由水平投影直接锁定从正解看p_x 和 p_y 只跟 q1 和 x_l 有关p_x x_l·cos(q1), p_y x_l·sin(q1)两边平方相加得 x_l² p_x² p_y²所以x_l sqrt(p_x² p_y²) 取正根这里有个选择x_l 取正还是取负取正表示机械臂永远朝外伸取负表示可以从基座轴的另一侧够过去。工程上一般取正因为躯干在中间会干涉而且取负会让 q1 的解变成两族相差 180°。我在实现时直接固定 x_l ≥ 0把 q1 唯一化这样后面的多解筛选只集中在肘部那两个分支上逻辑清爽很多。那么 q1 就是q1 atan2(p_y, p_x)用 atan2 而不是 atan是因为它能正确处理四个象限还在 p_x p_y 0 时返回 0不会抛除零错误。目标正好落在基座轴的正上方时x_l 0q1 变成不确定的这时候机械臂理论上可以绕圈实际处理方式是保持上一次的 q1 不变只动平面关节。平面内的目标高度是y_l p_z − h到这里三维问题已经变成平面问题了已知平面内目标点 (x_l, y_l) 和目标姿态角 φ求 q2、q3、q4。3.2 第二步用姿态角把末端连杆扣掉退化成两连杆这一步是整个逆解里最漂亮的技巧。既然末端连杆的方向角 φ 已经指定那么这段连杆的贡献是完全已知的末端连杆从腕心指向末端向量是 (L3·cos φ, L3·sin φ)。反过来说腕心坐标就是目标点减去这段向量x_w x_l − L3·cos(φ) y_w y_l − L3·sin(φ)腕心wrist center是个重要概念它在这里指的是第三关节轴线上的那个点。扣掉末端连杆之后剩下的就是一个标准的两连杆机构从肩关节出发经过 L1、L2 到达腕心。注意如果末端工具不是装在第四个关节的轴线上而是有个安装偏置那么扣掉时要先把这个偏置旋转到当前姿态下再扣公式变成x_w x_l − L3·cos(φ) − b·cos(φβ)这种形式。我的建议是结构设计阶段就让偏置为零。现在目标变成已知 (x_w, y_w)用长度 L1 和 L2 的两连杆去够它。这是一个从高中就开始接触的问题用余弦定理。3.3 第三步余弦定理求肘部两个根对应两种构型两连杆能构成三角形的前提是目标点到原点的距离 D 满足 |L1 − L2| ≤ D ≤ L1 L2D² x_w² y_w² cos(q3) (D² − L1² − L2²) / (2·L1·L2)cos(q3) 在 [−1, 1] 之外就说明目标不可达要么太远够不着要么太近缩不回来。数值实现里必须做 clamp否则 acos 会返回 NaN一路传下去会让舵机指令变成垃圾值。这是我踩过的坑某次目标点算出来出界 0.3 mmacos 直接 NaN整条轨迹卡死排查了半天才发现是边界问题。在定义域内q3 ±acos( cos(q3) )两个符号对应两种肘部构型。取正号是肘部向上相对腕心方向逆时针偏取负号是肘部向下。物理上手肘可以朝上也可以朝下两种都能到达同一个点这也是四轴逆解多解的主要来源。有了 q3q2 用角度差来算q2 atan2(y_w, x_w) − atan2( L2·sin(q3), L1 L2·cos(q3) )这个公式的推导思路是第一段连杆的方向角 目标方向角 − 三角形内角。前半段atan2(y_w, x_w)是肩到腕心的方向后半段是利用三角形内角关系算出来的修正量。用 atan2 的差形式比用 asin 或 acos 稳健得多因为它不会在分母趋零时炸掉。最后q4 φ − q2 − q3把姿态角减掉前两个角剩下的就是第四个关节该补的角度。注意 q4 不需要再做归一化处理的话实际下发前要把它绕到 [−π, π] 区间因为舵机是有行程限制的多转一圈会撞限位。3.4 多解筛选限位优先连续性次之加权距离兜底前面一算肘部就有两个解加上 x_l 取负的话还有更多。实际下发只能选一个。我的筛选顺序是这样的。第一步硬限位过滤。每个关节都有实际能到的角度范围比如 q3 只能在 [−140°, 5°] 之间因为你不可能让手肘往后折断。把不满足限位的分支直接扔掉。这一步通常就能砍掉一半的解。第二步连续性优先。如果机械臂正在运动优先选和上一时刻关节角最接近的解。具体做法是计算每个候选解与上一时刻解的欧氏距离可以加权比如基座关节的位移权重小一点因为它转一点点末端就动很多选距离最小的。这能避免机械臂在运动过程中突然翻肘那种动作既吓人又容易撞到东西。第三步工作空间偏好。如果两个解都满足限位且距离相近我一般优先选肘部向上的那个因为这种构型下末端离基座回转轴更远夹爪不容易碰到自己的大臂。这是经验性的具体看你的结构。def select_solution(candidates, q_prev, limits, weights(1.0, 1.0, 1.2, 0.8)): best, best_cost None, np.inf for q in candidates: if not np.all(q limits[:, 0]) or not np.all(q limits[:, 1]): continue dq np.array(q) - np.array(q_prev) cost np.sum(np.abs(dq) * np.array(weights)) if cost best_cost: best, best_cost q, cost return best这段代码有个细节值得说权重不要全取 1。第三个关节肘部对末端位置的杠杆最长它的变化最容易造成末端大幅移动所以权重稍微给大一点能有效抑制肘部反复翻转。4. 推导不成立的时候奇异性、不可达与数值兜底闭式解再漂亮也有它算不出来的地方。工作空间边界、奇异位形、目标点漂移出界这些在实机上天天发生。我的原则是闭式解优先出问题立刻切数值解兜底同时给出一个最近可达点作为降级方案绝不能因为解不出来就卡死整个控制循环。4.1 奇异位形的判定与处理平面两连杆的奇异位形有两类。第一类是边界奇异对应 cos(q3) ±1也就是 q3 0 或者 q3 ±180°。此时两段连杆共线机械臂完全伸直或者完全折叠。这时候 D L1 L2 或 |L1 − L2|雅可比矩阵的秩从 2 掉到 1q2 和 q3 的具体分配变得不确定——你可以让 q2 转 10°、q3 转 0°也可以让 q2 转 0°、q3 转 10°末端位置完全一样。处理办法很直接检测到 |cos(q3)| 0.999 的时候固定 q3 为一个预设的小角度比如 5°让 q2 单独去补位置。这样虽然牺牲了一点精度但姿态是确定的不会出现关节乱跳。第二类是腕心过原点也就是 x_w y_w 0。这时候atan2(0, 0)无意义q2 完全不确定。物理上腕心正好落在肩关节轴线上两段连杆的贡献互相抵消。处理方法同样是固定一个角让另一个去补。第三类在四轴上还存在是基座奇异x_l 0 时 q1 不确定。处理方式是保持上一时刻的 q1 不变只调整平面关节。4.2 出界时不要硬算投影到最近可达点目标点超出工作空间是很常见的尤其是做示教的时候手一抖多拖了几厘米。硬算会得到 NaN直接下发会让舵机的目标位置变成一个巨大的非法值。正确的做法是把目标点投影到工作空间边界上。对于这套构型投影可以按径向和高度分别处理。先算腕心的模长 D 和目标方向如果 D L1 L2就把腕心按方向缩放到 L1 L2 的长度上如果 D |L1 − L2|就缩放到 |L1 − L2|。高度方向同理如果超过基座顶部的极限高度截断到极限。def project_into_workspace(x_w, y_w, L1, L2): D np.hypot(x_w, y_w) D_min, D_max abs(L1 - L2), L1 L2 if D 1e-9: return D_min, 0.0 # 退化到肩轴线附近 D_c min(max(D, D_min), D_max) scale D_c / D return x_w * scale, y_w * scale这样即使目标不可达机械臂也会朝着目标方向尽可能地伸过去而不是原地不动或者乱动。在做拖拽示教的时候这个行为体验好很多。4.3 数值迭代雅可比加阻尼最小二乘当闭式解因为各种原因失效或者你想做纯位置的逆解不指定 φ只有三个坐标约束就需要数值方法了。四轴的雅可比矩阵是 3x4 的J [ ∂p_x/∂q1 ∂p_x/∂q2 ∂p_x/∂q3 ∂p_x/∂q4 ∂p_y/∂q1 ∂p_y/∂q2 ∂p_y/∂q3 ∂p_y/∂q4 ∂p_z/∂q1 ∂p_z/∂q2 ∂p_z/∂q3 ∂p_z/∂q4 ]把正解对四个角分别求偏导结果是∂p/∂q1 [ −x_l·sin(q1), x_l·cos(q1), 0 ] ∂p/∂q2 [ cos(q1)·dx2, sin(q1)·dx2, dy2 ] ∂p/∂q3 [ cos(q1)·dx3, sin(q1)·dx3, dy3 ] ∂p/∂q4 [ cos(q1)·dx4, sin(q1)·dx4, dy4 ]其中dx2 −L1·sin(q2) − L2·sin(q2q3) − L3·sin(q2q3q4)dy2 L1·cos(q2) L2·cos(q2q3) L3·cos(q2q3q4)dx3少掉第一项dx4只剩−L3·sin(q2q3q4)。逐项对着正解求导就能得到写代码的时候建议用符号计算工具自动生成手推容易漏项。注意 J 是 3x4 的不是方阵不能直接求逆。最朴素的做法是伪逆Δq J⁺ · e其中 e 是当前末端位置和目标位置的误差。伪逆的问题在于靠近奇异位形时 J 的最小奇异值趋近于零伪逆会给出巨大的关节增量机械臂会突然抽搐。解决办法是阻尼最小二乘Δq Jᵀ · ( J·Jᵀ λ²·I )⁻¹ · eλ 是阻尼系数取值在 0.01 到 0.1 之间比较常用。λ 越大越稳定但收敛越慢越小越快但在奇异附近越容易爆。我的做法是根据最小奇异值动态调 λ正常情况取 0.01检测到接近奇异时提到 0.1。def dls_step(J, e, lam0.05): A J J.T (lam ** 2) * np.eye(3) return J.T np.linalg.solve(A, e)迭代过程就是不断算误差、更新 Δq、限幅、再算误差直到误差小于阈值比如 0.1 mm或者迭代超过 50 次。限幅非常重要每次迭代最多让关节动 3° 到 5°防止一步跨到奇异区。4.4 两种解法的分工实际代码里我是这么安排的每次收到目标位姿先用闭式解快速算一遍。闭式解成功且所有候选都满足限位直接用耗时在微秒级。闭式解失败不可达、退化、限位全不满足切数值解同时把目标投影到工作空间内。数值解迭代次数也给它限死超时就直接保持当前位置不动并在日志里打一条警告。这样做的理由是闭式解是确定性的同一个目标永远给同一个结果这对重复精度很关键数值解依赖初值稍微变一下初值结果就差一点不适合做主路径。但它作为兜底非常可靠尤其在闭式解因为浮点误差在边界上抖动的时候。5. 代码工程化从单个函数到能跑的抓取动作公式推完了落到工程上还有一堆事。我习惯把运动学相关的代码拆成几个独立模块这样调试的时候能单独测每一层不会一锅粥。5.1 模块划分和数据流我的目录结构大概是这样的arm_kin/ config.py 连杆参数、关节限位、舵机标定表 transform.py dh_transform 和基本位姿工具 kinematics.py fk / ik 闭式解 / jacobian solver.py 多解筛选、工作空间投影、数值兜底 trajectory.py 关节空间与笛卡尔空间插值 driver.py 角度到舵机脉冲的映射与限速数据流是单向的上位机给一个目标位姿solver 出关节角序列trajectory 把序列插成密集的点driver 把每个点转成舵机指令。每一层都只依赖上一层的输出不反向调用。这样如果末端不动我可以直接打印关节角看是谁的锅如果是舵机响应慢那问题一定在 driver 层。5.2 用正解回代做验证别只看眼睛写完逆解第一件事不是接舵机是做回代验证。随机在关节空间里采一千组样本跑正解得到末端位姿再把这个位姿丢给逆解比较两次的关节角差和位置差。errs [] rng np.random.default_rng(42) for _ in range(1000): q rng.uniform(limits[:, 0], limits[:, 1]) T fk(q, L, h) p, phi T[:3, 3], np.arctan2(T[1, 2], T[0, 2]) # 从旋转矩阵提取平面姿态角 q_sol ik(p[0], p[1], p[2], phi, L, h) if q_sol is None: continue T_back fk(q_sol, L, h) errs.append(np.linalg.norm(T_back[:3, 3] - p)) print(np.max(errs), np.mean(errs))正常情况下最大误差应该在 1e-9 量级均值在 1e-12 量级。如果最大误差到 1e-3 以上说明某处有 bug通常是 atan2 的参数顺序反了或者 q4 归一化时把角度卷错了。我建议把这个测试做成单元测试每次改运动学代码都跑一遍比在实机上试快一百倍。还有一个容易被忽略的点从旋转矩阵里提取 φ 的时候要确认你提取的是正确的那个分量。对于这套构型末端连杆在平面内φ 可以通过atan2(T[1,2], T[0,2])或者取 T04 的前两列关系得到。第一次写的时候我提取错了列导致回代误差一直在 0.3 rad 附近查了半天才反应过来是提取方式不对。5.3 关节角到舵机脉冲零位、方向和限速运动学算出来的关节角是纯数学量跟真实舵机之间隔着一层标定。这层标定有三个参数。第一个是零位偏移。机械臂装配完成后关节角的零点几乎不可能正好对上数学模型的零点。比如数学上 q2 0 表示大臂水平实际装出来可能是水平再往上 3°。这个偏移要单独测量并加到映射里。映射公式是pulse pulse_center (q_math q_offset) * dir * pulse_per_rad其中pulse_center是舵机中位脉冲典型值 1500 us但不同舵机差别很大pulse_per_rad是每弧度对应多少微秒由舵机行程决定比如 270° 行程对应 2000 us 的话就是 2000 / (270/180×π) ≈ 424 us/rad。dir取 ±1用来修正装配方向。第二个是方向符号。不同关节的装配方向可能相反正向关节角对应舵机顺时针还是逆时针必须实测确认。我的做法是给一个小的正向角度指令看末端实际往哪走走反了就翻符号。第三个是限速与限加速。舵机尤其是总线舵机不能瞬间从 0° 跳到 90°那样会拉低母线电压、触发过流保护甚至打齿。我的做法是在 driver 层做梯形速度规划把关节角序列按最大角速度比如 60°/s和最大角加速度重采样。提示总线舵机通常会回报当前位置可以用这个反馈做闭环修正。但不要设太高的增益舵机的内部 PID 已经调过了你在外面再套一层容易震荡。5.4 关节空间插值和笛卡尔空间插值怎么选插值方式的选择取决于任务。如果是点位抓取比如从料盘拿一个东西放到另一个位置两个点之间怎么走不重要那用关节空间插值最省事。把起止两组关节角做五次多项式插值速度加速度都连续机械臂走起来很柔和。五次多项式的系数可以通过起止位置、速度、加速度六个边界条件唯一确定。如果是喷涂、涂胶、画线这类需要末端走直线的任务就必须用笛卡尔空间插值。做法是在起止位姿之间做直线插补每隔 1 mm 取一个点每个点都跑一次逆解。这里有个坑直线路径可能穿过奇异位形或者工作空间外的区域。我的做法是提前采样路径上的点做可达性检查发现不可达就把路径分段绕行而不是等走到一半再报错。还有一点笛卡尔插值得到的关节角序列相邻点之间的关节增量可能不均匀有些地方大有些地方小。我会再检查一遍最大关节增量如果超过安全值就在那个位置加密采样点。6. 装到机器上之后才会暴露的问题偏差、标定与补偿代码跑通只是开始。真实机械臂上会有一堆模型之外的误差这一节讲我实际遇到的处理方法。6.1 末端偏差的排查顺序末端重复定位不准原因可能有一堆。我的排查顺序是从大到小、从粗到精。先看重复性。让机械臂走同一个点二十次看末端标记点在多大范围内散。如果散度在 1 mm 以内说明重复性好问题出在绝对精度上也就是标定不准。如果散度超过 3 mm那先查机械结构关节虚位、连杆松动、同步带松紧、舵机齿轮间隙。再看绝对误差。重复性好但绝对精度差通常是三个原因连杆实际长度和模型不一致、零位偏移量测错、装配导致关节轴线不平行。这三个里零位偏移的影响最大因为它是角度误差会随连杆长度放大。基座零位差 1°末端在 400 mm 处就偏 7 mm。最后看区域相关性。如果误差在某个工作区域特别大别的地方都还好那很可能是坐标系建立的问题或者那个区域接近奇异位形误差被雅可比放大了。现象可能原因排查手段散度大3mm虚位、间隙、松动手动晃动每个关节看旷量散度小但整体偏零位偏移、连杆长度测量实际连杆重标零位特定区域偏大邻近奇异、坐标系错检查该区域雅可比条件数偏的方向随姿态变化关节轴线不平行建模时引入轴线夹角参数6.2 用几个已知点拟合参数是最省事的标定方法完整的运动学标定是个大工程涉及二十多个参数。对桌面级四轴来说没必要走那么远。我的做法是只标四个参数三个连杆长度和基座零位。具体操作在机械臂工作空间里选 6 到 8 个点用外部测量手段可以用一个带刻度的工作台或者装个相机看标记点记录末端的实际坐标。然后构造一个最小二乘问题让模型在这些点上的正解与实际测量值的误差平方和最小优化变量就是四个参数。用 scipy 几行就能写from scipy.optimize import least_squares def residual(params, q_samples, p_measured): L1, L2, L3, dq1 params preds [] for q in q_samples: q_adj q.copy() q_adj[0] dq1 T fk(q_adj, [L1, L2, L3], h) preds.append(T[:3, 3]) return (np.array(preds) - p_measured).ravel() res least_squares(residual, x0[0.20, 0.18, 0.08, 0.0], args(q_samples, p_measured))优化完通常能把误差从几毫米压到零点几毫米。注意不要用太多的参数去拟合太少的点那会过拟合在测量点上是准的别的地方反而更差。经验上参数数量的三倍是点的最低数量。6.3 重力矩对跟随误差的影响这是四轴舵机臂上最容易被忽视的一点。大臂水平伸出的时候肩关节承担的重力矩最大舵机要输出更大的电流才能保持位置。如果在重载姿态下要求舵机快速运动它可能跟不上指令产生明显的跟随滞后。表现出来就是末端轨迹在负载大的那段鼓出去一块。解决办法是加前馈的重力矩补偿。理论上肩关节需要补偿的力矩等于各段连杆的重力乘以对应的力臂τ m1·g·r1·cos(q2) m2·g·r2·cos(q2q3) m3·g·r3·cos(q2q3q4)其中 mi 是各段质量ri 是重心到关节的距离。这些参数可以用称重和估算得到不需要很精确能补偿到 70% 就能显著改善跟随误差。具体怎么用这个力矩如果是电流控制的舵机直接叠加到输出电流上如果是位置控制的舵机只能通过限制运动速度来间接缓解——在负载大的姿态区间把速度调低。我用的是后者在轨迹规划阶段根据当前姿态估算重力矩动态调整该段的角速度上限。提示重力矩补偿只在前馈层做别想着用大增益反馈去修正舵机内部已经在做这件事了外面再加一层会让系统变硬遇到扰动更容易震荡。6.4 手眼关系核对走一个十字最快如果机械臂上装了相机做抓取相机和末端之间的变换关系手眼关系必须标定。完整标定需要棋盘格和一堆求解但日常核对有没有明显错误有个土办法让末端走一个标准的十字轨迹用相机看末端标记点的轨迹是不是十字。如果相机看到的轨迹是十字但比例不对说明尺度有问题通常是相机内参或者安装高度不对。如果轨迹是个平行四边形或者歪掉的十字说明手眼旋转矩阵有偏差。如果轨迹整体平移了说明平移量不对。这个方法不能精确标定但能在五分钟内判断出问题出在哪个环节非常实用。手眼标定的精度直接影响抓取成功率。我的经验是如果抓取精度要求是 2 mm手眼标定的残差应该控制在 0.5 mm 以内留出足够余量给运动学误差和机械误差。这套东西我从第一版跑通到稳定前前后后改了六七轮。早期版本我只用数值解图省事结果每次断电重启、初值一变同一个点的关节角就不一样重复精度惨不忍睹。改成闭式解为主、数值解兜底之后同一个目标点每次给的结果完全一致重复精度才真正稳定下来。后来我又加了一个小改进把上一次成功求解的关节角存下来作为下一次的初值和解筛选的参考。这个简单的状态保持让机械臂在连续抓取多个相邻点位时动作连贯了很多不再出现两个点之间肩膀突然扭一下的情况。如果你也在这条路上折腾我建议优先把正解和逆解的回代验证做扎实传感器和舵机的问题可以慢慢查运动学公式错了后面全是白费功夫。
