JP2018021416A - Overturn prevention device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To prevent overturn of a construction machine while reducing computational complexity.SOLUTION: A ZMP calculation part 110 calculates a ZMP of a construction machine 1 based on a joint angle θ, an angular velocity θ', an angular acceleration θ" and a tilt angle β. A retrieval part 111 retrieves a tilt direction in which a ratio of decreasing a distance Lp between a boundary B2 of a safety domain D2 and any ZMP in relative to a present angular acceleration becomes maximum, in a first function F1 indicating the distance Lp as an angular acceleration θ" in a case where a present ZMP is positioned outside of the safety domain D2. The angular acceleration is changed in the retrieved tilt direction, thereby retrieving an angular acceleration that makes the distance Lp equal to or less than zero.SELECTED DRAWING: Figure 3

Description

  The present invention relates to a fall prevention device for a construction machine.

  In recent years, the fall of a robot using a ZMP (Zero Moment Point) indicating an action point when it is assumed that a normal component of the floor reaction force distributed over the entire ground contact portion of the structure is applied to one point Techniques for prevention are known.

  Patent Document 1 discloses the following technique. When the operating lever of the construction machine is returned from the operating state to the stop command position, ZMP of the construction machine is calculated by approximating the cylinder speed with a cubic function, and the safety until the construction machine stops is predicted. And when it is predicted that there is a possibility of falling, the speed gradient of the lever input signal is decreased with the passage of time to try to prevent falling. If sufficient stability cannot be ensured, the speed command value is decreased stepwise until stability is sufficient.

  Patent document 2 discloses the following technique. In order for a biped robot with arms to accomplish a specific task, a pre-calculated target trajectory (joint angle time-series information) is given to each joint of the robot. The target trajectory is designed so that the ZMP of the robot does not deviate from the support polygon during work. When an external force is applied to the robot and the ZMP of the robot deviates from the originally assumed point, the target trajectory is corrected so that the difference between the ZMP of the original target trajectory and the ZMP shifted due to the disturbance becomes zero.

International Publication No. 2012/169531 Japanese Patent Laid-Open No. 10-230485

  However, although the fall of the construction machine is caused by any acceleration that occurs during the work, in Patent Document 1, the control for preventing the fall is executed only at the time of a sudden stop. Therefore, Patent Document 1 cannot prevent the construction machine from falling over in a scene other than during a sudden stop, and cannot sufficiently prevent the construction machine from overturning.

  Patent Document 2 discloses a method for updating the calculated target trajectory according to the environment, but this method is effective in controlling a walking robot. However, it is difficult for the construction machine to set the target trajectory in advance because the target motion is changed every moment by the lever operation of the passenger. Therefore, the method of Patent Document 2 is not suitable for construction machines.

  An object of the present invention is to provide an overturn prevention device that can prevent overturning of a construction machine with a small amount of calculation.

One aspect of the present invention is a fall prevention device for preventing a fall of the construction machine in a construction machine including a movable portion that rotates about a rotation axis,
An actuator for operating the movable part;
An acquisition unit that acquires an inclination angle of the construction machine with respect to gravity and an angle, an angular velocity, and an angular acceleration of the movable unit with respect to the rotation axis;
A ZMP calculating unit that calculates a ZMP that is a dynamic center of gravity position of the construction machine based on the acquired inclination angle, angle, angular velocity, and angular acceleration;
A position information storage unit for storing position information of a support polygon set in the construction machine and a safety area set inside the support polygon;
When the current ZMP is located outside the safe area, a rate at which the distance decreases with respect to the current angular acceleration in the first function that indicates the distance between the boundary of the safe area and an arbitrary ZMP as an angular acceleration A search unit for searching for an angular acceleration in which the current ZMP is returned to the inside of the safety region by searching for the inclination direction in which the current ZMP is maximized and changing the angular acceleration along the searched inclination direction;
And a drive unit that controls the actuator so that the control amount of the actuator becomes a control amount corresponding to the searched angular acceleration.

  According to this aspect, when the current ZMP is located outside the safety region set inside the support polygon, in the first function indicating the distance between the boundary between any ZMP and the safety region in terms of angular acceleration, An angular acceleration that returns the current ZMP to the inside of the safe area is searched. Thereby, the fall prevention control for returning the current ZMP to the inside of the safety region is realized. Here, in this aspect, the angular acceleration is searched for along the inclination direction of the first function that maximizes the rate of decrease in the distance with respect to the current angular acceleration. Therefore, it is possible to prevent the fall of the construction machine with a small amount of calculation.

  In particular, since the target motion of the movable part varies from moment to moment depending on the operation of the occupant, it is difficult to predict the target motion of the movable part compared to a biped robot. Since this embodiment can calculate the angular acceleration that returns the current ZMP to the inside of the safe area with a small amount of calculation, it is suitable for the fall prevention control of a construction machine where it is difficult to predict the target operation.

