KR20230014611A - Manipulator and method for controlling thereof - Google Patents

Abstract

A manipulator and a control method of the manipulator are disclosed. The manipulator according to the present disclosure comprises: a plurality of links corresponding to each of the upper arm, lower arm, and hand of a user; a plurality of motors rotating the plurality of links; a communication interface; a memory for storing at least one instruction; and a processor for executing at least one instruction. The processor obtains information about a body coordinate system of the link corresponding to the lower arm based on first rotation angle information for the motors corresponding to the upper arm and lower arm among the plurality of motors, obtains equilibrium angle information that ensures that the body coordinate system is in balance with a predefined reference coordinate system, obtains second rotation angle information for the motors corresponding to the hand among the plurality of motors based on a sensing value and balance angle information when the sensing value indicating a hand posture is received from an external sensor through the communication interface, and controls the motors corresponding to the hand based on the second rotation angle information. Various other embodiments are possible. Accordingly, the manufacturing cost of the manipulator can be reduced.

Description

Manipulator and its control method {MANIPULATOR AND METHOD FOR CONTROLLING THEREOF}

The present disclosure relates to a manipulator and a control method thereof, and more particularly, to a manipulator that moves along a motion of a user's arm and a control method thereof.

Recently, thanks to the development of robot technology, various types of robots such as cleaning robots, service robots, and industrial robots are being used. Recently, research on a robot arm, that is, a manipulator, which moves according to the motion of a user's (ie, human) arm has been actively conducted.

In particular, if the manipulator can move the user's arm quickly and intuitively, the manipulator can be used in various application fields, such as shopping for the user, organizing things in a shopping mall, cooking at home, or giving a massage. In addition, the manipulator can be widely used in fields such as logistics, medical care, education, and other fields requiring non-face-to-face/non-contact depending on epidemics.

Meanwhile, a conventional manipulator determines the position and posture of a user's arm using a visual sensor or a wireless signal. To this end, additional equipment having a function of a camera or a radio signal transmitter/receiver had to be installed in the conventional manipulator, which led to an increase in manufacturing cost of the manipulator. In addition, the conventional manipulator cannot accurately follow the user's motion because the link is not mapped one-to-one with the link of the user's arm.

Therefore, there is a need for a manipulator capable of accurately following the movement of a user's arm without a visual sensor.

On the other hand, according to the prior art, the user wears a device similar to the joint structure of the manipulator, which causes fatigue or discomfort due to the limitation of the range of motion to the user when used for a long time. Since the position of the hand is controlled, it is pointed out that it is not intuitive to grasp and control the actual movement of the manipulator on the screen from a remote location.

Accordingly, there is a need for a technology capable of improving user convenience and more intuitively and sophisticatedly controlling the manipulator.

Meanwhile, according to the prior art, the posture of the manipulator's hand is estimated using quaternion information obtained from a sensor mounted on the user's hand. According to this, a posture error may occur according to the movement of the user's arm. This is because the degree of freedom (DOF) of the manipulator is smaller than the degree of freedom for the movement of the user's arm. will increase cumulatively.

Therefore, there is a need for a technology that more accurately expresses the movement of the user's hand by minimizing the posture error generated according to the movement of the user's arm.

The present disclosure is to solve the problems of the prior art as described above, and an object of the present disclosure is to provide a manipulator and a control method of the manipulator capable of precisely following the movement of a user's arm.

The technical problems of the present invention are not limited to the technical problems mentioned above, and other technical problems not mentioned will be clearly understood by those skilled in the art from the description below.

According to an exemplary embodiment of the present disclosure for solving the above-described technical problem, the manipulator includes a plurality of links corresponding to each of the user's upper arm, lower arm, and hand, a plurality of motors rotating the plurality of links, a communication interface, A memory for storing at least one instruction and a processor for executing the at least one instruction, wherein the processor is configured to perform rotation angle information of motors corresponding to the upper arm and the lower arm among the plurality of motors based on first rotation angle information. , Obtains information on the body coordinate system of the link corresponding to the lower arm, obtains equilibrium angle information for equilibrating the body coordinate system with a predefined reference coordinate system, and acquires the posture of the hand from an external sensor through the communication interface. When a sensing value representing is received, second rotation angle information for motors corresponding to the hand among the plurality of motors is obtained based on the sensing value and the balance angle information, and based on the second rotation angle information to control the motors corresponding to the hand.

Here, the processor calculates a coordinate transformation matrix for converting the first rotation angle information into sensing values representing the posture of the upper arm and the posture of the lower arm, and based on the coordinate transformation matrix, a body coordinate system corresponding to the lower arm. information can be obtained.

Meanwhile, the balance angle information includes roll balance angle information and pitch balance angle information, and the roll balance angle information is the body when the body coordinate system is rotated with respect to the first axis of the body coordinate system. An angle at which the second axis of the coordinate system is parallel to the xy plane of the reference coordinate system may be indicated.

Here, the pitch equilibrium angle information is such that when the body coordinate system is rotated based on the second axis of the body coordinate system rotated according to the roll equilibrium angle information, the third axis of the body coordinate system coincides with the z-axis of the reference coordinate system. angle can be indicated.

Here, the processor acquires third rotation angle information about motors corresponding to the hand based on a sensing value indicating the posture of the hand, and corrects the third rotation angle information based on the balance angle information to obtain the second rotation angle information. Each piece of information can be obtained.

Here, the plurality of links include a first link corresponding to the upper arm, a second link corresponding to the lower arm, and a third link corresponding to the hand, and the plurality of motors drive the first link to a first axis. A first motor rotating the first link based on a second axis, a second motor rotating the second link based on the first axis, a third motor rotating the second link based on the first axis, and the second link based on a third axis It may include a fourth motor for rotating, a fifth motor for rotating the third link based on the first axis, and a sixth motor for rotating the third link based on the second axis.

Meanwhile, when the first user's command to stop the operation is received while the manipulator is operating, the processor deactivates an external sensor for recognizing the posture of the arm of the first user, stops the operation of the manipulator, and operates the manipulator. to transmit information on a guide screen for compensating for a difference between a posture of the manipulator and a posture of an arm of the second user to a user terminal of the second user when a second user's command to start an operation for control is received The communication interface is controlled, and when it is identified that the difference between the posture of the manipulator and the posture of the arms of the second user is within a preset threshold range, the plurality of motors are controlled based on the posture of the arms of the second user. can

Meanwhile, the processor controls the plurality of motors based on the posture of the user's arm when the user's command to start the repeated motion is received, and when the command to stop the repeated motion is received, the command to start the repeated motion is received. A control signal corresponding to the operation of the manipulator from the time of reception to the time of receiving the command to stop the repeated operation is stored in the memory, and when the control signal is stored, the maximum number of motors corresponding to the control signal When a user input for controlling the communication interface to transmit information on the working speed to the user terminal of the user and setting the working speed of the manipulator based on the information on the maximum working speed is received, the set working speed It is possible to control the plurality of motors based on.

According to an exemplary embodiment of the present disclosure for solving the above-described technical problem, control of a manipulator including a plurality of links corresponding to each of a user's upper arm, lower arm, and hand, and a plurality of motors rotating the plurality of links. The method includes obtaining information on a body coordinate system of a link corresponding to the lower arm based on first rotation angle information of motors corresponding to the upper arm and the lower arm among the plurality of motors, Obtaining equilibrium angle information to achieve equilibrium with a defined reference coordinate system, when a sensing value representing the posture of the hand is received from an external sensor, the hand among the plurality of motors based on the sensed value and the equilibrium angle information Obtaining second rotation angle information for motors corresponding to and controlling the motors corresponding to the hand based on the second rotation angle information.

Here, the acquiring of the information on the body coordinate system includes calculating a coordinate transformation matrix for converting the first rotation angle information into sensed values representing the posture of the upper arm and the posture of the lower arm, and and obtaining information on a moving body coordinate system corresponding to the lower arm based on the lower arm.

Meanwhile, the balance angle information includes roll balance angle information and pitch balance angle information, and the roll balance angle information is the body when the body coordinate system is rotated with respect to the first axis of the body coordinate system. An angle at which the second axis of the coordinate system is parallel to the xy plane of the reference coordinate system may be indicated.

Here, the pitch equilibrium angle information is such that when the body coordinate system is rotated based on the second axis of the body coordinate system rotated according to the roll equilibrium angle information, the third axis of the body coordinate system coincides with the z-axis of the reference coordinate system. angle can be indicated.

Here, the obtaining of the second rotation angle information may include obtaining third rotation angle information of motors corresponding to the hand based on a sensing value representing a posture of the hand and third rotation angle information based on the balance angle information. It may include obtaining second rotation angle information by correcting rotation angle information.

Here, the plurality of links include a first link corresponding to the upper arm, a second link corresponding to the lower arm, and a third link corresponding to the hand, and the plurality of motors drive the first link to a first axis. A first motor rotating the first link based on a second axis, a second motor rotating the second link based on the first axis, a third motor rotating the second link based on the first axis, and the second link based on a third axis It may include a fourth motor for rotating, a fifth motor for rotating the third link based on the first axis, and a sixth motor for rotating the third link based on the second axis.

