JP2002177352A - Force controller - Google Patents

Abstract

PROBLEM TO BE SOLVED: To obtain a force controller for smoothly switching a position control to a torque control. SOLUTION: The force controller comprises a force commanding unit 2 for generating a force command to a motor 5, a position detector 21 for detecting a rotary angle of the motor 5, a speed detector 8 for detecting a rotational speed of the motor 3, a force detector 11 for detecting a force of a pressing unit 103, a speed command generator 207 for generating a speed command based on the rotary angle detected value of the detector 21, a subtracter 203 for obtaining a difference of the force command value and the detected value of the detector 121, a limiter 220 for outputting a prescribed value based on the input value from the subtracter 203 and altering an upper limit value or a lower limit value of the output based on the output of the generator 207 and the detected value of the detector 21, a subtracter 222 for obtaining a difference between the output value of the limiter 220 and the detected value of the detector 8, and a power amplifier 224 for driving the motor 5 based on the output of the subtracter 222.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an improvement in a force control device for smoothly switching from position control to torque control.

[0002]

2. Description of the Related Art A conventional force control device is disclosed in Japanese Patent Laid-Open No. 63-191.
This will be described with reference to FIG. Such a force control device is used for rehabilitation used to recover functions of the upper limb and the like. FIG.
, The force control device includes a drive unit, a detection unit, and a control unit. The drive unit includes an arm 1 having an end 2 to which an upper limb is attached, and a rotation shaft 4 fixed to a base end 3 of the arm 1.
And a servomotor 5 having an output shaft 6 connected to a speed reducer 7 via a rotating shaft 4.

A detecting unit includes a speed detector 8 for detecting the rotation speed of the output shaft 6 of the servomotor 5 and a force detector 1 fixed to the arm 1 and detecting a force acting on the arm 1.
1 and a position detector 21 for detecting a position based on the rotation angle of the arm 1.

[0004] The control unit includes a limiter 14 for limiting the detection value of the force detector 11 and a dead zone device 2 having a dead zone for outputting zero to a predetermined value with respect to the detection value of the position detector 21.
3 and a coefficient unit 2 for multiplying the output value of the dead band unit 23
5, a subtractor 27 for obtaining a difference between the output value of the coefficient unit 25 and the output value of the limiter 14, a subtractor 29 for obtaining a difference between the output value of the subtractor 27 and the detection value of the speed detector 8, And a power amplifier 30 for amplifying the output value of the unit 29.

[0005] The operation of the force control device configured as described above will be described with reference to FIG. When the upper limb member drives the arm 1, the position detector 21 is input to the dead zone 23 to detect the position of the arm 1, the dead zone 23 is provided to the coefficient unit 25 the output value theta _r based on the dead zone characteristic, The coefficient unit 25 inputs the value L _r obtained by multiplying the output value θr by the constant Ke to the subtractor 27. Subtractor 27, the output value V _a of the limiter 14,
Seeking speed command value V _r by subtracting the output value L _r of the coefficient unit 25 is input to the subtracter 29, the subtracter 29, the speed command value V
The value obtained by subtracting the detected velocity value V _t of _r and velocity detector 8 controls the servo motor 5 is given to the power amplifier 30.

If the angular position of the arm 1 is within a movable range of a preset value θ _{max to} θ _{min in} the dead band device 23, the detected value of the position detector 21 is input to the dead band device 23. Due to the dead zone characteristics, the output value θ _r of the dead zone device 23 becomes zero, and the output value L _r of the coefficient unit 25 also becomes zero. On the other hand, a force acts on the arm 1 by the upper limb member, the force detector 11 inputs a detected force value F _a to the limiter 14. Limiter 14 if Chikarachi F _a is small, and generates an output value V _a determined by the product of the force value F _a and inclination KL. Accordingly, the subtracter 27, a speed command value V _r based on the output value V _a of the limiter 14 is input to the subtracter 29, the subtracter 29 and the detected value V _t of the velocity command value V _r and the speed detector 8 The power amplifier 30 assists the rotation of the arm 1 of the upper limb by rotating the servomotor 5 based on the difference.

