JPWO2020158779A1

JPWO2020158779A1 - 軌道追従システム、軌道追従方法、及び、プログラム

Info

Publication number: JPWO2020158779A1
Application number: JP2020569669A
Authority: JP
Inventors: 達也吉本; 裕志吉田
Original assignee: NEC Corp
Current assignee: NEC Corp
Priority date: 2019-01-30
Filing date: 2020-01-29
Publication date: 2021-11-25
Anticipated expiration: 2040-01-29
Also published as: WO2020158779A1; US20220098835A1; JP7120332B2

Abstract

軌道追従制御部（２２）は、バケット（６）と、バケット（６）を支持するブーム（４）及びアーム（５）と、を含む複数の機構を備えたバックホウ（１）においてバケット（６）を目標軌道に追従制御する。収束判定部（３４）は、バケット（６）の状態が現在の目標状態に収束したか否かを判定する。目標状態更新部（３１）は、バケット（６）の状態が現在の目標状態に収束したと収束判定部（３４）が判定したら、バケット（６）の目標状態を更新するように構成されている。収束判定部（３４）は、第１収束判定部（３５）と第２収束判定部（３６）を備える。第１収束判定部（３５）は、各機構の揺動角度が対応する目標揺動角度に第１収束条件で収束したか否かを判定する。第２収束判定部（３６）は、各機構の揺動角度が対応する目標揺動角度に第１収束条件よりもきつい第２収束条件で収束したか否かを判定する。

Description

本発明は、軌道追従システム、軌道追従方法、及び、コンピュータ可読媒体に関する。

特許文献１は、土砂をダンプトラックやクラッシャ等に自動的に投入するための油圧ショベルの自動掘削方法を開示している。具体的には、オペレータが油圧ショベルを作動させた後、その作動をティーチングによりコントローラに記憶させ、以後、コントローラがオペレータに代わって油圧ショベルを作動させるようになっている。

特開平１０−２１２７４０号公報

油圧ショベルを自動制御する場合、例えば、オペレータが予めバケットの目標軌道を制御装置に入力し、この制御装置が目標軌道に沿ってバケットを移動させることが考えられる。このとき、制御装置は、例えば、目標軌道上で複数の目標点を定め、ｉ番目の目標点の近傍にバケットが収束したと判定したら、ｉ＋１番目の目標点にバケットが向かうように、バケットの目標状態を繰り返し更新することが考えられる。しかしながら、上記の収束判定に関して改善の余地が残されていた。

本開示の目的は、上述した課題の何れかを解決する技術を提供することにある。

本開示の第１の観点によれば、エンドエフェクタと、前記エンドエフェクタを支持する少なくとも１つの支持部と、を含む複数の機構を備えた作業機器において、前記エンドエフェクタを目標軌道に追従制御する軌道追従システムが提供される。軌道追従システムは、以下を備える。前記エンドエフェクタの少なくとも目標座標を含む目標状態を更新する目標状態更新部。前記エンドエフェクタの現在の目標状態に基づいて前記複数の機構の目標相対姿勢をそれぞれ算出する目標相対姿勢算出部。前記複数の機構の相対姿勢がそれぞれ対応する目標相対姿勢へ近づくように前記複数の機構の相対姿勢を制御する相対姿勢制御部。前記エンドエフェクタの状態が現在の目標状態に収束したか否かを判定する収束判定部。前記目標状態更新部は、前記エンドエフェクタの状態が現在の目標状態に収束したと前記収束判定部が判定したら、前記エンドエフェクタの目標状態を更新するように構成されている。前記収束判定部は、各機構の相対姿勢が対応する目標相対姿勢に第１収束条件で収束したか否かを判定する第１収束判定部を備える。前記収束判定部は、各機構の相対姿勢が対応する目標相対姿勢に前記第１収束条件よりもきつい第２収束条件で収束したか否かを判定する第２収束判定部と、を備える。

本開示の第２の観点によれば、エンドエフェクタと、前記エンドエフェクタを支持する少なくとも１つの支持部と、を含む複数の機構を備えた作業機器において、前記エンドエフェクタを目標軌道に追従制御する軌道追従方法が提供される。軌道追従方法は、以下を含む。前記エンドエフェクタの少なくとも目標座標を含む目標状態を更新する目標状態更新ステップ。前記エンドエフェクタの現在の目標状態に基づいて前記複数の機構の目標相対姿勢をそれぞれ算出する目標相対姿勢算出ステップ。前記複数の機構の相対姿勢がそれぞれ対応する目標相対姿勢へ近づくように前記複数の機構の相対姿勢を制御する相対姿勢制御ステップ。前記エンドエフェクタの状態が現在の目標状態に収束したか否かを判定する収束判定ステップ。前記目標状態更新ステップでは、前記エンドエフェクタの状態が現在の目標状態に収束したと前記収束判定ステップで判定したら、前記エンドエフェクタの目標状態を更新する。前記収束判定ステップは、各機構の相対姿勢が対応する目標相対姿勢に第１収束条件で収束したか否かを判定する第１収束判定ステップを含む。前記収束判定ステップは、各機構の相対姿勢が対応する目標相対姿勢に前記第１収束条件よりもきつい第２収束条件で収束したか否かを判定する第２収束判定ステップと、を含む。

