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平面的投影距离ρ₀，即肩-肘平面内的水平基准距离。

此时，肩-肘机构构成一个以ρ为底边、h为高、L₁与L₂为两腰的三角形。其几何关系如下图所示（文字描述）：

       末端点 (ρ, h)
           *
          /|
         / |
    L₂  /  | h
       /   |
      /    |
     /θ₂   |
    *------*
 肘关节   肩关节投影点 (ρ, 0)
     θ₁

关键观察：三角形的三个边长已知——底边ρ、高h、斜边L₁与L₂。因此，可直接应用余弦定理求解关节角。

3. 余弦定理的嵌入式实现与数值稳定性分析

3.1 关节角θ₁与θ₂的解析表达式

设肩关节角θ₁为前臂相对于水平面的夹角，肘关节角θ₂为后臂相对于前臂的补角（即内角）。根据余弦定理：

首先计算肩-肘-末端构成三角形的内角α（位于肘关节处）：

$$
\cos\alpha = \frac{L_1^2 + \rho^2 - L_2^2}{2 \cdot L_1 \cdot \rho}
$$

则肘关节角θ₂为：

$$
\theta_2 = \pi - \alpha = \arccos\left(\frac{L_1^2 + \rho^2 - L_2^2}{2 L_1 \rho}\right)
$$

注意：此处θ₂定义为前臂与后臂之间的夹角，而非后臂与水平线的夹角。这是舵机安装方式决定的——DS3218的零点位置设定为前臂水平时后臂亦水平，故θ₂直接驱动该舵机。

接着求解肩关节角θ₁。它由两部分组成：
- 水平基准角β = arctan(h/ρ)，即从肩关节指向末端投影点的仰角；
- 三角形内角γ（位于肩关节处），由余弦定理：

$$
\cos\gamma = \frac{L_1^2 + L_2^2 - \rho^2 - h^2}{2 L_1 L_2}
$$

但更简洁的推导是：在三角形中，θ₁ = β + γ，其中γ可通过前述α与三角形内角和关系得到。实际工程中，我们采用以下等效公式（经坐标系验证）：

$$
\theta_1 = \arctan2(h, \rho) + \arccos\left(\frac{\rho^2 + h^2 + L_1^2 - L_2^2}{2 L_1 \sqrt{\rho^2 + h^2}}\right)
$$

该式直接关联ρ、h、L₁、L₂，避免中间变量误差累积。

3.2 浮点运算的陷阱与防护机制

在STM32F4系列MCU上运行上述公式时，必须直面浮点运算的三大陷阱：

1. 除零异常
当ρ=0（即目标点位于Z轴上）时， cosα 分母为零。此时机械臂处于奇异位形（Singularity）：肩-肘机构退化为直线，θ₁与θ₂存在无穷多解。工程处理策略是：检测ρ < ε（ε取0.1mm），若成立，则强制设θ₁ = π/2（前臂竖直），θ₂ = 0（后臂与前臂共线），并向上位机返回警告标志。

2. 反余弦域外输入
arccos(x) 要求x ∈ [-1, 1]。由于测量误差、舍入误差，计算出的 cosα 可能为1.000001或-1.000001，导致 nan （Not a Number）。必须进行钳位：

float cos_alpha = (L1*L1 + rho*rho - L2*L2) / (2.0f * L1 * rho);
if (cos_alpha > 1.0f) cos_alpha = 1.0f;
else if (cos_alpha < -1.0f) cos_alpha = -1.0f;
float alpha = acosf(cos_alpha);

3. 小角度精度损失
当ρ或h极小时， atan2(h, rho) 的精度急剧下降。例如ρ=0.01mm, h=0.005mm时，浮点数有效位不足。解决方案是：当 sqrt(rho*rho + h*h) < 1.0f 时，改用泰勒展开近似 atan2(h, rho) ≈ h/rho （需保证rho≠0）。

4. C语言实现：面向嵌入式的轻量化算法

以下为在STM32 HAL库环境下实现的完整逆运动学求解函数。代码设计遵循嵌入式黄金法则：无动态内存分配、无递归、无浮点库以外的依赖、所有常量预计算。

#include "math.h"
#include "arm_math.h" // 可选：使用CMSIS-DSP的快速sin/cos

// 机械臂物理参数（单位：毫米，弧度）
#define L1_MM 125.0f   // 前臂长度
#define L2_MM 110.0f   // 后臂长度
#define EPSILON_MM 0.1f // 奇异位形检测阈值

