1. 项目缘起与整体方案拆解

1.1 为什么是4自由度而不是6轴

很多刚接触机械臂的朋友第一反应是“自由度越高越好”，恨不得直接上六轴工业臂。但真到自己动手做项目、写算法、调参数的时候就会发现，6轴带来的复杂度是成倍增长的。我做4自由度机械臂这个项目，出发点其实很朴素：想用最少的关节把一个空间抓取任务跑通，同时让运动学求解全程都能手算出来，不用依赖任何数值迭代库。

4自由度机械臂的典型构型是“基座旋转 + 三个俯仰关节”，也就是底座绕竖直轴转，接着大臂、小臂、腕部依次在一个竖直平面内摆动。这种构型的好处非常明显：逆运动学存在解析解，也就是说给定目标位置和姿态，我们能直接写出闭式公式把四个角度算出来，而不是靠雅可比矩阵迭代去凑。解析解意味着计算速度快、不存在迭代不收敛的问题，对嵌入式主控来说非常友好。

当然代价也是有的。4自由度只能控制空间中的位置（x、y、z）加上一个绕竖直轴的姿态，末端执行器的俯仰姿态是确定的、不能自由设定。换句话说，你没法像6轴那样让夹爪从任意角度去贴合物体表面。但对于绝大多数抓取、码垛、分拣场景，末端只要朝下或者保持水平就够用了。这也是很多桌面级机械臂、总线舵机机械臂、3D打印机械臂毕业设计都选4自由度的原因。

1.2 构型选定与坐标系约定

我采用的构型具体是：J1基座旋转关节（绕Z轴），J2肩部俯仰关节，J3肘部俯仰关节，J4腕部俯仰关节，末端再接一个夹爪。其中J2、J3、J4的旋转轴在初始位姿时互相平行，并且都垂直于基座的竖直轴。连杆参数我定义为：基座到肩关节的竖直偏置记为d1，肩到肘的连杆长度记为a2，肘到腕的连杆长度记为a3，腕关节到末端夹爪中心的距离记为a4。

坐标系约定上我踩过一次坑，这里必须先说清楚免得后面推导乱套。我用的是标准DH参数法的变体：Z轴沿着关节旋转轴，X轴沿着相邻两个Z轴的公垂线方向，原点取在公垂线与当前Z轴的交点。基坐标系原点放在基座旋转轴与安装平面的交点，Z0竖直向上。这样建立的好处是相邻变换矩阵形式统一，写代码的时候可以直接套模板。

提示：坐标系约定一旦定下来，后面推导、标定、代码里所有角度符号都以此为准。中途改约定是最容易出错的地方，宁可一开始多花半小时把图画清楚。

整体技术路线是：先用DH参数法推导正运动学得到末端位姿关于四个关节角的表达式；再利用4自由度的几何特征，用“基座角单独求解 + 平面三连杆几何求解”的思路推导逆解；最后用Python把公式落成代码，并用正解验证逆解、用可视化验证轨迹。热词里提到的“暴力枚举+推导公式+数学构造”，其实就是这个思路的通俗说法——逆解的多解用枚举挑选，主体靠公式推导，关节角的象限判断靠数学构造。

2. 正运动学公式推导：从DH参数到末端位姿

2.1 DH参数表的建立逻辑

正运动学的核心任务，是建立一个从关节空间到笛卡尔空间的映射：输入四个关节角θ1 θ2 θ3 θ4，输出末端在基坐标系下的位置和姿态。DH参数法就是完成这个映射的标准工具，它把每个连杆用四个参数描述清楚：连杆长度a、连杆扭转角α、连杆偏置d、关节角θ。

按照我前面说的构型，逐连杆列参数。连杆1从基座到肩部，关节轴Z0是竖直的，Z1是肩部的水平旋转轴。这里Z0到Z1的变换包含一个绕Z0的旋转θ1和一个沿Z0方向的偏置d1，公垂线长度为0，扭转角为90度（因为Z0竖直而Z1水平，两轴垂直）。连杆2从肩到肘，Z1和Z2平行，公垂线长度为a2，扭转角为0，偏置为0，关节角是θ2。连杆3从肘到腕，Z2和Z3平行，公垂线长度为a3，关节角是θ3。连杆4从腕到末端，Z3和Z4平行，公垂线长度为a4，关节角是θ4。

