JP2019094176A - Control device, control method, and pogram - Google Patents

Abstract

To smoothly move a tip of a boom.SOLUTION: A control device 10 comprises: an input receiving part 11; a speed vector generating part 12 that generates, on the basis of input contents received by the input receiving part 11, a speed vector indicating, in a three-dimensional coordinate system, a direction and speed in and at which a tip of a boom that can extend, rise and fall, and turn; a converting part 13 that converts the speed vector to an operation vector indicating respective operation directions and speeds of the extension, rise and fall, and turning of the boom, by multiplying the speed vector by an inverse matrix of Jacobian matrix from the left or by using a formula obtained using the inverse matrix of the Jacobian matrix; and an actuator control part 14 that operates an actuator that extends the boom, an actuator that raises and falls the boom, and an actuator that turns the boom in the operation direction and at the speed indicated by the operation vector.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a control device, a control method, and a program.

  Patent Documents 1 and 2 disclose techniques for controlling the position of a working unit attached to the tip of a boom by controlling the drive direction and drive speed of a hydraulic actuator with a hydraulic control device. Patent Document 1 generates a signal relating to the moving direction and speed of the working unit based on the tilting operation of the operating lever, and generates the moving direction and speed of the generated working unit using the trigonometric function. It is disclosed to convert to moving direction and speed.

  Patent Document 3 discloses a technique for following the position of the tip of a boom to the position where a position command transmitter that transmits ultrasonic waves is present.

Toko 7-29760 Patent No. 4021530 JP 2014-97875

  An object of the present invention is to provide a technique for smoothly moving the tip of a boom.

According to the invention
An input reception unit,
A velocity vector generation unit for generating a velocity vector indicating a direction and velocity for moving the tip of the expandable / retractable / retractable / swingable boom tip on the basis of the input content received by the input receiving unit;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. A converter for converting into a motion vector indicating the motion direction and speed of
An actuator for extending and retracting the boom, an actuator for raising and lowering the boom, and an actuator control unit for operating the actuator for pivoting the boom with the motion direction and speed indicated by the motion vector;
A control device is provided.

Moreover, according to the present invention,
The computer is
An input reception process,
A velocity vector generation step of generating a velocity vector indicating a direction and a velocity for moving the tip of the expandable / retractable / retractable / swingable boom tip on the basis of the input content received in the input reception step;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. Converting into a motion vector indicating the motion direction and speed of
An actuator control step of operating the actuator for expanding and contracting the boom, the actuator for raising and lowering the boom, and the actuator for pivoting the boom with the movement direction and speed indicated by the movement vector;
A control method for performing

Moreover, according to the present invention,
Computer,
Input acceptance means,
Velocity vector generating means for generating a velocity vector indicating the direction and velocity of moving the tip of the expandable / retractable / retractable and pivotable boom in a three-dimensional coordinate system based on the input contents received by the input receiving means;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. Conversion means for converting into a motion vector indicating the motion direction and speed of
Actuator control means for operating the actuator for expanding and contracting the boom, the actuator for raising and lowering the boom, and the actuator for rotating the boom with the movement direction and speed indicated by the movement vector;
A program to function as is provided.

  According to the present invention, the tip of the boom can be moved smoothly.

It is an example of a functional block diagram of a control device of this embodiment. It is a figure which shows an example of the hardware constitutions of the control apparatus of this embodiment. It is a figure which shows a spool displacement and a cylinder speed characteristic. It is a figure which shows the time change of the spool displacement with respect to the time change of target speed. It is a figure which shows the definition of various variables and constants. It is a figure which shows an example of a structure of a working machine. It is a figure which shows an example of a movement of the front-end | tip of the boom at the time of the control apparatus of this embodiment controlling. It is a figure which shows an example of a movement of the front-end | tip of the boom of a comparative example. It is a figure for demonstrating the effect of this embodiment. It is a figure for demonstrating the effect of this embodiment. It is a figure for demonstrating the effect of this embodiment. It is a figure which shows an example of a movement of the front-end | tip of the boom at the time of the control apparatus of this embodiment controlling. It is a figure which shows an example of a movement of the front-end | tip of the boom at the time of the control apparatus of this embodiment controlling.

