JP6454856B2 - 姿勢検出装置及び姿勢検出方法 - Google Patents

Description

本発明は、吊り下げられた移動体の姿勢検出装置及び姿勢検出方法に関する。

例えば、クレーンを用いた吊荷作業において、吊荷に発生する揺れ等を短時間で減衰させるために、吊荷の姿勢を検出する技術が提案されている。
特許文献１には、自動運転されるクレーンにおいて、吊具の旋回装置で吊荷を旋回する際、旋回に起因する吊荷の捩れ運動を短時間で減衰させる技術が、記載されている。
特許文献１では、クレーンは、建屋の両側の壁に沿って設けられたランウェイ上を移動可能な桁状のクレーンガーダと、クレーンガーダ上を横行するクラブと、クラブに備えられた巻き上げドラムと、ドラムのワイヤロープによって吊り下げられる吊具とを、備えている。

吊具は、ワイヤロープによって直接吊り下げられるフックシーブと、フックシーブと吊具との間に設けられる旋回装置とを介して、吊り下げられる。
旋回装置は、吊具を、上下方向の軸を中心軸として旋回させることができる。
フックシーブの上方の中心に、ジャイロセンサが設けられ、ジャイロセンサの検出結果を用いて、クラブに対するフックシーブの捻れ角度が、検出される。さらに、吊荷及び吊具それぞれの慣性モーメントと、吊荷を吊り下げるワイヤロープの長さ等からも、上記捻れ角度が推定される。
検出された捻れ角度と推定された捻れ角度とを併用して、旋回装置による吊具の旋回時における吊荷の捩れ運動を減衰させる制御が行われる。

特開平８−３１９０８４号公報

クレーンガーダが移動する際、及び、クラブが移動する際にも、吊荷は揺動し、この揺動には、捻れ運動つまりヨー動作以外のロール動作及びピッチング動作が含まれる。このため、吊荷の揺動の早期減衰のためには、吊荷つまり吊具のヨー方向、ロール方向及びピッチング方向の姿勢を含んだ高い精度の姿勢検出が必要になる。

本発明はこのような問題点を解決するためになされたものであり、吊り下げられた移動体の姿勢の検出精度の向上を図る姿勢検出装置及び姿勢検出方法を提供することを目的とする。

上記の課題を解決するために、本発明に係る姿勢検出装置は、移動可能な吊上げ装置から吊り下がる移動体の姿勢を検出する姿勢検出装置において、移動体に設けられ、直交する３軸に沿った直線加速度を検知する加速度センサと、移動体に設けられ、直交する３軸まわりの角速度を検知する角速度センサと、加速度センサ及び角速度センサから検知データを受け取るように構成され、移動体の姿勢を演算する演算部とを備え、演算部は、吊上げ装置による移動体の移動速度を用いた補正を伴って、加速度センサからの検知直線加速度データを用いて移動体の直線速度を算出し、直線速度と移動速度との差異である差異速度と、吊上げ装置からの移動体の吊り下げ長に基づく移動体の固有振動数とを用いて、角速度センサからの検知角速度データを補正して、移動体の姿勢角を算出する。

演算部は、角速度センサからの検知角速度データの補正では、差異速度の角速度成分に移動体の固有振動数に関連する要素を組み合わせたものを、角速度センサからの検知角速度データから除去してよい。
演算部は、直線速度の算出では、差異速度に移動体の固有振動数に関連する要素を組み合わせたものを、検知直線加速度データを時間積分して得られる速度から除去してよい。

本発明に係る姿勢検出方法は、移動可能な吊上げ装置から吊り下がる移動体の姿勢を検出する姿勢検出方法において、直交する３軸に沿った移動体の直線加速度データと、直交する３軸まわりの移動体の角速度データと、吊上げ装置による移動体の移動速度と、吊上げ装置からの移動体の吊り下げ長とを取得し、移動速度を用いた補正を伴って、直線加速度データを用いて直線速度を算出し、直線速度と移動速度との差異である差異速度と、吊り下げ長に基づく移動体の固有振動数とを用いて、角速度データを補正して、移動体の姿勢角を算出する。

