iFBalance

iFBalance-코딩으로 뽀샤버리기

7강. PID 제어 - 1부

Posted by INDIFROG on 2021-03-07
Words 1.5k and Reading Time 9 Minutes
Viewed Times

7강. PID 제어 - 1부

iFBalance 로봇을 이용하여 과학, 전자공학, 자동제어 등의 이론을 공부하는 목표중에 가장 중요한 것은 이번 강의에서 설명하는 바로 PID 제어입니다.

VSCODE_LIB

일반적으로 PID 제어를 공부하기 위해 위 Basic PID 제어 블럭도를 많이들 보셨을 것입니다. 개인적으로 PID 결합부에 +- 기호를 별도로 추가했습니다만, 세부적으로 혹은 코딩으로 PID 제어를 분석하다 보면 위와 같이 일반화된 PID 제어 이론 블럭도는 이해하는데 다소 어려운 부분이 있습니다. 그래서 iFBalance에 적합한 PID 제어 블럭도를 위해 Process, Output 및 기타 다른 부분들을 아래 그림처럼 변경을 해 봤습니다.

VSCODE_LIB

iFBalance 로봇은 벨런싱을 유지하기 위해 자동제어 이론에서 고전이라고 할 수 있는 PID 제어기, 일종의 피드백(feedback)제어기를 사용하고 있습니다. 밸런싱 로봇은 기본적으로 두 바퀴로 직립한 상태를 유지하도록 짧은 시간동안 연속해서 자세 제어를 하는 것입니다.

앞 6강에서 논의한 것처럼 IMU장치로 부터 Pitch 값을 측정하여 Input 값으로 그리고 0도 즉, 이를 원하고자 하는 설정값(Set Point)과 비교하여 오차(error)를 계산하고, 이 오차값을 이용하여 자세 제어에 필요한 비례-적분-미분 제어값을 계산해서 모터를 제어하는 구조로 되어 있습니다.

이 항들은 각각 오차값, 오차값의 적분(integral), 오차값의 미분(derivative)에 비례하기 때문에 비례-적분-미분 제어기 (Proportional–Integral–Derivative controller)라는 명칭을 가지고 있습니다.

이 세개의 항들의 직관적인 의미는 다음과 같습니다.

  비례항 : 현재 상태에서의 오차값의 크기에 비례한 제어 작용 
  적분항 : 정상상태(steady-state) 오차를 없애는 작용
  미분항 : 출력값(Output)의 급격한 변화를 완화시키기 위해 측정값(Input)의 변화량을 반영하여 
          input의 오버슛(overshoot)을 줄이고 안정성(stability)을 향상

PID 제어기는 위와 같은 표준식의 형태로 사용하기도 하지만, 경우에 따라서는 약간 변형된 형태로 사용하는 경우도 많습니다. 예를 들어, 비례항만을 가지거나, 혹은 비례-적분, 비례-미분항만을 가진 제어기의 형태로 단순화하여 사용하기도 하는데, 이때는 각각 P, PI, PD 제어기라 부릅니다.

위의 식에서 제어 파라메터 Kp, Ki, Kd를 이득값 혹은 게인(gain)이라고 하고, 적절한 이득 값을 수학적 혹은 실험적, 경험적 방법을 통해 계산하는 과정을 튜닝(tuning)이라고 합니다. PID 제어기의 튜닝에는 여러 가지 방법들이 있는데, 그중 가장 널리 알려진 것으로는 지글러-니콜스 방법이 있으며 최근엔 AI 알고리즘을 사용하여 튜닝하는 방법들도 발표되고 있습니다.

VSCODE_LIB
다양한 PID 파라미터 (Kp, Ki, Kd)가 시스템의 응답에 미치는 영향

[ 참고문헌 ] https://commons.wikimedia.org/wiki/File:PID_Compensation_Animated.gif

