JP2023050803A

JP2023050803A - 作業機械を制御するためのシステムおよび方法

Info

Publication number: JP2023050803A
Application number: JP2021161093A
Authority: JP
Inventors: 光鈴木; Hikari Suzuki; 力岩村; Riki Iwamura; 匠野崎; Takumi Nozaki; 竜二神田; Ryuji Kanda; 大司岩永; Daiji Iwanaga; 友一平尾; Yuichi Hirao; 悠太内田; Yuta Uchida; 佑基島野; Yuki Shimano; 淳佐々木; Atsushi Sasaki; 仁北嶋; Jin Kitajima
Original assignee: Komatsu Ltd
Current assignee: Komatsu Ltd
Priority date: 2021-09-30
Filing date: 2021-09-30
Publication date: 2023-04-11
Also published as: WO2023054603A1; DE112022003357T5; CN117836489A; KR20240033070A

Abstract

【課題】チルトローテータが取り付けられた作業機械の操作を支援する。【解決手段】計測値取得部は、複数のセンサから計測値を取得する。姿勢算出部は、計測値に基づいて、車体に対するアタッチメントの姿勢を算出する。操作信号取得部は、操作装置から操作信号を取得する。介入制御部は、操作信号に基づいて、アタッチメントの姿勢が保持されるようにチルトローテータの制御信号を生成する。出力部は、生成した制御信号を出力する。【選択図】図１

Description

本開示は、作業機械を制御するためのシステムおよび方法に関する。

特許文献１には、バケットの刃先の角度を傾斜可能なチルトバケットを備える作業機械において、バケットを傾斜した設計面に沿って移動させる技術が開示されている。チルトバケットのチルト軸は、バケットの開口方向に伸びる。

国際公開第２０１６／１８６２１９号

ところで、作業機械のアタッチメントを互いに直交する３つの軸回りに回転可能に支持するチルトローテータという部品が知られている。作業機械にチルトローテータを取り付けることで、アタッチメントを任意の方向に向けることができる。しかしながら、チルトローテータは回転の自由度が高い一方で、オペレータによる操作が困難となる。特許文献１にはチルト軸回りの動作を自動化することができるが、チルトローテータを備える作業機械の制御は開示されていない。
本開示の目的は、チルトローテータを介して支持部に支持されたアタッチメントを備える作業機械の操作を支援することができるシステムおよび方法を提供することにある。

本開示の一態様によれば、車体に動作可能に支持された支持部と、支持部の先端に取り付けられたチルトローテータと、チルトローテータを介して支持部に対して互いに異なる平面で交差する３つの軸回りに回転可能に支持されたアタッチメントとを備える作業機械を制御するためのシステムであって、プロセッサを備える。プロセッサは、複数のセンサから計測値を取得する。プロセッサは、計測値に基づいて、車体に対するアタッチメントの姿勢を算出する。プロセッサは、操作装置からの操作信号に基づいて、アタッチメントの姿勢が保持されるようにチルトローテータの制御信号を生成し、生成した制御信号を出力する。

上記態様によれば、システムはチルトローテータを介して支持部に支持されたアタッチメントを備える作業機械の操作を支援することができる。

第１の実施形態に係る作業機械の構成を示す概略図である。第１の実施形態に係るチルトローテータの構成を示す図である。第１の実施形態に係る作業機械の駆動系を示す図である。第１の実施形態に係る制御装置の構成を示す概略ブロック図である。第１の実施形態におけるバケット姿勢保持制御を示すフローチャートである。

〈第１の実施形態〉
《作業機械の構成》
以下、図面を参照しながら実施形態について詳しく説明する。
図１は、第１の実施形態に係る作業機械１００の構成を示す概略図である。第１の実施形態に係る作業機械１００は、例えば油圧ショベルである。作業機械１００は、走行体１２０、旋回体１４０、作業機１６０、運転室１８０、制御装置２００を備える。第１の実施形態に係る作業機械１００は、バケット１６４の刃先が設計面を越えないように制御する。

走行体１２０は、作業機械１００を走行可能に支持する。走行体１２０は、例えば左右１対の無限軌道である。
旋回体１４０は、走行体１２０に旋回中心回りに旋回可能に支持される。旋回体１４０は車体の一例である。走行体１２０は旋回体１４０を旋回可能に支持する基部の一例である。
作業機１６０は、旋回体１４０に動作可能に支持される。作業機１６０は、油圧により駆動する。作業機１６０は、ブーム１６１、アーム１６２、チルトローテータ１６３、およびアタッチメントであるバケット１６４を備える。ブーム１６１の基端部は、旋回体１４０に回動可能に取り付けられる。アーム１６２の基端部は、ブーム１６１の先端部に回動可能に取り付けられる。チルトローテータ１６３は、アーム１６２の先端部に回動可能に取り付けられる。バケット１６４は、チルトローテータ１６３に取り付けられる。バケット１６４は、チルトローテータ１６３を介して作業機１６０に対して互いに異なる平面で交差する３つの軸回りに回転可能に支持される。ここで、旋回体１４０のうち作業機１６０が取り付けられる部分を前部という。また、旋回体１４０について、前部を基準に、反対側の部分を後部、左側の部分を左部、右側の部分を右部という。ブーム１６１およびアーム１６２は、旋回体１４０に動作可能に支持された支持部の一例である。

図２は、第１の実施形態に係るチルトローテータ１６３の構成を示す図である。チルトローテータ１６３は、バケット１６４を支持するようにアーム１６２の先端に取り付けられる。チルトローテータ１６３は、取付部１６３１、チルト部１６３２、回転部１６３３を備える。取付部１６３１は、図示左右方向に伸びる軸回りに回転可能にアーム１６２の先端に取り付けられる。チルト部１６３２は、図示前後方向に伸びる軸回りに回転可能に取付部１６３１に取り付けられる。回転部１６３３は、図示上下方向に伸びる軸回りに回転可能にチルト部１６３２に取り付けられる。理想的には、取付部１６３１、チルト部１６３２、回転部１６３３の回転軸は互いに直交する。バケット１６４の基端部は、回転部１６３３に固定される。これにより、バケット１６４は、アーム１６２に対して互いに直交する３軸を中心に回転することができる。ただし、実際には取付部１６３１、チルト部１６３２、回転部１６３３の回転軸は設計誤差を含み、必ずしも直交しない可能性がある。