把这些整理成表格，就是施工级别的依据：

这张表是后面一切推导的源头。我建议你动手时也先把这张表画出来，然后逐行检查：公垂线长度是不是对应实际连杆、扭转角的正负是不是符合右手定则。很多朋友正解算错，八成是这张表某一行符号或者数值写反了。

2.2 相邻坐标系变换矩阵的推导

DH参数法的精髓在于，任意两个相邻坐标系之间的变换，都可以拆成四个基本变换的组合，顺序固定为：绕X轴转α、沿X轴平移a、绕Z轴转θ、沿Z轴平移d。这个顺序不能乱，因为旋转和平移是不可交换的。写成矩阵形式就是：

T(i-1, i) = Rot(X, α) · Trans(X, a) · Rot(Z, θ) · Trans(Z, d)

把它展开成4x4齐次变换矩阵，通用形式是这样的：

[ cosθ -sinθ 0 a ] [ sinθcosα cosθcosα -sinα -d·sinα ] [ sinθsinα cosθsinα cosα d·cosα ] [ 0 0 0 1 ]

这里我特意把通用形式写全，是因为后面直接套参数就能用，不需要每推导一个连杆就重新展开一次。拿连杆1代入：a=0, α=90°, d=d1。由于cos90°=0, sin90°=1，矩阵简化为：

T01 = [ cosθ1 0 sinθ1 0 ] [ sinθ1 0 -cosθ1 0 ] [ 0 1 0 d1 ] [ 0 0 0 1 ]

连杆2、3、4的扭转角都是0，代入通用形式后cosα=1, sinα=0，矩阵会退化成标准的平面旋转加平移形式：

T12 = [ cosθ2 -sinθ2 0 a2·cosθ2 ] [ sinθ2 cosθ2 0 a2·sinθ2 ] [ 0 0 1 0 ] [ 0 0 0 1 ]

T23和T34的形式完全一样，只是把下标和连杆长度换掉。这个退化的矩阵非常好理解，本质上就是一个二维平面内的“先绕原点转θ，再沿新X轴前进a”的操作。这也从侧面印证了：三个俯仰关节确实是在一个竖直平面内工作的。

2.3 从基座到末端的累积变换

单个连杆矩阵搭好之后，末端位姿就是这一串矩阵连乘：

T04 = T01 · T12 · T23 · T34

直接手算四次矩阵连乘会非常痛苦，但这里有一个技巧：因为T12、T23、T34都是平面变换，我们可以先把后三个乘起来，得到一个“平面三连杆”的等效变换，再和T01相乘。平面部分乘完之后，末端在平面内的坐标可以写成三个连杆投影之和：

x_p = a2·cosθ2 + a3·cos(θ2+θ3) + a4·cos(θ2+θ3+θ4) y_p = a2·sinθ2 + a3·sin(θ2+θ3) + a4·sin(θ2+θ3+θ4)

再把平面结果经过T01映射到基坐标系。T01的作用是绕Z轴转θ1并且抬高d1，而平面里的x_p方向对应基坐标系的水平径向，y_p方向对应竖直方向。所以最终末端位置为：

x = (a2·cosθ2 + a3·cos(θ2+θ3) + a4·cos(θ2+θ3+θ4)) · cosθ1 y = (a2·cosθ2 + a3·cos(θ2+θ3) + a4·cos(θ2+θ3+θ4)) · sinθ1 z = d1 + a2·sinθ2 + a3·sin(θ2+θ3) + a4·sin(θ2+θ3+θ4)

这里我为了推导直观，把x_p当作水平径向距离，用r来代表会更清爽：

r = a2·cosθ2 + a3·cos(θ2+θ3) + a4·cos(θ2+θ3+θ4) z = d1 + a2·sinθ2 + a3·sin(θ2+θ3) + a4·sin(θ2+θ3+θ4)

