[아두이노 쿼드콥터] 서보모터 라이브러리를 활용한 PID 제어

Posted by Doony
2015. 10. 22. 14:50 아두이노 드론 프로젝트

이전에 mpu6050 센서에서 각도가 튀는 걸 확인할 수 있었는데요. 그렇기에 [아두이노 쿼드콥터] mpu6050 센서값 보정하기에서 상보필터를 한 센서 값을 한번 더 보정할지 고민을 했었지요. 

하지만 그 전에, 혹시나 몰라서 필터 방법을 바꿔보기로 했습니다. 칼만필터로요 (칼만필터 출처 : Kristian Lauszus, TKJ Electronics). 그랬더니, 웬걸!!! 좌측에 나오는 값이 각도 값인데요. 이 값이 상당히 깔끔하게 나오더라구요.


한참을 기다려도 값이 튀지 않더라구요. 필터 알고리즘에 문제가 있었나 봅니다ㅠㅠ 추가 보정은 하지 않아도 되겠네요.


어쨋든 이렇게 센서 값이 튀는 것도 해결 됐으니 다시 PID 제어를 시작해야겠죠? 

저번에 모터 출력을 세분화 시키기 위해 사용했던 서보모터 라이브러리에 PID 제어 코드를 합쳐봤습니다. 이전에 PID 제어시 사용하던 필터 변수만 바꿔주는 것이기에 엄청 간단하겠죠?



오!! 동영상은 조금 어색해 보이지만 체감상 확실히 좋아진 것 같더라구요. PID 게인 값만 맞추면 잘 될 것 같다는 느낌이 드는데요.

자, 확실한 실험을 위해 손이 아닌 줄로 드론을 고정을 하구요. 

참... 허접합니다....

본격적으로 PID 게인 값을 맞추기 전에 [아두이노 쿼드콥터] 이중 PID 제어에 올렸던 코드를 보시면, 

 

error_x = desired_angle - angle_y; //angle def

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


 // angle rate gy_x;

 error_pid_x = P_angle_pid - rate_x / 100;


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


 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_x)으로 기울어지면 기울여진 모터에 PID 값으로 출력해라'라는 명령을 내렸는데요. 잘 생각해보면 저희가 이중 PID 제어에서 목표로 했던 값은 각속도 값이기에 error_x가 아닌 error_pid_x를 기준으로 if 구문을 써야합니다. 이런 실수를 하다니!


이후 게인 값을 여러번 수정한 다음 실험해봤습니다. 



아직 수평을 이루지 못하고 좌우로 흔들리긴 하지만, 예전처럼 확확 기울거나 그러진 않네요. 아무런 지지대가 없이도 확 기울어지지 않는걸 봐서 게인 값을 조금 더 맞추면 안정적인 중심잡기가 가능하지 않을까 싶어요. 기대가 됩니다!!


다음 번엔 조금 더 안정적인 중심 잡는 모습을 보여드리길 기대하면서 오늘 포스팅은 여기서 마칠게요! 

 





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

Posted by Doony
2015. 10. 19. 13:11 아두이노 드론 프로젝트

예전에 기울어진 각도에 따라 출력을 달리하는 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 제어에 성공했다는 기분 좋은 소식을 전달할 수 있기를 기대합니다! 좋은 하루 되세요~



[아두이노 쿼드콥터] mpu6050 센서값 보정하기

Posted by Doony
2015. 10. 14. 21:22 아두이노 드론 프로젝트

아두이노로 쿼드콥터를 제어하는 데 있어서 x, y, z축 각도값을 센싱하는 것은 필수적입니다.

저희는 가장 흔한 센서인 mpu6050을 사용하였는데요.


여기서 문제가 발생했습니다. 저희가 만든 알고리즘은, pid 제어를 시작하는 변수를 따로 두고 시작하기 전에는 각도값만 출력하도록 하고 있었습니다.

pid 제어를 시작하기 전에 시리얼 통신을 통해 출력되는 값은 아주 정상적이었습니다. 오차라고 해봐야 0.1도 정도 왔다갔다하는 정도???


그런데 pid제어를 시작하고 나면, 0.1도 정도 오차만을 가지던 값이 10배 정도로 오차가 커지더군요. 

그래서 엑셀로 그 데이터를 한번 뽑아봤습니다.



다 보이진 않지만, 정수는 시리얼 통신으로 얻어온 데이터 개수를 의미하고, 그 옆 소수는 왼쪽부터 각도값, 각속도값입니다.

각속도는 이전 포스팅을 보시면 아시겠지만, 루프를 돌기전 각도와 현재 각도 값 차이를 일정한 시간 간격으로 나누어준 값입니다. (대체로 루프 도는 시간이 0.0065~0.007 s 정도 되는 것을 확인했습니다.)


각도 오차가 0.1도 날때는 각속도 오차도 그만큼 작았지만, 각도 오차가 10배로 증가하니 각속도 값이 요동치는 것을 수치를 통해 확인하실 수 있는데요.


저희 생각에는...

