开源 6 自由度机械臂：从设计到控制全解析

typedef struct {
    float x, y, z;
    float rx, ry, rz;
} Pose_t;

Pose_t forward_kinematics(float joints[6]) {
    Pose_t pose;
    
    // D-H 变换矩阵连乘
    // T = A1 * A2 * A3 * A4 * A5 * A6
    
    // 提取位置
    pose.x = T[0][3];
    pose.y = T[1][3];
    pose.z = T[2][3];
    
    // 提取姿态 (欧拉角)
    pose.rx = atan2(T[2][1], T[2][2]);
    pose.ry = atan2(-T[2][0], sqrt(T[2][1]*T[2][1] + T[2][2]*T[2][2]));
    pose.rz = atan2(T[1][0], T[0][0]);
    
    return pose;
}

#define IK_MAX_ITERATIONS  100
#define IK_CONVERGENCE_TH  0.001

int inverse_kinematics(Pose_t target, float *joints) {
    float current_joints[6] = {0};
    float error;
    int iter = 0;
    
    while (iter < IK_MAX_ITERATIONS) {
        // 1. 计算当前位姿
        Pose_t current = forward_kinematics(current_joints);
        
        // 2. 计算误差
        error = sqrt(pow(target.x - current.x, 2) +
                     pow(target.y - current.y, 2) +
                     pow(target.z - current.z, 2));
        
        // 3. 收敛判断
        if (error < IK_CONVERGENCE_TH) {
            memcpy(joints, current_joints, sizeof(float)*6);
            return 0;  // 成功
        }
        
        // 4. 雅可比矩阵计算
        float J[6][6];
        compute_jacobian(current_joints, J);
        
        // 5. 计算关节增量 (伪逆)
        float dJ[6];
        float error_vec[6] = {
            target.x - current.x,
            target.y - current.y,
            target.z - current.z,
            target.rx - current.rx,
            target.ry - current.ry,
            target.rz - current.rz
        };
        
        // dJ = J^+ * error (伪逆)
        pseudo_inverse_multiply(J, error_vec, dJ);
        
        // 6. 更新关节角
        for (int i = 0; i < 6; i++) {
            current_joints[i] += 0.5 * dJ[i];  // 阻尼因子
        }
        
        iter++;
    }
    
    return -1;  // 失败
}

void joint_space_trajectory(float start[6], float end[6], 
                            float duration, float dt) {
    int steps = duration / dt;
    
    for (int i = 0; i <= steps; i++) {
        float t = (float)i / steps;
        
        // 五次多项式插值
        float s = 10*t*t*t - 15*t*t*t*t + 6*t*t*t*t*t;
        
        for (int j = 0; j < 6; j++) {
            float target = start[j] + (end[j] - start[j]) * s;
            motor_move_to(j, target);
        }
        
        delay_ms(dt * 1000);
    }
}

void cartesian_trajectory(Pose_t start, Pose_t end, 
                          float duration, float dt) {
    int steps = duration / dt;
    
    for (int i = 0; i <= steps; i++) {
        float t = (float)i / steps;
        
        // 直线插值
        Pose_t current;
        current.x = start.x + (end.x - start.x) * t;
        current.y = start.y + (end.y - start.y) * t;
        current.z = start.z + (end.z - start.z) * t;
        
        // 姿态插值 (SLERP)
        slerp(&start.rx, &end.rx, t, &current.rx);
        slerp(&start.ry, &end.ry, t, &current.ry);
        slerp(&start.rz, &end.rz, t, &current.rz);
        
        // 逆运动学解算
        float joints[6];
        if (inverse_kinematics(current, joints) == 0) {
            for (int j = 0; j < 6; j++) {
                motor_move_to(j, joints[j]);
            }
        }
        
        delay_ms(dt * 1000);
    }
}

void control_loop_2ms(void) {
    static uint32_t cycle_count = 0;
    
    // 1. 读取编码器
    for (int i = 0; i < 6; i++) {
        joint_angle[i] = encoder_read(i);
    }
    
    // 2. 运动学解算
    if (trajectory_active) {
        trajectory_update(joint_angle);
    }
    
    // 3. PID 控制
    for (int i = 0; i < 6; i++) {
        float error = target_angle[i] - joint_angle[i];
        float pwm = pid_calc(&pid[i], error);
        stepper_set_pwm(i, pwm);
    }
    
    // 4. 限位检测
    check_limit_switches();
    
    // 5. 状态上报
    if (cycle_count % 10 == 0) {
        send_status_report();
    }
    
    cycle_count++;
}

void parse_gcode(char *line) {
    char cmd[10];
    float params[26];  // A-Z
    
    sscanf(line, "%s", cmd);
    
    if (strcmp(cmd, "G0") == 0 || strcmp(cmd, "G1") == 0) {
        // 直线运动
        parse_params(line, params);
        
        Pose_t target;
        target.x = params['X'-'A'];
        target.y = params['Y'-'A'];
        target.z = params['Z'-'A'];
        
        float feedrate = params['F'-'A'];
        
        cartesian_move(&target, feedrate);
    }
    else if (strcmp(cmd, "G2") == 0 || strcmp(cmd, "G3") == 0) {
        // 圆弧运动
        // ...
    }
}