6自由度(6-DOF)机械臂逆运动学(IK)函数适配修复请求

Hey everyone, I'm stuck on a 6-DOF robotic arm project and could really use some help fixing my inverse kinematics (IK) code. Here's the breakdown of my issue in plain terms:

• My current code is built for a 3R planar arm (3 revolute joints, 2D motion only), but my physical arm is a full 6-DOF setup that needs to move freely in 3D x-y-z space with 6 joint angles.
• When I set the home position to 0, joints 3 and 6 (my code only controls 3 servos right now, but the physical arm has all 6) jump unpredictably to ~0.9 and ~0.3 radians instead of staying at 0.
• Most joints don't respond correctly to position commands; only a linear joint (if present) seems to move, but that's just a side effect of the IK algorithm not accounting for the arm's 3D structure or full 6 degrees of freedom.
• I tried cranking up servo torque, but that didn't fix anything—clearly, the problem is that my IK function doesn't match my arm's actual kinematic configuration.

I need help updating the IK function to properly handle 6-DOF 3D motion, so all 6 joints move to the correct angles when I input a target x-y-z position (and orientation, if needed). Here's my current code:

#include <Servo.h>
#include <math.h>

Servo joint1Servo;
Servo joint2Servo;
Servo joint3Servo;
// TODO: Add servos for joints 4,5,6 once IK is fixed
// Servo joint4Servo;
// Servo joint5Servo;
// Servo joint6Servo;

const float l1 = 100.0;  // Base to joint 2 link length (original 2D value)
const float l2 = 170.0;  // Joint 2 to 3 link length (original 2D value)
const float l3 = 130.0;  // Joint 3 to end effector link length (original 2D value)

float joint1 = 0.0;
float joint2 = 0.0;
float joint3 = 0.0;
float joint4 = 0.0;
float joint5 = 0.0;
float joint6 = 0.0;

void setup() {
  joint1Servo.attach(9);   // Base rotation (joint 1: revolute, z-axis)
  joint2Servo.attach(10);  // Shoulder (joint 2: revolute, y-axis)
  joint3Servo.attach(11);  // Elbow (joint 3: revolute, y-axis)
  // TODO: Attach remaining servos once IK is fixed
  // joint4Servo.attach(XX);
  // joint5Servo.attach(XX);
  // joint6Servo.attach(XX);

  Serial.begin(9600);
  Serial.println("Enter target x,y,z,roll,pitch,yaw (in mm/degrees):");
  
  // Set initial home position
  moveJoints(joint1, joint2, joint3, joint4, joint5, joint6);
}

// Original 2D IK function (for 3R planar arm) - THIS IS THE ROOT PROBLEM
bool inverseKinematics(float x, float y, float phi, float &theta1, float &theta2, float &theta3, bool elbowDown = true) {
  float phi_rad = phi * PI / 180.0;
  float x_w = x - l3 * cos(phi_rad);
  float y_w = y - l3 * sin(phi_rad);
  float d2 = x_w * x_w + y_w * y_w;
  
  if (d2 == 0) return false;
  float d = sqrt(d2);
  float cos_theta2 = (d2 - l1 * l1 - l2 * l2) / (2.0 * l1 * l2);
  
  if (abs(cos_theta2) > 1) return false;
  float theta2_rad = acos(cos_theta2);
  if (!elbowDown) theta2_rad = -theta2_rad;
  
  float sin_theta2 = sin(theta2_rad);
  float cos_theta2 = cos(theta2_rad);
  float theta1_rad = atan2(y_w, x_w) - atan2(l2 * sin_theta2, l1 + l2 * cos_theta2);
  float theta3_rad = phi_rad - theta1_rad - theta2_rad;
  
  theta1 = theta1_rad * 180.0 / PI;
  theta2 = theta2_rad * 180.0 / PI;
  theta3 = theta3_rad * 180.0 / PI;
  
  if (theta1 < 0 || theta1 > 180 || theta2 < 0 || theta2 > 180 || theta3 < 0 || theta3 > 180) {
    return false;
  }
  return true;
}

void moveJoints(float j1, float j2, float j3, float j4, float j5, float j6) {
  joint1Servo.write(j1);
  joint2Servo.write(j2);
  joint3Servo.write(j3);
  // TODO: Uncomment once servos are attached
  // joint4Servo.write(j4);
  // joint5Servo.write(j5);
  // joint6Servo.write(j6);
  
  // Update global joint variables
  joint1 = j1;
  joint2 = j2;
  joint3 = j3;
  joint4 = j4;
  joint5 = j5;
  joint6 = j6;
}

