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

안녕하세요 

잘 지내셨나요들? 날씨가 쌀쌀해지는데 감기 조심하세요..(훌쩍)

지난번까지 아두이노로 각각의 모터에 다른 신호를 주는 과정을 마쳤었는데요. 이제 모터마다 다른 출력을 낼 수 있으니, 이를 기반으로 모터의 자세 제어를 시도해봐야겠죠? 

오늘은 이러한 모터의 자세 제어, 그 중 수평을 유지하는 PID 제어에 대해 얘기해볼까 합니다. 혹시 PID 제어가 뭔지 모르시는 분은 예전에 포스팅한 쿼드콥터 PID 제어 [가장 쉽게 설명하기]를 참고해주세요!

예전에 DC 모터의 위치를 제어할 때는 원하는 목표 위치와 현재의 위치의 차를 에러값으로 잡았었는데요. 에러값을 줄이기 위해 얼마나 출력을 조절하며 에러값이 0이 되었을 시에는 출력을 멈추게 했었죠. 

마찬가지로 이번에는 각도차이를 에러값으로 잡고 제어하기로 했습니다. 각도 차이가 생긴다는 것은 쿼드콥터가 한쪽으로 기울어졌다는 의미이기에 이를 수평으로 맞추기 위해서는 양쪽 모터의 출력값이 달라야 되겠죠? 

여기서 중요한 사실 하나! 두둥!! (;;;;)

실제로 쿼드콥터 자세제어 할 때는 단순히 각도차이를 에러 값으로 잡고 제어하지 않는다고 합니다! 여러분은 그렇게 하지 않으시길 바라며, 저희처럼 이렇게 하면 나쁜 결과가 나온다는걸 알려드리려고 합니다;;;

다시 본론으로 돌아가서요. 우선 DC모터의 위치 정보를 엔코더를 이용해 받는 것처럼 쿼드 콥터의 각도에 대한 정보가 있어야겠죠? 저희는 mpu6050이라는 센서를 사용했는데요. 아래 그림 속 아두이노 옆에 작게 붙어 있는 저 녀석이 mpu6050이라는 놈입니다. 

mpu6050은 가속도와 자이로 센서가 결합된 센서로 쿼드콥터 자세 제어 때 많이 쓰이고 roll과 pitch에 대한 정보를 얻을 수 있어요. 그럼 Yaw에 대한 정보는 어디서 얻냐고 궁금해하실 분들이 계실텐데요? Yaw에 대한 정보를 얻기 위해선 지자기 센서라는 다른 센서를 이용해야 합니다.

약간 옆 길로 새서 가속도 센서와 자이로 센서에 대해 설명을 드리자면요. 

가속도 센서

-중력가속도 크기를 X,Y,Z 방향에 대해 측정

-외력(이동)과 진동(변위가 작은 이동)에 취약함

-예) 자유낙하 (a=0) 센서가 아무리 기울어져 있어도 측정이 되지 않는다

-노이즈가 심하고 움직임의 값이 왜곡되지만 (부정확), 오차는 누적되지 않는다

자이로센서

-센서에 회전 발생시 X,Y,Z 축의 각속도 측정

-각도(위치)를 구하기 위해서 적분

-실제 값과 비슷하지만 노이즈도 같이 적분되기 때문에 누적 오차 발생 

이렇게 각각의 센서는 장점과 단점이 있는데요. 이 두 센서의 장점을 합치는 상보 필터 또는 칼만 필터를 통해 저희가 원하는 값을 얻을 수 있다고 하네요. 시간 단축을 위해 상보필터의 공식을 활용하여 코드를 공유해주신 다른 블로거님의 코드를 활용했습니다. (출처는 확인한 뒤에 밝히겠습니다!) 

