KR0160999B1

KR0160999B1 - Error compensation method for the circular trajectory of robot

Info

Publication number: KR0160999B1
Application number: KR1019920017103A
Authority: KR
Inventors: 송진일
Original assignee: 윤종용; 삼성전자주식회사
Priority date: 1992-09-19
Filing date: 1992-09-19
Publication date: 1998-12-15
Also published as: KR940006720A

Abstract

본 발명은 원호상의 시작점과 중간점 및 종점위치를 판단하는 판단스텝과, 상기 판단스텝에서 원호상의 3점을 판독한 후 가감속이 없는 경우 원호상의 x,y 좌표를 구하는 제1좌표 계산스텝과, 상기 제1좌표 겨산스텝에서 구해진 x,y좌료의 각축으로 직선가감속을 행하여 가감속이 있는 경우 원호상의 x,y좌표를 구하는 제2좌표 계산스텝과, 상기 좌표계산 스텝에서 구해진 좌표를 매 샘플링 시간마다 임의의 시점에서의 원호보간을 위해 각축의 목표위치를 구하는 목표위치 계산스텝과, 상기 목표위치 계산스텝을 통해 판독한 각축의 현재위치값을 구한 후, x,y축의 위치편차를 구하는 위치편차 계산스텝과, 상기 위치편차 계산 스텝에서 산출된 위치편차에 대해서 비례적분 미분 피드 퍼워드(Feed Forward)제어를 행하여 서보모터를 제어하는 제어스텝으로 구성되어 직각좌표의 X-Y평면에서 로보트의 손끝이 원호를 따라 움직이며 각축의 가감속시 이산시간 상태 방정식에 의해 직선 가감속을 행하기 때문에 샘플링 시간을 줄일 수 있어 고속의 원호이동이 가능한 것이다.The present invention provides a judgment step for determining the starting point, the intermediate point and the end point position on the arc, a first coordinate calculation step for obtaining the x, y coordinates on the arc when there is no acceleration / deceleration after reading the three points on the arc in the determination step; The second coordinate calculation step of obtaining the x, y coordinates on the arc and the coordinates obtained in the coordinate calculation step every sampling time when the acceleration / deceleration is performed by performing linear acceleration / deceleration on each axis of the x, y coordinates obtained in the first coordinate drawing step. The target position calculation step of obtaining the target position of each axis for circular interpolation at any point of time, and the position deviation calculation of the position deviation of the x and y axes after calculating the current position value of each axis read through the target position calculation step. And a control step for controlling the servomotor by performing proportional integral derivative feed forward control on the position deviation calculated in the position deviation calculation step. It is it can reduce the sampling time is capable of high-speed of the circular movement because the fingertip of the robot in the X-Y plane of a Cartesian coordinate to perform the linear acceleration and deceleration by the movement and the discrete-time state equation during acceleration and deceleration of each axis along an arc.

Description

Circular interpolation method of robot

제1도는 X-Y 평면에서 원호보간시 X,Y축의 속도 Vx, Vy를 나타낸 그래프.1 is a graph showing the speeds Vx and Vy of the X and Y axes during circular interpolation in the X-Y plane.

제2도는 X-Y 평면에서 원호보간시 원의 속도를 직선 가감속한 경우 매 샘플링 시간마다 X-Y 평면에서 움직여야할 증분양을 나타낸 개념도.2 is a conceptual diagram illustrating an incremental amount to be moved in the X-Y plane every sampling time when the linear velocity of the circular interpolation in the X-Y plane is linearly decelerated.

제3도는 X-Y 평면에서 가감속이 없는 경우 매 샘플링 시간마다 이동해야 할 원호의 길이를 나타낸 개념도.3 is a conceptual diagram showing the length of an arc to be moved every sampling time in the absence of acceleration and deceleration in the X-Y plane.

제4도는 본 발명에 의한 로보트의 원호보간 방법을 실현하기 위한 위치제어 시스템의 개요도.4 is a schematic diagram of a position control system for realizing a robot circular interpolation method according to the present invention;

제5도는 본 발명에서 가감속이 없는 경우 매 샘플링시간마다 원호를 계산하기 위한 개념도.5 is a conceptual diagram for calculating an arc every sampling time when there is no acceleration and deceleration in the present invention.

