KR101214685B1

KR101214685B1 - Method for controlling robot manipulator device based on frequency analysys of impact on the device

Info

Publication number: KR101214685B1
Application number: KR1020110026019A
Authority: KR
Inventors: 송재복; 김준홍; 김영렬
Original assignee: 고려대학교 산학협력단
Priority date: 2011-03-23
Filing date: 2011-03-23
Publication date: 2013-01-09
Also published as: KR20120108318A

Abstract

본 발명은, 하나 이상의 구동 모터를 통하여 가동되는 하나 이상의 관절부를 구비하는 로봇 머니퓰레이터의 제어부의 감지 제어 신호에 따라 상기 관절부의 감지 관절 토크 정보를 취득하는 감지 단계와, 상기 감지 단계에서 취득된 상기 감지 관절 토크 정보 및 상기 제어부와 연결되는 저장부에 사전 설정 저장된 추정 데이터에 기초하여 상기 관절부의 추정 관절 토크가 산출되고, 상기 추정 관절 토크의 주파수 성분이 분석되는 추정 분석 단계와, 상기 추정 관절 토크의 주파수 성분과 상기 저장부에 저장된 사전 설정 작업 기준 데이터에 기초하여 상기 관절부의 사전 설정된 작업 수행 여부를 판단하고 판단 결과에 따라 상기 로봇 머니퓰레이터의 작업 모드를 설정하는 모드 결정 단계와, 상기 제어부의 모드 실행 제어 신호에 따라 상기 모드 결정 단계에서 결정된 모드를 실행하는 모드 실행 단계를 포함하는 주파수 기반 로봇 머니퓰레이터 제어 방법을 제공한다.According to the present invention, a sensing step of acquiring sensing joint torque information of the joint part according to a sensing control signal of a control unit of a robot manipulator having one or more joint parts operated through at least one driving motor, and the sensing acquired in the sensing step An estimation analysis step of calculating an estimated joint torque of the joint unit based on joint torque information and estimated data preset in a storage unit connected to the controller, and analyzing a frequency component of the estimated joint torque; A mode determination step of determining whether to perform a preset work of the joint part based on a frequency component and preset work reference data stored in the storage unit, and setting a work mode of the robot manipulator according to a determination result; and executing a mode of the controller Determine the mode according to the control signal It provides a frequency based on a robot manipulator control method comprising a mode execution step of executing a predetermined mode in the system.

Description

주파수 기반 로봇 머니퓰레이터 제어 방법{METHOD FOR CONTROLLING ROBOT MANIPULATOR DEVICE BASED ON FREQUENCY ANALYSYS OF IMPACT ON THE DEVICE}Frequency-Based Robot Manipulator Control Method {METHOD FOR CONTROLLING ROBOT MANIPULATOR DEVICE BASED ON FREQUENCY ANALYSYS OF IMPACT ON THE DEVICE}

본 발명은 안전 장치에 관한 것으로, 보다 상세하게는 보다 안전하고 신속한 충돌 제어를 이루기 위한 로봇 충돌 제어 방법에 관한 것이다. The present invention relates to a safety device, and more particularly to a robot collision control method for achieving safer and faster collision control.

로봇에 대한 수요는 산업용으로서 뿐만 아니라 가정용에 대한 수요도 증대되고 있고, 이에 따라 로봇 연구가 활발하게 진행되고 있다. 특히, 로봇의 동작에 관절 운동이 로봇 동작의 중요 부분을 이루는데, 종래 기술에 로봇은 강성과 위치 제어를 위하여 복잡한 구조와 상당한 장착 공간을 요구하였다.The demand for robots is not only for industrial use, but also for home use, and accordingly, research on robots is actively progressing. Particularly, the joint movement is an important part of the robot movement in the movement of the robot. In the conventional art, the robot requires a complicated structure and a considerable mounting space for rigidity and position control.

이러한 기계적 장치들은 통상 산업 설비 뿐만 아니라 일상 생활까지 활용범위를 넓혀가고 있고 이에 따라 안전성 확보는 로봇 등의 장치를 설계함에 있어 가장 중요한 설계 인자로 작용한다. 이와 같이 로봇 등에 장착되는 안전 장치는 수행되는 방식에 따라 수동 제어 방식과 능동 제어 방식으로 분류되는데, 능동 제어 방식은 외부 충격을 감지하여 이에 따라 소정의 안전 장치를 가동시킴으로써 감지된 외부 충격에 대응하는 구성을 취한다. 즉, 로봇과 외부 물체 간에 충돌이 발생하는 경우 센서 등이 충돌 여부 및 충돌 세기 등을 감지하여 이를 제어부로 전달하고, 제어부는 감지된 신호에 기초하여 소정의 제어 신호를 생성하고 안전 장치에 구비되는 액추에이터가 제어부로부터의 소정의 제어 신호에 따라 작동함으로써 외부 충격에 대응하는 기능을 실행한다. 수동 제어 방식은 센서 및 액추에이터를 사용하지 않고 입력되는 충격에 대하여 스프링 및/또는 댐퍼 등의 기계 요소를 통한 외부 충격에 대응한다. These mechanical devices are widely used not only in industrial facilities but also in everyday life. Accordingly, securing of safety is a most important design factor in designing devices such as robots. The safety device mounted on the robot is classified into a manual control system and an active control system according to the manner in which the robot is mounted. The active control system detects an external impact and operates a predetermined safety apparatus accordingly, . That is, when a collision occurs between the robot and an external object, the sensor or the like senses a collision, a collision intensity, and the like, and transmits it to the control unit. The control unit generates a predetermined control signal based on the sensed signal, The actuator performs a function corresponding to an external shock by operating in accordance with a predetermined control signal from the control unit. The manual control method corresponds to an external shock through a mechanical element such as a spring and / or a damper against a shock input without using a sensor and an actuator.

능동 제어 방식은 다양한 외부 충격에 대하여 능동적 대응을 가능케 하여 범용성을 확보할 수 있다는 장점이 있으나, 센서 감지 및 제어 신호 출력 등에 요구되는 상당한 제어 시간이 로봇팔이 외부 물체 등과 충돌이 발생하는 경우 대부분의 충격은 약 10ms 내지 20ms의 시간 이내에 발생하는 물리적 시간 제약보다 커서 원활한 능동 제어를 이루기 어렵다는 문제점이 있다. The active control method has the advantage of being able to proactively respond to various external shocks to secure versatility, but most control cases in which the robot arm collides with an external object require considerable control time required for sensor detection and control signal output. Since the impact is greater than the physical time constraint occurring within a time of about 10 ms to 20 ms, it is difficult to achieve smooth active control.

수동 제어 방식의 경우 원가 절감을 이루는 장점이 있으나, 비선형적 동작 제어가 어렵다. 즉, 로봇 팔에 스프링 등의 수동 제어 구성요소가 배치되는 경우 충격 흡수를 필요로 하지 않는 경우에도 외력에 비례하는 변형이 발생하고, 충격 흡수를 요하는 경우에도 스프링에 작용하는 물체의 중량에 비례하여 로봇팔의 처짐이 발생하는 등 소기의 안전 기능을 확보함과 동시에 설계 자유도가 증진이 제한되는 문제점이 수반되었다. In the case of the manual control method, there is an advantage of cost reduction, but it is difficult to control the nonlinear motion. That is, when a manual control component such as a spring is disposed on the robot arm, deformation occurs in proportion to the external force even when shock absorption is not required, and proportional to the weight of an object acting on the spring even when shock absorption is required. As a result, deflection of the robot arm occurred, the desired safety function was secured, and the degree of freedom in design was limited.

본 발명은 보다 간단한 구조의 능동적 제어 방식을 구현함에 있어 충돌 발생시 의도성 여부를 판단하여 보다 정확하고 안전한 제어를 실행하기 위한 주파수 기반 로봇 머니퓰레이터 제어 방법을 제공하는 것을 목적으로 한다. An object of the present invention is to provide a frequency-based robotic manipulator control method for implementing more precise and safe control by determining the intention in the event of a collision in implementing an active control scheme having a simpler structure.

