JP2017008659A - Ｚｍｐ演算装置およびｚｍｐ演算方法 - Google Patents

Abstract

【課題】簡潔なシステム構成で済みコスト低減を可能とし、三軸加速度センサの設置位置も制限されないＺＭＰ演算装置の提供であり、また、ＺＭＰ演算方法である。
【解決手段】本発明におけるＺＭＰ演算装置１およびＺＭＰ演算方法では、加速度^Ｗａ_Ａ，^Ｗａ_Ｂおよび回転角度θ_１，θ_２，θ_３，θ_４とに基づいてＺＭＰを演算するので、走行可能な一番目のリンクＬ１の三軸の加速度と、一番目以外のリンクＬ３の三軸の加速度と、隣り合うリンク同士の回転角度θ_１，θ_２，θ_３，θ_４を検出するだけでＺＭＰを求められる。よって、第一の三軸加速度検出部Ｇ１と、第二の三軸加速度検出部Ｇ２と、回転角度θ_１，θ_２，θ_３，θ_４を検出する角度検出部Ｓ１，Ｓ２，Ｓ３，Ｓ４とを備えていれば足り、各リンクの全てに三軸の加速度を設ける必要がなくなる。
【選択図】図１

Description

本発明は、ＺＭＰ演算装置およびＺＭＰ演算方法に関する。

土木建設工事等に使用される作業用の走行車両は、クローラを備えて走行可能な走行体と、走行体に旋回可能に取り付けた旋回体と、旋回体に上下方向に揺動可能に連結されたブームと、ブームに上下方向に揺動可能に連結されるアームと、アームの先端に上下方向に揺動可能に連結されたアタッチメントを備えているものが知られている。


このような走行車両にあっては、走行体上に取り付けられた旋回体、ブーム、アームおよびアタッチメントの姿勢を変えながら土木建設工事を行うため、無理な操作を行った場合に走行車両の重心が偏ってバランスを崩して転倒する恐れがある。

近年、転倒防止のため、ゼロモーメントポイント（以下、単に「ＺＭＰ」という）を活用する安全装置が開発されている。この装置は、ＺＭＰが平面視で見た走行体の安全範囲内に収まっているか演算して、転倒の可能性がある場合に走行車両のオペレータへ警告を発するようになっている（たとえば、特許文献１参照）。

ＺＭＰは、物体に作用する慣性力、重力および外力によるモーメントがゼロになる地面上の点のことであり、ＺＭＰが平面視で見た走行体の中心付近に位置する場合、走行車両を転倒させるようなモーメントが生じない。そのため、ＺＭＰが平面視で見た走行体の中心付近に位置する場合、走行車両が転倒しにくくなり、ＺＭＰが平面視で見た走行体の端に寄れば寄るほど走行車両が転倒しやすくなる。したがって、ＺＭＰがどこに位置しているのかを求めれば、走行車両の転倒し易さを評価できるのである。

再表２０１１／１４８９４６号公報

走行体が地面から受ける反力を検出する荷重センサを設けられれば、ＺＭＰを簡単に求められる。また、ＺＭＰは、物体に作用する慣性力、重力および外力によるモーメントがゼロになる地面上の点であるため、慣性力の総和、走行車両の重心位置および外力が分かれば求められる。しかしながら、走行車両が地面から受ける反力を検出する方法は、走行体の地面への設置面に荷重センサを設けるのが事実上困難であって採用できない。

そこで、前述した安全装置にあっては、慣性力を検出して、ＺＭＰを求める方法を採用している。前述した走行体に旋回体、ブーム、アームおよびアタッチメントといった多関節のリンクで構成されるような走行車両にあっては、各リンクに重心位置と、重心位置における加速度を検出する必要がある。

よって、安全装置は、旋回体のワールド座標系に対する機械基準座標系の傾きを検出する姿勢センサと、走行体と旋回体の旋回角度を検出する旋回角度センサと、旋回体に対するブームの回動角度とブームに対するアームの回動角度とアームに対するアタッチメントの回転角度をそれぞれ検出する角度センサと、走行体、旋回体、ブーム、及びアームの重心近傍にそれぞれ設けた三軸加速度センサと、アームとバケットを繋ぐピン等に設けられて外力の大きさと方向を検出する歪ゲージとを備えて、ＺＭＰの演算に必要な情報を収集するようになっている。


