webots小教程-PID控制

webots小教程-PID控制

此教程在上一教程PD控制基础上进行。
在上一个小教程中写了一个简单的PD控制例程，适用于在物体运动方向上没有阻力的情况下。输出F= K * ERR + C * dERR 效果类似于弹簧阻尼器系统，大致如下

但是在实际情况下，有时候在运动方向上会有个阻力，如重力和摩擦力等。

接下来我们将上图的系统竖直放置，就会变成下面这个状态。

我们会发现小方块会到达不了目标位置。这是因为公式中
F= K * ERR + C * dERR
当 ERR 越趋近于0，F就越小。此时刻的输出 F 就刚好满足：
F= K * ERR + C * dERR = G
当向上的动力F和向下的重力G相等，小方块就不再继续向上加速运动了。
而继续运动导致 ERR 减小， F 随之减小，此时 F < G ，加速度方向向下。于是小方块开始在此处震荡，通过微分项消除震荡后如上图所示，表现出达不到目标位置的结果。
此时将向上的动力大小打印出来发现
恰好与重力相近（质量为2kg,G = 2 * 9.8 = 19.6N）

那么这个始终存在的误差就是被称为稳态误差。
为了消除这个稳态误差以达到目标点，完成任务，我们引入积分项。
输出 F = K * ERR + C * dERR
而积分项Iout += Ki * ERR
将积分项的输出结果加在F上
则 F = K * ERR + C * dERR + Iout
由此可见只要误差存在，则 Iout 不断增加，F 不断增大， F > G ，加速度向上。
最后达成下图状态

小方块可以消除稳态误差到达目标位置

程序如下：

/*
 * File:          pid.c
 * Date:          2021.08.18
 * Description:   PD Controller test
 * Author:        HiYoung
 * Modifications: 2021.08.18
 */

#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/position_sensor.h>

#define TIME_STEP 64

int main(int argc, char **argv) 
{
  wb_robot_init();

  /*加载驱动*/
  WbDeviceTag motor = wb_robot_get_device("linear motor1");//直线电机
  WbDeviceTag sensor = wb_robot_get_device("position sensor1");//位置传感器
  wb_position_sensor_enable(sensor, TIME_STEP);//位置传感器使能

  double last_pos = 0;
  double kp = 50;//比例
  double ki = 2;//积分
  double kd = 220;//微分
  double i_out = 0;
  while (wb_robot_step(TIME_STEP) != -1) 
  {
      double pos = wb_position_sensor_get_value(sensor);//获取传感器位置信息
      double force = kp * (0 - pos) + kd *(last_pos - pos);//P项和D项
      i_out += ki * (0 - pos);//I项
      force += i_out;//P+I+D
      last_pos = pos;
      if (force > 200)
          force = 200;
      else if (force < -200)
          force = -200;
      wb_motor_set_force(motor, force);
      printf("err:%f\r\n", (0 - pos));
  };
  wb_robot_cleanup();
  return 0;
}