角速度データの補正では、差異速度の角速度成分に移動体の固有振動数に関連する要素を組み合わせたものを、角速度データから除去してよい。
直線速度の算出では、差異速度に移動体の固有振動数に関連する要素を組み合わせたものを、直線加速度データを時間積分して得られる速度から除去してよい。

本発明に係る姿勢検出装置及び姿勢検出方法によれば、吊り下げられた移動体の姿勢の検出精度の向上が可能になる。

本発明の実施の形態に係る姿勢検出装置の姿勢検出ユニットをフックシープに含むクレーンの概略図である。 本発明の実施の形態に係る姿勢検出装置の姿勢検出ユニットの概略図である。 本発明の実施の形態に係る姿勢検出装置の構成を示すブロック図である。 本発明の実施の形態に係る姿勢検出装置による姿勢角検出動作の流れを示す図である。

以下、本発明の実施の形態に係る姿勢検出装置について添付図面に基づいて説明する。なお、本実施の形態では、姿勢検出装置は、移動体としてのクレーンのフックシープ７の姿勢を検出するものとして、説明する。

図１を参照すると、クレーン１は、天井クレーンを構成している。
クレーン１は、間隔をあけて配置された２つの支持台２それぞれの上に配設された走行レール等のランウェイ３を備えている。支持台２及びランウェイ３は、図１の紙面に対して奥行き方向であり且つ水平方向である方向に平行に延在している。

クレーン１は、２つのランウェイ３によって両端が支持されると共にランウェイ３に沿って走行可能であるクレーンガーダ４を備えている。クレーンガーダ４は、ランウェイ３の延在方向に垂直であり且つ一方のランウェイ３から他方のランウェイ３に向かう水平方向である方向に平行に延在している。
さらに、クレーン１は、ランウェイ３に配設されたトロリ５を備えている。トロリ５は、ランウェイ３上を、ランウェイ３の軸方向であるＸ軸方向に沿って横行することができる。ここで、トロリ５は、吊上げ装置を構成している。

トロリ５には、ワイヤロープ６を巻き取るための巻取ドラム５ａが設けられている。
巻取ドラム５ａから延びるワイヤロープ６の先端は、下方にある吊フック７ａを備えたフックシープ７に通された後、トロリ５の底面５ｂに固定されている。つまり、ワイヤロープ６は、巻取ドラム５ａからフックシープ７へと下方に向かって延在した後、フックシープ７からトロリ５へと上方に向かって延在し、フックシープ７を吊り下げている。
フックシープ７のフック７ａには、吊具８が掛けられている。吊具８は、吊り荷である鋼板、紙等が巻回されたドラム９を吊るためのものである。

トロリ５には、トロリ５の底面５ｂからのフックシープ７の吊り下げ長さを検出するためのフック位置検知装置１０が、設けられている。本実施の形態では、フック位置検知装置１０は、ワイヤロープ６に設けられ、巻取ドラム５ａからのワイヤロープ６の引き出し量を検知する。フック位置検知装置１０は、無線通信を介して、検知結果を、いずれも離れた位置にある姿勢検出ユニット１０１と図３に示すクレーン１の操作部２００とに送ることができるように構成されている。
なお、フック位置検知装置１０は、上記構成に限定されるものでなく、光波、超音波、電磁波等の発信部とその受信部とをトロリ５とフックシープ７とに設けることによって、フックシープ７の吊り下げ長さを検出する構成であってもよく、トロリ５の底面からのフックシープ７の吊り下げ長さを検出可能な構成であれば、いかなるものであっても構わない。

さらに、トロリ５には、クレーンガーダ４の延在方向に沿ったクレーンガーダ４に対するトロリ５の相対速度を検知するトロリ速度検知装置１１が、設けられている。トロリ速度検知装置１１は、無線通信を介して、検知結果を、姿勢検出ユニット１０１と図３に示すクレーン１の操作部２００とに送ることができるように構成されている。
クレーンガーダ４には、ランウェイ３の延在方向に沿ったランウェイ３に対するクレーンガーダ４の相対速度を検知するガーダ速度検知装置１２が、設けられている。ガーダ速度検知装置１２は、無線通信を介して、検知結果を、姿勢検出ユニット１０１と図３に示すクレーン１の操作部２００とに送ることができるように構成されている。