제6도는 본 발명에 의한 로보트 원호보간 방법의 데이터 흐름도.6 is a data flow diagram of the robot circular interpolation method according to the present invention.

* 도면의 주요부분에 대한 부호의 설명* Explanation of symbols for main parts of the drawings

1 : 마이크로프로세서 2 : 디지탈/아날로그변환기1: Microprocessor 2: Digital / Analog Converter

3 : 서보제어부 4 : 서보모터3: Servo controller 4: Servo motor

5 : 타코제너레이터 6 : 엔코더5: taco generator 6: encoder

7 : 업/다운카운터 8 : 타이머7: Up / Down Counter 8: Timer

본 발명은 로보트의 원호보간 방법에 관한 것으로서, 특히 2개 이상의 아암을 가진 다관전형 로보트에 있어서 아암의 손끝(End-Effictor)을 현재 위치에서 목표위치까지 원호이송을 위한 방법에 관한 것이다.The present invention relates to a circular interpolation method of a robot, and more particularly to a method for circular arc transfer from the current position to the target position of the end of the arm in the multi-spectral robot having two or more arms.

일반적으로 로보트가 원호를 따라 보간할 때 X-Y 평면에서의 원에 따른 X, Y축의 속도분포(Vx, Vy)는 제1도에 도시된 바와 같이,In general, when the robot interpolates along an arc, the velocity distributions Vx and Vy along the circle in the X-Y plane are shown in FIG.

로 표시된다. 여기서 A는 원호 보간시 각축의 최대 속도를 나타낸다.Is displayed. Where A represents the maximum velocity of each axis during circular interpolation.

제1도에 도시된 그래프를 통해 알 수 있듯이 원호의 시작점에서 Y축은 Sin함수 형태로 속도가 변하기 때문에 진동이 발생하지 않으나 X축의 속도는 Cos 함수 형태로 변하기 때문에 속도가 영(ZERO)에서 최고속도 A까지 급격히 변하여 진동이 발생되고, 마찬가지로 원호의 임의의 점에서(0∼360도 사이의 임의의 점) 정지시 상기 식(1)에서 X, Y축의 속도가 영(ZERO)이 되어야 함으로 진동이 발생되기 때문에 고속원호 보간시 진동량이 크고 원의 왜곡도 심하게 발생하는 문제점이 있었다.As can be seen from the graph shown in FIG. 1, since the Y-axis changes speed in the form of Sin function at the starting point of the arc, no vibration occurs, but the velocity of the X-axis changes in the form of Cos function. The vibration suddenly changes to A, and vibration occurs. Similarly, the vibration of the X and Y axes should be zero (ZERO) in Equation (1) when stopping at any point of the arc (any point between 0 and 360 degrees). Because of the high speed circular interpolation, the amount of vibration is large and the distortion of the circle is also severely generated.

따라서 본 발명은 상기와 같은 제반 결점을 해결하기 위해 원호 이송시 속도분포를 직선 가감속 형태로 행함으로써 고속의 원호보간시 원호의 시작과 끝에서 발생하는 진동을 방지하고, 고속의 원호보간이 가능함과 동시에 속도 분포를 직선 가감속 형태로할 때 이산시간 상태 방정식에 의해 실현함으로써 샘플링 시간을 단축할 수 있어 고정도의 원호보간을 가능하게 하는 로보트의 원호보간 방법을 제공하는데 그 목적이 있다.Therefore, the present invention prevents the vibration occurring at the beginning and the end of the circular arc during the high speed circular interpolation and enables the high speed circular interpolation by performing the speed distribution in the form of a linear acceleration and deceleration during arc transfer to solve the above-mentioned shortcomings. At the same time, it is an object of the present invention to provide a circular arc interpolation method of a robot that enables high-precision circular interpolation by shortening the sampling time by realizing a discrete time state equation when the velocity distribution is in the form of linear acceleration and deceleration.