本発明によれば、収束判定に関する新しい技術が提供される。

軌道追従システムの機能ブロック図である。（第１実施形態）バックホウの側面図である。（第２実施形態）バックホウの機能ブロック図である。（第２実施形態）バケットの目標軌道の一例を示す図である。（第２実施形態）目標状態及び収束条件を示すテーブルである。（第２実施形態）バックホウの制御フローである。（第２実施形態）

（第１実施形態）
先ず、図１を参照して、第１実施形態の軌道追従システム１００を説明する。

軌道追従システム１００は、エンドエフェクタと、エンドエフェクタを支持する少なくとも１つの支持部と、を含む複数の機構を備えた作業機器において、エンドエフェクタを目標軌道に追従制御するシステムである。複数の機構は、エンドエフェクタと、エンドエフェクタを支持する少なくとも１つの支持部と、を含む。

軌道追従システム１００は、目標状態更新部１０１と、目標相対姿勢算出部１０２と、相対姿勢制御部１０３と、収束判定部１０４と、を備える。

目標状態更新部１０１は、エンドエフェクタの少なくとも目標座標を含む目標状態を更新する。

目標相対姿勢算出部１０２は、エンドエフェクタの現在の目標状態に基づいて複数の機構の目標相対姿勢をそれぞれ算出する。

相対姿勢制御部１０３は、複数の機構の相対姿勢がそれぞれ対応する目標相対姿勢へ近づくように複数の機構の相対姿勢を制御する。

収束判定部１０４は、エンドエフェクタの状態が現在の目標状態に収束したか否かを判定する。

そして、目標状態更新部１０１は、エンドエフェクタの状態が現在の目標状態に収束したと収束判定部１０４が判定したら、エンドエフェクタの目標状態を更新するように構成されている。

更に、収束判定部１０４は、第１収束判定部１０５と、第２収束判定部１０６と、を備える。

第１収束判定部１０５は、各機構の相対姿勢が対応する目標相対姿勢に第１収束条件で収束したか否かを判定する。

第２収束判定部１０６は、各機構の相対姿勢が対応する目標相対姿勢に第１収束条件よりもきつい第２収束条件で収束したか否かを判定する。

以上の構成によれば、収束判定に関する新しい技術が提供される。即ち、柔軟な収束判定を実現することができる。

（第２実施形態）
次に、図２から図６を参照して、バックホウ１について説明する。バックホウ１は、バケットが主として手前を向き、手前に向けて掘削する油圧ショベルである。バックホウ１は、例えば、処理設備からベルトコンベアで排出され、土砂ピットに蓄積された残土をダンプトラックに積み込む際に使用される。バックホウ１は、作業機器の一具体例である。

本実施形態では、作業機器としてバックホウ１を例示する。しかしながら、作業機器としては、バックホウ１に限らず、例えば進行方向に向けて掘削する油圧ショベルであるローディングショベルや、その他の建設重機であってもよい。また、作業機器としては、建設重機に限らず、例えば産業用ロボットに代表されるように、エンドエフェクタを少なくとも１つの支持部で支持した各種ロボットであってもよい。

図２は、バックホウ１の側面図を示している。図２に示すように、バックホウ１は、下部走行体２、旋回体３、ブーム４、アーム５、バケット６を備える。

旋回体３は、下部走行体２に水平旋回可能に搭載されている。

ブーム４は、第１関節７を介して旋回体３に揺動自在に支持されている。ブーム４は、機構を構成する支持部の一具体例である。

アーム５は、第２関節８を介してブーム４に揺動自在に支持されている。アーム５は、機構を構成する支持部の一具体例である。

バケット６は、第３関節９を介してアーム５に揺動自在に支持されている。バケット６は、機構を構成するエンドエフェクタの一具体例である。本実施形態では、機構として、ブーム４、アーム５、並びにバケット６の３つを例示している。しかし、機構はこれらに限らず、数も３つに限定されるものではない。

旋回体３、ブーム４、アーム５、バケット６は、この記載順に直列に連結されている。従って、バケット６は、アーム５及びブーム４をこの記載順に介して旋回体３に揺動自在に支持されている。本明細書において、ブーム４及びアーム５、バケット６は、何れも機構の一具体例であるから、単に『機構』と称することがある。

