KR102643316B1 - Detecting device and method for calculating amount of impact of robot manipulator - Google Patents

Abstract

A detection device and method for calculating the amount of impact of a robot manipulator are disclosed. The detection device calculates the angular velocity and angular acceleration of the joint using the joint angle and the interface unit that receives the joint angle for the joint of the robot manipulator, and applies the joint angle, angular velocity, and angular acceleration to the Jacobian matrix determined by robot kinematics. It includes a control unit that calculates the amount of impact, which is the force generated by collision with the outside.

Description

Detecting device and method for calculating amount of impact of robot manipulator}

The present invention relates to impact detection technology, and more specifically, to a detection device and method for calculating the impact amount of a robot manipulator, which calculates the impact amount generated in the robot manipulator from an external impact.

Recently, work using robot manipulators has been used in many fields, and its core lies in interaction with various environments. These tasks can be divided into contact and non-contact tasks, and as the number of contact tasks is steadily increasing, research is being actively conducted on robots that can easily contact the environment and people, such as collaborative robots.

For contact tasks, it is necessary to control the force exerted by the robot end on the environment or to constrain its movements for various interactions at the contact surface. In this process, the robot requires a transition process from non-contact to contact. At this time, the robot attempts to approach to interact with people and the environment, and a collision occurs due to positional errors between the end of the robot and the point of contact. Such collisions may injure people or cause damage to devices at the end of the robot and work objects.

To overcome this, it is necessary to detect the impulse and perform force control. However, detecting the amount of impact has the difficulty of requiring an additional external force measuring device on the robot or complex calculations. Therefore, a method to solve these problems is needed.

Korean Patent Publication No. 10-2226122 (2021.03.11.)

The problem that the present invention aims to solve is a robot manipulator that calculates the amount of impact generated by an external collision using the joint-based variables (joint angle, angular velocity, and angular acceleration) of the robot manipulator and time-delay control (TDC). To provide a detection device and method for calculating the amount of impact.

In order to solve the above problem, a detection device for calculating the amount of impact of a robot manipulator according to the present invention calculates the angular velocity and angular acceleration of the joint using an interface unit that receives the joint angle of the joint of the robot manipulator and the joint angle, It includes a control unit that applies the joint angle, the angular velocity, and the angular acceleration to a Jacobian matrix determined by robot kinematics to calculate the amount of impulse, which is a force generated by a collision with an external force.

In addition, the control unit calculates the amount of impact using a joint angle error indicating the difference between the desired position and the actual position of the joint angle, the angular velocity error of the joint obtained by differentiating the joint angle error, the desired angular acceleration of the joint, and the actual angular acceleration. It is characterized by

In addition, the control unit is characterized in that it calculates the amount of impact using the following equation.

[Equation]

Here, I means the impulse, J means the Jacobian matrix, means the inertia gain matrix, means the angular acceleration of the joint desired by the user, means the angular acceleration at the previous point in one control sample period, is the feedback gain matrix associated with the damping constant, which has the form of a positive constant diagonal matrix, means the error vector (first derivative) of the angular velocity, refers to the feedback gain matrix related to the spring constant in the form of a positive constant diagonal matrix, means the position error vector, and βt means the control sample period.

The detection device for calculating the amount of impact of a robot manipulator according to the present invention uses an interface unit that receives the amount of current for the motor of the robot manipulator and calculates the torque at the current time and the torque at the previous time for the joint of the robot manipulator using the amount of current. And a control unit that calculates the amount of impact, which is a force generated by a collision with an external force, by applying the torque change amount with respect to the torque at the current time and the torque at the previous time to the Jacobian matrix determined by robot kinematics.

In addition, the control unit calculates the corresponding force when a collision occurs by applying the torque change to the Jacobian matrix, and calculates the amount of impact using the calculated force and the control sample period.

In addition, the control unit is characterized in that it calculates the amount of impact using the following equation.

[Equation]

Here I means the impulse, means the corresponding force when a collision occurs, J means the Jacobian matrix, means the current joint torque, means the joint torque at the previous point of one control sample period, and βt means the control sample period.

Additionally, the control unit sets a range according to the size of the calculated impulse and controls the robot manipulator according to the set range.

In addition, the control unit controls the robot manipulator to control at least one of a control for lowering the driving speed of the robot manipulator when the impulse amount is within the first range, a control for lowering the driving force, and a control for changing the drive pattern to a preset movement pattern. drives the robot manipulator when the impulse amount falls within a second range that is larger than the first range, and when the impulse quantity falls within a third range that is larger than the second range, the operation of the robot manipulator is stopped. It is characterized by complete stoppage.