typedef struct {
    float theta0; // 底座角 (rad)
    float theta1; // 肩关节角 (rad)
    float theta2; // 肘关节角 (rad)
    uint8_t valid; // 解有效标志：1=有效，0=奇异/无解
} ik_solution_t;

/**
 * @brief 三自由度机械臂逆运动学求解
 * @param x_mm 目标X坐标（mm）
 * @param y_mm 目标Y坐标（mm）
 * @param z_mm 目标Z坐标（mm）
 * @param sol 输出解结构体
 * @return 0=成功，1=无解（超出工作空间）
 */
uint8_t ik_solve_3dof(float x_mm, float y_mm, float z_mm, ik_solution_t* sol) {
    // 步骤1：计算XY平面投影
    float rho = sqrtf(x_mm*x_mm + y_mm*y_mm);
    sol->theta0 = atan2f(y_mm, x_mm); // 底座角

    // 步骤2：检测奇异位形（ρ过小）
    if (rho < EPSILON_MM) {
        // 处理Z轴目标点：设前臂竖直，后臂共线
        sol->theta1 = 1.5708f; // π/2
        sol->theta2 = 0.0f;
        sol->valid = 1;
        return 0;
    }

    // 步骤3：计算垂直平面内三角形参数
    float h = z_mm; // 高度差
    float r_sq = rho*rho + h*h; // 末端到肩关节距离的平方
    float r = sqrtf(r_sq); // 末端到肩关节距离

    // 步骤4：检查工作空间边界（三角形不等式）
    if (r > (L1_MM + L2_MM) || r < fabsf(L1_MM - L2_MM)) {
        sol->valid = 0;
        return 1; // 超出可达空间
    }

    // 步骤5：计算肘关节角θ2（使用余弦定理）
    float cos_theta2 = (L1_MM*L1_MM + L2_MM*L2_MM - r_sq) / (2.0f * L1_MM * L2_MM);
    // 钳位处理
    if (cos_theta2 > 1.0f) cos_theta2 = 1.0f;
    else if (cos_theta2 < -1.0f) cos_theta2 = -1.0f;
    sol->theta2 = acosf(cos_theta2);

    // 步骤6：计算肩关节角θ1
    // 先求基准仰角
    float beta = atan2f(h, rho);
    // 再求三角形内角γ
    float cos_gamma = (r_sq + L1_MM*L1_MM - L2_MM*L2_MM) / (2.0f * r * L1_MM);
    if (cos_gamma > 1.0f) cos_gamma = 1.0f;
    else if (cos_gamma < -1.0f) cos_gamma = -1.0f;
    float gamma = acosf(cos_gamma);

    sol->theta1 = beta + gamma;
    sol->valid = 1;
    return 0;
}

// 舵机PWM映射函数（示例：MG996R 0-180° → 100-500 PWM）
static uint16_t angle_to_pwm(float rad_angle) {
    float deg = rad_angle * 57.2958f; // rad to deg
    // 裁剪到0-180°
    if (deg < 0.0f) deg = 0.0f;
    else if (deg > 180.0f) deg = 180.0f;
    return (uint16_t)(100 + (deg / 180.0f) * 400); // 100-500对应0-180°
}

该实现通过以下设计保障嵌入式可靠性：
- 全静态内存 ：无 malloc ，解结构体在栈上分配；
- 饱和保护 ：所有角度输出均经物理范围裁剪；
- 早期退出 ：在计算链前端即检测奇异与越界，避免无效运算；
- 整型优化提示 ：注释中明确标出可替换为Q31定点运算的潜在位置（如 arm_cos_f32() 替代 cosf() ），为资源受限场景预留升级路径。

5. 工作空间分析与实际部署约束

算法正确性不等于系统可用性。必须结合机械臂的实际物理约束进行工作空间（Workspace）验证。本系统的理论可达空间是一个旋转椭球体，但受以下因素压缩：

5.1 舵机行程限制的硬约束

• MG996R底座：θ₀ ∈ [0, π] rad（0–180°），导致工作空间被限制在半球内；
• DS3218肩部：θ₁ ∈ [0.35, 2.36] rad（20–135°），排除了前臂过度下压（易碰撞底座）与过度上扬（重心不稳）区域；
• DS3218肘部：θ₂ ∈ [0, 2.36] rad（0–135°），确保后臂始终在前臂前方，避免自碰撞。

