Traitement en cours

Veuillez attendre...

Paramétrages

Paramétrages

Aller à Demande

1. WO2020158779 - SYSTÈME DE SUIVI DE PISTE, PROCÉDÉ DE SUIVI DE PISTE ET SUPPORT LISIBLE PAR ORDINATEUR

Document

明 細 書

発明の名称 軌道追従システム、軌道追従方法、及び、コンピュータ可読媒体

技術分野

0001  

背景技術

0002  

先行技術文献

特許文献

0003  

発明の概要

発明が解決しようとする課題

0004   0005  

課題を解決するための手段

0006   0007  

発明の効果

0008  

図面の簡単な説明

0009  

発明を実施するための形態

0010   0011   0012   0013   0014   0015   0016   0017   0018   0019   0020   0021   0022   0023   0024   0025   0026   0027   0028   0029   0030   0031   0032   0033   0034   0035   0036   0037   0038   0039   0040   0041   0042   0043   0044   0045   0046   0047   0048   0049   0050   0051   0052   0053   0054   0055   0056   0057   0058   0059   0060   0061   0062   0063   0064   0065   0066   0067   0068   0069   0070   0071   0072   0073   0074   0075   0076   0077   0078   0079   0080   0081   0082   0083   0084   0085   0086   0087   0088   0089   0090   0091   0092   0093   0094   0095   0096   0097   0098   0099   0100   0101   0102   0103   0104   0105  

符号の説明

0106  

請求の範囲

1   2   3   4   5   6   7  

図面

1   2   3   4   5   6  

明 細 書

発明の名称 : 軌道追従システム、軌道追従方法、及び、コンピュータ可読媒体

技術分野

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

背景技術

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

先行技術文献

特許文献

[0003]
特許文献1 : 特開平10-212740号公報

発明の概要

発明が解決しようとする課題

[0004]
 油圧ショベルを自動制御する場合、例えば、オペレータが予めバケットの目標軌道を制御装置に入力し、この制御装置が目標軌道に沿ってバケットを移動させることが考えられる。このとき、制御装置は、例えば、目標軌道上で複数の目標点を定め、i番目の目標点の近傍にバケットが収束したと判定したら、i+1番目の目標点にバケットが向かうように、バケットの目標状態を繰り返し更新することが考えられる。しかしながら、上記の収束判定に関して改善の余地が残されていた。
[0005]
 本開示の目的は、上述した課題の何れかを解決する技術を提供することにある。

課題を解決するための手段

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

発明の効果

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

図面の簡単な説明

[0009]
[図1] 軌道追従システムの機能ブロック図である。(第1実施形態)
[図2] バックホウの側面図である。(第2実施形態)
[図3] バックホウの機能ブロック図である。(第2実施形態)
[図4] バケットの目標軌道の一例を示す図である。(第2実施形態)
[図5] 目標状態及び収束条件を示すテーブルである。(第2実施形態)
[図6] バックホウの制御フローである。(第2実施形態)

発明を実施するための形態

[0010]
(第1実施形態)
 先ず、図1を参照して、第1実施形態の軌道追従システム100を説明する。
[0011]
 軌道追従システム100は、エンドエフェクタと、エンドエフェクタを支持する少なくとも1つの支持部と、を含む複数の機構を備えた作業機器において、エンドエフェクタを目標軌道に追従制御するシステムである。複数の機構は、エンドエフェクタと、エンドエフェクタを支持する少なくとも1つの支持部と、を含む。
[0012]
 軌道追従システム100は、目標状態更新部101と、目標相対姿勢算出部102と、相対姿勢制御部103と、収束判定部104と、を備える。
[0013]
 目標状態更新部101は、エンドエフェクタの少なくとも目標座標を含む目標状態を更新する。
[0014]
 目標相対姿勢算出部102は、エンドエフェクタの現在の目標状態に基づいて複数の機構の目標相対姿勢をそれぞれ算出する。
[0015]
 相対姿勢制御部103は、複数の機構の相対姿勢がそれぞれ対応する目標相対姿勢へ近づくように複数の機構の相対姿勢を制御する。