このように、従来の安全装置にあっては、ＺＭＰの演算のために、多数のセンサが必要であり、コストが非常に嵩むだけでなく、システム構成が複雑となるといった問題がある。また、三軸加速度センサの設置箇所は、走行体、旋回体、ブーム、及びアームの重心近傍とされているが、作業用の走行車両の構造上、これらの重心近傍に三軸加速度センサを設けるのが困難な場合もある。

そこで、本願発明は、上記した問題を解決するために創案されたものであって、その目的とするところは、簡潔なシステム構成で済みコスト低減を可能とし、三軸加速度センサの設置位置も制限されないＺＭＰ演算装置およびＺＭＰ演算方法の提供である。

上記した目的を解決するために、本発明における課題解決手段におけるＺＭＰ演算装置およびＺＭＰ演算方法では、走行可能な一番目のリンクの三軸の加速度と、一番目以外のリンクの三軸の加速度と、隣り合うリンク同士の回転角度を検出するだけでＺＭＰを求められる。よって、第一の三軸加速度検出部と、第二の三軸加速度検出部と、回転角度を検出する角度検出部とを備えていれば、各リンクの全てに三軸の加速度を設ける必要がない。また、第一の三軸加速度検出部は、走行可能な一番目のリンクの移動による加速度を検出するために使用され、第二の三軸加速度検出部は、検出する三軸の加速度から第一の三軸加速検出部で検出する加速度を差し引きして走行車両の全体の揺動運動による三軸の加速度成分を得るために使用される。よって、第一の三軸加速度検出部は、走行可能な一番目のリンクのどこに設置されてもよく、また、第二の三軸加速度検出部にあっても、一番目のリンク以外のリンクのいずれに設置してもよいし、これらリンクの重心位置に設置されずともよい。

また、請求項２のＺＭＰ演算装置では、ＺＭＰ演算部が第一の三軸加速度検出部により検出される加速度と、第二の三軸加速度検出部により検出される加速度と、角度検出部により検出される回転角度に基づいて、ワールド座標における各リンクの重心位置の三軸の加速度ベクトルをそれぞれ推定し、推定される加速度ベクトルに基づいてＺＭＰを演算する。そのため、各リンクの重心位置の三軸の加速度を得るための加速度センサは不要である。

さらに、請求項３のＺＭＰ演算装置では、ＺＭＰ演算部が一番目のリンクの地面への接触点を含む仮想平面と一番目のリンクの中心から仮想平面へ引いた仮想垂線との交点を走行車両の全体の回転中心としてワールド座標における各リンクの重心位置の三軸の加速度ベクトルを推定するので、回転中心を求めることなく簡易にＺＭＰを求められる。

また、請求項４のＺＭＰ演算装置では、ＺＭＰ演算部が、（数１）の式を用いてワールド座標における各リンクの重心位置の三軸の加速度ベクトルを求める。よって、一番目のリンクの移動による加速度と、一番目のリンク以外のリンクの三軸の加速度と、隣り合うリンク同士の回転角度を得て、簡単な演算を行ってＺＭＰを演算できる。

そしてさらに、請求項５のＺＭＰ演算装置では、ＺＭＰ演算部が、走行車両の全体の地面に対する揺動運動により生じる各リンクの重心位置の三軸の加速度ベクトルを（数２）を用いて求め、ワールド座標における各リンクの重心位置の三軸の加速度ベクトルを求める。よって、一番目のリンクの移動による加速度と、一番目のリンク以外のリンクの三軸の加速度と、隣り合うリンク同士の回転角度を得て、簡単な演算を行ってＺＭＰを演算できる。

したがって、本発明のＺＭＰ演算装置およびＺＭＰ演算方法によれば、簡潔なシステム構成で済みコスト低減が可能となり、三軸加速度センサの設置位置も制限されずに済む。

一実施の形態のＺＭＰ演算装置のシステム構成を示す図である。 一実施の形態のＺＭＰ演算装置を走行車両に適用した状態を示す走行車両の概略図である。 走行車両の回転中心を説明する図である。 一実施の形態のＺＭＰ演算装置のＺＭＰの演算手順を示したフローチャートである。

以下、図に基づいて本発明を説明する。本実施の形態におけるＺＭＰ演算装置１は、図１および図２に示すように、走行車両Ｖに適用されて、走行車両ＶのＺＭＰを演算するようになっている。