这两个式子就是正运动学的最终结果，也是后面逆解推导的起点。末端姿态方面，由于三个俯仰关节在同一个平面内，末端连杆的俯仰角等于θ2+θ3+θ4，绕竖直轴的偏航角等于θ1。这就是4自由度机械臂姿态表达能力受限的根本原因：俯仰角被三个关节耦合决定，无法独立设定。

3. 逆运动学推导：几何法一步步解出四个关节角

3.1 为什么4自由度可以用几何法

逆运动学要解决的问题是：给定末端目标位置(x, y, z)和期望的末端俯仰姿态，反推四个关节角。一般6轴机械臂逆解要用数值方法或者复杂的解析推导，而4自由度之所以友好，是因为它的几何结构足够简单，可以直接用三角形和反三角函数拼出来。

思路分两层。第一层处理基座旋转：由于所有俯仰关节都在一个竖直平面内，末端的水平投影方向完全由θ1决定。所以θ1可以通过目标点的水平坐标直接算出来。第二层处理平面问题：确定θ1之后，目标点到基座旋转轴的水平距离r和高度z就确定了，问题降维成一个平面内的三连杆逆解，可以用几何法求解。

这里有一个关键决策点需要说明：4自由度的末端俯仰姿态是冗余的。什么意思？就是给定一个位置，末端可以有多种不同的朝向，甚至同一个位置、同一个朝向也可能有多组关节角解。这就引出了逆解的多解问题，也是热词里“暴力枚举”的用武之地。我的处理策略是：先把末端的期望俯仰角作为输入参数固定下来，这样平面三连杆就变成了“已知起点、终点和末端方向，求三个角度”，解就唯一了（在给定构型分支内）。

3.2 基座角与平面距离的求解

第一步求基座角θ1，直接对目标点的水平坐标取反正切：

θ1 = atan2(y, x)

这里的atan2而不是atan，是因为要用两个参数的符号来判断象限，atan(y/x)只能给出 -90° 到 90° 的范围，目标点跑到第三象限就会算错。这是我特别想强调的一点，很多人代码里写atan(y/x)，结果机械臂只能在前方半圆工作，一到后方就抽风。

但是θ1有两个解，atan2(y, x)和atan2(y, x) + π。这两个解都成立，对应机械臂从正面或者背面去够目标点。实际选哪个，要看机械臂的关节限位和避障需求。如果你的机械臂基座只能转 ±180°，那这两个解可能只有一个在限位内。

确定θ1之后，计算水平径向距离：

r = sqrt(x² + y²)

这一步要注意，如果目标点的水平距离小于某个最小值（比如机械臂基座半径），或者大于总臂展，那么逆解无解，要在代码里提前判断并报错，而不是让它算出一个NaN进去。

3.3 平面三连杆的几何求解

现在问题变成：在竖直平面内，已知起点位于(0, d1)，末端位于(r, z)，末端的朝向角为φ = θ2+θ3+θ4，连杆长度为a2, a3, a4，求θ2, θ3, θ4。

因为末端朝向已经给定，腕关节的位置可以反推出来。末端位置减去末端连杆在朝向方向上的投影，就得到腕关节位置：

r_w = r - a4·cosφ z_w = z - a4·sinφ

这一步是整个推导的“降维打击”——把原来的三连杆问题，转化成了从原点到腕关节的平面二连杆问题，而二连杆逆解是有标准闭式公式的。

二连杆的几何关系是：从起点(0, d1)到腕关节(r_w, z_w)的向量长度记为D：

D = sqrt(r_w² + (z_w - d1)²)

然后用余弦定理求肘关节角。这里要小心，肘关节角的定义和连杆夹角有关。根据我前面DH参数的约定，θ3是肘部关节的转角，它和两连杆夹角的余角相关。具体推导：

设从起点到腕关节的向量与水平面的夹角为β：

β = atan2(z_w - d1, r_w)

由余弦定理，肘部弯曲的补角γ满足：

cosγ = (a2² + a3² - D²) / (2·a2·a3) γ = acos(cosγ)

由于θ3是肘关节的转角，而γ是两连杆之间的内角补角，所以：

θ3 = ±(π - γ)

