KR20040060825A - Trajectory generation method and system for real-time response at motion control - Google Patents

Trajectory generation method and system for real-time response at motion control Download PDF

Info

Publication number
KR20040060825A
KR20040060825A KR1020030098264A KR20030098264A KR20040060825A KR 20040060825 A KR20040060825 A KR 20040060825A KR 1020030098264 A KR1020030098264 A KR 1020030098264A KR 20030098264 A KR20030098264 A KR 20030098264A KR 20040060825 A KR20040060825 A KR 20040060825A
Authority
KR
South Korea
Prior art keywords
velocity
acceleration
deceleration
real
initial
Prior art date
Application number
KR1020030098264A
Other languages
Korean (ko)
Other versions
KR100540588B1 (en
Inventor
하영열
한성호
우갑주
Original Assignee
삼성중공업 주식회사
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 삼성중공업 주식회사 filed Critical 삼성중공업 주식회사
Publication of KR20040060825A publication Critical patent/KR20040060825A/en
Application granted granted Critical
Publication of KR100540588B1 publication Critical patent/KR100540588B1/en

Links

Classifications

    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00Programme-controlled manipulators
    • B25J9/16Programme controls
    • B25J9/1656Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Abstract

PURPOSE: A trajectory generation method and a system for a real-time response at a motion control is provided to reflect the change of position or velocity commands within restrained time on moving a robot system. CONSTITUTION: When an initial velocity is zero, the velocity is divided into accelerated, uniform, and decelerated sections. When the initial velocity is not zero, the velocity is divided into blending, uniform, and decelerated sections. The accelerated and decelerated sections use a third polynomial expression and the uniform section uses a constant to determine a velocity profile. In the blending section, a velocity profile allowing the initial velocity to be zero is blended with the velocity profile. According to the determined trajectory, the changed position and velocity are reflected within restrained time at the motion of a robot.

Description

모션 제어의 실시간 응답을 위한 궤적발생방법 및 시스템{Trajectory generation method and system for real-time response at motion control}Trajectory generation method and system for real-time response at motion control

본 발명은 로봇제어시스템 등에서의 모션 제어에 관한 것으로, 보다 상세하게는, 위치 브랜딩(blending) 방법을 통해 이동 중 실시간으로 변경된 위치 또는 속도에 실시간 응답이 가능한 궤적을 발생하여 모션 제어에 반영시켜 외부 사건에 실시간으로 응답이 가능토록 한 모션 제어의 실시간 응답을 위한 궤적발생방법 및 시스템에 관한 것이다.The present invention relates to motion control in a robot control system and the like, and more specifically, to generate a trajectory capable of real-time response to a changed position or speed in real time during movement through a position branding method and reflecting the motion to an external control. The present invention relates to a trajectory generation method and system for real-time response of motion control to enable real-time response to an event.

최근 산업현장에는 다양한 종류의 로봇이 사용되고 있으며, 이들 중에는 철판 등의 각종 재료를 용접하는 용접용 로봇이 있다. 이러한 산업용 로봇 시스템은 로봇의 모션(motion)제어를 통해 이동경로를 변경한다. 모션 제어에서 위치 이동은 입력된 목표거리, 목표 속도 이외에 액츄에이터와 로봇의 기계적 한계에 따른 가속도, 감속도, 최고속도의 구속 조건으로 이동시간이 결정된다. 이동 중 속도나 위치의 변경을 하기 위한 기존의 방법으로는 고차 다항식을 이용하는 방법과 속도 브랜딩 방법이 있다. 고차 다항식을 이용하는 방법은 변경하고자 하는 시점에 다항식의 계수를 구하고, 구해진 계수로서 샘플링 시간마다 다항식의 해를 이용하여 이동해야 할 목표점을 구하는 방법이다. 이 방법은 다항식의 계수를 구함에 있어 복잡하고, 많은 계산량을 가진다는 단점이 있다. 속도 브랜딩을 이용한 방법은 계수가 정하여진 다항식을 이용하여, 감속과 가속을 합하는 형태의 방법인데, 이것의위치 브랜딩 적용에 있어 목표위치를 지정할 수 없다.Recently, various kinds of robots are used in industrial sites, and among them, there are welding robots for welding various materials such as steel plates. This industrial robot system changes the movement path through the motion control of the robot. In motion control, the position movement is determined by the constraints of acceleration, deceleration, and maximum velocity depending on the mechanical limits of the actuator and robot, in addition to the input target distance and target velocity. Conventional methods for changing the speed or position during movement include the use of higher order polynomials and the speed branding method. The method of using the higher order polynomial is a method of obtaining a coefficient of the polynomial at a time point to be changed, and finding a target point to be moved by using a solution of the polynomial for each sampling time as the obtained coefficient. This method is complicated in obtaining coefficients of polynomials and has a large amount of calculation. Velocity branding is a method in which deceleration and acceleration are combined using a polynomial having a predetermined coefficient. However, the target position cannot be specified in the application of position branding.

그런데, 다양한 센서로부터 즉각적인 반응을 보여야 하는 로봇시스템 등에서 모션 제어시 실시간 응답이 중요한 의미를 가진다. 위치 이동에 따른 모션 제어의 실시간 응답을 위해 종래에는 초기 속도와 가속도가 "0"이 아닌 경우의 속도 프로파일을 만드는 방법이 제시되었다.However, the real-time response in motion control has a significant meaning in robot systems and the like that must show immediate response from various sensors. For real-time response of motion control according to position movement, a method of creating a velocity profile when the initial velocity and acceleration are not "0" has been proposed.

이는 초기위치에서 최종위치로의 이동에 있어 초기 속도와 가속도, 최종 속도와 가속도를 가지기 위해 리프트-오프(lift-off)위치와 셋-다운(set-down)위치를 기준으로 3개의 구간으로 나눈다. 이들 구간 사이의 연속적인 속도, 가속도를 가지는 방법은 4-3-4궤적방식과 5개의 큐빅스플라인(cubic spline)을 이용하는 방식이 있다. 이들 방법은 초기 속도와 가속도, 최종 속도와 가속도가 "0"이 아닌 경우의 속도 프로파일 생성 때문에 고차 다항식 처리에 있어 비교적 많은 계산량을 가진다.It is divided into three sections based on lift-off position and set-down position to have initial velocity and acceleration, final velocity and acceleration in moving from initial position to final position. . There are 4-3-4 trajectories and 5 cubic splines using continuous velocity and acceleration between these sections. These methods have a relatively large amount of computation for higher order polynomial processing because of the velocity profile generation when the initial velocity and acceleration and the final velocity and acceleration are not "0".

물론, 기존에도 실시간 응답을 위한 궤적 발생에 대한 연구가 몇가지 있다. 그 일예로, Volpe의 "Task Space Velocity Blending for Real-Time Trajectory Generation" IEEE International Conference on Robotics and Automation, May 2-6 1993, Atlanta Georgia.와 Loyd의 "Trajectory Generation for Sensor-Driven and Time-Varying Tasks", International Journal of Robotics, August, 1993. 두 문헌이 있다. 이들 문헌에 언급된 방법은 초기 가속도가 "0"이 아닐 경우에는 적용할 수 없으며, 작업 공간에서 목표 위치 변경에 따른 궤적 발생에 대해서는 기재되어있지 않다.Of course, there are several studies on trajectory generation for real time response. For example, Volpe's "Task Space Velocity Blending for Real-Time Trajectory Generation" IEEE International Conference on Robotics and Automation, May 2-6 1993, Atlanta Georgia. And Loyd's "Trajectory Generation for Sensor-Driven and Time-Varying Tasks". ", International Journal of Robotics, August, 1993. Two documents. The methods mentioned in these documents are not applicable when the initial acceleration is not "0", and no description is made about the generation of the trajectory according to the change of the target position in the working space.

따라서, 본 발명의 목적은 기존의 다항식 방법보다 간단한 위치 브랜딩 방법을 이용하여 목표위치 또는 목표 속도 변경에 실시간 응답이 가능한 궤적 발생 알고리즘을 통해 이동 중 실시간으로 변경된 위치 또는 속도의 변경을 모션 제어에 반영할 수 있도록 하며 초기 가속도가 "0"이 아닐 경우에도 적용할 수 있는 모션 제어의 실시간 응답을 위한 궤적발생방법 및 시스템을 제공함에 있다.Accordingly, an object of the present invention is to reflect the change of the position or velocity changed in real time during motion through the trajectory generation algorithm that enables a real-time response to the target position or target velocity change using a simple position branding method than the conventional polynomial method. The present invention provides a trajectory generation method and system for real-time response of motion control that can be applied even when the initial acceleration is not "0".

도 1은 본 발명에 따른 선형/부드러운 방식의 속도 프로파일을 나타낸 그래프를 도시한 도면.1 shows a graph showing a velocity profile in a linear / smooth manner in accordance with the present invention.

도 2는 초기 가속도(a0)와 속도(v0)에 따른 속도 프로파일을 나타낸 그래프를 도시한 도면.2 is a graph showing a velocity profile according to initial acceleration a 0 and velocity v 0 .

도 3은 본 발명에 따른 모션제어 실시간응답을 위한 궤적발생시스템을 나타내는 시스템 구성도.3 is a system configuration diagram showing a trajectory generation system for a motion control real-time response according to the present invention.

도 4는 본 발명에 따른 도 3 시스템의 실시간 모션 제어 처리 모델을 보여주는 상태도.4 is a state diagram showing a real-time motion control processing model of the system of FIG. 3 in accordance with the present invention.

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

100 : 상위제어기(CPU) 200 : 외부하드웨어100: upper controller (CPU) 200: external hardware

10 : 미들웨어 20 : 모션제어기10: middleware 20: motion controller

21 : 커맨드디코더 22 : 궤적발생부21: command decoder 22: trajectory generation unit

23 : 역운동학계산부 25 : 인터페이스제어부23: inverse kinematics calculation unit 25: interface control unit

26 : 운동학계산부 30 : 다축위치제어부26: kinematic calculation unit 30: multi-axis position control unit

40 : 모터구동부 50 : 모터40: motor drive unit 50: motor

이와 같은 목적을 달성하기 위한 본 발명에 따른 모션 제어의 실시간 응답을 위한 궤적발생방법은, 소정의 작업시작점과 작업종료점 사이의 작업경로상에서 작업을 수행하는 로봇의 이동경로를 실시간을 변경하기 위한 방법에 있어서, (1) 초기 속도가 "0"인 경우 가속, 등속 및 감속의 세구간으로 나누고, 초기 속도가 "0"이 아닌 경우 브랜딩, 등속 및 감속의 세구간으로 나누어 구간을 설정하는 단계와, (2) 상기 설정된 세구간중 가속구간과 감속구간에서는 3차 다항식을 사용하고, 등속구간에서는 상수 속도를 사용하여 최종 위치로의 이동을 위한 속도 프로파일을 구하는 단계와, (3) 상기 설정된 세구간중 브랜딩구간에서는 주어진 변화시간 동안에 초기 속도에서 속도를 "0"으로 만드는 속도 프로파일과 상기 단계 (2)에서 구해진 속도 프로파일을 브랜딩하여 속도 프로파일을 구하는 단계와, (4) 상기 구해진 궤적에 따라 이동중 제한된 시간 내에 위치 및 속도 변경을 실시간 반영하여 모션 제어하는 단계를 포함한다.Trajectory generation method for the real-time response of the motion control according to the present invention for achieving the above object, a method for changing the real-time movement path of the robot performing a task on the work path between the predetermined work start point and the work end point (1) setting the section by dividing into three sections of acceleration, constant velocity and deceleration when the initial velocity is "0", and dividing into three sections of branding, constant velocity and deceleration if the initial velocity is not "0"; (2) obtaining a velocity profile for moving to the final position using a third order polynomial in the acceleration section and the deceleration section of the set three sections, and using a constant speed in the constant velocity section, and (3) In the branding section of the section, the velocity profile that makes the velocity "0" at the initial velocity for a given change time and the velocity profile obtained in step (2) And a step of motion control by reflecting the real-time position and velocity changes within a limited time during movement along the trajectory and obtaining a velocity profile, obtained above (4).

본 발명의 목적을 달성하기 위한 모션 제어의 실시간 응답을 위한 시스템은, 로봇의 작업수행중 이동경로를 실시간으로 변경할 수 있는 모션제어 시스템에 있어서, 로봇 조작자로부터의 로봇 제어명령을 입력받아 전달해주는 미들웨어와, 로봇의 모션제어를 수행하는 모션제어기를 구비하며, 초기 유휴(DORMANT)상태에서 모션제어 인에이블/디스에이블 및 이동의 사건이 발생하면 토큰이 발생하여 해당 작업을 수행하도록 제어하는 상위제어기(CPU), 및 상기 상위제어기의 제어에 따라 축들 위치 제어를 수행하는 다축위치제어부를 구비하는 외부하드웨어를 포함하며, 상기 모션제어기는 미들웨어와의 인터페이스를 수행하여 명령이 전달되면 해독 작업을 수행하여 축 단위 이동인지, 월드 좌표계에 대한 이동인지를 구별하는 커맨드디코더와, 상기 외부하드웨어에서 인터럽트 사건이 발생하면 상기 커맨드디코더에서 구별된 명령을 사전 처리하여 로봇의 가속, 감속 및 등속에 관한 속도 프로파일을 생성하여 궤적을 계산하는 궤적발생부와, 로봇의 다수개의 축값을 계산하는 역운동학계산부와, 축값으로부터 월드좌표값을 계산하는 운동학계산부, 및 다축위치제어부와의 인터페이스를 수행하여 목표위치값을 전달하는 인터페이스제어부를 구비한다.The system for real-time response of motion control to achieve the object of the present invention, in the motion control system that can change the movement path in real time during the operation of the robot, the middleware for receiving and transmitting a robot control command from the robot operator And a controller configured to perform motion control of the robot, and an upper controller configured to perform a task by generating a token when an event of motion control enable / disable and movement occurs in an initial idle state. CPU) and an external hardware having a multi-axis position control unit for controlling the position of the axes according to the control of the upper controller, wherein the motion controller performs an interface with the middleware to perform a decryption operation when a command is transmitted. A command decoder for distinguishing between unit movement or movement with respect to the world coordinate system; When an interrupt event occurs in the hardware, the command decoder preprocesses the distinguished command to generate a speed profile for acceleration, deceleration, and constant velocity of the robot, and calculates a trajectory, and calculates a plurality of axes of the robot. It includes a kinematics calculator, a kinematics calculator for calculating the world coordinate value from the axis value, and an interface controller for performing the interface with the multi-axis position controller to transfer the target position value.