If the upper limb increases the force of the arm 1 so that the angle of the arm 1 exceeds θ _max , the force value Fa detected by the force detector 11 exceeds + FL of the limiter 14 and the limiter 14 , And outputs the limited value VL (V _a ). Meanwhile, the position detector 21 inputs the detected position theta _a to dead band unit 23, the theta dead band unit 23 minus theta _max from the detection position theta _{a e,} a large slope K _p is multiplied value theta _r coefficients enter the vessel 25, coefficient unit 25 inputs the value L _r multiplied by the constant K _e of the value theta _r to the subtracter 27. The subtractor 27
Since it is subtracted from a value VL to the value L _r is the L _r »VL, arm servo motor 5 via the power amplifier 30 is given a speed command V _r in the opposite direction as the force of the arm 1 is applied to the subtracter 29 Rotate in the opposite direction to 1. Accordingly, the position feedback operation of the arm 1 is made, the arm 1 will not greatly exceed the movable range of the vertical position θ _{ma x} ~θ _min.

When the upper limb body applies a force to the arm 1 to perform constant velocity control and the position of the arm 1 becomes θ _{max and} θ _min , the position command L _r, that is, the output of the coefficient unit 25 becomes zero. The limiter 14 gives a speed command according to the force applied to the arm 1 by the upper limb to the subtractor 27 and the power amplifier 30, and rotates the servomotor 5 in the direction in which the upper limb is rotating the arm 1. Therefore, it is possible to prevent the arm 1 from suddenly stopping, and to prevent an impact force from being applied to the upper limb.

[0009]

However, in the force control device configured as described above, the range in which the position control operates is determined by the characteristics of the dead band device 23. That is, unless the angular positions θ _{max and} θ _min are exceeded, position control is not performed, and
At the angular positions θ _{max and} θ _min , constant speed control (torque control) is performed, and no position control is performed. Therefore, such a force control device executes torque control up to a predetermined position, and executes position control when it reaches the predetermined position. For this reason, a force control device that executes position control up to a predetermined position, and executes torque control when it reaches the predetermined position, for example, an arm that is placed on a platform and is separated by a predetermined distance. By moving the arm by position control,
There has been a problem that switching by pressing a predetermined amount cannot be performed smoothly by the torque control.

SUMMARY OF THE INVENTION The present invention has been made to solve the above problems, and has as its object to provide a force control device that smoothly switches from position control to torque control.

[0011]

A force control device according to a first aspect of the present invention is a force control device for continuously shifting a pressing portion driven by a motor from position control to torque control. A force command generating means for generating a force command to the motor; a position detecting means for detecting a rotation angle of the motor; a speed detecting means for detecting a rotation speed of the motor; and detecting a force of the pressing portion. Force detection means, speed command generation means for generating a speed command based on the rotation angle detection value of the position detection means, and first subtraction means for obtaining a difference between the force command value and the detection value of the force detection means And the first
Limit means for outputting a predetermined value based on an input value from the subtraction means, and changing an upper limit value or a lower limit value of the output based on an output of the speed command generation means and a detection value of the angle detection means. A second subtraction means for obtaining a difference between an output value of the limit means and a detection value of the speed detection means; and a control means for driving the motor based on the difference of the second subtraction means. It is characterized by having. According to such a solution, in the case of position control,
It operates at the command speed of the speed command generating means by the limiter means, and when it approaches the force command from the force detecting means, it automatically switches to the force command from the force command generating means. Can be switched without impact. Therefore, for example, the hardware placed on the table,
With the arm separated by a predetermined distance, the arm is moved by the position control, and the switching of pressing the predetermined amount by the torque control can be automatically and smoothly performed.

A force control device according to a second invention is characterized in that the limit means in the first invention continuously changes the upper limit value or the lower limit value based on the output value of the speed command generation means. It is. According to such a solution, in the case of the position control, the limiter continuously operates the limiter value based on the command speed of the speed command generating means by the limiter means.
There is an effect that switching can be performed.