フックシープ７には、フックシープ７の姿勢を検出するための姿勢検出ユニット１０１が、取り付けられている。姿勢検出ユニット１０１は、無線通信を介して、フック位置検知装置１０、トロリ速度検知装置１１及びガーダ速度検知装置１２の検知結果を受け取ると共に、受け取った検知結果及び自身による検出結果を図３に示すクレーン１の操作部２００に送信するように構成されている。

図２及び図３をあわせて参照すると、姿勢検出ユニット１０１、フック位置検知装置１０、トロリ速度検知装置１１及びガーダ速度検知装置１２は、姿勢検出装置１００を構成している。
姿勢検出ユニット１０１は、基板１０２と、ＣＰＵ回路等によって構成される演算部１０３と、３軸ジャイロセンサ１１０と、３軸加速度センサ１２０と、無線通信機１０４とを備えている。演算部１０３、３軸ジャイロセンサ１１０、３軸加速度センサ１２０及び無線通信機１０４は、基板１０２上に実装されている。
本実施の形態では、姿勢検出ユニット１０１は、フックシープ７が静止した状態で吊り下げられているときに基板１０２が水平になるように、フックシープ７に取り付けられている。ここで、３軸ジャイロセンサ１１０及び３軸加速度センサ１２０はそれぞれ、角速度センサ及び加速度センサを構成している。

図１〜図３をあわせて参照すると、３軸ジャイロセンサ１１０は、Ｘ軸周り角速度検知部１１１と、Ｙ軸周り角速度検知部１１２と、Ｚ軸周り角速度検知部１１３とを備えている。
Ｘ軸周り角速度検知部１１１は、Ｘ軸周りの姿勢検出ユニット１０１つまりフックシープ７のロール角の変化速度であるロール角速度を検知する。Ｙ軸周り角速度検知部１１２は、Ｙ軸周りの姿勢検出ユニット１０１つまりフックシープ７のピッチング角の変化速度であるピッチング角速度を検知する。Ｚ軸周り角速度検知部１１３は、Ｚ軸周りの姿勢検出ユニット１０１つまりフックシープ７のヨー角の変化速度であるヨー角速度を検知する。演算部１０３は、３軸ジャイロセンサ１１０が検知するフックシープ７のロール角速度、ピッチング角速度及びヨー角速度の情報を受け取るように構成されている。

なお、図１に示すように、Ｘ軸は、フックシープ７が静止した状態で吊り下げられているときに、クレーンガーダ４の延在方向である軸方向に平行であり且つ基板１０２の表面に沿う水平方向の軸である。Ｙ軸は、フックシープ７が静止した状態で吊り下げられているときに、ランウェイ３の延在方向である軸方向に平行であり且つ基板１０２の表面に沿う水平方向の軸である。Ｚ軸は、フックシープ７が静止した状態で吊り下げられているときに、Ｘ軸及びＹ軸に垂直であるフックシープ７の上下方向に沿い且つ基板１０２の表面に垂直な方向の軸である。Ｘ軸、Ｙ軸及びＺ軸は互いに直交する。Ｘ軸は、図１の紙面上で左方向を正とする。Ｙ軸は、図１の紙面の奥行き方向を正とする。Ｚ軸は、鉛直下方向を正とする。

３軸加速度センサ１２０は、Ｘ軸方向加速度検知部１２１と、Ｙ軸方向加速度検知部１２２と、Ｚ軸方向加速度検知部１２３とを備えている。Ｘ軸方向加速度検知部１２１は、Ｘ軸に沿った姿勢検出ユニット１０１つまりフックシープ７の直線加速度であるＸ軸方向直線加速度を検知する。Ｙ軸方向加速度検知部１２２は、Ｙ軸に沿った姿勢検出ユニット１０１つまりフックシープ７の直線加速度であるＹ軸方向直線加速度を検知する。Ｚ軸方向加速度検知部１２３は、Ｚ軸に沿った姿勢検出ユニット１０１つまりフックシープ７の直線加速度であるＺ軸方向直線加速度を検知する。演算部１０３は、３軸加速度センサ１２０が検知するＸ軸方向直線加速度、Ｙ軸方向直線加速度及びＺ軸方向直線加速度の情報を受け取るように構成されている。