The detection method for calculating the amount of impact of a robot manipulator according to the present invention includes the steps of receiving a joint angle for a joint of a robot manipulator by a detection device, and calculating the angular velocity and angular acceleration of the joint using the joint angle by the detection device. And a step where the detection device applies the joint angle, the angular velocity, and the angular acceleration to a Jacobian matrix determined by robot kinematics to calculate an impulse, which is a force generated by a collision with an external device.

The detection method for calculating the amount of impact of the robot manipulator according to the present invention includes the steps of a detection device receiving the amount of current for the motor of the robot manipulator, the detection device using the amount of current to determine the current torque for the joint of the robot manipulator and Calculating the torque at a previous point in time, and the detection device applying the torque at the current point in time and the amount of torque change relative to the torque at the previous point in time to the Jacobian matrix determined by robot kinematics to generate an impulse amount that is a force generated by a collision with the outside. It includes the step of calculating .

According to an embodiment of the present invention, without a separate torque sensor or impact sensor, the amount of impact generated due to an external collision is calculated using the joint-based variables (joint angle, angular velocity, angular acceleration) of the robot manipulator and a time delay control technique, Installation costs can be reduced by facilitating compatibility with robot manipulators.

In addition, after setting the range according to the size of the impact, you can control the robot manipulator appropriate for the range to increase product productivity by minimizing drive gaps while being safe.

1 is a diagram for explaining a detection system according to an embodiment of the present invention.
Figure 2 is a block diagram for explaining a detection device according to an embodiment of the present invention.
Figure 3 is a diagram for explaining the process of calculating the amount of impact based on the amount of torque change according to an embodiment of the present invention.
Figure 4 is a diagram for explaining the process of calculating the amount of impact based on joint angle-based variable values according to an embodiment of the present invention.
Figure 5 is a diagram briefly explaining the process of calculating the amount of impact in Figure 4.
Figure 6 is a flowchart illustrating a detection method for calculating the amount of impact based on the amount of torque change according to an embodiment of the present invention.
Figure 7 is a flowchart for explaining a detection method for calculating the amount of impact based on a joint angle-based variable value according to an embodiment of the present invention.
Figure 8 is a block diagram for explaining a computing device according to an embodiment of the present invention.

Below, with reference to the attached drawings, embodiments of the present invention will be described in detail so that those skilled in the art can easily implement the present invention. However, the present invention may be implemented in many different forms and is not limited to the embodiments described herein. In order to clearly explain the present invention in the drawings, parts that are not related to the description are omitted, and similar parts are given similar reference numerals throughout the specification.

In this specification and drawings (hereinafter referred to as “this specification”), duplicate descriptions of the same components are omitted.

Additionally, in this specification, when a component is mentioned as being 'connected' or 'connected' to another component, it may be directly connected or connected to the other component, but may be connected to the other component in the middle. It should be understood that may exist. On the other hand, in this specification, when it is mentioned that a component is 'directly connected' or 'directly connected' to another component, it should be understood that there are no other components in between.

Additionally, the terms used in this specification are merely used to describe specific embodiments and are not intended to limit the present invention.

Also, in this specification, singular expressions may include plural expressions, unless the context clearly dictates otherwise.

In addition, in this specification, terms such as 'include' or 'have' are only intended to designate the presence of features, numbers, steps, operations, components, parts, or combinations thereof described in the specification, and one or more It should be understood that this does not exclude in advance the presence or addition of other features, numbers, steps, operations, components, parts, or combinations thereof.

Also, in this specification, the term 'and/or' includes a combination of a plurality of listed items or any of the plurality of listed items. In this specification, 'A or B' may include 'A', 'B', or 'both A and B'.

Additionally, in this specification, detailed descriptions of well-known functions and configurations that may obscure the gist of the present invention will be omitted.

1 is a diagram for explaining a detection system according to an embodiment of the present invention.

Referring to FIG. 1, the detection system 300 uses the joint-based variables (joint angle, angular velocity, and angular acceleration) of the robot manipulator 200 and time-delay control (TDC) to detect collisions caused by external collisions. Calculate the amount of impact. The detection system 300 includes a detection device 100 and a robot manipulator 200.

