KR101529760B1

KR101529760B1 - Device for collision detection using band designed disterbanc observer, and the method

Info

Publication number: KR101529760B1
Application number: KR1020120044940A
Authority: KR
Inventors: 문형필; 최혁렬; 구자춘; 정병진
Original assignee: 성균관대학교산학협력단
Priority date: 2012-04-27
Filing date: 2012-04-27
Publication date: 2015-06-29
Also published as: KR20130121592A

Abstract

본 발명은 대역 외란 관측기를 이용한 매니퓰레이터의 충돌에 의한 외란 감지 장치에 관한 것으로, 좀 더 구체적으로 특정 주파수 밴드의 외란을 출력하는 대역 외란 관측기(Band Designed Disturbance Observer)를 이용해 외부에서 가해지는 충돌에 의한 외란을 다른 외란과 구분할 수 있는 대역 외란 관측기를 이용한 매니퓰레이터의 충돌에 의한 외란 감지 장치에 관한 것이다. 대역 외란 관측기를 이용한 매니퓰레이터의 충돌에 의한 외란 감지 장치는 매니퓰레이터의 충돌에 의한 외란을 감지하는 장치에 있어서, 매니퓰레이터의 측정 조인트 토크를 감지하고, 엔코더의 각도 및 각속도를 감지하는 측정부, 상기 엔코더의 각도와 각속도 및, 시스템 역모델을 이용해 매니퓰레이터의 현재 위치에 대한 추정 조인트 토크를 계산하는 계산부, 감지된 상기 측정 조인트 토크와 계산된 상기 추정 조인트 토크를 비교하고, 충돌에 의해 발생된 외란만을 구분하는 관측부 및 상기 관측부에서 출력된 외란을 통해 매니퓰레이터의 충돌 여부를 판단하는 판단부를 포함하는 것을 특징으로 한다.More particularly, the present invention relates to a disturbance sensing apparatus using a band disturbance observer, and more particularly, to a disturbance sensing apparatus using a band disturbance observer that outputs a disturbance of a specific frequency band, And more particularly, to a disturbance sensing apparatus using a band disturbance observer capable of distinguishing disturbances from other disturbances. An apparatus for detecting a disturbance caused by a collision of a manipulator, comprising: a measurement unit for sensing a measurement joint torque of a manipulator and sensing an angle and an angular velocity of the encoder; angle and angular velocity, and comparing the calculated and the calculation unit, the sensing the measured joint torque to calculate the estimated joint torque for the current position of the manipulator, the estimated joint torque using the system inverse model, separated only the disturbance caused by the collision And a determination unit for determining whether the manipulator collides with the observer based on the disturbance output from the observer.

Description

TECHNICAL FIELD [0001] The present invention relates to a device for detecting disturbance by a collision of a manipulator using a band disturbance observer, and a method of detecting the disturbance by using a band disturbance observer,

본 발명은 대역 외란 관측기를 이용한 매니퓰레이터의 충돌에 의한 외란 감지 장치에 관한 것으로, 좀 더 구체적으로 특정 주파수 밴드의 외란을 출력하는 대역 외란 관측기(Band Designed Disturbance Observer)를 이용해 외부에서 가해지는 충돌에 의한 외란을 다른 외란과 구분할 수 있는 대역 외란 관측기를 이용한 매니퓰레이터의 충돌에 의한 외란 감지 장치에 관한 것이다.More particularly, the present invention relates to a disturbance sensing apparatus using a band disturbance observer, and more particularly, to a disturbance sensing apparatus using a band disturbance observer that outputs a disturbance of a specific frequency band, And more particularly, to a disturbance sensing apparatus using a band disturbance observer capable of distinguishing disturbances from other disturbances.

매니퓰레이터는 사람의 팔에 상당하는 기능을 수행하기 위한 기계장치를 말하는데, 통상 복수의 링크(link)를 포함하며, 각 링크 사이에는 관절이 구비되어 어느 한 링크가 인접한 다른 링크에 대해 상대적으로 회동할 수 있게 되어 있다. 이러한 매니퓰레이터는 다양한 분야에서 사용되고 있으며, 복수의 관절을 구비하고 있으므로 특히 산업용 로봇으로 사용될 때는 다관절 로봇이라 칭하기도 한다.A manipulator is a mechanism for performing a function equivalent to a human's arm. The manipulator usually includes a plurality of links, and joints are provided between the links so that one of the links is relatively rotated with respect to another adjacent link It is possible. Such a manipulator is used in various fields, and has a plurality of joints. Therefore, when the robot is used as an industrial robot, it may be referred to as a multi-joint robot.

현재 매니퓰레이터의 이용 범위는 자동화 생산 공정이나 실험실 등과 같이 인간이 배제되어 있거나 충분히 모델링 된 작업 공간 내에 제한되어 있고, 셀 생산 및 가사 노동 등과 같이 인간과의 접촉이 잦거나 충분한 모델링이 이루어지지 않아 충돌 위험이 큰 작업 공간에는 투입되지 않고 있다. 이러한 제한된 이용 범위를 확장하고자 매니퓰레이터 설계 및 운용 시 충돌에 대응하는 기능에 대한 연구가 여러 방면에서 진행되고 있다.Currently, the range of the manipulator is restricted to the human-excluded or well-modeled workspace such as the automated production process or the laboratory, and there is no frequent or sufficient modeling of human contact such as cell production and housework, Is not put into a large work space. In order to extend this limited use range, researches on the function to cope with the collision in the design and operation of the manipulator are going on in various ways.