이하, 첨부한 도면들을 참조하여 본 발명의 바람직한 실시예를 상세히 기술하기로 한다.Hereinafter, exemplary embodiments of the present invention will be described in detail with reference to the accompanying drawings.

우선, 위치 이동에 따른 모션 제어의 실시간 응답이 가능하게 하기 위해서는 다음의 세가지 조건을 만족해야 한다.First, in order to enable real-time response of motion control according to position movement, the following three conditions must be satisfied.

첫째, 사건(event)이 발생한 임의의 순간의 위치, 속도, 가속도의 정보를 알아야 한다. 즉, 사건이 발생한 순간의 위치, 속도, 가속도를 알아야 하는 이유는 부드러운(smooth) 이동을 위해 초기 위치에서의 연속적인 속도와 가속도를 가지는 속도 프로파일을 만들기 위해 필요한 정보이기 때문이다.First, we need to know the position, velocity, and acceleration information at any moment when an event occurs. That is, the reason for knowing the position, velocity, and acceleration at the moment of occurrence of the event is that it is necessary information to create a velocity profile having continuous velocity and acceleration at the initial position for smooth movement.

둘째, 초기속도가 "0"이 아닌 경우의 주어진 목표 위치 또는 목표 속도에 따른 속도 프로파일을 만들 수 있어야 한다.Second, it should be possible to make a velocity profile according to a given target position or target velocity when the initial velocity is not "0".

셋째, 궤적 발생 알고리즘을 실시간으로 처리할 수 있는 제어방법이 있어야 한다.Third, there must be a control method that can process the trajectory generation algorithm in real time.

본 발명은 초기 속도와 가속도가 주어지고, 최종 속도와 가속도가 "0"인 경우의 속도 프로파일을 생성하기 위해 초기 속도와 가속도에 따라 비교적 적은 계산량을 가지는 다항식을 선택한다. 즉, 초기 속도와 가속도가 "0"일 경우와 초기 속도와 가속도가 "0"이 아닌 경우로 나누어 처리한다. 초기 속도와 가속도가 "0"일 경우의 속도 프로파일은 가속구간, 등속구간, 감속구간으로 나누어 처리하며, 가속구간과 감속구간에서는 3차 다항식으로 속도 프로파일을 구하고, 등속구간에서는 상수속도를 이용하여 속도 파형을 구한다.The present invention selects a polynomial having a relatively small amount of calculation according to the initial velocity and acceleration to generate a velocity profile when the initial velocity and acceleration are given and the final velocity and acceleration are "0". That is, the process is divided into the case where the initial velocity and the acceleration are "0" and the case where the initial velocity and the acceleration are not "0". When the initial velocity and acceleration are "0", the velocity profile is divided into acceleration section, constant velocity section and deceleration section. The velocity profile is calculated by 3rd order polynomial in acceleration section and deceleration section, and constant velocity is used in constant velocity section. Find the velocity waveform.

