JP6757240B2 - Control device, control method, program - Google Patents

Description

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

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

Japanese Unexamined Patent Publication No. 2015-105560

By the way, in the above-mentioned mechanical device, the amount of hydraulic pressure is adjusted based on the opening degree of the hydraulic control valve to determine the joint angle, which is the angle between two links rotating around the joint. In such a mechanical device, it has been required to accurately apply the gravity compensation torque that cancels the weight of the link itself that controls the joint angle to operate the feedback system of 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-mentioned problems.

According to the first aspect of the present invention, the drive mechanism in which the control device that hydraulically controls the drive mechanism of the mechanical device having the drive mechanism cancels the own weight of the drive mechanism component that controls and operates the drive mechanism. the current angle or gravity compensation torque calculation unit that calculates using gravity compensation equation gravity compensation torque corresponding to the target between links constituting the angle between the links constituting said gravity compensation torque, the drive mechanism Gravity that converts the gravity compensation torque into a gravity compensation valve opening that indicates the valve opening of the control valve that controls the flow rate of the hydraulic pressure, based only on the deviation between the current and the target of the joint angle indicating The valve opening degree control of the control valve is performed by adding the gravity compensation valve opening degree to the feedback valve opening degree determined by the compensation valve opening degree calculation unit, the current drive state amount of the drive mechanism, and the target drive state amount. It is characterized by including a valve opening degree control unit for calculating a command.

In the above-mentioned control device, the drive mechanism includes a plurality of links connected via the joint mechanism, and 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 own weight of the link, which is a component of the drive mechanism that controls and operates the joint angle, by using the gravity compensation equation, and the valve opening control unit. However, the control valve is based on the angle feedback valve opening degree and the gravity compensating valve opening degree determined by the joint angle which is the current driving state amount of the joint mechanism and the joint angle which is the target driving state amount. The valve opening control command may be calculated.

Further, in the above-mentioned control device, the gravity compensation valve opening degree calculation unit inputs the current joint angle into the gravity compensation equation to calculate the gravity compensation torque, the target joint angle, and the current joint angle. The gravity compensation valve opening may be calculated using the deviation between the joint angle and the target joint angle.

Further, in the above-mentioned control device, the gravity compensation valve opening degree calculation unit inputs the target joint angle into the gravity compensation equation to calculate the gravity compensation torque, the target joint angle, and the current current. The gravity compensation valve opening may be calculated using the deviation between the joint angle and the target joint angle.
Further, the above-mentioned control device may include a fluctuation suppression unit that suppresses the fluctuation frequency of the deviation.

Further, in the above-mentioned control device, the gravity compensation valve opening calculation unit changes the variable gain coefficient for converting the gravity compensation torque into the gravity compensation valve opening at a constant angular velocity, which exceeds the steady deviation of the joint angle. In the above cases, the value is increased to a predetermined value larger than the predetermined coefficient value in proportion to the magnitude of the angle, and if the angle is less than the steady deviation of the joint angle, the value is maintained at the predetermined coefficient value. You can.

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

Further, in the above-mentioned control device, the gravity compensation valve opening degree calculation unit changes the variable gain coefficient for converting the gravity compensation torque into the gravity compensation valve opening degree, which is the fastest until the joint angle shifts to the target angle. You may learn the gain coefficient.

According to the second aspect of the present invention, the control method is a control method of a control device that hydraulically controls the drive mechanism of a mechanical device having a drive mechanism, and is a drive mechanism that controls and operates the drive mechanism. the current angle or gravity compensation torque corresponding to the target between the links constituting the driving mechanism for canceling the dead weight of the components were calculated using the gravity compensation equations constituting said gravity compensation torque, the drive mechanism links Based only on the deviation between the current and target of the joint angle indicating the angle between the current and the target, the gravity compensation torque is changed to the gravity compensation valve opening indicating the valve opening of the control valve that controls the flow rate of the hydraulic pressure. The valve opening control command of the control valve is calculated by converting and adding the gravity compensation valve opening to the feedback valve opening determined by the current driving state amount of the driving mechanism and the target driving state amount. It is characterized by.