상기 목적을 달성하기 위하여 본 발명의 로보트 원호보간방법은, 원호상의 시작점과 중간점 및 종점위치를 판단하는 판단스텝과, 상기 판단스텝에서 원호상의 3점을 판독한 후 가감속이 없는 경우 원호상의 X,Y좌표를 구하는 제1좌표 계산스텝과, 상기 제1좌표 계산스텝에서 구해진 X,Y좌표의 각축으로 직선가감속을 행하여 가감속이 있는 경우 원호상의 X,Y좌표를 구하는 제2 좌표계산스텝과, 상기 좌표계산 스텝에서 구해진 좌표를 매샘플링 시간마다 임의의 시점에서의 원호보간을 위해 각축의 목표 위치를 구하는 목표위치 계산스텝과, 상기 목표위치 계산스텝을 통해 판독한 각축의 현재위치값을 구한 후 X,Y축의 위치 편차를 구하는 위치 편차계산 스텝과, 상기 위치편차 계산 스텝에서 산출된 위치편차에 대해서 비례적분 미분 피드 퍼워드(Feed Forward) 제어를 행하여 서보모터를 제어하는 제어스텝으로 이루어진 것을 특징으로 한다.In order to achieve the above object, the robot circular interpolation method of the present invention includes a judgment step of determining the start point, the intermediate point and the end point position of the arc, and the X on the arc if there is no acceleration / deceleration after reading the three points on the arc in the determination step. A first coordinate calculation step for obtaining Y coordinates and a second coordinate calculation step for obtaining X and Y coordinates on an arc when acceleration and deceleration are performed by performing linear acceleration and deceleration on each axis of the X and Y coordinates obtained in the first coordinate calculation step; A target position calculation step of obtaining a target position of each axis for circular interpolation at any point in time from the coordinate calculation step for each sampling time, and a current position value of each axis read through the target position calculation step is obtained. Position deviation calculation step for calculating position deviation of the X and Y axes, and proportional integral derivative feed forward with respect to the position deviation calculated in the position deviation calculation step. A control step of controlling the servomotor by performing the control is characterized by the above-mentioned.

이하, 본 발명의 일실시예를 도면을 참조하여 상세히 설명한다.Hereinafter, an embodiment of the present invention will be described in detail with reference to the drawings.

제1도는 X-Y 평면에서 원호보간시 X,Y축의 속도 분포를 도시한 것으로서 원의 시작점과 임의의 끝점에서 진동 발생 관계를 설명한 것이고, 제2도는 직각좌표상에서 원호보간시 속도분포를 직선 가감속한 경우 매 샘플링 시간마다 X-Y 평면에서 움직여야 할 증분양을 나타낸 것이며, 제3도는 가감속이 없는 경우의 원호 보간시 매 샘플링 시간마다 X-Y 평면에서 움직여야 할 증분양을 나타낸 것이다. 제4도는 본 발명에서 사용되는 위치제어 시스템을 나타낸 것으로, (1)은 속도 명령을 출력하는 마이크로프로세서이고, (2)는 상기 마이크로프로세서에서 출력된 디지탈값의 속도명령을 아날로그값으로 변환하는 디지탈/아날로그 변환기이다. 그리고 (3)은 모터(4)에 흐르는 전류(Ix)와 모터의 속도에 비례하여 발생되는 전압(Vox)을 타코제네레이터(5)의 출력을 귀환하여 모터에 흐르는 전류와 속도제어를 위한 서보제어부이며, (6)은 모터의 회전에 따라 펄스를 발생하는 엔코더이고, (7)은 업/다운 카운터로서 모터의 회전 방향에 따라 엔코더(6)에서 발생하는 펄스를 증감하는 카운터로서 마이크로 프로세서(1)에 모터의 현재 위치를 제공하며, (8)은 일정한 주기마다 상기 마이크로 프로세서(1)에 인터럽트를 걸어주기 위한 타이머로써, 상기 각 구성 장치는 한 축에 대해서만 설명한 것이다.1 is a diagram illustrating the velocity distribution of the X and Y axes during circular interpolation in the XY plane, and illustrates the relationship between the occurrence of vibration at the starting point and an arbitrary end point of the circle, and FIG. 2 shows the velocity distribution during the circular interpolation in the rectangular coordinates. Fig. 3 shows the incremental amount to be moved in the XY plane at every sampling time, and FIG. 3 shows the incremental amount to be moved in the XY plane at every sampling time during circular interpolation without acceleration or deceleration. 4 is a position control system used in the present invention, (1) is a microprocessor for outputting a speed command, and (2) a digital command for converting a speed command of a digital value output from the microprocessor to an analog value. / Analog converter. And (3), the servo control unit for controlling current and speed flowing through the motor by returning the output of the taco generator 5 to the current Ix flowing in the motor 4 and the voltage Vox generated in proportion to the speed of the motor. (6) is an encoder for generating a pulse in accordance with the rotation of the motor, (7) is an up / down counter as a counter for increasing or decreasing the pulse generated in the encoder (6) in accordance with the rotation direction of the motor microprocessor (1). (8) is a timer for interrupting the microprocessor 1 at regular intervals, and each of the components is described only for one axis.

