KR101715940B1 - Controlling trace apparatus and method for construction equipments using teaching and playback - Google Patents

Abstract

The present invention relates to an apparatus and method for controlling a work locus of a construction machine using teaching and playback, and more particularly, to a work locus control apparatus for controlling a work start point and a trajectory follow-up so as to compensate for a position error when a playback is selected so as to follow a teaching locus, The position error can be minimized by compensating the gravity due to the attitude change of the device.

Description

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an apparatus for controlling a work trajectory of a construction machine using teaching and playback,

The present invention relates to an apparatus and method for controlling a work locus of a construction machine using teaching and playback, and more particularly, to a work locus control apparatus for controlling a work start point and a trajectory follow-up so as to compensate for a position error when a playback is selected so as to follow a teaching locus, The present invention relates to an apparatus and method for controlling a work locus of a construction machine using teaching and playback, which can minimize a position error by compensating gravity due to a change in attitude of the apparatus.

Generally, an excavator generally includes a work machine (e.g., a boom, an arm and a bucket, etc.), a boom cylinder for driving each work device, an arm cylinder and a bucket cylinder, a swing motor for swinging the excavator body, And a hydraulic pump for supplying hydraulic fluid as a power source. An excavator is a hydraulic construction machine that performs various tasks such as digging, claying, and stopping.

After the hydraulic pump sucked in from the hydraulic tank is discharged by the main pump, the excavator is controlled by the control valve which is switched according to the operation of the joystick to control the flow rate and hydraulic pressure supplied to the boom cylinder, arm cylinder and bucket cylinder, Arms and buckets to enable the operator to perform desired tasks.

In order to perform various operations with an excavator, a highly skilled operation is required because the operator must simultaneously operate the joystick of each work device with a skilled operation according to each work. Since the excavation environment is very bad and dangerous, it is necessary to study intelligent excavator which is controlled automatically by using a predetermined control algorithm rather than manual operation of the operator gradually. That is, there is a need for an automatic excavation method that can be easily performed by unskilled persons.

Intelligent excavators can reduce labor costs and reduce the risk of accidents by performing simple repetitive excavation work automatically, not by manpower. As a part of this, it is essential for the operator to input information about excavation environment in real time through various sensors for automated excavation work.

Particularly, when the same work is repeatedly performed, the worker repeatedly performs a highly skilled operation in the same manner, which requires a lot of concentration for a long period of time, which may reduce the efficiency of the work. Further, there is a problem that the same operation must be performed again when the skill of the operator is insufficient and the joystick is largely deviated from the predetermined trajectory by erroneous operation. As described above, a trajectory control technology for reliably setting the same job and automatically repeating the same is desperately needed.

An object of the present invention is to provide an apparatus and method for controlling a workpiece trajectory of a construction machine using teaching and playback capable of minimizing a position error when an automatic work is performed so as to follow a teaching trajectory designated by an operator And to provide the above objects.

To this end, the apparatus according to the first aspect of the present invention is a device for controlling a work locus of a construction machine including at least one work device and a drive section for driving the work device, wherein the joystick signal And the operator can select and operate the teaching mode and the playback mode; A data storage unit for storing sign data generated based on the joystick signal or the driving data of the driving unit; Wherein the control unit stores the locus data corresponding to the operation of the operator at the time of selecting the teaching mode in the data storage unit and tracks the locus data of the work device stored in the data storage unit when the playback mode is selected, And a drive control unit for controlling the drive unit so that the actual position of the work device at the time when the playback mode is selected is started in the data storage unit The control unit controls the automatic driving to be started at a position at the time of the selection of the playback mode when the difference between the position of the automatic driving and the predetermined reference error is within a preset reference error, The control unit controls the driving unit to control the driving unit.

Meanwhile, the method according to the second aspect of the present invention includes at least one working device, a driving part for driving the working device, and an operating part for generating a joystick signal corresponding to the operation of the operator, A trajectory teaching step of storing the joystick signal generated by the operation of the operator and the driving data of the working apparatus as trajectory data when the teaching mode is selected; And a trajectory playback step of following the stored trajectory data when the playback mode is selected and controlling the operation device to be driven, wherein the trajectory playback step comprises: Comparing the difference between the position of the apparatus and the start position at which the teaching mode is started, and if the difference of the position is smaller than a preset reference error, an automatic operation for following the trajectory data at a selection point of the playback mode selected by the operator The control unit controls the stored trajectory data to follow the lapse of time.