運転室１８０は、旋回体１４０の前部に設けられる。運転室１８０内には、オペレータが作業機械１００を操作するための操作装置２７１、および制御装置２００のマンマシンインタフェースであるモニタ装置２７２が設けられる。操作装置２７１は、オペレータからの走行モータ３０４の操作量、旋回モータ３０５の操作量、ブームシリンダ３０６の操作量、アームシリンダ３０７の操作量、バケットシリンダ３０８の操作量、チルトシリンダ３０９の操作量、および回転モータ３１０の操作量の入力を受け付ける。操作装置２７１は、作業機械の操作量を示す操作信号を出力する。操作装置２７１は、オペレータによって操作され、ブーム１６１およびアーム１６２を動作させるための操作信号を出力する。操作装置２７１は、オペレータによって操作され、走行体１２０に対して旋回体１４０を旋回させるための操作信号を出力する。操作装置２７１は、オペレータによって操作され、チルトローテータ１６３を動作させるための操作信号を出力する。モニタ装置２７２は、オペレータからのバケット姿勢保持制御の設定および解除の入力を受け付ける。バケット姿勢保持制御とは、制御装置２００が自動的にグローバル座標系におけるバケット１６４の姿勢を保持するためにバケットシリンダ３０８、チルトシリンダ３０９および回転モータ３１０を制御する。モニタ装置２７２は、例えばタッチパネルを備えるコンピュータによって実現される。

制御装置２００は、オペレータによる操作装置２７１の操作に基づいて、走行体１２０、旋回体１４０、および作業機１６０を制御する。制御装置２００は、例えば運転室１８０の内部に設けられる。

《作業機械１００の駆動系》
図３は、第１の実施形態に係る作業機械１００の駆動系を示す図である。
作業機械１００は、作業機械１００を駆動するための複数のアクチュエータを備える。具体的には、作業機械１００は、エンジン３０１、油圧ポンプ３０２、コントロールバルブ３０３、一対の走行モータ３０４、旋回モータ３０５、ブームシリンダ３０６、アームシリンダ３０７、バケットシリンダ３０８、チルトシリンダ３０９、回転モータ３１０を備える。

エンジン３０１は、油圧ポンプ３０２を駆動する原動機である。
油圧ポンプ３０２は、エンジン３０１により駆動され、コントロールバルブ３０３を介して走行モータ３０４、旋回モータ３０５、ブームシリンダ３０６、アームシリンダ３０７およびバケットシリンダ３０８に作動油を供給する。
コントロールバルブ３０３は、油圧ポンプ３０２から走行モータ３０４、旋回モータ３０５、ブームシリンダ３０６、アームシリンダ３０７およびバケットシリンダ３０８へ供給される作動油の流量を制御する。
走行モータ３０４は、油圧ポンプ３０２から供給される作動油によって駆動され、走行体１２０を駆動する。
旋回モータ３０５は、油圧ポンプ３０２から供給される作動油によって駆動され、走行体１２０に対して旋回体１４０を旋回させる。

ブームシリンダ３０６は、ブーム１６１を駆動するための油圧シリンダである。ブームシリンダ３０６の基端部は、旋回体１４０に取り付けられる。ブームシリンダ３０６の先端部は、ブーム１６１に取り付けられる。
アームシリンダ３０７は、アーム１６２を駆動するための油圧シリンダである。アームシリンダ３０７の基端部は、ブーム１６１に取り付けられる。アームシリンダ３０７の先端部は、アーム１６２に取り付けられる。
バケットシリンダ３０８は、チルトローテータ１６３およびバケット１６４を駆動するための油圧シリンダである。バケットシリンダ３０８の基端部は、アーム１６２に取り付けられる。バケットシリンダ３０８の先端部は、リンク部材を介してチルトローテータ１６３に取り付けられる。

チルトシリンダ３０９は、チルト部１６３２を駆動するための油圧シリンダである。チルトシリンダ３０９の基端部は、取付部１６３１に取り付けられる。チルトシリンダ３０９の先端部は、チルト部１６３２に取り付けられる。
回転モータ３１０は、回転部１６３３を駆動するための油圧モータである。回転モータ３１０のブラケットおよび固定子は、チルト部１６３２に固定される。回転モータ３１０の回転軸および回転子は、図示上下方向に伸びるように設けられ、回転部１６３３に固定される。

《作業機械１００の計測系》
作業機械１００は、作業機械１００の姿勢、方位および位置を計測するための複数のセンサを備える。具体的には、作業機械１００は、傾斜計測器４０１、位置方位計測器４０２、ブーム角センサ４０３、アーム角センサ４０４、バケット角センサ４０５、チルト角センサ４０６、回転角センサ４０７を備える。

傾斜計測器４０１は、旋回体１４０の姿勢を計測する。傾斜計測器４０１は、水平面に対する旋回体１４０の傾き（例えば、ロール角、ピッチ角およびヨー角）を計測する。傾斜計測器４０１の例としては、ＩＭＵ（Inertial Measurement Unit：慣性計測装置）が挙げられる。この場合、傾斜計測器４０１は、旋回体１４０の加速度および角速度を計測し、計測結果に基づいて水平面に対する旋回体１４０の傾きを算出する。傾斜計測器４０１は、例えば運転室１８０の下方に設置される。傾斜計測器４０１は、計測値である旋回体１４０の姿勢データを制御装置２００へ出力する。

位置方位計測器４０２は、ＧＮＳＳ（Global Navigation Satellite System）により旋回体１４０の代表点の位置および旋回体１４０が向く方位を計測する。位置方位計測器４０２は、例えば旋回体１４０に取り付けられた図示しない２つのＧＮＳＳアンテナを備え、２つのアンテナの位置を結ぶ直線に直交する方位を作業機械１００が向く方位として検出する。位置方位計測器４０２は、計測値である旋回体１４０の位置データおよび方位データを制御装置２００へ出力する。