The detection device 100 determines whether there is a collision with the outside. When the detection device 100 determines that a collision has occurred, it calculates the amount of impact, which is the force generated by the collision, and controls the robot manipulator 200 using the calculated amount of impact. The detection device 100 can calculate the amount of impulse based on a time delay control technique without requiring a separate force/torque sensor at the end-effector (end-effector) of the robot manipulator 200. At this time, the detection device 100 may calculate the amount of impact using the torque change or joint-based variable values (joint angle, angular velocity, and angular acceleration) for the joints of the robot manipulator 200. Here, the torque for the joint can be calculated using the amount of current for the motor 240, and the joint angle among the joint-based variable values is measured from a joint angle measurement sensor (encoder, etc.) basically provided in the robot manipulator 200. , angular velocity and angular acceleration can be calculated using the measured joint angles. The detection device 100 divides the calculated impulse amount into stages and controls the robot manipulator 200 according to commands corresponding to the divided stages.

The robot manipulator 200 includes a manipulator with n-degrees of freedom (DOF). The robot manipulator 200 includes a manipulator 210, an end effector 230, and a motor 250.

The manipulator 210 is a robot arm and includes a base 211, a plurality of arms 212, and a plurality of joints 213. The base 211 supports a plurality of arms 212 and a plurality of joints 213. The base 211 may be the body of an industrial robot, a humanoid robot, or a vehicle-type robot. The base 211 is coupled to the lowest arm of the plurality of arms 212. The plurality of arms 212 are connected via at least one joint 213 connected therebetween, and a sufficient degree of freedom can be obtained through the plurality of joints 213. Each joint 213 includes an encoder (not shown) that can measure the position according to the operation of each manipulator 210.

The end effector 230 is detachably connected to the arm 212 of the manipulator 210 and includes a main body 231 and a work tool 232. In a general case, the main body 231 is connected to the uppermost (terminal) arm of the plurality of arms 212. That is, the main body 231 is connected to the uppermost arm at one end, and the work tool 232 is connected to the other end. The work tool 232 may be replaced depending on the type of work the robot manipulator 200 must perform. For example, the work tool 232 may include, but is not limited to, a gripper, a tool changer, a measuring jig, etc.

The motor 250 is connected to the base 211 and drives a plurality of arms 212 and a plurality of joints 213 connected to the base 211. The motor 250 transmits information about the amount of current to the detection device 100. At this time, the motor 250 may transmit the amount of current before the external collision occurred and the current amount to the detection device 100, but is not limited to this and may transmit the amount of current to the detection device 100 at a preset period or in real time. there is.

Here, a collision with the outside means a collision with an external environment (obstacle, structure, etc.) or a person, and may occur in the manipulator 210 or the end effector 230. Additionally, even if a collision occurs in the manipulator 210, the impact of the collision may be transmitted to the end effector 230, and even if a collision occurs in the end effector 230, the effects of the collision may be transmitted to the manipulator 210. Because of this, the detection device 100 can determine whether there is a collision based on the position value (joint angle) of the joint measured from the encoder included in each joint 213.

Figure 2 is a block diagram for explaining a detection device according to an embodiment of the present invention, Figure 3 is a diagram for explaining the process of calculating the amount of impact based on the amount of torque change according to an embodiment of the present invention, and Figure 4 is This is a diagram for explaining a process for calculating the amount of impact based on a joint angle-based variable value according to an embodiment of the present invention, and FIG. 5 is a diagram for briefly explaining the process for calculating the amount of impact in FIG. 4.

Referring to FIGS. 1 to 5 , the detection device 100 includes an interface unit 10 and a control unit 30, and may further include a storage unit 50.

The interface unit 10 performs communication with the robot manipulator 200. The interface unit 10 receives joint angles from an encoder included in each joint 213. Preferably, the interface 10 may receive a joint angle from an encoder of a joint connected to the uppermost (distal) arm of the plurality of joints 213, but is not limited to this and may receive a joint angle from an encoder of all joints. . Additionally, the interface unit 10 receives the amount of current from the motor 250.

The control unit 30 performs overall control of the detection device 100. The control unit 30 calculates the amount of impact generated due to an external collision. At this time, the control unit 30 can simply calculate the amount of impact using a time delay control technique to avoid calculating robot dynamics.

In general, the dynamics equation of a robot manipulator with n-degrees of freedom can be expressed as [Equation 1] in joint space.

is a vector composed of joint angles, meaning the actual joint angle, means the angular velocity obtained by first derivative of the vector composed of joint angles, means the angular acceleration obtained by second-differentiating the vector composed of joint angles, means the inertia matrix in joint space, means Coriolis and centrifugal force in the joint space, means the force of gravity in the joint space, means joint friction torque, refers to unmodeled dynamic terms including disturbances, means joint torque, which is a control input applied to each joint. At this time It is possible to directly obtain or estimate the torque converted into a current to be applied to the motor 250 from the detection device 100. Also, for simple expression, time t is omitted.

