KR20090044130A - Self collision control method for dual arm robot system - Google Patents
Self collision control method for dual arm robot system Download PDFInfo
- Publication number
- KR20090044130A KR20090044130A KR1020070110069A KR20070110069A KR20090044130A KR 20090044130 A KR20090044130 A KR 20090044130A KR 1020070110069 A KR1020070110069 A KR 1020070110069A KR 20070110069 A KR20070110069 A KR 20070110069A KR 20090044130 A KR20090044130 A KR 20090044130A
- Authority
- KR
- South Korea
- Prior art keywords
- virtual
- link
- links
- control method
- arm robot
- 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/1679—Programme controls characterised by the tasks executed
- B25J9/1682—Dual arm manipulator; Coordination of several manipulators
-
- 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
-
- 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
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/19—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by positioning or contouring control systems, e.g. to control position from one programmed point to another or to control movement along a programmed continuous path
Abstract
본 발명은 양팔 로봇 시스템의 제어방법에 관한 것으로, 더욱 상세하게는 상기 양팔 로봇의 각각의 링크는 링크의 일측부터 타측까지 링크의 부피를 포함하도록 가상의 구를 적어도 하나 이상 중첩하고 연속적으로 배치하여 가상링크를 구축하는 (S10)단계와; 상기 각각의 링크의 이동궤적을 생성하여 링크의 운행을 수행하는 (S20)단계와; 상기 가상의 구의 중심거리를 체크하고, 서로 다른 가상링크의 가상의 구의 중심 간의 거리가 반경의 합보다 작으면 가상링크 간의 중첩으로 판단하고, 반경의 합보다 크면 안전모드로 판단하는 (S30)단계와; 상기 (S30)단계의 판단이 가상링크 간의 중첩으로 판단되면, 각각의 링크의 이동궤적을 무시하고 링크의 운행을 정지 또는 감속하여 새로운 궤적 생성을 대기하는 (S40)단계와; 상기 (S30)단계의 판단이 안전모드이면, 각각의 링크의 이동궤적을 수행하며 새로운 궤적을 생성하는 (S50)단계를 포함하여 이루어지는 것을 특징으로 하는 양팔 로봇 시스템의 충돌제어 방법에 관한 것이다.The present invention relates to a control method of a two-arm robot system, and more particularly, each link of the two-arm robot overlaps at least one or more virtual spheres so as to include the volume of the link from one side to the other side of the link, and continuously arranges them. Establishing a virtual link (S10); Generating movement trajectories of the respective links to perform the operation of the links (S20); Checking the center distance of the virtual sphere, determining that the distance between the centers of the virtual spheres of different virtual links is smaller than the sum of the radiuses as overlap between the virtual links, and determining the safety mode if the distance between the virtual spheres is smaller than the sum of the radiuses (S30). Wow; If it is determined that the determination of the step S30 is overlapping between the virtual links, ignoring the movement trajectories of the respective links and stopping or decelerating the operation of the links to wait for generation of a new trajectory; If the determination of the step (S30) is in the safe mode, it relates to a collision control method of the two-arm robot system characterized in that it comprises the step (S50) of performing a movement trajectory of each link and generating a new trajectory.
로봇, 다관절 암, 가상링크, 충돌방지, 6자유도 Robot, articulated arm, virtual link, collision prevention, 6 degree of freedom
Description
본 발명은 양팔 로봇 시스템의 제어방법에 관한 것으로, 더욱 상세하게는 상기 양팔 로봇의 각각의 링크는 링크의 일측부터 타측까지 링크의 부피를 포함하도록 가상의 구를 적어도 하나 이상 중첩하고 연속적으로 배치하여 가상링크를 구축하는 (S10)단계와; 상기 각각의 링크의 이동궤적을 생성하여 링크의 운행을 수행하는 (S20)단계와; 상기 가상의 구의 중심거리를 체크하고, 서로 다른 가상링크의 가상의 구의 중심 간의 거리가 반경의 합보다 작으면 가상링크 간의 중첩으로 판단하고, 반경의 합보다 크면 안전모드로 판단하는 (S30)단계와; 상기 (S30)단계의 판단이 가상링크 간의 중첩으로 판단되면, 각각의 링크의 이동궤적을 무시하고 링크의 운행을 정지 또는 감속하여 새로운 궤적 생성을 대기하는 (S40)단계와; 상기 (S30)단계의 판단이 안전모드이면, 각각의 링크의 이동궤적을 수행하며 새로운 궤적을 생성하는 (S50)단계를 포함하여 이루어지는 것을 특징으로 하는 양팔 로봇 시스템의 충돌제어 방법에 관한 것이다.The present invention relates to a control method of a two-arm robot system, and more particularly, each link of the two-arm robot overlaps at least one or more virtual spheres so as to include the volume of the link from one side to the other side of the link, and continuously arranges them. Establishing a virtual link (S10); Generating movement trajectories of the respective links to perform the operation of the links (S20); Checking the center distance of the virtual sphere, determining that the distance between the centers of the virtual spheres of different virtual links is smaller than the sum of the radiuses as overlap between the virtual links, and determining the safety mode if the distance between the virtual spheres is smaller than the sum of the radiuses (S30). Wow; If it is determined that the determination of the step S30 is overlapping between the virtual links, ignoring the movement trajectories of the respective links and stopping or decelerating the operation of the links to wait for generation of a new trajectory; If the determination of the step (S30) is in the safe mode, it relates to a collision control method of the two-arm robot system characterized in that it comprises the step (S50) of performing a movement trajectory of each link and generating a new trajectory.
등록특허 10-0213713호(발명의 명칭: 듀얼 로봇 시스템 및 그 제어방법)는 스카라 로봇 2대가 서로 충돌하지 않고 제어되는 방법에 관해 기술되어 있다. Patent No. 10-0213713 (name of the invention: a dual robot system and a control method thereof) describes a method in which two Scara robots are controlled without colliding with each other.
구체적으로는, 단일의 로봇 베이스에 각각 제 1 및 제 2 아암을 포함하는 2 대의 로봇이 동시에 배치되고, 상기 각각의 로봇을 제어하는 로봇 제어부가 각각 마련되어 있는 듀얼 로봇 시스템에 있어서, 상기 각각의 로봇 제어부는 상기 각각의 로봇의 단위시간 후의 이동궤적에 대한 정보를 입출력하기 위한 별도의 공유기억장치에 각각 연결되어 있어서, 상기 공유기억장치에 입력된 상대 로봇의 단위시간 후의 이동궤적에 대한 정보를 이용함으로써 상기 각각의 로봇 상호간의 충돌을 피할 수 있도록 한 것을 특징으로 하는 듀얼 로봇 시스템과, 하나의 로봇 베이스에 장착되어 있는 2 대의 로봇을 구성하는 각각의 아암이 단위시간후에 이동되어야 하는 각각의 궤적을 생성하는 아암 궤적생성단계와, 상기 아암 궤적생성단계에서 생성된 각각의 아암의 단위시간 후의 이동 궤적의 좌표값을 이용하여 상기 각각의 아암을 직사각형으로 모델링하는 아암 모델링 단계와, 상기 아암 모델링 단계에서 모델링된 각각의 아암에 대한 직사각형 및 이미 모델링되어 공유기억장치에 저장되어 있는 로봇 베이스에 대한 직사각형이 서로 중첩되는지의 여부를 판단함으로써 상기 각각의 아암이 상기 로봇 베이스 및 다른 아암과 충돌이 일어나는지의 여부를 판단하는 아암 충돌판단단계와, 상기의 아암 충돌판단단계에서의 판단결과, 충돌이 일어나는 것으로 판단된 경우에는, 이미 생성된 상기 각각의 아암에 대한 각각의 궤적을 무시하고 상기 각각의 아암을 감속시켜 정지시킨 후, 다시 처음으로 돌아가서 상기 각각의 아암이 단위시간후에 이동되어야 하는 새로운 궤적을 생성하는 아암 충돌방지단계와, 상기의 아암 충돌판단단계에서의 판단결과, 충돌이 일어나지 않는 것으로 판단된 경우에는, 상기 각각의 아암의 생성된 궤적에 대한 모델링 정보를 상기 공유기억장치에 저장한 후, 상기 각각의 아암을 구동시켜 이동시키는 아암 구동단계와, 상기 각각의 아암이 목표위치까지 도달되었는지를 검사하는 아암 목표위치도달 판단단계와, 상기 아암 목표위치도달 판단단계에서의 판단결과, 상기 각각의 아암이 소정의 목표위치에 도달되지 않은 경우에는, 다시 처음으로 돌아가서 상기 각각의 아암이 단위시간후에 이동되어야 하는 새로운 궤적을 생성하는 단계 및 상기 아암 목표위치도달 판단단계에서의 판단결과, 상기 각각의 아암이 소정의 목표위치에 도달된 경우에는, 상기 각각의 아암의 구동을 정지하는 아암 구동정지단계로 구성된 것을 특징으로 하는 듀얼 로봇 시스템의 제어방법으로 구분된다.Specifically, in a single robot base, two robots each including first and second arms are disposed at the same time, and a robot control unit for controlling each of the robots is provided in the dual robot system. The control unit is connected to a separate shared memory device for inputting and outputting information on the movement trajectory after the unit time of each robot, and uses information on the movement trajectory after the unit time of the counterpart robot input to the shared memory device. The dual robot system characterized in that the collision between the respective robots can be avoided, and each trajectory of each of the two robots mounted on one robot base should be moved after a unit time. Arm trajectory generation step to generate, and each of the arms generated in the arm trajectory generation step An arm modeling step of modeling each arm into a rectangle using coordinate values of a trajectory after the shifting time; a rectangle for each arm modeled in the arm modeling step and a robot already modeled and stored in a shared memory device An arm collision determination step of determining whether each arm collides with the robot base and another arm by judging whether or not the rectangles for the base overlap each other, and the determination result in the arm collision determination step, If it is determined that a collision has occurred, the respective trajectories for the respective arms that have already been generated are ignored, the respective arms must be decelerated and stopped, and then returned to the beginning to be moved again after each unit of time. An arm collision preventing step of generating a new trajectory, As a result of the determination at the collision determination step, when it is determined that no collision occurs, the arm storing the modeling information about the generated trajectory of each arm in the shared memory device and then driving and moving the respective arms A driving step, an arm target position determination step of checking whether each arm has reached the target position, and as a result of the determination at the arm target position determination step, the respective arms do not reach a predetermined target position; In the case of returning to the beginning again, generating a new trajectory in which the respective arms should be moved after a unit time and determining the arm target position reaching step, the respective arms reach a predetermined target position. Dew, characterized in that the arm drive stop step of stopping the driving of each arm, characterized in that It is divided into a control system of the robot way.
이러한 기술은 두대의 스카라 로봇의 작업 공간이 협소하더라도 두 로봇의 충돌을 방지하기 위한 것으로 로봇의 링크의 형상을 직사각형으로 단순화하고 이 직사각형들 간의 간섭을 체크함으로써 두 로봇의 충돌을 예측하여 방지한다.This technique prevents collision between two robots even if the working space of two Scara robots is small. By simplifying the link shape of the robot into a rectangle and checking the interference between the rectangles, the collision between the two robots is predicted and prevented.
따라서, 상기 기술은 1개의 직사각형에 존재된 4개의 선분과 이 선분들의 교점이 존재하는지의 여부를 통해 직사각형의 간섭을 계산하는 것이 요지이며 링크간의 간섭여부가 계산되면 이에 따라 로봇의 운전방식을 결정할 수 있다. 즉 간섭이 발생되면(직사각형의 충돌이 예상되면) 로봇에 입력된 궤적명령을 무시하고 로봇을 감속하여 정지하는 것이다.Therefore, the technique is to calculate the interference of the rectangle based on whether the four line segments existing in one rectangle and the intersection of these line segments exist, and if the interference between links is calculated, the operation method of the robot is determined accordingly. You can decide. In other words, if an interference occurs (a rectangular collision is expected), the trajectory command input to the robot is ignored and the robot is decelerated and stopped.
그러나 상기 기술은 2자유도의 로봇베이스에 6자유도 운동을 하는 수직다관절로봇이 양측에 결합된 로봇에는 적용하기 어려운 단점이 있다. 이는 상기 기술이 상기 로봇에 적용하기 위해 각각의 링크를 12개의 선분이 존재하는 6면체로 모델링 하더라도 6면체는 3차원 공간상에서 6면체끼리 간섭이 발생한다 해도 항상 교점을 갖지는 않기 때문이다. 따라서 2차원적으로 선분들의 교점이 존재하는지 여부를 판단하여 링크의 충돌을 예상하는 상기 기술은 2자유도의 로봇베이스에 6자유도 운동을 하는 수직 다관절로봇이 양측에 결합된 로봇에는 적용될 수 없는 문제점을 갖는다.However, the above technique has a disadvantage in that it is difficult to apply to a robot in which a vertical articulated robot having six degrees of freedom in a two degree of freedom robot base is coupled to both sides. This is because even though the linkage is modeled as a hexahedron with 12 line segments in order to apply the technique to the robot, the hexahedron does not always have an intersection even if interference occurs between the hexahedrons in three-dimensional space. Therefore, the above-described technique for predicting the collision of the link by judging whether the intersection of line segments exists in two dimensions can be applied to the robot which is coupled to both sides of the vertical articulated robot which has six degrees of freedom movement on the robot base of two degrees of freedom. There is no problem.
본 발명은 상기와 같은 문제점을 해결하기 위하여 안출된 것으로, 본 발명의 목적은 2자유도의 베이스와 상기 베이스에 6자유도 이상의 다관절 암을 양측에 각각 결합한 로봇 시스템에 있어서, 왼쪽 암과 오른쪽 암과의 충돌, 또는 왼쪽 암과 베이스의 충돌, 또는 오른쪽 암과 베이스의 충돌을 사전에 방지하는 과정을 체크하여 로봇 시스템의 원활한 작업을 유도하는 양팔 로봇 시스템의 충돌제어 방법을 제공하는데 있다.The present invention has been made to solve the above problems, the object of the present invention is a robot system in which the two-degree of freedom base and the six-degree of freedom articulated arm on both sides of the robot system respectively coupled to the left arm and the right arm The present invention provides a collision control method of a two-arm robot system that induces a smooth operation of the robot system by checking a process of preventing a collision with or a collision between a left arm and a base or a collision between a right arm and a base in advance.
상기와 같은 본 발명의 목적은 양팔 로봇 시스템의 제어방법에 있어서, 상기 양팔 로봇의 각각의 링크는 링크의 일측부터 타측까지 링크의 부피를 포함하도록 가상의 구를 적어도 하나 이상 중첩하고 연속적으로 배치하여 가상링크를 구축하는 (S10)단계와; 상기 각각의 링크의 이동궤적을 생성하여 링크의 운행을 수행하는 (S20)단계와; 상기 가상의 구의 중심거리를 체크하고, 서로 다른 가상링크의 가상의 구의 중심 간의 거리가 반경의 합보다 작으면 가상링크 간의 중첩으로 판단하고, 반경의 합보다 크면 안전모드로 판단하는 (S30)단계와; 상기 (S30)단계의 판단이 가상링크 간의 중첩으로 판단되면, 각각의 링크의 이동궤적을 무시하고 링크의 운행을 정지 또는 감속하여 새로운 궤적 생성을 대기하는 (S40)단계와; 상기 (S30)단계의 판단이 안전모드이면, 각각의 링크의 이동궤적을 수행하며 새로운 궤적을 생성하는 (S50)단계를 포함하여 이루어지는 것을 특징으로 하는 양팔 로봇 시스템의 충돌제어 방법과,An object of the present invention as described above is a control method of a two-armed robot system, each link of the two-armed robot overlaps at least one or more virtual spheres and continuously arranged so as to include the volume of the link from one side to the other side of the link; Establishing a virtual link (S10); Generating movement trajectories of the respective links to perform the operation of the links (S20); Checking the center distance of the virtual sphere, determining that the distance between the centers of the virtual spheres of different virtual links is smaller than the sum of the radiuses as overlap between the virtual links, and determining the safety mode if the distance between the virtual spheres is smaller than the sum of the radiuses (S30). Wow; If it is determined that the determination of the step S30 is overlapping between the virtual links, ignoring the movement trajectories of the respective links and stopping or decelerating the operation of the links to wait for generation of a new trajectory; If the determination of the step (S30) is the safe mode, the collision control method of the two-arm robot system characterized in that it comprises a step (S50) of performing a movement trajectory of each link and generating a new trajectory,
다관절의 링크와 2자유도 베이스를 갖는 양팔 로봇 시스템의 제어방법에 있어서, 상기 양팔 로봇의 각각의 링크, 베이스는 링크, 베이스의 일측부터 타측까지 링크, 베이스의 부피를 포함하도록 가상의 구를 적어도 하나 이상 중첩하고 연속적으로 배치하여 가상링크를 구축하는 (S100)단계와; 상기 각각의 링크, 베이스의 이동궤적을 생성하여 이동궤적에 따라 운행을 수행하는 (S200)단계와; 상기 가상의 구의 중심거리를 체크하고, 서로 다른 가상링크의 가상의 구의 중심 간의 거리가 반경의 합보다 작으면 가상링크 간의 중첩으로 판단하고, 반경의 합보다 크면 안전모드로 판단하는 (S300)단계와; 상기 (S300)단계의 판단이 가상링크 간의 중첩으로 판단되면, 각각의 링크, 베이스의 이동궤적을 무시하고 링크, 베이스의 운행을 정지 또는 감속하여 새로운 궤적 생성을 대기하는 (S400)단계와; 상기 (S300)단계의 판단이 안전모드이면, 각각의 링크, 베이스의 이동궤적을 수행하며 새로운 궤적을 생성하는 (S500)단계를 포함하여 이루어지는 것을 특징으로 하는 양팔 로봇 시스템의 충돌제어 방법에 의해 달성된다.In a control method of a two-arm robot system having a multi-joint link and a two-degree of freedom base, each link and base of the two-arm robot includes a link, a link from one side of the base to the other side, and a volume of a virtual sphere. Overlapping at least one or more consecutively and establishing a virtual link (S100); Generating movement trajectories of the respective links and bases to perform driving according to the movement trajectories (S200); Checking the center distance of the virtual sphere, if the distance between the centers of the virtual spheres of different virtual links is smaller than the sum of the radii, determining that the virtual links overlap, and determining the safety mode if the distance between the virtual spheres is greater than the sum of the radii (S300). Wow; If the determination of the step (S300) is determined to overlap between the virtual links, ignoring the movement trajectory of each link, the base and stop or decelerate the operation of the link, the base (S400) and waiting for the generation of a new trajectory; If the determination of the step (S300) is a safe mode, by performing the movement trajectory of each link, the base and generating a new trajectory (S500) achieved by the collision control method of the two-arm robot system, characterized in that made do.
이상과 같은 본 발명에 의하면, 왼쪽 암과 오른쪽 암과의 충돌, 왼쪽 암과 베이스의 충돌, 오른쪽 암과 베이스의 충돌을 미연에 방지하면서 로봇의 효율적인 궤적명령을 수행할 수 있는 장점이 있다. 또한, 왼쪽 암의 각각의 링크, 오른쪽 암 의 각각의 링크, 2자유도의 각각의 베이스에 구축된 가상링크의 가상의 구의 크기를 증가 또는 감소하여 산업현장의 여건에 따라 로봇을 맞춤식으로 구축할 수 있다. 예컨대 산업현장의 여건이 효율적인 작업이 아닌 안전한 작업위주이면 가상의 구의 크기를 증가시키고, 산업현장의 여건이 안전한 작업위주보다 효율적인 작업을 요하는 경우엔 가상의 구의 크기를 감소시키는 등 산업현장의 여건에 따라 조절할 수 있는 장점이 있다.According to the present invention as described above, while preventing the collision between the left arm and the right arm, the collision of the left arm and the base, the collision between the right arm and the base in advance there is an advantage that can perform the efficient trajectory command of the robot. In addition, by increasing or decreasing the size of the virtual sphere of the virtual link established in each link of the left arm, each link of the right arm, and each base of two degrees of freedom, the robot can be customized according to the industrial conditions. have. For example, if the conditions of the industrial site are safe work rather than efficient work, increase the size of the virtual sphere, and if the conditions of the industrial site require more efficient work than the safe work, reduce the size of the virtual sphere. There is an advantage that can be adjusted according to.
본 발명은 양팔 로봇 시스템의 제어방법에 관한 것으로, 상기 양팔 로봇의 각각의 링크는 링크의 일측부터 타측까지 링크의 부피를 포함하도록 가상의 구를 적어도 하나 이상 중첩하고 연속적으로 배치하여 가상링크를 구축하는 (S10)단계와, 상기 각각의 링크의 이동궤적을 생성하여 링크의 운행을 수행하는 (S20)단계와, 상기 가상의 구의 중심거리를 체크하고, 서로 다른 가상링크의 가상의 구의 중심 간의 거리가 반경의 합보다 작으면 가상링크 간의 중첩으로 판단하고, 반경의 합보다 크면 안전모드로 판단하는 (S30)단계와, 상기 (S30)단계의 판단이 가상링크 간의 중첩으로 판단되면, 각각의 링크의 이동궤적을 무시하고 링크의 운행을 정지 또는 감속하여 새로운 궤적 생성을 대기하는 (S40)단계와, 상기 (S30)단계의 판단이 안전모드이면, 각각의 링크의 이동궤적을 수행하며 새로운 궤적을 생성하는 (S50)단계를 포함하여 이루어진다.The present invention relates to a control method of a two-arm robot system, wherein each link of the two-arm robot overlaps at least one or more virtual spheres so as to include the volume of the link from one side to the other side of the link to establish a virtual link. (S10) and generating a movement trajectory of each link (S20) to perform the operation of the link, check the center distance of the virtual sphere, the distance between the center of the virtual sphere of different virtual links If less than the sum of the radius is determined as the overlap between the virtual links, if greater than the sum of the radius is determined as the safety mode (S30) and if the determination of the step (S30) is determined as the overlap between the virtual links, each link (S40) of ignoring the movement trajectory of the link and stopping or decelerating the operation of the link to wait for generation of a new trajectory, and if the determination of the step (S30) is a safe mode, And performing a movement trajectory and generating a new trajectory (S50).
또한, 본 발명은 다관절의 링크와 2자유도 베이스를 갖는 양팔 로봇 시스템 의 제어방법에도 적용될 수 있다. 보다 구체적으로는 상기 양팔 로봇의 각각의 링크, 베이스는 링크, 베이스의 일측부터 타측까지 링크, 베이스의 부피를 포함하도록 가상의 구를 적어도 하나 이상 중첩하고 연속적으로 배치하여 가상링크를 구축하는 (S100)단계와, 상기 각각의 링크, 베이스의 이동궤적을 생성하여 이동궤적에 따라 운행을 수행하는 (S200)단계와, 상기 가상의 구의 중심거리를 체크하고, 서로 다른 가상링크의 가상의 구의 중심 간의 거리가 반경의 합보다 작으면 가상링크 간의 중첩으로 판단하고, 반경의 합보다 크면 안전모드로 판단하는 (S300)단계와, 상기 (S300)단계의 판단이 가상링크 간의 중첩으로 판단되면, 각각의 링크, 베이스의 이동궤적을 무시하고 링크, 베이스의 운행을 정지 또는 감속하여 새로운 궤적 생성을 대기하는 (S400)단계와, 상기 (S300)단계의 판단이 안전모드이면, 각각의 링크, 베이스의 이동궤적을 수행하며 새로운 궤적을 생성하는 (S500)단계를 포함하여 이루어진다.In addition, the present invention can be applied to a control method of a two-arm robot system having a multi-joint link and a two degree of freedom base. More specifically, each link of the two arms robot, the base is a link, the link from one side to the other side of the base, at least one virtual sphere overlapping and continuously arranged to include the volume of the base to build a virtual link (S100) Step (S200) of generating a movement trajectory of each of the links and the base and performing the operation according to the movement trajectory, and checking a center distance of the virtual sphere and between the centers of the virtual spheres of different virtual links. If the distance is smaller than the sum of the radius, it is determined that the overlap between the virtual links, and if the distance is greater than the sum of the radius (S300) and the determination of the step (S300) is determined as the overlap between the virtual links, each of the Ignoring the movement trajectory of the link and the base and stopping or decelerating the operation of the link and the base to wait for the generation of a new trajectory (S400) and the determination of the step (S300). If it is the full mode, and performing the movement trajectory of each link, the base and generating a new trajectory (S500).
본 발명이 제어하고자 하는 로봇은 도 1에 도시된 바와 같이 두개의 6자유도 운동을 하는 다관절 암(A,B)과 2자유도의 베이스(7,8,9)를 갖기 때문에, 종래의 로봇 제어방법에서 링크를 일정한 두께를 가지는 직사각형으로 모델링하고 2차원 평면상의 사각형의 간섭을 확인함으로써 두 로봇의 충돌을 연산할 수 있는 스카라 로봇(운동이 평면에서만 이루어짐)의 경우는 본 발명에서 제어하고자 하는 로봇의 링크(10,10')의 충돌을 확인하기 어렵다.Since the robot to be controlled by the present invention has two articulated arms (A, B) that perform two six degrees of freedom movement and two degrees of freedom (7, 8, and 9) as shown in FIG. In the control method, the model of the link is a rectangle having a certain thickness and the case of the Scara robot (movement is only in the plane) that can calculate the collision of the two robots by checking the interference of the rectangle on the two-dimensional plane in the present invention It is difficult to check the collision of the
따라서, 본 발명은 각각의 링크를 실제 링크(10,10')보다 큰 "가상링크(Virtual Link)"를 설정하였다. 즉, 가상링크(20)는 3차원 공간상에서 실제 링 크(10,10')의 부피를 모두 포함하는 가상의 볼륨이다. 이러한 가상링크(20)는 다수의 가상의 구(21,22,23,24,…,29)를 사용하여 만들어질 수 있다. 도 2는 다수의 가상의 구(21,22,23,24,…,29)를 사용하여 가상링크(20)를 구성한 일예를 보여주고 있다. 도 2에서는 크기가 큰 4개의 가상의 구와 비교적 크기가 작은 5개의 가상의 구를 연속적으로 배열하여 실제의 링크(10)를 완전히 감싸는 가상링크(20)를 구축한다.Accordingly, the present invention establishes a "virtual link" in which each link is larger than the
또한, 도 3에서는 이러한 가상링크(20)를 이용하여 어떻게 링크(10,10') 간의 충돌이 계산될 수 있는지에 대한 일예를 보여주고 있다. 도 3은 도시된 바와 같이 실제 링크(10,10')는 서로 충돌하지 않았지만 가상링크(20,20')에 속한 각각의 가상의 구(25,21')는 서로 충돌하였다. 이는 두개의 가상링크(20,20')의 충돌을 의미하고, 실제 2개의 링크(10,10')가 충돌하지 않았지만 2개의 가상링크(20,20')가 충돌했으므로 이를 실제 링크(10,10')가 충돌할 위험이 있는 상태로 간주한다.In addition, FIG. 3 shows an example of how a collision between
이러한 설정은 가상링크(20,20')의 부피를 크게 하면 충돌감지의 안정성은 향상되는 반면 로봇의 작업 계획의 측면이나 효율적인 작업의 측면에서는 비효율적이 된다. 또한, 이와 반대되는 개념으로 가상링크(20,20')의 부피를 작게 하면 충돌감지의 안정성은 감소되는 반면 로봇의 작업계획의 측면이나 효율적인 작업의 측면에서는 향상되는 효과를 갖는다. 여기서 가상링크(20,20')의 부피를 크게 하거나 작게 하는 것은 가상링크(20,20')를 이루는 가상의 구의 지름의 크기에 의해 정해진다.This setting increases the stability of the collision detection by increasing the volume of the
따라서, 가상의 구의 반경 및 개수를 조절하여 가상링크(20,20')의 부피를 크게 혹은 작게 하는 것은 로봇의 정밀도에 따라 사용자가 적절히 조절될 수 있다.Therefore, to increase or decrease the volume of the
도 4는 2대의 다관절 암(A,B)에 가상의 구를 설정하고 일련의 가상의 구들을 이용하여 가상링크를 설정하는 방법에 관한 일예를 보여주고 있다. 도 4에서 동일한 가상링크에 속한 가상의 구는 점선 혹은 실선으로 표시하여 그 구분이 용이하도록 하였다.4 shows an example of a method of setting up a virtual sphere in two articulated arms (A, B) and a virtual link using a series of virtual spheres. In FIG. 4, the virtual spheres belonging to the same virtual link are indicated by a dotted line or a solid line to easily distinguish them.
여기서, 가상링크가 충돌했는가의 여부는 충돌의 가능성이 있는 모든 가상링크의 모든 가상의 구들 중 한쌍이라도 충돌했는지의 여부를 계산함으로써 결정된다. 즉 2개의 가상의 구의 중심(Center) 간의 거리를 계산하고 이 거리가 두개의 가상의 구의 반경의 합보다 작으면 두 개의 가상의 구가 서로 충돌했음을 의미하며, 동시에 두 가상링크가 서로 충돌할 위험이 있음을 의미한다.Here, whether or not the virtual link has collided is determined by calculating whether or not a pair of all virtual spheres of all virtual links which have a possibility of collision have collided. In other words, if the distance between the centers of two virtual spheres is calculated and this distance is less than the sum of the radius of the two virtual spheres, it means that two virtual spheres collided with each other, and at the same time the risk of two virtual links colliding with each other This means that there is.
한편, 가상의 구의 충돌을 확인하기 위해서는 아래와 같은 작업을 요하게 된다.On the other hand, to check the collision of a virtual sphere, the following work is required.
우선, 로봇의 전진운동학(Foward Kinematics)을 계산하여 로봇의 링크에 고정된 좌표간 간의 관계를 규정하는 메트릭스(Transformation Matrix)를 결정한다. 또한, 메트릭스(Transformation Matrix)에 가상의 구의 중심좌표값(해당 링크에 고정된 좌표계에 대해 상대적으로 표현된 중심좌표값 : (x_k, y_k, z_k)을 나타내는 위치 Vector를 곱하여 Global 좌표계(로봇의 고정 좌표계)에 대해 가상의 구의 위치가 어디에 있는지(가상의 구의 절대위치 : (x, y, z))를 계산한다(도 5 및 도 6 참조). 그리고 상기에서 언급한 절차에 따라 모든 가상의 구의 절대위치가 계산되면 가상의 구의 중심점의 절대위치를 알기 때문에 가상의 구가 서로 충돌하는지의 여부는 도 7에 도시된 바와 같이 판단할 수 있다.First, the forward kinematics of the robot is calculated to determine a transformation matrix that defines the relationship between coordinates fixed to the link of the robot. In addition, the transformation matrix is multiplied by the position vector representing the center coordinate value of the imaginary sphere (relative to the coordinate system fixed to the link: (x_k, y_k, z_k). Calculate the location of the virtual sphere (absolute position of the virtual sphere: (x, y, z)) relative to the coordinate system (see Figs. 5 and 6), and in accordance with the above-mentioned procedure When the absolute position is calculated, since the absolute position of the center point of the virtual sphere is known, whether or not the virtual spheres collide with each other can be determined as shown in FIG.
도 7은 서로 다른 가상의 구(26,27')의 충돌을 나타내며 식으로는 d ≤ r1 + r2로 표현되며, 가상의 구들 간의 충돌이 하나라도 발생했다면 링크의 충돌 위험이 있는 것으로 판단한다.FIG. 7 illustrates collision of different
한편, 상기 과정에 의해 가상의 구들 간의 충돌을 검사하는 방법은 도 8에 도시된 바와 같다.On the other hand, the method for checking the collision between the virtual spheres by the above process is as shown in FIG.
도 8은 가상링크 설정을 위한 가상의 구 설정을 우선적으로 실시하고 링크에 부착된 좌표계에 대한 가상의 구의 상대 위치를 설정한 후 로봇의 운전을 시작한다.FIG. 8 preferentially sets a virtual sphere for virtual link setting, sets a relative position of the virtual sphere with respect to a coordinate system attached to the link, and then starts robot operation.
로봇은 새로운 궤적명령이 입력되면 로봇의 전진운동학(Forward Kinematics)를 계산하고, 전진운동학 연산 결과를 이용하여 모든 가상의 구의 중심위치를 로봇기준 좌표계에 대한 표현으로 변환하여 다관절 암(A)의 가상의 구들과 다른 다관절 암(B)의 가상의 구들 간에 충돌이 있는가를 판단하여 충돌이 있으면 로봇의 동작을 감속시키거나 정지시키고, 충돌이 없으면 궤적명령을 수행한 다음 새로운 궤적명령 입력을 대기하거나 운전을 종료하게 된다.When a new trajectory command is input, the robot calculates forward kinematics of the robot and converts the center positions of all virtual spheres into representations of the robot reference coordinate system using the results of the forward kinematics calculations. Determining whether there is a collision between the imaginary spheres and the imaginary spheres of another articulated arm (B), if there is a collision, slow down or stop the robot's operation, if there is no collision, perform the trajectory command, and then wait for a new trajectory command input. The operation ends.
도 1은 6자유도 다관절 암을 갖는 로봇의 일예를 나타낸 도면,1 is a view showing an example of a robot having a six degree of freedom articulated arm,
도 2는 본 발명에 따른 가상링크의 구축예를 나타낸 도면,2 is a view showing an example of construction of a virtual link according to the present invention;
도 3은 본 발명에 따른 가상링크를 이용한 링크의 충돌예를 나타낸 도면,3 is a view showing a collision example of a link using a virtual link according to the present invention;
도 4는 두개의 다관절 암에 가상의 구를 이용하여 가상링크의 구축예를 나타낸 도면,4 is a diagram showing an example of constructing a virtual link using a virtual sphere in two articulated arms;
도 5는 본 발명에 따른 링크의 기준 좌표계, k번째 링크에 부착된 좌표계, k번째 링크의 가상의 구들 중 임의의 가상의 구의 k번째 링크에 부착된 좌표계에 대한 상대적인 위치 좌표(x, y, z)를 나타낸 도면,5 is a position coordinate (x, y, relative to a reference coordinate system of a link according to the present invention, a coordinate system attached to a k-th link, and a coordinate system attached to a k-th link of any virtual sphere of the k-th link. z),
도 6은 본 발명에 따른 가상의 구의 중심점의 로봇 기준좌표계에 대한 절대위치를 계산하는 방법을 나타낸 도면,6 is a view showing a method for calculating an absolute position with respect to the robot reference coordinate system of the center point of the virtual sphere according to the present invention;
도 7은 본 발명에 따른 서로 다른 가상의 구의 반경이 중첩되어 충돌된 것을 나타낸 도면,7 is a view showing that the collision of overlapping radius of different virtual spheres according to the present invention,
도 8은 본 발명에 따른 양팔 로봇 시스템의 충돌방지 방법의 순서도.8 is a flow chart of the collision avoidance method of the two-arm robot system according to the present invention.
< 도면의 주요부분에 대한 부호의 설명 ><Description of Symbols for Major Parts of Drawings>
A,B: 다관절 암 7,8,9: 베이스A, B: articulated
10,10': 링크 20,20': 가상링크10,10 ': link 20,20': virtual link
Claims (6)
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| KR1020070110069A KR101012742B1 (en) | 2007-10-31 | 2007-10-31 | Self Collision Control Method for Dual Arm Robot System |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| KR1020070110069A KR101012742B1 (en) | 2007-10-31 | 2007-10-31 | Self Collision Control Method for Dual Arm Robot System |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| KR20090044130A true KR20090044130A (en) | 2009-05-07 |
| KR101012742B1 KR101012742B1 (en) | 2011-02-09 |
Family
ID=40854739
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| KR1020070110069A KR101012742B1 (en) | 2007-10-31 | 2007-10-31 | Self Collision Control Method for Dual Arm Robot System |
Country Status (1)
| Country | Link |
|---|---|
| KR (1) | KR101012742B1 (en) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP2559524A1 (en) * | 2011-08-16 | 2013-02-20 | Kabushiki Kaisha Yaskawa Denki | Robot control system and robot control method |
| CN103593992A (en) * | 2013-11-27 | 2014-02-19 | 时延昭 | Solar traffic police robot |
| CN103722565A (en) * | 2014-01-23 | 2014-04-16 | 哈尔滨工业大学 | Self-collision monitoring system for humanoid robot and monitoring method thereof |
| CN108789402A (en) * | 2017-04-29 | 2018-11-13 | 王恒升 | A kind of active collision prevention method of special type mobile robot multi-arm movement |
| CN112549017A (en) * | 2020-10-27 | 2021-03-26 | 南京凌华微电子科技有限公司 | Double-arm robot cooperative space solving method for avoiding joint limit |
| CN113855243A (en) * | 2020-06-30 | 2021-12-31 | 北京术锐技术有限公司 | Telecentric movement mechanism and surgical robot system |
| CN115716265A (en) * | 2022-10-31 | 2023-02-28 | 中国电器科学研究院股份有限公司 | Robot double-arm collision neural reflex control method |
| CN116394266A (en) * | 2023-06-08 | 2023-07-07 | 国网瑞嘉(天津)智能机器人有限公司 | Robot self-collision processing method and device, robot and medium |
| CN116985184A (en) * | 2023-09-27 | 2023-11-03 | 睿尔曼智能科技（北京）有限公司 | Model prediction-based robot self-collision detection method and system and robot |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104097205B (en) * | 2013-04-07 | 2016-02-17 | 同济大学 | The robot real time kinematics self collision in task based access control space avoids control method |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2895672B2 (en) * | 1992-01-28 | 1999-05-24 | ファナック株式会社 | Multiple robot control method |
| KR100213713B1 (en) * | 1997-04-24 | 1999-08-02 | 윤종용 | Dual robot system and its controlling method |
| JP2000039911A (en) * | 1998-07-22 | 2000-02-08 | Meidensha Corp | Robot controller |
| JP3975341B2 (en) * | 2002-05-15 | 2007-09-12 | 株式会社安川電機 | Robot interference check method |
-
2007
- 2007-10-31 KR KR1020070110069A patent/KR101012742B1/en active IP Right Grant
Cited By (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP2559524A1 (en) * | 2011-08-16 | 2013-02-20 | Kabushiki Kaisha Yaskawa Denki | Robot control system and robot control method |
| US9108315B2 (en) | 2011-08-16 | 2015-08-18 | Kabushiki Kaisha Yaskawa Denki | Robot control system and robot control method |
| CN103593992A (en) * | 2013-11-27 | 2014-02-19 | 时延昭 | Solar traffic police robot |
| CN103722565A (en) * | 2014-01-23 | 2014-04-16 | 哈尔滨工业大学 | Self-collision monitoring system for humanoid robot and monitoring method thereof |
| CN108789402A (en) * | 2017-04-29 | 2018-11-13 | 王恒升 | A kind of active collision prevention method of special type mobile robot multi-arm movement |
| CN113855243A (en) * | 2020-06-30 | 2021-12-31 | 北京术锐技术有限公司 | Telecentric movement mechanism and surgical robot system |
| CN113855243B (en) * | 2020-06-30 | 2024-01-23 | 北京术锐机器人股份有限公司 | Telecentric motion mechanism and surgical robot system |
| CN112549017A (en) * | 2020-10-27 | 2021-03-26 | 南京凌华微电子科技有限公司 | Double-arm robot cooperative space solving method for avoiding joint limit |
| CN115716265A (en) * | 2022-10-31 | 2023-02-28 | 中国电器科学研究院股份有限公司 | Robot double-arm collision neural reflex control method |
| CN115716265B (en) * | 2022-10-31 | 2023-11-10 | 中国电器科学研究院股份有限公司 | Robot double-arm collision nerve reflex control method |
| CN116394266A (en) * | 2023-06-08 | 2023-07-07 | 国网瑞嘉(天津)智能机器人有限公司 | Robot self-collision processing method and device, robot and medium |
| CN116394266B (en) * | 2023-06-08 | 2023-10-20 | 国网瑞嘉(天津)智能机器人有限公司 | Robot self-collision processing method and device, robot and medium |
| CN116985184A (en) * | 2023-09-27 | 2023-11-03 | 睿尔曼智能科技（北京）有限公司 | Model prediction-based robot self-collision detection method and system and robot |
| CN116985184B (en) * | 2023-09-27 | 2024-01-26 | 睿尔曼智能科技（北京）有限公司 | Model prediction-based robot self-collision detection method and system and robot |
Also Published As
| Publication number | Publication date |
|---|---|
| KR101012742B1 (en) | 2011-02-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR101012742B1 (en) | Self Collision Control Method for Dual Arm Robot System | |
| US8311731B2 (en) | Robots with collision avoidance functionality | |
| EP2699392B1 (en) | An industrial robot having a kinematically redundant arm and a method for controlling the robot | |
| US8160745B2 (en) | Robots with occlusion avoidance functionality | |
| EP4052866A1 (en) | Safe operation of machinery using potential occupancy envelopes | |
| JP5144785B2 (en) | Method and apparatus for predicting interference between target region of robot and surrounding object | |
| EP2561964B1 (en) | Robotic apparatus implementing collision avoidance scheme and associated methods | |
| US7664570B2 (en) | Method and apparatus for limiting the movement of a robot, and a robot equipped with said apparatus | |
| Balan et al. | Real-time 3D collision avoidance method for safe human and robot coexistence | |
| Dietrich et al. | Extensions to reactive self-collision avoidance for torque and position controlled humanoids | |
| EP2660014A1 (en) | Control device and teaching method for seven-shaft multi-joint robot | |
| JP2021160080A (en) | Optimization of fast robot motion with distance field | |
| TW202107232A (en) | Motion planning for multiple robots in shared workspace | |
| JP2012232408A (en) | Method and system for automatically preventing deadlock in multi-robot system | |
| CN112809682B (en) | Mechanical arm obstacle avoidance path planning method and system and storage medium | |
| JP2012056063A (en) | Smooth motion path generating device and smooth motion path generating method | |
| JP6743453B2 (en) | Robot controller, robot and simulation device | |
| US20220324111A1 (en) | Context-sensitve safety monitoring of collaborative work environments | |
| Selvaggio et al. | Towards a self-collision aware teleoperation framework for compound robots | |
| US20230173682A1 (en) | Context-sensitive safety monitoring of collaborative work environments | |
| Spencer et al. | Collision avoidance techniques for tele-operated and autonomous manipulators in overlapping workspaces | |
| Sun et al. | Development of the “Quad-SCARA” platform and its collision avoidance based on Buffered Voronoi Cell | |
| Fragkopoulos et al. | Dynamic efficient collision checking method of robot arm paths in configuration space | |
| Tika et al. | Model predictive control based cooperative robot manipulation and collision avoidance in shared workspaces | |
| Alkkiomaki et al. | Smooth transition from motion to force control in robotic manipulation using vision |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A201 | Request for examination | ||
| E902 | Notification of reason for refusal | ||
| AMND | Amendment | ||
| E601 | Decision to refuse application | ||
| J201 | Request for trial against refusal decision | ||
| AMND | Amendment | ||
| B601 | Maintenance of original decision after re-examination before a trial | ||
| J301 | Trial decision |
Free format text: TRIAL DECISION FOR APPEAL AGAINST DECISION TO DECLINE REFUSAL REQUESTED 20090529 Effective date: 20101230 |
|
| S901 | Examination by remand of revocation | ||
| GRNO | Decision to grant (after opposition) | ||
| GRNT | Written decision to grant | ||
| FPAY | Annual fee payment |
Payment date: 20131206 Year of fee payment: 4 |
|
| FPAY | Annual fee payment |
Payment date: 20141230 Year of fee payment: 5 |
|
| FPAY | Annual fee payment |
Payment date: 20151208 Year of fee payment: 6 |
|
| FPAY | Annual fee payment |
Payment date: 20161207 Year of fee payment: 7 |
|
| FPAY | Annual fee payment |
Payment date: 20171204 Year of fee payment: 8 |
|
| FPAY | Annual fee payment |
Payment date: 20181211 Year of fee payment: 9 |
|
| FPAY | Annual fee payment |
Payment date: 20191210 Year of fee payment: 10 |