According to the third aspect of the present invention, the program controls the computer of the control device that hydraulically controls the drive mechanism of the mechanical device having the drive mechanism, and operates by controlling the drive mechanism by its own weight of the drive mechanism component. The gravity compensation torque calculating means for calculating the gravity compensation torque corresponding to the current angle between the links constituting the drive mechanism or the target thereof by using the gravity compensation equation, the gravity compensation torque, and the drive mechanism are configured. Based only on the deviation between the current and target of the joint angle indicating the angle between the links and the target, the gravity compensating torque indicates the valve opening degree of the control valve that controls the flow rate of the hydraulic pressure. Gravity compensation valve opening degree calculating means to convert to, the gravity compensation valve opening degree is added to the feedback valve opening degree determined by the current drive state amount of the drive mechanism and the target drive state amount, and the valve of the control valve It is characterized in that it functions as a valve opening control means for calculating an opening control command.

According to the present invention, a link that operates only by 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 hydraulic flow rate. It is possible to calculate a valve opening control command for canceling its own weight. Therefore, it is possible to calculate a valve opening control command for canceling the own 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 device 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 device 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 outline of determination of the variable gain coefficient of the control device by 3rd Embodiment. It is a figure explaining the output timing of the gravity compensation torque of the control device by 4th Embodiment.

<First Embodiment>
Hereinafter, the control device according to the 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 is composed of 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 hydraulically controls the joint angle of the joint mechanism in the mechanical device 2. The mechanical device 2 may be, for example, a hydraulic manipulator.

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

The CPU of the control device 1 starts up based on the power being turned on to the control device 1 and executes the stored control program. As a result, the control device 1 is provided with the functions of the gravity compensation torque calculation unit 11, the gravity compensation valve opening degree calculation unit 12, the feedback control unit 13, and the valve opening degree control unit 14.

The gravity compensation torque calculation unit 11 calculates the gravity compensation torque τ _gn [Nm] that cancels the own weight of the link that controls and operates the joint angle of the joint mechanism provided in the mechanical device 2 by using the gravity compensation equation. In the present embodiment, the joint mechanism is one aspect of the drive mechanism constituting the mechanical device 2. The link is an aspect of a drive mechanism component that controls and operates the drive mechanism.
The gravity compensation valve opening degree calculation unit 12 converts the gravity compensation torque τ _gn [Nm] into a gravity compensation valve opening degree a _gn [%] indicating the valve opening degree 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 between the target angle and the current angle of the joint angle to compensate the gravity valve. The opening angle a _gn [%] is calculated.

The feedback control unit 13 calculates a valve opening 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 amount of the driving mechanism and the target driving state amount. Specifically, the feedback control unit 13 performs feedback control so that the joint angle becomes the target angle θ _rn by using the target angle θ _rn and the current angle θ _n of the joint angle of the joint mechanism provided in the mechanical device 2. .. The joint angle is one aspect of the drive state quantity of the drive mechanism. The feedback control unit 13 outputs the angle feedback valve opening a _fn [%] calculated by the feedback control.
The valve opening degree control unit 14 obtains the gravity compensation valve opening degree a _gn [%] obtained from the gravity compensation valve opening degree calculation unit 12 and the angle feedback valve opening degree a _fn [%] obtained from the feedback control unit 13. It is used to calculate the valve opening control command of the control valve.

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

FIG. 2 is a diagram showing a processing flow of the control device according to the first embodiment.
Next, the details of the processing of the control device 1 will be described.
First, as an example, the feedback control unit 13 acquires the deviation Δθ _n of the target angle θ _rn of the joint angle 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 the angle feedback valve opening degree a _fn [%] such that the Δθ _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 calculation unit 11 substitutes the current angle θ _n into the gravity compensation equation g (θ _n ) to calculate the gravity compensation torque τ _gn [Nm] that cancels the own weight of the link that controls and operates 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 degree calculation unit 12.

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

When the gravity compensation valve opening calculation unit 12 determines the variable gain coefficient K _gn , the variable gain coefficient K is based on the positive and negative signs of the value of τ _gn [Nm] × θ _rn and the positive and negative signs 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, in the process of step S105, when the value of τ _gn [Nm] × θ _rn shows + and the value of deviation Δθ _n shows +, the gravity compensation valve opening degree calculation unit 12 _opposes the link against gravity. It is determined that the situation is driving in the direction and the target has not been reached. 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 .
Further, when the value of τ _gn [Nm] × θ _rn indicates + and the value of deviation Δθ _n indicates −, the gravity compensation valve opening degree calculation unit 12 drives the link in the direction opposite to gravity. It is determined that the link has been activated beyond the target. In this case, the gravity compensation valve opening degree calculation unit 12 determines that the gravity compensation is too effective, and determines the variable gain coefficient K _{gn so as} to reduce the variable gain coefficient K _gn .

