KR20240076061A - Rehabilitation robot system method for controlling rehabilitation robot - Google Patents

Rehabilitation robot system method for controlling rehabilitation robot Download PDF

Info

Publication number
KR20240076061A
KR20240076061A KR1020220158148A KR20220158148A KR20240076061A KR 20240076061 A KR20240076061 A KR 20240076061A KR 1020220158148 A KR1020220158148 A KR 1020220158148A KR 20220158148 A KR20220158148 A KR 20220158148A KR 20240076061 A KR20240076061 A KR 20240076061A
Authority
KR
South Korea
Prior art keywords
assisting force
force
calculated
equation
end effector
Prior art date
Application number
KR1020220158148A
Other languages
Korean (ko)
Inventor
강상훈
황성일
송원경
Original Assignee
울산과학기술원
대한민국(국립재활원장)
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 울산과학기술원, 대한민국(국립재활원장) filed Critical 울산과학기술원
Priority to KR1020220158148A priority Critical patent/KR20240076061A/en
Publication of KR20240076061A publication Critical patent/KR20240076061A/en

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H1/00—Apparatus for passive exercising; Vibrating apparatus; Chiropractic devices, e.g. body impacting devices, external devices for briefly extending or aligning unbroken bones
    • A61H1/02—Stretching or bending or torsioning apparatus for exercising
    • A61H1/0274—Stretching or bending or torsioning apparatus for exercising for the upper limbs
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • 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
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H2201/00—Characteristics of apparatus not provided for in the preceding codes
    • A61H2201/12—Driving means
    • A61H2201/1207—Driving means with electric or magnetic drive
    • A61H2201/1215—Rotary drive
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H2201/00—Characteristics of apparatus not provided for in the preceding codes
    • A61H2201/16—Physical interface with patient
    • A61H2201/1602—Physical interface with patient kind of interface, e.g. head rest, knee support or lumbar support
    • A61H2201/1635—Hand or arm, e.g. handle
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H2201/00—Characteristics of apparatus not provided for in the preceding codes
    • A61H2201/16—Physical interface with patient
    • A61H2201/1657—Movement of interface, i.e. force application means
    • A61H2201/1671—Movement of interface, i.e. force application means rotational
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H2201/00—Characteristics of apparatus not provided for in the preceding codes
    • A61H2201/50—Control means thereof
    • A61H2201/5007—Control means thereof computer controlled
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H2201/00—Characteristics of apparatus not provided for in the preceding codes
    • A61H2201/50—Control means thereof
    • A61H2201/5058—Sensors or detectors
    • A61H2201/5069—Angle sensors

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Pain & Pain Management (AREA)
  • Epidemiology (AREA)
  • Human Computer Interaction (AREA)
  • Physical Education & Sports Medicine (AREA)
  • Rehabilitation Therapy (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Animal Behavior & Ethology (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Rehabilitation Tools (AREA)

Abstract

본 발명에 의하면, 운동 기구부; 상기 운동 기구부에 설치되어서 상기 운동 기구부의 운동에 의해 위치가 변하는 엔드 이펙터; 상기 운동 기구부를 운동시키는 구동력을 발생시키는 구동부; 상기 엔드 이펙터의 위치를 측정하는 위치 측정부; 상기 엔드 이펙터에 가해지는 외력을 측정하는 외력 측정부; 및 상기 구동력을 제어하여 상기 엔드 이펙터에 인가되는 인가 보조력을 조절하는 제어부를 포함하며, 상기 제어부는 설정된 제한 보조력 이하의 값에서 상기 인가 보조력을 아래 수학식 12에 의한 제어입력을 이용하여 산출되며,
[수학식 13]

Figure pat00143

상기 수학식 13에서,
Figure pat00144
는 상기 위치 측정부에 의해 측정되는 상기 엔드 이펙터의 위치이며,
Figure pat00145
,
Figure pat00146
,
Figure pat00147
는 임의로 설정되는 값이고,
Figure pat00148
는 아래 수학식 16과 같이 정의되며,
[수학식 16]
Figure pat00149

수학식 16에서,
Figure pat00150
는 상기 엔드 이펙터의 목표 위치이고,
Figure pat00151
는 상기 외력 측정부에 의한 측정 값이며,
Figure pat00152
는 이전 단계에서 계산된 값으로서 최초의
Figure pat00153
는 아래 수학식 17과 같이 산출되는, 재활 로봇 시스템이 제공된다.
[수학식 17]
Figure pat00154
According to the present invention, an exercise mechanism unit; an end effector installed on the exercise mechanism unit and whose position changes due to movement of the exercise mechanism unit; a driving unit that generates a driving force to move the exercise mechanism unit; a position measuring unit that measures the position of the end effector; An external force measuring unit that measures an external force applied to the end effector; and a control unit that controls the driving force to adjust the applied auxiliary force applied to the end effector, wherein the control unit adjusts the applied auxiliary force at a value below the set limit auxiliary force using a control input according to Equation 12 below. It is calculated,
[Equation 13]
Figure pat00143

In Equation 13 above,
Figure pat00144
is the position of the end effector measured by the position measuring unit,
Figure pat00145
,
Figure pat00146
,
Figure pat00147
is a value set arbitrarily,
Figure pat00148
is defined as Equation 16 below,
[Equation 16]
Figure pat00149

In equation 16,
Figure pat00150
is the target position of the end effector,
Figure pat00151
is the measured value by the external force measuring unit,
Figure pat00152
is the value calculated in the previous step and is the first
Figure pat00153
A rehabilitation robot system is provided, which is calculated as shown in Equation 17 below.
[Equation 17]
Figure pat00154

Description

재활 로봇 시스템 및 재활 로봇의 제어방법 {REHABILITATION ROBOT SYSTEM METHOD FOR CONTROLLING REHABILITATION ROBOT}Rehabilitation robot system and control method of rehabilitation robot {REHABILITATION ROBOT SYSTEM METHOD FOR CONTROLLING REHABILITATION ROBOT}

본 발명은 재활 로봇에 관한 것으로서, 더욱 상세하게는 재활 로봇을 제어하는 기술에 관한 것이다.The present invention relates to rehabilitation robots, and more specifically, to technology for controlling rehabilitation robots.

재활 로봇은 신체적 장애를 가진 환자와 상호작용하여 환자가 신체적 장애를 극복할 수 있도록 도와준다. 일반적으로 재활 로봇은 환자의 신체와 접촉하는 엔드 이펙터(end effector)와, 엔드 이펙터가 설치되고 관절을 구비하는 링크 구조물과, 관절에 토크를 가하는 구동모터를 구비한다. 재활 로봇은 환자에게 운동 부하를 인가한 상태에서 환자에게 이동 궤도를 제공하고 환자가 주어진 이동 궤도를 추종하지 못하는 경우 구동모터를 이용해 보조력을 제공한다.Rehabilitation robots interact with patients with physical disabilities and help them overcome their physical disabilities. Generally, a rehabilitation robot includes an end effector that contacts the patient's body, a link structure on which the end effector is installed and has a joint, and a drive motor that applies torque to the joint. The rehabilitation robot provides a movement trajectory to the patient while applying an exercise load to the patient, and provides assistance using a drive motor if the patient cannot follow the given movement trajectory.

본 발명의 기술분야인 재활 로봇의 제어방법에 관한 선행특허문헌인 등록특허 제10-1321791호에는 동작 제어기가 목표 궤적을 기준 입력으로 하여 가상의 힘 계산하고 로봇 모듈에 부여하여 로봇 모듈의 동작이 목표 궤적을 추종하도록 제어하고, 힘/토크 센서에서 외력이 측정되면, 측정된 외력의 값을 로봇 모듈에 추가로 부여하여 로봇 모듈이 외력에 순응하여 동작하도록 하는 구성이 기재되어 있다. 이러한 종래의 재활 로봇 제어 기술은 로봇 모듈의 동역학 모델을 필요로 한다.In Registered Patent No. 10-1321791, which is a prior patent document related to a control method of a rehabilitation robot, which is a technical field of the present invention, the motion controller calculates a virtual force using the target trajectory as a reference input and gives it to the robot module to ensure the operation of the robot module. A configuration is described that controls to follow a target trajectory, and when an external force is measured by a force/torque sensor, the value of the measured external force is additionally given to the robot module so that the robot module operates in compliance with the external force. This conventional rehabilitation robot control technology requires a dynamic model of the robot module.

대한민국 등록특허공보 등록번호 10-1321791 "능동형 신체 재활을 위한 로봇 동작 제어 시스템 및 제어 방법" (2013.10.28)Republic of Korea Patent Publication No. 10-1321791 “Robot motion control system and control method for active physical rehabilitation” (2013.10.28)

본 발명의 목적은 재활 로봇의 동역학 모델이 필요없이 제어가 가능한 재활 로봇 시스템 및 제어방법을 제공하는 것이다.The purpose of the present invention is to provide a rehabilitation robot system and control method that can be controlled without the need for a rehabilitation robot dynamics model.

상기한 본 발명의 해결하고자 하는 과제를 달성하기 위하여, 본 발명의 일 측면에 따르면, 운동 기구부; 상기 운동 기구부에 설치되어서 상기 운동 기구부의 운동에 의해 위치가 변하는 엔드 이펙터; 상기 운동 기구부를 운동시키는 구동력을 발생시키는 구동부; 상기 엔드 이펙터의 위치를 측정하는 위치 측정부; 상기 엔드 이펙터에 가해지는 외력을 측정하는 외력 측정부; 및 상기 구동력을 제어하여 상기 엔드 이펙터에 인가되는 인가 보조력을 조절하는 제어부를 포함하며, 상기 제어부는 설정된 제한 보조력 이하의 값에서 상기 인가 보조력을 아래 수학식 13에 의한 제어입력을 이용하여 산출되며,In order to achieve the problem to be solved by the present invention described above, according to one aspect of the present invention, an exercise mechanism unit; an end effector installed on the exercise mechanism unit and whose position changes due to movement of the exercise mechanism unit; a driving unit that generates a driving force to move the exercise mechanism unit; a position measuring unit that measures the position of the end effector; An external force measuring unit that measures an external force applied to the end effector; and a control unit that controls the driving force to adjust the applied auxiliary force applied to the end effector, wherein the control unit adjusts the applied auxiliary force at a value below the set limit auxiliary force using a control input according to Equation 13 below. It is calculated,

[수학식 13][Equation 13]

Figure pat00001
Figure pat00001

상기 수학식 13에서,

Figure pat00002
는 상기 위치 측정부에 의해 측정되는 상기 엔드 이펙터의 위치이며,
Figure pat00003
,
Figure pat00004
,
Figure pat00005
는 임의로 설정되는 값이고,
Figure pat00006
는 아래 수학식 16과 같이 정의되며,In Equation 13 above,
Figure pat00002
is the position of the end effector measured by the position measuring unit,
Figure pat00003
,
Figure pat00004
,
Figure pat00005
is a value set arbitrarily,
Figure pat00006
is defined as Equation 16 below,

[수학식 16][Equation 16]

Figure pat00007
Figure pat00007

수학식 16에서,

Figure pat00008
는 상기 엔드 이펙터의 목표 위치이고,
Figure pat00009
는 상기 외력 측정부에 의한 측정 값이며,
Figure pat00010
는 이전 단계에서 계산된 값으로서 최초의
Figure pat00011
는 아래 수학식 17과 같이 산출되는, 재활 로봇 시스템이 제공된다.In equation 16,
Figure pat00008
is the target position of the end effector,
Figure pat00009
is the measured value by the external force measuring unit,
Figure pat00010
is the value calculated in the previous step and is the first
Figure pat00011
A rehabilitation robot system is provided, which is calculated as shown in Equation 17 below.

[수학식 17][Equation 17]

Figure pat00012
Figure pat00012

상기한 본 발명의 해결하고자 하는 과제를 달성하기 위하여, 본 발명의 다른 측면에 따르면, 운동 기구부와, 상기 운동 기구부에 설치되어서 상기 운동 기구부의 운동에 의해 위치가 변하는 엔드 이펙터와, 상기 운동 기구부를 운동시키는 구동력을 발생시키는 구동부와, 상기 엔드 이펙터의 위치를 측정하는 위치 측정부와, 상기 엔드 이펙터에 가해지는 외력을 측정하는 외력 측정부와, 상기 구동력을 제어하여 상기 엔드 이펙터에 인가되는 인가 보조력을 조절하는 제어부를 포함하는 재활 로봇을 제어하는 방법으로서, 제어에 필요한 기본 상수 값들이 설정되는 설정 단계; 상기 위치 측정부에 의한 상기 엔드 이펙터의 측정 위치와 상기 외력 측정부에 의한 측정 외력이 상기 제어부에 의해 확인되는 측정 단계; 및 상기 제어부에 의해 보조력이 산출되는 보조력 산출 단계를 포함하며, 상기 보조력은 아래 수학식 13에 의한 제어입력을 이용하여 산출되며,In order to achieve the problem to be solved by the present invention described above, according to another aspect of the present invention, an exercise mechanism unit, an end effector installed on the exercise mechanism unit whose position changes due to movement of the exercise mechanism unit, and the exercise mechanism unit. A driving unit that generates a driving force for movement, a position measuring unit that measures the position of the end effector, an external force measuring unit that measures an external force applied to the end effector, and an application assistance that is applied to the end effector by controlling the driving force. A method of controlling a rehabilitation robot including a control unit for controlling force, comprising: a setting step in which basic constant values required for control are set; A measurement step in which the measured position of the end effector by the position measuring unit and the external force measured by the external force measuring unit are confirmed by the control unit; and an assisting force calculation step in which the assisting force is calculated by the control unit, wherein the assisting force is calculated using a control input according to Equation 13 below,

[수학식 13][Equation 13]

Figure pat00013
Figure pat00013

상기 수학식 13에서,

Figure pat00014
는 상기 측정 위치이며,
Figure pat00015
,
Figure pat00016
,
Figure pat00017
는 상기 설정 단계에서 임의로 설정되는 값이고,
Figure pat00018
는 아래 수학식 16과 같이 정의되며,In Equation 13 above,
Figure pat00014
is the measurement position,
Figure pat00015
,
Figure pat00016
,
Figure pat00017
is a value arbitrarily set in the setting step,
Figure pat00018
is defined as Equation 16 below,

[수학식 16][Equation 16]

Figure pat00019
Figure pat00019

수학식 16에서,

Figure pat00020
는 상기 목표 위치이고,
Figure pat00021
는 상기 측정 외력이며,
Figure pat00022
는 이전 단계에서 계산된 값으로서 최초의
Figure pat00023
는 아래 수학식 17과 같이 산출되는, 재활 로봇의 제어방법이 제공된다.In equation 16,
Figure pat00020
is the target position,
Figure pat00021
is the measured external force,
Figure pat00022
is the value calculated in the previous step and is the first
Figure pat00023
A control method for a rehabilitation robot is provided, which is calculated as shown in Equation 17 below.

[수학식 17][Equation 17]

Figure pat00024
Figure pat00024

본 발명에 의하면 앞서서 기재한 본 발명의 목적을 모두 달성할 수 있다. 구체적으로, 사용자가 임의로 설정할 수 있는 값인

Figure pat00025
,
Figure pat00026
,
Figure pat00027
, 측정 값인 엔드 이펙터의 위치, 속도, 가속도, 초기 외력으로 한 제어입력을 충분히 산출할 수 있으므로, 모델에 대한 정보가 필요 없게 되어서 시스템에 더욱 안정적이고 효율적으로 보조력을 제공할 수 있게 된다.According to the present invention, all of the objectives of the present invention described above can be achieved. Specifically, it is a value that the user can arbitrarily set.
Figure pat00025
,
Figure pat00026
,
Figure pat00027
Since the control input can be sufficiently calculated from the measured values of the end effector's position, speed, acceleration, and initial external force, information about the model is no longer needed, making it possible to provide assistance to the system more stably and efficiently.

도 1은 본 발명의 일 실시예에 따른 재활 로봇 시스템의 구성을 개략적으로 도시한 블록도이다.
도 2는 도 1에서 운동 기구부, 엔드 이펙터 및 구동부의 실시예를 도시한 사시도이다.
도 3은 도 2에서 운동 기구부와 엔드 이펙터를 도시한 평면도이다.
도 4는 도 1에 도시된 재활 로봇 시스템에 대한 제어블록선도이다.
도 5는 본 발명의 일 실시예에 따른 재활 로봇의 제어방법을 개략적으로 설명하는 순서도이다.Figure 1 is a block diagram schematically showing the configuration of a rehabilitation robot system according to an embodiment of the present invention.
FIG. 2 is a perspective view showing an embodiment of the exercise mechanism unit, end effector, and drive unit in FIG. 1.
FIG. 3 is a plan view showing the exercise mechanism and the end effector in FIG. 2.
FIG. 4 is a control block diagram for the rehabilitation robot system shown in FIG. 1.
Figure 5 is a flow chart schematically explaining a control method of a rehabilitation robot according to an embodiment of the present invention.

이하, 도면을 참조하여 본 발명의 실시예의 구성 및 작용을 상세히 설명한다.Hereinafter, the configuration and operation of an embodiment of the present invention will be described in detail with reference to the drawings.

도 1에는 본 발명의 일 실시예에 따른 재활 로봇 시스템의 개략적인 구성이 블록도로서 도시되어 있다. 도 1을 참조하면, 본 발명의 일 실시예에 따른 재활 로봇 시스템(100)은, 운동 기구부(110)와, 운동 기구부(110)에 설치되는 엔드 이펙터(end effector)(120)와, 운동 기구부(110)를 운동시키는 구동력을 발생시키는 구동부(130)와, 엔드 이펙터(120)의 위치를 측정하는 위치 측정부(140)와, 엔드 이펙터(120)에 가해지는 외력을 측정하는 외력 측정부(150)와, 엔드 이펙터(120)의 손잡이에 작용하는 압력을 측정하는 압력 센서(미도시)와, 엔드 이펙터(120)의 손잡이를 회전시키는 손잡이 회전 모터(미도시)와, 구동부(130)의 작동을 제어하는 제어부(160)를 포함한다.Figure 1 shows a schematic configuration of a rehabilitation robot system according to an embodiment of the present invention as a block diagram. Referring to FIG. 1, the rehabilitation robot system 100 according to an embodiment of the present invention includes an exercise mechanism unit 110, an end effector 120 installed in the exercise mechanism unit 110, and an exercise mechanism unit. A driving unit 130 that generates a driving force to move 110, a position measuring unit 140 that measures the position of the end effector 120, and an external force measuring unit that measures the external force applied to the end effector 120 ( 150), a pressure sensor (not shown) that measures the pressure acting on the handle of the end effector 120, a handle rotation motor (not shown) that rotates the handle of the end effector 120, and the drive unit 130. It includes a control unit 160 that controls operation.

운동 기구부(110)는 구동부(130)에 의해 운동하여 엔드 이펙터(120)의 위치를 이동시킨다. 도 2 및 도 3에는 운동 기구부(110)의 일 실시예가 도시되어 있다. 도 2 및 도 3을 참조하면, 운동 기구부(110)는 링크 구조물로서, 제1 링크(111)와, 제2 링크(112)와, 제2 링크(112)에 회전 가능하게 연결되는 제3 링크(113)와, 제1 링크(112) 및 제3 링크(113)에 회전 가능하게 연결되는 제4 링크(114)를 구비한다. 운동 기구부(110)는 제1 회전축선(A1)을 중심으로 회전한다.The exercise mechanism unit 110 is moved by the drive unit 130 to move the position of the end effector 120. 2 and 3 show an embodiment of the exercise mechanism unit 110. Referring to Figures 2 and 3, the exercise mechanism unit 110 is a link structure, including a first link 111, a second link 112, and a third link rotatably connected to the second link 112. (113) and a fourth link 114 rotatably connected to the first link 112 and the third link 113. The exercise mechanism unit 110 rotates around the first rotation axis A1.

제1 링크(111)는 제1 회전축선(A1)으로부터 반경방향을 따라서 직선으로 연장된다. 제1 링크(111)는 구동부(130)에 의해 제1 회전축선(A1)을 중심으로 회전한다. 제1 링크(111)의 반경방향 끝단에는 제4 링크(114)가 제1 회전축선(A1)과 평행한 제2 회전축선(A2)을 중심으로 회전 가능하게 연결된다. 본 실시예에서 제1 링크(111)의 길이(L1)는 제1 회전축선(A1)과 제2 회전축선(A2) 사이의 거리인 것으로 정의한다. 또한, 본 실시예에서 제1 링크(111)의 질량을 m1이라 하고, 제1 링크(111)의 무게 중심(C1)과 제1 회전축선(A1) 사이의 거리를 Lc1이라 하며, 제1 링크(111)의 무게중심(C1)에 대한 제1 링크(111)의 회전 관성을 Iz1이라 한다. 본 실시예에서 제1 링크(111)의 각변위를 θ1이라 한다.The first link 111 extends in a straight line along the radial direction from the first rotation axis A1. The first link 111 rotates about the first rotation axis A1 by the driving unit 130. A fourth link 114 is connected to the radial end of the first link 111 so as to be rotatable about a second rotation axis A2 parallel to the first rotation axis A1. In this embodiment, the length L 1 of the first link 111 is defined as the distance between the first rotation axis A1 and the second rotation axis A2. In addition, in this embodiment, the mass of the first link 111 is m 1 , the distance between the center of gravity C1 of the first link 111 and the first rotation axis A1 is L c1 , and the The rotational inertia of the first link 111 with respect to the center of gravity (C1) of the 1 link 111 is referred to as I z1 . In this embodiment, the angular displacement of the first link 111 is referred to as θ 1 .

제2 링크(112)는 제1 회전축선(A1)으로부터 반경방향을 따라서 직선 연장된다. 제2 링크(112)는 구동부(130)에 의해 제1 회전축선(A1)을 중심으로 회전한다. 제1 링크(111)와 제2 링크(112)는 각각 독립적으로 회전한다. 제2 링크(112)의 반경방향 끝단에는 제3 링크(113)가 제1 회전축선(A1)과 평행한 제3 회전축선(A3)을 중심으로 회전 가능하게 연결된다. 본 실시예에서 제2 링크(112)의 길이(L2)는 제1 회전축선(A1)과 제3 회전축선(A3) 사이의 거리인 것으로 정의한다. 또한, 본 실시예에서 제2 링크(112)의 질량을 m2이라 하고, 제2 링크(112)의 무게 중심(C2)과 제1 회전축선(A1) 사이의 거리를 Lc2이라 하며, 제2 링크(112)의 무게중심(C2)에 대한 제2 링크(112)의 회전 관성을 Iz2이라 한다. 본 실시예에서 제2 링크(112)의 각변위를 θ2이라 한다.The second link 112 extends straight along the radial direction from the first rotation axis A1. The second link 112 rotates about the first rotation axis A1 by the driving unit 130. The first link 111 and the second link 112 rotate independently. A third link 113 is connected to the radial end of the second link 112 so as to be rotatable about a third rotation axis A3 parallel to the first rotation axis A1. In this embodiment, the length L 2 of the second link 112 is defined as the distance between the first rotation axis A1 and the third rotation axis A3. In addition, in this embodiment, the mass of the second link 112 is m 2 , the distance between the center of gravity C2 of the second link 112 and the first rotation axis A1 is L c2 , and the The rotational inertia of the second link 112 with respect to the center of gravity (C2) of the second link 112 is referred to as I z2 . In this embodiment, the angular displacement of the second link 112 is referred to as θ 2 .

제3 링크(113)는 제2 링크(112)의 끝단에 제3 회전축선(A3)을 중심으로 회전 가능하게 연결된다. 제3 링크(113)는 제3 회전축선(A3)으로부터 반경방향을 따라서 직선으로 제1 링크(111)와 평행하게 연장된다. 제3 링크(113)의 끝단에는 제4 링크(114)가 제1 회전축선(A1)과 평행한 제4 회전축선(A4)을 중심으로 회전 가능하게 연결된다. 본 실시예에서 제3 링크(113)의 길이(L3)는 제3 회전축선(A3)과 제4 회전축선(A4) 사이의 거리인 것으로 정의한다. 본 실시예에서 제3 링크(113)의 질량을 m3이라 하고, 제3 링크(114)의 무게 중심(C3)과 제1 회전축선(A1) 사이의 거리를 Lc3이라 하며, 제3 링크(113)의 무게중심(C4)에 대한 제3 링크(113)의 회전 관성을 Iz3이라 한다. The third link 113 is rotatably connected to the end of the second link 112 about the third rotation axis A3. The third link 113 extends in a straight line parallel to the first link 111 along the radial direction from the third rotation axis A3. A fourth link 114 is connected to the end of the third link 113 so as to be rotatable about a fourth rotation axis A4 parallel to the first rotation axis A1. In this embodiment, the length L 3 of the third link 113 is defined as the distance between the third rotation axis A3 and the fourth rotation axis A4. In this embodiment, the mass of the third link 113 is m 3 , the distance between the center of gravity C3 of the third link 114 and the first rotation axis A1 is L c3 , and the third link 113 is referred to as m 3 . The rotational inertia of the third link 113 with respect to the center of gravity (C4) of (113) is called I z3 .

제4 링크(114)는 제1 링크(111)의 끝단에 제2 회전축선(A2)을 중심으로 회전 가능하게 연결된다. 제4 링크(114)는 제2 회전축선(A2)으로부터 반경방향을 따라서 직선으로 제2 링크(112)와 평행하게 연장된다. 제4 링크(114)의 반경방향 끝단에는 엔드 이펙터(120)가 위치한다. 본 실시예에서 제4 링크(114)의 길이(L4)는 제2 회전축선(A2)과 엔드 이펙터(120) 사이의 거리인 것으로 정의한다. 본 실시예에서 제4 링크(114)의 질량을 m4이라 하고, 제4 링크(114)의 무게 중심(C4)과 제1 회전축선(A1) 사이의 거리를 Lc4이라 하며, 제4 링크(114)의 무게중심(C4)에 대한 제4 링크(114)의 회전 관성을 Iz4이라 한다. 제4 링크(114) 상에는 제3 링크(113)가 제4 회전축선(A4)을 중심으로 회전 가능하게 연결된다.The fourth link 114 is rotatably connected to the end of the first link 111 about the second rotation axis A2. The fourth link 114 extends in a straight line parallel to the second link 112 along the radial direction from the second rotation axis A2. An end effector 120 is located at the radial end of the fourth link 114. In this embodiment, the length L 4 of the fourth link 114 is defined as the distance between the second rotation axis A2 and the end effector 120. In this embodiment, the mass of the fourth link 114 is m 4 , the distance between the center of gravity (C4) of the fourth link 114 and the first rotation axis (A1) is L c4 , and the fourth link The rotational inertia of the fourth link 114 with respect to the center of gravity (C4) of (114) is called I z4 . A third link 113 is connected to the fourth link 114 so as to be rotatable about the fourth rotation axis A4.

운동 기구부(110)에서 제1 링크(111)의 길이(L1)는 제3 링크(113)의 길이(L3)와 동일하며, 제2 링크(112)의 길이(L2)는 제4 링크(114)에서 제1 링크(111)가 연결되는 지점과 제3 링크(113)가 연결되는 지점 사이의 길이(L5)와 동일하다.In the exercise mechanism unit 110, the length (L 1 ) of the first link 111 is the same as the length (L 3 ) of the third link 113, and the length (L 2 ) of the second link 112 is the fourth It is equal to the length (L 5 ) between the point where the first link 111 is connected and the point where the third link 113 is connected in the link 114.

도 1 내지 도 3을 참조하면, 엔드 이펙터(120)는 운동 기구부(110)에서 제4 링크(114)의 끝단에 설치되어서 환자의 재활 대상 신체 부위와 접촉하며 상호작용한다. 엔드 이펙터(120)는 운동 기구부(110)에 의해 제1 회전축선(A1)과 직각을 이루는 평면 상에서 2차원 이동이 가능하다. 엔드 이펙터(120)의 위치는 위치 측정부(140)에 의해 실시간으로 측정된다. 본 실시예에서 엔드 이펙터(120)의 위치는 제1 회전축선(A1)의 위치를 원점(O)으로 하고 제1 회전축선(A1)과 직각을 이루는 평면 상에서 직교하는 길이방향 축(L)과 폭방향 축(W)을 갖는 기준 직교 좌표계 상의 좌표인 것으로 설명한다. 기준 직교 좌표계 상에서 엔드 이펙터(120)의 위치는 (XL, XW)로 표현되는데, XL는 엔드 이펙터(120)의 길이방향 축(L) 좌표값이고, XW는 엔드 이펙터(120)의 폭방향 축(W) 좌표값이다. 엔드 이펙터(120)에 가해지는 외력(Fe)은 외력 측정부(150)에 의해 측정되어서 제어부(160)로 전송된다.Referring to FIGS. 1 to 3 , the end effector 120 is installed at the end of the fourth link 114 in the exercise mechanism unit 110 and interacts with the patient's body part to be rehabilitated. The end effector 120 is capable of two-dimensional movement on a plane perpendicular to the first rotation axis A1 by the movement mechanism unit 110. The position of the end effector 120 is measured in real time by the position measurement unit 140. In this embodiment, the position of the end effector 120 is the longitudinal axis (L) orthogonal to the plane perpendicular to the first rotation axis (A1) with the position of the first rotation axis (A1) as the origin (O). It is explained as being a coordinate on a reference orthogonal coordinate system having a width direction axis (W). The position of the end effector 120 on the reference Cartesian coordinate system is expressed as (X L , X W ), where X L is the longitudinal axis (L) coordinate value of the end effector 120, and This is the width axis (W) coordinate value. The external force (F e ) applied to the end effector 120 is measured by the external force measuring unit 150 and transmitted to the control unit 160.

구동부(130)는 운동 기구부(110)를 운동시키는 구동력을 발생시킨다. 구동부(130)는 제1 링크(111)를 회전시키는 제1 토크(τ1)를 발생시키는 제1 구동 모터(132)와, 제2 링크(112)를 회전시키는 제2 토크(τ2)를 발생시키는 제2 구동 모터(135)를 구비한다. 제1 구동 모터(132)에 의한 제1 토크(τ1)와 제2 구동 모터(135)에 의한 제2 토크(τ2)가 합쳐져서 보조력(FA)를 발생시킨다.The driving unit 130 generates a driving force that moves the exercise mechanism unit 110. The drive unit 130 includes a first drive motor 132 that generates a first torque (τ 1 ) to rotate the first link 111 and a second torque (τ 2 ) to rotate the second link 112. It is provided with a second drive motor 135 that generates power. The first torque (τ 1 ) by the first drive motor 132 and the second torque (τ 2 ) by the second drive motor 135 are combined to generate auxiliary force ( FA ).

제1 구동 모터(132)는 제1 토크를 발생시켜서 제1 링크(111)를 제1 회전축선(A1)을 중심으로 회전시킨다. 제1 구동 모터(132)에 의한 제1 토크는 제1 구동 모터(132)의 작동을 제어하는 제어부(160)에 의해 조절된다.The first drive motor 132 generates a first torque to rotate the first link 111 about the first rotation axis A1. The first torque generated by the first drive motor 132 is adjusted by the control unit 160 that controls the operation of the first drive motor 132.

제2 구동 모터(135)는 제2 토크를 발생시켜서 제2 링크(112)를 제1 회전축선(A1)을 중심으로 회전시킨다. 제2 구동 모터(135)에 의한 제2 토크는 제2 구동 모터(135)의 작동을 제어하는 제어부(160)에 의해 조절된다.The second drive motor 135 generates a second torque to rotate the second link 112 about the first rotation axis A1. The second torque by the second drive motor 135 is adjusted by the control unit 160 that controls the operation of the second drive motor 135.

위치 측정부(140)는 엔드 이펙터(120)의 위치를 측정하여 위치 측정값을 생산한다. 본 실시예에서 엔드 이펙터(120)의 위치 측정값은 기준 직교 좌표계 상의 좌표인 (XL, XW)으로 표현되는 것으로 설명한다. 본 실시예에서 위치 측정부(140)는 제1 링크(111)의 각변위인 제1 각변위(θ1)와 제2 링크(112)의 각변위인 제2 각변위(θ2)를 측정함으로써, 위치 측정값을 생산하는 것으로 설명한다. 본 실시예에서 제1 각변위(θ1)와 제2 각변위(θ2)는 기준 직교 좌표계의 폭방향 축(W)에 대한 각변위인 것으로 설명한다. 위치 측정부(140)에 의해 생산된 위치 측정값은 구동부(130)의 제어를 위해 제어부(160)로 전송된다.The position measurement unit 140 measures the position of the end effector 120 and produces a position measurement value. In this embodiment, the position measurement value of the end effector 120 is described as being expressed as (X L , X W ), which are coordinates on a reference orthogonal coordinate system. In this embodiment, the position measuring unit 140 measures the first angular displacement (θ 1 ), which is the angular displacement of the first link 111, and the second angular displacement (θ 2 ), which is the angular displacement of the second link 112. By doing so, it is explained that a position measurement value is produced. In this embodiment, the first angular displacement (θ 1 ) and the second angular displacement (θ 2 ) are explained as angular displacements with respect to the width axis (W) of the reference orthogonal coordinate system. The position measurement value produced by the position measuring unit 140 is transmitted to the control unit 160 to control the driving unit 130.

외력 측정부(150)는 엔드 이펙터(120)에 가해지는 외력(Fe)을 측정하여 외력 측정값을 생산한다. 외력 측정부(150)에 의해 생산된 외력 측정값은 구동부(130)의 제어를 위해 제어부(160)로 전송된다.The external force measuring unit 150 measures the external force (F e ) applied to the end effector 120 and produces an external force measurement value. The external force measurement value produced by the external force measuring unit 150 is transmitted to the control unit 160 to control the driving unit 130.

압력 센서(미도시)는 엔드 이펙터(120)의 손잡이에 작용하는 압력을 측정하여 손잡이 압력 측정값을 생산한다. 압력 센서(미도시)에 의해 생산된 손잡이 압력 측정값은 제어부(160)로 전송되어서 환자에 의한 엔드 이펙터(120)의 파지 여부를 판단에 사용된다.A pressure sensor (not shown) measures the pressure acting on the handle of the end effector 120 and produces a handle pressure measurement value. The handle pressure measurement value produced by a pressure sensor (not shown) is transmitted to the control unit 160 and used to determine whether the patient is holding the end effector 120.

손잡이 회전 모터(미도시)는 엔드 이펙터(120)의 손잡이를 회전시킨다. 손잡이 회전 모터(미도시)의 작동은 제어부(160)에 의해 제어된다. 손잡이 회전 모터(미도시)는 엔드 이펙터(120)의 손잡이가 실시간으로 원하는 회전각도를 유지하도록 한다.A handle rotation motor (not shown) rotates the handle of the end effector 120. The operation of the handle rotation motor (not shown) is controlled by the control unit 160. The handle rotation motor (not shown) allows the handle of the end effector 120 to maintain the desired rotation angle in real time.

제어부(160)는 환자가 추종해야할 엔드 이펙터(120)의 목표 위치값을 생산하고, 위치 측정부(140)에 의해 생산된 위치 측정값과 외력 측정부(150)에 의해 생산된 외력 측정값을 이용하여 구동부(130)가 발생시켜야 하는 보조력(FA)을 산출한다. 제어부(160)에 의한 보조력(FA)의 산출을 구체적으로 설명하면 다음과 같다.The control unit 160 produces a target position value of the end effector 120 to be followed by the patient, and uses the position measurement value produced by the position measurement unit 140 and the external force measurement value produced by the external force measurement unit 150. The auxiliary force ( FA ) that the driving unit 130 must generate is calculated using this method. The calculation of the assisting force ( FA ) by the control unit 160 will be described in detail as follows.

아래 수학식 1은 재활 로봇 시스템(100)의 동역학 식이다.Equation 1 below is the dynamics equation of the rehabilitation robot system 100.

Figure pat00028
Figure pat00028

수학식 1에서In equation 1

구동 토크 행렬인

Figure pat00029
The driving torque matrix is
Figure pat00029

각변위 행렬인

Figure pat00030
Angular displacement matrix
Figure pat00030

관성행렬인inertial matrix

Figure pat00031
Figure pat00031

원심력 및 코리올리 힘 관련 항의 행렬인Protest procession related to centrifugal force and Coriolis force

Figure pat00032
Figure pat00032

Figure pat00033
: 마찰력
Figure pat00033
: friction force

Figure pat00034
: 외력
Figure pat00034
: external force

위의 수학식 1은 원심력 및 코리올리 힘 관련 항과 마찰력 항을 비선형 힘(Nθ)으로 정의하여 아래 수학식 2와 같이 표현될 수 있다.Equation 1 above can be expressed as Equation 2 below by defining the terms related to centrifugal force and Coriolis force and the frictional force term as a nonlinear force (N θ ).

Figure pat00035
Figure pat00035

수학식 2에서In equation 2

Figure pat00036
Figure pat00036

수학식 2를 Jacobian matrix(J)를 이용하여 cartesian coordinate에서 표현하면 아래 수학식 3과 같다.If Equation 2 is expressed in cartesian coordinates using the Jacobian matrix (J), it becomes Equation 3 below.

Figure pat00037
Figure pat00037

수학식 3에서In equation 3:

Figure pat00038
,
Figure pat00039
Figure pat00038
,
Figure pat00039

Figure pat00040
Figure pat00040

엔드 이펙터의 위치

Figure pat00041
Location of end effector
Figure pat00041

수학식 3은 아래 수학식 4와 같이 수정될 수 있다.Equation 3 can be modified as Equation 4 below.

Figure pat00042
Figure pat00042

수학식 4에서

Figure pat00043
은 수학식 3의
Figure pat00044
에서 비선형 항이 제외된 것이다.In equation 4:
Figure pat00043
In Equation 3,
Figure pat00044
The nonlinear term is excluded.

수학식 4로부터 재활 로봇 시스템(100)을 제어하기 위한 입력식을 아래 수학식 5와 같이 얻을 수 있다.From Equation 4, an input equation for controlling the rehabilitation robot system 100 can be obtained as shown in Equation 5 below.

Figure pat00045
Figure pat00045

시간지연추정(Time delay estimation)을 이용하여

Figure pat00046
를 한 샘플 전의 값(
Figure pat00047
)으로 정의하면, 수학식 5은 아래 수학식 6과 같이 표현된다.Using time delay estimation
Figure pat00046
The value before one sample (
Figure pat00047
), Equation 5 is expressed as Equation 6 below.

Figure pat00048
Figure pat00048

수학식 6을 재활 로봇의 동역학식에 적용하면 아래 수학식 7 및 수학식 8을 얻을 수 있다.By applying Equation 6 to the dynamics equation of the rehabilitation robot, Equation 7 and Equation 8 below can be obtained.

Figure pat00049
Figure pat00049

Figure pat00050
Figure pat00050

수학식 8에서

Figure pat00051
가 0이라 가정하면, 전체 동역학은
Figure pat00052
가 되고, 그에 따라 제어입력
Figure pat00053
는 아래 수학식 9와 같이 정의된다.In equation 8:
Figure pat00051
Assuming that is 0, the overall dynamics are
Figure pat00052
becomes, and the control input is accordingly
Figure pat00053
is defined as Equation 9 below.

Figure pat00054
Figure pat00054

수학식 9과 같이 입력을 제공하면, 전체적인 시스템의 동역학은 아래 수학식 10과 같다.If input is provided as shown in Equation 9, the dynamics of the overall system are as shown in Equation 10 below.

Figure pat00055
Figure pat00055

결론적으로 전체 동역학은 설계자가 임의로 설정할 수 있는 숫자 값인

Figure pat00056
와
Figure pat00057
을 이용하여 표현될 수 있고, 이를 통상적으로 사용되는 질량(
Figure pat00058
), 댐핑(
Figure pat00059
), 강성(
Figure pat00060
)을 활용하면 아래 수학식 11과 같이 표현될 수 있다.In conclusion, overall dynamics is a numerical value that can be arbitrarily set by the designer.
Figure pat00056
and
Figure pat00057
It can be expressed using the commonly used mass (
Figure pat00058
), damping (
Figure pat00059
), hardness(
Figure pat00060
) can be expressed as Equation 11 below.

Figure pat00061
Figure pat00061

수학식 11에서,

Figure pat00062
,
Figure pat00063
,
Figure pat00064
는 설계자가 임의로 변경할 수 있는 값이므로 로봇과 환자 사이에 상호작용하는 정도(보조력의 제공 정도)를 사용자가 정할 수 있게 된다.In equation 11,
Figure pat00062
,
Figure pat00063
,
Figure pat00064
is a value that can be arbitrarily changed by the designer, so the user can determine the degree of interaction (degree of provision of assistive force) between the robot and the patient.

추가적으로 정밀한 제어를 위하여 도 4에 도시된 제어블록선도와 같이 이상적인 출력 위치(

Figure pat00065
)와 실제 위치(
Figure pat00066
)의 차이를 피드백 해주는 항을 추가하기 위해
Figure pat00067
를 아래 수학식 12과 같이 정의한다.Additionally, for precise control, the ideal output position (as shown in the control block diagram shown in Figure 4)
Figure pat00065
) and actual location (
Figure pat00066
) to add a term that feeds back the difference between
Figure pat00067
is defined as in Equation 12 below.

Figure pat00068
Figure pat00068

도 4의 제어블록선도에서

Figure pat00069
는
Figure pat00070
로 정의된다.In the control block diagram of Figure 4
Figure pat00069
Is
Figure pat00070
It is defined as

도 4에 도시된 제어블록선도에 따라 제어입력

Figure pat00071
가 아래 수학식 13과 같이 새롭게 정의된다.Control input according to the control block diagram shown in Figure 4
Figure pat00071
is newly defined as shown in Equation 13 below.

Figure pat00072
Figure pat00072

수학식 13에서In equation 13,

Figure pat00073
이고,
Figure pat00074
Figure pat00073
ego,
Figure pat00074

수학식 13에서

Figure pat00075
(속도)와
Figure pat00076
(위치)는 측정할 수 있는 값이고,
Figure pat00077
,
Figure pat00078
,
Figure pat00079
는 설계자가 정할 수 있는 값이다. 따라서,
Figure pat00080
를 알 수 있다면
Figure pat00081
를 제어입력으로 사용할 수 있다.In equation 13,
Figure pat00075
(speed) and
Figure pat00076
(position) is a measurable value,
Figure pat00077
,
Figure pat00078
,
Figure pat00079
is a value that can be determined by the designer. thus,
Figure pat00080
If you can know
Figure pat00081
Can be used as a control input.

Figure pat00082
를 풀어서
Figure pat00083
에 적용시키면, 아래 수학식 14와 같다.
Figure pat00082
By solving
Figure pat00083
When applied to Equation 14 below:

Figure pat00084
Figure pat00084

도 4의 제어블록선도에 의하면,

Figure pat00085
는 아래 수학식 15와 같이 표현될 수 있는데, 이는 수학식 14의 마지막 항의 시간 지연 전의 값이다.According to the control block diagram of Figure 4,
Figure pat00085
Can be expressed as Equation 15 below, which is the value before the time delay in the last term of Equation 14.

Figure pat00086
Figure pat00086

수학식 15를 이용하면,

Figure pat00087
는 아래 수학식 16과 같이 정의될 수 있다.Using equation 15,
Figure pat00087
Can be defined as Equation 16 below.

Figure pat00088
Figure pat00088

수학식 16에서

Figure pat00089
를 산출하기 위해 필요한 정보는 모두 알 수 있는 값이다. 즉, 가속도(
Figure pat00090
), 속도(
Figure pat00091
), 위치(
Figure pat00092
)는 측정 값이며,
Figure pat00093
,
Figure pat00094
,
Figure pat00095
는 설계자가 설정한 값이고,
Figure pat00096
는 이전 단계에서 계산된 값이다. 최초의
Figure pat00097
는 아래 수학식 17과 같이 산출될 수 있다.In equation 16:
Figure pat00089
All of the information needed to calculate is a known value. That is, acceleration (
Figure pat00090
), speed(
Figure pat00091
), location(
Figure pat00092
) is the measurement value,
Figure pat00093
,
Figure pat00094
,
Figure pat00095
is the value set by the designer,
Figure pat00096
is the value calculated in the previous step. First
Figure pat00097
Can be calculated as in Equation 17 below.

Figure pat00098
Figure pat00098

추가로, 상기 수학식 16에서

Figure pat00099
는 시간 변수
Figure pat00100
는 아래 수학식 18과 같이 산출될 수 있다.Additionally, in Equation 16 above,
Figure pat00099
is a time variable
Figure pat00100
Can be calculated as in Equation 18 below.

Figure pat00101
Figure pat00101

수학식 18에서,

Figure pat00102
는 상기 파지가 확인되지 않는 시간에 따라 증가하고, 파지가 확인되는 시간에 따라 감소한다.In equation 18,
Figure pat00102
increases with the time the phage is not identified and decreases with the time the phage is identified.

또한, 제어부(150)는 엔드 이펙터(120)의 손잡이의 각도(

Figure pat00103
)가 원하는 각도(
Figure pat00104
)로 회전하도록 엔드 이펙터(120)에 설치되는 손잡이 회전 모터(미도시)를 구동시킨다. 이를 위하여, 손잡이의 각도(
Figure pat00105
)를 실시간으로 측정하는 각도 센서가 구비된다. 제어부(150)가 산출하는 손잡이 회전 모터의 제어입력은 아래 수학식 19와 같다.In addition, the control unit 150 controls the angle of the handle of the end effector 120 (
Figure pat00103
) is the desired angle (
Figure pat00104
) is driven to rotate the handle rotation motor (not shown) installed on the end effector 120. For this purpose, the angle of the handle (
Figure pat00105
) is equipped with an angle sensor that measures in real time. The control input of the handle rotation motor calculated by the control unit 150 is as shown in Equation 19 below.

Figure pat00106
Figure pat00106

도 5는 본 발명의 일 실시예에 따른 재활 로봇의 제어방법을 개략적으로 설명하는 순서도이다. 도 5를 참조하면, 본 발명의 일 실시예에 따른 재활 로봇의 제어방법은, 설정 단계(S110)와, 운동시작 확인 단계(S120)와, 측정 단계(S130)와, 위치 비교 단계(S140)와, 제1 보조력 산출 단계(S150)와, 제2 보조력 산출 단계(S160)와, 산출 보조력 기본 비교 단계(S170)와, 제1 인가 보조력 산출 단계(S180)와, 제1 보조력 인가 단계(S185)와, 제3 보조력 산출 단계(S187)와, 산출 보조력 추가 비교 단계(S189)와, 제2 인가 보조력 산출 단계(S190)와, 제2 보조력 인가 단계(S195)와, 제3 인가 보조력 산출 단계(S197)와, 제3 보조력 인가 단계(S199)를 포함한다. 도 5에 도시된 재활 로봇의 제어방법은 예를 들어서 도 1 내지 도 4를 통해 위에서 설명된 재활 로봇 시스템(100)에 적용되는 것으로서, 이하 각 단계들(S110, S120, S130, S140, S150, S160, S170, S180, S185, S187, S189, S190, S195, S197, S199)을 도 1 내지 도 4를 함께 참조하여 상세하게 설명한다.Figure 5 is a flowchart schematically explaining a control method of a rehabilitation robot according to an embodiment of the present invention. Referring to Figure 5, the control method of a rehabilitation robot according to an embodiment of the present invention includes a setting step (S110), an exercise start confirmation step (S120), a measurement step (S130), and a position comparison step (S140). , a first assisting force calculating step (S150), a second assisting force calculating step (S160), a calculated assisting force basic comparison step (S170), a first applied assisting force calculating step (S180), and a first assisting force. A force application step (S185), a third assisting force calculation step (S187), a calculated assisting force addition comparison step (S189), a second applied assisting force calculation step (S190), and a second assisting force application step (S195). ), a third applied assisting force calculation step (S197), and a third assisting force applying step (S199). The control method of the rehabilitation robot shown in FIG. 5 is applied to the rehabilitation robot system 100 described above with reference to FIGS. 1 to 4, for example, and includes the following steps (S110, S120, S130, S140, S150, S160, S170, S180, S185, S187, S189, S190, S195, S197, and S199) will be described in detail with reference to FIGS. 1 to 4.