ブーム角センサ４０３は、旋回体１４０に対するブーム１６１の角度であるブーム角を計測する。ブーム角センサ４０３は、ブーム１６１に取り付けられたＩＭＵであってよい。この場合、ブーム角センサ４０３は、ブーム１６１の水平面に対する傾きと傾斜計測器４０１が計測した旋回体の傾きとに基づいて、ブーム角を計測する。ブーム角センサ４０３の計測値は、例えば、ブーム１６１の基端と先端とを通る直線の方向が旋回体１４０の前後方向と一致するときにゼロを示す。なお、他の実施形態係るブーム角センサ４０３は、ブームシリンダ３０６に取り付けられたストロークセンサであってもよい。また、他の実施形態に係るブーム角センサ４０３は、旋回体１４０とブーム１６１とを回転可能に接続する関節軸に設けられた回転センサであってもよい。ブーム角センサ４０３は、計測値であるブーム角データを制御装置２００へ出力する。

アーム角センサ４０４は、ブーム１６１に対するアーム１６２の角度であるアーム角を計測する。アーム角センサ４０４は、アーム１６２に取り付けられたＩＭＵであってよい。この場合、アーム角センサ４０４は、アーム１６２の水平面に対する傾きとブーム角センサ４０３が計測したブーム角とに基づいて、アーム角を計測する。アーム角センサ４０４の計測値は、例えば、アーム１６２の基端と先端とを通る直線の方向がブーム１６１の基端と先端とを通る直線の方向と一致するときにゼロを示す。なお、他の実施形態に係るアーム角センサ４０４は、アームシリンダ３０７にストロークセンサを取付けて角度の算出を行ってもよい。また、他の実施形態に係るアーム角センサ４０４は、ブーム１６１とアーム１６２とを回転可能に接続する関節軸に設けられた回転センサであってもよい。アーム角センサ４０４は、計測値であるアーム角データを制御装置２００へ出力する。

バケット角センサ４０５は、アーム１６２に対するチルトローテータ１６３の角度であるバケット角を計測する。バケット角センサ４０５は、バケットシリンダ３０８に設けられたストロークセンサであってよい。この場合、バケット角センサ４０５は、バケットシリンダ３０８のストローク量に基づいてバケット角を計測する。バケット角センサ４０５の計測値は、例えば、バケット１６４の基端と刃先とを通る直線の方向がアーム１６２の基端と先端とを通る直線の方向と一致するときにゼロを示す。なお、他の実施形態に係るバケット角センサ４０５は、アーム１６２とチルトローテータ１６３の取付部１６３１とを回転可能に接続する関節軸に設けられた回転センサであってもよい。また、他の実施形態に係るバケット角センサ４０５は、バケット１６４に取付けられたＩＭＵであってもよい。バケット角センサ４０５は、計測値であるバケット角データを制御装置２００へ出力する。

チルト角センサ４０６は、チルトローテータ１６３の取付部１６３１に対するチルト部１６３２の角度であるチルト角を計測する。チルト角センサ４０６は、取付部１６３１とチルト部１６３２とを回転可能に接続する関節軸に設けられた回転センサであってよい。チルト角センサ４０６の計測値は、例えばアーム１６２の回転軸と回転部１６３３の回転軸とが直交するときにゼロを示す。なお、他の実施形態に係るチルト角センサ４０６は、チルトシリンダ３０９にストロークセンサを取付けて角度の算出を行ってもよい。チルト角センサ４０６は、計測値であるチルト角データを制御装置２００へ出力する。

回転角センサ４０７は、チルトローテータ１６３のチルト部１６３２に対する回転部１６３３の角度である回転角を計測する。回転角センサ４０７は、回転モータ３１０に設けられた回転センサであってよい。チルト角センサ４０６の計測値は、例えばバケット１６４の刃先が向く方向と作業機１６０の動作平面とが平行となるときにゼロを示す。回転角センサ４０７は、計測値である回転角データを制御装置２００へ出力する。

《制御装置２００の構成》
図４は、第１の実施形態に係る制御装置２００の構成を示す概略ブロック図である。
制御装置２００は、プロセッサ２１０、メインメモリ２３０、ストレージ２５０、インタフェース２７０を備えるコンピュータである。制御装置２００は、制御システムの一例である。制御装置２００は、傾斜計測器４０１、位置方位計測器４０２、ブーム角センサ４０３、アーム角センサ４０４、バケット角センサ４０５、チルト角センサ４０６および回転角センサ４０７から計測値を受信する。

ストレージ２５０は、一時的でない有形の記憶媒体である。ストレージ２５０の例としては、磁気ディスク、光ディスク、光磁気ディスク、半導体メモリ等が挙げられる。ストレージ２５０は、制御装置２００のバスに直接接続された内部メディアであってもよいし、インタフェース２７０または通信回線を介して制御装置２００に接続される外部メディアであってもよい。操作装置２７１およびモニタ装置２７２は、インタフェース２７０を介してプロセッサ２１０に接続される。

ストレージ２５０は、作業機械１００を制御するための制御プログラムを記憶する。制御プログラムは、制御装置２００に発揮させる機能の一部を実現するためのものであってもよい。例えば、制御プログラムは、ストレージ２５０に既に記憶されている他のプログラムとの組み合わせ、または他の装置に実装された他のプログラムとの組み合わせによって機能を発揮させるものであってもよい。なお、他の実施形態においては、制御装置２００は、上記構成に加えて、または上記構成に代えてＰＬＤ（Programmable Logic Device）などのカスタムＬＳＩ（Large Scale Integrated Circuit）を備えてもよい。ＰＬＤの例としては、ＰＡＬ(Programmable Array Logic)、ＧＡＬ(Generic Array Logic)、ＣＰＬＤ(Complex Programmable Logic Device)、ＦＰＧＡ（Field Programmable Gate Array）が挙げられる。この場合、プロセッサによって実現される機能の一部または全部が当該集積回路によって実現されてよい。

ストレージ２５０には、旋回体１４０、ブーム１６１、アーム１６２及びバケット１６４の寸法及び重心位置を表すジオメトリデータが記録される。ジオメトリデータは、所定の座標系における物体の位置を表すデータである。またストレージ２５０には、グローバル座標系における施工現場の設計面の形状を表す三次元データである設計面データが記録される。グローバル座標系は、緯線方向に伸びるＸ_ｇ軸、経線方向に伸びるＹ_ｇ軸、鉛直方向に伸びるＺ_ｇ軸から構成される座標系である。設計面データは、例えばＴＩＮ（Triangular Irregular Networks）データによって表される。

《ソフトウェア構成》
プロセッサ２１０は、制御プログラムを実行することで、操作信号取得部２１１、入力部２１２、表示制御部２１３、計測値取得部２１４、位置姿勢算出部２１５、介入判定部２１６、介入制御部２１７、制御信号出力部２１８を備える。