The force control device according to a third aspect of the present invention is the first or the second aspect of the invention, wherein the storage means for storing a stop position of the pressing portion, a stop start position for determining the start of the stop of the pressing portion, Judging means for judging that the stop start position has been exceeded based on the detection value of the detecting means; and when the judging means judges that the stop start position has been exceeded, the motor is controlled to stop at the stop position. Speed limiting means for limiting the rotation speed. According to such a solution, the determining means:
If it is determined based on the angle detecting means that the stop start position has been exceeded, the speed limiting means limits the rotational speed of the motor so that the motor stops at the stop position. Therefore, there is an effect that the motor can be stopped so as not to exceed the stop position even when the torque control is performed.

[0014]

DESCRIPTION OF THE PREFERRED EMBODIMENTS Embodiment 1 An embodiment of the present invention will be described with reference to FIGS. FIG. 1 is an overall block diagram of a force control device showing one embodiment of the present invention, FIG. 2 is a memory map diagram of a storage unit of a speed command generation unit,
FIG. 3 is a time chart showing the operation of each part of the force control device. In FIG. 1, the same reference numerals as those in the related art indicate the same or corresponding parts, and a description thereof will be omitted. 1 and 2, a force control device includes a hardware 107 mounted on a columnar table 105.
Is moved by the position control of the arm 1 which is separated by a predetermined distance, and is switched to the torque control to press the metal object 107 by the pressing portion 103 of the arm 1 by a predetermined amount.

The force control device includes a driving unit 1 having detecting means.
00, the control unit 200, and the driving unit 100
And a servomotor 5 having an output shaft 6 via a reduction shaft 7 to which the base end 3 of the arm 1 is fixed, and a reduction gear 7 connected to the rotation shaft 4. Consists of

The detecting means detects the rotation speed of the output shaft 6 of the servomotor 5, and has a speed detector 8 having an output characteristic 8a proportional to the rotation speed;
It comprises a force detector 11 having an output characteristic 11 a proportional to the force acting on the arm 3 and a position detector 21 detecting a position based on the rotation angle of the arm 1.

The control unit 200 includes a force commanding section 201 outputs a constant force command value F _r regardless of the rotational position of the servo motor 5, a force command value of the detected value and the force command unit 201 of the force detector 11 and , An amplifier 205 for amplifying the output value of the subtractor 203, a speed command generator 207 for generating a predetermined speed command based on the position detected by the position detector 21, and a speed command A limit unit 220 for limiting the output value to two levels VL ₁ and VL ₂ based on the command value of the generation unit 207 and the position of the pressing unit 103 of the arm 1;
A subtractor 222 for obtaining a difference between an output value of the limit unit 220 and a detection value of the speed detector 8; and a power amplifier 22 for amplifying the output value of the subtractor 222 and for controlling the driving of the motor 5.
4 is provided.

The speed command generating unit 207 generates a speed command for rotating the arm 1 as shown in FIG. 3 with respect to the distance from the pressing surface of the pressing unit 103 to the metal object 107. The pressing surface of the pressing portion 103 is
When the speed is reduced at a speed V _{r β} from a distance L ₁ which is slightly away from 7 and reaches a low speed V _r2 , the tip of the pressing portion 103 moves by a constant low speed V _r2 , and the pressing portion 103 presses a metal object 107 to A stop command is generated when the amount of pressing reaches L _P , and an input interface (hereinafter, the interface is referred to as an I / F) 209 for inputting a detection signal detected by the position detector 21 and a well-known interface. CPU 211
A storage unit 213 storing a processing procedure program, setting data, and the like; a D / A converter 215 for converting a digital output signal into an analog signal; and a D / A converter 21
5 is connected to the output I / F 215.

The storage unit 213 stores a first speed region 213a in which the first speed command V _r1 is stored and a second speed region 213b in which the creep speed as the second speed command V _r2 is stored.
When, from the first speed command V _r1 and the second speed switching position L ₁ is stored switching position region 213c to command V _r2, acceleration region 213d acceleration α is stored from the start until the speed command V _r1 And the speed command _Vr1 to the speed command V
Deceleration area 213e in which the deceleration until _r2 is stored
And

