JP6674862B2

JP6674862B2 - Fall prevention device

Info

Publication number: JP6674862B2
Application number: JP2016154482A
Authority: JP
Inventors: 山田　崇; 崇山田; 稗方　孝之; 孝之稗方; 聖士濱口
Original assignee: Kobelco Construction Machinery Co Ltd; Kobe Steel Ltd
Current assignee: Kobelco Construction Machinery Co Ltd; Kobe Steel Ltd
Priority date: 2016-08-05
Filing date: 2016-08-05
Publication date: 2020-04-01
Anticipated expiration: 2036-08-05
Also published as: JP2018021416A

Description

本発明は、建設機械の転倒防止装置に関するものである。 The present invention relates to a fall prevention device for a construction machine.

近年、構造体の地表面との接地部全体に分布する床反力の法線成分がある一点にかかっているとみなしたときの作用点を示すＺＭＰ（ＺｅｒｏＭｏｍｅｎｔＰｏｉｎｔ）を用いてロボットの転倒防止を図る技術が知られている。 2. Description of the Related Art In recent years, a robot falls over using a ZMP (Zero Moment Point) indicating an action point when it is considered that a normal component of a floor reaction force distributed over the entire contact portion with the ground surface of the structure is applied to a certain point. Techniques for prevention are known.

特許文献１は、下記の技術を開示する。建設機械の操作レバーが操作状態から停止指令位置まで戻されたとき、シリンダ速度を３次関数で近似して建設機械のＺＭＰを計算し、建設機械が停止するまでの安全性を予測する。そして、転倒の恐れがあることが予測された場合は、レバー入力信号の速度勾配を時間の経過に伴って減少させることにより、転倒防止を試みる。それでも安定性が十分に確保できない場合は、安定性が十分になるまで速度指令値を段階的に減少させる。 Patent Document 1 discloses the following technology. When the operation lever of the construction machine is returned from the operation state to the stop command position, the ZMP of the construction machine is calculated by approximating the cylinder speed by a cubic function, and the safety until the construction machine stops is predicted. Then, when it is predicted that there is a possibility of falling, an attempt is made to prevent falling by reducing the speed gradient of the lever input signal with the passage of time. If sufficient stability is still not ensured, the speed command value is gradually reduced until the stability is sufficient.

特許文献２は、下記の技術を開示する。腕を備えた２足歩行ロボットが特定の作業を達成するために、ロボットの各関節に予め計算された目標軌道（関節角の時系列情報）を与える。目標軌道は、作業中にロボットのＺＭＰが支持多角形を逸脱することのないように設計されている。そして、ロボットに外力が加わり、ロボットのＺＭＰが本来想定されている点からずれると、本来の目標軌道のＺＭＰと、外乱によってずれたＺＭＰとの差を０とするように目標軌道を修正する。 Patent Document 2 discloses the following technology. In order for a bipedal walking robot having an arm to accomplish a specific task, a target trajectory (joint angle time-series information) is given to each joint of the robot in advance. The target trajectory is designed so that the ZMP of the robot does not deviate from the supporting polygon during the operation. Then, 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 deviated by the disturbance becomes zero.

国際公開第２０１２／１６９５３１号International Publication No. 2012/169531 特開平１０−２３０４８５号公報JP-A-10-230485

しかし、建設機械の転倒は作業中に生じるあらゆる加速度に起因するが、特許文献１は、急停止時しか転倒防止の制御が実行されていない。そのため、特許文献１は、急停止時以外のシーンで建設機械の転倒防止を図ることができず、建設機械の転倒防止を十分に図ることができない。 However, the fall of the construction machine is caused by any acceleration that occurs during the work, but in Patent Literature 1, the control for preventing the fall is performed only at the time of sudden stop. For this reason, Patent Document 1 cannot prevent the construction machine from overturning in a scene other than a sudden stop, and cannot sufficiently prevent the construction machine from overturning.

特許文献２では、計算した目標軌道を環境に応じて更新する手法が開示されているが、この手法は歩行ロボットの制御では有効である。しかし、建設機械は、目標動作が搭乗者のレバー操作によって時々刻々変更されるため、事前に目標軌道を設定することが困難である。よって、特許文献２の手法は、建設機械には適さない。 Patent Document 2 discloses a method of updating the calculated target trajectory according to the environment. However, this method is effective for controlling a walking robot. However, since the target operation of the construction machine is changed every moment by the rider's lever operation, it is difficult to set the target trajectory in advance. Therefore, the method of Patent Document 2 is not suitable for construction machines.

本発明は、少ない計算量で建設機械の転倒防止を実現できる転倒防止装置を提供することを目的とする。 SUMMARY OF THE INVENTION It is an object of the present invention to provide a fall prevention device capable of preventing a construction machine from falling with a small amount of calculation.

本発明の一態様は、回転軸を中心に回転する可動部を備える建設機械において、前記建設機械の転倒を防止する転倒防止装置であって、
前記可動部を作動させるアクチュエータと、
重力に対する前記建設機械の傾斜角と、前記回転軸に対する前記可動部の角度、角速度、及び角加速度とを取得する取得部と、
前記取得された、傾斜角、角度、角速度、及び角加速度に基づいて、前記建設機械の動的な重心位置であるＺＭＰを算出するＺＭＰ算出部と、
前記建設機械に設定された支持多角形と、前記支持多角形の内側に設定された安全領域との位置情報を記憶する位置情報記憶部と、
現在のＺＭＰが前記安全領域の外側に位置する場合、前記安全領域の境界及び任意のＺＭＰ間の距離を角加速度で示す第１関数において、前記現在の角加速度に対して前記距離が減少する割合が最大となる傾斜方向を探索し、前記探索した傾斜方向に沿って前記角加速度を変化させることにより、前記現在のＺＭＰを前記安全領域の内側に戻す角加速度を探索する探索部と、
前記アクチュエータの制御量が前記探索された角加速度に応じた制御量となるように前記アクチュエータを制御する駆動部とを備える。 One aspect of the present invention is a fall prevention device that prevents a fall of the construction machine in a construction machine including a movable unit that rotates about a rotation axis,
An actuator for operating the movable part,
An inclination angle of the construction machine with respect to gravity, and an acquisition unit that acquires an angle, an angular velocity, and an angular acceleration of the movable unit with respect to the rotation axis,
A ZMP calculation unit that calculates a ZMP that is a dynamic center of gravity of the construction machine based on the acquired inclination angle, angle, angular velocity, and angular acceleration;
A support polygon set in the construction machine, and a position information storage unit that stores position information of a safety area set inside the support polygon,
When the current ZMP is located outside the safety area, a rate at which the distance decreases with respect to the current angular acceleration in a first function indicating a distance between the boundary of the safety area and an arbitrary ZMP as an angular acceleration. A search unit that searches for an inclination direction in which the maximum is returned, and searches for an angular acceleration that returns the current ZMP to the inside of the safety area by changing the angular acceleration along the searched inclination direction.
A drive unit that controls the actuator so that the control amount of the actuator is a control amount according to the searched angular acceleration.

本態様によれば、支持多角形の内側に設定された安全領域の外側に現在のＺＭＰが位置する場合、任意のＺＭＰ及び安全領域の境界間の距離を角加速度で示す第１関数において、前記現在のＺＭＰを前記安全領域の内側に戻す角加速度が探索される。これにより、現在のＺＭＰを安全領域の内側に戻す転倒防止制御が実現される。ここで、本態様では、現在の角加速度に対して距離が減少する割合が最大となる第１関数の傾斜方向に沿って角加速度が探索される。そのため、少ない計算量で建設機械の転倒防止を実現できる。 According to this aspect, when the current ZMP is located outside the safety region set inside the supporting polygon, the first function indicating the distance between the boundary of any ZMP and the safety region by an angular acceleration, An angular acceleration that returns the current ZMP inside the safe area is searched. Thereby, the overturning prevention control for returning the current ZMP to the inside of the safety area is realized. Here, in the present aspect, the angular acceleration is searched for along the inclination direction of the first function in which the rate of decrease in the distance with respect to the current angular acceleration is maximized. Therefore, it is possible to prevent the construction machine from overturning with a small amount of calculation.