操作信号取得部２１１は、操作装置２７１から各アクチュエータの操作量を示す操作信号を取得する。
入力部２１２は、モニタ装置２７２からオペレータによる操作入力を受け付ける。
表示制御部２１３は、モニタ装置２７２に表示させる画面データをモニタ装置２７２へ出力する。
計測値取得部２１４は、傾斜計測器４０１、位置方位計測器４０２、ブーム角センサ４０３、アーム角センサ４０４、バケット角センサ４０５、チルト角センサ４０６および回転角センサ４０７から計測値を取得する。

位置姿勢算出部２１５は、計測値取得部２１４が取得した各種計測値とストレージ２５０に記録されたジオメトリデータとに基づいて、グローバル座標系および車体座標系における作業機械１００の位置および姿勢を算出する。例えば位置姿勢算出部２１５は、グローバル座標系および車体座標系におけるバケット１６４の刃先の位置及び姿勢を算出する。車体座標系とは、旋回体１４０の代表点（例えば、旋回中心を通る点）を原点とする直交座標系である。位置姿勢算出部２１５の計算については後述する。位置姿勢算出部２１５は、旋回体１４０に対するバケット１６４の姿勢を算出する姿勢算出部の一例である。

介入判定部２１６は、位置姿勢算出部２１５が算出したバケット１６４の刃先の位置と設計面データが示す設計面との位置関係に基づいて、作業機１６０の速度を制限するか否かを判定する。以下、制御装置２００が作業機１６０の速度を制限することを介入制御ともいう。具体的には、介入判定部２１６は、設計面とバケット１６４との最短距離を求め、当該最短距離が所定距離以下である場合に、作業機１６０について介入制御をすると判定する。具体的には、介入判定部２１６は、傾斜計測器４０１および位置方位計測器４０２の計測値に基づいて、ストレージ２５０に記録された設計面データを回転および平行移動させることで、グローバル座標系で表された設計面の位置を車体座標系の位置に変換する。介入判定部２１６は、バケット１６４の複数の輪郭点のうち、最も設計面との距離が近いものを制御点として特定する。介入判定部２１６は、設計面データにおいて制御点の鉛直下方に位置する面（ポリゴン）を特定する。介入判定部２１６は、制御点を通るバケット座標系のＸ_ｂｋ－Ｚ_ｂｋ平面に平行な面と、特定した面との交線である第１設計線を算出する。介入判定部２１６は、制御点と第１設計線との距離が介入閾値以下であるか否かを判定する。また、介入判定部２１６は、入力部２１２がモニタ装置２７２からバケット姿勢保持制御の設定または解除の入力を受け付けたか否かに基づいて、バケット姿勢保持制御が設定されているか否かを判定する。

介入制御部２１７は、介入判定部２１６によって介入制御を行うと判定された場合に、操作信号取得部２１１が取得した操作量のうち介入対象の操作量を制御する。介入制御において介入制御部２１７は、設計線に作業機１６０が侵入しないように、ブーム１６１の操作量を制御する。これにより、バケット１６４の速度がバケット１６４と設計線との距離に応じた速度となるように、ブーム１６１が作動する。つまり介入制御部２１７は、オペレータがアーム１６２を操作して掘削作業を行うときに、設計面に応じてブーム１６１を上昇させることでバケット１６４の刃先の速度を制限する。

制御信号出力部２１８は、操作信号取得部２１１が取得した操作量、または介入制御部２１７によって制御された操作量をコントロールバルブ３０３に出力する。

《位置姿勢算出部２１５の計算》
ここで、位置姿勢算出部２１５による作業機械１００の外殻の点の位置の算出方法を説明する。位置姿勢算出部２１５は、計測値取得部２１４が取得した各種計測値とストレージ２５０に記録されたジオメトリデータとに基づいて外殻の点の位置を算出する。ストレージ２５０には、旋回体１４０、ブーム１６１、アーム１６２、チルトローテータ１６３（取付部１６３１、チルト部１６３２および回転部１６３３）およびバケット１６４の寸法を表すジオメトリデータが記録される。

旋回体１４０のジオメトリデータは、ローカル座標系である車体座標系において、旋回体１４０がブーム１６１を支持する関節軸の中心位置（ｘ_ｂｍ、ｙ_ｂｍ、ｚ_ｂｍ）を示す。車体座標系は、旋回体１４０の旋回中心を基準として前後方向に伸びるＸ_ｓｂ軸、左右方向に伸びるＹ_ｓｂ軸、上下方向に伸びるＺ_ｓｂ軸から構成される座標系である。なお、旋回体１４０の上下方向は、必ずしも鉛直方向と一致しない。

ブーム１６１のジオメトリデータは、ローカル座標系であるブーム座標系において、ブーム１６１がアーム１６２を支持する関節軸の位置（ｘ_ａｍ、ｙ_ａｍ、ｚ_ａｍ）を示す。ブーム座標系は、旋回体１４０とブーム１６１とを接続する関節軸の中心位置を基準として、長手方向に伸びるＸ_ｂｍ軸、関節軸が伸びる方向に伸びるＹ_ｂｍ軸、Ｘ_ｂｍ軸とＹ_ｂｍ軸に直交するＺ_ｂｍ軸から構成される座標系である。

アーム１６２のジオメトリデータは、ローカル座標系であるアーム座標系において、アーム１６２がチルトローテータ１６３の取付部１６３１を支持する関節軸の位置（ｘ_ｔ１、ｙ_ｔ１、ｚ_ｔ１）を示す。アーム座標系は、ブーム１６１とアーム１６２とを接続する関節軸の中心位置を基準として、長手方向に伸びるＸ_ａｍ軸、関節軸が伸びる方向に伸びるＹ_ａｍ軸、Ｘ_ａｍ軸とＹ_ａｍ軸に直交するＺ_ａｍ軸から構成される座標系である。

