坦克机器人和IMU的PID控制

#1 楼

假设您的电动机可以向前和向后驱动。 （这不是必须的，但是如果车辆仅向前行驶，则与双向驱动器相比，操纵将需要大量空间）

假设您要通过命令手动控制速度所有电机。

变量：
r =陀螺仪偏航率（可以为正或负，假设（+）表示向左偏航；假设我们对每秒+/- 10deg的幅度感兴趣）
H =磁力计的航向（0至360）
H_com =指令（所需）航向
M_com =速度项（电动机功率，由用户控制）

M_l =左电动机功率（可以为正或负）
M_r =右电动机功率（可以为正或负）

M_l = M_com + Kr * r + Kh *（H_com- H）
M_r = M_com-Kr * r-Kh *（H_com-H）

Kp和Kh将通过分析或反复试验得出。
（H_com-H）是控制器试图通过将车辆（H）移至H_com来归零的错误项。

注意：


您应该添加一些模数逻辑，因为当您只能以60度到期望的航向时，您不想走300度。
以最大速度（最大前进或最大后退）行驶时，偏航力受到限制（因为电动机速度达到最大值），因此可能需要更改Kr和Kh来作为速度的函数。
关于控制器调整策略，请参阅此问题。

#2 楼

在使用Arduino时，您应该充分利用社区库中的资源。我的建议是使用PID库。

在该页面中有关于库如何工作的详细说明。它是创建者博客的链接。还提供了有关如何调整Kp，Ki和Kd的说明。

我有一个类似的项目，对于我们仅使用磁力计来说，结果太不精确了。对于机器人而言，最糟糕的情况是靠近干扰源（例如电视机或大型金属结构），然后根据转弯时做出的决定移动几米。

为了解决这些问题，我们使用了陀螺仪（本例中为L3G4200D）。这是我们最终使用的代码：

  unsigned long amount_off_target ;
  L3G4200D gyroscope;
  while (!gyroscope.begin(L3G4200D_SCALE_250DPS, L3G4200D_DATARATE_400HZ_50))  // Set scale 250 dps and 400HZ Output data rate (cut-off 50)
  {
    Serial.println("Could not find a valid L3G4200D sensor, check wiring!");
    return;
  }
  gyroscope.calibrate(100);

  // here it goes some code referred to our encoders

  double Setpoint = 0, Input = 0, Output = 0;
  const double scale_factor = 7000000.0f;
  PID myPID(&Input, &Output, &Setpoint, 9.5 / scale_factor, 4 / scale_factor, 3.5 / scale_factor, DIRECT);

  myPID.SetMode(AUTOMATIC);


  myPID.SetOutputLimits(-50, 50);   // these parameters are related to your particular motor PWM values
  int min_power = MN_POWER+35;

  long z_total = 0;
  Input = 0;
  const long target_z = long(float(degs) * (90000000.0f / 90.0f));
  unsigned long last_off_target = millis();
  unsigned long last_read_sensor = micros();
  const unsigned long started_turn = millis();

  Setpoint = target_z;
  // Grace period to start moving
  long last_not_moving = millis() + 50;


  int direction;

  while ((millis() - last_off_target) < 100L && (millis() - started_turn) < 2000L)
  {
   // more code about our encoders goes here
    const long now_millis = millis();
    const long bump_min_speed_frequency = 10;


    amount_off_target = abs(z_total - target_z);
    if (amount_off_target > 2506516L) {   // here you decide that you are on target
      last_off_target = millis();
    }

    const unsigned long read_sensor_time = micros();
    Vector norm = gyroscope.readNormalize();
    const long time_since_read = (read_sensor_time - last_read_sensor);
    last_read_sensor = read_sensor_time;

    long yaw = (long)norm.ZAxis;
    z_total += yaw * time_since_read;

    Input = double(z_total);
    myPID.Compute();

    if (Output > 0) {
      direction = 1;
    }
    else {
      direction = -1;
    }

    int mtr_pwr = min_power + abs(Output);


      motorRight( mtr_pwr, -direction);
      motorLeft( mtr_pwr, direction);

  }

我希望这可以作为实现的一般原理。如果您查看此PDF对其进行解释，则可以轻松理解主要代码结构。

该代码是由与我们团队合作的真正的Rockstar编码器调优的，它在许多不同的表面上都表现出色。您可能会注意到，在玻璃上时，它会过冲，而在厚地毯上时，会过冲。

#3 楼

您将电动机的功率定义为：p_left = p_average + p_differential
和p_right = p_average-p_differential

您的PID调节器应将滤波后的角度作为输入并为您提供p_differential。这样，它将控制机器人的“转向”。
发生上述事件时，只需更改参考角即可。

#4 楼

有没有，或者可以在电机上增加编码器？如果您这样做，我建议您尝试将其用作第一步。编码器使您可以根据电动机的实际转速而不是PWM来控制电动机的转速。如果机器人仍然无法直行，则可以尝试应用UMBmark方法进行更正。对我来说，它产生了非常令人满意的结果，尽管我没有在tank-link平台上尝试过。

如果没有选择编码器，则可以尝试IMU。请记住，它也容易出错，因此在这里过滤其读数将至关重要。您可以直接读取磁力计或通过集成陀螺仪读数来获得航向。要同时使用这两种方法，应使用卡尔曼滤波器-在此处或此处进行一些阅读。

然后，只需将KF计算出的航向插入常规PID控制器即可，这样，期望航向和实际航向之间的误差就越大，左右车轮之间的速度差越大。