In the above aspect, the position information storage unit further stores position information of a non-control area where the fall prevention control of the construction machine set inside the safety area is not performed,
The search unit
When the current ZMP is located outside the non-control area and inside the safety area, as the current ZMP moves away from the non-control area, the amount of movement of the current ZMP toward the non-control area is increased. Set the target ZMP to be longer,
In the second function indicating the distance between the target ZMP and the arbitrary ZMP as the angular acceleration, a search is made for an inclination direction in which the rate of decrease of the distance with respect to the current angular acceleration is maximized, and the searched inclination You may search for the angular acceleration which makes the said distance zero by changing the said angular acceleration along a direction.

  In this aspect, even when the current ZMP is located outside the non-control area and inside the safety area, the fall prevention control is executed. Therefore, execution and non-execution of the overturn prevention control are repeated, and it is possible to prevent the actuator from being driven in vibration, and it is possible to prevent the control amount of the actuator from becoming excessive when the ZMP protrudes from the safe region. In addition, in this case, the target ZMP is set such that the amount of movement of the current ZMP toward the non-control region increases as the current ZMP moves away from the non-control region. Therefore, the fall prevention control can be executed so that the control amount increases as the risk of the fall increases. Furthermore, since the gentle fall prevention control is continued even after the ZMP returns to the safe area, it is possible to suppress the ZMP from protruding from the safe area.

  In the above aspect, the drive unit may increase the control amount according to the angular acceleration obtained by the search using the second function as the target ZMP moves away from the boundary of the non-control region.

  According to this aspect, when the ZMP is located outside the non-control region and inside the safe region, the control amount of the actuator can be reduced as the risk of falling falls. Therefore, it is possible to prevent the control amount of the actuator from becoming excessive when the ZMP protrudes from the safe area. Furthermore, since the gentle fall prevention control is continued even after the ZMP returns to the safe area, it is possible to suppress the ZMP from protruding from the safe area.

  In the above aspect, the search unit calculates the predicted position of the ZMP based on the current ZMP and the speed of the current ZMP, and when the calculated predicted position is located outside the safety region, The search using the first function may be executed.

  According to this aspect, even if the current ZMP does not actually protrude out of the safe area, the fall prevention control is performed to return the ZMP to the inside of the safe area in advance when the movement of the ZMP is large. Therefore, it is possible to prevent the control amount from becoming excessive and the actuator from being saturated when the ZMP protrudes from the safe area and the ZMP is returned to the safe area.

  In the above aspect, as a result of performing the search in an inclination direction in which the rate of decrease in the distance with respect to the current angular acceleration is maximized, the search unit may change the ZMP until the ZMP returns to the inside of the safety region. When the decrease in the distance cannot be expected, the search may be executed in the inclination direction in which the rate at which the distance decreases with respect to the current angular acceleration is the next largest.

  According to this aspect, even when the distance cannot be reduced to less than zero as a result of searching in the initially determined inclination direction, the search is performed again along the inclination direction in which the distance decreases. Is done. Therefore, it is possible to more reliably search for jerk that makes the distance less than zero.

  According to the present invention, it is possible to prevent the construction machine from falling over with a small amount of calculation.

BRIEF DESCRIPTION OF THE DRAWINGS It is an external view of the construction machine to which the fall prevention apparatus in Embodiment 1 of this invention was applied. It is explanatory drawing of ZMP. It is a block diagram which shows the structure of the fall prevention apparatus applied to the construction machine shown in FIG. It is a figure which shows a support polygon and a safe area | region when a construction machine is seen below from the upper direction. It is a figure which shows an example of how to take distance. It is a circuit diagram of a hydraulic circuit. It is a flowchart which shows the process of the fall prevention apparatus of embodiment. It is a flowchart which shows the detail of a search process. It is a graph which shows an example of the 1st function. It is a block diagram which shows the structure of the fall prevention apparatus which concerns on Embodiment 2 of this invention. It is a figure which shows a support polygon, a safety area | region, and a non-control area | region when a construction machine is seen from upper direction to the downward direction. It is a figure explaining the process of a search part. It is a flowchart which shows the detail of the search process which concerns on Embodiment 2 of this invention. It is a block diagram which shows the structure of the fall prevention apparatus which concerns on Embodiment 3 of this invention. It is a flowchart which shows the process of the fall prevention apparatus which concerns on Embodiment 3 of this invention. It is explanatory drawing of the predicted position of ZMP.

  Hereinafter, embodiments of the present invention will be described with reference to the accompanying drawings. The following embodiments are examples embodying the present invention, and are not of a nature that limits the technical scope of the present invention.

(Embodiment 1)
[Construction machinery]
FIG. 1 is an external view of a construction machine 1 to which a fall prevention device according to Embodiment 1 of the present invention is applied. Although the construction machine 1 is configured by a hydraulic excavator, this is an example, and any construction machine may be employed as long as the construction machine includes an upper swing body such as a crane. In FIG. 1, the + x direction indicates the front of the construction machine 1, and the −x direction indicates the rear of the construction machine 1. The front and rear are collectively referred to as the front-rear direction (x direction). The + z direction indicates the upper side of the construction machine 1, and the −z direction indicates the lower side of the construction machine 1. The upper part and the lower part are collectively referred to as the vertical direction (z direction). The + y direction indicates the left when the construction machine 1 is viewed from the rear to the front, and the -y direction indicates the right when the construction machine 1 is viewed from the rear to the front. The right side and the left side are collectively referred to as the left-right direction.

  The construction machine 1 includes a crawler-type lower traveling body 2, an upper revolving body 3 that is turnable on the lower traveling body 2, and a work device 4 that is attached to the upper revolving body 3.

  The work device 4 includes a boom 41 attached to the upper swing body 3 so as to be able to move up and down, an arm 42 attached so as to be swingable with respect to a distal end portion of the boom 41, and a swing with respect to the distal end portion of the arm 42. And a bucket 43 movably attached thereto.

  In addition, the working device 4 swings the boom 43 with respect to the upper swing body 3, the arm cylinder 52 with which the arm 42 swings with respect to the boom 41, and the bucket 43 with respect to the arm 42. And a bucket cylinder 53 to be operated.