충돌에 대응한 매니퓰레이터 설계 및 응용에 있어서, 안전 관절을 대표적인 예로 들 수 있다. 안전 관절이란 기계적인 장치, 혹은 알고리즘 설계를 통해 자체적으로 충돌 상황을 인지하고 충돌이 자신과 주변 환경에 미치는 피해를 최소화하도록 대응하는 관절을 의미한다.In the design and application of a manipulator in response to a collision, a safety joint is a typical example. A safety joint refers to a joint that recognizes a collision situation on its own by means of a mechanical device or algorithm design and minimizes the damage to the collision itself and the surrounding environment.

수동 안전 관절(passive safe joint)은 메커니즘적인 안전 관절이다. 이는 관절 혹은 링크에 비선형 탄성을 가지는 기구를 삽입하여 일정 문턱값을 넘어서는 외란이 작용할 경우 동력 전달을 끊은 구조를 가진다. 이러한 시스템은 로봇의 작동과 안전 시스템이 독립적으로 작동한다는 장점이 있으나, 질량이 추가되며 비선형 강성이 추가되는 한계가 있으며, 충돌에 의한 외란을 다른 외란들과 구분하기 어려운 문제가 존재한다.A passive safe joint is a mechanically safe joint. This is a structure that inserts a mechanism having nonlinear elasticity in a joint or a link and disconnects power transmission when a disturbance exceeding a certain threshold value is applied. This system has the advantage that the operation of the robot and the safety system operate independently, but there is a limitation that the mass is added and the nonlinear stiffness is added, and the disturbance caused by the collision is difficult to distinguish from other disturbances.

그리고, 충돌 감지 알고리즘을 이용한 안전 관절은 능동 안전 관절(active safe joint)로서, 엔코더나 조인트 센서 등의 데이터로부터 충돌 상황을 유추하고 충돌 지점의 접촉력(contact force)을 최소화하게 된다. 이러한 충돌 감지 알고리즘은 관성 상쇄를 위해 사용되는 외란 관측기(Disturbance Observer)와 조인트 간의 영향이나 중력 등을 상쇄시키고자 하는 잔여 관측기(residual observer) 등이 있다.And, the safety joint using the collision detection algorithm is an active safety joint, and the collision situation is deduced from the data of the encoder or the joint sensor and the contact force of the collision point is minimized. Such a collision detection algorithm includes a disturbance observer used for inertia cancellation and a residual observer for canceling gravity or influence between joints.

도 1은 종래 기술에 따라 매니퓰레이터의 시스템 다이어그램 상에서 외란 관측기가 도시된 블록다이어그램이며, 도 2는 또 다른 종래 기술에 따라 매니퓰레이터의 시스템 다이어그램 상에서 잔여 관측기가 도시된 블록다이어그램이다.FIG. 1 is a block diagram illustrating a disturbance observer on a system diagram of a manipulator according to the prior art, and FIG. 2 is a block diagram illustrating a remaining observer on a system diagram of a manipulator according to another conventional technique.

도 1을 참조하면, 종래 기술에 따른 매니퓰레이터의 외란 관측기는 엔코더 데이터와 시스템 입력 및, 시스템 역모델을 이용해 외란을 검출하는 관측기이다. 여기서, Q필터는 시스템 역모델의 인과관계 획득을 위해 사용되며, 저역통과필터(LPF) 형태를 가진다.Referring to FIG. 1, a disturbance observer of a conventional manipulator is an observer for detecting disturbance using encoder data, a system input, and a system inverse model. Here, the Q filter is used for the causal relationship acquisition of the system inverse model and has the form of a low-pass filter (LPF).

도 2를 참조하면, 잔여 관측기가 적용된 블록다이어그램으로, 다축 모델에서 코리올리, 원심력 등에 대한 보상이 추가된 1차 Q필터를 가지는 외란 관측기가 된다. Referring to FIG. 2, a disturbance observer having a first-order Q filter to which compensation for Coriolis, centrifugal force, and the like is added in a multiaxial model is a block diagram to which a residual observer is applied.

여기서, 종래의 방법들은 시스템 모델에 오차가 있을 경우, 관측 데이터에 모델링 오차에 의한 영향이 증가되며, 이는 관측 외란값에 문턱값을 설정하여 충돌을 판별하는 알고리즘이 모델링 오차 존재 시에 오작동할 가능성이 커지고, 성능 저하가 되는 원인이 된다.In this case, when there is an error in the system model, the conventional methods increase the influence of the modeling error on the observation data. This is because the algorithm that determines the collision by setting the threshold value to the observation disturbance value may malfunction in the presence of the modeling error And the performance is deteriorated.

또한, 상기와 같은 종래의 충돌 감지 알고리즘은 충돌에 의한 외란을 다른 요인으로 인한 외란과 구별하지 못하는 문제가 있었다.In addition, the conventional collision detection algorithm has a problem in that disturbance due to collision can not be distinguished from disturbance due to other factors.

