[드론] PID 제어 알고리즘 구현

PID 제어 알고리즘 구현

표준 PID 제어기 알고리즘 (출처 : Wikipedia)

위에 보이는 알고리즘은 표준 PID 알고리즘 입니다. 드론에서 이 알고리즘을 적용을 시키게 되면 드론이 뜨기는 뜨지만 안정적으로 비행을 하기 힘들며 조종하기도 힘듬니다. 그래서 드론에서는 이중 PID 제어 알고리즘을 사용하게 되는데 일단!! 이중 PID 제어 알고리즘을 알아보기 전 표준 PID 알고리즘을 토대로 프로그램도 작성을 해보고 플로워차트도 구현을 시켜보는게 좋을거 같아서 표준 PID 제어기 알고리즘에 대한 구현을 설명을 해드리도록 하겠습니다.

이중 PID 제어 알고리즘 (출처 : Wikipedia)

---

표준 PID 제어기 알고리즘 스케치 작성

1.이론적인 표준 PID 제어기 알고리즘

Error = Setpoint - Input

Error : 현재 오차

setpoint : 목표로 설정하는 값

Input : 현재 입력 값(제어하고자 하는 대상에 의해 정해지는 입력 값)

"Setpoint 변화는 갑작스런 Output 변화 유도"

.

.

.

.

비례항(P) PTerm = Kp x Error

Kp : 비례매개변수(게인)

"현재 제어값이 오차에 비례하여 제어량 변화"

.

.

.

.

적분항(I) ITerm += Ki x Error x dt

dt : 센서 입력 주기

Ki : 적분매개변수(게인)

"왜 반복적으로 더해주는가?"

-> 미세값을 계속 더하는 과정이 바로 적분이다

.

.

.

.

미분항을 구하기 위해서는 오차 변화율이 우선적으로 필요하다.

.

.

오차변화율 dError = Error - prevError

dError : 오차 변화율

Error : 현재 오차

prevError : 이전 오차

.

.

.

.

미분항(D) DTerm = Kd x (dError / dt)

Kd : 미분매개변수(게인)

dError : 오차 변화율

dt : 센서 입력 주기

.

.

.

OUTPUT = PTerm + ITerm + DTerm

---

위와 같이 하는건 지금까지 한 이론적인 알고리즘 구현 입니다.

근데 이제 수정을 해야할 부분이 있는데요 바로 미분항 부분 입니다.

그 이유는!

드론의 모터 속도를 갑자기 높이거나 낮춘다고 가정을 하고 위와 같은 미분항 루틴을 사용하게 된다면 드론을 불안정한 상태로 만들게 됩니다.

"그러면 해결책은 무엇일까요?"

해결책은 미분항과 오차변화율 루틴을 아래와 같이 변경 시켜주면 됩니다.

dInput = Input - prevInput

DTerm = -Kd x (dInput / dt)

---

최종 표준 PID 알고리즘

Error = SetPoint - Input PTerm = Kp x Error ITerm += Ki x Error x dt dInput = Input - prevInput DTerm = -Kd x (dInput / dt) Output = PTerm + ITerm + DTerm