JP2018097722A - Control device, control method, and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To apply a gravity compensation torque that cancels a weight of a link itself to a feedback system of a hydraulic control with high accuracy.SOLUTION: A joint angle of a joint mechanism of a mechanical device having a drive mechanism including a plurality of links connected via the joint mechanism is controlled by hydraulic pressure. A gravity compensation torque that cancels a weight of the link that is operated by controlling a joint angle is calculated. The gravity compensation torque is converted into a gravity compensation valve opening degree indicating a valve opening degree of a control valve for controlling a flow rate of hydraulic pressure. A valve opening degree control command of the control valve is calculated on the basis of an angle feedback valve opening degree and a gravity compensation valve opening degree determined by a current joint angle of the joint and a target joint angle.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a control device, a control method, and a program for controlling a joint angle of a joint mechanism constituting a mechanical device by hydraulic pressure.

  A mechanical device controlled by hydraulic pressure such as a hydraulic manipulator has an advantage of high energy efficiency per unit volume. On the other hand, mechanical devices controlled by hydraulic pressure are not good at controlling minute movements necessary for highly accurate positioning performance. Patent Document 1 discloses a technique of a working machine as an example of a mechanical device controlled by hydraulic pressure. The technique of Patent Document 1 compensates the weight of the working mechanism by the force of the spring.

JP2015-105560A

  Incidentally, in the above-described mechanical device, the amount of hydraulic pressure is adjusted based on the opening of the hydraulic control valve, and the joint angle, which is the angle between two links that rotate about the joint, is determined. In such a mechanical apparatus, it has been required to accurately apply a gravity compensation torque that cancels the weight of the link itself that is operated by controlling the joint angle to a feedback system for hydraulic control.

  Therefore, an object of the present invention is to provide a control device, a control method, and a program that solve the above-described problems.

  According to the first aspect of the present invention, the control device that controls the drive mechanism of the mechanical device having the drive mechanism by hydraulic pressure cancels the weight of the drive mechanism component that controls and operates the drive mechanism. Gravity compensation torque calculation unit for calculating the gravity compensation torque using a gravity compensation equation, and gravity compensation valve opening calculation for converting the gravity compensation torque into a gravity compensation valve opening indicating a valve opening of a control valve that controls the flow rate of the hydraulic pressure And a valve opening degree control command for the control valve based on a feedback valve opening degree determined by a current driving state quantity of the driving mechanism and a target driving state quantity and the gravity compensation valve opening degree And an opening degree control unit.

  In the above-described control device, the drive mechanism includes a plurality of links connected via a joint mechanism, and the control device controls a joint angle of the joint mechanism of a mechanical device having the drive mechanism by the hydraulic pressure. The gravity compensation torque calculation unit calculates the gravity compensation torque that cancels the weight of the link that is the drive mechanism component that operates by controlling the joint angle using a gravity compensation equation, and the valve opening degree control unit Is based on an angle feedback valve opening determined by a joint angle that is the current driving state amount of the joint mechanism and a joint angle that is the target driving state amount, and the gravity compensation valve opening. The valve opening degree control command may be calculated.

  In the above-described control device, the gravity compensation valve opening degree calculation unit may calculate the gravity compensation torque calculated by inputting the current joint angle into the gravity compensation equation, the target joint angle, and the current joint angle. The gravity compensation valve opening may be calculated using a deviation between the joint angle and the target joint angle.