전술한 목적을 달성하기 위한 본 발명은, 하나 이상의 구동 모터를 통하여 가동되는 하나 이상의 관절부를 구비하는 로봇 머니퓰레이터의 제어부의 감지 제어 신호에 따라 상기 관절부의 감지 관절 토크 정보를 취득하는 감지 단계와, 상기 감지 단계에서 취득된 상기 감지 관절 토크 정보 및 상기 제어부와 연결되는 저장부에 사전 설정 저장된 추정 데이터에 기초하여 상기 관절부의 추정 관절 토크가 산출되고, 상기 추정 관절 토크의 주파수 성분이 분석되는 추정 분석 단계와, 상기 추정 관절 토크의 주파수 성분과 상기 저장부에 저장된 사전 설정 작업 기준 데이터에 기초하여 상기 관절부의 사전 설정된 작업 수행 여부를 판단하고 판단 결과에 따라 상기 로봇 머니퓰레이터의 작업 모드를 설정하는 모드 결정 단계와, 상기 제어부의 모드 실행 제어 신호에 따라 상기 모드 결정 단계에서 결정된 모드를 실행하는 모드 실행 단계를 포함하는 주파수 기반 로봇 머니퓰레이터 제어 방법을 제공한다.The present invention for achieving the above object, the sensing step of acquiring the sensing joint torque information of the joint portion in accordance with the detection control signal of the control unit of the robot manipulator having one or more joints that are operated through one or more drive motors; Estimation analysis step in which the estimated joint torque of the joint part is calculated based on the sensed joint torque information acquired in the sensing step and estimated data preset in a storage unit connected to the control unit, and the frequency component of the estimated joint torque is analyzed. And a mode determining step of determining whether to perform the preset work of the joint part based on the frequency component of the estimated joint torque and the preset work reference data stored in the storage unit, and setting the work mode of the robot manipulator according to the determination result. And mode execution control of the controller. It provides a frequency-based robot manipulator control method comprising a mode execution step of executing the mode determined in the mode determination step according to the signal.

상기 주파수 기반 로봇 머니퓰레이터 제어 방법에 있어서, 상기 추정 분석 단계는: 상기 감지 관절 토크 정보와 상기 추정 데이터에 기초하여 상기 제어부의 추정 제어 신호에 따라 외력 추정 관측기에서 외력 추정 토크가 연산되는 외력 추정 토크 연산 단계와, 상기 외력 추정 토크에 기초하여 상기 제어부의 분석 제어 신호에 따라 외력 변화 추정 관측기에서 상기 외력 추정 토크의 고주파 성분을 추출하여 외력 추정 고주파 토크를 산출하는 주파수 분석 단계를 포함할 수도 있다. In the frequency-based robot manipulator control method, the estimation analysis step includes: an external force estimation torque calculation in which an external force estimation torque is calculated in an external force estimation observer according to an estimated control signal of the controller based on the sensed joint torque information and the estimation data. And extracting a high frequency component of the external force estimation torque from an external force change estimator based on the analysis control signal of the controller based on the external force estimation torque to calculate an external force estimation high frequency torque.

상기 주파수 기반 로봇 머니퓰레이터 제어 방법에 있어서, 상기 모드 결정 단계는: 상기 외력 추정 토크 및 상기 외력 추정 고주파 토크를 상기 사전 설정 작업 기준 데이터와 비교하는 모드 판단 단계와, 상기 모드 판단 단계에서 판단된 결과에 기초하여 실행되어야 할 작동 모드를 설정하는 모드 설정 단계를 포함할 수도 있다. In the frequency-based robot manipulator control method, the mode determining step includes: a mode determination step of comparing the external force estimation torque and the external force estimation high frequency torque with the preset work reference data, and a result determined in the mode determination step. It may also comprise a mode setting step of setting an operation mode to be executed based on.

상기 주파수 기반 로봇 머니퓰레이터 제어 방법에 있어서, 상기 모드 판단 단계는: 상기 제어부가 상기 외력 추정 고주파 토크와 상기 사전 설정 작업 기준 데이터의 고주파 문턱값을 비교하는 예상 충돌 판단 단계와, 상기 예상 충돌 판단 단계에서 상기 외력 추정 고주파 토크가 상기 고주파 문턱값 미만인 경우, 상기 제어부가 상기 외력 추정 토크와 상기 사전 설정 작업 기준 데이터의 안전 문턱값을 비교하는 안전 판단 단계를 포함할 수도 있다. In the frequency-based robot manipulator control method, the mode determining step includes: an anticipated collision determination step of the controller comparing the external force estimation high frequency torque with a high frequency threshold value of the preset work reference data; When the external force estimated high frequency torque is less than the high frequency threshold, the controller may include a safety determining step of comparing the external force estimated torque with a safety threshold of the preset work reference data.

상기 주파수 기반 로봇 머니퓰레이터 제어 방법에 있어서, 상기 모드 설정 단계는: 상기 안전 판단 단계에서 상기 외력 추정 토크가 상기 안전 문턱값 미만인 경우 실행되어야 할 작업 모드로 사용자에 의하여 설정된 입력 작업을 수행하는 작업 수행 모드로 설정하는 작업 수행 모드 설정 단계와, 상기 예상 충돌 판단 단계에서 상기 외력 추정 고주파 토크가 상기 고주파 문턱값 이상이 경우와, 상기 안전 판단 단계에서 상기 외력 추정 토크가 상기 안전 문턱값 이상인 경우에 실행되어야 할 작업 모드로 충돌 대응 모드를 설정하는 충돌 대응 모드 설정 단계를 포함할 수도 있다. In the frequency-based robot manipulator control method, the mode setting step includes: a work execution mode for performing an input operation set by a user in a work mode to be executed when the external force estimated torque is less than the safety threshold in the safety determination step; It is to be executed when the operation execution mode setting step of setting, the external force estimation high frequency torque in the anticipated collision determination step is more than the high frequency threshold value, and the external force estimation torque in the safety determination step is more than the safety threshold value. The method may further include a collision response mode setting step of setting the collision response mode as the to-do mode.

상기 주파수 기반 로봇 머니퓰레이터 제어 방법에 있어서, 상기 작업 수행 모드 설정 단계에서 실행되어야 할 작업 모드가 작업 수행 모드로 설정된 경우, 상기 모드 실행 단계는: 입력부를 통하여 작업자에 의한 명령을 수신하는 작업자 명령 수신 단계와, 상기 작업자 명령 수신 단계에서 작업자에 의한 수신된 명령이 존재하는지 여부를 판단하는 수신 판단 단계와, 상기 수신 판단 단계에서의 수신 여부에 따라 형성된 작업을 실행하는 작업 수행 단계를 포함할 수도 있다. In the frequency-based robot manipulator control method, when the work mode to be executed in the work execution mode setting step is set to the work execution mode, the mode execution step includes: an operator command receiving step of receiving a command by an operator through an input unit; And a reception determination step of determining whether there is a command received by the worker in the worker command reception step, and a job execution step of executing a job formed according to whether or not the reception command is received.

상기 주파수 기반 로봇 머니퓰레이터 제어 방법에 있어서, 상기 작업 수행 모드 설정 단계에서 실행되어야 할 작업 모드가 충돌 상태로 판단하여 대응 실행이 요구되는 충돌 대응 모드로 설정된 경우, 상기 모드 실행 단계는: 상기 관절부의 감지 관절 토크 정보로부터 충돌 방향을 검출하는 충돌 방향 검출 단계와, 상기 제어부가 상기 관절부의 감지 관절 토크 정보로부터 상기 관절부의 충돌 위치를 확인하는 충돌 위치 확인 단계와, 상기 제어부가 상기 저장부에 저장된 사전 설정 대응 위치만큼 상기 관절부를 상기 충돌 방향과 반대 방향으로 가동시키는 대응 위치를 산출하는 대응 위치 결정 단계와, 상기 구동 모터에 상기 관절부의 상기 대응 위치로의 이동을 위한 구동 제어 신호를 인가하는 충돌 대응 단계를 포함할 수도 있다. In the frequency-based robot manipulator control method, when the work mode to be executed in the work execution mode setting step is set to a collision response mode in which a corresponding execution is required by determining a collision state, the mode execution step may include: detecting the joint part; A collision direction detecting step of detecting a collision direction from the joint torque information, a collision position checking step of the control unit confirming a collision position of the joint unit from the detected joint torque information of the joint unit, and a preset setting of the control unit stored in the storage unit A corresponding positioning step of calculating a corresponding position for moving the joint part in the direction opposite to the collision direction by a corresponding position; and a collision corresponding step of applying a drive control signal for movement to the corresponding position of the joint part to the drive motor; It may also include.

상기 주파수 기반 로봇 머니퓰레이터 제어 방법에 있어서, 상기 감지 관절 토크 정보는 감지부의 관절 토크 측정부에서 감지될 수도 있다.In the frequency-based robot manipulator control method, the sensed joint torque information may be sensed by the joint torque measurement unit of the detector.

