이중 PID제어

이전에 사용한 PID제어는 단순히 각도를 PID제어한 것이었다. 그 결과 자세를 안정화하는데 시간이 걸려서 자세제어가 잘 되지않았다. 그럼 어떻게해야할까 고민하며 구글링을 해보았는데, 원래 PID제어로는 자세 제어가 잘 안되고 이중 PID제어를 해야한다는 것이다. 이중 PID제어도 어떻게 하냐에 따라 다른 것같았다. PI-PI제어를 하기도 하던데 내가 찾은 것 중 가장 괜찮다고 생각한 것은 외부 루프로 각도를 P제어한 후, 그 결과를 Set point로 하여 내부 루프로 각속도를 PID제어를 하는 것이다. 단순히 각도만으로 제어하는 것보다 각도로 제어한 값을 각속도로 융합해서 제어하므로 효율이 더 좋을 거라 생각했다.

 

errorAngle = targetAngle - currentAngle; //각도 오차 계산
targetRate = kop * errorAngle; //외부 루프 각도 P제어, 내부 루프 각속도 제어의 target rate가 된다.
errorRate = targetRate - currentRate; //각속도 오차 계산

pControl = kp * errorRate; //내부 루프 각속도 P제어

integral = integral + errorRate * DT; //각속도 오차 누적
if(throttle < 2) //throttle 범위가 0~100이라 했을 때, 착륙했다면 적분(오차 누적값) 초기화
{
    integral = 0;
}
iControl += integral; //각속도 I제어

dControl = kd * ((currentRate - preRate) / DT); //각속도 D제어
preRate = currentRate; //D제어에서 이전 rate값이 필요하므로 저장

 

 

[참고 논문] https://www.eiric.or.kr/literature/ser_view.php?searchCate=literature&SnxGubun=INLE&mode=total&gu=INKO011A0&cmd=qryview&SnxIndxNum=223284&rownum=1&f1=MN&q1=%C0%CC%BB%F3%B7%CF&totalCnt=4