Inertial gain matrix When using (constant diagonal matrix), When applied to [Equation 1], it is expressed as [Equation 2].

means the inertia gain matrix, refers to the unknown uncertainty vector containing the robot dynamics.

In general, the control input for controlling [Equation 2] can be configured as in [Equation 4].

means the estimation vector of N, means the control input vector. means the joint angle desired by the user, means the angular velocity of the joint desired by the user, means the angular acceleration of the joint desired by the user.

The control unit 30 uses time-delay estimation. can be calculated easily. When using the time delay estimation technique, the control unit 30 determines the One control sample period to calculate ( ) at the time before in It can be estimated using . Through this, the control unit 30 can calculate [Equation 5] based on [Equation 2].

refers to the unknown uncertainty vector containing the robot dynamics, means the estimated vector of N for the current time (t), refers to N at the previous time point of one control sample period, refers to the joint torque at the previous point in one control sample period, means the angular acceleration at the previous point in one control sample period.

At this time, the control unit 30 uses [Equation 4] to obtain the desired motion in the robot manipulator 200. Assign error dynamics to . For this purpose, the position error vector Is It is defined as means the joint angle desired by the user, is a vector composed of joint angles, meaning the actual joint angle. also Is It is defined as That is, the control unit 30 In this case, the desired error dynamics can be obtained as in [Equation 6] using [Equation 5].

means the error vector of the angular acceleration for the joint, is the feedback gain matrix associated with the damping constant, which has the form of a positive constant diagonal matrix, means the error vector of the angular velocity for the joint, refers to the feedback gain matrix related to the spring constant in the form of a positive constant diagonal matrix, is the position error vector of the joint angle, It is defined as in other words represents the angle difference between the desired and actual positions of the joint.

Therefore, the control unit 30 can express the time delay control technique in an equation such as [Equation 7] using the time delay estimation technique of [Equation 5] and the error dynamics of [Equation 6].

The control unit 30 controls the joint torque at local time t and all time points of one control sample cycle. for The force acting on the end effector 230 of the robot manipulator 200 is calculated using Equation 8.

refers to the force (damage) generated by external collision at the end effector, which is the end of the robot manipulator, refers to the force applied to the end effect of the robot manipulator at the current point, means the force applied to the end effect of the robot manipulator at the previous point of one control sample cycle, and J means the Jacobian matrix of the robot manipulator. Here, when shock due to collision occurs, the position error vector generated at each joint of the robot manipulator 200 has a relatively large value, and this position error is significantly changed by the control input creates . Therefore, when a collision occurs, the control unit 30 generates the corresponding force can be calculated. Additionally, the control unit 30 The impulse can be calculated as in [Equation 9] using . That is, the control unit 30 can apply the torque change to the Jacobian matrix to calculate the corresponding force when a collision occurs, and calculate the amount of impact using the calculated force and the control sample period (see FIG. 3).

I refers to the amount of shock generated by an external collision.

Meanwhile, the control unit 30 can derive [Equation 10] using the time delay control technique of [Equation 7].

Meanwhile, through [Equation 10], go Since it can be seen that it can be used instead of , the control unit 30 can convert [Equation 9] to [Equation 11] (see FIGS. 4 and 5). That is, the control unit 30 provides a joint angle error (joint angle error) representing the difference between the desired position and the actual position of the joint angle. ), the angular velocity error of the joint obtained by differentiating the joint angle error ( ), the impulse is calculated using the desired angular acceleration and actual angular acceleration of the joint.

means the error vector of the joint angle, which represents the difference between the desired position and the actual position of the joint, Is It means the error vector of the angular velocity for the joint by first differentiation. also means the desired value of angular acceleration, means the actual angular acceleration value. Therefore, the control unit 30 can calculate the amount of impact using values for joint position, speed, and acceleration without using torque or current through [Equation 11].

The control unit 30 sets a range according to the size of the calculated impulse and controls the robot manipulator 200 according to the set range. For example, the control unit 30 sets the first range if the impulse amount is 0 to 3 (N·s), sets the second range if the impulse amount is 4 (N·s) to 10 (N·s), and sets the impulse amount to the first range. If it is 11 (N·s) or more, it can be set as the third range, but it is not limited to this.