설정 단계(S110)에서는 재활 로봇 시스템(100)의 제어에 필요한 기본 상수 값들이 재활 로봇 시스템(100)의 운영자에 의해 설정된다. 설정 단계(S110)에서 설정되는 기본 상수 값들은 초기 보조력(FAI), 제한 보조력(FALimit), 제어입력을 산출하는 수학식 12에서의 Md, Bd, Kd를 포함한다.In the setting step (S110), basic constant values required for controlling the rehabilitation robot system 100 are set by the operator of the rehabilitation robot system 100. The basic constant values set in the setting step (S110) include M d , B d , and K d in Equation 12 for calculating the initial assisting force (F AI ), limiting assisting force (F ALimit ), and control input.

초기 보조력(FAI)은 재활 로봇 시스템(100)이 작동을 시작할 때 인가되는 보조력이다. 초기 보조력(FAI)은 길이방향(도 3의 L) 성분인 길이방향 초기 보조력(FAIL)과, 폭방향(도 3의 W)인 폭방향 초기 보조력(FAIW)을 포함한다.The initial assisting force (F AI ) is an assisting force applied when the rehabilitation robot system 100 starts operating. The initial assisting force (F AI ) includes a longitudinal initial assisting force (F AIL ), which is a component in the longitudinal direction (L in Fig. 3), and an initial assisting force in the width direction (F AIW ), which is a component in the width direction (W in Fig. 3). .