チルトローテータ１６３の取付部１６３１のジオメトリデータは、ローカル座標系である第１チルトローテート座標系において、取付部１６３１がチルト部１６３２を支持する関節軸の位置（ｘ_ｔ２、ｙ_ｔ２、ｚ_ｔ２）と関節軸の傾き（φ_ｔ）を示す。関節軸の傾きφ_ｔは、チルトローテータ１６３の設計誤差に係る角度であり、チルトローテータ１６３のキャリブレーション等によって求められる。第１チルトローテート座標系は、アーム１６２と取付部１６３１とを接続する関節軸の中心位置を基準として、アーム１６２と取付部１６３１とを接続する関節軸が伸びる方向に伸びるＹ_ｔ１軸、取付部１６３１とチルト部１６３２とを接続する関節軸が伸びる方向に伸びるＺ_ｔ１軸、およびＹ_ｔ１軸とＺ_ｔ１軸に直交するＸ_ｔ１軸から構成される座標系である。

チルトローテータ１６３のチルト部１６３２のジオメトリデータは、ローカル座標系である第２チルトローテート座標系における回転モータ３１０の回転軸の先端位置（ｘ_ｔ３ｂｋ、ｙ_ｔ３、ｚ_ｔ３）と回転軸の傾き（φ_ｒ）を示す。回転軸の傾きφ_ｒは、チルトローテータ１６３の設計誤差に係る角度であり、チルトローテータ１６３のキャリブレーション等によって求められる。第２チルトローテート座標系は、取付部１６３１とチルト部１６３２とを接続する関節軸の中心位置を基準として、取付部１６３１とチルト部１６３２とを接続する関節軸が伸びる方向に伸びるＸ_ｔ２軸、回転モータ３１０の回転軸が伸びる方向に伸びるＺ_ｔ２軸、およびＸ_ｔ２軸とＺ_ｔ２軸に直交するＹ_ｔ２軸から構成される座標系である。

チルトローテータ１６３の回転部１６３３のジオメトリデータは、ローカル座標系である第３チルトローテート座標系におけるバケット１６４の取り付け面の中心位置（ｘ_ｔ４、ｙ_ｔ４、ｚ_ｔ４）を示す。第３チルトローテート座標系は、バケット１６４の取り付け面の中心位置を基準として、回転モータ３１０の回転軸が伸びる方向に伸びるＺ_ｔ３軸、回転軸に直交するＸ_ｔ３軸およびＹ_ｔ３軸から構成される座標系である。なお、バケット１６４は、刃先がＹ_ｔ３軸と平行になるように回転部１６３３に取り付けられる。

バケット１６４のジオメトリデータは、第３チルトローテート座標系におけるバケット１６４の複数の輪郭点の位置（ｘ_ｂｋ、ｙ_ｂｋ、ｚ_ｂｋ）を示す。輪郭点の例としては、バケット１６４の刃先の両端および中央の位置、バケット１６４の底部の両端および中央の位置、ならびにバケット１６４の尻部の両端および中央の位置が挙げられる。

位置姿勢算出部２１５は、計測値取得部２１４が取得したブーム角θ_ｂｍの計測値と、旋回体１４０のジオメトリデータとに基づいて、下記式（１）により、ブーム座標系から車体座標系へ変換するためのブーム－車体変換行列Ｔ^ｂｍ _ｓｂを生成する。ブーム－車体変換行列Ｔ^ｂｍ _ｓｂは、Ｙ_ｂｍ軸回りにブーム角θ_ｂｍだけ回転させ、かつ車体座標系の原点とブーム座標系の原点の偏差（ｘ_ｂｍ、ｙ_ｂｍ、ｚ_ｂｍ）だけ平行移動させる行列である。

位置姿勢算出部２１５は、計測値取得部２１４が取得したアーム角θ_ａｍの計測値と、ブーム１６１のジオメトリデータとに基づいて、下記式（２）により、アーム座標系からブーム座標系へ変換するためのアーム－ブーム変換行列Ｔ^ａｍ _ｂｍを生成する。アーム－ブーム変換行列Ｔ^ａｍ _ｂｍは、Ｙ_ａｍ軸回りにアーム角θ_ａｍだけ回転させ、かつブーム座標系の原点とアーム座標系の原点の偏差（ｘ_ａｍ、ｙ_ａｍ、ｚ_ａｍ）だけ平行移動させる行列である。また、位置姿勢算出部２１５は、ブーム－車体変換行列Ｔ^ｂｍ _ｓｂとアーム－ブーム変換行列Ｔ^ａｍ _ｂｍの積を求めることで、アーム座標系から車体座標系へ変換するためのアーム－車体変換行列Ｔ^ａｍ _ｓｂを生成する。

位置姿勢算出部２１５は、計測値取得部２１４が取得したバケット角θ_ｂｋの計測値と、アーム１６２のジオメトリデータとに基づいて、下記式（３）により、第１チルトローテート座標系からアーム座標系へ変換するための第１チルト－アーム変換行列Ｔ^ｔ１ _ａｍを生成する。第１チルト－アーム変換行列Ｔ^ｔ１ _ａｍは、Ｙ_ｔ１軸回りにバケット角θ_ｂｋだけ回転させ、かつアーム座標系の原点と第１チルトローテート座標系の原点の偏差（ｘ_ｔ１、ｙ_ｔ１、ｚ_ｔ１）だけ平行移動させ、さらにチルト部１６３２の関節軸の傾きφ_ｔだけ傾ける行列である。また、位置姿勢算出部２１５は、アーム－車体変換行列Ｔ^ａｍ _ｓｂと第１チルト－アーム変換行列Ｔ^ｔ１ _ａｍの積を求めることで、第１チルトローテート座標系から車体座標系へ変換するための第１チルト－車体変換行列Ｔ^ｔ１ _ｓｂを生成する。

位置姿勢算出部２１５は、計測値取得部２１４が取得したチルト角θ_ｔの計測値と、チルトローテータ１６３のジオメトリデータとに基づいて、下記式（４）により、第１チルトローテート座標系から第２チルトローテート座標系へ変換するための第２チルト－第１チルト変換行列Ｔ^ｔ２ _ｔ１を生成する。第２チルト－第１チルト変換行列Ｔ^ｔ２ _ｔ１は、Ｘ_ｔ２軸回りにチルト角θ_ｔだけ回転させ、かつ第１チルトローテート座標系の原点と第２チルトローテート座標系の原点の偏差（ｘ_ｔ２、ｙ_ｔ２、ｚ_ｔ２）だけ平行移動させ、さらに回転部１６３３の回転軸の傾きφ_ｒだけ傾ける行列である。また、位置姿勢算出部２１５は、第１チルト－車体変換行列Ｔ^ｔ１ _ｓｂと第２チルト－第１チルト変換行列Ｔ^ｔ２ _ｔ１の積を求めることで、第２チルトローテート座標系から車体座標系へ変換するための第２チルト－車体変換行列Ｔ^ｔ２ _ｓｂを生成する。