According to the present invention, a position error can be compensated when a playback is selected so as to follow a teaching trajectory, so that a worker can be driven with a desired trajectory.

In addition, even if the position of the work device at the playback selection time differs from the starting point of the teaching trajectory learned by the operator, the worker can advance the work of the desired work area in a short time if the difference is not large.

Further, there is an effect that the position error can be minimized by compensating for the change of the gravity due to the posture change of the working device.

1 is a block diagram of an apparatus for controlling a work locus of a construction machine using teaching and playback according to an embodiment of the present invention.
FIG. 2 is a detailed block diagram of an embodiment of a work locus control device in a playback mode according to the present invention.
FIG. 3 is a flow chart of an embodiment of a method for controlling a work locus in a teaching mode according to the present invention,
4 is a flowchart of a method of controlling a work locus in a playback mode according to an embodiment of the present invention.

Hereinafter, embodiments of the present invention will be described in detail with reference to the accompanying drawings. The configuration of the present invention and the operation and effect thereof will be clearly understood through the following detailed description. Before describing the present invention in detail, the same components are denoted by the same reference symbols as possible even if they are displayed on different drawings. In the case where it is judged that the gist of the present invention may be blurred to a known configuration, do.

The present invention can be applied to a system in which a worker stores a locus on a work device (e.g., an excavator and a wheel loader) and performs a predetermined operation on the work device (hereinafter, this work is referred to as teaching) The position of the end of the bucket) so as to minimize the position error when the playback device is driven to automatically operate the work device. Hereinafter, an example in which the present invention is applied to an excavator in a working apparatus will be described.

1 is a block diagram of an apparatus for controlling a work locus of a construction machine using teaching and playback according to an embodiment of the present invention.

1, the operation locus control apparatus 100 according to the present invention includes an operation unit 110, a drive control unit 120, a gravity compensation unit 130, a drive unit 140, and a data storage unit 150 . Here, the work locus control device 100 controls the work locus of the construction machine, and includes at least one work device and a drive part 140 for driving the work device. Hereinafter, an example in which the present invention is applied to an excavator in a working apparatus will be described.

The operation locus control device 100 operates in a teaching mode or a playback mode. Here, the operation unit 110 generates a joystick signal by an operator's operation, and the operator can select and operate the teaching mode and the playback mode.

First, the teaching mode means a mode in which an operator teaches a work process of an excavator and learns a work process. When the operator operates the joystick in the teaching mode, the operation locus control device 100 stores the joystick signal according to the joystick operation and the cylinder length data (hereinafter, referred to as 'driving data') of the driving unit 140. Here, the operator can start or end the teaching mode through the teaching start and end buttons provided on the operation unit 110 of the excavator.

Specifically, the operation unit 110 transmits the joystick signal generated by the operation of the operator's joystick to the drive control unit 120 while the teaching mode is in operation. At this time, the driving control unit 120 receives the joystick signal from the operation unit 110 and drives and controls the driving unit 140. When the teaching mode is selected, the driving control unit 120 stores the driving data of the working device corresponding to the operation of the operator and the locus data generated by processing the joystick signal in the data storage unit 150. [ That is, the drive control unit 120 stores the joystick signal transmitted from the operation unit 110 in the data storage unit 150 as joystick data. The drive control unit 120 stores cylinder length data of the boom, arm, and bucket driven by the drive unit 140 or angle data of each link in the boom, arm, and bucket in the data storage unit 150.