상기한 바와 같은 구성을 갖는 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법은 다음과 같은 효과를 갖는다. The frequency-based robot manipulator control method according to the present invention having the configuration as described above has the following effects.

첫째, 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법은, 주파수 기반 로봇 머니퓰레이터의 외부 물체 내지 사람과의 접촉시, 발생한 접촉이 의도된 상황 하에서의 접촉인지 여부를 판단하여 보다 정확하고 안전한 제어 과정을 실행할 수 있다. First, the method for controlling a frequency-based robotic manipulator according to the present invention can execute a more accurate and safe control process by determining whether a contact occurs under an intended situation when contacting an external object or a person of the frequency-based robotic manipulator. have.

둘째, 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법은, 주파수 기반 로봇 머니퓰레이터의 외부 물체 내지 사람과의 접촉시, 발생한 접촉이 의도된 상황 하에서의 접촉인지 여부를 판단하여 힘 제어 기반 이외 교시 및 재현 기능을 갖춘 산업용 로봇에서보터 사람과 협업하는 서비스 머니퓰레이터까지 다양한 적용 가능성을 제공할 수 있다.Second, the frequency-based robot manipulator control method according to the present invention, when the contact with the external object or a person of the frequency-based robot manipulator determines whether or not the contact occurs under the intended situation to teach and reproduce functions other than the force control based From industrial robots equipped to service manipulators collaborating with boat people, we can offer a wide range of applications.

셋째, 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법은, 경우에 따라 가속도 정보를 배제하여 추가적인 센서 구비를 배제함으로써 제조 비용을 현저하게 저감시킬 수도 있다. Third, the frequency-based robotic manipulator control method according to the present invention may significantly reduce the manufacturing cost by eliminating the acceleration information in some cases and eliminating additional sensors.

본 발명은 도면에 도시된 일실시예들을 참고로 설명되었으나 이는 예시적인 것에 불과하며, 본 기술 분야의 통상의 지식을 가진 자라면 이로부터 다양한 변형 및 균등한 다른 실시예가 가능하다는 점을 이해할 것이다. 따라서, 본 발명의 진정한 기술적 보호 범위는 첨부된 특허 청구 범위의 기술적 사상에 의해 정해져야 할 것이다.While the present invention has been particularly shown and described with reference to exemplary embodiments thereof, it will be understood by those of ordinary skill in the art that various changes in form and details may be made therein without departing from the scope of the present invention. Accordingly, the true scope of the present invention should be determined by the technical idea of the appended claims.

도 1은 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법을 실행하는 주파수 기반 로봇 머니퓰레이터의 개략적인 모식도이다.
도 2a 및 도 2b는 의도하지 않은 충돌에 의한 충격량 변화 및 의도된 충돌에 의한 충격량 변화를 나타내는 선도이다.
도 2c는 주파수 기반 로봇 머니퓰레이터의 의도 여부에 따른 토크 변화를 나타내는 선도이다.
도 3은 본 발명의 일실시예에 따른 주파수 기반 로봇 머니퓰레이터의 개략적인 블록 선도이다.
도 4는 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법의 개략적인 흐름도이다.
도 5는 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법의 추정 분석 단계에 대한 흐름도이다.
도 6은 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법의 모드 결정 단계에 대한 흐름도이다.
도 7은 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법의 모드 실행 단계에 대한 흐름도이다.1 is a schematic diagram of a frequency-based robot manipulator for executing the method for controlling a frequency-based robot manipulator according to the present invention.
2A and 2B are diagrams showing an impact amount change due to an unintended collision and an impact amount change due to an intended collision.
Figure 2c is a diagram showing a torque change depending on whether the frequency-based robot manipulator.
3 is a schematic block diagram of a frequency-based robotic manipulator according to an embodiment of the present invention.
4 is a schematic flowchart of a frequency-based robot manipulator control method according to the present invention.
5 is a flowchart illustrating an estimation analysis step of the method for controlling a frequency-based robotic manipulator according to the present invention.
6 is a flowchart illustrating a mode determination step of the method for controlling a frequency-based robotic manipulator according to the present invention.
7 is a flowchart illustrating a mode execution step of the frequency-based robot manipulator control method according to the present invention.

이하에서는 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법에 대하여 도면을 참조하여 설명하기로 한다. Hereinafter, a method for controlling a frequency-based robot manipulator according to the present invention will be described with reference to the drawings.

도 1에는 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법을 실행하는 주파수 기반 로봇 머니퓰레이터의 개략적인 모식도가 도시되고, 도 2a 및 도 2b에는 의도하지 않에는 충돌에 의한 충격량 변화 및 의도된 충돌에 의한 충격량 변화를 나타내는 선도가 도시되고, 도 2c에는 주파수 기반 로봇 머니퓰레이터의 의도 여부에 따른 토크 변화를 나타내는 선도가 도시되고, 도 3에는 본 발명의 일실시예에 따른 주파수 기반 로봇 머니퓰레이터의 개략적인 블록 선도가 도시되고, 도 4에는 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법의 개략적인 흐름도가 도시되고, 도 5에는 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법의 추정 분석 단계에 대한 흐름도가 도시되고, 도 6에는 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법의 모드 결정 단계에 대한 흐름도가 도시되고, 도 7에는 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법의 모드 실행 단계에 대한 흐름도가 도시된다.Figure 1 is a schematic diagram of a frequency-based robot manipulator for executing the method for controlling the frequency-based robot manipulator according to the present invention, Figure 2a and 2b is not intended to change the impact amount by the impact and the impact amount by the intended collision A diagram showing the change is shown, FIG. 2C is a diagram showing the torque change depending on the intention of the frequency-based robot manipulator, and FIG. 3 is a schematic block diagram of the frequency-based robot manipulator according to an embodiment of the present invention. 4 is a schematic flowchart of a method for controlling a frequency-based robotic manipulator according to the present invention, FIG. 5 is a flowchart for an estimation analysis step of the method for controlling a frequency-based robotic manipulator according to the present invention, and FIG. Frequency-based robot manipulator according to the present invention A flowchart of the mode determination step of the method is shown, and FIG. 7 is a flowchart of the mode execution step of the frequency-based robotic manipulator control method according to the present invention.

본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법은 주파수 기반 로봇 머니퓰레이터(10)의 입력된 작업 모드 수행 내지 충돌시 이에 대응하는 제어 방법을 제공한다. 본 발명의 주파수 기반 로봇 머니퓰레이터(10)는 하나 이상의 관절부(100)를 구비하는데, 관절부(100)에는 가동을 위한 구동 모터(1)가 배치된다. 또한, 주파수 기반 로봇 머니퓰레이터(10)는 제어부(20)와 저장부(30)를 구비하고, 제어부(20)와 연결되는 연산부(40)를 구비할 수 있다.The frequency-based robot manipulator control method according to the present invention provides a control method corresponding to the execution of the input work mode of the frequency-based robot manipulator 10 or a collision. The frequency-based robotic manipulator 10 of the present invention includes one or more joint parts 100, and a driving motor 1 for operation is disposed in the joint part 100. In addition, the frequency-based robot manipulator 10 may include a control unit 20 and a storage unit 30, and may include an operation unit 40 connected to the control unit 20.

주파수 기반 로봇 머니퓰레이터(10)는 제어부(20)를 포함하는데, 제어부(20)는 감지부(5)에서 감지되는 관절부(100)의 관절 토크인 감지 관절 토크 정보를 포함하는 감지 정보를 취득하여 제어부(20)로 전달한다. The frequency-based robot manipulator 10 includes a control unit 20. The control unit 20 acquires sensing information including sensing joint torque information, which is a joint torque of the joint unit 100 detected by the sensing unit 5, and controls the control unit. Deliver to 20.

감지부(5)는 관절 위치 측정부(2)와 관절 속도 측정부(3)와 관절 토크 측정부(4)를 포함할 수 있는데, 관절 위치 측정부(2)와 관절 속도 측정부(3)는 구동 모터(1)에 내장될 수 있고, 경우에 따라 관절 위치 측정과 관절 속도 측정은 구동 모터(1)에 인가되는 전류 신호를 사용하여 연산되어 추출 사용되는 구조를 취할 수도 있는 등 설계 사양에 따라 다양한 구성이 가능하다. 관절 위치 측정부(2)와 관절 속도 측정부(3)는 관절부(100)에 배치되는 엔코더로 구현될 수 있고 경우에 따라 회전 속도를 감지하기 위한 자이로 센서를 더 구비할 수도 있다. The sensing unit 5 may include a joint position measuring unit 2, a joint speed measuring unit 3, and a joint torque measuring unit 4, which include a joint position measuring unit 2 and a joint speed measuring unit 3. May be embedded in the drive motor 1, and in some cases, the joint position measurement and the joint speed measurement may be calculated by using a current signal applied to the drive motor 1, and may take a structure used for extraction. Various configurations are possible. The joint position measuring unit 2 and the joint speed measuring unit 3 may be implemented as encoders disposed in the joint unit 100, and may further include a gyro sensor for detecting a rotation speed in some cases.