这些约束必须在上位机规划层强制执行，否则IK解虽数学有效，但下位机无法执行。典型做法是在上位机中构建一个三维查找表（LUT），对每个（x,y,z）网格点预计算θ₁、θ₂，并标记是否在舵机行程内。运行时仅做查表+插值，极大降低MCU负担。

5.2 动态负载与响应延迟的隐性约束

实测发现：当末端负载≥200g时，DS3218在θ₂接近135°位置会出现明显抖动。根源在于舵机内部电位器反馈噪声在高增益区被放大。解决方案不是修改算法，而是调整机械设计——在肘部增加配重块，降低转动惯量。这印证了一个嵌入式工程师的常识： 算法永远无法弥补糟糕的机电耦合设计 。

5.3 串行通信的时序约束

本系统采用USART2以115200bps速率接收上位机坐标。单次坐标包（x,y,z各为float，共12字节）传输耗时约1.04ms。而IK求解在STM32F407上耗时约85μs（Keil MDK, O2优化）。这意味着MCU有充足时间处理——但前提是中断服务程序（ISR）必须足够精简。我们禁用所有非必要外设中断，USART2 ISR仅做DMA接收缓冲区管理，解算任务交由FreeRTOS任务队列处理，确保主循环周期稳定在10ms（100Hz控制频率）。

6. 误差溯源与校准实践

即使算法完美，实测定位误差仍达±3mm。通过系统性排查，我们定位到三个主要误差源：

6.1 机械装配误差

激光测距仪测量显示：前臂L₁实测值为124.3mm，而非标称125mm；后臂L₂为109.7mm。这种0.5%的制造公差在末端被几何放大。解决方案是：在 ik_solve_3dof() 中将L₁/L₂改为可配置参数，并通过上位机GUI提供校准界面——用户输入已知靶点坐标，系统自动迭代调整L₁/L₂直至误差最小。

6.2 舵机零点漂移

DS3218在连续运行2小时后，零点偏移达0.8°。根源是内部电位器温漂。应对策略：每次上电执行自校准——发送PWM=300（对应90°）信号，用高精度倾角传感器（如BNO055）读取实际角度，计算偏差Δθ，后续所有θ₁/θ₂输出均减去Δθ。

6.3 坐标系安装偏斜

MCU板卡与底座平面存在0.5°夹角，导致Z轴测量引入系统误差。解决方法最简单：在 ik_solve_3dof() 入口处，对输入z_mm进行补偿 z_mm += x_mm * tanf(0.0087f) （0.5°≈0.0087rad），该补偿系数通过三点标定法获得。

这些校准步骤全部固化为设备启动流程的一部分，用户无需干预。真正的工程成熟度，体现在将误差从“需要用户调试”转变为“系统自动消化”。

7. 从算法到产品的最后一公里

当IK算法在开发板上稳定运行后，真正的挑战才开始：如何将其封装为可复用、可维护、可诊断的嵌入式模块？

我们采用分层架构：
- 硬件抽象层（HAL） ： servo_driver.c 封装TIMx PWM输出、GPIO方向控制，向上提供 servo_set_angle(uint8_t id, float rad) 接口；
- 运动控制层（MCL） ： ik_solver.c 实现前述算法，提供 ik_solve_and_apply(x,y,z) ，内部调用HAL；
- 应用层（APP） ： main.c 中创建FreeRTOS任务 vIKTask ，从串口队列获取坐标，调用MCL，失败时触发LED报警。

最关键的工程实践是 状态机设计 。 vIKTask 不直接执行解算，而是维护一个五状态机：
1. IDLE ：等待新坐标；
2. VALIDATING ：检查坐标有效性（如NaN、Inf）；
3. SOLVING ：调用 ik_solve_3dof() ；
4. APPLYING ：将解映射为PWM并写入定时器；
5. ERROR_HANDLING ：记录错误码（如 IK_ERR_SINGULARITY ），通过USB CDC发送诊断信息。

每个状态转换均有超时保护（如SOLVING状态>5ms则强制跳转ERROR_HANDLING），杜绝死循环。这种设计让故障可追溯、可复现，远胜于“裸奔式”的无限循环。

最后分享一个血泪教训：在首次整机联调时，机械臂在到达目标点后持续微颤。示波器捕获到PWM信号存在100Hz纹波。根源竟是电源设计——舵机驱动芯片（TMC2209）的地线与MCU地线未单点共接，形成接地环路。最终解决方案是：在PCB上将所有舵机电源地汇聚于一点，再通过0Ω电阻连接至MCU地。 再完美的算法，也需干净的硬件土壤才能扎根 。