[Outline of ZMP]
FIG. 2 is an explanatory diagram of ZMP. In the present embodiment, the fall prevention control of the construction machine 1 is performed using the total center-of-gravity position c of the construction machine 1 and the ZMP (Zero Moment Point). ZMP indicates the center position of the reaction force that the object receives from the ground, and indicates the dynamic center of gravity position. For example, if the working device 4 is accelerating to the left with reference to FIG. 1, the ZMP moves to the right of the total center of gravity position c.

  Please refer to FIG. In the overturning prevention control using the ZMP, the work device 4 is controlled so that the ZMP is located inside the support polygon D1 not including the boundary B1. The support polygon D1 is a polygon that surrounds a region where the object is in contact with the ground. In the case of the construction machine 1, since the pair of left and right crawlers 21 and 22 constituting the lower traveling body 2 are in contact with the ground, the substantially rectangular shape in which the crawlers 21 and 22 are convexly surrounded by the two regions. The region becomes the support polygon D1.

  The static overturning stability of the construction machine 1 is satisfied by the presence of the total center of gravity position c inside the machine support polygon D1. However, when acceleration is applied to the construction machine 1 by an external force or the like, even if the total center of gravity position c exists within the support polygon D1, if the support polygon D1 protrudes or is positioned at the boundary B1, The construction machine 1 falls. For example, if the ZMP is located at the point p1, the construction machine 1 does not fall, but if the ZMP is located at the point p2, the construction machine 1 falls. Therefore, it is necessary to make a fall determination by ZMP taking into consideration the dynamic characteristics of the construction machine 1.

<Block diagram>
FIG. 3 is a block diagram showing the configuration of the overturn prevention device 10 applied to the construction machine 1 shown in FIG. The overturn prevention device 10 is a device that prevents the construction machine 1 from overturning in the construction machine 1 including a movable part that rotates about a rotation axis. Hereinafter, the boom 41, the arm 42, and the bucket 43 which comprise the working apparatus 4 shown in FIG. However, this is merely an example, and any component part of the construction machine 1 that rotates about the rotation axis can be applied as the movable part. For example, a jib of a hydraulic crane may be the movable part. Further, an attachment other than the bucket 43 may be a movable part, and the upper swing body 3 may be a movable part.

  The fall prevention device 10 includes an angle sensor 101 (an example of an acquisition unit), a tilt sensor 102 (an example of an acquisition unit), a controller 103, an actuator 104, and a hydraulic circuit 105.

  The angle sensor 101 includes, for example, three angle sensors that detect the joint angle θ1 of the boom 41, the joint angle θ2 of the arm 42, and the joint angle θ3 of the bucket 43, respectively. The joint angle θ <b> 1 indicates the amount of undulation of the boom 41 with respect to the upper swing body 3. The joint angle θ <b> 2 indicates the swing amount of the arm 42 with respect to the boom 41. The joint angle θ <b> 3 indicates the amount of swing of the bucket 43 relative to the arm 42. Hereinafter, the joint angles θ1 to θ3 are collectively referred to as the joint angle θ. The angle sensor 101 may be composed of a rotary encoder or a potentiometer.

  The inclination sensor 102 is composed of, for example, a gravity sensor, and detects an inclination angle β indicating the amount of inclination of the construction machine 1 with respect to a horizontal plane.

  The controller 103 includes, for example, a computer including a CPU, a RAM, and a ROM, and includes a ZMP calculation unit 110, a search unit 111, a drive unit 112, and a position information storage unit 113.

  The ZMP calculation unit 110 calculates the ZMP of the construction machine 1 based on the joint angle θ, the angular velocity θ ′, the angular acceleration θ ″, and the inclination angle β. Here, the ZMP calculation unit 110 is detected by the angle sensor 101. The angular velocity θ ′ may be calculated by first-order differentiation of the joint angle θ, and the angular acceleration θ ″ may be calculated by second-order differentiation of the joint angle θ. Or if the angular velocity sensor which detects the angular velocity of the boom 41, the arm 42, and the bucket 43 is provided, the ZMP calculation part 110 should just acquire the angular velocity detected by the angular velocity sensor. This is the same when an angular acceleration sensor is provided.

<Calculation of ZMP>
Specifically, the ZMP calculating unit 110 may calculate ZMP using the following equations (1) and (2).

式（１）、（２）では、図２に示すように、建設機械１が接している地面上にｘ軸、ｙ軸が設定され、地面に対して垂直上方にｚ軸が設定されている。式（１）、（２）において、左辺のｐはＺＭＰを示す。ｐはｘ，ｙ成分のみで構成され、ｚ成分は０である。これは、ＺＭＰは地面上に設定された支持多角形Ｄ１上の点だからである。

In the formulas (1) and (2), as shown in FIG. 2, the x axis and the y axis are set on the ground with which the construction machine 1 is in contact, and the z axis is set vertically above the ground. . In the expressions (1) and (2), p on the left side represents ZMP. p is composed only of x and y components, and the z component is zero. This is because ZMP is a point on the support polygon D1 set on the ground.

  The total mass M and the gravitational acceleration g (gx, gy, gz) are constants. The total center-of-gravity position c (cx, cy, cz) is the center of gravity of the entire construction machine 1 and is a variable that depends on the joint angle θ of the construction machine 1. The total momentum P and the total angular momentum L are the vector sum of the momentum and angular momentum of each movable part. The total momentum P and the total angular momentum L are variables that depend on the joint angle θ, the angular velocity θ ′, and the angular acceleration θ ″. By changing the direction of the gravitational acceleration g in accordance with the inclination angle β, ZMP calculation on an inclined ground is also possible. From equations (1) and (2), it can be seen that the ZMP can be placed at the target location by adjusting the total momentum P and the total angular momentum L.

  Thus, the calculation of ZMP requires the total center-of-gravity position c, the total momentum P, and the total angular momentum L of the construction machine 1. These physical quantities are obtained kinematically from the joint angle θ, the angular velocity θ ′, the angular acceleration θ ″ of the construction machine 1 and the inclination angle β with respect to the gravitational acceleration g of the construction machine. Therefore, the equations (1) and (2 ) Is a function of the joint angle θ, the angular velocity θ ′, and the angular acceleration θ ″.

  The joint angle θ is a vector having components corresponding to the number of joints. Since the construction machine 1 in FIG. 1 has three joints, the joint angle θ is expressed by a vector composed of three components θ1, θ2, and θ3.

  Therefore, if the number of joints is n, the joint angle θ is represented by a vector composed of n components shown in Expression (3).

