05 Control System

一、PID控制器

1.1 控制原理

下图来自 Sun Feb 08, 2015

Crazyflie控制系统

控制循环

• ESPlane有两个PID控制回路,Rate控制和Attitude控制,其中Rate主要使用陀螺仪数据(角速度)作为输入量,控制频率为500hz；Attitude控制使用拟合后的角度为输入量,控制频率为100hz.

• 注意在工程中,并不是所有的自由度都使用两级PID控制,默认pitch和roll自由度使用Attitude控制,yaw使用Rate控制.

可以在crtp_commander_rpyt.c中调整如下参数选择
static RPYType stabilizationModeRoll  = ANGLE; // Current stabilization type of roll (rate or angle)
static RPYType stabilizationModePitch = ANGLE; // Current stabilization type of pitch (rate or angle)
static RPYType stabilizationModeYaw   = RATE;  // Current stabilization type of yaw (rate or angle)

1.2 实现代码

void controllerPid(control_t *control, setpoint_t *setpoint,
                                         const sensorData_t *sensors,
                                         const state_t *state,
                                         const uint32_t tick)
{
  if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { //该宏定义用于控制PID的计算频率,时间基准来自MPU6050触发的中断
    // Rate-controled YAW is moving YAW angle setpoint
    if (setpoint->mode.yaw == modeVelocity) {                                                    //rata模式,对yaw做修正
       attitudeDesired.yaw += setpoint->attitudeRate.yaw * ATTITUDE_UPDATE_DT;
      while (attitudeDesired.yaw > 180.0f)
        attitudeDesired.yaw -= 360.0f;
      while (attitudeDesired.yaw < -180.0f)
        attitudeDesired.yaw += 360.0f;
    } else {                                                                                                               //attitude模式
      attitudeDesired.yaw = setpoint->attitude.yaw;
    }
  }

  if (RATE_DO_EXECUTE(POSITION_RATE, tick)) {                                               //位置控制
    positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
  }

  if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
    // Switch between manual and automatic position control
    if (setpoint->mode.z == modeDisable) {
      actuatorThrust = setpoint->thrust;
    }
    if (setpoint->mode.x == modeDisable || setpoint->mode.y == modeDisable) {
      attitudeDesired.roll = setpoint->attitude.roll;
      attitudeDesired.pitch = setpoint->attitude.pitch;
    }

    attitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw,
                                attitudeDesired.roll, attitudeDesired.pitch, attitudeDesired.yaw,
                                &rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw);

    // For roll and pitch, if velocity mode, overwrite rateDesired with the setpoint
    // value. Also reset the PID to avoid error buildup, which can lead to unstable
    // behavior if level mode is engaged later
    if (setpoint->mode.roll == modeVelocity) {
      rateDesired.roll = setpoint->attitudeRate.roll;
      attitudeControllerResetRollAttitudePID();
    }
    if (setpoint->mode.pitch == modeVelocity) {
      rateDesired.pitch = setpoint->attitudeRate.pitch;
      attitudeControllerResetPitchAttitudePID();
    }

    // TODO: Investigate possibility to subtract gyro drift.
    attitudeControllerCorrectRatePID(sensors->gyro.x, -sensors->gyro.y, sensors->gyro.z,
                             rateDesired.roll, rateDesired.pitch, rateDesired.yaw);

    attitudeControllerGetActuatorOutput(&control->roll,
                                        &control->pitch,
                                        &control->yaw);

    control->yaw = -control->yaw;
  }

  if (tiltCompensationEnabled)
  {
    control->thrust = actuatorThrust / sensfusion6GetInvThrustCompensationForTilt();
  }
  else
  {
    control->thrust = actuatorThrust;
  }

  if (control->thrust == 0)
  {
    control->thrust = 0;
    control->roll = 0;
    control->pitch = 0;
    control->yaw = 0;

    attitudeControllerResetAllPID();
    positionControllerResetAllPID();

    // Reset the calculated YAW angle for rate control
    attitudeDesired.yaw = state->attitude.yaw;
  }
}

参数调整大致思路 PID coefficients can be optimized using Particle Swarm Optimization, or many other algorithms. This is called automatic tuning, and is my second goal for the refactoring I'm trying to do. For manual tuning, tune the Kp gain first, with Ki and Kd at 0. The Kp tuning will give you the majority of the stabilization. If you want smooth motion, a smaller Kp is the goal. For aerobatic maneuvers you'll want a higher Kp to force a faster return to the setpoint. After finding a stable Kp (Too high and you'll have marginal stability or oscillations), tune the Ki to help prevent drift and hold your heading. Finally tune the Kd to adjust the snappiness of returning to the setpoint. By snappiness, I mean how quickly it eliminates overshoot from the adjustment. 链接

二、Mellinger控制器

/* This controller is based on the following publication: Daniel Mellinger, Vijay Kumar: Minimum snap trajectory generation and control for quadrotors. IEEE International Conference on Robotics and Automation (ICRA), 2011.

Crazyflie project added the following:

• Integral terms (compensates for: battery voltage drop over time, unbalanced center of mass due to asymmmetries, and uneven wear on propellers and motors)

• D-term for angular velocityer

• Support to use this controller as an attitude-only controller for manual flight

  */