Further, in the above-described control device, the gravity compensation valve opening calculation unit is configured to input the target joint angle to the gravity compensation equation, calculate the gravity compensation torque, the target joint angle, and the current The gravity compensation valve opening may be calculated using a deviation between the joint angle and the target joint angle.
In the above-described control device, a fluctuation suppressing unit that suppresses the fluctuation frequency of the deviation may be provided.

  Further, in the above-described control device, the gravity compensation valve opening calculation unit is an angle exceeding a steady deviation of the joint angle that changes a variable gain coefficient for converting the gravity compensation torque into the gravity compensation valve opening at a constant angular velocity. In the above case, it is increased to a predetermined value larger than the predetermined coefficient value in proportion to the size of the angle, and when the angle is less than the steady deviation of the joint angle, the predetermined coefficient value is held. It's okay.

  In the above-described control device, the gravity compensation torque calculation unit calculates a gravity compensation torque at a time preceding by a predetermined time determined based on a response time until the control valve reaches the calculated valve opening. May be output.

  In the above-described control device, the gravity compensation valve opening degree calculation unit may change a variable gain coefficient that converts the gravity compensation torque into the gravity compensation valve opening degree that becomes the fastest until the joint angle shifts to the target angle. Gain factors may be learned.

  According to the second aspect of the present invention, the control method is a control method of a control device that controls the drive mechanism of a mechanical device having a drive mechanism by hydraulic pressure, and the drive mechanism controls and operates the drive mechanism. A gravity compensation torque that cancels the weight of the component is calculated using a gravity compensation equation, and the gravity compensation torque is converted into a gravity compensation valve opening that indicates a valve opening of a control valve that controls the flow rate of the hydraulic pressure, and the drive A valve opening degree control command for the control valve is calculated based on a feedback valve opening degree determined by a current driving state quantity of the mechanism and a target driving state quantity and the gravity compensation valve opening degree.

  According to the third aspect of the present invention, the program controls the computer of the control device that controls the drive mechanism of the mechanical device having the drive mechanism by hydraulic pressure, and controls the drive mechanism to operate the own weight of the drive mechanism component. Gravity compensation torque calculating means for calculating a gravity compensation torque that cancels using a gravity compensation equation, gravity compensation for converting the gravity compensation torque into a gravity compensation valve opening degree indicating a valve opening degree of a control valve that controls the flow rate of the hydraulic pressure Based on a feedback valve opening determined by the current drive state quantity of the drive mechanism and a target drive state quantity and the gravity compensation valve opening, a valve opening degree control command for the control valve is provided. It is made to function as a valve opening degree control means to calculate.

According to the present invention, only the configuration for converting the gravity compensation torque τ _gn [Nm] into the gravity compensation valve opening a _gn [%] indicating the valve opening of the control valve that controls the flow rate of the hydraulic pressure can be used. A valve opening control command for canceling the dead weight can be calculated. Therefore, it is possible to calculate a valve opening control command for canceling the weight of the link to be operated without having to construct a complicated logic.

It is a block diagram which shows the structure of the control apparatus by 1st embodiment. It is a figure which shows the processing flow of the control apparatus by 1st embodiment. It is a block diagram which shows the structure of the control apparatus by 2nd embodiment. It is a figure which shows the processing flow of the control apparatus by 2nd embodiment. It is a figure which shows the determination outline | summary of the variable gain coefficient of the control apparatus by 3rd embodiment. It is a figure explaining the output timing of the gravity compensation torque of the control apparatus by 4th embodiment.

<First embodiment>
Hereinafter, a control device according to a first embodiment will be described with reference to the drawings.
FIG. 1 is a block diagram showing a configuration of a control device according to the first embodiment.
As shown in this figure, the control system includes a control device 1 and a mechanical device 2. The control device 1 controls a mechanical device 2 having a drive mechanism including a plurality of links connected via a joint mechanism. The control device 1 controls the joint angle of the joint mechanism in the mechanical device 2 by hydraulic pressure. The mechanical device 2 may be a hydraulic manipulator, for example.

  The control device 1 is a computer and is configured by hardware such as a ROM (Read Only Memory), a RAM (Random Access Memory), a storage unit such as an SSD (Solid State Drive), a CPU (Central Processing Unit), and a communication interface. Good.

  The CPU of the control device 1 is activated based on the power being supplied to the control device 1 and executes the stored control program. As a result, the control device 1 has the functions of a gravity compensation torque calculation unit 11, a gravity compensation valve opening calculation unit 12, a feedback control unit 13, and a valve opening control unit 14.

The gravity compensation torque calculation unit 11 calculates a gravity compensation torque τ _gn [Nm] that cancels the weight of the link operated by controlling the joint angle of the joint mechanism provided in the mechanical device 2 using the gravity compensation equation. In the present embodiment, the joint mechanism is an aspect of a drive mechanism that constitutes the mechanical device 2. The link is one aspect of a drive mechanism component that controls and operates the drive mechanism.
The gravity compensation valve opening calculation unit 12 converts the gravity compensation torque τ _gn [Nm] into a gravity compensation valve opening a _gn [%] indicating the valve opening of the control valve that controls the hydraulic flow rate. Specifically, the gravity compensation valve opening calculation unit 12 uses the gravity compensation torque τ _gn [Nm], the target angle of the joint angle, and the deviation of the target angle and the current angle of the joint angle. The opening degree a _gn [%] is calculated.