演算部１０３は、無線通信機１０４を用いた無線通信を介して、フック位置検知装置１０、トロリ速度検知装置１１及びガーダ速度検知装置１２からの検知結果と、クレーン１の操作部２００からの指令とを受け取ることができる。さらに、演算部１０３は、無線通信機１０４を用いた無線通信を介して、受け取った検知結果及び演算結果等の情報を操作部２００に送ることができる。

次に、姿勢検出装置１００によるフックシープ７の姿勢検出動作を説明する。
図１〜図４をあわせて参照すると、クレーン１の稼働中、フック位置検知装置１０は、巻取ドラム５ａからのワイヤロープ６の引き出し量を連続的に検知して、検知結果を姿勢検出ユニット１０１の演算部１０３及びクレーン１の操作部２００に送る。トロリ速度検知装置１１及びガーダ速度検知装置１２はそれぞれ、トロリ５及びクレーンガーダ４の移動速度を連続的に検知して、検知結果を演算部１０３及びクレーン１の操作部２００に送る。３軸加速度センサ１２０は、フックシープ７の３軸周りの角速度を連続的に検知し、検知結果を演算部１０３に送る。３軸加速度センサ１２０は、フックシープ７の３軸方向の加速度を連続的に検知し、検知結果を演算部１０３に送る。

演算部１０３は、３軸加速度センサ１２０から連続的に受け取る直線加速度αと、３軸ジャイロセンサ１１０から連続的に受け取る角速度ωと、フック位置検知装置１０から連続的に受け取るワイヤロープ６の引き出し量Ｌｗと、トロリ速度検知装置１１及びガーダ速度検知装置１２それぞれから連続的に受け取るトロリ５及びクレーンガーダ４の速度νＴ及びνＧとを用いて、フックシープ７の姿勢角を連続的に算出する。

フックシープ７の姿勢角を連続的に算出する過程において、演算部１０３は、既に算出したフックシープ７の姿勢角θを用いて、３軸加速度センサ１２０から受け取る直線加速度αを、水平面に平行な直交する２つの軸Ｘ’及びＹ’と水平面に垂直な１つの軸Ｚ’とによる三次元座標系に、座標変換する。

具体的には、直線加速度αは、Ｘ軸、Ｙ軸及びＺ軸成分として、α_Ｘ、α_Ｙ及びα_Ｚを含んでいる。α_Ｘ及びα_Ｙは、基板１０２に平行な軸成分であり、α_Ｚは、基板１０２に垂直な軸成分である。
そして、成分α_Ｘ、α_Ｙ及びα_Ｚと、姿勢角θの成分θ_Ｘ、θ_Ｙ及びθ_Ｚと用いて座標変換することによって、直線加速度αは、成分α_Ｘ’、α_Ｙ’及びα_Ｚ’を有する直線加速度α’となる。
なお、成分θ_Ｘ、θ_Ｙ及びθ_Ｚはそれぞれ、Ｘ軸、Ｙ軸及びＺ軸に対する姿勢角の角度成分である。さらに、成分α_Ｘ’は、Ｘ軸を水平面に投影したものであるＸ’軸の成分であり、成分α_Ｙ’は、Ｘ’軸と同一の水平面上でＸ’軸に垂直なＹ’軸の成分であり、成分α_Ｚ’は、Ｘ’軸及びＹ’軸に垂直である鉛直方向のＺ’軸の成分である。

次いで、演算部１０３は、フックシープ７の直線速度Ｖを求める。
このとき、演算部１０３は、直線加速度α’（α_Ｘ’，α_Ｙ’，α_Ｚ’）に対して、３軸加速度センサ１２０の特性に起因する検出結果の誤差の影響と重力による影響とを排除して、直線加速度を時間積分し、直線速度Ｖ（Ｖ_Ｘ’，Ｖ_Ｙ’，Ｖ_Ｚ’）を算出する。
さらに、演算部１０３は、直線速度Ｖと、この直線速度Ｖの時点に対応する時点でのフックシープ７の実際の移動速度とを比較する。このとき、演算部１０３は、フックシープ７の実際の移動速度として、この直線速度Ｖの時点に対応する時点でトロリ速度検知装置１１及びガーダ速度検知装置１２によって検知される速度νＴ及びνＧを用いる。
なお、フックシープ７のＸ’軸方向の移動速度成分は、νＴであり、Ｙ’軸方向の移動速度成分は、νＧである。トロリ５又はクレーンガーダ４の移動時、巻取ドラム５ａは静止しているため、フックシープ７のＺ’軸方向の移動速度成分は０である。