void loop() {
  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n');
    // For 6-DOF, we need x,y,z,roll,pitch,yaw instead of 2D x,y,phi
    float x, y, z, roll, pitch, yaw;
    int comma1 = input.indexOf(',');
    int comma2 = input.indexOf(',', comma1 + 1);
    int comma3 = input.indexOf(',', comma2 + 1);
    int comma4 = input.indexOf(',', comma3 + 1);
    int comma5 = input.indexOf(',', comma4 + 1);
    
    if (comma1 != -1 && comma2 != -1 && comma3 != -1 && comma4 != -1 && comma5 != -1) {
      x = input.substring(0, comma1).toFloat();
      y = input.substring(comma1 + 1, comma2).toFloat();
      z = input.substring(comma2 + 1, comma3).toFloat();
      roll = input.substring(comma3 + 1, comma4).toFloat();
      pitch = input.substring(comma4 + 1, comma5).toFloat();
      yaw = input.substring(comma5 + 1).toFloat();
      
      float newJ1, newJ2, newJ3, newJ4, newJ5, newJ6;
      // Call the NEW 6-DOF IK function instead of the old 2D one
      if (sixDOFInverseKinematics(x, y, z, roll, pitch, yaw, newJ1, newJ2, newJ3, newJ4, newJ5, newJ6)) {
        moveJoints(newJ1, newJ2, newJ3, newJ4, newJ5, newJ6);
        Serial.print("Moving to x="); Serial.print(x);
        Serial.print(", y="); Serial.print(y);
        Serial.print(", z="); Serial.print(z);
        Serial.print(" | Joints: ");
        Serial.print(newJ1); Serial.print(", ");
        Serial.print(newJ2); Serial.print(", ");
        Serial.print(newJ3); Serial.print(", ");
        Serial.print(newJ4); Serial.print(", ");
        Serial.print(newJ5); Serial.print(", ");
        Serial.println(newJ6);
      } else {
        Serial.println("No valid solution for given position/orientation!");
      }
    } else {
      Serial.println("Invalid input format! Use: x,y,z,roll,pitch,yaw (mm/degrees)");
    }
  }
}

