JP2020197402A - 移動体、センサーモジュール及びセンサーモジュールの較正方法 - Google Patents

Abstract

【課題】センサーモジュールの較正を容易に行うことが可能な移動体を提供すること。【解決手段】所定の軸周りに回動する可動部と、前記可動部又は前記可動部と連動する部位に設けられているセンサーモジュールと、前記可動部及び前記センサーモジュールを制御する制御装置と、を備え、前記制御装置は、前記センサーモジュールが第１の姿勢となるように前記可動部を制御し、かつ、前記センサーモジュールに較正指示を行い、前記センサーモジュールは、慣性センサーと、前記較正指示に応じて、前記慣性センサーの出力信号に基づいて前記センサーモジュールの姿勢を算出し、算出した姿勢と前記第１の姿勢との差に基づいて補正情報を生成する較正部と、前記補正情報に基づいて、前記慣性センサーの出力信号を補正する補正部と、を含む、移動体。【選択図】図３

Description

本発明は、移動体、センサーモジュール及びセンサーモジュールの較正方法に関する。

特許文献１には、レーザー照射器を利用して生成された基準線上の複数の基準点に作業点が位置するように作業機を動作させたとき、複数の基準点での作業点位置を算出し、算出した複数の基準点での作業点の位置が基準線の一次方程式を充足し得ることを利用して、角度変換パラメーター、寸法パラメーター及び直線パラメーターの較正値を算出することにより、複数の角度センサーを較正する技術が記載されている。

特開２０１８−１６８５８４号公報

しかしながら、特許文献１に記載のセンサーの較正手法では、基準線を生成するためにレーザー照射器が必要であるため、例えば、雨天時のようにレーザー光が上手く反射しない場合のように、レーザー照射器を使用することができない環境やレーザー照射器等の測量機を使用可能な人員が存在しないような作業現場ではセンサーの較正の実施が煩雑になってしまう。

本発明に係る移動体の一態様は、
所定の軸周りに回動する可動部と、
前記可動部又は前記可動部と連動する部位に設けられているセンサーモジュールと、
前記可動部及び前記センサーモジュールを制御する制御装置と、を備え、
前記制御装置は、前記センサーモジュールが第１の姿勢となるように前記可動部を制御し、かつ、前記センサーモジュールに較正指示を行い、
前記センサーモジュールは、
慣性センサーと、
前記較正指示に応じて、前記慣性センサーの出力信号に基づいて前記センサーモジュールの姿勢を算出し、算出した姿勢と前記第１の姿勢との差に基づいて補正情報を生成する較正部と、
前記補正情報に基づいて、前記慣性センサーの出力信号を補正する補正部と、を含む。

前記移動体の一態様において、
前記制御装置は、前記第１の姿勢の情報を前記センサーモジュールに送信してもよい。

前記移動体の一態様において、
前記第１の姿勢は、あらかじめ決められた姿勢であってもよい。

前記移動体の一態様において、
前記制御装置は、前記補正部が補正した前記慣性センサーの出力信号に基づいて、前記可動部を制御してもよい。

前記移動体の一態様において、
前記可動部は、ブーム、アーム、バケットのいずれかであってもよい。

本発明に係るセンサーモジュールの一態様は、
所定の軸周りに回動する可動部と、前記可動部を制御する制御装置とを備える移動体の前記可動部又は前記可動部と連動する部位に取り付けられるセンサーモジュールであって、
慣性センサーと、
前記制御装置からの較正指示に応じて、前記慣性センサーの出力信号に基づいて前記センサーモジュールの姿勢を算出し、算出した姿勢と第１の姿勢との差に基づいて補正情報を生成する較正部と、
前記補正情報に基づいて、前記慣性センサーの出力信号を補正する補正部と、を含む。

本発明に係るセンサーモジュールの較正方法の一態様は、
慣性センサーを含み、可動部又は前記可動部と連動する部位に設けられているセンサーモジュールを較正するセンサーモジュールの較正方法であって、
制御装置が、前記センサーモジュールが第１の姿勢となるように前記可動部を制御する工程と、
前記制御装置が、前記センサーモジュールに較正指示を行う工程と、
前記センサーモジュールが、前記較正指示に応じて、前記慣性センサーの出力信号に基づいて前記センサーモジュールの姿勢を算出し、算出した姿勢と前記第１の姿勢との差に基づいて補正情報を生成する工程と、
前記センサーモジュールが、前記補正情報に基づいて、前記慣性センサーの出力信号を補正する工程と、を含む。

本実施形態の移動体１の一例を示す図。 センサーモジュールと制御装置との接続例を示す図。 センサーモジュールの構成例を示す図。 第１実施形態におけるセンサーモジュールの第１の姿勢の一例を示す図。 第１実施形態における制御装置の構成例を示す図。 第１実施形態におけるセンサーモジュールの較正方法の手順の一例を示すフローチャート図。 第２実施形態における制御装置の構成例を示す図。 第２実施形態におけるセンサーモジュールの第１の姿勢の一例を示す図。 第２実施形態におけるセンサーモジュールの較正方法の手順の一例を示すフローチャート図。

以下、本発明の好適な実施形態について図面を用いて詳細に説明する。なお、以下に説明する実施の形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また以下で説明される構成の全てが本発明の必須構成要件であるとは限らない。

１．第１実施形態
１−１．移動体の構成
図１は、本実施形態の移動体１の一例を示す図である。図１では、移動体１として建設機械の一例である油圧ショベルを例示している。

