[아두이노 쿼드콥터] 이중 PID 제어

예전에 기울어진 각도에 따라 출력을 달리하는 PID 제어 실험하는 영상을 올려드렸는데요. 이 영상이었는데요.

영상을 보시면 아시겠지만 기울어짐에 따라 출력 조절이 잘 되지 않기에 급격하게 방향이 전환되는걸 보실 수 있어요.

이를 수정하기 위해 알아보던 중 중요한 사실 몇가지를 알게되었습니다. 

우선 드론의 자세 제어를 할 때는, 기존의 위치제어 할 때처럼 Error값을 단순한 각도차이 (Error=목표 각도-현재 각도)로 하지 않는다는 사실을요. 대신 이중 PID 제어를 한다고 하더군요. 

이중 PID 제어는 다음과 같습니다. 우선 목표 각도와 현재 각도의 차이를 P 제어합니다. 그리고 이 P 제어한 값을 각속도 목표값으로 하여 현재 각속도와의 에러를 활용하여 PID 제어를 한다고 하네요. 처음에 이 내용을 보고 잠시 엥?? 하고 어리둥절 했으나, 조금 생각해보니 어떤 느낌인지 알 거 같더군요. 

저희가 생각하기에도 단순히 각도 차이를 Error 값으로 잡기엔 각도 변화가 너무 빠르게 일어나기에 (위 영상에서 볼 수 있듯이ㅠㅠ) 제어가 쉽지 않을거 같습니다. 그렇기에 각도가 아닌 각속도 차이를 통해 제어하려고 하는데, 대신 각도 차이에 P제어 해준 값 (각도차이*상수 P)을 목표 각속도로 잡음으로써 더 많이 기울어질수록 목표 각속도 또한 더 커지게 되는 것이죠. 이렇게 되면 각도 제어가 아닌 속도제어가 되기 때문에 지금보다 드론이 훨씬 안정적으로 제어될 것 같습니다. 확실히 맞는지는 모르겠지만 저희가 이해한 바는 이렇습니다. 저희가 잘못 이해하고 있으면 언제든지 지적해주세요!

문제가 해결될 거라는 희망을 품고!!! 들뜬 마음으로 코딩 했습니다.

void PID_control_x() {

 currenttime = millis();

 T2 = (currenttime - previoustime) / 1000; // time

 // X-direction P + PID

 error_x = desired_angle - angle_y; //angle def

 P_angle_pid = P_angle_gain * error_x; //angle def + P control

 rate_x = (error_x - error_x1) / T2;

 // angle rate gy_x;

 error_pid_x = P_angle_pid - rate_x / 100; // Pcontrol_angle - angle rate = PID Goal

 m_x = m_x1 + G1 * error_pid_x + G2 * error_pid_x1 + G3 * error_pid_x2; // PID VALUE

 PID_value = constrain(0.1 * abs(m_x), b, 20);

 if (error_x >= 0) {

   r2 = PID_value;

 }

 if (error_x < 0) {

   r4 = PID_value;

 }

 error_x1 = error_x;

 error_pid_x2 = error_pid_x1;

 error_pid_x1 = error_pid_x;

 m_x1 = m_x;

 previoustime = currenttime;

} 

위와 같이요. Error 설정하는 부분 이외에는 크게 달라진 부분은 없습니다. 

코딩을 해봤으니 어떻게 작동하는지 봐야겠죠?

이전보다는 확실히 나아진 거 같더라구요. 그래도 여전히 부족한 감이 있네요.

저희끼리 얘기를 좀 해본 결과, 모터의 출력이 문제인 거 같았습니다. 현재 모터에 들어가는 PWM값을 조금씩 조정해봤는데 모터의 출력이 실제로 변한건 10번 정도였습니다. 제일 느린 회전 속도에서 제일 빠른 속도를 도달하는데 단 10단계만 거치게 된다면, 섬세한 제어는 힘든 것은 당연지사.. 