特に、建設機械は搭乗者の操作によって可動部の目標動作が時々刻々変動するので、２足歩行ロボットと比べて可動部の目標動作を予測することが困難である。本態様は、少ない計算量で現在のＺＭＰを安全領域の内側に戻す角加速度が算出できるので、目標動作の予測が困難な建設機械の転倒防止制御に適している。 In particular, since the target operation of the movable part of the construction machine fluctuates every moment due to the operation of the occupant, it is more difficult to predict the target operation of the movable part as compared with a bipedal walking robot. This mode can calculate the angular acceleration for returning the current ZMP to the inside of the safety area with a small amount of calculation, and is thus suitable for the overturn prevention control of the construction machine in which 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 in which 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 to the non-control area side increases. Set the target ZMP to be longer,
In a second function indicating the distance between the target ZMP and the arbitrary ZMP by the angular acceleration, a tilt direction in which the rate of decrease in the distance with respect to the current angular acceleration is maximized is searched, and the searched tilt is The angular acceleration that makes the distance zero may be searched for by changing the 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, the execution and non-execution of the overturn prevention control are repeated, so that the actuator can be prevented from being vibrated and the control amount of the actuator can be prevented from being excessive when the ZMP is out of the safe area. Moreover, in this case, the target ZMP is set such that the moving amount of the current ZMP toward the non-control area becomes longer as the current ZMP moves away from the non-control area. Therefore, it is possible to execute the fall prevention control so that the control amount increases as the risk of falling increases. Further, even after the ZMP returns to the safety region, the gentle fall prevention control is continued, so that the ZMP can be suppressed from protruding from the safety region.

上記態様において、前記駆動部は、前記第２関数を用いた前記探索によって得られた角加速度に応じた制御量を、前記目標ＺＭＰが前記非制御領域の境界から離れるにつれて増大させてもよい。 In the above aspect, the driving unit may increase a control amount according to the angular acceleration obtained by the search using the second function as the target ZMP moves away from a boundary of the non-control region.

本態様によれば、ＺＭＰが非制御領域の外側且つ安全領域の内側に位置する場合、転倒の危険性が低下するにつれてアクチュエータの制御量を低下させることができる。そのため、ＺＭＰが安全領域からはみ出したときに、アクチュエータの制御量が過大になることを防止できる。更に、ＺＭＰが安全領域に戻った後も、緩やかな転倒防止制御が継続されるため、ＺＭＰが安全領域からはみ出ることを抑制できる。 According to this aspect, when the ZMP is located outside the non-control area and inside the safety area, the control amount of the actuator can be reduced as the risk of falling is reduced. Therefore, it is possible to prevent the control amount of the actuator from being excessive when the ZMP is out of the safety region. Further, even after the ZMP returns to the safety region, the gentle fall prevention control is continued, so that the ZMP can be suppressed from protruding from the safety region.

上記態様において、前記探索部は、前記現在のＺＭＰと前記現在のＺＭＰの速度とに基づいて、前記ＺＭＰの予測位置を算出し、前記算出した予測位置が前記安全領域の外側に位置する場合、前記第１関数を用いた前記探索を実行してもよい。 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 performed.

本態様によれば、実際に現在のＺＭＰが安全領域外にはみ出していなくても、ＺＭＰの移動の勢いが大きい場合は事前にＺＭＰを安全領域の内側に戻す転倒防止制御が行われる。そのため、ＺＭＰが安全領域からはみ出て、いざ、ＺＭＰを安全領域に戻そうとしたときに、制御量が過大になり、アクチュエータが飽和することを防止できる。 According to this aspect, even if the current ZMP does not actually protrude outside the safety area, if the momentum of the ZMP movement is large, the overturn prevention control for returning the ZMP to the inside of the safety area is performed in advance. Therefore, when the ZMP protrudes from the safety region and the ZMP is to be returned to the safety region, the control amount becomes excessively large, and the saturation of the actuator can be prevented.

上記態様において、前記探索部は、前記現在の角加速度に対して前記距離が減少する割合が最大となる傾斜方向において前記探索を行った結果、前記ＺＭＰが前記安全領域の内側に戻るまでに前記距離の減少が見込めなくなった場合、前記現在の角加速度に対して前記距離が減少する割合が次に大きな傾斜方向において前記探索を実行してもよい。 In the above aspect, as a result of performing the search in the inclination direction in which the rate of decrease in the distance with respect to the current angular acceleration is maximized, the search unit determines that the ZMP returns until the ZMP returns to the inside of the safety area. When the decrease in the distance cannot be expected, the search may be performed in the inclination direction in which the ratio of the decrease in the distance 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 the search in the initially determined inclination direction, the rate of decrease in the distance is re-searched along the next largest inclination direction. Is performed. Therefore, the jerk that makes the distance less than zero can be more reliably searched.

本発明によれば、少ない計算量で建設機械の転倒防止を実現できる。 ADVANTAGE OF THE INVENTION According to this invention, the fall prevention of a construction machine can be implement | achieved with a small amount of calculation.

本発明の実施の形態１における転倒防止装置が適用された建設機械の外観図である。1 is an external view of a construction machine to which a fall prevention device according to Embodiment 1 of the present invention has been applied. ＺＭＰの説明図である。It is explanatory drawing of ZMP. 図１に示す建設機械に適用された転倒防止装置の構成を示すブロック図である。FIG. 2 is a block diagram illustrating a configuration of a fall prevention device applied to the construction machine illustrated in FIG. 1. 建設機械を上方から下方に見たときの支持多角形と安全領域とを示す図である。It is a figure which shows the support polygon and safety area | region when a construction machine is seen from the upper part downward. 距離の取り方の一例を示す図である。It is a figure showing 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. 第１関数の一例を示すグラフである。5 is a graph illustrating an example of a first function. 本発明の実施の形態２に係る転倒防止装置の構成を示すブロック図である。FIG. 9 is a block diagram illustrating a configuration of a fall prevention device according to Embodiment 2 of the present invention. 建設機械を上方から下方に見たときの支持多角形と安全領域と非制御領域とを示す図である。It is a figure which shows a support polygon and a safety area | region and a non-control area | region when a construction machine is seen from above from below. 探索部の処理を説明する図である。FIG. 6 is a diagram for explaining processing of a search unit. 本発明の実施の形態２に係る探索処理の詳細を示すフローチャートである。13 is a flowchart illustrating details of a search process according to Embodiment 2 of the present invention. 本発明の実施の形態３に係る転倒防止装置の構成を示すブロック図である。FIG. 9 is a block diagram illustrating a configuration of a fall prevention device according to Embodiment 3 of the present invention. 本発明の実施の形態３に係る転倒防止装置の処理を示すフローチャートである。9 is a flowchart illustrating a process of a fall prevention device according to Embodiment 3 of the present invention. ＺＭＰの予測位置の説明図である。It is explanatory drawing of the prediction position of ZMP.

以下、添付図面を参照しながら、本発明の実施の形態について説明する。なお、以下の実施の形態は、本発明を具体化した例であって、本発明の技術的範囲を限定する性格のものではない。 Hereinafter, embodiments of the present invention will be described with reference to the accompanying drawings. The following embodiments are specific examples of the present invention and do not limit 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. The construction machine 1 is configured by a hydraulic shovel, but this is merely 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 the rear are collectively called the front-back direction (x direction). The + z direction indicates above the construction machine 1, and the -z direction indicates below the construction machine 1. The upper part and the lower part are collectively called a vertical direction (z direction). The + y direction refers to the left when viewing the construction machine 1 from the back to the front, and the -y direction refers to the right when viewing the construction machine 1 from the back to the front. The right side and the left side are collectively referred to as left and right directions.

建設機械１は、クローラ式の下部走行体２と、下部走行体２上に旋回可能に設けられた上部旋回体３と、上部旋回体３に取り付けられた作業装置４とを備えている。 The construction machine 1 includes a crawler-type lower traveling body 2, an upper revolving body 3 rotatably provided on the lower traveling body 2, and a working device 4 attached to the upper revolving body 3.