종래와 같은 문제를 해결하기 위해 본 발명의 목적은 특정 주파수 밴드의 외란을 출력하는 대역 외란 관측기(Band Designed Disturbance Observer)를 이용함으로써, 충돌에 의한 외란을 다른 외란과 구분할 수 있는 환경을 제공하고자 한다.An object of the present invention is to provide an environment in which a disturbance due to a collision can be distinguished from other disturbances by using a band designed disturbance observer that outputs a disturbance of a specific frequency band .

또한, 본 발명의 다른 목적은 다양한 종류의 필터들을 통합한 대역 외란 관측기를 이용함으로써, 충돌에 의한 외란을 다른 외란과 구분함과 동시에 충돌 감지 오류를 줄일 수 있는 환경을 제공하고자 한다.It is another object of the present invention to provide an environment capable of distinguishing disturbance caused by collision from other disturbance and reducing collision detection error by using a band disturbance observer incorporating various kinds of filters.

또한, 본 발명의 다른 목적은 대역 외란 관측기를 통해 더 작은 문턱값(threshold)을 설정함으로써, 충돌 감지에 소요되는 지연 시간을 줄이고, 빠르게 충돌을 감지할 수 있는 환경을 제공하고자 한다.Another object of the present invention is to provide an environment in which a delay time required for collision detection is reduced and a collision can be detected quickly by setting a smaller threshold through a band disturbance observer.

상기 목적들을 달성하기 위한 본 발명의 대역 외란 관측기를 이용한 매니퓰레이터의 충돌에 의한 외란 감지 장치는 매니퓰레이터의 충돌에 의한 외란을 감지하는 장치에 있어서, 매니퓰레이터의 측정 조인트 토크를 감지하고, 엔코더의 각도 및 각속도를 감지하는 측정부, 상기 엔코더의 각도와 각속도 및, 시스템 역모델을 이용해 매니퓰레이터의 현재 위치에 대한 추정 조인트 토크를 계산하는 계산부, 감지된 상기 측정 조인트 토크와 계산된 상기 추정 조인트 토크를 비교하고, 충돌에 의해 발생된 외란만을 구분하여 출력하는 관측부 및, 상기 관측부에서 출력된 외란을 통해 매니퓰레이터의 충돌 여부를 판단하는 판단부를 포함하는 것을 특징으로 한다.In order to achieve the above objects, there is provided an apparatus for detecting a disturbance caused by a collision of a manipulator, comprising: a sensing unit for sensing a measurement joint torque of a manipulator; A calculation unit for calculating an estimated joint torque with respect to a current position of the manipulator by using an angle and an angular velocity of the encoder and an inverse model of the system, a comparison unit for comparing the sensed measured joint torque with the calculated estimated joint torque An observer for outputting only the disturbance generated by the collision, and a determiner for determining whether the manipulator collides with the disturbance output from the observer.

또한, 상기 관측부는 특정 주파수 대역의 외란을 출력하는 대역 외란 관측기(Band designed Disturbance Observer)를 이용하되, 상기 대역 외란 관측기가 충돌에 의해 발생되는 외란만을 출력하는 것을 특징으로 한다.Also, the observing unit may use a band designed disturbance observer outputting a disturbance of a specific frequency band, and the band disturbance observer outputs only the disturbance generated by the collision.

또한, 상기 대역 외란 관측기는 조인트 토크의 노이즈와 엔코더의 노이즈를 제거하고, 충돌에 의해 발생된 외란의 주파수 대역을 설정하여, 상기 주파수 대역의 외란만을 출력하는 것을 특징으로 한다.In addition, the band disturbance observer removes the noise of the joint torque and the noise of the encoder, sets the frequency band of the disturbance generated by the collision, and outputs only the disturbance of the frequency band.

또한, 상기 주파수 대역은 조인트 입력 속도보다 빠른 최소주파수를 가지고, 센서 노이즈의 지배적 주파수(dominant frequency)보다 낮은 최대주파수를 가지는 것을 특징으로 한다.
In addition, the frequency band has a minimum frequency faster than the joint input speed and has a maximum frequency lower than the dominant frequency of the sensor noise.

그리고, 본 발명의 충돌에 의한 외란 감지 방법은 매니퓰레이터를 구동하는 구동 단계, 상기 구동에 따른 매니퓰레이터의 조인트 토크를 측정하고, 엔코더의 각도 및 각속도를 측정하는 측정 단계, 엔코더의 상기 속도 및 각속도로부터 예상되는 추정 조인트 토크를 계산하는 계산 단계, 상기 측정 조인트 토크와 상기 추정 조인트 토크를 비교하고, 충돌에 의해 발생된 외란를 출력하는 비교 관측 단계 및, 출력된 상기 외란으로부터 매니퓰레이터의 충돌 여부를 판단하는 충돌 판단 단계를 포함하는 것을 특징으로 한다.A method for detecting a disturbance caused by a collision according to the present invention includes a driving step of driving a manipulator, a measurement step of measuring a joint torque of the manipulator according to the driving, a measuring step of measuring an angle and an angular velocity of the encoder, A comparison observation step of comparing the measured joint torque with the estimated joint torque and outputting the disturbance generated by the collision, and a step of comparing the measured joint torque with the estimated joint torque to determine whether the manipulator collides with the output disturbance The method comprising the steps of:

