이전 글에서 밸런싱 로봇의 동역학 모델을 구축하고 모터 동역학 모델을 추가하여 시뮬레이터를 만들었습니다. 이제 시뮬레이터를 기반으로 LQR 제어기를 설계하려 합니다. LQR제어기는 선형 시스템에서 2차 성능 지수를 가지는 최적 제어기로 많은 장점들을 가지고 있습니다.
다음 문서는 밸런싱 로봇의 동역학 모델과 모터의 동역학 모델을 유도하고, 비선형 시스템을 선형으로 근사화 한 후, 시스템을 상태방정식으로 표현하는 과정입니다.
상기 문서대로 MATLAB에서 제어 시뮬레이션 할 수 있도록 프로그래밍 합니다. MATLAB에서 작성한 상기 블럭선도와 LQR 제어 게인을 계산하기 위한 코드를 추가합니다.
balancingRobot_LQR.mdl 파일에서는 시뮬레이터와 제어기를 합친 블록을 다음과 같이 모델링 합니다. 여기서 Balancing Robot Simulator 블럭은 이전 글에서 balancingRobot.mdl 파일의 블럭과 동일한 블럭입니다. 여기에 사용자 입력을 만들기 위한 Pulse Generator와 constant 블럭이 추가되었고, LQR 제어 게인 K를 곱하기 위한 Gain 블럭도 추가되었습니다. 그리고 전진과 회전 명령(u(1), u(2))를 좌우 모터에 전압으로 분배하기 위한 블럭들도 추가되었습니다.
balancingRobot_LQR_Gain.m 파일에서는 LQR 게인을 계산하기 위한 MATLAB 함수가 작성되어 있습니다.
function [ K ] = balancingRobot_LQR_Gain()
g=9.81;
m=1; %Mass of motor and wheel
M=5; %Mass of body
n=50; %Gear ratio
R=0.2; %Radius of wheel
L=1.0; %Height of Center of Mass
W=0.3; %Distance between two wheels
J_psy=0.1; %Inertia of body (pitch)
J_phi=0.1; %Inertia of body (yaw)
J_w=0.01; %Inertia of Wheel
J_m=0.0; %Inertia of motor axis
f_m=0.05; %Friction coefficient of wheel axis
% Motor constants
Rm = 5;
Kt = 0.5;
Kb = 0.5;
alpha=n*Kt/Rm;
beta=n*Kt*Kb/Rm + f_m;
a_11 = (2*m + M)*R^2 + 2*J_w + 2*n^2*J_m;
a_12 = M*L*R - 2*n^2*J_m;
a_21 = M*L*R - 2*n^2*J_m;
a_22 = M*L^2 + J_psy + 2*n^2*J_m;
a_33 = 0.5*m*W^2 + J_phi + W^2/(2*R^2)*(J_w + n^2*J_m);
c_22 = -M*g*L;
A = [a_11 a_12 0 ; a_21 a_22 0 ; 0 0 a_33];
C = [0 0 0 ; 0 c_22 0 ; 0 0 0];
B = [-2*beta 2*beta 0 ; 2*beta -2*beta 0 ; 0 0 -W^2/(2*R^2)*beta];
D = [alpha 0 ; -alpha 0 ; 0 W/(2*R)*alpha];
AA(1:3, 1:3) = zeros(3);
AA(1:3, 4:6) = eye(3);
AA(4:6, 1:3) = -inv(A)*C;
AA(4:6, 4:6) = inv(A)*B;
BB(1:3, 1:2) = zeros(3,2);
BB(4:6, 1:2) = inv(A)*D;
QQ=diag([10 50 1 5 10 1]);
RR=diag([1 1]);
K = LQR(AA, BB, QQ, RR, 0);
end상기 함수를 사용하는 방식은 다음과 같이 간단히 함수를 호출하여 결과를 K에 담는것으로 끝납니다.
>> K = balancingRobot_LQR_Gain()
K =
-3.1623 -101.5452 0.0000 -5.7826 -34.2867 -0.0000
0.0000 0.0000 1.0000 0.0000 0.0000 0.5267
>>
이제 게인 K를 구했으니, 시뮬레이션을 합니다. * balancingRobot_LQR.mdl 파일의 LQR Gain K 블럭에서 전역변수 K를 참조하기 때문에 시뮬레이션 전에 LQR 게인 K를 꼭 구해 둬야 합니다.
제어기가 잘 동작하는지 보기위해 입력 블럭(Pulse Generator)에서 바퀴를 5초 동안 pi 만큼 전진시켰다가 이후 5초 동안 제자리로 돌아오게 합니다. 다음 그래프를 보시면 theta가 지령치를 잘 추종하고 있습니다.
(노란색(theta) - 바퀴의 회전 각도, 보라색(psi) - 바디의 기울어진 각도, 하늘색(phi) - 로봇의 방향)
좀 더 실감나게 그려보도록 하겠습니다.