作業装置４は、上部旋回体３に対して起伏可能に取り付けられたブーム４１と、ブーム４１の先端部に対して揺動可能に取り付けられたアーム４２と、アーム４２の先端部に対して揺動可能に取り付けられたバケット４３とを備えている。 The working device 4 includes a boom 41 attached to the upper swing body 3 so as to be able to undulate, an arm 42 attached to the tip of the boom 41 so as to be swingable, and a swing And a bucket 43 movably mounted.

また、作業装置４は、上部旋回体３に対してブーム４１を起伏させるブームシリンダ５１と、ブーム４１に対してアーム４２を揺動させるアームシリンダ５２と、アーム４２に対してバケット４３を揺動させるバケットシリンダ５３とを備えている。 The working device 4 also includes a boom cylinder 51 that raises and lowers the boom 41 with respect to the upper swing body 3, an arm cylinder 52 that rocks the arm 42 with respect to the boom 41, and a bucket 43 that rocks And a bucket cylinder 53 for causing the fuel to flow.

［ＺＭＰの概要］
図２は、ＺＭＰの説明図である。本実施の形態では建設機械１の全重心位置ｃとＺＭＰ（ＺｅｒｏＭｏｍｅｎｔＰｏｉｎｔ）とを用いて、建設機械１の転倒防止制御を行う。ＺＭＰは、物体が地面から受ける反力の中心位置を示し、動的な重心位置を示す。例えば、図１を参照して作業装置４が左側に加速しているとすると、ＺＭＰは全重心位置ｃの右側に移動する。 [Overview 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 position of the center of gravity c of the construction machine 1 and a ZMP (Zero Moment Point). ZMP indicates the center position of the reaction force received by the object 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 full center of gravity position c.

図２を参照する。ＺＭＰを用いた転倒防止制御は、ＺＭＰが支持多角形Ｄ１の境界Ｂ１を含まない内側に位置するように作業装置４を制御する。支持多角形Ｄ１は、物体が地面と接している領域を凸に取り囲む多角形である。建設機械１の場合、下部走行体２を構成する左右一対のクローラ２１，２２が地面と接しているので、クローラ２１，２２が地面と接している２つの領域を凸に取り囲んだほぼ矩形状の領域が支持多角形Ｄ１となる。 Please refer to FIG. In the fall prevention control using the ZMP, the work apparatus 4 is controlled such that the ZMP is positioned inside the supporting polygon D1 without including the boundary B1. The support polygon D1 is a polygon that protrudes around an area where the object is in contact with the ground. In the case of the construction machine 1, since the pair of right and left crawlers 21 and 22 constituting the lower traveling body 2 are in contact with the ground, a substantially rectangular shape that protrudes around two regions where the crawlers 21 and 22 are in contact with the ground is provided. The area becomes the support polygon D1.

建設機械１の静的な転倒安定性は、全重心位置ｃが機械の支持多角形Ｄ１の内側に存在することによって満たされる。しかし、外力などにより建設機械１に加速度が加わると、全重心位置ｃが支持多角形Ｄ１内に存在したとしても、ＺＭＰが支持多角形Ｄ１がはみ出している或いは境界Ｂ１に位置していれば、建設機械１は転倒する。例えば、ＺＭＰが点ｐ１に位置すれば、建設機械１は転倒しないが、ＺＭＰが点ｐ２に位置すれば、建設機械１は転倒する。そこで、建設機械１の動特性を考慮に入れた、ＺＭＰによる転倒判定が必要となる。 The static overturning stability of the construction machine 1 is satisfied by the fact that the entire center of gravity position c exists inside the support polygon D1 of the machine. However, when acceleration is applied to the construction machine 1 due to external force or the like, even if the entire center of gravity position c exists within the support polygon D1, if the ZMP is located outside the support polygon D1 or located at the boundary B1, The construction machine 1 falls down. 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 determine the overturn by ZMP in consideration of the dynamic characteristics of the construction machine 1.

＜ブロック図＞
図３は、図１に示す建設機械１に適用された転倒防止装置１０の構成を示すブロック図である。転倒防止装置１０は、回転軸を中心に回転する可動部を備える建設機械１において、建設機械１の転倒を防止する装置である。以下、可動部としては、図１に示す作業装置４を構成するブーム４１、アーム４２、及びバケット４３が採用できる。但し、これは一例であり、回転軸を中心に回転する建設機械１の構成部品であればどのような構成部品であっても可動部として適用可能である。例えば、油圧クレーンのジブが可動部とされてもよい。また、バケット４３以外のアタッチメントが可動部とされてもよいし、上部旋回体３が可動部とされてもよい。 <Block diagram>
FIG. 3 is a block diagram showing a configuration of a fall prevention device 10 applied to the construction machine 1 shown in FIG. The overturn prevention device 10 is a device that prevents the overturn of the construction machine 1 in the construction machine 1 including a movable portion that rotates about a rotation axis. Hereinafter, as the movable portion, the boom 41, the arm 42, and the bucket 43 constituting the working device 4 shown in FIG. 1 can be employed. However, this is only an example, and any component of the construction machine 1 that rotates about the rotation axis can be used as the movable part. For example, a jib of a hydraulic crane may be a movable part. Further, an attachment other than the bucket 43 may be a movable part, or 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), an inclination 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 a joint angle θ1 of the boom 41, a joint angle θ2 of the arm 42, and a joint angle θ3 of the bucket 43, respectively. The joint angle θ1 indicates the amount of ups and downs of the boom 41 with respect to the upper swing body 3. The joint angle θ2 indicates the amount of swing of the arm 42 with respect to the boom 41. The joint angle θ3 indicates the swing amount of the bucket 43 with respect to the arm 42. Hereinafter, when the joint angles θ1 to θ3 are collectively referred to as joint angles θ. The angle sensor 101 may be constituted by a rotary encoder or may be constituted by a potentiometer.

傾斜センサ１０２は、例えば、重力センサで構成され、水平面に対する建設機械１の傾斜量を示す傾斜角βを検出する。 The inclination sensor 102 is configured by, 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 detects the ZMP The angular velocity θ ′ may be calculated by first-order differentiation of the obtained joint angle θ, and the angular acceleration θ ″ may be calculated by second-order differentiation of the joint angle θ. Alternatively, if an angular velocity sensor that detects the angular velocity of the boom 41, the arm 42, and the bucket 43 is provided, the ZMP calculator 110 may obtain the angular velocity detected by the angular velocity sensor. This is the same when an angular acceleration sensor is provided.

＜ＺＭＰの計算＞
詳細には、ＺＭＰ算出部１１０は、下記の式（１）、（２）を用いてＺＭＰを算出すればよい。 <Calculation of ZMP>
More specifically, the ZMP calculating section 110 may calculate the ZMP using the following equations (1) and (2).

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

In equations (1) and (2), as shown in FIG. 2, the x-axis and the y-axis are set on the ground in contact with the construction machine 1, and the z-axis is set vertically above the ground. . In equations (1) and (2), p on the left side indicates ZMP. p is composed of only x and y components, and the z component is 0. This is because ZMP is a point on the supporting 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 the angular momentum of each movable part. The total momentum P and the total angular momentum L are variables dependent on the joint angle θ, the angular velocity θ ′, and the angular acceleration θ ″. By changing the direction of the gravitational acceleration g according to the inclination angle β, the calculation of the ZMP on the slope is also possible. Equations (1) and (2) show that ZMP can be arranged at a target location by adjusting the total momentum P and the total angular momentum L.

このように、ＺＭＰの計算には、建設機械１の全重心位置ｃ、全運動量Ｐ、及び全角運動量Ｌが必要となる。これらの物理量は、建設機械１の関節角θ、角速度θ’、角加速度θ”、及び建設機械の重力加速度ｇに対する傾斜角βから運動学的に求められる。よって、式（１）、（２）は、関節角θ、角速度θ’、及び角加速度θ”の関数となる。 As described above, the calculation of the ZMP requires the total gravity center 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 β of the construction machine with respect to the gravitational acceleration g. Therefore, the equations (1) and (2) ) Is a function of the joint angle θ, the angular velocity θ ′, and the angular acceleration θ ″.