상술한 바와 같이, 본 발명의 대역 외란 관측기를 이용한 매니퓰레이터의 충돌에 의한 외란 감지 장치는 특정 주파수 밴드의 외란을 출력하는 대역 외란 관측기(Band Designed Disturbance Observer)를 이용함으로써, 충돌에 의한 외란을 다른 외란과 구분할 수 있는 환경을 제공한다.As described above, the apparatus for detecting a disturbance by a collision of a manipulator using the band disturbance observer of the present invention uses a band designed disturbance observer that outputs a disturbance of a specific frequency band, And the like.

또한, 본 발명은 다양한 종류의 필터들을 통합한 대역 외란 관측기를 이용함으로써, 충돌에 의한 외란을 다른 외란과 구분함과 동시에 충돌 감지 오류를 줄일 수 있는 환경을 제공한다.The present invention also provides an environment for distinguishing disturbance due to collision from other disturbance and reducing collision detection error by using a band disturbance observer combining various kinds of filters.

또한, 본 발명은 대역 외란 관측기를 통해 더 작은 문턱값(threshold)을 설정함으로써, 충돌 감지에 소요되는 지연 시간을 줄이고, 빠르게 충돌을 감지할 수 있는 환경을 제공한다.In addition, the present invention provides an environment in which the delay time required for collision detection can be reduced and the collision can be detected quickly by setting a smaller threshold value through the band disturbance observer.

도 1은 종래 기술에 따라 매니퓰레이터의 시스템 다이어그램 상에 외란 관측기가 도시된 블록다이어그램이다.
도 2는 또 다른 종래 기술에 따라 매니퓰레이터의 시스템 다이어그램 상에 잔여 관측기가 도시된 블록다이어그램이다.
도 3은 본 발명에 따른 대역 외란 관측기를 이용한 매니퓰레이터의 충돌에 의한 외란 감지 장치를 간략히 도시한 블록 구성도이다.
도 4는 다수의 각 센서에 필요한 필터들이 사용된 통상적인 외란 관측기의 실제 적용을 도시한 블록다이어그램이다.
도 5는 대역 Q필터를 이용해 본 발명에 따른 대역 외란 관측기를 통해 구현한 블록다이어그램이다.
도 6은 본 발명에 따른 외란 감지 방법을 간략히 도시한 순서도이다.
도 7은 외란 관측기의 적용을 위해 충돌에 의한 주파수 대역을 설정하는 과정을 간략히 도시한 순서도이다.
도 8은 본 발명의 외란 감지 장치에 의해 충돌을 감지한 결과를 나타내는 그래프이다.1 is a block diagram illustrating a disturbance observer on a system diagram of a manipulator in accordance with the prior art.
Figure 2 is a block diagram illustrating a residual observer on a system diagram of a manipulator in accordance with yet another prior art.
3 is a block diagram schematically illustrating an apparatus for detecting a disturbance due to a collision of a manipulator using a band disturbance observer according to the present invention.
4 is a block diagram illustrating an actual application of a conventional disturbance observer using filters required for a plurality of angular sensors.
5 is a block diagram of a band disturbance observer according to the present invention using a band-Q filter.
6 is a flowchart briefly showing a disturbance detection method according to the present invention.
7 is a flowchart schematically illustrating a process of setting a frequency band due to collision for application of a disturbance observer.
8 is a graph showing a result of detecting a collision by the disturbance detecting device of the present invention.

본 발명의 실시예는 여러 가지 형태로 변형될 수 있으며, 본 발명의 범위가 아래에서 서술하는 실시예로 인해 한정되어지는 것으로 해석되어서는 안된다. 본 실시예는 당업계에서 평균적인 지식을 가진 자에게 본 발명을 보다 완전하게 설명하기 위해서 제공되는 것이다. 따라서 도면에서의 구성 요소의 형상 등은 보다 명확한 설명을 강조하기 위해서 과장되어진 것이다.The embodiments of the present invention can be modified into various forms and the scope of the present invention should not be interpreted as being limited by the embodiments described below. The present embodiments are provided to enable those skilled in the art to more fully understand the present invention. Therefore, the shapes and the like of the components in the drawings are exaggerated in order to emphasize a clearer explanation.

이하 첨부된 도 3 내지 도 8를 참조하여 본 발명의 실시예를 상세히 설명한다.
Hereinafter, embodiments of the present invention will be described in detail with reference to FIGS. 3 to 8 attached hereto.

도 3은 본 발명에 따른 대역 외란 관측기를 이용한 매니퓰레이터의 충돌에 의한 외란 감지 장치를 간략히 도시한 블록 구성도이다.3 is a block diagram schematically illustrating an apparatus for detecting a disturbance due to a collision of a manipulator using a band disturbance observer according to the present invention.