According to an exemplary embodiment of the present disclosure for solving the above technical problem, a manipulator includes a plurality of links including a first link and a second link, a plurality of motors rotating the plurality of links, and a circuit. It includes a communication interface, a memory for storing at least one instruction, and a processor, wherein the processor receives a sensed value of an external sensor for detecting a posture of a user's arm through the communication interface, and obtains a value obtained based on the sensed value. A second vector corresponding to the posture of the user's arm is obtained based on a matrix and a first vector previously stored in the memory, posture information of the user's arm is obtained based on the second vector, and posture of the user's arm Controls driving of the plurality of motors based on information, and the processor acquires a third vector corresponding to the first link based on a sensing value of a first external sensor, and based on a sensing value of a second external sensor. to obtain a fourth vector corresponding to the second link, obtain attitude information corresponding to the second link based on the third vector and the fourth vector, and obtain attitude information corresponding to the second link Based on the control to drive the plurality of motors corresponding to the second link.

Here, the processor obtains a quaternion vector corresponding to the posture of the user's arm by applying the sensed value to an Altitude and Heading Reference System (AHRS) algorithm stored in the memory, and based on the quaternion vector, the processor matrix can be obtained.

Meanwhile, the second vector includes a 2-1 vector corresponding to the x-axis and a 2-2 vector corresponding to the z-axis, and the posture information of the user's arm includes a roll angle corresponding to the x-axis, A pitch angle corresponding to the y-axis and a yaw angle corresponding to the z-axis, wherein the processor obtains a first yaw angle based on the 2-1 vector, and the 2-2 A second yaw angle may be obtained based on the vector, and the yaw angle may be obtained based on the first yaw angle and the second yaw angle.

Meanwhile, the processor may obtain the yaw angle by applying weights to the first yaw angle and the second yaw angle based on a predefined weight function.

Meanwhile, the processor acquires a first matrix based on a first sensed value corresponding to an initially set posture of the user's arm, stores it in the memory, and based on a second sensed value corresponding to a current posture of the user's arm The matrix may be obtained based on the obtained second matrix and the first matrix.

Meanwhile, the processor obtains an angle corresponding to the x-axis based on a dot product of the third vector and the fourth vector, and obtains an angle corresponding to the z-axis based on a cross product of the third vector and the fourth vector. can be obtained

According to various embodiments of the present disclosure as described above, the manipulator can accurately follow the movement of the user's arm. Accordingly, user convenience and satisfaction are improved. Also, since the manipulator can operate without a visual sensor, manufacturing cost of the manipulator can be reduced.

In addition, effects that can be obtained or predicted due to the embodiments of the present disclosure will be directly or implicitly disclosed in the detailed description of the embodiments of the present disclosure. For example, various effects predicted according to an embodiment of the present disclosure will be disclosed within the detailed description to be described later.

1A is a diagram for explaining a concept of a manipulator according to an embodiment of the present disclosure.
1B is a diagram showing the structure of a manipulator according to an embodiment of the present disclosure.
2 is a block diagram showing the configuration of a manipulator according to an embodiment of the present disclosure.
3 is a diagram for explaining coordinate conversion of vectors.
4 is a graph of a weight function according to an embodiment of the present disclosure.
5 is a diagram for explaining a method of controlling driving of a motor according to an embodiment of the present disclosure.
6 is a diagram for explaining a method of obtaining a second angle according to an embodiment of the present disclosure.
7 is a flowchart illustrating a method of controlling a manipulator according to an embodiment of the present disclosure.
8 is a flowchart illustrating a control method of a manipulator according to an embodiment of the present disclosure. FIG. 9 is a description of a coordinate system according to an embodiment of the present disclosure and a process for obtaining information about a body coordinate system of a link corresponding to a lower arm. It is a drawing for explanation of the process.
10 is a diagram for explaining in detail a process of obtaining a roll equilibrium angle according to an embodiment of the present disclosure.
11 is a diagram for explaining in detail a process of obtaining a pitch equilibrium angle according to an embodiment of the present disclosure.
12 is a diagram for explaining an embodiment related to a process of controlling a manipulator by a plurality of users according to an embodiment of the present disclosure. And,
13 is a diagram for explaining an embodiment related to controlling a repetitive motion of a manipulator according to an embodiment of the present disclosure.

Terms used in this specification will be briefly described, and the present disclosure will be described in detail.

The terms used in the embodiments of the present disclosure have been selected from general terms that are currently widely used as much as possible while considering the functions in the present disclosure, but they may vary depending on the intention or precedent of a person skilled in the art, the emergence of new technologies, and the like. . In addition, in a specific case, there is also a term arbitrarily selected by the applicant, and in this case, the meaning will be described in detail in the description of the disclosure. Therefore, terms used in the present disclosure should be defined based on the meaning of the term and the general content of the present disclosure, not simply the name of the term.

Embodiments of the present disclosure may apply various transformations and may have various embodiments, and specific embodiments are illustrated in the drawings and described in detail in the detailed description. However, this is not intended to limit the scope to specific embodiments, and should be understood to include all transformations, equivalents, and substitutes included in the spirit and scope of technology disclosed. In describing the embodiments, if it is determined that a detailed description of a related known technology may obscure the subject matter, the detailed description will be omitted.

Terms such as first and second may be used to describe various components, but the components should not be limited by the terms. Terms are only used to distinguish one component from another.

Singular expressions include plural expressions unless the context clearly dictates otherwise. In this application, the terms "comprise" or "consist of" are intended to designate that there is a feature, number, step, operation, component, part, or combination thereof described in the specification, but one or more other It should be understood that the presence or addition of features, numbers, steps, operations, components, parts, or combinations thereof is not excluded in advance. Below, with reference to the accompanying drawings, the present disclosure pertains to embodiments of the present disclosure. It will be described in detail so that those skilled in the art can easily implement it. However, the present disclosure may be implemented in many different forms and is not limited to the embodiments described herein. And in order to clearly describe the present disclosure in the drawings, parts irrelevant to the description are omitted, and similar reference numerals are attached to similar parts throughout the specification.

1A is a diagram for explaining a concept of a manipulator according to an embodiment of the present disclosure.

The manipulator 100 may follow the movement of the user's arm 1 based on the sensing value of the external sensor 10 for detecting the posture of the user's arm 1 . The manipulator 100 may obtain posture information of the user's arm 1 based on a sensing value of the external sensor 10 (eg, an IMU sensor or a geomagnetic sensor). Here, the external sensor 10 is a first external sensor 11 attached to the upper arm (A) of the user's arm 1, a second external sensor 12 attached to the lower arm (B), and attached to the back of the hand (C). It may include a third external sensor 13 and a flex sensor 14 attached to the finger D.

The manipulator 100 may include a first link 111 corresponding to the upper arm A of the user's arm 1 and a second link 112 corresponding to the lower arm B. Also, the manipulator 100 may include a hand 120 including a plurality of fingers 121 . The manipulator 100 may operate based on posture information of the user's arm 1 . For example, the manipulator 100 may grip an object using the hand 120 .

1B is a diagram showing the structure of a manipulator according to an embodiment of the present disclosure.

The manipulator 100 may include a link 110 , a hand 120 and a motor 130 . The link 110 may include a first link 111 and a second link 112 . The motor 130 may include first to sixth motors 131 , 132 , 133 , 134 , 135 , and 136 .

The first motor 131 and the second motor 132 may be connected to the first link 111 to rotate the first link 111 . Specifically, the first motor 131 may rotate the first link 111 based on the first axis. Here, the first axis means the x-axis of the moving body coordinate system to be described later. The second motor 132 may rotate the first link 111 based on the second axis. Here, the second axis means the y-axis of the moving body coordinate system.

The third motor 133 and the fourth motor 134 may be connected to the first link 111 and the second link 112 to rotate the second link 112 . Specifically, the third motor 133 may rotate the second link 112 based on the first axis. The fourth motor 134 may rotate the second link 112 based on the third axis. Here, the third axis means the z-axis of the moving body coordinate system.

The fifth motor 135 and the sixth motor 136 may be connected to the second link 112 and the hand 120 to rotate the hand 120 . Specifically, the fifth motor 135 may rotate the hand 120 based on the first axis. The sixth motor 136 may rotate the hand 120 based on the second axis.

Meanwhile, the number of links 110 and motors 130 according to FIG. 1B is just an example, and the number of links 110 and motors 130 is not limited thereto.

2 is a block diagram showing the configuration of a manipulator according to an embodiment of the present disclosure. The manipulator 100 may include a link 110 , a hand 120 , a motor 130 , a communication interface 140 , a memory 150 and a processor 160 .

The link 110 may include a first link 111 corresponding to the upper arm of the user's (ie, human) arm and a second link 112 corresponding to the lower arm. The first link 111 and the second link 112 are connected through a motor 120, and may rotate along three axes according to the drive of the motor 120.

Hand 120 may include a plurality of fingers. The hand 120 may move each finger under the control of the processor 160 to grip the object or release the grip.

The motor 130 may include a plurality of motors 131 , 132 , 133 , 134 , 135 , and 136 . The motor 130 may be driven under the control of the processor 160 to move the link 110 and the hand 120 .