The feedback control unit 13 calculates a valve opening degree control command for the control valve based on the feedback valve opening degree and the gravity compensation valve opening degree determined by the current driving state quantity of the driving mechanism and the target driving state quantity. Specifically, the feedback control unit 13 performs feedback control using the target angle θ _rn and the current angle θ _{n of the} joint mechanism of the mechanical device 2 so that the joint angle becomes the target angle θ _rn. . The joint angle is an aspect of the drive state amount of the drive mechanism. The feedback control unit 13 outputs the angle feedback valve opening degree a _fn [%] calculated by the feedback control.
The valve opening degree control unit 14 uses the gravity compensation valve opening degree a _gn [%] acquired from the gravity compensation valve opening degree calculation unit 12 and the angle feedback valve opening degree a _fn [%] acquired from the feedback control unit 13. Use to calculate a valve opening control command of the control valve.

According to the processing of the control device 1, the gravity compensation torque τ _gn [Nm] can be converted into the gravity compensation valve opening a _gn [%] indicating the valve opening of the control valve that controls the hydraulic flow rate. Therefore, the valve opening degree control command for canceling the dead weight of the link to be operated can be calculated with a simple configuration.

FIG. 2 is a diagram illustrating a processing flow of the control device according to the first embodiment.
Next, details of the processing of the control device 1 will be described.
First, as an example, the feedback control unit 13 acquires a deviation Δθ _n between the target angle θ _{rn of} the joint mechanism of the joint mechanism to which the two links are connected and the current angle θ _n (step S101). The deviation can be calculated by θ _rn −θ _n . The feedback control unit 13 uses the deviation Δθ _n to calculate an angle feedback valve opening degree a _fn [%] such that Δθ _n becomes 0 by feedback control (step S102). The feedback control unit 13 outputs the angle feedback valve opening degree a _fn [%] to the valve opening degree control unit 14.

On the other hand, the gravity compensation torque calculation unit 11 acquires the current angle θ _n of the joint mechanism from the mechanical device 2. The gravity compensation torque calculator 11 substitutes the current angle θ _n into the gravity compensation equation g (θ _n ), and calculates the gravity compensation torque τ _gn [Nm] that cancels the weight of the link to be operated by controlling the joint angle ( Step S103). The gravity compensation torque calculation unit 11 outputs the gravity compensation torque τ _gn [Nm] to the gravity compensation valve opening calculation unit 12.

The gravity compensation valve opening calculation unit 12 acquires the gravity compensation torque τ _gn [Nm], the target angle θ _rn, and the deviation Δθ _n . The gravity compensation valve opening calculation unit 12 calculates a value of τ _gn [Nm] × θ _rn (step S104). The gravity compensation valve opening degree calculation unit 12 is based on the value of τ _gn [Nm] × θ _{rn and} the value of the deviation Δθ _n , and the variable gain coefficient K of the gravity compensation valve opening degree calculation expression represented by the equation (1). _gn is determined (step S105).

When determining the variable gain coefficient K _gn , the gravity compensation valve opening calculator 12 determines the variable gain coefficient K based on the sign of the value of τ _gn [Nm] × θ _rn and the sign of the deviation Δθ _n. Determine whether to increase or decrease _gn . Further, the numerical value of the variable gain coefficient K _gn may be specified by a predetermined calculation formula based on the value of τ _gn [Nm] × θ _{rn and} the value of the deviation Δθ _n .

As an example, when the value of τ _gn [Nm] × θ _rn indicates + and the value of deviation Δθ _n indicates + in the process of step S105, the gravity compensation valve opening calculator 12 counters the link with gravity. It is determined that the vehicle is driving in the direction and has not reached the target. In this case, the gravity compensation valve opening degree calculation unit 12 determines that the gravity compensation is insufficient, and determines the variable gain coefficient K _{gn so as} to increase the variable gain coefficient K _gn .
The gravity compensation valve opening calculation unit 12 drives the link in a direction against gravity when the value of τ _gn [Nm] × θ _rn indicates + and the value of the deviation Δθ _n indicates − It is determined that the link has been activated beyond the target. In this case, the gravity compensation valve opening calculation unit 12 determines that the gravity compensation is too effective, and determines the variable gain coefficient K _{gn so as} to decrease the variable gain coefficient K _gn .