도 3의 일실시예에 따른 본 발명의 대역 외란 관측기를 이용한 매니퓰레이터의 충돌에 의한 외란 감지 장치는 측정부(10), 제어부(20) 및, 출력부(30)를 포함한다.The apparatus for detecting disturbance by a collision of a manipulator using a band disturbance observer according to an embodiment of FIG. 3 includes a measurement unit 10, a control unit 20, and an output unit 30.

측정부(10)는 매니퓰레이터의 구동에 따른 측정 조인트 토크를 감지하고, 엔코더의 각도 및 각속도를 감지하는 수단이다. 여기서, 측정부(10)는 본 발명의 일실시예에 따라 측정 조인트 토크를 감지하는 제1측정부(미도시)와 엔코더의 각도 및 각속도를 감지하는 제2측정부(미도시)로 구분되어 각각 측정할 수도 있다. 여기서, 본 발명의 측정부는 별도의 센서를 포함할 필요없이, 기존의 매니퓰레이터에 구비된 조인트 센서와 엔코더를 이용해 측정하도록 구현할 수 있다.The measurement unit 10 is a means for sensing the measurement joint torque according to the driving of the manipulator and sensing the angle and angular velocity of the encoder. Here, the measuring unit 10 is divided into a first measuring unit (not shown) for sensing a measurement joint torque and a second measuring unit (not shown) for detecting an angle and an angular velocity of the encoder according to an embodiment of the present invention Respectively. Here, the measuring unit of the present invention can be implemented to measure by using a joint sensor and an encoder provided in a conventional manipulator without needing to include a separate sensor.

제어부(20)는 본 발명의 외란 감지 장치의 각부 구성요소가 원활하게 수행되도록 제어하며, 매니퓰레이터에 발생된 전체 외란으로부터 충돌에 의한 외란만을 구분해 출력할 수 있는 수단이다. The control unit 20 controls each component of the disturbance detection apparatus of the present invention to be performed smoothly and outputs only the disturbance caused by the collision from all the disturbances generated in the manipulator.

그리고, 본 발명의 일실시예에 따라 제어부(20)는 설정부(22), 계산부(24), 관측부(26) 및, 판단부(28)를 포함한다.The control unit 20 includes a setting unit 22, a calculation unit 24, an observation unit 26, and a determination unit 28 according to an embodiment of the present invention.

설정부(22)는 충돌에 의한 외란을 다른 외란과 구별하기 위해 필요한 각종 데이터들에 대한 환경 설정을 하는 수단이다. 여기서, 설정부(22)는 대역 외란 관측기를 본 발명에 적용하기 위해 충돌에 의해 발생될 주파수 대역을 설정하거나, 그에 따른 문턱값을 설정할 수 있다.The setting unit 22 is a means for setting environment settings for various data necessary for distinguishing disturbance due to collision from other disturbances. In order to apply the band disturbance observer to the present invention, the setting unit 22 may set a frequency band to be generated by the collision and set a threshold value according to the frequency band.

계산부(24)는 측정부(10)에서 측정한 엔코더의 각도와 각속도 및, 시스템 역모델을 이용해 매니퓰레이터의 현재 위치에 대한 추정 조인트 토크를 계산하는 수단이다. The calculating unit 24 is a means for calculating an estimated joint torque with respect to the current position of the manipulator using the angle and angular velocity of the encoder measured by the measuring unit 10 and the system inverse model.

관측부(26)는 매니퓰레이터에 발생된 다양한 외란들 사이에서 충돌에 의한 외란만을 관측해 출력하는 수단이다. 관측부(26)는 측정부(10)에서 감지된 측정 조인트 토크와 계산부(24)에서 계산된 추정 조인트 토크를 비교하여,이들의 차이로부터 충돌에 의해 발생된 외란을 출력할 수 있게 된다.The observer 26 is a means for observing and outputting disturbance due to collision between various disturbances generated in the manipulator. The observing unit 26 compares the measured joint torque detected by the measuring unit 10 with the estimated joint torque calculated by the calculating unit 24 and outputs the disturbance generated by the collision from these differences.

여기서, 관측부(26)는 본 발명의 일실시예에 따라 대역 외란 관측기(Band Designed Disturbance Observer)로 구현된다.Here, the observing unit 26 is implemented as a band designed disturbance observer according to an embodiment of the present invention.

대역 외란 관측기는 특정 주파수 대역의 외란을 출력하는 것으로, 매니퓰레이터의 충돌에 의해 발생되는 외란만을 출력한다. The band disturbance observer outputs a disturbance in a specific frequency band, and outputs only the disturbance generated by the collision of the manipulator.

그리고, 본 발명의 대역 외란 관측기는 조인트 토크의 노이즈와 엔코더의 노이즈를 제거하고, 충돌에 의해 발생된 외란의 주파수 대역을 설정하여, 상기 주파수 대역의 외란을 출력할 수 있게 된다. 따라서, 본 발명의 대역 외란 관측기는 다양한 용도의 필터를 포함하거나, 이들을 통합하여 구현한 대역 Q필터(Band designed Q filter)를 포함할 수 있다. 이에 대해서는, 이하의 도 4와 도 5에 대한 설명에서 상세히 설명하겠다.The band disturbance observer of the present invention can remove the noise of the joint torque and the noise of the encoder, set the frequency band of the disturbance generated by the collision, and output the disturbance of the frequency band. Therefore, the band disturbance observer of the present invention may include filters for various purposes, or may include a band designed Q filter integrated with them. This will be described in detail below with reference to FIG. 4 and FIG. 5.

