[22‘ 전람회] 전국과학전람회(1) - PID 이야기

이번에는 전국과학전람회에 진출 한 뒤에 가장 많은 시간을 들여서 공부했던 PID에 대해서 글을 써보고자 한다.

 

먼저, 나는 일반적으로 그런 알고리즘을 전혀 모르던 상황이었다. 그런데 줄줄이로 인해서 자동제어를 해야 하는 상황이 오게 되었고, 결국 pid를 공부하기 시작했다.

 

1. 정의

PID제어란 비례-적분-미분 제어기(Proportional-Integral-Differential controller)로 실제 응용분야에서 가장 많이 사용되는 대표적인 형태의 제어기법이다.

출처 : https://velog.io/@717lumos/Control-PID-%EC%A0%9C%EC%96%B4

다음과 같은 그림의 모습으로 이루어지는 제어기이다.

기본적으로 feedback 제어기의 형태를 가지고 있으며, 제어하고자 하는 대상의 output을 측정하여 원하고자 하는 Set point과 비교해서 error값을 계산하고 이 오차값을 이용해서 제어에 필요한 제어값을 계산하는 구조로 되어있다.

 

이때 제어 파라미터 $K_p, K_i, K_d$를 gain이라고 부르며이 값을 적절하게 계산하는 과정을 tuning이라고 부른다.

 

출처 : https://velog.io/@717lumos/Control-PID-%EC%A0%9C%EC%96%B4

그리고 다음과 같은 방식을 통해서 목표값까지 제어를 진행한다.

 

PID제어에 대한 세부적인 설명

P(비례) 제어는 제어량과 목표값의 차이(편차)에 비례하여 제어하는 방법이다.
편차가 크면 조작량을 크게, 편차가 작으면 조작량을 작게 하여 제어량을 목푯값에 유연하게 도달시킨다.
$K_p$값을 크게 하면 편차에 따른 조작량이 크므로, 상승시간이 줄어 빠르게 목푯값에 도달하는 대신, 오버슈트 값이 크고 시스템에 무리를 줄 위험이 있다.
반대로 $K_p$을 작게 하면 상승시간은 길어져 목표값에 느리게 도달한다.
P제어의 문제점은 목표치에 가까워질 수록 편차가 작아져 세밀한 제어가 힘들게 된다는 점이다.

I(적분) 제어는 편차를 시간에 대해 누적(적분)하고, 이 누적값이 특정값이 되면 조작량을 증가시켜 편차를 없앰으로써 목푯값에 더욱 정밀하게 접근하도록 한다.
$K_i$값이 크면 오버슈트가 커지고 상승시간이 약간 감소한다.
$K_i$값이 작으면 그 반대가 된다. I 제어의 문제점은 반응속도(응답시간)가 느려지는 것이다.

D(미분) 제어는 목표량과 제어량의 편차를 비교해 이와 반대되는 쪽(기울기)으로 조작하는 방식이다.
급격히 어느 한 방향으로 힘이 쏠리면 반대방향으로 힘을 주어 그 힘을 상쇄하려는 제어이다.
외란과 목표값의 편차, 혹은 이번 편차와 직전 편차를 비교해 이 크기에 따라 조작량을 결정하며, D 제어를 통해 외란에 대한 응답성을 개선하고 안정성을 높일 수 있다.
$K_d$값이 클수록 안정화에 걸리는 시간이 줄어들어 정착시간이 감소된다.

 

2. 줄줄이와 PID의 관계

줄줄이는 모터를 움직여서 앞뒤로 이동하는 능력을 토대로 일정 각도를 맞춰야 한다.

이러한 부분에서 원하는 각도를 set 하고, imu를 이용해서 현재 각도를 구한다면 error 값을 구할 수 있게 된다.

 

이후 이 e 값을 최소화하는 방향으로 만들어 각도를 조절하기 위해서 PID 제어를 이용하면 원하는 각도를 만들 수 있다는 가설을 세우게 되었다.

 

이 부분에서 encoder 모터를 이용하면 되지 않을까? 하는 생각도 해보았다. 하지만, encoder모터의 장점은 정확히 어느 정도를 움직였는지 알고, 이를 통해서 보정이 가능하다는 점이라고 생각한다. 그리고 우리의 목표인 각도 조절을 위해서는 실시간 피드백이 필요했으므로 결국 PID제어가 필요하다는 결론에 도달하였다.

 

가장 먼저 사용한 PID의 방식은 다음 글에서 이미 한번 작성하였다.