本例では、図２に示すように、ＺＭＰ演算装置１は、建設用作業車両を走行車両ＶとしてＺＭＰを演算する。走行車両Ｖは、地面５を走行可能なクローラを備えた走行体Ｌ１を一番目のリンクとし、走行体Ｌ１の上方に旋回可能に連結される旋回体Ｌ２を二番目のリンクとし、旋回体Ｌ２に上下方向に揺動可能に連結されるブームＬ３を三番目のリンクとし、ブームＬ３に上下方向に揺動可能に連結されるアームＬ４を四番目のリンクとし、アームＬ４に上下方向に揺動可能に連結されるアタッチメントＬ５を五番目のリンクとしている。

このように、走行車両Ｖは、走行可能な一番目のリンクである走行体Ｌ１に対して、二番目のリンクである旋回体Ｌ２、三番目のリンクであるブームＬ３、四番目のリンクであるアームＬ４および五番目のリンクであるアタッチメントＬ５が回転自在に直列接続されて構成されている。

なお、アタッチメントＬ５は、図示したバケットのほか、作業の用途に応じてブレーカー、カッター、グラップル等に変更できる。また、図示したところでは、旋回体Ｌ２、ブームＬ３、アームＬ４およびアタッチメントＬ５を備えた多関節のリンク機構とされているが、走行体Ｌ１に取り付けられるリンク機構のリンク数は、２以上であって走行車両Ｖの用途に応じて任意に設定される。たとえば、走行車両Ｖは、クレーン車等とされてもよい。

ＺＭＰ演算装置１は、図１および図２に示すように、走行体Ｌ１の三軸の加速度を検出する第一の三軸加速度検出部としての加速度センサＧ１と、ブームＬ３の三軸の加速度を検出する第二の三軸加速度検出部としての加速度センサＧ２と、走行体Ｌ１に対する旋回体Ｌ２の回転角度θ_１を検出する角度センサＳ１と、旋回体Ｌ２に対するブームＬ３の回転角度θ_２を検出する角度センサＳ２と、ブームＬ３に対するアームＬ４の回転角度θ_３を検出する角度センサＳ３と、アームＬ４に対するアタッチメントＬ５の回転角度θ_４を検出する角度センサＳ４と、これら各センサＧ１，Ｇ２，Ｓ１，Ｓ２，Ｓ３，Ｓ４で検出した加速度および角度θ_１，θ_２，θ_３，θ_４に基づいてＺＭＰを求めるＺＭＰ演算部２とを備えて構成されている。

第一の三軸加速度検出部Ｇ１は、設置個所におけるｘ軸、ｙ軸、ｚ軸の三軸の加速度を検出し、加速度ベクトル^Ｗａ_ＡとしてＺＭＰ演算部２へ出力する。また、第二の三軸加速度検出部Ｇ２もまた、設置個所におけるｘ軸、ｙ軸、ｚ軸の三軸の加速度を検出し、加速度ベクトル^Ｗａ_ＢとしてＺＭＰ演算部２へ出力する。なお、角度センサＳ１，Ｓ２，Ｓ３，Ｓ４は、それぞれ隣り合うリンク同士の回転角度θ_１，θ_２，θ_３，θ_４を検出しており、これらで角度検出部を構成している。

ＺＭＰの演算は、走行体Ｌ１、旋回体Ｌ２、ブームＬ３、アームＬ４およびアタッチメントＬ５のワールド座標系における重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５における重心位置ベクトルと、これらのワールド座標系における重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５の三軸の加速度ベクトルが分かれば求められる。

ワールド座標は、空間全体の座標であり、図２に示すようにワールド座標における原点Ｏ_Ｗは、走行車両Ｖに設定されるローカル座標における原点Ｏとは別に設定される。