The communication interface 140 includes at least one circuit and can perform communication with various types of external devices according to various types of communication methods. For example, the communication interface 140 may receive a sensing value of the external sensor 10 from the external sensor 10 .

Here, the external sensor 10 may include a plurality of Inertial Measurement Unit (IMU) sensors, a plurality of geomagnetic sensors, and a flex sensor as a configuration for detecting the motion or posture of the user's arm 1 . For example, referring to FIG. 1A , a first external sensor 11 including a first IMU sensor and a first geomagnetic sensor may be attached to the upper arm of the user's arm 1 . The second external sensor 12 including the second IMU sensor and the second geomagnetic sensor may be attached to the lower arm of the user's arm 1 . A third external sensor 13 including a third IMU sensor and a third geomagnetic sensor may be attached to the back of the hand of the user's arm 1 . Also, the flex sensor 14 may be attached to a user's finger. Each external sensor may detect the posture of the user's arm (1). Meanwhile, in FIG. 1A, the first external sensor 11, the second external sensor 12, the third external sensor 13, and the flex sensor 14 are each shown attached to the user's arm 1, but the user It is also possible to wear one wearable device including the first external sensor 11 , the second external sensor 12 , the third external sensor 13 and the flex sensor 14 .

Meanwhile, the communication interface 140 may include a wireless communication module and a wired communication module. Wireless communication modules include BLE (Bluetooth Low Energy) module, Wi-Fi communication module, cellular communication module, 3G (3rd generation) mobile communication module, 4G (4th generation) mobile communication module, 4th generation LTE (Long Term Evolution) communication module, 5G (5th generation) may include at least one of mobile communication modules. The wired communication module may include an Ethernet module. Also, the communication interface 140 may include at least one communication terminal.

The memory 150 may store an Operating System (OS) for controlling overall operations of the components of the manipulator 100 and commands or data related to the components of the manipulator 100 . For example, the memory 150 may store data related to an Altitude and Heading Reference System (AHRS) algorithm for obtaining quaternion information based on a value sensed by the external sensor 10 . Meanwhile, the memory 150 may be implemented as a non-volatile memory (ex: hard disk, solid state drive (SSD), flash memory), volatile memory, or the like.

The processor 160 is electrically connected to the memory 150 to control overall functions and operations of the manipulator 100. In particular, the processor 160 may control driving of the motor 130 that rotates the link 110 and the hand 120 .

Hereinafter, an operation of the processor 160 for controlling driving of the first motor 131 and the second motor 132 for rotating the first link 111 corresponding to the upper arm of the user's arm 1 will be described. let it do For the description of the operation, a body frame and a navigation frame defined in the first link 111, the second link 112, and the hand 120 of the manipulator 100 are first defined.

The body coordinate system is a coordinate system that changes according to the movement of the manipulator 100 . Referring to FIG. 1B , the first link 111, the second link 112, and the hand 120 of the manipulator 100 are aligned in a downward direction, that is, in the direction of gravity. At this time, the X axis of the body coordinate system is defined as the direction of the first link 111, and the Z axis is the first link 111 when the manipulator 100 is moved by the second motor 132 so as to be perpendicular to the direction of gravity. is defined in the direction of The Y-axis of the body coordinate system is determined by the direction of the thumb when the right hand is closed in the direction from the Z-axis to the X-axis (right-hand coordinate system). In the present disclosure, the X axis of the moving body coordinate system is also referred to as a first axis, the Y axis as a second axis, and the Z axis as a third axis.

The navigation coordinate system is a fixed coordinate system regardless of the movement of the manipulator 100 . A coordinate conversion matrix between the navigation coordinate system and the moving body coordinate system may be obtained based on quaternion information or Euler angles.

The processor 160 may obtain quaternion information based on a sensing value of an external sensor. The quaternion information represents information about the rotation of the user's arm, and may include a rotation vector and a rotation angle. The processor 160 may obtain quaternion information by applying the sensing value of the external sensor to the AHRS algorithm stored in the memory 150 . For example, quaternion information may be a vector (q) such as [Equation 1].

[Equation 1]

Meanwhile, in controlling a robot arm in which a rotation axis is physically fixed, quaternion information needs to be converted into Euler angles. Accordingly, the existing robot arm control technology converts quaternion information into Euler angles as shown in [Equation 2].

[Equation 2]

)이 +90도나 -90도로 접근할수록 롤 각(

) 및 요 각(

)의 분자 및 분모가 0으로 수렴하게 되어, 롤 각(

) 및 요 각(

)의 추정이 어려워지는 문제가 발생한다. 이를, 소위 짐벌 락 문제(Gimbal lock problem)라 한다.However, according to [Equation 2], the pitch angle (

) approaches +90 degrees or -90 degrees, the roll angle (

) and yaw angle (

), the numerator and denominator converge to 0, so that the roll angle (

) and yaw angle (

) becomes difficult to estimate. This is called the so-called gimbal lock problem.

)을 획득할 수 있다. 여기서, 좌표변환행렬은 매니퓰레이터(100)를 기준으로 한 제1 벡터를 기준 좌표계의 제2 벡터로 변환하기 위한 행렬을 의미한다.Meanwhile, in order to solve the gimbal lock problem, the processor 160 according to the present disclosure obtains a vector for each axis of the body coordinate system analyzed in the navigation coordinate system using a matrix obtained based on quaternion information. For example, the processor 160 converts the coordinates from the quaternion information to the navigation coordinate system from the moving body coordinate system based on [Equation 3] below (

) can be obtained. Here, the coordinate conversion matrix means a matrix for converting a first vector based on the manipulator 100 into a second vector of the reference coordinate system.

[Equation 3]

3 is a diagram for explaining coordinate conversion of vectors.

)는 각각 동체 좌표계에서 표현된 x축 방향의 단위 벡터(

) 및 z축 방향의 단위 벡터(

)일 수 있다. 제1 벡터(

)는 미리 정해진 값으로 메모리(150)에 저장되어 있을 수 있다.Referring to FIG. 3 , a first vector based on the manipulator 100 (

) is a unit vector in the x-axis direction, each expressed in the body coordinate system (

) and the unit vector in the z-axis direction (

) can be. The first vector (

) may be stored in the memory 150 as a predetermined value.

) 및 제1 벡터(

)에 기초하여 제2 벡터(

)를 획득할 수 있다. 구체적으로, 프로세서(160)는 [수학식 4]에 기초하여 항법 좌표계에서 표현된 제2 벡터(

)를 획득할 수 있다. 제2 벡터(

)는 제2-1 벡터(

) 및 제2-2 벡터(

)를 포함할 수 있다. 한편, 본 개시의 벡터의 윗 첨자 b, n은 각각 동체 좌표계 및 항법 좌표계에서 해석된 위치 벡터를 의미한다.The processor 160 is a coordinate transformation matrix from the body coordinate system to the navigation coordinate system (

) and the first vector (

) based on the second vector (

) can be obtained. Specifically, the processor 160 is a second vector expressed in the navigation coordinate system based on [Equation 4] (

) can be obtained. The second vector (

) is the 2-1 vector (

) and the 2-2 vector (

) may be included. Meanwhile, superscripts b and n of the vectors of the present disclosure mean position vectors analyzed in the body coordinate system and the navigation coordinate system, respectively.

[Equation 4]

)에 기초하여 사용자 팔의 자세 정보를 획득할 수 있다. 사용자 팔의 자세 정보는, x축에 대응되는 롤(roll) 각도, y축에 대응되는 피치(pitch) 각도 및 z축에 대응되는 요(yaw) 각도를 포함할 수 있다. 본 발명에서는 제1 링크(111)의 롤(roll) 각은 무시하였다. 제1 링크(111)의 롤(roll) 회전각에 의한 제2 링크(112) 및 핸드(120)의 자세 변화는 제3 모터(133)로 표현될 수 있다. 항법 좌표계를 기준으로 제 1링크(111)의 모든 위치 벡터는 제1 모터 (131) 및 제2 모터(132)에 의해 각각 요각 및 피치각으로 표현될 수 있다.The processor 160 generates a second vector (

), posture information of the user's arm may be obtained. The posture information of the user's arm may include a roll angle corresponding to the x-axis, a pitch angle corresponding to the y-axis, and a yaw angle corresponding to the z-axis. In the present invention, the roll angle of the first link 111 is ignored. Changes in posture of the second link 112 and the hand 120 due to the roll rotation angle of the first link 111 may be expressed by the third motor 133 . Based on the navigation coordinate system, all position vectors of the first link 111 may be expressed as yaw angles and pitch angles by the first motor 131 and the second motor 132, respectively.

The processor 160 may obtain the pitch angle (pitch), the first yaw angle (yaw1), and the second yaw angle (yaw2) based on [Equation 5].