The control unit 30 controls the robot manipulator 200 with at least one of a control to lower the driving speed of the robot manipulator 200 when the impulse amount is within the first range, a control to lower the driving force, and a control to change the drive pattern to a preset movement pattern. (200) is driven. Here, the control for lowering the driving speed means lowering the moving speed when driving the robot manipulator 200 than the default setting speed, and the control for lowering the driving force means lowering the moving force when driving the robot manipulator 200 than the default setting force. It means lowering. Additionally, control to change the driving pattern means changing the default movement pattern when driving the robot manipulator 200 to a movement pattern set as the next priority, or changing it to a movement pattern that avoids the location where a collision occurred. The control unit 30 temporarily stops the operation of the robot manipulator 200 when the impulse amount falls within the second range greater than the first range, and completely stops the operation of the robot manipulator 200 when the impulse quantity falls within the third range greater than the second range. I order it.

The storage unit 50 stores an algorithm or program for driving the detection device 100. The storage unit 50 stores the joint angle, angular velocity, and angular acceleration of the robot manipulator 100, and stores the current amount of the motor 250. The storage unit 50 stores countermeasures according to the amount of impact at each stage, and the corresponding information may be stored in the form of a lookup table. The storage unit 50 includes a flash memory type, hard disk type, multimedia card micro type, card type memory (for example, SD or XD memory, etc.), Random Access Memory (RAM), Static Random Access Memory (SRAM), Read-Only Memory (ROM), Electrically Erasable Programmable Read-Only Memory (EEPROM), Programmable Read-Only Memory (PROM), magnetic memory, It may include at least one storage medium of a magnetic disk and an optical disk.

Figure 6 is a flowchart illustrating a detection method for calculating the amount of impact based on the amount of torque change according to an embodiment of the present invention.

Referring to Figures 1 and 6, the detection method calculates the amount of impact generated by an external collision using a time delay control technique without a separate torque sensor or impact sensor.

In step S110, the detection device 100 receives the amount of current for the motor 250. Here, the amount of current received becomes a value that indirectly estimates the torque of the joint. That is, the detection device 100 can estimate the torque of the joint without a separate torque sensor.

In step S120, the detection device 100 calculates the amount of impact, which is the force generated by a collision with the outside. The detection device 100 uses the received amount of current to calculate the torque at the current time and the torque at the previous time for the joints of the robot manipulator 200. The detection device 100 calculates the amount of impulse by applying the torque change amount with respect to the torque at the current time and the torque at the previous time to the Jacobian matrix determined by robot kinematics. At this time, the detection device 100 can calculate the amount of impact using [Equation 9] described above.

In step S130, the detection device 100 performs drive control corresponding to the calculated impulse amount. The detection device 100 sets a range according to the size of the impulse and controls the robot manipulator 200 according to the set range.

Figure 7 is a flowchart illustrating a detection method for calculating the amount of impact based on a joint angle-based variable value according to an embodiment of the present invention.

Referring to Figures 1 and 7, the detection method calculates the amount of shock generated by an external collision using joint-based variables (joint angle, angular velocity, and angular acceleration) of the robot manipulator without a separate torque sensor or impact sensor.

In step S210, the detection device 100 receives a variable value based on the joint angle. That is, the detection device 100 receives the joint angle from the encoder included in each joint 213. Preferably, the interface 10 may receive a joint angle from an encoder of a joint connected to the uppermost (distal) arm of the plurality of joints 213, but is not limited to this and may receive a joint angle from an encoder of all joints. .

In step S220, the detection device 100 calculates the amount of impact, which is the force generated by the collision with the outside. The detection device 100 calculates the angular velocity and angular acceleration of the joints of the robot manipulator 200 using the received joint angles. The detection device 100 calculates the impulse by applying the joint angle, angular velocity, and angular acceleration to the Jacobian matrix determined by robot kinematics. At this time, the detection device 100 can calculate the amount of impact using [Equation 11] described above.

In step S230, the detection device 100 performs drive control corresponding to the calculated impulse amount. The detection device 100 sets a range according to the size of the impulse and controls the robot manipulator 200 according to the set range.

Figure 8 is a block diagram for explaining a computing device according to an embodiment of the present invention. The computing device TN100 of FIG. 8 may be a device (eg, a detection device, etc.) described herein.

The computing device TN100 may include at least one processor TN110, a transceiver device TN120, and a memory TN130. Additionally, the computing device TN100 may further include a storage device TN140, an input interface device TN150, an output interface device TN160, etc. Components included in the computing device TN100 may be connected by a bus TN170 and communicate with each other.

