(57)【要約】【課題】少ない計算量で建設機械の転倒防止を実現する。【解決手段】ＺＭＰ算出部１１０は、関節角θ、角速度θ’、角加速度θ”、及び傾斜角βに基づいて、建設機械１のＺＭＰを算出する。探索部１１１は、現在のＺＭＰが安全領域Ｄ２の外側に位置する場合、安全領域Ｄ２の境界Ｂ２及び任意のＺＭＰ間の距離Ｌｐを角加速度θ”で示す第１関数Ｆ１において、現在の角加速度に対して距離Ｌｐが減少する割合が最大となる傾斜方向を探索し、探索した傾斜方向に沿って角加速度を変化させることにより、距離Ｌｐが零以下になる角加速度を探索する。【選択図】図３
2018021416.tif 000002
【特許請求の範囲】
【請求項１】
回転軸を中心に回転する可動部を備える建設機械において、前記建設機械の転倒を防止する転倒防止装置であって、
前記可動部を作動させるアクチュエータと、
重力に対する前記建設機械の傾斜角と、前記回転軸に対する前記可動部の角度、角速度、及び角加速度とを取得する取得部と、
前記取得された、傾斜角、角度、角速度、及び角加速度に基づいて、前記建設機械の動的な重心位置であるＺＭＰを算出するＺＭＰ算出部と、
前記建設機械に設定された支持多角形と、前記支持多角形の内側に設定された安全領域との位置情報を記憶する位置情報記憶部と、
現在のＺＭＰが前記安全領域の外側に位置する場合、前記安全領域の境界及び任意のＺＭＰ間の距離を角加速度で示す第１関数において、前記現在の角加速度に対して前記距離が減少する割合が最大となる傾斜方向を探索し、前記探索した傾斜方向に沿って前記角加速度を変化させることにより、前記現在のＺＭＰを前記安全領域の内側に戻す角加速度を探索する探索部と、
前記アクチュエータの制御量が前記探索された角加速度に応じた制御量となるように前記アクチュエータを制御する駆動部とを備える転倒防止装置。
【請求項２】
前記位置情報記憶部は、前記安全領域の内側に設定された前記建設機械の転倒防止制御が行われない非制御領域の位置情報を更に記憶し、
前記探索部は、
前記非制御領域の外側且つ前記安全領域の内側に前記現在のＺＭＰが位置する場合、前記現在のＺＭＰが前記非制御領域から離れるにつれて、前記現在のＺＭＰの前記非制御領域側への移動量が長くなるように目標ＺＭＰを設定し、
前記目標ＺＭＰ及び前記任意のＺＭＰ間の距離を前記角加速度で示す第２関数において、前記現在の角加速度に対して前記距離が減少する割合が最大となる傾斜方向を探索し、前記探索した傾斜方向に沿って前記角加速度を変化させることにより、前記距離を零にする角加速度を探索する請求項１記載の転倒防止装置。
【請求項３】
前記駆動部は、前記第２関数を用いた前記探索によって得られた角加速度に応じた制御量を、前記目標ＺＭＰが前記非制御領域の境界から離れるにつれて増大させる請求項２記載の転倒防止装置。
【請求項４】
前記探索部は、前記現在のＺＭＰと前記現在のＺＭＰの速度とに基づいて、前記ＺＭＰの予測位置を算出し、前記算出した予測位置が前記安全領域の外側に位置する場合、前記第１関数を用いた前記探索を実行する請求項１〜３のいずれかに記載の転倒防止装置。
【請求項５】
前記探索部は、前記現在の角加速度に対して前記距離が減少する割合が最大となる傾斜方向において前記探索を行った結果、前記ＺＭＰが前記安全領域の内側に戻るまでに前記距離の減少が見込めなくなった場合、前記現在の角加速度に対して前記距離が減少する割合が次に大きな傾斜方向において前記探索を実行する請求項１〜４のいずれかに記載の転倒防止装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、建設機械の転倒防止装置に関するものである。
【背景技術】
【０００２】
近年、構造体の地表面との接地部全体に分布する床反力の法線成分がある一点にかかっているとみなしたときの作用点を示すＺＭＰ（Ｚｅｒｏ  Ｍｏｍｅｎｔ  Ｐｏｉｎｔ）を用いてロボットの転倒防止を図る技術が知られている。
【０００３】
特許文献１は、下記の技術を開示する。建設機械の操作レバーが操作状態から停止指令位置まで戻されたとき、シリンダ速度を３次関数で近似して建設機械のＺＭＰを計算し、建設機械が停止するまでの安全性を予測する。そして、転倒の恐れがあることが予測された場合は、レバー入力信号の速度勾配を時間の経過に伴って減少させることにより、転倒防止を試みる。それでも安定性が十分に確保できない場合は、安定性が十分になるまで速度指令値を段階的に減少させる。
【０００４】
特許文献２は、下記の技術を開示する。腕を備えた２足歩行ロボットが特定の作業を達成するために、ロボットの各関節に予め計算された目標軌道（関節角の時系列情報）を与える。目標軌道は、作業中にロボットのＺＭＰが支持多角形を逸脱することのないように設計されている。そして、ロボットに外力が加わり、ロボットのＺＭＰが本来想定されている点からずれると、本来の目標軌道のＺＭＰと、外乱によってずれたＺＭＰとの差を０とするように目標軌道を修正する。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】国際公開第２０１２／１６９５３１号
【特許文献２】特開平１０−２３０４８５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかし、建設機械の転倒は作業中に生じるあらゆる加速度に起因するが、特許文献１は、急停止時しか転倒防止の制御が実行されていない。そのため、特許文献１は、急停止時以外のシーンで建設機械の転倒防止を図ることができず、建設機械の転倒防止を十分に図ることができない。
【０００７】
特許文献２では、計算した目標軌道を環境に応じて更新する手法が開示されているが、この手法は歩行ロボットの制御では有効である。しかし、建設機械は、目標動作が搭乗者のレバー操作によって時々刻々変更されるため、事前に目標軌道を設定することが困難である。よって、特許文献２の手法は、建設機械には適さない。
【０００８】
本発明は、少ない計算量で建設機械の転倒防止を実現できる転倒防止装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
本発明の一態様は、回転軸を中心に回転する可動部を備える建設機械において、前記建設機械の転倒を防止する転倒防止装置であって、
前記可動部を作動させるアクチュエータと、
重力に対する前記建設機械の傾斜角と、前記回転軸に対する前記可動部の角度、角速度、及び角加速度とを取得する取得部と、
前記取得された、傾斜角、角度、角速度、及び角加速度に基づいて、前記建設機械の動的な重心位置であるＺＭＰを算出するＺＭＰ算出部と、
前記建設機械に設定された支持多角形と、前記支持多角形の内側に設定された安全領域との位置情報を記憶する位置情報記憶部と、
現在のＺＭＰが前記安全領域の外側に位置する場合、前記安全領域の境界及び任意のＺＭＰ間の距離を角加速度で示す第１関数において、前記現在の角加速度に対して前記距離が減少する割合が最大となる傾斜方向を探索し、前記探索した傾斜方向に沿って前記角加速度を変化させることにより、前記現在のＺＭＰを前記安全領域の内側に戻す角加速度を探索する探索部と、
前記アクチュエータの制御量が前記探索された角加速度に応じた制御量となるように前記アクチュエータを制御する駆動部とを備える。
【００１０】
本態様によれば、支持多角形の内側に設定された安全領域の外側に現在のＺＭＰが位置する場合、任意のＺＭＰ及び安全領域の境界間の距離を角加速度で示す第１関数において、前記現在のＺＭＰを前記安全領域の内側に戻す角加速度が探索される。これにより、現在のＺＭＰを安全領域の内側に戻す転倒防止制御が実現される。ここで、本態様では、現在の角加速度に対して距離が減少する割合が最大となる第１関数の傾斜方向に沿って角加速度が探索される。そのため、少ない計算量で建設機械の転倒防止を実現できる。
【００１１】
特に、建設機械は搭乗者の操作によって可動部の目標動作が時々刻々変動するので、２足歩行ロボットと比べて可動部の目標動作を予測することが困難である。本態様は、少ない計算量で現在のＺＭＰを安全領域の内側に戻す角加速度が算出できるので、目標動作の予測が困難な建設機械の転倒防止制御に適している。
【００１２】
上記態様において、前記位置情報記憶部は、前記安全領域の内側に設定された前記建設機械の転倒防止制御が行われない非制御領域の位置情報を更に記憶し、
前記探索部は、
前記非制御領域の外側且つ前記安全領域の内側に前記現在のＺＭＰが位置する場合、前記現在のＺＭＰが前記非制御領域から離れるにつれて、前記現在のＺＭＰの前記非制御領域側への移動量が長くなるように目標ＺＭＰを設定し、
前記目標ＺＭＰ及び前記任意のＺＭＰ間の距離を前記角加速度で示す第２関数において、前記現在の角加速度に対して前記距離が減少する割合が最大となる傾斜方向を探索し、前記探索した傾斜方向に沿って前記角加速度を変化させることにより、前記距離を零にする角加速度を探索してもよい。
【００１３】
本態様では、非制御領域の外側且つ安全領域の内側に現在のＺＭＰが位置する場合においても、転倒防止制御を実行する。そのため、転倒防止制御の実行と非実行とが繰り返され、アクチュエータを振動的に駆動させることを防止できると共に、ＺＭＰが安全領域からはみ出したときにアクチュエータの制御量が過大になることを防止できる。しかも、この場合、現在のＺＭＰが非制御領域から離れるにつれて、現在のＺＭＰの非制御領域側への移動量が長くなるように目標ＺＭＰが設定される。そのため、転倒の危険性が高くなるにつれて制御量が増大するように転倒防止制御を実行することができる。更に、ＺＭＰが安全領域に戻った後も、緩やかな転倒防止制御が継続されるため、ＺＭＰが安全領域からはみ出ることを抑制できる。
【００１４】
上記態様において、前記駆動部は、前記第２関数を用いた前記探索によって得られた角加速度に応じた制御量を、前記目標ＺＭＰが前記非制御領域の境界から離れるにつれて増大させてもよい。
【００１５】
本態様によれば、ＺＭＰが非制御領域の外側且つ安全領域の内側に位置する場合、転倒の危険性が低下するにつれてアクチュエータの制御量を低下させることができる。そのため、ＺＭＰが安全領域からはみ出したときに、アクチュエータの制御量が過大になることを防止できる。更に、ＺＭＰが安全領域に戻った後も、緩やかな転倒防止制御が継続されるため、ＺＭＰが安全領域からはみ出ることを抑制できる。
【００１６】
上記態様において、前記探索部は、前記現在のＺＭＰと前記現在のＺＭＰの速度とに基づいて、前記ＺＭＰの予測位置を算出し、前記算出した予測位置が前記安全領域の外側に位置する場合、前記第１関数を用いた前記探索を実行してもよい。
【００１７】
本態様によれば、実際に現在のＺＭＰが安全領域外にはみ出していなくても、ＺＭＰの移動の勢いが大きい場合は事前にＺＭＰを安全領域の内側に戻す転倒防止制御が行われる。そのため、ＺＭＰが安全領域からはみ出て、いざ、ＺＭＰを安全領域に戻そうとしたときに、制御量が過大になり、アクチュエータが飽和することを防止できる。
【００１８】
上記態様において、前記探索部は、前記現在の角加速度に対して前記距離が減少する割合が最大となる傾斜方向において前記探索を行った結果、前記ＺＭＰが前記安全領域の内側に戻るまでに前記距離の減少が見込めなくなった場合、前記現在の角加速度に対して前記距離が減少する割合が次に大きな傾斜方向において前記探索を実行してもよい。
【００１９】
本態様によれば、最初に決定した傾斜方向において探索した結果、距離を零未満にすることができなくなった場合であっても、距離の減少する割合が次に大きな傾斜方向に沿って再探索が行われる。そのため、距離を零未満にする加加速度をより確実に探索できる。
【発明の効果】
【００２０】
本発明によれば、少ない計算量で建設機械の転倒防止を実現できる。
【図面の簡単な説明】
【００２１】
【図１】本発明の実施の形態１における転倒防止装置が適用された建設機械の外観図である。
【図２】ＺＭＰの説明図である。
【図３】図１に示す建設機械に適用された転倒防止装置の構成を示すブロック図である。
【図４】建設機械を上方から下方に見たときの支持多角形と安全領域とを示す図である。
【図５】距離の取り方の一例を示す図である。
【図６】油圧回路の回路図である。
【図７】実施の形態の転倒防止装置の処理を示すフローチャートである。
【図８】探索処理の詳細を示すフローチャートである。
【図９】第１関数の一例を示すグラフである。
【図１０】本発明の実施の形態２に係る転倒防止装置の構成を示すブロック図である。
【図１１】建設機械を上方から下方に見たときの支持多角形と安全領域と非制御領域とを示す図である。
【図１２】探索部の処理を説明する図である。
【図１３】本発明の実施の形態２に係る探索処理の詳細を示すフローチャートである。
【図１４】本発明の実施の形態３に係る転倒防止装置の構成を示すブロック図である。
【図１５】本発明の実施の形態３に係る転倒防止装置の処理を示すフローチャートである。
【図１６】ＺＭＰの予測位置の説明図である。
【発明を実施するための形態】
【００２２】
以下、添付図面を参照しながら、本発明の実施の形態について説明する。なお、以下の実施の形態は、本発明を具体化した例であって、本発明の技術的範囲を限定する性格のものではない。
【００２３】
（実施の形態１）
［建設機械］
図１は、本発明の実施の形態１における転倒防止装置が適用された建設機械１の外観図である。建設機械１は、油圧ショベルで構成されているが、これは一例であり、クレーン等の上部旋回体を備える建設機械であればどのような建設機械が採用されてもよい。図１において、＋ｘ方向は建設機械１の前方を示し、−ｘ方向は建設機械１の後方を示す。前方及び後方を総称して前後方向（ｘ方向）と呼ぶ。＋ｚ方向は建設機械１の上方を示し、−ｚ方向は建設機械１の下方を示す。上方と下方とを総称して上下方向（ｚ方向）と呼ぶ。＋ｙ方向は後方から前方に建設機械１を見たときの左方を指し、−ｙ方向は後方から前方に建設機械１を見たときの右方を指す。右方と左方とを総称して左右方向と呼ぶ。
【００２４】
建設機械１は、クローラ式の下部走行体２と、下部走行体２上に旋回可能に設けられた上部旋回体３と、上部旋回体３に取り付けられた作業装置４とを備えている。
【００２５】
作業装置４は、上部旋回体３に対して起伏可能に取り付けられたブーム４１と、ブーム４１の先端部に対して揺動可能に取り付けられたアーム４２と、アーム４２の先端部に対して揺動可能に取り付けられたバケット４３とを備えている。
【００２６】
また、作業装置４は、上部旋回体３に対してブーム４１を起伏させるブームシリンダ５１と、ブーム４１に対してアーム４２を揺動させるアームシリンダ５２と、アーム４２に対してバケット４３を揺動させるバケットシリンダ５３とを備えている。
【００２７】
［ＺＭＰの概要］
図２は、ＺＭＰの説明図である。本実施の形態では建設機械１の全重心位置ｃとＺＭＰ（Ｚｅｒｏ  Ｍｏｍｅｎｔ  Ｐｏｉｎｔ）とを用いて、建設機械１の転倒防止制御を行う。ＺＭＰは、物体が地面から受ける反力の中心位置を示し、動的な重心位置を示す。例えば、図１を参照して作業装置４が左側に加速しているとすると、ＺＭＰは全重心位置ｃの右側に移動する。
【００２８】
図２を参照する。ＺＭＰを用いた転倒防止制御は、ＺＭＰが支持多角形Ｄ１の境界Ｂ１を含まない内側に位置するように作業装置４を制御する。支持多角形Ｄ１は、物体が地面と接している領域を凸に取り囲む多角形である。建設機械１の場合、下部走行体２を構成する左右一対のクローラ２１，２２が地面と接しているので、クローラ２１，２２が地面と接している２つの領域を凸に取り囲んだほぼ矩形状の領域が支持多角形Ｄ１となる。
【００２９】
建設機械１の静的な転倒安定性は、全重心位置ｃが機械の支持多角形Ｄ１の内側に存在することによって満たされる。しかし、外力などにより建設機械１に加速度が加わると、全重心位置ｃが支持多角形Ｄ１内に存在したとしても、ＺＭＰが支持多角形Ｄ１がはみ出している或いは境界Ｂ１に位置していれば、建設機械１は転倒する。例えば、ＺＭＰが点ｐ１に位置すれば、建設機械１は転倒しないが、ＺＭＰが点ｐ２に位置すれば、建設機械１は転倒する。そこで、建設機械１の動特性を考慮に入れた、ＺＭＰによる転倒判定が必要となる。
【００３０】
＜ブロック図＞
図３は、図１に示す建設機械１に適用された転倒防止装置１０の構成を示すブロック図である。転倒防止装置１０は、回転軸を中心に回転する可動部を備える建設機械１において、建設機械１の転倒を防止する装置である。以下、可動部としては、図１に示す作業装置４を構成するブーム４１、アーム４２、及びバケット４３が採用できる。但し、これは一例であり、回転軸を中心に回転する建設機械１の構成部品であればどのような構成部品であっても可動部として適用可能である。例えば、油圧クレーンのジブが可動部とされてもよい。また、バケット４３以外のアタッチメントが可動部とされてもよいし、上部旋回体３が可動部とされてもよい。
【００３１】
転倒防止装置１０は、角度センサ１０１（取得部の一例）、傾斜センサ１０２（取得部の一例）、コントローラ１０３、アクチュエータ１０４、及び油圧回路１０５を備える。
【００３２】
角度センサ１０１は、例えば、ブーム４１の関節角θ１と、アーム４２の関節角θ２とバケット４３の関節角θ３とのそれぞれを検出する３つの角度センサで構成される。関節角θ１は、上部旋回体３に対するブーム４１の起伏量を示す。関節角θ２は、ブーム４１に対するアーム４２の揺動量を示す。関節角θ３は、アーム４２に対するバケット４３の揺動量を示す。以下、関節角θ１〜θ３を総称する場合、関節角θと表す。角度センサ１０１は、ロータリーエンコーダで構成されてもよいし、ポテンショメータで構成されてもよい。
【００３３】
傾斜センサ１０２は、例えば、重力センサで構成され、水平面に対する建設機械１の傾斜量を示す傾斜角βを検出する。
【００３４】
コントローラ１０３は、例えば、ＣＰＵ、ＲＡＭ、及びＲＯＭを備えるコンピュータで構成され、ＺＭＰ算出部１１０、探索部１１１、駆動部１１２、及び位置情報記憶部１１３を備える。
【００３５】
ＺＭＰ算出部１１０は、関節角θ、角速度θ’、角加速度θ”、及び傾斜角βに基づいて、建設機械１のＺＭＰを算出する。ここで、ＺＭＰ算出部１１０は、角度センサ１０１で検出された関節角θを１階微分することで角速度θ’を算出し、関節角θを２階微分することで角加速度θ”を算出すればよい。或いは、ブーム４１、アーム４２、及びバケット４３の角速度を検出する角速度センサが設けられているのであれば、ＺＭＰ算出部１１０は、角速度センサにより検出された角速度を取得すればよい。このことは、角加速度センサが設けられている場合も同じである。
【００３６】
＜ＺＭＰの計算＞
詳細には、ＺＭＰ算出部１１０は、下記の式（１）、（２）を用いてＺＭＰを算出すればよい。
【００３７】
【数１】
2018021416.tif 000003
式（１）、（２）では、図２に示すように、建設機械１が接している地面上にｘ軸、ｙ軸が設定され、地面に対して垂直上方にｚ軸が設定されている。式（１）、（２）において、左辺のｐはＺＭＰを示す。ｐはｘ，ｙ成分のみで構成され、ｚ成分は０である。これは、ＺＭＰは地面上に設定された支持多角形Ｄ１上の点だからである。
【００３８】
全質量Ｍ及び重力加速度ｇ（ｇｘ，ｇｙ，ｇｚ）は定数である。全重心位置ｃ（ｃｘ，ｃｙ，ｃｚ）は建設機械１全体の重心であり、建設機械１の関節角θに依存する変数である。全運動量Ｐ及び全角運動量Ｌは、各可動部の運動量及び角運動量のベクトル和である。全運動量Ｐ及び全角運動量Ｌは関節角θと角速度θ’と角加速度θ”とに依存する変数である。傾斜角βに応じて重力加速度ｇの方向を変えることにより、傾斜地におけるＺＭＰの計算も可能である。式（１）、（２）から、全運動量Ｐ及び全角運動量Ｌを調整することでＺＭＰが目的の場所に配置可能であることが分かる。
【００３９】
このように、ＺＭＰの計算には、建設機械１の全重心位置ｃ、全運動量Ｐ、及び全角運動量Ｌが必要となる。これらの物理量は、建設機械１の関節角θ、角速度θ’、角加速度θ”、及び建設機械の重力加速度ｇに対する傾斜角βから運動学的に求められる。よって、式（１）、（２）は、関節角θ、角速度θ’、及び角加速度θ”の関数となる。
【００４０】
なお、関節角θは関節数に応じた成分を持つベクトルである。図１の建設機械１は関節数が３つなので、関節角θはθ１，θ２，θ３の３成分からなるベクトルで表される。
【００４１】
したがって、関節数がｎ個であれば、関節角θは式（３）で示すｎ成分からなるベクトルで表される。
【００４２】
【数２】
2018021416.tif 000004
＜安全領域＞
前述の通り、ＺＭＰが建設機械１の支持多角形Ｄ１の内側に存在すれば、理論上は建設機械１は転倒しない。しかし、現実にはモデル化誤差や計測誤差によってＺＭＰの計算結果には誤差が生じる。そこで、本実施の形態は、建設機械１の支持多角形Ｄ１内にさらに安全領域Ｄ２を設定し、ＺＭＰが安全領域Ｄ２内にあるかどうかを転倒判定の基準とする。
【００４３】
位置情報記憶部１１３は、建設機械１に設定された支持多角形Ｄ１の位置情報と、支持多角形Ｄ１の内側に設定された安全領域Ｄ２の位置情報とを記憶する。図４は、建設機械１を上方から下方に見たときの支持多角形Ｄ１と安全領域Ｄ２とを示す図である。
【００４４】
図４の例では、安全領域Ｄ２は支持多角形Ｄ１と相似な矩形の形状を持つ。また、図４の例では、安全領域Ｄ２の中心は支持多角形Ｄ１の中心と同じ位置に設定されている。安全領域Ｄ２の境界Ｂ２と支持多角形Ｄ１の境界Ｂ１との間の領域は調整領域Ｄ３である。
【００４５】
ＺＭＰが安全領域Ｄ２内に存在する場合、転倒防止装置１０は転倒防止制御を行わない。ＺＭＰが調整領域Ｄ３内に存在する場合、転倒防止装置１０は、ＺＭＰが安全領域Ｄ２に戻るように転倒防止制御を行う。
【００４６】
支持多角形Ｄ１は、建設機械１のクローラ２１，２２の形状から予め定められた大きさ及び形状が採用できる。支持多角形Ｄ１の位置情報のデータ構造としては、例えば、ｘ，ｙ，ｚの３軸からなる建設機械１の座標系において、ある位置を原点としたときの支持多角形Ｄ１の４つの頂点の座標が採用できる。
【００４７】
安全領域Ｄ２の大きさは、建設機械１の仕様に応じて予め定められた値が採用できる。例えば、安全領域Ｄ２の大きさとしては、ＺＭＰが支持多角形Ｄ１の境界Ｂ１付近に存在する場合であっても、アクチュエータ１０４が飽和することなく、ＺＭＰを安全領域Ｄ２内に戻すことが可能な値が採用できる。
【００４８】
安全領域Ｄ２の位置情報のデータ構造としては、例えば、建設機械１の座標系において、安全領域Ｄ２の４つの頂点の座標が採用できる。
【００４９】
図３に参照を戻す。探索部１１１は、現在のＺＭＰが安全領域Ｄ２の外側に位置する場合、安全領域Ｄ２の境界Ｂ２及び任意のＺＭＰ間の距離Ｌｐを角加速度θ”で示す第１関数Ｆ１を設定する。そして、探索部１１１は、第１関数Ｆ１において、現在の角加速度θ”に対して距離Ｌｐが減少する割合が最大となる傾斜方向を探索し、探索した傾斜方向に沿って角加速度を変化させることにより、距離Ｌｐが零以下になる角加速度を探索する。この探索のアルゴリズムとしては、例えば最急降下法が採用できる。
【００５０】
＜距離Ｌｐの取り方＞
図５は、距離Ｌｐの取り方の一例を示す図である。図５の例では、点ｐは、任意のＺＭＰを示す。図５の例では、安全領域Ｄ２の中心Ｏと点ｐとを繋ぐ直線Ｋ１上における任意のＺＭＰ（点ｐ）と境界Ｂ２との長さが距離Ｌｐとして採用されている。
【００５１】
任意のＺＭＰ（点ｐ）は、上記の式（１）、（２）で表される。式（１）、（２）は、上述したように、関節角θ、角速度θ’、及び角加速度θ”の関数なので、距離Ｌｐは、関節角θ、角速度θ’、及び角加速度θ”の関数となる。
【００５２】
また、探索部１１１は、ＺＭＰが境界Ｂ２の外側に存在する場合、Ｌｐ＞０となるように距離Ｌｐを定める。したがって、探索部１１１は、距離Ｌｐが零未満（Ｌｐ＜０）であれば、ＺＭＰが安全領域Ｄ２の内側に存在するので、建設機械１は安定であると判定する。一方、探索部１１１は、距離Ｌｐが零以上（Ｌｐ≧０）であれば、ＺＭＰが安全領域Ｄ２の外側に存在するので建設機械１は安定でないと判定する。
【００５３】
なお、距離Ｌｐの取り方は、図５に示す取り方に限定されない。例えば、距離Ｌｐは、安全領域Ｄ２の境界Ｂ２から任意のＺＭＰ（点ｐ）までの最短距離が採用されてもよい。
【００５４】
建設機械１のＺＭＰが安全領域Ｄ２からはみ出して調整領域Ｄ３に侵入した場合（Ｌｐ＞０）、又は、ＺＭＰが安全領域Ｄ２の境界Ｂ２上に位置する場合（Ｌｐ＝０）、建設機械１のアクチュエータ１０４の制御量を調整し、ＺＭＰを安全領域Ｄ２内に留めることを考える。
【００５５】
式（１），（２）において、全運動量Ｐ及び全角運動量Ｌは、関節角θ、角速度θ’、及び角加速度θ”に依存する。よって、ある時刻において、センサにより関節角θ及び角速度θ’が既知であれば、角加速度θ”を調節することによりＺＭＰを所望の場所に配置できる。この場合、ある時刻における距離Ｌｐを示す第１関数Ｆ１は角加速度θ”の関数として考えられる。そこで、探索部１１１は、距離Ｌｐを第１関数Ｆ１（θ”）として取り扱う。
【００５６】
詳細には、転倒防止装置１０が角速度センサを備えているのであれば、探索部１１１は、関節角θ及び角速度θ’に角度センサ１０１及び角速度センサの検出値を代入することで、第１関数Ｆ１を角加速度θ”の関数に設定すればよい。また、転倒防止装置１０が角速度センサを備えていないのであれば、探索部１１１は、関節角θに、角度センサ１０１の検出値を代入すると共に、角速度θ’に、一つ前のサンプリング点における角加速度θ”から得られた角速度θ’を代入することで、第１関数Ｆ１を角加速度θ”の関数に設定すればよい。
【００５７】
なお、第１関数Ｆ１を角加速度θ”の関数としたのは、関節角θ、角速度θ’、及び角加速度θ”のうち、角加速度θ”の応答性が高いからである。
【００５８】
図３に参照を戻す。駆動部１１２は、アクチュエータ１０４の制御量が探索部１１１により探索された角加速度θ”になるように、油圧回路１０５を通じてアクチュエータ１０４を制御する。なお、駆動部１１２は、可動部が複数存在するのであれば、複数の可動部のそれぞれを個別に制御する。図１の例では、駆動部１１２は、ブームシリンダ５１、アームシリンダ５２、及びバケットシリンダ５３を個別に制御する。
【００５９】
アクチュエータ１０４は、可動部が複数存在するのであれば、複数の可動部に対応して複数のアクチュエータ１０４が存在する。図１の例では、アクチュエータ１０４は、ブームシリンダ５１、アームシリンダ５２、及びバケットシリンダ５３に対応する３つのアクチュエータ１０４を含む。
【００６０】
油圧回路１０５は、駆動部１１２の制御の下、アクチュエータ１０４を作動させる。
【００６１】
＜油圧回路＞
図６は、油圧回路１０５の回路図である。油圧回路１０５は、方向切替弁６０２、逆比例弁６０３，６０４、操作部６０５、油圧ポンプ６０６、及びタンク６０７を備える。ここでは、ブームシリンダ５１を作動させる油圧回路１０５を例に挙げて説明する。
【００６２】
方向切替弁６０２は、パイロットポート６０２ａ、６０２ｂを有するパイロット切替弁を備える。方向切替弁６０２は、パイロットポート６０２ａにパイロット圧が供給されると、シリンダ６０１のヘッドＨ側にパイロット圧に応じた供給量で作動油を供給し、シリンダ６０１のロッドＲ側の作動油をパイロット圧に応じた排出量でタンク６０７に排出する。一方、方向切替弁６０２は、パイロットポート６０２ｂにパイロット圧が供給されると、シリンダ６０１のロッドＲ側にパイロット圧に応じた供給量で作動油を供給し、シリンダ６０１のヘッドＨ側の作動油をパイロット圧に応じた排出量でタンク６０７に排出する。
【００６３】
操作部６０５は、操作レバー６０５ａを含むリモコン弁で構成され、操作レバー６０５ａの操作量に応じたパイロット圧を出力する。ここで、操作部６０５は、操作レバー６０５ａがブーム４１を第１方向（起立方向）に回転させる方向に操作されると、操作量に応じたパイロット圧をパイロット管路６０８ａを通じてパイロットポート６０２ａに入力する。一方、操作部６０５は、操作レバー６０５ａがブームを第１方向と反対の第２方向（倒伏方向）に回転させる方向に操作されると、操作量に応じたパイロット圧をパイロット管路６０８ｂを通じてパイロットポート６０２ｂに出力する。
【００６４】
シリンダ６０１は、例えば、図１に示すブームシリンダ５１で構成され、ブーム４１を起伏させる。
【００６５】
逆比例弁６０３は、操作部６０５及びパイロットポート６０２ａ間を繋ぐパイロット管路６０８ａに設けられている。逆比例弁６０４は、操作部６０５及びパイロットポート６０２ｂ間を繋ぐパイロット管路６０８ｂに設けられている。
【００６６】
逆比例弁６０３，６０４は、コントローラ１０３からの制御信号に応じて、パイロット圧を減圧させる。
【００６７】
コントローラ１０３の駆動部１１２は、現在の角加速度θ”と探索部１１１で探索された角加速度θ”との偏差が零となるようにシリンダー６０１をフィードバック制御する。ここでは、駆動部１１２は、逆比例弁６０３，６０４の減圧量を調整することで、シリンダー６０１をフィードバック制御する。駆動部１１２は、操作レバー６０５ａが第１方向に操作されていたとすると、逆比例弁６０３を制御し、操作レバー６０５ａが第２方向に操作されていたとすると、逆比例弁６０４を制御すればよい。
【００６８】
＜フローチャート＞
図７は、実施の形態１の転倒防止装置１０の処理を示すフローチャートである。このフローチャートは、例えば、建設機械１がキーオンされている期間、所定のサンプリング周期で実行される。
【００６９】
まず、ＺＭＰ算出部１１０は、関節角θ、角速度θ’、及び角加速度θ”を取得する（Ｓ７０１）。ここで、ＺＭＰ算出部１１０は、角度センサ１０１から関節角θを取得し、関節角θを一階微分することで角速度θ’を取得し、関節角θを二階微分することで角加速度θ”を取得すればよい。
【００７０】
次に、ＺＭＰ算出部１１０は、傾斜センサ１０２から傾斜角βを取得する（Ｓ７０２）。次に、ＺＭＰ算出部１１０は、傾斜角βで式（１）、（２）の重力加速度ｇの方向を修正し、修正後の式（１）、（２）に関節角θ、角速度θ’、及び角加速度θ”を入力することで、現在のＺＭＰを算出する（Ｓ７０３）。
【００７１】
次に、探索部１１１は、現在のＺＭＰが安全領域Ｄ２の内側に存在すれば（Ｓ７０４でＹＥＳ）、建設機械１は転倒防止制御の必要がないと判定し、処理を終了する。一方、探索部１１１は、現在のＺＭＰが安全領域Ｄ２の外側に存在すれば（Ｓ７０４でＮＯ）、転倒防止制御の必要があると判定し、探索処理を実行する（Ｓ７０５）。探索処理の詳細は、図８を用いて後述する。
【００７２】
次に、駆動部１１２は、アクチュエータ１０４の制御量が探索処理の結果得られた角加速度θ”になるようにアクチュエータ１０４を制御し（Ｓ７０６）、処理を終了する。
【００７３】
図８は、探索処理の詳細を示すフローチャートである。まず、探索部１１１は、第１関数Ｆ１において、現在の角加速度θ”に対して距離Ｌｐが減少する割合が最大となる傾斜方向を示す傾斜方向αを算出する（Ｓ８０１）。
【００７４】
図９は、第１関数Ｆ１の一例を示すグラフであり、縦軸は距離Ｌｐを示し、奥行軸は関節角θ１に対応する角加速度θ１”を示し、横軸は関節角θ２に対応する角加速度θ２”を示している。なお、図９では、説明の便宜上、角加速度θ”はθ１”，θ２”の２成分のベクトルで構成されている。但し、これは一例であり、角加速度θ”は、関節数の個数に応じた成分のベクトルで構成される。また、図９では、説明の便宜上、第１関数Ｆ１は、複数の交差する稜線で示されている。
【００７５】
図９において、点Ｐαは現在の角加速度θ”が位置する第１関数Ｆ１上の点を示している。探索部１１１は、第１関数Ｆ１おいて現在の角加速度θ”（θ１”，θ２”）に対応する点Ｐαを設定し、点Ｐαを中心に角加速度を微小変化させながら、距離Ｌｐが減少する割合が最大となる方向を探索することで傾斜方向αを算出すればよい。図９の例では、探索の結果、矢印で示す方向が傾斜方向αとして算出されている。なお、傾斜方向αはα１，α２の２成分のベクトルで構成され、α１は関節角θ１に対応し、α２は関節角θ２に対応する。また、傾斜方向αの大きさは、｜α｜＝１である。
【００７６】
図８に示すＳ８０２では、探索部１１１は、角加速度をθ”＋ｄ・αに設定したときの距離Ｌｐを算出する。変数ｄは微小変数であり、正の実数である。「ｄ・α」は、傾斜方向αを向く大きさが変数ｄのベクトルである。
【００７７】
次に、探索部１１１は、距離Ｌｐが零未満（Ｌｐ＜０）であるか否かを判定する（Ｓ８０３）。距離Ｌｐが零未満であれば（Ｓ８０３でＹＥＳ）、探索部１１１は、ＺＭＰが安全領域Ｄ２内に入ったので、Ｓ８０２で用いた変数ｄを確定変数ｄ＊として確定する（Ｓ８０４）。
【００７８】
次に、探索部１１１は、確定変数ｄ＊における角加速度（θ”＋ｄ＊・α）を探索結果として決定する（Ｓ８０５）。
【００７９】
一方、距離Ｌｐが零未満でなければ（Ｌｐ≧０）（Ｓ８０３でＮＯ）、探索部１１１は、変数ｄの変更の前後で距離Ｌｐが減少する見込みがあるか否かを判定する（Ｓ８０６）。ここで、探索部１１１は、距離Ｌｐの変化量ΔＬｐを求め、ΔＬｐ＜０であれば、距離Ｌｐが減少する見込みがあると判定し（Ｓ８０６でＹＥＳ）、変数ｄを一定量増加させ（Ｓ８０７）、処理をＳ８０２に戻す。一方、ΔＬｐ≧０であれば、距離Ｌｐは減少する見込みがないと判定し（Ｓ８０６でＮＯ）、処理をＳ８０１に戻す。但し、変化量ΔＬｐは、変更前の変数ｄをｄｔ−１、変更後の変数ｄをｄｔとすると、ΔＬｐ＝Ｌｐ（θ”＋ｄｔ・α）−Ｌｐ（θ”＋ｄｔ−１・α）である。
【００８０】
図９を参照して図８の処理を説明する。距離Ｌｐが減少する限り、探索部１１１は、変数ｄを増大させながら、第１関数Ｆ１上において点Ｐαを傾斜方向αの方向に沿ってずらしていく（Ｓ８０２→Ｓ８０３：ＮＯ→Ｓ８０６→Ｓ８０７→Ｓ８０２）。そして、距離Ｌｐが零未満になれば（Ｓ８０３：ＹＥＳ）、探索部１１１は、ＺＭＰを安全領域Ｄ２に戻す角加速度θ”が探索できたと判定する。
【００８１】
一方、点Ｐαを傾斜方向αの方向に沿ってずらしていっても、距離Ｌｐが減少しなくなると、探索部１１１は、これ以上、傾斜方向αに点Ｐαをずらしていっても、ＺＭＰを安全領域Ｄ２に戻す角速度θ”を探索できる見込みが低いと判定する（Ｓ８０６でＮＯ）。そして、探索部１１１は、処理をＳ８０１に戻し、次の傾斜方向αを算出する。この場合、傾斜方向αは前回算出した調整方向の次に距離Ｌｐの減少する割合が大きな方向を傾斜方向αとして決定すればよい。なお、次の傾斜方向αでもΔＬｐ≧０となれば、探索部１１１は、その次に距離Ｌｐの減少する割合が大きい方向を傾斜方向αとして算出し、その傾斜方向αに沿って探索すればよい。
【００８２】
このように、転倒防止装置１０は、支持多角形Ｄ１の内側に設定された安全領域Ｄ２の外側に現在のＺＭＰが位置する場合、任意のＺＭＰ及び安全領域Ｄ２の境界Ｂ２間の距離Ｌｐを角加速度θ”で示す第１関数Ｆ１において、距離Ｌｐが零未満となる角加速度θ”を探索する。これにより、現在のＺＭＰを安全領域Ｄ２の内側に戻す転倒防止制御が実現される。ここで、転倒防止装置１０は、現在の角加速度θ”に対して距離Ｌｐが減少する割合が最大となる第１関数Ｆ１の傾斜方向αに沿って角加速度を探索する。そのため、少ない計算量で建設機械１の転倒防止を実現できる。
【００８３】
特に、建設機械１は搭乗者の操作によって可動部の目標動作が時々刻々変動するので、２足歩行ロボットに比べて可動部の目標動作を予測することが困難である。転倒防止装置１０は、少ない計算量で現在のＺＭＰを安全領域Ｄ２の内側に戻す角加速度θ”が算出できるので、目標動作の予測が困難な建設機械１における転倒防止制御に適している。
【００８４】
（実施の形態２）
図１０は、本発明の実施の形態２に係る転倒防止装置１０Ａの構成を示すブロック図である。転倒防止装置１０Ａは、安全領域Ｄ２の内側に更に非制御領域Ｄ４（図１１）を設ける点を特徴とする。なお、実施の形態２において、実施の形態１と同一の構成要素には同一の符号を付し、説明を省く。
【００８５】
転倒防止装置１０Ａは、探索部１１１Ａ、駆動部１１２Ａ、及び位置情報記憶部１１３Ａが実施の形態１の探索部１１１、駆動部１１２、及び位置情報記憶部１１３と異なる。
【００８６】
位置情報記憶部１１３Ａは、支持多角形Ｄ１の位置情報と、安全領域Ｄ２の位置情報とに加えて、更に、非制御領域Ｄ４の位置情報を記憶する。図１１は、建設機械１を上方から下方に見たときの支持多角形Ｄ１と安全領域Ｄ２と非制御領域Ｄ４とを示す図である。
【００８７】
実施の形態２では、安全領域Ｄ２及び非制御領域Ｄ４は円形である。安全領域Ｄ２及び非制御領域Ｄ４は中心が支持多角形Ｄ１の中心Ｏに設定されている。非制御領域Ｄ４は、コントローラ１０３による転倒防止制御が行われない領域であり、安全領域Ｄ２の内側に設定されている。
【００８８】
本実施の形態では、安全領域Ｄ２及び非制御領域Ｄ４は円形なので、安全領域Ｄ２及び非制御領域Ｄ４の位置情報としては、安全領域Ｄ２及び非制御領域Ｄ４の半径が採用できる。非制御領域Ｄ４の大きさとしては、建設機械１の仕様に応じて予め定められた値が採用できる。
【００８９】
図１０を参照し、探索部１１１Ａは、非制御領域Ｄ４の外側且つ安全領域Ｄ２の内側に現在のＺＭＰが位置する場合、現在のＺＭＰが非制御領域Ｄ４から離れるにつれて、現在のＺＭＰの非制御領域Ｄ４側への移動量が長くなるように目標ＺＭＰを設定する。そして、探索部１１１Ａは、目標ＺＭＰ及び任意のＺＭＰ間の距離Ｌｐを角加速度θ”で示す第２関数Ｆ２を設定する。そして、探索部１１１Ａ、第２関数Ｆ２において、実施の形態１と同様の探索を実行し、距離Ｌｐを零にする角加速度を探索する。
【００９０】
図１２は、探索部１１１Ａの処理を説明する図である。図１２を参照し、点ｐは現在のＺＭＰである。点ｐ（ハット）は目標ＺＭＰである。なお、明細書中では、記載の制約上、記号「∧」を（ハット）と記載する。
【００９１】
この場合、探索部１１１Ａは、現在のＺＭＰ（点ｐ）及び非制御領域Ｄ４の境界Ｂ３間の距離Ｌ１と、現在のＺＭＰ（点ｐ）及び安全領域Ｄ２の境界Ｂ２間の距離Ｌ２とを求める。
【００９２】
次に、探索部１１１Ａは、距離Ｌ１を距離ｌ１と距離ｌ２とで内分した目標ＺＭＰ（点ｐ（ハット））を目標ＺＭＰとして決定する。但し、ｌ１：ｌ２＝Ｌ１：Ｌ２である。つまり、ｐ（ハット）＝Ｌ１・（ｌ１／（ｌ１＋ｌ２））である。したがって、現在のＺＭＰ（点ｐ）が非制御領域Ｄ４から離れるにつれて、現在のＺＭＰ（点ｐ）の非制御領域Ｄ４側への移動量が長くなるように目標ＺＭＰ（点ｐ（ハット））が設定される。
【００９３】
次に、探索部１１１Ａは、目標ＺＭＰ（ｐ（ハット））及び任意のＺＭＰ間の距離を角加速度θ”で示す第２関数Ｆ２を設定する。第１関数Ｆ１は任意のＺＭＰ及び境界Ｂ２間の距離Ｌｐを角加速度θ”で示す関数であった。一方、第２関数Ｆ２は、任意のＺＭＰ及び目標ＺＭＰ（点ｐ（ハット））間の距離Ｌｐを角加速度θ”で示す関数である。図１２において、点ｐを任意のＺＭＰとすると、第２関数Ｆ２では、点ｐ及び目標ＺＭＰ（点ｐ（ハット））間の距離が距離Ｌｐとなる。図１２の例では、安全領域Ｄ２及び非制御領域Ｄ４は同心円であるため、距離Ｌｐは点ｐと目標ＺＭＰ（点ｐ（ハット））との最短距離になる。それ以外は、第２関数Ｆ２は第１関数Ｆ１と同じである。そのため、第２関数Ｆ２も、第１関数Ｆ１と同様、角加速度θ”の関数となる。
【００９４】
駆動部１１２Ａは、第２関数Ｆ２を用いた探索により得られた角加速度θ”を、目標ＺＭＰが非制御領域Ｄ４の境界Ｂ３に近づくにつれて低下させるように、角加速度θ”を修正する。
【００９５】
詳細には、駆動部１１２Ａは、修正後の角加速度θ”をθｂ”、修正前の角加速度θ”をθａ”とすると、下記の式を用いて角加速度θ”を修正する。
【００９６】
θｂ”＝（ｌ１／ｌ１＋ｌ２）・θａ”
＜フローチャート＞
図１３は、本発明の実施の形態２に係る探索処理の詳細を示すフローチャートである。なお、実施の形態２では、メインルーチンは実施の形態１と同様、図７のフローチャートが採用される。
【００９７】
図１３において、図８との相違点は、Ｓ８０３に代えてＳ１３０１が設けられている点と、Ｓ８０５に続いてＳ１３０２が設けられている点にある。それ以外は、図１３の処理は図８と同じである。Ｓ１３０１では、探索部１１１Ａは、距離ＬｐがＬｐ＝０であるか否かを判定し、Ｌｐ＝０であれば（Ｓ１３０１でＹＥＳ）、処理をＳ８０４に進める。一方、探索部１１１Ａは、距離ＬｐがＬｐ＝０でなければ（Ｓ１３０１でＮＯ）、処理をＳ８０６に進める。
【００９８】
このように、実施の形態２では、Ｌｐ＝０、すなわち、現在のＺＭＰを目標ＺＭＰに位置決めするための角加速度θ”が探索される。
【００９９】
Ｓ１３０２では、駆動部１１２Ａは、Ｓ８０５で決定された角加速度θａ”にｌ１／ｌ１＋ｌ２を乗じて角加速度θｂ”を算出し、角加速度θａ”を修正する（Ｓ１３０２）。実施の形態２では、図７のＳ７０６において、駆動部１１２Ａは、アクチュエータ１０４の制御量が角加速度θｂ”となるようにアクチュエータ１０４を制御する。
【０１００】
このように、実施の形態２に係る転倒防止装置１０Ａによれば、非制御領域Ｄ４の外側且つ安全領域Ｄ２の内側に現在のＺＭＰが位置する場合においても、転倒防止制御を実行する。そのため、転倒防止制御の実行と非実行とが繰り返され、アクチュエータを振動的に駆動させることを防止できる共に、ＺＭＰが安全領域Ｄ２からはみ出したときにアクチュエータ１０４の制御量が過大になることを防止できる。しかも、この場合、現在のＺＭＰが非制御領域Ｄ４から離れるにつれて、現在のＺＭＰの非制御領域Ｄ４側への移動量が長くなるように目標ＺＭＰが設定されると共に、目標ＺＭＰにするために必要な角加速度θ”が現在のＺＭＰが非制御領域Ｄ４から離れるにつれて大きくなるように修正される。
【０１０１】
そのため、転倒の危険性が高くなるにつれて制御量が増大するように転倒防止制御を実行することがでる。更に、ＺＭＰが安全領域Ｄ２に戻った後も、緩やかな転倒防止制御が継続されるため、ＺＭＰが安全領域Ｄ２からはみ出ることを抑制できる。
【０１０２】
（実施の形態３）
図１４は、本発明の実施の形態３に係る転倒防止装置１０Ｂの構成を示すブロック図である。転倒防止装置１０Ｂは、実施の形態１の転倒防止装置１０において、ＺＭＰの位置を予測して転倒防止制御を行う点を特徴とする。実施の形態３において実施の形態１と同一の構成要素は同一の符号を付し、説明を省く。
【０１０３】
図１４において、実施の形態１の構成を示す図３との相違点は、探索部１１１に代えて、探索部１１１Ｂが設けられている点にある。それ以外は実施の形態１と同じである。
【０１０４】
探索部１１１Ｂは、現在のＺＭＰと現在のＺＭＰの速度とに基づいて、ＺＭＰの予測位置を算出し、算出した予測位置が安全領域Ｄ２の外側に位置する場合、第１関数Ｆ１を用いた探索を実行する。
【０１０５】
＜フローチャート＞
図１５は、本発明の実施の形態３に係る転倒防止装置１０Ｂの処理を示すフローチャートである。図１５において、図７との相違点は、Ｓ７０３に続いてＳ１６０１，Ｓ１６０２の処理が追加され、Ｓ７０４に代えてＳ１６０３が設けられている点にある。
【０１０６】
Ｓ１６０１では、探索部１１１Ｂは、ＺＭＰの速度を算出する。ここで、探索部１１１Ｂは、例えば、前回算出したＺＭＰと今回算出したＺＭＰとの差をサンプリング周期で割ることで、ＺＭＰの速度を求めればよい。
【０１０７】
次に、探索部１１１Ｂは、ＺＭＰの予測位置を算出する（Ｓ１６０２）。図１６は、ＺＭＰの予測位置の説明図である。図１６の例では、安全領域Ｄ２は、実施の形態２と同様、中心が支持多角形Ｄ１の中心に位置する円である。
【０１０８】
図１６において、点ｐは現在のＺＭＰを示し、ｐ’は現在のＺＭＰの速度を示す。なお、速度ｐ’は、ＺＭＰが２成分で構成されるため、２成分で構成されるベクトルである。Δｔはサンプリング周期である。点ｐ（ハット）は予測位置を示す。探索部１１１Ｂは、下記の式を用いて予測位置（点ｐ（ハット））を算出する。「ｐ’・Δｔ」はＺＭＰの移動の勢いを示す。
【０１０９】
ｐ（ハット）＝ｐ＋ｐ’・Δｔ
探索部１１１Ｂは、予測位置（点ｐ（ハット））が安全領域Ｄ２の内側に位置すれば（Ｓ１６０３でＹＥＳ）、建設機械１は転倒防止制御の必要がないと判定し、処理を終了する。一方、探索部１１１Ｂは、現在のＺＭＰが安全領域Ｄ２の外側に存在すれば（Ｓ１６０３でＮＯ）、転倒防止制御の必要があると判定し、探索処理を実行する（Ｓ７０５）。
【０１１０】
このように、転倒防止装置１０Ｂによれば、現在のＺＭＰの移動の勢いを考慮に入れて、実際に現在のＺＭＰが安全領域Ｄ２外にはみ出していなくても、ＺＭＰの移動の勢いが大きい場合は事前にＺＭＰを安全領域Ｄ２の内側に戻す転倒防止制御が行われる。そのため、ＺＭＰが安全領域Ｄ２からはみ出て、いざ、ＺＭＰを安全領域Ｄ２に戻そうとしたときに、制御量が過大になり、アクチュエータ１０４が飽和することを防止できる。そのため、アクチュエータ１０４の飽和により、安全領域Ｄ２にＺＭＰを速やかに戻すことができなくなることを回避できる。
【０１１１】
（変形例１）
実施の形態１〜３では、駆動部１１２は、アクチュエータ１０４の制御量として角加速度θ”を採用したが、本発明はこれに限定されない。例えば、トルクをアクチュエータ１０４の制御量として算出してもよい。この場合、駆動部１１２は、探索された角加速度θ”に慣性モーメントを乗じてトルクを算出し、そのトルクを出力するようにアクチュエータ１０４を制御すればよい。
【０１１２】
制御量としてトルクを採用した場合、実施の形態２において、トルクは下記の式で修正される。但し、Ｔａは探索により得られた角加速度θａ”に応じたトルクであり、Ｔｂは修正後のトルクである。
【０１１３】
Ｔｂ＝（ｌ１／ｌ１＋ｌ２）・Ｔａ
（変形例２）
実施の形態３では、実施の形態２の内容を考慮しなかったが、実施の形態２の内容を考慮に入れても良い。この場合、転倒防止装置１０Ｂは、予測位置（点ｐ（ハット））が安全領域Ｄ２の内側に位置すれば、実施の形態２の手法をそのまま適用すればよい。
【符号の説明】
【０１１４】
Ｂ１，Ｂ２，Ｂ３  境界
Ｄ１  支持多角形
Ｄ２  安全領域
Ｄ３  調整領域
Ｄ４  非制御領域
Ｆ１  第１関数
Ｆ２  第２関数
ΔＬｐ  変化量
α  傾斜方向
β  傾斜角
θ  関節角
θ’  角速度
θ”  角加速度
１  建設機械
２  下部走行体
３  上部旋回体
４  作業装置
１０，１０Ａ，１０Ｂ  転倒防止装置
１０１  角度センサ
１０２  傾斜センサ
１０３  コントローラ
１０４  アクチュエータ
１１０  ＺＭＰ算出部
１１１，１１１Ａ，１１３Ｂ  探索部
１１２，１１２Ａ  駆動部
１１３，１１３Ａ  位置情報記憶部
【図１】
2018021416.tif 000005
【図２】
2018021416.tif 000006
【図３】
2018021416.tif 000007
【図４】
2018021416.tif 000008
【図５】
2018021416.tif 000009
【図６】
2018021416.tif 000010
【図７】
2018021416.tif 000011
【図８】
2018021416.tif 000012
【図９】
2018021416.tif 000013
【図１０】
2018021416.tif 000014
【図１１】
2018021416.tif 000015
【図１２】
2018021416.tif 000016
【図１３】
2018021416.tif 000017
【図１４】
2018021416.tif 000018
【図１５】
2018021416.tif 000019
【図１６】
2018021416.tif 000020