さらに、演算部１０３は、直線速度Ｖと、この直線速度Ｖの時点に対応する時点でのフックシープ７の移動速度νＴ及びνＧとの差異である差異速度Ｖｄ’（Ｖｄ_Ｘ’，Ｖｄ_Ｙ’，Ｖｄ_Ｚ’）を求める。差異速度Ｖｄ’（Ｖｄ_Ｘ’，Ｖｄ_Ｙ’，Ｖｄ_Ｚ’）は、以下のとおりとなる。
Ｖｄ_Ｘ’＝Ｖ_Ｘ’−νＴ
Ｖｄ_Ｙ’＝Ｖ_Ｙ’−νＧ
Ｖｄ_Ｚ’＝Ｖ_Ｚ’−０

そして、演算部１０３は、次にフックシープ７の直線速度Ｖを算出する際に、上記で算出した差異速度Ｖｄ’（Ｖｄ_Ｘ’，Ｖｄ_Ｙ’，Ｖｄ_Ｚ’）と重力加速度ｇとを用いた補正速度計算を行う。つまり、演算部１０３は、３軸加速度センサ１２０の特性に起因する検出結果の誤差の影響と重力による影響とを排除する補正計算を行う。
このとき、演算部１０３は、直線加速度α’（α_Ｘ’，α_Ｙ’，α_Ｚ’）と重力加速度ｇと用いた加速度α’ｇ（α_Ｘ’，α_Ｙ’，α_Ｚ’＋ｇ）を時間積分して補正前直線速度Ｖ１（Ｖ１_Ｘ’，Ｖ１_Ｙ’，Ｖ１_Ｚ’）を算出する。これにより、重力による影響が排除される。
さらに、演算部１０３は、補正前直線速度Ｖ１に対して、以下の計算式による補正計算を行って、直線速度Ｖを求める。これにより、３軸加速度センサ１２０の特性に起因する検出結果の誤差による影響が排除される。
Ｖ＝Ｖ１−Ｖｄ’×Ｋ１

Ｋ１は、図１に示すトロリ５の底面からのフックシープ７の吊り下げ長さＬ等に応じて値が変わるレベリング定数である。つまり、定数Ｋ１は、長さＬに対応したフックシープ７の固有振動数ｆ_０、３軸加速度センサ１２０の特性、３軸ジャイロセンサ１１０の特性、姿勢検出ユニット１０１の取付位置等に応じて設定される定数である。演算部１０３は、ワイヤロープ６の引き出し量Ｌｗからフックシープ７の吊り下げ長さＬを算出し、定数Ｋ１を算出するが、その詳細は、後述する。
よって、直線速度Ｖでは、３軸加速度センサ１２０の特性に起因する検出結果の誤差による影響に、フックシープ７を吊るワイヤロープ６がフックシープ７の動作に与える影響を加味した影響が、排除されることになる。

そして、演算部１０３は、上述のようにして算出した直線速度Ｖと、この直線速度Ｖの時点に対応する時点でのフックシープ７の実際の移動速度との差異速度の算出を繰り返し、この差異速度を用いてさらなる直線速度の算出を繰り返す。

