KR20040034167A - 6축 수직다관절 매니퓰레이터의 직선이동 제어방법 - Google Patents

Abstract

본 발명은 수직다관절 매니퓰레이터의 기구적 특이점에서 유연한 관절운동을 하도록 하는 제어방법에 관한 것이다.

기구적 특이성이 존재하는 위치, 즉 자코비안(Jacobian)이 0이 되는 위치에서도 직선보간 운동이 이루어질 뿐 아니라 직선보간 이동시 모터의 최대 속도에서 벗어나지 않으므로 소음이나 진동이 최소화 되는 효과를 가진다.

Description

6축 수직다관절 매니퓰레이터의 직선이동 제어방법{The method of control- ling straight-line movement of vertical multi-joint six-axis manipulator}

본 발명은 6축 수직다관절 매니퓰레이터의 직선이동 제어방법에 관한 것으로 더욱 상세하게는 매니퓰레이터가 공작물을 고속으로 핸들링 할 때 일어나는 특이점 에서의 유연한 관절운동을 하도록 하는 것에 관한 것이다.

일반적으로 다관절 매니퓰레이터는 여러개의 링크들이 서로 연결되어 구성되고, 각 링크의 연결부위를 관절(Joint)이라 하는데 매니퓰레이터는 이러한 링크와 관절들 사이의 기하학적인 관계에 따라 운동 특이성이 결정된다.

이 기하학적인 관계를 수학적으로 표현한 것을 기구학(Kinematics)이라하며 매니퓰레이터는 이러한 기구학적 특성에 따라 고유의 특이점(Singularity-Point)을 갖게된다.

특이점이란 기구적 특이성이 존재하는 위치, 즉 함수행렬식(Jacobian)이 0이 되는 위치로, 이 특이점에서 또는 특이점 근방에서는 매니퓰레이터의 손 끝(End -effector)을 특정한 방향으로 이동시키고자 할 때 매우 큰 관절속도를 필요로 한다.

하지만 실제 매니퓰레이터의 제어기는 물리적인 한계가 있으므로 이러한 영역에서는 필요한 만큼의 관절속도를 제공할 수 없게 되고 따라서 매니퓰레이터는 주어진 명령을 수행할 수 없게된다.

따라서 종래에는 특이점 회피방법으로 다음과 같은 세가지 방법이 있었다.

첫째, 티칭(Teaching)시 로봇의 궤적이 특이점 근처에 있게 되면 이를 오퍼레이터에 알려 재 티칭 하도록 하는 방법과,

둘째, 티칭 궤적이 특이점 근처를 통과하게 되면 실제 운동시에는 그 주위로 우회하도록 하는 궤적제어 방법과,

셋째, 로봇의 6축 이외의 별도의 한축을 추가하여 특이점 부근을 통과하지 못하도록 하는 회피제어 방법이 있었다.

그러나 위와같은 방법은 특이점을 인식하기 위한 데이터리스트화 작업이 비효율적이고 프로세서의 계산량이 많아 작업시간이 길어지게 되어 고속 핸들링이 어렵게 되거나 매니퓰레이터의 구조가 복잡해지는 문제점이 있었다.

따라서 본 발명은 상기와 같은 문제점을 해결하기 위한 것으로 기구적 특이성이 존재하는 위치에서 직선보간(補間)속도에 따른 등간격의 직선보간 경유점을 구하고, 단위구간 이동속도가 모터의 최대속도를 초과하지 않도록 단위시간 변화에 따른 속도제한을 가하여 새로운 직선보간 경유점을 구하므로 로봇핸드가 이 최종 보간 경유점을 따라 이동하는 직선보간 방법을 제공하는 것을 목적으로 한다.

도 1은 본 발명의 제어방법을 나타내는 블록도

도 2는 본 발명의 순/역 기구학을 정의하기 위한 D-H(Denavit-Hatenberg) 모식도

도 3은 본 발명을 이용한 시물레이션 결과를 도시한 그래프

※도면의 주요부분에 대한 부호의 설명

Z₀~ Z₅: 제1회전축 ~ 제6회전축

θ₁~ θ₆: 제1회전축모터각 ~ 제6회전축모터각

a₁~ a₃: 관절간의거리(X축방향)

d₁, d₄, d₆: Y축방향거리

J₁~J₆: 제1관절 ~ 제6관절

이하 첨부된 도면을 참조하여 본 발명을 상세히 설명한다.

도 2는 본 발명이 적용되는 매니퓰레이터의 예를 도시한 도면으로 우선 기구학 해석은 아래식을 기본으로 순/역 기구학 해석을 적용한다.

6축 수직다관절 로봇의 경우 각 관절사이에 6개의 변형행렬(Transformation Matrix)이 존재하며 순 기구학적 방정식에 의한 점 데이터는 아래의 식에서 구한다.

여기서 S = sin, C = cos 이고, 숫자 1 ~ 6은를 나타낸다.

위식에서로써 로봇운동의 궤적을 나타내는 점데이타이다.

손목의평면상의 위치를라고 하면 역기구학에 의한 Sampling time(u)간의 모터회전각은 아래식으로 각각 나타낼 수 있다.

단,, 그리고는 아래와 같다.

는행렬에서와 같은 위치의 원소이다.

구하여진 이동경로에 대하여 각 모터의 회전각 θ₁~ θ₆을 구하여 제어를 한다.

이 경우 궤적산출과 모터의 회전각을 기구학 해석으로부터 구하게 된다.

이때 θ₃와 θ₅가 0또는 π가 될 때는 모터각이 아주 크게 나온다.

이러한 형태를 특이성(Singularity)이라 한다.