초기 속도와 가속도가 "0"이 아닐 경우에는 브랜딩구간, 등속구간, 감속구간으로 나누어 처리하며, 브랜딩구간에서는 주어진 변화시간 동안에 초기속도에서 속도를 "0"으로 만드는 속도 프로파일과 최종위치로의 이동을 위한 가속구간 속도프로파일을 브랜딩한다. 초기속도에서 속도를 "0"으로 만드는 속도프로파일 생성방법은 4차 또는 5차 다항식이 사용된다. 최종 위치로의 이동을 위한 속도프로파일 생성 방법은 가속구간과 감속구간에서는 3차 다항식이 사용되고, 등속구간에는 상수속도를 이용한다. 따라서, 초기속도가 "0"이 아닐 경우에는 브랜딩구간과 가속구간이 비대칭적인 형태를 지닌다.If the initial speed and acceleration are not "0", the process is divided into a branding section, a constant speed section and a deceleration section. In the branding section, the velocity profile makes the speed "0" at the initial speed and moves to the final position for a given change time. Brand the velocity profile for the acceleration section. The velocity profile generation method that sets velocity to "0" at the initial velocity uses the 4th or 5th order polynomial. In the velocity profile generation method for moving to the final position, the third order polynomial is used in the acceleration section and the deceleration section, and the constant velocity is used in the constant velocity section. Therefore, when the initial speed is not "0", the branding section and the acceleration section have an asymmetrical form.

먼저, 축 공간에 있어 궤적을 변경하고자 하는 초기속도가 "0"인 경우와 그렇지 않은 경우로 나누어 축 좌표 괘적 발생에 대해 설명한다.First, the generation of the axis coordinate trajectory will be described by dividing the initial velocity to change the trajectory in the axis space into " 0 " and the other case.

초기속도가 "0"인 경우의 속도 파형 생성 방법은 다음과 같다.The velocity waveform generation method when the initial velocity is "0" is as follows.

이동거리(So), 가속도의 크기(aa), 감속도의 크기(ad), 목표속도의 크기(Vd)에 따른 연속적인 속도를 제공하기 위한 궤적(Sn(t)) 생성 방법은 다음의 수학식 1과 같다.The method of generating the trajectory (S n (t)) for providing continuous velocity according to the moving distance (So), the magnitude of the acceleration (aa), the magnitude of the deceleration (ad), and the magnitude of the target velocity (Vd) is as follows. Equation 1

여기서, facc(s)는 가속의 형태를 결정하는 정규화된(normalized) 특성함수이고, fdec(s)는 감속의 형태를 결정하는 정규화된 특성함수이다. s는 가속시간, 감속시간에 대한 정규화된 시간파라메터로 s ∈ [0.1] 이다. Sa, Su, Sd는 가속구간, 등속구간, 감속구간에서 움직이는 거리이고, ta, tu, td는 가속, 등속, 감속이 끝나는 시간을 나타낸다.Here, f acc (s) is a normalized characteristic function for determining the form of acceleration, and f dec (s) is a normalized characteristic function for determining the form of deceleration. s is a normalized time parameter for acceleration time and deceleration time s ∈ [0.1]. Sa, Su, and Sd are the moving distances in the acceleration section, the constant speed section, and the deceleration section, and ta, tu, and td represent the time at which the acceleration, constant speed, and deceleration ends.

본 실시예에서, 가속구간과 감속구간에서 움직이는 거리(Sa,Sd)는 가속이 끝나는 시간(ta)과 감속이 끝나는 시간(td)에서 등속이 끝나는 시간(tu)을 뺀 시간(td-tu)을 등속으로 움직이는 거리(Su)의 1/2인 특성을 가지는 선형적인 방식과 부드러운방식을 사용한다.In the present embodiment, the moving distances Sa and Sd in the acceleration section and the deceleration section are the time td-tu minus the time ta of the acceleration and the time td of the deceleration ending (tu). The linear method and the smooth method are used to have the characteristic that is 1/2 of the distance (Su) moving at constant speed.

주어진 이동거리(So), 가속도 크기(aa), 목표속도 크기(Vd), 감속도 크기(ad)로 가능한 최고속도(Vm)을 하기 수학식 2처럼 구한다.Given the moving distance So, the acceleration magnitude (aa), the target velocity magnitude (Vd), and the deceleration magnitude (ad), the maximum possible velocity Vm is obtained as in Equation 2 below.

위의 수학식 2에 의해 구해진 최고속도(Vm)를 이용하여 가속구간, 등속구간, 감속구간에서의 움직이는 거리(Sa,Su,Sd) 및 끝나는 시간(ta,tu,td)를 알 수 있다. 이를 수학식 3에 정의한다.Using the maximum speed (Vm) obtained by the above equation (2) it can be seen the moving distance (Sa, Su, Sud) and the ending time (ta, tu, td) in the acceleration section, constant velocity section, deceleration section. This is defined in equation (3).

여기서, d는 모션 제어에서 이동거리(So)의 부호(sign)와 관계없이 목표속도(Vd)를 입력받기 위해 최고속도(Vm)의 부호를 결정하는 값으로, 다음 수학식 4로 정의된다.Here, d is a value for determining the sign of the maximum speed (Vm) in order to receive the target speed (Vd) regardless of the sign of the moving distance (So) in the motion control, it is defined by the following equation (4).

위치 궤적 발생에 있어 위의 속도파형생성방법을 적용하기 위해서는 가속의 형태와 감속의 형태를 결정하는 특성함수(facc(s), fdec(s))는 다음 조건 1을 만족해야 한다.In order to apply the above velocity waveform generation method to the position trajectory, the characteristic functions f acc (s) and f dec (s) that determine the form of acceleration and deceleration must satisfy the following condition 1.

조건 1Condition 1

가속과 감속구간의 시작인 경우, facc(0) = 0, fdec(0) = 0을 만족한다.For the start of the acceleration and deceleration sections, f acc (0) = 0 and f dec (0) = 0 are satisfied.

가속과 감속구간의 끝인 경우, facc(1) = 1, fdec(1) = 1을 만족한다.At the end of the acceleration and deceleration section, f acc (1) = 1 and f dec (1) = 1 are satisfied.

위의 조건 1을 만족하는 최소 차수 함수는 아래와 같은 표 1의 선형적인 속도특성을 가지는 특성함수이다.The minimum order function that satisfies condition 1 above is a characteristic function having the linear velocity characteristics shown in Table 1 below.

속도특성선형방식부드러운방식가속감속Speed characteristics Linear method Smooth method Acceleration and deceleration

부드러운 가감속 형태를 가지기 위해서는 아래 조건 2도 만족해야 한다.In order to have a smooth acceleration / deceleration pattern, the following condition 2 must also be satisfied.

조건 2Condition 2

가속구간과 감속구간의 시작인 경우,f''acc(0)=0, f''dec(0)=0을 만족해야 한다.For the start of the acceleration and deceleration sections, f '' acc (0) = 0 and f''dec (0) = 0 must be satisfied.

가속구간과 감속구간의 끝인 경우,f''acc(1)=0와 f''dec(1)=0을 만족해야 한다.At the end of the acceleration and deceleration sections, f '' acc (1) = 0 and f''dec (1) = 0 must be satisfied.

가속구간과 감속구간 내인 경우, facc(s)와 fdec(s)는 s∈[0,1]에서 연속적인 함수이어야 한다.In the acceleration and deceleration sections, facc (s) and fdec (s) must be continuous functions at s∈ [0,1].

이러한 조건들을 만족하는 최소 차수 함수는 표 1의 3차 다항식 속도 특성을 가지는 부드러운 특성 함수이다. 실시간 응답을 가지는 속도 파형을 만들기 위해서는 임의의 순간의 위치, 속도, 가속도 정보를 알아야 한다. 앞서 서술한 속도 프로파일 생성 방법에서의 임의의 순간의 속도(v(t))와 가속도(a(t))는 다음의 수학식 5 및 6과 같이 구할 수 있다.The minimum order function that satisfies these conditions is a smooth characteristic function with the third order polynomial velocity characteristic of Table 1. To create a velocity waveform with a real-time response, you need to know the position, velocity, and acceleration information at any moment. The instantaneous velocity v (t) and acceleration a (t) in the above-described velocity profile generation method can be obtained as in Equations 5 and 6 below.

이러한 속도특성에 따른 속도 프로파일을 도 1에 도시하였다.The speed profile according to these speed characteristics is shown in FIG.

도 1은 본 발명에 따른 선형/부드러운 방식의 속도 프로파일을 나타내는 그래프를 도시한 도면이다. 도 1의 가로축은 시간(t)이고, 세로축은 속도(v(t))를 나타낸다.1 is a graph showing a velocity profile in a linear / smooth manner according to the present invention. 1, the horizontal axis represents time t, and the vertical axis represents speed v (t).