위 PID 애니메이션 그림을 통해서 다음과 같은 의미있는 내용을 유추해 볼 수 있습니다.

  1. PID 튜닝 방법을 대충 유추해서 알 수 있습니다.

    • 먼저, 순서가 중요합니다. iFBalance를 - Pitch값으로 약간 기울여 놓은 상태에서 ….
      Ki, Kd를 0으로 두고 Kp를 서시히 증가해 가면서 Pitch 각도가 + 값으로 넘어가는 값을 찾습니다.

    • 그 다음, Kp는 고정하고 Ki를 증가해 가면서 서시히 Pitch 각도가 +, - 값이 반복되는
      즉, 어느 정도 균일한 진동이 발생하는 값을 찾습니다.

    • 마지막으로 Kp, Ki를 고정한 상태에서 Kd를 서시히 증가해 가면서 안정된 값을 찾습니다.

  3. Kp는 Ki, Kd 보다도 훨씬 큰 값입니다. Kd를 사용해서 오버슈트를 어느 정도 제거가 되는 것을 볼 수 있습니다.

추후 보다 자세히 PID 파라미터 튜닝에 관련해서 강의를 다뤄보도록 하겠습니다.

그러나 위 PID 애니메이션은 PID 제어기가 대충 이런 방식으로 동작한다라는 것을 알 수 있는 하나의 예제이지 실제로 iFBalance PID에 대한 결과는 아닙니다. 벨런싱을 유지해야 하는 시스템의 요구사항때문에 이보다는 훨씬 복잡하다고 말할 수 있으며 위 그래프처럼 데이터를 구하기도 어렵습니다. 왜냐하면 자연상태에서 스스로 제자리에 가만히 정지해 있는 로봇이 아니기 때문입니다.

자 그럼 이제 iFBalance PID 블럭도를 기반으로 기초적인 iFPID 클래스를 작성해 보도록 하겠습니다.

iFPID.h

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
/******************************************************************************
iFPID.h - iFLine, iFBalance Arduino Library 
hkkim@koreaits.com ( KITS Lab )
original creation date: April 3, 2020

Development environment specifics:
Arduino IDE 1.8.x
iFZero, iFLine, iFBalance
******************************************************************************/

#pragma once

#include <Arduino.h>
#include <NeoPixelBus.h>


class iFPID {

    private:
        double setpoint = 0.0;

        double input;
        double output;

        double Kp = 20.0;
        double Ki = 1.0;
        double Kd = 0.5;

        unsigned long timeCurrent, timeLast ;
        double outMin, outMax;

    public:        

        // 5 ms => 0.005 sec : smapling time
        double dt = 0.005;  

         double error = 0.0;

        // Integral term
        double errorI = 0.0; 

        // Differential term
        double errorD = 0.0; 
        double inputLast = 0.0; // previous time errorK

    public:
        iFPID();

        void init();
        void init(double Kp, double Ki, double Kd);
        void calcParameter(int Ku, double Pu);
        
        void calc();
        void calc(double in, double* pMotorValues);

        void setPoint(double sp);
        void setInput(double in);
        double outInput();

        void SetOutputLimits(double outMin, double outMax); 
};

iFPID.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
/******************************************************************************
iFPID.h - iFLine, iFBalance Arduino Library 
hkkim@koreaits.com ( KITS Lab )
original creation date: April 3, 2020

Development environment specifics:
Arduino IDE 1.8.x
iFZero, iFLine, iFBalance
******************************************************************************/

#include <Arduino.h>
#include "iFPID.h"

iFPID::iFPID(){
    calcParameter( 65, 0.18); // 2020.09.21
}

void iFPID::calcParameter(int Ku, double Pu){

    Kp = 0.6 * Ku;

    // classic PID
    double Ti = 0.5 * Pu;    
    Ki = Kp / Ti;
     
    // Pessen Integral Rule
    Kd = Kp * Pu / 6.3;
    // Kd = Kp * Pu / 8;
}

void iFPID::init(){
    timeLast = timeCurrent = millis();
}