// NEW 6-DOF 3D IK FUNCTION - ADAPT THIS TO YOUR PHYSICAL ARM!
// This is a generic implementation for a standard 6-DOF arm (similar to PUMA 560)
// YOU MUST UPDATE DH PARAMETERS TO MATCH YOUR ARM'S EXACT MEASUREMENTS!
bool sixDOFInverseKinematics(float x, float y, float z, float roll, float pitch, float yaw, 
                             float &j1, float &j2, float &j3, float &j4, float &j5, float &j6) {
  // Convert orientation angles to radians
  float roll_rad = roll * PI / 180.0;
  float pitch_rad = pitch * PI / 180.0;
  float yaw_rad = yaw * PI / 180.0;

  // --------------------------
  // Step 1: Calculate Joint 1 (Base Rotation)
  // --------------------------
  float r = sqrt(x*x + y*y);
  if (r < 0.001) {
    j1 = 0.0;  // Default to 0 if x/y are at origin
  } else {
    j1 = atan2(y, x) * 180.0 / PI;
  }

  // --------------------------
  // Step 2: Calculate Wrist Center Position
  // --------------------------
  // Exact length from joint 6 to end effector - MEASURE THIS!
  const float l6 = 80.0;  // Example value: UPDATE TO YOUR ARM'S LENGTH!
  
  // Build end effector rotation matrix (roll-pitch-yaw)
  float R[3][3];
  R[0][0] = cos(yaw_rad)*cos(pitch_rad);
  R[0][1] = cos(yaw_rad)*sin(pitch_rad)*sin(roll_rad) - sin(yaw_rad)*cos(roll_rad);
  R[0][2] = cos(yaw_rad)*sin(pitch_rad)*cos(roll_rad) + sin(yaw_rad)*sin(roll_rad);
  R[1][0] = sin(yaw_rad)*cos(pitch_rad);
  R[1][1] = sin(yaw_rad)*sin(pitch_rad)*sin(roll_rad) + cos(yaw_rad)*cos(roll_rad);
  R[1][2] = sin(yaw_rad)*sin(pitch_rad)*cos(roll_rad) - cos(yaw_rad)*sin(roll_rad);
  R[2][0] = -sin(pitch_rad);
  R[2][1] = cos(pitch_rad)*sin(roll_rad);
  R[2][2] = cos(pitch_rad)*cos(roll_rad);

  // Wrist center = end effector position minus end effector link along z-axis
  float wc_x = x - l6 * R[0][2];
  float wc_y = y - l6 * R[1][2];
  float wc_z = z - l6 * R[2][2];

  // --------------------------
  // Step 3: Calculate Joints 2 & 3 (Shoulder/Elbow)
  // --------------------------
  // YOUR ARM'S LINK LENGTHS - MEASURE THESE EXACTLY!
  const float dh_l1 = 100.0;  // Height from base to joint 2 axis
  const float dh_l2 = 170.0;  // Length from joint 2 to joint 3 axis
  const float dh_l3 = 130.0;  // Length from joint 3 to wrist center

  // Project wrist center onto base plane
  float wc_r = sqrt(wc_x*wc_x + wc_y*wc_y);
  float wc_z_rel = wc_z - dh_l1;  // Z position relative to joint 2

  // Calculate joint 3 (elbow angle)
  float d_sq = wc_r*wc_r + wc_z_rel*wc_z_rel;
  float cos_j3 = (d_sq - dh_l2*dh_l2 - dh_l3*dh_l3) / (2*dh_l2*dh_l3);
  if (abs(cos_j3) > 1.0) return false;  // Target is outside workspace
  float j3_rad = acos(cos_j3);
  j3 = j3_rad * 180.0 / PI;  // Elbow down configuration

  // Calculate joint 2 (shoulder angle)
  float k1 = dh_l2 + dh_l3*cos_j3;
  float k2 = dh_l3*sin(j3_rad);
  float j2_rad = atan2(wc_z_rel, wc_r) - atan2(k2, k1);
  j2 = j2_rad * 180.0 / PI;

  // --------------------------
  // Step 4: Calculate Wrist Joints (4/5/6)
  // --------------------------
  float j1_rad = j1 * PI / 180.0;
  float j2_rad = j2 * PI / 180.0;
  float j3_rad = j3 * PI / 180.0;

  // Rotation matrices for joints 1-3
  float R1[3][3] = {
    {cos(j1_rad), -sin(j1_rad), 0},
    {sin(j1_rad), cos(j1_rad), 0},
    {0, 0, 1}
  };
  float R2[3][3] = {
    {cos(j2_rad), 0, sin(j2_rad)},
    {0, 1, 0},
    {-sin(j2_rad), 0, cos(j2_rad)}
  };
  float R3[3][3] = {
    {cos(j3_rad), 0, sin(j3_rad)},
    {0, 1, 0},
    {-sin(j3_rad), 0, cos(j3_rad)}
  };

  // Combine rotations R1*R2*R3
  float R12[3][3], R123[3][3];
  matrixMultiply(R1, R2, R12);
  matrixMultiply(R12, R3, R123);

  // Invert R123 (transpose, since it's a rotation matrix)
  float R123_inv[3][3];
  matrixInverse(R123, R123_inv);

  // Wrist rotation matrix = inverse(R123) * end effector rotation
  float R_wrist[3][3];
  matrixMultiply(R123_inv, R, R_wrist);

  // Extract wrist joint angles
  j4 = atan2(R_wrist[1][2], R_wrist[0][2]) * 180.0 / PI;
  j5 = atan2(-R_wrist[2][2], sqrt(R_wrist[0][2]*R_wrist[0][2] + R_wrist[1][2]*R_wrist[1][2])) * 180.0 / PI;
  j6 = atan2(R_wrist[2][1], R_wrist[2][0]) * 180.0 / PI;

  // --------------------------
  // Step 5: Clamp to Physical Joint Limits
  // --------------------------
  // UPDATE THESE TO MATCH YOUR SERVOS' MAX/MIN ROTATION!
  j1 = constrain(j1, 0, 180);
  j2 = constrain(j2, -90, 90);
  j3 = constrain(j3, -90, 90);
  j4 = constrain(j4, -180, 180);
  j5 = constrain(j5, -90, 90);
  j6 = constrain(j6, -180, 180);

  return true;
}

// Helper: Multiply two 3x3 matrices
void matrixMultiply(float a[3][3], float b[3][3], float result[3][3]) {
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      result[i][j] = 0;
      for (int k = 0; k < 3; k++) {
        result[i][j] += a[i][k] * b[k][j];
      }
    }
  }
}

// Helper: Invert a 3x3 rotation matrix (transpose, since rotation matrices are orthogonal)
void matrixInverse(float mat[3][3], float inv[3][3]) {
  inv[0][0] = mat[0][0]; inv[0][1] = mat[1][0]; inv[0][2] = mat[2][0];
  inv[1][0] = mat[0][1]; inv[1][1