여기서, 대역 외란 관측기에 설정된 주파수 대역은 조인트 입력 속도보다 빠른 최소주파수를 가지고, 센서 노이즈의 지배적 주파수(dominant frequency)보다 낮은 최대주파수를 가지도록 구현되는 것을 특징으로 한다.Here, the frequency band set in the band disturbance observer is configured to have a minimum frequency faster than the joint input speed and have a maximum frequency lower than the dominant frequency of the sensor noise.

판단부(28)는 관측부(26)에서 출력된 외란을 통해 매니퓰레이터의 충돌 여부를 판단하고, 충돌이 발생한 경우 애러 처리한 후 매니퓰레이터를 정지시키게 된다.The determination unit 28 determines whether or not the manipulator collides with the disturbance output from the observation unit 26, and stops the manipulator after performing the irregularity processing when a collision occurs.

출력부(30)는 제어부(20)에서 발생되는 신호들을 출력하는 것으로, 각종 외란들의 신호와 충돌에 의한 외란의 신호를 출력해주어 사용자가 매니퓰레이터의 작동 상태를 모니터링할 수 있게 한다. 그리고, 본 발명의 일실시예에 따라 출력부(30)는 표시부(미도시)에 상기 신호들을 출력하고, 표시부(미도시)가 이를 표시할 수 있을 것이다.
The output unit 30 outputs signals generated by the control unit 20 and outputs a disturbance signal due to a collision with signals of various disturbances so that the user can monitor the operation state of the manipulator. According to an embodiment of the present invention, the output unit 30 may output the signals to a display unit (not shown), and a display unit (not shown) may display the signals.

도 4는 다수의 각 센서에 필요한 필터들이 사용된 통상적인 외란 관측기의 실제 적용을 도시한 블록다이어그램이다.4 is a block diagram illustrating an actual application of a conventional disturbance observer using filters required for a plurality of angular sensors.

우선, 매니퓰레이터의 동작에 의해 발생되는 외란들을 분석하면 아래와 같다.First, the disturbances generated by the operation of the manipulator are analyzed as follows.

매니퓰레이터의 동작에 있어서 입력되는 외란(d)는 아래 수학식 1과 같이 충돌에 의한 토크(

) 뿐만 아나라 조인트 마찰(

), 조인트 베어링의 점성 댐핑(

), 기어 백래쉬(

), 센서 노이즈(

)의 영향 등이 함께 작용한다.The input disturbance d in the operation of the manipulator is expressed by the following equation (1)

) Only the joint friction (

), Viscous damping of joint bearings

), Gear backlash (

), Sensor noise

), And the like.

여기서, 실제 관측기 출력(r)에는 아래의 수학식 7과 같이 모델링 오차나 작업중 변화하는 작업툴, 작업물 등의 모델링되지 않은 요소에 의한 관측오차(

)가 함께 나타난다.Here, the actual observer output (r) is expressed by the following equation (7), such as modeling errors and observation errors due to un-modeled elements such as work tools,

).

이와 같이, 관측기 출력에 나타나는 여러가지 외란 요소들 중 충돌에 의한 외란은 이상적인 상태에서 모든 주파수 영역에 걸치는 임펄스(impulse) 형태의 외란이며, 실제로는 작용시간이 매우 짧은 고주파 성분이 강한 외란으로 불 수 있다.Among the various disturbance factors appearing on the observer output, the disturbance caused by the collision is an impulse-type disturbance extending from the ideal state to all the frequency regions. In practice, the high-frequency component having a short operating time may be a strong disturbance .

여기서, 조인트 댐핑에 의한 외란은 댐핑 상수(B)와 조인트 입력 속도(

)에 종속된다. 또한, 모델링 오차에 의한 외란은 노미널 모델(nominal model)과 실제 모델 간의 차이(

,

)와 조인트 입력에 따라 달라지며, 이들은 각각 아래의 수학식 3과 수학식 4와 같이 나타낼 수 있다. 여기서, 이들 외란들은 조인트 입력 속도와 연관된 주파수를 가지는 외란이라는 공통점이 있다.Here, the disturbance due to joint damping is expressed by the damping constant (B) and the joint input speed

). Also, the disturbance due to modeling error is the difference between the nominal model and the actual model

,

) And the joint input, which can be expressed by the following equations (3) and (4), respectively. Here, these disturbances have a common point of disturbance having a frequency associated with the joint input speed.

그리고, 조인트 마찰에 의한 외란은 매니퓰레이터가 부드러운 경로를 따라 움직이는 것으로 가정하면 아래 수학식 5와 같이 구현할 수 있다.The disturbance due to the joint friction can be expressed by Equation (5) below assuming that the manipulator moves along a smooth path.

