企业官网建设流程全解析

手里那台四轴桌面臂，去年折腾了我整整两个周末。现象很典型：示教好的抓取点，跑十几遍之后末端总往一个方向偏五六毫米，换一组姿态又好了。我一开始怀疑舵机虚位、怀疑结构刚度，拆了装装了拆，最后发现问题出在逆解上——4自由度机械臂正逆解公式推导与代码实现这件事，看着是课本里的老题目，真落到一台带舵机、带装配误差的机器上，坑全在推导之外的细节里。

这篇东西我打算按自己实际推一遍、写一遍、调一遍的顺序来组织。核心是那套四轴平面构型：一个绕竖直轴回转的基座，加上三个互相平行、都垂直于基座回转轴的水平关节。这套构型的正解能写成非常干净的形式，逆解也能完全闭式求解，不需要迭代。适合正在做毕业设计、做桌面级机械臂、或者要给舵机臂写运动控制的人参考。有线性代数和一点三角函数基础就能跟上，我把每一步为什么这么建坐标系、为什么这么化简都写出来。

1. 四轴机械臂的解为什么比六轴更"拧巴"

很多人第一次接触四轴，会下意识把六轴那套"位置和姿态各三自由度、腕部三轴交于一点"的经验直接搬过来，然后发现公式怎么推都不对。问题不在推导能力，在于任务维度和驱动维度对不上。先把这件事捋清楚，后面的公式才写得下去。

1.1 三加一：四轴真正能干的事

末端在空间里的完整描述是六维的：三个位置分量 x、y、z，三个姿态分量（通常用欧拉角或者旋转矩阵来描述）。六轴机械臂有六个驱动，刚好一对一匹配，所以任意一个可达位姿都有解。四轴只有四个驱动，从维度上就是欠约束的，你不可能同时把六个分量都指定死。

那四轴靠什么工作？两条路。第一条是把姿态要求交给结构本身去满足，比如末端装一个始终朝下的平行夹爪，或者末端是吸盘、是喷头，它天然只关心位置和一个朝向角。第二条是显式降维：只指定三个位置分量加一个平面内的姿态角，加起来四维，正好对上四个驱动。

我这里用的是第二条路。所谓"平面内姿态角"，指的是末端连杆在基座回转后的那个竖直平面里，相对于水平方向的夹角。这个角度是可以自由指定的，也是四轴最有用的那个额外自由度——它决定了你的夹爪是垂直向下扎，还是斜着伸进去。

提示：如果你手上的四轴任务必须控制末端绕竖直轴的偏航角，那基本做不到。要么加一个第五轴，要么在末端加一个独立的舵机做工具旋转，别硬在运动学里绕。

1.2 三种常见的四轴构型，解法难度差很多

四轴机械臂的物理形态差别很大，选错构型会让逆解从"一页纸"变成"必须迭代"。我按可解性从易到难排一下。

第一种是平面关节型，也就是本文主推的构型：J1 绕竖直轴回转，J2、J3、J4 三条轴线互相平行且垂直于 J1 轴线，整个臂在 J1 转过之后的那个竖直平面里运动。这种构型位置只由 J1、J2、J3 决定，姿态由 J4 补偿，可以做到完全闭式求解，没有迭代、没有初值依赖。

第二种是全回转型，四个轴都平行于竖直方向，类似 SCARA 的思路。这种构型末端只能在一个水平面附近工作，Z 方向通常靠一根丝杠或者一个直线模组实现，严格说不能叫"四自由度串联"。它的平面逆解就是经典的两连杆问题，非常简单，但工作空间很扁。

第三种是带平行四边形的码垛构型。为了把驱动电机放到基座附近减小惯量，厂家会用一套平行四边形连杆把 J3 的动力引过去。这种机构在运动学上等效于平面三连杆，但连杆参数和关节角的映射需要额外处理，而且平行四边形会引入耦合，标定的时候要多一组参数。

还有一种常见的是偏置腕构型，末端最后一个关节的轴线与前两个不共面，好处是姿态能力更强，坏处是腕部不再解耦，闭式解会变得很难看。入门阶段我不建议碰。

1.3 腕部解耦是能不能拿到闭式解的分水岭

闭式解能不能推出来，关键看一点：能不能把位置问题和姿态问题拆开。在六轴里这个条件是"腕部三个轴交于一点"，满足了就能把末端位置减去腕心偏移，先算腕心位置，再反推腕部三个角。

四轴这边条件更宽松一些，因为姿态只要求一个角。只要末端工具那段连杆的长度已知、方向由最后一个关节角唯一决定，就可以先按目标姿态把这段连杆"扣掉"，把问题退化成两连杆的位置逆解。我上面说的平面构型天然满足这个条件，L3 的方向完全由 q2+q3+q4 决定，扣掉之后腕心就落在一个两连杆可达的平面上。

反过来说，如果你的机构里末端工具和最后一个关节之间还夹着一个偏置量，比如夹爪安装面偏离了关节轴线，那扣掉的时候就要做一次旋转和平移的复合，公式会长一截，但思路不变。我建议在三维建模阶段就把这个偏置尽量做成零，能省掉后面一大堆调试。

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 是肩关节相对基座底面的高度。

这里 α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(q2+q3) + L3·cos(q2+q3+q4) y_l = L1·sin(q2) + L2·sin(q2+q3) + L3·sin(q2+q3+q4)