[0016]
 収束判定部104は、エンドエフェクタの状態が現在の目標状態に収束したか否かを判定する。
[0017]
 そして、目標状態更新部101は、エンドエフェクタの状態が現在の目標状態に収束したと収束判定部104が判定したら、エンドエフェクタの目標状態を更新するように構成されている。
[0018]
 更に、収束判定部104は、第1収束判定部105と、第2収束判定部106と、を備える。
[0019]
 第1収束判定部105は、各機構の相対姿勢が対応する目標相対姿勢に第1収束条件で収束したか否かを判定する。
[0020]
 第2収束判定部106は、各機構の相対姿勢が対応する目標相対姿勢に第1収束条件よりもきつい第2収束条件で収束したか否かを判定する。
[0021]
 以上の構成によれば、収束判定に関する新しい技術が提供される。即ち、柔軟な収束判定を実現することができる。
[0022]
(第2実施形態)
 次に、図2から図6を参照して、バックホウ1について説明する。バックホウ1は、バケットが主として手前を向き、手前に向けて掘削する油圧ショベルである。バックホウ1は、例えば、処理設備からベルトコンベアで排出され、土砂ピットに蓄積された残土をダンプトラックに積み込む際に使用される。バックホウ1は、作業機器の一具体例である。
[0023]
 本実施形態では、作業機器としてバックホウ1を例示する。しかしながら、作業機器としては、バックホウ1に限らず、例えば進行方向に向けて掘削する油圧ショベルであるローディングショベルや、その他の建設重機であってもよい。また、作業機器としては、建設重機に限らず、例えば産業用ロボットに代表されるように、エンドエフェクタを少なくとも1つの支持部で支持した各種ロボットであってもよい。
[0024]
 図2は、バックホウ1の側面図を示している。図2に示すように、バックホウ1は、下部走行体2、旋回体3、ブーム4、アーム5、バケット6を備える。
[0025]
 旋回体3は、下部走行体2に水平旋回可能に搭載されている。
[0026]
 ブーム4は、第1関節7を介して旋回体3に揺動自在に支持されている。ブーム4は、機構を構成する支持部の一具体例である。
[0027]
 アーム5は、第2関節8を介してブーム4に揺動自在に支持されている。アーム5は、機構を構成する支持部の一具体例である。
[0028]
 バケット6は、第3関節9を介してアーム5に揺動自在に支持されている。バケット6は、機構を構成するエンドエフェクタの一具体例である。本実施形態では、機構として、ブーム4、アーム5、並びにバケット6の3つを例示している。しかし、機構はこれらに限らず、数も3つに限定されるものではない。
[0029]
 旋回体3、ブーム4、アーム5、バケット6は、この記載順に直列に連結されている。従って、バケット6は、アーム5及びブーム4をこの記載順に介して旋回体3に揺動自在に支持されている。本明細書において、ブーム4及びアーム5、バケット6は、何れも機構の一具体例であるから、単に『機構』と称することがある。
[0030]
 図3には、バックホウ1の機能ブロック図を示している。図3に示すように、バックホウ1は、更に、エンジン10、油圧ポンプ11、油圧制御弁12、電子制御弁13、ブームシリンダ14、アームシリンダ15、バケットシリンダ16を備えている。また、バックホウ1は、旋回油圧モータ17、走行油圧モータ18を備えている。また、バックホウ1は、ブーム姿勢センサ19、アーム姿勢センサ20、バケット姿勢センサ21を備えている。バックホウ1は、更に、軌道追従制御部22を備えている。
[0031]
 ブームシリンダ14、アームシリンダ15、バケットシリンダ16、旋回油圧モータ17、走行油圧モータ18は、何れも、エンジン10により駆動される油圧ポンプ11からの圧油を油圧制御弁12を介して受けて作動する。油圧制御弁12は、電子制御弁13からのパイロット圧力を受けて切り替わり、圧油をブームシリンダ14、アームシリンダ15、バケットシリンダ16、旋回油圧モータ17、走行油圧モータ18に適宜、供給する。電子制御弁13は、軌道追従制御部22からの制御信号を受けて切り替わり、パイロット圧力を油圧制御弁12に供給することで油圧制御弁12を切り替える。
