Balancing Robot 설계 - 동역학 모델 구축과 시뮬레이션

Balancing Robot은 Microprocessor Control, Software, Hardware를 잘 활용하여 하나의 완전한 시스템을 구축해 볼 수 있기 때문에 로봇 공학에서의 이론과 실제를 잘 이해하고 배울 수 있는 과제라고 생각되어 동료들과 같이 로봇의 동역학 모델링과 제어기 설계, 실제 제작까지 해보기로 하였습니다.

밸런싱 로봇의 모델링을 위해 참조한 사이트와 논문은 다음과 같습니다.

- KAIST 최동일 "2륜자기균형이동차의 개발 및 외란 측정기를 이용한 강인제어"

- Jonsson Per "Two wheeled balancing LEGO robot"

- http://pinkwink.kr/category/프로젝트/Robot

먼저 밸런싱 로봇의 동역학 모델링을 하였습니다. 다음 문서는 밸런싱 로봇의 동역학 모델을 유도하는 과정입니다.

Two Wheeled balancing Robot - Dynamics.pdf

0.18MB

다음으로 밸런싱 로봇의 동역학 모델로부터 MATLAB 상에서 시뮬레이터를 만들어 우리가 유도한 동역학 모델이 우리가 생각하는 방식대로 움직이는지 살펴볼 예정입니다. MATLAB 상에서 동역학 모델을 balancingRobot.mdl 파일에 작성하고 시뮬레이션 결과를 보기위한 함수(DrawSimResult.m)를 만듭니다.

balancingRobot.mdl 파일에는 다음 그림과 같이 외부 Torque를 입력받아 Y(theta, psy, phi)와 Y_dot(theta_dot, psy_dot, phi_dot)을 출력하는 Subsystem이 있습니다. 로봇에 외력으로 작용하는 Torque를 생성하기 위해서는 Pulse Generator를 사용하였습니다. 시뮬레이션 결과인 로봇의 모양은 Scope2로 출력이 됩니다. 여기서는 그래프로 바퀴의 회전각(theta), 바디의 기울기 각(psi), 로봇의 방향(phi)을 표시합니다.

Subsystem이 외부에서 인가되는Torque와 현재 상태 Y(U)와 Y_dot(U_dot)으로 새로운 U_2dot을 계산합니다. Dynamics 블록에는 밸런싱 로봇의 동역학 모델을 작성한 코드가 들어가 있습니다. U_2dot을 적분하여 U_dot을 만들고 U_dot을 적분하여 U를 만들어 이를 Dynamics에 피드백 하는 구조로 시뮬레이션을 하게 됩니다.

Dynamics 블록의 코드는 다음과 같습니다.

function U_2dot = Dynamics(U_dot, U, T)

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

cfc = 0.1;  % cfc - coulomb friction coefficient
vfc = 0.2;  % vfc - viscous friction coefficient

T = T - vfc*U_dot - cfc*sign(U_dot);

theta = U(1,1);
psy = U(2,1);
phi = U(3,1);

theta_dot = U_dot(1,1);
psy_dot = U_dot(2,1);
phi_dot = U_dot(3,1);

a_11 = (2*m + M)*R^2 + 2*J_w + 2*n^2*J_m;
a_12 = M*L*R*cos(psy) - 2*n^2*J_m;
a_21 = M*L*R*cos(psy) - 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) + M*L^2*(sin(psy))^2;

b_1 = -M*L*R*psy_dot^2*sin(psy);
b_2 = -M*g*L*sin(psy) - M*L^2*phi_dot^2*sin(psy)*cos(psy);
b_3 = 2*M*L^2*psy_dot*phi_dot*sin(psy)*cos(psy);

A=[a_11 a_12 0;a_21 a_22 0;0 0 a_33];
B=[b_1;b_2;b_3];

U_2dot = inv(A)*(T-B);

시뮬레이션 간격을 일정하게 하기 위해서 balancingRobot.mdl 파일을 연 윈도우에서 simulation - Configuration Parameters 메뉴를 실행한 후, Solver options 그룹에서 Type를 Fixed-step으로 수정하고 Fixed-step size를 0.01로 설정합니다. Solver는 적당히 선택하시면 됩니다.

시뮬레이션을 시작하면 10초간 0.01초 간격으로 시뮬레이션 되기때문에 1000개의 출력 데이터가 만들어집니다. 이를 Scope를 통해서 볼 수 있는데, theta, psi, phi를 그래프로만 보자니 제대로 감이 오지 않습니다. 그래서 MATLAB의 draw 기능을 이용하여 비주얼 하게 밸런싱 로봇의 거동을 보기위한 작업을 하였습니다.

먼저 Scope2의 데이터를 workspace에 저장하기 위한 설정을 합니다. Scope2의 그래프 윈도우에서 parameters아이콘(두 번째 아이콘)을 클릭 합니다. 아래와 같은 대화상자가 뜨는데, 여기서 Data History 탭슬 선택후 Save data to workspace를 체크한 후 Variable name으로 SimResult를 설정하고 Format은 Array로 설정합니다.