这个形式的物理意义非常直观：x_l 是从基座回转轴到末端的水平距离（也就是极坐标里的半径），y_l 是末端相对肩关节的高度。q1 负责把这个半径"甩"到正确的方位角上，其余三个角全在平面内解决。很多人把这个结构叫"柱坐标加平面臂"，名字很贴切。

再看末端姿态。末端连杆在平面内的方向角 φ 就是三个角之和：

φ = q2 + q3 + q4

这个 φ 就是前面说的第四维。它决定了夹爪在平面内的朝向，比如 φ = −90° 表示末端连杆竖直向下，此时夹爪是垂直扎下去的，这是最常见的抓取姿态。

2.4 用一组数据把正解验一遍

推导对不对，跑一组数就知道。取 L1 = 0.20 m，L2 = 0.18 m，L3 = 0.08 m，h = 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)
• q2+q3 = −15°，L2 分量：(0.18·cos(−15°), 0.18·sin(−15°)) = (0.1739, −0.0466)
• q2+q3+q4 = 0°，L3 分量：(0.08, 0)

所以 x_l = 0.1414 + 0.1739 + 0.08 = 0.3953，y_l = 0.1414 − 0.0466 + 0 = 0.0948。

世界坐标：p_x = 0.3953·cos30° = 0.3423，p_y = 0.3953·sin30° = 0.1977，p_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 = 0），q1 变成不确定的，这时候机械臂理论上可以绕圈，实际处理方式是保持上一次的 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 + L2：

D² = x_w² + y_w² cos(q3) = (D² − L1² − L2²) / (2·L1·L2)

cos(q3) 在 [−1, 1] 之外就说明目标不可达，要么太远够不着，要么太近缩不回来。数值实现里必须做 clamp，否则 acos 会返回 NaN，一路传下去会让舵机指令变成垃圾值。这是我踩过的坑：某次目标点算出来出界 0.3 mm，acos 直接 NaN，整条轨迹卡死，排查了半天才发现是边界问题。

在定义域内：

q3 = ±acos( cos(q3) )

两个符号对应两种肘部构型。取正号是"肘部向上"（相对腕心方向逆时针偏），取负号是"肘部向下"。物理上手肘可以朝上也可以朝下，两种都能到达同一个点，这也是四轴逆解多解的主要来源。

有了 q3，q2 用角度差来算：

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 掉到 1，q2 和 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(q2+q3) − L3·sin(q2+q3+q4)，dy2 = L1·cos(q2) + L2·cos(q2+q3) + L3·cos(q2+q3+q4)；dx3少掉第一项，dx4只剩−L3·sin(q2+q3+q4)。逐项对着正解求导就能得到，写代码的时候建议用符号计算工具自动生成，手推容易漏项。

注意 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, lam=0.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。

最后看区域相关性。如果误差在某个工作区域特别大，别的地方都还好，那很可能是坐标系建立的问题，或者那个区域接近奇异位形，误差被雅可比放大了。

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(q2+q3) + m3·g·r3·cos(q2+q3+q4)

其中 mi 是各段质量，ri 是重心到关节的距离。这些参数可以用称重和估算得到，不需要很精确，能补偿到 70% 就能显著改善跟随误差。

具体怎么用这个力矩？如果是电流控制的舵机，直接叠加到输出电流上；如果是位置控制的舵机，只能通过限制运动速度来间接缓解——在负载大的姿态区间把速度调低。我用的是后者，在轨迹规划阶段根据当前姿态估算重力矩，动态调整该段的角速度上限。

提示：重力矩补偿只在前馈层做，别想着用大增益反馈去修正，舵机内部已经在做这件事了，外面再加一层会让系统变硬，遇到扰动更容易震荡。

6.4 手眼关系核对：走一个十字最快

如果机械臂上装了相机做抓取，相机和末端之间的变换关系（手眼关系）必须标定。完整标定需要棋盘格和一堆求解，但日常核对有没有明显错误，有个土办法：让末端走一个标准的十字轨迹，用相机看末端标记点的轨迹是不是十字。

如果相机看到的轨迹是十字但比例不对，说明尺度有问题，通常是相机内参或者安装高度不对。如果轨迹是个平行四边形或者歪掉的十字，说明手眼旋转矩阵有偏差。如果轨迹整体平移了，说明平移量不对。这个方法不能精确标定，但能在五分钟内判断出问题出在哪个环节，非常实用。

手眼标定的精度直接影响抓取成功率。我的经验是，如果抓取精度要求是 2 mm，手眼标定的残差应该控制在 0.5 mm 以内，留出足够余量给运动学误差和机械误差。

这套东西我从第一版跑通到稳定，前前后后改了六七轮。早期版本我只用数值解，图省事，结果每次断电重启、初值一变，同一个点的关节角就不一样，重复精度惨不忍睹。改成闭式解为主、数值解兜底之后，同一个目标点每次给的结果完全一致，重复精度才真正稳定下来。

后来我又加了一个小改进：把上一次成功求解的关节角存下来，作为下一次的初值和解筛选的参考。这个简单的状态保持，让机械臂在连续抓取多个相邻点位时动作连贯了很多，不再出现两个点之间肩膀突然扭一下的情况。如果你也在这条路上折腾，我建议优先把正解和逆解的回代验证做扎实，传感器和舵机的问题可以慢慢查，运动学公式错了，后面全是白费功夫。