드론 프로젝트에 맞춰 변형한 PID 제어기 구조와 제어기 출력값을 이용한 모터의 제어 방법

이번 포스팅은 드론의 자세 제어를 위해 사용했던 PID 제어기에 대해서 설명하도록 하겠습니다.

PID 제어기란?

PID 제어기란 비례-적분-미분 제어로서, 가장 많이 사용되는 대표적인 형태의 제어 기법입니다.

PID 제어기는 제어하고자 하는 대상의 출력값을 측정하여 이를 원하고자 하는 설정값과 비교하여 오차를 계산하고, 이 오차값을 제어에 필요한 제어값으로 변환하는 구조로 되어 있습니다.

< PID 제어의 순서 >

< PID 제어기의 구조(출처 : 위키백과) >

표준적인 형태의 PID 제어기는 위와 같이 세 개의 항을 더하여 제어값(Output)을 계산하도록 되어있습니다. 
P항, I항, D항은 각각 오차값, 오차값의 적분, 오차값의 미분에 비례하며, 각 항에 들어가는 이득값(Gain)인 Kp, Ki, Kd를 적적하게 바꾸어 가면서 최적의 output을 만들어낼 수 있습니다.

프로젝트에 맞춰 변형한 PID 제어기의 구조

< 프로젝트에 맞춘 PID 구조 >

위 이미지에 나와있는 sbus cmd라는 것은 sbus 프로토콜을 사용하는 조종기 리시버의 값을 하나의 명령으로 바꾼 것을 의미합니다. 

< SBUS 프로토콜 Futaba의 R7008SB 리시버 >

어찌 되었건,  리시버에 들어온 조종기 신호값을 하나의 command로 만듭니다.
command라는 말이 너무 추상적이라서 예를 들자면 이렇다고 할 수 있습니다.
"Roll 축을 기준으로 30도 회전하라", "앞으로 10도 기울어라" 등등 이런 식으로 명령을 만드는 것이죠.

이렇게 생성된 command와 현재 센서값의 차이를 계산하여 error라는 값을 만들어 냅니다.
다시 error라는 값의 예를 들자면
"드론이 Roll 축을 기준으로 15도 기울여져 있다!"라는 센서 값과 "Roll 축을 기준으로 30도 기울여라!"라는 sbus command의 차이로 "15도 기울여라"라는 error를 만들게 됩니다.

P항은 error의 크기에 비례한 제어작용을 하며, 
I항은 error를 적분해가기 때문에 원하는 제어량과 실제 제어량의 편차를 없앱니다. 
D항은 출력값의 급격한 변화를 방지하여 안정성을 향상시킵니다.

구현 코드

먼저 sbus 리시버 값을 command로 만들어주는 코드입니다.

< sbus 리시버 값을 command로 만들어주는 코드 >

위와 같은 코드를 작성한 이유는 sbus_data_buff의 0번, 1번, 3번 인덱스의 값들은 각각 조종기의 1채널, 2채널, 4채널에 해당되는 값들인데, 이는 각각 roll, pitch, yaw 제어에 사용됩니다.
sbus의 각각의 채널들은 조종기의 스틱을 최대로 내렸을 때 352, 조종기의 스틱을 최대로 올렸을 때 1696이 나옵니다.

roll, pitch에 대한 제어 command는 최대 +-30도, yaw에 대한 제어 command는 최대 +-150도가 되도록 하였습니다.

실제 PID 제어가 구현되는 코드는 다음과 같습니다.

< PID 제어가 들어가게 되는 코드 >

반복문을 통해 roll에 대한 PID 제어, pitch에 대한 PID 제어, yaw에 대한 PID 제어로 roll, pitch, yaw의 출력값을 계산하고, 3채널인 throttle(드론의 상승과 하강을 담당하는 채널) 값은 PID 제어없이 출력값으로 설정합니다(비교할 센서값이 없으므로).