[Equation 5]

) 또는 제2-1 벡터(

)가 xy 평면상에 정사영된 벡터(

)에 기초하여 피치 각도(pitch) 및 제1 요 각도(yaw1)를 획득할 수 있다. 또한, 프로세서(160)는 제2-2 벡터(

) 또는 제2-2 벡터(

)가 xy 평면상에 정사영된 벡터(

)에 기초하여 제2 요 각도(yaw2)를 획득할 수 있다. The processor 160 is the 2-1 vector (

) or the 2-1 vector (

) is a vector projected onto the xy plane (

), the pitch angle pitch and the first yaw angle yaw1 may be obtained. In addition, the processor 160 is a 2-2 vector (

) or the 2-2 vector (

) is a vector projected onto the xy plane (

), the second yaw angle yaw2 may be obtained.

)를 획득할 수 있다. 구체적으로, 프로세서(160)는 [수학식 6]에 기초하여 최종 요 각도(

)를 획득할 수 있다.Then, the processor 160 determines the final yaw angle (based on the first yaw angle yaw1 , the second yaw angle yaw2 , and a predefined weight function W).

) can be obtained. Specifically, the processor 160 based on [Equation 6] the final yaw angle (

) can be obtained.

[Equation 6]

)를 획득할 수 있다. 예로, 가중치 함수(W)는 도 4에 도시된 바와 같이 그려질 수 있다.The processor 160 applies weights based on the weight function W to the first yaw angle yaw1 and the second yaw angle yaw2 to obtain a final yaw angle (

) can be obtained. For example, the weight function W can be plotted as shown in FIG. 4 .

)에 기초하여 제1 모터(131)를 제어할 수 있다. 또한, 프로세서(160)는 획득된 피치 각도(pitch)에 기초하여 제2 모터(132)의 구동을 제어할 수 있다. 이에 따라, 매니퓰레이터(100)는 사용자 팔(1)의 상박의 움직임을 따라할 수 있다.The processor 160 determines the final yaw angle (

), it is possible to control the first motor 131 based on. Also, the processor 160 may control driving of the second motor 132 based on the obtained pitch angle. Accordingly, the manipulator 100 can follow the movement of the upper arm of the user's arm 1 .

In the above, the method of controlling the driving of the first motor 131 and the second motor 132 for rotating the first link 111 corresponding to the upper arm of the user's arm 1 has been described. Hereinafter, an operation of the processor 160 for controlling driving of the third motor 133 and the fourth motor 134 for rotating the second link 112 corresponding to the lower arm of the user's arm 1 will be described. let it do

5 is a diagram for explaining a method of controlling driving of a motor according to an embodiment of the present disclosure.

)와 제2 링크(112)의 방향에 대응되는 제 2-2-2 벡터(

)가 정의될 수 있다. 제1 각도(

)는 제1 링크(111) 및 제2 링크(112)의 방향을 나타내는 제3 벡터(

)와 제4 벡터(

)의 사잇각을 의미한다. Referring to FIG. 5, the 2-1-1 vector corresponding to the direction of the first link 111 (

) and the 2-2-2 vector corresponding to the direction of the second link 112 (

) can be defined. The first angle (

) Is a third vector representing the direction of the first link 111 and the second link 112 (

) and the fourth vector (

) means the angle between

)를 산출하기 위해 다음의 두 벡터를 정의한다. 먼저 제4 벡터(

)를 제3 벡터(

)방향으로 정사영한 제5 벡터(

)가 정의된다. 제 5 벡터(

)를 중심으로 하고 반지름이 제4 벡터(

) 및 제5 벡터(

) 사이의 거리인 원이 제5 벡터 (

)와 수직인 평면 상에 존재할 수 있다. 이 때, 원을 이루는 평면과 항법 좌표계에서 Z축 벡터와 제5 벡터(

)를 포함하는 평면의 교선 벡터로부터 제6 벡터(

)가 정의된다. 제2 각도(

)는 제5 벡터(

)와 제6 벡터(

)의 사잇각을 의미한다. The second angle (

), the following two vectors are defined. First, the fourth vector (

) to the third vector (

), the fifth vector projected in the direction (

) is defined. The fifth vector (

) as the center and the radius is the fourth vector (

) and the fifth vector (

) is the fifth vector (

) and may exist on a plane perpendicular to At this time, the Z-axis vector and the fifth vector (

) From the intersection vector of the plane containing the sixth vector (

) is defined. The second angle (

) is the fifth vector (

) and the sixth vector (

) means the angle between

)에 기초하여 제4 모터(134)의 구동을 제어하며, 제2 각도(

)에 기초하여 제3 모터(133)의 구동을 제어할 수 있다. 이하에서는, 제1 각도(

) 및 제2 각도(

)를 산출하기 위한 프로세서(160)의 동작에 대하여 살펴본다.The processor 160 has a first angle (

) and controls the driving of the fourth motor 134 based on the second angle (

), it is possible to control the driving of the third motor 133 based on. Below, the first angle (

) and the second angle (

Let's look at the operation of the processor 160 for calculating ).

) 및 제4 벡터(

)를 획득할 수 있다. 제3 벡터(

)는 도 3의 제2-1 벡터(

)와 동일한 바, 그 상세한 설명은 생략한다. 또한, 제4 벡터(

)는 제2-1 벡터(

)와 동일한 방법으로 획득될 수 있다. 구체적으로, 프로세서(160)는 사용자 팔(1)의 하박에 부착된 제2 외부 센서(12)의 센싱 값을 획득할 수 있다. 프로세서(160)는 AHRS 알고리즘을 이용하여 제2 외부 센서(12)의 센싱 값에 대응되는 쿼터니언 정보를 획득할 수 있다. 프로세서(160)는 [수학식 3]에 기초하여 제2 외부 센서(12)의 센싱 값에 대응되는 좌표변환행렬을 획득할 수 있다. 프로세서(160)는 [수학식 4]에 기초하여 제4 벡터(

)를 획득할 수 있다. The processor 160 generates a third vector (

) and the fourth vector (

) can be obtained. The third vector (

) is the 2-1 vector of FIG. 3 (

), the detailed description thereof is omitted. In addition, the fourth vector (

) is the 2-1 vector (

) can be obtained in the same way as Specifically, the processor 160 may obtain a sensing value of the second external sensor 12 attached to the lower arm of the user's arm 1 . The processor 160 may obtain quaternion information corresponding to the sensed value of the second external sensor 12 by using the AHRS algorithm. The processor 160 may obtain a coordinate conversion matrix corresponding to the sensed value of the second external sensor 12 based on [Equation 3]. Based on [Equation 4], the processor 160 generates a fourth vector (

) can be obtained.

) 및 제4 벡터(

)의 내적에 기초하여 제2 링크(112)의 제1 각도(

)를 획득할 수 있다. 구체적으로, 프로세서(160)는 [수학식 7]에 기초하여 제1 각도(

)를 획득할 수 있다.The processor 160 generates a third vector (

) and the fourth vector (

Based on the dot product of ), the first angle of the second link 112 (

) can be obtained. Specifically, the processor 160, based on [Equation 7], the first angle (

) can be obtained.

[Equation 7]

6 is a diagram for explaining a method of obtaining a second angle according to an embodiment of the present disclosure.

)를 획득할 수 있다. The processor 160 calculates a second angle based on [Equation 8] and [Equation 9].

) can be obtained.

[Equation 8]

[Equation 9]

Here, [Equation 8] and [Equation 9] may be derived by the geometric relationship shown in FIG. 6 .

)는 사용자 팔(1)의 상박의 자세가 반영된 하박의 자세(

)를 [수학식 9-2]와 같이 보정함으로써 획득될 수 있다.The posture of the back of the hand of the user's arm 1 is influenced by the posture of the upper arm and lower arm of the user's arm 1, and the posture of the lower arm of the user's arm 1 is influenced by the posture of the upper arm of the user's arm 1. receive Therefore, the posture of the back of the hand independent of the posture of the upper arm and lower arm of the user's arm 1 (

) is the posture of the lower arm in which the posture of the upper arm of the user's arm 1 is reflected (

) can be obtained by correcting [Equation 9-2].

[Equation 9-2]

및

라고 할 때 두 회전각은 [수학식 9-3]으로 표현된다.In FIG. 1B, the rotation angles of the fifth motor 135 and the sixth motor 136 for expressing the roll and pitch of the back of the hand

and

When it is said, the two rotation angles are expressed by [Equation 9-3].

[Equation 9-3]

)을 이용하여 오차를 보정할 수 있다. 구체적으로, 프로세서(160)는 [수학식 10]에 기초하여 오차가 보정된 좌표변환행렬(

)을 획득할 수 있다. 그리고, 프로세서(160)는 전술한 제2 벡터(

), 제3 벡터(

) 및 제4 벡터(

)를 획득할 때, 오차가 보정된 좌표변환행렬(

)을 이용할 수 있다.Meanwhile, the sensing value of the external sensor 10 attached to the user's arm 1 may include an error due to differences in physical characteristics (eg, muscle mass, skin curvature, etc.) for each user. The processor 160 generates an initial coordinate transformation matrix (for example, an operation of extending an arm horizontally to the ground) of a user's initial setting posture.

) can be used to correct the error. Specifically, the processor 160 is a coordinate transformation matrix in which the error is corrected based on [Equation 10] (

) can be obtained. And, the processor 160 uses the aforementioned second vector (

), the third vector (

) and the fourth vector (

), the error-corrected coordinate transformation matrix (

) can be used.