＜安全領域＞
前述の通り、ＺＭＰが建設機械１の支持多角形Ｄ１の内側に存在すれば、理論上は建設機械１は転倒しない。しかし、現実にはモデル化誤差や計測誤差によってＺＭＰの計算結果には誤差が生じる。そこで、本実施の形態は、建設機械１の支持多角形Ｄ１内にさらに安全領域Ｄ２を設定し、ＺＭＰが安全領域Ｄ２内にあるかどうかを転倒判定の基準とする。

<Safe area>
As described above, if the ZMP exists inside the support polygon D1 of the construction machine 1, the construction machine 1 does not fall down theoretically. However, in reality, an error occurs in the ZMP calculation result due to modeling error or measurement error. Therefore, in the present embodiment, a safety region D2 is further set in the support polygon D1 of the construction machine 1, and whether or not the ZMP is in the safety region D2 is used as a reference for the fall determination.

  The position information storage unit 113 stores the position information of the support polygon D1 set in the construction machine 1 and the position information of the safety area D2 set inside the support polygon D1. FIG. 4 is a diagram showing the support polygon D1 and the safety area D2 when the construction machine 1 is viewed from above.

  In the example of FIG. 4, the safety area D2 has a rectangular shape similar to the support polygon D1. In the example of FIG. 4, the center of the safety area D2 is set at the same position as the center of the support polygon D1. An area between the boundary B2 of the safety area D2 and the boundary B1 of the support polygon D1 is an adjustment area D3.

  When the ZMP exists in the safety area D2, the fall prevention device 10 does not perform the fall prevention control. When the ZMP exists in the adjustment area D3, the fall prevention device 10 performs the fall prevention control so that the ZMP returns to the safety area D2.

  The support polygon D1 can adopt a size and a shape determined in advance from the shapes of the crawlers 21 and 22 of the construction machine 1. As the data structure of the position information of the support polygon D1, for example, in the coordinate system of the construction machine 1 composed of three axes x, y, and z, the four vertices of the support polygon D1 when the position is the origin are used. Coordinates can be adopted.

  A value determined in advance according to the specifications of the construction machine 1 can be adopted as the size of the safety region D2. For example, the size of the safety area D2 can be returned to the safety area D2 without saturating the actuator 104 even when the ZMP exists near the boundary B1 of the support polygon D1. Value can be adopted.

  As the data structure of the position information of the safety area D2, for example, the coordinates of the four vertices of the safety area D2 in the coordinate system of the construction machine 1 can be adopted.

  Returning to FIG. When the current ZMP is located outside the safe area D2, the search unit 111 sets the first function F1 indicating the distance Lp between the boundary B2 of the safe area D2 and an arbitrary ZMP by the angular acceleration θ ″. In the first function F1, the search unit 111 searches for a tilt direction in which the rate of decrease of the distance Lp with respect to the current angular acceleration θ ″ is maximized, and changes the angular acceleration along the searched tilt direction. The angular acceleration at which the distance Lp is less than or equal to zero is searched. As the search algorithm, for example, the steepest descent method can be adopted.

<How to take distance Lp>
FIG. 5 is a diagram illustrating an example of how to obtain the distance Lp. In the example of FIG. 5, the point p indicates an arbitrary ZMP. In the example of FIG. 5, the length between an arbitrary ZMP (point p) and the boundary B2 on the straight line K1 connecting the center O of the safety region D2 and the point p is adopted as the distance Lp.

  Arbitrary ZMP (point p) is represented by the above formulas (1) and (2). Since the expressions (1) and (2) are functions of the joint angle θ, the angular velocity θ ′, and the angular acceleration θ ″ as described above, the distance Lp is determined by the joint angle θ, the angular velocity θ ′, and the angular acceleration θ ″. It becomes a function.

  The search unit 111 determines the distance Lp so that Lp> 0 when the ZMP exists outside the boundary B2. Therefore, if the distance Lp is less than zero (Lp <0), the search unit 111 determines that the construction machine 1 is stable because the ZMP exists inside the safety region D2. On the other hand, if the distance Lp is greater than or equal to zero (Lp ≧ 0), the search unit 111 determines that the construction machine 1 is not stable because ZMP exists outside the safety region D2.

  Note that the method of obtaining the distance Lp is not limited to the method illustrated in FIG. For example, the shortest distance from the boundary B2 of the safety region D2 to an arbitrary ZMP (point p) may be adopted as the distance Lp.

  When the ZMP of the construction machine 1 protrudes from the safety region D2 and enters the adjustment region D3 (Lp> 0), or when the ZMP is located on the boundary B2 of the safety region D2 (Lp = 0), Consider that the control amount of the actuator 104 is adjusted to keep the ZMP within the safety region D2.

  In the equations (1) and (2), the total momentum P and the total angular momentum L depend on the joint angle θ, the angular velocity θ ′, and the angular acceleration θ ″. Therefore, at a certain time, the joint angle θ and the angular velocity θ are detected by the sensor. If 'is known, the ZMP can be placed at a desired location by adjusting the angular acceleration θ ". In this case, the first function F1 indicating the distance Lp at a certain time is considered as a function of the angular acceleration θ ″. Therefore, the search unit 111 handles the distance Lp as the first function F1 (θ ″).

  Specifically, if the overturn prevention device 10 includes an angular velocity sensor, the search unit 111 substitutes the detection values of the angle sensor 101 and the angular velocity sensor into the joint angle θ and the angular velocity θ ′, thereby obtaining the first function. F1 may be set as a function of the angular acceleration θ ″. If the fall prevention device 10 does not include the angular velocity sensor, the search unit 111 substitutes the detected value of the angle sensor 101 for the joint angle θ. In addition, the first function F1 may be set as a function of the angular acceleration θ ″ by substituting the angular velocity θ ′ obtained from the angular acceleration θ ″ at the previous sampling point into the angular velocity θ ′.

  The reason why the first function F1 is the function of the angular acceleration θ ″ is that the responsiveness of the angular acceleration θ ″ is high among the joint angle θ, the angular velocity θ ′, and the angular acceleration θ ″.

  Returning to FIG. The drive unit 112 controls the actuator 104 through the hydraulic circuit 105 so that the control amount of the actuator 104 becomes the angular acceleration θ ″ searched by the search unit 111. The drive unit 112 has a plurality of movable units. In this case, each of the plurality of movable parts is individually controlled, and in the example of Fig. 1, the drive part 112 individually controls the boom cylinder 51, the arm cylinder 52, and the bucket cylinder 53.

  If the actuator 104 has a plurality of movable parts, a plurality of actuators 104 exist corresponding to the plurality of movable parts. In the example of FIG. 1, the actuator 104 includes three actuators 104 corresponding to the boom cylinder 51, the arm cylinder 52, and the bucket cylinder 53.

  The hydraulic circuit 105 operates the actuator 104 under the control of the drive unit 112.

