Scara Robot逆运动学问题求助：Arduino代码数学错误排查

核心问题分析 #

• 数据类型不匹配：

  • L1、L2是带小数的长度，用int类型会直接截断小数部分，完全丢失计算精度，必须改为float或double。
  • alpha、beta存储的是三角函数返回的弧度值，用int会丢失所有小数精度，需改为float类型。

• 数学公式错误：

  1. 计算beta时运算符优先级错误：公式分母应为2*L1*L2，原代码写成/2*L1*L2，实际会先除以2再乘L1L2，正确写法是/(2*L1*L2)。
  2. 计算alpha时逻辑错误：
     • 应使用atan2(y, x)而非atan(y/x)，避免x=0时的除零错误，同时atan2能正确处理所有象限的角度。
     • 后半部分分式括号缺失，正确应为atan2(L2*sin(beta), L1 + L2*cos(beta))，原代码括号位置导致计算逻辑完全错误。

• Serial输出用法错误：Serial.println(alpha, beta)不符合函数语法，需拆分输出两个变量。

• 定义域未校验：acos的参数必须在[-1, 1]范围内，否则会返回NaN（非数值），需要先判断目标点是否在机器人工作空间内（即(L1-L2)² ≤ x²+y² ≤ (L1+L2)²）。

修正后的代码 #

const float L1 = 202.25;
const float L2 = 193.35;

float alpha = 0.0;
float beta = 0.0;

void calc_IK(float x, float y){
  // 计算目标点到原点的距离平方
  float r_sq = x*x + y*y;
  // 校验目标点是否可达
  float min_r_sq = pow(L1 - L2, 2);
  float max_r_sq = pow(L1 + L2, 2);
  
  if(r_sq < min_r_sq || r_sq > max_r_sq){
    Serial.println("目标点超出工作范围");
    return;
  }
  
  // 修正beta的计算
  float beta_numerator = r_sq - L1*L1 - L2*L2;
  float beta_denominator = 2 * L1 * L2;
  beta = acos(beta_numerator / beta_denominator);
  
  // 修正alpha的计算
  float alpha_part = atan2(L2*sin(beta), L1 + L2*cos(beta));
  alpha = atan2(y, x) - alpha_part;
  
  // 输出弧度结果
  Serial.print("alpha(弧度): ");
  Serial.print(alpha);
  Serial.print(" | beta(弧度): ");
  Serial.println(beta);
  
  // 若需要角度输出，替换上面的输出：
  // Serial.print("alpha(角度): ");
  // Serial.print(alpha * 180 / PI);
  // Serial.print(" | beta(角度): ");
  // Serial.println(beta * 180 / PI);
}

void setup() {
  Serial.begin(9600);
}

void loop() {
  calc_IK(3.0, 5.0);
  delay(2000); // 延长延迟避免输出刷屏
}

额外说明 #

• SCARA逆运动学存在两组解（肘部向上/向下），如果需要另一组解，只需将beta取反：beta = -acos(beta_numerator / beta_denominator)，再重新计算alpha即可。
• 所有三角函数返回的是弧度值，如需转换为角度，乘以180/PI即可。

内容的提问来源于stack exchange，提问作者Finn