애초에 mpu6050을 통해 각도값 만들어낼때, 상보필터니 칼만필터니 하는 걸로 각도 오차를 최소화 시키는 것 같은데..

왜 이런 오차가 또 발생하는걸까요? 이해가 안갔습니다.


아두이노가 불량인건지, 센서가 불량인건지.. 온갖 추측들을 실험을 통해 검증해보았지만.. 전부 문제가 없는 것 같더라구요. 즉, 알고리즘 상에서 pid 제어를 시작하게 되면서 뭔가 꼬이는 듯 했습니다.



왜 꼬이는지 이유를 아직 판단하지 못해서 저희 나름대로 대안을 세워봤습니다.

상보필터나 칼만필터를 통해 얻어진 각도값을.. 한번더 필터링해서 오차를 최소화해보자는 것입니다.


그래서 생각한 알고리즘은, 일정한 개수만큼의 데이터의 평균값을 사용하는 것입니다.

즉, 지난 9개의 각도값과 현재 계산된 각도값 1개를 더한 뒤 10으로 나눠주는 식인거죠.


지난 값들이 있기 때문에, 바로 다음 값에서 오차가 좀 심하게 나더라도, 평균화하는 과정에서 그 비율이 최소화되지 않을까 했습니다.


그래서 MATLAB으로, 위 엑셀 데이터를 불러들여 코딩을 해보았습니다.






angle_sum=0.0;             %% 일정 개수만큼의 각도값의 합

angle_average(1:424)=0.0;          %% 평균값은 행렬 형태로 정의

rate(1:424)=0.0;            %%각속도도 마찬가지로 행렬로 정의

a=30;                    %%a는 위에서 언급한 '일정 개수'. 즉, a=30이면 30개의 평균치를 사용

for i=1:424

    if i<=a            %%초기값, 즉 30개가 되기 전에는 

        angle_sum=angle(i)+angle_sum;    %%하나씩 더해가면서 평균을 구함

        angle_average(i)=angle_sum/i;

        if i>=2        %% 두번째 이상의 루프에서는, 속도를 구할 수 있음.

            rate(i)=(angle_average(i)-angle_average(i-1))/0.65;

        end

    else

        angle_sum=angle_sum-angle(i-a);    %%30번 이상의 루프에서.

        angle_sum=angle_sum+angle(i);

        angle_average(i)=angle_sum/a;

        rate(i)=(angle_average(i)-angle_average(i-1))/0.65;

        

    end

    x(i)=i;

end

plot(x, angle_average, x, angle, 'r', x, rate, 'g');

legend('original','adjusted');

ylabel('angle value');

xlabel('numbers');






그래서 plot된 그래프를 비교해보았습니다.

빨간색이 원래 데이터, 파란색이 평균치를 사용한 데이터입니다.




1. a=5


5개씩 평균치를 통해 출력한 각도 값입니다.

보시다시피, 기존 빨간 그래프와 큰 차이가 없습니다. 시리얼 baud rate으로 저희가 230400을 사용하는데, 무지 빠른 속도로 출력되는 값을 생각해보면 사실 당연한 결과입니다. 5개가 출력되는데 걸리는 시간은 매우 짧죠. 즉 그만큼 평균치도 큰 의미가 없다고 볼 수 있습니다.




2. a=10


이번엔 10개를 평균으로 잡아보았습니다. 위와는 확실히 다르죠? 오차가 좀 더 보정된 것을 보실 수 있습니다.

그런데 y축을 보시면, 역시 각도 값이 0.5도는 튀는 것을 보실 수 있죠.. 

(참고로 가운데 이상하게 튀는 구간은, 센싱 문제가 아니라 저희가 손으로 살짝 건드린 부분 같습니다.)





3. a=20


이번엔 20개를 평균치로 구해봤습니다. 오차가 확실히 줄어들었죠. 심지어 저희가 손으로 친 부분도 크게 줄었는데요.

이 이상 넘어가면, 오차뿐만 아니라 실제 변화량까지 줄여버리는게 아닐까 걱정이 슬슬 되네요...




4. a=40


심심해서 40개도 평균치를 구해봤습니다. 확실히.. 줄었죠.





위와 같은 알고리즘을 통해 오차를 한번 더 필터링 할 수 있을 것으로 기대됩니다. 그러나 실제 적용했을 때, 적당한 개수를 잘 구하는 게 관건이 될 듯 하네요. 

너무 작게 잡으면 오차 보정 효과가 미미할 것이고, 너무 크게 잡으면 실제 각도 변화까지 오차로 인식해버릴 우려가 있습니다.

실험을 통해 검증해봐야겠죠?
사실 결론적으로는, 이 방법을 사용하지 않아도 될지도 모릅니다. 센싱이 갑자기 튀는 이유만 밝혀낼 수 있다면요!


오늘 포스팅은 여기까지입니다!! 학기가 시작하고 나니, 일 진행이 좀 더딘 감이 좀 있네요. 하지만 금방 해낼 수 있으리라 믿습니다!!!!!!!!