また、演算部１０３は、差異速度Ｖｄ’をＸ軸、Ｙ軸及びＺ軸からなる座標系に座標変換して、差異速度Ｖｄ（Ｖｄ_Ｘ，Ｖｄ_Ｙ，Ｖｄ_Ｚ）を求め、さらに、差異速度ＶｄからＸ軸周り、Ｙ軸周り及びＺ軸周りの角速度成分ωｄ_Ｘ，ωｄ_Ｙ，ωｄ_Ｚからなる差異角速度ωｄを算出する。差異角速度ωｄ（ωｄ_Ｘ，ωｄ_Ｙ，ωｄ_Ｚ）は、以下のとおりとなる。なお、ｒは、定数であり、３軸ジャイロセンサ１１０の構成に応じて定められる。
ωｄ_Ｘ＝（Ｖｄ_Ｙ ^２＋Ｖｄ_Ｚ ^２）^１／２／ｒ
ωｄ_Ｙ＝（Ｖｄ_Ｘ ^２＋Ｖｄ_Ｚ ^２）^１／２／ｒ
ωｄ_Ｚ＝（Ｖｄ_Ｘ ^２＋Ｖｄ_Ｙ ^２）^１／２／ｒ

次いで、演算部１０３は、３軸ジャイロセンサ１１０から受け取る角速度ω（ω_Ｘ，ω_Ｙ，ω_Ｚ）の各成分から差異角速度ωｄ（ωｄ_Ｘ，ωｄ_Ｙ，ωｄ_Ｚ）を減じる補正角速度計算を行って、補正角速度ωａ（ωａ_Ｘ，ωａ_Ｙ，ωａ_Ｚ）を算出する。補正角速度ωａの各角速度成分は、以下のとおりとなる。
ωａ_Ｘ＝ω_Ｘ−Ｋ２×ωｄ_Ｘ
ωａ_Ｙ＝ω_Ｙ−Ｋ２×ωｄ_Ｙ
ωａ_Ｚ＝ω_Ｚ−Ｋ２×ωｄ_Ｚ

なお、Ｋ２は、図１に示すトロリ５の底面からのフックシープ７の吊り下げ長さＬに対応したフックシープ７の固有振動数ｆ_０に応じて設定される定数である。定数Ｋ２は、以下のように定義される。
ω_０ ^２＝（２πｆ_０）^２＝Ｋ２×ｇ／Ｒ０
なお、Ｒ０は、定数Ｋ１及び定数Ｋ２に依らない不変な定数である。
さらに、定数Ｋ１と定数Ｋ２との間には、以下の関係がある。
ζ＝Ｋ１／（２×ω_０）＝（Ｋ１／２）×｛Ｒ０／（Ｋ２×ｇ）｝^１／２
ζは、３軸ジャイロセンサ１１０の応答周波数と、３軸ジャイロセンサ１１０における入力と出力との比であるゲイン（単位：ｄＢ）との関係における減衰定数であり、３軸ジャイロセンサ１１０の特性に依存する定数である。
このため、演算部１０３は、補正角速度ωａを算出する際、ワイヤロープ６の引き出し量Ｌｗからフックシープ７の吊り下げ長さＬを算出し、定数Ｋ２を算出する。さらに、演算部１０３は、算出した定数Ｋ２と、減衰定数ζとから、定数Ｋ１を算出し、直線速度Ｖの算出に使用する。

よって、補正角速度ωａ（ωａ_Ｘ，ωａ_Ｙ，ωａ_Ｚ）では、３軸加速度センサ１２０の特性に起因する検出結果の誤差による影響に、ワイヤロープ６がフックシープ７の動作に与える影響を加味した影響が、角速度ω（ω_Ｘ，ω_Ｙ，ω_Ｚ）に対して排除されていることになる。

さらに、演算部１０３は、補正角速度ωａ（ωａ_Ｘ，ωａ_Ｙ，ωａ_Ｚ）に対して時間積分を含むクォータニオン積算を実施し、フックシープ７つまり姿勢検出ユニット１０１の姿勢角θのＸ軸、Ｙ軸及びＺ軸に対する方向余弦ｃｏｓθ_Ｘ、ｃｏｓθ_Ｙ及びｃｏｓθ_Ｚを算出する。演算部１０３は、算出した方向余弦ｃｏｓθ_Ｘ、ｃｏｓθ_Ｙ及びｃｏｓθ_Ｚに基づき、Ｘ軸、Ｙ軸及びＺ軸に対する姿勢角θの各角度成分θ_Ｘ、θ_Ｙ及びθ_Ｚを算出して、姿勢変化量及び姿勢角を求める。