First Embodiment
FIG. 1 shows an example of a functional block diagram of a control device 10 of the present embodiment. As illustrated, the control device 10 includes an input reception unit 11, a velocity vector generation unit 12, a conversion unit 13, and an actuator control unit 14.

  The input receiving unit 11 receives various inputs. The velocity vector generation unit 12 generates, based on the input content received by the input reception unit 11, a velocity vector that indicates the direction and velocity for moving the tip of the expandable / retractable / retractable and pivotable boom in a three-dimensional coordinate system. The transformation unit 13 stretches, contracts, and turns the boom by using the formula obtained by multiplying the generated velocity vector by the inverse matrix of the Jacobian matrix from the left or by using the inverse matrix of the Jacobian matrix. The motion vector which shows the motion direction and speed of is calculated. The actuator control unit 14 operates an actuator that extends and retracts the boom, an actuator that raises and lowers the boom, and an actuator that pivots the boom in the motion direction and speed indicated by the motion vector.

  Such a control device 10 realizes an operation of smoothly moving the tip of the boom. Hereinafter, details of the control device 10 will be described.

  First, an example of the hardware configuration of the control device 10 will be described. Each functional unit included in the control device 10 according to the present embodiment includes a central processing unit (CPU) of any computer, a memory, a program loaded to the memory, and a storage unit such as a hard disk storing the program (the device is shipped in advance In addition to programs stored from the stage, it can also store storage media such as CD (Compact Disc) and programs downloaded from a server on the Internet, etc.) Arbitrary hardware and software options centering on the network connection interface It is realized by the combination of And it is understood by those skilled in the art that there are various modifications in the implementation method and apparatus.

  FIG. 2 is a block diagram illustrating the hardware configuration of the control device 10 according to the present embodiment. As shown in FIG. 2, the control device 10 has a processor 1A, a memory 2A, an input / output interface 3A, a peripheral circuit 4A, and a bus 5A. Peripheral circuit 4A includes various modules. The control device 10 may be configured of a plurality of physically and / or logically divided devices. In this case, each of the plurality of devices may have a processor 1A, a memory 2A, an input / output interface 3A, a peripheral circuit 4A, and a bus 5A.

  The bus 5A is a data transmission path for the processor 1A, the memory 2A, the peripheral circuit 4A, and the input / output interface 3A to mutually transmit and receive data. The processor 1A is an arithmetic processing unit such as a CPU or a graphics processing unit (GPU), for example. The memory 2A is, for example, a memory such as a random access memory (RAM) or a read only memory (ROM). The input / output interface 3A is an interface for acquiring information from an input device (eg, a keyboard, a mouse, a microphone, a physical key, a touch panel display, a code reader, etc.), an external device, an external server, an external sensor, etc. Example: Display, speaker, printer, mailer etc.), an external device, an interface for outputting information to an external server, etc. are included. The processor 1A can issue an instruction to each module and perform an operation based on the result of the operation.

  Next, a working machine provided with a boom controlled by the control device 10 of the present embodiment will be described. The work machine and the control device 10 may be physically and / or logically integrated, or may be physically and / or logically separated.

  The work machine has an actuator (for example, a boom telescopic cylinder) that extends and retracts the boom, an actuator (for example, a boom hoisting cylinder) that raises and lowers the boom, and an actuator (for example, boom rotation motor) that turns the boom. At least one of the plurality of actuators (eg, all of the plurality of actuators) is hydraulically controlled.

  The operation of the boom is controlled by control valves and spools. That is, the amount of pressure oil supplied to each actuator is adjusted by displacing and adjusting the amount of discharge of pressure oil from the control valve using a spool. By adjusting the amount of pressure oil supplied to each actuator, the operating direction and speed of each actuator are adjusted. Then, the movement direction and speed of the tip of the boom are adjusted by adjusting the operation of each actuator.

  Here, the characteristics of the spool provided in the working machine of the present embodiment will be described with reference to FIG. FIG. 3 shows an example of the relationship between spool displacement and cylinder speed. Based on this characteristic curve, the spool displacement for moving the actuator at a desired speed can be determined. For example, if it is desired to move the actuator at a speed of 20% of the rated speed, the spool displacement is set to 51% of the maximum displacement. Also, if you want to move the actuator at 80% of rated speed, set the spool displacement to 94% of the maximum displacement.

  The boom length (boom length), the elevation angle and the turning angle are determined by the operations of the actuator for extending and retracting the boom, the actuator for raising and lowering the boom, and the actuator for turning the boom. The speed of each actuator is the expansion / contraction speed of the boom (change in boom length per unit time), the elevation angular velocity (change in elevation angle of the boom per unit time), and the turning angular velocity (rotation of the boom per unit time) Corresponding to the change in angle).

  As shown in FIG. 3, the spool of the control valve has a "constant dead zone" within a constant stroke range near its neutral position. That is, while the spool displacement is in the dead zone, no pressure oil is supplied to the actuator and the actuator does not move. In order to supply pressure oil to the actuator and move it, the spool displacement must go out of the dead zone.

  FIG. 4 shows the relationship between the temporal change of the target velocity of the cylinder expressed linearly and the temporal change of the spool displacement for realizing the target velocity. The target velocity of the cylinder is the velocity of the cylinder for moving the tip of the boom at a desired velocity, and is defined as the terminal velocity after stabilization at a constant velocity.

  The work machine also has means (differential transformer) for detecting the displacement and speed of the spool. Furthermore, the work machine has a boom length detector that detects the length of the boom, a boom angle detector that detects the elevation angle of the boom, and a pivot angle detector that detects the pivot angle of the boom.

  Next, the control device 10 will be described. The controller 10 controls the operation of the actuator. As shown in the functional block diagram of FIG. 1, the control device 10 includes an input reception unit 11, a velocity vector generation unit 12, a conversion unit 13, and an actuator control unit 14.

  The input receiving unit 11 receives an input specifying a direction and a speed for moving the tip of the boom through the operation lever (input device). The input receiving unit 11 may receive an input for specifying the direction and speed for moving the tip of the boom through another input device such as a touch panel display, a physical button, a microphone, a keyboard, or a mouse.

  The input reception unit 11 detects the operation content of the operation lever at each control cycle, converts the detected operation content of the operation lever into a velocity vector indicating the direction and speed at which the tip of the boom is moved, and outputs it. The velocity vector is a three-dimensional coordinate with the predetermined position of the work machine as the origin and the predetermined direction of the work machine as the x-axis (horizontal axis), y-axis (horizontal axis) and z-axis (vertical axis) It is indicated by a system (hereinafter sometimes referred to as "work machine coordinate system"). The control cycle is, for example, 1 millisecond to 100 milliseconds.

  The input receiving unit 11 determines the direction in which the tip of the boom is moved based on the direction in which the control lever is tilted, and determines the speed in which the tip of the boom is moved based on the angle in which the control lever is tilted. For example, the direction in which the control lever is inclined may be associated in advance with the predetermined direction of the work machine coordinate system. In addition, the angle at which the control lever is inclined may be associated in advance with the speed at which the tip of the boom is moved. Then, based on the correspondence relationship, the input reception unit 11 may convert the detected operation content of the operation lever into a velocity vector indicating the direction and velocity at which the tip of the boom is moved. For example, two control levers may be prepared, the horizontal control direction and speed may be designated by the first control lever, and the vertical control direction and speed may be designated by the second control lever.

  The velocity vector generation unit 12 sets the direction and speed for moving the tip of the boom capable of expansion, contraction, undulation, and rotation on a three-dimensional coordinate system (work machine coordinate system) based on the input content received by the input reception unit 11 for each control cycle. Generate and output the velocity vector shown in). Hereinafter, the velocity vector generated by the velocity vector generation unit 12 will be referred to as a designated velocity vector.

  Here, an example of generation of the designated velocity vector will be described.

"Indicated velocity vector generation example 1"
The velocity vector generation unit 12 can use the velocity vector output by the input reception unit 11 as the designated velocity vector as it is.