[Equation 10]

는 외부 센서(10)의 현재 센싱 값에 기초하여 획득되는 좌표변환행렬을 의미한다. 또한, 프로세서(160)는 사용자가 초기 설정 자세를 취할 때 획득되는 외부 센서(10)의 센싱 값과 [수학식 3]에 기초하여 초기 좌표변환행렬(

)을 획득하여 메모리(150)에 저장할 수 있다. here,

denotes a coordinate conversion matrix obtained based on the current sensing value of the external sensor 10 . In addition, the processor 160 performs an initial coordinate transformation matrix (based on the sensing value of the external sensor 10 obtained when the user assumes the initial setting posture and [Equation 3]).

) may be acquired and stored in the memory 150.

Meanwhile, the processor 160 may apply an IIR filter to motor control information for motion smoothing of the manipulator 100 . The processor 160 may adjust the size of the control input applied to the motor 130 based on [Equation 11].

[Equation 11]

는 모터(130)의 입력으로 사용되는 필터의 출력 값을,

는 현재 시점 바로 이전의 모터 제어 입력, M은 IIR 필터에 의해 필터링 되기 전 현재 시점에서 추정된 모터 제어 입력을 의미한다. TH는 모터(130)의 입력을 제한하기 위한 기설정된 값(예로, 10 degrees)이며, k는 미리 정해진 값(예로, 0.7)을 의미한다.here,

is the output value of the filter used as the input of the motor 130,

is the motor control input right before the current time, and M is the motor control input estimated at the current time before being filtered by the IIR filter. TH is a predetermined value (eg, 10 degrees) for limiting the input of the motor 130, and k means a predetermined value (eg, 0.7).

Meanwhile, the processor 160 may control driving of the fifth motor 135 and the sixth motor 136 to control the movement of the hand 120 . For example, the processor 160 may obtain posture information of the user's hand based on a sensing value of the third external sensor 13 attached to the wrist of the user's arm 1 . Also, the processor 160 may control driving of the fifth motor 135 and the sixth motor 136 based on the acquired attitude information.

)을 획득하고, 보정된 센싱값(

)에 기초하여 핑거의 동작을 제어할 수 있다.Also, the processor 160 may control operations of fingers included in the hand 120 . The processor 160 may obtain posture information of the user's finger based on the sensing value of the flex sensor 14 received through the communication interface 140 . Also, the processor 160 may control the operation of the finger based on the acquired posture information. Meanwhile, the sensing value of the flex sensor 14 may include an error due to a difference in the shape or size of the user's hand. The processor 160 detects the error corrected based on [Equation 12] (

) is obtained, and the corrected sensing value (

), it is possible to control the operation of the finger.

[Equation 12]

는 현재 시점에서 획득된 플렉스 센서(14)의 센싱값,

는 메모리(150)에 저장된 플렉스 센서(14)의 센싱값 중 최소값,

는 메모리(150)에 저장된 플렉스 센서(14)의 센싱값 중 최대값을 의미한다.here,

Is the sensing value of the flex sensor 14 obtained at the current time point,

Is the minimum value of the sensing values of the flex sensor 14 stored in the memory 150,

Means the maximum value of the sensed values of the flex sensor 14 stored in the memory 150.

The processor 160 may accumulate and store the sensed values of the flex sensor 14 in the memory 150 . In particular, the sensed values of the flex sensor 14 corresponding to the user's initially set posture (eg, fist clenched posture or open palm posture) may be accumulated and stored in the memory 150 . Also, the processor 160 may identify the minimum and maximum values among the stored sensing values of the flex sensor 14 .

7 is a flowchart illustrating a method of controlling a manipulator according to an embodiment of the present disclosure.

Referring to FIG. 7 , the manipulator 100 may receive a sensing value of an external sensor for detecting the posture of the user's arm (S710). Here, the external sensors may include a first external sensor attached to the upper arm of the user's arm, a second external sensor attached to the lower arm, a third external sensor attached to the wrist, and a flex sensor attached to the finger.

The manipulator 100 may obtain a second vector corresponding to the posture of the user's arm based on the matrix obtained based on the sensed value of the external sensor and the first vector pre-stored in the memory (S720). Specifically, the manipulator 100 may obtain a quaternion vector corresponding to the posture of the user's arm by applying the sensed value to an Altitude and Heading Reference System (AHRS) algorithm stored in the manipulator 100. Also, the manipulator 100 may obtain a matrix based on the quaternion vector (quaternion information). For example, the manipulator 100 may obtain a matrix based on [Equation 3].

Meanwhile, the obtained matrix may be a value obtained by correcting an error according to a difference in physical characteristics of each user. For example, the manipulator 100 may obtain a first matrix based on a first sensing value corresponding to an initially set posture of the user's arm and store the first matrix in the manipulator 100 . Also, the manipulator 100 may obtain a corrected matrix based on the second matrix obtained based on the second sensing value corresponding to the current posture of the user's arm and the stored first matrix.

Meanwhile, the second vector may include a 2-1 vector corresponding to the x-axis and a 2-2 vector corresponding to the z-axis. In addition, the posture information of the user's arm may include a roll angle corresponding to the x-axis, a pitch angle corresponding to the y-axis, and a yaw angle corresponding to the z-axis. The manipulator 100 may obtain the first yaw angle based on the 2-1 vector. The manipulator 100 may obtain the second yaw angle based on the 2-2 vector. Also, the manipulator 100 may obtain the yaw angle by applying weights to the first yaw angle and the second yaw angle based on a predefined weight function. The manipulator 100 may obtain the yaw angle based on [Equation 5] and [Equation 6] described above.

Meanwhile, the manipulator 100 may obtain posture information of the lower arm of the user's arm and control the movement of the second link 112 based on the posture information of the lower arm of the user's arm. The manipulator 100 may obtain a third vector corresponding to the first link 111 based on a sensing value of a first external sensor attached to the upper arm of the user's arm. Also, the manipulator 100 may obtain a fourth vector corresponding to the second link 112 based on a sensing value of a second external sensor attached to the lower arm of the user's arm.

Also, the manipulator 100 may obtain posture information corresponding to the second link 112 based on the third vector and the fourth vector. For example, the manipulator 100 may obtain posture information corresponding to the second link 112 based on [Equation 7], [Equation 8], and [Equation 9] described above. The manipulator 100 may control driving of a plurality of motors corresponding to the second link 112 based on posture information corresponding to the second link 112 .

In the above, a method for acquiring motor input values corresponding to the upper arm and lower arm of the manipulator from quaternion information corresponding to the upper arm and lower arm of the user has been described. Hereinafter, a method for estimating a hand posture independent of the postures of the upper arm and lower arm of the manipulator according to another embodiment of the present disclosure will be described.

8 is a flowchart illustrating a control method of a manipulator according to an embodiment of the present disclosure.

All quaternion information estimated from the upper arm, lower arm, or hand of the user represents the posture of an independent body coordinate system centered on the reference coordinate system. This means that if the Euler angle is calculated from quaternion information estimated for each part, the posture of the moving body coordinate system can be matched to the posture of the reference coordinate system through coordinate transformation.

If the posture of the manipulator's hand is independent of the posture of the manipulator's upper arm and lower arm, as in the case of the actual hand posture of the user, the posture of the manipulator's hand can be determined only with quaternion information estimated from the user's hand, which is This means that the posture of the manipulator's hand can be easily implemented.

However, the posture of the manipulator hand changes according to the posture of the manipulator lower arm, and the posture of the manipulator lower arm changes according to the posture of the manipulator upper arm. That is, it can be said that the posture of the manipulator's hand is dependent on the postures of the upper arm and lower arm of the manipulator. Therefore, in order to accurately control the posture of the manipulator hand, the posture of the upper arm and the lower arm of the manipulator should be considered.

Therefore, the manipulator according to the present disclosure can estimate the posture of the manipulator's upper arm and lower arm and the posture of the hand independently, and this process may be referred to as a hand balancing process. Hereinafter, an embodiment related to a hand balancing process according to the present disclosure will be described.

In the embodiments described below, as in the previous embodiments, it is assumed that the manipulator according to the present disclosure includes the configuration shown in FIG. 1B. Specifically, the manipulator may include a plurality of links, a plurality of motors, a communication interface, a memory, and a processor.

The plurality of links include a first link corresponding to the upper arm, a second link corresponding to the lower arm, and a third link corresponding to the hand, and the plurality of motors rotate the first link about the first axis. A motor, a second motor for rotating the first link with respect to the second axis, a third motor for rotating the second link with respect to the first axis, a fourth motor for rotating the second link with respect to the third axis, and a third link It may include a sixth motor for rotating the fifth motor and the third link relative to the first axis for rotating the second axis.

Since a detailed description of the configuration of other manipulators has been previously described, duplicate descriptions of the same content will be omitted.

Referring to FIG. 8 , the manipulator may obtain information about the body coordinate system of the link corresponding to the lower arm based on first rotation angle information of motors corresponding to the upper arm and lower arm among a plurality of motors (S810). .

