高度速度制御を追加する

[tmori]

理論とシミュレーションが一致しない件

理論のモデルと結果

{
    "plants": [
        {
            "comment": "Rotor1",
            "num": [
                "1"
            ],
            "den": [
                "Tr",
                "1"
            ]
        },
        {
            "comment": "Speed Altitude",
            "num": [
                "1"
            ],
            "den": [
                "m",
                "d"
            ]
        },
        {
            "comment": "Altitude",
            "num": [
                "1"
            ],
            "den": [
                "1",
                "0"
            ]
        }
    ],
    "controllers": [
        {
            "num": [
                "VAlt_Kd",
                "VAlt_Kp",
                "VAlt_Ki"
            ],
            "den": [
                "1",
                "0"
            ]
        },
        {
            "num": [
                "Alt_Kd",
                "Alt_Kp",
                "Alt_Ki"
            ],
            "den": [
                "1",
                "0"
            ]
        }
    ],
    "constants": {
        "m": 0.71,
        "g": 9.81,
        "N": 4,
        "d": 0.05,
        "A": 8.3e-07,
        "Tr": 0.0193,
        "Kr": 2896,
        "W0": 1448,
        "Ix": 0.0061,
        "Iy": 0.00653,
        "Iz": 0.0116,
        "VAlt_Kp": 1,
        "VAlt_Ki": 0.0,
        "VAlt_Kd": 1,
        "Alt_Kp": 1,
        "Alt_Ki": 0.0,
        "Alt_Kd": 1,
        "VRoll_Kp": 0.2,
        "VRoll_Ki": 1,
        "VRoll_Kd": 0.032,
        "Roll_Kp": 1,
        "Roll_Ki": 0,
        "Roll_Kd": 1
    },
    "pd_args": {
        "PM": 60,
        "Wc": 1,
        "Ki": 0.0
    }
}

ステップ応答：

シミュレーションの実装と結果

    DroneAltOutputType run(DroneALtInputType &in)
    {
        DroneAltOutputType out = prev_out;
        if (simulation_time >= control_cycle) {
            simulation_time = 0;
            /*
             * speed control
             */
            double target_spd = pos_control->calculate(in.target_altitude, in.pos.z);
            target_spd = flight_controller_get_limit_value(target_spd, 0, -max_spd, max_spd);
            /*
             * position control
             */
            //機体座標系の速度を地上座標系に変換
            EulerType  e = {in.euler.x, in.euler.y, in.euler.z};
            VectorType v = {in.spd.u, in.spd.v, in.spd.w};
            VectorType g_v = ground_vector_from_body(v, e);
            //地上座標系の速度でPID制御
            double throttle_power = spd_control->calculate(target_spd, g_v.z);
            throttle_power = flight_controller_get_limit_value(throttle_power, 0, -max_power, max_power);
            /*
             * thrust
             */
            out.thrust = (mass * gravity) + (throttle_gain * throttle_power);
        }
        simulation_time += delta_time;
        return out;
    }

シミュレーション結果：

NG c(Steady state value)  : 1.000   (Target: -1±-0.010 m)
OK T_r(Rise time)         : 4.944 s (Target: ≤ 10.000 s)
OK T_d(Delay time)        : 2.700 s (Target: ≤ 5.000 s)
OK O_s(Maximum overshoot) : 0.000   (Target: ≤ 1.000 m)
OK T_s(5% settling time)  : 6.970 s (Target: ≤ 20.000 s)

パラメータ：

SIMULATION_DELTA_TIME   0.001
MASS                    0.71
GRAVITY                 9.81
PID_ALT_CONTROL_CYCLE   0.0
PID_ALT_MAX_POWER       9.81
PID_ALT_THROTTLE_GAIN   1.0
PID_ALT_MAX_SPD         5.0
PID_ALT_Kp              1.0
PID_ALT_Ki              0.0
PID_ALT_Kd              1.0
PID_ALT_SPD_Kp          1.0
PID_ALT_SPD_Ki          0.0
PID_ALT_SPD_Kd          1.0