소형 무인 비행체 2 - 과제

 

 

 

시뮬링크 애니메이션

 비행 동역학 제어기 학습에서 비행체의 운동을 시각화 할수 있어야 합니다. 이번 장에서는 매트립과 시뮬링크에서 애니메이션을 만드는 방법을 살펴보겠습니다

 

C.1 매트랩에서 그래픽 핸들

 매트랩에서 plot 같은 그래픽 함수를 사용할떄 이 함수들은 plot에 대한 핸들 hanlde을 반환합니다. 그래픽 핸들은 C/C++에서 포인터와 비슷한 개념인데, plot의 설정에 접근할수 있게 만들어 줍니다.

 

 예를 들어 매트랩 입력을 준다면

 

>> plot_handle=plot(t,sin(t))

 

 이는 sin(t)의 plot에 대한 포인터(핸들) 반환합니다. 플롯의 설정값들은 plot 명령에다가 추가로 적기 보다는 handle을 이용해서 보통 바꾸게 됩니다. 예를들면

 

>> set((plot_handle, 'YData', cos(t))

 

 는 cos(t)에 대한 플롯을 축이나, 타이틀, 라밸 등 다른 요소들을 다시 작성하지 않고, 해당 plot을 변경시킵니다. 만약 플롯이 여러 물체의 그래프를 포함하고 있다면, 핸들을 이용해서 각 오브젝트에 접근할수 있습니다. 예를들면

 

>>  plot_handle1 = plot(t,sin(t))

>> hold_on

>> plot_handle2 = plot(t,cos(t))

 

 sin(t), cos(t)를 같은 플롯에다가 그리고 각각의 오브젝트에 대한 핸들을 구하였습니다. 이 오브젝트들은 핸들을 이용해서 다시 그릴 필요 없이 별개로 변경 시킬수 있습니다. 예를들어 cos(t)를 cos(2t)로 바꿀경우 명령은

 

>> set(plot_handle2, 'YData', cos(2*t) )

 

 앞으로 우리는 이것을 이용해서 시뮬링크 시뮬레이션을 시간 변화에 따른 애니메이션을 그릴건데, C.2에서 2차원 역 도립진자를 C.3에서 3차원 우주선 애니메이션을, C.4에선 우주선 애니메이션을 점과 면 데이터로 생성해보겠습니다.

 

그림 C.1 역도립진자 그림.

 

C.2 역도립진자 애니메이션 예제

 그림 C.1의 역 도립진자를 보면, 설정 값으로 카트의 위치 y, 막대 각 $\theta$가 있고, 물리적 파라미터로 막대 길이 L, 몸체 폭 w, 몸체 높이 h, 그리고 지상과 몸체 사이의 갭인 g로 이루어집니다. 첫번째로 해야 할 일은 이 애니메이션을 그리는 점들의 위치를 결정해야 하는데, 역 도립진자의 경우 베이스 몸체의 4개의 점

 

 

 과 막대의 두 점을 아래와 같이 정리할 수 있습니다.

 

 

 베이스와 막대는 독립적으로 움직이므로, 각각을 피겨 핸들로 다루겠습니다. 우선 drawBase 명령을 아래의 매트랩 코드로 구현하겠습니다.

 

function handle = drawBase(y, width, height, gap, handle, mode)
    X = [y-width/2, y+width/2, y+width/2, y-width/2];
    Y = [gap, gap, gap+height, gap+height];
    if isempty(handle),
       	handle = fill(X, Y, 'm', 'EraseMode', mode);
    else
        set(handle, 'XData', X, 'Ydata', Y);
    end

 

 1. 핸들을 입력과 출력으로 설정

 2. 베이스의 코너 점들의 X, Y 위치를 정의

 3. 핸들이 존재하지 않으면 fill명령어로 그림

 4. 핸들이 존재하면 set으로 다시 설정

 

막대를 그리는 매트랩 코드는 비슷하게 다음과 같이 그립니다.

 

function handle = drawRod(y, theta, L, gap, height, handle, mode)
    X = [y, y+L*sin(theta)];
    Y = [gap+height, gap+height + L*cos(theta)];
    if isempty(handle),
       	handle = fill(X, Y, 'g', 'EraseMode', mode);
    else
        set(handle, 'XData', X, 'Ydata', Y);
    end

 

 파라미터 mode는 매트랩에서 EraseMode를 명시해주는데 사용하는데, EraseMode는 normal, none, xor, background 등을 설정할 수 있습니다. 이 모드들의 차이는 매트랩 핼프데스크에서 Image Properties를 보시면 찾을수 있습니다.

 

 팬듈럼 애니메이션의 메인 루틴은 다음과 같습니다.

 

 

function handle = drawPendulum(u)
    % process inputs to function
    y		= u(1);
    theta	= u(2);
    t		= u(3);
    
    % drawing parammeters
    L = 1;
    gap = 0.01;
    width = 1.0;
    height = 0.1;;
    
    %define persistent variables
    persistent base_handle
    persistent rod_handle
    
    % first time function is called, initialize plot
    % and peersistent vars
    if t==0,
        figure(1), clf
        track_width=3;
        plot([-track_width,track_width], [0,0], 'k');
        hold on
        base_handle = drawBase(y, width, height, gap, [], 'normal');
        rod_handle = drawRod(y, theta, L, gap, height, [], 'normal');
        axis([-track_width, track_width, -L, 2*track_width-L]);
    % at every other time step, redraw base and rod
    else
        drawBase(y, width, height, gap, base_handle);
        drawRod(y, theta, L, gap, height, rod_handle);
    end
    

 이 drawPendulum 루틴을 그림 C.2처럼 시뮬링크 파일에서 호출하겠습니다. 여기서 3개의 입력 값으로 위치 y, 각 $\theta$, 시간 t를 사용하겠습니다.

 위 코드를 정리하면

 1. 입력 u를 y, $\theta$, t로 이름 재설정

 2. 그림 파라미터 설정

 3. 그래픽 핸들 유지(persistent)

 4. 처음 호출되면 애니메이션 초기화

 5. 이후 입력에 대해 다시 그리기

 

그림 C.2 시뮬링크 파일. 슬라이더 게인 사용

 

 

C.3 애니메이션 예제: 선을 이용한 우주선

 이전 장에서는 단순한 2차원 애니메이션을 다뤘다면 이번에는 6자유도를 가지는 3차원 우주선에 대해서 살펴보겠습니다. 그림 C.3은 선을 이용해서 그린 간단한 우주선으로 바닥은 태양을 향하는 태양광 패널이 됩니다. 

 

그림 C.3 우주선 애니메이션

 

 첫번째로 할 일은 우주선의 각 점들을 라벨링하고, 동체 고정 좌표계 상에서 각점들의 좌표를 정해야 합니다. 표준 항공역학 축을 따라 X 축은 우주선의 정면, Y축은 우주선의 오른쪽, Z축은 바닥을 향하게 됩니다. 그림 C.3에서 점 1~12가 라밸 되어있고, 각각의 좌표들이 명시되어있습니다. 선을 만들기 위해서 이 점들을 연결시키면 되는데, 연속적인 선들로 만들기 위해 다음 순서대로 노드들을 평행이동 시키겠습니다. 1-2-3-4-1-5-6-2-6-7-3-7-8-4-8-5-1-9-10-2-10-11-3-11-12-4-12-9. 우주선의 지역 좌표계를 매트랩으로 구현하면 다음과 같습니다.

 

function XYZ=spacecraftPoints
 % define points on the spacecraft in local NED coordinates
 XYZ = [...
 1 1 0;... % point 1
 1 −1 0;... % point 2
 −1 −1 0;... % point 3
 −1 1 0;... % point 4
 1 1 0;... % point 1
 1 1 −2;... % point 5
 1 −1 −2;... % point 6
 1 −1 0;... % point 2
 1 −1 −2;... % point 6
 −1 −1 −2;... % point 7
 −1 −1 0;... % point 3
 −1 −1 −2;... % point 7
 −1 1 −2;... % point 8
 −1 1 0;... % point 4
 −1 1 −2;... % point 8
 1 1 −2;... % point 5
 1 1 0;... % point 1
 1.5 1.5 0;... % point 9
 1.5 −1.5 0;... % point 10
 1 −1 0;... % point 2
 1.5 −1.5 0;... % point 10
 −1.5 −1.5 0;... % point 11
 −1 −1 0;... % point 3
 −1.5 −1.5 0;... % point 11
 −1.5 1.5 0;... % point 12
 −1 1 0;... % point 4
 −1.5 1.5 0;... % point 12
 1.5 1.5 0;... % point 9
]';

 

 우주선의 상태들로 오일러각 $\phi$, $\theta$, $\psi$가 있으며 이는 롤, 피치, 요 각도를 의미하고, $p_n$, $p_d$, $p_d$는 북, 동, 아래 방향을 의미합니다. 우주선의 점들은 다음의 매트랩 코드를 이용하여 회전과 평행이동을 하게 됩니다.

 

function XYZ=rotate(XYZ,phi,theta,psi)
 % define rotation matrix
 R_roll = [...
 1, 0, 0;...
 0, cos(phi), −sin(phi);...
 0, sin(phi), cos(phi)];
 
 R_pitch = [...
 cos(theta), 0, sin(theta);...
 0, 1, 0;...
 −sin(theta), 0, cos(theta)];
 
 R_yaw = [...
 cos(psi), −sin(psi), 0;...
 sin(psi), cos(psi), 0;...
 0, 0, 1];
 
 R = R_roll*R_pitch*R_yaw;
 % rotate vertices
 XYZ = R*XYZ;

function XYZ = translate(XYZ,pn,pe,pd)
 XYZ = XYZ + repmat([pn;pe;pd],1,size(XYZ,2));

 

 원하는 자세로 우주선을 그리기위해 다음의 매트랩 코드를 작성합시다.

 

function handle = drawSpacecraftBody(pn,pe,pd,phi,theta,psi, handle, mode)
 % define points on spacecraft in local NED
 %coordinates
 NED = spacecraftPoints;
 % rotate spacecraft by phi, theta, psi
 NED = rotate(NED,phi,theta,psi);
 % translate spacecraft to [pn; pe; pd]
 NED = translate(NED,pn,pe,pd);
 % transform vertices from NED to XYZ
 R=[...
 0, 1, 0;...
 1, 0, 0;...
 0, 0, −1;...
 ];
 XYZ = R*NED;
 % plot spacecraft
 if isempty(handle),
   handle = plot3(XYZ(1,:),XYZ(2,:),XYZ(3,:), `EraseMode', mode);
 else
   set(handle,`XData',XYZ(1,:),`YData',XYZ(2,:),`ZData',XYZ(3,:));
   drawnow
 end

 위 식에서 plot3은 처음 우주선을 그리는 명령이고, set으로 X,Y,ZData를 변경하며, 이 우주선을 그린 결과는 C.4가 됩니다.

그림 C.4 우주선

 

 spacecraftPoints 함수로 애니메이션 구현시 문제점은 애니메이션이 갱신될때마다 이 함수가 호출되게 됩니다. 이 점은 정적이므로, 한번만 정의하면 되기 때문에 시뮬레이션 시작시 마스크 함수를 써서 정의해 주면 됩니다. drawSpacecraft m file을 마스킹하려면 edit mask를 클릭하고 그림 C.5처럼 입력해주면 됩니다. 이러면 우주선의 점들이 초기화 시점에서 정의되어 drawSpacecraft 파일의 파라미터로 사용됩니다.

그림 C.5 마스크 함수. 시뮬레이션 시작시 우주선 점들을 초기화 함.

 

 

수정 코드

function handle = drawSpacecraftBody(u)
persistent spacecraft_handle; 
pn = u(1);
pe = u(2);
pd = u(3);
phi = u(4);
theta = u(5);
psi = u(6);
% define points on spacecraft in local NED
NED=spacecraftPoints;
 % rotate spacecraft by phi, theta, psi
 NED = rotate(NED,phi,theta,psi);
 % translate spacecraft to [pn; pe; pd]
 NED = translate(NED,pn,pe,pd);
 % transform vertices from NED to XYZ
 R=[0, 1, 0;
     1, 0, 0;
     0, 0, -1];
 XYZ = R*NED;
 % plot spacecraft
 if isempty(spacecraft_handle),
   spacecraft_handle = plot3(XYZ(1,:),XYZ(2,:),XYZ(3,:), 'eraseMode',  'normal');
   grid on;
 else
   set(spacecraft_handle,'XData',XYZ(1,:),'YData',XYZ(2,:),'ZData',XYZ(3,:));
   drawnow
 end

 

 

 

C.4 애니메이션 예제 : 점과 면을 이용한 우주선

 그림 C.4은 점과 면 구조로 vertex-face 로 구현할수 있는데, plot3 대신 patch 명령을 사용합니다. 점들과, 면, 색상을 다음의 코드로 정의 합니다.

 

function [V, F, patchcolors]=spacecraftVFC
% Define the vertices (physical location of vertices
V=[...
1 1 0;... % point 1
1 -1 0;... % point 2
-1 -1 0;... % point 3
-1 1 0;... % point 4
1 1 -2;... % point 5
1 -1 -2;... % point 6
-1 -1 -2;... % point 7
-1 1 -2;... % point 8
1.5 1.5 0;... % point 9
1.5 -1.5 0;... % point 10
-1.5 -1.5 0;... % point 11
-1.5 1.5 0;... % point 12
];

% define faces as a list of vertices numbered above
F=[...
1, 2, 6, 5;... % front
4, 3, 7, 8;... % back
1, 5, 8, 4;... % right
2, 6, 7, 3;... % left
5, 6, 7, 8;... % top
9, 10, 11, 12;... % bottom
];
% define colors for each face
myred = [1, 0, 0];
mygreen = [0, 1, 0];
myblue = [0, 0, 1];
myyellow = [1, 1, 0];
mycyan = [0, 1, 1];
patchcolors = [...
    myred;... % front
    mygreen;... % back
    myblue;... % right
    myyellow;... % left
    mycyan;... % top
    mycyan;... % bottom
];

 

 그림 C.3처럼 점들을 설정하고, 면들을 정의 하였는데 정면의 경우 점 1-2-6-5 으로 만들었습니다. 각 면들의 색상도 정의하고 비행체 그리는 코드를 아래와 같이 정리하였습니다.

 

function handle = drawSpacecraftBody2(u)
persistent spacecraft_handle;

pn = u(1);
pe = u(2);
pd = u(3);
phi = u(4);
theta = u(5);
psi = u(6);

[V, F, patchcolors] = spacecraftVFC;
% define points on spacecraft
V = rotate(V', phi, theta, psi)';
% rotate spacecraft
V = translate(V', pn, pe, pd)';
% translate spacecraft
R=[...
    0, 1, 0;...
    1, 0, 0;...
    0, 0, -1;...
];

V=V*R; % transform vertices from NED to XYZ
if isempty(spacecraft_handle),
    spacecraft_handle = patch('Vertices', V, 'Faces', F, ...
    'FaceVertexCData',patchcolors,...
    'FaceColor','flat',...
    'EraseMode', 'normal');
    grid on;
else
    set(spacecraft_handle,'Vertices',V,'Faces',F);
end