crazyflie控制算法

http://merafour.blog.163.com/blog/static/219102011201451935827412/

看Bitcraze的控制代码，我个人觉得需要有一个回路的概念，比如反馈回路。这在电路里边会碰到，再此我不过多复述。

Bitcraze在四轴姿态控制中使用了二级控制回路，记住不是两个控制回路，这是有区别的。也就是说这两个控制回路是级联的关系。第一级控制回路为姿态控制，此过程陀螺不参与。其输出作为第二级控制回路的输入。第二级控制回路为角速率控制，此过程我们会看到陀螺参与了运算。

细细想来，二级控制回路至少有两个好处。DIY过四轴的人可能都遇到过这样的问题，那就是加强姿态，陀螺就相对削弱了。反过来也是一样。然而Bitcraze由于把姿态与陀螺放在不同级的控制回路当中，从而避免了这个问题。另外一个好处跟网上某些人所谓低速融合与高速融合类似，我将其成为低速处理与高速处理。刚开始做的时候，我只会使用陀螺积分计算姿态。做过的人都知道这样子得到的姿态都由那些问题。于是我就在网上看到有人说将加速计与陀螺互补，于是就有了低速融合与高速融合。但对于机体做法我其实还不是很明白。知道一次次啃了Bitcraze的源码，我才知道Bitcraze将这个问题很好地解决了。

我们先来看看Bitcraze中的主循环：

 

static void stabilizerTask(void* param)

{

  uint32_t attitudeCounter = 0;

  uint32_t lastWakeTime;

 

  vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR);

 

  //Wait for the system to be fully started to start stabilization loop

  systemWaitStart();

 

  lastWakeTime = xTaskGetTickCount ();

 

  while(1)

  {

    vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ));

    imu6Read(&gyro, &acc);     

 

    if (imu6IsCalibrated())

    {

      commanderGetRPY(&eulerRollDesired, &eulerPitchDesired, &eulerYawDesired);

      commanderGetRPYType(&rollType, &pitchType, &yawType);

 

      if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER)

      {

        sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT);

        sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual);

        controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual,

                                     eulerRollDesired, eulerPitchDesired, -eulerYawDesired,

                                     &rollRateDesired, &pitchRateDesired, &yawRateDesired);

        attitudeCounter = 0;

      }

      if (rollType == RATE)

      {

        rollRateDesired = eulerRollDesired;

      }

      if (pitchType == RATE)

      {

        pitchRateDesired = eulerPitchDesired;

      }

      if (yawType == RATE)

      {

        yawRateDesired = -eulerYawDesired;

      }

 

      // TODO: Investigate possibility to subtract gyro drift.

      controllerCorrectRatePID(gyro.x, -gyro.y, gyro.z,

                               rollRateDesired, pitchRateDesired, yawRateDesired);

 

      controllerGetActuatorOutput(&actuatorRoll, &actuatorPitch, &actuatorYaw);

      commanderGetTrust(&actuatorThrust);

      if (actuatorThrust > 0)

      {

#if defined(TUNE_ROLL)

        distributePower(actuatorThrust, actuatorRoll, 0, 0);

#elif defined(TUNE_PITCH)

        distributePower(actuatorThrust, 0, actuatorPitch, 0);

#elif defined(TUNE_YAW)

        distributePower(actuatorThrust, 0, 0, -actuatorYaw);

#else

        distributePower(actuatorThrust, actuatorRoll, actuatorPitch, -actuatorYaw);

#endif

      }

      else

      {

        distributePower(0, 0, 0, 0);

        controllerResetAllPID();

      }

#if 0

     static int i = 0;

      if (++i > 19)

      {

        uartPrintf("%i, %i, %i\n",

            (int32_t)(eulerRollActual*100),

            (int32_t)(eulerPitchActual*100),

            (int32_t)(eulerYawActual*100));

        i = 0;

      }

#endif

    }

  }

}

需要提一下的是Bitcraze使用了一个嵌入式操作系统FreeRTOS。像vTaskDelayUntil则是由系统提供的函数，目的是控制主循环的执行频率，即IMU_UPDATE_FREQ。该宏定义如下：

#define IMU_UPDATE_FREQ   500

那么现在我们就知道了，在Bitcraze的四轴Crazyflie中主循环是两毫秒执行一次。也就是说PWM信号2ms刷新一次，这是很重要的。而且Crazyflie将IMU校准放到主循环中想比我自己之前放在初始化过程要好得多。IMU校准完了之后，首先做的当然是获取遥控器数据。然后我们看到的语句：

if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER)

就是用来控制姿态刷新频率的，也就是我前面所说的低速运算。我们说陀螺是快速反应的，从这里也可以看出。或者说姿态并不需要像陀螺那么高的速度。而姿态计算完了之后的RATE部分刚开始我是很不明白的。知道明白了Crazyflie的二级控制回路之后才搞懂。这样说吧，姿态控制需要姿态参与，但是航向角控制并不需要，此时就只需要一级控制回路就可以了。那自然就将遥控器的数据直接作为第二级控制回路的输入。经过第二级控制回路之后剩下的就是输出到各个电机输出。

       以上当然只是Crazyflie控制的一个基本思路，我们并没有详细去深究每一行代码与每一个细节。像主循环中用到的一些变量和函数了解了主循环的基本框架之后稍微有点经验的都能够看个七七八八。若是结合源码那就没有障碍了。

下面是我整理的Crazyflie与MWC姿态控制的流程图。