회전형 역진자 시스템의 동역학 Rotary Inverted Pendulum

역진자 시스템(Inverted Pendulum system)은 제어이론을 공부하는 학생들에게는 거의 필수적인 코스입니다. 그 중에서 오늘 이야기할 회전형 역진자 시스템은 동일한 형태의 카트형 시스템에 비해 그 구현이 간편하기 때문인데요. 문제는 뉴턴역학으로 동역학을 유도하는 경우 카트형 보다는 약간 초반 접근이 어려울 수도 있습니다. 이미 저의 블로그에서는 예전에 회전형 역진자 시스템의 "동역학 유도 -> 선형화 -> LQR 제어기 설계 -> 실제제작"이라는 일련의 과정을 한번 이야기했었습니다.[관련글] 그러나 당시에는 뉴턴역학을 이용했었고, 또 그 후 약간의 유도과정에서 오류가 있다는 것을 발견했습니다. 그래서 최근 제 블로그에서 유도되는 동역학들이 대부분 라그랑지 방법을 이용하고 있는데요. 역시 회전형 역진자 시스템의 동역학 역시 라그랑지 방정식으로 한번 유도해 보겠습니다.

먼저 암이 회전하는 각도는 alpha로, 폴이 넘어지는 각도는 theta로 두었습니다. 암과 폴이 만나는 곳의 위치벡터를 r_alpha로 두고, 폴의 무게중심의 위치벡터를 r_p라고 두었습니다. 사실 위 그림을 보시면 바로 원통좌표계를 이용하거나 혹은 회전행렬(rotation matrix)을 이용해면 상당히 편해집니다만... 기구학을 배우지 않는 제가 아직 회전행렬에 대해 얼마전에 가진 의문이 해소되지 않아서 직각좌표계에서 바로 구현하겠습니다.^^

x-y평면에서 다시 바라본 그림입니다. r_alpha는 변할 것이 없고, 폴의 무게중심을 가르키는 r_p벡터가 x-y평면에 정사영된 벡터를 t_p 벡터라고 하겠습니다. 이때, 벡터의 기본 개념상 위 그림에서 t_p벡터는 실선이든 점선이든 같은 벡터이므로 점선을 된 것을 구하도록 하겠습니다.

r_alpha 벡터는 원위의 한 점의 좌표를 읽는 것으로 상당히 쉽게 구할 수 있습니다.

먼저 첫 그림에서 r_p벡터가 정사영되었다고 보고, 또 이때 theta의 방향을 고려하면 cos(pi/2-theta)는 이해가 되실 겁니다. 그리고, 역시 alpha의 방향때문에 pi/2-alpha라고cos, sin안의 항을 잡았습니다. 이제 r_p는

r_alpha벡터에서 t_p벡터를 거쳐서 높이만큼 올라가주면 되겠습니다.

위 식이 r_p벡터입니다.

이제 라그랑지 방법을 사용하기 위해 운동에너지와 위치에너지를 구해야하니까.. 속도벡터를 구해야합니다. 여기서 시간에 따른 변수는 alpha와 theta입니다.

그리고, 속도벡터를 이용해서 운동에너지를 구해보면

위 식이 되겠습니다. 정리하면

이 됩니다. 암의 위치에너지는 고려할 필요가 없으니 폴만 고려해서 위치에너지를 구하면

이 됩니다.

위 식이 그 유명한 라그랑지 방정식이죠...^^ 저 식에 대입하기 위해 L은 운동에너지에서 위치에너지를 뺀것이구요. 그래서 정리한 결과가

입니다. 비선형 동역학은 저 상태에서 바로 만들면 되구요. 선형화된 동역학 모델을 얻을려면 위 식에서 바로 선형화를 시도해도 됩니다. 

일단 그 전에 F_alpha와 F_theta를 완성해야겠네요. 여기서는 DC모터를 사용한다고 가정하구요. DC모터의 일반적인 동역학은 [관련글]에서 다룬대로

로 두겠습니다. 그 후 전류를 

로 둘 수 있으니 다시 정리하면

이렇게 되겠네요. 그러나 F_theta의 경우는 구동하는 모터같은 것은 연결되어 있지 않으니

마찰만 생각해주면 되겠습니다.

아까 구한 비선형 라그랑지 방정식에서 선형화를 시키고난 후 F_theta와 F_alpha를 대입하고 정리한 것입니다. 

치환된 식은 위에 있구요. 이제 이를 다시 정리하면

과

이 됩니다.

이때, 상태를

로 두고

의 형태를 가지는 상태방정식을 만들면

이 됩니다. 상태방정식에서 v는 모터에 인가하는 전압입니다.