The limiter unit 200 receives the signal from the amplifier 205
Input value F_iAs shown in FIG.
Detecting position of the pressing part 103 of the arm 1
Position L detected by detector 21_a<Switching position L₁so,
And speed command V_r≧ V_r1In the input value from zero to F₁Until
Is the speed value VL₁At the limit of
Input value F_iOutputs a value proportional to the speed command V
The upper and lower limits are continuously increased in proportion to the value of r.
It is formed to be restricted. In addition, a position detector
21 detection position L_a> Switching position L₁Over speed finger
Order V _r≧ V_r2Then, the input value is changed from zero to F_TwoUp to the speed value
VL_TwoInput value F from amplifier 205 at the limit of_iCompared to
Outputs the value shown in the example and the speed command V_rThe magnitude of the value of
The upper and lower limits are continuously limited in proportion to
Is formed.

The amplifier 205 is formed so that the output value F _i obtained by multiplying the input value _Fr by the gain G is larger than the input values F ₁ and F ₂ of the limiter unit 220 (expressed by an equation). Then, F _i = F _r × G> F ₂ > F ₁ ). this is,
Position control, that is, the detection value of the force detector 11 is zero,
If the force commanding section 201 of the force command value F _r, the limiter portion 22
0, the output becomes a limit VL ₁ or VL _2, without the control by the force command, in order to operate the servo motor 5 by the speed command based on the position command.

FIG. 1 shows the force control device configured as described above.
This will be described with reference to FIGS. FIG. 4 is a flowchart showing the operation of the speed command generator. Table 1 as shown in FIG.
And hardware 107 loaded on the 05, in a state where the tip of the pressing portion 103 is separated a predetermined distance, at time t _s, the start command of the pressing portion 103 is input to the control unit 200, the force commanding section 201 Outputs a constant force command _Fr ,
The pressing portion 103, the force is not applied, it detected value of the force detector 11 is zero, the subtracter 203 obtains a deviation between the force command F _r and detected value, the amplifier 205, deviation Is amplified and input to the limiter unit 220.

On the other hand, the CPU 21 of the speed command generator 207
1 determines the start of operation based on whether or not a start command has been input (step S101).
Enter the position L _a detected from the position detector 21 I / F20
, Which becomes the amount of change to the previous position together with the reading via
The sum with the distance is determined as the current position (step S10).
3) compares the start position L ₁ switched current position (step S105), since it is not between it began to move the pressing portion 103 of the arm 1, the position L ₁ current position <switching.

[0024] Therefore, CPU 211 adds the [alpha] t _r obtained by multiplying the calculation cycle t _r the acceleration α to the previous speed command V _r, determine the time of the speed command V _r (step S107),
The speed command V _r is determined whether it exceeds a speed command V _r1 (step S109), initially accelerating the arms 1, because it is the speed command V _r <V _r1, outputs a speed command V _r I / F
217.

On the other hand, the subtractor 203
Command value F_rValue obtained by subtracting the detection value zero of the force detector 11 from
F_rIs input to the amplifier 205, and the amplifier 205_r
Multiplied by the gain G_iTo the limiter unit 220
input. The limiter unit 220 detects the position of the position detector 21.
Position L_aAnd switching position L₁Is L_a<L₁, Input value F
_i> F₁Therefore, the limit value VL₁Speed in the range
Limit value V according to the value of speed command Vr of command generating section 207
L, and the subtractor 222 detects the speed
Rotation speed value V of the servo motor 5_aAnd the limiter 2
20 and a deviation from the output value VL of the power amplifier 20 is obtained.
The deviation is amplified to drive and control the servo motor 5 to control the arm.
Rotate 1 That is, as shown in FIG.
Speed V_{r α}In the stage of accelerating at the limit part 22
0 is the force command F of the force command unit 201_rLimit value VL₁of
In the range, the position detector from the speed command generator 207
Speed command V based on 21 detection commands _rBased on servo
Since the motor 5 is controlled, the arm 1 performs position control.
Is what you are doing.