Specifically, the manipulator calculates a coordinate transformation matrix for converting the first rotation angle information into sensing values representing the posture of the upper arm and the posture of the lower arm, and obtains information on the body coordinate system corresponding to the lower arm based on the coordinate transformation matrix. can do.

In other words, the manipulator does not use quaternion information corresponding to the upper arm and lower arm, and calculates a coordinate transformation matrix inversely based on the first rotation angle information, which is an actual motor input value for the motors corresponding to the upper arm and lower arm. Based on this, it is possible to obtain information on a body coordinate system representing a posture of a link corresponding to an actual lower arm. This is to reduce an error occurring when the high-dimensional posture information expressed in the user's arm is reduced in dimension to the low-dimensional posture expressed in the link corresponding to the manipulator's arm.

When the information on the body coordinate system of the link corresponding to the lower arm is obtained, the manipulator may obtain balance angle information for equilibrating the body coordinate system with a predefined reference coordinate system (S820).

Here, the balance angle information may include roll balance angle information and pitch balance angle information. In addition, the roll equilibrium angle information may indicate an angle at which a second axis of the body coordinate system is parallel to the xy plane of the reference coordinate system when the body coordinate system is rotated based on the first axis of the body coordinate system, and the pitch equilibrium angle information is the roll equilibrium angle information. When the body coordinate system is rotated based on the second axis of the body coordinate system rotated according to the equilibrium angle information, an angle at which the third axis of the body coordinate system coincides with the z-axis of the reference coordinate system may be indicated. Here, the first axis, the second axis, and the third axis of the moving body coordinate system are used as terms for specifying the x-axis, y-axis, and z-axis of the moving body coordinate system by distinguishing them from the x-axis, y-axis, and z-axis of the reference coordinate system.

Specifically, when the manipulator rotates the lower arm body coordinate system based on the first axis of the body coordinate system based on the posture information of the lower arm body coordinate system expressed around the reference coordinate system, the second axis is included in the xy plane of the reference coordinate system. The roll equilibrium angle can be calculated.

When the roll equilibrium angle is calculated, the manipulator rotates the lower arm body coordinate system based on the second axis based on the posture information of the newly created lower arm body coordinate system when the lower arm body coordinate system is rotated by the roll equilibrium angle with respect to the first axis. The pitch equilibrium angle can be calculated so that the three axes coincide with the z-axis of the reference coordinate system.

After the pitch equilibrium angle is calculated, when the manipulator is rotated by the pitch equilibrium angle around the second axis of the lower arm body coordinate system, the plane composed of the first axis and the second axis of the lower arm body coordinate system becomes perpendicular to the earth's gravitational acceleration vector, and thus the hand Balancing can be done. This means that independent hand postures can be obtained regardless of the rotation angles of the motors that drive the upper arm and lower arm of the manipulator.

A process of calculating the roll equilibrium angle and the pitch equilibrium angle will be described in more detail with reference to FIGS. 8 to 10 .

When a sensing value representing the hand position is received from an external sensor after the equilibrium angle information is acquired, the manipulator generates second rotation angle information for motors corresponding to the hand among a plurality of motors based on the sensing value and the equilibrium angle information. It can be obtained (S830). When the second rotation angle information is acquired, the manipulator may control motors corresponding to the hand based on the second rotation angle information (S840).

Specifically, the manipulator obtains third rotation angle information for motors corresponding to the hand based on a sensing value representing the posture of the hand, and corrects the third rotation angle information based on the equilibrium angle information to obtain the second rotation angle information. information can be obtained. More specifically, the manipulator obtains second rotation angle information according to a result of correcting the third rotation angle information by adding equilibrium angle information to third rotation angle information for motors corresponding to the hand, and obtains second rotation angle information according to a result of correcting the third rotation angle information. Motors corresponding to the hand may be controlled based on the rotation angle information.

According to the embodiment described above with reference to FIG. 8 , when the postures of the upper arm and lower arm of the user's arm are arbitrarily given in consideration of the postures of the manipulator's upper arm and lower arm, the degree to which the hand posture is distorted with respect to the reference coordinate system is compensated. By performing hand balancing, it is possible to minimize a posture error generated from an external sensor attached to the user's arm and estimate a hand posture independent of the postures of the upper arm and lower arm of the manipulator. And, accordingly, the user can quickly and precisely perform the desired task.

9 is a view for explaining a coordinate system according to an embodiment of the present disclosure and a process of obtaining information on a moving body coordinate system of a link corresponding to a lower arm.

The coordinate systems shown in FIG. 9 are a reference frame according to the present disclosure and body coordinate systems corresponding to each of a plurality of links included in the manipulator, that is, a body coordinate system corresponding to the user's upper arm, lower arm (fore arm). The body coordinate system corresponding to the arm and the body coordinate system corresponding to the hand are respectively indicated.

Here, the yaw angle of the reference coordinate system is defined as a coordinate system that moves equally according to the yaw angle of the upper arm, and all body coordinate systems are interpreted centering on the reference coordinate system. And, according to the yaw and pitch angles corresponding to the rotation of the third axis and the second axis in the body coordinate system of the manipulator upper arm, and the roll angle and yaw angle corresponding to the rotation of the first and third axes in the body coordinate system of the manipulator lower arm, the upper arm of the manipulator and the posture of the lower arm is determined.

On the other hand, as described above, in order to accurately control the posture of the manipulator hand, the posture of the upper arm and lower arm of the manipulator should be considered. According to the prior art, quaternion information obtained from the IMU and geomagnetic sensor mounted on the user's hand to obtain the coordinate transformation matrix between the body coordinate system of the hand and the reference coordinate system, and the inverse matrix of the coordinate transformation matrix between the body coordinate system and the reference coordinate system Then, by multiplying the coordinate transformation matrix of the hand by the inverse matrix of the coordinate transformation matrix obtained from the lower arm, the relative posture of the hand with respect to the lower arm can be obtained.

However, according to the prior art, an error may occur when the posture of the IMU and the geomagnetic sensor attached to the user's arm is changed while the user bends and straightens the arm or raises and lowers the arm. For example, when the user bends the arm and muscles of the upper arm are united, the tilt of the surface on which the sensor is mounted changes, and this phenomenon is reflected in the quaternion information and may appear as an error in the posture of the upper arm. Even in the case of the lower arm, a posture error may occur due to a similar reason as the case of the upper arm. Further, the posture error of the upper arm and the lower arm causes the hand posture error, and as a result, it is difficult for the user to perform an intuitive and fast operation according to the difference between the actual hand posture and the manipulator's hand posture.

The present disclosure is to solve the problems of the prior art as described above. According to the present disclosure, when obtaining the posture of the upper arm and the lower arm, quaternion information obtained from the IMU and the geomagnetic sensor mounted on the upper arm and the lower arm are not used, and the actual manipulator's By using the quaternion information obtained based on the rotation angles of the motors mounted on the upper and lower arms, the manipulator's hand can maintain equilibrium in the reference coordinate system. This means that the posture of the manipulator's hand is affected only by quaternion information obtained from the IMU and geomagnetic sensor attached to the user's hand.

Hereinafter, referring to [Equation 13], [Equation 14], and [Equation 15], the x, y, and z-axis unit vectors of the lower body coordinate system are respectively X=[1,0,0] ^T , Y When =[0,1,0] ^T , Z=[0,0,1] ^T , a method for representing the change in unit vector according to the change in posture of the upper arm and lower arm in the reference coordinate system will be described. Later, with reference to FIGS. 10 and 11, a process of obtaining a roll equilibrium angle and a process of obtaining a pitch equilibrium angle according to the present disclosure will be described.

[Equation 13] shows a process of obtaining transformed vectors X1, Y1, and Z1 when unit vectors X, Y, and Z are coordinately transformed through rotation of the yaw angle of the lower arm.

[Equation 13]

X1=C1*X

Y1=C1*Y

Z1=C1*Z

[Equation 14] shows a process of obtaining transformed vectors X2, Y2, Z2 when coordinate transformation is performed on vectors X1, Y1, and Z1 through rotation of the roll angle of the upper arm.

[Equation 14]

X2=C2*X1

Y2=C2*Y1

Z2=C2*Z1

[Equation 15] represents transformed vectors X3, Y3, and Z3 when coordinate transformation is performed on vectors X2, Y2, and Z2 through rotation of the pitch angle of the upper arm.

[Equation 15]

X3=C3*X2

Y3=C3*Y2

Z3=C3*Z2

In conclusion, the vectors X3, Y3, and Z3 are the result of analyzing X, Y, and Z, which are unit vectors of the body coordinate system of the manipulator lower arm, in the reference coordinate system through coordinate transformation, and are shown in FIG. 10. When the rotation amount of the two motors corresponding to the manipulator's hand is 0, the body coordinate system of the lower arm is always the same as the body coordinate system of the back of the hand. Therefore, in order to keep the body coordinate system of the hand in an equilibrium state in the reference coordinate system, it is necessary to find the equilibrium conditions of X3, Y3, and Z3 of the body coordinate system of Habak viewed from the reference coordinate system. This will be described with reference to FIGS. 10 and 11 below.

10 is a diagram for explaining in detail a process of obtaining a roll equilibrium angle according to an embodiment of the present disclosure.

