Crazyflie firmware(1)-Position control

我们一直努力试图为Crazyflie 2.0开发a local positioning system, 我们现在完成了主要的两步:(1)improve the stabilizer code architecture, (2)move the position control code into the firmware. 

从an external system获取position, 但是为了控制Crazyflie在desired position,我们依然需要去控制pitch, roll and yaw。之前,我们一直在an external computer(即在Crazyflie之外)跑控制算法,但是现在我们可以将该控制算法移到the Crazyflie itself上跑。该控制算法是一个简单的PID控制器,并且还可以继续改善,这项工作的主要目的是improve the architecture in this area。我们还没有在firmware上实现estimation of the position,但是现在这个architecture可以支持开发者进行该implementation。

我们的整体架构architecture如下:

  • 上图架构中的Crazyflie上的sensors(传感器)有gyroscope, accelerometer and pressure sensor。我们未来可以加入更多,例如 position和altitude的测量 。
  • 上图架构中的state estimator(状态估计器)利用传感器的测量值,尽可能准确地估计Crazyflie的状态。状态包括Crazyflie的姿态角 (roll, pitch, yaw), position和speed。 目前我们的state estimator有针对姿态角和高度。在不久的将来,将会估计完整的position。
  • 上图架构中的commander会给出让Crazyflie跟踪的setpoint。目前是通过Crazyradio或者bluetooth从地面站上的CFclient对Crazyflie发送commander packet(即setpoint)。
  • 上图架构中的state controller会给出control inputs(目前使用的PID),使得Crazyflie可以到达指定的setpoint。
  • 上图架构中的power distribution将control inputs转换成可以驱动电机转动的指令。

以上的架构是在stabilizer.c的文件中实现的,架构中的每一个模块有对应的file。具体stabilizer.c代码请参见github的repo, 对应的模块分别有sensors.h, estimator.h, commander.h, controller.h以及power_distribution.h。

while(1) {
  vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP)); // 1KHz

  sensorsAcquire(&sensorData, tick);
  stateEstimator(&state, &sensorData, tick);
  commanderGetSetpoint(&setpoint, &state);
  stateController(&control, &sensorData, &state, &setpoint, tick);
  powerDistribution(&control);
  tick++;
}

我们新架构的哲学是keep the code as simple as possible and to allow customization at compile time。所以如果明天我们有了一个新的controller,那么我们只需要重新编译一下。

Leave a Comment