[0032]
 旋回油圧モータ17は、旋回体3を旋回させるための油圧モータである。
[0033]
 走行油圧モータ18は、下部走行体2を走行させるための油圧モータである。
[0034]
 ブーム姿勢センサ19は、揺動角度センサであって、図2に示すブーム4の旋回体3に対する揺動角度θ1を検出し、検出結果を軌道追従制御部22に出力する。ここで、揺動角度θ1とは、図2に示すように、ブーム4の第1関節7及び第2関節8を結ぶ線分と鉛直方向の間の角度である。揺動角度θ1は、ブーム4の相対姿勢を意味している。
[0035]
 アーム姿勢センサ20は、揺動角度センサであって、アーム5のブーム4に対する揺動角度θ2を検出し、検出結果を軌道追従制御部22に出力する。ここで、揺動角度θ2とは、アーム5の第2関節8及び第3関節9を結ぶ線分と、ブーム4の第1関節7及び第2関節8を結ぶ線分と、の間の角度である。揺動角度θ2は、アーム5の相対姿勢を意味している。
[0036]
 バケット姿勢センサ21は、揺動角度センサであって、バケット6のアーム5に対する揺動角度θ3を検出し、検出結果を軌道追従制御部22に出力する。ここで、揺動角度θ3とは、バケット6の第3関節9とバケット刃先6aを結ぶ線分と、アーム5の第2関節8及び第3関節9を結ぶ線分と、の間の角度である。揺動角度θ3は、バケット6の相対姿勢を意味している。
[0037]
 ブーム姿勢センサ19、アーム姿勢センサ20、バケット姿勢センサ21は、例えばロータリーエンコーダによって構成されている。しかしながら、ブーム姿勢センサ19は、ブームシリンダ14のシリンダの長さを検出するセンサとし、このセンサの検出結果に基づいて軌道追従制御部22が揺動角度θ1を算出するように構成してもよい。また、ブーム姿勢センサ19として加速度センサを採用し、加速度センサの検出結果に基づいて軌道追従制御部22が揺動角度θ1を算出するようにしてもよい。アーム姿勢センサ20及びバケット姿勢センサ21についても同様である。
[0038]
 ここで、図2を参照して、バケット刃先6aの座標及び絶対姿勢と、各揺動角度θ1、θ2、θ3との関係を説明する。図2に示すように、バケット刃先6aのX座標をxとし、Y座標をyとし、バケット刃先6aの絶対姿勢をθとする。バケット刃先6aの絶対姿勢θは、バケット刃先6aの第3関節9とバケット刃先6aを結ぶ線分と水平との間の角度である。ブーム4の第1関節7及び第2関節8を結ぶ線分の長さをL1とし、アーム5の第2関節8及び第3関節9を結ぶ線分の長さをL2とし、バケット6の第3関節9とバケット刃先6aを結ぶ線分の長さをL3とする。すると、バケット6のX座標x、Y座標y、絶対姿勢θは、それぞれ、下記の式(1)から(3)で表現される。ただし、バケット6のX座標x、Y座標yの原点は第1関節7としている。
[0039]
[数1]


[0040]
[数2]


[0041]
[数3]


[0042]
 従って、バケット6のX座標x、Y座標y、絶対姿勢θが所望の値となるような揺動角度θ1、θ2、θ3は、下記の式(4)から(12)で求められる。
[0043]
[数4]


[0044]
[数5]


[0045]
[数6]


[0046]
[数7]


[0047]
[数8]


[0048]
[数9]


[0049]
[数10]


[0050]
[数11]


[0051]
[数12]