또한, 센서 노이즈에 의한 신호는 센서의 사양에 따라 다르지만, 일반적으로 매니퓰레이터와 외란의 작동 영역에 비해 매우 높은 주파수 영역을 가지거나 전 주파수 영역에 걸쳐서 무시할 만큼 작은 크기의 화이트 노이즈(white noise) 형태를 취할 것이다.In addition, although the signal due to the sensor noise varies depending on the specifications of the sensor, generally, a white noise form having a very high frequency range or negligibly small size over the entire frequency range as compared with the manipulator and the disturbance operating region I will take it.

도 4를 참조하면, 본 발명은 일실시예에 따라 다양한 종류의 필터(Q₁,Q₂,Q₃,Q₄)를 사용해 출력함으로써, 충돌에 의한 외란을 매니퓰레이터에 발생되는 다른 외란들과 구별하도록 구현할 수 있을 것이다.4, the present invention outputs various types of filters (Q ₁ , Q ₂ , Q ₃ , and Q ₄ ) according to an exemplary embodiment to distinguish disturbance due to collision from other disturbances generated in the manipulator .

여기서, Q₁은 시스템 역모델의 인과관계(causality)를 보정하기 위한 Q필터이고, Q₂는 엔코더 신호의 노이즈를 상쇄하기 위한 저역통과필터(LPF)이며, Q₃는 조인트 마찰 및 점성 댐핑, 고장모델(fault model)의 영향을 제거하기 위한 고역통과필터(HPF)이고, Q₄는 조인트 센서의 노이즈를 상쇄하기 위한 저역통과필터이다.Here, Q ₁ is Q filter for correcting the cause and effect (causality) of the system inverse model, Q ₂ is a low pass filter (LPF) for canceling noise in the encoder signal, Q ₃ is the joint friction, viscous damping, Pass filter (HPF) for eliminating the influence of the fault model, and Q ₄ is a low-pass filter for canceling the noise of the joint sensor.

그러나, 도 4와 같이 추가적인 필터들이 요구되는 시스템은 불안전한 시스템이며, Q필터 외에 여러 주파수 대역의 필터가 독립적으로 사용되기 때문에 필터 각각의 시간 지연이 관측된 데이터 출력에 영향을 미치게 되는 단점이 존재한다.
However, as shown in FIG. 4, a system in which additional filters are required is an insecure system. Since the filters of various frequency bands other than the Q filter are independently used, there is a disadvantage that the time delay of each filter affects the observed data output do.

도 5는 대역 Q필터를 이용해 본 발명에 따른 대역 외란 관측기를 통해 구현한 블록다이어그램이다.5 is a block diagram of a band disturbance observer according to the present invention using a band-Q filter.

도 5를 참조하면, 본 발명은 도 4에 따른 Q필터들을 통합한 대역 Q필터(Band Designed Q filter)를 통해 대역 외란 관측기(Band Designed Disturbance Observer)를 이용해 구현할 수 있음을 보여준다. Referring to FIG. 5, the present invention can be implemented using a band designed disturbance observer through a band-Q filter integrated with Q filters according to FIG.

여기서, 본 발명은 도 4에 따른 필터들이 결과적으로 관측 데이터에 있어서 특정 주파수 대역의 외란 신호들을 관측하는 대역 필터와 같은 작용을 하므로, 도 5에서는 시스템 역모델의 인과관계를 보상한 후에 남은 성분들을 시스템 내에 필요한 필터들의 대역을 반영한 대역 필터 형태로 대체할 수 있게 된다.
Here, since the filters according to FIG. 4 result in the same function as the band-pass filter for observing the disturbance signals in a specific frequency band in the observation data, FIG. 5 shows that the components remaining after compensating the causal relationship of the system inverse model It can be replaced with a band-pass filter that reflects the band of filters required in the system.

도 6은 본 발명에 따른 외란 감지 방법을 간략히 도시한 순서도이다.6 is a flowchart briefly showing a disturbance detection method according to the present invention.

도 6에 따른 본 발명의 외란 감지 방법은 구동 단계, 측정 단계, 계산 단계, 비교 관측 단계 및, 충돌 판단 단계를 포함한다.The disturbance sensing method of the present invention according to FIG. 6 includes a driving step, a measuring step, a calculating step, a comparative observing step, and a collision determining step.

로봇 또는 매니퓰레이터를 구동하면서, 구동에 따른 매니퓰레이터의 조인트 토크를 측정하고, 엔코더의 각도 및 각속도를 측정한다(S10,S12,S14).The joint torque of the manipulator is measured while driving the robot or the manipulator, and the angle and angular velocity of the encoder are measured (S10, S12, S14).

그리고, 이때 측정된 엔코더의 상기 속도 및 각속도로부터 매니퓰레이터의 현재 위치에서 예상되는 추정 조인트 토크를 계산하며(S16), 비교 관측 단계는 측정된 상기 측정 조인트 토크와 계산된 상기 추정 조인트 토크를 비교해서 충돌에 의해 발생된 외란를 출력하게 된다(S18).Then, the estimated joint torque at the current position of the manipulator is calculated from the velocity and the angular velocity of the measured encoder at this time (S16), and the comparison observation step compares the measured joint torque torque with the calculated estimated joint torque, (S18). &Lt; / RTI >