관절 토크 측정부(4)는 토크 센서로 구현되는데, 토크 센서로 구현되는 관절 토크 측정부는 관절부(100)에 가해지는 관절 토크를 감지하여 제어부(20)로 전달한다. The joint torque measuring unit 4 is implemented as a torque sensor, and the joint torque measuring unit implemented as the torque sensor detects the joint torque applied to the joint unit 100 and transmits the joint torque to the control unit 20.

저장부(30)는 하기되는 제어부(20)와 연결되는데, 저장부(30)는 사전 설정 데이터를 포함하여 소정의 제어 신호 인가 내지 관측 기능을 통한 예측치 연산 등의 기초가 되는 데이터를 제공할 수 있다. The storage unit 30 is connected to the control unit 20 to be described below. The storage unit 30 may provide data that is the basis for applying a predetermined control signal or calculating a predicted value through an observation function, including preset data. have.

제어부(20)는 제어기(21)와 관측기(23)를 포함하는데, 제어기(21)는 감지부(5)로부터 감지된 감지 정보 내지 작업자에 의하여 입력부(6)를 통하여 입력되는 입력 정보에 기초하여 구동 모터(1)에 소정의 구동 제어 신호를 인가함으로써 로봇 머니퓰레이터(10)를 구성하는 가동부인 링크를 가동시키기 위한 구동력을 제공할 수 있다. 또한, 본 발명에 따른 제어부(20)의 제어기(21)는 현재 접촉이 예상치 못한 충돌인지 예상된 충돌인지 여부를 판단하여 각각의 경우에 대한 소정의 사전 설정된 작동 모드를 실행하기 위한 제어 신호를 인가한다. The controller 20 includes a controller 21 and an observer 23. The controller 21 is based on sensing information detected from the sensing unit 5 or input information input through the input unit 6 by an operator. By applying a predetermined drive control signal to the drive motor 1, it is possible to provide a driving force for actuating a link, which is a movable part constituting the robot manipulator 10. In addition, the controller 21 of the control unit 20 according to the present invention determines whether the current contact is an unexpected collision or an expected collision, and applies a control signal for executing a predetermined preset operation mode for each case. do.

관측기(23)는 감지부(5)로부터 감지된 감지 관절 토크 정보를 포함하는 감지 정보 및 저장부(30)에 저장되는 사전 설정 데이터에 기초하여 소정의 관절부(100)를 통하여 연결되는 관절 링크(200)의 가동을 예측하여 관절부(100)에 가해지는 관절 토크를 추정할 수 있는데, 관측기(23)는 외력 추정 관측기(23a)와 외력 변화 추정 관측기(23b)를 포함한다. 외력 추정 관측기(23a)는 주파수 기반 로봇 머니퓰레이터(10)의 모델 정보 및 관절 위치 등의 정보에 기초하여 관절부(100)에 인가되는 관절 토크를 추정하여 추정 관절 토크를 산출하고, 외력 변화 추정 관측기(23b)는 추정 관절 토크로부터 빠르게 변화하는 성분만을 추출하여 외력 고주파 토크를 산출한다.The observer 23 is a joint link connected through a predetermined joint part 100 based on sensing information including sensing joint torque information sensed by the sensing unit 5 and preset data stored in the storage unit 30 ( It is possible to estimate the joint torque applied to the joint part 100 by predicting the operation of the 200, the observer 23 includes an external force estimation observer 23a and an external force change estimation observer 23b. The external force estimator 23a estimates joint torque applied to the joint part 100 based on model information of the frequency-based robotic manipulator 10 and information such as joint position, and calculates an estimated joint torque, and estimates an external force change estimator ( 23b) extracts only the fast changing component from the estimated joint torque to calculate the external force high frequency torque.

본 발명에 따른 주파수 기반 로봇 머니퓰레이터 제어 방법은 감지 단계(S10)와 추정 분석 단계(S20)와 모드 결정 단계(S30)와 모드 실행 단계(S40)를 포함한다. 감지 단계(S10)에서 제어부(20)의 제어기(21)는 후속적인 추정 분석 단계(S20)를 실행하기 위한 관절부(100)의 감지 관절 토크 정보를 취득한다. 감지부(5)의 토크 센서로 구현되는 관절 토크 측정부(4)로부터 관절부(100)에 인가되는 관절 토크를 감지하고 이러한 감지 관절 토크 정보는 제어부(20)의 제어기(21) 및/또는 관측기(233)로 전달된다. The method for controlling a frequency-based robotic manipulator according to the present invention includes a sensing step S10, an estimation analysis step S20, a mode determination step S30, and a mode execution step S40. In the sensing step S10, the controller 21 of the controller 20 acquires the sensed joint torque information of the joint part 100 for executing the subsequent estimation analysis step S20. The joint torque applied to the joint part 100 is detected from the joint torque measuring unit 4 implemented by the torque sensor of the detector 5, and the detected joint torque information is transmitted to the controller 21 and / or the observer of the controller 20. Forwarded to 233.

제어부(20)는 감지 단계에서 취득된 감지 관절 토크 정보 및 제어부(20)와 연계되는 저장부(30)에 사전 설정되어 저장된 추정 데이터에 기초하여 관절부(100)의 추정 관절 토크를 산출하고, 추정 관절 토크의 주파수 성분을 분석하는 추정 분석 단계(S20)를 실행한다. 추정 분석 단계(S20)는 외력 추정 토크 연산 단계(S21)와 주파수 분석 단계(S23)를 포함하는데, 외력 추정 토크 연산 단계(S21)에서 감지 토크 정보와 추정 데이터에 기초하여 제어부(20)의 추정 제어 신호에 따라 외력 추정 관측기(23)에서 외력 추정 토크가 연산되고, 주파수 분석 단계(S23)에서 외력 추정 토크에 기초하여 제어부(20)의 분석 제어 신호에 따라 외력 변화 추정 관측기에서 외력 추정 토크의 고주파 성분을 추출하여 외력 추정 고주파 토크를 산출한다. The control unit 20 calculates an estimated joint torque of the joint unit 100 based on the detected joint torque information acquired in the sensing step and the estimated data preset and stored in the storage unit 30 associated with the control unit 20. The estimation analysis step S20 of analyzing the frequency component of the joint torque is performed. The estimation analysis step S20 includes an external force estimation torque calculation step S21 and a frequency analysis step S23. The external force estimation torque calculation step S21 estimates the control unit 20 based on the detected torque information and the estimated data. The external force estimation torque is calculated by the external force estimation observer 23 according to the control signal, and the external force change torque is estimated by the external force change estimation observer according to the analysis control signal of the controller 20 based on the external force estimation torque in the frequency analysis step S23. The high frequency component is extracted to calculate an external force estimated high frequency torque.

먼저, 외력 추정 토크 연산 단계(S10)에서 외력 추정 토크가 연산된다. 주파수 기반 로봇 머니퓰레이터(10)의 관성 및 길이 등과 같은 모델 정보는 사전 설정되어 저장부(30)에 저장되는데, 주파수 기반 로봇 머니퓰레이터(10)는 다음과 같이 모델링될 수 있다(도 1 참조).First, the external force estimation torque is calculated in the external force estimation torque calculation step S10. Model information such as the inertia and the length of the frequency-based robotic manipulator 10 is preset and stored in the storage unit 30. The frequency-based robotic manipulator 10 may be modeled as follows (see FIG. 1).

여기서, M(q)는 관절부(100)를 통하여 연결되는 관절 링크의 관성 행렬을,

는 코리올리 및 구심력을 나타내는 행렬을, g(q)는 중력 벡터를, τ는 구동 모터(1)를 통하여 전달되는 감지 관절 토크를, τe는 외력 토크 벡터를 나타낸다. 여기서, τ는 상기한 바와 같이 토크 센서로 구현되는 관절 토크 측정부(4)를 통하여 감지되는 감지 신호가 사용될 수도 있고, 경우에 따라 구동 모터(1)에 인가되는 전류의 세기로부터 역추산될 수도 있다. 관절 위치 측정부(3) 및/또는 관절 속도 측정부(4)에서의 감지 정보를 활용하여 외력 토크 벡터(τe)를 추정할 수 있다.Here, M (q) is the inertia matrix of the joint link connected through the joint portion 100,

Denotes a matrix representing Coriolis and centripetal force, g (q) denotes a gravity vector, τ denotes sense joint torque transmitted through the drive motor 1, and τe denotes an external force torque vector. Here, τ may be used as a sensing signal sensed through the joint torque measuring unit 4 implemented as a torque sensor as described above, and in some cases it may be inferred from the intensity of the current applied to the drive motor (1) have. The external force torque vector tau e may be estimated using the sensing information of the joint position measuring unit 3 and / or the joint speed measuring unit 4.

경우에 따라 주파수 기반 로봇 머니퓰레이터(10)가 작업자에 의하여 사전 설정된 계획된 경로를 잘 추종하고 경로(

)가 시간에 대하여 미분 가능하다고 할 때, 다음과 같은 관계가 성립될 수 있다.In some cases, the frequency-based robotic manipulator 10 follows well the planned route preset by the operator,

) Is differentiable with respect to time, the following relationship can be established.

