JP5556558B2 - Robot arm control device, control method, and control program - Google Patents

Description

  The present invention relates to control of a robot arm, and more particularly to safety control when an object such as a human body is sandwiched between link mechanisms of a robot arm.

  A robot arm that performs various operations by autonomously controlling the link mechanism requires safety control when an object such as a human body is sandwiched between the links. The importance of such safety control is increasing as the number of cases in which robot arms are used in human living spaces is increasing.

  FIG. 5 shows a functional configuration of the joint device 101 according to Patent Document 1. The joint device 101 includes a comparison unit 102, an error amplification unit 103, a motor 104, a speed reducer 105, a position detection unit 106, and a torque limit value generation unit 107. The comparison unit 102 outputs a position tracking deviation obtained by subtracting the detection value of the joint position (angle) detected by the position detection unit 106 from the position command (target value). The error amplifying unit 103 outputs a torque command that minimizes the position tracking deviation within the range of the torque limit value from the torque limit value generating unit 107. The motor 104 generates motor torque in response to the torque command. The reducer 105 converts the motor torque into an assist force that drives the link mechanism of the robot arm. The position detection unit 106 is installed on the same rotation axis as the speed reducer 105, and outputs a position detection value indicating a joint angle. The torque limit value generation unit 107 outputs a torque limit value corresponding to the joint angle and the direction of change of the joint angle according to the position detection value and the position tracking deviation.

  FIG. 6 schematically shows the configuration of the pressure distribution sensor 201 according to Patent Document 2. The pressure distribution sensor 201 includes a sensor unit 202, a sensor mesh 203, and an optical fiber 204. The sensor mesh 203 is configured by arranging a plurality of sensor units 202 in a matrix. The pressure detection signal detected by each sensor unit 202 is output to the outside through a plurality of optical fibers 204 connected to each sensor unit 202. The pressure at the position of each sensor mesh 203 is known from each output pressure detection signal. By installing such a pressure distribution sensor 201 on the link surface of the robot arm, it is possible to detect that an object is sandwiched between the links and the position where the object is sandwiched.

JP2000-6065 JP-A-6-82320

  When the object is sandwiched between the links of the robot arm, the joint device 101 according to Patent Document 1 limits the motor torque depending on only the joint angle regardless of the place where the object is sandwiched. Therefore, when the sandwiched position is close to the joint connecting the two links, a large force may be applied to the sandwiched object. Further, when the sandwiched position is far from the joint, the motor torque may be suppressed more than necessary. That is, even if the robot arm can continue the work without causing harm to the human body, the work may be interrupted.

  In addition, when using the pressure distribution sensor 201 according to Patent Document 2, in order to accurately detect the pinched position, a large number of sensor units 202 must be installed on the link surface. For this reason, there are problems such as an increase in manufacturing cost, calculation load, weight, failure rate, etc. of the control device, and electromagnetic interference.

  Therefore, the present invention can accurately detect the position where the object is pinched (pinching position) without using a sensor or the like that detects the pressure distribution on the link, and can optimally control the torque of the joint mechanism according to the pinching position. The purpose is to do so.

  A first aspect of the present invention is a robot arm control device that controls a motor torque that drives a joint portion of a link mechanism, and a robot arm control device that controls a motor torque that drives a joint portion of the link mechanism. By applying a restriction based on a safety motor torque profile set in consideration of safety to a basic torque calculated so that a detected value of a joint angle of the joint portion follows a target value, a safety motor Motor torque calculation means for calculating torque, and changes in the safety motor torque with respect to the detected value of the joint angle are defined based on torque for supporting the weight of the link connected to the distal end side from the joint portion. When it is determined that the object has deviated from the reference value, the pinch detection unit that determines that an object is pinched between the adjacent links and the pinch detection unit When the object is caught, the joint angle when the deviation from the reference value is detected, the detected joint angle, the physical property value preset for the object, and the motor torque A pinch position indicating a distance from the joint part to a position where the object is pinched based on a pinch torque excluding a torque for supporting the weight of the link coupled to the tip side from at least the joint part from a detection value And the upper limit value of the safety motor torque increases as the clamping position increases, so that the upper limit value of the safety motor torque decreases as the clamping position decreases. Thus, a safe motor torque calculating means for generating the safe motor torque profile is provided.

  According to the above aspect, the object pinching position can be accurately estimated without using an expensive sensor or the like, and an appropriate torque command can be generated according to the pinching position. In addition, it is possible to reliably prevent harm to a human body or the like being sandwiched, and to continue the original operation of the robot arm within a range that does not cause harm to the human body or the like.

  Moreover, it is preferable that the said safe motor torque profile is a saturation function or a sigmoid function which makes the said basic torque a variable and makes the upper limit of the said safe motor torque the maximum value.

  Further, the reference value is defined based on the weight of the link and, when the link holds a predetermined object, a weight obtained by adding the weight of the object. The sine function is preferably represented by

  The sine function can be expressed by the following equation (1), for example.