First, in the case of coordinate rotation around the vector X3 in FIG. 10, a roll equilibrium angle (roll_hand in FIG. 10) can be defined such that the vector Y3 exists on the XY plane of the reference coordinate system. That is, the roll equilibrium angle is the angle formed by the vector Y3 and the vector Vinter of the intersection of the Y3Z3 plane and the XY plane. The intersection vector Vinter can be calculated according to [Equation 16] below. [Equation 16] includes a plurality of equations and an operation process for them.

[Equation 16]

Equation in the XY plane: z=0

Equation of the Y3Z3 plane: X3[0]*x+X3[1]*y+X3[2]*z=0 (however, X3=[X3[0], X3[1], X3[2]])

Combining the above two equations,

X3[0]*x+X3[1]*y=0 & z=0,

So Vinter=[-X3[1], X3[0], 0].

And, the roll equilibrium angle can be calculated according to [Equation 17] below. [Equation 17] includes a plurality of equations and an operation process for them.

[Equation 17]

Let Vcross be the cross product of vector Y3 and vector Vinter,

Vcross = cross_product (Y3, Vinter).

Defining |V| as the magnitude of vector V,

if(Vcross[0]>=0)

roll_hand= asin(|Vcross| / |Y3| / |Vinter|)

else

roll_hand= -1.0*asin(|Vcross| / |Y3| / |Vinter|)

if(X3[0]<0)

roll_hand=-1.0* roll_hand

11 is a diagram for explaining in detail a process of obtaining a pitch equilibrium angle according to an embodiment of the present disclosure.

As shown in FIG. 11, when the vector Y3 and the vector Z3 are rotated by the rotation angle -1.0 * roll_hand around the vector X3, if they are converted into vectors Y4 and Z4, respectively, the vector Y4 exists on the XY plane of the reference coordinate system do.

Here, if the angle formed by the vector Z4 and the unit vector Z of the reference coordinate system is defined as the pitch equilibrium angle (pitch_hand in FIG. 11), when the vector X3 and the vector Z4 are rotated by -1.0 * pitch_hand with respect to the vector Y4, the vector Z4 The direction of the vector transformed through the rotation of is coincident with the direction of the Z axis of the reference coordinate system, and the vector transformed through the rotation of the vector X3 exists on the XY plane of the reference coordinate system. Accordingly, the hand balancing process according to the present disclosure may be performed based on the roll equilibrium angle and the pitch equilibrium angle.

Here, the case where the vector Z3 coincides with the vector Z4 by rotating the vector X3 by the rotation angle -1.0 * roll_hand as the center is expressed using the definition of quaternion information as shown in [Equation 18] below. [Equation 18] includes a plurality of equations and an operation process for them.

[Equation 18]

If quaternion information is defined as Q = [Q[0], Q[1], Q[2], Q[3]],

The rotation vector of the quaternion information is Xu=X3/|X3| and the rotation angle is the roll equilibrium angle.

Q=[cos(roll_hand/2), sin(roll_hand/2)*Xu[0], sin(roll_hand/2)*Xu[1], sin(roll_hand/2)*Xu[2] ]

When quaternion information is given, the coordinate transformation matrix Cq is calculated from it, and the rotation transformation vector Z4 of the vector Z3 can be obtained. [Equation 19] below shows this process. [Equation 19] includes a plurality of equations and an operation process for them.

[Equation 19]

Cq[0][0] = Q[0]*Q[0]+Q[1]*Q[1]-Q[2]*Q[2]-Q[3]*Q[3]

Cq[0][1] = 2.0*(Q[1]*Q[2]-Q[0]*Q[3])

Cq[0][2] = 2.0*(Q[1]*Q[3]+Q[0]*Q[2])

Cq[1][0] = 2.0*(Q[1]*Q[2]+Q[0]*Q[3])

Cq[1][1] = Q[0]*Q[0]-Q[1]*Q[1]+Q[2]*Q[2]-Q[3]*Q[3]

Cq[1][2] = 2.0*(Q[2]*Q[3]-Q[0]*Q[1])

Cq[2][0] = 2.0*(Q[1]*Q[3]-Q[0]*Q[2])

Cq[2][1] = 2.0*(Q[2]*Q[3]+Q[0]*Q[1])

Cq[2][2] = Q[0]*Q[0]-Q[1]*Q[1]-Q[2]*Q[2]+Q[3]*Q[3]

Z4=Cq*Z3

Finally, the pitch equilibrium angle may be calculated according to [Equation 20] below. [Equation 20] includes a plurality of equations and an operation process for them.

[Equation 20]

Vcross=cross_product(Z, Z4)

Here, Vcross=[Vcross[0], Vcross[1], Vcross[2]] means the cross product of Z and Z4.

if(Vcross[0]>=0)

pitch_hand = asin(|Vcross| / |Z4| )

else

pitch_hand = -1.0*asin(|Vcross| / |Z4|)

When the roll balance angle and the pitch balance angle are calculated according to [Equation 17] and [Equation 20], the manipulator rotates the two motors corresponding to the hand by the roll balance angle and the pitch balance angle to perform hand balancing. .

12 is a diagram for explaining an embodiment related to a process of controlling a manipulator by a plurality of users according to an embodiment of the present disclosure.

1 to 11, the process of acquiring the input values of the motors corresponding to the upper arm and lower arm and the hand balancing process described above can be performed even when a plurality of users remotely control one manipulator. can be applied

Referring to FIG. 12, the manipulator may receive an operation stop command from the first user while the manipulator is operating (S1210). Also, when the first user's operation stop command is received, the manipulator deactivates an external sensor for recognizing the first user's arm posture (S1220) and stops the manipulator operation (S1230).

Specifically, the manipulator may deactivate the external sensor by transmitting a control signal for deactivating the external sensor to the external sensor, and even if a sensing value is received from the external sensor after an operation stop command, the manipulator operates based on the received sensing value. It is possible to stop the operation of the manipulator in such a way as not to control it.

When the second user's command to start an operation for controlling the manipulator is received (S1240), the manipulator transmits information on a guide screen for compensating for a difference between the posture of the manipulator and the posture of the arm of the second user to the second user. It can be transmitted to the user terminal (S1250).

Specifically, the manipulator transmits information on the guide screen to the user terminal of the second user so that it is displayed on the display of the user terminal of the second user.

Here, the guide screen may include information about the posture of the manipulator according to the last posture of the arm of the first user and information about the current posture of the arm of the second user. You can change the motion of your arm to match the posture of your arm with the posture of the manipulator according to the last posture of .

If the difference between the posture of the manipulator and the posture of the arm of the second user is identified as being within the threshold range (S1260-Y), the manipulator may control the operation of the manipulator based on the posture of the arm of the second user (S1270). Meanwhile, the manipulator continuously determines whether the difference between the posture of the manipulator and the posture of the arm of the second user is within the threshold range until it is identified that the difference between the posture of the manipulator and the posture of the arm of the second user is within the threshold range. It can be identified periodically (S1260-N).

Accordingly, in the manipulator according to the present disclosure, even when a plurality of users remotely control the manipulator, the plurality of users can quickly understand the movement situation of the manipulator and intuitively attempt correct control, and accordingly, the plurality of users It is possible to stably operate the manipulator while ensuring continuity between operations. Meanwhile, the embodiment of FIG. 12 may be similarly applied even when one user controls manipulators located in several places.

13 is a diagram for explaining an embodiment related to controlling a repetitive motion of a manipulator according to an embodiment of the present disclosure.

1 to 11 , the process of acquiring the input values of the motors corresponding to the upper arm and lower arm and the hand balancing process described above can be applied to a process in which the manipulator repeatedly performs a certain motion. .

The manipulator may receive a user's command to start repeating motion (S1310). Also, when the user's command to start a repeated motion is received, the manipulator may control the motion of the manipulator based on the posture of the user's arm (S1320).

When the user's command to stop the repeated motion is received after the manipulator starts operating (S1330), the manipulator stores control signals corresponding to the manipulator's motions from the time the command to start the repeated motion is received to the time the command to stop the repeated motion is received. It can (S1340).

When the control signal corresponding to the operation of the manipulator is stored, the manipulator may transmit information on the maximum work speed of each motor of the manipulator corresponding to the control signal to the user terminal (S1350). Specifically, the manipulator transmits information on the maximum working speed of each motor of the manipulator corresponding to the control signal to the user's terminal, so that it is displayed on the display of the user's terminal.

When a user input for setting the working speed of the manipulator based on the information on the maximum working speed is received (S1360), the manipulator can control the manipulator based on the set working speed (S1370).

That is, the user can designate the repetition speed of the manipulator within the maximum permissible torque range of each motor as needed, and accordingly, the user's work efficiency can be remarkably improved.

Meanwhile, various embodiments described above may be implemented in a recording medium readable by a computer or a similar device using software, hardware, or a combination thereof. In some cases, the embodiments described herein may be implemented in a processor itself. According to software implementation, embodiments such as procedures and functions described in this specification may be implemented as separate software modules. Each of the software modules may perform one or more functions and operations described herein.