演算部１０３は、算出したフックシープ７の姿勢角情報を逐次クレーン１の操作部２００に送る。クレーン１が自動運転の場合、操作部２００の電子制御ユニットは、受信したフックシープ７の姿勢角情報に基づき、フックシープ７の揺動が早期に収束するように、トロリ５及びクレーンガーダ４等の動作を制御する。クレーン１が手動運転の場合、操作者が、受信したフックシープ７の姿勢角情報に基づき、フックシープ７の揺動が早期に収束するように、トロリ５及びクレーンガーダ４等の動作を制御する。

上述で説明したように、本発明の実施の形態に係る姿勢検出装置１００は、クレーン１の移動可能なトロリ５から吊り下がるフックシープ７の姿勢を検出するものであり、フックシープ７に設けられ且つ直交する３軸に沿った直線加速度を検知する３軸加速度センサ１２０と、フックシープ７に設けられ且つ直交する３軸まわりの角速度を検知する３軸ジャイロセンサ１１０と、３軸加速度センサ１２０及び３軸ジャイロセンサ１１０から検知データを受け取るように構成され且つフックシープ７の姿勢を演算する演算部１０３とを備える。演算部１０３は、トロリ５によるフックシープ７の移動速度を用いた補正を伴って、３軸加速度センサ１２０からの検知直線加速度データを用いてフックシープ７の直線速度を算出し、直線速度と移動速度との差異である差異速度と、トロリ５からのフックシープ７の吊り下げ長に基づくフックシープ７の固有振動数とを用いて、３軸ジャイロセンサ１１０からの検知角速度データを補正して、フックシープ７の姿勢角を算出する。

上述の構成において、演算部１０３は、フックシープ７の直線速度を算出する際に、フックシープ７の実際の移動速度を用いた補正を行う。そして、補正して求められた直線速度とフックシープ７の移動速度との差異速度は、３軸加速度センサ１２０の特性に起因する検出誤差を反映するものである。さらに、差異速度とフックシープ７の固有振動数とを用いて補正された３軸ジャイロセンサ１１０の検知角速度データでは、３軸加速度センサ１２０の特性に起因する検出誤差による影響と、フックシープ７が吊り下げられていることによる影響とが、補正されている。よって、フックシープ７の姿勢角の高精度な検出が可能になる。

また、姿勢検出装置１００において、演算部１０３は、３軸加速度センサ１２０からの検知角速度データの補正では、差異速度の角速度成分にフックシープ７の固有振動数に関連する要素を組み合わせたものを、３軸加速度センサ１２０からの検知角速度データから除去する。上述の構成において、検知角速度データの補正では、３軸加速度センサ１２０の特性に起因する検出結果の誤差による影響に、フックシープ７を吊るワイヤロープ６がフックシープ７の動作に与える影響を加味した影響が、除去されることになる。よって、角速度の補正精度が、向上する。

また、姿勢検出装置１００において、演算部１０３は、直線速度の算出では、差異速度にフックシープ７の固有振動数に関連する要素を組み合わせたものを、検知直線加速度データを時間積分して得られる速度から除去する。上述の構成において、検知角速度データの補正では、３軸加速度センサ１２０の特性に起因する検出結果の誤差による影響に、フックシープ７を吊るワイヤロープ６がフックシープ７の動作に与える影響を加味した影響が、除去されることになる。よって、直線速度の算出精度が、向上する。

また、実施の形態に係る姿勢検出装置１００では、演算部１０３は、補正速度計算実施後の直線速度とフックシープ７の実際の移動速度との差異速度を用いて、補正角速度計算を行っていたが、これに限定されるものでない。演算部１０３は、補正前直線速度とフックシープ７の実際の移動速度との差異速度を補正角速度計算に用いてもよい。
また、実施の形態に係る姿勢検出装置１００では、演算部１０３による演算において、フックシープ７の直線速度とフックシープ７の実際の移動速度との差異速度を求める際、フックシープ７の移動速度の鉛直方向成分を０としていたが、これに限定されるものでなく、フックシープ７の移動速度の鉛直方向成分は、０以外であってもよい。