这里正负号对应“肘部向上”和“肘部向下”两个构型分支，也就是经典的多解。同样地，肩部角θ2由两部分组成：向量角β加上一个由连杆长度决定的修正角δ：

δ = atan2(a3·sinγ, a2 + a3·cosγ) θ2 = β - δ （对应肘部向下的分支） θ2 = β + δ （对应肘部向上的分支）

最后，腕部角由末端朝向反推：

θ4 = φ - θ2 - θ3

推导到这里就完整了。四个角度全部解出，而且每一步都有明确的几何意义：θ1是水平朝向，θ2是肩部抬升，θ3是肘部弯曲，θ4是腕部补偿。这套公式我实测下来非常稳定，计算一次逆解在普通单片机上也就几十微秒。

注意：acos的输入必须做数值裁剪，把(a2² + a3² - D²) / (2·a2·a3)限制在[-1, 1]区间内。浮点误差会让本来等于1的值算成1.0000000001，导致acos返回NaN。这个坑我在实际调试中踩过，机械臂突然不动，查了半天才发现是这里。

4. Python代码实现：从公式到可运行模块

4.1 依赖选择与代码结构设计

代码实现这块，我选择用纯Python加numpy，不引入任何机器人学框架。原因有两个：一是新手需要看到每一步的计算过程，框架会把细节藏起来；二是公式已经推导出来了，直接实现反而更轻量，也更容易移植到嵌入式平台的C代码。

整体代码分成三个模块。第一个是参数配置模块，放所有连杆长度和关节限位，方便标定后统一修改。第二个是正解模块，输入四个关节角返回末端位姿。第三个是逆解模块，输入目标位置和末端姿态返回四个关节角，并且处理多解选择和无解报错。最后加一个验证模块，用随机生成的正解结果喂给逆解，检查能否还原回原角度。

依赖只需要numpy做矩阵运算和三角函数，几个函数调用就够。如果你的运行环境装不了numpy，其实用math库也能写，只是矩阵连乘要手写循环，稍微麻烦一点。

4.2 正运动学代码实现

先看正解的代码，核心就是把前面推导的公式照着敲一遍：

import numpy as np # 连杆参数，单位mm，根据实际机械臂标定后修改 D1 = 80.0 # 基座到肩关节高度 A2 = 120.0 # 大臂长度 A3 = 100.0 # 小臂长度 A4 = 60.0 # 腕到末端距离 def forward_kinematics(theta1, theta2, theta3, theta4): """输入四个关节角(弧度)，返回末端位置(x,y,z)和俯仰角phi""" # 平面内三连杆投影 r = (A2 * np.cos(theta2) + A3 * np.cos(theta2 + theta3) + A4 * np.cos(theta2 + theta3 + theta4)) z_plane = (A2 * np.sin(theta2) + A3 * np.sin(theta2 + theta3) + A4 * np.sin(theta2 + theta3 + theta4)) # 映射到基坐标系 x = r * np.cos(theta1) y = r * np.sin(theta1) z = D1 + z_plane phi = theta2 + theta3 + theta4 return x, y, z, phi

这段代码逻辑非常直白，就是把上一章的公式逐项翻译。有个细节是phi的返回，它代表末端俯仰角，在逆解验证时会用到。我习惯在正解里顺便把它算出来，省得调用方再算一遍。

4.3 逆运动学代码实现

逆解稍微复杂一些，要处理多解和无解：