[0026] Eventually, at step S109 the speed of the pressing portion 103 of the arm 1 is raised, the V _r> V _r1 is satisfied, and the speed command V _{r 1} of the speed command generating portion 207 through the output I / F 217 The data is input to the limiter unit 220 (step S111). Furthermore, moves arm 1, the pressing portion 103 is greater than the position L ₁ switching the vicinity of the hardware 107, the position L ₁ current position> switching is satisfied in step S105, CPU 211 may deceleration β from the previous velocity command computation cycle t _r by subtracting the [beta] t _r multiplied by seeking this speed command (step S119), the speed command V
The _r, compared to the creep velocity V _{r 2} which is determined in advance (step S121), the speed command V _r is higher than the creep rate V _{r 2,} a digital command to an analog command by D / A converter 215 , And input to the limiter unit 220 via the output I / F 217. Eventually, the pressing portion 1 of the arm 1
03 reaches _Vr2 , the speed command generator 207
Inputs V _r2 to the limiter unit 220 (step S1).
23).

Switching position L₁Exceeds the limiter unit 2
The maximum limit output of 20 is VL₁To VL _TwoBecome
Then, the pressing portion 103 at the tip of the arm 1 comes into contact with the hardware 107.
Starting position L₀, The force detector 11 detects
Force value F_aIncreases, and the subtractor 203 outputs the force command F_rAnd the force value
F_aAnd the deviation is reduced, and the amplifier 205
Is F_TwoSmaller value F_iIs input to the limiter unit 220.
You. Therefore, the limiter unit 220 outputs the input value F_iCompared to
The example output value VL is output, and the subtractor 222 detects the speed.
Value V of detector 8_aAnd the deviation between the output value VL and
The amplifier 224 drives the servo motor 5 based on the deviation.
Move. That is, the pressing portion 103 of the arm 1 is
7, the force command value F of the force command unit 201 is_r
And the detection value F of the force detector 11_aAnd the difference F_iBased on the
The limit unit 220 has a limit value VL₁Generates output VL within
The torque control is being performed.

Eventually, the force command value F of the force command unit 201
and _i, when the detected value F _a of the force detector 11 matches, zero output of the subtracter 203, also stops the rotation of the servo motor 5 becomes an output also zero limiter portion 220, the arm 1
Pressing portion 103 also stops at a position L _p.

Embodiment 2 Another embodiment of the present invention will be described with reference to FIG. FIG. 5 is a block diagram of a force control device showing another embodiment of the present invention. 5, the same reference numerals as those in FIG. 1 denote the same or corresponding parts, and a description thereof will be omitted. In this embodiment, even if torque control is performed in the force control device, if the arm 1 rotates and moves beyond the stop start position, the rotation speed of the arm 1 is limited to stop at the stop position. is there. In FIG. 5, the direction in which the arm 1 rotates counterclockwise is referred to as forward movement, and an example in which the arm 1 is stopped at a predetermined stop position by movement in the forward direction will be described below.

In FIG. 5, the stroke limit unit 30
0 is a signal for generating a speed limit command to the servomotor 5 based on the stop start position in the forward movement of the arm 1. The detection signal of the speed detector 8, the output signal of the limiter unit 220, and the detection signal of the position detector 21 are Input I / F 309 to input
, A well-known CPU 311, a storage unit 313 storing a processing procedure program and setting data, and a D / A converter 315 for converting a digital output signal into an analog signal.
And an output I / F connected to the output of the D / A converter 315
317.

The storage unit 313 stores a stop position L _{s of the} arm 1.
There a stop position region 313a stored, the start position area 31 start position L _f is stored the arm 1 starts decelerating
Consists and 3b, a top speed region 313c of maximum speed V _max of the deceleration-start of the arm 1 is stored, the maximum deceleration time domain 313d that stores the maximum deceleration time until the arm 1 is stopped from the maximum speed .

Now, the storage unit 313 stores the start position L
_f (m), the stop position L _s (m), maximum velocity V _max (m /
s), the actual measured value of the maximum deceleration time t _{β max} (s) is stored in each storage area, and when the arm 1 exceeds the stop start position _Lf , the rotational speed V of the arm 1 is limited.
_rL (m / s) is obtained as follows. First, necessary deceleration distance L to stop the arm 1 to the stopping position L _{s
When e} (m), the following equation is established. L _f = L _s −L _e (1) Further, _assuming that the current speed V _p (m / s) and the deceleration time t _{β r} (s) from the current speed V _p to the stop, the required deceleration distance _Le
Becomes the following equation. _{L e = V p · t β} r / 2 ···· (2)

