KR101973709B1 - Method of collision detection of robot arm manipulator - Google Patents
Method of collision detection of robot arm manipulator Download PDFInfo
- Publication number
- KR101973709B1 KR101973709B1 KR1020160150474A KR20160150474A KR101973709B1 KR 101973709 B1 KR101973709 B1 KR 101973709B1 KR 1020160150474 A KR1020160150474 A KR 1020160150474A KR 20160150474 A KR20160150474 A KR 20160150474A KR 101973709 B1 KR101973709 B1 KR 101973709B1
- Authority
- KR
- South Korea
- Prior art keywords
- robot manipulator
- collision
- mode
- torque
- operation type
- Prior art date
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/02—Sensing devices
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
본 발명은, 구동 모터를 통하여 가동되는 관절부를 구비하고 힘제어 동작을 이루는 로봇 머니퓰레이터의 제어부가 상기 제어부와 연결되는 저장부에 사전 설정 저장되는 사전 설정 데이터 및 상기 제어부의 감지 제어 신호에 따라 감지부가 취득하는 상기 관절부의 회전각을 포함하는 감지 관절 정보에 기초하여, 상기 로봇 머니퓰레이터에 인가되는 외력 토크(τext)를 산출하는 외력 토크 산출 단계(S20)와, 상기 제어부가 상기 로봇 머니퓰레이터에 인가하고 상기 사전 설정 데이터에 포함되는 작업 유형에 따라, 상기 제어부가 구비하고 상기 로봇 머니퓰레이터의 충돌 여부를 판단하기 위한 충돌 감지기를 산출하는 충돌 감지기 산출 단계(S30)와, 상기 제어부가 상기 충돌 감지기와 상기 사전 설정 데이터에 포함되는 충돌 판단 기준값과 비교하여 상기 로봇 머니퓰레이터의 충돌 여부를 확인하고 작업 유형을 수행하는 작업 수행 모드와 충돌 상황에 대응하는 충돌 모드 중 실행되어야 할 실행 모드를 설정하는 충돌 판단 단계(S40)와, 상기 충돌 판단 단계(S40)에서의 판단 결과 및 설정 모드에 따라 해당 모드를 실행하는 모드 실행 단계(S50)를 포함하는 것을 특징으로 하는 로봇 머니퓰레이터 제어 방법을 제공한다.According to the present invention, there is provided a robot manipulator including a joint portion that is actuated through a driving motor, and a control unit of a robot manipulator that performs a force control operation is preset in a storage unit connected to the control unit, An external force torque calculating step (S20) of calculating an external force torque? Ext applied to the robot manipulator based on the sensing joint information including the rotation angle of the joint part to be acquired; A collision detector calculation step (S30) for calculating a collision detector, provided in the control part, for determining whether the robot manipulator collides with the operation type included in the preset data; and a controller The collision determination reference value included in the data, A collision determination step (S40) of determining a collision state of the bot manipulator and setting an execution mode to be executed among a collision mode corresponding to a collision situation and a work execution mode for performing a task type; And a mode execution step (S50) of executing the corresponding mode according to the determination result and the setting mode.
Description
본 발명은 로봇 머니퓰레이터 팔에 발생하는 물리적인 충돌을 감지하는 방법에 관한 것으로, 더욱 상세하게는 작업 수행을 위해서 의도적으로 로봇 머니퓰레이터에 힘이 인가되는 상황에서도 충돌을 감지하는 방법에 관한 것이다.The present invention relates to a method of detecting a physical collision occurring in a robot manipulator arm, and more particularly, to a method of detecting a collision even when a force is applied to a robot manipulator intentionally for performing an operation.
인간과 작업공간을 공유하는 로봇 머니퓰레이터가 작업자 또는 작업환경과 충돌할 경우에 작업자의 상해, 로봇 머니퓰레이터의 고장, 작업물의 파손 등과 같은 다양한 문제가 발생할 수 있다. 본 발명은 이러한 문제를 해결하기 위한 것으로, 위치 및 속도 제어를 수행하는 로봇 머니퓰레이터가 스스로 충돌을 감지하는 방법에 관한 것이다. 따라서 본 발명은 로봇 머니퓰레이터의 제어 기술이다. 또한, 로봇 머니퓰레이터가 충돌을 감지하여 적절한 대응을 취함으로써 작업자의 안전을 보장할 수 있다. 따라서 본 발명은 로봇 머니퓰레이터의 안전기술이다. When a robot manipulator that shares a work space with a human collides with an operator or a work environment, various problems such as an injury of the operator, a failure of the robot manipulator, and damage to the workpiece may occur. SUMMARY OF THE INVENTION The present invention is directed to solving such a problem, and relates to a method of detecting a collision by a robot manipulator performing position and speed control by itself. Therefore, the present invention is a control technique of a robot manipulator. In addition, the robot manipulator can detect the collision and take appropriate action to ensure the safety of the operator. Therefore, the present invention is a safety technology of a robot manipulator.
로봇 머니퓰레이터가 수행하는 작업은 여러 종류가 있으며, 작업을 수행하기 위하여 로봇 머니퓰레이터가 외부환경과 물리적인 힘을 교환하는 경우가 있다. 이러한 경우에는 로봇 머니퓰레이터가 정상적인 작업을 수행함에도 불구하고, 외부환경으로부터 로봇 머니퓰레이터에 외력이 작용한다. 예를 들어, 로봇 머니퓰레이터가 외부환경과 접촉하여 힘 제어를 수행하는 경우에 로봇 머니퓰레이터가 외부환경에 인가한 힘의 반력이 로봇 머니퓰레이터에 작용한다.There are many kinds of operations performed by the robot manipulator. In order to perform the task, the robot manipulator sometimes exchanges physical force with the external environment. In this case, an external force acts on the robot manipulator from the external environment even though the robot manipulator performs a normal operation. For example, when the robot manipulator is in contact with the external environment to perform force control, reaction force of force applied to the external environment by the robot manipulator acts on the robot manipulator.
한편, 종래의 충돌감지 기술은 로봇 머니퓰레이터에 인가된 외력은 외부환경과의 충돌에 의한 충격력이라 가정하므로, 종래의 충돌감기 기술을 로봇 머니퓰레이터에 적용할 경우에 외력의 발생 원인과 무관하게 로봇 머니퓰레이터는 모든 외력을 충격력으로 간주한다. 즉, 종래의 충돌감지 기술을 외부환경과 물리적인 상호작용을 수행하는 로봇 머니퓰레이터에 적용할 경우에 로봇 머니퓰레이터가 정상적으로 운행됨에도 불구하고, 로봇 머니퓰레이터가 작업 수행을 위해서 로봇 머니퓰레이터에 작용하는 외력을 충격력으로 감지할 수 있다. 따라서 종래의 충돌감지 기술은 외부환경과 물리적인 접촉을 수행하지 않는 로봇 머니퓰레이터에만 적용할 수 있었으며, 이러한 문제를 해결하기 위한 새로운 기술의 개발이 필요하다.On the other hand, in the conventional collision sensing technology, when the external force applied to the robot manipulator is an impact force due to a collision with the external environment, when the conventional collision winding technique is applied to the robot manipulator, All external forces are considered to be impact forces. That is, when the conventional collision detection technology is applied to a robot manipulator that physically interacts with the external environment, although the robot manipulator is operated normally, the external force acting on the robot manipulator Can be detected. Therefore, the conventional collision detection technology can be applied only to a robot manipulator that does not make physical contact with the external environment, and it is necessary to develop a new technique to solve such a problem.
로봇 머니퓰레이터가 작업을 수행하는 외부환경의 수학적 모델이 완벽하게 주어진 경우에는 이 모델을 이용하여 외부환경으로부터 로봇 머니퓰레이터에 가해지는 외력을 계산할 수 있으며, 이 경우에는 외부환경의 수학적 모델을 바탕으로 계산된 외력과 실제 로봇 머니퓰레이터에 인가되고 있는 외력을 비교함으로써 충돌을 감지할 수 있다. 예를 들어, 파지한 물체의 동역학 모델을 정확히 알고 있는 경우에는 이 동역학 모델을 이용하여 파지된 물체에 작용하는 중력과 관성력을 계산함으로써, 파지된 물체를 이송하는 로봇 머니퓰레이터에 인가되는 외력을 산출하고, 이를 로봇 머니퓰레이터에 가해진 외력과 비교할 수 있다. 하지만 실제 산업 및 서비스 현장에서 외부환경의 수학적 모델을 로봇 머니퓰레이터의 사용자가 알고 있는 경우는 매우 드물며, 로봇 머니퓰레이터가 다양한 외부환경에서 작업을 수행하는 경우에는 이러한 방법을 적용하는 것이 현실적으로 불가능하다. 따라서 외부환경에 대한 수학적 모델이 없는 상황에서도 충돌을 감지할 수 있는 기술의 발명이 필요하다. If the mathematical model of the external environment in which the robot manipulator is performing is given perfectly, then this model can be used to calculate the external force applied to the robot manipulator from the external environment. In this case, The collision can be detected by comparing the external force with the external force applied to the actual robot manipulator. For example, when the kinetic model of the gripped object is known accurately, the external force applied to the robot manipulator for transferring the gripped object is calculated by calculating gravity and inertial force acting on the gripped object using the kinetic model , It can be compared with the external force applied to the robot manipulator. However, it is very rare that the user of the robot manipulator knows the mathematical model of the external environment at the actual industrial and service sites. It is practically impossible to apply this method when the robot manipulator performs work in various external environments. Therefore, it is necessary to invent a technique capable of detecting a collision without a mathematical model for the external environment.
로봇 머니퓰레이터에 6축 힘/토크 센서를 부착하면, 작업 중 외부환경으로부터 로봇 머니퓰레이터에 인가되는 외력을 측정할 수 있으며, 이렇게 측정된 외력과 로봇 머니퓰레이터에 인가되는 외력을 비교함으로써 충돌의 발생 여부를 알 수 있다. 그러나 6축 힘/토크 센서는 매우 고가일 뿐만 아니라, 일반적으로 단순 위치 및 속도 제어를 수행하는 로봇 머니퓰레이터에는 이러한 6축 힘/토크 센서가 구비되어 있지 않다. 따라서 고가의 6축 힘/토크 센서를 사용하지 않고, 저가의 관절 토크센서를 사용하거나 센서를 사용하지 않고 충돌을 감지할 수 있는 기술의 발명이 필요하다. By attaching a six-axis force / torque sensor to the robot manipulator, it is possible to measure the external force applied to the robot manipulator from the external environment during the operation. By comparing the external force thus measured and the external force applied to the robot manipulator, . However, the six-axis force / torque sensor is not only very expensive, but also has a six-axis force / torque sensor in a robot manipulator that generally performs simple position and speed control. Therefore, there is a need for an invention of a technology capable of detecting a collision without using a costly six-axis force / torque sensor, using a low-cost joint torque sensor, or without using a sensor.
작업의 수행을 위해서 의도적으로 로봇 머니퓰레이터에 인가되는 외력이 비교적 저주파이고, 예기치 못한 충돌로 인하여 로봇 머니퓰레이터에 인가되는 충격력은 고주파라 가정하면, 로봇 머니퓰레이터에 인가된 외력의 발생 원인을 주파수를 기반으로 구분할 수 있다. 즉, 로봇 머니퓰레이터에 인가된 외력을 고역통과필터에 통과시켜 외력에 포함된 저주파 성분을 제거함으로써 충격력을 검출할 수 있다. 하지만 작업의 수행을 위해서 로봇 머니퓰레이터에 의도적으로 인가되는 외력은 로봇 머니퓰레이터가 수행하는 작업, 경로 생성 방법에 따라서 그 주파수의 영역이 매우 상이하다. 또한, 로봇 머니퓰레이터가 저속으로 운행되는 경우에는 저주파의 충격력이 로봇 머니퓰레이터에 가해질 수 있다. 따라서 이러한 주파수 기반의 충돌감지 방법은 적용범위가 매우 제한적인 방법이며, 충격력의 주파수와 무관하게 모든 충돌을 감지할 수 있는 기술의 발명이 필요하다.Assuming that the external force applied to the robot manipulator is intentionally low and the impact force applied to the robot manipulator due to unexpected impact is high frequency, the cause of the external force applied to the robot manipulator is classified based on the frequency . That is, the impact force can be detected by passing the external force applied to the robot manipulator through the high-pass filter to remove the low-frequency component contained in the external force. However, the external force intentionally applied to the robot manipulator for performing the task is very different according to the work performed by the robot manipulator and the path generation method. In addition, when the robot manipulator is operated at a low speed, a low frequency impact force can be applied to the robot manipulator. Therefore, this frequency - based collision detection method has a very limited range of application, and it is necessary to invent a technology capable of detecting all collisions regardless of the frequency of the impact force.
본 발명의 목적은 상기된 문제점들을 해결하기 위한 것으로, 로봇 머니퓰레이터가 미지의 물체를 파지한 상태에서 충돌로 인하여 로봇 머니퓰레이터에 가해진 외력을 감지하는 충돌 감지기를 발명하는 것이다. 이를 위하여 로봇 머니퓰레이터에 인가된 외력 중 로봇 머니퓰레이터의 작업 수행을 위하여 정상적으로 로봇 머니퓰레이터에 인가된 외력에는 무관하며 충격력에만 반응하는 충돌 감지 방법을 고안한다. 고안된 방법은 로봇 머니퓰레이터가 파지한 물체의 동역학 정보를 사용하지 않아 로봇 머니퓰레이터가 미지의 물체를 파지하는 경우에도 적용 가능하며, 6축 힘/토크 센서를 사용하지 않아 다양한 로봇 머니퓰레이터에 적용이 가능하다. 또한, 충격력의 주파수와 무관하게 모든 충돌을 감지할 수 있도록 한다. SUMMARY OF THE INVENTION An object of the present invention is to solve the above problems and to provide a collision detector for detecting an external force applied to a robot manipulator due to a collision while a robot manipulator grasps an unknown object. For this purpose, a collision detection method which is independent of the external force normally applied to the robot manipulator and reacts only to the impact force is devised for performing the operation of the robot manipulator among the external forces applied to the robot manipulator. The proposed method can be applied to the case where the robot manipulator does not use the dynamics information of the object held by the robot manipulator, and can be applied to various robot manipulators without using the 6-axis force / torque sensor. Also, it can detect all collisions irrespective of the frequency of the impact force.
전술한 목적을 달성하기 위한 본 발명은, 로봇 머니퓰레이터가 물체를 이송하는 작업을 수행하는 데 있어서, 로봇 머니퓰레이터가 파지한 물체에 의해서 로봇 머니퓰레이터의 각 관절에 인가되는 외부토크에는 반응하지 않고, 로봇 머니퓰레이터과 외부환경과의 충돌에만 반응하는 충돌감지기를 고안하는 과정을 제공한다. According to an aspect of the present invention, there is provided a robot manipulator for performing an operation of transferring an object, the robot manipulator not responding to an external torque applied to each joint of the robot manipulator by an object held by the robot manipulator, It provides a process of devising a collision detector that only responds to collisions with the external environment.
또한, 본 발명은 로봇 머니퓰레이터가 외부환경과 접촉하는 작업을 수행하는 데 있어서, 로봇 머니퓰레이터의 말단부에 장착된 작업공구와 외부환경 사이의 접촉력으로 인하여 로봇 머니퓰레이터의 각 관절에 인가되는 외부토크와는 무관하고, 충돌에만 반응하는 충돌감지기를 고안하는 과정을 제공한다. In addition, the present invention relates to a robot manipulator for performing a contact operation with an external environment, which is independent of an external torque applied to each joint of a robot manipulator due to a contact force between a working tool mounted on a distal end portion of the robot manipulator and an external environment And provides a process of devising a collision detector that only responds to collisions.
또한, 본 발명의 충돌 감지기를 고안하는 과정은 로봇 머니퓰레이터의 작업수행을 위해서 로봇 머니퓰레이터 관절에 인가되는 외부토크를 해당 외부토크가 존재하는 선형공간과 수직한 공간으로 투영함으로써 소거한다는 점을 제공할 수도 있다. The process of devising the collision detector of the present invention may also provide that the external torque applied to the robot manipulator joint is erased by projecting the external torque into a space perpendicular to the linear space in which the external torque exists have.
또한, 본 발명의 로봇 머니퓰레이터 각 관절에 인가되는 외부토크를 투영하기 위한 투영행렬은 로봇 머니퓰레이터가 물체를 파지함으로써 로봇 머니퓰레이터의 말단에 인가되는 중력과 관성력의 방향을 이용하여 산출된다는 점을 제공할 수도 있다. In addition, the projection matrix for projecting the external torque applied to each joint of the robot manipulator of the present invention may be provided by using the direction of gravity and inertial force applied to the end of the robot manipulator by grasping the object by the robot manipulator have.
또한, 본 발명의 로봇 머니퓰레이터를 제어하는 과정은 로봇 머니퓰레이터가 수행하는 작업을 세 가지 경우로 구분하여 작업에 따라 각각 다른 충돌 감지기를 사용한다는 점을 제공할 수도 있다. In addition, the process of controlling the robot manipulator of the present invention may be divided into three types of tasks performed by the robot manipulator, and each of them may be provided with a different collision detector depending on the task.
즉, 본 발명은, 구동 모터를 통하여 가동되는 관절부를 구비하고 힘제어 동작을 이루는 로봇 머니퓰레이터의 제어부가 상기 제어부와 연결되는 저장부에 사전 설정 저장되는 사전 설정 데이터 및 상기 제어부의 감지 제어 신호에 따라 감지부가 취득하는 상기 관절부의 회전각을 포함하는 감지 관절 정보에 기초하여, 상기 로봇 머니퓰레이터에 인가되는 외력 토크(τext)를 산출하는 외력 토크 산출 단계(S20)와, 상기 제어부가 상기 로봇 머니퓰레이터에 인가하고 상기 사전 설정 데이터에 포함되는 작업 유형에 따라, 상기 제어부가 구비하고 상기 로봇 머니퓰레이터의 충돌 여부를 판단하기 위한 충돌 감지기를 산출하는 충돌 감지기 산출 단계(S30)와, 상기 제어부가 상기 충돌 감지기와 상기 사전 설정 데이터에 포함되는 충돌 판단 기준값과 비교하여 상기 로봇 머니퓰레이터의 충돌 여부를 확인하고 작업 유형을 수행하는 작업 수행 모드와 충돌 상황에 대응하는 충돌 모드 중 실행되어야 할 실행 모드를 설정하는 충돌 판단 단계(S40)와, 상기 충돌 판단 단계(S40)에서의 판단 결과 및 설정 모드에 따라 해당 모드를 실행하는 모드 실행 단계(S50)를 포함하는 것을 특징으로 하는 로봇 머니퓰레이터 제어 방법을 제공한다.That is, according to the present invention, a control unit of a robot manipulator having a joint part that is operated through a driving motor and performs a force control operation is provided with preset data stored in advance in a storage unit connected to the control unit, An external force torque calculating step (S20) of calculating an external force torque (? Ext) to be applied to the robot manipulator based on the sensing joint information including the rotation angle of the joint part acquired by the sensing unit (S30) for calculating a collision detector for determining whether or not the robot manipulator collides with the control unit, according to a job type included in the preset data, and a control unit for controlling the collision detector In comparison with the collision judgment reference value included in the preset data, A collision determination step (S40) of determining a collision state of the robot manipulator, setting a work execution mode for performing a task type and an execution mode to be executed among collision modes corresponding to the collision situation, And a mode execution step (S50) of executing the corresponding mode according to the determination result of the robot manipulator and the setting mode.
상기 로봇 머니퓰레이터 제어 방법에 있어서, 상기 외력 토크 산출 단계(S20)는: 상기 구동 모터에 인가되는 모터 전류 및 상기 관절부의 회전각을 포함하는 감지 관절 정보를 감지하는 외력 토크 산출 감지 단계(S21)와, 상기 모터 전류, 상기 감지 관절 정보 및 상기 사전 설정 데이터에 기초하여, 상기 로봇 머니퓰레이터에 인가되는 외력 토크를 확정하는 외력 토크 산출 확정 단계(S23)를 포함할 수도 있다. In the robot manipulator control method, the external force torque calculation step (S20) may include an external force torque calculation step (S21) of sensing the sensing joint information including the motor current applied to the driving motor and the rotation angle of the joint part (S23) of determining an external force torque to be applied to the robot manipulator based on the motor current, the sensing joint information, and the preset data.
상기 로봇 머니퓰레이터 제어 방법에 있어서, 상기 외력 토크 산출 확정 단계(S23)에서, 상기 외력 토크(τext)는: 
상기 로봇 머니퓰레이터 제어 방법에 있어서, 상기 충돌 감지기 산출 단계는: 상기 로봇 머니퓰레이터의 자유 운동 상태를 이루는 자유 운동 모드, 대상체 또는 작업도구를 이송하는 이송 운동 모드, 및 상기 로봇 머니퓰레이터의 외부 환경과 물리적으로 접촉하는 동작을 실행하는 컨택 운동 모드 중 상기 제어부가 인가하여 실행되는 작업 유형을 확인하는 작업 유형 확인 단계(S31)와, 상기 작업 유형 확인 단계(S31)에서 확인된 작업 유형을 판단하는 작업 유형 판단 단계(S33,S35,S37)와, 상기 작업 유형 판단 단계에서의 판단 결과에 대응하는 해당 충돌 감지기를 확정하는 충돌 감지기 확정 단계(S34,S36,S38)을 포함할 수도 있다. In the robot manipulator control method, the collision sensor calculating step may include: a free motion mode constituting a free motion state of the robot manipulator, a transfer motion mode of transferring a target object or a work tool, and a physical contact with an external environment of the robot manipulator (S31) of confirming a task type to be executed by the control unit during a contact movement mode for performing an operation for confirming a task type determined in the task type checking step (S31) (S33, S35, S37), and a collision detector determination step (S34, S36, S38) of determining a corresponding collision detector corresponding to the determination result in the operation type determination step.
상기 로봇 머니퓰레이터 제어 방법에 있어서, 상기 작업 유형 판단 단계에서 판단된 작업 유형이 상기 자유 운동 모드인 경우, 상기 로봇 머니퓰레이터에 발생하는 충돌로 인한 충격력 Fc로 유발되는 외부 토크(τc)이고, 상기 충돌 감지기는: 
상기 로봇 머니퓰레이터 제어 방법에 있어서, 상기 작업 유형 판단 단계에서 판단된 작업 유형이 상기 이송 운동 모드인 경우, 상기 로봇 머니퓰레이터에 발생하는 충돌로 인한 충격력 Fc로 유발되는 외부 토크(τc)이고, 상기 로봇 머니퓰레이터가 파지하는 대상체에 작용하는 중력 벡터(Fg) 및 관성 벡터(Fi)의 합력(Fg)에 의한 외부 토크(τp)라 할 때, 상기 외력 토크(τext)는: 
여기서, 
상기 로봇 머니퓰레이터 제어 방법에 있어서, 상기 작업 유형 판단 단계에서 판단된 작업 유형이 상기 컨택 운동 모드인 경우, 상기 로봇 머니퓰레이터에 발생하는 충돌로 인한 충격력 Fc로 유발되는 외부 토크(τc)이고, 상기 로봇 머니퓰레이터의 단부에 작용하는 접촉 외력(Fe)에 의한 외부 토크(τe)라 할 때, 상기 외력 토크(τext)는: 
상기 로봇 머니퓰레이터 제어 방법에 있어서, 상기 충돌 판단 단계(S40)는: 상기 로봇 머니퓰레이터의 작업 유형을 확인하는 판단 작업 유형 확인 단계(S41)와, 상기 판단 작업 유형 확인 단계(S41)에서 확인된 작업 유형을 판단하는 판단 작업 유형 판단 단계(S43,S45,S47)와, 상기 판단 작업 유형 판단 단계에서 확인된 작업 유형에 따라, 상기 판단 작업 유형 판단 단계에서의 판단 결과에 대응하는 해당 충돌 감지기를 통하여 충돌 여부를 확인하는 충돌 여부 판단 단계(S44,S46,S48)를 포함할 수도 있다. In the robot manipulator control method, the conflict determination step (S40) may include: a determination operation type confirmation step (S41) of confirming the operation type of the robot manipulator; a determination operation type determination step (S41) (S43, S45, S47) for judging whether or not a collision has occurred, and a determination step (S43, S45, S47) for determining a collision type through a collision detector corresponding to the determination result in the determination type determination step (S44, S46, S48) for determining whether or not a collision is detected.
상기 로봇 머니퓰레이터 제어 방법에 있어서, 상기 충돌 여부 판단 단계에서 판단 결과에 따라 상기 제어부가 실행 인가할 모드를 설정하는 모드 설정 단계를 더 구비할 수도 있다. The robot manipulator control method may further include a mode setting step of setting a mode to be executed by the controller according to the determination result in the collision determination step.
상기 로봇 머니퓰레이터 제어 방법에 있어서, 상기 충돌 여부 판단 단계에서 상기 충돌 감지기의 값이 영이 아닌 경우, 상기 제어부는 상기 모드 설정 단계에서 실행되어야 할 모드는 상기 작업 유형을 실행하는 작업 모드로 설정할 수도 있다. In the robot manipulator control method, when the value of the collision detector is not zero in the collision determination step, the controller may set a mode to be executed in the mode setting step to a work mode for executing the task type.
상기 로봇 머니퓰레이터 제어 방법에 있어서, 상기 충돌 여부 판단 단계에서 상기 충돌 감지기의 값이 영인 경우, 상기 제어부는 상기 모드 설정 단계에서 실행되어야 할 모드는 상기 작업 유형의 실행을 중단하고 충돌 상태에 대응하는 충돌 모드로 설정할 수도 있다. In the robot manipulator control method, when the value of the collision detector is zero in the collision determination step, the controller determines that the mode to be executed in the mode setting step is to stop execution of the operation type, Mode.
상기한 바와 같은 구성을 갖는 본 발명에 따른 로봇 머니퓰레이터 충돌 감지 제어 방법은 다음과 같은 효과를 갖는다. The robot manipulator collision detection and control method according to the present invention having the above-described configuration has the following effects.
본 발명에서 고안된 충돌 감지기 및 로봇 머니퓰레이터 제어방법을 사용하면 로봇 머니퓰레이터가 외부환경과 물리적인 상화작용을 수행하면서 충돌을 감지할 수 있다. 이러한 사실을 바탕으로 요약한 본 발명의 효과는 다음과 같다. Using the collision detector and the robot manipulator control method devised in the present invention, the robot manipulator can detect a collision while physically performing a virtualization operation with the external environment. The effect of the present invention summarized on the basis of this fact is as follows.
1) 본 발명에서 고안한 충돌 감지기를 사용하여 로봇 머니퓰레이터가 작업자 또는 외부환경과 충돌한 경우에 이를 감지할 수 있으며, 충돌 감지 결과에 따라 로봇 머니퓰레이터의 작업을 일시적으로 중단함으로써, 로봇 머니퓰레이터과 협업하는 작업자의 상해와 작업물 또는 로봇 머니퓰레이터의 파손을 방지할 수 있다. 1) It is possible to detect when the robot manipulator collides with the operator or the external environment by using the collision detector devised in the present invention. By temporarily stopping the operation of the robot manipulator according to the collision detection result, And damage to the workpiece or the robot manipulator can be prevented.
2) 제안된 충돌 감지기는 충격력의 크기, 주파수와 상관없이 모든 충돌을 감지할 수 있고, 로봇 머니퓰레이터가 파지한 물체에 대한 동역학 정보와 같은 외부환경의 모델 정보를 전혀 사용하지 않는다. 또한, 로봇 머니퓰레이터 말단에 부착된 고가의 6축 힘/토크 센서를 필요로 하지 않는다. 따라서 그 적용범위가 매우 넓고, 다양한 산업 및 서비스 현장에서 사용되는 로봇 머니퓰레이터에 적용할 수 있다. 2) The proposed collision detector can detect all collisions irrespective of the magnitude and frequency of the impact force, and does not use model information of the external environment such as kinematics information about the object held by the robot manipulator at all. Also, an expensive six-axis force / torque sensor attached to the end of the robot manipulator is not required. Therefore, it can be applied to robot manipulators used in various industrial and service fields.
본 발명은 도면에 도시된 실시 예들을 참고로 설명되었으나, 이는 예시적인 것에 불과하며, 본 기술 분야의 통상의 지식을 가진 자라면 이로부터 다양한 변형 및 균등한 다른 실시 예가 가능하다는 점을 이해할 것이다. 따라서 본 발명의 진정한 기술적 보호 범위는 첨부된 특허 청구 범위의 기술적 사상에 의해 정해져야 할 것이다.Although the present invention has been described with reference to the embodiments shown in the drawings, it is to be understood that various changes and modifications may be made therein without departing from the scope of the invention as defined by the appended claims. Accordingly, the true scope of the present invention should be determined by the technical idea of the appended claims.
도 1은 본 발명의 간략화된 로봇 머니퓰레이터(10)의 개략적인 상태도이다.
도 2는 본 발명의 로봇 머니퓰레이터(10)의 개략적인 블록선도이다.
도 3 내지 도 7은 본 발명의 로봇 머니률레이터 제어 방법의 개략적인 내지 세부적인 흐름도이다.
도 8은 본 발명의 로봇 머니퓰레이터에 인가된 외부토크의 산출 원리를 나타낸다.
도 9는 본 발명의 로봇 머니퓰레이터가 수행하는 작업을 구분한 것이다.
도 10은 로봇 머니퓰레이터가 물체를 이송하는 작업을 수행하는 경우에 로봇 머니퓰레이터에 인가될 수 있는 외력들이다.
도 11은 로봇 머니퓰레이터가 외부환경과 접촉하는 작업을 수행하는 경우에 로봇 머니퓰레이터에 인가될 수 있는 외력들 이다.
도 12는 고안된 충돌 감지기를 이용하여 로봇 머니퓰레이터를 제어하는 알고리즘의 흐름도이다.1 is a schematic state diagram of a
2 is a schematic block diagram of the
3 to 7 are schematic and detailed flowcharts of a method of controlling the robot money rate regulator of the present invention.
8 shows the principle of calculation of the external torque applied to the robot manipulator of the present invention.
Fig. 9 is a view showing the operation performed by the robot manipulator of the present invention.
10 is external forces that can be applied to the robot manipulator when the robot manipulator carries out an operation of transferring an object.
11 is external forces that can be applied to the robot manipulator when the robot manipulator is in contact with the external environment.
12 is a flowchart of an algorithm for controlling a robot manipulator using a collision detector designed.
이하에서는 본 발명에 따른 로봇 머니퓰레이터 충돌 감지기 및 로봇 머니퓰레이터 충돌 감지 제어 방법에 대하여 도면을 참조하여 설명하기로 한다. Hereinafter, a robot manipulator collision detector and a robot manipulator collision detection and control method according to the present invention will be described with reference to the drawings.
전술한 목적을 달성하기 위한 본 발명은, 세 가지 과정으로 구성된다. 첫 번째 과정은 로봇 머니퓰레이터에 인가되는 외부토크를 산출하는 과정이다. 두 번째 과정은 로봇 머니퓰레이터가 충돌감지기를 이용하여 충돌을 감지하는 과정이다. 이 과정에서 로봇 머니퓰레이터에 적용되는 충돌감지기는 로봇 머니퓰레이터가 수행하는 작업에 따라 세 가지로 구분된다. 세 번째 과정은 충돌의 감지 여부를 바탕으로 로봇 머니퓰레이터가 적절히 대응 하는 과정인데, 이러한 로봇 머니퓰레이터 제어 방법을 실행하기 위하여 로봇 머니퓰레이터(10)가 제공된다. In order to accomplish the above object, the present invention comprises three processes. The first step is to calculate the external torque applied to the robot manipulator. The second process is a process in which the robot manipulator detects a collision using a collision detector. In this process, the collision detector applied to the robot manipulator is classified into three types according to the work performed by the robot manipulator. The third process is a process in which a robot manipulator appropriately responds based on whether or not a collision is detected. A
본 발명에 따른 로봇 머니퓰레이터 제어 방법은 로봇 머니퓰레이터(10)의 동작시 충돌로 인한 외력을 감지하는 충돌 감지기를 제공하여 보다 신속하고 정확한 충돌 감지를 통한 제어를 가능하게 할 수 있다. The robot manipulator control method according to the present invention can provide a collision detector that detects an external force due to a collision during operation of the
본 발명의 로봇 머니퓰레이터 제어 방법이 실행되는 로봇 머니퓰레이터(10)는 하나 이상의 관절부(100)를 구비하는데, 관절부(100)에는 가동을 위한 구동 모터(1)가 배치된다. 도 1에는 간략화된 로봇 머니퓰레이터(10)의 개략적인 상태도가 도시되고, 도 2에는 로봇 머니퓰레이터(10)의 개략적인 블록선도가 도시되고, 도 3 내지 도 7에는 본 발명의 로봇 머니률레이터 제어 방법의 개략적인 내지 세부적인 흐름도가 도시된다. 도 1에서 일축 로봇 머니퓰레이터를 도시하였으나, 본 발명은 이에 국한되지 않고 다축 로봇 머니퓰레이터에도 적용 가능하다. The
본 발명의 로봇 머니퓰레이터 제어 방법은 고가의 토크 센서 내지 가속도 센서를 배제하고 엔코더로서의 감지부의 관절 위치 측정부를 통하여 감지되는 회전각과 구동 모터에 인가되는 전류를 이용하여 각 관절부에서의 토크를 관측 추정 산출할 수도 있는데, 이러한 추정 토크는 로봇 머니퓰레이터(10)의 힘-토크 제어를 실행하는데 사용되거나, 로봇 머니퓰레이터(10)의 이상 작동 여부를 감지 대응하는데 사용되거나, 또는 관절에서의 로봇 머니퓰레이터(10)와 외부 환경, 예를 들어 작업자 내지 물체 등과의 충돌로 인하여 발생하여 로봇 머니퓰레이터(10)에 인가되는 비정상적인 외력에 의한 외력 토크를 감지하는데 사용될 수 있는 등 다양한 제어 용도로 사용될 수 있다. The robotic manipulator control method of the present invention estimates and calculates the torque at each joint by using the rotation angle sensed through the joint position measuring unit of the sensing unit as an encoder and the current applied to the drive motor without the expensive torque sensor or the acceleration sensor The estimated torque may be used to execute the force-torque control of the
도 2에 도시된 바와 같이, 로봇 머니퓰레이터(10)는 제어부(20)와 저장부(30)를 구비하고, 제어부(20)와 연결되는 연산부(40)를 구비할 수 있다. 입력부(60)를 통하여 작업자에 의한 입력 정보 제공도 가능한데, 입력부(60)는 별도의 통신부(50)를 통하여 제어부(20)와 신호 전달을 이루는 구조를 취할 수도 있고 경우에 따라 입력부(60)가 직접 제어부(20)와 직결되어 신호 전달을 이루는 구조를 취할 수도 있는 등 설계 사양에 따라 다양한 변형이 가능하다. 2, the
또한, 여기서 구체적으로 명시되지는 않으나 로봇 머니퓰레이터(10)는 관절 토크 센서 기반으로 힘제어를 실행하여 로봇 머니퓰레이터(10)의 엔드이펙터, 즉 말단에서 작업 대상에 대한 소정의 힘을 가하여 설정된 소정의 작업을 실행하는데, 이는 관절 토크 센서 기반의 힘제어의 통상적인 과정을 이루는바 이에 대한 구체적 설명은 생략한다. Although not specifically described herein, the
로봇 머니퓰레이터(10)는 제어부(20)를 포함하는데, 제어부(20)는 감지부(5)에서 감지되는 관절부(100)의 관절 토크인 감지 관절 토크 정보를 포함하는 감지 정보를 취득하여 제어부(20)로 전달한다. The
감지부(5)는 관절 위치 측정부(2)와 관절 속도 측정부(3)를 포함할 수 있는데, 관절 위치 측정부(2)와 관절 속도 측정부(3)는 구동 모터(1)에 내장될 수 있고, 경우에 따라 관절 위치 측정과 관절 속도 측정은 구동 모터(1)에 인가되는 전류 신호를 사용하여 연산되어 추출 사용되는 구조를 취할 수도 있고, 또는 구동 모터 및/또는 감속기(미도시)를 통하여 연결되는 별도의 외부 엔코더를 통하는 등 설계 사양에 따라 다양한 구성이 가능하다. 관절 위치 측정부(2)와 관절 속도 측정부(3)는 관절부(100)에 배치되는 엔코더로 구현될 수 있고 경우에 따라 회전 속도를 감지하기 위한 자이로 센서를 더 구비할 수도 있으나 본 실시예는 엔코더로 관절부에서의 관절 위치 측정 및 관절 속도 측정을 실행하는 것으로 구현하여 제조 원가를 절감하고 구성을 간단하게 형성하도록 한다. The
또한, 본 실시에에서는 각 관절에서의 각가속도를 감지하는 별도의 관절 가속도 측정부를 구비하지 않고, 가속도 항목을 배제하는 방식의 동역학적 모델을 통하여 각 관절에서의 회전각 및 각속도를 이용하여 각 관절부에서의 마찰토크를 추정하는 방식을 중심으로 설명한다.In this embodiment, a separate joint acceleration measuring unit for sensing the angular acceleration of each joint is not provided, and a rotational kinetic model of a method of excluding the acceleration item is used to calculate the angular velocity at each joint The friction torque is estimated based on the estimated friction torque.
저장부(30)는 하기되는 제어부(20)와 연결되는데, 저장부(30)는 사전 설정 데이터를 포함하여 소정의 제어 신호 인가 내지 관측 기능을 통한 예측치 연산 등의 기초가 되는 사전 설정 추정 데이터를 포함하는 정보 데이터를 제공할 수 있다. The
제어부(20)는 제어기(21)와 충돌 감지기(25)를 포함하고 경우에 따라 관측기(23)를 더 포함하는데, 제어기(21)는 감지부(5)로부터 감지된 감지 정보 내지 작업자에 의하여 입력부(6)를 통하여 입력되는 입력 정보에 기초하여 구동 모터(1)에 소정의 구동 제어 신호를 인가함으로써 로봇 머니퓰레이터(10)를 구성하는 가동부인 링크를 가동시키기 위한 구동력을 제공하거나 관측기(23)에서 관측되는 마찰 추정 토크를 이용하여 실제로 구동 모터 및/또는 감속기에서 발생하는 마찰 토크를 보상하여 목표로 설정된 구동력을 제공하도록 구동력 제어를 실행하는데 사용될 수도 있으며, 이러한 산출 정보를 이용하여 충돌 감지기(25)가 로봇 머니퓰레이터의 충돌 여부를 감지 판단할 수도 있다. The
또한, 본 발명에 따른 제어부(20)의 제어기(21)는 충돌 감지기(25)에서 감지 판단된 충돌 여부를 정보를 활용하여 관절부에서의 오작동 여부 내지는 현재 접촉이 예상치 못한 충돌인지 예상된 충돌인지 여부를 판단하여 각각의 경우에 대한 소정의 사전 설정된 작동 모드를 실행하기 위한 제어 신호를 인가할 수도 있다. In addition, the
경우에 따라 관측기(23)는 감지부(5)로부터 감지된 감지 관절 정보를 포함하는 감지 정보 및 저장부(30)에 저장되는 사전 설정 데이터에 기초하여 소정의 관절부를 통하여 연결되는 관절 링크(도 1 참조)의 가동을 예측하여 관절부에서 발생하는 기계적 요소로 인한 마찰 토크(τf) 가해지는 관절 토크를 추정 산출할 수도 있다.In some cases, the
본 발명에 따른 로봇 머니퓰레이터 제어 방법은 외력 토크 산출 단계(S20)와, 충돌 감지기 산출 단계(S30)와, 충돌 판단 단계(S40)와, 모드 실행 단계(S50)를 포함한다. The robot manipulator control method according to the present invention includes an external force torque calculation step S20, a collision sensor calculation step S30, a collision determination step S40, and a mode execution step S50.
먼저, 외력 토크 산출 단계(S20)에서는 로봇 머니퓰레이터의 제어부가 로봇 머니퓰레이터에 인가되는 외력 토크(τext)를 산출하는데, 로봇 머니퓰레이터는 구동 모터를 통하여 가동되는 관절부를 구비하고 로봇 머니퓰레이터 제어부의 제동를 통하여 힘제어 동작을 이룬다. 로봇 머니퓰레이터의 제어부(20)가 로봇 머니퓰레이터에 인가되는 외력 토크(τext)를 산출하는데, 이러한 로봇 머니퓰레이터에 인가되는 외력 토크(τext) 산출은 사전 설정 데이터 및 감지 관절 정보에 기초한 산출 연산 과정으로 이루어진다. 사전 설정 데이터는 제어부(20)와 연결되는 저장부(30)에 사전 설정 저장되고, 감지 관절 정보는 제어부(20)의 감지 제어 신호에 따라 감지부(5)가 취득하는 정보로서 관절부의 회전각을 포함한다. First, in the external torque calculating step S20, the control unit of the robot manipulator calculates an external force torque? Ext applied to the robot manipulator. The robot manipulator has a joint part that is operated through a driving motor, Operation. The
그런 후, 충돌 감지기 산출 단계(S30)가 실행된다. 앞서 기술한 바와 같이, 제어부(20)는 제어기(21)와 충돌 감지기(25)를 포함하고 경우에 따라 관측기(23)를 더 포함할 수 있는데, 제어기(21)는 감지부(5)로부터 감지된 감지 정보 내지 작업자에 의하여 입력부(6)를 통하여 입력되는 입력 정보에 기초하여 구동 모터(1)에 소정의 구동 제어 신호를 인가함으로써 로봇 머니퓰레이터(10)를 구성하는 가동부인 링크를 가동시키기 위한 구동력을 제공하거나 관측기(23)에서 관측되는 마찰 추정 토크를 이용하여 실제로 구동 모터 및/또는 감속기에서 발생하는 마찰 토크를 보상하여 목표로 설정된 구동력을 제공하도록 구동력 제어를 실행하는데 사용될 수도 있으며, 이러한 산출 정보를 이용하여 충돌 감지기(25)가 로봇 머니퓰레이터의 충돌 여부를 감지 판단할 수도 있다. Then, the collision sensor calculating step S30 is executed. The
충돌 감지기 산출 단계(S30)에서 이러한 제어부(20)의 충돌 감지기(25)는 로봇 머니퓰레이터의 작업 유형에 따라, 로봇 머니퓰레이터의 충돌 여부를 판단한다.In the collision detector calculation step S30, the
그런 후, 충돌 판단 단계(S40)에서 제어부(20)의 제어기(21)는 충돌 감지기(25)와 사전 설정 데이터에 포함되는 충돌 판단 기준값과 비교하여 로봇 머니퓰레이터의 충돌 여부를 확인하고 작업 유형을 수행하는 작업 수행 모드와 충돌 상황에 대응하는 충돌 모드 중 실행되어야 할 실행 모드를 설정한다.In step S40, the
그런 후, 모드 실행 단계(S50)가 실행되는데, 모드 실행 단계(S50)에서 제어부(20)의 제어기(21)는 충돌 판단 단계(S40)에서의 판단 결과 및 설정 모드에 따라 해당 모드를 실행한다. Then, the mode execution step S50 is executed. In the mode execution step S50, the
보다 상세하게, 앞서 외력 토크 산출 단계(S20)는 외력 토크 산출 감지 단계(S21)와, 외력 토크 산출 확정 단계(S23)를 포함한다. More specifically, the external force torque calculation step S20 includes an external force torque calculation step S21 and an external force torque calculation step S23.
외력 토크 산출 감지 단계(S21)에서는 제어부(20)의 제어기(21)의 감지 제어 신호에 따라 구동 모터에 인가되는 모터 전류 및 로봇 머니퓰레이터의 관절부의 회전각을 포함하는 감지 관절 정보가 감지된다. In the external torque torque detection step S21, sensing joint information including the motor current applied to the driving motor and the rotation angle of the joint of the robot manipulator is sensed according to the sensing control signal of the
그런 후, 외력 토크 산출 확정 단계(S23)에서 외력 토크가 확정 산출되는데, 외력 토크 산출 확정 단계(S23)에서는 이렇게 감지 산출된 감지 관절 정보와 모터 전류 및 저장부(30)에 저장된 사전 설정 데이터에 기초하여 외력 토크를 확정 산출한다. Then, in the external torque torque determination step S23, the external force torque is determined and calculated. In the external force torque determination step S23, the sensed joint information, the motor current, and the preset data stored in the
외력 토크 산출 확정 단계(S23)에서, 제어부(20)는 외력 토크(τext)를 다음과 같이 확정 산출한다. In the external torque torque determination step S23, the
외부토크를 산출하는 과정의 주 원리는 로봇 머니퓰레이터의 각 관절에서 출력된 토크와 실제로 로봇 머니퓰레이터의 링크에 인가된 토크를 비교하는 것이다. 도 1에 나타낸 바와 같이 인간-로봇 머니퓰레이터 충돌이 발생하면 로봇 머니퓰레이터에 외력이 인가되고, 이 힘으로 인하여 로봇 머니퓰레이터의 각 관절에는 외부토크가 가해진다. 이러한 경우에 로봇 머니퓰레이터의 각 관절에 장착된 모터 및 감속기에서 출력된 토크 중 일부는 외부토크를 보상하는 데 사용되므로 손실되고, 손실된 토크 이외의 나머지 토크는 각 링크에 전달되어 로봇 머니퓰레이터의 구동에 사용된다. 이러한 사실을 수식으로 표현하면 외력 토크(τext)는 아래와 같다. The main principle in calculating the external torque is to compare the torque output from each joint of the robot manipulator with the torque actually applied to the link of the robot manipulator. As shown in FIG. 1, when a human-robot manipulator collision occurs, an external force is applied to the robot manipulator, and external torque is applied to each joint of the robot manipulator due to this force. In this case, some of the torque output from the motor and the speed reducer mounted on each joint of the robot manipulator is used to compensate for the external torque, so that the remaining torque other than the lost torque is transmitted to each link, Is used. Expressing this fact as an equation, the external torque (τext) is as follows.
여기서, τext 는 충돌 등의 외력에 의해서 로봇 머니퓰레이터의 각 관절에 작용하는 외부토크 벡터이고, τj 은 각 관절에 설치된 모터, 즉 구동 모터와 감속기에 의해서 로봇 머니퓰레이터 링크, 즉 관절 링크에 전달되는 관절토크 벡터이다. M(q)는 로봇 머니퓰레이터의 각 관절부를 통하여 연결되는 관절 링크의 관성 행렬을, 
앞서 기술한 바와 같이, 충돌 감지기 산출 단계(S30)에서 이러한 제어부(20)의 충돌 감지기(25)는 로봇 머니퓰레이터의 작업 유형에 따라, 로봇 머니퓰레이터의 충돌 여부를 판단하는데, 충돌 감지기 산출 단계(S30)는 작업 유형 확인 단계(S31)와, 작업 유형 판단 단계(S33,S35,S37)와, 충돌 감지기 확정 단계(S34,S36,S38)를 포함한다. As described above, in the collision detector calculation step S30, the
작업 유형 확인 단계(S31)에서 제어부(20)의 제어기(21)는 로봇 머니퓰레이터의 자유 운동 상태를 이루는 자유 운동 모드, 대상체 또는 작업도구를 이송하는 이송 운동 모드, 및 로봇 머니퓰레이터의 외부 환경과 물리적으로 접촉하는 동작을 실행하는 컨택 운동 모드 중 사전 설정 데이터에 포함되고 상기 제어부가 인가하여 실행되는 작업 유형을 확인한다. 즉, 사전 설정 데이터는 자유 운동 모드, 이송 운동 모드, 및 컨택 운동 모드에 대한 데이터를 모두 포함하는데, 단계 S31에서 제어부가 인가하여 실행되는 작업 유형이 세 가지 운동 모드 중 어느 것인지를 확인하는 과정이 수행된다. The
작업 유형 판단 단계(S33,S35,S37)에서는 제어부(20)가 작업 유형 확인 단계(S31)에서 확인된 작업 유형을 판단한다. 충돌 감지기 확정 단계(S34,S36,S38)에서는 제어부(20)는 작업 유형 판단 단계에서의 판단 결과에 대응하는 해당 충돌 감지기를 확정한다. In the job type determination step (S33, S35, S37), the
작업 유형 판단 단계(S33,S35,S37)에서 확인되고 충돌 감지기 확정 단계(S34,S36,S38)에서 해당 충돌 감지기가 선택되는 작업 유형은 본 실시예에서 3가지로 구분된다. In the present embodiment, there are three types of job types that are confirmed in the job type determination steps (S33, S35, S37) and in which the corresponding collision detectors are selected in the collision detector determination steps (S34, S36, S38).
즉, 본 발명의 로봇 머니퓰레이터가 충돌감지기를 이용하여 충돌을 감지하는 과정의 핵심 원리는 작업수행을 위해서 의도적으로 로봇 머니퓰레이터에 인가되는 외력과 무관하고, 충돌로 인하여 로봇 머니퓰레이터에 인가된 충격력에만 반응하는 충돌감지기를 이용하는 것인데, 본 발명의 로봇 머니퓰레이터가 수행하는 작업의 일반적인 유형은 도 9에서와 같이 세 가지로 경우로 구분할 수 있다. That is, the core principle of the process in which the robot manipulator of the present invention detects a collision using a collision detector is that it is independent of the external force applied to the robot manipulator intentionally for performing the task, and only responds to the impact force applied to the robot manipulator A collision detector is used. The general type of work performed by the robot manipulator of the present invention can be classified into three cases as shown in FIG.
첫 번? 경우는 로봇 머니퓰레이터가 외부환경와 물리적인 접촉이 없이 자유공간 상에서 움직이는 경우이다. 두 번째 경우는 로봇 머니퓰레이터가 작업물 또는 작업도구와 같은 물체를 파지한 채로 자유공간 상에서 움직이는 경우이다. 세 번째 경우는 로봇 머니퓰레이터가 외부환경과 접촉하여 작업을 수행하는 경우이다. 이러한 세 가지 경우는 로봇 머니퓰레이터에 인가되는 외력의 특징이 서로 다르므로 각기 다른 충돌감지기를 사용한다.first? The case is when the robot manipulator moves in free space without physical contact with the external environment. The second case is when the robot manipulator moves in free space while holding an object such as a workpiece or a work tool. In the third case, the robot manipulator is in contact with the external environment and performs work. These three cases use different collision detectors because the characteristics of the external force applied to the robot manipulator are different from each other.
먼저, 첫번째의 경우로 로봇 머니퓰레이터가 외부환경와 물리적인 접촉을 수행하지 않고 움직이는 경우에는, 로봇 머니퓰레이터가 정상적으로 동작하는 상황에서 로봇 머니퓰레이터에 인가되는 외력은 존재하지 않는다. 하지만 로봇 머니퓰레이터과 작업자 사이에 충돌이 발생하면 충격력 F c 가 로봇 머니퓰레이터에 작용하며, 이러한 인간-로봇 머니퓰레이터 충돌 상황에서는 F c 로 인하여 각 관절에 유발되는 외부토크 τ c 를 바탕으로 외력 토크τ ext 를 다음과 같이 표현할 수 있다. First, when the robot manipulator moves without performing physical contact with the external environment in the first case, there is no external force applied to the robot manipulator in a state where the robot manipulator normally operates. However, the robot meonipyulreyiteogwa If a conflict occurs between the worker impact force F c is applied to the robot manipulator, and this man-based on the external torque τ c the robot manipulator collision situation due to the F c caused in the joint external force torque τ ext and then Can be expressed as follows.
따라서 로봇 머니퓰레이터가 외부환경와 물리적인 접촉을 수행하지 않는 경우에는 상기한 방법으로 산출된 τ ext 를 이용하여 τ c 와 F c 의 발생 여부를 판단할 수 있고, τ ext 를 충돌감지기로 사용할 수 있다. 달리 표현하면, 제어부(20)가 작업 유형 판단 단계(S33,S35,S37)에서 판단된 작업 유형이 자유 운동 모드인 경우, 로봇 머니퓰레이터에 발생하는 충돌로 인한 충격력 Fc로 외부 토크(τc)가 유발되고, 제어부(20)의 충돌 감지기(25)는 
반면, 두번째의 경우로 로봇 머니퓰레이터가 작업물이나 작업도구를 이송하는 작업을 수행하는 경우에는 로봇 머니퓰레이터가 정상적으로 운행되는 상황에서도 로봇 머니퓰레이터가 파지한 물체에 작용하는 중력과 관성력이 로봇 머니퓰레이터의 말단부에 전달된다. 도 10은 은 로봇 머니퓰레이터가 작업물 또는 작업도구를 이송하는 작업을 수행하는 경우에 로봇 머니퓰레이터에 인가될 수 있는 외력들을 나타낸 것이다. 도 10의 F g 와 F i 는 각각 로봇 머니퓰레이터가 파지한 물체에 작용하는 중력 벡터과 관성력 벡터이고, F g 와 F i 는 각각 힘 성분 f g , f i 와 모멘트 성분 n g , n i 로 구성된다. F p 는 F g 와 F i 의 합력으로 로봇 머니퓰레이터가 정상적인 작업을 수행하는 도중에는 로봇 머니퓰레이터에 인가되는 외력이다. 충돌이 발생한 경우에는 F p 와 F c 가 동시에 로봇 머니퓰레이터에 작용하며, τ ext 에는 F p 로 인하여 각 관절에 인가되는 외부토크 τ p 와 충돌로 인하여 발생한 외부토크 τ c 가 모두 포함되어 있다. 따라서 τ ext 를 다음과 같이 표현할 수 있다. On the other hand, in the second case, when the robot manipulator carries work or work tool, the gravity and inertia force acting on the object gripped by the robot manipulator is transmitted to the distal end of the robot manipulator even when the robot manipulator is normally operated do. 10 shows external forces that can be applied to the robot manipulator when the silver robot manipulator carries out a task of transferring a workpiece or a work tool. In Fig. 10, F g and F i are gravity vectors and inertia force vectors acting on the object held by the robot manipulator, respectively, and F g and F i are composed of force components f g and f i and moment components n g and n i , respectively . F p is the external force applied to the robot manipulator during the normal operation of the robot manipulator due to the resultant force of F g and F i . When a collision occurs, F p and F c simultaneously act on the robot manipulator, and τ ext includes both the external torque τ p applied to each joint due to F p and the external torque τ c caused by the collision. Therefore, τ ext can be expressed as follows.
따라서 충돌이 발생하지 않아 τ c = 0인 상황에서도 τ p ≠0이므로 τ ext ≠0이며, 단순히 τ ext 를 바탕으로 충돌을 감지할 수 없으며, 로봇 머니퓰레이터가 물체를 이송하는 작업을 수행하는 경우에는 본 발명을 통해 고안된 충돌감지기를 사용한다.Therefore, since no collision occurs, τ c = 0, τ ext ≠ 0 because τ p ≠ 0, and it is impossible to simply detect a collision based on τ ext . In the case where the robot manipulator carries out an object transfer operation, the collision detector designed by the present invention use.
즉 달리 표현하자면, 작업 유형 판단 단계에서 판단된 작업 유형이 이송 운동 모드인 경우, 로봇 머니퓰레이터에 발생하는 충돌로 인한 충격력 Fc로 유발되는 외부 토크(τc)이고, 로봇 머니퓰레이터가 파지하는 대상체에 작용하는 중력 벡터(Fg) 및 관성 벡터(Fi)의 합력(Fg)에 의한 외부 토크(τp)라 할 때, 외력 토크(τext)와 충돌 감지기는 다음과 같이 표현된다. In other words, in the case where the operation type determined in the operation type determination step is the movement mode, it is an external torque (tc) caused by the impact force Fc due to a collision occurring in the robot manipulator, The external force tauext and the crash sensor are expressed as follows when the external torque tau by the resultant force Fg of the gravity vector Fg and the inertia vector Fi.
여기서, 
그리고, 외력 토크(τext)를 외부 토크(τp)가 존재하는 공간 Sp(도 13 참조)에 수직한 공간 ⊥Sp로 투영하기 위한 투영 행렬(P)은 다음과 같이 표현된다. The projection matrix P for projecting the external force torque? Ext to the space? Sp perpendicular to the space Sp in which the external torque? P exists (see FIG. 13) is expressed as follows.
이를 보다 상세하게 기술하면, 고안된 충돌감지기의 주 원리는 관절 토크센서에 의해서 측정되거나 모터전류로부터 추정된 τ ext 를 특정 선형공간으로 투영함으로써, τ ext 에 포함된 τ p 를 소거하는 것이다. If this in greater detail, the main principle of the impact sensor is designed to measure, or by projecting a τ ext estimated from the motor current in a particular linear space, erasing the τ p τ ext by including the joint torque sensor.
τ ext , τ c , τ p 는 모두 벡터이며, 임의의 벡터를 해당 벡터가 존재하는 공간과 수직한 공간으로 투영한 결과는 항상 0이 된다. τ ext , τ c , and τ p are vectors, and the result of projecting an arbitrary vector into a space perpendicular to the space in which the vector exists is always zero.
즉, 도 13에 도시된 바와 같이, 관절 토크 벡터는 서로 직교하는 xyz 축으로 구성된 3차원 공간에 존재하며, 벡터 τ p 는 항상 xy 평면에 존재한다. 따라서, τ p 가 존재하는 xy평면은 2차원으로 xyz 축으로 구성된 3차원 공간의 부분공간이고, z축은 항상 수직이다. 즉, 본 실시예에서 공간 Sp가 xy평면이고, 수직한 공간 ⊥Sp는 z축이다. 도 13에서 충돌 감지기 d는 임의의 관절토크 벡터를 Sp(도 13 참조)에 수직한 공간 ⊥Sp에 해당하는 z축으로 투영한 결과이다. That is, as shown in FIG. 13, the joint torque vector exists in a three-dimensional space composed of xyz axes orthogonal to each other, and the vector ? P is always present in the xy plane. Thus, the xy plane in which τ p exists is a subspace of a three-dimensional space consisting of two-dimensional xyz axes, and the z axis is always perpendicular. That is, in this embodiment, the space Sp is in the xy plane and the vertical space? Sp is the z axis. In FIG. 13, the collision detector d is a result of projecting an arbitrary joint torque vector on the z axis corresponding to the space? S p perpendicular to Sp (see FIG. 13).
따라서 벡터 τ p 를 τ p 가 존재하는 공간 S p 와 수직한 공간 ⊥S p 으로 투영하면, 그 결과가 항상 0이며, τ ext 를 공간 ⊥S p 으로 투영함으로써 τ ext 에 포함된 τ p 를 소거하고, τ c 에만 반응하는 충돌감지기를 고안할 수 있다.Therefore vector τ p the τ p is present if projected area S p of space ⊥ S p perpendicular to that, that the result is always zero, erasing the τ p included in the τ ext by projecting the τ ext by space ⊥ S p , And devise a collision sensor that only responds to τ c .
τ ext 를 공간 ⊥S p 으로 투영하기 위해서는 임의의 벡터를 공간 ⊥S p 으로 투영하기 위한 투영행렬 P가 필요하며, P는 앞서 기술한 바와 같이 정의된다. In order to project τ ext to the space ⊥ S p , a projection matrix P for projecting an arbitrary vector to the space ⊥ S p is required, and P is defined as described above.
여기서 I는 항등행렬(identity matrix)이고, J p +는 J p 의 의사역행렬(pseudo inverse matrix)이다. 위 수식의 J p 는 다음과 같이 정의한다. Where I is an identity matrix and J p + is a pseudo inverse matrix of J p . J p in the above formula is defined as follows.
여기서 J는 로봇 머니퓰레이터의 자코비안 행렬이고, J T 는 자코비안 행렬의 전치행렬이다. B p 는 F p 가 존재하는 공간의 기저(basis)로 구성된 행렬이고, B p T 는 B p 의 전치행렬이며, 위 수식에서 행렬 B p 는 다음과 같이 정의 된다. Where J is the Jacobian matrix of the robot manipulator and J T is the transpose of the Jacobian matrix. B p is a matrix consisting of a base (basis) of a space where F p exists, B p T is a transposed matrix of B p, B p in the above formula, the matrix is defined as follows:
여기서 u i 와 u g 는 각각 f i , f g 와 방향이 같은 단위벡터(unit vector)이다. f i 는 로봇 머니퓰레이터 말단의 가속도와 방향이 같고, 로봇 머니퓰레이터 말단의 가속도는 로봇 머니퓰레이터의 제어과정에서 사용되는 각 관절 가속도의 목표값으로부터 추정할 수 있다. 또한, f g 는 중력방향으로 작용하므로 항상 일정한 방향으로 작용한다. 따라서 위 수식의 B F 는 로봇 머니퓰레이터가 파지하는 있는 물체에 대한 정보를 모르는 상태에서도 구성할 수 있다. Where u i and u g are unit vectors in the same direction as f i and f g , respectively. f i is the same as the acceleration of the end of the robot manipulator and the acceleration at the end of the robot manipulator can be estimated from the target value of each joint acceleration used in the control process of the robot manipulator. In addition, since f g acts in the direction of gravity, it always acts in a constant direction. Therefore, B F of the above formula can be configured without knowing the information about the object gripped by the robot manipulator.
투영행렬 P는 임의의 벡터를 공간 ⊥S p 으로 투영하며, 상기한 바와 같이 τ p 를 공간 ⊥S p 으로 투영한 결과는 항상 0이다. 즉, 아래 수식과 같은 관계가 성립한다. The projection matrix P projects an arbitrary vector into the space ⊥ S p , and the result of projecting τ p to the space ⊥ S p as described above is always zero. That is, the following relationship holds.
결과적으로 τ ext 를 공간 ⊥S p 으로 투영함으로써 τ ext 에 포함된 τ p 를 소거할 수 있으며, 로봇 머니퓰레이터가 물체를 이송하는 작업을 수행하는 경우에 사용하는 충돌 감지기 d p 는 아래와 같다. As a result, by projecting τ ext to the space ⊥ S p , τ p included in τ ext can be canceled, and the collision detector d p used when the robot manipulator carries an object is as follows.
위 수식에서 확인할 수 있듯이 d p 는 τ c 의 함수이며, τ p 와는 무관하다. 따라서 고안된 충돌감지 기를 이용하여 로봇 머니퓰레이터가 물체를 이송하는 작업을 수행하는 경우에서도 충돌을 감지할 수 있다. 또한, 로봇 머니퓰레이터가 이송하는 물체에 대한 동역학 모델을 전혀 사용하지 않으므로, 미지의 로봇 머니퓰레이터가 미지의 물체를 이송하는 상황에서도 적용 가능하다. 그리고 충격력의 주파수와도 전혀 무관하게 모든 충돌을 감지할 수 있다.As can be seen in the above equation, d p is a function of τ c and is independent of τ p . Therefore, even when the robot manipulator carries out the object transportation using the designed collision detector, the collision can be detected. Further, since the robot manipulator does not use a kinetic model for the object to be conveyed at all, it is applicable even in a situation where an unknown robot manipulator transfers an unknown object. And it can detect all collisions regardless of the frequency of impact force.
세번째로, 로봇 머니퓰레이터가 외부환경과 접촉하는 작업을 수행하는 경우에는 로봇 머니퓰레이터과 외부환경 사이의 접촉력이 로봇 머니퓰레이터에 전달된다. 도 11에는 로봇 머니퓰레이터가 이러한 작업을 수행하는 경우 로봇 머니퓰레이터에 인가될 수 있는 외력들이 도시된다. 도 11의 F e 는 로봇 머니퓰레이터의 말단부(즉, 작업도구나 작업물)에 전달되는 외력으로, 충돌이 발생하지 않아도 로봇 머니퓰레이터가 외부환경과 접촉을 유지하는 동안 로봇 머니퓰레이터에 지속적으로 작용하는 힘이다. 충돌이 발생한 경우에는 F e 와 F c 가 동시에 로봇 머니퓰레이터에 작용하며, τ ext 에는 F e 로 인하여 각 관절에 인가되는 외부토크 τ e 와 충돌로 인하여 발생한 외부토크 τ c 가 모두 포함되어 있다. 따라서 τ ext 를 다음과 같이 표현할 수 있다. Third, when the robot manipulator is in contact with the external environment, the contact force between the robot manipulator and the external environment is transmitted to the robot manipulator. Fig. 11 shows external forces that can be applied to the robot manipulator when the robot manipulator performs this operation. F e of Figure 11 is a force which constantly acts on the robot manipulator, while maintaining contact with the external force transmitted to the distal end of the robot manipulator (i.e., Laguna also work the work piece), the robot manipulator external environment even if the conflicts . If collision occurs, it is F e and F c is applied to the robot manipulator, and at the same time, τ ext is due to F e contains all the external torque τ c caused by external torque τ e and impact applied to each joint. Therefore, τ ext can be expressed as follows.
따라서 로봇 머니퓰레이터가 외부환경과 접촉하는 작업을 수행하는 경우에도 단순히 τ ext 를 바탕으로 충돌을 감지할 수 없으며, 본 발명을 통해 고안된 충돌감지기를 사용하여 충돌을 감지한다.Therefore, even when the robot manipulator is in contact with the external environment, the collision can not be detected simply based on the τ ext , and the collision is detected using the collision detector designed by the present invention.
즉, 달리 표현하면, 작업 유형 판단 단계에서 판단된 작업 유형이 컨택 운동 모드인 경우, 로봇 머니퓰레이터에 발생하는 충돌로 인한 충격력 Fc로 유발되는 외부 토크(τc)이고, 로봇 머니퓰레이터의 단부에 작용하는 접촉 외력(Fe)에 의한 외부 토크(τe)라 할 때, 외력 토크(τext)는 다음과 같이 표현된다. That is, in other words, when the operation type determined in the operation type determination step is the contact movement mode, the external torque tc caused by the impact force Fc due to the impact generated in the robot manipulator, and the contact force acting on the end of the robot manipulator And the external torque? E due to the external force Fe, the external force torque? Ext is expressed as follows.
여기서, J는 로봇 머니퓰레이터의 자코비안 행렬이고, 
상술하자면, 로봇 머니퓰레이터가 외부환경과 접촉하는 작업을 수행하는 경우에 사용하는 충돌감지기 d e 의 원리는 앞서 설명한 충돌감지기 d p 와 유사하다. 즉, 상기한 d p 를 유도하는 과정에서 F p 가 존재하는 공간의 기저행렬인 B p 를 F e 가 존재하는 공간의 기저행렬인 B e 로 치환함으로써 산출할 수 있다. 하지만 F p 와 달리 F e 는 특정 힘들의 합력이 아니라, 말단에서 인가되는 모든 힘이 될 수 있으므로 B e 는 아래와 같이 정의된다.In other words, the principle of the collision detector d e used when the robot manipulator is in contact with the external environment is similar to the collision detector d p described above. That is, in the process of deriving the above-described d p , B p , which is the base matrix of the space in which F p exists Can be calculated by replacing F e with B e which is the base matrix of the space in which the F e exists. However, unlike F p , F e is not the resultant of certain forces but can be all forces applied at the ends, so B e is defined as:
위와 같이 정의된 B e 를 바탕으로 유도된 d e 는 아래와 같다.The derived d e based on the above-defined B e is as follows.
위 수식에서 확인할 수 있듯이, d e 는 τ c 의 함수이며, τ e 와는 무관하다. 따라서 고안된 본 발명의 충돌감지기 d e 를 이용하여 로봇 머니퓰레이터가 외부환경과 접촉하는 작업을 수행하는 경우에서 충돌을 감지할 수 있다. As can be seen in the above equation, d e is a function of τ c and is independent of τ e . Therefore, the collision can be detected in the case where the robot manipulator is in contact with the external environment by using the collision detector d e of the present invention.
또한, 로봇 머니퓰레이터가 접촉하고 있는 외부환경에 대한 정보를 전혀 사용하지 않으므로, 다양한 외부환경과 접촉하는 상황에서도 적용 가능하며, 충격력의 주파수와 무관하게 모든 충돌을 감지할 수 있다.In addition, since the robot manipulator does not use any information about the external environment with which it is in contact, it can be applied to situations in contact with various external environments, and can detect any collision irrespective of the frequency of the impact force.
도 3 및 도 4의 본 발명의 로봇 머니퓰레이터의 제어 과정에 보다 구체적인 흐름이 도시된 도 5, 도 6 및 도 7은 본 발명의 충돌 감지기를 이용하여 로봇 머니퓰레이터를 제어하는 알고리즘을 나타낸 것이다. 본 발명의 로봇 머니퓰레이터는 도 5에 도시된 바와 같이 상기한 방법을 이용하여 충돌감지기 τ ext 를 측정 또는 추정하고, τ ext 를 이용하여 또 다른 충돌감지기 d p 와 d e 를 산출한다. 그리고 로봇 머니퓰레이터가 수행하고 있는 작업 정보를 상위제어기로부터 수신 받아 로봇 머니퓰레이터가 수행하고 있는 작업이 도 9에 도시된 분류된 작업 유형의 세 가지 분류 중에 어느 경우에 속하는지를 판단한다. FIGS. 5, 6 and 7 illustrate an algorithm for controlling the robot manipulator using the collision detector of the present invention, which is a more specific flow of the control process of the robot manipulator of the present invention shown in FIG. 3 and FIG. The robotic manipulator of the present invention measures or estimates the collision detector τ ext using the above method as shown in FIG. 5, and calculates another collision sensors d p and d e using τ ext . Then, the task information received by the robot manipulator is received from the host controller, and it is determined which of the three types of classified task types shown in FIG. 9 belongs to the task being performed by the robot manipulator.
즉, 도 5에서의 충돌 감지기 산출 단계(S30)가 실행된 후, 제어부(20)는 도 6에 도시된 바와 같은 충돌 판단 단계(S40)를 실행한다. That is, after the collision detector calculation step S30 in FIG. 5 is executed, the
충돌 판단 단계(S40)에서는 제어부(20)가 충돌 감지기(25)와 저장부(30)의 사전 설정 데이터에 포함되는 충돌 판단 기준값을 비교하여 로봇 머니퓰레이터의 충돌 여부를 확인하고 작업 유형을 수행하는 작업 수행 모드와 충돌 상황에 대응하는 충돌 모드 중 실행되어야 할 실행 모드를 설정한다. 충돌 판단 단계(S40)는 판단 작업 유형 확인 단계(S41)와, 판단 작업 유형 판단 단계(S43,S45,S47)와, 충돌 여부 판단 단계(S44,S46,S48)를 포함한다. In the collision determination step S40, the
판단 작업 유형 확인 단계(S41)에서 제어부(20)는 로봇 머니퓰레이터의 작업 유형을 확인하고, 그런 후 판단 작업 유형 판단 단계(S43,S45,S47)에서 판단 작업 유형 확인 단계(S41)에서 확인된 작업 유형을 판단한다. In the judgment operation type confirmation step S41, the
작업 유형이 판단된 후, 제어부(20)는 충돌 여부 판단 단계(S44,S46,S48)를 실행하는데, 판단 작업 유형 판단 단계에서 확인된 작업 유형에 따라, 판단 작업 유형 판단 단계에서의 판단 결과에 대응하는 해당 충돌 감지기를 통하여 충돌 여부를 확인한다. After the determination of the type of operation, the
그리고, 본 발명의 로봇 머니퓰레이터 제어 방법은 충돌 여부 판단 단계에서의 판단 결과에 따라 제어부(20)가 실행 인가할 모드를 설정하는 모드 설정 단계를 더 포함한다. The robot manipulator control method of the present invention further includes a mode setting step of setting a mode to be executed by the
모드 설정 단계(50)에서 제어부(20)는 먼저 실행 모드 확인 단계(S51)를 실행한다. 앞선 단계에서 설정된 실행 모드가 어떠한 것인지의 플랙을 확인한다. In the
그런 후, 단계 S52에서 작업 유형 수행 모드 여부를 확인한다. 즉, 충돌 여부 판단 단계에서 상기 충돌 감지기의 값이 영인 경우, 단계 S52에서 제어부는 모드 설정 단계에서 실행되어야 할 모드는 작업 유형의 실행을 중단하고 충돌 상태에 대응하는 충돌 모드로 설정하여 단계 S58과 S59를 실행한다. Then, in step S52, it is confirmed whether or not the operation mode is the operation type execution mode. That is, if the value of the collision detector is zero in the collision determination step, the controller stops the execution of the operation type in the mode setting step and sets the collision mode corresponding to the collision state in step S52, S59.
작업 유형 수행 모드가 아닌 경우, 제어부(20)는 제어 흐름을 충돌 감지 단계(S58)를 실행하고 충돌 감지 단계(S58)의 결과에 따라 작업 중단 경로 재설정을 실행하는 작업 중단 경로 재설정 단계(S59)를 실행하여 충돌 발생시 안전 사고 발생을 방지 내지 최소화하도록 작업을 중단시키고 로봇 머니퓰레이터의 가동하는 경로를 재설정한다. If the mode is not the operation mode, the
반면, 단계 S52에서 작업 유형 수행 모드 여부 확인시, 즉 충돌 여부 판단 단계에서 상기 충돌 감지기의 값이 영이 아닌 경우, 제어부는 모드 설정 단계에서 실행되어야 할 모드는 작업 유형을 실행하는 작업 모드로 설정하는데, 제어부(20)가 단계 S52에서 작업 유형 수행 모드라고 판단한 경우, 제어부(20)는 제어 흐름을 단계 S53 이하의 단계로 진행하여 작업 유형이 앞서 기술된 첫번째 유형(I 유형)인지(S53), 두번째 유형인지(II 유형)(S55),의 판단 단계를 거쳐, 각 결과에 따라 첫번째 유형(I 유형)(S54), 두번째 유형(II 유형)(S56), 세번째 유형(III유형)(S57)의 작업 모드를 설정한다. On the other hand, when it is determined in step S52 whether or not the operation type execution mode is set, that is, when the value of the collision detector is not zero in the conflict determination step, the controller sets a mode to be executed in the mode setting step to a work mode for executing the operation type , The
본 발명의 일실시예에 따른 로봇 머니퓰레이터가 작업 모드 수행 단계를 실행하는 경우, 수행하는 작업에 따라 각기 다른 충돌감지기를 사용하여 어떠한 유형의 작업시 충돌이 발생하였는지를 판단한다. 로봇 머니퓰레이터에 충돌이 발생한 경우에는 로봇 머니퓰레이터의 작업을 일시적으로 중단하고 작업 경로를 재설정한다. 도 11은 물체를 파지하여 이송하고, 조립하는 작업을 수행하는 로봇 머니퓰레이터에 도 3 내지 도 7에 도시된 본 발명의 로봇 머니퓰레이터의 제어 과정을 적용한 실시예의 유형을 도시한 것이다. When the robot manipulator according to the embodiment of the present invention executes the operation mode performing step, it is determined which type of collision occurred during operation using different collision detectors according to an operation to be performed. When a collision occurs in the robot manipulator, the operation of the robot manipulator is temporarily stopped and the work path is reset. FIG. 11 shows a type of an embodiment in which the control process of the robot manipulator of the present invention shown in FIGS. 3 to 7 is applied to a robot manipulator for gripping, transporting, and assembling an object.
도 11에 도시된 바와 같이 고안된 충돌감지기들과 로봇 머니퓰레이터 제어 알고리즘을 이용할 경우에 물체를 파지하여 이송하거나 외부환경과 접촉하는 작업이 포함된 로봇 머니퓰레이터의 전 작업과정에 걸쳐서 충돌을 감지할 수 있다. When using the collision detectors and the robot manipulator control algorithm designed as shown in FIG. 11, it is possible to detect a collision over the whole operation process of the robot manipulator including an operation of grasping and transporting an object or contacting an external environment.
Claims (11)
상기 외력 토크 산출 단계(S20)는: 상기 구동 모터에 인가되는 모터 전류 및 상기 관절부의 회전각을 포함하는 감지 관절 정보를 감지하는 외력 토크 산출 감지 단계(S21)와, 상기 모터 전류, 상기 감지 관절 정보 및 상기 사전 설정 데이터에 기초하여, 상기 로봇 머니퓰레이터에 인가되는 외력 토크를 확정하는 외력 토크 산출 확정 단계(S23)를 포함하고,
상기 충돌 감지기 산출 단계는: 상기 로봇 머니퓰레이터의 자유 운동 상태를 이루는 자유 운동 모드, 대상체 또는 작업도구를 이송하는 이송 운동 모드, 및 상기 로봇 머니퓰레이터의 외부 환경과 물리적으로 접촉하는 동작을 실행하는 컨택 운동 모드 중 상기 제어부가 인가하여 실행되는 작업 유형을 확인하는 작업 유형 확인 단계(S31)와, 상기 작업 유형 확인 단계(S31)에서 확인된 작업 유형을 판단하는 작업 유형 판단 단계(S33,S35,S37)와, 상기 작업 유형 판단 단계에서의 판단 결과에 대응하는 해당 충돌 감지기를 확정하는 충돌 감지기 확정 단계(S34,S36,S38)을 포함하고,
상기 작업 유형 판단 단계에서 판단된 작업 유형이 상기 자유 운동 모드인 경우, 상기 로봇 머니퓰레이터에 발생하는 충돌로 인한 충격력 Fc로 유발되는 외부 토크(τc)이고, 상기 충돌 감지기는:
상기 작업 유형 판단 단계에서 판단된 작업 유형이 상기 이송 운동 모드인 경우, 상기 로봇 머니퓰레이터에 발생하는 충돌로 인한 충격력 Fc로 유발되는 외부 토크(τc)이고, 상기 로봇 머니퓰레이터가 파지하는 대상체에 작용하는 중력 벡터(Fg) 및 관성 벡터(Fi)의 합력(Fg)에 의한 외부 토크(τp)라 할 때, 상기 외력 토크(τext)는:
상기 충돌 감지기는:
여기서,
상기 fi,fg는 상기 관성 벡터 및 상기 중력벡터의 힘성분을, 상기 ni,ng는 상기 관성 벡터 및 상기 중력벡터의 모멘트 성분을 나타내고,
상기 외력 토크(τext)를 상기 외부 토크(τp)가 존재하는 공간 Sp에 수직한 공간 ⊥Sp로 투영하기 위한 투영 행렬(P)은:
I는 항등행렬,
J는 상기 로봇 머니퓰레이터의 자코비안 행렬이고,
ui와 ug 는 각각 상기 fi,fg와 방향이 같은 단위 벡터를 나타낼 경우,
상기 작업 유형 판단 단계에서 판단된 작업 유형이 상기 컨택 운동 모드인 경우, 상기 로봇 머니퓰레이터에 발생하는 충돌로 인한 충격력 Fc로 유발되는 외부 토크(τc)이고, 상기 로봇 머니퓰레이터의 단부에 작용하는 접촉 외력(Fe)에 의한 외부 토크(τe)라 할 때, 상기 외력 토크(τext)는:
J는 상기 로봇 머니퓰레이터의 자코비안 행렬이고,
상기 충돌 감지기는:
The external force torque calculation step (S20) includes: an external force torque calculation step (S21) of sensing the sensing joint information including the motor current applied to the driving motor and the rotation angle of the joint part; (S23) of determining an external force torque to be applied to the robot manipulator, on the basis of the information and the preset data,
The collision detector calculating step may include: a free motion mode that forms a free motion state of the robot manipulator, a transfer motion mode that conveys an object or a work tool, and a contact motion mode that performs an operation to physically contact the external environment of the robot manipulator (S33, S35, S37) for determining a task type identified in the task type checking step (S31) and a work type determining step (S33) for checking a task type to be executed by the controller (S34, S36, S38) for determining a corresponding collision detector corresponding to the determination result in the operation type determination step,
Wherein the collision detector is an external torque? C caused by an impact force Fc due to a collision occurring in the robot manipulator when the operation type determined in the operation type determination step is the free motion mode,
And an external torque (? C) caused by an impact force Fc due to a collision occurring in the robot manipulator when the operation type determined in the operation type determination step is the transfer motion mode, wherein the gravity acting on the object gripped by the robot manipulator Is an external torque? P by a resultant force Fg of the vector Fg and the inertia vector Fi, the external force torque? Ext is:
The collision detector comprising:
here,
Wherein fi and fg represent force components of the inertia vector and the gravity vector, ni and ng represent moment components of the inertia vector and the gravity vector,
The projection matrix P for projecting the external force torque? Ext in a space? Sp perpendicular to the space Sp in which the external torque?
I is the identity matrix,
J is a Jacobian matrix of the robot manipulator,
When ui and ug denote unit vectors having the same direction as fi and fg, respectively,
And an external torque τc caused by an impact force Fc due to a collision occurring in the robot manipulator when the operation type determined in the operation type determination step is the contact movement mode, Fe), the external force torque? Ext is:
J is a Jacobian matrix of the robot manipulator,
The collision detector comprising:
상기 외력 토크 산출 확정 단계(S23)에서,
상기 외력 토크(τext)는:
τj 은 상기 로봇 머니퓰레이터의 각 관절부에 설치된 구동 모터와 감속기에 의해서 상기 로봇 머니퓰레이터의 관절 링크에 전달되는 관절토크 벡터를, M(q)는 상기 로봇 머니퓰레이터의 각 관절부를 통하여 연결되는 관절 링크의 관성 행렬을,
In the external force torque calculation step S23,
The external force torque? Ext is:
τ j is a joint torque vector transmitted to a joint link of the robot manipulator by a driving motor and a speed reducer provided at each joint of the robot manipulator, M (q) is an inertia torque of a joint link connected through each joint of the robot manipulator The matrix,
상기 충돌 판단 단계(S40)는:
상기 로봇 머니퓰레이터의 작업 유형을 확인하는 판단 작업 유형 확인 단계(S41)와,
상기 판단 작업 유형 확인 단계(S41)에서 확인된 작업 유형을 판단하는 판단 작업 유형 판단 단계(S43,S45,S47)와,
상기 판단 작업 유형 판단 단계에서 확인된 작업 유형에 따라, 상기 판단 작업 유형 판단 단계에서의 판단 결과에 대응하는 해당 충돌 감지기를 통하여 충돌 여부를 확인하는 충돌 여부 판단 단계(S44,S46,S48)를 포함하는 것을 특징으로 하는 로봇 머니퓰레이터 제어 방법.The method according to claim 1,
The conflict determination step S40 may include:
A judgment operation type confirmation step (S41) of confirming the operation type of the robot manipulator,
A determination operation type determination step (S43, S45, S47) for determining the operation type confirmed in the determination operation type confirmation step (S41)
(S44, S46, and S48) for determining whether or not a collision is detected through the corresponding collision detector corresponding to the determination result in the determination operation type determination step, according to the operation type identified in the determination operation type determination step Wherein the robot manipulator control method comprises:
상기 충돌 여부 판단 단계에서 판단 결과에 따라 상기 제어부가 실행 인가할 모드를 설정하는 모드 설정 단계를 더 구비하는 것을 특징으로 하는 로봇 머니퓰레이터 제어 방법.9. The method of claim 8,
Further comprising a mode setting step of setting a mode to be executed by the controller according to a result of the determination in the collision determination step.
상기 충돌 여부 판단 단계에서 상기 충돌 감지기의 값이 영이 아닌 경우, 상기 제어부는 상기 모드 설정 단계에서 실행되어야 할 모드는 상기 작업 유형을 실행하는 작업 모드로 설정하는 것을 특징으로 하는 로봇 머니퓰레이터 제어 방법.10. The method of claim 9,
Wherein the control unit sets the mode to be executed in the mode setting step to a work mode in which the task type is executed when the value of the collision detector is not zero in the step of determining whether or not the collision is detected.
상기 충돌 여부 판단 단계에서 상기 충돌 감지기의 값이 영인 경우, 상기 제어부는 상기 모드 설정 단계에서 실행되어야 할 모드는 상기 작업 유형의 실행을 중단하고 충돌 상태에 대응하는 충돌 모드로 설정하는 것을 특징으로 하는 로봇 머니퓰레이터 제어 방법.10. The method of claim 9,
Wherein if the value of the collision detector is zero at the determining step, the controller stops execution of the operation type and sets the collision mode to a collision mode corresponding to the collision state, Robot manipulator control method.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| KR1020160150474A KR101973709B1 (en) | 2016-11-11 | 2016-11-11 | Method of collision detection of robot arm manipulator |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| KR1020160150474A KR101973709B1 (en) | 2016-11-11 | 2016-11-11 | Method of collision detection of robot arm manipulator |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| KR20180053482A KR20180053482A (en) | 2018-05-23 |
| KR101973709B1 true KR101973709B1 (en) | 2019-04-30 |
Family
ID=62452538
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| KR1020160150474A KR101973709B1 (en) | 2016-11-11 | 2016-11-11 | Method of collision detection of robot arm manipulator |
Country Status (1)
| Country | Link |
|---|---|
| KR (1) | KR101973709B1 (en) |
Families Citing this family (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR102156231B1 (en) | 2018-12-20 | 2020-09-15 | 한국전자기술연구원 | Apparatus, system and method for controlling manipulator in mixed environment |
| DE102019108390B3 (en) * | 2019-04-01 | 2020-08-06 | Franka Emika Gmbh | Specifying safe speeds for a robot manipulator |
| KR102169879B1 (en) * | 2019-06-03 | 2020-10-27 | 한국기계연구원 | collision detection system and method for robot by learning |
| CN110480640B (en) * | 2019-08-26 | 2021-01-29 | 中科新松有限公司 | Robot foot end track planning method for walking on terraced terrain |
| KR102289968B1 (en) | 2019-11-27 | 2021-08-13 | 한국로봇융합연구원 | Apparatus for controlling robot manipulator and method therefor |
| KR102259247B1 (en) | 2019-12-06 | 2021-06-01 | 한국로봇융합연구원 | Strong Localization Method and System through Convergence of Multiple Localization Based on Covariance Matrix |
| DE102019219930B3 (en) * | 2019-12-18 | 2021-01-14 | Kuka Deutschland Gmbh | Method and system for controlling a robot |
| CN111823235A (en) * | 2020-07-22 | 2020-10-27 | 西北农林科技大学 | Collision detection method for picking mechanical arm |
| KR102312982B1 (en) | 2020-08-28 | 2021-10-13 | 한국로봇융합연구원 | Apparatus for recognizing welding target area based on vision sensing and method therefor |
| WO2022097929A1 (en) * | 2020-11-06 | 2022-05-12 | 삼성전자주식회사 | Robot and control method therefor |
| CN113246137A (en) * | 2021-06-09 | 2021-08-13 | 上海机器人产业技术研究院有限公司 | Robot collision detection method based on external moment estimation model |
| CN113442118B (en) * | 2021-06-30 | 2022-12-09 | 山东大学 | Collision response control method and system for wearable outer limb robot |
| CN114872034A (en) * | 2022-06-16 | 2022-08-09 | 北京市商汤科技开发有限公司 | Mechanical arm driving structure, arm type robot and driving method |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101214685B1 (en) * | 2011-03-23 | 2013-01-09 | 고려대학교 산학협력단 | Method for controlling robot manipulator device based on frequency analysys of impact on the device |
-
2016
- 2016-11-11 KR KR1020160150474A patent/KR101973709B1/en active IP Right Grant
Also Published As
| Publication number | Publication date |
|---|---|
| KR20180053482A (en) | 2018-05-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR101973709B1 (en) | Method of collision detection of robot arm manipulator | |
| CN105034025B (en) | The security monitoring device of robot | |
| Lee et al. | Sensorless collision detection for safe human-robot collaboration | |
| US11534918B2 (en) | Device and method for controlling cooperative robot | |
| KR20140147681A (en) | Monitoring of a kinematically redundant robot | |
| EP2586577A1 (en) | Robot control device | |
| US20180164170A1 (en) | Robot collision detection method | |
| JP2016087700A (en) | Control equipment having feature of verifying designation of load information | |
| JP7262847B2 (en) | System Identification of Industrial Robot Dynamics for Safety-Critical Applications | |
| US20190001504A1 (en) | Method For Detecting A Collision Of A Robot Arm With An Object, And A Robot With A Robot Arm | |
| US20040128030A1 (en) | Robot control apparatus and method | |
| US11602863B2 (en) | Device, method and program for estimating weight and position of gravity center of load by using robot | |
| JP6044511B2 (en) | Robot control method and robot system | |
| US11173614B2 (en) | Control apparatus and robot system | |
| JP7481097B2 (en) | Robot Control Device | |
| KR101329853B1 (en) | Collision detection system of manipulator using torque filtering and control system and method of manipulator using the same | |
| Qin et al. | A new approach to the dynamic parameter identification of robotic manipulators | |
| KR101233432B1 (en) | Method for estimating external force 0n robotic system using time-delay estimation | |
| JP2013169609A (en) | Method for detecting collision of robot | |
| CN113352331B (en) | Method for force cooperation between robot and external object and cooperative robot | |
| KR101487624B1 (en) | Method for controlling robot manipulator device with a redundant dof for detecting abnormal external force | |
| US20230090384A1 (en) | Calibrating a virtual force sensor of a robot manipulator | |
| Kurdas et al. | Online payload identification for tactile robots using the momentum observer | |
| KR101064516B1 (en) | Method for Calibrating Force Sensor Detecting Signal to estimate Pure External Interaction Force | |
| KR102643316B1 (en) | Detecting device and method for calculating amount of impact of robot manipulator |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A201 | Request for examination | ||
| E902 | Notification of reason for refusal | ||
| E902 | Notification of reason for refusal | ||
| E701 | Decision to grant or registration of patent right | ||
| GRNT | Written decision to grant |