走行体Ｌ１、旋回体Ｌ２、ブームＬ３、アームＬ４およびアタッチメントＬ５のワールド座標系における重心位置ベクトル^ＷＰ_Ｇｉは、以下の（数３）の式で表現される。重心位置ベクトル^ＷＰ_Ｇｉの添え字ｉは、リンクの番号を示している。例を挙げれば、たとえば、^ＷＰ_Ｇ１との表現は、一番目のリンクである走行体Ｌ１のワールド座標系における重心位置ベクトルを示し、また、^ＷＰ_Ｇ３との表現は、三番目のリンクであるブームＬ３のワールド座標系における重心位置ベクトルを示す。また、重心位置ベクトル^ＷＰ_Ｇｉの添え字Ｗは、重心位置ベクトル^ＷＰ_Ｇｉがワールド座標系のベクトルである旨を示している。添え字ｉについての表記は、他の数式においても同様にリンクの番号を示し、添え字Ｗも他の数式においても前述と同様の意味を示す。なお、重心位置ベクトル^ＷＰ_Ｇｉは、前述の角度θ_１，θ_２，θ_３，θ_４から求められる。

走行体Ｌ１、旋回体Ｌ２、ブームＬ３、アームＬ４およびアタッチメントＬ５のワールド座標系における重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５の三軸の加速度ベクトル^Ｗａ_Ｇｉは、以下の（数４）の式で表現される。

ＺＭＰの演算は、重心位置ベクトル^ＷＰ_Ｇｉと加速度ベクトル^Ｗａ_Ｇｉを以下の（数５）の式を代入すれば求められる。なお、^Ｗｘ_ＺＭＰは、ＺＭＰのｘ座標を示し、^Ｗｙ_ＺＭＰは、ＺＭＰのｙ座標を示している。（数５）の式では、Ｎ個のリンクを備えた走行車両のＺＭＰを求める演算式となっており、本実施の形態にあっては、走行車両Ｖが５個のリンクで構成されているので、Ｎ＝５として演算すればよい。また、Ｍｉは、ｉ番目のリンクの質量を示している。

ここで、走行体Ｌ１、旋回体Ｌ２、ブームＬ３、アームＬ４およびアタッチメントＬ５のワールド座標系における重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５の加速度ベクトル^Ｗａ_Ｇｉを得るには、各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５の重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５の三軸の加速度の検出を要する。

従来では、これらの検出に各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５へ三軸の加速度センサを設置していたが、本発明では、各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５の重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５の加速度ベクトル^Ｗａ_Ｇｉを推定する。

各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５の重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５の加速度ベクトル^Ｗａ_Ｇｉは、走行体Ｌ１の移動によって発生する加速度ベクトル^Ｗａ_Ａと、走行車両Ｖのローカル座標系における各リンクの重心位置の三軸の加速度ベクトル^Ｏａ_Ｇｉと、地面５に対する走行車両Ｖの全体の揺動運動による各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５の重心位置の三軸の加速度ベクトル^Ｍａ_Ｇｉとにより、以下の（数６）の式を利用して求められる。

ローカル座標は、図２に示すように、原点Ｏをワールド座標とは別に走行車両Ｖに設定した座標である。

走行体Ｌ１の移動によって発生する加速度ベクトルは、加速度センサＧ１によって得られる加速度ベクトル^Ｗａ_Ａであり、これをそのまま数６へ代入すればよい。走行車両Ｖのローカル座標系における各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５の重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５の三軸の加速度ベクトル^Ｏａ_Ｇｉは、以下の（数７）の式で表現される各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５のローカル座標系の重心位置ベクトル^ＯＰ_Ｇｉを二回微分して求められる。

重心位置ベクトル^ＯＰ_Ｇｉの添え字Ｏは、重心位置ベクトル^ＷＰ_Ｇｉがローカル座標系のベクトルである旨を示している。

なお、ローカル座標系の原点Ｏは、図３に示すように、走行体Ｌ１の地面５へ接触する全ての接触点を含む仮想平面ＶＧと走行体Ｌ１の中心Ｃから仮想平面ＶＧへ引いた仮想垂線ＰＬとの交点とされている。走行体Ｌ１、旋回体Ｌ２、ブームＬ３、アームＬ４およびアタッチメントＬ５の単体の重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５は予め分かっている。よって、このようにローカル座標系の原点Ｏを設定すると、角度センサＳ１，Ｓ２，Ｓ３，Ｓ４で検出する角度θ_１，θ_２，θ_３，θ_４から走行体Ｌ１、旋回体Ｌ２、ブームＬ３、アームＬ４およびアタッチメントＬ５のローカル座標系における重心位置ベクトル^ＯＰ_Ｇｉを求められる。

地面５に対する走行車両Ｖの全体の揺動運動による各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５の重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５の加速度ベクトル^Ｍａ_Ｇｉは、以下の（数８）の式を利用して求められる。なお、式中のｓｇｎは、符号関数を示している。