図３には、バックホウ１の機能ブロック図を示している。図３に示すように、バックホウ１は、更に、エンジン１０、油圧ポンプ１１、油圧制御弁１２、電子制御弁１３、ブームシリンダ１４、アームシリンダ１５、バケットシリンダ１６を備えている。また、バックホウ１は、旋回油圧モータ１７、走行油圧モータ１８を備えている。また、バックホウ１は、ブーム姿勢センサ１９、アーム姿勢センサ２０、バケット姿勢センサ２１を備えている。バックホウ１は、更に、軌道追従制御部２２を備えている。

ブームシリンダ１４、アームシリンダ１５、バケットシリンダ１６、旋回油圧モータ１７、走行油圧モータ１８は、何れも、エンジン１０により駆動される油圧ポンプ１１からの圧油を油圧制御弁１２を介して受けて作動する。油圧制御弁１２は、電子制御弁１３からのパイロット圧力を受けて切り替わり、圧油をブームシリンダ１４、アームシリンダ１５、バケットシリンダ１６、旋回油圧モータ１７、走行油圧モータ１８に適宜、供給する。電子制御弁１３は、軌道追従制御部２２からの制御信号を受けて切り替わり、パイロット圧力を油圧制御弁１２に供給することで油圧制御弁１２を切り替える。

旋回油圧モータ１７は、旋回体３を旋回させるための油圧モータである。

走行油圧モータ１８は、下部走行体２を走行させるための油圧モータである。

ブーム姿勢センサ１９は、揺動角度センサであって、図２に示すブーム４の旋回体３に対する揺動角度θ１を検出し、検出結果を軌道追従制御部２２に出力する。ここで、揺動角度θ１とは、図２に示すように、ブーム４の第１関節７及び第２関節８を結ぶ線分と鉛直方向の間の角度である。揺動角度θ１は、ブーム４の相対姿勢を意味している。

アーム姿勢センサ２０は、揺動角度センサであって、アーム５のブーム４に対する揺動角度θ２を検出し、検出結果を軌道追従制御部２２に出力する。ここで、揺動角度θ２とは、アーム５の第２関節８及び第３関節９を結ぶ線分と、ブーム４の第１関節７及び第２関節８を結ぶ線分と、の間の角度である。揺動角度θ２は、アーム５の相対姿勢を意味している。

バケット姿勢センサ２１は、揺動角度センサであって、バケット６のアーム５に対する揺動角度θ３を検出し、検出結果を軌道追従制御部２２に出力する。ここで、揺動角度θ３とは、バケット６の第３関節９とバケット刃先６ａを結ぶ線分と、アーム５の第２関節８及び第３関節９を結ぶ線分と、の間の角度である。揺動角度θ３は、バケット６の相対姿勢を意味している。

ブーム姿勢センサ１９、アーム姿勢センサ２０、バケット姿勢センサ２１は、例えばロータリーエンコーダによって構成されている。しかしながら、ブーム姿勢センサ１９は、ブームシリンダ１４のシリンダの長さを検出するセンサとし、このセンサの検出結果に基づいて軌道追従制御部２２が揺動角度θ１を算出するように構成してもよい。また、ブーム姿勢センサ１９として加速度センサを採用し、加速度センサの検出結果に基づいて軌道追従制御部２２が揺動角度θ１を算出するようにしてもよい。アーム姿勢センサ２０及びバケット姿勢センサ２１についても同様である。

ここで、図２を参照して、バケット刃先６ａの座標及び絶対姿勢と、各揺動角度θ１、θ２、θ３との関係を説明する。図２に示すように、バケット刃先６ａのX座標をxとし、Y座標をyとし、バケット刃先６ａの絶対姿勢をθとする。バケット刃先６ａの絶対姿勢θは、バケット刃先６ａの第３関節９とバケット刃先６ａを結ぶ線分と水平との間の角度である。ブーム４の第１関節７及び第２関節８を結ぶ線分の長さをL1とし、アーム５の第２関節８及び第３関節９を結ぶ線分の長さをL2とし、バケット６の第３関節９とバケット刃先６ａを結ぶ線分の長さをL3とする。すると、バケット６のX座標x、Y座標y、絶対姿勢θは、それぞれ、下記の式（１）から（３）で表現される。ただし、バケット６のX座標x、Y座標yの原点は第１関節７としている。

従って、バケット６のX座標x、Y座標y、絶対姿勢θが所望の値となるような揺動角度θ1、θ２、θ３は、下記の式（４）から（１２）で求められる。