따라서, 이와 같은 경우 관절 속도 측정부(3)는 배제되는 구성이 취해질 수도 있고, 관절 속도 측정부(3)없이 임의의 경로를 이동하는 주파수 기반 로봇 머니퓰레이터(10)에 적용되는 외력 토크 벡터(τe)를 추정하는 관측기(23)는 다음과 같이 설계될 수 있다. Therefore, in such a case, a configuration in which the joint velocity measuring unit 3 may be excluded may be taken, and the external force torque vector τe applied to the frequency-based robotic manipulator 10 moving any path without the joint velocity measuring unit 3. The observer 23 estimating) may be designed as follows.

여기서, Mij와 Cij는 각각 M(q) 및

행렬의 i행j열의 요소이며, gi, τi, τe,i는 각각 g(q), τ, τe 벡터의 i번째 요소이다. 이때 Cij는 일반적으로 유일해를 갖지 않으므로, Cij를 다음과 같이 정의한다. Where Mij and Cij are M (q) and

It is an element of the i row j columns of the matrix, and gi, τ i, τ e, i are the i th elements of the g (q), τ, τ e vectors, respectively. Since Cij does not generally have a unique solution, Cij is defined as follows.

이와 같이 설정된 Cij는

가 교대 대칭 행렬(skew-symmmetric matrix)가 되도록 구성하는데, M(q)는 대칭이므로 다음과 같이 기술될 수 있다.Cij set like this is

Is configured to be a skew-symmmetric matrix. Since M (q) is symmetric, it can be described as follows.

본 실시예에 따른 외력 추정 토크 연산 단계(S21)는 아래와 같이 정의되는 일반화 운동량 p를 사용하여 가속도 정보없이 외력 추정 토크를 산출한다. The external force estimation torque calculation step S21 according to the present embodiment calculates the external force estimation torque without acceleration information by using the generalized momentum p defined as follows.

일반화 운동량 p를 시간에 대하여 미분하고, 상기에서 언급된 관계를 활용하여 다음과 같은 관계가 도출된다. Differentiate the generalized momentum p with respect to time and, using the above-mentioned relationship, the following relationship is derived.

여기서,

이다.here,

to be.

일반화 운동량의 추정치를

라 할 때,

이 실제 일반화 운동량 p에 수렴하도록 하기 위하여, 일반화 운동량의 추정 오차를 r로 정의한다. Estimate of generalized momentum

When we say

In order to converge to this actual generalized momentum p, the estimation error of the generalized momentum is defined as r.

여기서, K는 대각행렬로서 각 요소는 관측기(23)의 차단 주파수에 해당되고, 일반화 운동량 추정 오차(r)의 미분값은 다음과 같이 기술된다. Here, K is a diagonal matrix and each element corresponds to the cutoff frequency of the observer 23, and the derivative value of the generalized momentum estimation error r is described as follows.

이를 라플라스 변환을 이루면 다음과 같이 표현된다. The Laplace transform is expressed as follows.

여기서, 하첨자 i는 i번째 관절부(100)를 나타내고 ri는 i번째 관절부(100)에 작용하는 외력 추정치, 즉 추정 관절 토크를 나타내는데, 차단주파수인 Ki가 작으면 잡음에 강인하지만 반응속도가 느리고 Ki가 크면 잡음에 민감하지만 반응속도가 빠른 시스템이 구성된다. 따라서, Ki를 적절한 값을 선택하여 관절 토크 측정부(4) 또는 관절 위치 측정부(2) 등에서 감지되는 감지 정보의 잡음의 영향을 조정할 수 있다. Here, the subscript i represents the i-th joint part 100 and ri represents an external force estimate acting on the i-th joint part 100, i.e., estimated joint torque. Large Ki results in a noise sensitive but fast response system. Accordingly, by selecting an appropriate value for Ki, the influence of noise of the sensed information detected by the joint torque measuring unit 4 or the joint position measuring unit 2 may be adjusted.

상기와 같은 관계식들로부터 추정 관절 토크(r)를 재구성하면 다음과 같은 구조의 관측기를 구성할 수 있다. By reconstructing the estimated joint torque r from the relations as described above, an observer having the following structure can be constructed.

도 3b에 도시된 바와 같이, 상기와 같은 구조의 관측기(23)를 통하여 주파수 기반 로봇 머니퓰레이터(10)의 모델 정보와 관절부(100)의 위치(q), 속도(

), 감지 관절 토크(τ)를 사용하여 외력 토크의 추정치를 도출할 수 있다.As shown in FIG. 3B, the model information of the frequency-based robot manipulator 10, the position q of the joint part 100, and the velocity (

), The estimated joint torque τ can be used to derive an estimate of the external force torque.

이러한 관측기(23)를 통한 연산 과정을 통하여 얻어진 외력 추정 토크(r)에 기초하여 주파수 분석 단계(S23)에서 외력 추정 고주파 토크(rf)가 산출된다. 제어부(200)의 외력 변화 추정 관측기(23b)는 다음과 같이 구성된다.The external force estimation high frequency torque rf is calculated in the frequency analysis step S23 based on the external force estimation torque r obtained through the calculation process through the observer 23. The external force change estimation observer 23b of the control unit 200 is configured as follows.

여기서, KN은 관측 위치 측정기(2), 관측 속도 측정기(3) 및/또는 관측 토크 측정기(4)의 감지 잡음에 의한 양향을 제거하기 위한 이득을 나타내고, KS는 외력 변화 추정 관측기(23b)의 민감도(sensitivity)를 설정하기 위한 이득이다. 상기 관계를 일반화 운동량을 사용하여 정리하면 다음과 같이 표현될 수 있다.Here, KN represents a gain for removing the echo caused by the sensed noise of the observation position measuring device 2, the observation speed measuring device 3, and / or the observation torque measuring device 4, and KS denotes a gain of the external force change estimation observer 23b. This is the gain to set the sensitivity. Summarizing the relationship using the generalized momentum, it can be expressed as follows.

여기서, a1=KN+Ks, a2=KN KS, 그리고 b1=KN이다. 외력 변화 추정 관측기(23b)는 도 3c에 도시된 바와 같이 고주파 대역의 성분인 외력 고주파 토크(rf)를 추출함으로써, 외력의 크기가 동일하더라도 외력이 작업자가 인지하는 의도된 접촉에 의한 것인지 아니면 예기치 못한 충돌에 의한 것인지 외력 추정 고주파 토크를 사용하여 의도성 여부를 판단할 수 있다. Where a1 = KN + Ks, a2 = KN KS, and b1 = KN. The external force change estimator 23b extracts the external force high frequency torque rf, which is a component of the high frequency band, as shown in FIG. It may be determined whether the intention is due to the collision or the external force estimation high frequency torque.