도 1의 (a) 및 (c)는 선형(linear)방식을 사용했을 경우의속도프로파일이고, 도 1의 (b) 및 (d)는 부드러운(smooth)방식을 사용했을 경우의 속도프로파일이다. 그리고, 도 1의 (a) 및 (b)는 최고속도(Vm)와 목표속도(Vd)가 같지 않을 경우(Vm ≠Vd)의 속도프로파일이고, 도 1의 (c) 및 (d)는 최고속도(Vm)와 목표속도(Vd)가 같을 경우(Vm = Vd)의 속도프로파일이다.(A) and (c) of FIG. 1 are velocity profiles when a linear method is used, and FIG. 1 (b) and (d) are velocity profiles when a smooth method is used. And (a) and (b) of Figure 1 is the velocity profile of the maximum speed (Vm) and the target speed (Vd) is not the same (Vm ≠ Vd), Figures (c) and (d) of Figure 1 is the highest This is the speed profile when the speed Vm and the target speed Vd are the same (Vm = Vd).

다음으로, 초기속도가 "0"아닌 경우의 속도파형생성방법을 설명한다.Next, the velocity waveform generation method when the initial velocity is not "0" will be described.

초기 속도와 가속도가 "0"이 아닐 경우에는 브랜딩구간, 등속구간, 감속구간으로 나눈다. 브랜딩구간에서는 주어진 변화시간(tb)동안에 초기속도(v0)에서 속도를 "0"으로 만드는 속도 프로파일과 주어진 이동거리 S0와 초기 속도에서 속도를 “0”으로 만들기 위해 움직인 거리 Sb의 차이(S0-Sb)를 움직이기 위한 가속구간 프로파일을 브랜딩한다. 주어진 전이시간(tb)동안에 초기속도(v0)에서 속도를 "0"으로 만드는 속도 프로파일은 다음과 같다.If the initial velocity and acceleration are not "0", it is divided into branding section, constant speed section and deceleration section. In the branding section, the difference between the velocity profile that makes the velocity "0" at the initial velocity v0 and the distance Sb that has been moved to make the velocity "0" at the initial velocity (S0) during the given change time (tb). -Brand the acceleration section profile to move Sb). The velocity profile that makes velocity zero at the initial velocity v0 for a given transition time tb is

초기 속도(v0)와 가속도(a0)의 곱(v0*a0)의 부호에 따라 사건 발생 순간이 이전 이동의 가속구간, 등속구간, 감속구간중 어떠한 시기인지 예측할 수 있다. 즉, 초기속도와 가속도의 곱(v0*a0)이 "0" 보다 큰 경우(v0*a0>0)는 이전 이동의 가속구간에 사건이 발생한 것이고, "0" 보다 작은 경우(v0*a0<0)는 이전 이동의 감속구간에 사건이 발생한 것이고, 초기 가속도(a0)가 "0"일 경우는 등속구간에서 사건이 발생한 경우이다.According to the sign of the product of the initial velocity v0 and the acceleration a0 (v0 * a0), it is possible to predict whether the moment of occurrence of the event is the acceleration section, the constant velocity section, or the deceleration section of the previous movement. That is, if the product of the initial velocity and the acceleration (v0 * a0) is greater than "0" (v0 * a0> 0), the event occurred in the acceleration section of the previous movement, and less than "0" (v0 * a0 < 0) means that the event occurred in the deceleration section of the previous movement, and when the initial acceleration a0 is "0", the event occurred in the constant speed section.

초기 가속도(a0)가 "0"인 경우 즉, 이전 이동의 등속구간에서 사건이 발생한 경우의 브랜딩 특성 함수는 전술한 표 1의 부드러운방식의 감속 특성함수이다. 그리고, 이 경우 이동거리(Sb)는 전이시간(tb)과 초기속도(v0)의 곱(tb*v0)을 1/2로나누어 얻어진다. 여기서, 초기 가속도(a0)가 "0"이 아닌 경우 브랜딩 특성함수는 하기 조건 3에 의해 결정된다.The branding characteristic function when the initial acceleration a0 is "0", that is, when an event occurs in the constant velocity section of the previous movement, is the deceleration characteristic function of the smooth method of Table 1 described above. In this case, the moving distance Sb is obtained by dividing the product (tb * v0) of the transition time tb and the initial speed v0 by 1/2. Here, when the initial acceleration a0 is not "0", the branding characteristic function is determined by the following condition 3.

조건 3Condition 3

브랜딩구간의 시작인 경우, fblend(0)=0, f'blend(0)=tb 2ao/Sb, f''blend(0)=tbvo/Sb F blend (0) = 0, f ' blend (0) = t b 2 a o / S b , f'' blend (0) = t b v o / S b

브랜딩구간의 끝인 경우, fblend(1)=1, f'blend(1)=0, f''blend(1)=0Fblend (1) = 1, f'blend (1) = 0, f''blend (1) = 0

여기서는, 이전 이동의 감속구간일때와 가속구간일 경우에 적용함수를 달리한다.Here, the application function is different in the case of the deceleration section and the acceleration section of the previous movement.

이전 이동의 감속구간과 가속구간에 따른 브랜딩 특성함수(fblend)는 하기 표 2와 같다.The branding characteristic function (fblend) according to the deceleration section and the acceleration section of the previous movement is shown in Table 2 below.

브랜딩 특성함수(fblend)의 파라메터(α, β, γ, δ, Sb)는 위의 조건 3 및 표 2와 같이 구해진다.The parameters α, β, γ, δ, and Sb of the branding characteristic function fblend are obtained as shown in Condition 3 and Table 2 above.

브랜딩 특성함수(fblend(s))로 초기 가속도(a0)와 속도(v0)를 "0"으로 만드는 위치 괘적(Sbb(t)) 생성방법은 다음의 수학식 7로 정의된다.A method of generating a position rule Sbb (t) which makes the initial acceleration a0 and the velocity v0 "0" by the branding characteristic function fblend (s) is defined by Equation 7 below.

감속구간에서 움직이는 거리(Sd)를 위한 속도 프로파일 생성방법은 다음과 같이 변경한다.The speed profile generation method for the moving distance Sd in the deceleration section is changed as follows.

주어진 이동거리 S0와 Sb의 차이(S0-Sb)를 움직이기 위한 속도 프로파일 생성방법 은 가속시간이 전이시간(tb)으로 지정되었다는 것 이외에는 초기속도가 "0"인 경우의 속도 프로파일 생성방법과 동일하다. 즉, 상기 수학식 3의 가속구간이 끝나는 시간(ta) 이 전이시간(tb)이므로, 최고속도(Vm)는 가속도 크기(aa)에 관계없이 수학식 2 대신 다음의 수학식 8로 결정된다.The velocity profile generation method for moving the difference (S0-Sb) between the given moving distances S0 and Sb is the same as the velocity profile generation method when the initial velocity is "0" except that the acceleration time is designated as the transition time (tb). Do. That is, since the time ta at which the acceleration section of Equation 3 ends is the transition time tb, the maximum speed Vm is determined by Equation 8 instead of Equation 2 regardless of the acceleration magnitude aa.

여기서,이다.here, to be.

초기 속도(v0)와 가속도(a0)를 "0"으로 만드는 속도 프로파일과 주어진 이동거리 S0와 초기 속도에서 속도를 “0”으로 만들기 위해 움직인 거리 Sb의 차이(S0-Sb)를 움직이기 위한 속도 프로파일을 브랜딩하여 얻은 실시간 응답 가능한 궤적(S(t)) 발생 방법은 다음의 수학식 9와 같다.For moving the velocity profile that makes initial velocity (v0) and acceleration (a0) "0" and the difference (S0-Sb) between the given movement distance S0 and the distance Sb traveled to make velocity "0" at the initial velocity. A method of generating a real-time responsive trajectory S (t) obtained by branding a velocity profile is shown in Equation 9 below.

S(t) = Sn(t) + Sbb(t)S (t) = S n (t) + S bb (t)

초기 속도(v0)와 가속도(a0)에 따른 속도 프로파일을 도 2에 도시하였다.The velocity profile according to the initial velocity v0 and the acceleration a 0 is shown in FIG. 2.

도 2는 위의 속도 프로파일 생성방법에서의 초기 속도(v0)와 가속도(a0)에 따른 속도 프로파일을 보여주는 그래프로, 가로축은 시간(t)을 나타내고 세로축은 속도(v(t))를 나타낸다. 도 2의 그래프상에 점선(dotted line)은 초기속도(v0)에서 속도를 "0"으로 만드는 속도 프로파일을 표시하고, 선과 점으로 표시된 것은 주어진 이동거리 S0와 Sb의 차이(S0-Sb)를 움직이기 위한 속도 프로파일이고,2 is a graph showing a velocity profile according to an initial velocity v 0 and an acceleration a 0 in the velocity profile generation method, in which the horizontal axis represents time t and the vertical axis represents velocity v (t). Indicates. The dotted line on the graph of FIG. 2 indicates the velocity profile that makes the velocity "0" at the initial velocity v 0 , and the dotted line and dot indicate the difference between the given moving distances S0 and Sb (S0-Sb). Is a velocity profile for moving

선으로 표시된 그래프는 브랜딩된 속도 프로파일이다.The graph represented by the line is a branded velocity profile.