本実施形態におけるバックホウ１では、バケット６を自動制御するために、先ず、バケット６の目標軌道に従ってバケット６の目標とするX座標x、Y座標y、絶対姿勢θが離散的に決定される。そして、バケット６のX座標x、Y座標y、絶対姿勢θを実現するための揺動角度θ１、θ２、θ３が算出され、揺動角度θ１、θ２、θ３が自動制御における制御対象とされる。以下、バケット６のX座標x、Y座標y、絶対姿勢θをバケット６の状態と称する。また、バケット６のX座標x、Y座標y、絶対姿勢θの目標値をそれぞれ、目標X座標xr、目標Y座標yr、目標絶対姿勢θrと表記する。また、揺動角度θ１、θ２、θ３の目標値をそれぞれ、目標揺動角度θｒ１、θｒ２、θｒ３と表記する。添字ｒは、参照を意味するreferenceの頭文字である。

次に、図３を参照して、軌道追従制御部２２を詳細に説明する。

軌道追従制御部２２は、軌道追従システムの一具体例である。本実施形態では、バックホウ１が軌道追従制御部２２を備えている。しかしながら、軌道追従制御部２２は、バックホウ１の外部に設けてもよい。軌道追従制御部２２は、単一の装置により実現されてもよいし、複数の装置により実現されてもよい。軌道追従制御部２２を複数の装置により実現する場合は、複数の装置は地理的に同一の場所に配置されてもよく、地理的に離れた場所に配置されてもよい。

軌道追従制御部２２は、中央演算処理器としてのCPU２２ａと、読み書き自由のRAM２２ｂ、読み出し専用のROM２２ｃを備えている。そして、CPU２２ａがROM２２ｃに記憶されている制御プログラムを読み出して実行することで、制御プログラムは、CPU２２ａなどのハードウェアを、目標軌道記憶部３０として機能させる。同様に、制御プログラムは、CPU２２ａなどのハードウェアを、目標状態更新部３１、目標相対姿勢算出部３２、相対姿勢制御部３３、収束判定部３４、として機能させる。

目標軌道記憶部３０は、バケット６の目標軌道を記憶する。図４には、バケット６の目標軌道の一例を示している。図４の目標軌道は、土砂ピットに蓄積された残土を掘削してバケット６に収容するためのバケット６の軌道である。図４の目標軌道は、バケット６の離散化された複数の目標状態P1〜P10によって構成されている。各目標状態P1〜P10は、少なくともバケット６のバケット刃先６ａの目標座標である目標X座標xr及び目標Y座標yrを含む。本実施形態では、各目標状態P1〜P10は、更に、バケット６のバケット刃先６ａの目標絶対姿勢θrを含む。従って、本実施形態において、バケット６の各目標状態P1〜P10は、バケット６のバケット刃先６ａの目標X座標xr、目標Y座標yr、目標絶対姿勢θrで構成されている。軌道追従制御部２２は、バケット６の状態が目標状態P1〜P10を順次追従するように揺動角度θ１、θ２、θ３を制御する。

図５には、バケット６の離散化された複数の目標状態P1〜P10を示している。図５において、目標状態Pnに対応するバケット６のバケット刃先６ａの目標X座標xrをxr(n)で示し、目標Y座標yrをyr(n)で示し、目標絶対姿勢θrをθr(n)で示している。ここで、nは、１から１０までの整数である。

目標状態更新部３１は、図５に示すテーブルを参照して、追従制御の開始の際にはバケット６の目標状態を設定し、追従制御が進むにつれてバケット６の目標状態を更新する。

目標相対姿勢算出部３２は、バケット６の現在の目標状態に基づいて、目標揺動角度θｒ１、θｒ２、θｒ３をそれぞれ算出する。具体的な算出方法は、前述の式（４）から（１２）を参照されたい。図５において、目標状態Pnに対応する目標揺動角度θｒ１を目標揺動角度θｒ１(n)で示す。目標状態Pnに対応する目標揺動角度θｒ２を目標揺動角度θｒ２(n)で示す。目標状態Pnに対応する目標揺動角度θｒ３を目標揺動角度θｒ３(n)で示す。

相対姿勢制御部３３は、揺動角度θ１、θ２、θ３がそれぞれ対応する目標揺動角度θｒ１、θｒ２、θｒ３に近づくように揺動角度θ１、θ２、θ３を制御する。

図３に戻り、収束判定部３４は、バケット６の状態が現在の目標状態に収束したか否かを判定する。収束判定部３４は、第１収束判定部３５と第２収束判定部３６を備えている。

第１収束判定部３５は、各機構の相対姿勢に対応する揺動角度が対応する目標揺動角度θｒ１、θｒ２、θｒ３に第１収束条件で収束したか否かを判定する。第２収束判定部３６は、各機構の相対姿勢に対応する揺動角度が対応する目標揺動角度θｒ１、θｒ２、θｒ３に第１収束条件よりもきつい第２収束条件で収束したか否かを判定する。