그 다음 과정은 PID 제어기를 거쳐 나온 출력값을 실제 모터에 분배해주는 작업입니다.
이 과정을 보통 mixer라고 표현합니다.

< 드론의 모터 배열 >

드론의 모터의 위치 배열은 다음과 같이 되어 있습니다. 
위 이미지의 우측 상단부터 M1, 그리고 시계반대방향으로 순서대로 M2, M3, M4입니다.

드론의 올바른 자세 제어를 위해 PID 제어기를 거친 roll, pitch, yaw의 output과 throttle 값을 모터의 위치에 맞게 알맞게 분배하게 됩니다.

실제 모터에 분배해준 코드는 다음과 같습니다.

< PID output 값을 모터에 분배해주는 코드(mixer) >

motor_output의 0번, 1번, 2번, 3번 인덱스에 해당되는 값들은 각각 M1, M2, M3, M4에 해당되는 값들입니다.
pid_output의 0번, 1번, 2번, 3번 인덱스에 해당되는 값들은 각각 PID 제어기를 거친 roll output, pitch output, yaw output 그리고 사실상 제어기를 거치지 않은 throttle 값입니다.

모터마다 더해지고 감해지는 값들이 각각 다른데 그 이유는 다음과 같습니다.

드론의 roll, pitch, yaw 축은 아래 그림과 같습니다.

각 축들을 기준으로 

오른손 법칙과 같이 엄지손가락을 치켜드는 방향을 축의 화살표 방향이라고 생각하고 나머지 손가락들을 안쪽으로 감아쥐는 방향으로 드론이 회전하면 센서값이 양(+)이 되고 그에 대한 반대 방향으로 회전하면 음(-)이 됩니다. 
또한, 각 축의 PID output 값이 양(+)이면 드론은 그 축을 기준으로 엄지를 제외한 나머지 손가락을 감아쥐는 방향으로 회전해야 하고, 음(-)이면 그 축을 기준으로 그에 대한 반대 방향으로 회전해야 합니다.

각 축의 PID output의 부호에 따라 M1, M2, M3 ,M4의 동작들을 살펴보겠습니다.


먼저 roll 축에 대해 살펴보겠습니다.

Roll의 PID output에 대한 제어 방법

roll PID output이 양(+)이면 roll 축을 기준으로 나머지 손가락을 감아쥐는 방향으로 회전해야 하므로(우측으로 기울여져야 하므로) M2, M3의 출력값은 증가시키고, M1, M4의 출력값은 감소시켜야 합니다. 

roll PID output이 음(-)이면 양(+)일 때와 반대로 동작합니다.

Pitch의 PID output에 대한 제어 방법

pitch PID output이 양(+)이면 pitch 축을 기준으로 나머지 손가락을 감아쥐는 방향으로 회전해야 하므로(뒤쪽으로 기울여져야 하므로) M1, M2의 출력값은 증가시키고, M3, M4의 출력값은 감소시켜야 합니다.

pitch PID output이 음(-)이면 양(+)일 떄와 반대로 동작합니다.

Yaw의 PID output에 대한 제어 방법

yaw PID output이 양(+)이면 yaw 축을 기준으로 나머지 손가락을 감아쥐는 방향으로 회전해야 하므로(오른쪽으로 회전해야 하므로) M1, M3의 출력값은 증가시키고, M2, M4의 출력값은 감소시켜야 합니다.
yaw PID output이 음(-)이면 양(+)일 떄와 반대로 동작합니다.

이를 표로 정리하면 다음과 같습니다.

< PID 제어에 따른 모터 제어 표 >

이상 드론 프로젝트를 진행하면서 공부하고 응용했던 PID 제어기와 PID 제어기를 거친 output을 통한 모터 위치에 각기 다른 분배 방법에 대해서 살펴보았습니다.
혹시 이상한 점이나 조언하고 싶으신 점들은 댓글로 달아주시면 감사하겠습니다.