なお、関節角θは関節数に応じた成分を持つベクトルである。図１の建設機械１は関節数が３つなので、関節角θはθ１，θ２，θ３の３成分からなるベクトルで表される。 Note that the joint angle θ is a vector having a component corresponding to the number of joints. Since the construction machine 1 in FIG. 1 has three joints, the joint angle θ is represented by a vector including 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).

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

<Safety 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 over in theory. However, in reality, errors occur in the ZMP calculation results due to modeling errors and measurement errors. Therefore, in the present embodiment, a safety area D2 is further set in the support polygon D1 of the construction machine 1, and whether or not the ZMP is within the safety area D2 is used as a criterion for overturn 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 illustrating the support polygon D1 and the safety area D2 when the construction machine 1 is viewed from above from below.

図４の例では、安全領域Ｄ２は支持多角形Ｄ１と相似な矩形の形状を持つ。また、図４の例では、安全領域Ｄ２の中心は支持多角形Ｄ１の中心と同じ位置に設定されている。安全領域Ｄ２の境界Ｂ２と支持多角形Ｄ１の境界Ｂ１との間の領域は調整領域Ｄ３である。 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. The area between the boundary B2 of the safety area D2 and the boundary B1 of the support polygon D1 is the adjustment area D3.

ＺＭＰが安全領域Ｄ２内に存在する場合、転倒防止装置１０は転倒防止制御を行わない。ＺＭＰが調整領域Ｄ３内に存在する場合、転倒防止装置１０は、ＺＭＰが安全領域Ｄ２に戻るように転倒防止制御を行う。 When the ZMP is within 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 predetermined size and shape from the shapes of the crawlers 21 and 22 of the construction machine 1. As a data structure of the position information of the support polygon D1, for example, in a coordinate system of the construction machine 1 including three axes of x, y, and z, four vertexes of the support polygon D1 when a certain position is set as an origin. Coordinates can be adopted.

安全領域Ｄ２の大きさは、建設機械１の仕様に応じて予め定められた値が採用できる。例えば、安全領域Ｄ２の大きさとしては、ＺＭＰが支持多角形Ｄ１の境界Ｂ１付近に存在する場合であっても、アクチュエータ１０４が飽和することなく、ＺＭＰを安全領域Ｄ２内に戻すことが可能な値が採用できる。 As the size of the safety region D2, a value predetermined according to the specifications of the construction machine 1 can be adopted. For example, as the size of the safety area D2, even when the ZMP exists near the boundary B1 of the support polygon D1, the ZMP can be returned to the safety area D2 without the actuator 104 being saturated. Values can be adopted.

安全領域Ｄ２の位置情報のデータ構造としては、例えば、建設機械１の座標系において、安全領域Ｄ２の４つの頂点の座標が採用できる。 As the data structure of the position information of the safety area D2, for example, in the coordinate system of the construction machine 1, the coordinates of the four vertices of the safety area D2 can be adopted.

図３に参照を戻す。探索部１１１は、現在のＺＭＰが安全領域Ｄ２の外側に位置する場合、安全領域Ｄ２の境界Ｂ２及び任意のＺＭＰ間の距離Ｌｐを角加速度θ”で示す第１関数Ｆ１を設定する。そして、探索部１１１は、第１関数Ｆ１において、現在の角加速度θ”に対して距離Ｌｐが減少する割合が最大となる傾斜方向を探索し、探索した傾斜方向に沿って角加速度を変化させることにより、距離Ｌｐが零以下になる角加速度を探索する。この探索のアルゴリズムとしては、例えば最急降下法が採用できる。 Referring back to FIG. When the current ZMP is located outside the safety region D2, the search unit 111 sets a first function F1 indicating the distance Lp between the boundary B2 of the safety region D2 and an arbitrary ZMP by an angular acceleration θ ″. The search unit 111 searches the first function F1 for an inclination direction in which the rate of decrease of the distance Lp with respect to the current angular acceleration θ ″ is the largest, and changes the angular acceleration along the searched inclination direction. , The angular acceleration at which the distance Lp becomes zero or less is searched. As the algorithm for this search, for example, the steepest descent method can be adopted.

＜距離Ｌｐの取り方＞
図５は、距離Ｌｐの取り方の一例を示す図である。図５の例では、点ｐは、任意のＺＭＰを示す。図５の例では、安全領域Ｄ２の中心Ｏと点ｐとを繋ぐ直線Ｋ１上における任意のＺＭＰ（点ｐ）と境界Ｂ２との長さが距離Ｌｐとして採用されている。 <How to set the 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 a straight line K1 connecting the center O of the safety region D2 and the point p is adopted as the distance Lp.

任意のＺＭＰ（点ｐ）は、上記の式（１）、（２）で表される。式（１）、（２）は、上述したように、関節角θ、角速度θ’、及び角加速度θ”の関数なので、距離Ｌｐは、関節角θ、角速度θ’、及び角加速度θ”の関数となる。 An arbitrary ZMP (point p) is represented by the above equations (1) and (2). As described above, the equations (1) and (2) are functions of the joint angle θ, the angular velocity θ ′, and the angular acceleration θ ″, and therefore, the distance Lp is equal to the joint angle θ, the angular velocity θ ′, and the angular acceleration θ ″. Function.

また、探索部１１１は、ＺＭＰが境界Ｂ２の外側に存在する場合、Ｌｐ＞０となるように距離Ｌｐを定める。したがって、探索部１１１は、距離Ｌｐが零未満（Ｌｐ＜０）であれば、ＺＭＰが安全領域Ｄ２の内側に存在するので、建設機械１は安定であると判定する。一方、探索部１１１は、距離Ｌｐが零以上（Ｌｐ≧０）であれば、ＺＭＰが安全領域Ｄ２の外側に存在するので建設機械１は安定でないと判定する。 In addition, when the ZMP exists outside the boundary B2, the search unit 111 determines the distance Lp such that Lp> 0. Therefore, when 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 area D2. On the other hand, when the distance Lp is equal to or greater than zero (Lp ≧ 0), the search unit 111 determines that the construction machine 1 is not stable because the ZMP exists outside the safety area D2.

なお、距離Ｌｐの取り方は、図５に示す取り方に限定されない。例えば、距離Ｌｐは、安全領域Ｄ２の境界Ｂ２から任意のＺＭＰ（点ｐ）までの最短距離が採用されてもよい。 Note that the way of obtaining the distance Lp is not limited to the way shown in FIG. For example, the shortest distance from the boundary B2 of the safety area 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 area D2 and enters the adjustment area D3 (Lp> 0), or when the ZMP is located on the boundary B2 of the safety area D2 (Lp = 0), It is considered that the control amount of the actuator 104 is adjusted to keep the ZMP within the safety area D2.