On the other hand, the deceleration time t _{β r is changed} to the maximum deceleration time t _β
max, the relationship between the maximum velocity V _max is the following formula. t _{β r} = t _{β max} · (V _p / V _max ) ··· (3) When the deceleration time t _{β r} in equation (3) is substituted into equation (2) and rearranged, the required deceleration distance _Le becomes It becomes an expression. ^{_{L e = V p 2 · t}} β max / (2V max) ···· (4) Therefore, the stop starting position L _f is the following formula. ^{_{L f = L s -V p 2}} · t β max / (2V max) ····· (5)

When the arm 1 rotates counterclockwise and exceeds the stop start position, the speed limit _VrL for stopping the arm 1 at the stop position is _obtained by _{modifying the} above (5) by the following equation. . V _{r L} = {(2L _e · V _max ) / t _{β max} } ^1/2 (6) That is, the CPU 311 of the stroke limiter unit 300
You may enter the detection position L _a from the position detector 21 I / F3
Read through 09, determined by comparing the start position L _f of the set stopped in the storage unit 313, when the start position L _f exceeds the by the speed limit V _rL of the arm 1 (6) , The speed limit command V _rL from the output I / F ₃₀₉ via the D / A converter 315 is input to the subtractor 222 to drive the servo motor 5.

[0035] Incidentally, the stroke limiter 300, the arm 1 is to over start position L _f stop, simply operates as a buffer circuit. Further, the embodiment is
Although the forward movement for rotating the arm 1 in the counterclockwise direction has been described, the speed for the backward movement for rotating the arm 1 in the clockwise direction may be limited based on the same concept as in the above embodiment.

[Brief description of the drawings]

FIG. 1 is a block diagram illustrating an overall configuration of a force control device according to an embodiment of the present invention.

FIG. 2 is a memory map diagram of a storage unit of the speed command generator shown in FIG.

FIG. 3 is a time chart showing the operation of each part of the force control device shown in FIG.

FIG. 4 is a flowchart showing an operation of a speed command generator shown in FIG. 1;

FIG. 5 is a block diagram showing an overall configuration of a force control device showing another embodiment of the present invention.

6 is a memory map diagram of a storage unit of the speed command generator shown in FIG.

FIG. 7 is an overall block diagram showing a conventional force control device.

[Explanation of symbols]

5 servo motor, 8 speed detector (speed detection means),
11 force detector (force detection means), 21 position detector (angle detection means), 201 force command unit (force command generation means),
203 first subtraction means (subtractor), 207 speed command generation section (speed command generation means), 220 limiter section (limit means), 222 second subtraction means (subtractor), 2
24 power amplifier (control means), 300 stroke limiter section (speed limiting means), 313 storage section (storage means).

Claims (3)

[Claims] 1. A force control device for continuously shifting a pressing portion driven by a motor from a position control to a torque control, a force command generating means for generating a force command to the motor, and a rotation angle of the motor. Position detecting means for detecting
Speed detection means for detecting the rotation speed of the motor, force detection means for detecting the force of the pressing portion, speed command generation means for generating a speed command based on a rotation angle detection value of the position detection means, A first subtraction means for obtaining a difference between the force command value and the detection value of the force detection means; a predetermined value based on an input value from the first subtraction means; A limiter for changing an upper limit value or a lower limit value of the output based on the output of the angle detector and the detection value of the angle detector, and a second method for calculating a difference between an output value of the limiter and a detection value of the speed detector. A force control device comprising: subtraction means; and control means for driving the motor based on the difference between the second subtraction means. 2. The force control device according to claim 1, wherein said limit means continuously changes said upper limit value or lower limit value based on an output value of said speed command generating means. 3. A storage unit for storing a stop position of the pressing unit and a stop start position for determining a stop start of the pressing unit, and determining that the stop start position has been exceeded based on a detection value of the angle detection unit. And a speed limiter that limits the rotation speed of the motor so that the motor stops at the stop position when the stop start position is determined by the determiner. The force control device according to claim 1 or 2, wherein