Inverted Pendulum - 2.Stabilizing

Stabilizing

pole이 균형을 잃지않고 수직을 유지하는것을 stabilizing이라고 하고 이를 위해서는 pole의 움직임에 따라 모터에 적절한 전압을 가해 카트를 움직여줘야한다.

전에 구한 두 식을 다시 표현하면 다음과 같다.

식을 간단하 하기 위해 $\alpha$를 사용한다. $\alpha = (M+m)(I+ml^2)-m^2l^2$

상태공간 표현

위 방정식들은 선형이기 때문에 아래와 같이 나타낼 수 있다.

$\dot{X}=AX+BU$

$Y=CX+DU$ 와 같이 일반적인 선형 시스템의 동적 방정식을 구했다.

가제어성

동적 방정식으로 제어기를 설계하기 전에 가제어성을 먼저 확인한다. 시스템의 특정상 모든 상테를 제어하는 것이 불가능 할 수 있기 때문에 제어를 하기 전에 먼저 제어 간능한 시스템인지 확인할 필요가 있다. 유한한 시간동안 입력을 가하여 한 임의의 상태를 다른 임의의 상태로 바꿀 수 있다면 제어가능하다고 하며 시스템이 가제어성(controllability)을 갖는다고 말한다.

가제어성 행렬이 4X4 이므로 가제어성 행렬의 rank가 4이면 가제어성을 갖는다. 가제어성 행렬은 아래와 같으며 MATLAB을 사용하여 구한다. $M=[B \quad AB \quad A^2B \quad…\quad A^{n-1}B]$

다음과 같이 rank가 4인 것을 확인할 수 있고 이는 가제어성을 갖는 것을 보여준다.

상태방정식과 성능지표

일반적인 선형시스템 의 상태방정식은 다음과 같다.

이때 $U=-KX=-(k_{p1}x +k_{d1}\dot{x} +k_{p2}\phi +k_{d2}x\dot{\phi})$ 이며 여기서 $k_{p1}, k_{d1}$ 은 각각 카트의 위치와 속도에 대한 제어상수이며 $k_{p2}, k_{d2}$ 는 각각 진자의 각도와 각속도에 대한 제어상수이다.

성능지표 $J=\int\limits_{0}^{\infty}(X^TX)dt$ 이며 벡터 X의 제곱의 합으로 표현한 것이며 성능지표가 최소화 되어야 한다. $\frac{d}{dt}(X^TPX)=-X^TX$ 일반적인 시스템에서 위 수식을 만족하면 stable하다고 볼 수 있고 성립하는 행렬 P를 찾아야 한다.

이 식을 정리하면 $H^TP+PH=-I$ 가 되며 이를 만족하는 P와 H를 찾아야 한다.

앞에서 구했던 성능지표에 대입하여 풀면

아래 두 식을 만족하는 P와 H를 찾으면 제어 게인 K를 찾을 수 있다.

좀 더 나아가 일반적으로 성능 지표는 각 상태에 가중치를 줄 수 있고, 제어입력의 크기에 대해서도 가중치를 설정할 수 있도록 다음과 같이 사용한다.

$J= \int\limits_{0}^{\infty}(X^TQX+U^TRU)dt$

MATLAB으로 LQR게인 구하기

위에서 구한 상태방정식을 아래와 같은 표현으로 바꿔주기위해 A,B,C,D를 매트랩에서 표현한다.

$\dot{X}=AX+BU$

$Y=CX+DU$

위와 같이 매트렙에서 설정하고 lqr툴박스를 통해 게인값 k를 얻을 수 있다.

Q(1,1) 과 (3,3)은 각각 이동거리와 기울기에 대한 가중치이며 최적의 값을 찾기 위해 try and error방식으로 Q의 가중치 값을 정한다. 이때 R값은 클수록 시스템이 안정적이나 반응속도가 느려지는 점을 고려하여 R은 낮은 값으로 고정하고 Q의 값을 조절하였다.