[0052]
 本実施形態におけるバックホウ1では、バケット6を自動制御するために、先ず、バケット6の目標軌道に従ってバケット6の目標とするX座標x、Y座標y、絶対姿勢θが離散的に決定される。そして、バケット6のX座標x、Y座標y、絶対姿勢θを実現するための揺動角度θ1、θ2、θ3が算出され、揺動角度θ1、θ2、θ3が自動制御における制御対象とされる。以下、バケット6のX座標x、Y座標y、絶対姿勢θをバケット6の状態と称する。また、バケット6のX座標x、Y座標y、絶対姿勢θの目標値をそれぞれ、目標X座標xr、目標Y座標yr、目標絶対姿勢θrと表記する。また、揺動角度θ1、θ2、θ3の目標値をそれぞれ、目標揺動角度θr1、θr2、θr3と表記する。添字rは、参照を意味するreferenceの頭文字である。
[0053]
 次に、図3を参照して、軌道追従制御部22を詳細に説明する。
[0054]
 軌道追従制御部22は、軌道追従システムの一具体例である。本実施形態では、バックホウ1が軌道追従制御部22を備えている。しかしながら、軌道追従制御部22は、バックホウ1の外部に設けてもよい。軌道追従制御部22は、単一の装置により実現されてもよいし、複数の装置により実現されてもよい。軌道追従制御部22を複数の装置により実現する場合は、複数の装置は地理的に同一の場所に配置されてもよく、地理的に離れた場所に配置されてもよい。
[0055]
 軌道追従制御部22は、中央演算処理器としてのCPU22aと、読み書き自由のRAM22b、読み出し専用のROM22cを備えている。そして、CPU22aがROM22cに記憶されている制御プログラムを読み出して実行することで、制御プログラムは、CPU22aなどのハードウェアを、目標軌道記憶部30として機能させる。同様に、制御プログラムは、CPU22aなどのハードウェアを、目標状態更新部31、目標相対姿勢算出部32、相対姿勢制御部33、収束判定部34、として機能させる。
[0056]
 目標軌道記憶部30は、バケット6の目標軌道を記憶する。図4には、バケット6の目標軌道の一例を示している。図4の目標軌道は、土砂ピットに蓄積された残土を掘削してバケット6に収容するためのバケット6の軌道である。図4の目標軌道は、バケット6の離散化された複数の目標状態P1~P10によって構成されている。各目標状態P1~P10は、少なくともバケット6のバケット刃先6aの目標座標である目標X座標xr及び目標Y座標yrを含む。本実施形態では、各目標状態P1~P10は、更に、バケット6のバケット刃先6aの目標絶対姿勢θrを含む。従って、本実施形態において、バケット6の各目標状態P1~P10は、バケット6のバケット刃先6aの目標X座標xr、目標Y座標yr、目標絶対姿勢θrで構成されている。軌道追従制御部22は、バケット6の状態が目標状態P1~P10を順次追従するように揺動角度θ1、θ2、θ3を制御する。
[0057]
 図5には、バケット6の離散化された複数の目標状態P1~P10を示している。図5において、目標状態Pnに対応するバケット6のバケット刃先6aの目標X座標xrをxr(n)で示し、目標Y座標yrをyr(n)で示し、目標絶対姿勢θrをθr(n)で示している。ここで、nは、1から10までの整数である。
[0058]
 目標状態更新部31は、図5に示すテーブルを参照して、追従制御の開始の際にはバケット6の目標状態を設定し、追従制御が進むにつれてバケット6の目標状態を更新する。
[0059]
 目標相対姿勢算出部32は、バケット6の現在の目標状態に基づいて、目標揺動角度θr1、θr2、θr3をそれぞれ算出する。具体的な算出方法は、前述の式(4)から(12)を参照されたい。図5において、目標状態Pnに対応する目標揺動角度θr1を目標揺動角度θr1(n)で示す。目標状態Pnに対応する目標揺動角度θr2を目標揺動角度θr2(n)で示す。目標状態Pnに対応する目標揺動角度θr3を目標揺動角度θr3(n)で示す。
[0060]
 相対姿勢制御部33は、揺動角度θ1、θ2、θ3がそれぞれ対応する目標揺動角度θr1、θr2、θr3に近づくように揺動角度θ1、θ2、θ3を制御する。