In addition, the gravity compensation valve opening calculation unit 12 drives the link in the direction of gravity when the value of τ _gn [Nm] × θ _rn indicates − and the value of the deviation Δθ _n indicates +. It is determined that the situation has not been reached. In this case, the gravity compensation valve opening calculation unit 12 determines that the gravity compensation is too effective, and determines the variable gain coefficient K _{gn so as} to decrease the variable gain coefficient K _gn .
Further, the gravity compensation valve opening calculation unit 12 drives the link in the direction of gravity when the value of τ _gn [Nm] × θ _rn indicates − and the value of the deviation Δθ _n indicates −. It is determined that the link has been activated beyond that. In this case, the gravity compensation valve opening calculator 12 determines that the gravity compensation is insufficient, and determines the variable gain coefficient K _{gn so as} to increase or decrease the variable gain coefficient K _gn .

When the gravity compensation valve opening degree calculation unit 12 determines the variable gain coefficient K _gn , the gravity compensation valve opening degree a _gn [%] is calculated by the above equation (1) (step S106). The gravity compensation valve opening calculation unit 12 outputs the gravity compensation valve opening a _gn [%] to the valve opening control unit 14.

The valve opening control unit 14 adds the angle feedback valve opening a _fn [%] acquired from the feedback control unit 13 and the gravity compensation valve opening a _gn [%] acquired from the gravity compensation valve opening calculation unit 12. Then, a valve opening control command is calculated (step S107). The valve opening control unit 14 outputs a valve opening control command to the target control valve of the mechanical device 2 (step S108).

According to the above processing, the gravity compensation torque τ _gn [Nm] can be converted into the gravity compensation valve opening degree a _gn [%] indicating the opening degree of the control valve that controls the hydraulic flow rate. Therefore, it is possible to calculate a valve opening degree control command for canceling the dead weight of the link to be operated.

Further, according to the above-described processing, only the configuration in which the gravity compensation torque τ _gn [Nm] is converted into the gravity compensation valve opening a _gn [%] indicating the valve opening of the control valve that controls the flow rate of the hydraulic pressure is operated. A valve opening degree control command for canceling the dead weight of the link to be made can be calculated. Therefore, it is possible to calculate a valve opening control command for canceling the weight of the link to be operated without having to construct a complicated logic.
Further, since it is determined whether to increase or decrease the variable gain coefficient K _gn based on the sign of τ _gn [Nm] × θ _rn and the sign of the deviation Δθ _n , the gravity compensation torque τ _gn [ Nm] and the target angle, and an appropriate gravity compensation valve opening degree a _gn [%] corresponding to the sign of the deviation Δθ _n can be calculated.

<Second Embodiment>
Hereinafter, a control apparatus according to the second embodiment will be described with reference to the drawings.
FIG. 3 is a block diagram showing the configuration of the control device according to the second embodiment.
The hardware configurations of the control device 1 and the machine device 2 in the second embodiment are the same as those in the first embodiment.
The CPU of the control device 1 is activated based on the power being supplied to the control device 1 and executes the stored control program. As a result, the control device 1 includes functions of a gravity compensation torque calculation unit 11, a gravity compensation valve opening calculation unit 12, a feedback control unit 13, a valve opening control unit 14, and a fluctuation suppression unit 15, as shown in FIG. Is provided.

The gravity compensation torque calculation unit 11 calculates a gravity compensation torque τ _gn [Nm] that cancels the weight of the link operated by controlling the angle of the joint provided in the mechanical device 2 using the gravity compensation equation.
The gravity compensation valve opening calculation unit 12 converts the gravity compensation torque τ _gn [Nm] into a gravity compensation valve opening a _gn [%] indicating the valve opening of the control valve that controls the hydraulic flow rate. Specifically, the gravity compensation valve opening degree calculation unit 12 inputs the target angle θ _rn of the joint angle to the gravity compensation equation and calculates the gravity compensation torque τ _gn [Nm] and the target angle θ _{rn of the} joint angle. Then, the gravity compensation valve opening is calculated using the current angle θ _n of the joint angle and the deviation Δθ _n of the target angle θ _rn of the joint angle.