<Hydraulic circuit>
FIG. 6 is a circuit diagram of the hydraulic circuit 105. The hydraulic circuit 105 includes a direction switching valve 602, inverse proportional valves 603 and 604, an operation unit 605, a hydraulic pump 606, and a tank 607. Here, the hydraulic circuit 105 that operates the boom cylinder 51 will be described as an example.

  The direction switching valve 602 includes a pilot switching valve having pilot ports 602a and 602b. When the pilot pressure is supplied to the pilot port 602a, the direction switching valve 602 supplies the hydraulic oil to the head H side of the cylinder 601 at a supply amount corresponding to the pilot pressure, and the hydraulic oil on the rod R side of the cylinder 601 is piloted. Discharge to the tank 607 with a discharge amount corresponding to the pressure. On the other hand, when the pilot pressure is supplied to the pilot port 602b, the direction switching valve 602 supplies hydraulic oil to the rod R side of the cylinder 601 with a supply amount corresponding to the pilot pressure, and the hydraulic oil on the head H side of the cylinder 601. Is discharged to the tank 607 at a discharge amount corresponding to the pilot pressure.

  The operation unit 605 includes a remote control valve including an operation lever 605a, and outputs a pilot pressure corresponding to the operation amount of the operation lever 605a. Here, when the operation lever 605a is operated in a direction in which the boom 41 is rotated in the first direction (stand-up direction), the operation unit 605 inputs a pilot pressure corresponding to the operation amount to the pilot port 602a through the pilot line 608a. To do. On the other hand, when the operation lever 605a is operated in a direction in which the operation lever 605a rotates the boom in the second direction (the lying down direction) opposite to the first direction, the pilot pressure corresponding to the operation amount is piloted through the pilot line 608b. Output to port 602b.

  The cylinder 601 is constituted by, for example, the boom cylinder 51 shown in FIG.

  The inverse proportional valve 603 is provided in a pilot pipe line 608a that connects the operation unit 605 and the pilot port 602a. The inverse proportional valve 604 is provided in a pilot pipe line 608b that connects the operation unit 605 and the pilot port 602b.

  The inverse proportional valves 603 and 604 reduce the pilot pressure according to the control signal from the controller 103.

  The drive unit 112 of the controller 103 feedback-controls the cylinder 601 so that the deviation between the current angular acceleration θ ″ and the angular acceleration θ ″ searched by the search unit 111 becomes zero. Here, the drive unit 112 performs feedback control of the cylinder 601 by adjusting the pressure reduction amount of the inverse proportional valves 603 and 604. The drive unit 112 may control the inverse proportional valve 603 if the operation lever 605a is operated in the first direction, and may control the inverse proportional valve 604 if the operation lever 605a is operated in the second direction. .

<Flowchart>
FIG. 7 is a flowchart illustrating processing of the overturn prevention device 10 according to the first embodiment. This flowchart is executed, for example, at a predetermined sampling period while the construction machine 1 is keyed on.

  First, the ZMP calculation unit 110 acquires the joint angle θ, the angular velocity θ ′, and the angular acceleration θ ″ (S701). Here, the ZMP calculation unit 110 acquires the joint angle θ from the angle sensor 101, and the joint angle. The angular velocity θ ′ may be obtained by first-order differentiation of θ and the angular acceleration θ ″ may be obtained by second-order differentiation of the joint angle θ.

  Next, the ZMP calculation unit 110 acquires the inclination angle β from the inclination sensor 102 (S702). Next, the ZMP calculation unit 110 corrects the direction of the gravitational acceleration g in the equations (1) and (2) with the inclination angle β, and the joint angle θ and the angular velocity θ ′ in the corrected equations (1) and (2). And the angular acceleration θ ″ are input to calculate the current ZMP (S703).

  Next, if the current ZMP exists inside the safety region D2 (YES in S704), the search unit 111 determines that the construction machine 1 does not need the overturn prevention control, and ends the process. On the other hand, if the current ZMP exists outside the safe area D2 (NO in S704), the search unit 111 determines that the overturn prevention control is necessary, and executes search processing (S705). Details of the search process will be described later with reference to FIG.

  Next, the drive unit 112 controls the actuator 104 so that the control amount of the actuator 104 becomes the angular acceleration θ ″ obtained as a result of the search process (S706), and the process ends.

  FIG. 8 is a flowchart showing details of the search process. First, in the first function F1, the search unit 111 calculates a tilt direction α indicating a tilt direction in which the rate of decrease of the distance Lp with respect to the current angular acceleration θ ″ is maximized (S801).

  FIG. 9 is a graph illustrating an example of the first function F1, the vertical axis indicates the distance Lp, the depth axis indicates the angular acceleration θ1 ″ corresponding to the joint angle θ1, and the horizontal axis indicates the angle corresponding to the joint angle θ2. The acceleration θ2 ″ is shown. In FIG. 9, for convenience of explanation, the angular acceleration θ ″ is composed of two-component vectors of θ1 ″ and θ2 ″. However, this is an example, and the angular acceleration θ ″ is the number of joints. It consists of a vector of corresponding components. In FIG. 9, the first function F1 is indicated by a plurality of intersecting ridge lines for convenience of explanation.

  In FIG. 9, a point Pα indicates a point on the first function F1 where the current angular acceleration θ ″ is located. The searching unit 111 uses the first function F1 to determine the current angular acceleration θ ″ (θ1 ″, θ2). The inclination direction α may be calculated by setting a point Pα corresponding to “)” and searching for a direction in which the rate of decrease in the distance Lp is maximized while slightly changing the angular acceleration around the point Pα. In the example of FIG. 9, as a result of the search, the direction indicated by the arrow is calculated as the tilt direction α. Note that the tilt direction α is composed of two-component vectors α1 and α2, α1 corresponds to the joint angle θ1, and α2 corresponds to the joint angle θ2. Further, the magnitude of the inclination direction α is | α | = 1.

  In S802 illustrated in FIG. 8, the search unit 111 calculates the distance Lp when the angular acceleration is set to θ ″ + d · α. The variable d is a minute variable and is a positive real number “d · α”. Is a vector of a variable d whose size is directed in the tilt direction α.