[0061]
 図3に戻り、収束判定部34は、バケット6の状態が現在の目標状態に収束したか否かを判定する。収束判定部34は、第1収束判定部35と第2収束判定部36を備えている。
[0062]
 第1収束判定部35は、各機構の相対姿勢に対応する揺動角度が対応する目標揺動角度θr1、θr2、θr3に第1収束条件で収束したか否かを判定する。第2収束判定部36は、各機構の相対姿勢に対応する揺動角度が対応する目標揺動角度θr1、θr2、θr3に第1収束条件よりもきつい第2収束条件で収束したか否かを判定する。
[0063]
 なお、ブーム4の相対姿勢とは、揺動角度θ1である。アーム5の相対姿勢とは、揺動角度θ2である。バケット6の相対姿勢とは、揺動角度θ3である。
[0064]
 図4では、第1拘束条件をδA及びδBで示す円でイメージしており、第2拘束条件をεA及びεBで示す円でイメージしている。なお、第1収束条件及び第2収束条件で収束判定される対象はあくまでブーム4及びアーム5、バケット6の揺動角度θ1、θ2、θ3である。図4では、理解促進のために、第1拘束条件及び第2拘束条件で収束判定される対象があたかもバケット6のバケット刃先6aの座標であるかのように描いていることに留意されたい。
[0065]
 図5には、第1拘束条件による収束判定で用いられる第1閾値δと、第2拘束条件による収束判定で用いられる第2閾値εを示している。第1閾値δ及び第2閾値εは、機構毎に設定されており、また、目標状態毎に設定されている。図5に示す第1閾値δ1及び第2閾値ε1は、ブーム4の揺動角度θ1の収束判定に用いられる閾値である。同様に、第1閾値δ2及び第2閾値ε2は、アーム5の揺動角度θ2の収束判定に用いられる閾値であり、第1閾値δ3及び第2閾値ε3は、バケット6の揺動角度θ3の収束判定に用いられる閾値である。
[0066]
 本実施形態において、目標状態P1からP7において用いられる第1閾値δ1として、単一の第1閾値δ1(A)を採用している。同様に、目標状態P1からP7において用いられる第1閾値δ2として、単一の第1閾値δ2(A)を採用しており、目標状態P1からP7において用いられる第1閾値δ3として、単一の第1閾値δ3(A)を採用している。
[0067]
 目標状態P8からP10において用いられる第1閾値δ1として、単一の第1閾値δ1(B)を採用している。同様に、目標状態P8からP10において用いられる第1閾値δ2として、単一の第1閾値δ2(B)を採用しており、目標状態P8からP10において用いられる第1閾値δ3として、単一の第1閾値δ3(B)を採用している。
[0068]
 そして、第1閾値δ1(A)は、第1閾値δ1(B)よりも大きい。第1閾値δ2(A)は、第1閾値δ2(B)よりも大きい。第1閾値δ3(A)は、第1閾値δ3(B)よりも大きい。第1閾値δは収束判定に用いられる閾値であって、目標値からのズレと比較される。従って、第1閾値δが大きければ、収束判定条件はゆるくなる。
[0069]
 上記の通り、バケット6の目標軌道の前半では第1拘束条件をゆるく設定し、後半では第1拘束条件をきつく設定している。これは、バケット6が土砂を掘削するに際し、バケット6が土砂にアプローチする際はバケット6の軌道に高い精度が求められていない一方、バケット6を実際に掘削する際はバケット6の軌道に高い精度が求められるからである。
[0070]
 図5に戻り、本実施形態において、目標状態P1からP7において用いられる第2閾値ε1として、単一の第2閾値ε1(A)を採用している。同様に、目標状態P1からP7において用いられる第2閾値ε2として、単一の第2閾値ε2(A)を採用しており、目標状態P1からP7において用いられる第2閾値ε3として、単一の第2閾値ε3(A)を採用している。
[0071]
 目標状態P8からP10において用いられる第2閾値ε1として、単一の第2閾値ε1(B)を採用している。同様に、目標状態P8からP10において用いられる第2閾値ε2として、単一の第2閾値ε2(B)を採用しており、目標状態P8からP10において用いられる第2閾値ε3として、単一の第2閾値ε3(B)を採用している。