図１に示すように、移動体１は、車体が、下部走行体４２と、下部走行体４２上に旋回可能に搭載された上部旋回体４１とで構成され、上部旋回体４１の前部側に上下方向に回動可能な複数の部材で構成された作業機構５０が設けられている。上部旋回体４１には不
図示の運転席が設けられ、運転席には、作業機構５０を構成する各部材を操作する不図示の操作装置が設けられている。そして、上部旋回体４１には、上部旋回体４１の傾斜角を検出するセンサーモジュール３０が設けられている。

作業機構５０は、複数の部材として、それぞれ所定の軸周りに回動する可動部である、ブーム４３と、アーム４４と、バケット４５とを備えている。また、作業機構５０は、バケットリンク４６と、ブームシリンダー４７と、アームシリンダー４８と、バケットシリンダー４９とを備えている。

ブーム４３は、上部旋回体４１の前部側に俯仰動可能に取付けられている。アーム４４は、ブーム４３の先端側に俯仰動可能に取付けられている。バケットリンク４６は、アーム４４の先端側に回動可能に取付けられている。バケット４５は、アーム４４およびバケットリンク４６の先端側に回動可能に取付けられている。ブームシリンダー４７は、ブーム４３を駆動する。アームシリンダー４８は、アーム４４を駆動する。バケットシリンダー４９、バケット４５を、バケットリンク４６を介して駆動する。

ブーム４３の基端側は、上部旋回体４１に上下方向に回動可能に支持され、ブームシリンダー４７の伸縮によってブーム４３が上部旋回体４１に対して相対的に回転駆動される。そして、ブーム４３には、ブーム４３の動きの状態を検出するセンサーモジュール１０ｃが設けられている。

ブーム４３の先端側には、アーム４４の一端側が回転可能に支持され、アームシリンダー４８の伸縮によってアーム４４がブーム４３に対して相対的に回転駆動される。アーム４４には、アーム４４の動きの状態を検出するセンサーモジュール１０ｂが設けられている。

アーム４４の先端側には、バケットリンク４６とバケット４５とが回動可能に支持されていて、バケットシリンダー４９の伸縮に応じてバケットリンク４６がアーム４４に対して相対的に回転駆動され、それに連動してバケット４５がアーム４４に対して相対的に回転駆動される。そして、バケットリンク４６には、バケットリンク４６の動きの状態を検出するセンサーモジュール１０ａが設けられている。

センサーモジュール１０ａ，１０ｂ，１０ｃは、着脱可能であり、それぞれ、可動部であるバケット４５と連動する部位であるバケットリンク４６、可動部であるアーム４４、可動部であるブーム４３に取り付けられている。そして、センサーモジュール１０ａ，１０ｂ，１０ｃは、それぞれバケットリンク４６、アーム４４、ブーム４３に作用する角速度及び加速度を検出することができる。また、センサーモジュール３０は、上部旋回体４１に作用する角速度及び加速度を検出することができる。なお、本実施形態では、センサーモジュール１０ａは、バケット４５に加わる衝撃によって受ける物理的なストレスを低減させるために、バケット４５ではなくバケットリンク４６に設けられている。

更に、移動体１には、上部旋回体４１の傾斜角や作業機構５０を構成するブーム４３、アーム４４、バケット４５の位置及び姿勢を演算する制御装置２０が設けられている。

制御装置２０は、可動部であるブーム４３、アーム４４、バケット４５、及びセンサーモジュール１０ａ，１０ｂ，１０ｃを制御する。具体的には、制御装置２０は、センサーモジュール１０ａ，１０ｂ，１０ｃの出力信号に基づいて、ブーム４３、アーム４４、バケット４５の位置及び姿勢を演算し、センサーモジュール３０の出力信号に基づいて上部旋回体４１の傾斜角を演算する。そして、制御装置２０は、演算されたブーム４３、アーム４４、バケット４５の位置及び姿勢や、演算された上部旋回体４１の傾斜角に基づいて
、ブーム４３、アーム４４、バケット４５及び上部旋回体４１の動作を制御する。また、演算されたブーム４３、アーム４４、バケット４５の位置及び姿勢や上部旋回体４１の傾斜角は、運転席の不図示のモニター装置の表示に用いられる。

また、制御装置２０は、センサーモジュール１０ａ，１０ｂ，１０ｃに較正指示を行い、後述するセンサーモジュール１０ａ，１０ｂ，１０ｃの較正処理のタイミングを制御する。

図２は、センサーモジュール１０ａ，１０ｂ，１０ｃ，３０と制御装置２０との接続例を示す図である。図２に示すように、センサーモジュール１０ａ，１０ｂ，１０ｃは、直列的に接続され、検出信号を制御装置２０に送信することができる。このように、センサーモジュール１０ａ，１０ｂ，１０ｃを直列接続することにより、可動領域内における検出信号を送信するための配線数を減らし、コンパクトな配線構造を得ることができる。コンパクトな配線構造により、配線の敷設方法の選択が容易となり、配線の劣化や損傷などの発生を低減させることが可能となる。