Next, the search unit 111 determines whether or not the distance Lp is less than zero (Lp <0) (S803). If the distance Lp is less than zero (YES in S803), the search unit 111 confirms the variable d used in S802 as the deterministic variable d ^* because ZMP has entered the safe area D2 (S804).

Next, the search unit 111 determines the angular acceleration (θ ″ + d ^* · α) in the definite variable d ^* as a search result (S805).

On the other hand, if the distance Lp is not less than zero (Lp ≧ 0) (NO in S803), the search unit 111 determines whether or not the distance Lp is likely to decrease before and after the change of the variable d (S806). . Here, the search unit 111 obtains the change amount ΔLp of the distance Lp. If ΔLp <0, the search unit 111 determines that the distance Lp is likely to decrease (YES in S806), and increases the variable d by a certain amount (S807). ), The process returns to S802. On the other hand, if ΔLp ≧ 0, it is determined that the distance Lp is unlikely to decrease (NO in S806), and the process returns to S801. However, the amount of change ΔLp is expressed as ΔLp = Lp (θ ″ + d _t · α) −Lp (θ ″ + d _t−1 ·) where d _t−1 is the variable d before the change and d _t is the variable d after the change. α).

  The process of FIG. 8 will be described with reference to FIG. As long as the distance Lp decreases, the search unit 111 shifts the point Pα along the inclination direction α on the first function F1 while increasing the variable d (S802 → S803: NO → S806 → S807 → S802). If the distance Lp is less than zero (S803: YES), the search unit 111 determines that the angular acceleration θ ″ for returning the ZMP to the safe region D2 has been searched.

  On the other hand, if the distance Lp does not decrease even if the point Pα is shifted along the direction of the inclination direction α, the search unit 111 further changes ZMP even if the point Pα is shifted in the inclination direction α. It is determined that the probability that the angular velocity θ ″ to be returned to the safety region D2 can be searched is low (NO in S806). Then, the search unit 111 returns the process to S801 and calculates the next inclination direction α. α may be determined as the inclination direction α in the direction in which the decrease rate of the distance Lp is next to the previously calculated adjustment direction, and if ΔLp ≧ 0 even in the next inclination direction α, the search unit 111 Next, the direction in which the rate of decrease of the distance Lp is large may be calculated as the tilt direction α and searched along the tilt direction α.

  As described above, when the current ZMP is located outside the safety area D2 set inside the support polygon D1, the fall prevention device 10 determines the angle Lp between any ZMP and the boundary B2 of the safety area D2 as an angle. In the first function F1 indicated by the acceleration θ ″, an angular acceleration θ ″ that makes the distance Lp less than zero is searched. Thereby, the fall prevention control for returning the current ZMP to the inside of the safety area D2 is realized. Here, the fall prevention device 10 searches for the angular acceleration along the inclination direction α of the first function F1 that maximizes the rate at which the distance Lp decreases with respect to the current angular acceleration θ ″. Therefore, the amount of calculation is small. Can prevent the construction machine 1 from falling.

  In particular, the construction machine 1 is difficult to predict the target motion of the movable part as compared to a biped robot, because the target motion of the movable part varies from moment to moment depending on the operation of the passenger. Since the fall prevention device 10 can calculate the angular acceleration θ ″ that returns the current ZMP to the inside of the safety region D2 with a small amount of calculation, it is suitable for the fall prevention control in the construction machine 1 in which it is difficult to predict the target operation.