def inverse_kinematics(x, y, z, phi, elbow_up=True): """输入目标位置(x,y,z)和末端俯仰角phi，返回四个关节角。 elbow_up决定肘部构型分支""" # 第一步：基座角 theta1 = np.arctan2(y, x) r = np.sqrt(x**2 + y**2) # 检查工作空间 if r < 1e-6: return None # 目标点在基座轴线上，θ1不唯一 # 第二步：反推腕关节位置 r_w = r - A4 * np.cos(phi) z_w = z - D1 - A4 * np.sin(phi) # 第三步：平面二连杆逆解 D_sq = r_w**2 + z_w**2 D = np.sqrt(D_sq) # 工作空间检查 if D > A2 + A3 or D < abs(A2 - A3): return None # 超出可达范围 # 余弦定理求肘部角 cos_gamma = (A2**2 + A3**2 - D_sq) / (2 * A2 * A3) cos_gamma = np.clip(cos_gamma, -1.0, 1.0) # 数值裁剪 gamma = np.arccos(cos_gamma) if elbow_up: theta3 = -(np.pi - gamma) delta = np.arctan2(A3 * np.sin(gamma), A2 + A3 * np.cos(gamma)) theta2 = np.arctan2(z_w, r_w) + delta else: theta3 = np.pi - gamma delta = np.arctan2(A3 * np.sin(gamma), A2 + A3 * np.cos(gamma)) theta2 = np.arctan2(z_w, r_w) - delta # 第四步：腕部角 theta4 = phi - theta2 - theta3 return np.array([theta1, theta2, theta3, theta4])

这里我特意把elbow_up做成参数，让调用方自己选分支。np.clip那行就是前面强调的数值裁剪，看起来不起眼，但没有它代码在某些边界位姿下会直接崩。工作空间检查也很重要，实际运行时如果上位机下发了一个够不到的目标，逆解应该返回None让上层处理，而不是算出一组错误角度然后让机械臂撞限位。

4.4 验证与可视化调试

写完正逆解，必须做闭环验证。我的方法是：随机生成一批关节角，用正解算出末端位姿，再把位姿喂给逆解，看能不能还原回原来的角度（或者等价的多解）。验证代码大概是这样：

def test_round_trip(n=1000): """正逆解闭环验证""" errors = [] for _ in range(n): # 生成限位内的随机关节角 t = np.random.uniform(-1.5, 1.5, 4) x, y, z, phi = forward_kinematics(*t) # 逆解 sol = inverse_kinematics(x, y, z, phi) if sol is None: continue # 用逆解结果再算一次正解，比较位置误差 x2, y2, z2, _ = forward_kinematics(*sol) err = np.sqrt((x-x2)**2 + (y-y2)**2 + (z-z2)**2) errors.append(err) print(f"平均位置误差: {np.mean(errors):.6f} mm") print(f"最大位置误差: {np.max(errors):.6f} mm")

实测下来，位置误差在1e-10量级，基本就是浮点精度极限。如果误差偏大，说明正逆解公式之间有不一致的地方，九成是某个角的符号或者象限处理有问题。可视化方面，我用matplotlib画了一个简单的三维散点图，把正解得到的末端轨迹和逆解要求的目标点画在一起，肉眼就能看出是否吻合。这一步虽然简单，但非常提效，比逐行打印日志快得多。

提示：验证时不要只测工作空间中心区域，一定要测边界附近和奇异位形附近，那些地方才是公式最容易暴露问题的地方。

5. 实测踩坑记录与排查经验

5.1 关节角多解与象限判断的坑

逆解的多解问题在实际调试中比想象中更烦人。前面公式里θ1有两个解、θ3有两个解，组合起来就是四组解。如果你不加选择地随便取一组，机械臂可能从意想不到的方向去够目标，轻则姿态别扭，重则直接撞到自己的底座或者旁边的设备。

我的处理策略是给每组解算一个“代价”，综合关节运动量、距离限位边界的距离、是否接近奇异位形来打分，选代价最低的那组。具体来说，我会优先选与当前关节角最接近的解，这样机械臂运动最平滑，不会出现突然大角度翻转。这个技巧在连续轨迹控制里尤其重要，因为相邻两个路径点的解如果跳变了，机械臂会抽风似地甩一下。

另外atan2的返回值范围是(-π, π]，而关节实际可能转过超过一圈。如果你的机械臂基座是连续旋转的，就需要自己维护角度累积，把atan2的结果加上圈数偏移，让它和上一时刻的角度连续。这一块在总线舵机机械臂上尤其常见，很多朋友反馈“机械臂走到一半突然反向转一大圈”，基本都是这个问题。

5.2 奇异位形与工作空间边界