１−２．センサーモジュールの構成
本実施形態では、センサーモジュール１０ａ，１０ｂ，１０ｃ，３０のそれぞれは、互いに直交する３軸であるＸ軸、Ｙ軸、Ｚ軸が定義されており、Ｘ軸、Ｙ軸、Ｚ軸の各軸周りの角速度及びＸ軸、Ｙ軸、Ｚ軸の各軸方向の加速度を検出する。センサーモジュール１０ａ，１０ｂ，１０ｃ，３０は、互いのＸ軸、Ｙ軸、Ｚ軸が一致している必要はない。センサーモジュール１０ａ，１０ｂ，１０ｃの構成は同じであるため、以下では、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれをセンサーモジュール１０と称する場合がある。なお、本実施形態では、センサーモジュール３０は、以下に説明するセンサーモジュール１０と同じ構成である。

図３は、センサーモジュール１０の構成例を示す図である。図３に示すように、センサーモジュール１０は、慣性センサー１００と、マイクロコントローラー１１０と、通信インターフェース回路１２０とを含む。慣性センサー１００、マイクロコントローラー１１０及び通信インターフェース回路１２０は、不図示のパッケージに収容されている。例えば、マイクロコントローラー１１０及び通信インターフェース回路１２０は、１チップの集積回路装置に含まれており、慣性センサー１００と当該集積回路装置とがパッケージに収容されていてもよい。

慣性センサー１００は、Ｘ軸角速度センサー１０１と、Ｙ軸角速度センサー１０２と、Ｚ軸角速度センサー１０３と、３軸加速度センサー１０４とを含む。

Ｘ軸角速度センサー１０１は、角速度を検出する不図示の角速度検出素子と、角速度検出素子からの信号に対して、増幅、同期検波、ゲイン調整等を行ってＸ軸周りの角速度の向きや大きさに応じた検出信号を生成する不図示の検出回路とを含む。検出回路は、検出信号をデジタル信号に変換するＡ／Ｄ変換回路を含む。Ｘ軸角速度センサー１０１は、角速度検出素子の検出軸がＸ軸に沿うように配置されており、Ａ／Ｄ変換回路から出力されるデジタル信号をＸ軸角速度データとしてマイクロコントローラー１１０に送信する。

Ｙ軸角速度センサー１０２は、Ｘ軸角速度センサー１０１と同じ構成であるが、角速度検出素子の検出軸がＹ軸に沿うように配置されており、Ａ／Ｄ変換回路から出力されるデジタル信号をＹ軸角速度データとしてマイクロコントローラー１１０に送信する。

Ｚ軸角速度センサー１０３は、Ｘ軸角速度センサー１０１及びＹ軸角速度センサー１０２と同じ構成であるが、角速度検出素子の検出軸がＺ軸に沿うように配置されており、Ａ
／Ｄ変換回路から出力されるデジタル信号をＺ軸角速度データとしてマイクロコントローラー１１０に送信する。

３軸加速度センサー１０４は、３つの加速度検出素子と、各加速度検出素子からの信号に対して、増幅、同期検波、ゲイン調整等を行って各軸周りの加速度の向きや大きさに応じた検出信号を生成する不図示の検出回路とを含む。検出回路は、３軸の検出信号を３軸のデジタル信号にそれぞれ変換するＡ／Ｄ変換回路を含む。３軸加速度センサー１０４は、３つの加速度検出素子の検出軸がそれぞれＸ軸、Ｙ軸、Ｚ軸に沿うように配置されており、Ａ／Ｄ変換回路から出力される３軸のデジタル信号をＸ軸加速度データ、Ｙ軸加速度データ及びＺ軸加速度データとしてマイクロコントローラー１１０に送信する。

なお、Ｘ軸角速度センサー１０１、Ｙ軸角速度センサー１０２、Ｚ軸角速度センサー１０３及び３軸加速度センサー１０４がそれぞれアナログ信号である検出信号を出力し、マイクロコントローラー１１０が各検出信号をデジタル信号に変換してもよい。

マイクロコントローラー１１０は、較正部１１１と、補正部１１２と、演算部１１３と、記憶部１１４とを含む。

較正部１１１は、センサーモジュール１０の各軸の向きを、センサーモジュール３０の対応する各軸の向きと揃えるための較正処理を行う。例えば、制御装置２０が通信インターフェース回路１２０を介して記憶部１１４に含まれる不図示の所定のレジスターに所定のデータを書き込んだ場合に、較正部１１１が較正処理を行ってもよい。本実施形態では、センサーモジュール１０の３軸とセンサーモジュール３０の３軸との対応関係は任意に設定可能であり、例えば、センサーモジュール１０のＹ軸、Ｚ軸、Ｘ軸を、それぞれセンサーモジュール３０のＸ軸、Ｙ軸、Ｚ軸と対応させてもよい。

具体的には、較正部１１１は、制御装置２０から通信インターフェース回路１２０を介して受信した較正指示に応じて、慣性センサー１００の出力信号に基づいてセンサーモジュール１０の姿勢を算出し、算出した姿勢と第１の姿勢との差に基づいて補正情報１１５を生成する。例えば、較正部１１１は、慣性センサー１００から出力されるＸ軸加速度データ、Ｙ軸加速度データ及びＺ軸加速度データに基づいて重力方向を特定し、Ｘ軸、Ｙ軸及びＺ軸と重力方向との関係からセンサーモジュール１０の姿勢を算出することができる。

補正情報１１５は、例えば、算出されたセンサーモジュール１０の姿勢を第１の姿勢と一致させるように、直交性を保ったままＸ軸、Ｙ軸、Ｚ軸を回転させる回転行列であってもよい。較正部１１１が生成した補正情報１１５は、記憶部１１４に記憶される。第１の姿勢は、センサーモジュール３０の３軸で定義される座標系におけるセンサーモジュール１０の基本姿勢である。

