Webots環境における五杆式輪脚（ホイールレッグ）ロボットのシミュレーション構築について解説します。この并联（パラレル）リンク構造を持つロボットは、車輪による走行と脚部による跳躍や段差乗り越えを両立できる点が特徴です。本稿では、C言語を用いた制御プログラムの設計、特にPID制御、運動学逆解、および動的なモーション生成に焦点を当てます。

ロボットハードウェアの抽象化とデータ構造

制御システムを構築する際、まずはロボットの物理構成をC言語の構造体として抽象化します。ここでは、2つの駆動輪と4つの関節アクチュエータ（五杆機構を構成）を管理する構造体を定義します。

typedef struct {
    WbDeviceTag drive_wheels[2];       // 左右の駆動輪モーター
    WbDeviceTag joint_actuators[4];    // 脚部リンクを駆動するサーボモーター
    double current_leg_extension[2];   // 各脚の現在の伸展長
    PIDController velocity_controller; // 車輪速度制御用PID
    PIDController altitude_controller; // 車体高さ制御用PID
} WheeledLeggedBot;

PID制御アルゴリズムと積分飽和対策

車輪の速度制御や車体高さの維持にはPID制御が不可欠です。特に微分項のノイズ対策と積分項のワインドアップ（飽和）防止が重要となります。以下は、微分項にローパスフィルタ的な処理を施し、積分項にクリッピングを実装したPID計算関数です。

double compute_pid_output(PIDController *ctrl, double current_error, double dt) {
    // 比例項
    double p_term = ctrl->kp * current_error;
    
    // 積分項とアンチワインドアップ
    ctrl->integral_sum += current_error * dt;
    ctrl->integral_sum = fmin(fmax(ctrl->integral_sum, ctrl->i_limit_min), ctrl->i_limit_max);
    double i_term = ctrl->ki * ctrl->integral_sum;
    
    // 微分項（急激な変化を抑制するための係数適用）
    double raw_derivative = (current_error - ctrl->prev_error) / dt;
    ctrl->filtered_derivative = 0.8 * ctrl->filtered_derivative + 0.2 * raw_derivative;
    double d_term = ctrl->kd * ctrl->filtered_derivative;
    
    ctrl->prev_error = current_error;
    return p_term + i_term + d_term;
}

積分項の上限値を適切に設定しないと、目標値をオーバーシュートした際に制御が不安定になるため、物理モデルの慣性に合わせたチューニングが必要です。

五杆機構の運動学逆解

脚部の目標伸展長（高さ）から各関節の目標角度を算出するには、幾何学的な運動学逆解を解く必要があります。五杆機構の特性を利用し、余弦定理と正弦定理を組み合わせて関節角度を導出します。

void solve_inverse_kinematics(double target_extension, double *theta_hip, double *theta_knee) {
    const double length_proximal = PROXIMAL_LINK_LEN;
    const double length_distal = DISTAL_LINK_LEN;
    
    // 仮想対角線の長さを余弦定理で算出
    double virtual_diagonal = sqrt(pow(length_proximal, 2) + pow(length_distal, 2) 
                                 - 2 * length_proximal * length_distal * cos(M_PI - target_extension));
    
    // 各関節角度の計算
    *theta_hip = asin((length_distal * sin(target_extension)) / virtual_diagonal);
    *theta_knee = M_PI - (*theta_hip + target_extension);
    
    // 物理的な可動域制限（ジョイントリミット）
    *theta_hip = fmin(fmax(*theta_hip, HIP_MIN_LIMIT), HIP_MAX_LIMIT);
    *theta_knee = fmin(fmax(*theta_knee, KNEE_MIN_LIMIT), KNEE_MAX_LIMIT);
}

シミュレーション上でリンクが干渉したり、特異点で関節が反転したりするのを防ぐため、計算結果に対して必ず機械的なリミット値を適用します。

キーボード入力と状態遷移管理

ユーザーからの操作入力はノンブロッキングで取得し、状態機械（ステートマシン）を用いてロボットの動作モードを切り替えます。

void handle_keyboard_events(int key_code, WheeledLeggedBot *bot) {
    switch(key_code) {
        case WB_KEYBOARD_UP:
            bot->velocity_controller.setpoint += 0.15;
            break;
        case WB_KEYBOARD_DOWN:
            bot->velocity_controller.setpoint -= 0.15;
            break;
        case 'C': // 車体姿勢を低くする（沈み込み）
            bot->current_leg_extension[0] *= 0.75;
            bot->current_leg_extension[1] *= 0.75;
            break;
        case WB_KEYBOARD_SPACE: // 跳躍トリガー
            is_jumping = true;
            jump_phase_angle = 0.0;
            break;
    }
}

跳躍モーションのトルク制御

跳躍動作は、単に脚を伸ばすだけでなく、沈み込みからの爆発的なトルク出力が必要です。ここでは、位相角に基づく正弦波プロファイルを用いてモーターへのトルク指令を生成します。

void execute_jump_sequence(WheeledLeggedBot *bot) {
    if (is_jumping) {
        // 跳躍フェーズに応じたトルクカーブを生成
        double applied_torque = MAX_JUMP_TORQUE * sin(jump_phase_angle);
        
        wb_motor_set_torque(bot->joint_actuators[0], applied_torque);
        wb_motor_set_torque(bot->joint_actuators[1], applied_torque);
        
        jump_phase_angle += PHASE_INCREMENT;
        
        // 1周期（πラジアン）終了で跳躍シーケンスを完了
        if (jump_phase_angle >= M_PI) {
            is_jumping = false;
            wb_motor_set_torque(bot->joint_actuators[0], 0.0);
            wb_motor_set_torque(bot->joint_actuators[1], 0.0);
        }
    }
}

トルクのピークタイミングをロボットの質量やバネ定数に合わせることで、効率的に床反力を得て跳躍させることができます。

IMUフィードバックによる動的バランシングと差速旋回

走行中の外乱に対する姿勢維持と、旋回時の安定性を高めるため、IMU（慣性計測装置）のロール角データをフィードバックします。さらに、旋回時には内側と外側の脚長を非対称に制御し、遠心力による転倒を防ぎます。

void maintain_balance_and_turn(double roll_angle, double yaw_rate_command, WheeledLeggedBot *bot) {
    // ロール角に基づく高さ補正
    double roll_compensation = roll_angle * ROLL_GAIN;
    bot->current_leg_extension[0] += roll_compensation;
    bot->current_leg_extension[1] -= roll_compensation;
    
    // ヨーレート指令に基づく差速旋回と脚長調整
    if (fabs(yaw_rate_command) > 0.01) {
        double speed_diff = yaw_rate_command * YAW_GAIN;
        double left_wheel_target = bot->velocity_controller.setpoint + speed_diff;
        double right_wheel_target = bot->velocity_controller.setpoint - speed_diff;
        
        wb_motor_set_velocity(bot->drive_wheels[0], left_wheel_target);
        wb_motor_set_velocity(bot->drive_wheels[1], right_wheel_target);
        
        // 旋回内側の脚を縮めて重心を下げ、外側の脚を伸ばして支持基底面を拡大
        double leg_adjustment = (yaw_rate_command > 0) ? 0.92 : 1.08;
        bot->current_leg_extension[0] *= leg_adjustment;
        bot->current_leg_extension[1] *= (2.0 - leg_adjustment);
    }
}

このアプローチにより、ロボットは不整地やカーブにおいても、車体の傾きをアクティブに吸収しながら安定した走行を実現します。各ゲインパラメータは、シミュレーション環境内の物理エンジンが扱う質量や摩擦係数に応じて慎重に調整する必要があります。