Further, when the value of τ _gn [Nm] × θ _rn indicates − and the value of deviation Δθ _n indicates +, the gravity compensation valve opening calculation unit 12 sets the target in the situation of driving the link in the direction of gravity. Judge that the situation has not been reached. In this case, the gravity compensation valve opening degree calculation unit 12 determines that the gravity compensation is too effective, and determines the variable gain coefficient K _{gn so as} to reduce the variable gain coefficient K _gn .
Further, when the value of τ _gn [Nm] × θ _rn indicates − and the value of deviation Δθ _n indicates −, the gravity compensation valve opening degree calculation unit 12 sets a target in a situation where the link is driven in the direction of gravity. It is determined that the link has been activated beyond that. 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 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 calculation unit 12 calculates the gravity compensation valve opening degree a _gn [%] by the above equation (1) (step S106). The gravity compensation valve opening degree calculation unit 12 outputs the gravity compensation valve opening degree a _gn [%] to the valve opening degree control unit 14.

The valve opening control unit 14 adds the angle feedback valve opening a _fn [%] obtained from the feedback control unit 13 and the gravity compensating valve opening a _gn [%] obtained from the gravity compensating valve opening calculation unit 12. Then, the valve opening control command is calculated (step S107). 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 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 valve opening degree of the control valve that controls the hydraulic flow rate. Therefore, it is possible to calculate a valve opening control command for canceling the own weight of the link to be operated.