図４は、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれの第１の姿勢の一例を示す図である。図４では、移動体１の一部が簡略化して図示されている。図４の例では、センサーモジュール１０ａはバケットリンク４６に設けられ、センサーモジュール１０ｂはアーム４４の側面に設けられ、センサーモジュール１０ｃはブーム４３の側面に設けられている。そして、センサーモジュール１０ａ，１０ｂ，１０ｃのＺ軸が同じ方向を向いている。なお、センサーモジュール３０は、例えば、上部旋回体４１の運転席の床面に設けられている。

本実施形態では、センサーモジュール３０の姿勢は、Ｘ軸周りのロール角、Ｙ軸周りのピッチ角及びＺ軸周りのヨー角で定義される。また、図４の例では、センサーモジュール
１０ａ，１０ｂ，１０ｃのＹ軸、Ｚ軸、Ｘ軸が、それぞれセンサーモジュール３０のＸ軸、Ｙ軸、Ｚ軸と対応するように設定されており、センサーモジュール１０ａ，１０ｂ，１０ｃの姿勢は、Ｙ軸周りのロール角、Ｚ軸周りのピッチ角及びＸ軸周りのヨー角で定義される。較正処理においては、センサーモジュール３０のロール角、ピッチ角、ヨー角はすべて０°であるものとし、センサーモジュール１０ａ，１０ｂ，１０ｃのＹ軸、Ｚ軸、Ｘ軸が、それぞれセンサーモジュール３０のＸ軸、Ｙ軸、Ｚ軸と一致するとき、センサーモジュール１０ａ，１０ｂ，１０ｃのロール角、ピッチ角、ヨー角はすべて０°である。

図４の例では、較正処理の実施中、ブーム４３、アーム４４及びバケット４５は折れ曲がった状態で静止しており、センサーモジュール１０ａの第１の姿勢は、ロール角が０°、ピッチ角がφ１、ヨー角が０°である。また、センサーモジュール１０ｂの第１の姿勢は、ロール角が０°、ピッチ角がφ２、ヨー角が０°である。また、センサーモジュール１０ｃの第１の姿勢は、ロール角が０°、ピッチ角がφ３、ヨー角が０°である。例えば、φ１＜φ２＜０＜φ３である。したがって、センサーモジュール１０ａの較正部１１１は、慣性センサー１００の出力信号に基づいて算出したロール角、ピッチ角、ヨー角をそれぞれ０°、φ１、０°に補正するための補正情報１１５を生成する。また、センサーモジュール１０ｂの較正部１１１は、慣性センサー１００の出力信号に基づいて算出したロール角、ピッチ角、ヨー角をそれぞれ０°、φ２、０°に補正するための補正情報１１５を生成する。また、センサーモジュール１０ｃの較正部１１１は、慣性センサー１００の出力信号に基づいて算出したロール角、ピッチ角、ヨー角をそれぞれ０°、φ３、０°に補正するための補正情報１１５を生成する。

図３に戻り、補正部１１２は、補正情報１１５に基づいて、慣性センサー１００の出力信号を補正する。例えば、補正部１１２は、補正情報１１５が回転行列であれば、慣性センサー１００から出力されるＸ軸角速度データ、Ｙ軸角速度データ、Ｚ軸角速度データ、Ｘ軸加速度データ、Ｙ軸加速度データ、Ｚ軸加速度データと回転行列との行列積を計算することにより、各データを補正する。

演算部１１３は、補正部１１２により補正されたＸ軸角速度データ、Ｙ軸角速度データ、Ｚ軸角速度データ、Ｘ軸加速度データ、Ｙ軸加速度データ、Ｚ軸加速度データに基づいて所定の演算を行う。例えば、演算部１１３は、Ｘ軸加速度データ、Ｙ軸加速度データ、Ｚ軸加速度データに基づいて、センサーモジュール１０の位置を計算してもよい。また、例えば、演算部１１３は、Ｘ軸角速度データ、Ｙ軸角速度データ、Ｚ軸角速度データに基づいて、センサーモジュール１０の姿勢、すなわち、ロール角、ピッチ角、ヨー角を演算してもよい。また、例えば、センサーモジュール１０が温度センサーを含む場合、演算部１１３は、当該温度センサーの出力信号に基づいて、Ｘ軸角速度データ、Ｙ軸角速度データ、Ｚ軸角速度データ、Ｘ軸加速度データ、Ｙ軸加速度データ、Ｚ軸加速度データの温度特性を補正してもよい。

記憶部１１４は、半導体メモリーやレジスターなどにより実現され、マイクロコントローラー１１０による処理に必要な各種の情報を記憶する。本実施形態では、記憶部１１４は、補正情報１１５を記憶する。また、記憶部１１４は、通信インターフェース回路１２０を介して制御装置２０から受信した第１の姿勢の情報を記憶してもよい。