式（１），（２）において、全運動量Ｐ及び全角運動量Ｌは、関節角θ、角速度θ’、及び角加速度θ”に依存する。よって、ある時刻において、センサにより関節角θ及び角速度θ’が既知であれば、角加速度θ”を調節することによりＺＭＰを所望の場所に配置できる。この場合、ある時刻における距離Ｌｐを示す第１関数Ｆ１は角加速度θ”の関数として考えられる。そこで、探索部１１１は、距離Ｌｐを第１関数Ｆ１（θ”）として取り扱う。 In the formulas (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 θ ″. 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 can be considered as a function of the angular acceleration θ ″. Therefore, the search unit 111 treats the distance Lp as the first function F1 (θ ″).

詳細には、転倒防止装置１０が角速度センサを備えているのであれば、探索部１１１は、関節角θ及び角速度θ’に角度センサ１０１及び角速度センサの検出値を代入することで、第１関数Ｆ１を角加速度θ”の関数に設定すればよい。また、転倒防止装置１０が角速度センサを備えていないのであれば、探索部１１１は、関節角θに、角度センサ１０１の検出値を代入すると共に、角速度θ’に、一つ前のサンプリング点における角加速度θ”から得られた角速度θ’を代入することで、第１関数Ｆ１を角加速度θ”の関数に設定すればよい。 More specifically, if the fall 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 for the joint angle θ and the angular velocity θ ′ to obtain the first function. F1 may be set as a function of the angular acceleration θ ″. If the fall prevention device 10 does not include an angular velocity sensor, the search unit 111 substitutes the detection value of the angle sensor 101 for the joint angle θ. At the same time, 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 immediately preceding sampling point into the angular velocity θ ′.

なお、第１関数Ｆ１を角加速度θ”の関数としたのは、関節角θ、角速度θ’、及び角加速度θ”のうち、角加速度θ”の応答性が高いからである。 The reason why the first function F1 is a function of the angular acceleration θ ″ is that the responsiveness of the angular acceleration θ ″ among the joint angle θ, the angular velocity θ ′, and the angular acceleration θ ″ is high.

図３に参照を戻す。駆動部１１２は、アクチュエータ１０４の制御量が探索部１１１により探索された角加速度θ”になるように、油圧回路１０５を通じてアクチュエータ１０４を制御する。なお、駆動部１１２は、可動部が複数存在するのであれば、複数の可動部のそれぞれを個別に制御する。図１の例では、駆動部１１２は、ブームシリンダ５１、アームシリンダ５２、及びバケットシリンダ５３を個別に制御する。 Referring back 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 the example of Fig. 1, the drive unit 112 controls the boom cylinder 51, the arm cylinder 52, and the bucket cylinder 53 individually.

アクチュエータ１０４は、可動部が複数存在するのであれば、複数の可動部に対応して複数のアクチュエータ１０４が存在する。図１の例では、アクチュエータ１０４は、ブームシリンダ５１、アームシリンダ５２、及びバケットシリンダ５３に対応する３つのアクチュエータ１０４を含む。 If the actuator 104 has a plurality of movable parts, there are a plurality of actuators 104 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, an inverse proportional valve 603, 604, an operation unit 605, a hydraulic pump 606, and a tank 607. Here, the hydraulic circuit 105 for operating 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 hydraulic oil to the head H side of the cylinder 601 at a supply amount according to the pilot pressure, and supplies the hydraulic oil on the rod R side of the cylinder 601 to the pilot port. The liquid is discharged to the tank 607 at 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 the hydraulic oil to the rod R side of the cylinder 601 at a supply amount corresponding to the pilot pressure, and the hydraulic oil on the head H side of the cylinder 601. 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 according to the operation amount of the operation lever 605a. Here, when the operation lever 605a is operated in a direction to rotate the boom 41 in the first direction (the upright direction), the operation unit 605 inputs a pilot pressure corresponding to the operation amount to the pilot port 602a through the pilot line 608a. I do. On the other hand, when the operation lever 605a is operated in a direction for rotating the boom in a second direction (downward direction) opposite to the first direction, the operation unit 605 applies a pilot pressure corresponding to the operation amount to the pilot through the pilot line 608b. Output to port 602b.

シリンダ６０１は、例えば、図１に示すブームシリンダ５１で構成され、ブーム４１を起伏させる。 The cylinder 601 includes, for example, the boom cylinder 51 illustrated in FIG.

逆比例弁６０３は、操作部６０５及びパイロットポート６０２ａ間を繋ぐパイロット管路６０８ａに設けられている。逆比例弁６０４は、操作部６０５及びパイロットポート６０２ｂ間を繋ぐパイロット管路６０８ｂに設けられている。 The inverse proportional valve 603 is provided in a pilot pipeline 608a that connects between the operation unit 605 and the pilot port 602a. The inverse proportional valve 604 is provided in a pilot pipe 608b connecting between the operation unit 605 and the pilot port 602b.

逆比例弁６０３，６０４は、コントローラ１０３からの制御信号に応じて、パイロット圧を減圧させる。 The inverse proportional valves 603 and 604 reduce the pilot pressure according to a control signal from the controller 103.

コントローラ１０３の駆動部１１２は、現在の角加速度θ”と探索部１１１で探索された角加速度θ”との偏差が零となるようにシリンダー６０１をフィードバック制御する。ここでは、駆動部１１２は、逆比例弁６０３，６０４の減圧量を調整することで、シリンダー６０１をフィードバック制御する。駆動部１１２は、操作レバー６０５ａが第１方向に操作されていたとすると、逆比例弁６０３を制御し、操作レバー６０５ａが第２方向に操作されていたとすると、逆比例弁６０４を制御すればよい。 The drive unit 112 of the controller 103 performs feedback control of the cylinder 601 so that the deviation between the current angular acceleration θ ″ and the angular acceleration θ ″ searched for by the search unit 111 becomes zero. Here, the drive unit 112 performs feedback control of the cylinder 601 by adjusting the amount of pressure reduction of the inverse proportional valves 603 and 604. The drive unit 112 may control the inverse proportional valve 603 when the operation lever 605a is operated in the first direction, and may control the inverse proportional valve 604 when the operation lever 605a is operated in the second direction. .

＜フローチャート＞
図７は、実施の形態１の転倒防止装置１０の処理を示すフローチャートである。このフローチャートは、例えば、建設機械１がキーオンされている期間、所定のサンプリング周期で実行される。 <Flow chart>
FIG. 7 is a flowchart illustrating a process of the fall prevention device 10 according to the first embodiment. This flowchart is executed at a predetermined sampling cycle, for example, while the construction machine 1 is on.

まず、ＺＭＰ算出部１１０は、関節角θ、角速度θ’、及び角加速度θ”を取得する（Ｓ７０１）。ここで、ＺＭＰ算出部１１０は、角度センサ１０１から関節角θを取得し、関節角θを一階微分することで角速度θ’を取得し、関節角θを二階微分することで角加速度θ”を取得すればよい。 First, the ZMP calculating unit 110 obtains the joint angle θ, the angular velocity θ ′, and the angular acceleration θ ″ (S701). Here, the ZMP calculating unit 110 obtains the joint angle θ from the angle sensor 101, and obtains 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 Expressions (1) and (2) with the inclination angle β, and substitutes the joint angle θ and the angular velocity θ ′ into Expressions (1) and (2) after correction. , And the angular acceleration θ ″, the current ZMP is calculated (S703).

次に、探索部１１１は、現在のＺＭＰが安全領域Ｄ２の内側に存在すれば（Ｓ７０４でＹＥＳ）、建設機械１は転倒防止制御の必要がないと判定し、処理を終了する。一方、探索部１１１は、現在のＺＭＰが安全領域Ｄ２の外側に存在すれば（Ｓ７０４でＮＯ）、転倒防止制御の必要があると判定し、探索処理を実行する（Ｓ７０５）。探索処理の詳細は、図８を用いて後述する。 Next, if the current ZMP is inside the safety area D2 (YES in S704), the search unit 111 determines that the construction machine 1 does not need the overturn prevention control, and ends the processing. On the other hand, when the current ZMP is outside the safety area D2 (NO in S704), the search unit 111 determines that the fall prevention control is necessary, and executes the search processing (S705). Details of the search processing 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 processing (S706), and ends the processing.

図８は、探索処理の詳細を示すフローチャートである。まず、探索部１１１は、第１関数Ｆ１において、現在の角加速度θ”に対して距離Ｌｐが減少する割合が最大となる傾斜方向を示す傾斜方向αを算出する（Ｓ８０１）。 FIG. 8 is a flowchart illustrating details of the search process. First, the search unit 111 calculates, in the first function F1, an inclination direction α indicating an inclination direction in which the rate of decrease in the distance Lp with respect to the current angular acceleration θ ″ is maximized (S801).

図９は、第１関数Ｆ１の一例を示すグラフであり、縦軸は距離Ｌｐを示し、奥行軸は関節角θ１に対応する角加速度θ１”を示し、横軸は関節角θ２に対応する角加速度θ２”を示している。なお、図９では、説明の便宜上、角加速度θ”はθ１”，θ２”の２成分のベクトルで構成されている。但し、これは一例であり、角加速度θ”は、関節数の個数に応じた成分のベクトルで構成される。また、図９では、説明の便宜上、第１関数Ｆ１は、複数の交差する稜線で示されている。 FIG. 9 is a graph showing an example of the first function F1, where the vertical axis represents the distance Lp, the depth axis represents the angular acceleration θ1 ″ corresponding to the joint angle θ1, and the horizontal axis represents 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 equal to the number of joints. It is composed of a vector of the corresponding component. In FIG. 9, the first function F1 is indicated by a plurality of intersecting ridge lines for convenience of explanation.

図９において、点Ｐαは現在の角加速度θ”が位置する第１関数Ｆ１上の点を示している。探索部１１１は、第１関数Ｆ１おいて現在の角加速度θ”（θ１”，θ２”）に対応する点Ｐαを設定し、点Ｐαを中心に角加速度を微小変化させながら、距離Ｌｐが減少する割合が最大となる方向を探索することで傾斜方向αを算出すればよい。図９の例では、探索の結果、矢印で示す方向が傾斜方向αとして算出されている。なお、傾斜方向αはα１，α２の２成分のベクトルで構成され、α１は関節角θ１に対応し、α２は関節角θ２に対応する。また、傾斜方向αの大きさは、｜α｜＝１である。 9, a point Pα indicates a point on the first function F1 where the current angular acceleration θ ″ is located. The search unit 111 determines the current angular acceleration θ ″ (θ1 ″, θ2 in the first function F1). )), The inclination direction α may be calculated by searching for a direction in which the rate at which the distance Lp decreases becomes maximum 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 inclination direction α. The inclination direction α is composed of two component vectors α1 and α2, where α1 corresponds to the joint angle θ1 and α2 corresponds to the joint angle θ2. 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 a positive real number. Is a vector whose size in the inclination direction α is a variable d.