Further, according to the above processing, the operation is performed only by converting the gravity compensation torque τ _gn [Nm] into the gravity compensation valve opening degree a _gn [%] indicating the valve opening degree of the control valve that controls the hydraulic flow rate. It is possible to calculate a valve opening control command for canceling the own weight of the link to be operated. Therefore, it is possible to calculate a valve opening control command for canceling the own 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 positive and negative signs of the value of τ _gn [Nm] × θ _rn and the positive and negative signs of the deviation Δθ _n , the gravity compensation torque τ _gn [ It is possible to calculate an appropriate gravity compensating valve opening degree a _gn [%] according to the relationship between [Nm] and the target angle and the positive / negative of the deviation Δθ _n .

<Second embodiment>
Hereinafter, the control device according to the second embodiment will be described with reference to the drawings.
FIG. 3 is a block diagram showing a configuration of a control device according to the second embodiment.
The hardware configuration of the control device 1 and the mechanical device 2 in the second embodiment is the same as that in the first embodiment.
The CPU of the control device 1 starts up based on the power being turned on to the control device 1 and executes the stored control program. As a result, as shown in FIG. 3, the control device 1 has the functions of the gravity compensation torque calculation unit 11, the gravity compensation valve opening degree calculation unit 12, the feedback control unit 13, the valve opening degree control unit 14, and the fluctuation suppression unit 15. Is equipped.

The gravity compensation torque calculation unit 11 calculates the gravity compensation torque τ _gn [Nm] that cancels the own weight of the link that controls the angle of the joint provided in the mechanical device 2 and operates by using the gravity compensation equation.
The gravity compensation valve opening degree calculation unit 12 converts the gravity compensation torque τ _gn [Nm] into a gravity compensation valve opening degree a _gn [%] indicating the valve opening degree of the control valve that controls the hydraulic flow rate. Specifically, the gravity compensation valve opening calculation unit 12 inputs the target angle θ _rn of the joint angle into the gravity compensation equation to calculate the gravity compensation torque τ _gn [Nm] and the target angle θ _{rn of the} joint angle. The gravity compensation valve opening degree is calculated by 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 so that the joint angle becomes the target angle θ _rn by using the target angle θ _rn of the joint angle of the joint mechanism provided in the mechanical device 2 and the current angle θ _n . The feedback control unit 13 outputs the angle feedback valve opening a _fn [%] calculated by the feedback control.
The valve opening degree control unit 14 has a gravity compensation valve opening degree a _gn [%] obtained from the gravity compensation valve opening degree calculation unit 12 and an angle feedback valve opening degree a _fn [%] obtained 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 suppression unit 15 may be configured by a low-pass filter. By the processing of the fluctuation suppression 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 calculation unit 12 is suppressed in the frequency of increase / decrease. To.

FIG. 4 is a diagram showing a processing flow of the control device according to the second embodiment.
Next, the details of the processing of the control device 1 according to the second embodiment will be described.
First, the feedback control unit 13 acquires the deviation Δθ _n of the target angle θ _rn and the current angle θ _n of the joint angle of the joint mechanism to which the two links are connected (step S201). The deviation can be calculated by θ _rn − θ _n . The feedback control unit 13 uses the deviation Δθ _n to calculate the 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 calculation unit 11 substitutes the target angle θ _rn into the gravity compensation equation g (θ _rn ) to calculate the gravity compensation torque τ _gn [Nm] that cancels the own weight of the link that controls and operates 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 degree calculation unit 12.

The fluctuation suppression unit 15 acquires the deviation Δθ _n . The fluctuation suppression unit 15 suppresses the acquired fluctuation frequency of the deviation Δθ _n and outputs it to the gravity compensation valve opening degree calculation unit 12 (step S204). The gravity compensation valve opening degree 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 degree calculation unit 12 calculates a value of τ _gn [Nm] × θ _rn (step S205). The gravity compensation valve opening calculation unit 12 has a variable gain coefficient K of the gravity compensation valve opening calculation formula represented by the equation (1) based on the value of τ _gn [Nm] × θ _{rn and} the value of the deviation Δθ _{n. The gn} is determined (step S206).

Similar to the first embodiment, the gravity compensation valve opening degree calculation unit 12 of the second embodiment determines the positive / negative sign of the value of τ _gn [Nm] × θ _rn when determining the variable gain coefficient K _gn . It is determined whether to increase or decrease the variable gain coefficient K _gn based on the positive and negative signs of the deviation Δθ _n . 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 calculation unit 12 calculates the gravity compensation valve opening degree a _gn [%] by the above equation (1) (step S207). The gravity compensation valve opening degree calculation unit 12 outputs the gravity compensation valve opening degree a _gn [%] to the valve opening degree control unit 14.

The valve opening control unit 14 adds the angle feedback valve opening a _fn [%] obtained from the feedback control unit 13 and the gravity compensating valve opening a _gn [%] obtained from the gravity compensating valve opening calculation unit 12. Then, the 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 processing, in addition to the same effect as in 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 idea of self-weight compensation is to compensate for static (no dynamic characteristics) weight load changes, and it may not be necessary to finely change the amount of compensation. In addition, the disturbance caused by not compensating can be covered by feedback. Rather, sudden changes may cause disturbance, and in order to prevent this, the input signal has little fluctuation.

<Third embodiment>
Hereinafter, the control device according to the 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 degree calculation unit 12 changes the variable gain coefficient K _gn that converts the gravity compensation valve opening degree τ _gn [Nm] into the gravity compensation valve opening degree a _gn [%] based on the value of the deviation Δθ _n . For example, when the gravity compensating valve opening degree calculation unit 12 moves the joint angle beyond the steady deviation of the joint angle changed at a constant angular velocity, it is proportional to the magnitude of the joint angle and starts from a predetermined coefficient value K1. The variable gain coefficient K _gn is increased up to a large predetermined value K2.

The gravity compensation valve opening degree 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 deviation of the joint angle that is changed at a constant angular velocity.

When the deviation Δθ _n becomes a predetermined magnitude Δθ ₁ , the gravity compensation valve opening degree calculation unit 12 proportionally sets the variable gain coefficient K _gn from the predetermined coefficient value K1 according to the magnitude of the deviation Δθ _n. To increase. Δθ1 is determined based on the steady-state deviation of the joint angle generated in the control of the joint angle 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 the outputable valve opening saturation value and the like. The coefficient value K1 is determined by using 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 the 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.
The method for determining the variable gain coefficient K _gn according to the third embodiment may be applied to the first embodiment and the second embodiment system value.

<Fourth Embodiment>
Hereinafter, the control device according to the fourth embodiment will be described with reference to the drawings.
FIG. 6 is a diagram for explaining the output timing of the gravity compensation torque of the control device according to the fourth embodiment.
The waveform shown in FIG. 6 shows the temporal change of the joint angle of the joint mechanism until it reaches the target angle θ _rn . Response time is required for the joint angle of the joint mechanism to reach the target angle θ _rn . When the target trajectory θ _rn is given in advance, the gravity compensation torque calculation unit 11 advances the response time λ of the control valve separately measured by a step response or the like to cancel the gravity 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 degree 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 by the response time λ of the control valve separately measured by a step response or the like. Then, the gravity compensation torque τ _gn [Nm] that cancels the own weight of the link is calculated by the gravity compensation equation g (θ _rn (t + λ)), and the gravity compensation torque τ _gn [Nm] is calculated at time t to open the gravity compensation valve. Output to the degree calculation unit 12.

Since the hydraulic system has a strong non-linearity of minute opening, the response delay cannot be ignored. Therefore, this is regarded as wasted time, and the gravity compensation torque τ _gn [Nm] that cancels the own weight of the link in advance is calculated by the gravity compensation equation g (θ _rn ), and the gravity compensation torque τ _gn [Nm] is gravity compensation. Output to the valve opening degree calculation unit 12. As a result, for example, in the third embodiment, Δθ _n becomes large positive at the beginning of movement, and therefore Δθ _n becomes negative and the variable gain coefficient K _gn becomes small because the variable gain coefficient K _gn becomes large. It is possible to prevent hunting of _gn .

<Fifth Embodiment>
The gravity compensation valve opening calculation unit 12 sets the variable gain coefficient K _gn, which converts the gravity compensation torque τ _gn [Nm] into the gravity compensation valve opening a _gn [%], to be 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 degree calculation unit 12 makes the variable gain coefficient K _gn variable as K _gn (θn') with the angular velocity as an input variable. The variable gain coefficient K _gn (θn') may be _trained by moving it in various trajectories several times at a certain angular velocity (θn').

In each of the above embodiments, the drive mechanism includes a plurality of links connected via the joint mechanism, and the control device 1 hydraulically controls the joint angle of the joint mechanism of the mechanical device 2 having the drive mechanism. There is. Then, the gravity compensation torque calculation unit 11 calculates the gravity compensation torque that cancels the own weight of the link, which is a component of the drive mechanism that controls and operates the joint angle, by using the gravity compensation equation. Further, the valve opening degree control unit 14 is based on the angle feedback valve opening degree and the gravity compensation valve opening degree determined by the joint angle which is the current driving state amount of the joint mechanism and the joint angle which is the target driving state amount. Calculate the valve opening control command of the control valve.
However, the drive mechanism may be a mechanism other than the joint mechanism. For example, the drive mechanism may be another mechanism such as a telescopic mechanism. When the drive mechanism is a telescopic mechanism, the drive state amount may indicate an expansion amount or a contraction amount.

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

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

Further, even when the drive mechanism is other than the joint mechanism, a fluctuation suppression 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 calculation unit 12 changes the variable gain coefficient for converting the gravity compensation torque into the gravity compensation valve opening to a certain degree, and the steady-state deviation of the drive state amount. When the amount of driving state exceeds, the amount is increased to a predetermined value larger than the predetermined coefficient value in proportion to the magnitude of the amount of driving state, and when the amount of driving state is less than the steady deviation of the amount of driving state. 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 preceded by a predetermined time determined based on the response time until the control valve reaches the calculated valve opening degree. The torque may be calculated and output.

Further, even when the drive mechanism is other than the joint mechanism, the gravity compensation valve opening calculation unit 12 shifts the variable gain coefficient for converting the gravity compensation torque to the gravity compensation valve opening so that the drive state amount shifts to the target drive state amount. You may try to learn the fastest variable gain coefficient by.

The control device 1 described above has a computer system inside. A program for causing the control device 1 to perform each of the above-described processes is stored in a computer-readable recording medium of the control device 1, and the computer of the control device 1 reads and executes this program. , The above processing is performed. Here, the computer-readable recording medium refers to a magnetic disk, a magneto-optical disk, a CD-ROM, a DVD-ROM, a semiconductor memory, or the like. Further, this computer program may be distributed to a computer via a communication line, and the computer receiving the distribution may execute the program.

Further, the above program may be for realizing a part of the functions of the above-mentioned processing units. Further, a so-called difference file (difference program) may be used, which can realize the above-mentioned functions in combination with a program already recorded in the computer system.

1 ... Control device 2 ... Mechanical device 11 ... Gravity compensation torque calculation unit 12 ... Gravity compensation valve opening calculation unit 13 ... Feedback control unit 14 ... Valve opening control unit 15.・ ・ Fluctuation suppression unit

Claims (10)

A control device that hydraulically controls the drive mechanism of a mechanical device having a drive mechanism
Gravity compensation torque for calculating the gravity compensation torque corresponding to the current angle between the links constituting the drive mechanism or its target by using the gravity compensation equation to cancel the own weight of the drive mechanism component that controls and operates the drive mechanism. Calculation part and
Wherein a gravity compensation torque, current and deviation between the target joint angles indicating the angle between the links constituting the driving mechanism, based only on the target, said gravity compensation torque, control for controlling the flow rate of the hydraulic Gravity compensation valve opening calculation unit that converts to gravity compensation valve opening indicating the valve opening of the valve,
Valve opening control that calculates the valve opening control command of the control valve by adding the gravity compensation valve opening to the feedback valve opening determined by the current driving state amount of the driving mechanism and the target driving state amount. Department and
A control device comprising. The drive mechanism comprises 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 own weight of the link which is a component of the drive mechanism that controls and operates the joint angle by using the gravity compensation equation.
The valve opening control unit determines the angle feedback valve opening and the gravity compensating valve opening determined by the joint angle which is the current driving state amount of the joint mechanism and the joint angle which 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 degree calculation unit inputs the current joint angle into the gravity compensation equation to 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 degree is calculated using the deviation from the angle. The gravity compensation valve opening degree calculation unit inputs the target joint angle into the gravity compensation equation to 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 degree is calculated using the deviation from the angle. The control device according to claim 3 or 4, further comprising a fluctuation suppressing unit that suppresses the fluctuation frequency of the deviation. The gravity compensation valve opening calculation unit changes the variable gain coefficient for converting the gravity compensation torque into the gravity compensation valve opening at a constant angular velocity. When the angle exceeds the steady deviation of the joint angle, the angle is equal to or greater than the constant deviation. Claims 2 to 5 increase the value to a predetermined value larger than the predetermined coefficient value in proportion to the magnitude of the above, and hold the joint angle at the predetermined coefficient value when the angle is less than the steady deviation of the joint angle. The control device according to any one of the above. From claim 2, the gravity compensation torque calculation unit calculates and outputs the gravity compensation torque at a time preceding by a predetermined time determined based on the response time until the control valve reaches the calculated valve opening degree. The control device according to any one of claims 6. The claim that the gravity compensation valve opening degree calculation unit learns the variable gain coefficient which converts the gravity compensation torque into the gravity compensation valve opening degree and becomes the fastest until the joint angle shifts to a target angle. The control device according to any one of claims 2 to 7. A control method for a control device that hydraulically controls the drive mechanism of a mechanical device having a drive mechanism.
The gravity compensation torque corresponding to the current angle between the links constituting the drive mechanism or its target, which cancels the own weight of the drive mechanism component that controls and operates the drive mechanism, is calculated by using the gravity compensation equation.
Wherein a gravity compensation torque, current and deviation between the target joint angles indicating the angle between the links constituting the driving mechanism, based only on the target, said gravity compensation torque, control for controlling the flow rate of the hydraulic Converted to gravity compensation valve opening, which indicates the valve opening of the valve,
A control method for calculating a valve opening control command of the control valve by adding the gravity compensating valve opening to the feedback valve opening determined by the current driving state amount of the driving mechanism and the target driving state amount. A computer of a control device that hydraulically controls the drive mechanism of a mechanical device having a drive mechanism.
Gravity compensation torque for calculating the gravity compensation torque corresponding to the current angle between the links constituting the drive mechanism or its target by using the gravity compensation equation to cancel the own weight of the drive mechanism component that controls and operates the drive mechanism. Calculation method,
Wherein a gravity compensation torque, current and deviation between the target joint angles indicating the angle between the links constituting the driving mechanism, based only on the target, said gravity compensation torque, control for controlling the flow rate of the hydraulic Gravity compensation valve opening calculation means for converting to gravity compensation valve opening indicating the valve opening of the valve,
Valve opening control that calculates the valve opening control command of the control valve by adding the gravity compensation valve opening to the feedback valve opening determined by the current driving state amount of the driving mechanism and the target driving state amount. means,
A program that functions as.

Images