（数８）中の^ＯＰ_Ｂは、以下の（数９）の式で示されるように、加速度センサＧ２の設置位置の位置ベクトルであるので、角度θ_１，θ_２を用いて演算できる。

また、（数１０）の式で示すように、加速度センサＧ２で得られる加速度ベクトル^Ｗａ_Ｂから加速度センサＧ１で得られる加速度ベクトル^Ｗａ_Ａを差し引くと、加速度センサＧ２の設置位置における加速度ベクトル^Ｍａ_Ｂが求められる。^Ｍａ_Ｂは、地面５に対する走行車両Ｖの全体の揺動運動による加速度センサＧ２の取付位置における加速度成分を示しており、（数８）に代入される。つまり、^Ｍａ_Ｂは、加速度センサＧ２の設置位置における走行車両Ｖの全体を原点Ｏ周りに回転させる加速度成分を示している。

なお、加速度ベクトル^Ｍａ_Ｂは、（数１１）の式で示すように表現される。

こうして求められた^ＯＰ_Ｂの二回微分した結果と、加速度ベクトル^Ｍａ_Ｂの三軸の値を（数８）の式に代入すると、地面５に対する走行車両Ｖの全体の揺動運動による各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５の重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５の加速度ベクトル^Ｍａ_Ｇｉが求められる。


そして、前述した演算を行って、加速度^Ｗａ_Ａと、加速度ベクトル^Ｍａ_Ｇｉと、各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５の重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５の加速度ベクトル^Ｏａ_Ｇｉを求めて、これらを（数６）の式に代入すれば、各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５の重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５の加速度ベクトル^Ｗａ_Ｇｉが求められる。

このように、ＺＭＰ演算装置１は、ＺＭＰ演算部２が各センサＧ１，Ｇ２，Ｓ１，Ｓ２，Ｓ３，Ｓ４で検出した加速度^Ｗａ_Ａ，^Ｗａ_Ｂおよび角度θ_１，θ_２，θ_３，θ_４を（数６）から（数１０）の式に代入し演算処理を行い、加速度ベクトル^Ｗａ_Ｇｉを推定する。

そして、ＺＭＰ演算部２は、推定した加速度ベクトル^Ｗａ_Ｇｉを（数５）に代入して演算し、ＺＭＰを求める。整理すると、ＺＭＰ演算装置１は、図４に示すように、一番目のリンクである走行体Ｌ１の三軸の加速度（加速度ベクトル^Ｗａ_Ａ）と、三番目のリンクであるブームＬ３の三軸の加速度（加速度ベクトル^Ｗａ_Ｂ）と、隣り合うリンク同士の回転角度θ_１，θ_２，θ_３，θ_４をそれぞれ検出し（ステップＦ１）、つづいて、得られた加速度ベクトル^Ｗａ_Ａ，^Ｗａ_Ｂおよび角度θ_１，θ_２，θ_３，θ_４から各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５の重心位置の三軸の加速度ベクトル^Ｗａ_Ｇｉを推定し（ステップＦ２）、加速度ベクトル^Ｗａ_Ｇｉを用いてＺＭＰを求める（ステップＦ３）。

このように本発明のＺＭＰ演算装置１およびＺＭＰ演算方法では、加速度^Ｗａ_Ａ，^Ｗａ_Ｂおよび回転角度θ_１，θ_２，θ_３，θ_４とに基づいてＺＭＰを演算するので、走行可能な一番目のリンクＬ１の三軸の加速度と、一番目以外のリンクＬ３の三軸の加速度と、隣り合うリンク同士の回転角度θ_１，θ_２，θ_３，θ_４を検出するだけでＺＭＰを求められる。よって、ＺＭＰ演算装置１では、第一の三軸加速度検出部Ｇ１と、第二の三軸加速度検出部Ｇ２と、回転角度θ_１，θ_２，θ_３，θ_４を検出する角度検出部Ｓ１，Ｓ２，Ｓ３，Ｓ４とを備えていれば、各リンクの全てに三軸の加速度を設ける必要がない。よって、本発明のＺＭＰ演算装置１によれば、センサ数が従来システムより少なく済むので、システム構成が簡潔となってコスト低減が可能となる。