"Instructed velocity vector generation example 2"
The velocity vector generation unit 12 can adopt the movement direction indicated by the velocity vector output from the input reception unit 11 as it is as the movement direction of the designated velocity vector. Then, the velocity of the designated velocity vector may be determined as follows.

  If “(the current speed of the tip of the boom) <(the speed indicated by the speed vector output from the input reception unit 11)”, the speed vector generation unit 12 sets “(the speed of the instructed speed vector) = (boom) It may be calculated by the current velocity of the tip of (+) (acceleration) × (time for control cycle) ". The acceleration may be a predetermined fixed value.

  Immediately after the start of the operation of the boom, etc., the speed of the tip of the boom is small, so that the condition is easily satisfied. In this case, by determining the velocity of the designated velocity vector as described above, a rapid increase in velocity can be suppressed and a smooth operation can be realized.

  On the other hand, when “(the current speed of the tip of the boom) ≧ (the speed indicated by the speed vector output by the input reception unit 11)”, the speed vector generation unit 12 detects the speed output by the input reception unit 11 The velocity indicated by the vector may be adopted as the velocity of the indicated velocity vector. A certain amount of time has passed since the start of the operation of the boom, and it becomes easy to satisfy the condition when the moving speed of the tip of the boom is stable.

  The transformation unit 13 multiplies the indicated velocity vector output from the velocity vector generation unit 12 by the inverse matrix of the Jacobian matrix from the left to indicate the movement direction and velocity of each of the expansion, contraction, undulation and turning of the boom. It is converted into a motion vector (see Equations (2) and (14) below). The conversion unit 13 may convert the designated velocity vector into a motion vector using a calculation formula obtained using the inverse matrix of the Jacobian matrix (see Formula (15) below).

  Here, the inverse matrix of the Jacobian matrix will be described.

  When the boom is extended, retracted, or turned, the tip of the boom moves at a predetermined speed in a predetermined direction. That is, a velocity vector is generated at the tip of the boom. In the case of simultaneously performing a plurality of stretching, undulation, and turning, the velocity vector of the tip of the boom can be determined by adding the velocity vectors generated at the tip of the boom by each operation.

  The motion vector of the boom and the velocity vector of the tip of the boom are represented by the Jacobian matrix J as shown in equation (1).

  The letter with a black dot above x indicates the velocity component of movement of the tip of the boom in the work machine coordinate system in the x-axis direction. In the specification, the velocity component is represented as "x '". The letter with a black dot above y indicates the velocity component of movement of the tip of the boom in the y-axis direction in the work machine coordinate system. In the specification, the velocity component is referred to as "y '". The letter with a black dot above z indicates the velocity component of the movement of the tip of the boom in the z-axis direction in the work machine coordinate system. In the specification, the velocity component is referred to as "z '".

characters marked with black dots on the q ₁ shows the angular velocity of the turning of the boom (the amount of change per unit turning angle time). In the specification, the velocity component is referred to as "q ₁ '". The letter with a black dot above q ₂ indicates the angular velocity of the boom undulation (the amount of change in undulation angle per unit time). In the specification, the velocity component is referred to as "q ₂ '". characters marked with black dots on the q ₃ shows the telescopic speed of the boom (variation per unit boom length time). The specification, representing the velocity component and "q _{3 '."}

Telescopic boom, undulations, the degree of freedom of the speed of the tip of the swing speed and the boom is the same (J is a square matrix), and, when the boom is not singular, the use of the inverse matrix J ^-1, the formula ( As shown in 2), the motion vector can be obtained from the velocity vector of the tip of the boom. Note that the unusual posture of the boom means, for example, a state in which the boom is directed right up or down.

  When the matrix calculation is expanded, it becomes like Formula (3).

Here, the inverse matrix J ⁻¹ is obtained as follows.

"Forward kinematics calculation using homogeneous transformation matrix (q ₁ , q ₂ , q ₃ → x, y, z)"
First, variables and constants are defined as shown in FIG. 5 and equation (4). H _outrigger , H _column , D _column , and D _boom are fixed values determined based on the configuration of the working machine.

The homogeneous transformation matrix of the reference coordinate system → turn table coordinate system is as shown in Expression (5). The reference coordinate system is the _{0 0} coordinate system shown in FIG. The ground immediately below the center of column rotation is the origin, the left side of the vehicle is the X ₀ axis, the rear of the vehicle is the Y ₀ axis, and the vertically upper is the Z ₀ axis. Turntable coordinate system is a sigma ₁ coordinate system shown in FIG. 5, the turntable center as the origin, the azimuth angle of the boom is rotated the reference coordinate system translation and Z ₁ about an axis to coincide with the Y ₁ axis It is a thing. Equation (5) is, _{Z 0} in the axial direction and _{H outrigger} translation means to _{q 1} rotates in _{Z 1} axis.

The homogeneous transformation matrix of the turntable coordinate system → column coordinate system is as shown in equation (6). Column coordinate system is a sigma ₂ coordinate system shown in FIG. 5, rotating the boom foot pin center as the origin, the translation and the X ₂ axis around the turntable coordinate system so that the boom longitudinal direction coincides with the Y ₂ axially It is Equation (6), _{H outrigger} to _{Z 1} axial direction, _{-D column} translates to _{Y 1} axially, it means that _{q 2} rotates in the _{X 2} axis.

The homogeneous transformation matrix of the column coordinate system → top sheave coordinate system is as shown in Expression (7). Top wheel coordinate system is a sigma ₃ coordinate system shown in FIG. 5, it is obtained by translating the column coordinate system so that the top wheel center is the origin. Equation (7) means that _{q 3} translational movement to the _{Z 2} axial direction _-D boom, the _{Y 2} axially.

  The homogeneous transformation matrix of the reference coordinate system → top sheave coordinate system is as shown in equation (8).

  When the origin (0, 0, 0) of the top sheave coordinate system is expressed in the reference coordinate system using the equation (8), the equation (9) is obtained.

  If each coefficient of Formula (9) is calculated and expanded, it will become like Formula (10).

"Inverse kinematics calculation using the inverse matrix of the Jacobian matrix _{(x', y', z'→ q} 1 ', q 2', q 3 ') "
If time differentiation of x, y and z is carried out and q ₁ ′, q ₂ ′ and q ₃ ′ are put together, it becomes like Formula (11).

  If this is made into matrix notation, it will become like Formula (12).

If J- ¹ is multiplied from the left to both sides and the left side and the right side are interchanged, it becomes like Formula (13).

Calculating J ⁻¹ gives equation (14).