The processor TN110 may execute a program command stored in at least one of the memory TN130 and the storage device TN140. The processor TN110 may refer to a central processing unit (CPU), a graphics processing unit (GPU), or a dedicated processor on which methods according to embodiments of the present invention are performed. Processor TN110 may be configured to implement procedures, functions, and methods described in connection with embodiments of the present invention. The processor TN110 may control each component of the computing device TN100.

Each of the memory TN130 and the storage device TN140 can store various information related to the operation of the processor TN110. Each of the memory TN130 and the storage device TN140 may be comprised of at least one of a volatile storage medium and a non-volatile storage medium. For example, the memory TN130 may be comprised of at least one of read only memory (ROM) and random access memory (RAM).

The transceiving device TN120 can transmit or receive wired signals or wireless signals. The transmitting and receiving device (TN120) can be connected to a network and perform communication.

Meanwhile, the embodiments of the present invention are not only implemented through the apparatus and/or method described so far, but may also be implemented through a program that realizes the function corresponding to the configuration of the embodiment of the present invention or a recording medium on which the program is recorded. This implementation can be easily implemented by anyone skilled in the art from the description of the above-described embodiments.

Although the embodiments of the present invention have been described in detail above, the scope of the present invention is not limited thereto, and various modifications and improvements made by those skilled in the art using the basic concept of the present invention defined in the following claims are also possible. It falls within the scope of invention rights.

10: Interface part
30: control unit
50: storage unit
100: detection device
200: Robot manipulator
210: Manipulator
211: base
212: cancer
213: joints
230: End effector
231: body
232: sensor
233: Work tools
250: motor
300: detection system

Claims (10)

An interface unit that receives joint angles for joints of the robot manipulator; and
Calculate the angular velocity and angular acceleration of the joint using the joint angle, and apply the joint angle, the angular velocity, and the angular acceleration to the Jacobian matrix determined by robot kinematics to calculate the impulse, which is a force generated by collision with the outside, , a control unit that sets a range according to the size of the calculated impulse and controls the robot manipulator according to the set range;
The control unit,
When the impulse amount falls within the first range, the robot manipulator is driven by at least one control selected from the group consisting of a control for lowering the driving speed of the robot manipulator, a control for lowering the driving force, and a control for changing the drive pattern to a preset movement pattern, When the impulse amount falls within a second range greater than the first range, the operation of the robot manipulator is temporarily stopped, and when the impulse quantity falls within a third range greater than the second range, the operation of the robot manipulator is completely stopped. A detection device that calculates the amount of impact of a robot manipulator. According to clause 1,
The control unit,
A robot characterized in that the impulse is calculated using a joint angle error representing the difference between the desired position and the actual position of the joint angle, the angular velocity error of the joint obtained by differentiating the joint angle error, and the desired angular acceleration and actual angular acceleration of the joint. A detection device that calculates the amount of impulse of the manipulator. According to clause 2,
The control unit,
A detection device for calculating the amount of impact of a robot manipulator, characterized in that the amount of impact is calculated using the following equation.
[Equation]

(Here I refers to the impulse, J refers to the Jacobian matrix, means the inertia gain matrix, means the angular acceleration of the joint desired by the user, means the angular acceleration at the previous point in one control sample period, is the feedback gain matrix associated with the damping constant, which has the form of a positive constant diagonal matrix, means the error vector (first derivative) of the angular velocity, refers to the feedback gain matrix related to the spring constant in the form of a positive constant diagonal matrix, means the position error vector, and βt means the control sample period) delete delete delete delete delete A detection device receiving joint angles for joints of a robot manipulator;
calculating, by the detection device, the angular velocity and angular acceleration of the joint using the joint angle;
Calculating an impulse, which is a force generated by a collision with an outside, by the detection device applying the joint angle, the angular velocity, and the angular acceleration to a Jacobian matrix determined by robot kinematics; and
Including, the detection device setting a range according to the size of the calculated impulse, and controlling the robot manipulator according to the set range,
The step of controlling the robot manipulator is,
When the impulse amount falls within the first range, the robot manipulator is driven by at least one control selected from the group consisting of a control for lowering the driving speed of the robot manipulator, a control for lowering the driving force, and a control for changing the drive pattern to a preset movement pattern, When the impulse amount falls within a second range greater than the first range, the operation of the robot manipulator is temporarily stopped, and when the impulse quantity falls within a third range greater than the second range, the operation of the robot manipulator is completely stopped. A detection method for calculating the amount of impact of a robot manipulator. delete

Images