The feedback control unit 13 performs feedback control using the target angle θ _rn of the joint mechanism of the joint mechanism provided in the mechanical device 2 and the current angle θ _n so that the joint angle becomes the target angle θ _rn . The feedback control unit 13 outputs the angle feedback valve opening degree a _fn [%] calculated by the feedback control.
The valve opening control unit 14 includes the gravity compensation valve opening a _gn [%] acquired from the gravity compensation valve opening calculation unit 12 and the angle feedback valve opening a _fn [%] acquired from the feedback control unit 13. Is used to calculate the valve opening control command of the control valve.
The fluctuation suppressing unit 15 suppresses the fluctuation frequency of the deviation Δθ _n between the current angle θ _n of the joint angle and the target angle θ _rn of the joint angle. For example, the fluctuation suppressing unit 15 may be configured by a low pass filter. By the process of the fluctuation suppressing unit 15, the deviation Δθ _n between the current angle θ _n of the joint angle and the target angle θ _rn of the joint angle input to the gravity compensation valve opening degree calculating unit 12 is suppressed from increasing or decreasing. The

FIG. 4 is a diagram showing a processing flow of the control device according to the second embodiment.
Next, details of processing of the control device 1 according to the second embodiment will be described.
First, the feedback control unit 13 acquires a deviation Δθ _n between the target angle θ _{rn of} the joint mechanism of the joint mechanism to which the two links are connected and the current angle θ _n (step S201). The deviation can be calculated by θ _rn −θ _n . The feedback control unit 13 uses the deviation Δθ _n to calculate an angle feedback valve opening degree a _fn [%] such that the Δθ _n becomes 0 by feedback control (step S202). The feedback control unit 13 outputs the angle feedback valve opening degree a _fn [%] to the valve opening degree control unit 14.

On the other hand, the gravity compensation torque calculation unit 11 acquires the target angle θ _rn of the joint mechanism. The gravity compensation torque calculator 11 substitutes the target angle θ _rn into the gravity compensation equation g (θ _rn ), and calculates the gravity compensation torque τ _gn [Nm] that cancels the weight of the link operated by controlling the joint angle ( Step S203). The gravity compensation torque calculation unit 11 outputs the gravity compensation torque τ _gn [Nm] to the gravity compensation valve opening calculation unit 12.

The fluctuation suppressing unit 15 acquires the deviation Δθ _n . The fluctuation suppressing unit 15 suppresses the obtained fluctuation frequency of the deviation Δθ _n and outputs the suppressed fluctuation frequency to the gravity compensation valve opening degree calculating unit 12 (step S204). The gravity compensation valve opening calculation unit 12 acquires the gravity compensation torque τ _gn [Nm], the target angle θ _rn, and the deviation Δθ _n in which the fluctuation frequency is suppressed. The gravity compensation valve opening calculator 12 calculates the value of τ _gn [Nm] × θ _rn (step S205). The gravity compensation valve opening degree calculation unit 12 is based on the value of τ _gn [Nm] × θ _{rn and} the value of the deviation Δθ _n , and the variable gain coefficient K of the gravity compensation valve opening degree calculation expression represented by the equation (1). _gn is determined (step S206).

Similar to the first embodiment, when the gravity compensation valve opening calculation unit 12 of the second embodiment determines the variable gain coefficient K _gn , the sign of the value of τ _gn [Nm] × θ _rn , Based on the sign of the deviation Δθ _n , it is determined whether to increase or decrease the variable gain coefficient K _gn . Further, the numerical value of the variable gain coefficient K _gn may be specified by a predetermined calculation formula based on the value of τ _gn [Nm] × θ _{rn and} the value of the deviation Δθ _n .

When the gravity compensation valve opening degree calculation unit 12 determines the variable gain coefficient K _gn , the gravity compensation valve opening degree a _gn [%] is calculated by the above formula (1) (step S207). The gravity compensation valve opening calculation unit 12 outputs the gravity compensation valve opening a _gn [%] to the valve opening control unit 14.

The valve opening control unit 14 adds the angle feedback valve opening a _fn [%] acquired from the feedback control unit 13 and the gravity compensation valve opening a _gn [%] acquired from the gravity compensation valve opening calculation unit 12. Then, a valve opening control command is calculated (step S208). The valve opening degree control unit 14 outputs a valve opening degree control command to the target control valve of the mechanical device 2 (step S209).