(Embodiment 2)
FIG. 10 is a block diagram showing a configuration of a fall prevention device 10A according to Embodiment 2 of the present invention. The fall prevention device 10A is characterized in that a non-control region D4 (FIG. 11) is further provided inside the safety region D2. In the second embodiment, the same components as those in the first embodiment are denoted by the same reference numerals, and the description thereof is omitted.

  In the fall prevention device 10A, a search unit 111A, a drive unit 112A, and a position information storage unit 113A are different from the search unit 111, the drive unit 112, and the position information storage unit 113 of the first embodiment.

  The position information storage unit 113A stores position information of the non-control area D4 in addition to the position information of the support polygon D1 and the position information of the safety area D2. FIG. 11 is a diagram illustrating the support polygon D1, the safety region D2, and the non-control region D4 when the construction machine 1 is viewed from above.

  In the second embodiment, the safety area D2 and the non-control area D4 are circular. The center of the safety area D2 and the non-control area D4 is set to the center O of the support polygon D1. The non-control area D4 is an area where the controller 103 does not perform the overturn prevention control, and is set inside the safety area D2.

  In the present embodiment, since the safety area D2 and the non-control area D4 are circular, the radius of the safety area D2 and the non-control area D4 can be adopted as the position information of the safety area D2 and the non-control area D4. As the size of the non-control region D4, a value determined in advance according to the specification of the construction machine 1 can be adopted.

  Referring to FIG. 10, when the current ZMP is located outside the non-control region D4 and inside the safety region D2, the search unit 111A performs non-control of the current ZMP as the current ZMP moves away from the non-control region D4. The target ZMP is set so that the amount of movement toward the region D4 becomes longer. Then, the search unit 111A sets a second function F2 indicating the distance Lp between the target ZMP and an arbitrary ZMP as an angular acceleration θ ″. Then, the search unit 111A and the second function F2 are the same as in the first embodiment. The angular acceleration that makes the distance Lp zero is searched.

  FIG. 12 is a diagram for explaining the processing of the search unit 111A. Referring to FIG. 12, point p is the current ZMP. Point p (hat) is the target ZMP. In the specification, the symbol “∧” is described as (hat) due to the limitation of description.

  In this case, the search unit 111A obtains the distance L1 between the current ZMP (point p) and the boundary B3 of the non-control region D4 and the distance L2 between the current ZMP (point p) and the boundary B2 of the safety region D2. .

  Next, the search unit 111A determines a target ZMP (point p (hat)) obtained by internally dividing the distance L1 by the distance l1 and the distance l2 as the target ZMP. However, l1: l2 = L1: L2. That is, p (hat) = L1 · (l1 / (l1 + l2)). Accordingly, the target ZMP (point p (hat)) is set such that the amount of movement of the current ZMP (point p) toward the non-control region D4 increases as the current ZMP (point p) moves away from the non-control region D4. Is set.

  Next, the search unit 111A sets a second function F2 indicating the distance between the target ZMP (p (hat)) and an arbitrary ZMP as an angular acceleration θ ″. The first function F1 is between the arbitrary ZMP and the boundary B2. The distance Lp is a function indicating the angular acceleration θ ″. On the other hand, the second function F2 is a function indicating a distance Lp between an arbitrary ZMP and a target ZMP (point p (hat)) by an angular acceleration θ ″. In FIG. In the two function F2, the distance between the point p and the target ZMP (point p (hat)) is the distance Lp In the example of Fig. 12, the safety region D2 and the non-control region D4 are concentric circles, so the distance Lp is a point. The second function F2 is the same as the first function F1 except for the shortest distance between p and the target ZMP (point p (hat)), so the second function F2 is the same as the first function F1. , Which is a function of the angular acceleration θ ″.

  The drive unit 112A corrects the angular acceleration θ ″ so that the angular acceleration θ ″ obtained by the search using the second function F2 decreases as the target ZMP approaches the boundary B3 of the non-control region D4.

  Specifically, the drive unit 112A corrects the angular acceleration θ ″ using the following equation, where θb ″ is the corrected angular acceleration θ ″ and θa ″ is the corrected angular acceleration θ ″.

θb ″ = (l1 / l1 + l2) · θa ″
<Flowchart>
FIG. 13 is a flowchart showing details of the search processing according to Embodiment 2 of the present invention. In the second embodiment, the main routine adopts the flowchart of FIG. 7 as in the first embodiment.

  13 differs from FIG. 8 in that S1301 is provided instead of S803 and that S1302 is provided subsequent to S805. Otherwise, the process of FIG. 13 is the same as that of FIG. In S1301, the search unit 111A determines whether or not the distance Lp is Lp = 0. If Lp = 0 (YES in S1301), the process proceeds to S804. On the other hand, if distance Lp is not Lp = 0 (NO in S1301), search unit 111A advances the process to S806.

  Thus, in the second embodiment, Lp = 0, that is, the angular acceleration θ ″ for positioning the current ZMP to the target ZMP is searched.

  In S1302, the driving unit 112A calculates the angular acceleration θb ″ by multiplying the angular acceleration θa ″ determined in S805 by l1 / l1 + l2 to correct the angular acceleration θa ″ (S1302). 7, the driving unit 112 </ b> A controls the actuator 104 so that the control amount of the actuator 104 becomes the angular acceleration θb ″.

  Thus, according to the fall prevention device 10A according to the second embodiment, the fall prevention control is executed even when the current ZMP is located outside the non-control area D4 and inside the safety area D2. Therefore, the execution and non-execution of the overturn prevention control can be repeated to prevent the actuator from being driven vibrationally, and the control amount of the actuator 104 can be prevented from becoming excessive when the ZMP protrudes from the safety region D2. it can. In addition, in this case, as the current ZMP moves away from the non-control region D4, the target ZMP is set so that the amount of movement of the current ZMP toward the non-control region D4 becomes longer, and is necessary for achieving the target ZMP. Is corrected so as to increase as the current ZMP moves away from the non-control region D4.

  Therefore, the fall prevention control can be executed so that the control amount increases as the risk of the fall increases. Furthermore, since the gentle fall prevention control is continued even after the ZMP returns to the safety region D2, it is possible to suppress the ZMP from protruding from the safety region D2.

(Embodiment 3)
FIG. 14 is a block diagram showing a configuration of a toppling prevention device 10B according to Embodiment 3 of the present invention. The fall prevention device 10B is characterized in that the fall prevention device 10 of the first embodiment performs the fall prevention control by predicting the position of the ZMP. In the third embodiment, the same components as those of the first embodiment are denoted by the same reference numerals, and description thereof is omitted.

  14 is different from FIG. 3 showing the configuration of the first embodiment in that a search unit 111B is provided instead of the search unit 111. In FIG. The rest is the same as in the first embodiment.

  The search unit 111B calculates the predicted position of the ZMP based on the current ZMP and the current ZMP speed, and when the calculated predicted position is located outside the safe area D2, the search using the first function F1 Execute.