通信インターフェース回路１２０は、制御装置２０とデータ通信を行うための回路である。通信インターフェース回路１２０は、例えば、ＳＰＩ（Serial Peripheral Interface）の通信規格やＩ２Ｃ（Inter-Integrated Circuit）の通信規格のインターフェース処理を行ってもよい。本実施形態では、通信インターフェース回路１２０は、補正部１１２により補正されたＸ軸角速度データ、Ｙ軸角速度データ、Ｚ軸角速度データ、Ｘ軸加速度データ、Ｙ軸加速度データ、Ｚ軸加速度データを受け取って制御装置２０に送信する。ま
た、通信インターフェース回路１２０は、演算部１１３の演算により得られたデータを制御装置２０に送信してもよい。また、通信インターフェース回路１２０は、制御装置２０から較正指示や第１の姿勢の情報を受信し、マイクロコントローラー１１０に出力する。

１−３．制御装置の構成
図５は、制御装置２０の構成例を示す図である。図５に示すように、制御装置２０は、マイクロコントローラー２１０と、通信インターフェース回路２２０とを含む。

マイクロコントローラー２１０は、較正指示部２１１と、姿勢設定部２１２と、演算部２１３と、記憶部２１４とを含む。

較正指示部２１１は、通信インターフェース回路２２０を介して、センサーモジュール１０に較正指示を行う。例えば、不図示の外部装置が記憶部２１４に含まれる不図示の所定のレジスターに所定のデータを書き込んだ場合に、較正指示部２１１がセンサーモジュール１０に較正指示を行ってもよい。

姿勢設定部２１２は、通信インターフェース回路２２０を介して、第１の姿勢の情報をセンサーモジュール１０に送信する。例えば、不図示の外部装置が記憶部２１４に含まれる不図示の所定のレジスターに第１の姿勢の情報を書き込んだ場合に、姿勢設定部２１２が当該第１の姿勢の情報をセンサーモジュール１０に送信してもよい。なお、第１の姿勢の情報を較正指示として兼用してもよい。すなわち、姿勢設定部２１２が較正指示部２１１として機能してもよい。

演算部２１３は、センサーモジュール１０から通信インターフェース回路２２０を介して各種のデータを受信し、当該各種のデータと記憶部２１４に記憶されている作業情報２１５とに基づいて所定の演算を行う。特に、本実施形態では、演算部２１３は、センサーモジュール１０において補正部１１２が補正した慣性センサー１００の出力信号であるＸ軸角速度データ、Ｙ軸角速度データ、Ｚ軸角速度データ、Ｘ軸加速度データ、Ｙ軸加速度データ、Ｚ軸加速度データに基づいて、可動部であるブーム４３、アーム４４及びバケット４５を制御する。例えば、演算部２１３は、Ｘ軸角速度データ、Ｙ軸角速度データ、Ｚ軸角速度データ、Ｘ軸加速度データ、Ｙ軸加速度データ及びＺ軸加速度データに基づいて、ブーム４３、アーム４４及びバケット４５の位置や姿勢を演算し、演算結果に基づいて、作業情報２１５で指定される作業が実施されるようにブーム４３、アーム４４及びバケット４５の位置や姿勢を制御する。これにより、バケット４５の刃先の位置が制御され、作業情報２１５で指定される作業がセミオートで実施される。なお、センサーモジュール１０が自身の位置や姿勢を演算して制御装置２０に送信する場合には、演算部２１３は、当該位置や姿勢を用いてブーム４３、アーム４４及びバケット４５の位置や姿勢を制御してもよい。

また、演算部２１３は、センサーモジュール３０から出力されるＸ軸角速度データ、Ｙ軸角速度データ、Ｚ軸角速度データ、Ｘ軸加速度データ、Ｙ軸加速度データ及びＺ軸加速度データに基づいて、上部旋回体４１の傾斜角を演算し、演算結果に基づいて上部旋回体４１の動作を制御する。例えば、演算部２１３は、ブーム４３、アーム４４及びバケット４５の位置や姿勢とともに上部旋回体４１の傾斜角に基づいて、作業情報２１５で指定される作業が実施されるようにブーム４３、アーム４４、バケット４５及び上部旋回体４１の位置や姿勢を制御してもよい。

また、演算部２１３は、較正指示部２１１によるセンサーモジュール１０への較正指示の前に、センサーモジュール１０が第１の姿勢となるように可動部であるブーム４３、アーム４４及びバケット４５を制御する。例えば、作業者が、センサーモジュール１０が第
１の姿勢となるように操作装置を操作し、演算部２１３は、当該操作に従ってブーム４３、アーム４４及びバケット４５が所定の状態で静止するように制御してもよい。また、例えば、作業者によらず、演算部２１３が、自動的にセンサーモジュール１０が第１の姿勢となるように、ブーム４３、アーム４４及びバケット４５が所定の状態で静止するように制御してもよい。

記憶部２１４は、半導体メモリーやレジスターなどにより実現され、マイクロコントローラー２１０による処理に必要な各種の情報を記憶する。本実施形態では、記憶部２１４は、移動体１が実施すべき作業が規定された作業情報２１５を記憶する。また、記憶部２１４は、通信インターフェース回路１２０を介して制御装置２０から受信した各種のデータを記憶してもよい。

通信インターフェース回路２２０は、センサーモジュール１０とデータ通信を行うための回路である。通信インターフェース回路２２０は、例えば、ＳＰＩの通信規格やＩ２Ｃの通信規格のインターフェース処理を行ってもよい。本実施形態では、通信インターフェース回路２２０は、センサーモジュール１０からＸ軸角速度データ、Ｙ軸角速度データ、Ｚ軸角速度データ、Ｘ軸加速度データ、Ｙ軸加速度データ、Ｚ軸加速度データを受信してマイクロコントローラー２１０に出力する。また、通信インターフェース回路２２０は、マイクロコントローラー２１０から較正指示や第１の姿勢の情報を受け取ってセンサーモジュール１０に送信する。

