如何为Arduino编写4 DOF机械臂的坐标转伺服角度计算函数？

Hey Rohit, let's tackle this problem of getting your 4DOF arm to move to specific coordinates automatically—no more tedious manual angle tweaking! 🎉

核心思路：逆运动学（IK） #

Right now you're using teach-style programming (manually recording angles), but to hit specific coordinates, we need inverse kinematics: working backwards from the end-effector's target (x,y,z) position to calculate each joint's required rotation angle.

First, let's align with your arm's structure (matching your servo setup):

1. Servo 0 (Pin 3): Base rotation (horizontal axis, 0-180°)
2. Servo 1 (Pin 5): Shoulder pitch (30-180°, per your code comments)
3. Servo 2 (Pin 9): Elbow pitch (0-100°, per your code comments)
4. Servo 3 (Pin 11): Gripper (90° closed, 75° open—unrelated to position, handled separately)

Step 1: Measure Your Arm's Critical Parameters #

Before writing code, you need to physically measure these values (use cm for consistency):

• L1: Length of the upper arm (from shoulder joint to elbow joint)
• L2: Length of the lower arm (from elbow joint to gripper tip)
• H: Vertical height from the base center to the shoulder joint (0 if the shoulder sits directly on the base)

Step 2: Inverse Kinematics Calculation Logic #

Here's the math to convert a target (x,y,z) coordinate to servo angles:

1. Base Rotation Angle (θ₀) #

The base handles horizontal rotation—calculate the angle between the x-axis and your target's (x,y) position:

θ₀ = atan2(y, x) * (180/PI);

Note: Convert radians to degrees, and add an offset if your servo's 0° doesn't align with the x-axis.

2. Horizontal Projection & Vertical Offset #

Calculate the horizontal distance from the base to the target's projection, plus the vertical difference from the shoulder:

float r = sqrt(x*x + y*y); // Horizontal distance from base to target projection
float h = z - H; // Vertical offset from shoulder to target

3. Check Reachability #

First, verify the target is within the arm's physical range:

float d = sqrt(r*r + h*h); // Straight-line distance from shoulder to target
if (d > L1 + L2 || d < abs(L1 - L2)) {
  Serial.println("Target is out of arm's reach!");
  return;
}

4. Elbow Angle (θ₂) #

Use the Law of Cosines to find the elbow's required angle:

float cosTheta2 = (L1*L1 + L2*L2 - d*d) / (2*L1*L2);
cosTheta2 = constrain(cosTheta2, -1.0, 1.0); // Fix floating-point errors
θ₂ = acos(cosTheta2) * (180/PI);

Constrain this to your elbow's 0-100° range.

5. Shoulder Angle (θ₁) #

Calculate two helper angles to find the shoulder's pitch:

float alpha = atan2(h, r) * (180/PI);
float cosBeta = (L1*L1 + d*d - L2*L2) / (2*L1*d);
cosBeta = constrain(cosBeta, -1.0, 1.0);
float beta = acos(cosBeta) * (180/PI);
θ₁ = alpha + beta;

Constrain this to your shoulder's 30-180° range, and add an offset if needed.

Step 3: Arduino Code Implementation #

Here's the full code integrating this logic with your existing setup:

#include <Servo.h>

Servo Servos[4];
// Replace these with your actual measured values
const float L1 = 12.0;  // Upper arm length (cm)
const float L2 = 11.5;  // Lower arm length (cm)
const float H = 4.0;    // Base to shoulder height (cm)
// Servo zero-offset adjustments (tweak based on your hardware)
const float BASE_OFFSET = 90.0;
const float SHOULDER_OFFSET = 80.0;
const float ELBOW_OFFSET = 0.0;

void setup() {
  Serial.begin(9600);
  // Attach servos to their pins (matches your original code)
  Servos[0].attach(3);  // Base
  Servos[1].attach(5);  // Shoulder
  Servos[2].attach(9);  // Elbow
  Servos[3].attach(11); // Gripper
  
  reset();
  // Test moving to a target coordinate (x=15, y=0, z=10, gripper closed)
  moveToCoordinate(15, 0, 10, 90);
  delay(3000);
  // Move back to initial position
  reset();
  detachServos();
}

// Core function: Move arm to target (x,y,z) with specified gripper angle
void moveToCoordinate(float x, float y, float z, int gripperAngle) {
  // 1. Calculate base rotation angle
  float theta0 = atan2(y, x) * (180.0 / PI);
  theta0 += BASE_OFFSET;
  theta0 = constrain(theta0, 0, 180);

  // 2. Calculate horizontal/vertical distances
  float r = sqrt(x*x + y*y);
  float h = z - H;
  float d = sqrt(r*r + h*h);

  // Check if target is reachable
  if (d > L1 + L2 || d < abs(L1 - L2)) {
    Serial.println("Error: Target out of reach!");
    return;
  }

  // 3. Calculate elbow angle
  float cosTheta2 = (L1*L1 + L2*L2 - d*d) / (2*L1*L2);
  cosTheta2 = constrain(cosTheta2, -1.0, 1.0);
  float theta2 = acos(cosTheta2) * (180.0 / PI);
  theta2 += ELBOW_OFFSET;
  theta2 = constrain(theta2, 0, 100);

  // 4. Calculate shoulder angle
  float alpha = atan2(h, r) * (180.0 / PI);
  float cosBeta = (L1*L1 + d*d - L2*L2) / (2*L1*d);
  cosBeta = constrain(cosBeta, -1.0, 1.0);
  float beta = acos(cosBeta) * (180.0 / PI);
  float theta1 = alpha + beta;
  theta1 += SHOULDER_OFFSET;
  theta1 = constrain(theta1, 30, 180);

  // 5. Move servos (matches your original reverse order for smooth motion)
  Servos[3].write(gripperAngle);
  delay(15);
  Servos[2].write(theta2);
  delay(15);
  Servos[1].write(theta1);
  delay(15);
  Servos[0].write(theta0);
  delay(15);
}

// Reset arm to initial position (matches your original first array)
void reset() {
  short first[] = { 180 , 80 , 0 , 90 };
  for(int i=3; i>=0; i--) {
    Servos[i].write(first[i]);
    delay(15);
  }
}

// Detach servos to save power
void detachServos() {
  Servos[0].detach();
  Servos[1].detach();
  Servos[2].detach();
  Servos[3].detach();
}

void loop(){}

Key Tips for Calibration #

• Measure accurately: Even small errors in L1, L2, or H will throw off your positioning.
• Tweak offsets: If the arm doesn't move to the exact spot, adjust the *_OFFSET values to match your servo's physical zero position.
• Smooth motion: For smoother movement, add angle interpolation (gradually move from current angles to target angles instead of jumping).

Alternatives If You Want to Skip the Math #

If inverse kinematics feels overwhelming, try open-source libraries like the Arduino Robot Arm Library—many pre-built libraries handle 4DOF IK calculations out of the box.

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