If Formula (14) is substituted and expand | deployed to J ^<-1> of Formula (13), it will become like Formula (15). By using the equation (15), the movement direction and speed (motion vector) of each of the plurality of actuators for moving the tip of the boom in a predetermined direction at a predetermined speed can be determined.

  Returning to FIG. 1, the actuator control unit 14 operates the actuator that extends and retracts the boom, the actuator that raises and lowers the boom, and the actuator that pivots the boom in the direction and speed indicated by the motion vector.

  That is, the actuator control unit 14 uses the “motion vector” calculated by the conversion unit 13 and “predetermined information indicating the relationship between the spool displacement and the velocity of the actuator (eg, a function, a table, etc.)”. Based on this, the spool displacement for moving at the speed indicated by the motion vector in the direction indicated by the motion vector is determined. Then, the spool displacement is set as determined.

  Here, for reference, FIG. 6 shows an example of the configuration of the control device 10 of the present embodiment. In addition, it is an example to the last, and it is not limited to this.

  Next, an example of the flow of processing by the control device 10 of the present embodiment and the movement of the tip of the boom according to that will be described using FIG. 7. FIG. 7 is a schematic view of the boom observed from above. The tip of the boom moves in the order of (1) → (2) → (3) → (4). A change of (1) → (2) occurs in one control cycle, a change of (2) → (3) occurs in the next control cycle, and a change of (3) → (4) in the next control cycle I shall do. Note that these assumptions also apply to FIGS. 8, 12 and 13 described below.

  In FIG. 7, when the position of the tip of the boom is stopped at (1), the input receiving unit 11 is in a state of waiting for an instruction input of the first control cycle (first control cycle). Then, when the operator performs an operation to tilt the operation lever in a predetermined direction by a predetermined angle, the input reception unit 11 detects the operation content of the operation lever. Then, the input reception unit 11 converts the detected operation content of the operation lever into a velocity vector indicating the direction and velocity for moving the tip of the boom, and outputs the velocity vector as the first control cycle. Here, it is assumed that the velocity vector in the direction indicated by the arrow extending from the tip of the boom of (1) is output.

  Thereafter, the velocity vector generation unit 12 generates and outputs an instruction velocity vector of the first control cycle based on any of the above-described instruction velocity vector generation examples 1 to 3.

  Thereafter, the conversion unit 13 multiplies the indicated velocity vector by the inverse matrix of the Jacobian matrix from the left to operate the indicated velocity vector of the first control cycle to indicate the movement direction and velocity of each of the boom expansion and contraction, undulation and turning. Convert to a vector (see Equations (2) and (14)). Note that the conversion unit 13 converts the indicated velocity vector of the first control cycle into a motion vector (a motion vector of the first control cycle) using a calculation formula obtained using the inverse matrix of the Jacobian matrix. (See equation (15)).

  After that, the actuator control unit 14 uses the “motion vector of the first control cycle” and the “held information indicating the relationship between the spool displacement and the velocity of the actuator (eg, function, table, etc.)” held in advance. The spool displacement for moving at the speed indicated by the motion vector in the direction indicated by the motion vector of the first control cycle is determined. Then, the spool displacement is set as determined.

  As a result, the tip of the boom moves during the first control cycle, and the position of the tip of the boom after the first control cycle becomes (2) as illustrated. Note that as shown in the drawing, due to an error or the like of the operation of the boom, a shift may occur from the direction indicated by the designated speed vector. In the case of the drawing, vertical displacement in the drawing occurs.

  The input reception unit 11 waits for an instruction input of the next control cycle (second control cycle) after an arbitrary timing after detecting the operation of the operation lever corresponding to the first control cycle. Then, when the operator performs an operation to tilt the operation lever in a predetermined direction by a predetermined angle, the input reception unit 11 detects the operation content of the operation lever. Then, the input reception unit 11 converts the detected operation content of the operation lever into a velocity vector indicating the direction and velocity for moving the tip of the boom, and outputs the velocity vector as the second control cycle. The operator may continuously tilt the control lever in the predetermined direction at a predetermined angle.

  Thereafter, the velocity vector generation unit 12 generates and outputs an instruction velocity vector of the first control cycle based on any of the above-described instruction velocity vector generation examples 1 to 3.

  After that, the conversion unit 13 multiplies the instructed velocity vector by the inverse matrix of the Jacobian matrix from the left to operate the instructed velocity vector of the second control cycle to indicate the movement direction and velocity of each of the boom expansion and contraction, undulation and swing. Convert to a vector (see Equations (2) and (14)). The conversion unit 13 converts the indicated velocity vector of the second control cycle into a motion vector (motion vector of the second control cycle) using a calculation formula obtained using the inverse matrix of the Jacobian matrix. (See equation (15)).

  After that, the actuator control unit 14 uses the “motion vector of the second control cycle” and the “held information indicating the relationship between the spool displacement and the velocity of the actuator (eg, function, table, etc.)” held in advance. The spool displacement for moving at the speed indicated by the motion vector in the direction indicated by the motion vector of the second control cycle is determined. Then, after the end of the first control cycle (after a predetermined time has elapsed since control is started with the motion vector of the first control cycle), the displacement of the spool is set as obtained based on the motion vector of the second control cycle Do.

  As a result, the tip of the boom moves during the second control cycle, and the position of the tip of the boom after the second control cycle becomes (3) shown. Note that as shown in the drawing, due to an error or the like of the operation of the boom, a shift may occur from the direction indicated by the designated speed vector. In the case of the drawing, vertical displacement in the drawing occurs.

  Thereafter, the same processing is repeated. The control cycle is, for example, 1 millisecond to 100 milliseconds.

  Next, the operation and effect of the control device 10 of the present embodiment will be described.