１−４．センサーモジュールの較正方法
図６は、センサーモジュール１０の較正方法の手順の一例を示すフローチャート図である。

図６に示すように、まず、工程Ｓ１において、制御装置２０が、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれが第１の姿勢となるように可動部であるブーム４３、アーム４４及びバケット４５を制御する。

次に、工程Ｓ２において、制御装置２０が、第１の姿勢の情報をセンサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれに送信する。

次に、工程Ｓ３において、制御装置２０が、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれに較正指示を行う。

次に、工程Ｓ４において、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれが、較正指示に応じて、慣性センサー１００の出力信号に基づいてセンサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれの姿勢を算出し、算出した姿勢と第１の姿勢との差に基づいて補正情報１１５を生成する。

最後に、工程Ｓ５において、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれが、補正情報１１５に基づいて、慣性センサー１００の出力信号を補正する。

１−５．作用効果
以上に説明したように、第１実施形態では、制御装置２０は、センサーモジュール１０ａ，１０ｂ，１０ｃが第１の姿勢となるように可動部であるブーム４３、アーム４４及びバケット４５を制御し、かつ、センサーモジュール１０ａ，１０ｂ，１０ｃに較正指示を行う。そして、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれは、較正指示に応じて、慣性センサー１００の出力信号に基づいてセンサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれの姿勢を算出し、算出した姿勢と第１の姿勢との差に基づいて補正情報１
１５を生成し、補正情報１１５に基づいて、慣性センサー１００の出力信号を補正する。したがって、第１実施形態によれば、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれが、制御装置２０から送信された第１の姿勢の情報に基づいて自身で較正を行うので、レーザー照射器等の測量機を用いる必要がなく、センサーモジュール１０ａ，１０ｂ，１０ｃの較正を容易に行うことができる。

また、第１実施形態によれば、制御装置２０は、第１の姿勢の情報をセンサーモジュール１０ａ，１０ｂ，１０ｃに送信するので、可動部の配置や構造に応じて第１の姿勢を任意に設定することができ、高い利便性を実現することができる。

また、第１実施形態によれば、制御装置２０は、補正した慣性センサー１００の出力信号に基づいて可動部を制御するので、可動部を高精度に制御することができる。

２．第２実施形態
以下、第２実施形態の移動体１について、第１実施形態と同様の構成については同じ符号を付し、第１実施形態と同様の説明は省略又は簡略し、主として第１実施形態と異なる内容について説明する。以下では、第２実施形態の移動体１として、第１実施形態と同様、図１に示した油圧ショベルを例に挙げて説明する。

図７は、第２実施形態における制御装置２０の構成例を示す図である。図７に示すように、第２実施形態における制御装置２０は、第１実施形態と同様、慣性センサー１００と、マイクロコントローラー２１０と、通信インターフェース回路２２０とを含む。

マイクロコントローラー２１０は、第１実施形態と同様、較正指示部２１１と、演算部２１３と、記憶部２１４とを含む。

較正指示部２１１は、第１実施形態と同様、通信インターフェース回路２２０を介して、センサーモジュール１０に較正指示を行う。

演算部２１３は、第１実施形態と同様、センサーモジュール１０において補正部１１２が補正した慣性センサー１００の出力信号であるＸ軸角速度データ、Ｙ軸角速度データ、Ｚ軸角速度データ、Ｘ軸加速度データ、Ｙ軸加速度データ、Ｚ軸加速度データに基づいて、可動部であるブーム４３、アーム４４及びバケット４５を制御する。また、演算部２１３は、第１実施形態と同様、較正指示部２１１によるセンサーモジュール１０への較正指示の前に、センサーモジュール１０が第１の姿勢となるように可動部であるブーム４３、アーム４４及びバケット４５を制御する。

記憶部２１４は、第１実施形態と同様、マイクロコントローラー２１０による処理に必要な各種の情報、例えば、作業情報２１５や通信インターフェース回路１２０を介して制御装置２０から受信した各種のデータを記憶する。

通信インターフェース回路２２０は、第１実施形態と同様、通信インターフェース回路２２０は、センサーモジュール１０からＸ軸角速度データ、Ｙ軸角速度データ、Ｚ軸角速度データ、Ｘ軸加速度データ、Ｙ軸加速度データ、Ｚ軸加速度データを受信してマイクロコントローラー２１０に出力する。

第２実施形態では、マイクロコントローラー２１０は、第１実施形態と異なり、姿勢設定部２１２を含まない。したがって、通信インターフェース回路２２０は、マイクロコントローラー２１０から較正指示を受け取ってセンサーモジュール１０に送信するが、第１の姿勢の情報をセンサーモジュール１０に送信しない。

第２実施形態におけるセンサーモジュール１０の構成例は図３と同様であるため、その図示を省略するが、センサーモジュール１０は、第１実施形態と同様、慣性センサー１００と、マイクロコントローラー１１０と、通信インターフェース回路１２０とを含む。慣性センサー１００の構成及び機能は第１実施形態と同様であるため、その説明を省略する。

マイクロコントローラー１１０は、第１実施形態と同様、較正部１１１と、補正部１１２と、演算部１１３と、記憶部１１４とを含む。補正部１１２、演算部１１３及び記憶部１１４の機能は第１実施形態と同様であるため、その説明を省略する。