<Flowchart>
FIG. 15 is a flowchart showing processing of the overturn prevention device 10B according to Embodiment 3 of the present invention. 15 is different from FIG. 7 in that the processes of S1601 and S1602 are added following S703, and S1603 is provided instead of S704.

  In S1601, the search unit 111B calculates the speed of the ZMP. Here, for example, the search unit 111B may obtain the speed of the ZMP by dividing the difference between the ZMP calculated last time and the ZMP calculated this time by the sampling period.

  Next, the search unit 111B calculates the predicted position of the ZMP (S1602). FIG. 16 is an explanatory diagram of a predicted position of ZMP. In the example of FIG. 16, the safety region D2 is a circle whose center is located at the center of the support polygon D1, as in the second embodiment.

  In FIG. 16, the point p indicates the current ZMP, and p 'indicates the current ZMP speed. The speed p ′ is a vector composed of two components because the ZMP is composed of two components. Δt is a sampling period. A point p (hat) indicates a predicted position. Search unit 111B calculates a predicted position (point p (hat)) using the following equation. “P ′ · Δt” indicates the momentum of movement of the ZMP.

p (hat) = p + p ′ · Δt
If the predicted position (point p (hat)) is located inside the safety region D2 (YES in S1603), the search unit 111B determines that the fall prevention control is not necessary, and ends the process. On the other hand, if the current ZMP exists outside the safe area D2 (NO in S1603), the search unit 111B determines that the overturn prevention control is necessary, and executes search processing (S705).

  As described above, according to the fall prevention device 10B, when the current momentum of the ZMP is taken into consideration, the momentum of the movement of the ZMP is large even if the current ZMP does not actually protrude from the safety area D2. The fall prevention control for returning the ZMP to the inside of the safety area D2 is performed in advance. Therefore, it is possible to prevent the ZMP from protruding from the safety region D2 and when the ZMP is returned to the safety region D2, the control amount becomes excessive and the actuator 104 is saturated. Therefore, it can be avoided that the ZMP cannot be promptly returned to the safety region D2 due to the saturation of the actuator 104.

(Modification 1)
In the first to third embodiments, the driving unit 112 employs the angular acceleration θ ″ as the control amount of the actuator 104, but the present invention is not limited to this. For example, even if the torque is calculated as the control amount of the actuator 104, In this case, the drive unit 112 may calculate the torque by multiplying the searched angular acceleration θ ″ by the moment of inertia and control the actuator 104 so as to output the torque.

  When torque is employed as the control amount, the torque is corrected by the following equation in the second embodiment. However, Ta is a torque according to the angular acceleration θa ″ obtained by the search, and Tb is a corrected torque.

Tb = (l1 / l1 + l2) · Ta
(Modification 2)
In the third embodiment, the contents of the second embodiment are not considered, but the contents of the second embodiment may be taken into consideration. In this case, if the predicted position (point p (hat)) is located inside the safety region D2, the fall prevention device 10B may apply the method of the second embodiment as it is.

B1, B2, B3 boundary D1 support polygon D2 safety area D3 adjustment area D4 non-control area F1 first function F2 second function ΔLp variation α inclination direction β inclination angle θ joint angle θ ′ angular velocity θ ”angular acceleration 1 construction machine 2 Lower traveling body 3 Upper revolving body 4 Working device 10, 10A, 10B Fall prevention device 101 Angle sensor 102 Inclination sensor 103 Controller 104 Actuator 110 ZMP calculation unit 111, 111A, 113B Search unit 112, 112A Drive unit 113, 113A Position information Memory

Claims (5)

In a construction machine including a movable part that rotates about a rotation axis, the fall prevention device prevents the construction machine from overturning, and
An actuator for operating the movable part;
An acquisition unit that acquires an inclination angle of the construction machine with respect to gravity and an angle, an angular velocity, and an angular acceleration of the movable unit with respect to the rotation axis;
A ZMP calculating unit that calculates a ZMP that is a dynamic center of gravity position of the construction machine based on the acquired inclination angle, angle, angular velocity, and angular acceleration;
A position information storage unit for storing position information of a support polygon set in the construction machine and a safety area set inside the support polygon;
When the current ZMP is located outside the safe area, a rate at which the distance decreases with respect to the current angular acceleration in the first function that indicates the distance between the boundary of the safe area and an arbitrary ZMP as an angular acceleration A search unit for searching for an angular acceleration in which the current ZMP is returned to the inside of the safety region by searching for the inclination direction in which the current ZMP is maximized and changing the angular acceleration along the searched inclination direction;
A fall prevention device comprising: a drive unit that controls the actuator so that the control amount of the actuator becomes a control amount corresponding to the searched angular acceleration. The position information storage unit further stores position information of a non-control area where the fall prevention control of the construction machine set inside the safety area is not performed,
The search unit
When the current ZMP is located outside the non-control area and inside the safety area, as the current ZMP moves away from the non-control area, the amount of movement of the current ZMP toward the non-control area is increased. Set the target ZMP to be longer,
In the second function indicating the distance between the target ZMP and the arbitrary ZMP as the angular acceleration, a search is made for an inclination direction in which the rate of decrease of the distance with respect to the current angular acceleration is maximized, and the searched inclination The fall prevention device according to claim 1, wherein an angular acceleration that makes the distance zero is searched by changing the angular acceleration along a direction.   The fall prevention device according to claim 2, wherein the drive unit increases a control amount according to an angular acceleration obtained by the search using the second function as the target ZMP moves away from a boundary of the non-control region. .   The search unit calculates a predicted position of the ZMP based on the current ZMP and the speed of the current ZMP, and when the calculated predicted position is located outside the safety region, the first function The fall prevention device according to any one of claims 1 to 3, wherein the search using a computer is executed.   The search unit, as a result of performing the search in an inclination direction in which the rate at which the distance decreases with respect to the current angular acceleration is maximized, reduces the distance until the ZMP returns to the inside of the safety region. The fall prevention device according to any one of claims 1 to 4, wherein, when it becomes impossible to predict, the search is executed in a tilt direction in which the rate at which the distance decreases with respect to the current angular acceleration is the next largest.

Images