도 2의 (a) 및 (b)는 초기가속도(a0)가 "0" 보다 큰 경우(a0 > 0)인, 이전 이동의 가속구간에서 위치와 속도가 변한 속도 프로파일이다. 도 2의 (c) 및 (d)는 초기가속도(a0)가 "0" 보다 작은 경우(a0 < 0)인, 이전 이동의 감속구간에서 위치와 속도가 변한 속도 프로파일이다. 도 2의 (a) 및 (c)는 초기속도(v0)와 최고속도(Vm)의 곱이 "0" 보다 큰 경우(v0Vm > 0)인, 초기속도(v0)와 최고속도(Vm)의 방향이 같을 경우의 속도 프로파일이다. 도 2의 (b) 및 (d)는 초기속도(v0)와 최고속도(Vm)의 곱이 "0" 보다 작은 경우(v0Vm < 0)인, 초기속도(v0)와 최고속도(Vm)의 방향이 같지 않을 경우의 속도 프로파일이다.(A) and (b) of FIG. 2 are velocity profiles whose position and velocity have changed in the acceleration section of the previous movement, where the initial acceleration a0 is greater than "0" (a0> 0). (C) and (d) of FIG. 2 are speed profiles in which the position and the speed are changed in the deceleration section of the previous movement, when the initial acceleration a0 is smaller than "0" (a0 <0). 2 (a) and 2 (c) show directions of initial velocity v0 and maximum velocity Vm, where the product of initial velocity v0 and maximum velocity Vm is greater than " 0 " (v0Vm > 0). This is the velocity profile in this case. 2 (b) and (d) show directions of initial velocity v0 and maximum velocity Vm, where the product of initial velocity v0 and maximum velocity Vm is less than " 0 " (v0Vm < 0). This is the velocity profile when it is not the same.

이러한 속도파형을 생성하고 속도변화하는 위치궤적발생방법을 적용한 본 발명에 따른 모션 제어의 실시간 응답을 위한 시스템을 도 6에 도시하였다.FIG. 6 illustrates a system for real-time response of motion control according to the present invention applying the position trajectory generation method for generating the velocity waveform and changing the velocity.

도 3은 본 발명에 따른 모션제어 실시간응답을 위한 궤적발생시스템을 나타내는 시스템 구성도이다. 도 3을 참조하여 설명하면, 도 3에 나타낸 시스템은 크게 두개의 프로세서로 구성된다. 즉, 로봇시스템을 전체적으로 제어하는 상위제어기인 CPU(100)와 축들 위치 제어를 수행하는 외부하드웨어(external hardware)(200)로 구성된다. CPU(100)는 미들웨어(middleware)(10)와 모션제어기(20)를 구비한다. 미들웨어(10)는 클라이언트에서 서버에 있는 응용프로그램이나 자원(resource)을 불러 쓰기 위해 클라이언트와 서버 가운데 놓는 중간자로서, 여기서는 로봇 조작자와 모션제어기(20) 사이에 로봇 제어명령을 전달해주는 수단이다. 모션제어기(20)는 로봇 의존적인 계산을 수행하기 위해 커맨드디코더(command decoder)(21), 궤적발생부(trajectory generation)(22), 역운동학계산부(inverse kinematics calculation)(23), 인터페이스제어부(interface control)(24), 그리고 운동학계산부(forward kinematics calculation)(25)로 이루어진다. CPU(100)는 로봇이 멈추어 있는 경우에 다른 상위 프로그램을 실행할 수 있다.커맨드디코더(21)는 미들웨어(10)와의 인터페이스를 수행한다. 궤적발생부(22)는 로봇의 가속, 감속, 등속에 관한 계산을 하고, 역운동학계산부(23)는 로봇의 다수개의 축 값을 계산하며, 운동학계산부(25)는 축값으로부터 월드(world)좌표값을 계산하여 커맨드디코더(21)로 전달한다. 인터페이스제어부(24)는 축들 위치를 제어하는 외부하드웨어(200)와의 인터페이스를 수행한다. 외부하드웨어(200)는 모션제어기(20)로부터 전달되는 목표 위치값에 따라 로봇의 다수개의 축의 위치를 제어하는 다축위치제어부(30)와, 모터구동부(40)를 구비한다. 다축위치제어부(30)는 다수개의 축을 동기화하여 위치제어하거나 아니면 특정 축만을 위치제어할 수 있다. 모터구동부(40)는 다축위치제어부(30)의 제어에 의해 다수개의 축이 목표위치값만큼 제어될 수 있도록 모터(50)를 구동한다. 이러한 구성을 갖는 도 6 시스템에서 모션 제어를 위한 실시간 처리과정에 대해 도 7을 통해 구체적으로 설명한다.3 is a system configuration diagram showing a trajectory generation system for a motion control real-time response according to the present invention. Referring to FIG. 3, the system shown in FIG. 3 is largely composed of two processors. That is, it is composed of a CPU 100, which is an upper controller that controls the robot system as a whole, and external hardware 200 that performs axis position control. The CPU 100 includes middleware 10 and a motion controller 20. The middleware 10 is an intermediary placed between the client and the server in order to load an application program or resource on the server from the client, and is a means for transmitting a robot control command between the robot operator and the motion controller 20. The motion controller 20 may include a command decoder 21, a trajectory generation 22, an inverse kinematics calculation 23, and an interface controller to perform robot-dependent calculations. (interface control) 24, and forward kinematics calculation 25. The CPU 100 may execute another upper level program when the robot is stopped. The command decoder 21 performs an interface with the middleware 10. The trajectory generation unit 22 calculates the acceleration, deceleration, and constant velocity of the robot, and the inverse kinematic calculation unit 23 calculates a plurality of axis values of the robot, and the kinematic calculation unit 25 uses the world from the axis values. The coordinate value is calculated and transmitted to the command decoder 21. The interface controller 24 performs an interface with the external hardware 200 that controls the positions of the axes. The external hardware 200 includes a multi-axis position controller 30 and a motor driver 40 for controlling the positions of the plurality of axes of the robot according to the target position value transmitted from the motion controller 20. The multi-axis position controller 30 may control the position by synchronizing a plurality of axes or position control only a specific axis. The motor driver 40 drives the motor 50 so that a plurality of axes can be controlled by a target position value by the control of the multi-axis position controller 30. The real-time processing for motion control in the system of FIG. 6 having such a configuration will be described in detail with reference to FIG. 7.

도 4는 본 발명에 따른 도 3 시스템의 실시간 모션 제어 처리 모델을 보여주는 상태도이다. 도 4를 참조하여 보면, 기울림 글씨(이텔릭체)는 CPU(100)에서 발생하는 사건 또는 상태이고, 굵은 글씨는 다축위치제어부(30)에서 발생하는 사건 또는 상태이다. 바모양은 전이를 나타내며, CPU(100)에서 주어진 작업을 수행한다. 원모양은 CPU(100)와 다축위치제어부(30)의 상태를 나타낸다.4 is a state diagram showing a real time motion control processing model of the system of FIG. 3 in accordance with the present invention. Referring to FIG. 4, italic type (italic) is an event or state occurring in the CPU 100, and bold letters are an event or state occurring in the multi-axis position control unit 30. Bars represent transitions and perform tasks given by the CPU 100. The circle shows the state of the CPU 100 and the multi-axis position control unit 30.

도 4에서 보면, 초기 토큰은 유휴(DORMANT)상태에 있으며, 사건이 발생하게 되면 토큰이 발생한다. CPU(100)가 발생하는 사건은 모션제어 인에이블 및 디스에이블(MC_ENABLE,MC_DISABLE), 모션제어 이동(MC_MOVE)이고, CPU(100)의 상태는 사용가능한 상태이나 실제적인 작업이 없는 유휴(DORMANT), 준비(READY),대기(WAITING_UPDATE)상태가 있다. 전이가 발생하는 시기와 전이가 발생할 때 CPU(100)에서 처리하는 작업은 다음과 같다.In FIG. 4, the initial token is in a dormant state, and a token is generated when an event occurs. The events generated by the CPU 100 are motion control enable and disable (MC_ENABLE, MC_DISABLE), motion control move (MC_MOVE), and the state of the CPU 100 is either an available state or no idle operation. , READY and WAITING_UPDATE. When the transition occurs and when the transition occurs, the processing performed by the CPU 100 is as follows.