なお、ブーム４の相対姿勢とは、揺動角度θ１である。アーム５の相対姿勢とは、揺動角度θ２である。バケット６の相対姿勢とは、揺動角度θ３である。

図４では、第１拘束条件をδA及びδBで示す円でイメージしており、第２拘束条件をεA及びεBで示す円でイメージしている。なお、第１収束条件及び第２収束条件で収束判定される対象はあくまでブーム４及びアーム５、バケット６の揺動角度θ１、θ２、θ３である。図４では、理解促進のために、第１拘束条件及び第２拘束条件で収束判定される対象があたかもバケット６のバケット刃先６ａの座標であるかのように描いていることに留意されたい。

図５には、第１拘束条件による収束判定で用いられる第１閾値δと、第２拘束条件による収束判定で用いられる第２閾値εを示している。第１閾値δ及び第２閾値εは、機構毎に設定されており、また、目標状態毎に設定されている。図５に示す第１閾値δ1及び第２閾値ε1は、ブーム４の揺動角度θ１の収束判定に用いられる閾値である。同様に、第１閾値δ2及び第２閾値ε2は、アーム５の揺動角度θ２の収束判定に用いられる閾値であり、第１閾値δ3及び第２閾値ε3は、バケット６の揺動角度θ３の収束判定に用いられる閾値である。

本実施形態において、目標状態P1からP7において用いられる第１閾値δ1として、単一の第１閾値δ1（A）を採用している。同様に、目標状態P1からP7において用いられる第１閾値δ2として、単一の第１閾値δ2（A）を採用しており、目標状態P1からP7において用いられる第１閾値δ3として、単一の第１閾値δ3（A）を採用している。

目標状態P8からP10において用いられる第１閾値δ1として、単一の第１閾値δ1（B）を採用している。同様に、目標状態P8からP10において用いられる第１閾値δ2として、単一の第１閾値δ2（B）を採用しており、目標状態P8からP10において用いられる第１閾値δ3として、単一の第１閾値δ3（B）を採用している。

そして、第１閾値δ1（A）は、第１閾値δ1（B）よりも大きい。第１閾値δ2（A）は、第１閾値δ2（B）よりも大きい。第１閾値δ3（A）は、第１閾値δ3（B）よりも大きい。第１閾値δは収束判定に用いられる閾値であって、目標値からのズレと比較される。従って、第１閾値δが大きければ、収束判定条件はゆるくなる。

上記の通り、バケット６の目標軌道の前半では第１拘束条件をゆるく設定し、後半では第１拘束条件をきつく設定している。これは、バケット６が土砂を掘削するに際し、バケット６が土砂にアプローチする際はバケット６の軌道に高い精度が求められていない一方、バケット６を実際に掘削する際はバケット６の軌道に高い精度が求められるからである。

図５に戻り、本実施形態において、目標状態P1からP7において用いられる第２閾値ε1として、単一の第２閾値ε1（A）を採用している。同様に、目標状態P1からP7において用いられる第２閾値ε2として、単一の第２閾値ε2（A）を採用しており、目標状態P1からP7において用いられる第２閾値ε3として、単一の第２閾値ε3（A）を採用している。

目標状態P8からP10において用いられる第２閾値ε1として、単一の第２閾値ε1（B）を採用している。同様に、目標状態P8からP10において用いられる第２閾値ε2として、単一の第２閾値ε2（B）を採用しており、目標状態P8からP10において用いられる第２閾値ε3として、単一の第２閾値ε3（B）を採用している。

そして、第２閾値ε1（A）は、第２閾値ε1（B）よりも大きい。第２閾値ε2（A）は、第２閾値ε2（B）よりも大きい。第２閾値ε3（A）は、第２閾値ε3（B）よりも大きい。第２閾値εは収束判定に用いられる閾値であって、目標値からのズレと比較される。従って、第２閾値εが大きければ、収束判定条件はゆるくなる。

第１収束判定部３５は、各機構の相対姿勢に対応する揺動角度が対応する目標揺動角度θｒ１、θｒ２、θｒ３に第１収束条件で収束したか否かを下記式（１３）に基づいて判定する。

上記式（１３）において、左辺の第１項は、目標揺動角度θriである。左辺の第２項は、各機構の現在の揺動角度θiである。右辺は、第１閾値δiである。何れにおいても、添字iは１から３までの整数である。

第２収束判定部３６は、各機構の相対姿勢に対応する揺動角度が対応する目標揺動角度θｒ１、θｒ２、θｒ３に第２収束条件で収束したか否かを下記式（１４）に基づいて判定する。