According to the above process, in addition to the same effects as those of the first embodiment, the fluctuation suppressing unit 15 suppresses the fluctuation frequency of the deviation, so that the fluctuation of the variable gain coefficient K _gn can be suppressed. The concept of self-weight compensation compensates for a change in weight load that is static (having no dynamic characteristics), and it may not be necessary to change the compensation amount in detail. Disturbances due to non-compensation can be covered by feedback. Rather, sudden changes may cause disturbances, and input signals with little fluctuation are used to prevent this.

<Third embodiment>
Hereinafter, a control device according to a third embodiment will be described with reference to the drawings.
FIG. 5 is a diagram showing an outline of determination of the variable gain coefficient of the control device according to the third embodiment.
The gravity compensation valve opening calculation unit 12 changes the variable gain coefficient K _gn for converting the gravity compensation torque τ _gn [Nm] into the gravity compensation valve opening a _gn [%] based on the value of the deviation Δθ _n . For example, when the joint angle is moved beyond an angle that exceeds the steady-state deviation of the joint angle that is changed at a constant angular velocity, the gravity compensation valve opening calculation unit 12 uses a predetermined coefficient value K1 in proportion to the magnitude of the joint angle. The variable gain coefficient K _gn is increased to a large predetermined value K2.

The gravity compensation valve opening calculation unit 12 holds the variable gain coefficient K _gn at a predetermined coefficient value K1 when the joint angle is moved below an angle that does not exceed the steady-state deviation of the joint angle that is changed at a constant angular velocity.

When the deviation Δθ _n reaches a predetermined magnitude Δθ1, the gravity compensation valve opening calculator 12 proportionally changes the variable gain coefficient K _gn from the predetermined coefficient value K1 according to the magnitude of the deviation Δθ _n. Increase to. Δθ1 is determined based on a steady-state deviation of the joint angle generated in the joint angle control performed at a constant angular velocity. K2, which is the upper limit of the variable gain coefficient K _gn , is set to a realistic value from a valve opening saturation value that can be output. The coefficient value K1 is determined based on the value of the variable gain coefficient K _{gn in} a state where there is almost no feedback control output and the control output for gravity compensation is main when the joint mechanism is controlled in a stationary state.

According to the method for determining the variable gain coefficient K _gn according to the third embodiment, it is not necessary to adjust the value each time, and since the lower limit value is clearly set, the variable gain coefficient K _gn is set to a significant range. Can be converged within.
Note that the variable gain coefficient K _gn determination method according to the third embodiment may be applied to the first embodiment and the second implementation system value.

<Fourth embodiment>
Hereinafter, a control device according to a fourth embodiment will be described with reference to the drawings.
FIG. 6 is a diagram illustrating the output timing of the gravity compensation torque of the control device according to the fourth embodiment.
The waveform shown in FIG. 6 shows a temporal change in the angle until the joint angle of the joint mechanism reaches the target angle θ _rn . Response time is required until the joint angle of the joint mechanism reaches the target angle θ _rn . When the target trajectory θ _rn is given in advance, the gravity compensation torque calculation unit 11 precedes the response time λ of the control valve, which is separately measured by a step response or the like, and cancels the weight of the link. _gn [Nm] is calculated by the gravity compensation equation g (θ _n (t + λ)), and the gravity compensation torque τ _gn [Nm] is output to the gravity compensation valve opening calculation unit 12 at time t.

In the case of the second embodiment, when the target trajectory θ _rn is given in advance, the gravity compensation torque calculation unit 11 precedes the response time λ of the control valve that is separately measured by a step response or the like. Then, the gravity compensation torque τ _gn [Nm] for canceling the link weight is calculated by the gravity compensation equation g (θ _rn (t + λ)), and the gravity compensation torque τ _gn [Nm] is _opened at the time t. Output to the degree calculator 12.

Since the hydraulic system has a strong non-linear nonlinearity, the response delay cannot be ignored. Therefore, this is regarded as a dead time, and a gravity compensation torque τ _gn [Nm] that cancels the link weight in advance is calculated by the gravity compensation equation g (θ _rn ), and the gravity compensation torque τ _gn [Nm] is calculated as the gravity compensation. Output to the valve opening calculator 12. As a result, for example, in the third embodiment, Δθ _n becomes large positive at the beginning of movement, and therefore the variable gain coefficient K _gn increases, so that Δθ _n becomes negative and the variable gain coefficient K _gn becomes small. Hunting of _gn can be prevented.