較正部１１１は、第１実施形態と同様、センサーモジュール１０の各軸の向きを、センサーモジュール３０の対応する各軸の向きと揃えるための較正処理を行う。具体的には、較正部１１１は、制御装置２０から通信インターフェース回路１２０を介して受信した較正指示に応じて、慣性センサー１００の出力信号に基づいてセンサーモジュール１０の姿勢を算出し、算出した姿勢と第１の姿勢との差に基づいて補正情報１１５を生成する。

第１実施形態と同様、第１の姿勢は、センサーモジュール３０の３軸で定義される座標系におけるセンサーモジュール１０の基本姿勢である。ただし、第２実施形態では、第１の姿勢はあらかじめ決められた姿勢であり、較正部１１１は第１の姿勢を認識している。したがって、第１実施形態では、制御装置２０が第１の姿勢の情報をセンサーモジュール１０に送信するが、第２実施形態では、制御装置２０は第１の姿勢の情報をセンサーモジュール１０に送信しない。

図８は、第２実施形態における、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれの第１の姿勢の一例を示す図である。図８では、移動体１の一部が簡略化して図示されている。図８の例では、センサーモジュール１０ａはバケットリンク４６に設けられ、センサーモジュール１０ｂはアーム４４の側面に設けられ、センサーモジュール１０ｃはブーム４３の側面に設けられている。そして、センサーモジュール１０ａ，１０ｂ，１０ｃのＸ軸、Ｙ軸、Ｚ軸がそれぞれ同じ方向を向いている。なお、センサーモジュール３０は、例えば、上部旋回体４１の運転席の床面に設けられている。

第２実施形態でも、センサーモジュール３０の姿勢は、Ｘ軸周りのロール角、Ｙ軸周りのピッチ角及びＺ軸周りのヨー角で定義される。また、図８の例では、センサーモジュール１０ａ，１０ｂ，１０ｃのＹ軸、Ｚ軸、Ｘ軸が、それぞれセンサーモジュール３０のＸ軸、Ｙ軸、Ｚ軸と対応するように設定されており、センサーモジュール１０ａ，１０ｂ，１０ｃの姿勢は、Ｙ軸周りのロール角、Ｚ軸周りのピッチ角及びＸ軸周りのヨー角で定義される。図８の例では、較正処理の実施中、ブーム４３、アーム４４及びバケット４５は直線状に伸びた状態で静止しており、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれの第１の姿勢は、ロール角が０°、ピッチ角が０°、ヨー角が０°である。したがって、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれの較正部１１１は、慣性センサー１００の出力信号に基づいて算出したロール角、ピッチ角、ヨー角をすべて０°に補正するための補正情報１１５を生成する。

図９は、第２実施形態におけるセンサーモジュール１０の較正方法の手順の一例を示すフローチャート図である。

図９に示すように、まず、工程Ｓ１１において、制御装置２０が、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれが第１の姿勢となるように可動部であるブーム４３、アーム４４及びバケット４５を制御する。

次に、工程Ｓ１２において、制御装置２０が、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれに較正指示を行う。

次に、工程Ｓ１３において、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれが、較正指示に応じて、慣性センサー１００の出力信号に基づいてセンサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれの姿勢を算出し、算出した姿勢と第１の姿勢との差に基づいて補正情報１１５を生成する。

最後に、工程Ｓ１４において、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれが、補正情報１１５に基づいて、慣性センサー１００の出力信号を補正する。

以上に説明したように、第２実施形態では、制御装置２０は、センサーモジュール１０ａ，１０ｂ，１０ｃが第１の姿勢となるように可動部であるブーム４３、アーム４４及びバケット４５を制御し、かつ、センサーモジュール１０ａ，１０ｂ，１０ｃに較正指示を行う。そして、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれは、較正指示に応じて、慣性センサー１００の出力信号に基づいてセンサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれの姿勢を算出し、算出した姿勢と第１の姿勢との差に基づいて補正情報１１５を生成し、補正情報１１５に基づいて、慣性センサー１００の出力信号を補正する。したがって、第２実施形態によれば、センサーモジュール１０ａ，１０ｂ，１０ｃのそれぞれが、制御装置２０から送信された第１の姿勢の情報に基づいて自身で較正を行うので、レーザー照射器等の測量機を用いる必要がなく、センサーモジュール１０ａ，１０ｂ，１０ｃの較正を容易に行うことができる。

また、第２実施形態によれば、第１の姿勢は、あらかじめ決められた姿勢であるので、制御装置２０が第１の姿勢の情報をセンサーモジュール１０ａ，１０ｂ，１０ｃに送信する必要がなく、第１実施形態よりも、制御装置２０の処理負荷が軽減され、較正処理に要する時間が短縮される。

また、第２実施形態によれば、制御装置２０は、補正した慣性センサー１００の出力信号に基づいて可動部を制御するので、可動部を高精度に制御することができる。

３．変形例
上記の各実施形態では、センサーモジュール１０に含まれる慣性センサー１００は、３軸の角速度及び３軸の加速度を検出しているが、１軸、２軸又は４軸以上の角速度を検出してもよいし、１軸、２軸又は４軸以上の加速度を検出してもよい。また、慣性センサー１００は、角速度センサー又は加速度センサー以外のセンサー、例えば、温度センサー等を有していてもよい。