[0072]
 そして、第2閾値ε1(A)は、第2閾値ε1(B)よりも大きい。第2閾値ε2(A)は、第2閾値ε2(B)よりも大きい。第2閾値ε3(A)は、第2閾値ε3(B)よりも大きい。第2閾値εは収束判定に用いられる閾値であって、目標値からのズレと比較される。従って、第2閾値εが大きければ、収束判定条件はゆるくなる。
[0073]
 第1収束判定部35は、各機構の相対姿勢に対応する揺動角度が対応する目標揺動角度θr1、θr2、θr3に第1収束条件で収束したか否かを下記式(13)に基づいて判定する。
[0074]
[数13]


[0075]
 上記式(13)において、左辺の第1項は、目標揺動角度θriである。左辺の第2項は、各機構の現在の揺動角度θiである。右辺は、第1閾値δiである。何れにおいても、添字iは1から3までの整数である。
[0076]
 第2収束判定部36は、各機構の相対姿勢に対応する揺動角度が対応する目標揺動角度θr1、θr2、θr3に第2収束条件で収束したか否かを下記式(14)に基づいて判定する。
[0077]
[数14]


[0078]
 上記式(14)において、右辺は、第2閾値εiである。
[0079]
 そして、第1閾値δは、第2閾値εよりも大きく設定されている。即ち、第1閾値δi(A)は第2閾値εi(A)よりも大きく、第1閾値δi(B)は第2閾値εi(B)よりも大きい。
[0080]
 以上の構成で、目標状態更新部31は、バケット6の状態が現在の目標状態に収束したと収束判定部34が判定したら、バケット6の目標状態が次の目標状態となるように、バケット6の目標状態を更新するように構成されている。
[0081]
 次に、図6を参照して、バックホウ1の制御フローを説明する。
[0082]
 S100:
 先ず、目標状態更新部31が、バケット6の目標状態を目標状態P1に設定する。
[0083]
 S110:
 次に、目標相対姿勢算出部32が、目標状態更新部31によって設定された現在の目標状態に基づいて、すべての機構の目標揺動角度θr1、θr2、θr3をそれぞれ算出する。
[0084]
 S120:
 次に、相対姿勢制御部33が、すべての機構の揺動角度θ1、θ2、θ3がそれぞれ対応する目標揺動角度θr1、θr2、θr3へ近づくようにすべての機構の揺動角度θ1、θ2、θ3の制御を開始する。揺動角度の制御とは、機構の揺動角度を変化させることであってもよい。揺動角度の制御は、揺動角度を変化させる制御スピードを変化させることを含んでもよい。制御スピードの変化とは、制御スピードを減速または加速させることであってもよい。相対姿勢制御部33は、第1収束条件で収束した機構の揺動角度が対応する目標揺動角度へ更に近づくように継続して当該機構の相対姿勢を制御する場合、当該機構の制御スピードを減速させてもよい。
[0085]
 S130:
 次に、第1収束判定部35が、すべての機構の揺動角度θ1、θ2、θ3が対応する目標揺動角度θr1、θr2、θr3に第1収束条件で収束したか判定する。S130でYESの場合、第1収束判定部35は、処理をS140に進める。一方、S130でNOの場合、第1収束判定部35は、処理をS200に進める。
[0086]
 S140:
 目標状態更新部31は、次の目標状態があるか判定する。S140でNOの場合、軌道追従制御部22は、処理を終了する。一方、S140でYESの場合、目標状態更新部31は、処理をS150に進める。
[0087]
 S150:
 目標状態更新部31は、バケット6の目標状態を次の目標状態に更新し、処理をS110に戻す。
[0088]
 S200-S250:
 S210からS230までの処理は、機構i毎に独立して実行される。S200、S240、S250における処理は、機構i毎に独立して処理を実行するためのループ処理である。機構1、機構2、機構3は、ブーム4、アーム5、バケット6にそれぞれ対応している。
