PID 제어 시스템으로 두발 바퀴의 마인드스톰 직립 보행시키기
PID제어를 통해 2족 마인드스톰 로봇 직립보행시키기
(1)직립 보행 로봇에 대한 이해와 광센서를 이용한 바퀴 회전 제어
직립 보행 로봇은 사진 (1)-1 같이 인간처럼 서 있는 형태로 가운데에
달려 있는 광센서는 지면에 빛을 쏴서 반사되는 빛의 양을 측정한다.
광센서가 지면에서 떨어질수록 들어오는 빛의 양은 적어지며, 사진
(1)-2와 같이 앞으로 넘어지는 상태는 광센서에서 많은 양의 빛이 측정되고,
사진 (1)-3과 같이 뒤로 넘어지는 상태는 광센서에서 적은 양의 빛이 측정된다.
이때 넘어지는 방향으로 바퀴를 회전하게 되면 직립보행 로봇을 넘어지지 않게
할 수 있으므로, 광센서에서 측정되는 빛의 양에 따라서 PID제어를 통해 직립을
유지할 수 있도록 한다.
(1)-1 (1)-2 (1)-3
(2) PID 제어
위 식은 PID 제어식을 수학적으로 표시한 것으로 PID 제어를 위한 설계 변수는 위 식
에서 3개의 상수 K(p), K(i), K(d)이며 각각 비례 이득, 적분 이득, 미분 이득이라고
한다.
비례 상수는 오차에 대한 피드백을 받아 오차를 0이 되도록 제어하며,
적분 상수는 설정된 상태와 오차와의 차이를 천천히 감소되도록(제거되도록) 제어하며,
미분 상수는 급격한 변화를 완화시켜 오차를 최소화 할 수 있도록 제어해준다.
일반적으로 적분 이득 K(i)는 작은 값을 사용하여, 정상-상태 오차가 천천히 감소되도록
하며, K(p)를 크게 할수록 미분 이득 K(d)는 작게 해서 적당한 값을 찾게 한다.
적절한 PID 제어를 하기 위해서는 다음과 같은 순서와 방법으로 이득 값을 결정한다.
(ㄱ) K(p)값을 작은 값에 놓고, 바퀴 회전 값을 바꾸어가며 가장 만족스런 결과를 얻는다.
(ㄴ) K(p)값을 서서히 증가시키며 실험을 반복하며 적당한 값을 얻는다.
*너무 큰 K(p) 값은 overshoot 또는 진동을 발생시킨다.
(ㄷ) K(d)값을 증가시키거나 감소시켜가면서 적절한 K(d)값을 구한다.
*로봇 구동기가 포화되면 K(p)값을 줄이고, 출력 속도가 너무 느리면 K(d) 증가시킨다.
(ㄹ) K(i)즐 작게 설정하여 바꿔가며 실험을 반복한다.
(3) 보행 원리와 PID 제어를 통한 [[[ (1)과 (2)를 통한 ]]] 개발 내용
#include "NXCDefs.h"
int REF_ANGLE = 0; //기본 앵글을 0으로 설정 int P_Gain = 2500; // 비례 int I_Gain = 19; // 적분 int D_Gain = 20; // 미분 int ScaleFactor = 100; long Last_Err=0; long Integ_Err=0;
//PID값을 얻는 Fcn long GetPID(long Err) { long Diff_Err; long Gain; Diff_Err = Err - Last_Err; Integ_Err = Integ_Err + Err; Gain = P_Gain*Err + I_Gain*Integ_Err + D_Gain*Diff_Err; Last_Err = Err; return (Gain/ScaleFactor); }
void Move_Robot(long Vel) { if(Vel>100) //예외처리 { Vel=99; } if(Vel<-100) //예외처리 { Vel=-99; } if(Vel>0) { OnRev(OUT_AB,Vel); // 뒤로가는 함수 } else { OnFwd(OUT_AB,-1*Vel); // 앞으로가는 함수 } }
task main() { //앵글과 PID변수들 long Angle; long Err; long Gain; long Gaintwo = 0; int CalAngle = 0; int CheckOK = 0;
//초기 앵글자동설정을 위한 변수들 int loopCnt=0; int median =0; int loopVal=0;
//A,B 초기화 및 센서 설정 ResetRotationCount(OUT_B); ResetRotationCount(OUT_A); SetSensorLight(IN_2);
NumOut(0,LCD_LINE4,REF_ANGLE);
//자세제어를 위한 무한루프 while(1) { Angle = SENSOR_2; //센서값얻기
Err = REF_ANGLE – Angle; //얻어진 센서값과 기준앵글의 차이를 에러로 저장 Gain = GetPID(Err); //에어를 이용해 Gain을 얻는다 NumOut(0,LCD_LINE2,Gain); Move_Robot(Gain); //Gain값을 토대로 스탭모터를 제어한다
NumOut(0,LCD_LINE1,Angle); NumOut(0,LCD_LINE3,Err); } }
|
Ⅲ. 검증
강의실 앞 복도에서 약 30분간 동영상 촬영. 베터리 문제로 17분+12분 두 개의 동 영상 파일로 저장. 초기 중심만 잘 잡아준다면 흔들림이 크지 않고 30분간 거의 자신의 자리에서 크게 벗어나지 않음을 확인함.
--광센서와 PID 제어를 통한 마인드스톰 직립보행 시연 영상-----------------------