"First effect"
First, as a control method of moving the tip of the boom linearly to the destination, means (position feedback) for repeatedly controlling the position of the tip of the boom can be considered. This is a means different from the control method of the present embodiment.

  When performing control by position feedback, for example, the current position of the tip of the boom is compared with the target position, and a positive output is output if the current position has not reached the target position, and the current position exceeds the target position Makes the target position and the current position coincide by outputting a negative output. The target position is calculated from the spool displacement, for example, based on the operation content (inclination direction, inclination angle) of the operation lever.

  Here, in position feedback, consider the case where the tip of the boom is continuously moved in the right direction of the figure by turning and stretching as shown in FIG. In this condition, the basic movement direction is "right" for turning and "stretching" for expansion and contraction. The “basic operating direction” is the operating direction of the boom for realizing the movement (rightward in the case of FIG. 8) of the tip of the boom specified by the operation content of the operating lever.

  In the case of position feedback, as shown, a target position is determined for each control cycle, and the position of the tip of the boom is matched with the target position for each control cycle. When the current position does not reach the target position, a positive output is provided, and when the current position exceeds the target position, a negative output is provided to match the target position with the current position.

  (2) to (4) The correction of each current position → target position is as follows. In (2), the turning correction is "left" and the expansion and contraction correction is "shrinkage". In (3), the turning correction is "left" and the expansion / contraction correction is "stretching". In (4), the turning correction is "right" and the expansion / contraction correction is "shrinkage".

  In the case of position feedback, as described above, the direction of correction of each operation may be reversed at each timing. In addition, as described above, the direction of correction may be reversed from the basic operation direction with respect to "swing: right, expansion / contraction: extension" as described above.

  When the control cycle is shortened to improve the performance of position control, or when the correction gain is increased, or when the moving speed by the basic operation is slow, there is a high possibility that the correction direction with respect to the basic operation direction is reversed. Also, the possibility that the correction direction is reversed for each control cycle is also increased. In addition, the target speed is minute for correction. Here, FIG. 9 shows a spool displacement for satisfying the target speed when a minute target speed is generated.

  As shown in FIG. 9, when the target speed changes finely near 0, the target spool position changes drastically so as to exceed the positive side threshold and the negative side threshold along with the sign of the target speed. The positive threshold and the negative threshold are respectively the upper limit and the lower limit of the dead zone shown in FIG. In the case of electrical control using an alternating current like an industrial manipulator, it is possible to change the applied voltage and the frequency of the inverter violently, but in the case of controlling the oil pressure through the spool, the response speed of the spool is exceeded. Therefore, response delay and overshoot occur in the spool displacement (see FIG. 10). Since the characteristic of FIG. 3 appears for the response delayed and overshooted spool, the actual actuator speed becomes oscillatory as shown in FIG.

  When the actuator is operated with contents such that the operation direction is strongly switched as shown in FIG. 11, a larger overrun occurs, for example, when the boom is broken, which causes the control system to oscillate.

  On the other hand, in the present embodiment, as described with reference to FIG. 7, control based on the speed of the tip of the boom is performed. In order to keep moving the tip of the boom to the right, find and control the speed of turning and stretching so that the velocity is generated rightward at the tip of the boom per unit time (one control cycle) . Since the speed of turning and stretching to meet the speed of the tip of the boom depends on the current position of the boom, it can be updated every control cycle.

  As long as the attitude of the boom does not change significantly, the target speeds of turning and extension with respect to the target speed of the tip of the boom do not change significantly, and the basic operation direction does not change. Therefore, for example, if the speed of turning and stretching exceeds the target speed, it is corrected by decelerating as in the case of accelerator control of the automobile, and it is not necessary to change the sign of the moving direction of turning and stretching. The spool position is also controlled in the range of speed 0 to the maximum speed, and it is only necessary to control in the range from the dead zone to the maximum speed, since it is not necessary to consider inversion. As long as the target speed does not change rapidly, the feedback value of the spool displacement amount does not deviate, and stable control can be performed.

"Second effect"
As a speed calculation means for controlling on a speed basis, a means (comparison means of a comparative example) for determining the target position for each control cycle and obtaining the speed from the difference between the current position and the target position can be considered. This is a means different from the means of this embodiment.

  However, in the case of the calculation means of the comparative example, the influence of the minute calculation error in the floating point type operation becomes a problem. For example, if it is desired to raise the tip of the boom vertically, it is not necessary to make a turn. However, in the case of the calculation means of the comparative example, due to an error such as rounding in the floating point type calculation, a minute target speed is generated in a form in which the sign changes frequently with respect to the turning unnecessary. In addition to the calculation error, there is a possibility that a minute target speed may be generated even when a slight amount of deviation occurs in the turning position due to the play of the boom or the like. The minute target speed accompanied by the change of sign adversely affects the control of the control valve. Although an error occurs in the floating-point type operation with respect to other operations, it does not greatly affect the basic operation speed because it is small.

  Moreover, when calculating | requiring the angular velocity of turning or roughening with the calculation means of a comparative example, it is necessary to calculate using a square root. This increases the computational cost.

  On the other hand, the control device 10 according to the present embodiment expands and contracts, undulates, and turns the speed vector indicating the direction and speed for moving the tip of the boom based on the inverse matrix of the Jacobian matrix shown in Equation (14). It is converted into a motion vector indicating each motion direction and speed.

  Each element of the inverse matrix of the Jacobian matrix shown in equation (14) is formed by four arithmetic operations of trigonometric functions and does not include a square root. For this reason, the calculation cost for obtaining each element does not increase. Also, as shown in equation (15), the calculation equation obtained using the inverse matrix of the Jacobian matrix does not include the square root. For this reason, the calculation cost based on Formula (15) does not increase either.

Further, when x ′ and y ′ are 0 from equation (15), q ₁ ′ indicating the speed vector of turning is 0. That is, when moving the tip of the crane in the vertical direction, the speed vector of turning can be made zero. For this reason, it is possible to avoid the disadvantage that a minute target speed occurs due to a calculation error as in the calculation means of the comparative example.

  Further, according to equation (15), since the motion vector is obtained by the product and the sum, the calculation cost is small. Also, the solution of the motion vector is uniquely obtained.

  According to the control device 10 of this embodiment, the tip of the boom can be moved linearly and smoothly to the destination.

