机械臂实战：从舵机PID控制到DH建模与正逆运动学求解
1. 舵机PID控制：从抖舵到平滑运动
第一次用Arduino控制舵机机械臂时，最让我头疼的就是"抖舵"问题——明明代码里设置了90度位置，舵机却像得了帕金森似的来回震颤。后来才发现，直接给目标角度就像让汽车瞬间加速到100km/h，舵机内部的齿轮组根本承受不了这种突变。
解决这个问题的关键就是PID控制。想象一下开车时的油门控制：发现车速慢了就多踩点油门（比例控制），长时间低于目标速度就持续加深油门（积分控制），快要超速时提前收油（微分控制）。舵机控制也是同样的道理：
// Arduino平台的位置式PID实现 float Position_PID(float CurrentAngle, float TargetAngle) { static float Error, LastError, Integral; Error = TargetAngle - CurrentAngle; Integral += Error; float Output = Kp * Error + Ki * Integral + Kd * (Error - LastError); LastError = Error; return constrain(Output, -255, 255); // 限制PWM输出范围 }实测发现参数调校有门道：Kp值太大会导致超调振荡，我通常先用Ziegler-Nichols法初步确定参数：
- 先将Ki和Kd设为0
- 逐渐增大Kp直到出现持续振荡
- 取振荡时Kp值的60%作为最终Kp
- 积分时间Ti=振荡周期/2，微分时间Td=振荡周期/8
对于常见的SG90舵机，经过多次测试得到的黄金参数是：Kp=5.0，Ki=0.02，Kd=1.5。要注意不同型号舵机的扭矩和转速特性不同，比如MG996R这类大扭矩舵机需要更小的微分项来避免机械振动。
2. 坐标系搭建：从混乱到清晰的数学表达
当机械臂的关节数超过两个时，我发现单纯用角度计算末端位置就像在迷宫里转悠——各个关节的坐标系方向不统一，计算起来简直是一场灾难。直到接触了DH参数法，才明白为什么它被称为机械臂的"通用语言"。
DH参数的核心是四个关键参数：
- 连杆长度a：沿着x轴从当前z轴指向下一个z轴的距离
- 连杆转角α：当前z轴绕x轴旋转到下一个z轴的角度
- 连杆偏距d：沿着z轴从当前x轴指向下一个x轴的距离
- 关节角度θ：当前x轴绕z轴旋转到下一个x轴的角度
以三自由度机械臂为例，其DH参数表如下：
| 关节 | θ(°) | d(mm) | a(mm) | α(°) |
|---|---|---|---|---|
| 1 | θ1 | 150 | 0 | 90 |
| 2 | θ2 | 0 | 200 | 0 |
| 3 | θ3 | 0 | 150 | 0 |
建立坐标系时有个实用技巧：用彩色胶带标记各轴方向（红色-X，绿色-Y，蓝色-Z），这样在调试时能直观验证坐标系是否正确。我曾经因为把Z轴方向搞反，导致后续所有计算全部出错，这个教训让我养成了可视化验证的习惯。
3. 正运动学：像搭积木一样组合变换矩阵
有了DH参数后，正运动学就变成了矩阵乘法的游戏。每个关节的变换矩阵可以拆解为四个基本动作：
- 绕z轴旋转θ角度
- 沿z轴平移d距离
- 沿x轴平移a距离
- 绕x轴旋转α角度
用Python的numpy库实现特别方便：
import numpy as np from math import cos, sin def dh_matrix(theta, d, a, alpha): return np.array([ [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta)], [sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta)], [0, sin(alpha), cos(alpha), d], [0, 0, 0, 1] ])验证正运动学时，我常用"三点法"：
- 手动将机械臂摆到零位（所有关节角度为0）
- 根据DH参数计算末端理论位置
- 用尺子实际测量末端位置
- 误差超过5mm就需要检查DH参数
有个容易忽略的细节：矩阵乘法顺序不能错！T_total = T1 @ T2 @ T3 表示从基座到末端依次变换，如果顺序颠倒会得到完全错误的结果。我在早期项目中因为这个错误调试了整整两天。
4. 逆运动学：几何解法在嵌入式端的实践
当需要在STM32上实时计算逆运动学时，解析法复杂的运算根本跑不动，这时几何解法就成了救命稻草。以三自由度平面机械臂为例，可以将其看作两个三角形组合：
- 首先计算腕部坐标(x',y') = (x - L3cosφ, y - L3sinφ)
- 利用余弦定理求θ2：
float c2 = (x*x + y*y - L1*L1 - L2*L2) / (2*L1*L2); float theta2 = acos(c2); // 注意解的双重性 - 通过atan2求θ1：
float k1 = L1 + L2*cos(theta2); float k2 = L2*sin(theta2); float theta1 = atan2(y,x) - atan2(k2,k1);
在实现时要注意三个坑：
- acos函数的输入必须做clamp处理，防止浮点误差导致NaN
- atan2比atan更安全，能自动处理象限问题
- 对于多解情况要根据关节限制选择合理解
我在无人机吊舱控制项目中，将逆解计算优化到只需0.3ms（STM32F407 @168MHz），关键是把所有浮点运算转换为定点数处理，并用ARM的DSP库加速三角函数计算。
5. 轨迹规划：让运动更优雅
单纯求解逆运动学得到的是一系列离散点，直接执行会导致机械臂抖动。加入轨迹规划后，就像给运动加了缓动效果：
def cubic_interpolation(q0, q1, t, T): """三次多项式插值""" a0 = q0 a1 = 0 a2 = 3*(q1-q0)/(T**2) a3 = -2*(q1-q0)/(T**3) return a0 + a1*t + a2*t**2 + a3*t**3实际测试对比：
- 无轨迹规划时电流波动达1.2A
- 加入三次插值后电流平稳在0.6A左右
- 运动时间增加约15%，但机械损耗大幅降低
对于更复杂的空间轨迹，我推荐用梯形速度规划：加速-匀速-减速三个阶段，既能保证效率又避免冲击。在扫地机器人机械臂上应用后，关节齿轮寿命提升了3倍。