また、第一の三軸加速度検出部Ｇ１は、走行可能な一番目のリンクである走行体Ｌ１の移動による加速度を検出するために使用される。第二の三軸加速度検出部Ｇ２は、検出する三軸の加速度から第一の三軸加速検出部Ｇ１で検出する加速度を差し引きして走行車両Ｖの機体全体の揺動運動による三軸の加速度成分を得るために使用される。よって、第一の三軸加速度検出部Ｇ１は、走行可能な一番目のリンクのどこに設置されてもよく、また、第二の三軸加速度検出部Ｇ２にあっても、一番目のリンクＬ１以外のリンクＬ２，Ｌ３，Ｌ４，Ｌ５のいずれに設置してもよいし、これらリンクＬ２，Ｌ３，Ｌ４，Ｌ５の重心位置に設置されずともよい。よって、本発明のＺＭＰ演算装置１にあっては、三軸加速度センサの設置位置も制限されない。なお、一番目のリンクＬ１に連結されるリンクを含め、総リンク数がＮ個（Ｎは、３以上の整数）である場合、角度検出部はＮ−１の角度センサを備えていれば足り、角度検出部からの回転角度と第一および第二の三軸加速度検出部Ｇ１，Ｇ２からの三軸加速度に基づいてＺＭＰを演算できる。


また、本発明のＺＭＰ演算装置１では、ＺＭＰ演算部２が第一の三軸加速度検出部Ｇ１により検出される三軸の加速度と、第二の三軸加速度検出部Ｇ２により検出される三軸の加速度と、角度検出部Ｓ１，Ｓ２，Ｓ３，Ｓ４により検出される回転角度θ_１，θ_２，θ_３，θ_４に基づいて、ワールド座標における各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５の重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５の三軸の加速度ベクトル^Ｗａ_Ｇｉをそれぞれ推定し、推定される加速度ベクトル^Ｗａ_Ｇｉに基づいてＺＭＰを演算するようになっている。

そのため、各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５の重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５の三軸の加速度を得るための加速度センサは不要である。

さらに、本発明のＺＭＰ演算装置１では、ＺＭＰ演算部２が一番目のリンクＬ１の地面への接触点を含む仮想平面ＶＧと一番目のリンクＬ１の中心から仮想平面ＶＧへ引いた仮想垂線ＰＬとの交点を走行車両Ｖの全体の回転中心としてワールド座標における各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５の重心位置の三軸の加速度ベクトル^Ｗａ_Ｇｉを推定するので、回転中心を求めることなく簡易にＺＭＰを求められる。なお、第一および第二の三軸加速度検出部Ｇ１，Ｇ２にもう一つ加速度検出部を設けて、走行車両Ｖの回転中心を都度、演算によって求め、加速度ベクトル^Ｗａ_Ｇｉを推定してもよい。

また、本発明のＺＭＰ演算装置１では、ＺＭＰ演算部２が、前述の（数６）の式を用いてワールド座標における各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５の重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５の加速度ベクトル^Ｗａ_Ｇｉを求める。よって、一番目のリンクＬ１の移動による加速度ベクトル^Ｗａ_Ａと、一番目のリンクＬ１以外のリンクの加速度ベクトル^Ｗａ_Ｂと、隣り合うリンク同士の回転角度θ_１，θ_２，θ_３，θ_４を得て、（数６）の簡単な演算を行ってＺＭＰを演算できる。

そしてさらに、本発明のＺＭＰ演算装置１では、ＺＭＰ演算部２が、走行車両Ｖの全体の地面に対する揺動運動により生じる各リンクＬ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５の重心位置ＬＧ１，ＬＧ２，ＬＧ３，ＬＧ４，ＬＧ５の加速度ベクトル^Ｍａ_Ｇｉを（数８）を用いて求め、加速度ベクトル^Ｗａ_Ｇｉを求める。

よって、一番目のリンクＬ１の移動による加速度ベクトル^Ｗａ_Ａと、一番目のリンクＬ１以外のリンクの加速度ベクトル^Ｗａ_Ｂと、隣り合うリンク同士の回転角度θ_１，θ_２，θ_３，θ_４を得て、（数８）の簡単な演算を行ってＺＭＰを演算できる。

以上で、本発明の実施の形態についての説明を終えるが、本発明の範囲は図示されまたは説明された詳細そのものには限定されないことは勿論である。