Second Embodiment
The control device 10 of the present embodiment differs from the first embodiment in the configuration of the velocity vector generation unit 12. The other configuration is the same as that of the first embodiment. Further, the configuration of the work machine is also the same as that of the first embodiment.

  In the case of the first embodiment, as described with reference to FIG. 7, there is a possibility that the moving direction of the tip of the boom may deviate from the target direction. The velocity vector generation unit 12 of the present embodiment generates a commanded velocity vector so as to correct the deviation.

  First, the velocity vector generation unit 12 determines the target position of the tip of the boom for each control cycle based on the velocity vector output from the input reception unit 11. The target position is indicated in the work machine coordinate system.

  For example, the velocity vector generation unit 12 holds position information (shown by the work machine coordinate system) of the tip of the boom at the start of movement, and uses the velocity vector output from the input reception unit 11 starting from the position. The position after moving for the control period at the velocity indicated by the velocity vector in the indicated direction is determined as the target position of the first control period.

  Thereafter, the velocity vector generation unit 12 causes the velocity vector to be indicated by the velocity vector in the direction indicated by the velocity vector output from the input reception unit 11 corresponding to the target position of the n-1th control period to the nth control period. The position after moving for a control period at a fixed speed is determined as the target position of the nth control period (n is an integer of 2 or more).

  Then, the velocity vector generation unit 12 generates the first velocity vector determined based on the input content received by the input reception unit 11 corresponding to the nth control period, and the boom after the (n-1) th control period. A composite vector obtained by adding together the second velocity vector from the position of the tip to the target position of the tip of the boom in the (n-1) th control cycle is generated as a designated velocity vector in the nth control cycle. The first velocity vector is a designated velocity vector generated based on any of the above-described designated velocity vector generation examples 1 to 3 based on the input content accepted by the input accepting unit 11 corresponding to the nth control period. is there.

  A specific example will be described using FIG. The position of the tip of the boom at the start of movement is (1), the state after the first control cycle is (2), the state after the second control cycle is (3), the state after the third control cycle (4)

  From (2), it can be seen that the current position after the first control cycle is different from the target position of the first control cycle. The velocity vector generation unit 12 sets the first velocity vector determined corresponding to the second control period as the designated velocity vector of the second control period, and the position of the tip of the boom after the first control period. A composite vector is determined by adding together a second velocity vector directed to the target position of the tip of the boom in one control cycle.

  Similarly, it can be seen from (3) that the current position after the second control cycle is different from the target position of the second control cycle. The velocity vector generation unit 12 sets the first velocity vector determined corresponding to the third control period as the designated velocity vector of the third control period, and the position of the tip of the boom after the second control period. A composite vector is determined by adding together a second velocity vector toward the target position of the tip of the boom in two control cycles.

  From the figure, the composite vector is a vector in the direction of returning the tip position of the boom onto the target trajectory. Note that the velocity vector generation unit 12 may set a composite vector obtained by adding the second velocity vector multiplied by a predetermined coefficient and the first velocity vector as an instruction velocity vector. By adjusting the coefficient, it is possible to adjust the magnitude of the correction to return to the target trajectory. The coefficients may be given in advance.

  Next, a modification of this embodiment will be described. The actuator control unit 14 performs, among the expansion, contraction, and rotation of the boom indicated by the motion vector obtained by converting the composite vector, the expansion, contraction, and undulation of the boom indicated by the first movement vector obtained by converting the first velocity vector. An operation in which the direction of movement coincides with that of each swing may be performed, and an operation in which the direction of movement is different may not be performed.

  The first motion vector is a calculation obtained by multiplying the first velocity vector by the inverse matrix of the Jacobian matrix from the left (see Equations (2) and (14)) or using the inverse matrix of the Jacobian matrix Using the equation (see equation (15)), it is a motion vector obtained by converting the first velocity vector.

  Next, the operation and effect of the present embodiment will be described. The control device 10 of the present embodiment can achieve the same effects as those of the first embodiment.

  Further, in the case of the control of the first embodiment shown in FIG. 7, there is a possibility that position errors may be accumulated. According to the control device 10 of the present embodiment in which the above-described combined vector is used as the designated velocity vector, the position error can be reduced.

  When it is desired to move the target trajectory strictly, it is necessary to immediately eliminate the position error occurring at the present time (eg, in the next control cycle). However, when the control is performed, the operating direction and speed of each operation of the boom may change significantly. As a result, the smooth movement of the boom can not be realized.

  In the case of the present embodiment, the position error is controlled not to be eliminated immediately (e.g., in the next control cycle), but to be gradually eliminated in a plurality of control cycles. Therefore, it is possible to realize smooth movement of the boom while eliminating the position error.

  Further, in the case of the present embodiment in which the position correction is performed with the second velocity vector, the operation direction of each actuator determined based on the input content of the operator and the operation direction of each actuator indicated by the operation vector may be reversed. There is. However, by adopting the above modification, it is possible to avoid the inconvenience of operating in a direction different from the operation direction of each actuator determined based on the input content of the operator. As a result, smooth movement of the boom is realized.

Third Embodiment
The control device 10 of the present embodiment differs from the first and second embodiments in the configuration of the input reception unit 11 and the velocity vector generation unit 12. The other configuration is the same as in the first and second embodiments. In addition, the configuration of the work machine is also the same as in the first and second embodiments.

  The input receiving unit 11 receives an input for specifying the final target position of the tip of the boom. The final target position is indicated in the work machine coordinate system. For example, the technology described in Patent Document 3 may be used. That is, a plurality of ultrasonic wave receiving units are installed in the work machine, and an ultrasonic wave transmitter is installed at the final target position. The input reception unit 11 receives an input of a time difference between the ultrasonic wave transmission time and the ultrasonic wave reception time. Then, the input receiving unit 11 calculates and outputs the final target position of the tip of the boom based on the input information.

  The velocity vector generation unit 12 generates, for each control cycle, a designated velocity vector from the current position of the tip of the boom to the final target position.

  That is, the velocity vector generation unit 12 acquires the current position of the tip of the boom after each control cycle. The current position is indicated in the work machine coordinate system. Then, the velocity vector generation unit 12 determines a velocity vector heading from the current position of the boom after the (n-1) th control period to the final target position as the designated velocity vector of the nth control period. FIG. 13 shows a conceptual diagram of control of this embodiment. A commanded velocity vector is generated from the current position of the tip of the boom toward the final target position.

  Here, an example of determination of the velocity of the designated velocity vector will be described.

"Example 1 of determining the speed of commanded speed vector"
A fixed value may be given in advance. Then, the velocity vector generation unit 12 may set the fixed value as the velocity of the designated velocity vector.

"Example 2 of determining the speed of the indicated speed vector"
The input accepting unit 11 may accept an input for specifying the speed from the operator by any means. Then, the velocity vector generation unit 12 may determine the velocity of the designated velocity vector based on the content of the input.