ただし、記号の意味は以下の通りである。
Ｔm：２つの隣合う前記リンクが前記物体を挟んでいない場合の前記モータトルク
ｍ：前記２つの隣合うリンクのうち前記ロボットアームを備えたサービスロボットの本体から遠い方のリンクの質量（前記対象物を把持している場合はその質量も含む）
ｇ：重力加速度
ｌ：前記関節から前記２つの隣合うリンクのうち前記サービスロボットの本体から遠い方のリンクの重心までの距離
θ：前記関節角度の検出値
θ0：前記２つの隣合うリンクのうち前記サービスロボットの本体に近い方のリンクの水平となす角度

However, the meaning of the symbols is as follows.
Tm: Motor torque when two adjacent links do not sandwich the object m: Mass of the link farther from the body of the service robot having the robot arm among the two adjacent links (the target (If the object is gripped, its mass is included)
g: Gravity acceleration l: Distance from the joint to the center of gravity of the link farther from the main body of the service robot among the two adjacent links θ: Detected value of the joint angle θ0: Of the two adjacent links Angle formed by the horizontal of the link closer to the main body of the service robot

  The physical property value is preferably defined based on a Young's modulus and Poisson's ratio of a part of the human body.

  Moreover, the said pinching position is computable by following formula (2), for example.

ただし、記号の意味は以下の通りである。
θ：前記関節角度の検出値
θc：前記正弦関数からの乖離が検出された時の関節角度
Ｔc：前記挟みトルク
Ｅ：人体のヤング率
ν：人体のポアソン比

However, the meaning of the symbols is as follows.
θ: detected value of the joint angle θc: joint angle when a deviation from the sine function is detected Tc: pinching torque E: Young's modulus of human body ν: Poisson's ratio of human body

  A second aspect of the present invention is a control method based on the same technical idea as that of the control device.

  Furthermore, a third aspect of the present invention is a control program based on the same technical idea as the control device and the control method.

  According to the present invention, an object pinching position can be accurately estimated without using an expensive sensor or the like, and an appropriate torque command can be generated according to the pinching position. In addition, it is possible to reliably prevent harm to a human body or the like being sandwiched, and to continue the original operation of the robot arm within a range that does not cause harm to the human body or the like.

It is a block diagram which shows the functional structure of the robot arm which concerns on Embodiment 1 of this invention. It is a figure which shows schematically the state which the 1st and 2nd link which adjoins each has a point contact with the human body. The state which the 1st and 2nd link which adjoins each has surface contact with the human body is shown schematically. 3 is a flowchart showing a flow of control in the robot arm according to the first embodiment. 5 is a graph showing a relationship between a safety motor torque command and time according to the first embodiment. 4 is a graph showing a relationship between pinching force and time according to Embodiment 1. 4 is a graph showing a relationship between a detected joint angle value and time according to the first embodiment. It is a block diagram which shows the functional structure of the joint apparatus based on patent document 1. FIG. It is a figure which shows schematically the structure of the pressure distribution sensor which concerns on patent document 2. FIG.

Embodiment 1
Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 shows a functional configuration of a robot arm 1 according to Embodiment 1 of the present invention. The robot arm 1 includes an arm unit 11, a joint angle command generation unit 12, and a driving force suppression control unit 13.

  The arm unit 11 includes a link mechanism 21, a joint motor 22, and a joint angle detection unit 23. The link mechanism 21 includes a plurality of links that are basically non-deformable structures, a joint mechanism that connects the links, and the like. The link mechanism 21 is designed according to the purpose of use of the robot arm 1 and is configured so as to be able to grip and carry an object, for example. The drive shaft of the joint motor 22 is controlled in accordance with the electric signal, and changes the relative positional relationship between adjacent links. The joint angle detection unit 23 detects the rotation amount and the rotation direction of the drive shaft of each joint motor 22, converts the detection result into an electrical signal, and outputs the electrical signal.

  The joint angle command generating unit 12 outputs a joint angle command θr indicating a target value of a joint angle (rotation angle of the drive shaft of the joint mechanism) necessary for the arm unit 11 to perform a predetermined operation. The joint angle command θr is normally generated according to a main control program that controls the operation that the arm unit 11 should originally perform.

  The driving force suppression control unit 13 includes a motor torque calculation unit 31, a current control unit 32, a pinch detection unit 33, a pinch position estimation unit 34, and a safety motor torque calculation unit 35.

  The motor torque calculation unit 31 inputs the joint angle command θr, the detected joint angle value θ, and the safety motor torque profile Ps output by the safety motor torque calculation unit 35, and outputs the safety motor torque command Ts. The safety motor torque command Ts is generated by applying the safety motor torque profile Ps to the basic motor torque command T0 generated so that the detected joint angle value θ follows the joint angle command θr. The application here corresponds to, for example, expressing Ps by a function having T0 as a variable and calculating Ts = Ps (T0). For example, the function Ps is preferably a saturation function, a sigmoid function, or the like having a predetermined value as an upper limit value and a value obtained by inverting the sign of the predetermined value as a lower limit value.

  The current control unit 32 inputs the safety motor torque command Ts from the motor torque calculation unit 31 and outputs a motor current to the joint motor 22 such that the motor torque Tm generated by the joint motor 22 matches the safety motor torque command Ts. To do. The motor torque Tm is detected by a known mechanism and used for feedback control and the like.

  The pinch detection unit 33 receives the joint angle detection value θ and the safety motor torque command Ts, and detects that an object is pinched by the link mechanism 21 from the feature of the change of the safety motor torque command Ts with respect to the joint angle detection value θ. The pinch detection signal S is output.

  The pinch position estimation unit 34 receives the joint angle detection value θ, the safety motor torque command Ts, and the pinch detection signal S, and estimates the pinch position x indicating the position where the object is pinched (distance from the joint to the position). Then, this estimation result is output.

  The safety motor torque calculator 35 receives the pinching position x, and generates and outputs a safe motor torque profile Ps corresponding to the pinching position x.

  The joint angle command generation unit 12 and the driving force suppression control unit 13 are configured by a combination of a programmable electronic control system using a microprocessor or the like, various electric circuits (logic circuits), and the like.

  FIG. 2A schematically shows a state in which the adjacent first and second links 41 and 42 are in point contact with the human body 43, respectively. FIG. 2B schematically shows a state in which the adjacent first and second links 41 and 42 are in surface contact with the human body 43, respectively. The first and second links 41 and 42 are rotatably connected by a joint 44. For example, when the robot arm imitates a human, the first link 41 corresponds to the upper arm, the joint 44 corresponds to the elbow, and the second link 42 corresponds to the forearm.

  In FIG. 2A, the distance R between the center of the cross section of the human body 43 and the contact point of the first or second link 41, 42, the distance x between the joint 44 and the contact point (pinching position) x, the first and second The joint angle θc (θc / 2) in a state where the links 41 and 42 are in point contact with the human body 43 is shown. FIG. 2A shows a state in which the second link 42 rotates clockwise with respect to the first link 41 and begins to pinch the human body 43.

  When the second link 42 is further rotated clockwise from the state of FIG. 2A, the human body 43 is crushed and becomes a state as shown in FIG. 2B. In the state of FIG. 2B, the human body 43 is in surface contact with the first and second links 41 and 42. In FIG. 2B, half of the cross section of the contact surface a and the joint angle θ (θ / 2) are shown. In the present embodiment, the joint angle θ is assumed to be 0 [rad] when the second link 42 is overlapped with the first link 41, and can be varied up to π [rad] counterclockwise.

As shown in FIG. 2A, the relational expression of the sandwiching angle θc, the distance R, and the sandwiching position x is the following expression (3).

  Half of the length of the cross section of the contact surface when the human body 43 is crushed by being sandwiched between the first and second links 41 and 42 is the contact mechanics, Johnson, K, L, Cambridge University Press, Cambridge, United From Kingdom, 1985, it is given by the following formula (4).

ただし、Ｅは人体のヤング率、νは人体のポアソン比、Ｔcは挟みトルクである。ここで、挟みトルクＴcとは、モータトルクＴmから第２のリンク４２の重量（第２のリンク４２が物体を把持等している場合は当該物体の重量を含む）、慣性力等に対抗するための成分を除いたものである。

Where E is the Young's modulus of the human body, ν is the Poisson's ratio of the human body, and Tc is the pinching torque. Here, the pinching torque Tc opposes the weight of the second link 42 from the motor torque Tm (including the weight of the object when the second link 42 grips the object), inertia force, and the like. The component for this is excluded.

The following formula (5) is obtained from FIG. 2B and formula (4).

The sandwiching position x is expressed by the following formula (6) from the formulas (3) and (5).

  As described above, the pinching position x is determined by the pinching angle θc, the predetermined physical property value determined from the detected joint angle θ, E, ν, and the like, and the pinching torque Tc. In FIG. 2B, the mass of the second link 42 (including the gripped object and the like) is m, the distance from the joint 44 to the center of gravity of the second link 42 is l, and the angle formed with the horizontal of the first link 41 is θ0. When the gravitational acceleration is g, the theoretical motor torque Tm in FIG. 2B is derived from the following equation (7) using the equation (5).

  In Expression (7), the above expression (0 <θ ≦ θc) is applied when the human body 43 is sandwiched, and the lower expression (θc <θ ≦ π) is applied when the human body 43 is not sandwiched. It is what is done. In the above equation, the first term on the right side is a motor torque component for supporting the weight of the second link 42, and the second term is the pinching torque Tc. Since the robot arm used for the user's life assistance generally performs only a slow motion, the inertia force of the second link 42 is not considered in the above formula. If the inertia force cannot be ignored, the inertia component may be added to the right side.

  In equation (7), the angle θ 0 formed with the horizontal of the first link 41 is known, and the distance l from the joint 44 to the center of gravity of the second link 42 is known from the design value. Therefore, the mass m of the second link 42 can be calculated from the motor torque Tm when the first link 41 and the second link 42 are not in contact with the human body 43 (θc <θ ≦ π). . Then, when the motor torque Tm in the following equation (7) is subtracted from the actual motor torque Tm, and the absolute value of the result exceeds a threshold value (minute value), it is determined that the human body 43 has been caught. That is, in the range of θc <θ ≦ π where the human body 43 is not sandwiched, the theoretical change in Tm is a sine function of the detected joint angle value θ, so the actual motor torque Tm, that is, the safe motor torque command Ts. It can be determined that the object 43 is caught in the human body 43 at the time when the change in the value deviates from the sine function. The joint angle detection value θ at this time becomes the sandwiching angle θc.

  In the present embodiment, the pinching torque Tc is controlled so as not to exceed the limit safe motor torque Tlim that does not harm the human body 43. The limit safety motor torque Tlim is obtained based on the limit safety external force Flim that is the maximum value of the external force that does not cause harm to the human body 43 and the clamping position x. The marginal safety external force Flim is widely known in the field of industrial safety (Takeshi Saitoh, Hiroyasu Ikeda “Measurement of human body pain sensation tolerance against mechanical stimuli of human cooperative robots”, National Institute of Industrial Safety, Special Research Report, NIIS -SRR-NO.33, September 2005; see "Risk assessment handbook for consumer products, first edition", Ministry of Economy, Trade and Industry, etc.). The limit safe motor torque Tlim according to the present embodiment is given by the following formula (8). In addition, the said numerical formula is an example, and the meaning of the said matter is that the limit safe motor torque Tlim increases as the pinch position x increases so that the limit safe motor torque Tlim decreases as the pinch position x decreases. It is. When the pinching position x is small (close to the joint 44), it is necessary to set Tlim to be small even if the original motion required for the robot arm 1 is no longer possible in order to reliably prevent harm to the human body 43. There is. On the other hand, when the pinching position x is large (distant from the joint 44), there is a high possibility that harm to the human body 43 can be prevented even if Tlim is set to be large to some extent. Therefore, when the pinching position x is relatively large, it is effective to set Tlim relatively large to increase the possibility that the original operation can be continued.

  The safe motor torque profile Ps is preferably given as a saturation function or a sigmoid function having the maximum value of the limit safe motor torque Tlim in Equation (8) and the minimum value obtained by inverting the sign. Thereby, the safe motor torque command Ts (= Ps (T0)) is given as a value that continuously changes within the range of -Tlim <Ts <Tlim.

  FIG. 3 shows a control flow in the robot arm 1 according to the present embodiment. First, the motor torque calculation unit 31 (see FIG. 1) calculates a basic motor torque command T0 so that the detected joint angle value θ follows the joint angle command θr (S1), and the safety motor torque calculation unit 35 calculates the safety motor. The torque profile Ps is read (S2), and a safe motor torque command Ts is calculated (S3). Next, the pinch detection unit 33 determines whether or not the curve representing the change of Ts with respect to θ deviates from the sine wave (S4). Based on the above, the joint motor 22 of the arm unit 11 is controlled (S10).

  On the other hand, when it is determined in step S4 that there is a divergence (Y), the pinch detection unit 33 outputs a pinch detection signal S (S5). When the pinch position estimation unit 34 receives the pinch detection signal S, the pinch position estimation unit 34 inputs the joint angle detection value θ and the safety motor torque command Ts, and based on the equations (6), (7) and the like as described above, the pinch position x Is estimated (S6). Next, the safe motor torque calculation unit 35 determines the limit safe motor torque Tlim based on the pinching position x based on the equation (8) as described above (S7), and generates the safe motor torque profile Ps ( S8). Then, the motor torque calculation unit 31 calculates the safety motor torque command Ts by applying the safety motor torque profile Ps generated in step S8 (S9), and performs motor control based on the calculation result (S10).

  According to the arm robot 1 having the above configuration, it is possible to accurately estimate the pinching position of an object without using an expensive sensor or the like, and it is possible to generate an appropriate torque command according to the pinching position. In addition, it is possible to reliably prevent harm to the human body or the like that is sandwiched, and to continue the original operation of the robot arm 1 within a range that does not cause harm to the human body or the like.

The simulation result of the robot arm 1 according to the first embodiment is shown below. The numerical values used for the simulation are as follows.
m = 2.0 [kg]
l = 0.13 [m]
g = 9.8 [s ^ -2]
E = 25 × 10 ^ 3 [MPa]
ν = 0.25
θ0 = 200 · π / 180 [rad]
R = 20 × 10 ^ -3 [m]
x = 50 × 10 ^ -3 [m]
Flim = 50 [N]
The above numerical values are based on the physical property values of the human forearm.

  In this simulation, when the joint angle of the robot arm shown in FIGS. 2A and 2B is controlled, the force applied to the sandwiched human body 43 is controlled to be within the limit safe external force Flim.

  FIG. 4A shows the relationship between the motor torque Tm (Ts) and time according to the first embodiment. In the figure, the solid line indicates the motor torque according to the present invention, and the alternate long and short dash line indicates the motor torque according to the prior art.

  In the section from 0 [s] to 2.73 [s] in FIG. 4A, the waveform of the change of the motor torque Tm with respect to the joint angle detection value θ is a sine wave, and the first and second links 41 and 42 It was determined that the human body 43 was not sandwiched between them. At 2.73 [s], the motor torque Tm rapidly changed and deviated from the sine wave, and it was detected that the human body 43 was sandwiched. Thereafter, the pinching position x is estimated, the limit safe motor torque Tlim is determined, the safe motor torque profile Ps is generated, and the safe motor torque command Ts is output by applying the Ps. That is, after 2.73 [s], the pinching torque Tc indicated by the second term on the right side of the upper expression of the equation (7) is calculated so that the absolute value thereof does not exceed the limit safe motor torque Tlim of the equation (8). Limited. In 3.14 [s], the absolute value of the pinching torque Tc reaches the limit safe motor torque Tlim, and then the motor torque Tm is limited so as not to exceed a certain value.

  On the other hand, in the prior art, there is no limit of the pinching torque Tc by the limit safe motor torque Tlim, so the absolute value of the motor torque continues to increase until a preset torque limit value is reached.

  Here, the sandwiching position x in 2.73 [s] was estimated using Equation (6), and this estimation result was 50 × 10 ^ -3 [m], which was consistent with the actual measurement value. Further, the limit safe motor torque Tlim is calculated using the equation (8), and a safety motor torque profile Ps such that only the absolute value of the pinching torque Tc among the motor torque components is limited to the limit safety motor torque Tlim is obtained. It is output. The safety motor torque command Ts is output by applying the safety motor torque profile Ps to the motor torque command T0 in which the detected joint angle value θ follows the joint angle command θr.

  FIG. 4B shows the relationship between the pinching force Fc and time according to the first embodiment. This pinching force Fc is a force by which the first link 41 and the second link 42 pinch the human body 43. The pinching force Fc is calculated by dividing the pinching torque Tc by the pinching position x. In FIG. 4B, the solid line indicates the pinching force Fc according to the present invention, the alternate long and short dash line indicates the pinching force according to the prior art, and the broken line indicates the limit safe external force Flim. According to the present invention, the pinching force Fc does not exceed the limit safe external force Flim, whereas according to the prior art, it is understood that the pinching force Fc continues to increase until reaching a preset torque limit value.

  FIG. 4C shows the relationship between the detected joint angle value θ and time according to the first embodiment. In the figure, the solid line indicates the joint angle detection value according to the present invention, and the alternate long and short dash line indicates the joint angle detection value according to the prior art. In the present embodiment, the joint angle command generation unit 12 outputs a joint angle command θr that linearly changes from 1.04 [rad] to 0.52 [rad]. According to the prior art, the joint angle detection value follows the joint angle command θr regardless of the magnitude of the pinching force Fc received by the human body 43. On the other hand, according to the present invention, in order to prevent the pinching force Fc from exceeding the limit safe external force Flim after 3.14 [s], the joint angle detection value θ is suppressed from being smaller than 0.71 [rad]. .

  As described above, according to the present invention, an object pinching position can be accurately estimated without using an expensive sensor or the like, and an appropriate torque command can be generated according to the pinching position. In addition, it is possible to reliably prevent harm to the human body or the like that is sandwiched, and to continue the original operation of the robot arm 1 within a range that does not cause harm to the human body or the like.

  Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention.

  The present invention can be applied to all robots equipped with a link mechanism such as a housework assisting robot, a customer service robot, a care robot, and a rehabilitation robot.

DESCRIPTION OF SYMBOLS 1 Robot arm 11 Arm part 12 Joint angle command generation part 13 Driving force suppression control part 21 Link mechanism 22 Joint motor 23 Joint angle detection part 31 Motor torque calculation part 32 Current control part 33 Clamping detection part 34 Pinch position estimation part 35 Safety motor Torque calculator 41 First link 42 Second link 43 Human body (object)
44 joints

Claims (13)

A control device for a robot arm that controls a motor torque for driving a joint portion of a link mechanism,
The safety motor torque is calculated by applying a restriction based on the safety motor torque profile set in consideration of safety to the basic torque calculated so that the detected value of the joint angle of the joint follows the target value. Motor torque calculating means for
When it is determined that the change in the safety motor torque with respect to the detected value of the joint angle deviates from a reference value defined based on the torque for supporting the weight of the link connected to the distal end side from the joint portion. And pinch detection means for determining that an object is pinched between the adjacent links,
A joint angle when a deviation from the reference value is detected when the pinching detection unit detects pinching of the object, a detection value of the joint angle, a physical property value set in advance for the object, and The distance from the joint portion to the position where the object is pinched based on the pinching torque excluding the torque for supporting the weight of the link connected to the tip side from at least the joint portion from the detected value of the motor torque The pinch position estimating means for estimating the pinch position indicating
The safety motor torque profile is generated so that the upper limit value of the safety motor torque decreases as the pinch position decreases and the upper limit value of the safety motor torque increases as the pinch position increases. Safe motor torque calculation means;
A robot arm control device comprising: The safety motor torque profile is a saturation function or sigmoid function with the basic torque as a variable and an upper limit value of the safety motor torque as a maximum value.
The robot arm control device according to claim 1. The reference value is a sine function of the detected value of the joint angle defined based on the weight of the link and, if the link holds a predetermined object, the weight obtained by adding the weight of the object Represented by
The robot arm control device according to claim 1 or 2. The sine function is represented by the following formula (1):
The robot arm control device according to claim 3.

However, the meaning of the symbols is as follows.
Tm: Motor torque when two adjacent links do not sandwich the object m: Mass of the link farther from the body of the service robot having the robot arm among the two adjacent links (the target (If the object is gripped, its mass is included)
g: Gravity acceleration l: Distance from the joint to the center of gravity of the link farther from the main body of the service robot among the two adjacent links θ: Detected value of the joint angle θ0: Of the two adjacent links Angle formed by the horizontal of the link closer to the main body of the service robot The physical property value is defined based on the Young's modulus and Poisson's ratio of a part of the human body.
The control apparatus of the robot arm as described in any one of Claims 1-4. The sandwiching position is calculated by the following formula (2).
The robot arm control device according to any one of claims 1 to 5.

However, the meaning of the symbols is as follows.
θ: detected value of the joint angle θc: joint angle when a deviation from the sine function is detected Tc: pinching torque E: Young's modulus of human body ν: Poisson's ratio of human body A robot arm control method for controlling a motor torque for driving a joint portion of a link mechanism,
Calculating a basic torque so that a detection value of a joint angle of the joint portion follows a target value;
Calculating a safe motor torque by applying a restriction by a safe motor torque profile set in consideration of safety to the basic torque; and
When it is determined that the change in the safety motor torque with respect to the detected value of the joint angle deviates from a reference value defined based on the torque for supporting the weight of the link connected to the distal end side from the joint portion. Determining that an object is sandwiched between the adjacent links;
Detection of a joint angle when a deviation from the reference value is detected, a detected value of the joint angle, a physical property value set in advance for the object, and the motor torque when the object is caught A pinching position indicating a distance from the joint portion to a position where the object is pinched based on a pinching torque excluding a torque for supporting a weight of the link connected to the tip side from at least the joint portion from a value Estimating, and
The safety motor torque profile is generated so that the upper limit value of the safety motor torque decreases as the pinch position decreases and the upper limit value of the safety motor torque increases as the pinch position increases. Steps,
A method for controlling a robot arm comprising: The safety motor torque profile is a saturation function or sigmoid function with the basic torque as a variable and an upper limit value of the safety motor torque as a maximum value.
The method for controlling a robot arm according to claim 7. The reference value is represented by a sine function defined based on the weight of the link and, when the link holds a predetermined object, a weight obtained by adding the weight of the object.
The method for controlling a robot arm according to claim 7 or 8. The sine function is represented by the following formula (3):
The robot arm control method according to claim 9.

However, the meaning of the symbols is as follows.
Tm: Motor torque when two adjacent links do not sandwich the object m: Mass of the link farther from the body of the service robot having the robot arm among the two adjacent links (the target (If the object is gripped, its mass is included)
g: Gravity acceleration l: Distance from the joint to the center of gravity of the link farther from the main body of the service robot among the two adjacent links θ: Detected value of the joint angle θ0: Of the two adjacent links Angle formed by the horizontal of the link closer to the main body of the service robot The physical property value is defined based on the Young's modulus and Poisson's ratio of a part of the human body.
The method for controlling a robot arm according to any one of claims 7 to 10. The sandwiching position is calculated by the following formula (4).
The robot arm control method according to any one of claims 7 to 11.

However, the meaning of the symbols is as follows.
θ: detected value of the joint angle θc: joint angle when a deviation from the sine function is detected Tc: pinching torque E: Young's modulus of human body ν: Poisson's ratio of human body A program for controlling a robot arm that controls a motor torque for driving a joint portion of a link mechanism,
In a computer that controls the robot arm,
A method according to any one of claims 7 to 12 is performed.
Robot arm control program.

Images