また、実施の形態に係る姿勢検出装置１００では、演算部１０３は、補正角速度計算に関して、３軸ジャイロセンサ１１０から受け取る角速度ωの各成分から差異角速度ωｄを減じたものに対して時間積分を含むクォータニオン積算を実施していたが、これに限定されるものでない。演算部１０３は、角速度ωと差異角速度ωｄとに対して個別に時間積分を含むクォータニオン積算を実施した後に、それぞれの姿勢角の方向余弦からフックシープ７の姿勢角を算出してもよい。

また、実施の形態に係る姿勢検出装置１００では、演算部１０３は、ＸＹＺ軸による座標系とＸ’Ｙ’Ｚ’軸による座標系との間で、座標変換を実施していたが、座標変換を実施せずにＸＹＺ軸による座標系のみを用いて演算してもよい。
また、実施の形態に係る姿勢検出装置１００では、３軸ジャイロセンサ１１０と３軸加速度センサ１２０とは、同じのＸ軸、Ｙ軸及びＺ軸を共有していたが、互いのＸ軸、Ｙ軸及びＺ軸が異なっていてもよい。この場合、演算部１０３は、演算時、３軸ジャイロセンサ１１０及び３軸加速度センサ１２０のうちの一方に対して、検知データの座標変換をしてもよい。
また、実施の形態に係る姿勢検出装置１００の姿勢検出ユニット１０１は、フックシープ７に取り付けられていたが、いかなる吊り下げられる移動体に設けられてもよい。

１ クレーン、５ トロリ（吊上げ装置）、７ フックシープ（移動体）、１００ 姿勢検出装置、１０１ 姿勢検出ユニット、１０３ 演算部、１１０ ３軸ジャイロセンサ（角速度センサ）、１２０ ３軸加速度センサ（加速度センサ）。

Claims (6)

1. 移動可能な吊上げ装置から吊り下がる移動体の姿勢を検出する姿勢検出装置において、
   前記移動体に設けられ、直交する３軸に沿った直線加速度を検知する加速度センサと、
   前記移動体に設けられ、直交する３軸まわりの角速度を検知する角速度センサと、
   前記加速度センサ及び前記角速度センサから検知データを受け取るように構成され、前記移動体の姿勢を演算する演算部と
   を備え、
   前記演算部は、
   前記吊上げ装置による前記移動体の移動速度を用いた補正を伴って、前記加速度センサからの検知直線加速度データを用いて前記移動体の直線速度を算出し、
   前記直線速度と前記移動速度との差異である差異速度と、前記吊上げ装置からの前記移動体の吊り下げ長に基づく前記移動体の固有振動数とを用いて、前記角速度センサからの検知角速度データを補正して、前記移動体の姿勢角を算出する、姿勢検出装置。
2. 前記演算部は、前記角速度センサからの検知角速度データの補正では、前記差異速度の角速度成分に前記移動体の前記固有振動数に関連する要素を組み合わせたものを、前記角速度センサからの検知角速度データから除去する請求項１に記載の姿勢検出装置。
3. 前記演算部は、前記直線速度の算出では、前記差異速度に前記移動体の前記固有振動数に関連する要素を組み合わせたものを、前記検知直線加速度データを時間積分して得られる速度から除去する請求項１または２に記載の姿勢検出装置。
4. 移動可能な吊上げ装置から吊り下がる移動体の姿勢を検出する姿勢検出方法において、
   直交する３軸に沿った前記移動体の直線加速度データと、直交する３軸まわりの前記移動体の角速度データと、前記吊上げ装置による前記移動体の移動速度と、前記吊上げ装置からの前記移動体の吊り下げ長とを取得し、
   前記移動速度を用いた補正を伴って、前記直線加速度データを用いて直線速度を算出し、
   前記直線速度と前記移動速度との差異である差異速度と、前記吊り下げ長に基づく前記移動体の固有振動数とを用いて、前記角速度データを補正して、前記移動体の姿勢角を算出する方法。
5. 前記角速度データの補正では、前記差異速度の角速度成分に前記移動体の前記固有振動数に関連する要素を組み合わせたものを、前記角速度データから除去する請求項４に記載の方法。
6. 前記直線速度の算出では、前記差異速度に前記移動体の前記固有振動数に関連する要素を組み合わせたものを、前記直線加速度データを時間積分して得られる速度から除去する請求項４または５に記載の方法。

Images