The driving unit 140 drives the boom, the arm, and the cylinder of the bucket according to the drive control of the drive control unit 120. [

Second, the playback mode is a mode in which the operation locus control device 100 automatically plays the stored work process in the teaching mode.

The data storage unit 150 stores trail data (e.g., joystick data and cylinder length data) learned in the teaching mode. Also, the data storage unit 150 may store the joystick data and angle data of each link of the boom, arm, and bucket as the locus data.

When the playback mode is selected, the drive control unit 120 controls the driving unit 140 to follow the track data stored in the teaching mode in the data storage unit 150 so that the work device is automatically driven. First, at the time when the playback mode is selected, the position of the work device is compared with the start position of the work device stored in the data storage unit 150. If the difference in position is within a predetermined reference error, the drive control unit 120 controls the automatic driving to start at a position at the time when the playback mode is selected. The measurement of this position error can be made using any of the working devices. In the present embodiment, a control method in which the position of the bucket, which is directly contacted with the work surface during the work such as excavation and is considered to be the center of the work, is set as a control reference will be described as an example. That is, it is possible to control the playback mode by comparing the position difference between the current position of the bucket end and the initial position of the predetermined bucket end. At this time, the driving control unit 120 controls the driving unit 140 to follow the pre-stored driving trajectory as the time for the automatic driving progresses. This is to enable the automatic operation to be carried out at the beginning of the working range as the time elapses even if it starts at a point that the driver did not want. Meanwhile, the drive control unit 120 compensates for the position error between the locus data stored in the data storage unit 150 during the automatic driving of the work device. In this embodiment, the driving force of the excavator is controlled by compensating the gravity corresponding to the attitude of the working device so as to minimize the position error of the bucket end of the excavator. Here, the drive control unit 120 updates the sign data by applying the gravity compensation value calculated by the gravity compensation unit 130, and controls the driving unit 140 based on the updated sign data. Of the locus data, each cylinder length data of the excavator can be replaced with angle data of each link in the boom, arm and bucket. The gravity compensation unit 130 calculates the gravity compensation value by obtaining the mass moment of inertia due to the current attitude change of the excavator. This is to minimize the position error of the bucket end due to the attitude change of the excavator. This gravity compensation result is used to compensate the discharge flow rate of the pump or the switching amount of the control valve, thereby enabling the operator to follow the driving speed of the work device originally intended. The operator can control the start / end of the playback mode through the playback start and end buttons provided on the operation unit 110. [

The above-described control method is described as an example in which, when the user desires to use the playback mode again after the playback mode is completed, the worker must move the work device to the initial position of the desired work again. However, the above-described playback mode is not necessarily limited thereto. That is, the present invention can be applied to a case in which the playback mode is automatically repeated. When the playback mode is automatically repeated, it is possible to automatically re-work the portion that is not operated at the start of the first playback mode as described above. This is because when the second repetition is performed, the working device automatically moves from the teaching mode to the initial position of the stored work, and then the playback mode is again performed. According to the present invention, even if the operation of the operation device is insufficient, even if the operation of the apparatus is started at an incorrect position, the worker can perform the operation of the intended work area when the repeated operation is performed.

On the other hand, if the position error at the start of the playback mode is larger than the preset reference error, it is notified that the playback mode can not be started because the automatic operation of the operation area intended by the driver is impossible, You can make them wait. On the contrary, in order to enable the working device to actively work, when the above-described position error is larger than the reference error, the working device is automatically set so that each of the working devices is automatically set so that the bucket is located at the starting point of the stored playback mode in the teaching mode A control method can also be used.

If a new joystick signal is generated by the operator from the operation unit 110 for a predetermined period of time in the above-described playback mode, the drive control unit 120 stops the playback mode and outputs a new joystick signal It is preferable to control the driving unit 140 so as to be able to prepare for an emergency.

2 is a detailed block diagram of an embodiment of a work locus control apparatus in a playback mode according to the present invention.

The drive control unit 120 controls the starting point and the trajectory estimation to compensate for the position error of the bucket when the playback button is selected and minimizes the position error so as to compensate for the gravity due to the attitude change of the working device.

That is, at the start of playback, the drive control unit 120 measures the distance between the current bucket end position and the bucket end position at which the teaching mode is started, and calculates the difference value between the current position of the bucket end and the initial position of the predetermined bucket end With a predetermined reference error (e.g., 10 cm, etc.). This is usually done by the operator placing the work device in the intended position and then starting the playback operation. When the playback operation is started, the posture / position of the work device where the playback starts and the pre- I have to take into consideration the fact that I have to do it. As described above, when the difference value of the initial position is smaller than the reference error, when the playback is started, the work device starts to be immediately located at the position set by the operation of the operator, and is controlled to approach the taught driving trajectory as time elapses. In this case, some of the work areas intended by the operator at the initial stage of playback may be unworked. In this case, if the playback is repeated, the work device is moved to the initial position It can be solved. On the other hand, if the posture / position of the work device from which playback starts is different from the pre-stored initial playback position, it is possible to indicate the inability of the playback operation and wait for additional operator's operation, There is a method of allowing the robot to follow the traced trajectory after the robot is automatically positioned and to continue the work. In this embodiment, an example in which a playback operation is performed after the work device is moved will be described as an example.

After starting the playback, the drive control unit 120 fetches the taught joystick data (Joy_ref data) and cylinder length data (Cyl_ref data) in advance. The driving control unit 120 sums the joystick signal O_Joy, the position error signal O_PI1 and the gravity compensation value Ga and outputs the driving control signal Com_out to the driving unit 140.

Specifically, the driving control unit 120 fetches the joystick signal O_Joy from the joystick data Joy_ref data. In addition, the drive control unit 120 obtains the error signal Er by subtracting the cylinder length signal and the currently measured signal from the cylinder length data Cyl_ref data. Then, the drive control unit 120 calculates the position error signal O_PI1 through the PI controller on the error signal Er.

The gravity compensation unit 130 calculates the gravity compensation value Ga by obtaining the mass moment of inertia in the current posture of the working device.

When the gravity compensation value Ga is determined, the drive control section 120 calculates the drive output value O_joy + O_PI1 + Ga obtained by adding the joystick signal O_Joy, the position error signal O_PI1 and the gravity compensation value Ga do. The driving control unit 120 converts the summed driving output value O_joy + O_PI1 + Ga into a driving control signal Com_out and outputs the driving control signal Com_out to the driving unit 140.

If the joystick signal is generated for more than a predetermined time (e.g., 0.3 sec) during execution of playback in the operation unit 110, the drive control unit 120 regards the situation as an emergency situation and responds to the joystick signal generated in the operation unit 110 And drives and controls the driving unit 140.

3 is a flowchart of a method of controlling a work locus in a teaching mode according to the present invention.

The operation trajectory control method according to the present invention includes at least one working apparatus, a driving unit 140 for driving the working apparatus, and an operation unit 150 for generating a joystick signal corresponding to an operation of an operator, Is applied to the operation locus control device (100) of a construction machine capable of selecting a playback mode.

The work locus control device 100 confirms whether a start button signal indicating the start of teaching by the operator is input (302).

The work locus control device 100 stores the joystick signal generated by the operator's operation and the drive data of the work device as locus data when the teaching mode is selected. That is, when the start button signal is inputted as the confirmation result (302), the operation locus control device 100 stores the angle at which the joystick moves by the operator after the start button signal in a predetermined time unit (e.g., 10 ms) , The length of each cylinder of the boom, the arm, and the bucket is sensed and stored (304). For example, the operation locus control device 100 may store the angle and the cylinder length of the joystick in units of 10 ms. Alternatively, the operation locus control device 100 can sense and store the angle of each link of the boom, the arm, and the bucket. Here, the operation locus control device 100 can calculate the position of the bucket end by kinematically calculating the cylinder length of the boom, the arm, and the bucket or the angle of each link. On the other hand, if the start button signal is not inputted (202), the operation locus control device 100 continuously monitors whether or not the start button signal is input.

Thereafter, the operation locus control device 100 confirms whether a completion button signal for notifying the end of teaching is input (306).

If the completion button signal is input as the confirmation result (306), the operation locus control device 100 stores the angle of the joystick stored so far and the cylinder length of the boom, arm, and bucket as one locus data. On the other hand, when the completion button signal is not inputted, the operation locus control device 100 performs the process again from the step " 304 " in which the locus data is stored.

4 is a flowchart of a method of controlling a work locus in a playback mode according to an embodiment of the present invention.

The operation locus control device 100 confirms whether a playback start button signal for notifying the start of playback is inputted by the operator (402).

When the playback start button signal is input by the operator, the work locus control device 100 measures the distance between the current bucket end position and the position of the bucket end where the teaching starts, (404) whether the difference value between the position and the initial position of the predetermined bucket end exceeds a predetermined reference error (for example, 10 cm or the like). On the other hand, if the playback start button signal is not input by the operator, the operation trajectory control device 100 performs the process from step 402 until the playback start button signal is input.

If the position difference between the current position of the bucket end and the initial position is equal to or greater than the preset reference error, the operation locus control device 100 controls the bucket so that the current position of the bucket end is set to a predetermined initial position (406). The work locus control device 100 controls each actuator to move the bucket so that the position difference value of the bucket end is within 10 cm. Here, when the automatic operation by the selection of the playback mode is repeatedly selected in a state where the difference in position is larger than the reference error, the operation locus control device 100 determines that the work device is in a position Controls the automatic operation to proceed after return.

If the position difference between the current position of the bucket end and the initial position is less than a preset reference error, that is, the position of the end of the bucket is within 10 cm, the operation locus control device 100 determines that the current position A playback signal is output every 10 ms so that the work unit is automatically driven and used as a reference playback signal. At this time, even if the initial position of the working device differs from the teaching position by using the following position compensation method, once the playback operation has been performed, the data is compensated so as to follow the trajectory as soon as possible, .

Specifically, the operation locus control device 100 fetches the previously stored joystick signal O_joy every 10 ms (408).

Then, the operation locus control device 100 calculates an error (Er = Cyl_ref-Cyl_cur) between the cylinder length data (Cyl_ref) stored for each cylinder and the currently measured data (Cyl_cur) (410).

Then, the work locus control device 100 checks whether the error between the cylinder length stored for each cylinder and the currently measured cylinder length is equal to or greater than a predetermined cylinder length error value (for example, 5 cm or the like) even for one cylinder among the three cylinders (412).

If the difference between the reference cylinder length and the currently measured cylinder length is equal to or greater than a preset cylinder length error value (for example, 5 cm, etc.), the operation locus control device 100 displays an operation incapability message to the worker And terminates the trajectory control (414).

If the error between the reference cylinder length and the currently measured cylinder length is less than a predetermined cylinder length error value (for example, 5 cm or the like), the operation locus control device 100 outputs the proportional integral (PI) Proportional Integral The position error signal (O_PI1 = Kp * Er + Ki * sum (Er)) is calculated 416 to perform feedback control through the controller.

Then, the work locus control device 100 calculates the compensation value Ga by calculating the mass moment of inertia in the current posture, and calculates the position error signal O_PI1 calculated in the step 416, The control signal O_PI = O_PI1 + Ga is obtained (418). For example, because an excavator is heavily weighted for booms, arms, and buckets, the boom, arm, and bucket are all in unfolded condition and the pressures required to move them together are different. Therefore, the work locus control device 100 controls the gravity of the boom, the arm, and the bucket more quickly and accurately by compensating for different states. That is, when the boom, the arm, and the bucket are deployed, the operation locus control device 100 adds the gravity compensation value Ga corresponding to the gravity compensation to the position error signal O_PI1 calculated in the step 416, So that they can go out.

Then, the operation locus control device 100 outputs the joystick output signal O_joy obtained in the process 408 as the final output, the PI error signal O_PI calculated in the process of the position error signal O_PI1 and 418 in the step 416, In addition, the work trajectory control apparatus 100 calculates the gravity compensation value using the gravity change due to the posture change of the working apparatus, And the calculated gravity compensation value is applied so that the working device can follow the driving speed of the working device included in the locus data stored in the teaching mode.

The operation locus control device 100 confirms whether the length of operation corresponds to the buffer length stored in the data storage unit 150 (422).

If the check result (422) indicates that the execution length does not match the buffer length, the operation locus control device 100 outputs the calculated drive output value in the process of '422' and performs the process again from the process of '408'. On the other hand, if the performance length matches the buffer length, the operation locus control device 100 ends the locus control together with the operation completion message.

As described above, the operation locus control device 100 follows the locus data stored when the playback mode is selected, and controls the operation device to be driven. At this time, the operation locus control device 100 compares the difference between the position of the working device at the selection time of the playback mode and the start position of the start of the teaching mode. If the position difference is smaller than the preset reference error, An automatic operation for following the trajectory data is performed at a selected point in the playback mode, and the stored trajectory data is controlled to follow the time as the time elapses.

If the user inputs a joystick signal over a predetermined time (for example, 0.3 sec or the like) during the trajectory control, the operation locus control device 100 regards the situation as an emergency and drives and controls the joystick signal.

The foregoing description is merely illustrative of the present invention, and various modifications may be made by those skilled in the art without departing from the spirit of the present invention. Accordingly, the embodiments disclosed in the specification of the present invention are not intended to limit the present invention. The scope of the present invention should be construed according to the following claims, and all the techniques within the scope of equivalents should be construed as being included in the scope of the present invention.

According to the present invention, it is possible to control an operation start point and a trajectory tracking so as to compensate for a position error at the time of playback selection so as to follow a teaching trajectory, and to minimize a position error by compensating gravity due to a posture change of the working apparatus.

100: operation locus control device 110:
120: drive control unit 130: gravity compensation unit
140: Driving unit 150: Data storage unit

Claims (8)

1. A work locus control device for a construction machine, comprising: at least one work device; and a drive part for driving the work device,
An operation unit which generates a joystick signal by an operation of an operator and allows the operator to perform a selection operation of a teaching mode and a playback mode;
A data storage unit for storing sign data generated based on the joystick signal or the driving data of the driving unit;
Wherein the control unit stores the locus data corresponding to the operation of the operator at the time of selecting the teaching mode in the data storage unit and tracks the locus data of the work device stored in the data storage unit when the playback mode is selected, A driving control unit for controlling the driving unit to be automatically driven; And
A gravity compensation unit for calculating a gravity compensation value corresponding to the posture of the working device;
/ RTI >
When the actual position of the work device at the time when the playback mode is selected is different from a position at which the operation of the work device stored in the data storage unit is started within a predetermined reference error, And controls the driving unit to follow the pre-stored driving trajectory as the time for the automatic driving progresses,
Wherein the drive control unit updates the sign data by applying the gravity compensation value and controls the drive unit based on the updated sign data. The method according to claim 1,
The drive control unit may include:
When the operator selects the playback mode, the position difference between the current position of the bucket end of the working device and the initial position of the predetermined bucket end is compared to move the end of the bucket to the preset initial position, And controls the driving unit to perform the automatic operation. delete The method according to claim 1,
The drive control unit may include:
And controls the driving unit according to the generated new joystick signal when a new joystick signal is generated for a predetermined time from the operation unit in the playback mode. And a control unit for generating a joystick signal corresponding to an operation of a worker, the control unit comprising: at least one working device; a driving unit for driving the working device; In the method,
A trajectory teaching step of storing the joystick signal generated by the operation of the operator at the time of selecting the teaching mode and the driving data of the working apparatus as trajectory data; And
And a trajectory playback step of following the stored trajectory data when the playback mode is selected and controlling the operation device to be driven,
The trajectory playback step may include calculating a gravity compensation value corresponding to the posture of the working device, updating the sign data by applying the calculated gravity compensation value, and determining a position of the working device at the selected time of the playback mode Comparing the difference between the starting position where the teaching mode is started and the starting position where the teaching mode is started and proceeding to follow the trajectory data at a selection point of the playback mode selected by the operator when the difference of the position is smaller than a preset reference error, And controlling the stored trajectory data to follow the lapse of time as the time elapses. delete 6. The method of claim 5, wherein the trajectory playback step comprises:
And controls the automatic operation to follow the locus data from the start position after moving the work device to the start position of the locus data when the difference between the positions is larger than the reference error. A method for controlling a work trajectory. 6. The method of claim 5,
When the automatic operation by the selection of the playback mode is repeatedly performed in a state where the difference of the position is larger than the reference error, the work device returns to the position where the teaching mode is started after the first automatic operation, And controlling the operation of the work machine so that the work is progressed.

Images