다음은 제4도에 도시된 바와 같이 위치제어 시스템을 가진 로보트의 원호 보간 방법을 제6도에 도시된 데이터 흐름도에 따라 상세히 기술한다.Next, a circular interpolation method of a robot having a position control system as shown in FIG. 4 will be described in detail according to the data flow diagram shown in FIG.

제6도에 도시된 데이터 흐름도는 ½원을 보간하는 방법을 설명한 것으로서, 원호 보간을 하기 위해서는 스텝 S1과 같이 원호상의 시작점(A), 중간점(B), 종점(C)위치를 판독하고 원호 보간 방향을 읽어 들인다. 현재 위치 A(X1, Y1)에서 가감속이 없는 경우에 이동해야 할 원호상의 점을 아래와 같이 구한다.The data flow diagram shown in FIG. 6 describes a method of interpolating a ½ circle. To perform circular interpolation, as shown in step S1, the starting point (A), the middle point (B), and the end point (C) of the circular arc are read and the circular arc is read. Read the interpolation direction. Find the point on the arc to be moved when there is no acceleration / deceleration at the current position A (X1, Y1) as follows.

따라서 원호상의 2번째 점은 아래와 같이 구해진다.Therefore, the second point on the arc is obtained as follows.

따라서 (2)식을 이용하여 (3)식을 다시 쓰면 (4)식과 같이 된다.Therefore, if you rewrite equation (3) using equation (2), you will get something like equation (4).

여기서 Cos△θ Sin△θ는 △θ가 일정(Constant)하기 때문에 일정한 값을 가지므로Here, CosΔθ SinΔθ has a constant value because Δθ is constant.

로 놓고 (4)식을 일반적으로 쓰면 아래와 같이 된다.In general, the equation (4) is written as follows.

여기서 K는 샘플링 시간마다 증가된다.Where K is increased for each sampling time.

여기서 X(0)=X1, Y(0)=Y1이고, X(0)는 원호의 시작점의 X좌표 X1이고 Y(0)는 원호의 시작점의 Y좌표 Y1이다.Where X (0) = X1, Y (0) = Y1, X (0) is X coordinate X1 of the starting point of the arc and Y (0) is Y coordinate Y1 of the starting point of the arc.

따라서, 가감속이 없는 경우 스텝S2와 같이 (6)식에 의해 원호상의 점(중간목표위치)를 구할 수 있고 X-Y 평면에서 제3도에 도시된 바와 같이 매 샘플링 시간마다 이동된다.Therefore, when there is no acceleration / deceleration, the point (intermediate target position) on the arc can be obtained by the formula (6) as shown in step S2, and is moved every sampling time in the X-Y plane as shown in FIG.

따라서 (6)식에 의해 가감속이 없는 경우의 스텝S3와 같이 원호상에서의 X,Y축의 위치 증분양을 구한 후 아래의 식에 의해 각축의 원호상의 좌표값을 구하면 X-Y평면상에서의 원의 속도 분포가 제2도와 같이 직선 가감속 형태가 된다.Therefore, as in step S3 when there is no acceleration / deceleration by Equation (6), after calculating the position increment amount of the X and Y axes on the arc, and calculating the coordinate values on the arc of each axis by the following equation, the velocity distribution of the circle on the XY plane is obtained. Becomes a linear acceleration / deceleration form as shown in FIG.

매 샘플링 시간마다 임의 시점에서의 원호 보간을 위해 각축의 목표를 스텝S4에서 구하는데, 이는 아래와 같이 계산된다.The target of each axis is obtained in step S4 for circular interpolation at any point in time at each sampling time, which is calculated as follows.