次に、探索部１１１は、距離Ｌｐが零未満（Ｌｐ＜０）であるか否かを判定する（Ｓ８０３）。距離Ｌｐが零未満であれば（Ｓ８０３でＹＥＳ）、探索部１１１は、ＺＭＰが安全領域Ｄ２内に入ったので、Ｓ８０２で用いた変数ｄを確定変数ｄ^＊として確定する（Ｓ８０４）。 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), since the ZMP has entered the safe area D2, the search unit 111 determines the variable d used in S802 as the determined variable d ^* (S804).

次に、探索部１１１は、確定変数ｄ^＊における角加速度（θ”＋ｄ^＊・α）を探索結果として決定する（Ｓ８０５）。 Next, the search unit 111 determines an angular acceleration (θ ″ + d ^* · α) for the fixed 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 the distance Lp is likely to decrease before and after the change of the variable d (S806). . Here, the search unit 111 calculates the change amount ΔLp of the distance Lp, and if ΔLp <0, determines that the distance Lp is likely to decrease (YES in S806), and increases the variable d by a certain amount (S807). ), And 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, _{d t-1} and before the change variable d, and the variable d after the change, and _{d t, ΔLp = Lp (θ} "+ d t · α) -Lp (θ" + d t-1 · α).

図９を参照して図８の処理を説明する。距離Ｌｐが減少する限り、探索部１１１は、変数ｄを増大させながら、第１関数Ｆ１上において点Ｐαを傾斜方向αの方向に沿ってずらしていく（Ｓ８０２→Ｓ８０３：ＮＯ→Ｓ８０６→Ｓ８０７→Ｓ８０２）。そして、距離Ｌｐが零未満になれば（Ｓ８０３：ＹＥＳ）、探索部１１１は、ＺＭＰを安全領域Ｄ２に戻す角加速度θ”が探索できたと判定する。 The processing 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 becomes smaller than zero (S803: YES), the search unit 111 determines that the angular acceleration θ ″ that returns the ZMP to the safe area D2 has been searched.

一方、点Ｐαを傾斜方向αの方向に沿ってずらしていっても、距離Ｌｐが減少しなくなると、探索部１１１は、これ以上、傾斜方向αに点Ｐαをずらしていっても、ＺＭＰを安全領域Ｄ２に戻す角速度θ”を探索できる見込みが低いと判定する（Ｓ８０６でＮＯ）。そして、探索部１１１は、処理をＳ８０１に戻し、次の傾斜方向αを算出する。この場合、傾斜方向αは前回算出した調整方向の次に距離Ｌｐの減少する割合が大きな方向を傾斜方向αとして決定すればよい。なお、次の傾斜方向αでもΔＬｐ≧０となれば、探索部１１１は、その次に距離Ｌｐの減少する割合が大きい方向を傾斜方向αとして算出し、その傾斜方向αに沿って探索すればよい。 On the other hand, even if the point Pα is shifted along the direction of the inclination direction α, if the distance Lp does not decrease, the search unit 111 further sets the ZMP even if the point Pα is further shifted in the inclination direction α. It is determined that it is unlikely that the angular velocity θ ″ to be returned to the safe area D2 can be searched (NO in S806). Then, the search unit 111 returns the processing to S801 and calculates the next inclination direction α. α may be determined as a direction in which the rate of decrease of the distance Lp next to the previously calculated adjustment direction is larger as the inclination direction α.If ΔLp ≧ 0 also in the next inclination direction α, the search unit 111 Next, a direction in which the rate of decrease in the distance Lp is large is calculated as the inclination direction α, and the search may be performed along the inclination 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 sets the distance Lp between an arbitrary ZMP and the boundary B2 of the safety area D2 to an angle. In the first function F1 represented by the acceleration θ ″, an angular acceleration θ ″ at which the distance Lp is less than zero is searched. Thereby, the overturning 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 at which the rate of decrease of the distance Lp with respect to the current angular acceleration θ ″ becomes the maximum. Thus, the fall of the construction machine 1 can be prevented.

特に、建設機械１は搭乗者の操作によって可動部の目標動作が時々刻々変動するので、２足歩行ロボットに比べて可動部の目標動作を予測することが困難である。転倒防止装置１０は、少ない計算量で現在のＺＭＰを安全領域Ｄ２の内側に戻す角加速度θ”が算出できるので、目標動作の予測が困難な建設機械１における転倒防止制御に適している。 In particular, since the target operation of the movable part of the construction machine 1 fluctuates every moment due to the operation of the occupant, it is more difficult to predict the target operation of the movable part as compared with the bipedal walking robot. The fall prevention device 10 is suitable for fall prevention control in the construction machine 1 in which it is difficult to predict a target operation because the angular acceleration θ ″ that returns the current ZMP to the inside of the safety region D2 can be calculated with a small amount of calculation.

