Webots轮腿机器人，轮足机器人，五杆双足轮式机器人仿真，并联腿结构仿真。 代码是c编写的，有详细的注释。 提供完整模型以及代码。 涉及PID和运动学逆解，实现运动控制。 可以通过使用键盘按键实现前进，后退，左转，右转，原地转向，抬升，降落，跳跃动作并调速，同时在运动过程中可以调节双腿高度保持平衡等功能。 提供代码的注释

最近在折腾Webots里的轮腿机器人仿真，发现五杆双足轮式结构真是个体力活。这种并联腿结构看着像螃蟹腿，运动起来比想象中带劲——既能当轮子滚又能当腿蹦跶，今天主要聊聊怎么用C语言让它在地板上撒欢儿。

先看这货的硬件配置：两条五连杆机械腿通过舵机驱动，底盘上顶着俩驱动轮。控制代码里有个特别实在的struct：

typedef struct {
    WbDeviceTag wheels[2];    // 驱动轮
    WbDeviceTag leg_motors[4];// 四条舵机控制五杆结构
    double leg_lengths[2];    // 实时腿长
    PIDParams pid_wheel;      // 轮速PID
    PIDParams pid_height;     // 腿高PID
} Robot;

PID参数配置这块儿特别像老司机调校汽车悬挂。比如轮速控制用的增量式PID：

double update_pid(PIDParams *pid, double error) {
    // 微分项用最近两次误差计算防止突变
    double diff = (error - pid->last_error) / TIME_STEP;
    pid->integral += error * TIME_STEP;
    
    // 积分抗饱和处理
    if(pid->integral > MAX_INTEGRAL) pid->integral = MAX_INTEGRAL;
    else if(pid->integral < -MAX_INTEGRAL) pid->integral = -MAX_INTEGRAL;
    
    double output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * diff;
    pid->last_error = error;
    return output;
}

调试时发现Ki参数超过0.5就开始抽风，轮子会突然加速到飞起。后来加了个积分限幅才稳住，这跟家里热水器温度过冲一个道理。

运动学逆解算是个几何题。当我们要设定目标腿长时，得反过来算舵机角度：

void calculate_leg_angles(double target_length, double *angle1, double *angle2) {
    // 五杆机构几何解算
    double a = LINK1, b = LINK2;
    double c = sqrt(a*a + b*b - 2*a*b*cos(M_PI - target_length)); 
    *angle1 = asin((b*sin(target_length))/c);
    *angle2 = M_PI - (*angle1 + target_length);
    
    // 机械限位保护
    if(*angle1 > MAX_ANGLE) *angle1 = MAX_ANGLE;
    if(*angle2 < MIN_ANGLE) *angle2 = MIN_ANGLE;
}

这里用余弦定理硬算出来的角度还得考虑机械臂的实际运动范围，不然仿真时能看见机械腿扭成麻花。

键盘控制这块儿用了非阻塞输入，代码里有个状态机记录按键组合：

void process_input(char key, Robot *robot) {
    switch(key) {
        case 'W': 
            robot->target_speed += 0.2;
            break;
        case 'S':
            robot->target_speed -= 0.2;
            break;
        case 'J':  // 跳跃准备
            robot->leg_lengths[0] *= 0.8;
            robot->leg_lengths[1] *= 0.8;
            break;
        case ' ':  // 执行跳跃
            jump_trigger = 1;
            break;
        //...其他按键处理
    }
}

有趣的是跳跃动作的实现——先下蹲蓄力再弹起。代码里用了个状态标记，当检测到空格键按下时启动跳跃序列：

if(jump_trigger) {
    double torque = 30.0 * sin(jump_phase); // 正弦波输出扭矩
    set_motor_torque(leg_motors[0], torque);
    set_motor_torque(leg_motors[1], torque);
    jump_phase += 0.1;
    if(jump_phase > M_PI) jump_trigger = 0; // 跳跃周期结束
}

调试这个的时候没少翻车，扭矩给大了机器人能原地后空翻，小了又像蹲坑起不来。最后发现用正弦波控制发力时机刚好能让它跃起半米左右。

平衡调节藏在主循环里，每帧计算底盘倾斜度：

double roll = get_imu_data();
double height_adjust = roll * 0.3; // 根据倾斜度调整腿长
robot->leg_lengths[0] += height_adjust;
robot->leg_lengths[1] -= height_adjust;

这招让机器人跑起来像滑雪运动员，左摇右摆的时候自动调节双腿高度找平衡。实测在0.3倍增益下，就算故意推它一把也能在三步内恢复平衡。

整套代码最带劲的是转弯控制。当收到转向指令时，左右轮差速配合腿长调整：

void turn_robot(double angle, Robot *robot) {
    double diff = angle * TURN_GAIN;
    robot->target_speed_left = robot->target_speed + diff;
    robot->target_speed_right = robot->target_speed - diff;
    
    // 内侧腿抬升
    robot->leg_lengths[0] *= (diff > 0) ? 0.9 : 1.1;
    robot->leg_lengths[1] *= (diff > 0) ? 1.1 : 0.9;
}

这么搞出来的漂移转弯特别有拉力赛感觉，转弯时内侧腿自动收缩降低重心，外侧腿伸展增大支撑面。

代码仓库里有个隐藏菜单——在运行时长按L键能调出实时参数调节界面，直接修改PID参数看效果。这功能救了我无数次调试，比重新编译快多了。