도 2에는 충돌 상황을 인지한 경우의 충격량 변화 및 충돌 상황을 인지하지 못한 경우의 충격량 변화를 나타내는 선도가 도시된다. 즉, 도 2a에 도시된 바와 같이, 충돌 상황을 인지한 경우 충격량 변화가 완만한 곡선을 이루는 반면, 충돌 상황을 인지하지 못한 경우 충격량 변화는 급격하게 변화하는 곡선을 이룬다. 도 2c에는 주파수 기반 로봇 머니퓰레이터에 의도된 충격이 가해진 경우와 의도하지 않은 충격이 가해진 경우 감지된 감지 관절 토크의 변화가 도시되는데, 이는 도 2a 및 도 2b의 경우와 유사하다. 따라서, 상기한 바와 같은 외력 변화 추정 관측기(23b)는 고역 통과 필터 기능을 실행하여 고주파 성분의 외력 고주파 토크만을 추출하게 되고 완만한 변화를 이루는 의도된 충격은 제거된다. 2 is a diagram showing a change in impact amount when a collision situation is recognized and a change in impact amount when a collision situation is not recognized. That is, as illustrated in FIG. 2A, when the impact situation is recognized, the change in the amount of impact forms a gentle curve, while when the collision situation is not recognized, the change in the amount of impact forms a rapidly changing curve. FIG. 2C shows the change in sensed joint torque sensed when an intended shock is applied to the frequency-based robotic manipulator and when an unintended shock is applied, which is similar to the case of FIGS. 2A and 2B. Therefore, the external force change estimation observer 23b as described above performs the high pass filter function to extract only the external force high frequency torque of the high frequency component, and the intended impact of making a gentle change is eliminated.

그런 후, 제어부(20)의 제어기(21)는 모드 결정 단계(S30)를 실행한다. 모드 결정 단계에서 외력 추정 토크의 주파수 성분, 특히 고주파 성분인 외력 고주파 토크와 저장부(30)에 저장된 사전 설정 작업 기준 데이터에 기초하여 관절부(100)의 사전 설정된 작업 수행 여부를 판단하고 판단 결과에 따라 로봇 머니퓰레이터의 작업 모드를 설정한다. 모드 결정 단계(S30)는 모드 판단 단계(S31)와 모드 설정 단계(S33)를 포함하는데, 모드 판단 단계(S31)는 외력 추정 토크(r) 및 외력 추정 고주파 토크(rf)를 사전 설정 작업 기준 데이터와 비교하고, 모드 설정 단계(S33)는 모드 판단 단계(S31)에서 판단된 결과에 기초하여 실행되어야 할 작동 모드를 설정한다. Then, the controller 21 of the control unit 20 executes the mode determination step S30. In the mode determination step, it is determined whether the joint part 100 performs the preset work based on the frequency component of the external force estimation torque, in particular, the external force high frequency torque, which is a high frequency component, and the preset work reference data stored in the storage unit 30. Set the working mode of the robot manipulator accordingly. The mode determining step S30 includes a mode determining step S31 and a mode setting step S33, wherein the mode determining step S31 is based on a preset work reference based on the external force estimation torque r and the external force estimation high frequency torque rf. Compared with the data, the mode setting step S33 sets an operation mode to be executed based on the result determined in the mode determining step S31.

모드 판단 단계(S31)는 예상 충돌 판단 단계(S311)와 안전 판단 단계(S313)를 포함하는데, 예상 충돌 판단 단계(S311)에서 제어부(20)의 제어기(21)는 외력 추정 고주파 토크(rf)가 사전 설정 작업 기준 데이터의 고주파 문턱값(rf,th)을 비교한다. 제어부(20)의 제어기(21)가 외력 추정 고주파 토크(rf)가 고주파 문턱값(rf,th) 미만이라고 판단한 경우, 제어기(21)는 후속적으로 안전 판단 단계(S313)를 실행한다. 안전 판단 단계(S313)에서 제어기(21)는 외력 추정 토크(r)와 사전 설정 작업 기준 데이터의 안전 문턱값(rth)를 비교한다. 안전 판단 단계(S313)에서 제어기(21)가 외력 추정 토크와 사전 설정 작업 기준 데이터의 안전 문턱값(rth)과의 비교를 통해 각각의 비교 결과에 따라 모드 설정 단계(S33)의 개별 단계로 제어 흐름을 전달한다. 제어부(20)의 제어기(21)가 안전 판단 단계(S313)에서 외력 추정 토크(r)가 사전 설정 작업 기준 데이터의 안전 문턱값(rth) 이상이라고 판단한 경우 과도한 외력이 인가되는 것으로 판단하고 소정의 충돌 대응 동작을 실행하기 위한 제어 흐름을 선택하고 반면, 외력 추정 토크(r)가 사전 설정 작업 기준 데이터의 안전 문턱값(rth) 미만이라고 판단한 경우 안전 범위 이하의 외력이 인가되는 것으로 판단하고 소정의 설정된 작업을 수행하도록 하기 위한 제어 흐름을 선택한다. The mode determination step S31 includes an anticipated collision determination step S311 and a safety determination step S313. In the anticipated collision determination step S311, the controller 21 of the control unit 20 performs the external force estimation high frequency torque rf. Compares the high frequency thresholds (rf, th) of the preset working reference data. When the controller 21 of the controller 20 determines that the external force estimated high frequency torque rf is less than the high frequency threshold rf, th, the controller 21 subsequently executes the safety determination step S313. In the safety determination step S313, the controller 21 compares the external force estimation torque r with the safety threshold value rth of the preset work reference data. In the safety determination step (S313), the controller 21 controls the individual step of the mode setting step (S33) according to each comparison result by comparing the external force estimated torque with the safety threshold value rth of the preset work reference data. Convey flow. If the controller 21 of the control unit 20 determines that the external force estimation torque r is greater than or equal to the safety threshold value rth of the preset work reference data in the safety determination step S313, it is determined that excessive external force is applied and the predetermined If the control flow for executing the collision response operation is selected and the external force estimated torque r is determined to be less than the safety threshold value rth of the preset work reference data, it is determined that an external force below the safety range is applied and the predetermined Select the control flow to perform the set task.

모드 설정 단계(S33)는 작업 수행 모드 설정 단계(S331)와 충돌 대응 모드 설정 단계(S333)를 포함하는데, 작업 수행 모드 설정 단계(S331)는 안전 판단 단계(S313)에서 외력 추정 토크(r)가 안전 문턱값(rth) 미만인 경우 실행되어야 작업 모드로 사용자인 작업자에 의하여 설정된 입력 작업을 수행하도록 하는 작업 수행 모드를 설정한다. 반면, 충돌 대응 모드 설정 단계(S333)는 예상 충돌 판단 단계(S311)에서 외력 추정 고주파 토크(rf)가 고주파 문턱값(rf,th) 이상인 경우와, 안전 판단 단계(S313)에서 외력 추정 토크(r)가 안전 문턱값(rth) 미만인 경우 실행되어야 작업 모드로 충돌 상태에 대응하는 충돌 대응 모드를 작업 모드로 설정한다. The mode setting step S33 includes a work execution mode setting step S331 and a collision response mode setting step S333. The work execution mode setting step S331 includes the external force estimation torque r in the safety determination step S313. When the operation is less than the safety threshold (rth) is set to the operation mode to execute the input operation set by the operator as a user to the operation mode. On the other hand, in the collision response mode setting step S333, the external force estimation high frequency torque rf is equal to or higher than the high frequency threshold value rf and th in the anticipated collision determination step S311, and the external force estimation torque (S313) in the safety determination step S313. When r) is less than the safety threshold rth, the collision response mode corresponding to the collision state is set to the work mode to be executed.