이에 여러 과정을 거치다 저희는 서보모터 라이브러리를 활용해보기로 했는데요. 모터의 PWM 범위를 서보모터의 작동 범위인 0~180도에 mapping 시켜서 드론 제어가 가능하다고 예전에 읽은 기억이 얼핏 나고, 또 서보모터의 경우 섬세한 움직임을 정밀할 때 사용하기 때문에 출력 단계를 훨씬 더 세분화시켜 주지 않을까 기대했습니다. 

그럼 이번엔 정말 문제가 해결되기 기대하며!!

우선 서보모터 라이브러리로 모터의 회전 속도가 되는지 확인해봅시다. 

서보모터 라이브러리 코드는 간단한데요. 기존에 아두이노에서 썼던 pinMode , digitalWrite 또는 analogWrite가 아닌 attach나 write 명령어를 쓰게 되는데요. 이러한 명령어들은 아두이노 홈페이지에서 서보 라이브러리를 검색하시면 확인하실 수 있어요. 굉장히 간단하니 이해하는데 어려움은 없으실 겁니다.

저희는 우선 PID 제어를 배제하고 모터의 출력을 변화시켜 보기로 했는데요. 

void serialEvent()

{

  while (Serial.available()) {

    sig_front = Serial.parseInt();

    sig_back = Serial.parseInt();

    sig_left = Serial.parseInt();

    sig_right = Serial.parseInt();

    r1=Serial.parseInt();

  }

}

위의 코드를 통해 시리얼 통신을 통해서 저희가 원하는 출력값을 입력하고

void loop() {

  // put your main code here, to run repeatedly:

  sig_front=constrain(sig_front,0,179);

  sig_back=constrain(sig_back,0,179);

  sig_left=constrain(sig_left,0,179);

  sig_right=constrain(sig_right,0,179);

  front.write(sig_front);

  back.write(sig_back);

  left.write(sig_left);

  right.write(sig_right);

}

루프에서 저희가 입력한 출력값을 기반으로 모터를 회전시키게 됩니다.

전체 코드 또한 아래에 첨부합니다!

#include <Servo.h>

Servo front, back, left, right;

int sig_front=0, sig_back=0, sig_left=0, sig_right=0,r1=0;

void setup() {

  // put your setup code here, to run once:

  Serial.begin(9600);

  front.attach(7,1000,2000);

  back.attach(8,1000,2000);

  left.attach(9,1000,2000);

  right.attach(4,1000,2000);

  front.write(0);

  back.write(0);

  left.write(0);

  right.write(0);

}

void loop() {

  // put your main code here, to run repeatedly:

  sig_front=constrain(sig_front,0,179);

  sig_back=constrain(sig_back,0,179);

  sig_left=constrain(sig_left,0,179);

  sig_right=constrain(sig_right,0,179);

  front.write(sig_front);

  back.write(sig_back);

  left.write(sig_left);

  right.write(sig_right);

}

void serialEvent()

{

  while (Serial.available()) {

    sig_front = Serial.parseInt();

    sig_back = Serial.parseInt();

    sig_left = Serial.parseInt();

    sig_right = Serial.parseInt();

    r1=Serial.parseInt();

  }

}

 

결과는 대성공!!! 

서보모터 라이브러리를 활용하니, 모터의 출력 정도를 10단계가 아닌 훨씬 더 많은 단계로 세분화시킬 수 있었는데요. 

이 영상은 서보모터 라이브러리를 활용하여 모터의 출력을 조절하는 영상인데요. 

이제 이 서보모터 라이브러리를 활용한 코드에다 PID 코드만 가져와서 합치면 PID 제어가 어느 정도 완성되지 않을까 싶습니다. (물론, 칼만 필터를 통해 센서값이 튀는걸 먼저 막아야겠지요!)

조만간 PID 제어에 성공했다는 기분 좋은 소식을 전달할 수 있기를 기대합니다! 좋은 하루 되세요~