외부하드웨어(200)의 다축위치제어부(30)에서 발생하는 사건은 인터럽트(INTERRUPT)이고, 다축위치제어부(30)의 상태는 다축 위치 제어 처리(PROCESSING_JPC)상태이다. 모션제어 인에이블(MC_ENABLE) 사건은 모션제어기(20)를 사용하고자 할 경우 초기에 한번 발생시킨다. 유휴(DORMANT)상태에 토큰이 있을 경우 초기(INITIALIZE) 작업이 수행된다. 초기(INITIALIZE)작업을 마치면 CPU(100)의 준비(READY)상태가 되며, 다축위치제어부(30)는 다축 위치제어 처리(PROCESSING_JPC)상태가 된다. 초기(INITIALIZE)작업은 모션제어기(20) 내부 데이터 초기작업을 수행하고, 다축위치제어부(30)에게 시작한다는 신호를 보내는 것이다. 모션제어 디스에이블(MC_DISABLE) 사건은 모션제어기(20)를 더 이상 사용하지 않을 경우 발생시킨다. 준비(READY)상태에 토큰이 있고 다축위치제어부(30)에서 인터럽트(INTERRUPT)사건이 발생하면 최종(FINALIZE)작업을 수행한다. 최종(FINALIZE)작업은 모션제어기(20) 내부 데이타를 지우고, 다축위치제어부(30)에서 끝낸다는 신호를 보내는 것이다. 모션제어 이동(MC_MOVE) 사건은 목표위치 또는 목표속도가 바뀔 경우 발생한다. 준비(READY)상태에 토큰이 있을 경우 즉, 미들웨어(10)를 통해 로봇 조작자의 명령이 전달되면 모션제어기(20)내의 커맨드디코더(21)에서 명령_해독(COMMAND_DECODE)작업이 수행된다. 명령_해독(COMMAND_DECODE)작업은 축 단위 이동인지, 월드 좌표계에 대한 이동인지를 구별한다. CPU(100)가 대기_갱신(WAITING_UPDATE)상태에서 다축위치제어부(30)로부터 인터럽트(INTERRUPT) 사건이 발생하게 되면 이렇게 구별된 명령을 사전 처리(PRE_PROCESSING)상태에서 처리한다. 사전 처리(PRE_PROCESSING) 상태로의 전이에서 속도 프로파일을 생성하고 궤적을 구하는 데 필요한 파라메터들(Sa, Su. Sd, ta. tu 및 td)이 계산된다. 이들 파라메터에 대한 계산식은 앞서 서술하였다. 로봇이 움직일 경우, 외부 하드웨어(200)는 주기적으로 주어진 시간(Δt)마다 각 축들 위치(Pj(t))로 제어한다. 이때, 주기적으로 발생하는 사건이 인터럽트(INTERRUPT)이다. 인터럽트(INTERRUPT)사건이 발생하면 주기적인 처리(PERIODIC_PROCESSING) 작업을 수행하게 된다.An event occurring in the multi-axis position control unit 30 of the external hardware 200 is an interrupt (INTERRUPT), the state of the multi-axis position control unit 30 is a multi-axis position control processing (PROCESSING_JPC) state. The motion control enable (MC_ENABLE) event occurs once initially when the motion controller 20 is to be used. If there is a token in the DORMANT state, the initial operation is performed. Upon completion of the initial (INITIALIZE) operation, the CPU 100 is in a READY state, and the multi-axis position control unit 30 enters the multi-axis position control processing (PROCESSING_JPC) state. The initial (INITIALIZE) operation is to perform the initial operation of the internal data of the motion controller 20 and send a signal to start the multi-axis position control unit 30. The motion control disable (MC_DISABLE) event occurs when the motion controller 20 is no longer used. If there is a token in the READY state and an INTERRUPT event occurs in the multi-axis position control unit 30, the final (FINALIZE) operation is performed. The final (FINALIZE) operation is to delete the internal data of the motion controller 20 and send a signal that the multi-axis position controller 30 ends. The motion control movement (MC_MOVE) event occurs when the target position or target velocity changes. When there is a token in the READY state, that is, when a command of the robot operator is transmitted through the middleware 10, a command_decode (COMMAND_DECODE) operation is performed in the command decoder 21 in the motion controller 20. The COMMAND_DECODE operation distinguishes between movement in axis or relative to the world coordinate system. If an interrupt (INTERRUPT) event occurs from the multi-axis position control unit 30 in the WAITING_UPDATE state, the CPU 100 processes the distinguished command in the PRE_PROCESSING state. The parameters Sa, Su. Sd, ta. Tu and td required for generating the velocity profile and finding the trajectory in the transition to the PRE_PROCESSING state are calculated. The formula for these parameters has been described above. When the robot moves, the external hardware 200 periodically controls each axis position Pj (t) at a given time Δt. At this time, an event that occurs periodically is an interrupt (INTERRUPT). If an interrupt (INTERRUPT) event occurs, periodic processing (PERIODIC_PROCESSING) is performed.

이 작업은 초기 속도에 따라 Sn(t) 또는 S(t)를 계산하여 로봇 기구에 맞는 역기구학을 구하여 위치 제어기에 목적 위치를 전달한다.This task calculates Sn (t) or S (t) according to the initial velocity, obtains the inverse kinematics for the robot mechanism, and delivers the target position to the position controller.

이러한 방식으로 실시간 처리를 할 경우 최대 명령 수행 지연시간은 인터럽트 발생 주기이다.In the real time processing in this way, the maximum instruction execution delay time is the interrupt generation cycle.

이처럼, 모션제어기(20)가 실시간 응답을 가능하게 하기 위해 초기 속도와 가속도가 "0"이 아닌 경우의 궤적발생법은 기존의 4-3-4법과 5 cubic spline방법에 비해 작은 계산량을 가지고 있어 실시간 처리에 용이한 형태이며, 초기 가속도가 "0"이 아닌 경우에도 가속도 연속적인 속도 프로파일 생성이 가능하여 부드러운 이동이 가능하다. 따라서, 본 실시예에서는 궤적발생알고리즘을 직교좌표계의 직선운동에 적용하여 위치변경과 속도변경에 실시간 응답이 가능하다는 것을 보여준다.As such, the trajectory generation method when the initial velocity and acceleration are not "0" in order to enable the motion controller 20 to have a real-time response has a smaller calculation amount than the conventional 4-3-4 and 5 cubic spline methods. It is an easy form for real-time processing, and even if the initial acceleration is not "0", it is possible to generate a continuous velocity profile of the acceleration, thereby enabling smooth movement. Therefore, the present embodiment shows that the trajectory generation algorithm is applied to the linear motion of the Cartesian coordinate system to enable real-time response to position change and speed change.

상술한 바와 같이, 본 발명에 따른 모션 제어의 실시간 응답을 위한 궤적발생방법 및 시스템은, 센서와 같은 외부 사건에 실시간으로 응답하여야 하는 로봇시스템에서 이동중 제한된 시간내에 속도 또는 위치 명령의 변경을 반영할 수 있으며, 기존보다 적은 계산량으로 비교적 느린 CPU에서도 실시간 응답을 기대할 수 있는 효과를 갖는다.As described above, the trajectory generation method and system for the real-time response of the motion control according to the present invention, reflects the change of the speed or position command within a limited time during the movement in the robot system that must respond in real time to an external event such as a sensor. In addition, it has the effect of expecting a real-time response even on a relatively slow CPU with a small amount of computation.

Claims (16)

