JP4247382B2 - Control device for rotating mechanism - Google Patents

Description

[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a rotation mechanism control device that controls a rotation angle in a rotation portion of a rotation mechanism in consideration of torque generated in a rotation mechanism such as a robot arm joint.
[0002]
[Prior art]
On orbits such as the Earth orbit, it is necessary to capture moving objects such as rendezvous objects and satellite remnants with robot arms provided on other satellites. In order to bring the robot arm on the satellite and the target object that moves relative to the satellite closer to each other, for example, if a force is applied to the satellite by a thruster, the orbit of the satellite is changed and also for capture. When the arm is operated, the position and attitude of the satellite itself change due to the reaction. Therefore, the control of the distance and speed between the two is much more complicated than the case of both the ground. In addition, when the robot arm comes into contact with or collides with the target object during capture, the relative distance and relative distance between the two even before the target object is captured so that the control amount such as force and torque acting between the two does not become excessive. It is necessary to control the speed precisely. When the target object is captured with a large relative inertia when the mutual load inertia is large, the target object is decelerated and stopped by friction of the rotating machine. Unexpected load may be applied to each part of the satellite, the target object, or the robot arm, and an unexpected situation may occur.
[0003]
Therefore, when capturing an object to be moved by a robot such as an arm or manipulator provided on the satellite, the object is captured after following the movement of the target by reducing the relative position and speed between them as much as possible. Thus, it has been proposed to reduce the impact at the time of capture (see Patent Document 2) or to strengthen the capture mechanism so that it can withstand some impact at the time of capture (see Patent Document 3).
[0004]
On the other hand, it is mounted on a spacecraft such as a space shuttle or an artificial satellite, and pins are inserted and bolted / loosened to work on its own or other spacecraft, such as construction of a space station or capture of other satellites. In addition, a multi-joint robot for space work that performs various kinds of work such as handle work has been proposed. In such an articulated robot, each joint angle is controlled so as to achieve an angle target value set for each joint, but it is necessary to control a force applied to the end effector so as not to damage the object. As a robot control device that performs force control, compliance control is known in which a virtual impedance is indirectly realized to relieve a force applied to the tip of a robot arm. In compliance control, there is a method that enables stable and high-speed operation by directly implementing the compliance operation by installing hardware that gives flexibility to the robot arm, but the characteristics of the hardware are set once Then, there is a problem that the change cannot be made and the control is not flexible. Thus, for example, an end effector provided at the tip of a robot arm is assumed to be a virtual compliance mechanism having a virtual mass, a virtual viscosity coefficient, and a virtual spring stiffness by software. The characteristics of the virtual compliance mechanism reflect the actuality well and are expressed in a simplified manner, so that the inertial force term including the second derivative of the position, the viscous force term including the first derivative of the position, and the spring force as the first term of the position It is assumed that it is expressed by a second-order linear differential equation including a term. In the virtual compliance control, the motion of the virtual compliance mechanism is calculated in real time by a predetermined algorithm, and the operation of the robot follows the operation of the virtual compliance mechanism. Since the characteristics of the virtual compliance mechanism can be changed by software according to the contents of work, optimum flexibility can be given according to the situation (see Patent Document 4).
[0005]
In order to make it easy to capture a target floating in outer space, a space robot control device has been proposed that detects the impact force at the time of capture with a sensor and modifies the capture mechanism to reduce the impact (Patent Literature). 1). According to this space robot control device, by using a dynamic model, the position / velocity of the artificial satellite is detected by a sensor mounted on the artificial satellite, and the artificial satellite accompanying the operation of the manipulator is based on this detection information. Generates the tip speed of the manipulator that takes into account changes in the position and orientation of the manipulator, and performs the tracking operation of the manipulator to the target. Is used to generate an approach trajectory to the target, and a force sensor provided on the wrist part of the manipulator detects the force acting on the manipulator when the gripper grips the target. D The gripping operation is executed while driving the motor of each axis of the purulator.
[0006]
The space robot control device will be described in detail. In the manipulator tip speed generator, assuming a damper (viscosity damping coefficient C) having a dead zone at the tip position of the manipulator, when the gripper grips the gripped portion of the target, The tip speed of the manipulator is generated so as to be the same as the motion when the force detected by the force sensor is applied, and the swing of the target when the target is gripped is reduced. That is, when the force detected by the force sensor exceeds a predetermined force setting value, the tip speed of the manipulator is generated, and when the force is less than the set value, the tip speed of the manipulator is set to zero. Yes. Further, when grasping the target, the manipulator tip position corrector assumes an inertial body of mass M connected by a spring having a spring constant K and a damper having a viscous damping coefficient C around the target position of the manipulator. The corrected sequential target position of the manipulator is generated so as to be the same as the motion when the force detected by the force sensor is applied. Further, in the manipulator tip speed generator, the tip speed of the manipulator is generated by PID control from the corrected sequential target position and the current position, and the manipulator joint speed generator obtains the speed command value of each joint of the manipulator, Each axis motor of the manipulator is driven.
[0007]
However, when the load inertia of the object to be moved is large, when the robot arm or the rotation mechanism performs a control operation such as decelerating / stopping after capturing the object to be moved, a sudden deceleration occurs due to the friction of the rotation mechanism, and the motor shaft By stopping rapidly, an excessive load is applied to each mechanism part. Such an excessive load constitutes a heavy burden / restriction on each mechanism part such as a robot arm (joint part, gripping part), an arm mounting part on a satellite, a target part to be gripped.
[0008]
[Patent Document 1]
JP-A-5-104469 (paragraphs [0009] to [0022],
(Fig. 2)
[Patent Document 2]
JP-A-6-127495 (paragraphs [0008] to [0022],
1 to 3)
[Patent Document 3]
JP 2001-260997 A (paragraph [0016], FIG. 1)
[Patent Document 4]
JP-A-10-12885 (paragraphs [0001] to [0009]
3 to 5)
[0009]
[Problems to be solved by the invention]
Therefore, the engaging portion is engaged with the object to be moved, such as when the engaging portion provided on the robot arm or manipulator having a rotating portion on the base end side collides with the object to be moved or captures or grips the object to be moved. When torque acts on the rotating part due to the combination, when it is necessary to control the rotating part at a predetermined rotation angle, an excessive force is prevented from acting on the rotating part, There is a problem to be solved in terms of controlling the rotation angle of the rotating part.
[0010]
The object of the present invention is to consider the torque that acts on the rotating mechanism after engaging with the object to be moved when the engaging part such as a robot arm or manipulator provided in the rotating part collides, captures or grips with the object to be moved. By controlling the rotation angle of the rotation part of the rotation mechanism while preventing an excessive force from acting on the rotation part, the rotation part on which the torque acts can be controlled at a predetermined rotation angle. A control device for a rotating mechanism is provided.
[0011]
[Means for Solving the Problems]
In order to solve the above-described problems, a control device for a rotating mechanism according to the present invention provides a target rotation angle θ as a control signal to a motor that rotates a rotating portion of the rotating mechanism. _ref In the rotation mechanism control device that controls the rotation angle of the rotation unit by outputting the target braking torque τ from the braking torque profile generation unit _com Based on the target compliance angular displacement θ _cc Target command angle θ from the torque follow-up control unit and the braking operation profile generation unit _com Is detected by the rotation angle detection unit. _es Corrected target command angle θ ′ corrected based on _com The torque limit control unit that outputs the detected torque τ acting on the rotating unit detected by the torque detection unit _s Virtual compliance angular displacement θ _d A virtual compliance angular displacement θ. _d The target compliance angular displacement θ _cc And the corrected target command angle θ ′ _com And the target rotation angle θ _ref Is provided with a compliance control unit that outputs.
[0012]
According to this rotation mechanism control device, the compliance control unit detects the detected torque τ acting on the rotation unit detected by the torque detection unit. _s The compliance control filter receives the input of the virtual compliance angular displacement θ _d Is output. The torque follow-up control unit receives the target braking torque τ from the braking torque profile generation unit. _com Based on the target compliance angular displacement θ _cc Is output. In addition, the torque limit control unit receives the target command angle θ from the braking operation profile generation unit. _com The rotation angle θ detected by the rotation angle detection unit _es Corrected target command angle θ ′ corrected based on _com Is output. The compliance control unit converts the corrected target command angle to the virtual compliance angular displacement θ _d And target compliance angular displacement θ _cc And the target rotation angle θ as a control signal to the motor that rotates the rotating part of the rotating mechanism. _ref Is output. As described above, a virtual dynamic model for the rotation mechanism is set, and the detected torque τ _s Virtual compliance angular displacement θ based on the dynamic model _d The virtual compliance angular displacement θ _d The target command angle θ from each profile generator for braking force and braking torque _com And target braking torque τ _com And the target rotation angle θ obtained by the correction _ref Since the rotation angle of the rotating portion is controlled by outputting the rotation angle, the rotation angle of the rotating portion of the rotating mechanism is controlled in consideration of the braking torque and the braking force, and an excessive force is applied to each mechanism portion. Does not act on.
[0013]
In this rotation mechanism control device, the compliance control filter is described by a transfer function described based on virtual inertia, virtual viscosity, and virtual rigidity. The equation of motion established between the angle and the torque is generally expressed as an inertia term expressed as a second derivative term of the angle, a velocity (viscosity) term expressed as a first derivative term of the angle, and a proportional term of the angle. It is expressed by a second order differential equation consisting of expressed force (torque) terms. A model of a rotating mechanism in which a transfer function is described based on virtual inertia, virtual viscosity, and virtual stiffness is a virtual dynamic model.
[0014]
In this rotation mechanism control device, the torque limit control unit is configured to output the target command angle θ. _com With respect to the target command angle θ _com And the detected rotation angle θ _es The corrected target command angle θ ′ is corrected by correcting the difference input with the output obtained by performing the dead band process. _com Can be output. By performing the dead band process, the detected rotation angle θ _es Is the target command angle θ _com Target command angle θ _com Is directly output from the torque limit control unit without being corrected.
[0015]
In this rotation mechanism control device, the dead zone processing is performed by the target command angle θ. _com And the detected rotation angle θ _es Because the output is not output when the difference between the target command angle θ and the target command angle θ _com Without being corrected, the corrected target command angle θ ′ _com As the output of the torque limit control unit. When the difference exceeds the predetermined deviation, the target command angle θ _com For example, the target command angle θ _com The output is corrected based on the difference according to the value of the target command angle θ. _com Is subtracted by the corrected output, and the result is used as the output of the torque limit control unit.
[0016]
In the rotation mechanism control device, the torque follow-up control unit may include the target braking torque τ. _com Is the target compliance angular displacement θ _cc It consists of output as. In compliance control, taking the case where the tip of the arm is pressed against the wall, which is the object, as an example, if the target value does not bite into the wall (that is, the target value is not virtual bite), the tip of the arm is moved. Actually, it is impossible to generate a pressing force against the wall. When the target is in a virtual bite state but the tip of the arm is actually restricted by the wall, the drive system presses the tip of the arm against the wall in an attempt to eliminate the difference between the target and the reality. At this time, it is assumed that the above difference is absorbed by the displacement of the virtual compliance mechanism, and the pressing force can be computationally controlled by the spring term and the viscosity term corresponding to the displacement. Therefore, in order to brake the moving object by the robot, the compliance angle displacement θ as a target is set in the compliance control unit. _cc Need to give. Target compliance angular displacement θ _cc As the target braking torque τ _com However, it is preferable to adopt a simple one in order to avoid complicated control. Therefore, given target braking torque τ _com An angular displacement (simply a target braking torque τ) that is assumed to occur only in accordance with the virtual torsional rigidity of the virtual compliance mechanism without taking into account dynamic fluctuations (which itself changes) _com Proportional to the target compliance angular displacement θ _cc Adopt as.
[0017]
In this rotation mechanism control device, the braking torque profile generator generates the target braking torque τ. _com Is set as a constant value until a predetermined time, and is set as a function that gradually decreases from the predetermined time to the stop time of the rotating unit. Target braking torque τ _com By setting the value to a constant value until a predetermined time, it is possible to control so as not to generate an excessive braking torque with a large damage to each mechanism unit. If the braking of the rotating part proceeds to a certain extent, the rotating part can be reliably stopped even if the subsequent target braking torque is set small.
[0018]
In this rotation mechanism control device, the braking operation profile generation unit is set as a function of the braking force with respect to the speed of the tip of the arm provided in the rotation unit, and the function is the speed of the tip of the arm. Is increased according to the speed of the tip portion up to a predetermined speed, and is saturated to a constant braking force with respect to a speed equal to or higher than the predetermined speed. When the speed of the tip of the arm is equal to or higher than the predetermined speed, the braking force is not saturated and increased, so that the arm moves at a high speed after the arm, for example, is engaged with the object to be moved. Even if it is, the braking force is limited to avoid an excessive braking torque, and when the speed of the tip of the arm is lower than the predetermined speed, the braking force is reduced so that the arm is an object to be moved. The generation of a large braking force is prevented at the initial stage and the final stage of braking.
[0019]
DETAILED DESCRIPTION OF THE INVENTION
Embodiments of a rotation mechanism control device according to the present invention will be described below with reference to the accompanying drawings. FIG. 1 is a block diagram showing an example of a joint motion control device as an embodiment of a rotation mechanism control device according to the present invention, FIG. 2 is a schematic diagram of a joint system to which this rotation mechanism control device is applied, and FIG. 4 is a diagram showing an example of a dead zone characteristic of a torque limit control unit in the joint motion control device shown in FIG. 1, FIG. 4 is a diagram showing an example of a deceleration operation profile in the joint motion control device shown in FIG. 1, and FIG. It is a diagram which shows an example of the braking torque profile in the joint operation control apparatus shown.
[0020]
According to the joint system shown in FIG. 2, the joint mechanism 2 attached to the airframe 1 such as an artificial satellite as the rotation mechanism in the present invention can be relatively rotated around the inner speed reduction mechanism 3 and the speed reduction mechanism 3. And the arm 5 attached to the rotating unit 4. The arm 5 is provided with a hand 6 that can be engaged with the movement target object A at the tip. The speed reduction mechanism 3 is connected to the output shaft of the motor 7 fixed to the machine body 1, and the rotation output of the motor 7 is decelerated by the speed reduction mechanism 3 and given to the rotating unit 4. In order to detect the rotation angle of the rotation unit 4, the joint mechanism 2 is provided with a joint angle detector (corresponding to a “rotation angle detection unit” in the present invention) 8. Further, a joint torque detector (corresponding to a “torque detector” in the present invention) 9 is provided on the joint output shaft portion (also integrated with the arm 5) of the joint mechanism 2. Detected joint angle θ detected by the joint angle detector 8 _es Detected joint torque τ detected by the joint torque detector 9 _s Are input to a joint motion control device (hereinafter abbreviated as “control device”) 10.
[0021]
Now, when trying to capture the moving target object A with the arm 5, the target object A collides with the hand 6 of the arm 5, and then the rotation of the rotating unit 4 to be rotated is decelerated and stopped. It is conceivable to mitigate the collision by controlling. When the mass of the object A to be moved is large, the load inertia until the stop is large. Therefore, in order to stop in a short time, the braking operation is a sudden deceleration / stop by the friction of the rotating mechanism. In other words, an excessive load acts on each mechanism such as a motor shaft, which becomes a heavy burden and restriction on the operation of the joint system. Even in such a case, the control device 10 tries to control the movement of the joint mechanism 2 so as to stop the movement target object A without causing excessive torque to be generated in the joint. Detected joint torque τ which is sensor output information from the joint torque detector 9 arranged in the section _s The control device for driving the motor 7 that attempts to apply the braking torque to the movement target object A according to the target braking torque profile.
[0022]
As shown in FIG. 1, the control device 10 includes a compliance control unit 11, a torque limit control unit 12, a torque follow-up control unit 13, a braking operation profile generation unit 14, a braking torque profile generation unit 15, and the like. That is, the control device 10 detects the detected joint angle θ that is detected and input from the joint angle detector 8 every moment. _es Detection information and detected joint torque τ input from the joint torque detector 9 every moment _s On the basis of the detected information of the target joint angle θ as a command to the motor 7 which is a joint angle control unit. _ref Has the function to output the momentarily.
[0023]
The compliance control unit 11 detects the detected joint torque τ detected by the joint torque detector 9. _s Detected joint torque τ _s The low-pass filter 11a that passes only the low-frequency component of the filter, and the torque signal τ filtered by the low-pass filter 11a _f And a compliance control filter 11b that performs a compliance filter process on the motor 7 serving as a joint angle controller. _ref Is output. The low-pass filter 11a detects the detected joint torque τ that is an input signal. _s The signal noise is removed by cutting the high-frequency component of the signal, and the gain is lowered in the high frequency range to ensure a gain margin. The compliance control filter 11b generates a torque signal τ _f In the S (complex parameter in Laplace transform) region, the joint compliance filter processing represented by the following equation using the virtual joint impedance parameter is performed, and the compliance angular displacement θ _d Is calculated. The filtering process by the compliance control filter 11b is realized by a digital filter calculation process.
θ _d = (1 / (m _c ・ S ² + C _c ・ S + k _c )) ・ Τ _f (1)
Where m _c : Joint virtual load inertia
c _c : Virtual joint viscosity coefficient
k _c : Joint virtual torsional rigidity
τ _f : Detected joint torque τ _S Joint torque after low-pass filtering
[0024]
The torque limit control unit 12 detects the detected joint angle θ detected by the joint angle detector 8. _es Commanded joint angle θ from a braking action profile generation unit 14 to be described later _com And a controller that outputs to the compliance controller 11 with saturation characteristics. As shown in FIG. 3A, the control characteristic of the torque limit control unit 12 is a region including the input zero (−ε ₀ ≤ε _com (= Θ _com −θ _es ) ≦ ε ₀ )), A joint angle correction amount (θ ′) for correcting the target joint angle as shown in FIG. _com −θ _es ) Can have saturation characteristics. Command joint angle θ _com Detected joint angle θ _es Deviation ε minus _com Is the predetermined value ε ₀ Is smaller than the torque limit control unit 12, the command joint angle θ _com Is output without modification. Command joint angle θ _com And detected joint angle θ _es Deviation from _com Is the predetermined value ε ₀ Is exceeded, the torque limit controller 12 determines the commanded joint angle θ _com The deviation ε _com Correct according to the correction command joint angle θ ' _com Is output. In FIG. 3B, the detected joint angle θ _es Commanded joint angle θ relative to _com And correction command joint angle θ ' _com Shows the relationship. Deviation ε _com Is the predetermined value ε ₀ In the range exceeding the correction command joint angle θ ' _com And detected joint angle θ _es It can be seen that the difference between and is saturated. The torque limit control unit 12 functions to prevent excessive entrainment of the target value (command value) that tends to occur in the compliance control due to the saturation characteristic.
[0025]
An example of the braking force profile generated by the braking operation profile generation unit 14 is shown in FIG. The braking force profile is set in advance in the form of a function of the arm tip speed and a readout numerical value table. The braking force profile shown in FIG. 4 is configured as a pattern of the braking force Fb with respect to the arm tip speed Va. Specifically, the arm tip speed Va is proportional to the arm tip speed Va until the arm tip speed Va reaches a certain magnitude Va0. Although the braking force F is increased to F0, for a speed equal to or higher than the constant arm tip speed Va0, the pattern is maintained at the magnitude of F0 without increasing the braking force F. In the braking operation profile generation unit 14, the target tip position X of the arm 5 every moment is calculated according to the following formula (2) and formula (3) according to the braking force profile shown in FIG. 4. _com Is calculated.
V _com = ∫ (−F / M) dt + v ₀ (2)
X _com = ∫V _com dt + x ₀ (3)
Where v ₀ Is the arm tip speed at the start of this control, x ₀ Is the arm tip position at the start of this control.
Equation (2) is the arm tip speed v at the start of this control. ₀ Is an equation for adding the speed obtained by integrating the acceleration (-F / M) obtained from the assumed load inertia M of the arm and the braking force F read from the braking force profile with time t, and the time from the start of control. Target tip speed (command value) V that the tip of the arm 5 should have after t elapses _com Represents. Equation (3) is the arm tip position x at the start of control. ₀ In addition, the target tip speed (command value) V shown in Expression (2) _com Are added to each other, and a target tip position (command value) X that the tip of the arm 5 should have after a lapse of time t from the start of control. _com Represents.
[0026]
Target tip position X _com From the commanded joint angle θ of each joint every moment by the inverse kinematic transformation of the arm (calculation of the kinematics equation) _com Is calculated. In the torque limit control unit 12, the command joint angle θ _com The detected joint angle θ at that time _es Deviation for ε _com = (Θ _com −θ _es ) Is kept from exceeding a certain level. That is, the dead zone processing has the characteristics shown in FIG. _es Is the commanded joint angle θ _com Command joint angle θ _com Command joint angle θ _com Is directly used as the output of the torque limit control unit 12. Detection joint angle θ _es Is the commanded joint angle θ _com The dead zone processing is the commanded joint angle θ _com The torque limit control unit 12 outputs a correction command joint angle θ ′. _com Is detected joint angle θ _es Is output as a value that is not significantly separated from.
[0027]
The torque follow-up control unit 13 generates a target joint output torque τ output from a braking torque profile generation unit 15 described later. _com Target joint output torque τ _com The joint virtual torsional rigidity k _c By dividing by the target compliance angular displacement θ _cc And calculate the target compliance angular displacement θ _cc Is output to the compliance control unit 11. Target joint output torque τ _com For example, a profile as shown in FIG. 5 is generated. That is, the target braking torque profile illustrated in FIG. 5 takes time T (time after the start of this control operation) since the movement target object A is captured on the horizontal axis, and the vertical axis indicates the magnitude of the braking torque. This graph is shown in the graph, and until the time T1, the target joint output torque τ _com Is set as a pattern that gradually decreases (in this example, linearly) from the constant target joint output torque from time T1 and becomes zero at time T2. The setting mode may be, for example, a function of time T or a readout numerical value table. The braking torque profile generation unit 15 applies the braking force F to be applied by the arm 5 every moment according to the target braking force profile F (v) having the setting pattern shown in FIG. _com And the target joint output torque τ as shown in the following equation (4) using the Jacobian J that is a conversion matrix from the joint rotational speed of the arm 15 to the tip speed Va: _com Is calculated from moment to moment.
τ _com = J ^t F _com (4)
[0028]
In the compliance control unit 11, the target compliance angular displacement θ calculated by the torque follow-up control unit 13. _cc And the compliance angular displacement θ output from the compliance control filter 11b. _d Difference with (θ _cc −θ _d ) Compliance deviation ε _c Is calculated. Compliance deviation ε _c And the corrected command joint angle θ ′ that is the output of the torque limit control unit 12 _com Is the target joint angle θ _ref Is output to the joint angle control unit (motor 7). That is, the joint torque τ detected by the joint torque detector 9 _s , Torque target value, and target braking force and detected joint angle are the compliance angle displacement θ obtained from the virtual compliance mechanism, respectively. _d , Target compliance angular displacement θ from torque follow-up control unit 13 _cc Or correction command joint angle θ ′ _com As the target joint angle θ _ref It is reflected in. The detected joint angle is always reflected in the control in the joint angle controller 7. Note that the element RL additionally provided for each value is an incremental limiter, and an excessive force is applied to the rotating mechanism when the movement of the arm cannot be followed because the change of the movement of the arm is too fast when the moving object is captured. Is prevented from working.
[0029]
Hereinafter, the operation of the joint motion control device 10 will be described. For example, as shown in FIG. 6, let us consider an operation in which the robot arm 5 follows and captures the movement target object A and brakes and stops it. According to the joint motion control apparatus 10 shown in FIG. 1, basically, virtual compliance control is performed based on a virtual compliance mechanism having virtual mass, virtual torsional rigidity, and virtual viscosity coefficient. Detected joint torque τ detected by the torque detector 9 and input to the compliance control unit 11 _s Of these, only the low-frequency component from which the high-frequency component serving as signal noise has been removed is input to the compliance control filter 11b including virtual parameters. By the compliance control filter 11b, the detected joint torque τ _s Virtual compliance angular displacement θ according to _d Is calculated by digital processing. That is, the detected joint torque τ is digitally calculated using a virtual compliance mechanism. _s The joint angle corresponding to is estimated, and the virtual compliance angular displacement θ _d Is obtained.
[0030]
On the other hand, the joint motion control device 10 also performs torque tracking control. In the torque follow-up control, the target joint output torque τ obtained from the braking torque profile generation unit 15 depending on the time from the start of control. _com Virtual torsional rigidity k _c (Here, the virtual mass m of the tip 6 is ₁ And the virtual viscosity coefficient are not considered) _com Target compliance angular displacement θ as an angular displacement reflecting _cc The target compliance angular displacement θ _cc Virtual compliance angular displacement θ as an estimate from _d Minus the compliance angle deviation ε _c (Θ _cc −θ _d ) Is output to the joint angle controller (motor 7). Compliance angle deviation ε _c (Θ _cc −θ _d ) Is output, the control device 10 causes the compliance angle deviation ε _c (Θ _cc −θ _d ) To zero, and as a result, the joint mechanism 2 does not generate excessive torque due to friction or the like during braking, and applies braking torque along the target braking torque profile to perform braking operation. It can be carried out. In the braking torque profile generation unit 15, the actual state of the braking operation is detected based on the detected joint angle θ. _es By constantly monitoring the braking torque profile, the braking torque profile time T1 is automatically adjusted when the deceleration is earlier or slower than expected. Specifically, when the speed is greater than the predetermined speed assumed in advance after the time T1 has elapsed (not sufficiently braked), processing for extending the times T1 and T2 is performed.
[0031]
Command joint angle θ generated by braking operation profile generation unit 14 _com In the torque limit control unit 12, the detected joint angle θ _es If the deviation is small, the dead zone processing output is set to zero. If the deviation is equal to or greater than a certain value, a signal based on the deviation is output. As the output of the torque limit control unit 12, the command joint angle θ _com Is the detected joint angle θ _es Command joint angle θ _com However, if the deviation is above a certain level, the commanded joint angle θ _com A signal saturated by taking the difference is output to the compliance control unit 11. The control device 10 determines that the joint angle is the commanded joint angle θ _com The joint angle control unit (motor 7) is controlled so as to coincide with. Output of torque limit control unit 12 and compliance angle deviation (θ _cc −θ _d ) Is the target joint angle θ _ref Is output from the compliance control unit 11 to the joint angle controller (motor 7). By this control, the joint angle control unit changes the joint angle to the commanded joint angle θ. _com Simultaneously, the joint torque is set to the target joint torque τ _com The joint angle is controlled so as to match. As a result, the tip 6 of the robot arm 5 can capture the target in a state substantially synchronized with the movement speed of the target object A by following the movement target object A. In addition, after the movement target object A is captured by the robot arm 5, the virtual mass m is added to the common center of gravity of the target object A and the tip 6 of the arm 5. ₂ May be put.
[0032]
【The invention's effect】
As described above, the present invention assumes a virtual compliance mechanism as the rotation mechanism, receives a detected torque, obtains a virtual compliance angular displacement based on the virtual compliance mechanism, and determines the virtual compliance angular displacement as a braking force and a braking force. Since the torque is corrected based on the target command angle and the target braking torque from each profile generation unit, and the rotation angle of the rotation unit is controlled by outputting the target rotation angle obtained by the correction, The rotation angle of the rotating part of the rotating mechanism is controlled in consideration of the braking torque and braking force, so that an excessive load is not applied to each mechanism part of the rotating mechanism, and the movement of the load object can be controlled. Brakes such as deceleration and stop can be performed. That is, according to the present invention, when an engaging portion such as a robot arm or a manipulator provided in the rotating portion collides with, captures, or grasps the object to be moved, the control that acts on the rotating mechanism after engaging with the object to be moved. By controlling the rotation angle of the rotating part of the rotating mechanism while preventing excessive force from acting on the rotating part in consideration of power and braking torque, the rotating part on which the torque acts can be controlled at a predetermined rotation angle. A control device for a rotating mechanism that can be controlled can be provided.
[Brief description of the drawings]
FIG. 1 is a block diagram showing an example of a joint motion control device as an embodiment of a rotation mechanism control device according to the present invention;
FIG. 2 is a schematic diagram of a joint system to which a control device for a rotation mechanism according to the present invention is applied.
FIG. 3 is a diagram showing an example of a dead zone characteristic of a torque limit control unit in the joint motion control apparatus shown in FIG.
4 is a diagram showing an example of a deceleration operation profile in the joint operation control apparatus shown in FIG.
FIG. 5 is a diagram showing an example of a braking torque profile in the joint motion control device shown in FIG. 1;
FIG. 6 is a diagram showing an outline of compliance control according to the present invention.
[Explanation of symbols]
1 Airframe 2 Joint mechanism
3 Deceleration mechanism 4 Rotating part
5 arms 6 hands
7 Motor 8 Joint angle detector
9 Torque detector 10 Control device
11 Compliance Control Department
11a Low-pass filter 11b Compliance control filter 12 Torque limit control unit 13 Torque tracking control unit
14 Braking operation profile generator 15 Braking torque profile generator
θ _es Detection joint angle τ _s Detected joint torque
θ _ref Target joint angle τ _com Target braking torque
θ _cc Target compliance angular displacement command
ε _com Joint angle θ _com And detected joint angle θ _es Deviation from
θ _com Command joint angle θ ' _com Correction command joint angle
θ _d Virtual compliance angular displacement
ε _c Compliance angle deviation ε _c (Θ _cc −θ _d )
ε _com Deviation (θ _com −θ _es )
m _c Joint virtual load inertia c _c Joint virtual viscosity coefficient
k _c Joint virtual torsional rigidity
τ _f Detected joint torque τ _S Joint torque after low-pass filtering