따라서 임의의 K시점에서 각축마다 업/다운 카운터에서 읽은 로보트의 현재 위치값을 Cx(K), Cy(K)라하면 임의의 K시점에서 각축의 현재 편차 Pex(K), Pey(K)는 스텝S5를 통해 아래와 같이 계산하여 구한다.Therefore, if the current position value of the robot read by the up / down counter for each axis at any time K is Cx (K), Cy (K), the current deviation Pex (K), Pey (K) of each axis at any time K is The following calculation is made through step S5.

따라서 스텝S6에서는 각축의 위치편차에 대해 비례, 적분, 미분 피드퍼워드(Feedforward)제어를 행하며 아래와 같이 계산한다.Therefore, in step S6, proportional, integral, and differential feedforward control is performed on the positional deviation of each axis, and the calculation is performed as follows.

상기 식(11)에서 T는 샘플링 시간을 나타내고 Dxout, Dyout은 각축의 디지탈/아날로그 변환기로 입력되는 값이고, Kpx,Kpy는 각축의 비례(P)게인이며, KIx, KIy는 각축의 적분(Ⅰ)게인이다.In Equation (11), T denotes a sampling time, Dxout and Dyout are values input to the digital / analog converter of each axis, Kpx and Kpy are proportional gains of each axis, and KIx and KIy are integrals of each axis. It is gain.

또한 KDx, KDy는 각축의 미분(D) 게인이며 KFx, KFy는 각축의 피드포워드 게인이다.KDx and KDy are derivative gains of each axis, and KFx and KFy are feedforward gains of each axis.

각축의 비례적분(PID), 피드퍼워드(FEEDFORWARD) 게인은 서보모터에 부착된 기구부하의 운동상태가 최적이 되도록 한 후 나아가서 스텝S7에서 최종목표위치 인가를 판별하여 최종위치가 아니면 상기 스텝S1로 복귀되고, 최종위치이면 원호보간 수행을 종료한다.The proportional integral (PID) and the feedforward (FEEDFORWARD) gain of each axis are optimized so that the motion state of the mechanical load attached to the servo motor is optimized. Then, in step S7, it is determined whether the final target position is applied. If it is the last position, the circular interpolation is completed.

상술한 본 발명에 의하면 직각좌표의 X-Y평면에서 로보트의 손끝이 원호를 따라 움직이며 원호의 속도 분포가 직선 형태가 되어 원의 시작과 끝에서도 진동이 발생하지 않고 부드러운 동작이 가능하며, 이산시간상태 방정식에 의해 직선 가감속을 행하기 때문에 샘플링 시간을 줄일 수 있어 원호의 정도가 향상되는 장점이 있다.According to the present invention described above, the fingertips of the robot move along the arc in the XY plane of the rectangular coordinates, and the speed distribution of the arc becomes a straight line, so that smooth operation is possible without generating vibration even at the beginning and the end of the circle. Since linear acceleration / deceleration is performed by the equation, the sampling time can be reduced, and the degree of arc is improved.

Claims

A judging step of judging the position of the starting point, the intermediate point and the end point of the circular arc; a first coordinate calculation step of obtaining the X and Y coordinates of the circular arc when there is no acceleration / deceleration after reading the three points on the circular arc in the judgment step; The second coordinate calculation step of obtaining the X and Y coordinates on the arc when the acceleration and deceleration is performed by performing linear acceleration and deceleration on each axis of the X and Y coordinates obtained in the coordinate calculation step, and the coordinates obtained by the coordinate calculation step at any time for each sampling time. A target position calculation step of obtaining a target position of each axis for circular interpolation, a position deviation calculation step of obtaining positional deviations of X and Y axes after obtaining a current position value of each axis read through the target position calculation step, and Control step for controlling servomotor by performing proportional integral derivative feed forward control on the position deviation calculated in position deviation calculation step Robot circular interpolation method characterized in that.

According to claim 1, X, Y coordinates on the arc in the first coordinate calculation step,

A robotic circular interpolation method characterized by the above-described method, wherein K is a constant increased for each sampling time, a: Cos Δθ, and b: SinΔθ are respectively displayed.