[0089]
 S210:
 第1収束判定部35は、機構iの揺動角度θiが目標揺動角度θriに第1収束条件で収束したか否かを判定する。S210においてYESの場合、第1収束判定部35は、処理をS220に進める。一方、S210においてNOの場合、第1収束判定部35は、処理をS240に進める。
[0090]
 S220:
 第2収束判定部36は、機構iの揺動角度θiが目標揺動角度θriに第2収束条件で収束したか否かを判定する。S220においてYESの場合、第2収束判定部36は、処理をS230に進める。一方、S220においてNOの場合、第2収束判定部36は、処理をS240に進める。
[0091]
 S230:
 相対姿勢制御部33は、機構iの揺動角度θiの制御スピードを減速させて、揺動角度θiの制御を停止させる。
[0092]
 S240:
 S240において、すべての機構iについてS210からS230までの処理を実行したら、軌道追従制御部22は、処理をS130に戻す。
[0093]
 S210からS230までの処理によれば、相対姿勢制御部33は、下記前提条件(1)及び(2)を満たす場合、次のように制御する。即ち、相対姿勢制御部33は、既に対応する目標揺動角度に第1収束条件で収束した機構の揺動角度が対応する目標揺動角度へ更に近づくように継続して当該機構の揺動角度を制御する。前提条件(1)は、少なくとも何れか1つの機構の揺動角度が対応する目標揺動角度に第1収束条件で収束していることである。前提条件(2)は、他の少なくとも何れか1つの機構の揺動角度が対応する目標揺動角度に第1収束条件で収束していないことである。以上の制御によれば、バックホウ1のブーム4、アーム5、バケット6の動作を滑らかにすることができる。その理由は以下の通りである。
[0094]
 即ち、複数の機構の揺動角度が対応する目標揺動角度に第1収束条件で収束するタイミングは通常異なっている。従って、各機構において揺動角度が対応する目標揺動角度に第1収束条件で収束したら直ちに減速して停止させるようにすると、次の通りとなる。即ち、他の機構よりも先に揺動角度が目標揺動角度に収束した機構の揺動角度の角度変化は、他のすべての機構の揺動角度が目標揺動角度に第1収束条件で収束するまでゼロにして待機していなければならない。即ち、機構間の関節角度の変化を一時的に停止させる必要がある。このように機構間の関節角度の変化を一時的に停止させると、変化と停止を繰り返すことになり、バックホウ1のブーム4、アーム5、バケット6の動作がぎこちないものとなる。
[0095]
 これに対し、本実施形態では、各機構の揺動角度が対応する目標揺動角度に第1収束条件で収束しても直ちに減速して停止させないようにしたことで、次の通りとなる。即ち、他の機構よりも先に揺動角度が目標揺動角度に第1収束条件で収束した機構の揺動角度の角度変化は、収束後も維持されることになる。即ち、機構間の関節角度の変化を一時的に停止させる必要性が低くなる。これにより、バックホウ1のブーム4、アーム5、バケット6の動作を滑らかなものにすることができる。
[0096]
 以上に、第2実施形態を説明したが、上記第2実施形態は、以下の特徴を有する。
[0097]
 軌道追従制御部22(軌道追従システム)は、複数の機構を備えたバックホウ1(作業機器)においてバケット6を目標軌道に追従制御する。複数の機構は、バケット6(エンドエフェクタ)と、バケット6を支持するブーム4及びアーム5(少なくとも1つの支持部)と、を含む。軌道追従制御部22は、目標状態更新部31と、目標相対姿勢算出部32と、相対姿勢制御部33と、収束判定部34と、を備える。目標状態更新部31は、バケット6の少なくとも目標座標を含む目標状態を更新する。目標相対姿勢算出部32は、バケット6の現在の目標状態に基づいて複数の機構の目標揺動角度(目標相対姿勢)をそれぞれ算出する。相対姿勢制御部33は、複数の機構の揺動角度(相対姿勢)がそれぞれ対応する目標揺動角度へ近づくように複数の機構の揺動角度を制御する。収束判定部34は、バケット6の状態が現在の目標状態に収束したか否かを判定する。目標状態更新部31は、バケット6の状態が現在の目標状態に収束したと収束判定部34が判定したら、バケット6の目標状態を更新するように構成されている。収束判定部34は、第1収束判定部35と第2収束判定部36を備える。第1収束判定部35は、各機構の揺動角度が対応する目標揺動角度に第1収束条件で収束したか否かを判定する。第2収束判定部36は、各機構の揺動角度が対応する目標揺動角度に第1収束条件よりもきつい第2収束条件で収束したか否かを判定する。以上の構成によれば、収束判定に関する新しい技術が提供される。即ち、柔軟な収束判定を実現することができる。
