図面を参照しながら本発明の実施形態について説明する。図1は、一実施形態に係るショベル100の側面図である。ショベル100の下部走行体1には旋回機構2を介して上部旋回体3が搭載されている。上部旋回体3にはブーム4が取り付けられ、ブーム4の先端にはアーム5が取り付けられ、アーム5の先端にはエンドアタッチメントとしてのバケット6が取り付けられている。
ブーム4、アーム5及びバケット6は、アタッチメントの一例としての掘削アタッチメントを構成している。ブーム4はブームシリンダ7によって駆動され、アーム5はアームシリンダ8によって駆動され、バケット6はバケットシリンダ9によって駆動される。ブーム4にはブーム角度センサS1が取り付けられ、アーム5にはアーム角度センサS2が取り付けられ、バケット6にはバケット角度センサS3が取り付けられている。
ブーム角度センサS1は、ブーム4の回動角度を検出するように構成されている。本実施形態では、ブーム角度センサS1は、水平面に対する傾斜を検出して、上部旋回体3に対するブーム4の回動角度を検出する加速度センサである。
アーム角度センサS2は、アーム5の回動角度を検出するように構成されている。本実施形態では、アーム角度センサS2は、水平面に対する傾斜を検出して、ブーム4に対するアーム5の回動角度を検出する加速度センサである。
バケット角度センサS3は、バケット6の回動角度を検出するように構成されている。本実施形態では、バケット角度センサS3は、水平面に対する傾斜を検出して、アーム5に対するバケット6の回動角度を検出する加速度センサである。
ブーム角度センサS1は、対応する連結ピン回りのブーム4の回転に関する情報を取得するポテンショメータ若しくはロータリエンコーダ、又は、対応する油圧シリンダのストローク量に関する情報を取得するストロークセンサ等であってもよい。アーム角度センサS2及びバケット角度センサS3についても同様である。
上部旋回体3にはキャビン10が設けられ、且つ、動力源としてのエンジン11等が搭載されている。また、上部旋回体3には機体傾斜センサS4及び旋回角度センサS5が取り付けられている。
機体傾斜センサS4は、上部旋回体3の傾斜に関する情報を取得するように構成されている。本実施形態では、機体傾斜センサS4は、上部旋回体3に作用する加速度に関する情報を取得する加速度センサであり、旋回軸SAよりも前方の位置で上部旋回体3に取り付けられている。但し、機体傾斜センサS4は、旋回軸SAよりも後方の位置で上部旋回体3に取り付けられていてもよく、旋回軸SA上の位置で上部旋回体3に取り付けられていてもよい。また、機体傾斜センサS4は、加速度センサと角速度センサ(ジャイロセンサ)との組み合わせである慣性センサであってもよい。この場合、機体傾斜センサS4は、地磁気センサ等を含んでいてもよい。
旋回角度センサS5は、上部旋回体3の旋回角度θsに関する情報を取得するように構成されている。旋回角度θsは、旋回軸SA回りの上部旋回体3の回動角度である。本実施形態では、旋回角度センサS5は、上部旋回体3の角速度を検出する角速度センサ(ジャイロセンサ)であり、上部旋回体3に取り付けられている。但し、旋回角度センサS5は、下部走行体1に取り付けられていてもよい。
旋回角度センサS5は、上部旋回体3の角速度を積分することで上部旋回体3の旋回角度θsを導き出す。但し、旋回角度センサS5は、旋回用モータの回転を測定するポテンショメータ若しくはロータリエンコーダが出力する情報、旋回用モータとしての旋回用油圧モータに流入する作動油の流量に関する情報、又は、上部旋回体3に取り付けられたカメラ若しくはライダが出力する情報等に基づき、上部旋回体3の旋回角度θsに関する情報を取得するように構成されていてもよい。
また、旋回角度センサS5は、機体傾斜センサS4のように、加速度センサと角速度センサ(ジャイロセンサ)との組み合わせである慣性センサであってもよい。この場合、機体傾斜センサS4は、地磁気センサ等を含んでいてもよい。旋回角度センサS5は、一対のGNSS受信機で構成されていてもよい。
機体傾斜センサS4と旋回角度センサS5とは、別々に上部旋回体3に取り付けられていてもよく、一つの基板に搭載されて一体となって上部旋回体3に取り付けられていてもよい。
以下では、ブーム角度センサS1、アーム角度センサS2、バケット角度センサS3及び機体傾斜センサS4を集合的に「姿勢センサ」と称する場合がある。
キャビン10内には、入力装置D1、音声出力装置D2、表示装置D3、記憶装置D4、ゲートロックレバーD5、コントローラ30及び演算装置50等が設置されている。
コントローラ30は、ショベル100の駆動制御を行う主制御部として機能するように構成されている。本実施形態では、コントローラ30は、CPU、揮発性記憶装置及び不揮発性記憶装置を含むマイクロコンピュータで構成されている。コントローラ30の各種機能は、不揮発性記憶装置に記憶されたプログラムをCPUが実行することで実現される。
演算装置50は、ショベル100の操作を支援できるように構成されている。本実施形態では、演算装置50は、例えば、操作者が設定した目標地形の表面とバケット6の先端(爪先)位置との鉛直方向における距離を視覚的に且つ聴覚的に操作者に知らせることができるように構成されている。このようにして、演算装置50は操作者によるショベル100の操作を支援できる。なお、演算装置50は、その距離を視覚的に操作者に知らせるのみであってもよく、聴覚的に操作者に知らせるのみであってもよい。具体的には、演算装置50は、コントローラ30と同様、CPU、揮発性記憶装置及び不揮発性記憶装置を含むマイクロコンピュータで構成される。演算装置50の各種機能は、不揮発性記憶装置に記憶されたプログラムをCPUが実行することで実現される。演算装置50は、コントローラ30とは別個に設けられてもよく、或いは、コントローラ30に組み込まれていてもよい。また、演算装置50は、マシンガイダンス機能ばかりでなく、操作者によるショベル100の操作を自律的に支援するマシンコントロール機能を実行するように構成されていてもよい。
入力装置D1は、ショベル100の操作者がコントローラ30又は演算装置50に各種情報を入力する際に利用される装置である。本実施形態では、入力装置D1は、表示装置D3の表面に取り付けられるメンブレンスイッチである。入力装置D1はタッチパネル等であってもよい。
音声出力装置D2は、警報装置の一例であり、各種音声情報を出力できるように構成されている。本実施形態では、音声出力装置D2は、演算装置50に直接接続される車載スピーカであり、演算装置50からの音声出力指令に応じて各種音声情報を出力する。なお、音声出力装置D2は、ブザー等であってもよい。
表示装置D3は、警報装置の別の一例であり、各種情報を表示できるように構成されている。本実施形態では、表示装置D3は、演算装置50に直接接続される車載液晶ディスプレイであり、演算装置50からの指令に応じて各種画像情報を表示する。
記憶装置D4は、各種情報を記憶できるように構成されている。本実施形態では、記憶装置D4は、不揮発性記憶媒体としての半導体メモリであり、演算装置50等が出力する各種情報を記憶する。
ゲートロックレバーD5は、ショベル100が誤って操作されるのを防止する機構である。本実施形態では、ゲートロックレバーD5は、キャビン10のドアと運転席との間に配置されている。キャビン10から操作者が退出し難くなるようにゲートロックレバーD5が引き上げられた場合に、各種操作装置は操作可能となる。一方、キャビン10から操作者が退出し易くなるようにゲートロックレバーD5が押し下げられた場合には、各種操作装置は操作不能となる。
図2は、図1のショベル100の駆動系の構成例を示す図である。図2では、機械的動力系は二重線で示され、作動油ラインは太実線で示され、パイロットラインは破線で示され、電気制御系は細実線で示されている。
エンジン11はショベル100の動力源である。本実施形態では、エンジン11は、エンジン負荷の増減にかかわらずエンジン回転数を一定に維持するアイソクロナス制御を採用したディーゼルエンジンである。エンジン11における燃料噴射量、燃料噴射タイミング及びブースト圧等は、エンジンコントローラD7により制御される。
エンジンコントローラD7はエンジン11を制御するように構成されている。本実施形態では、エンジンコントローラD7は、オートアイドル機能及びオートアイドルストップ機能等の各種機能を実行できるように構成されている。
オートアイドル機能は、所定の条件が満たされた場合にエンジン回転数を通常回転数(例えば2000rpm)からアイドル回転数(例えば800rpm)に低減させる機能である。本実施形態では、エンジンコントローラD7は、コントローラ30からのオートアイドル指令に応じてオートアイドル機能を作動させてエンジン回転数をアイドル回転数まで低減させる。
オートアイドルストップ機能は、所定の条件が満たされた場合にエンジン11を停止させる機能である。本実施形態では、エンジンコントローラD7は、コントローラ30からのオートアイドルストップ指令に応じてオートアイドルストップ機能を作動させてエンジン11を停止させる。
エンジン11の出力軸には油圧ポンプとしてのメインポンプ14及びパイロットポンプ15のそれぞれの入力軸が接続されている。メインポンプ14には作動油ライン16を介してコントロールバルブ17が接続されている。
コントロールバルブ17は、ショベル100の油圧系の制御を行う油圧制御装置である。右側走行用油圧モータ1A、左側走行用油圧モータ1B、ブームシリンダ7、アームシリンダ8、バケットシリンダ9及び旋回用油圧モータ21等の油圧アクチュエータは、作動油ラインを介してコントロールバルブ17に接続されている。パイロットポンプ15にはパイロットライン25を介して操作装置26が接続されている。旋回用油圧モータ21は、旋回用電動発電機で置き換えられてもよい。
操作装置26は、ショベル100を操作するために操作者が使用できるように構成されている。本実施形態では、操作装置26は、レバー及びペダルを含み、作動油ライン27及びゲートロック弁D6を介してコントロールバルブ17に接続されている。また、操作装置26は、作動油ライン28を介して圧力センサ29に接続されている。具体的には、操作装置26は、アーム操作レバーと旋回操作レバーとを兼ねる左操作レバー、ブーム操作レバーとバケット操作レバーとを兼ねる右操作レバー、走行レバー及び走行ペダルを含む。
ゲートロック弁D6は、コントロールバルブ17と操作装置26とを接続する作動油ライン27の連通・遮断を切り換えできるように構成されている。本実施形態では、ゲートロック弁D6は、コントローラ30からの指令に応じて作動油ライン27の連通・遮断を切り換える電磁弁である。コントローラ30は、ゲートロックレバーD5が出力する状態信号に基づいてゲートロックレバーD5の状態を判定する。そして、コントローラ30は、ゲートロックレバーD5が引き上げられた状態にあると判定した場合に、ゲートロック弁D6に対して連通指令を出力する。連通指令を受けると、ゲートロック弁D6は開いて作動油ライン27を連通させる。その結果、操作装置26に対する操作者の操作は有効となる。すなわち、操作者は、油圧アクチュエータを動作させることができるようになる。一方、コントローラ30は、ゲートロックレバーD5が引き下げられた状態にあると判定した場合に、ゲートロック弁D6に対して遮断指令を出力する。遮断指令を受けると、ゲートロック弁D6は閉じて作動油ライン27を遮断する。その結果、操作装置26に対する操作者の操作は無効となる。すなわち、操作者は、油圧アクチュエータを動作させることができないようになる。
圧力センサ29は、操作装置26の操作内容を圧力の形で検出できるように構成されている。本実施形態では、圧力センサ29は、操作装置26が操作されたときに生成されるパイロット圧を検出できるように構成されている。圧力センサ29は、検出値をコントローラ30に対して出力する。但し、操作装置26の操作内容は、レバー傾斜角度等、圧力以外の他の物理量に基づいて検出されてもよい。
次に、図3を参照し、演算装置50に設けられた各種機能要素について説明する。図3は、演算装置50の構成例を示す図である。
本実施形態では、コントローラ30は、演算装置50によるマシンガイダンス機能及びマシンコントロール機能等の支援機能を実行させるか否かを決定できるように構成されている。具体的には、コントローラ30は、ゲートロックレバーD5及び圧力センサ29等のそれぞれの出力に基づいて、ショベル100が休止中であるか否かを判定する。そして、コントローラ30は、ショベル100が休止中であると判定したときは、演算装置50による支援機能を中止するように、演算装置50に中止指令を送る。なお、演算装置50とコントローラ30とは、CANを通じて互いに通信可能に接続されている。
或いは、コントローラ30は、オートアイドルストップ指令をエンジンコントローラD7に対して出力する際に、中止指令を演算装置50に対して出力してもよい。
本実施形態では、演算装置50は、ブーム角度センサS1、アーム角度センサS2、バケット角度センサS3、機体傾斜センサS4、旋回角度センサS5、入力装置D1及びコントローラ30等が出力する情報を受信できるように構成されている。そして、演算装置50は、受信した情報に基づいてアタッチメント(例えば、バケット6)の目標位置と実際の位置とを算出できるように構成されている。また、演算装置50は、アタッチメントの実際の位置が目標位置と異なる場合、音声出力装置D2及び表示装置D3に警報指令を送信し、警報を出力させることができるように構成されている。
演算装置50は、様々な機能を実行する機能部を含む。本実施形態では、演算装置50は、アタッチメントの動きをガイドするための機能部として、傾斜角算出部501、傾斜角推定部502、高さ算出部503、比較部504、警報制御部505及びガイダンスデータ出力部506を含む。
傾斜角算出部501は、機体傾斜センサS4の出力に基づいて上部旋回体3の傾斜角(姿勢角)を算出する。上部旋回体3の傾斜角は、例えば、上部旋回体3の前後軸回りの傾斜角(ロール角)、及び、上部旋回体3の左右軸回りの傾斜角(ピッチ角)を含む。具体的には、傾斜角算出部501は、機体傾斜センサS4が出力する情報を用いて、水平面に対する上部旋回体3のピッチ角及びロール角を算出する。
傾斜角推定部502は、上部旋回体3が旋回しているときの上部旋回体3の傾斜角を推定する。具体的には、傾斜角推定部502は、上部旋回体3の旋回が開始される前に傾斜角算出部501が算出した傾斜角と、上部旋回体3が旋回しているときに旋回角度センサS5が出力する情報とに基づき、上部旋回体3が旋回しているときの上部旋回体3の傾斜角を推定する。本実施形態では、傾斜角算出部501は、上部旋回体3が旋回動作を行っているときには、上部旋回体3の傾斜角を算出しない。傾斜角算出部501が算出する傾斜角は、上部旋回体3が旋回動作を行っているときには、その旋回動作による遠心力の影響を受けて実際の傾斜角からずれてしまうためである。この構成により、演算装置50は、上部旋回体3が旋回動作を行っているときであっても、上部旋回体3の傾斜角を正確に推定できる。
高さ算出部503は、上部旋回体3の傾斜角と、ブーム4、アーム5及びバケット6のそれぞれの回動角度とから、目標地形の表面に対するバケット6の先端(爪先)の高さを算出する。本実施形態では、上部旋回体3の傾斜角は、傾斜角算出部501によって算出され、或いは、傾斜角推定部502によって推定される。ブーム4の回動角度は、ブーム角度センサS1の出力に基づいて算出され、アーム5の回動角度は、アーム角度センサS2の出力に基づいて算出され、バケット6の回動角度は、バケット角度センサS3の出力に基づいて算出される。
バケット6の先端で掘削を行う場合、バケット6の先端(爪先)は、エンドアタッチメントの作業部位に相当する。バケット6の背面で土砂をならすような作業を行う場合には、バケット6の背面は、エンドアタッチメントの作業部位に相当する。エンドアタッチメントとしてブレーカが用いられる場合、ブレーカの先端は、エンドアタッチメントの作業部位に相当する。
比較部504は、作業部位の実際の高さと目標高さとを比較する。本実施形態では、比較部504は、高さ算出部503が算出したバケット6の先端(爪先)の高さと、ガイダンスデータ出力部506が出力するバケット6の先端(爪先)の目標高さとを比較する。
警報制御部505は、警報装置を制御する。本実施形態では、警報制御部505は、例えば、比較部504での比較結果に基づき、警報を出力する必要があるか否かを判定する。そして、警報制御部505は、警報を出力する必要があると判定した場合、警報指令を音声出力装置D2及び表示装置D3の少なくとも一方に送信する。音声出力装置D2及び表示装置D3の少なくとも一方は、警報指令を受けると、ショベル100の操作者に対して所定の警報を出力する。
例えば、警報制御部505は、作業部位の実際の高さと目標高さと差が所定値以下となった場合に、音声出力装置D2から断続音を出力させる。そして、警報制御部505は、その差が小さくなるにつれて断続音の間隔を短くし、その差がゼロになった場合に音声出力装置D2から連続音を出力させる。この構成により、操作者は、断続音の間隔に基づいて作業部位の実際の高さと目標高さと差を直感的に把握できる。
ガイダンスデータ出力部506は、記憶装置D4に予め記憶されていた設計データから、バケット6の目標高さに関する情報を導き出し、比較部504に対して出力する。
次に、図4を参照し、上部旋回体3の旋回中に機体傾斜センサS4の出力に基づいて演算装置50が上部旋回体3の傾斜角を算出するときの問題について説明する。図4は、ショベル100の側面図であり、図4(A)〜図4(C)を含む。図4(A)は、上部旋回体3が旋回していないときに機体傾斜センサS4が検出する重力加速度GAを示す。図4(B)は、上部旋回体3が旋回しているときに機体傾斜センサS4が検出する、重力加速度GAと遠心加速度CAとの合成加速度RAを示す。図4(C)は、演算装置50が合成加速度RAを重力加速度と誤認したときに演算装置50が導き出してしまう傾斜角(+α度のピッチ角)を示す。すなわち、図4(C)は、演算装置50が誤って導き出してしまうショベル100の仮想的な状態を示す。ピッチ角が正値であることは、上部旋回体3の前側が低くなっていることを表す。
図4の例では、ショベル100は水平面上に位置している。そして、機体傾斜センサS4は、上部旋回体3の旋回軸SAよりも前方の位置で上部旋回体3に取り付けられている。また、機体傾斜センサS4は、上部旋回体3の前後軸(前方を+X、後方を−XとするX軸)に沿った方向に作用する加速度を検出する第1加速度センサと、上部旋回体3の左右軸(右方を−Y、左方を+YとするY軸)に沿った方向に作用する加速度を検出する第2加速度センサと、上部旋回体3の旋回軸SA(上方を+Z、下方を−ZとするZ軸)に沿った方向に作用する加速度を検出する第3加速度センサと、を含む。
図4(A)に示すように、上部旋回体3が旋回していないとき、機体傾斜センサS4に作用する加速度は、重力加速度GAのみである。そのため、演算装置50は、第3加速度センサのみが重力加速度GAを検出している状態に基づき、ショベル100が水平面に位置していると判定する。すなわち、演算装置50は、0度のピッチ角と0度のロール角を算出する。
図4(B)に示すように、上部旋回体3が旋回しているときには、機体傾斜センサS4に作用する加速度は、重力加速度GAと遠心加速度CAとを含む。遠心加速度CAは、+X方向を向く加速度である。そのため、演算装置50は、第3加速度センサが重力加速度GAを検出し、且つ、第1加速度センサが遠心加速度CAを検出している状態に基づき、重力加速度GAと遠心加速度CAとの合成加速度RAを導き出す。
このとき、合成加速度RAを重力加速度と誤認した場合、すなわち、合成加速度RAが作用する方向を鉛直下方と誤認した場合、演算装置50は、ショベル100が実際には水平面上に位置しているにもかかわらず、ショベル100があたかも図4(C)に示すような斜面上に位置しているものと判定してしまう。すなわち、演算装置50は、+α度のピッチ角を算出してしまう。
このような誤認を防止するため、演算装置50は、以下で説明するように、上部旋回体3が旋回しているときには、旋回開始前の機体傾斜センサS4の出力と、旋回中の旋回角度センサS5の出力とに基づき、上部旋回体3の傾斜角を推定するように構成されている。
次に、図5を参照し、斜面上に位置するショベル100に搭載されている演算装置50が上部旋回体3の傾斜角を導き出す処理(以下、「傾斜角導出処理」とする。)の概要について説明する。図5は、傾斜角導出処理を説明する図であり、図5(A1)、図5(A2)、図5(B1)及び図5(B2)を含む。図5(A1)は、斜面上で静止しているショベル100の側面図であり、図5(A2)は、その静止しているショベル100に搭載されている演算装置50が算出する重力加速度ベクトルGAsを表すセンサ座標系である。本実施形態では、局所座標系としてのセンサ座標系は、機体傾斜センサS4の中心を原点Oとし、上部旋回体3の前後軸に平行なX軸と、上部旋回体3の左右軸に平行なY軸と、旋回軸SAに平行なZ軸とを有する3次元直交座標系である。図5(B1)は、斜面上で旋回しているショベル100の側面図を示し、図5(B2)は、その旋回しているショベル100に搭載されている演算装置50が推定する重力加速度ベクトルGAsを表すセンサ座標系である。図5(B2)における点線で示された軸は、図5(A2)におけるX軸及びY軸を表している。
図5の例では、図4の例と同様に、機体傾斜センサS4は、上部旋回体3の旋回軸SAよりも前方の位置で上部旋回体3に取り付けられている。そして、機体傾斜センサS4を構成する3つ(3軸)の加速度センサは、重力加速度GAを鉛直上向きの重力加速度ベクトルGAsとして検出し、且つ、遠心加速度CAを旋回軸SA向きの加速度ベクトルとして検出するように構成されている。3つの加速度センサは、X軸方向に作用する加速度を検出する第1加速度センサと、Y軸方向に作用する加速度を検出する第2加速度センサと、Z軸方向に作用する加速度を検出する第3加速度センサと、を含む。旋回角度センサS5は、旋回用油圧モータ21の回転軸の回転角度を検出するポテンショメータである。
ショベル100が旋回していないときには、演算装置50は、遠心加速度CA等の外乱が発生しないため、機体傾斜センサS4の出力に基づき、上部旋回体3の傾斜角(ピッチ角及びロール角)を算出する。具体的には、演算装置50は、センサ座標系における重力加速度ベクトルGAsの向きを導き出すことで、上部旋回体3のピッチ角及びロール角を算出する。
図5の例では、演算装置50は、第1加速度センサが検出した重力加速度ベクトルGAsのX軸成分Gxと、第3加速度センサが検出した重力加速度ベクトルGAsのZ軸成分Gzとに基づき、XZ平面上の座標点P1を向く重力加速度ベクトルGAsを導き出している。なお、X軸成分Gxの大きさはx1であり、Z軸成分Gzの大きさはz1である。図5(A1)及び図5(A2)の例では、重力加速度ベクトルGAsのY軸成分Gyの大きさy1は0であるため、座標点P1のY座標の値は0である。そのため、重力加速度ベクトルGAsは、(1)式で表される。
この場合、演算装置50は、以下の(2)式を用いてピッチ角αの値を算出でき、(3)式を用いてロール角βの値を算出できる。但し、ピッチ角α及びロール角βは、角度の取り方によって変化する。そのため、(2)式及び(3)式は、単なる一例であり、ピッチ角α及びロール角βを導き出すための式は、これらの式に限定されない。(2)式を用いて算出されるピッチ角αは−α1であり、(3)式を用いて算出されるロール角βは0である。なお、演算装置50は、角速度センサ又は地磁気センサ等の他のセンサの出力を追加的に利用するセンサフュージョン技術によって傾斜角の算出精度を高めるようにしてもよい。
ショベル100が旋回しているときには、演算装置50は、遠心加速度CA等の外乱が発生するため、機体傾斜センサS4の出力を使用しない。その代わりに、演算装置50は、旋回角度センサS5の出力と、ショベル100が旋回していないときに記憶された機体傾斜センサS4の出力に関する情報とに基づき、上部旋回体3の傾斜角(ピッチ角及びロール角)を推定する。ショベル100が旋回していないときの機体傾斜センサS4の出力に関する情報は、例えば、旋回開始直前に記憶された機体傾斜センサS4の検出値、又は、その検出値に基づいて算出され且つ記憶された上部旋回体3の傾斜角等を含む。具体的には、演算装置50は、旋回開始前の上部旋回体3の傾斜角と、旋回開始後の旋回軸SA回りの上部旋回体3の回動角度である旋回角度θsとを導き出すことで、上部旋回体3のピッチ角及びロール角を推定する。
図5の例では、演算装置50は、例えば、旋回開始直前に旋回角度センサS5が出力する情報に基づいて基準角度位置を決定する。そして、演算装置50は、例えば、基準角度位置と、上部旋回体3が旋回しているときに旋回角度センサS5が出力する情報に基づいて継続的に算出される現在の角度位置との差を旋回角度θsとして算出する。
そして、演算装置50は、算出した旋回角度θsに基づいて旋回軸SA回りのセンサ座標系の回転を表す回転行列Rsを生成する。回転行列Rsは、旋回開始直前に上部旋回体3に作用していた重力加速度ベクトルGAsと、旋回中の現時点で上部旋回体3に作用している重力加速度ベクトルGAs'との関係を表す行列である。以下の(4)式は回転行列Rsを表し、(5)式は重力加速度ベクトルGAs'を表し、(6)式は、回転行列Rsと重力加速度ベクトルGAsと重力加速度ベクトルGAs'との関係を表す。
(6)式は、世界座標系においては、旋回開始前の上部旋回体3に作用している重力加速度ベクトルGAsと、旋回中の上部旋回体3に作用している重力加速度ベクトルGAs'とが同じであるという前提に基づく。世界測地系は、地球の重心に原点をおき、第1軸をグリニッジ子午線と赤道との交点の方向にとり、第2軸を東経90度の方向にとり、第3軸を北極の方向にとる三次元直交座標系である。
より具体的には、演算装置50は、旋回開始前に機体傾斜センサS4の出力に基づいて算出された重力加速度ベクトルGAsと、旋回中に旋回角度センサS5の出力に基づいて生成された回転行列Rsと、に基づき、旋回中の上部旋回体3に作用している重力加速度ベクトルGAs'を推定する。
そして、演算装置50は、重力加速度ベクトルGAs'のX軸成分Gx'の大きさx2とZ軸成分Gz'の大きさz2と式(2)とを用いて上部旋回体3の現在のピッチ角αを推定する。この場合、(2)式を用いて算出されるピッチ角αは+α2である。また、演算装置50は、重力加速度ベクトルGAs'のY軸成分Gy'の大きさy2とZ軸成分Gz'の大きさz2と式(3)とを用いて上部旋回体3の現在のロール角βを推定する。この場合、(3)式を用いて算出されるロール角βは−β2である。
次に、図6を参照し、傾斜角導出処理の流れについて説明する。図6は、傾斜角導出処理の流れの一例を示すフローチャートである。演算装置50は、ショベル100の稼動中、所定の制御周期で繰り返しこの傾斜角導出処理を実行する。
最初に、演算装置50は、上部旋回体3が旋回しているか否かを判定する(ステップST1)。本実施形態では、演算装置50は、圧力センサ29が取得する旋回操作レバーの操作内容に関する情報に基づいて上部旋回体3が旋回しているか否かを判定する。具体的には、演算装置50は、旋回操作レバーが生成するパイロット圧に関する情報に基づいて上部旋回体3が旋回しているか否かを判定する。但し、演算装置50は、上部旋回体3の旋回角度θsの変化に関する情報、及び、上部旋回体3の旋回角速度の変化に関する情報等の少なくとも1つに基づいて上部旋回体3が旋回しているか否かを判定してもよい。
上部旋回体3が旋回していないと判定した場合(ステップST1のNO)、演算装置50は、旋回開始直前であるか否かを判定する(ステップST2)。本実施形態では、演算装置50は、圧力センサ29が取得する旋回操作レバーの操作内容に関する情報に基づいて上部旋回体3の旋回がまさに開始されようとしているか否かを判定する。すなわち、演算装置50は、旋回操作レバーが既に操作されたが上部旋回体3が未だ回転していない状態であるか否かを判定する。
旋回開始直前であると判定した場合(ステップST2のYES)、演算装置50は、加速度と基準角度位置とを記憶する(ステップST3)。本実施形態では、演算装置50は、機体傾斜センサS4を構成する第1加速度センサ〜第3加速度センサのそれぞれが出力する3つの加速度の値を重力加速度ベクトルGAsの3成分として揮発性記憶装置に記憶する。演算装置50は、3つの加速度の値に基づいて算出される上部旋回体3の傾斜角(ピッチ角α及びロール角β)を記憶してもよい。また、演算装置50は、旋回角度センサS5が出力する情報に基づいて算出される上部旋回体3の角度位置を基準角度位置として揮発性記憶装置に記憶する。
その後、演算装置50は、旋回角度θsと記憶した加速度とに基づいて上部旋回体3の傾斜角を推定して出力する(ステップST4)。本実施形態では、演算装置50は、基準角度位置と現在の角度位置との差を旋回角度θsとして算出する。そして、演算装置50は、旋回角度θsと、ステップST3で揮発性記憶装置に記憶した重力加速度ベクトルGAsの3成分である加速度と、(6)式とを用い、旋回中の現時点で上部旋回体3に作用している重力加速度ベクトルGAs'を導き出す。その上で、演算装置50は、重力加速度ベクトルGAs'の3成分と(2)式及び(3)式とを用い、上部旋回体3のピッチ角α及びロール角βを推定して出力する。
ステップST1において、旋回中であると判定した場合(ステップST1のNO)、演算装置50は、ステップST2の判定処理及びステップST3の記憶処理を実行せずに、ステップST4を実行する。
この場合、演算装置50は、旋回開始時に記憶された基準角度位置と現在の角度位置との差を旋回角度θsとして算出する。そして、演算装置50は、その旋回角度θsと、旋回開始時に記憶された重力加速度ベクトルGAsの3成分である加速度と、(6)式とを用い、旋回中の現時点で上部旋回体3に作用している重力加速度ベクトルGAs'を導き出す。その上で、演算装置50は、重力加速度ベクトルGAs'の3成分と(2)式及び(3)式とを用い、上部旋回体3のピッチ角α及びロール角βを推定して出力する。
ステップST2において、旋回開始直前ではないと判定した場合(ステップST2のNO)、演算装置50は、加速度に基づいて上部旋回体3の傾斜角を算出して出力する(ステップST5)。本実施形態では、演算装置50は、機体傾斜センサS4を構成する第1加速度センサ〜第3加速度センサのそれぞれが出力する3つの加速度の値、すなわち、重力加速度ベクトルGAsの3成分と(2)式及び(3)式とを用い、上部旋回体3のピッチ角α及びロール角βを算出して出力する。
なお、演算装置50は、ステップST2で旋回開始直前であると判定する前に、ステップST1で旋回中であると判定した場合、揮発性記憶装置等に記憶された過去のデータに基づき、旋回開始時点を決定してもよい。ステップST4で用いられる旋回開始前の上部旋回体3の傾斜角及び基準角度位置を決定するためである。演算装置50は、例えば、旋回操作レバーが生成するパイロット圧を検出する圧力センサ29が制御周期毎に出力するデータを所定時間にわたって揮発性記憶装置等に記憶しておき、旋回中であると判定した後で、記憶しておいた過去のデータに基づいて旋回開始時点を決定してもよい。過去のデータは、旋回操作レバーの操作内容に関するデータ、上部旋回体3の旋回角度θsの変化に関するデータ、及び、上部旋回体3の旋回角速度の変化に関するデータ等であってもよい。
そして、演算装置50は、過去のデータに基づいて旋回開始時点を決定した上で、揮発性記憶装置等に記憶しておいた、機体傾斜センサS4が出力した加速度に関する情報に基づき、旋回開始前の上部旋回体3の傾斜角及び基準角度位置を決定する。
この構成により、演算装置50は、旋回中であると判定したときには未だ旋回開始時点を特定できていない場合であっても、遅滞なく旋回開始時点を特定した上で、旋回開始前の上部旋回体3の傾斜角及び基準角度位置を決定することができる。その結果、旋回中の上部旋回体3の傾斜角を高精度に推定することができる。
次に、図7を参照し、演算装置50が旋回中の上部旋回体3の傾斜角を推定することによる効果について説明する。図7は、水平面上に位置するショベル100がダンプトラックDTに土砂を積み込んでいる作業の様子の一例を示す。具体的には、図7(A)は作業現場の上面図である。図7(B)及び図7(C)は、図7(A)の矢印AR1で示す方向から作業現場を見たときの図である。図7(B)及び図7(C)では、明瞭化のため、ショベル100(バケット6を除く。)の図示が省略されている。
図7の例では、演算装置50は、操作者による右旋回操作が行われたときに旋回動作に合わせてブーム4を自律的に上昇させるマシンコントロール機能を実行している。「ブーム4を自律的に上昇させる」は、ブーム操作レバーが操作されていない場合であってもブーム4を上昇させることを含む。
図7(A)において、実線で描かれたショベル100は掘削動作が終了したときの状態を表し、破線で描かれたショベル100は旋回中の状態を表し、一点鎖線で描かれたショベル100は排土動作が開始する前の状態を表す。同様に、図7(B)及び図7(C)において、実線で描かれたバケット6Aは掘削動作が終了したときのバケット6の状態を表し、破線で描かれたバケット6Bは旋回中のバケット6の状態を表し、一点鎖線で描かれたバケット6Cは排土動作が開始する前のバケット6の状態を表す。また、図7(A)〜図7(C)における太い点線は、バケット6の背面にある所定点(以下、「作業点」とする。)が通る軌道を表す。
より具体的には、図7(B)は、演算装置50が傾斜角導出処理を実行して旋回中の上部旋回体3の傾斜角を推定する場合のバケット6の動きを示し、図7(C)は、演算装置50が傾斜角導出処理を実行せずに、機体傾斜センサS4の出力を利用して旋回中の上部旋回体3の傾斜角を継続的に算出する場合のバケット6の動きを示す。なお、この例では、機体傾斜センサS4は、図4の場合と異なり、旋回軸SAよりも後方の位置で上部旋回体3に取り付けられている。
演算装置50は、上部旋回体3に取り付けられたカメラ又はライダ等(図示せず。)を用いて把握したダンプトラックDTに関する情報に基づき、右旋回操作が開始される前に或いは開始されたときに目標軌道を設定する。
そして、演算装置50は、姿勢センサの出力に基づき、作業点の位置を特定しながら、その作業点が目標軌道に沿って移動するよう、右方向への旋回動作に合わせてブーム4を自律的に上昇させる。
また、演算装置50は、傾斜角導出処理により、すなわち、旋回中では旋回角度θsに基づいて上部旋回体3の傾斜角を推定することにより、目標軌道に沿って作業点を移動させることができる。その結果、演算装置50は、図7(B)に示すように、バケット6をダンプトラックDTに接触させることなく、且つ、バケット6を過度に持ち上げることなく、バケット6をダンプトラックDTの荷台の上に移動させることができる。旋回中に上部旋回体3に作用する遠心加速度CAの影響を受けることなく上部旋回体3の傾斜角を正確に推定できるため、すなわち、作業点の位置を正確に推定できるためである。
しかしながら、旋回中に機体傾斜センサS4が出力する情報を利用した場合、演算装置50は、旋回中に上部旋回体3に作用する遠心加速度CAの影響を受け、上部旋回体3の傾斜角を誤って算出してしまう。典型的には、演算装置50は、図4に示すように、実際にはショベル100が水平面上に位置しているにもかかわらず、あたかもショベル100が上り勾配の斜面上に位置しているかのように、上部旋回体3の傾斜角を誤って認識してしまう。この場合、演算装置50は、目標軌道に沿って作業点を移動させようとすると、図7(C)に示すように、実際には目標軌道よりも低い軌道に沿ってその所定点を移動させてしまう。図7(C)は、所定点の実際の軌道を太い一点鎖線で示す。その結果、演算装置50は、バケット6をダンプトラックDTに接触させてしまうおそれがある。
このように、演算装置50は、傾斜角導出処理を実行し、旋回中においては、機体傾斜センサS4が出力する情報を用いずに、旋回開始直前の上部旋回体3の傾斜角と旋回角度θsとに基づいて現在の傾斜角を推定する。そのため、演算装置50は、作業点の位置が目標軌道から逸脱してしまうのを防止でき、積み込み作業中にバケット6をダンプトラックDTに接触させてしまうといった事態が発生してしまうのを防止できる。
上述のように、本発明の実施形態に係るショベル100は、下部走行体1と、下部走行体1に旋回可能に搭載される上部旋回体3と、上部旋回体3に作用する加速度に関する情報を取得する加速度センサとしての機体傾斜センサS4と、上部旋回体3の旋回角度θsに関する情報を取得する旋回角度センサS5と、上部旋回体3の旋回角度θsに関する情報と、旋回開始前における上部旋回体3に作用する加速度に関する情報とに基づいて上部旋回体3が旋回しているときの上部旋回体3の傾斜角を推定する演算装置50と、を備えている。
この構成により、ショベル100は、上部旋回体3が旋回中であっても遠心加速度等による影響を受けずに上部旋回体3の傾斜角を導き出すことができる。ショベル100は、上部旋回体3の旋回中に機体傾斜センサS4が出力する情報を利用せずに上部旋回体3の傾斜角を推定できるためである。
演算装置50は、例えば、旋回操作レバーの操作内容に関する情報、旋回操作レバーが生成するパイロット圧に関する情報、上部旋回体3の旋回角度θsの変化に関する情報、及び、上部旋回体3の旋回角速度の変化に関する情報等の少なくとも1つに基づいて上部旋回体3が旋回するか否かを判定してもよい。そして、演算装置50は、上部旋回体3が旋回すると判定した場合に、機体傾斜センサS4の出力に基づき、旋回開始前における上部旋回体3に作用する加速度に関する情報を取得してもよい。
この構成により、演算装置50は、旋回開始前における上部旋回体3の傾斜角と、旋回角度θsとに基づき、旋回中の現在の上部旋回体3の傾斜角を推定できる。すなわち、演算装置50は、上部旋回体3が旋回中であっても遠心加速度等による影響を受けずに上部旋回体3の傾斜角を導き出すことができる。
演算装置50は、例えば、旋回操作レバーの操作内容に関する情報、旋回操作レバーが生成するパイロット圧に関する情報、上部旋回体3の旋回角度θsの変化に関する情報、及び、上部旋回体3の旋回角速度の変化に関する情報等の少なくとも1つである基本情報に基づいて上部旋回体3が旋回しているか否かを判定してもよい。そして、演算装置50は、上部旋回体3が旋回していると判定した場合に、過去の基本情報に基づいて旋回開始時点を推定し、その旋回開始時点の前における機体傾斜センサS4の出力に基づき、旋回開始前における上部旋回体3に作用する加速度に関する情報を取得してもよい。
この構成により、演算装置50は、旋回開始時点をリアルタイムに特定できなかった場合であっても、上部旋回体3が旋回していると判定した時点で、旋回開始時点を特定できる。すなわち、演算装置50は、上部旋回体3の旋回が開始した後で、揮発性記憶装置等に記憶された基本情報に関する過去のデータに基づいて旋回開始時点を特定できる。その結果、演算装置50は、旋回開始時点における上部旋回体3の傾斜角及び基準角度位置を特定でき、それら特定した情報に基づいて旋回中の上部旋回体3の傾斜角を正確に推定できる。
旋回角度センサS5は、例えば、旋回用モータの回転を測定するポテンショメータ若しくはロータリエンコーダが出力する情報、上部旋回体3の角速度を測定する角速度センサが出力する情報、又は、上部旋回体3に取り付けられたカメラ若しくはライダが出力する情報等に基づき、上部旋回体3の旋回角度に関する情報を取得する。旋回用モータは、旋回用油圧モータであってもよく、旋回用電動発電機であってもよい。或いは、旋回角度センサS5は、旋回用油圧モータに流入する作動油の流量に関する情報等に基づき、上部旋回体3の旋回角度に関する情報を取得してもよい。
この構成により、旋回角度センサS5は、上部旋回体3の旋回角度に関する情報を確実且つ容易に取得できる。
演算装置50は、例えば、機体傾斜センサS4の出力に基づいてセンサ座標系における旋回開始前の重力加速度ベクトルを導き出してもよい。そして、演算装置50は、上部旋回体3が旋回しているときには、旋回角度センサS5の出力に基づき、所定の制御周期毎にセンサ座標系と重力加速度ベクトルとを相対的に回転させ、回転させた後のセンサ座標系と重力加速度ベクトルとの関係から上部旋回体3の傾斜角を推定してもよい。
この構成により、演算装置50は、旋回中に上部旋回体3に作用する遠心加速度等の影響を受けることなく、旋回中の上部旋回体3の傾斜角を正確に推定できる。
演算装置50は、例えば、上部旋回体3が旋回しているときに、所定の制御周期毎に推定される上部旋回体3の傾斜角に基づいて作業部位の位置を算出してもよい。
この構成により、演算装置50は、上部旋回体3の旋回中であっても、遠心加速度等の影響を受けることなく、作業部位の位置を継続的に且つ正確に算出できる。その結果、演算装置50は、例えば、上部旋回体3の旋回中に実行されるマシンコントロール機能の位置決め精度を高めることができる。
以上、本発明の好ましい実施形態について詳説した。しかしながら、本発明は、上述した実施形態に制限されることはない。上述した実施形態は、本発明の範囲を逸脱することなしに、種々の変形又は置換等が適用され得る。また、別々に説明された特徴は、技術的な矛盾が生じない限り、組み合わせが可能である。
例えば、上述の実施形態は、旋回角度センサS5が取り付けられた上部旋回体3の傾斜角を計測する機体傾斜センサS4を備えるショベルに関する。しかしながら、本発明は、旋回角度センサS5に相当するエンコーダ等のセンサが取り付けられた、上部旋回体3に相当する回転部の傾斜角を計測する傾斜センサ(加速度センサ)を備える他の建設機械にも同様に適用される。