（実施の形態２）
図１０は、本発明の実施の形態２に係る転倒防止装置１０Ａの構成を示すブロック図である。転倒防止装置１０Ａは、安全領域Ｄ２の内側に更に非制御領域Ｄ４（図１１）を設ける点を特徴とする。なお、実施の形態２において、実施の形態１と同一の構成要素には同一の符号を付し、説明を省く。 (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 overturn prevention device 10A is characterized in that a non-control area D4 (FIG. 11) is further provided inside the safety area D2. In the second embodiment, the same components as those in the first embodiment are denoted by the same reference numerals, and description thereof will be omitted.

転倒防止装置１０Ａは、探索部１１１Ａ、駆動部１１２Ａ、及び位置情報記憶部１１３Ａが実施の形態１の探索部１１１、駆動部１１２、及び位置情報記憶部１１３と異なる。 The fall prevention device 10A is different from the search unit 111, the drive unit 112, and the position information storage unit 113 of the first embodiment in the search unit 111A, the drive unit 112A, and the position information storage unit 113A.

位置情報記憶部１１３Ａは、支持多角形Ｄ１の位置情報と、安全領域Ｄ２の位置情報とに加えて、更に、非制御領域Ｄ４の位置情報を記憶する。図１１は、建設機械１を上方から下方に見たときの支持多角形Ｄ１と安全領域Ｄ２と非制御領域Ｄ４とを示す図である。 The position information storage unit 113A stores the position information of the non-control region D4 in addition to the position information of the support polygon D1 and the position information of the safety region D2. FIG. 11 is a diagram illustrating the support polygon D1, the safety area D2, and the non-control area 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 centers of the safety area D2 and the non-control area D4 are set to the center O of the support polygon D1. The non-control area D4 is an area where the fall prevention control is not performed by the controller 103, 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 area D4, a value predetermined according to the specifications of the construction machine 1 can be adopted.

図１０を参照し、探索部１１１Ａは、非制御領域Ｄ４の外側且つ安全領域Ｄ２の内側に現在のＺＭＰが位置する場合、現在のＺＭＰが非制御領域Ｄ４から離れるにつれて、現在のＺＭＰの非制御領域Ｄ４側への移動量が長くなるように目標ＺＭＰを設定する。そして、探索部１１１Ａは、目標ＺＭＰ及び任意のＺＭＰ間の距離Ｌｐを角加速度θ”で示す第２関数Ｆ２を設定する。そして、探索部１１１Ａ、第２関数Ｆ２において、実施の形態１と同様の探索を実行し、距離Ｌｐを零にする角加速度を探索する。 Referring to FIG. 10, when current ZMP is located outside non-control area D4 and inside safety area D2, search unit 111A determines whether the current ZMP is uncontrolled as the current ZMP moves away from non-control area D4. The target ZMP is set so that the amount of movement to the area 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 θ ″. The search unit 111A and the second function F2 are the same as in the first embodiment. To search for an angular acceleration that makes the distance Lp zero.

図１２は、探索部１１１Ａの処理を説明する図である。図１２を参照し、点ｐは現在のＺＭＰである。点ｐ（ハット）は目標ＺＭＰである。なお、明細書中では、記載の制約上、記号「∧」を（ハット）と記載する。 FIG. 12 is a diagram illustrating the process 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 restrictions on the 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 area D4 and the distance L2 between the current ZMP (point p) and the boundary B2 of the safety area D2. .

次に、探索部１１１Ａは、距離Ｌ１を距離ｌ１と距離ｌ２とで内分した目標ＺＭＰ（点ｐ（ハット））を目標ＺＭＰとして決定する。但し、ｌ１：ｌ２＝Ｌ１：Ｌ２である。つまり、ｐ（ハット）＝Ｌ１・（ｌ１／（ｌ１＋ｌ２））である。したがって、現在のＺＭＰ（点ｐ）が非制御領域Ｄ４から離れるにつれて、現在のＺＭＰ（点ｐ）の非制御領域Ｄ４側への移動量が長くなるように目標ＺＭＰ（点ｐ（ハット））が設定される。 Next, the search unit 111A determines a target ZMP (point p (hat)) obtained by internally dividing the distance L1 by the distances l1 and l2 as the target ZMP. However, l1: l2 = L1: L2. That is, p (hat) = L1 · (11 / (11 + 12)). Therefore, the target ZMP (point p (hat)) is set so that the moving amount of the current ZMP (point p) toward the non-control area D4 becomes longer as the current ZMP (point p) moves away from the non-control area 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 by using an angular acceleration θ ″. The first function F1 is between the arbitrary ZMP and the boundary B2. Is a function represented by 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 functions F2, the distance between the point p and the target ZMP (point p (hat)) is the distance Lp.In the example of Fig. 12, since the safety area D2 and the non-control area D4 are concentric circles, the distance Lp is the point Lp. 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 that the second function F2 is the same as the first function F1. , Angular acceleration θ ″.

駆動部１１２Ａは、第２関数Ｆ２を用いた探索により得られた角加速度θ”を、目標ＺＭＰが非制御領域Ｄ４の境界Ｂ３に近づくにつれて低下させるように、角加速度θ”を修正する。 The drive unit 112A corrects the angular acceleration θ ″ such 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, assuming that the corrected angular acceleration θ ″ is θb ″ and the uncorrected angular acceleration θ ″ is θa ″, the drive unit 112A corrects the angular acceleration θ ″ using the following equation.

θｂ”＝（ｌ１／ｌ１＋ｌ２）・θａ”
＜フローチャート＞
図１３は、本発明の実施の形態２に係る探索処理の詳細を示すフローチャートである。なお、実施の形態２では、メインルーチンは実施の形態１と同様、図７のフローチャートが採用される。 θb ″ = (l1 / l1 + l2) · θa ″
<Flow chart>
FIG. 13 is a flowchart showing details of the search processing according to Embodiment 2 of the present invention. Note that in the second embodiment, the main routine employs 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 following S805. Otherwise, the processing in FIG. 13 is the same as in FIG. In S1301, search unit 111A determines whether distance Lp is Lp = 0, and 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.

このように、実施の形態２では、Ｌｐ＝０、すなわち、現在のＺＭＰを目標ＺＭＰに位置決めするための角加速度θ”が探索される。 As described above, 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 drive unit 112A calculates the angular acceleration θb ″ by multiplying the angular acceleration θa ″ determined in S805 by l1 / 11 + l2, and corrects the angular acceleration θa ″ (S1302). In S706 of 7, the drive unit 112A controls the actuator 104 such that the control amount of the actuator 104 becomes the angular acceleration θb ″.

このように、実施の形態２に係る転倒防止装置１０Ａによれば、非制御領域Ｄ４の外側且つ安全領域Ｄ２の内側に現在のＺＭＰが位置する場合においても、転倒防止制御を実行する。そのため、転倒防止制御の実行と非実行とが繰り返され、アクチュエータを振動的に駆動させることを防止できる共に、ＺＭＰが安全領域Ｄ２からはみ出したときにアクチュエータ１０４の制御量が過大になることを防止できる。しかも、この場合、現在のＺＭＰが非制御領域Ｄ４から離れるにつれて、現在のＺＭＰの非制御領域Ｄ４側への移動量が長くなるように目標ＺＭＰが設定されると共に、目標ＺＭＰにするために必要な角加速度θ”が現在のＺＭＰが非制御領域Ｄ４から離れるにつれて大きくなるように修正される。 As described above, 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 region D4 and inside the safety region D2. Therefore, the execution and non-execution of the overturn prevention control are repeated to prevent the actuator from being vibrated, and to prevent the control amount of the actuator 104 from becoming excessive when the ZMP is out of the safety area D2. it can. Moreover, in this case, the target ZMP is set such that the moving amount of the current ZMP toward the non-control area D4 becomes longer as the current ZMP moves away from the non-control area D4, and it is necessary to set the target ZMP. Is corrected so as to increase as the current ZMP moves away from the non-control area D4.

そのため、転倒の危険性が高くなるにつれて制御量が増大するように転倒防止制御を実行することがでる。更に、ＺＭＰが安全領域Ｄ２に戻った後も、緩やかな転倒防止制御が継続されるため、ＺＭＰが安全領域Ｄ２からはみ出ることを抑制できる。 Therefore, it is possible to execute the overturn prevention control so that the control amount increases as the risk of overturn increases. Furthermore, even after the ZMP returns to the safety area D2, the gentle fall prevention control is continued, so that the ZMP can be suppressed from protruding from the safety area D2.

（実施の形態３）
図１４は、本発明の実施の形態３に係る転倒防止装置１０Ｂの構成を示すブロック図である。転倒防止装置１０Ｂは、実施の形態１の転倒防止装置１０において、ＺＭＰの位置を予測して転倒防止制御を行う点を特徴とする。実施の形態３において実施の形態１と同一の構成要素は同一の符号を付し、説明を省く。 (Embodiment 3)
FIG. 14 is a block diagram showing a configuration of a fall 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 according to the first embodiment performs the fall prevention control by predicting the position of the ZMP. In the third embodiment, the same components as those in the first embodiment are denoted by the same reference numerals, and description thereof will be omitted.

図１４において、実施の形態１の構成を示す図３との相違点は、探索部１１１に代えて、探索部１１１Ｂが設けられている点にある。それ以外は実施の形態１と同じである。 FIG. 14 differs from FIG. 3 showing the configuration of the first embodiment in that a search unit 111B is provided instead of search unit 111. Otherwise, the configuration is the same as that of the first embodiment.

探索部１１１Ｂは、現在のＺＭＰと現在のＺＭＰの速度とに基づいて、ＺＭＰの予測位置を算出し、算出した予測位置が安全領域Ｄ２の外側に位置する場合、第１関数Ｆ１を用いた探索を実行する。 The search unit 111B calculates a predicted position of the ZMP based on the current ZMP and the current speed of the ZMP, and when the calculated predicted position is located outside the safety region D2, the search using the first function F1. Execute

＜フローチャート＞
図１５は、本発明の実施の形態３に係る転倒防止装置１０Ｂの処理を示すフローチャートである。図１５において、図７との相違点は、Ｓ７０３に続いてＳ１６０１，Ｓ１６０２の処理が追加され、Ｓ７０４に代えてＳ１６０３が設けられている点にある。 <Flow chart>
FIG. 15 is a flowchart showing a process of the fall prevention device 10B according to Embodiment 3 of the present invention. 15 differs from FIG. 7 in that the processes of S1601 and S1602 are added after S703, and that S1603 is provided instead of S704.

Ｓ１６０１では、探索部１１１Ｂは、ＺＭＰの速度を算出する。ここで、探索部１１１Ｂは、例えば、前回算出したＺＭＰと今回算出したＺＭＰとの差をサンプリング周期で割ることで、ＺＭＰの速度を求めればよい。 In S1601, the search unit 111B calculates the speed of ZMP. Here, the search unit 111B may obtain the ZMP speed by, for example, dividing the difference between the previously calculated ZMP and the currently calculated ZMP by the sampling cycle.

次に、探索部１１１Ｂは、ＺＭＰの予測位置を算出する（Ｓ１６０２）。図１６は、ＺＭＰの予測位置の説明図である。図１６の例では、安全領域Ｄ２は、実施の形態２と同様、中心が支持多角形Ｄ１の中心に位置する円である。 Next, the search unit 111B calculates a predicted position of the ZMP (S1602). FIG. 16 is an explanatory diagram of the predicted position of the ZMP. In the example of FIG. 16, the safety area 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. The point p (hat) indicates the predicted position. The search unit 111B calculates a predicted position (point p (hat)) using the following equation. “P ′ · Δt” indicates the momentum of ZMP movement.

ｐ（ハット）＝ｐ＋ｐ’・Δｔ
探索部１１１Ｂは、予測位置（点ｐ（ハット））が安全領域Ｄ２の内側に位置すれば（Ｓ１６０３でＹＥＳ）、建設機械１は転倒防止制御の必要がないと判定し、処理を終了する。一方、探索部１１１Ｂは、現在のＺＭＰが安全領域Ｄ２の外側に存在すれば（Ｓ１６０３でＮＯ）、転倒防止制御の必要があると判定し、探索処理を実行する（Ｓ７０５）。 p (hat) = p + p ′ · Δt
If the predicted position (point p (hat)) is located inside the safety area D2 (YES in S1603), the search unit 111B determines that the construction machine 1 does not need the overturn prevention control, and ends the process. On the other hand, when the current ZMP is outside the safe area D2 (NO in S1603), the search unit 111B determines that the fall prevention control is necessary, and executes the search processing (S705).

このように、転倒防止装置１０Ｂによれば、現在のＺＭＰの移動の勢いを考慮に入れて、実際に現在のＺＭＰが安全領域Ｄ２外にはみ出していなくても、ＺＭＰの移動の勢いが大きい場合は事前にＺＭＰを安全領域Ｄ２の内側に戻す転倒防止制御が行われる。そのため、ＺＭＰが安全領域Ｄ２からはみ出て、いざ、ＺＭＰを安全領域Ｄ２に戻そうとしたときに、制御量が過大になり、アクチュエータ１０４が飽和することを防止できる。そのため、アクチュエータ１０４の飽和により、安全領域Ｄ２にＺＭＰを速やかに戻すことができなくなることを回避できる。 As described above, according to the fall prevention device 10B, the momentum of the movement of the ZMP is large even if the current momentum of the ZMP does not actually protrude out of the safety area D2 in consideration of the momentum of the movement of the current ZMP. In this case, the fall prevention control for returning the ZMP to the inside of the safety area D2 is performed in advance. Therefore, when the ZMP protrudes from the safety area D2 and the ZMP is to be returned to the safety area D2, the control amount becomes excessively large, and the saturation of the actuator 104 can be prevented. Therefore, it is possible to prevent the ZMP from being quickly returned to the safe area 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. However, 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 found angular acceleration θ ″ by the moment of inertia, and control the actuator 104 to output the torque.

制御量としてトルクを採用した場合、実施の形態２において、トルクは下記の式で修正される。但し、Ｔａは探索により得られた角加速度θａ”に応じたトルクであり、Ｔｂは修正後のトルクである。 When the torque is adopted as the control amount, in the second embodiment, the torque is corrected by the following equation. Here, Ta is a torque corresponding to the angular acceleration θa ″ obtained by the search, and Tb is a corrected torque.

Ｔｂ＝（ｌ１／ｌ１＋ｌ２）・Ｔａ
（変形例２）
実施の形態３では、実施の形態２の内容を考慮しなかったが、実施の形態２の内容を考慮に入れても良い。この場合、転倒防止装置１０Ｂは、予測位置（点ｐ（ハット））が安全領域Ｄ２の内側に位置すれば、実施の形態２の手法をそのまま適用すればよい。 Tb = (l1 / l1 + l2) · Ta
(Modification 2)
Although the contents of the second embodiment are not considered in the third embodiment, 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 area D2, the method of the second embodiment may be applied to the fall prevention device 10B as it is.

Ｂ１，Ｂ２，Ｂ３境界
Ｄ１支持多角形
Ｄ２安全領域
Ｄ３調整領域
Ｄ４非制御領域
Ｆ１第１関数
Ｆ２第２関数
ΔＬｐ変化量
α 傾斜方向
β 傾斜角
θ 関節角
θ’ 角速度
θ” 角加速度
１建設機械
２下部走行体
３上部旋回体
４作業装置
１０，１０Ａ，１０Ｂ転倒防止装置
１０１角度センサ
１０２傾斜センサ
１０３コントローラ
１０４アクチュエータ
１１０ＺＭＰ算出部
１１１，１１１Ａ，１１３Ｂ探索部
１１２，１１２Ａ駆動部
１１３，１１３Ａ位置情報記憶部

B1, B2, B3 Boundary D1 Supporting polygon D2 Safety area D3 Adjustment area D4 Non-control area F1 First function F2 Second function ΔLp Change α Tilting direction β Tilting angle θ Joint angle θ 'Angular velocity θ ”Angular acceleration 1 Construction machinery 2 Lower traveling body 3 Upper revolving superstructure 4 Work 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

In a construction machine having a movable part that rotates about a rotation axis, a fall prevention device that prevents the construction machine from falling,
An actuator for operating the movable part,
An inclination angle of the construction machine with respect to gravity, and an acquisition unit that acquires an angle, an angular velocity, and an angular acceleration of the movable unit with respect to the rotation axis,
A ZMP calculation unit that calculates a ZMP that is a dynamic center of gravity of the construction machine based on the acquired inclination angle, angle, angular velocity, and angular acceleration;
A support polygon set in the construction machine, and a position information storage unit that stores position information of a safety area set inside the support polygon,
When the current ZMP is located outside the safety area, a rate at which the distance decreases with respect to the current angular acceleration in a first function indicating a distance between the boundary of the safety area and an arbitrary ZMP as an angular acceleration. A search unit that searches for an inclination direction in which the maximum is returned, and searches for an angular acceleration that returns the current ZMP to the inside of the safety area by changing the angular acceleration along the searched inclination direction.
A drive unit that controls the actuator such that the control amount of the actuator is a control amount according to the searched angular acceleration.

The position information storage unit further stores position information of a non-control area in which 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 to the non-control area side increases. Set the target ZMP to be longer,
In a second function indicating the distance between the target ZMP and the arbitrary ZMP by the angular acceleration, a tilt direction in which the rate of decrease in the distance with respect to the current angular acceleration is maximized is searched, and the searched tilt is 2. The fall prevention device according to claim 1, wherein the angular acceleration that changes the angular acceleration along a direction is searched for an angular acceleration that makes the distance zero.

3. The fall prevention device according to claim 2, wherein the driving unit increases a control amount according to the angular acceleration obtained by the search using the second function as the target ZMP moves away from a boundary of the non-control region. 4. .

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 is performed using a search.

The search unit performs the search in the inclination direction in which the rate of decrease in the distance with respect to the current angular acceleration is maximized, and as a result, the decrease in the distance is reduced until the ZMP returns to the inside of the safety area. The fall prevention device according to any one of claims 1 to 4, wherein when it is no longer possible to perform the search, the search is performed in a tilt direction in which the rate of decrease in the distance with respect to the current angular acceleration is the next largest.