void anglevalue() {

  

  Wire.beginTransmission(mpu_add) ; //get acc data

  Wire.write(0x3B) ;

  Wire.endTransmission(false) ;

  Wire.requestFrom(mpu_add, 6, true) ;

  ac_x = Wire.read() << 8 | Wire.read() ;

  ac_y = Wire.read() << 8 | Wire.read() ;

  ac_z = Wire.read() << 8 | Wire.read() ;

  Wire.beginTransmission(mpu_add) ; //get gyro data

  Wire.write(0x43) ;

  Wire.endTransmission(false) ;

  Wire.requestFrom(mpu_add, 6, true) ;

  gy_x = Wire.read() << 8 | Wire.read() ;

  gy_y = Wire.read() << 8 | Wire.read() ;

  gy_z = Wire.read() << 8 | Wire.read() ;

  deg_x = atan2(ac_x, ac_z) * 180 / PI ;  //rad to deg

  deg_y = atan2(ac_y, ac_z) * 180 / PI ;

  deg_z = atan2(ac_x, ac_y) * 180 / PI ;

  dgy_x = gy_y / 131. ;  //16-bit data to 250 deg/sec

  dgy_y = gy_z / 131. ;  //16-bit data to 250 deg/sec

  dgy_z = gy_x / 131. ;  //16-bit data to 250 deg/sec

  angle_x = (0.95 * (angle_x + (dgy_x * 0.001))) + (0.05 * deg_x) ; //complementary filter

  angle_y = (0.95 * (angle_y + (dgy_y * 0.001))) + (0.05 * deg_y) ;

  angle_z = (0.95 * (angle_z + (dgy_z * 0.001))) + (0.05 * deg_z) ;

}

이 중 저희가 관심있는 부분은 마지막 세 줄인 angle 값들인데요. 그 중에서 저희는 x축에 대해 먼저 실험했어요. 

이번 포스팅의 핵심인 PID 제어에 대한 코딩입니다. 이전 포스팅에서 말씀드렸듯이 PID 제어는 P(roportional) 제어, I(ntegration) 제어, D(ifferentiation) 제어로 구성되어 있는데요. 각각의 제어에 대한 식을 컴퓨터에서 활용하기 위해 discrete한 형태로 나타내면 아래의 그림 속 식처럼 나타낼 수 있어요. 그 중 맨 아래 적혀 있는 세 값의 합이 PID 값입니다. 

그런데 간단한 수식 전개를 통해 위 수식이 아닌 아래의 코드(void PID_setup())처럼 같이 표현할 수도 있어요. 저희는 이 표현이 더 편해서 이렇게 사용합니다. 

void PID_setup() {

G1 = Pgain + Igain*T + Dgain/T;

G2 = -(Pgain + 2*Dgain/T);

G3 = Dgain/T;

}

실제 PID 제어를 하는 함수인데요.

void PID_control() {

  

  error_x=0-angle_x;

  m_x = m_x1 + G1*error_x + G2*error_x1 + G3*error_x2;

  PID_value=constrain(abs(m_x), 4, 7);

  

  if (error_x>=0) {

    r2=PID_value;

  }

  if (error_x<0) {

    r4=PID_value;    

  }

  

  error_x2=error_x1;

  error_x1=error_x;

  m_x1=m_x;

}

아까 에러값은 목표치-현재값이라고 했듯이, 코드에서도 error_x 값이 저희의 목표치인 0도-angle_x라고 되어 있네요. 그렇게 나온 error_x 값에 대해 PID 제어를 하게 되고 이 값을 4와 7의 범위로 제한해주게 됩니다. 

그리고 그 아래 if 구문을 통해 error의 값이 양수냐 음수이냐에 따라 (기울어진 방향에 따라) 어떤 모터를 더 세게 출력해줄 것인지 제어합니다. 

드론을 기울임에 따라 기울어진 방향의 모터 출력이 강해짐을 볼 수 있네요..... 다만 출력 조절이 아직 제대로 되지 않네요.

조금 나아지긴 했지만 여전히 너무 확확확 바뀌네요. 드론 잡고 있던 친구가 놓칠까봐 어찌나 무섭던지..... 

글을 쓰다보니 시간이 너무 늦어져서 이 문제를 극복하는 과정은 다음 포스팅에서 알려드릴게요! 

모두들 좋은 밤 되세요~