Manipulator simulation [matlab][2] – 로봇 팔, 3 link manipulator, forward kinematics, inverse kinematics, 위치 제어

1편 – Manipulator simulation [matlab][1] – Robotics Toolbox (petercorke) 이용해서 one link manipulator PID 제어 <- 링크
2편 – Manipulator simulation [matlab][2] – 로봇 팔, 3 link manipulator, forward kinematics, inverse kinematics, 위치 제어 <- 링크

1. 3 link manipulator 구성

이전 1편에서 plot.m 파일을 만들었을 것이다. 이 부분의 코드를 아래와 같이 수정한다.

function [sys,x0,str,ts] = plot_3DOF_Manipulator(t,x,u,flag)

    % 로봇 모델 초기화
    L(1) = Link([pi/2 0 0.4 0]); 
    L(1).offset = -pi/2;
    
    L(2) = Link([0 0 0.4 0]);     
    L(2).offset = 0;
    
    L(3) = Link([0 0 0.4 0]);   
    L(3).offset = 0;
    
    % 3 링크를 직렬 연결한 로봇 모델
    three_link = SerialLink(L, 'name', 'three link');
    three_link.base = [0 1 0 0; 0 0 1 0; 1 0 0 0; 0 0 0 1];

    switch flag

        case 0         
            sizes = simsizes;
            sizes.NumContStates  = 0;
            sizes.NumDiscStates  = 0;
            sizes.NumOutputs     = 0;
            sizes.NumInputs      = 3;  % 세 개의 입력 (각 링크의 각도)
            sizes.DirFeedthrough = 1;
            sizes.NumSampleTimes = 1;

            sys = simsizes(sizes);
            x0  = [];
            str = [];
            ts  = [-1 0]; 
            
            % 초기 각도는 0으로 설정하여 플롯
            three_link.plot([0, 0, 0], 'workspace', [-1 1 -1 1 -1 1], 'scale', 0.5); 
            view(135, 20);  
            % teach 함수를 통해 링크와 조인트가 모두 보이도록 설정할 수 있습니다
            three_link.teach();

        case 1
            % Nothing to do for derivatives

        case {2,9}
            % 애니메이션을 위한 각 링크의 관절 각도 업데이트
            if length(u) >= 3
                three_link.animate([u(1), u(2), u(3)]);
            else
                error('입력 u는 세 개의 각도를 포함해야 합니다.');
            end

        case 3
            % Nothing to do at each time step

        otherwise
            error(['unhandled flag = ', num2str(flag)]);
    end
end

이렇게 하면 이제 우리는 3 link manipulator 를 다루게 된다.

1 link 일 때의 link-1 과 같은 kinematic structure 를 이어서 추가하여 3 link manipulator 구현

각 링크의 길이 = 0.4

2. 전체 overview

y축값은 0으로 고정한뒤에 x, y position 을 입력으로 넣어주면 inverse kinematics 를 거쳐 u1, u2, u3 가 로봇 입력으로 들어감

로봇 current 값을 가지고 forward kinematics 계산을 하여 현재 current 값을 관찰

3. forward kinematics

function pos = forward_kinematics(theta)

% 링크 길이 설정

L1 = 0.4; % 첫 번째 링크 길이

L2 = 0.4; % 두 번째 링크 길이

L3 = 0.4; % 세 번째 링크 길이

% 각도 추출

theta1 = theta(1);

theta2 = theta(2);

theta3 = theta(3);

% 순방향 운동학 계산

% 각 링크의 x, y, z 위치 계산

x = L1 * cos(theta1) + L2 * cos(theta1 + theta2) + L3 * cos(theta1 + theta2 + theta3);

y = 0; % 모든 조인트가 Y축을 중심으로 회전하므로 y는 항상 0

z = L1 * sin(theta1) + L2 * sin(theta1 + theta2) + L3 * sin(theta1 + theta2 + theta3);

% 출력 위치 벡터

pos = [x; y; z];

end

3. inverse kinematics

function theta = inverse_kinematics(x, z)
    % 링크 길이 설정
    L1 = 0.4; % 첫 번째 링크 길이
    L2 = 0.4; % 두 번째 링크 길이
    L3 = 0.4; % 세 번째 링크 길이

    % 목표 위치가 매니퓰레이터의 가용 범위 내에 있는지 확인
    % if sqrt(x^2 + z^2) > (L1 + L2 + L3)
    %     error('목표 위치가 작업 공간 범위를 벗어났습니다.');
    % end
    
    % 단계 1: θ3 계산
    % psi: 엔드 이펙터의 방향 (예를 들어, 원하는 방향 각도를 설정할 수 있음)
    psi = atan2(z, x);  % 기본적으로 x와 z에 따른 목표 방향 설정

    % 목표 위치로부터 P2 좌표 계산
    p2x = x - L3 * cos(psi);
    p2y = z - L3 * sin(psi);

    % 각도 θ2 계산
    c2 = (p2x^2 + p2y^2 - L1^2 - L2^2) / (2 * L1 * L2);
    c2 = max(min(c2, 1), -1); % 도메인 오류 방지
    s2 = sqrt(1 - c2^2);
    theta2 = atan2(s2, c2);

    % 각도 θ1 계산
    k1 = L1 + L2 * cos(theta2);
    k2 = L2 * sin(theta2);
    theta1 = atan2(p2y, p2x) - atan2(k2, k1);

    % 각도 θ3 계산
    theta3 = psi - (theta1 + theta2);

    % 출력 각도 배열
    theta = [theta1; theta2; theta3];
end

4. 제어 결과

Target x,y=1.2, 0.2 ->

Target x,y=0.8, -0.2 ->

각 조인트 각도에 대한 에러

End effector position error