"Example 3 of determining the speed of commanded speed vector"
If “(distance between final target position and current position of boom) ≦ (threshold)”, the velocity vector generation unit 12 sets “(the velocity of the indicated velocity vector) = (current velocity of the tip of the boom)”. It may be calculated by “(acceleration) × (time for control cycle)”. The acceleration may be a predetermined fixed value.

  If the tip of the boom is approaching the final target position, the condition is satisfied. In this case, by determining the velocity of the indicated velocity vector as described above, the velocity can be gradually decreased when the timing to reach the final target position approaches. As a result, smooth operation is realized.

  On the other hand, when “(distance between final target position and current position of boom)> (threshold)”, the velocity vector generation unit 12 determines whether the velocity vector generation unit 12 determines velocity 1 of the indicated velocity vector or At 2, the velocity of the commanded velocity vector may be determined. If the distance to the final target position is somewhat large, the condition is satisfied.

  The threshold may be, for example, "the distance required to decelerate from the current speed at a constant deceleration and stop." As an example, it may be calculated by “(threshold) = (current speed) × (time required to stop when decelerating from current speed at a constant deceleration) / 2”. Alternatively, it may be calculated by “(threshold) = (current speed) × (current speed) / (2 × deceleration)”. Note that “the time required to stop when the current speed decelerates at a constant deceleration” is “(time required to stop when the current speed decelerates at a constant deceleration) = (current speed) It may be calculated by / (deceleration) ".