모드 결정 단계(S30)가 완료된 후, 제어부(20)의 제어기(21)는 모드 실행 단계(S40)를 실행하는데, 먼저 모드 확인 단계(S41)에서 현재 제어부(20)의 제어기(21)에 의하여 선택된 작업 모드가 무엇인지를 판단한다. 모드 확인 단계(S41)에서 선택된 작업 모드가 작업 수행 모드인 것으로 판단되는 경우, 제어부(20)의 제어기(21)는 제어 흐름을 단계 S43으로 진행한다. 단계 S43은 작업자 명령 수신 단계(S431)와 수신 판단 단계(S433)와 작업 수행 단계(S435)를 포함하는데, 작업자 명령 수신 단계(S43)에서 작업자에 의하여 송신되어 수신된 명령이 존재하는지 여부를 판단한다. 즉, 도 1에 도시된 바와 같이, 주파수 기반 로봇 머니퓰레이터(10)는 통신부(50)를 구비하는데, 통신부(50)를 통하여 이격 배치되는 별도의 입력부(60)를 통한 신호 입력이 존재하는 경우 이를 수신한다. 그런 후, 수신 판단 단계(S433)에서 제어부(20)의 제어기(21)는 통신부(50)를 통하여 수신된 명령이 존재하는지 여부를 판단하는데, 수신된 명령이 존재하는 경우 제어부(20)의 제어기(21)는 제어 흐름을 단계 S437로 진행하여 수신된 명령에 따른 작업을 수행하도록 하는 작업 수행 단계(S437)를 실행하여 주파수 기반 로봇 머니퓰레이터(10)를 가동시킨다. 반면, 단계 S433에서 수신된 명령이 존재하지 않는 것으로 판단되는 경우, 제어부(20)의 제어기(21)는 제어 흐름을 단계 S435으로 전환하여 작업 계획 재생성 단계(S435)를 실행한다. 작업 계획 재생성 단계(S435)는 설계 사양에 따라 다양한 구성을 취할 수도 있는데, 사전 설정되어 저장부(30)에 저장된 사전 설정 작업 계획을 실행하도록 작업 계획을 생성할 수도 있고 바로 전 단계에서의 작업을 지속적으로 진행하도록 하는 구성을 취할 수도 있으면, 경우에 따라 대기 상태를 이룰 때 배치되는 원위치로의 복귀 계획으로 설정될 수도 있는 등 작업 환경 조건에 따라 다양한 선택이 가능하다.After the mode determination step S30 is completed, the controller 21 of the control unit 20 executes the mode execution step S40. First, in the mode checking step S41, the controller 21 of the current control unit 20 is executed. Determine what work mode is selected. When it is determined that the work mode selected in the mode check step S41 is the work execution mode, the controller 21 of the control unit 20 proceeds to step S43. Step S43 includes a worker command receiving step S431, a reception determining step S433, and a job performing step S435, which determine whether there is a command transmitted and received by the worker in the worker command receiving step S43. do. That is, as shown in Figure 1, the frequency-based robot manipulator 10 has a communication unit 50, if there is a signal input through a separate input unit 60 is spaced apart through the communication unit 50 Receive. Then, in the reception determination step (S433), the controller 21 of the controller 20 determines whether there is a command received through the communication unit 50, and if the received command exists, the controller of the controller 20. 21 executes the work execution step S437 for proceeding the control flow to step S437 to perform a job according to the received command to start the frequency-based robot manipulator 10. On the other hand, if it is determined that the command received in step S433 does not exist, the controller 21 of the control unit 20 switches the control flow to step S435 to execute the job plan regeneration step S435. The job plan regeneration step S435 may take various configurations according to the design specification. The job plan may be generated to execute a preset job plan that is preset and stored in the storage unit 30, or the job in the previous step may be generated. If it is possible to take the configuration to proceed continuously, various choices are possible depending on the working environment conditions, such as the case may be set to return to the original position arranged when the standby state.

한편, 모드 확인 단계(S41)에서 선택된 작업 모드가 충돌 대응 모드인 것으로 판단되는 경우, 제어부(20)의 제어기(21)는 제어 흐름을 단계 S45로 진행한다. 단계 S45는 충돌 방향 검출 단계(S451)와 충돌 위치 확인 단계(S453)와 대응 위치 결정 단계(S455)와 충돌 대응 단계(S457)를 포함한다. 충돌 방향 검출 단계(S451)에서 제어부(20)의 제어기(21)는 관절부(100)의 감지 관절 토크 정보로부터 충돌 방향을 검출하는데, 주파수 기반 머니퓰레이터(10)의 관절부(100)에서의 감지 관절 토크 정보로부터 시계 방향 내지 반시계 방향 인지 여부를 판단하여 방향성(d)을 도출한다. 본 실시예에서는 시계 방향을 "1"로 그리고 반시계 방향을 "-1"의 부호를 부여하였으나 반대되는 구조를 취할 수도 있음은 명백하다.On the other hand, when it is determined that the work mode selected in the mode check step S41 is the collision response mode, the controller 21 of the control unit 20 proceeds to the control flow to step S45. Step S45 includes a collision direction detecting step S451, a collision position checking step S453, a corresponding position determining step S455, and a collision correspondence step S457. In the collision direction detection step S451, the controller 21 of the controller 20 detects the collision direction from the sensed joint torque information of the joint part 100, and senses the joint torque at the joint part 100 of the frequency-based manipulator 10. From the information, it is determined whether it is clockwise or counterclockwise to derive the directionality d. In the present embodiment, the clockwise direction is designated as "1" and the counterclockwise direction is denoted by "-1", but it is obvious that the opposite structure may be taken.

그런 후, 제어부(20)의 제어기(21)는 관절부(100)의 감지 관절 토크 정보로부터 관절부(100)의 충돌 위치를 확인하는데, 구동 모터를 통하여 인가된 토크 신호와는 다른 감지 관절 토크 정보의 변화가 발생하는 관절부를 확인하여 해당 관절부의 위치(qc)를 충돌 위치로 선정한다. 그런 후, 제어부(20)의 제어기(21)는 저장부(30)에 사전 설정 대응되는 사전 설정 대응 위치(qr)만큼 관절부(100)를 충돌 방향과 반대 방향으로 가동시키는 대응 위치(qd)를 산출하는 대응 위치 결정 단계(S455)를 실행한다. 대응 위치 결정 단계(S455)에서 사용되는 사전 설정 대응 위치(q)는 본 실시예에서 5도 정도로 설정되었는데, 본 발명의 사전 설정 대응 위치는 이에 국한되지 않고 설계 사양에 따라 다양한 변형이 가능하다. 따라서, 대응 위치 결정 단계(S455)에서 산출되는 대응 위치(qd)는 다음과 같은 관계를 갖는다.Then, the controller 21 of the control unit 20 confirms the collision position of the joint unit 100 from the detected joint torque information of the joint unit 100, which is different from the torque signal applied through the driving motor. Check the joint where the change occurs and select the position qc of the joint as the collision position. Then, the controller 21 of the control unit 20 sets the corresponding position qd for moving the joint part 100 in the direction opposite to the collision direction by the preset corresponding position qr corresponding to the storage unit 30. The corresponding positioning step (S455) to be calculated is executed. The preset correspondence position q used in the correspondence positioning step S455 is set to about 5 degrees in this embodiment, but the preset correspondence position of the present invention is not limited thereto, and various modifications may be made according to design specifications. Accordingly, the correspondence position qd calculated in the correspondence position determination step S455 has the following relationship.

상기와 같은 대응 위치(qd)가 산출된 후, 제어부(20)의 제어기(21)는 구동 모터(1)에 구동 제어 신호를 인가하여 소정의 충돌 상황으로 인한 손상 내지 피해를 방지하기 위하여 충돌 대응 동작을 실행하는 충돌 대응 단계(S457)를 실시한다.
After the corresponding position qd is calculated as described above, the controller 21 of the controller 20 applies a driving control signal to the driving motor 1 to prevent damage or damage due to a predetermined collision situation. The collision response step S457 of executing the operation is performed.

상기한 바와 같이, 본 발명에 따른 주파수 기반 로봇 머니퓰레이터 충돌 대응 제어 방법은, 감지 관절 토크의 고주파 성분을 추출하여 이를 통한 충돌 상태의 의도성 여부를 판단하는 범위에서 다양한 변화를 도출하는 다양한 구성이 가능하다. As described above, the frequency-based robotic manipulator collision response control method according to the present invention, a variety of configurations can be derived to extract a high frequency component of the sense joint torque to derive various changes in the range to determine the intention of the collision state through it Do.

1...구동 모터 2...관절 위치 측정부
3...관절 속도 측정부 4...관절 토크 측정부
5...감지부 10...주파수 기반 로봇 머니퓰레이터
20...제어부 30...저장부
40...연산부 50...통신부
60...입력부1 ... drive motor 2 ... joint position measuring unit
3 ... joint speed measurement unit 4 ... joint torque measurement unit
5 ... detector 10 ... frequency-based robot manipulator
20 ... control section 30 ... storage section
40.Calculation unit 50 ... Communication unit
60 ... input

Claims