位置姿勢算出部２１５は、計測値取得部２１４が取得したローテート角θ_ｒの計測値と、チルトローテータ１６３のジオメトリデータとに基づいて、下記式（５）により、第２チルトローテート座標系から第３チルトローテート座標系へ変換するための第３チルト－第２チルト変換行列Ｔ^ｔ３ _ｔ２を生成する。第３チルト－第２チルト変換行列Ｔ^ｔ３ _ｔ２は、Ｚ_ｔ３軸回りに回転角θ_ｒだけ回転させ、かつ第２チルトローテート座標系の原点と第３チルトローテート座標系の原点の偏差（ｘ_ｔ３、ｙ_ｔ３、ｚ_ｔ３）だけ平行移動させる行列である。また、位置姿勢算出部２１５は、第２チルト－車体変換行列Ｔ^ｔ２ _ｓｂと第３チルト－第２チルト変換行列Ｔ^ｔ３ _ｔ２の積を求めることで、第３チルトローテート座標系から車体座標系へ変換するための第３チルト－車体変換行列Ｔ^ｔ３ _ｓｂを生成する。

位置姿勢算出部２１５は、バケット１６４の取り付け面の中心位置（ｘ_ｔ４、ｙ_ｔ４、ｚ_ｔ４）とバケット１６４のジオメトリデータが示す第３チルトローテート座標系における複数の輪郭点の位置（ｘ_ｂｋ、ｙ_ｂｋ、ｚ_ｂｋ）との和と、第３チルト－車体変換行列Ｔ^ｂｋ _ｓｂとの積を求めることで、車体座標系におけるバケット１６４の複数の輪郭点の位置を求めることができる。

ところで、作業機械１００の接地面に対するバケット１６４の刃先の角度、すなわち車体座標系のＸ_ｓｂ－Ｙ_ｓｂ平面と第３チルトローテート座標系のＹ_ｔ３軸とがなす角は、ブーム角θ_ｂｍ、アーム角θ_ａｍ、バケット角θ_ｂｋ、チルト角θ_ｔおよびローテート角θ_ｒによって定まる。そこで、位置姿勢算出部２１５は、図１に示すように、バケット１６４の基端部、すなわちチルトローテータ１６３におけるバケット１６４の取り付け面の中心位置を起点とするバケット座標系を特定する。バケット座標系は、バケット１６４の刃先が向く方向に伸びるＸ_ｂｋ軸、Ｘ_ｂｋ軸に直交しかつバケット１６４の刃先に沿って伸びるＹ_ｂｋ軸、ならびにＸ_ｂｋ軸およびＹ_ｂｋ軸に直交するＺ_ｂｋ軸から構成される直交座標系である。以下、Ｘ_ｂｋ軸をバケットチルト軸、Ｙ_ｂｋ軸をバケットピッチ軸、Ｚ_ｂｋ軸をバケット回転軸ともいう。バケットチルト軸Ｘ_ｂｋ、バケットピッチ軸Ｙ_ｂｋおよびバケット回転軸Ｚ_ｂｋは仮想的な軸であり、チルトローテータ１６３の関節軸とは異なる。なお、回転モータ３１０の回転軸の傾きがゼロである場合、バケット座標系と第３チルトローテート座標系とは一致する。

位置姿勢算出部２１５は、チルトローテータ１６３のジオメトリデータに基づいて、下記式（６）により、第３チルトローテート座標系からバケット座標系へ変換するためのバケット－第３チルト変換行列Ｔ^ｂｋ _ｔ３を生成する。バケット－第３チルト変換行列Ｔ^ｂｋ _ｔ３は、Ｙ_ｔ３軸回りに回転軸の傾きφ_ｒだけ回転させる行列である。

《バケット姿勢保持制御》
以下、第１の実施形態に係るバケット姿勢保持制御について説明する。バケット姿勢保持制御は、グローバル座標系におけるバケットの姿勢を保持するための制御である。バケット姿勢保持制御は、ブーム動作、アーム動作および旋回動作の少なくともいずれかが行われた場合であっても、グローバル座標系におけるバケットの姿勢が保持されるようにチルトローテータ１６３を制御する。具体的には、バケット姿勢保持制御は、グローバル座標系におけるバケット座標系の３軸（バケットチルト軸Ｘ_ｂｋ，バケットピッチ軸Ｙ_ｂｋ，バケット回転軸Ｚ_ｂｋ）の軸方向が保持されるように、少なくともバケットシリンダ３０８、チルトシリンダ３０９および回転モータ３１０のいずれかを作動させる制御である。

図５は、第１の実施形態におけるバケット姿勢保持制御を示すフローチャートである。作業機械１００のオペレータが作業機械１００の操作を開始すると、制御装置２００は、以下に示す制御を所定の制御周期（例えば、１０００ミリ秒）ごとに実行する。

計測値取得部２１４は、傾斜計測器４０１、位置方位計測器４０２、ブーム角センサ４０３、アーム角センサ４０４、バケット角センサ４０５、チルト角センサ４０６および回転角センサ４０７の計測値を取得する（ステップＳ１０１）。

位置姿勢算出部２１５は、ステップＳ１０１で取得した計測値に基づいて車体座標系におけるバケットの姿勢を算出する（ステップＳ１０２）。車体座標系におけるバケットの姿勢は車体座標系におけるバケット座標系の各軸（Ｘ_ｂｋ、Ｙ_ｂｋ、Ｚ_ｂｋ）の方向を示す姿勢行列Ｒ_ｃｕｒによって表される。バケット１６４の姿勢を表す姿勢行列Ｒ_ｃｕｒの平行移動成分はすべてゼロとする。

