이전 포스팅(PID 제어에 대해서..)에서는 PID 제어의 의미에 대해서 이야기 했었다. PID 제어기에 대한 수식은 다음처럼 구현 가능하다.
그런데 드론을 조정하기 위해서 SetPoint 를 조정하게 되는 상황에서 어떤 목표치로 SetPoint를 조정하게 되면 그 순간의 Error의 변동은 커지게 되고(dError값) 이에 따라 D 값이 커지게 되어, Output 에 갑작스런 변화를 일으키게 된다.
따라서 SetPoint가 변하는 상황에서는 미분항에서 SetPoint로 인한 영향을 없애야 하는데, 다음과 같이 dError 를 이용해서 D를 계산한다.
이를 기반으로 calStandardPID 함수를 다음과 같이 구현할 수 있다.
void calStandardPID ( float& setpoint, float& input, float& prev_input, float& kp, float& ki, float& kd, float& i, float& output ) { float error; float dInput; float p, d; error = setpoint - input; dInput = input - prev_input; prev_input = input; p = kp * error; i += ki * error * dt; d = -kd * dInput / dt; output = p + i + d; } |
이 함수를 이전 포스팅했던 "MPU6050 상보필터 적용하기"에서 MPU6050에서 측정되는 Yaw, Roll, Pitch 값을 넣어주면 된다.
이 Output을 모터에 적용하기 위해서는 원하는 방향과 모터의 동작에 대해서 알아야 할 필요가 있다. 즉 Yaw 을 특정 각도로 움직이려면 어떤 모터들의 강약을 조절해야 하는지에 대해서 알아야 한다. 다음의 그림은 쿼드콥터의 Yaw, Roll, Pitch의 방향에 대한 것이다.
모터는 다음 그림처럼 회전한다.
(https://quadcopterproject.wordpress.com/quadcopters/)
위 그림에서 MPU-6050의 X,Y,Z 의 방향을 다음처럼 정렬한다고 하자.
위 그림을 가정하여 Yaw 방향으로 회전하고자 할때 모터는 아래에서 왼쪽으로 돌러면 4,2를 강하게 1,3을 약하게 회전시켜야 하며 오른쪽으로 돌리려면 그 반대가 된다. 실제 이론적인 부분은 회전에 따른 토크 등을 기술한 수식을 통해서 알 수 있다. (http://www.slideshare.net/hichamberkouk/design-and-implementation-of-a-quadrotor-helicopter 참고)
Roll은 1,2번 모터와 4,3번 모터의 강약을 조정하면 된다. Pitch는 1,4번 모터 및 2,3번 모터의 강약을 쌍으로 조정하면 된다.
즉 Yaw 출력은 2,4번 모터에 정비례관계, 1,3번 모터에 반비례 관계가 되며
Roll 출력은 3,4 번 모터에 정비례관계, 1,2번 모터에 반비례관계가 되며
Pitch 출력은 1,4번 모터에 정비례관계, 2,3번 모터에 반비례관계가 된다.
이에 따라서 모터의 출력은 다음과 같은 수식이 나온다.