奇异位形在4自由度机械臂上主要有两种表现。第一种是肘部完全伸直或者完全折叠，也就是前面公式里D接近A2+A3或abs(A2-A3)的时候，此时cos_gamma接近 ±1，γ接近 0 或 π，肘关节角对末端位置的变化极其敏感，一点点位置扰动就会导致角度剧变。第二种是腕关节角度导致θ4补偿量过大，实际已经超出腕关节限位。

处理办法很直接：在逆解代码里加工作空间检查，把D限制在[abs(A2-A3)+margin, A2+A3-margin]范围内，margin 取个几毫米。这样虽然会牺牲一小部分理论工作空间，但换来的是运行稳定，不会在边界附近出现剧烈抖动。我实测下来，留 5mm 的 margin 对实际抓取任务几乎没有影响，但抖动问题彻底消失了。

还有一种情况是目标点恰好落在基座轴线上，也就是r接近0，此时θ1不唯一。代码里我直接返回 None，让上层重新规划路径，避免出现除零或者任意角度的情况。

5.3 舵机执行端的偏差补偿

公式算得再准，最终还是要靠舵机去执行。总线舵机机械臂的实际偏差主要来自三个地方：舵机中位安装误差、连杆加工公差导致的长度偏差、以及舵机本身的回程差。这些机械误差会让理论角度和实际到达位置产生几毫米到十几毫米的偏差。

我的补偿方案分两步。第一步是标定连杆实际长度，用尺子量a2、a3、a4和d1，把实测值写进代码参数，而不是用设计值。别小看这一步，3D打印件的收缩和装配间隙能让设计值偏差 3% 以上，直接导致末端定位差好几毫米。第二步是给每个关节加一个零位偏移量，通过让机械臂走到几个已知标定点，反推出每个关节的实际角度和理论角度的差，作为常数补偿。这套标定做完，末端重复定位精度能从原来的 ±8mm 提升到 ±2mm 左右，对抓取任务来说完全够用。

这张表是我调试过程中反复遇到问题的汇总，建议你在自己的项目里也建一张类似的速查表，遇到问题先查表，能省下大量瞎猜的时间。

5.4 代码从公式到硬件的移植经验

Python验证通过之后，下一步通常是移植到树莓派、ESP32或者STM32这类主控上。移植时有几个点要特别注意。第一，嵌入式平台可能没有numpy，需要用math库逐个替换，np.arctan2换成math.atan2，矩阵运算要手写。第二，浮点精度不同，单片机上通常用float而不是double，边界处的数值裁剪要比Python里做得更保守一点。第三，三角函数计算耗时在单片机上不可忽略，如果控制频率要求高，可以考虑查表法或者定点数优化。

我自己的项目最后是跑在树莓派上的，直接复用Python代码，通过串口把角度发给舵机控制板。这个方案开发最快，适合原型验证阶段。如果后续要产品化，再考虑把逆解用C重写，移植到实时性更好的MCU上。

5.5 关于ros集成与后续扩展的一些体会

热词里提到了不少ros机械臂开发和仿真相关的内容，这块我也有过尝试。把4自由度机械臂的运动学封装成ROS的节点，好处是可以直接复用rviz做可视化、复用moveit做轨迹规划。但要注意的是，moveit对4自由度的支持不如6轴那么顺滑，配置的时候需要手动处理自由度数量和关节限位。我个人的建议是，先把逆解公式和代码吃透，再考虑往ROS里搬，否则出了问题你分不清是公式错了还是配置错了。

后续扩展方向的话，可以做轨迹规划、重力补偿、视觉抓取这些。但基础永远是运动学正逆解，把这部分搞扎实，后面的东西都是在这个基础上叠加。我自己踩过的所有坑，归根结底都是对坐标系、符号、象限这些基础细节不够严谨造成的。公式推导的时候多花点时间把每个角的几何含义想清楚，比事后调试省事得多。

最后分享一个我在实际项目里养成的小习惯：每改一次连杆参数或者坐标系约定，就把正逆解闭环验证跑一遍，看误差是否还在浮点精度量级。这个习惯帮我逮住了好几次因为参数笔误引入的隐性bug。机械臂这种东西，数值上的一点小错，到末端可能就是几十毫米的偏差，早发现早省心。