<Fifth embodiment>
The gravity compensation valve opening calculation unit 12 sets the variable gain coefficient K _gn that converts the gravity compensation torque τ _gn [Nm] to the gravity compensation valve opening a _gn [%] as the fastest until the joint angle shifts to the target angle. The variable gain coefficient may be learned.
For example, the gravity compensation valve opening calculation unit 12 makes the variable gain coefficient K _gn variable as K _gn (θn ′) as a variable having the angular velocity as an input. The variable gain coefficient K _gn (θn ′) may be learned by moving several times along various trajectories at a certain angular velocity (θn ′).

In each of the embodiments described above, the drive mechanism includes a plurality of links connected via the joint mechanism, and the control device 1 controls the joint angle of the joint mechanism of the mechanical device 2 having the drive mechanism by hydraulic pressure. Yes. Then, the gravity compensation torque calculation unit 11 calculates a gravity compensation torque that cancels the weight of the link, which is a drive mechanism component operated by controlling the joint angle, using the gravity compensation equation. Further, the valve opening control unit 14 is based on the angle feedback valve opening and the gravity compensation valve opening determined by the joint angle that is the current driving state amount of the joint mechanism and the joint angle that is the target driving state amount. A valve opening control command for the control valve is calculated.
However, the drive mechanism may be a mechanism other than the joint mechanism. For example, the drive mechanism may be another mechanism such as an expansion / contraction mechanism. The driving state quantity may indicate the amount of expansion or contraction when the driving mechanism is an expansion / contraction mechanism.

  Even when the drive mechanism is other than the joint mechanism, the gravity compensation valve opening degree calculation unit 12 inputs the current drive state quantity into the gravity compensation equation, the gravity compensation torque calculated, the target drive state quantity, The gravity compensation valve opening may be calculated using a deviation between the drive state quantity and the target drive state quantity.

  Further, even when the drive mechanism is other than the joint mechanism, the gravity compensation valve opening degree calculation unit 12 inputs the target drive state quantity into the gravity compensation equation, the gravity compensation torque calculated, the target drive state quantity, The gravity compensation valve opening may be calculated using the deviation between the current drive state quantity and the target drive state quantity.

  Further, when the drive mechanism is other than the joint mechanism, a fluctuation suppressing unit that suppresses the fluctuation frequency of the deviation may be provided.

  Further, even when the drive mechanism is other than the joint mechanism, the gravity compensation valve opening degree calculation unit 12 changes the variable gain coefficient for converting the gravity compensation torque into the gravity compensation valve opening degree by a certain degree, and the steady deviation of the drive state amount If the driving state quantity exceeds the driving state quantity, the driving state quantity is increased in proportion to the magnitude of the driving state quantity to a predetermined value larger than a predetermined coefficient value. May be held at a predetermined coefficient value.

  Even when the drive mechanism is other than the joint mechanism, the gravity compensation torque calculation unit 11 performs gravity compensation at a time preceding the predetermined time determined based on the response time until the control valve reaches the calculated valve opening. Torque may be calculated and output.

  Further, even when the drive mechanism is other than the joint mechanism, the gravity compensation valve opening degree calculation unit 12 shifts the variable gain coefficient for converting the gravity compensation torque into the gravity compensation valve opening degree so that the drive state quantity shifts to the target drive state quantity. You may make it learn the variable gain coefficient used as the fastest until now.

  The above-described control device 1 has a computer system inside. And the program for making the control apparatus 1 perform each process mentioned above is memorize | stored in the computer-readable recording medium of the said control apparatus 1, The computer of the control apparatus 1 reads and runs this program The above processing is performed. Here, the computer-readable recording medium means a magnetic disk, a magneto-optical disk, a CD-ROM, a DVD-ROM, a semiconductor memory, or the like. Alternatively, the computer program may be distributed to the computer via a communication line, and the computer that has received the distribution may execute the program.

  Further, the program may be for realizing a part of the functions of each processing unit described above. Furthermore, what can implement | achieve the function mentioned above in combination with the program already recorded on the computer system, and what is called a difference file (difference program) may be sufficient.