上記式（１４）において、右辺は、第２閾値εiである。

そして、第１閾値δは、第２閾値εよりも大きく設定されている。即ち、第１閾値δi（A）は第２閾値εi(A)よりも大きく、第１閾値δi（B）は第２閾値εi(B)よりも大きい。

以上の構成で、目標状態更新部３１は、バケット６の状態が現在の目標状態に収束したと収束判定部３４が判定したら、バケット６の目標状態が次の目標状態となるように、バケット６の目標状態を更新するように構成されている。

次に、図６を参照して、バックホウ１の制御フローを説明する。

S100:
先ず、目標状態更新部３１が、バケット６の目標状態を目標状態P1に設定する。

S110:
次に、目標相対姿勢算出部３２が、目標状態更新部３１によって設定された現在の目標状態に基づいて、すべての機構の目標揺動角度θｒ１、θｒ２、θｒ３をそれぞれ算出する。

S120:
次に、相対姿勢制御部３３が、すべての機構の揺動角度θ１、θ２、θ３がそれぞれ対応する目標揺動角度θｒ１、θｒ２、θｒ３へ近づくようにすべての機構の揺動角度θ１、θ２、θ３の制御を開始する。揺動角度の制御とは、機構の揺動角度を変化させることであってもよい。揺動角度の制御は、揺動角度を変化させる制御スピードを変化させることを含んでもよい。制御スピードの変化とは、制御スピードを減速または加速させることであってもよい。相対姿勢制御部３３は、第１収束条件で収束した機構の揺動角度が対応する目標揺動角度へ更に近づくように継続して当該機構の相対姿勢を制御する場合、当該機構の制御スピードを減速させてもよい。

S130:
次に、第１収束判定部３５が、すべての機構の揺動角度θ１、θ２、θ３が対応する目標揺動角度θｒ１、θｒ２、θｒ３に第１収束条件で収束したか判定する。S130でYESの場合、第１収束判定部３５は、処理をS140に進める。一方、S130でNOの場合、第１収束判定部３５は、処理をS200に進める。

S140:
目標状態更新部３１は、次の目標状態があるか判定する。S140でNOの場合、軌道追従制御部２２は、処理を終了する。一方、S140でYESの場合、目標状態更新部３１は、処理をS150に進める。

S150:
目標状態更新部３１は、バケット６の目標状態を次の目標状態に更新し、処理をS110に戻す。

S200-S250：
S210からS230までの処理は、機構ｉ毎に独立して実行される。S200、S240、S250における処理は、機構ｉ毎に独立して処理を実行するためのループ処理である。機構１、機構２、機構３は、ブーム４、アーム５、バケット６にそれぞれ対応している。

S210:
第１収束判定部３５は、機構ｉの揺動角度θｉが目標揺動角度θｒiに第1収束条件で収束したか否かを判定する。S210においてYESの場合、第１収束判定部３５は、処理をS220に進める。一方、S210においてNOの場合、第１収束判定部３５は、処理をS240に進める。

S220:
第２収束判定部３６は、機構ｉの揺動角度θｉが目標揺動角度θｒｉに第２収束条件で収束したか否かを判定する。S220においてYESの場合、第２収束判定部３６は、処理をS230に進める。一方、S220においてNOの場合、第２収束判定部３６は、処理をS240に進める。

S230:
相対姿勢制御部３３は、機構ｉの揺動角度θｉの制御スピードを減速させて、揺動角度θｉの制御を停止させる。

S240:
S240において、すべての機構ｉについてS210からS230までの処理を実行したら、軌道追従制御部２２は、処理をS130に戻す。

S210からS230までの処理によれば、相対姿勢制御部３３は、下記前提条件（１）及び（２）を満たす場合、次のように制御する。即ち、相対姿勢制御部３３は、既に対応する目標揺動角度に第１収束条件で収束した機構の揺動角度が対応する目標揺動角度へ更に近づくように継続して当該機構の揺動角度を制御する。前提条件（１）は、少なくとも何れか１つの機構の揺動角度が対応する目標揺動角度に第１収束条件で収束していることである。前提条件（２）は、他の少なくとも何れか１つの機構の揺動角度が対応する目標揺動角度に第１収束条件で収束していないことである。以上の制御によれば、バックホウ１のブーム４、アーム５、バケット６の動作を滑らかにすることができる。その理由は以下の通りである。