void iFPID::init(double Kp, double Ki, double Kd){
    this->Kp = Kp;
    this->Ki = Ki;
    this->Kd = Kd;
   
    timeLast = timeCurrent = millis();
}

void iFPID::calc(){
    timeCurrent = millis();

    dt = (double) ( timeCurrent - timeLast ) * 0.001 ; 

    error = setpoint - input;

    // Integral term
    errorI += error * dt;
    if(errorI > outMax) errorI= outMax;
    else if(errorI < outMin) errorI= outMin;

    // Differential term
    errorD = (input - inputLast) / dt;

    ////////////////////////////////////////////////
    // 
    output = Kp*error + Ki*errorI - Kd*errorD;

    if(output > outMax) output = outMax;
    else if(output < outMin) output = outMin;    

    inputLast = input;
    timeLast = timeCurrent;
}

void iFPID::calc(double in, double* pMotorValues){
    setInput( in );

    calc();
    pMotorValues[0] = -output;  // Left Motor
    pMotorValues[1] = output;   // Right Motor

}

void iFPID::setPoint(double setpoint){
    this->setpoint = setpoint;
}

void iFPID::setInput(double input){
    this->input = input;
}

double iFPID::outInput(){
    return output;
}

void iFPID::SetOutputLimits(double outMin, double outMax){
    this->outMin = outMin;
    this->outMax = outMax;
}

위 코드에서 볼 수 있는 것처럼 iFPID 클래스는 아주 기초적인 PID 기능을 수행할 것이라고 예상할 수 있습니다. 이 코드에서 중요한 점은 시간의 단위가 msec가 아니 sec라는 점입니다. 쉽게 간과할 수 있는 이주 중요한 요소입니다.

iFBalance에 위 코드를 적용했을 때, 바로 밸런싱을 위해 동작할까요? 아니지요… 그렇게 쉽게 될리가 없습니다.몇가지 추가적인 작업이 반드시 필요합니다.

VSCODE_LIB

iFBalance의 벨런싱을 유지하는 기능만 구현한다고 가정할 때, setpoint 를 0으로 설정하고 나면 단지, Pitch 값에 의해서만 PID 제어를 수행하게 됩니다.
그러나 두 바퀴가 존재하기 때문에 모터는 왼쪽, 오른쪽 각각 제어를 해야 합니다. 다만, 왼쪽의 PWM 출력에 (-) 값을 붙여주어야 합니다. 오른쪽과 반대방향으로 모터가 회전해야하기 때문이며 기울어지는 방향으로 로봇이 빠르게 이동을 해서 균형을 유지하도록 합니다.

따라서, 각각의 모터 토크가 동일하다는 가정하에 동일한 PID 출력값을 모터에게 PWM으로 가한다면 밸런싱 로봇은 균형을 유지할 것입니다.

iFMixer.h

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
/******************************************************************************
iFPID.h - iFLine, iFBalance Arduino Library 
hkkim@koreaits.com ( KITS Lab )
original creation date: April 3, 2020

Development environment specifics:
Arduino IDE 1.8.x
iFZero, iFLine, iFBalance
******************************************************************************/

#pragma once

#include <Arduino.h>
#include "iFMotor.h"

#define MAXMOTORS   2

#define LEFT_F      7
#define LEFT_R      8  
#define LEFT_F_CH   (0)
#define LEFT_R_CH   (1)

#define RIGHT_F     27
#define RIGHT_R     26   
#define RIGHT_F_CH  (2)
#define RIGHT_R_CH  (3)

class iFMixer {

    private:

        iFMotor motors[2] = { 
            iFMotor(LEFT_F, LEFT_R, LEFT_F_CH, LEFT_R_CH), 
            iFMotor(RIGHT_F, RIGHT_R, RIGHT_F_CH, RIGHT_R_CH), 
        };

    public:        
 
    public:
        iFMixer();

        void init();
        void writeMotor(uint8_t index, int value);
        void stop(void);
        void run(double *motorVals);

        int getMaxOutput();
};