次に、介入判定部２１６は、バケット姿勢保持制御が設定されているか否かを判定する（ステップＳ１０３）。第１の実施形態おいては、介入判定部２１６は、入力部２１２がモニタ装置２７２からバケット姿勢保持制御の設定または解除の入力を受け付けたか否かに基づいて、バケット姿勢保持制御が設定されているか否かを判定する。バケット姿勢保持制御が設定されていない場合（ステップＳ１０３：ＮＯ）、制御装置２００はバケット姿勢保持制御を行わない。他方、バケット姿勢保持制御が設定されている場合（ステップＳ１０３：ＹＥＳ）、介入判定部２１６は、操作信号取得部２１１が取得した操作装置２７１からの操作信号に基づいて、バケットシリンダ３０８、チルトシリンダ３０９および回転モータ３１０の何れかの操作を受けたか否かを判定する（ステップＳ１０４）。

バケットシリンダ３０８、チルトシリンダ３０９および回転モータ３１０の何れかの操作を受けている場合（ステップＳ１０４：ＹＥＳ）、オペレータがチルトローテータ１６３を操作する意志を持っていることが推測されるため、制御装置２００はバケット姿勢保持制御を行わない。他方、バケットシリンダ３０８、チルトシリンダ３０９および回転モータ３１０の何れかの操作も受けていない場合（ステップＳ１０４：ＮＯ）、介入制御部２１７は、操作信号取得部２１１が操作装置２７１から取得した旋回モータ３０５、ブーム１６１およびアーム１６２の操作量、ならびに計測値取得部２１４が取得した傾斜計測器４０１の計測値に基づいて、単位時間（制御周期）後のバケット１６４の姿勢を表す姿勢行列Ｒ_ｍａｎ求める（ステップＳ１０５）。

姿勢行列Ｒ_ｍａｎは、現在の作業機械１００の車体座標系で表される。つまり、旋回モータ３０５の旋回は姿勢行列Ｒ_ｍａｎに反映される。次に、介入制御部２１７は、ステップＳ１０２で算出したバケット１６４の姿勢行列Ｒ_ｃｕｒと姿勢行列Ｒ_ｍａｎを用いて、下記式（７）－（９）によって、バケットシリンダ３０８の角速度の目標値θ_{ｂｋ＿ｔｇｔ}、チルトシリンダ３０９の角速度の目標値θ_{ｔ＿ｔｇｔ}および回転モータ３１０の角速度の目標値θ_{ｒ＿ｔｇｔ}を求める（ステップＳ１０６）。

式（７）においてεは誤差閾値を示し、Ｋはゲインを示す。回転行列は正規直交行列であるため、回転行列の転置行列は回転行列の逆行列と等しい。そのため、式（７）－（９）によれば、介入制御部２１７は、操作信号取得部２１１が操作装置２７１から取得した操作量に従ってアクチュエータを作動させたときのバケット１６４の姿勢Ｒ_ｍａｎと現在のバケット１６４の姿勢Ｒ_ｃｕｒとの差分を相殺するための角速度θ_{ｂｋ＿ｔｇｔ}、θ_{ｔ＿ｔｇｔ}、θ_{ｒ＿ｔｇｔ}を求めることができる。介入制御部２１７は、ステップＳ２０２で求めた角速度の目標値に基づいて、各アクチュエータ（バケットシリンダ３０８、チルトシリンダ３０９および回転モータ３１０）の制御信号を生成する（ステップＳ１０７）。

そして、制御信号出力部２１８は、介入制御部２１７が生成した各アクチュエータ（バケットシリンダ３０８、チルトシリンダ３０９および回転モータ３１０）の制御信号をコントロールバルブ３０３に出力する（ステップＳ１０８）。

《作用・効果》

また、第１の実施形態によれば、オペレータが姿勢保持制御を設定することで、旋回体１４０、ブーム１６１およびアーム１６２が操作されてもグローバル座標系からみたバケット１６４の姿勢を一定に保持させることができる。例えば、設計面より十分に高い場所を掘削する場合などに、バケット１６４の姿勢を維持することで、容易に掘削方向に刃先を向けつづけさせることができる。また例えば、バケット１６４に代えてグラップルなどのアタッチメントを作業機１６０に取り付けて荷を移動させる場合に、グローバル座標系におけるアタッチメントの姿勢が保持されるように、チルトローテータを制御することで、姿勢変化による荷の落下を抑制することができる。

また、制御装置２００は、チルトローテータ１６３を操作するための操作信号、すなわちバケットシリンダ３０８、チルトシリンダ３０９および回転モータ３１０の何れかの操作信号が入力された場合に、介入制御部２１７はチルトローテータの制御信号を生成しない。オペレータによってチルトローテータ１６３を操作するための操作信号が入力されたということは、オペレータは、バケット１６４が向く方向を自ら操作したいという意志を持っている可能性が高い。そのため、このような場合に制御装置２００がチルトローテータの制御信号を生成しないことで、オペレータの操作を妨げない。

（第１の実施形態の変形例）
上述した第１の実施形態では、入力部２１２がモニタ装置２７２からバケット姿勢保持制御の設定の入力を受け付けることで、バケット姿勢保持制御が設定されるものとして説明した。つまり、第１の実施形態では、バケット姿勢保持制御はオペレータによる操作によって開始される。しかし、他の実施形態においてはこの態様に限定されない。例えば、第１の実施形態の変形例に係る制御装置２００は、以下のような機能を有していてもよい。

第１の実施形態の変形例に係る介入判定部２１６は、バケット姿勢保持制御の設定条件として、バケット１６４が地面から所定の距離だけ近づいた場合に、バケット姿勢保持制御を開始する。

バケット１６４が地面から所定の距離だけ近づいたかどうかの判定は、例えば、上述の介入判定部２１６の機能を利用できる。即ち、介入判定部２１６は、バケット１６４の刃先の位置と設計面との最短距離を時々刻々と求めている。そして、介入判定部２１６は、バケット１６４が下降する最中において、時々刻々と算出している最短距離が所定の判定閾値以下となった時点で所定の制御開始条件を満たしたと判定し、ステップＳ１０４の処理を開始する。

このようにすることで、バケット姿勢保持制御を実行するにあたりオペレータによる操作を排することができ、整地作業等の一層の簡便化を図ることができる。