하나 이상의 구동 모터를 통하여 가동되는 하나 이상의 관절부를 구비하는 로봇 머니퓰레이터의 제어부의 감지 제어 신호에 따라 상기 관절부의 감지 관절 토크 정보를 취득하는 감지 단계와,
상기 감지 단계에서 취득된 상기 감지 관절 토크 정보 및 상기 제어부와 연결되는 저장부에 사전 설정 저장된 추정 데이터에 기초하여 상기 관절부의 추정 관절 토크가 산출되고, 상기 추정 관절 토크의 주파수 성분이 분석되는 추정 분석 단계와,
상기 추정 관절 토크의 주파수 성분과 상기 저장부에 저장된 사전 설정 작업 기준 데이터에 기초하여 상기 관절부의 사전 설정된 작업 수행 여부를 판단하고 판단 결과에 따라 상기 로봇 머니퓰레이터의 작업 모드를 설정하는 모드 결정 단계와,
상기 제어부의 모드 실행 제어 신호에 따라 상기 모드 결정 단계에서 결정된 모드를 실행하는 모드 실행 단계를 포함하는 주파수 기반 로봇 머니퓰레이터 제어 방법.A sensing step of acquiring sensing joint torque information of the joint part according to a sensing control signal of a controller of the robot manipulator having at least one joint part operated through at least one driving motor;
Estimated analysis in which an estimated joint torque of the joint part is calculated based on the sensed joint torque information acquired in the sensing step and estimated data preset in a storage unit connected to the control unit, and a frequency component of the estimated joint torque is analyzed. Steps,
A mode determination step of determining whether to perform the preset work of the joint part based on the frequency component of the estimated joint torque and the preset work reference data stored in the storage unit, and setting a working mode of the robot manipulator according to the determination result;
And a mode execution step of executing a mode determined in the mode determination step according to a mode execution control signal of the controller.

제 1항에 있어서,
상기 추정 분석 단계는:
상기 감지 관절 토크 정보와 상기 추정 데이터에 기초하여 상기 제어부의 추정 제어 신호에 따라 외력 추정 관측기에서 외력 추정 토크가 연산되는 외력 추정 토크 연산 단계와,
상기 외력 추정 토크에 기초하여 상기 제어부의 분석 제어 신호에 따라 외력 변화 추정 관측기에서 상기 외력 추정 토크의 고주파 성분을 추출하여 외력 추정 고주파 토크를 산출하는 주파수 분석 단계를 포함하는 것을 특징으로 하는 주파수 기반 로봇 머니퓰레이터 제어 방법.The method of claim 1,
The estimation analysis step is:
An external force estimation torque calculation step of calculating an external force estimation torque in an external force estimation observer according to the estimated control signal of the controller based on the sensed joint torque information and the estimated data;
And a frequency analysis step of extracting a high frequency component of the external force estimation torque from an external force change estimation observer and calculating an external force estimation high frequency torque based on the analysis control signal of the controller based on the external force estimation torque. Manipulator control method.

제 2항에 있어서,
상기 모드 결정 단계는:
상기 외력 추정 토크 및 상기 외력 추정 고주파 토크를 상기 사전 설정 작업 기준 데이터와 비교하는 모드 판단 단계와,
상기 모드 판단 단계에서 판단된 결과에 기초하여 실행되어야 할 작동 모드를 설정하는 모드 설정 단계를 포함하는 것을 특징으로 하는 주파수 기반 로봇 머니퓰레이터 제어 방법.The method of claim 2,
The mode determination step is:
A mode determination step of comparing the external force estimation torque and the external force estimation high frequency torque with the preset work reference data;
And a mode setting step of setting an operation mode to be executed based on the result determined in the mode determining step.

제 3항에 있어서,
상기 모드 판단 단계는:
상기 제어부가 상기 외력 추정 고주파 토크와 상기 사전 설정 작업 기준 데이터의 고주파 문턱값을 비교하는 예상 충돌 판단 단계와,
상기 예상 충돌 판단 단계에서 상기 외력 추정 고주파 토크가 상기 고주파 문턱값 미만인 경우, 상기 제어부가 상기 외력 추정 토크와 상기 사전 설정 작업 기준 데이터의 안전 문턱값을 비교하는 안전 판단 단계를 포함하는 것을 특징으로 하는 주파수 기반 로봇 머니퓰레이터 제어 방법.The method of claim 3,
The mode determination step is:
An expected collision determination step of the control unit comparing the external force estimation high frequency torque with a high frequency threshold of the preset working reference data;
And in the predicted collision determination step, when the external force estimated high frequency torque is less than the high frequency threshold value, the controller includes a safety judgment step of comparing the external force estimated torque with a safety threshold value of the preset work reference data. Frequency-based robot manipulator control method.

제 4항에 있어서,
상기 모드 설정 단계는:
상기 안전 판단 단계에서 상기 외력 추정 토크가 상기 안전 문턱값 미만인 경우 실행되어야 할 작업 모드로 사용자에 의하여 설정된 입력 작업을 수행하는 작업 수행 모드로 설정하는 작업 수행 모드 설정 단계와,
상기 예상 충돌 판단 단계에서 상기 외력 추정 고주파 토크가 상기 고주파 문턱값 이상이 경우와, 상기 안전 판단 단계에서 상기 외력 추정 토크가 상기 안전 문턱값 이상인 경우에 실행되어야 할 작업 모드로 충돌 대응 모드를 설정하는 충돌 대응 모드 설정 단계를 포함하는 것을 특징으로 하는 주파수 기반 로봇 머니퓰레이터 제어 방법.5. The method of claim 4,
The mode setting step is:
A work execution mode setting step of setting a work execution mode to perform an input operation set by a user as a work mode to be executed when the external force estimation torque is less than the safety threshold in the safety determination step;
Setting the collision response mode to a work mode to be executed when the external force estimated high frequency torque is greater than or equal to the high frequency threshold in the anticipated collision determination step and when the external force estimated torque is greater than or equal to the safety threshold value in the safety determining step. And a collision response mode setting step.

제 5항에 있어서,
상기 작업 수행 모드 설정 단계에서 실행되어야 할 작업 모드가 작업 수행 모드로 설정된 경우,
상기 모드 실행 단계는:
입력부를 통하여 작업자에 의한 명령을 수신하는 작업자 명령 수신 단계와,
상기 작업자 명령 수신 단계에서 작업자에 의한 수신된 명령이 존재하는지 여부를 판단하는 수신 판단 단계와,
상기 수신 판단 단계에서의 수신 여부에 따라 형성된 작업을 실행하는 작업 수행 단계를 포함하는 것을 특징으로 하는 주파수 기반 로봇 머니퓰레이터 제어 방법.6. The method of claim 5,
If the work mode to be executed in the work execution mode setting step is set to the work execution mode,
The mode execution step is:
A worker command receiving step of receiving a command by an operator through an input unit,
A reception determination step of determining whether a command received by the worker exists in the worker command reception step;
And a task execution step of executing a task formed according to the reception in the reception determination step.

제 5항에 있어서,
상기 작업 수행 모드 설정 단계에서 실행되어야 할 작업 모드가 충돌 상태로 판단하여 대응 실행이 요구되는 충돌 대응 모드로 설정된 경우,
상기 모드 실행 단계는:
상기 관절부의 감지 관절 토크 정보로부터 충돌 방향을 검출하는 충돌 방향 검출 단계와,
상기 제어부가 상기 관절부의 감지 관절 토크 정보로부터 상기 관절부의 충돌 위치를 확인하는 충돌 위치 확인 단계와,
상기 제어부가 상기 저장부에 저장된 사전 설정 대응 위치만큼 상기 관절부를 상기 충돌 방향과 반대 방향으로 가동시키는 대응 위치를 산출하는 대응 위치 결정 단계와,
상기 구동 모터에 상기 관절부의 상기 대응 위치로의 이동을 위한 구동 제어 신호를 인가하는 충돌 대응 단계를 포함하는 것을 특징으로 하는 주파수 기반 로봇 머니퓰레이터 제어 방법.6. The method of claim 5,
When the work mode to be executed in the work execution mode setting step is determined to be a collision state and is set to the collision response mode in which a corresponding execution is required,
The mode execution step is:
A collision direction detection step of detecting a collision direction from the sensed joint torque information of the joint part;
A collision position checking step of checking, by the controller, a collision position of the joint part from the sensed joint torque information of the joint part;
A corresponding position determining step of calculating, by the control unit, a corresponding position for moving the joint part in a direction opposite to the collision direction by a preset corresponding position stored in the storage unit;
And a collision corresponding step of applying a drive control signal for movement to the corresponding position of the joint part to the drive motor.

제 1항에 있어서,
상기 감지 관절 토크 정보는 감지부의 관절 토크 측정부에서 감지되는 것을 특징으로 하는 주파수 기반 로봇 머니퓰레이터 제어 방법.

The method of claim 1,
The sensing joint torque information is detected by the joint torque measuring unit of the sensing unit, characterized in that the robot manipulator control method.