iFMixer.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
/******************************************************************************
iFMixer.h - iFLine, iFBalance Arduino Library 
hkkim@koreaits.com ( KITS Lab )
original creation date: April 3, 2020

Development environment specifics:
Arduino IDE 1.8.x
iFZero, iFLine, iFBalance
******************************************************************************/

#include <Arduino.h>
#include "iFMixer.h"

iFMixer::iFMixer(){

}

void iFMixer::init(){

}

void iFMixer::writeMotor(uint8_t index, int value){
    motors[index].move( value );
}

void iFMixer::run(double *motorVals)
{

    for (uint8_t i = 0; i < MAXMOTORS; i++) {
        writeMotor(i, (int)motorVals[i]);
    }


}

void iFMixer::stop(void)
{
    for (uint8_t i = 0; i < MAXMOTORS; i++) {
        writeMotor(i, 0);
    }
}

int iFMixer::getMaxOutput(){
    return motors[0].getMaxSpeed();
}

main.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
/******************************************************************************
main.cpp - iFLine, iFBalance Arduino Library 
hkkim@koreaits.com ( KITS Lab )
original creation date: April 3, 2020

Development environment specifics:
Arduino IDE 1.8.x
iFBalance
******************************************************************************/

#include <Arduino.h>
#include "iFLED.h"
#include "iFMixer.h"
#include "iFEncoder.h"
#include "MPU9250.h"
#include "iFPID.h"

#define SerialDebug   false  // Set to true to get Serial output for debugging
#define I2Cclock      400000
#define MPU_INT_PIN   5 // Interrupt Pin definitions

iFLED         ifLed;
iFMixer       ifMixer;
iFEncoder     ifEncoder;
MPU9250       ifMpu;
iFPID         ifPid;


void setup()
{ 
  Serial.begin( 921600 );

  Wire.begin(21, 22, I2Cclock);

  ifMpu.selectFilter( QuatFilterSel::MADGWICK);
  ifMpu.setup(0x68);  // change to your own address
  delay(2000);      // to get stable data of MPU9250

  ///////////////////////////////////////////////////////
  // Color LED
  ifLed.init();  
  ifLed.setMode( BLINK );

  ifMixer.init();
  ifPid.init();
  ifPid.SetOutputLimits( -ifMixer.getMaxOutput(), ifMixer.getMaxOutput());

  ///////////////////////////////////////////////////////
  // Encoder
  ifEncoder.init(25, 14, 13, 15);  

}

void loop() {

  if (ifMpu.update()) {
    
    double motorVals[2];

    ifPid.calc(ifMpu.getPitch(), &motorVals[0]);
    ifMixer.run( &motorVals[0]);

  }
}

위 예제를 통해서 iFBalance 로봇은 기본적인 벨런싱 기능을 갖추게 되었습니다.
가끔씩 깊히 생각지도 않고 PID제어는 쉽다고 얘기하시는 분들을 보고는 합니다. 게다가 벨런싱 로봇은 이미 10여년 전에 나온 한물간 기능이 아닌가하는 얘기도 듣기도 합니다.

그것은 개인적인 생각의 차이일 수도 있습니다.

저는 이와 같이 쉽다고 생각하는 로봇에 직접 코딩을 하면서 이론에 접근하는 방식의 학습을 권유해 드립니다. PID 제어 이론과 알고리즘을 기반으로 실제 C/C++ 클래스를 코딩으로 구현할 수 있는 능력은 엔지니어로서 갖추어야 할 매우 중요한 능력입니다.

개발자로서 기본적으로 작동하는 소스를 오픈함으로써 더불어 PID 제어와 같은 자동제어 이론을 빠르게 체득하고 보다 창의적인 아이디어를 구현하는데 도움이 되었으면 하는 바램입니다.

iFBalance 7강 소스 파일 : iFBalance_07.zip

iFBalance
iFBalance PID 동작 주행영상
Shoot by Serimo on 2021-3-08
iFBalance PID 동작 영상