また、上記の各実施形態では、センサーモジュール１０ａはバケット４５と連動する部位であるバケットリンク４６に設けられているが、バケット４５に設けられていてもよい。また、センサーモジュール１０ｂはアーム４４に設けられているが、アーム４４と連動する部位であるアームシリンダー４８に設けられていてもよい。同様に、センサーモジュール１０ｃはブーム４３に設けられているが、ブーム４３と連動する部位であるブームシリンダー４７に設けられていてもよい。

また、上記の各実施形態では、センサーモジュール１０が設けられる可動部又は可動部と連動する部位がブーム４３、アーム４４及びバケットリンク４６の３つである移動体１を例に挙げたが、移動体１において、センサーモジュール１０が設けられる可動部又は可動部と連動する部位は１つでもよいし、２つでもよいし、４つ以上であってもよい。

上記の各実施形態では、移動体１として油圧ショベルを例に挙げたが、移動体１は、所定の軸周りに回転する可動部を有するものであればよく、例えば、ラフテレーンクレーン、ブルドーザー、ホイールローダー等の建設機械、農業機械、あるいはロボットであってもよい。

本発明は本実施形態に限定されず、本発明の要旨の範囲内で種々の変形実施が可能である。

上述した実施形態および変形例は一例であって、これらに限定されるわけではない。例えば、各実施形態および各変形例を適宜組み合わせることも可能である。

本発明は、実施の形態で説明した構成と実質的に同一の構成（例えば、機能、方法及び結果が同一の構成、あるいは目的及び効果が同一の構成）を含む。また、本発明は、実施の形態で説明した構成の本質的でない部分を置き換えた構成を含む。また、本発明は、実施の形態で説明した構成と同一の作用効果を奏する構成又は同一の目的を達成することができる構成を含む。また、本発明は、実施の形態で説明した構成に公知技術を付加した構成を含む。

１…移動体、１０，１０ａ，１０ｂ，１０ｃ…センサーモジュール、２０…制御装置、３０…センサーモジュール、４１…上部旋回体、４２…下部走行体、４３…ブーム、４４…アーム、４５…バケット、４６…バケットリンク、４７…ブームシリンダー、４８…アームシリンダー、４９…バケットシリンダー、５０…作業機構、１００…慣性センサー、１０１…Ｘ軸角速度センサー、１０２…Ｙ軸角速度センサー、１０３…Ｚ軸角速度センサー、１０４…３軸加速度センサー、１１０…マイクロコントローラー、１１１…較正部、１１２…補正部、１１３…演算部、１１４…記憶部、１１５…補正情報、１２０…通信インターフェース回路、２１０…マイクロコントローラー、２１１…較正指示部、２１２…姿勢設定部、２１３…演算部、２１４…記憶部、２１５…作業情報、２２０…通信インターフェース回路

Claims (7)

1. 所定の軸周りに回動する可動部と、
   前記可動部又は前記可動部と連動する部位に設けられているセンサーモジュールと、
   前記可動部及び前記センサーモジュールを制御する制御装置と、を備え、
   前記制御装置は、前記センサーモジュールが第１の姿勢となるように前記可動部を制御し、かつ、前記センサーモジュールに較正指示を行い、
   前記センサーモジュールは、
   慣性センサーと、
   前記較正指示に応じて、前記慣性センサーの出力信号に基づいて前記センサーモジュールの姿勢を算出し、算出した姿勢と前記第１の姿勢との差に基づいて補正情報を生成する較正部と、
   前記補正情報に基づいて、前記慣性センサーの出力信号を補正する補正部と、を含む、移動体。
2. 前記制御装置は、前記第１の姿勢の情報を前記センサーモジュールに送信する、請求項１に記載の移動体。
3. 前記第１の姿勢は、あらかじめ決められた姿勢である、請求項１に記載の移動体。
4. 前記制御装置は、前記補正部が補正した前記慣性センサーの出力信号に基づいて、前記可動部を制御する、請求項１乃至３のいずれか一項に記載の移動体。
5. 前記可動部は、ブーム、アーム、バケットのいずれかである、請求項１乃至４のいずれか一項に記載の移動体。
6. 所定の軸周りに回動する可動部と、前記可動部を制御する制御装置とを備える移動体の前記可動部又は前記可動部と連動する部位に取り付けられるセンサーモジュールであって、
   慣性センサーと、
   前記制御装置からの較正指示に応じて、前記慣性センサーの出力信号に基づいて前記センサーモジュールの姿勢を算出し、算出した姿勢と第１の姿勢との差に基づいて補正情報を生成する較正部と、
   前記補正情報に基づいて、前記慣性センサーの出力信号を補正する補正部と、を含む、センサーモジュール。
7. 慣性センサーを含み、可動部又は前記可動部と連動する部位に設けられているセンサーモジュールを較正するセンサーモジュールの較正方法であって、
   制御装置が、前記センサーモジュールが第１の姿勢となるように前記可動部を制御する工程と、
   前記制御装置が、前記センサーモジュールに較正指示を行う工程と、
   前記センサーモジュールが、前記較正指示に応じて、前記慣性センサーの出力信号に基づいて前記センサーモジュールの姿勢を算出し、算出した姿勢と前記第１の姿勢との差に基づいて補正情報を生成する工程と、
   前記センサーモジュールが、前記補正情報に基づいて、前記慣性センサーの出力信号を補正する工程と、を含む、センサーモジュールの較正方法。

Images