[0098]
 また、収束判定部34は、すべての機構の揺動角度が対応する目標揺動角度に第1収束条件で収束した場合、バケット6の状態が現在の目標状態に収束したと判定する。
[0099]
 また、相対姿勢制御部33は、少なくとも何れか1つの機構の揺動角度が対応する目標揺動角度に第1収束条件で収束しており、他の少なくとも何れか1つの機構の揺動角度が対応する目標揺動角度に第1収束条件で収束していない場合、以下の通りとする。即ち、既に対応する目標揺動角度に第1収束条件で収束した機構の揺動角度が対応する目標揺動角度へ更に近づくように継続して当該機構の揺動角度を制御する。以上の構成によれば、各機構の動作を滑らかにすることができる。
[0100]
 また、相対姿勢制御部33は、各機構の揺動角度が対応する目標揺動角度に第2収束条件で収束したら、当該機構の揺動角度の制御を停止させる。
[0101]
 また、バケット6の目標状態は、更に、バケット6の目標絶対姿勢を含む。
[0102]
 また、軌道追従方法は、目標状態更新ステップ(S150)と、目標相対姿勢算出ステップ(S110)と、相対姿勢制御ステップ(S120)と、収束判定ステップ(S130,S210,S220)と、を含む。目標状態更新ステップ(S150)では、バケット6の状態が現在の目標状態に収束したと収束判定ステップ(S130:YES)で判定したら、バケット6の目標状態を更新する。収束判定ステップ(S130,S210,S220)は、第1収束判定ステップ(S210)と、第2収束判定ステップ(S220)と、を含む。第1収束判定ステップ(S210)では、各機構の揺動角度が対応する目標揺動角度に第1収束条件で収束したか否かを判定する。第2収束判定ステップ(S220)では、各機構の揺動角度が対応する目標揺動角度に第1収束条件よりもきつい第2収束条件で収束したか否かを判定する。以上の方法によれば、収束判定に関する新しい技術が提供される。即ち、柔軟な収束判定を実現することができる。
[0103]
 上述の例において、プログラムは、様々なタイプの非一時的なコンピュータ可読媒体(non-transitory computer readable medium)を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体(tangible storage medium)を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体(例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ)、光磁気記録媒体(例えば光磁気ディスク)を含む。非一時的なコンピュータ可読媒体の例は、更に、CD-ROM(Read Only Memory)、CD-R、CD-R/W、半導体メモリ(例えば、マスクROMを含む。非一時的なコンピュータ可読媒体の例は、更に、PROM(Programmable ROM)、EPROM(Erasable PROM)、フラッシュROM、RAM(random access memory))を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体(transitory computer readable medium)によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
[0104]
 以上、実施の形態を参照して本願発明を説明したが、本願発明は上記によって限定されるものではない。本願発明の構成や詳細には、発明のスコープ内で当業者が理解し得る様々な変更をすることができる。
[0105]
 この出願は、2019年1月30日に出願された日本出願特願2019-014687を基礎とする優先権を主張し、その開示の全てをここに取り込む。

符号の説明

[0106]
100 軌道追従システム
101 目標状態更新部
102 目標相対姿勢算出部
103 相対姿勢制御部
104 収束判定部
105 第1収束判定部
106 第2収束判定部

請求の範囲

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

図面

[ 図 1]

[ 図 2]

[ 図 3]

[ 図 4]

[ 図 5]

[ 図 6]