제한 보조력(FALimit)은 환자의 안전을 위하여 환자의 운동 능력을 고려하여 재활 로봇 시스템(100)의 운영자에 의해 설정되는 값이다. 재활 로봇 시스템(100)은 보조력(FA)이 설정된 제한 보조력(FALimit)을 넘지 않도록 제어된다. 제한 보조력은 복수의 제한 보조력으로 설정될 수 있다. 본 실시예에서는 제1 제한 보조력과, 제1 제한 보조력보다 큰 제2 제한 보조력으로 설정되는 것으로 설명한다.The limited assistive force (F ALimit ) is a value set by the operator of the rehabilitation robot system 100 in consideration of the patient's exercise ability for the patient's safety. The rehabilitation robot system 100 is controlled so that the assistive force (F A ) does not exceed the set limited assistive force (F ALimit ). The limited assistive force may be set to a plurality of limited assistive forces. In this embodiment, it is explained that the first limited assisting force and the second limited assisting force that are greater than the first limited assisting force are set.

Md, Bd, Kd는 제어입력을 산출하는 수학식 13에 사용되는 값으로서, 재활 로봇 시스템(100)의 운영자에 의해 설정된다. Md, Bd, Kd에 따라서 재활 로봇 시스템(100)이 제공하는 보조력(FA)의 수준이 조절될 수 있다.M d , B d , and K d are values used in Equation 13 to calculate the control input, and are set by the operator of the rehabilitation robot system 100. The level of assistive force (F A ) provided by the rehabilitation robot system 100 can be adjusted according to M d , B d , and K d .