DESCRIPTION OF SYMBOLS 1 ... Control apparatus 2 ... Mechanical apparatus 11 ... Gravity compensation torque calculation part 12 ... Gravity compensation valve opening degree calculation part 13 ... Feedback control part 14 ... Valve opening degree control part 15 ..Fluctuation suppression unit

Claims (10)

A control device for controlling the drive mechanism of a mechanical device having a drive mechanism by hydraulic pressure,
A gravity compensation torque calculation unit that calculates a gravity compensation torque that cancels the weight of a drive mechanism component that controls and operates the drive mechanism using a gravity compensation equation;
A gravity compensation valve opening calculator for converting the gravity compensation torque into a gravity compensation valve opening indicating a valve opening of a control valve that controls the flow rate of the hydraulic pressure;
Valve opening control for calculating a valve opening control command for the control valve based on a feedback valve opening determined by a current driving state amount and a target driving state amount of the driving mechanism and the gravity compensation valve opening And
A control device comprising: The drive mechanism includes a plurality of links connected via a joint mechanism;
The control device controls the joint angle of the joint mechanism of the mechanical device having the drive mechanism by the hydraulic pressure;
The gravity compensation torque calculation unit calculates the gravity compensation torque that cancels the weight of the link that is the drive mechanism component that operates by controlling the joint angle using a gravity compensation equation,
The valve opening degree control unit includes an angle feedback valve opening degree and a gravity compensation valve opening degree determined by a joint angle that is the current driving state amount of the joint mechanism and a target joint angle that is the target driving state amount. The control device according to claim 1, wherein a valve opening control command for the control valve is calculated based on the control valve. The gravity compensation valve opening calculation unit is configured to input the current joint angle to the gravity compensation equation, calculate the gravity compensation torque, the target joint angle, the current joint angle, and the target joint. The control device according to claim 2, wherein the gravity compensation valve opening is calculated using a deviation from the angle. The gravity compensation valve opening calculation unit is configured to input the target joint angle to the gravity compensation equation, calculate the gravity compensation torque, the target joint angle, the current joint angle, and the target joint. The control device according to claim 2, wherein the gravity compensation valve opening is calculated using a deviation from the angle.   The control device according to claim 3, further comprising a fluctuation suppressing unit that suppresses the fluctuation frequency of the deviation. The gravity compensation valve opening calculator calculates a variable gain coefficient for converting the gravity compensation torque into the gravity compensation valve opening when the angle exceeds a steady deviation of the joint angle that changes at a constant angular velocity. 6. The predetermined coefficient value is increased to a predetermined value larger than a predetermined coefficient value in proportion to the magnitude of the joint angle, and is held at the predetermined coefficient value when the angle is less than a steady deviation of the joint angle. The control device according to any one of the above. The gravity compensation torque calculation unit calculates and outputs a gravity compensation torque at a time preceding by a predetermined time determined based on a response time until the control valve reaches the calculated valve opening. The control device according to claim 6. The gravity compensation valve opening degree calculation unit learns a variable gain coefficient that converts the gravity compensation torque into the gravity compensation valve opening degree so that the variable gain coefficient becomes the fastest before the joint angle shifts to a target angle. The control device according to any one of claims 2 to 7. A control method of a control device for controlling the drive mechanism of a mechanical device having a drive mechanism by hydraulic pressure,
Calculate the gravity compensation torque that counteracts the weight of the drive mechanism component that controls and operates the drive mechanism using the gravity compensation equation,
Converting the gravity compensation torque into a gravity compensation valve opening indicating a valve opening of a control valve that controls the flow rate of the hydraulic pressure;
A control method for calculating a valve opening degree control command of the control valve based on a feedback valve opening degree determined by a current driving state quantity and a target driving state quantity of the driving mechanism and the gravity compensation valve opening degree. A computer of a control device for controlling the drive mechanism of a mechanical device having a drive mechanism by hydraulic pressure;
Gravity compensation torque calculation means for calculating a gravity compensation torque that cancels the weight of the drive mechanism component that controls and operates the drive mechanism using a gravity compensation equation;
Gravity compensation valve opening calculating means for converting the gravity compensation torque into a gravity compensation valve opening indicating a valve opening of a control valve that controls the flow rate of the hydraulic pressure;
Valve opening control for calculating a valve opening control command for the control valve based on a feedback valve opening determined by a current driving state amount and a target driving state amount of the driving mechanism and the gravity compensation valve opening means,
Program to function as.

Images