이 경우 단위시간 동안의 이동거리가 커짐으로 관절의 변화율이 구동모터의 최대속도를 벗어나고, 또한 특이성이 있는 위치에서의 속도변화가 불연속적으로 이루어지기 때문에 진동 및 소음이 발생하므로 위와같은 문제점을 해결하기 위한 수단으로 본 발명은 경로의 시작점을 0, 끝점을 1로하여 입력된 좌표의 임의 위치를 정한다.

입력된 좌표로부터 순 기구학적 방정식을 이용하여 각으로 부터의 각위치와 자세를 입력 받는다.

입력받은 툴의 위치, 자세로부터 모터의 회전각을 구하는 것은 0과 위치 1에서 역 기구학적 방정식을 이용한 해로 각축의 변위각을 이용한 모터의 회전각을 구한다.

증분값에 대한 셈플링 타임 동안의 이동변위(각)에 따른 각축의 변위각을 계산하고 계산된 모터회전각으로부터 요구속도와 가감속도를 구한 뒤 모터의 최대허용속도와 최대허용선속도(선형보간)를 비교한다.

요구 가감속도와 최대허용가감속도를 비교하여 속도제한을 하게 되는데 여기서 실제요구속도/요구속도의 비를 계산하여 최소값을 해당구간의 비율로 정한다.

비율에 의해 각축별 회전각을 변화시키게 된다.

결과적으로 최대허용속도에 비추어 정해진 이동거리에 맞는 시간(sampling time)이 변경되게 되고 이 변경된 시간에 따른 증분구간의 전체 회전각들을 재계산하여 적용한다.

도 1을 참고하면, 요구속도와 가감속도가 최대허용치를 벗어나는지를 비교한다(S7).

최대허용치를 벗어나지 않으면 곧바로 모터회전각으로 저장, 실행하게 된다.

S7에서 벗어날 경우 새로운 경로 매개변수 증분값을 산출, 적용시켜 속도와 모터회전각을 계산하여 이것을 저장하게 된다.

마지막으로 저장된 모터회전각이 끝점과 일치하는지를 비교하여 끝점에 도달하지 않을시에는 S4에서부터 반복시행을 한다.

이하, 본 발명의 각 단계별 세부설명을 첨부된 도면(도1)에 따라 상세히 설명한다.

S1; 시작점(P1)과 끝점(P2)에서의 시간에 따른 증분값(sampling time) u를 정한다.

S2; 식(2)를 사용하여 좌표상의 위치(Position)와 자세(Orientation)를 입력받는다.

S3; 입력된 툴(Tool)의 위치와 자세로부터 각축의 현재각도를 구한다.

S4; 현재각과 요구각 사이의 차를 구한다.

S5; 최대요구 각속도와 선속도를 비교분석 한다.

S6; 실제 요구각과 허용각과의 비()를 구한다.

의 최소값을 기준으로 1보다 큰 경우가 존재하면 u값을 u'으로 저장한다.

여기서 재계산된 u'을 사용하여 회전각을 구한다.

S7; 요구속도와 계산된 최대허용속도를 비교한다.

S8; S7에서 요구속도가 모터의 최대허용속도보다 클 경우 단계로 S6의 과정을 거쳐 모터 회전각을 계산한다.

S9; 새로구한 값으로부터 모터회전각을 재계산한다.

S10; 모터각, 위치, 그리고 자세를 저장시킨다. 그리고 S7에서 "NO"일 경우 S10을 바로 수행하도록 한다.

S11; 모터를 제어한다.

S12; 종점까지 위치했는지를 비교하여 끝점에 이르지 않았다면 S3으로 돌아가서 반복수행한다.(이때는 초기설정된 Sampling time u를 사용한다.)

본 발명은 6축 매니퓰레이터의 특이성 위치에서의 부드러운 경로추적이 가능하며 모터회전각의 최대속도를 넘지 않기 때문에 특이점 위치에서도 로봇이 멈추지 않고 이동을 할 수 있다.

도 3은 본 발명의 검증을 위한 경로 궤적에 대한 시물레이션이다.

본 발명의 적용으로 보다 정확하고 실제적인 특이점 회피를 도 3에서와 같이 하게됨을 알 수 있다.

도 3은 제안된 방법을 최종적으로 적용한 것으로서 각축의 허용속도를 넘지않고 경로궤적이 이루어졌음을 볼 수 있다.

선속도 또한 모터의 최대속도를 넘지않아 전체적으로 로봇의 부드러운 동작을 할 수 있는 것이다.

아울러, 종래의 획일적으로 속도를 줄이는 방법에 비해 작업 효율을 향상시킬수 있고, 자코비안 매트릭스와 같은 복잡한 연산을 필요로 하지 않으므로 처리시간을 단축할 수 있는 효과가 있다.

Claims (1)

1. 기구학적 고유의 특이점을 갖는 매니퓰레이터의 최대허용 선속도를 제어하는 방법에 있어서, 출발위치에서 목표위치로 일정한 직선보간 속도에 따른 등간격의 직선보간 경유점을 산출하는 직선보간 경유점의 위치와 자세 산출단계와, 산출된 관절의 변화율에 따른 모터 최대허용속도와 실제요구속도를 비교하는 단계와, 실제요구각과 허용각과의 비를 구하는 단계와, 요구속도와 계산된 최대허용속도를 비교하여 새로구한 값으로부터 모터회전각을 재계산하는 단계와, 다음단계의 경유점을 이동시 단위시간을 초기값으로 다시 사용하여 적용하는 단계로 이루어진 것을 특징으로하는 6축 수직다관절 매니퓰레이터의 선형보간 방법.