即ち、複数の機構の揺動角度が対応する目標揺動角度に第１収束条件で収束するタイミングは通常異なっている。従って、各機構において揺動角度が対応する目標揺動角度に第１収束条件で収束したら直ちに減速して停止させるようにすると、次の通りとなる。即ち、他の機構よりも先に揺動角度が目標揺動角度に収束した機構の揺動角度の角度変化は、他のすべての機構の揺動角度が目標揺動角度に第１収束条件で収束するまでゼロにして待機していなければならない。即ち、機構間の関節角度の変化を一時的に停止させる必要がある。このように機構間の関節角度の変化を一時的に停止させると、変化と停止を繰り返すことになり、バックホウ１のブーム４、アーム５、バケット６の動作がぎこちないものとなる。

これに対し、本実施形態では、各機構の揺動角度が対応する目標揺動角度に第１収束条件で収束しても直ちに減速して停止させないようにしたことで、次の通りとなる。即ち、他の機構よりも先に揺動角度が目標揺動角度に第１収束条件で収束した機構の揺動角度の角度変化は、収束後も維持されることになる。即ち、機構間の関節角度の変化を一時的に停止させる必要性が低くなる。これにより、バックホウ１のブーム４、アーム５、バケット６の動作を滑らかなものにすることができる。

以上に、第２実施形態を説明したが、上記第２実施形態は、以下の特徴を有する。

軌道追従制御部２２（軌道追従システム）は、複数の機構を備えたバックホウ１（作業機器）においてバケット６を目標軌道に追従制御する。複数の機構は、バケット６（エンドエフェクタ）と、バケット６を支持するブーム４及びアーム５（少なくとも１つの支持部）と、を含む。軌道追従制御部２２は、目標状態更新部３１と、目標相対姿勢算出部３２と、相対姿勢制御部３３と、収束判定部３４と、を備える。目標状態更新部３１は、バケット６の少なくとも目標座標を含む目標状態を更新する。目標相対姿勢算出部３２は、バケット６の現在の目標状態に基づいて複数の機構の目標揺動角度（目標相対姿勢）をそれぞれ算出する。相対姿勢制御部３３は、複数の機構の揺動角度（相対姿勢）がそれぞれ対応する目標揺動角度へ近づくように複数の機構の揺動角度を制御する。収束判定部３４は、バケット６の状態が現在の目標状態に収束したか否かを判定する。目標状態更新部３１は、バケット６の状態が現在の目標状態に収束したと収束判定部３４が判定したら、バケット６の目標状態を更新するように構成されている。収束判定部３４は、第１収束判定部３５と第２収束判定部３６を備える。第１収束判定部３５は、各機構の揺動角度が対応する目標揺動角度に第１収束条件で収束したか否かを判定する。第２収束判定部３６は、各機構の揺動角度が対応する目標揺動角度に第１収束条件よりもきつい第２収束条件で収束したか否かを判定する。以上の構成によれば、収束判定に関する新しい技術が提供される。即ち、柔軟な収束判定を実現することができる。

また、収束判定部３４は、すべての機構の揺動角度が対応する目標揺動角度に第１収束条件で収束した場合、バケット６の状態が現在の目標状態に収束したと判定する。

また、相対姿勢制御部３３は、少なくとも何れか１つの機構の揺動角度が対応する目標揺動角度に第１収束条件で収束しており、他の少なくとも何れか１つの機構の揺動角度が対応する目標揺動角度に第１収束条件で収束していない場合、以下の通りとする。即ち、既に対応する目標揺動角度に第１収束条件で収束した機構の揺動角度が対応する目標揺動角度へ更に近づくように継続して当該機構の揺動角度を制御する。以上の構成によれば、各機構の動作を滑らかにすることができる。

また、相対姿勢制御部３３は、各機構の揺動角度が対応する目標揺動角度に第２収束条件で収束したら、当該機構の揺動角度の制御を停止させる。

また、バケット６の目標状態は、更に、バケット６の目標絶対姿勢を含む。

また、軌道追従方法は、目標状態更新ステップ（S150）と、目標相対姿勢算出ステップ（S110）と、相対姿勢制御ステップ（S120）と、収束判定ステップ（S130,S210,S220）と、を含む。目標状態更新ステップ（S150）では、バケット６の状態が現在の目標状態に収束したと収束判定ステップ（S130：YES）で判定したら、バケット６の目標状態を更新する。収束判定ステップ（S130,S210,S220）は、第１収束判定ステップ（S210）と、第２収束判定ステップ（S220）と、を含む。第１収束判定ステップ（S210）では、各機構の揺動角度が対応する目標揺動角度に第１収束条件で収束したか否かを判定する。第２収束判定ステップ（S220）では、各機構の揺動角度が対応する目標揺動角度に第１収束条件よりもきつい第２収束条件で収束したか否かを判定する。以上の方法によれば、収束判定に関する新しい技術が提供される。即ち、柔軟な収束判定を実現することができる。