소정의 작업시작점과 작업종료점 사이의 작업경로상에서 작업을 수행하는 로봇의 이동경로를 실시간을 변경하기 위한 방법에 있어서,In the method for changing the movement path of the robot performing the work on the work path between the predetermined work start point and the work end point in real time, (1) 초기 속도가 "0"인 경우 상기 로봇의 이동 속도에 따라 가속, 등속 및 감속의 세구간으로 나누고, 초기 속도가 "0"이 아닌 경우 브랜딩, 등속 및 감속의 세구간으로 나누어 구간을 설정하는 단계;(1) When the initial speed is "0", the robot is divided into three sections of acceleration, constant speed, and deceleration according to the moving speed of the robot, and when the initial speed is not "0", it is divided into three sections of branding, constant speed, and deceleration. Setting up; (2) 상기 설정된 세구간중 가속구간과 감속구간에서는 3차 다항식을 사용하고, 등속구간에서는 상수 속도를 사용하여 최종 위치로의 이동을 위한 속도 프로파일을 구하는 단계;(2) obtaining a velocity profile for moving to the final position using a third order polynomial in the acceleration section and the deceleration section of the set three sections, and using a constant speed in the constant velocity section; (3) 상기 설정된 세구간중 브랜딩구간에서는 주어진 변화시간 동안에 초기 속도에서 속도를 "0"으로 만드는 속도 프로파일과 상기 단계 (2)에서 구해진 속도 프로파일을 브랜딩하여 속도 프로파일을 구하는 단계;(3) obtaining a velocity profile by branding a velocity profile that makes the velocity "0" at an initial velocity and a velocity profile obtained in the step (2) in a branding section of the set three sections; (4) 상기 구해진 궤적에 따라 이동중 제한된 시간 내에 위치 및 속도 변경을 실시간 반영하여 모션 제어하는 단계를 포함하는 모션제어의 실시간 응답을 위한 궤적발생방법.(4) a trajectory generation method for real-time response of motion control, comprising: controlling motion by reflecting a position and speed change in real time within a limited time during movement according to the obtained trajectory. 제 1항에 있어서, 상기 단계 (2)는 초기 속도가 "0"인 경우 축 공간에서의 연속적인 속도 제공을 위한 궤적(Sn(t))을 다음의 식에 근거하여 생성함을 특징으로하는 모션제어의 실시간 응답을 위한 궤적발생방법.The method of claim 1, wherein the step (2) generates a trajectory S n (t) for providing a continuous velocity in axial space when the initial velocity is "0" based on the following equation. Trajectory generation method for real-time response of motion control. 여기서, Sa, Su 및 Sd는 가속, 등속 및 감속구간에서 움직이는 거리이고, facc 및 fdec는 가속 및 감속형태를 결정하는 정규화된 특성함수이며, ta, tu 및 td는 가속, 등속 및 감속이 끝나는 시간을 나타낸다.Here, Sa, Su, and Sd are the moving distances in the acceleration, constant speed, and deceleration sections, facc and fdec are normalized characteristic functions that determine the acceleration and deceleration types, and ta, tu, and td are the times when acceleration, constant speed, and deceleration end. Indicates. 제 2항에 있어서, 상기 단계 (2)의 가속, 등속 및 감속구간에서 움직이는 거리(Sa,Su,Sd)는 다음의 식에 근거하여 구해짐을 특징으로 하는 모션제어의 실시간응답이 가능한 궤적발생방법.The method of claim 2, wherein the moving distances Sa, Su, and Sd in the acceleration, constant speed, and deceleration sections of step (2) are obtained based on the following equation. . 여기서, d는 모션 제어시 이동거리(So)의 부호와 관계없이 목표속도(Vd)를 입력받기 위해 최고속도(Vm)의 부호를 결정하는 값이며, aa는 가속도 크기이고, ad는 감속도 크기이다.Here, d is a value for determining the sign of the maximum speed (Vm) to receive the target speed (Vd) regardless of the sign of the moving distance (So) during motion control, aa is the acceleration magnitude, ad is the deceleration magnitude to be. 제 3항에 있어서, 상기 단계 (2)의 가속, 등속 및 감속이 끝나는 시간(ta,tu,td)은 다음의 식에 근거하여 구해짐을 특징으로 하는 모션제어의 실시간 응답이 가능한 궤적발생방법.4. The trajectory generation method as claimed in claim 3, wherein the time (ta, tu, td) at which the acceleration, constant speed, and deceleration ends in step (2) is obtained based on the following equation. 제 2항에 있어서, 상기 단계 (2)는 가속 및 감속구간의 시작인 경우 가속 및 감속형태를 결정하는 특성함수가 facc(0)=0과 fdec(0)=0을 만족하고, 가속 및 감속구간의 끝인 경우 가속과 감속형태를 결정하는 특성함수가 facc(1)=1과 fdec(1)=1을 만족하는 선형적인 속도특성을 가짐을 특징으로 하는 모션제어의 실시간 응답을 위한 궤적발생방법.The method according to claim 2, wherein the step (2) is characterized by the fact that the characteristic functions for determining the acceleration and deceleration type when fac (0) = 0 and fdec (0) = 0 are satisfied. Trajectory generation method for real-time response of motion control, characterized by the linear velocity characteristics satisfying facc (1) = 1 and fdec (1) = 1 at the end of the interval. . 제 5항에 있어서, 상기 단계 (2)는 가속구간에서 f'acc(s)=2s 및 감속구간에서 f'dec(s)=-2s+2의 다항식을 사용함을 특징으로 하는 모션제어의 실시간 응답을 위한 궤적발생방법.6. The real-time motion control of claim 5, wherein the step (2) uses a polynomial of f'acc (s) = 2s in the acceleration section and f'dec (s) = -2s + 2 in the deceleration section. Trajectory generation method for response. 제 5항에 있어서, 상기 단계 (2)는 가속 및 감속구간의 시작인 경우 가속및 감속형태를 결정하는 특성함수가 f''acc(0)=0과 f''dec(0)=0을 만족하고, 가속 및 감속 구간의 끝인 경우 가속과 감속 형태를 결정하는 특성함수가 f''acc(1)=0, f''dec(1)=0을 만족하며, 가속 및 감속구간 내인 경우 연속적인 함수에 따라 부드러운 속도특성을 가짐을 특징으로 하는 모션제어의 실시간 응답을 위한 궤적발생방법.6. The method according to claim 5, wherein step (2) is characterized in that f &quot; acc (0) = 0 and f &quot; (0) = 0 If the function is satisfied and the end of the acceleration / deceleration section is satisfied, the characteristic function that determines the acceleration and deceleration pattern satisfies f''acc (1) = 0, f''dec (1) = 0. Trajectory generation method for real-time response of motion control, characterized by smooth velocity characteristics according to 제 7항에 있어서, 상기 단계 (2)는 가속구간에서 f'acc(s)=-4s3+ 6s2및 f'dec(s)=4s3- 6s2+ 2의 3차 다항식을 사용함을 특징으로 하는 모션제어의 실시간 응답을 위한 궤적발생방법.The method of claim 7, wherein the step (2) in the acceleration stage f'acc (s) = - and uses the third order polynomial of 6s 2 + 2 - 4s 3 + 6s 2 and f'dec (s) = 4s 3 Trajectory generation method for real-time response of the motion control characterized in that. 제 6항 또는 제 8항 중 어느 한 항에 있어서, 상기 단계 (2)는 구해진 속도 프로파일에서 임의의 순간의 속도(v(t)) 및 가속도(a(t))를 다음의 식에 근거하여 각각 구함을 특징으로 하는 모션제어의 실시간 응답을 위한 궤적발생방법.9. The method according to any one of claims 6 to 8, wherein step (2) is based on the following equation based on the velocity v (t) and acceleration a (t) of any instant in the obtained velocity profile: Trajectory generation method for real-time response of motion control characterized in that each desired. 제 8항에 있어서, 상기 단계 (3)은9. The method of claim 8, wherein step (3) (3a) 초기 속도와 가속도의 곱의 부호에 따라 사건 발생 순간이 이전 이동의 가속, 등속 및 감속구간중 어떠한 시기인지 예측하는 단계;(3a) predicting which of the acceleration, constant velocity and deceleration periods of the previous movement is an instant of occurrence of the event according to the sign of the product of the initial velocity and the acceleration; (3b) 상기 예측결과에 따라 브랜딩 특성함수를 결정하는 단계;(3b) determining a branding characteristic function according to the prediction result; (3c) 상기 결정된 브랜딩 특성함수로 초기 속도와 가속도를 "0"으로 만드는 속도 프로파일을 구하는 단계; 및(3c) obtaining a velocity profile that makes initial velocity and acceleration "0" with the determined branding characteristic function; And (3d) 상기 구한 속도 프로파일과 주어진 이동거리 S0와 Sb의 차이(S0-Sb)를 움직이기 위한 속도 프로파일을 브랜딩하여 실시간 응답이 가능한 궤적을 발생하는 단계를 구비함을 특징으로 하는 모션제어의 실시간 응답이 가능한 궤적발생방법.And (3d) branding a velocity profile for moving the difference between the obtained velocity profile and a given movement distance S0 and Sb (S0-Sb) to generate a trajectory capable of real-time response. Possible trajectory generation method. 제 10항에 있어서, 상기 단계 (3b)는 초기 가속도가 "0"인 이전 이동의 등속구간에서 사건이 발생한 경우 상기 부드러운 속도특성의 함수를 브랜딩 특성함수로결정하고, 초기 가속도가 "0"이 아닌 이전 이동의 감속 및 가속구간에서 사건이 발생한 경우 다음의 4차 및 5차 다항식을 브랜딩 특성함수(fblend(s))로 결정함을 특징으로 하는 모션제어의 실시간 응답이 가능한 궤적발생방법.11. The method according to claim 10, wherein step (3b) determines that the function of the smooth velocity characteristic is a branding characteristic function when an event occurs in the constant velocity section of a previous movement with an initial acceleration of "0" and the initial acceleration is "0". If the event occurs in the deceleration and acceleration section of the previous movement, the trajectory generation method capable of real-time response of motion control, characterized in that the following 4th and 5th polynomials are determined by the branding characteristic function (fblend (s)). 감속구간에 따른 브랜딩 특성함수( Branding characteristic function according to deceleration section 가속구간에 따른 브랜딩 특성함수( Branding characteristic function according to acceleration section ( 제 11항에 있어서, 상기 단계 (4)는 초기 속도벡터로부터 속도를 "0"으로 만들기 위한 속도 프로파일로 이동한 거리를 결정하고, 임의의 시간의 초기속도를 "0"으로 만들기 위한 위치궤적을 구하며, 결정된 속도 프로파일로의 이동거리와 목표위치를 이용하여 목표 이동속도의 방향으로 이동한 거리를 구하여 구해진 파라메터들로 주어진 전이시간에 속도변화하여 이동하는 위치궤적을 구함을 특징으로 하는 모션제어의 실시간 응답을 위한 궤적발생방법.12. The method according to claim 11, wherein step (4) determines the distance traveled from the initial velocity vector to the velocity profile for making the velocity "0", and sets the position trajectory for making the initial velocity of any time "0". By using the movement distance and the target position to the determined speed profile, the distance moved in the direction of the target movement speed is obtained. Trajectory generation method for real-time response. 제 11항에 있어서, 상기 단계 (4)는 초기 가속도에 따라 비대칭적인 특성함수들을 사용하여 가속도 연속적인 위치궤적을 생성함을 특징으로 하는 모션제어의 실시간 응답을 위한 궤적발생방법.12. The method as claimed in claim 11, wherein the step (4) generates acceleration continuous position trajectories using asymmetric characteristic functions according to the initial acceleration. 로봇의 작업수행중 이동경로를 실시간으로 변경할 수 있는 모션제어 시스템에 있어서,In the motion control system that can change the movement path in real time during the operation of the robot, 로봇 조작자로부터의 로봇 제어명령을 입력받아 전달해주는 미들웨어와, 로봇의 모션제어를 수행하는 모션제어기를 구비하며, 초기 유휴(DORMANT)상태에서 모션제어 인에이블/디스에이블 및 이동의 사건이 발생하면 토큰이 발생하여 해당 작업을 수행하도록 제어하는 상위제어기(CPU); 및Middleware that receives and transfers robot control commands from the robot operator, and a motion controller that performs the motion control of the robot, and tokens when an event of motion control enable / disable and movement occurs in the initial idle state. The upper controller (CPU) to control the occurrence of this operation occurs; And 상기 상위제어기의 제어에 따라 축들 위치 제어를 수행하는 다축위치제어부를 구비하는 외부하드웨어를 포함하며,It includes an external hardware having a multi-axis position control unit for performing the position control of the axis in accordance with the control of the upper controller, 상기 모션제어기는,The motion controller, 미들웨어와의 인터페이스를 수행하여 명령이 전달되면 해독 작업을 수행하여 축 단위 이동인지, 월드 좌표계에 대한 이동인지를 구별하는 커맨드디코더;A command decoder configured to perform an interface with the middleware and perform a decryption operation when the command is transmitted to discriminate whether it is a movement in an axis unit or a movement in a world coordinate system; 상기 외부하드웨어에서 인터럽트 사건이 발생하면 상기 커맨드디코더에서 구별된 명령을 사전 처리하여 로봇의 가속, 감속 및 등속에 관한 속도 프로파일을 생성하여 궤적을 계산하는 궤적발생부;A trajectory generation unit configured to calculate a trajectory by generating a speed profile related to acceleration, deceleration, and constant velocity of the robot by preprocessing a command distinguished from the command decoder when an interrupt event occurs in the external hardware; 로봇의 다수개의 축값을 계산하는 역운동학계산부;An inverse kinematic calculation unit for calculating a plurality of axis values of the robot; 축값으로부터 월드좌표값을 계산하는 운동학계산부; 및A kinematic calculation unit for calculating a world coordinate value from an axis value; And 다축위치제어부와의 인터페이스를 수행하여 목표위치값을 전달하는 인터페이스제어부를 구비함을 특징으로 하는 모션제어의 실시간 응답을 위한 시스템.System for real-time response of motion control, characterized in that it comprises an interface control unit for performing the interface with the multi-axis position control unit to deliver a target position value. 제 14항에 있어서, 상기 다축위치제어부는 주기적으로 주어진 시간마다 인터럽트 사건을 발생하여 각 축들 위치로 제어함을 특징으로 하는 모션제어의 실시간 응답을 위한 시스템.15. The system of claim 14, wherein the multi-axis position control unit periodically generates an interrupt event at a given time and controls each axis position. 제 15항에 있어서, 모션 제어의 최대 명령 수행 지연시간은 인터럽트 발생 주기임을 특징으로 하는 모션제어의 실시간 응답을 위한 시스템.16. The system of claim 15, wherein the maximum command execution delay time of the motion control is an interrupt occurrence period.
KR1020030098264A 2002-12-27 2003-12-27 Trajectory generation method and system for real-time response at motion control KR100540588B1 (en)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
KR20020085301 2002-12-27
KR1020020085301 2002-12-27

Publications (2)

Publication Number Publication Date
KR20040060825A true KR20040060825A (en) 2004-07-06
KR100540588B1 KR100540588B1 (en) 2006-01-10

Family

ID=37352581

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020030098264A KR100540588B1 (en) 2002-12-27 2003-12-27 Trajectory generation method and system for real-time response at motion control

Country Status (1)

Country Link
KR (1) KR100540588B1 (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100722857B1 (en) * 2006-01-12 2007-05-30 삼성전자주식회사 Velocity profile generating device and control method thereof
KR101259819B1 (en) * 2010-12-24 2013-04-30 삼성중공업 주식회사 Method of controlling continuous path movement of robot and robot system performing the same
CN109814568A (en) * 2019-02-19 2019-05-28 百度在线网络技术(北京)有限公司 Generation method, device and the storage medium of speed trajectory
CN116871775A (en) * 2023-09-08 2023-10-13 惠生(南通)重工有限公司 Welding robot system for cross ground rail type 3D grid frame structure

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101363358B1 (en) * 2012-12-06 2014-02-17 (주)동부로봇 System for controlling robot and method the same

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5602968A (en) * 1994-05-02 1997-02-11 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Task space angular velocity blending for real-time trajectory generation
KR100242447B1 (en) * 1997-12-31 2000-04-01 윤종용 Method and system for path planning in robot control
JP2002346957A (en) * 2001-05-21 2002-12-04 Univ Waseda Movement trajectory control method for bipedal walking robot
US6845287B2 (en) * 2002-11-20 2005-01-18 Asml Holding N.V. Method, system, and computer program product for improved trajectory planning and execution

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100722857B1 (en) * 2006-01-12 2007-05-30 삼성전자주식회사 Velocity profile generating device and control method thereof
KR101259819B1 (en) * 2010-12-24 2013-04-30 삼성중공업 주식회사 Method of controlling continuous path movement of robot and robot system performing the same
CN109814568A (en) * 2019-02-19 2019-05-28 百度在线网络技术(北京)有限公司 Generation method, device and the storage medium of speed trajectory
CN109814568B (en) * 2019-02-19 2022-05-03 阿波罗智能技术(北京)有限公司 Method and device for generating speed track of unmanned vehicle and storage medium
US11511748B2 (en) 2019-02-19 2022-11-29 Apollo Intelligent Driving Technology (Beijing) Co., Ltd. Velocity trajectory generation method, apparatus, and storage medium
CN116871775A (en) * 2023-09-08 2023-10-13 惠生(南通)重工有限公司 Welding robot system for cross ground rail type 3D grid frame structure
CN116871775B (en) * 2023-09-08 2023-11-14 惠生(南通)重工有限公司 Welding robot system for cross ground rail type 3D grid frame structure

Also Published As

Publication number Publication date
KR100540588B1 (en) 2006-01-10

Similar Documents

Publication Publication Date Title
EP0130570A2 (en) Method and apparatus for controlling a robot hand along a predetermined path
JP2010170435A (en) System and method for instructing motion control, and motion control system
JP2017127964A (en) Robot apparatus having learning function
KR20170048164A (en) System and method for controlling robot
JP2019061523A (en) Information processing unit, information processing method, and program
JP5858168B2 (en) Real-time weaving motion control apparatus and method
KR100540588B1 (en) Trajectory generation method and system for real-time response at motion control
CN107309873B (en) Mechanical arm motion control method and system
JP2020107315A (en) Synchronization control device, synchronization control system, synchronization control method, and simulation device
US20190291274A1 (en) Robot control apparatus, robot control method, and program
JPH0916241A (en) Method for setting acceleration/deceleration operation of robot
CN112894827A (en) Mechanical arm motion control method, system and device and readable storage medium
CN111331577B (en) Robot control device and control method
Kubus et al. 1kHz is not enough—How to achieve higher update rates with a bilateral teleoperation system based on commercial hardware
JP2005014149A (en) Robot system
WO2020137286A1 (en) Synchronous control device, synchronous control system, synchronous control method, and simulation device
KR20090107864A (en) Apparatus for generating Velocity Profile, Apparatus for Controlling Motor and Method thereof
CN111699078A (en) Operation of the robot
JP3121920B2 (en) Acceleration / deceleration control device
CN110362040A (en) Robot motion&#39;s method for planning track based on B-spline
CN114131613B (en) Inching operation control method based on S curve
JP2996445B2 (en) Multi-axis drive
RU2724775C1 (en) Complex of dynamic control of anthropomorphic manipulator
JPH078160B2 (en) Motor controller
JPH05297916A (en) Track control method for robot

Legal Events

Date Code Title Description
A201 Request for examination
E701 Decision to grant or registration of patent right
GRNT Written decision to grant
FPAY Annual fee payment

Payment date: 20121130

Year of fee payment: 8

FPAY Annual fee payment

Payment date: 20131202

Year of fee payment: 9

FPAY Annual fee payment

Payment date: 20141201

Year of fee payment: 10