벨런싱 로봇이 서 있는 상태를 유지하기 위해서는 MPU6050 에서 측정되는 값이 어떤 목표치에 근접하도록 모터를 움직여 줘야 한다. 즉 다음 그림에서 목표 각도와 현재 각도의 차이인 error를 해소하기 위해서 보정 움직임이 필요한 것이다.
일반적으로 이런 목표하는 센서 값으로 움직임을 제어하기 위한 방법으로 PID 제어기를 사용한다. PID 제어기에 대한 내용은 이전 포스팅 "PID제어에 대하여" 및 "PID제어기 구현하기" 를 참고 바란다.
앞 포스팅에서 MPU6050에서 Yaw/Pitch/Roll의 각도를 가져오는 부분에 대해서 설명했는데, 이번 포스팅에서는 PID 제어기를 붙여보겠다. Balanced 상태를 Pitch 가 0인 경우로 생각하고 0을 맞추기 위해서 보정 움직임을 계산하는 코드를 설명한다.
PID 제어기는 PTerm, ITerm, DTerm 을 계산해야 한다. Error의 경우 목표 Pitch 값(=0)에서 현재 입력되는 센서의 값의 차이가 된다. 즉 다음처럼 구할 수 있다.
Error=SetAngle-CurAngle
PTerm은 다음과 같다.
PTerm=Kp * Error
ITerm은 다음과 같다
integSum=integSum+Error*dt;
ITerm=Ki * integSum
DTerm은 다음과 같다
DTerm=Kd * (Error - prevError)/dt
그리고 Output은 이 3가지 항목의 곱에 모터 움직임을 위한 상수값 K 를 곱한 것이다.
Output=K(PTerm + ITerm + DTerm)
다음은 PID 값을 출력하는 코드이다.
struct timeval tv, tv2; double Kp = 2.5; // 2.5 double Ki = 0.8; // 1.0 double Kd = 8.0; // 8.0 double K = 1; double targetAngle=0;//target angle(balanced angle) double pTerm,iTerm,dTerm; double deltaT,lastCalTime; double prevError=0; double integratedSum=0; unsigned long long getTimestamp() { gettimeofday(&tv, NULL); return (unsigned long long) tv.tv_sec * 1000000 + tv.tv_usec; } double pid(float current){ double error=targetAngle-current; pTerm=Kp*error; integratedSum=integratedSum+error*deltaT; iTerm=Ki*integratedSum; deltaT=(double)(getTimestamp()-lastCalTime)/1000000.0; printf("deltaT:%7.2f, lastCalTime:%7.2f ",deltaT,lastCalTime); lastCalTime=(double)getTimestamp(); printf("CurrentTime:%7.2f\n",lastCalTime); dTerm=Kd*(error-prevError)/deltaT; double output=K*(pTerm+iTerm+dTerm); printf("PID pTerm:%7.2f, ITerm:%7.2f, DTerm:%7.2f DeltaT:%7.2f, Output:%7.2f\n",pTerm,iTerm,dTerm,deltaT,output); prevError=error; return output; } |
