三自由度机械臂逆运动学嵌入式实现与优化

1. 逆运动学求解的工程背景与建模基础

在机械臂控制系统中，逆运动学（Inverse Kinematics, IK）是连接上位机指令与下位机执行的核心算法环节。其本质是：已知末端执行器在笛卡尔空间中的目标坐标（x₀, y₀, z₀），反向求解各关节所需的角度值（θ₁, θ₂, θ₃, …），使机械臂实际末端能精确到达该点。这一过程并非数学推导的终点，而是嵌入式系统中实时性、精度、鲁棒性三者博弈的起点。

本节所讨论的机械臂为典型的三自由度（3-DOF）平面串联结构，由底座旋转关节（θ₀）、肩部俯仰关节（θ₁）和肘部俯仰关节（θ₂）构成，末端抓手视为刚性延伸段。该模型虽简化了真实机械臂的复杂性（如手腕偏航、翻滚等自由度），却完整保留了逆解算法的核心逻辑——几何约束下的三角关系求解，且具备极强的工程可移植性。实际项目中，我们采用自制三电机结构：底座使用MG996R舵机（0–180°范围，12-bit分辨率），肩部与肘部均采用DS3218（0–270°，12-bit），所有关节轴线相互正交，满足标准Denavit-Hartenberg（D-H）参数建模前提。

建模的第一步是建立统一的空间参考系。我们以底座旋转中心为原点O，定义右手笛卡尔坐标系：
- X轴沿底座初始朝向水平向前；
- Y轴水平向左（符合右手法则）；
- Z轴垂直向上。

此坐标系与STM32或ESP32的物理安装姿态严格对齐：当MCU板卡平放于底座平台时，其加速度计X/Y/Z轴输出即直接对应上述坐标分量。这种硬件-坐标系绑定避免了后续数据变换引入的误差源，是嵌入式运动控制中常被忽视但至关重要的底层约定。

2. 几何分解：从三维空间到二维平面投影

逆运动学求解的关键在于降维。对于本三自由度结构，底座旋转（θ₀）独立决定末端在XY平面的方位角，而肩-肘关节组合则共同决定末端在垂直平面（即包含Z轴与当前径向矢量的平面）内的高度z₀与径向距离ρ₀。因此，整个问题可解耦为两个正交子问题：

2.1 底座旋转角θ₀的求解

设上位机下发的目标坐标为（x₀, y₀, z₀）。首先计算该点在XY平面的投影距离ρ₀与方位角φ₀：

$$
\rho_0 = \sqrt{x_0^2 + y_0^2}
$$

$$
\phi_0 = \atan2(y_0, x_0)
$$

此处必须使用 atan2(y, x) 而非 atan(y/x) ，原因在于后者无法区分第二、三象限（x<0时）及第四象限（y<0时）的符号特性。例如，当目标点位于（-1, 1）与（-1, -1）时， atan(1/-1)=atan(-1/-1)=-π/4 ，结果完全错误；而 atan2(1,-1)=3π/4 ， atan2(-1,-1)=-3π/4 ，准确反映象限位置。在嵌入式C语言实现中，需包含 math.h 并确保浮点运算库已链接（如ARM GCC的 -lm 选项），否则 atan2f() 将不可用。

底座关节的实际旋转角度θ₀即为φ₀。但需注意物理限制：MG996R舵机标称范围为0–180°，对应电平脉宽0.5–2.5ms。若φ₀超出[0, π]弧度（即[0°, 180°]），必须进行裁剪或映射。常见做法是将θ₀限定在[0, π]内，并在上位机层面约束任务空间——例如禁止机械臂向后大范围伸展。更鲁棒的方案是启用舵机的“连续旋转”模式（需硬件改造），但会牺牲绝对位置精度，本系统未采用。

2.2 垂直平面内的双关节解算

完成底座定向后，问题转化为二维平面内的经典“两连杆”问题。定义：
- L₁：肩关节到肘关节的距离（前臂长度），实测为125 mm；
- L₂：肘关节到末端中心的距离（后臂长度），实测为110 mm；
- h：末端目标点z₀与底座旋转中心z坐标之差。因底座中心即坐标系原点，故h = z₀；
- ρ：末端在XY平面的投影距离ρ₀，即肩-肘平面内