上述の例において、プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）を含む。非一時的なコンピュータ可読媒体の例は、更に、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭを含む。非一時的なコンピュータ可読媒体の例は、更に、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

以上、実施の形態を参照して本願発明を説明したが、本願発明は上記によって限定されるものではない。本願発明の構成や詳細には、発明のスコープ内で当業者が理解し得る様々な変更をすることができる。

この出願は、２０１９年１月３０日に出願された日本出願特願２０１９−０１４６８７を基礎とする優先権を主張し、その開示の全てをここに取り込む。

１００軌道追従システム
１０１目標状態更新部
１０２目標相対姿勢算出部
１０３相対姿勢制御部
１０４収束判定部
１０５第１収束判定部
１０６第２収束判定部

Claims

エンドエフェクタと、前記エンドエフェクタを支持する少なくとも１つの支持部と、を含む複数の機構を備えた作業機器において、前記エンドエフェクタを目標軌道に追従制御する軌道追従システムであって、
前記エンドエフェクタの少なくとも目標座標を含む目標状態を更新する目標状態更新部と、
前記エンドエフェクタの現在の目標状態に基づいて前記複数の機構の目標相対姿勢をそれぞれ算出する目標相対姿勢算出部と、
前記複数の機構の相対姿勢がそれぞれ対応する目標相対姿勢へ近づくように前記複数の機構の相対姿勢を制御する相対姿勢制御部と、
前記エンドエフェクタの状態が現在の目標状態に収束したか否かを判定する収束判定部と、
を備え、
前記目標状態更新部は、前記エンドエフェクタの状態が現在の目標状態に収束したと前記収束判定部が判定したら、前記エンドエフェクタの目標状態を更新するように構成されており、
前記収束判定部は、
各機構の相対姿勢が対応する目標相対姿勢に第１収束条件で収束したか否かを判定する第１収束判定部と、
各機構の相対姿勢が対応する目標相対姿勢に前記第１収束条件よりもきつい第２収束条件で収束したか否かを判定する第２収束判定部と、
を備える、
軌道追従システム。
請求項１に記載の軌道追従システムであって、
前記収束判定部は、すべての機構の相対姿勢が対応する目標相対姿勢に前記第１収束条件で収束した場合、前記エンドエフェクタの状態が現在の目標状態に収束したと判定する、
軌道追従システム。
請求項２に記載の軌道追従システムであって、
前記相対姿勢制御部は、少なくとも何れか１つの機構の相対姿勢が対応する目標相対姿勢に前記第１収束条件で収束しており、他の少なくとも何れか１つの機構の相対姿勢が対応する目標相対姿勢に前記第１収束条件で収束していない場合、前記第１収束条件で収束した機構の相対姿勢が対応する目標相対姿勢へ更に近づくように継続して当該機構の相対姿勢を制御する、
軌道追従システム。
請求項１から３までの何れか１項に記載の軌道追従システムであって、
前記相対姿勢制御部は、各機構の相対姿勢が対応する目標相対姿勢に前記第２収束条件で収束したら、当該機構の相対姿勢制御を停止させる、
軌道追従システム。
請求項１から４までの何れか１項に記載の軌道追従システムであって、
前記エンドエフェクタの前記目標状態は、更に、前記エンドエフェクタの目標絶対姿勢を含む、
軌道追従システム。
エンドエフェクタと、前記エンドエフェクタを支持する少なくとも１つの支持部と、を含む複数の機構を備えた作業機器において、前記エンドエフェクタを目標軌道に追従制御する軌道追従方法であって、
前記エンドエフェクタの少なくとも目標座標を含む目標状態を更新する目標状態更新ステップと、
前記エンドエフェクタの現在の目標状態に基づいて前記複数の機構の目標相対姿勢をそれぞれ算出する目標相対姿勢算出ステップと、
前記複数の機構の相対姿勢がそれぞれ対応する目標相対姿勢へ近づくように前記複数の機構の相対姿勢を制御する相対姿勢制御ステップと、
前記エンドエフェクタの状態が現在の目標状態に収束したか否かを判定する収束判定ステップと、
を備え、
前記目標状態更新ステップでは、前記エンドエフェクタの状態が現在の目標状態に収束したと前記収束判定ステップで判定したら、前記エンドエフェクタの目標状態を更新し、
前記収束判定ステップは、
各機構の相対姿勢が対応する目標相対姿勢に第１収束条件で収束したか否かを判定する第１収束判定ステップと、
各機構の相対姿勢が対応する目標相対姿勢に前記第１収束条件よりもきつい第２収束条件で収束したか否かを判定する第２収束判定ステップと、
を含む、
軌道追従方法。
コンピュータに、請求項６に記載の軌道追従方法を実行させるためのプログラムを格納する非一時的なコンピュータ可読媒体。