운동시작 확인 단계(S120)에서는 재활 로봇 시스템(100)을 이용한 환자의 재활 운동의 시작이 확인된다. 운동시작 확인 단계(S120)는 제어부(도 1의 160)가 재활 로봇 시스템(100)의 작동 시작을 확인함으로써 수행될 수 있다.In the exercise start confirmation step (S120), the start of the patient's rehabilitation exercise using the rehabilitation robot system 100 is confirmed. The exercise start confirmation step (S120) may be performed by the control unit (160 in FIG. 1) confirming the start of operation of the rehabilitation robot system 100.

측정 단계(S130)에서는 엔드 이펙터(120)의 위치(도 3에서 X)와 엔드 이펙터(120)에 가해지는 외력(도 3에서 Fe)이 실시간으로 측정된다. 측정 단계(S130)는 위치 측정부(도 1의 140)가 엔드 이펙터(120)의 위치(X)를 실시간으로 측정하고, 외력 측정부(도 1의 150)가 엔드 이펙터(120)에 가해지는 외력(Fe)을 실시간으로 측정함으로써 수행된다. 또한, 측정 단계(S130)에서 엔드 이펙터(120)에 부착되는 압력센서(미도시)에 의해 환자에 의한 엔드 이펙터(120) 파지 여부가 함께 실시간으로 확인될 수 있다. 측정 단계(S130)를 통해 측정된 엔드 이펙터(120)의 위치(X) 데이터와 엔드 이펙터(120)에 가해지는 외력(Fe) 데이터와 환자에 의한 엔드 이펙터(120)의 파지 여부는 제어부(도 1의 160)로 전송되어서 제어부(160)에 의해 확인된다.In the measurement step (S130), the position of the end effector 120 (X in FIG. 3) and the external force applied to the end effector 120 (F e in FIG. 3) are measured in real time. In the measurement step (S130), the position measuring unit (140 in FIG. 1) measures the position (X) of the end effector 120 in real time, and the external force measuring unit (150 in FIG. 1) measures the force applied to the end effector 120. This is performed by measuring the external force (F e ) in real time. Additionally, in the measurement step (S130), whether the patient is holding the end effector 120 can be confirmed in real time by a pressure sensor (not shown) attached to the end effector 120. The position ( It is transmitted to 160 in FIG. 1 and confirmed by the control unit 160.

위치 비교 단계(S140)에서는 엔드 이펙터(120)의 측정 위치(X)와 목표 위치(도 3에서 Xd)가 제어부(도 1의 160)에 의해 비교된다. 구체적으로 위치 비교 단계(S140)를 통해 엔드 이펙터(120)의 실제 길이방향 위치(XL)와 목표 길이방향 위치(XLd)가 비교된다. 도 3에서 파선의 화살표는 엔드 이펙터(120)의 목표 이동 궤적을 나타낸다. 위치 비교 단계(S140)에서 엔드 이펙터(120)의 실제 길이방향 위치(XL)가 목표 길이방향 위치(XLd)보다 앞서는 것으로 판단되는 경우에, 제1 보조력 산출 단계(S150)가 수행되고, 위치 비교 단계(S140)에서 엔드 이펙터(120)의 실제 길이방향 위치(XL)가 목표 길이방향 위치(XLd)보다 앞서지 않는 것으로 판단되는 경우에, 제2 보조력 산출 단계(S160)가 수행된다.In the position comparison step (S140), the measured position (X) of the end effector 120 and the target position (X d in FIG. 3) are compared by the control unit (160 in FIG. 1). Specifically, the actual longitudinal position (X L ) and the target longitudinal position (X Ld ) of the end effector 120 are compared through the position comparison step (S140). In FIG. 3 , the dashed arrow indicates the target movement trajectory of the end effector 120. If it is determined in the position comparison step (S140) that the actual longitudinal position (X L ) of the end effector 120 is ahead of the target longitudinal position (X Ld ), the first assisting force calculation step (S150) is performed, , when it is determined in the position comparison step (S140) that the actual longitudinal position (X L ) of the end effector 120 is not ahead of the target longitudinal position (X Ld ), the second assisting force calculation step (S160) is performed. It is carried out.

제1 보조력 산출 단계(S150)에서는 제어부(도 1의 160)에 의해 보조력(FA)이 산출된다. 제1 보조력 산출 단계(S150)는 위치 비교 단계에서 엔드 이펙터(120)의 실제 길이방향 위치(XL)가 목표 길이방향 위치(XLd)보다 앞서는 것으로 판단되는 경우에 수행된다. 제1 보조력 산출 단계(S150)에서 산출되는 보조력(FA)을 제1 산출 보조력이라 한다. 제1 산출 보조력은 수학식 12에서 설정된 Kd를 0으로 하는 제어입력에 의해 산출된다. 그에 따라, 제1 산출 보조력은 목표 위치보다 앞서나가는 엔드 이펙터(120)를 뒤로 끌지 않게 된다.In the first assisting force calculation step (S150), assisting force ( FA ) is calculated by the control unit (160 in FIG. 1). The first assisting force calculation step (S150) is performed when it is determined that the actual longitudinal position (X L ) of the end effector 120 is ahead of the target longitudinal position (X Ld ) in the position comparison step. The assisting force (F A ) calculated in the first assisting force calculation step (S150) is called the first calculated assisting force. The first calculated auxiliary force is calculated by a control input that sets K d set in Equation 12 to 0. Accordingly, the first calculated assisting force does not pull back the end effector 120, which is ahead of the target position.

제2 보조력 산출 단계(S160)에서는 제어부(도 1의 160)에 의해 보조력(FA)이 산출된다. 제2 보조력 산출 단계(S160)는 위치 비교 단계에서 엔드 이펙터(120)의 실제 길이방향 위치(XL)가 목표 길이방향 위치(XLd)보다 앞서지 않는 것으로 판단되는 경우에 수행된다. 제2 보조력 산출 단계(S160)에서 산출되는 보조력(FA)을 제2 산출 보조력이라 한다. 제2 산출 보조력은 수학식 12의 제어입력을 이용하여 산출된다.In the second assisting force calculation step (S160), assisting force ( FA ) is calculated by the control unit (160 in FIG. 1). The second assisting force calculation step (S160) is performed when it is determined that the actual longitudinal position (X L ) of the end effector 120 is not ahead of the target longitudinal position (X Ld ) in the position comparison step. The assisting force (F A ) calculated in the second assisting force calculation step (S160) is called the second calculated assisting force. The second calculated assisting force is calculated using the control input of Equation 12.

산출 보조력 기본 비교 단계(S170)에서는 제어부(도 1의 160)에 의해 제1 보조력 산출 단계(S150)를 통해 산출된 제1 산출 보조력 또는 제2 보조력 산출 단계(S160)를 통해 산출된 제2 산출 보조력이 설정 단계(S110)에서 설정된 제1 제한 보조력과 비교된다. 산출 보조력 기본 비교 단계(S170)에서 제1 산출 보조력 또는 제2 산출 보조력의 크기가 제1 제한 보조력의 크기보다 크지 않은 것으로 판단되는 경우에, 제1 인가 보조력 산출 단계(S180)가 수행되고, 제1 산출 보조력 또는 제2 산출 보조력의 크기가 제1 제한 보조력의 크기보다 큰 것으로 판단되는 경우에 제3 보조력 산출 단계(S187)가 수행된다.In the calculation auxiliary force basic comparison step (S170), the first auxiliary force calculated by the control unit (160 in FIG. 1) through the first auxiliary force calculation step (S150) or the second auxiliary force calculation step (S160) is calculated. The calculated second assisting force is compared with the first limited assisting force set in the setting step (S110). If it is determined in the calculation assisting force basic comparison step (S170) that the size of the first calculated assisting force or the second calculated assisting force is not greater than the size of the first limited assisting force, the first applied assisting force calculating step (S180) is performed, and when it is determined that the size of the first calculated assisting force or the second calculated assisting force is greater than the size of the first limited assisting force, the third assisting force calculation step (S187) is performed.

제1 인가 보조력 산출 단계(S180)에서는 제어부(도 1의 160)에 의해 엔드 이펙터(120)에 인가되는 제1 인가 보조력이 산출된다. 제1 인가 보조력 산출 단계(S180)는 산출 보조력 기본 비교 단계(S170)에서 제1 산출 보조력 또는 제2 산출 보조력의 크기가 제1 제한 보조력의 크기보다 크지 않은 것으로 판단되는 경우에 수행된다. 제1 인가 보조력 산출 단계(S180)에서 산출되는 제1 인가 보조력은 제1 보조력 산출 단계(S150)를 통해 산출된 제1 산출 보조력 또는 제2 보조력 산출 단계(S160)를 통해 산출된 제2 산출 보조력이다. 제1 인가 보조력 산출 단계(S180)를 통해 제1 인가 보조력이 산출된 후에는 제1 보조력 인가 단계(S185)가 수행된다.In the first applied assisting force calculation step (S180), the first applied assisting force applied to the end effector 120 is calculated by the control unit (160 in FIG. 1). The first approved assisting force calculation step (S180) is performed when it is determined that the size of the first calculated assisting force or the second calculated assisting force is not greater than the size of the first limited assisting force in the calculated assisting force basic comparison step (S170). It is carried out. The first applied assisting force calculated in the first applied assisting force calculation step (S180) is calculated through the first assisting force calculated through the first assisting force calculating step (S150) or the second assisting force calculated through the second assisting force calculating step (S160). This is the second output auxiliary force. After the first applied assisting force is calculated through the first applied assisting force calculating step (S180), the first assisting force applying step (S185) is performed.

제1 보조력 인가 단계(S185)에서는 제1 인가 보조력 산출 단계(S180)에서 산출된 제1 인가 보조력이 제어부(도 1의 160)에 의해 엔드 이펙터(120)에 인가된다. 제1 보조력 인가 단계(S185)는 제어부(160)가 제1 인가 보조력을 발생시키도록 제1 구동 모터(132) 및 제2 구동 모터(135)를 제어함으로써 수행된다.In the first assisting force application step (S185), the first applied assisting force calculated in the first applied assisting force calculation step (S180) is applied to the end effector 120 by the control unit (160 in FIG. 1). The first auxiliary force application step (S185) is performed by the controller 160 controlling the first drive motor 132 and the second drive motor 135 to generate the first applied auxiliary force.

제3 보조력 산출 단계(S187)에서는 제어부(도 1의 160)에 의해 보조력(FA)이 산출된다. 제3 보조력 산출 단계(S187)는 산출 보조력 기본 비교 단계(S170)에서 제1 산출 보조력 또는 제2 산출 보조력의 크기가 설정된 제1 제한 보조력의 크기보다 큰 것으로 판단되는 경우에 수행된다. 제3 보조력 산출 단계(S187)에서 산출되는 보조력(FA)을 제3 산출 보조력이라 한다. 제3 산출 보조력은 아래 수학식 20의 제어입력을 이용하여 산출된다.In the third assisting force calculation step (S187), assisting force F A is calculated by the control unit (160 in FIG. 1). The third assisting force calculation step (S187) is performed when the size of the first calculated assisting force or the second calculated assisting force is determined to be greater than the size of the set first limited assisting force in the calculation assist force basic comparison step (S170). do. The assisting force (F A ) calculated in the third assisting force calculation step (S187) is called the third calculated assisting force. The third calculated auxiliary force is calculated using the control input of Equation 20 below.

Figure pat00107
Figure pat00107

제3 보조력 산출 단계(S187)를 통해 제3 산출 보조력이 산출된 후에는 산출 보조력 추가 비교 단계(S189)가 수행된다.After the third calculated assisting force is calculated through the third assisting force calculation step (S187), the calculated assisting force additional comparison step (S189) is performed.

산출 보조력 추가 비교 단계(S189)에서는 제어부(도 1의 160)에 의해 제3 보조력 산출 단계(S187)를 통해 산출된 제3 산출 보조력이 설정 단계(S110)에서 설정된 제2 제한 보조력과 비교된다. 산출 보조력 추가 비교 단계(S189)에서 제3 산출 보조력의 크기가 제2 제한 보조력의 크기보다 크지 않은 것으로 판단되는 경우에, 제2 인가 보조력 산출 단계(S190)가 수행되고, 제3 산출 보조력의 크기가 제2 제한 보조력의 크기보다 큰 것으로 판단되는 경우에 제3 인가 보조력 산출 단계(S197)가 수행된다.In the calculated assisting force addition comparison step (S189), the third calculated assisting force calculated by the control unit (160 in FIG. 1) through the third assisting force calculation step (S187) is the second limited assisting force set in the setting step (S110). compared to If it is determined in the calculated assisting force addition comparison step (S189) that the size of the third calculated assisting force is not greater than the size of the second limited assisting force, the second applied assisting force calculation step (S190) is performed, and the third applied assisting force calculation step (S190) is performed. When it is determined that the size of the calculated assisting force is greater than the size of the second limited assisting force, the third applied assisting force calculation step (S197) is performed.

제2 인가 보조력 산출 단계(S190)에서는 제어부(도 1의 160)에 의해 엔드 이펙터(120)에 인가되는 제2 인가 보조력이 산출된다. 제2 인가 보조력 산출 단계(S190)는 산출 보조력 기본 비교 단계(S170)에서 제1 산출 보조력 또는 제2 산출 보조력의 크기가 제1 제한 보조력의 크기보다 크고 산출 보조력 추가 비교 단계(S189)에서 제3 산출 보조력이 제2 제한 보조력보다 크지 않은 것으로 판단되는 경우에 수행된다. 제2 인가 보조력 산출 단계(S190)에서 산출되는 제2 인가 보조력은 제3 보조력 산출 단계(S187)에서 산출되는 제3 산출 보조력과 동일하다. 제2 인가 보조력 산출 단계(S190)를 통해 제2 인가 보조력이 산출된 후에는 제2 보조력 인가 단계(S195)가 수행된다.In the second applied assisting force calculation step (S190), the second applied assisting force applied to the end effector 120 is calculated by the control unit (160 in FIG. 1). In the second authorized assisting force calculation step (S190), the size of the first calculated assisting force or the second calculated assisting force is greater than the size of the first limited assisting force in the calculated assisting force basic comparison step (S170) and the calculated assisting force additional comparison step is performed. This is performed when it is determined that the third calculated assisting force is not greater than the second limiting assisting force in (S189). The second applied assisting force calculated in the second applied assisting force calculation step (S190) is the same as the third calculated assisting force calculated in the third assisting force calculating step (S187). After the second applied assisting force is calculated through the second applied assisting force calculating step (S190), the second assisting force applying step (S195) is performed.

제2 보조력 인가 단계(S195)에서는 제2 인가 보조력 산출 단계(S190)에서 산출된 제2 인가 보조력인 제3 산출 보조력이 제어부(도 1의 160)에 의해 엔드 이펙터(120)에 인가된다. 제2 보조력 인가 단계(S195)는 제어부(160)가 제2 인가 보조력을 발생시키도록 제1 구동 모터(132) 및 제2 구동 모터(135)를 제어함으로써 수행된다.In the second assisting force application step (S195), the third calculated assisting force, which is the second applied assisting force calculated in the second applied assisting force calculating step (S190), is applied to the end effector 120 by the control unit (160 in FIG. 1). approved. The second auxiliary force application step (S195) is performed by the controller 160 controlling the first drive motor 132 and the second drive motor 135 to generate the second applied auxiliary force.

제3 인가 보조력 산출 단계(S197)에서는 제어부(도 1의 160)에 의해 엔드 이펙터(120)에 인가되는 제3 인가 보조력이 산출된다. 제3 인가 보조력 산출 단계(S197)는 산출 보조력 추가 비교 단계(S189)에서 제3 산출 보조력이 제2 제한 보조력의 크기보다 큰 것으로 판단되는 경우에 수행된다. 제3 인가 보조력 산출 단계에서 산출되는 제3 인가 보조력은 제2 제한 보조력과 동일하다. 제3 인가 보조력 산출 단계(S197)를 통해 제3 인가 보조력이 산출된 후에는 제3 보조력 인가 단계(S199)가 수행된다.In the third applied assisting force calculation step (S197), the third applied assisting force applied to the end effector 120 is calculated by the control unit (160 in FIG. 1). The third applied assisting force calculation step (S197) is performed when it is determined that the third calculated assisting force is greater than the size of the second limited assisting force in the calculated assisting force addition comparison step (S189). The third applied assisting force calculated in the third applied assisting force calculation step is the same as the second limited assisting force. After the third applied assisting force is calculated through the third applied assisting force calculating step (S197), the third assisting force applying step (S199) is performed.

제3 보조력 인가 단계(S199)에서는 제3 인가 보조력 산출 단계(S197)에서 산출된 제3 인가 보조력인 제2 제한 보조력이 제어부(도 1의 160)에 의해 엔드 이펙터(120)에 인가된다. 제3 보조력 인가 단계(S199)는 제어부(160)가 제3 인가 보조력을 발생시키도록 제1 구동 모터(132) 및 제2 구동 모터(135)를 제어함으로써 수행된다. 그에 따라, 제3 인가 보조력은 제2 제한 보조력을 넘지 않게 된다.In the third assisting force application step (S199), the second limited assist force, which is the third applied assist force calculated in the third applied assist force calculation step (S197), is applied to the end effector 120 by the control unit (160 in FIG. 1). approved. The third auxiliary force application step (S199) is performed by the controller 160 controlling the first drive motor 132 and the second drive motor 135 to generate the third applied auxiliary force. Accordingly, the third applied assisting force does not exceed the second limited assisting force.

추가로, 재활 로봇의 제어방법은 상기 수학식 19와 같은 제어입력으로 엔드 이펙터(120)의 손잡이 각도를 제어하는 단계를 더 포함할 수 있다.Additionally, the control method of the rehabilitation robot may further include controlling the handle angle of the end effector 120 using a control input such as Equation 19 above.

상기 실시예에서는 두 개의 제한 보조력이 설정되는 것으로 설명하였으나, 본 발명은 이에 제한되는 것은 아니다. 제한 보조력은 하나 또는 셋 이상이 설정될 수도 있으며, 이 또한 본 발명의 범위에 속하는 것이다. 제한 보조력이 하나인 경우에, 산출 보조력이 제한 보조력보다 크면 제한 보조력인 인가 보조력으로 산출될 수 있다.In the above embodiment, it has been described that two limited assist forces are set, but the present invention is not limited thereto. One or three or more limiting assist forces may be set, and this also falls within the scope of the present invention. In the case where there is only one limited assisting force, if the calculated assisting force is greater than the limited assisting force, it can be calculated as the applied assisting force that is the limited assisting force.

이상 실시예를 통해 본 발명을 설명하였으나, 본 발명은 이에 제한되는 것은 아니다. 상기 실시예는 본 발명의 취지 및 범위를 벗어나지 않고 수정되거나 변경될 수 있으며, 본 기술분야의 통상의 기술자는 이러한 수정과 변경도 본 발명에 속하는 것임을 알 수 있을 것이다.Although the present invention has been described through the above examples, the present invention is not limited thereto. The above embodiments may be modified or changed without departing from the spirit and scope of the present invention, and those skilled in the art will recognize that such modifications and changes also fall within the present invention.

100: 재활 로봇 시스템 110: 운동 기구부
111: 제1 링크 112: 제2 링크
113: 제3 링크 114: 제4 링크
120: 엔드 이펙터 130: 구동부
132: 제1 구동 모터 135: 제2 구동 모터
140: 위치 측정부 150: 외력 측정부
160: 제어부100: Rehabilitation robot system 110: Exercise mechanism unit
111: first link 112: second link
113: Third link 114: Fourth link
120: end effector 130: driving unit
132: first driving motor 135: second driving motor
140: Position measuring unit 150: External force measuring unit
160: control unit

Claims (16)

운동 기구부;
상기 운동 기구부에 설치되어서 상기 운동 기구부의 운동에 의해 위치가 변하는 엔드 이펙터;
상기 운동 기구부를 운동시키는 구동력을 발생시키는 구동부;
상기 엔드 이펙터의 위치를 측정하는 위치 측정부;
상기 엔드 이펙터에 가해지는 외력을 측정하는 외력 측정부;
상기 구동력을 제어하여 상기 엔드 이펙터에 인가되는 인가 보조력을 조절하는 제어부를 포함하며,
상기 제어부는 설정된 제한 보조력 이하의 값에서 상기 인가 보조력을 아래 수학식 13에 의한 제어입력을 이용하여 산출되며,
[수학식 13]
Figure pat00108

상기 수학식 13에서,
Figure pat00109
는 상기 위치 측정부에 의해 측정되는 상기 엔드 이펙터의 위치이며,
Figure pat00110
,
Figure pat00111
,
Figure pat00112
는 임의로 설정되는 값이고,
Figure pat00113
는 아래 수학식 16과 같이 정의되며,
[수학식 16]
Figure pat00114

수학식 16에서,
Figure pat00115
는 상기 엔드 이펙터의 목표 위치이고,
Figure pat00116
는 상기 외력 측정부에 의한 측정 값이며,
Figure pat00117
는 이전 단계에서 계산된 값으로서 최초의
Figure pat00118
는 아래 수학식 17과 같이 산출되는,
[수학식 17]
Figure pat00119

재활 로봇 시스템.exercise mechanism;
an end effector installed on the exercise mechanism unit and whose position changes due to movement of the exercise mechanism unit;
a driving unit that generates a driving force to move the exercise mechanism unit;
a position measuring unit that measures the position of the end effector;
An external force measuring unit that measures an external force applied to the end effector;
It includes a control unit that controls the driving force to adjust the applied auxiliary force applied to the end effector,
The control unit calculates the applied assisting force at a value below the set limit assisting force using a control input according to Equation 13 below,
[Equation 13]
Figure pat00108

In Equation 13 above,
Figure pat00109
is the position of the end effector measured by the position measuring unit,
Figure pat00110
,
Figure pat00111
,
Figure pat00112
is a value set arbitrarily,
Figure pat00113
is defined as Equation 16 below,
[Equation 16]
Figure pat00114

In equation 16,
Figure pat00115
is the target position of the end effector,
Figure pat00116
is the measured value by the external force measuring unit,
Figure pat00117
is the value calculated in the previous step and is the first
Figure pat00118
is calculated as in Equation 17 below,
[Equation 17]
Figure pat00119

Rehabilitation robotic system. 청구항 1에 있어서,
상기
Figure pat00120
의 길이방향 성분 값이
Figure pat00121
의 길이방향 성분 값보다 큰 경우에 상기 수학식 13에서 상기
Figure pat00122
는 0의 값을 갖는,
재활 로봇 시스템.In claim 1,
remind
Figure pat00120
The longitudinal component value of
Figure pat00121
In Equation 13, if it is greater than the longitudinal component value of
Figure pat00122
has a value of 0,
Rehabilitation robotic system. 청구항 1에 있어서,
상기 운동 기구부는 회전 가능하게 연결되는 복수 개의 링크들을 구비하는,
재활 로봇 시스템.In claim 1,
The exercise mechanism unit includes a plurality of links rotatably connected,
Rehabilitation robotic system. 청구항 3에 있어서,
상기 운동 기구부는 구동부에 의해 회전하는 제1 링크와, 상기 구동부에 의해 상기 제1 링크와는 별개로 독립적으로 회전하는 제2 링크와, 상기 제2 링크에 회전 가능하게 연결되는 제3 링크와, 상기 제1 링크에 회전 가능하게 연결되고 상기 엔드 이펙터가 설치되는 제4 링크를 구비하며,
상기 제3 링크는 상기 제4 링크에 회전 가능하게 연결되는,
재활 로봇 시스템.In claim 3,
The exercise mechanism includes a first link rotated by a drive unit, a second link rotated independently of the first link by the drive unit, and a third link rotatably connected to the second link, A fourth link is rotatably connected to the first link and the end effector is installed,
The third link is rotatably connected to the fourth link,
Rehabilitation robotic system. 청구항 3에 있어서,
상기 구동부는 상기 제1 링크를 제1 회전축선을 중심으로 회전시키는 제1 구동 모터와, 상기 제2 링크를 상기 제1 회전축선을 중심으로 회전시키는 제2 구동 모터를 구비하는,
재활 로봇 시스템.In claim 3,
The driving unit includes a first drive motor that rotates the first link about the first rotation axis, and a second drive motor that rotates the second link about the first rotation axis,
Rehabilitation robotic system. 운동 기구부와, 상기 운동 기구부에 설치되어서 상기 운동 기구부의 운동에 의해 위치가 변하는 엔드 이펙터와, 상기 운동 기구부를 운동시키는 구동력을 발생시키는 구동부와, 상기 엔드 이펙터의 위치를 측정하는 위치 측정부와, 상기 엔드 이펙터에 가해지는 외력을 측정하는 외력 측정부와, 상기 구동력을 제어하여 상기 엔드 이펙터에 인가되는 인가 보조력을 조절하는 제어부를 포함하는 재활 로봇을 제어하는 방법으로서,
제어에 필요한 기본 상수 값들이 설정되는 설정 단계;
상기 위치 측정부에 의한 상기 엔드 이펙터의 측정 위치와 상기 외력 측정부에 의한 측정 외력이 상기 제어부에 의해 확인되는 측정 단계; 및
상기 제어부에 의해 보조력이 산출되는 보조력 산출 단계를 포함하며,
상기 보조력은 아래 수학식 13에 의한 제어입력을 이용하여 산출되며,
[수학식 13]
Figure pat00123

상기 수학식 13에서,
Figure pat00124
는 상기 측정 위치이며,
Figure pat00125
,
Figure pat00126
,
Figure pat00127
는 상기 설정 단계에서 임의로 설정되는 값이고,
Figure pat00128
는 아래 수학식 16과 같이 정의되며,
[수학식 16]
Figure pat00129

수학식 16에서,
Figure pat00130
는 상기 목표 위치이고,
Figure pat00131
는 상기 측정 외력이며,
Figure pat00132
는 이전 단계에서 계산된 값으로서 최초의
Figure pat00133
는 아래 수학식 17과 같이 산출되는,
[수학식 17]
Figure pat00134

재활 로봇의 제어방법.An exercise mechanism unit, an end effector installed on the exercise mechanism unit whose position changes due to movement of the exercise mechanism unit, a drive unit that generates a driving force to move the exercise mechanism unit, and a position measuring unit that measures the position of the end effector; A method of controlling a rehabilitation robot including an external force measuring unit that measures an external force applied to the end effector, and a control unit that controls the driving force to adjust the applied assistance force applied to the end effector,
A setting step in which basic constant values required for control are set;
A measurement step in which the measured position of the end effector by the position measuring unit and the external force measured by the external force measuring unit are confirmed by the control unit; and
It includes an assisting force calculation step in which the assisting force is calculated by the control unit,
The auxiliary force is calculated using the control input according to Equation 13 below,
[Equation 13]
Figure pat00123

In Equation 13 above,
Figure pat00124
is the measurement position,
Figure pat00125
,
Figure pat00126
,
Figure pat00127
is a value arbitrarily set in the setting step,
Figure pat00128
is defined as Equation 16 below,
[Equation 16]
Figure pat00129

In equation 16,
Figure pat00130
is the target position,
Figure pat00131
is the measured external force,
Figure pat00132
is the value calculated in the previous step and is the first
Figure pat00133
is calculated as in Equation 17 below,
[Equation 17]
Figure pat00134

Control method of rehabilitation robot. 청구항 6에 있어서,
상기 위치 비교 단계에서 상기 측정 위치의 길이방향 위치가 상기 목표 위치의 길이방향 위치보다 앞서는 경우에 상기 보조력 산출 단계에서 상기
Figure pat00135
는 0으로 재설정되어서 상기 보조력이 산출되는,
재활 로봇의 제어방법.In claim 6,
If the longitudinal position of the measured position is ahead of the longitudinal position of the target position in the position comparison step, in the assisting force calculation step,
Figure pat00135
is reset to 0 so that the auxiliary force is calculated,
Control method of rehabilitation robot. 청구항 6에 있어서,
상기 보조력 산출 단계에서 산출된 산출 보조력이 상기 제어부에 의해 상기 설정 단계에서 설정된 제1 제한 보조력과 비교되는 산출 보조력 기본 비교 단계와,
상기 산출 보조력 기본 비교 단계의 결과에 따라 상기 엔드 이펙터에 인가되는 인가 보조력이 상기 제어부에 의해 산출되는 인가 보조력 산출 단계를 더 포함하는,
재활 로봇의 제어방법.In claim 6,
A calculation assisting force basic comparison step in which the calculated assisting force calculated in the assisting force calculation step is compared with a first limited assisting force set by the control unit in the setting step;
Further comprising an applied assisting force calculation step in which the applied assisting force applied to the end effector is calculated by the control unit according to the result of the calculating assisting force basic comparison step,
Control method of rehabilitation robot. 청구항 8에 있어서,
상기 산출 보조력 기본 비교 단계에서 상기 산출 보조력의 크기가 상기 제1 제한 보조력의 크기보다 크지 않은 것으로 판단되는 경우에 상기 산출 보조력이 상기 인가 보조력으로 산출되는,
재활 로봇의 제어방법.In claim 8,
If it is determined in the calculation assisting force basic comparison step that the size of the calculated assisting force is not greater than the size of the first limited assisting force, the calculated assisting force is calculated as the applied assisting force,
Control method of rehabilitation robot. 청구항 9에 있어서,
상기 설정 단계에서 상기 제1 제한 보조력보다 큰 제2 제한 보조력이 추가로 설정되며,
상기 산출 보조력 기본 비교 단계에서 상기 산출 보조력의 크기가 상기 제1 제한 보조력의 크기보다 큰 것으로 판단되는 경우에, 상기 제어부에 의해 제3 보조력이 산출되는 제3 보조력 산출 단계와,
상기 제어부에 의해 상기 제3 보조력의 크기와 상기 제2 제한 보조력의 크기가 비교되는 산출 보조력 추가 비교 단계를 더 포함하며,
상기 산출 보조력 기본 비교 단계에서 상기 산출 보조력이 상기 제1 제한 보조력보다 크고, 상기 산출 보조력 추가 비교 단계에서 상기 제3 보조력이 상기 제2 제한 보조력보다 크지 않은 것으로 판단되는 경우에, 상기 제3 보조력이 상기 인가 보조력으로 산출되는,
재활 로봇의 제어방법.In claim 9,
In the setting step, a second limiting assisting force greater than the first limiting assisting force is additionally set,
A third assisting force calculation step in which a third assisting force is calculated by the control unit when the magnitude of the calculated assisting force is determined to be greater than the magnitude of the first limited assisting force in the calculating assisting force basic comparison step;
It further includes a calculated assisting force additional comparison step in which the size of the third assisting force and the size of the second limited assisting force are compared by the control unit,
In the case where it is determined that the calculation assisting force is greater than the first limiting assisting force in the calculation assisting force basic comparison step, and the third assisting force is determined to be not greater than the second limiting assisting force in the calculating assisting force additional comparison step, , wherein the third auxiliary force is calculated as the applied auxiliary force,
Control method of rehabilitation robot. 청구항 9에 있어서,
상기 설정 단계에서 상기 제1 제한 보조력보다 큰 제2 제한 보조력이 추가로 설정되며,
상기 산출 보조력 기본 비교 단계에서 상기 산출 보조력의 크기가 상기 제1 제한 보조력의 크기보다 큰 것으로 판단되는 경우에, 상기 제어부에 의해 제3 보조력이 산출되는 제3 보조력 산출 단계와,
상기 제어부에 의해 상기 제3 보조력의 크기와 상기 제2 제한 보조력의 크기가 비교되는 산출 보조력 추가 비교 단계를 더 포함하며,
상기 산출 보조력 기본 비교 단계에서 상기 산출 보조력이 상기 제1 제한 보조력보다 크고, 상기 산출 보조력 추가 비교 단계에서 상기 제3 보조력이 상기 제2 제한 보조력보다 큰 것으로 판단되는 경우에, 상기 제2 제한 보조력이 상기 인가 보조력으로 산출되는,
재활 로봇의 제어방법.In claim 9,
In the setting step, a second limiting assisting force greater than the first limiting assisting force is additionally set,
A third assisting force calculation step in which a third assisting force is calculated by the control unit when the magnitude of the calculated assisting force is determined to be greater than the magnitude of the first limited assisting force in the calculating assisting force basic comparison step;
It further includes a calculated assisting force additional comparison step in which the size of the third assisting force and the size of the second limited assisting force are compared by the control unit,
When the calculated assisting force is determined to be greater than the first limited assisting force in the calculation assist force basic comparison step, and the third assisting force is determined to be greater than the second limited assisting force in the calculation assist force additional comparison step, The second limited assisting force is calculated as the applied assisting force,
Control method of rehabilitation robot. 청구항 9에 있어서,
상기 제어부가 상기 산출 보조력을 상기 엔드 이펙터로 인가되는 보조력 인가 단계를 더 포함하는,
재활 로봇의 제어방법.In claim 9,
The control unit further includes an auxiliary force application step in which the calculated auxiliary force is applied to the end effector,
Control method of rehabilitation robot. 청구항 8에 있어서,
상기 산출 보조력 기본 비교 단계에서 상기 산출 보조력의 크기가 상기 제1 제한 보조력의 크기보다 큰 것으로 판단되는 경우에 상기 제1 제한 보조력이 상기 인가 보조력으로 산출되는,
재활 로봇의 제어방법.In claim 8,
When the size of the calculated assisting force is determined to be greater than the size of the first limited assisting force in the calculated assisting force basic comparison step, the first limited assisting force is calculated as the applied assisting force,
Control method of rehabilitation robot. 청구항 13에 있어서,
상기 제어부가 상기 제1 제한 보조력을 상기 엔드 이펙터로 인가되는 보조력 인가 단계를 더 포함하는,
재활 로봇의 제어방법.In claim 13,
Further comprising an assisting force application step in which the control unit applies the first limited assisting force to the end effector,
Control method of rehabilitation robot. 청구항 6에 있어서,
상기 측정 단계에서 상기 엔드 이펙터의 파지 여부가 확인되며,
상기 수학식 16에서
Figure pat00136
는 시간 변수
Figure pat00137
는 아래 수학식 18과 같이 산출되며,
[수학식 18]
Figure pat00138

수학식 18에서,
Figure pat00139
는 상기 파지가 확인되지 않는 시간에 따라 증가하고, 파지가 확인되는 시간에 따라 감소하는,
재활 로봇의 제어방법.In claim 6,
In the measurement step, it is confirmed whether the end effector is gripped,
In Equation 16 above,
Figure pat00136
is a time variable
Figure pat00137
is calculated as in equation 18 below,
[Equation 18]
Figure pat00138

In equation 18,
Figure pat00139
increases with the time the phage is not identified and decreases with the time the phage is identified,
Control method of rehabilitation robot. 청구항 6에 있어서,
상기 재활 로봇은 상기 손잡이를 회전시키는 손잡이 회전 모터와, 상기 손잡이의 각도를 실시간으로 측정하는 각도 센서를 더 포함하며,
상기 제어부는 상기 손잡이의 각도가 원하는 각도로 회전하도록 상기 손잡이 회전 모터의 제어입력을 아래 수학식 19와 같이 산출하며,
[수학식 19]
Figure pat00140

상기 수학식 19에서,
Figure pat00141
, 상기 손잡이의 측정된 회전 각도이고,
Figure pat00142
는 상기 손잡이의 목표 회전 각도인,
재활 로봇의 제어방법.In claim 6,
The rehabilitation robot further includes a handle rotation motor that rotates the handle, and an angle sensor that measures the angle of the handle in real time,
The control unit calculates the control input of the handle rotation motor as shown in Equation 19 below so that the angle of the handle rotates to the desired angle,
[Equation 19]
Figure pat00140

In Equation 19 above,
Figure pat00141
, is the measured rotation angle of the handle,
Figure pat00142
is the target rotation angle of the handle,
Control method of rehabilitation robot.
KR1020220158148A 2022-11-23 2022-11-23 Rehabilitation robot system method for controlling rehabilitation robot KR20240076061A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR1020220158148A KR20240076061A (en) 2022-11-23 2022-11-23 Rehabilitation robot system method for controlling rehabilitation robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR1020220158148A KR20240076061A (en) 2022-11-23 2022-11-23 Rehabilitation robot system method for controlling rehabilitation robot

Publications (1)

Publication Number Publication Date
KR20240076061A true KR20240076061A (en) 2024-05-30

Family

ID=91275713

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020220158148A KR20240076061A (en) 2022-11-23 2022-11-23 Rehabilitation robot system method for controlling rehabilitation robot

Country Status (1)

Country Link
KR (1) KR20240076061A (en)

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101321791B1 (en) 2012-01-02 2013-10-28 한국과학기술연구원 Robot Motion Control System and Method for Active Body Rehabilitation

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101321791B1 (en) 2012-01-02 2013-10-28 한국과학기술연구원 Robot Motion Control System and Method for Active Body Rehabilitation

Similar Documents

Publication Publication Date Title
CN108430375B (en) Robot system and method for back driving the same
Moore et al. Cobot implementation of virtual paths and 3D virtual surfaces
JP4822063B2 (en) Robot direct teaching device
WO2019107454A1 (en) Skill transferring machinery
WO2014129110A1 (en) Robot, robot control device and control method, and robot control program
JP6934173B2 (en) Force control device, force control method and force control program
JPH0532197B2 (en)
JP2018058181A (en) Disturbance observer and robot control device
CN109129525B (en) Load center-of-gravity position estimation device and load center-of-gravity position estimation method for robot
JP2604929B2 (en) Robot control device
JP4054984B2 (en) Robot control apparatus and control method
KR20240076061A (en) Rehabilitation robot system method for controlling rehabilitation robot
JP4842561B2 (en) Force controller device
JP2004082243A (en) Actuator control device and method
JP2003039348A (en) Remotely-controlled robot control device
CN110621456B (en) Power transmission system for mechanical device
JPH0890461A (en) Device and method for controlling robot
JPH0445304B2 (en)
JPH01222311A (en) Curve profile controller for multidegree of freedom work machine
KR102288886B1 (en) Method and system for controlling motion of wearable robot
JP4376712B2 (en) Operating device
JP4577619B2 (en) Object gripping method for articulated multi-fingered hands
Kozuka et al. Motion-Assist Arm with a Passive Joint for an Upper Limb
JP4376732B2 (en) Auxiliary device and auxiliary method of conveyance work
KR20230102084A (en) Rehabilitation robot system method for controlling rehabilitation robot