충돌 판단 단계에서는 비교 관측 단계에서 출력된 상기 외란으로부터 매니퓰레이터의 충돌 여부를 판단하고, 충돌된 것으로 판단된 경우 로봇이나 매니퓰레이터의 작동을 정지시킬 수 있게 된다(S20,S22).
In the collision determination step, it is determined whether or not the manipulator collides with the disturbance output from the comparison observation step, and if it is determined that the manipulator is collided, the operation of the robot or the manipulator can be stopped (S20, S22).

도 7은 외란 관측기의 적용을 위해 충돌에 의한 주파수 대역을 설정하는 과정을 간략히 도시한 순서도이다.7 is a flowchart schematically illustrating a process of setting a frequency band due to collision for application of a disturbance observer.

본 발명의 외란 감지 방법은 도 6과 같이 외란을 감지하기 위해서, 매니퓰레이터를 구동하기 전에 이에 필요한 각종 데이터의 환경설정을 필요로 한다. 도 7을 참조하면, 엔코더 노이즈와 힘센서 노이즈 등을 감지하여 환경설정 데이터를 입력하고(S30), 이때의 데이터들을 이용해 노이즈 레벨보다 낮고 조인트 입력 속도보다 큰 주파수 밴드를 설정하여, 대역 외란 관측기에서 충돌에 의한 외란을 감지하기 위한 문턱값을 설정하게 된다(S32,S34).
The disturbance detection method of the present invention requires environment setting of various data necessary for driving the manipulator in order to detect a disturbance as shown in FIG. Referring to FIG. 7, environment setting data is input by sensing encoder noise, force sensor noise, and the like (S30). Using the data at this time, a frequency band lower than the noise level and higher than the joint input speed is set, A threshold value for detecting the disturbance due to the collision is set (S32, S34).

도 8은 본 발명의 외란 감지 장치에 의해 충돌을 감지한 결과를 나타내는 그래프이다.8 is a graph showing a result of detecting a collision by the disturbance detecting device of the present invention.

도 8을 참조하면, 매니퓰레이터에 충돌(A)이 발생한 경우, 본 발명의 외란 감지 장치가 충돌에 의한 외란을 감지해 고주파 신호를 출력하게 된다. 본 발명의 감지 장치는 충돌에 의한 외란을 구별할 수 있으며, 또한 종래의 잔여 관측기(residual ovserver)에 비해 빠르게 충돌을 감지하게 된다.
Referring to FIG. 8, when a collision (A) occurs in the manipulator, the disturbance sensing apparatus of the present invention detects a disturbance due to collision and outputs a high frequency signal. The sensing device of the present invention can distinguish disturbance due to collision and also detect collision faster than a conventional residual ovserver.

이상에서, 본 발명에 따른 대역 외란 관측기를 이용한 매니퓰레이터의 충돌에 의한 외란 감지 장치의 구성 및 작용을 상세한 설명과 도면에 따라 도시하였지만, 이는 실시예를 들어 설명한 것에 불과하며, 본 발명의 기술적 사상을 벗어나지 않는 범위 내에서 다양한 변화 및 변경이 가능하다.While the present invention has been particularly shown and described with reference to exemplary embodiments thereof, it will be understood by those skilled in the art that various changes and modifications may be made without departing from the scope of the invention as defined in the appended claims. Various changes and modifications are possible without departing from the scope of the invention.

10: 측정부 20: 제어부
22: 설정부 24: 계산부
26: 관측부 28: 판단부
30: 출력부10: measuring section 20:
22: setting section 24: calculating section
26: Observation unit 28:
30: Output section

Claims

An apparatus for detecting a disturbance caused by a collision of a manipulator,
A measuring unit for sensing a measurement joint torque of the manipulator and sensing an angle and an angular velocity of the encoder;
A calculation unit for calculating an estimated joint torque with respect to a current position of the manipulator using an angle and an angular velocity of the encoder and a system inverse model;
The measured joint torque is compared with the calculated estimated joint torque, the frequency outputted from the manipulator is measured by disturbance using a band designed disturbance observer, and the noise of the joint torque and the noise of the encoder An observation unit for removing noise to obtain a frequency due to the collision; And
And a determiner for determining whether the manipulator collides with the disturbance output from the observer based on the disturbance detected by the manipulator.

delete

The method according to claim 1,
The band disturbance observer
Wherein the frequency band of the disturbance generated by the collision is set so that only the disturbance of the frequency band is output, and the disturbance detection device by the collision of the manipulator using the band disturbance observer.

The method of claim 3,
The frequency band
And a maximum frequency lower than a dominant frequency of the sensor noise, and having a minimum frequency faster than the joint input speed, and having a maximum frequency lower than the dominant frequency of the sensor noise.

A driving step of driving the manipulator;
Measuring a joint torque of the manipulator according to the driving and measuring an angle and an angular velocity of the encoder;
A calculation step of calculating an estimated joint torque expected from the velocity and the angular velocity of the encoder;
The frequency of the collision is obtained by comparing the measured joint torque with the estimated joint torque, measuring the frequency output from the manipulator by disturbance using a band disturbance observer, removing the noise of the joint torque and the noise of the encoder among the frequencies Observation phase; And
And determining a collision of the manipulator based on the output disturbance.