１・・・ＺＭＰ演算装置、２・・・ＺＭＰ演算部、Ｇ１・・・加速度センサ（第一の三軸加速度検出部）、Ｇ２・・・加速度センサ（第二の三軸加速度検出部）、Ｓ１，Ｓ２，Ｓ３，Ｓ４・・・角度センサ（角度検出部）、Ｌ１・・・走行体（一番目のリンク）、Ｌ２・・・旋回体（リンク）、Ｌ３・・・ブーム（リンク）、Ｌ４・・・アーム（リンク）、Ｌ５・・・アタッチメント（リンク）、ＰＬ・・・仮想垂線、Ｖ・・・走行車両、ＶＧ・・・仮想平面

Claims (6)

1. 走行可能な一番目のリンクと前記一番目のリンク上に回転可能に直列接続される複数のリンクとを有する走行車両における前記一番目のリンクの三軸の加速度を検出する第一の三軸加速度検出部と、
   前記リンクのうち一番目以外の任意のリンクの三軸の加速度を検出する第二の三軸加速度検出部と、
   隣り合うリンク同士の回転角度を検出する角度検出部と、
   前記第一の三軸加速度検出部により検出される三軸の加速度と、前記第二の三軸加速度検出部により検出される三軸の加速度と、前記角度検出部により検出される回転角度とに基づいてゼロモーメントポイントを演算するＺＭＰ演算部と
   を備えたことを特徴とする走行車両のＺＭＰ演算装置。
2. 前記ＺＭＰ演算部は、前記第一の三軸加速度検出部により検出される三軸の加速度と、前記第二の三軸加速度検出部により検出される三軸の加速度と、前記角度検出部により検出される回転角度に基づいて、ワールド座標における前記各リンクの重心位置の加速度ベクトルをそれぞれ推定し、推定される前記加速度ベクトルに基づいて前記ゼロモーメントポイントを演算する
   ことを特徴とする請求項１に記載の走行車両のＺＭＰ演算装置。
3. 前記ＺＭＰ演算部は、前記一番目のリンクの地面への接触点を含む仮想平面と前記一番目のリンクの中心から前記仮想平面へ引いた仮想垂線との交点を前記走行車両の全体の回転中心としてワールド座標における前記各リンクの重心位置の加速度ベクトルを推定する
   ことを特徴とする請求項２に記載の走行車両のＺＭＰ演算装置。
4. 前記ＺＭＰ演算部は、前記第一の三軸加速度検出部により検出される加速度ベクトルを^Ｗａ_Ａとし、前記走行車両のローカル座標系におけるｉ番目のリンクの重心位置の加速度ベクトル^Ｏａ_Ｇｉとし、前記走行車両の全体の地面に対する揺動運動により生じるｉ番目のリンクの重心位置の加速度ベクトルを^Ｍａ_Ｇｉとして、下記（数１）によってワールド座標における前記各リンクの重心位置の加速度ベクトルを求める
   ことを特徴とする請求項２または３に記載の走行車両のＺＭＰ演算装置。
   

   
5. 前記ＺＭＰ演算部は、ｉ番目のリンクのローカル座標系の重心位置ベクトルを^ＯＰ_Ｇｉとし、第二の三軸加速度検出部の設置位置の位置ベクトルを^ＯＰ_Ｂとして、前記走行車両の全体の地面に対する揺動運動により生じる前記各リンクの重心位置の加速度ベクトル^Ｍａ_Ｇｉを下記（数２）を用いて求め、前記加速度ベクトル^Ｍａ_Ｇｉを用いて、ワールド座標における前記各リンクの重心位置の三軸の加速度ベクトルをそれぞれ推定する
   ことを特徴とする請求項４に記載の走行車両のＺＭＰ演算装置。
   

   
6. 走行可能な一番目のリンクと前記一番目のリンク上に回転可能に直列接続される複数のリンクとを有する走行車両における前記一番目のリンクの三軸の加速度と、前記リンクのうち一番目以外の任意のリンクの三軸の加速度と、隣り合うリンク同士の回転角度とを検出する検出ステップと、
   前記一番目のリンクの三軸の加速度と、前記リンクのうち一番目以外の任意のリンクの三軸の加速度と、隣り合うリンク同士の回転角度とに基づいてゼロモーメントポイントを演算する演算ステップと
   を備えたことを特徴とする走行車両のＺＭＰ演算方法。

Images