〈他の実施形態〉
以上、図面を参照して一実施形態について詳しく説明してきたが、具体的な構成は上述のものに限られることはなく、様々な設計変更等をすることが可能である。すなわち、他の実施形態においては、上述の処理の順序が適宜変更されてもよい。また、一部の処理が並列に実行されてもよい。
上述した実施形態に係る制御装置２００は、単独のコンピュータによって構成されるものであってもよいし、制御装置２００の構成を複数のコンピュータに分けて配置し、複数のコンピュータが互いに協働することで制御装置２００として機能するものであってもよい。このとき、制御装置２００を構成する一部のコンピュータが作業機械の内部に搭載され、他のコンピュータが作業機械の外部に設けられてもよい。例えば、他の実施形態においては操作装置２７１およびモニタ装置２７２が作業機械１００から遠隔に設けられ、制御装置２００のうち計測値取得部２１４および制御信号出力部２１８以外の構成が遠隔のサーバに設けられてもよい。

また上述した実施形態に係る作業機械１００は油圧ショベルであるが、これに限られない。例えば、他の実施形態に係る作業機械１００は地上に固設された自走しない作業機械であってもよい。また他の実施形態に係る作業機械１００は旋回体を有しない作業機械であってもよい。

上述した実施形態に係る作業機械１００は、作業機１６０のアタッチメントとしてバケット１６４を備えるが、これに限られない。例えば、他の実施形態に係る作業機械１００は、アタッチメントとしてブレーカ、フォーク、グラップルなどを備えてもよい。この場合も、制御装置２００はバケット座標系と同様にアタッチメントの刃先が向く方向に伸びるＸ_ｂｋ軸と、刃先に沿う方向に伸びるＹ_ｂｋ軸と、Ｘ_ｂｋ軸およびＹ_ｂｋ軸に直交するＺ_ｂｋ軸とからなるローカル座標系によってチルトローテータ１６３を制御する。

また他の実施形態において、チルトローテータ１６３の各軸は互いに異なる平面で交差するものであれば、直交していなくてもよい。具体的には、アーム１６２と取付部１６３１とを接続する関節軸に係る軸ＡＸ１、取付部１６３１とチルト部１６３２とを接続する関節軸に係る軸ＡＸ２、および回転モータ３１０の回転軸ＡＸ３について、チルトローテータ１６３のチルト角および回転角がゼロであるときに、軸ＡＸ１および軸ＡＸ２に平行な面と、軸ＡＸ２および軸ＡＸ３に平行な面と、軸ＡＸ３および軸ＡＸ１に平行な面とが、それぞれ異なるものであればよい。

また、他の実施形態に係る制御装置２００は、設計面の設定機能を有しないものであってもよい。この場合にも、制御装置２００はバケット姿勢保持制御を行うことでチルトローテータ１６３を自動制御することができる。例えば、オペレータは、設計面を設定することなく簡易的な整地作業を実施することができる。

１００…作業機械１２０…走行体１４０…旋回体１６０…作業機１６１…ブーム１６２…アーム１６３…チルトローテータ１６３１…取付部１６３２…チルト部１６３３…回転部１６４…バケット１８０…運転室２００…制御装置２１０…プロセッサ２１１…操作信号取得部２１２…入力部２１３…表示制御部２１４…計測値取得部２１５…位置姿勢算出部２１６…介入判定部２１７…介入制御部２１８…制御信号出力部２３０…メインメモリ２５０…ストレージ２７０…インタフェース２７１…操作装置２７２…モニタ装置３０１…エンジン３０２…油圧ポンプ３０３…コントロールバルブ３０４…走行モータ３０５…旋回モータ３０６…ブームシリンダ３０７…アームシリンダ３０８…バケットシリンダ３０９…チルトシリンダ３１０…回転モータ４０１…傾斜計測器４０２…位置方位計測器４０３…ブーム角センサ４０４…アーム角センサ４０５…バケット角センサ４０６…チルト角センサ４０７…回転角センサ

Claims

車体に動作可能に支持された支持部と、前記支持部の先端に取り付けられたチルトローテータと、前記チルトローテータを介して前記支持部に対して互いに異なる平面で交差する３つの軸回りに回転可能に支持されたアタッチメントとを備える作業機械を制御するためのシステムであって、
プロセッサを備え、
前記プロセッサは、
複数のセンサから計測値を取得し、
前記計測値に基づいて、前記車体に対する前記アタッチメントの姿勢を算出し、
操作装置からの操作信号に基づいて、前記アタッチメントの前記姿勢が保持されるように前記チルトローテータの制御信号を生成し、
生成した前記制御信号を出力する
を備えるシステム。
前記プロセッサは、
前記操作装置から前記支持部を動作させるための操作信号を取得し、
前記支持部を動作させるための前記操作信号が示す操作量に基づいて、前記アタッチメントの前記姿勢が保持されるように前記チルトローテータの制御信号を生成する
請求項１に記載のシステム。
前記作業機械は、前記車体を旋回可能に支持する基部を備え、
前記プロセッサは、
前記操作装置から前記基部に対して前記車体を旋回させるための操作信号を取得し、
前記車体を旋回させるための前記操作信号が示す操作量に基づいて、前記アタッチメントの前記姿勢が保持されるように前記チルトローテータの制御信号を生成する
請求項１に記載のシステム。
前記プロセッサは、
グローバル座標系における前記アタッチメントの前記姿勢が保持されるように前記チルトローテータの制御信号を生成する
請求項１から請求項３のいずれか一項に記載のシステム。
前記プロセッサは、
前記操作信号が示す操作量に基づいて、前記アタッチメントが前記姿勢を保持するための前記３つの軸回りの角速度を算出する
請求項１から請求項４のいずれか一項に記載のシステム。
前記プロセッサは、前記操作装置から前記チルトローテータを動作させるための操作信号が入力された場合、生成した前記チルトローテータの制御信号を出力せず、入力された前記操作信号に基づく制御信号を出力する
請求項１から請求項５のいずれか一項に記載のシステム。
車体を旋回可能に支持する基部と、前記車体に動作可能に支持された支持部と、前記支持部の先端に取り付けられたチルトローテータと、前記チルトローテータを介して前記支持部に対して互いに異なる平面で交差する３つの軸回りに回転可能に支持されたアタッチメントとを備える作業機械を制御するための方法であって、
複数のセンサから計測値を取得するステップと、
前記計測値に基づいて、前記車体に対する前記アタッチメントの姿勢を算出するステップと、
操作装置からの操作信号に基づいて、前記アタッチメントの前記姿勢が保持されるように前記チルトローテータの制御信号を生成するステップと、
生成した前記制御信号に従って前記チルトローテータを制御するステップと
を備える方法。