Meanwhile, computer instructions for performing processing operations according to various embodiments of the present disclosure described above may be stored in a non-transitory computer-readable medium. Computer instructions stored in such a non-transitory computer readable medium may cause a specific device to perform processing operations according to various embodiments described above when executed by a processor.

A non-transitory computer readable medium is a medium that stores data semi-permanently and is readable by a device, not a medium that stores data for a short moment, such as a register, cache, or memory. Specific examples of the non-transitory computer readable media may include CD, DVD, hard disk, Blu-ray disk, USB, memory card, ROM, and the like.

Although the preferred embodiments of the present disclosure have been shown and described above, the present disclosure is not limited to the specific embodiments described above, and is commonly used in the technical field belonging to the present disclosure without departing from the gist of the present disclosure claimed in the claims. Of course, various modifications and implementations are possible by those with knowledge of, and these modifications should not be individually understood from the technical spirit or perspective of the present disclosure.

100: manipulator 110: link
120: hand 130: motor
140: communication interface 150: memory
160: processor

Claims (20)

In the manipulator,
a plurality of links corresponding to each of the user's upper arm, lower arm, and hand;
A plurality of motors for rotating the plurality of links;
communication interface;
a memory storing at least one instruction; and
a processor to execute the at least one instruction; including,
the processor,
Obtaining information about a body coordinate system of a link corresponding to the lower arm based on first rotation angle information of motors corresponding to the upper arm and the lower arm among the plurality of motors;
Obtaining equilibrium angle information for equilibrating the body coordinate system with a predefined reference coordinate system;
When a sensing value representing the posture of the hand is received from an external sensor through the communication interface, second rotation angle information for motors corresponding to the hand among the plurality of motors based on the sensing value and the balance angle information to obtain,
Controlling motors corresponding to the hand based on the second rotation angle information
manipulator. According to claim 1,
the processor,
Calculating a coordinate transformation matrix for converting the first rotation angle information into sensed values representing the posture of the upper arm and the posture of the lower arm;
Obtaining information on the body coordinate system corresponding to the lower arm based on the coordinate transformation matrix
manipulator. According to claim 1,
The equilibrium angle information includes roll equilibrium angle information and pitch equilibrium angle information,
The roll equilibrium angle information indicates an angle at which a second axis of the body coordinate system is parallel to the xy plane of the reference coordinate system when the body coordinate system is rotated based on the first axis of the body coordinate system.
manipulator. According to claim 3,
The pitch equilibrium angle information is an angle at which the third axis of the body coordinate system coincides with the z-axis of the reference coordinate system when the body coordinate system is rotated based on the second axis of the body coordinate system rotated according to the roll equilibrium angle information. representing
manipulator. According to claim 4,
the processor,
Obtaining third rotation angle information for motors corresponding to the hand based on a sensed value representing the posture of the hand;
Obtaining second rotation angle information by correcting third rotation angle information based on the equilibrium angle information
manipulator. According to claim 5,
The plurality of links include a first link corresponding to the upper arm, a second link corresponding to the lower arm, and a third link corresponding to the hand;
The plurality of motors include a first motor for rotating the first link about a first axis, a second motor for rotating the first link about a second axis, and a rotation of the second link about the first axis. A third motor, a fourth motor for rotating the second link about a third axis, a fifth motor for rotating the third link about the first axis, and a rotation of the third link about the second axis Including a sixth motor
manipulator. According to claim 1,
the processor,
If a first user's stop command is received while the manipulator is operating, an external sensor for recognizing the posture of the arm of the first user is deactivated and the manipulator stops operating;
When the second user's command to start an operation for controlling the manipulator is received, information on a guide screen for compensating for a difference between the posture of the manipulator and the posture of the arm of the second user is transmitted to the user terminal of the second user. Control the communication interface to transmit to,
Controlling the plurality of motors based on the posture of the arms of the second user when it is identified that the difference between the posture of the manipulator and the posture of the arms of the second user is within a preset threshold range.
manipulator. According to claim 1,
the processor,
Controlling the plurality of motors based on the posture of the user's arm when the user's command to start the repetitive motion is received;
When the user's command to stop the repeated operation is received, a control signal corresponding to the manipulator operation from the time the command to start the repeated operation is received to the time the command to stop the repeated operation is received is stored in the memory;
When the control signal is stored, controlling the communication interface to transmit information on the maximum working speed for each of the plurality of motors corresponding to the control signal to the user terminal of the user,
Controlling the plurality of motors based on the set working speed when a user input for setting the working speed of the manipulator based on the information on the maximum working speed is received.
manipulator. A control method of a manipulator including a plurality of links corresponding to each of a user's upper arm, lower arm, and hand and a plurality of motors rotating the plurality of links,
obtaining information about a moving body coordinate system of a link corresponding to the lower arm based on first rotation angle information of motors corresponding to the upper arm and the lower arm among the plurality of motors;
obtaining equilibrium angle information for equilibrating the body coordinate system with a predefined reference coordinate system;
obtaining second rotation angle information for motors corresponding to the hand among the plurality of motors based on the sensed value and the balance angle information when a sensed value representing the posture of the hand is received from an external sensor; and
Controlling motors corresponding to the hand based on the second rotation angle information; comprising
Manipulator control method. According to claim 9,
The step of obtaining information on the moving body coordinate system,
calculating a coordinate conversion matrix for converting the first rotation angle information into sensed values representing the posture of the upper arm and the posture of the lower arm; and
obtaining information on a body coordinate system corresponding to the lower arm based on the coordinate transformation matrix; containing
Manipulator control method. According to claim 9,
The equilibrium angle information includes roll equilibrium angle information and pitch equilibrium angle information,
The roll equilibrium angle information indicates an angle at which a second axis of the body coordinate system is parallel to the xy plane of the reference coordinate system when the body coordinate system is rotated based on the first axis of the body coordinate system.
Manipulator control method. According to claim 11,
The pitch equilibrium angle information is an angle at which the third axis of the body coordinate system coincides with the z-axis of the reference coordinate system when the body coordinate system is rotated based on the second axis of the body coordinate system rotated according to the roll equilibrium angle information. representing
Manipulator control method. According to claim 12,
Obtaining the second rotation angle information,
obtaining third rotation angle information about motors corresponding to the hand based on a sensed value representing a posture of the hand; and
obtaining second rotation angle information by correcting third rotation angle information based on the equilibrium angle information; containing
Manipulator control method. According to claim 13,
The plurality of links include a first link corresponding to the upper arm, a second link corresponding to the lower arm, and a third link corresponding to the hand;
The plurality of motors include a first motor for rotating the first link about a first axis, a second motor for rotating the first link about a second axis, and a rotation of the second link about the first axis. A third motor, a fourth motor for rotating the second link about a third axis, a fifth motor for rotating the third link about the first axis, and a rotation of the third link about the second axis Including a sixth motor
Manipulator control method. In the manipulator,
a plurality of links including a first link and a second link;
A plurality of motors for rotating the plurality of links;
a communication interface comprising circuitry;
a memory storing at least one instruction; and
Including; processor;
the processor,
Receiving a sensing value of an external sensor for detecting a posture of a user's arm through the communication interface;
Obtaining a second vector corresponding to the posture of the user's arm based on a matrix obtained based on the sensed value and a first vector pre-stored in the memory;
Obtaining posture information of the user's arm based on the second vector;
Control driving of the plurality of motors based on the posture information of the user's arm;
the processor,
Obtaining a third vector corresponding to the first link based on a sensing value of a first external sensor;
Obtaining a fourth vector corresponding to the second link based on a sensing value of a second external sensor;
obtaining attitude information corresponding to the second link based on the third vector and the fourth vector;
Controlling driving of the plurality of motors corresponding to the second link based on attitude information corresponding to the second link
manipulator. According to claim 15,
the processor,
Acquiring a quaternion vector corresponding to the posture of the user's arm by applying the sensed value to an Altitude and Heading Reference System (AHRS) algorithm stored in the memory;
Obtaining the matrix based on the quaternion vector
manipulator. According to claim 15,
The second vector is,
Including the 2-1 vector corresponding to the x-axis and the 2-2 vector corresponding to the z-axis,
The posture information of the user's arm,
Including a roll angle corresponding to the x-axis, a pitch angle corresponding to the y-axis, and a yaw angle corresponding to the z-axis,
the processor,
Obtaining a first yaw angle based on the 2-1 vector,
Obtaining a second yaw angle based on the 2-2 vector,
Obtaining the yaw angle based on the first yaw angle and the second yaw angle
manipulator. According to claim 17,
the processor,
Obtaining the yaw angle by applying a weight to the first yaw angle and the second yaw angle based on a predefined weight function
manipulator. According to claim 15,
the processor,
A first matrix is obtained based on a first sensed value corresponding to the initially set posture of the user's arm, stored in the memory, and a second matrix obtained based on a second sensed value corresponding to the current posture of the user's arm. and obtaining the matrix based on the first matrix.
manipulator. According to claim 15,
the processor,
Obtaining an angle corresponding to the x-axis based on a dot product of the third vector and the fourth vector;
Obtaining an angle corresponding to the z-axis based on the cross product of the third vector and the fourth vector
manipulator.

Images