Matlab을 사용한 도립 2륜 로봇 안정화

Self-Balancing Robot의 제어를 구현하기 위해 문헌 「도립 이륜 로봇(모터 1축)의 안정화와 주행 제어」을 Matlab에서 추적했습니다.

프로그램에 떨어뜨림

문헌내 「2. 도립 이륜 로봇의 개략과 모델링」과 같은 파라미터를 사용해, 「3. 도립 이륜 로봇의 안정화」에 있는 상태 방정식을 프로그램으로 합니다.

여기서 다음과 같이 2점의 수정을 실시했습니다.
1. 게인 a [Nm/A]에 대한 표기가 없었기 때문에 결과의 역산에서
a = 토크 상수 Kt × 기어비 i × 감속기 효율 75%
라고 하기로 했습니다.

2. 또한 상태 방정식의 정리 부분을 다음과 같이 수정합니다.

\begin{equation}
\frac{d}{dt}
\begin{bmatrix}
θ\\
φ\\
\dot{θ}\\
\dot{φ}
\end{bmatrix}
=
\begin{bmatrix}
0_{2×2} &I_{2×2} \\
-α^{-1}γ &-α^{-1}β\\
\end{bmatrix}
\begin{bmatrix}
θ\\
φ\\
\dot{θ}\\
\dot{φ}
\end{bmatrix}
+
\begin{bmatrix}
0_{2×2}\\
-α^{-1}δ
\end{bmatrix}
u
\end{equation}

여기서, 이 식으로 정리할 때 δu는 이항되지 않기 때문에 -α가 되지 않고,
또한, 0 매트릭스는 δ의 벡터에 맞출 필요가 있기 때문에, 2×2가 아니라,

\begin{equation}
\frac{d}{dt}
\begin{bmatrix}
θ\\
φ\\
\dot{θ}\\
\dot{φ}
\end{bmatrix}
=
\begin{bmatrix}
0_{2×2} &I_{2×2} \\
-α^{-1}γ &-α^{-1}β\\
\end{bmatrix}
\begin{bmatrix}
θ\\
φ\\
\dot{θ}\\
\dot{φ}
\end{bmatrix}
+
\begin{bmatrix}
0_{2×1}\\
α^{-1}δ
\end{bmatrix}
u
\end{equation}

됩니다.

프로그램

이것들을 Matlab 프로그램에 나타내면 다음과 같이 됩니다.
(이 프로그램을 실행하려면 ControlSystemToolbox가 필요합니다.)

balancing_robot.m

m=0.686 %kg
M=0.0605 %kg
Jp=0.00316 %kg m^2
Jt=  0.00000535 %kg m^2
Jm=0.00000013 %kg m^2
l=0.148 %m
r=0.02 %m
c=0.0001 %kgm^2/s
K = 0.00279 %Nm/A
i = 30
a = K * i * 0.75

g=9.81 %m/s^2

alpha11 = (M+m)*r^2 + m*l*r + Jt + i*Jm
alpha12 = (M+m)*r^2 + Jt + i^2*Jm
alpha21 = (M+m)*r^2 + 2*m*l*r + m*l^2 + Jp + Jt + Jm
alpha22 = (M+m)*r^2 + m*l*r + Jt + i*Jm
alpha=[alpha11,alpha12;alpha21,alpha22]

beta = [0,c;0,0]

gamma = [0,0;-m*g*l,0]

delta = [a;0]

I = eye(2)
O22 = zeros(2)
O21 = zeros(2,1)

A = [O22,I;-(alpha^-1)*gamma,-(alpha^-1)*beta]
B = [O21;(alpha^-1)*delta]

Q = [1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1]
R=500

[K,S,e] = lqr(A,B,Q,R)

실행 결과

위의 프로그램을 실행한 결과

K =

  -11.7357   -0.0447   -1.4987   -0.0638


S =

   1.0e+03 *

    3.9639    0.0335    0.5610    0.0415
    0.0335    0.0014    0.0051    0.0005
    0.5610    0.0051    0.0815    0.0063
    0.0415    0.0005    0.0063    0.0006


e =

  -19.3440
   -7.9842
   -4.4475
   -1.0114

되었습니다. 문헌 내에서
K= [-11.7400 -0.0447 -1.4994 -0.0638]
반대
K= [-11.7357 -0.0447 -1.4987 -0.0638]
와 오차가 있는 결과가 되었습니다.
아마 a나 g 등 명기되어 있지 않은 파라미터의 오차라고 생각할 수 있습니다.

제어 시뮬레이션

여기에서 얻어진 이득 K를 사용하여,

u(t)=-Kx(t)

가 되도록 실행하는 것으로, 로봇이 도립할 수 있을 것입니다.
이것을 simulink 모델로 기술하고 시뮬레이션을 실행합니다.
이전 씹는 5 ° 위치로 시작하여 초기 값을
x0 = [5*pi/180;0;0;0]
했다.

simulink 모델은 다음과 같습니다.


각각의 모델내는 아래의 그림과 같이 설정합니다. 시뮬레이션 시간은 6s입니다.




여기서 재생 버튼으로 실행하면 t, x, u가 변수로서 결과가 출력됩니다.
이것들을 그래프로 출력한 결과가 아래와 같습니다.

로봇의 자세 θ는 한 번 반대 방향으로 쓰러진 후 3s 정도로 직립되어 있습니다.

타이어의 각도는 6s 정도에 걸쳐 천천히 초기 위치로 돌아가고 있습니다.




더 빨리 수렴시키기 위해서는 문헌과 같이 가중치 행렬 Q를 변경함으로써 게인을 높여 응답성을 높일 수 있습니다.
다만, 필요한 u(t)가 커져 가기 때문에, 모터의 한계 출력을 넘지 않는 범위에서 생각할 필요가 있는 것 같습니다.

요약

이것으로 도립 2륜의 한가지의 계산 과정을 알 수 있었으므로, 그 중 실기로 시험해 보고 싶습니다.

참고

math works 선형 2차 레귤레이터(LQG) 설계