"Example 4 of determining the velocity of the indicated velocity vector"
If “(the current velocity of the tip of the boom) <(the velocity determined in the velocity determination example 1 or 2 of the designated velocity vector)” is satisfied, the velocity vector generation unit 12 sets “(the velocity of the designated velocity vector) = ( The present velocity of the tip of the boom) + (acceleration) × (time for control cycle) ”may be calculated. The acceleration may be a predetermined fixed value.

  Immediately after the start of the operation of the boom, etc., the speed of the tip of the boom is small, so that the condition is easily satisfied. In this case, by determining the velocity of the designated velocity vector as described above, a rapid increase in velocity can be suppressed and a smooth operation can be realized.

  On the other hand, when “(the current speed of the tip of the boom) ((the speed determined in speed determination example 1 or 2 of the specified speed vector)” is satisfied, the speed vector generation unit 12 instructs as the speed of the specified speed vector. The velocity determined in velocity vector determination example 1 or 2 may be determined. A certain amount of time has passed since the start of the operation of the boom, and it becomes easy to satisfy the condition when the moving speed of the tip of the boom is stable.

  According to this embodiment, the same operation and effect as those of the first and second embodiments can be realized. Further, according to the present embodiment, it is possible to acquire the current position of the tip of the boom for each control cycle, and to generate an indicated speed vector heading therefrom to the final target position. For this reason, the tip of the boom can be smoothly moved without the movement direction being largely deviated from the direction toward the final target position.

であり、
Ｃ_１＝ｃｏｓ（ｑ_１）、Ｃ_２＝ｃｏｓ（ｑ_２）、Ｓ_１＝ｓｉｎ（ｑ_１）、Ｓ_２＝ｓｉｎ（ｑ２）、ｑ_１は前記ブームの旋回角、ｑ_２は前記ブームの起伏角、ｑ_３は前記ブームの長さ、Ｄ_ｂｏｏｍは前記ブームの軸とトップシーブのオフセット距離、Ｄ_{ｃｏｌｕｍｎ}はコラム回転軸と接続ピンの水平オフセット距離である制御装置。
３． １又は２に記載の制御装置において、
前記入力受付部は、制御周期毎に、前記速度ベクトルを指定する入力を受付け、
前記速度ベクトル生成部は、前記制御周期毎に、前記入力受付部が受付けた入力内容に基づき前記速度ベクトルを生成する制御装置。
４． １又は２に記載の制御装置において、
前記入力受付部は、前記ブームの先端の最終目標位置を指定する入力を受付け、
前記速度ベクトル生成部は、制御周期毎に、前記ブームの先端の現在位置から前記最終目標位置に向かう前記速度ベクトルを生成する制御装置。
５． １又は２に記載の制御装置において、
前記入力受付部は、前記速度ベクトルを指定する入力を受付け、
前記速度ベクトル生成部は、
移動開始位置と前記入力受付部が受付けた入力内容とに基づき、制御周期毎の前記ブームの先端の目標位置を算出し、
第ｎ（ｎは２以上の整数）の制御周期に対応して前記入力受付部が受付けた入力内容に基づき決定される第１の速度ベクトルと、第ｎ−１の制御周期後の前記ブームの先端の位置から第ｎ−1の制御周期の前記ブームの先端の目標位置に向かう第２の速度ベクトルとを足し合わせた合成ベクトルを、第ｎの制御周期における前記速度ベクトルとして生成する制御装置。
６． ５に記載の制御装置において、
前記アクチュエータ制御部は、
前記動作ベクトルで示される前記ブームの伸縮、起伏及び旋回の内、前記第１の速度ベクトルを変換した第１の動作ベクトルで示される前記ブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、前記動作方向が異なる動作は実行させない制御装置。
７． １から６のいずれかに記載の制御装置において、
複数の前記アクチュエータは油圧制御される制御装置。
８． コンピュータが、
入力受付工程と、
前記入力受付工程で受付けた入力内容に基づき、伸縮、起伏及び旋回可能なブームの先端を移動させる方向及び速度を３次元座標系で示す速度ベクトルを生成する速度ベクトル生成工程と、
前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを、前記ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する変換工程と、
前記ブームを伸縮させるアクチュエータ、前記ブームを起伏させるアクチュエータ及び前記ブームを旋回させるアクチュエータを、前記動作ベクトルで示される動作方向及び速度で動作させるアクチュエータ制御工程と、
を実行する制御方法。
９． コンピュータを、
入力受付手段、
前記入力受付手段が受付けた入力内容に基づき、伸縮、起伏及び旋回可能なブームの先端を移動させる方向及び速度を３次元座標系で示す速度ベクトルを生成する速度ベクトル生成手段、
前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを、前記ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する変換手段、
前記ブームを伸縮させるアクチュエータ、前記ブームを起伏させるアクチュエータ及び前記ブームを旋回させるアクチュエータを、前記動作ベクトルで示される動作方向及び速度で動作させるアクチュエータ制御手段、
として機能させるプログラム。 Hereinafter, an example of a reference form is added.
1. An input reception unit,
A velocity vector generation unit for generating a velocity vector indicating a direction and velocity for moving the tip of the expandable / retractable / retractable / swingable boom tip on the basis of the input content received by the input receiving unit;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. A converter for converting into a motion vector indicating the motion direction and speed of
An actuator for extending and retracting the boom, an actuator for raising and lowering the boom, and an actuator control unit for operating the actuator for pivoting the boom with the motion direction and speed indicated by the motion vector;
A control device having
2. In the control device described in 1,
The inverse of the Jacobian matrix is

And
C ₁ = cos (q ₁ ), C ₂ = cos (q ₂ ), S ₁ = sin (q ₁ ), S ₂ = sin (q ₂ ), q ₁ is the swing angle of the boom, q ₂ is the boom A controller comprising a hoisting angle, q ₃ is a length of the boom, D _boom is an offset distance between the axis of the boom and the top sheave, and D _column is a horizontal offset distance between a column rotation axis and a connection pin.
3. In the control device according to 1 or 2,
The input receiving unit receives an input specifying the velocity vector for each control cycle,
The control device, wherein the velocity vector generation unit generates the velocity vector based on the input content accepted by the input acceptance unit for each control cycle.
4. In the control device according to 1 or 2,
The input receiving unit receives an input specifying a final target position of the tip of the boom,
The control device, wherein the velocity vector generation unit generates the velocity vector from the current position of the tip of the boom to the final target position for each control cycle.
5. In the control device according to 1 or 2,
The input receiving unit receives an input for specifying the velocity vector,
The velocity vector generation unit
Calculating a target position of the tip of the boom for each control cycle based on the movement start position and the input content received by the input reception unit;
A first velocity vector determined based on the input content received by the input receiving unit corresponding to the nth (n is an integer of 2 or more) control cycle, and the boom after the (n-1) th control cycle The control device which generates the synthetic vector which added the 2nd speed vector which goes to the end position of the tip of the boom of the (n-1) th control cycle from the position of the tip as the speed vector in the nth control cycle.
6. In the control device described in 5,
The actuator control unit
Of the expansion, contraction, and rotation of the boom indicated by the movement vector, an operation in which the movement direction coincides with each of the expansion, contraction, and rotation, indicated by the first movement vector converted from the first velocity vector And a control unit that does not execute operations with different operation directions.
7. In the control device according to any one of 1 to 6,
A controller in which the plurality of actuators are hydraulically controlled.
8. The computer is
An input reception process,
A velocity vector generation step of generating a velocity vector indicating a direction and a velocity for moving the tip of the expandable / retractable / retractable / swingable boom tip on the basis of the input content received in the input reception step;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. Converting into a motion vector indicating the motion direction and speed of
An actuator control step of operating the actuator for expanding and contracting the boom, the actuator for raising and lowering the boom, and the actuator for pivoting the boom with the movement direction and speed indicated by the movement vector;
Control method to execute.
9. Computer,
Input acceptance means,
Velocity vector generating means for generating a velocity vector indicating the direction and velocity of moving the tip of the expandable / retractable / retractable and pivotable boom in a three-dimensional coordinate system based on the input contents received by the input receiving means;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix Conversion means for converting into a motion vector indicating the motion direction and speed of
Actuator control means for operating the actuator for expanding and contracting the boom, the actuator for raising and lowering the boom, and the actuator for rotating the boom with the movement direction and speed indicated by the movement vector;
A program to function as

1A processor 2A memory 3A input / output I / F
4A Peripheral Circuit 5A Bus 10 Controller 11 Input Reception Unit 12 Velocity Vector Generator 13 Converter 14 Actuator Controller

Claims (9)

An input reception unit,
A velocity vector generation unit for generating a velocity vector indicating a direction and velocity for moving the tip of the expandable / retractable / retractable / swingable boom tip on the basis of the input content received by the input receiving unit;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. A converter for converting into a motion vector indicating the motion direction and speed of
An actuator for extending and retracting the boom, an actuator for raising and lowering the boom, and an actuator control unit for operating the actuator for pivoting the boom with the motion direction and speed indicated by the motion vector;
A control device having In the control device according to claim 1,
The inverse of the Jacobian matrix is

And
C ₁ = cos (q ₁ ), C ₂ = cos (q ₂ ), S ₁ = sin (q ₁ ), S ₂ = sin (q ₂ ), q ₁ is the swing angle of the boom, q ₂ is the boom A controller comprising a hoisting angle, q ₃ is a length of the boom, D _boom is an offset distance between the axis of the boom and the top sheave, and D _column is a horizontal offset distance between a column rotation axis and a connection pin. In the control device according to claim 1 or 2,
The input receiving unit receives an input specifying the velocity vector for each control cycle,
The control device, wherein the velocity vector generation unit generates the velocity vector based on the input content accepted by the input acceptance unit for each control cycle. In the control device according to claim 1 or 2,
The input receiving unit receives an input specifying a final target position of the tip of the boom,
The control device, wherein the velocity vector generation unit generates the velocity vector from the current position of the tip of the boom to the final target position for each control cycle. In the control device according to claim 1 or 2,
The input receiving unit receives an input for specifying the velocity vector,
The velocity vector generation unit
Calculating a target position of the tip of the boom for each control cycle based on the movement start position and the input content received by the input reception unit;
A first velocity vector determined based on the input content received by the input receiving unit corresponding to the nth (n is an integer of 2 or more) control cycle, and the boom after the (n-1) th control cycle The control device which generates the synthetic vector which added the 2nd speed vector which goes to the end position of the tip of the boom of the (n-1) th control cycle from the position of the tip as the speed vector in the nth control cycle. In the control device according to claim 5,
The actuator control unit
Of the expansion, contraction, and rotation of the boom indicated by the movement vector, an operation in which the movement direction coincides with each of the expansion, contraction, and rotation, indicated by the first movement vector converted from the first velocity vector And a control unit that does not execute operations with different operation directions. The control device according to any one of claims 1 to 6.
A controller in which the plurality of actuators are hydraulically controlled. The computer is
An input reception process,
A velocity vector generation step of generating a velocity vector indicating a direction and a velocity for moving the tip of the expandable / retractable / retractable / swingable boom tip on the basis of the input content received in the input reception step;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. Converting into a motion vector indicating the motion direction and speed of
An actuator control step of operating the actuator for expanding and contracting the boom, the actuator for raising and lowering the boom, and the actuator for pivoting the boom with the movement direction and speed indicated by the movement vector;
Control method to execute. Computer,
Input acceptance means,
Velocity vector generating means for generating a velocity vector indicating the direction and velocity of moving the tip of the expandable / retractable / retractable and pivotable boom in a three-dimensional coordinate system based on the input contents received by the input receiving means;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. Conversion means for converting into a motion vector indicating the motion direction and speed of
Actuator control means for operating the actuator for expanding and contracting the boom, the actuator for raising and lowering the boom, and the actuator for rotating the boom with the movement direction and speed indicated by the movement vector;
A program to function as

Images