2023.03.03 - [탐구 💾/2022 탐구 이야기] - [22' 전람회] 충북과학전람회 (4) - 줄다리기를 로봇으로 구현하자 (2)

 

PID를 블로그에서 사용한, 그리고 유튜브에서 사용한 이것저것을 떼와서 사용했었다. 하지만 이는 내가 PID를 이해하지 않고 그냥 사용하는 것이라고 생각해서 제대로 PID를 코드로 언제든 구현하고 싶다는 욕심이 생겼다. 따라서 많은 서칭을 통해서 여러 코드를 뜯어보게 되었다.

 

BlancingWii라는 코드를 찾았고, 그 안의 파일을 하나하나 뜯어보게 되었다.

그런데 문제가 생겼었다.

 

코드가 1000줄 이상인 것들이 많은 것은 물론이고 독일어였다.

그렇게 저녁 5시정도부터 아침 7시까지 코드만 해석하고 이를 결국 해석을 한 결과는 굉장히 허무하였다.

 

정말 이론적인 내용 그자체가 코드였던 것이다.

#include "Arduino.h"
#include "config.h"
#include "def.h"
#include "types.h"
#include "BalancingWii.h"
#include "IMU.h"
#include "Sensors.h"

void getEstimatedAttitude();

void computeIMU () {
  uint8_t axis;
  static int16_t gyroADCprevious[3] = {0,0,0};
  int16_t gyroADCp[3];
  int16_t gyroADCinter[3];
  static uint32_t timeInterleave = 0;

  //we separate the 2 situations because reading gyro values with a gyro only setup can be acchieved at a higher rate
  //gyro+nunchuk: we must wait for a quite high delay betwwen 2 reads to get both WM+ and Nunchuk data. It works with 3ms
  //gyro only: the delay to read 2 consecutive values can be reduced to only 0.65ms
  #if defined(NUNCHUCK)
    annexCode();
    while((uint16_t)(micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
    timeInterleave=micros();
    ACC_getADC();
    getEstimatedAttitude(); // computation time must last less than one interleaving delay
    while((uint16_t)(micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
    timeInterleave=micros();
    f.NUNCHUKDATA = 1;
    while(f.NUNCHUKDATA) ACC_getADC(); // For this interleaving reading, we must have a gyro update at this point (less delay)

    for (axis = 0; axis < 3; axis++) {
      // empirical, we take a weighted value of the current and the previous values
      // /4 is to average 4 values, note: overflow is not possible for WMP gyro here
      imu.gyroData[axis] = (imu.gyroADC[axis]*3+gyroADCprevious[axis])>>2;
      gyroADCprevious[axis] = imu.gyroADC[axis];
    }
  #else
    #if ACC
      ACC_getADC();
      getEstimatedAttitude();
    #endif
    #if GYRO
      Gyro_getADC();
    #endif
    for (axis = 0; axis < 3; axis++)
      gyroADCp[axis] =  imu.gyroADC[axis];
    timeInterleave=micros();
    annexCode();
    uint8_t t=0;
    while((uint16_t)(micros()-timeInterleave)<650) t=1; //empirical, interleaving delay between 2 consecutive reads
    if (!t) annex650_overrun_count++;
    #if GYRO
      Gyro_getADC();
    #endif
    for (axis = 0; axis < 3; axis++) {
      gyroADCinter[axis] =  imu.gyroADC[axis]+gyroADCp[axis];
      // empirical, we take a weighted value of the current and the previous values
      imu.gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis])/3;
      gyroADCprevious[axis] = gyroADCinter[axis]>>1;
      if (!ACC) imu.accADC[axis]=0;
    }
  #endif
  #if defined(GYRO_SMOOTHING)
    static int16_t gyroSmooth[3] = {0,0,0};
    for (axis = 0; axis < 3; axis++) {
      imu.gyroData[axis] = (int16_t) ( ( (int32_t)((int32_t)gyroSmooth[axis] * (conf.Smoothing[axis]-1) )+imu.gyroData[axis]+1 ) / conf.Smoothing[axis]);
      gyroSmooth[axis] = imu.gyroData[axis];
    }
  #elif defined(TRI)
    static int16_t gyroYawSmooth = 0;
    imu.gyroData[YAW] = (gyroYawSmooth*2+imu.gyroData[YAW])/3;
    gyroYawSmooth = imu.gyroData[YAW];
  #endif
}

// **************************************************
// Simplified IMU based on "Complementary Filter"
// Inspired by http://starlino.com/imu_guide.html
//
// adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198
//
// The following ideas was used in this project:
// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
// 3) C. Hastings approximation for atan2()
// 4) Optimization tricks: http://www.hackersdelight.org/
//
// Currently Magnetometer uses separate CF which is used only
// for heading approximation.
//
// **************************************************

//******  advanced users settings *******************
/* Set the Low Pass Filter factor for ACC
   Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. */
#ifndef ACC_LPF_FACTOR
  #define ACC_LPF_FACTOR 120.0f
#endif

/* Set the Gyro Weight for Gyro/Acc complementary filter
   Increasing this value would reduce and delay Acc influence on the output of the filter*/
#ifndef GYR_CMPF_FACTOR
  #define GYR_CMPF_FACTOR 700
#endif

/* Set the Gyro Weight for Gyro/Magnetometer complementary filter
   Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
#define GYR_CMPFM_FACTOR 250

//****** end of advanced users settings *************
#define INV_GYR_CMPF_FACTOR   (1.0f / (GYR_CMPF_FACTOR  + 1.0f))
#define INV_GYR_CMPFM_FACTOR  (1.0f / (GYR_CMPFM_FACTOR + 1.0f))

typedef struct fp_vector {		
  float X,Y,Z;		
} t_fp_vector_def;

typedef union {		
  float A[3];		
  t_fp_vector_def V;		
} t_fp_vector;

typedef struct int32_t_vector {
  int32_t X,Y,Z;
} t_int32_t_vector_def;

typedef union {
  int32_t A[3];
  t_int32_t_vector_def V;
} t_int32_t_vector;

int16_t _atan2(int32_t y, int32_t x){
  float z = (float)y / x;
  int16_t a;
  if ( abs(y) < abs(x) ){
     a = 573 * z / (1.0f + 0.28f * z * z);
   if (x<0) {
     if (y<0) a -= 1800;
     else a += 1800;
   }
  } else {
   a = 900 - 573 * z / (z * z + 0.28f);
   if (y<0) a -= 1800;
  }
  return a;
}

float InvSqrt (float x){ 
  union{  
    int32_t i;  
    float   f; 
  } conv; 
  conv.f = x; 
  conv.i = 0x5f3759df - (conv.i >> 1); 
  return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
}

// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateV(struct fp_vector *v,float* delta) {
  fp_vector v_tmp = *v;
  v->Z -= delta[ROLL]  * v_tmp.X + delta[PITCH] * v_tmp.Y;
  v->X += delta[ROLL]  * v_tmp.Z - delta[YAW]   * v_tmp.Y;
  v->Y += delta[PITCH] * v_tmp.Z + delta[YAW]   * v_tmp.X;
}


static float accLPF[3] = {0, 0, ACC_1G};
static float invG; // 1/|G|

static t_fp_vector EstG;
static t_int32_t_vector EstG32;
#if MAG
  static t_int32_t_vector EstM32;
  static t_fp_vector EstM;
#endif

void getEstimatedAttitude(){
  uint8_t axis;
  int32_t accMag = 0;
  float scale, deltaGyroAngle[3];
  uint8_t validAcc;
  
  static uint16_t previousT;

  uint16_t currentT = micros();

  scale = (currentT - previousT) * GYRO_SCALE; // GYRO_SCALE unit: radian/microsecond
  previousT = currentT;

  // Initialization
  for (axis = 0; axis < 3; axis++) {
    deltaGyroAngle[axis] = imu.gyroADC[axis]  * scale; // radian

    accLPF[axis] = accLPF[axis] * (1.0f - (1.0f/ACC_LPF_FACTOR)) + imu.accADC[axis] * (1.0f/ACC_LPF_FACTOR);
    imu.accSmooth[axis] = accLPF[axis];
    
    accMag += (int32_t)imu.accSmooth[axis]*imu.accSmooth[axis] ;
  }

  rotateV(&EstG.V,deltaGyroAngle);
  #if MAG
    rotateV(&EstM.V,deltaGyroAngle);
  #endif

  accMag = accMag*100/((int32_t)ACC_1G*ACC_1G);
  validAcc = 72 < (uint16_t)accMag && (uint16_t)accMag < 133;
  // Apply complimentary filter (Gyro drift correction)
  // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
  // To do that, we just skip filter, as EstV already rotated by Gyro
  for (axis = 0; axis < 3; axis++) {
    if ( validAcc )
      EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + imu.accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
    EstG32.A[axis] = EstG.A[axis]; //int32_t cross calculation is a little bit faster than float	
    #if MAG
      EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR  + imu.magADC[axis]) * INV_GYR_CMPFM_FACTOR;
      EstM32.A[axis] = EstM.A[axis];
    #endif
  }
  
  if ((int16_t)EstG32.A[2] > ACCZ_25deg)
    f.SMALL_ANGLES_25 = 1;
  else
    f.SMALL_ANGLES_25 = 0;

  // Attitude of the estimated vector
  int32_t sqGX_sqGZ = sq(EstG32.V.X) + sq(EstG32.V.Z);
  invG = InvSqrt(sqGX_sqGZ + sq(EstG32.V.Y));
  att.angle[ROLL]  = _atan2(EstG32.V.X , EstG32.V.Z);
  att.angle[PITCH] = _atan2(EstG32.V.Y , InvSqrt(sqGX_sqGZ)*sqGX_sqGZ);

  #if MAG
    att.heading = _atan2(
      EstM32.V.Z * EstG32.V.X - EstM32.V.X * EstG32.V.Z,
      (EstM.V.Y * sqGX_sqGZ  - (EstM32.V.X * EstG32.V.X + EstM32.V.Z * EstG32.V.Z) * EstG.V.Y)*invG ); 
    att.heading += conf.mag_declination; // Set from GUI
    att.heading /= 10;
  #endif

  #if defined(THROTTLE_ANGLE_CORRECTION)
    cosZ = EstG.V.Z / ACC_1G * 100.0f;                                                        // cos(angleZ) * 100 
    throttleAngleCorrection = THROTTLE_ANGLE_CORRECTION * constrain(100 - cosZ, 0, 100) >>3;  // 16 bit ok: 200*150 = 30000  
  #endif
}

#define UPDATE_INTERVAL 25000    // 40hz update rate (20hz LPF on acc)
#define BARO_TAB_SIZE   21

#define ACC_Z_DEADBAND (ACC_1G>>5) // was 40 instead of 32 now


#define applyDeadband(value, deadband)  \
  if(abs(value) < deadband) {           \
    value = 0;                          \
  } else if(value > 0){                 \
    value -= deadband;                  \
  } else if(value < 0){                 \
    value += deadband;                  \
  }

#if BARO
uint8_t getEstimatedAltitude(){
  int32_t  BaroAlt;
  static float baroGroundTemperatureScale,logBaroGroundPressureSum;
  static float vel = 0.0f;
  static uint16_t previousT;
  uint16_t currentT = micros();
  uint16_t dTime;

  dTime = currentT - previousT;
  if (dTime < UPDATE_INTERVAL) return 0;
  previousT = currentT;

  if(calibratingB > 0) {
    logBaroGroundPressureSum = log(baroPressureSum);
    baroGroundTemperatureScale = (baroTemperature + 27315) *  29.271267f;
    calibratingB--;
  }

  // baroGroundPressureSum is not supposed to be 0 here
  // see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
  BaroAlt = ( logBaroGroundPressureSum - log(baroPressureSum) ) * baroGroundTemperatureScale;

  alt.EstAlt = (alt.EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)

  #if (defined(VARIOMETER) && (VARIOMETER != 2)) || !defined(SUPPRESS_BARO_ALTHOLD)
    //P
    int16_t error16 = constrain(AltHold - alt.EstAlt, -300, 300);
    applyDeadband(error16, 10); //remove small P parametr to reduce noise near zero position
    BaroPID = constrain((conf.pid[PIDALT].P8 * error16 >>7), -150, +150);

    //I
    errorAltitudeI += conf.pid[PIDALT].I8 * error16 >>6;
    errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
    BaroPID += errorAltitudeI>>9; //I in range +/-60
 
    // projection of ACC vector to global Z, with 1G subtructed
    // Math: accZ = A * G / |G| - 1G
    int16_t accZ = (imu.accSmooth[ROLL] * EstG32.V.X + imu.accSmooth[PITCH] * EstG32.V.Y + imu.accSmooth[YAW] * EstG32.V.Z) * invG;

    static int16_t accZoffset = 0;
    if (!f.ARMED) {
      accZoffset -= accZoffset>>3;
      accZoffset += accZ;
    }  
    accZ -= accZoffset>>3;
    applyDeadband(accZ, ACC_Z_DEADBAND);

    static int32_t lastBaroAlt;
    //int16_t baroVel = (alt.EstAlt - lastBaroAlt) * 1000000.0f / dTime;
    int16_t baroVel = (alt.EstAlt - lastBaroAlt) * (1000000 / UPDATE_INTERVAL);
    lastBaroAlt = alt.EstAlt;

    baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
    applyDeadband(baroVel, 10); // to reduce noise near zero

    // Integrator - velocity, cm/sec
    vel += accZ * ACC_VelScale * dTime;

    // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). 
    // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
    vel = vel * 0.985f + baroVel * 0.015f;

    //D
    alt.vario = vel;
    applyDeadband(alt.vario, 5);
    BaroPID -= constrain(conf.pid[PIDALT].D8 * alt.vario >>4, -150, 150);
  #endif
  return 1;
}
#endif //BARO

이 코드로 기억을 하고 있으며 너무 직관적이었기에 허무했던 기억을 가지고 있다.