Claims (7)

In the rotation mechanism control device that controls the rotation angle of the rotation unit by outputting the target rotation angle θref as a control signal to a motor that rotates the rotation unit of the rotation mechanism,
A torque follow-up control unit that outputs a target compliance angular displacement θcc based on the target braking torque τcom from the braking torque profile generation unit;
Obtained by multiplying the deviation εcom between the target command angle θcom from the braking operation profile generation unit and the detected rotation angle θes detected by the rotation angle detection unit by a gain or zero determined according to the deviation εcom. A torque limit control unit that outputs a corrected target command angle θ′com obtained by inverting and inputting a value to the target command angle θcom (a coupling difference) , and a detected torque acting on the rotating unit detected by the torque detection unit a compliance control filter that receives an input of τs and outputs a virtual compliance angular displacement θd, and corrects the virtual compliance angular displacement θd with the target compliance angular displacement θcc and the corrected target command angle θ′com to achieve the target rotation A rotation mechanism control device comprising a compliance control unit that outputs an angle θref.   The rotation mechanism control device according to claim 1, wherein the compliance control filter is described by a transfer function described based on virtual inertia, virtual viscosity, and virtual rigidity.   The torque limit control unit corrects the target target angle θcom with an output obtained by performing a dead zone process on an input of a difference between the target command angle θcom and the detected rotation angle θes. The rotation mechanism control device according to claim 1, further comprising outputting a command angle θ′com.   In the dead zone processing, when the difference between the target command angle θcom and the detected rotation angle θes is within a predetermined deviation, the target command angle θcom is directly used as the corrected target command angle θ′com without the output. 4. The rotating mechanism control device according to claim 3, wherein when the difference exceeds the predetermined deviation, a value corresponding to a value of the target command angle θcom is used as the output, and the target command angle θcom is subtracted by the output. .   The rotation mechanism control device according to claim 2, wherein the torque follow-up control unit outputs a value obtained by dividing the target braking torque τcom by the virtual rigidity as the target compliance angular displacement θcc.   2. The braking torque profile generation unit is configured to set the target braking torque τcom as a function that is a constant value until a predetermined time and gradually decreases from the predetermined time to a stop time of the rotating unit. The control apparatus for rotation mechanisms as described in 2.   The braking operation profile generation unit is set as a function of the braking force with respect to the speed of the tip of the arm provided in the rotating unit, and the function is a function of the tip of the tip until the speed of the tip of the arm reaches a predetermined speed. 2. The rotation mechanism control device according to claim 1, wherein the rotation mechanism is saturated to a constant braking force with respect to a speed equal to or higher than the predetermined speed.

Images