JP2021086181A

JP2021086181A - 軌道生成装置、自動位置制御装置および軌道生成方法

Info

Publication number: JP2021086181A
Application number: JP2019212268A
Authority: JP
Inventors: 平川　学; Manabu Hirakawa; 学平川; 雅一一之瀬; Masakazu Ichinose
Original assignee: Fanuc Corp
Current assignee: Fanuc Corp
Priority date: 2019-11-25
Filing date: 2019-11-25
Publication date: 2021-06-03
Anticipated expiration: 2039-11-25
Also published as: JP7444588B2

Abstract

【課題】高精度な位置制御のための軌道生成装置を提供する。【解決手段】軌道生成装置は、ｎ個の点Ｐ１，Ｐ２，…，Ｐｎを通過する軌道を生成する装置であって、第１の曲線によって表され、ｉ−１番目からｉ＋２番目の４つの点Ｐｉ−１，Ｐｉ，Ｐｉ＋１，Ｐｉ＋２によって定義されるｉ番目の点Ｐｉとｉ＋１番目の点Ｐｉ＋１との間の第１の部分軌道と、第２の曲線によって表され、ｉ番目からｉ＋３番目の４つの点Ｐｉ，Ｐｉ＋１，Ｐｉ＋２，Ｐｉ＋３によって定義されるｉ＋１番目の点Ｐｉ＋１とｉ＋２番目の点Ｐｉ＋２との間の第２の部分軌道とを算出し、ただし、１≦ｉ≦ｎ−２であり、点Ｐｉ＋１での第１の曲線の第１、第２および第３の導関数の値が、点Ｐｉ＋１での第２の曲線の第１、第２および第３の導関数の値とそれぞれ一致する。【選択図】図２

Description

本発明は、軌道生成装置、自動位置制御装置および軌道生成方法に関するものである。

従来、数値制御（ＮＣ）工作機械または産業用ロボット等の分野において、ツールの通過点として与えられた離散的な点の間を補間することによって、ツールの軌道を生成する方法が知られている（例えば、特許文献１参照。）。
一般に、隣接する２点間の区間毎に補間曲線を算出し、補間曲線を連結することによって軌道が生成される。ツールが減速することなく通過点を通過するためには、補間曲線同士が通過点で滑らかに接続される必要がある。

特許文献１では、点Ｐ_ｍ，Ｐ_ｍ＋１間の区間の補間曲線Ｓ_ｍ（ｔ）の算出において、隣接する区間の補間曲線との接続を考慮するために、隣接する区間の点Ｐ_ｍ−１，Ｐ_ｍ＋２も使用している。具体的には、特許文献１では、３つの点Ｐ_ｍ−１，Ｐ_ｍ，Ｐ_ｍ＋１を通過する２次曲線と、３つの点Ｐ_ｍ，Ｐ_ｍ＋１，Ｐ_ｍ＋２を通過する２次曲線とを導出し、点Ｐ_ｍ，Ｐ_ｍ＋１での２次曲線の１次微係数および２次微係数に基づいて補間曲線Ｓ_ｍ（ｔ）を算出している。

特公平０６−０５８６０３号公報

特許文献１では、前述の方法によって、補間曲線Ｓ_ｍ（ｔ）の１次微係数および２次微係数が、隣接する補間曲線Ｓ_ｍ−１（ｔ）の１次微係数および２次微係数と点Ｐ_ｍにおいてそれぞれ連続するように、補間曲線Ｓ_ｍ（ｔ）を算出している。このような軌道によって、ツールの滑らかな動きを実現することができる。しかし、ツールの非常に高精度な位置制御の要求に応えるためには、軌道のさらなる改善が求められる。

本開示の一態様は、ｎ個の点Ｐ_１，Ｐ_２，…，Ｐ_ｎを通過する軌道を生成する軌道生成装置であって、第１の曲線によって表され、ｉ−１番目からｉ＋２番目の４つの点Ｐ_ｉ−１，Ｐ_ｉ，Ｐ_ｉ＋１，Ｐ_ｉ＋２によって定義される、ｉ番目の点Ｐ_ｉとｉ＋１番目の点Ｐ_ｉ＋１との間の第１の部分軌道と、第２の曲線によって表され、ｉ番目からｉ＋３番目の４つの点Ｐ_ｉ，Ｐ_ｉ＋１，Ｐ_ｉ＋２，Ｐ_ｉ＋３によって定義される、ｉ＋１番目の点Ｐ_ｉ＋１とｉ＋２番目の点Ｐ_ｉ＋２との間の第２の部分軌道とを算出し、ただし、１≦ｉ≦ｎ−２であり、点Ｐ_ｉ＋１での前記第１の曲線の１階の導関数の値と、点Ｐ_ｉ＋１での前記第２の曲線の１階の導関数の値とが、相互に一致し、点Ｐ_ｉ＋１での前記第１の曲線の２階の導関数の値と、点Ｐ_ｉ＋１での前記第２の曲線の２階の導関数の値とが、相互に一致し、点Ｐ_ｉ＋１での前記第１の曲線の３階の導関数の値と、点Ｐ_ｉ＋１での前記第２の曲線の３階の導関数の値とが、相互に一致する、軌道生成装置である。

一実施形態に係る自動位置制御装置および軌道生成装置のブロック図である。軌道生成方法を説明する図である。関数Ｋ（ｕ）の一例および比較例の関数ｆ（ｕ），ｇ（ｕ），ｈ（ｕ）のグラフである。

以下に、一実施形態に係る軌道生成装置、自動位置制御装置および軌道生成方法について図面を参照して説明する。
自動位置制御装置２は、制御対象の位置を制御するものである。図１に示されるように、制御対象４ａの一例は、産業用ロボット４のロボットアームの先端、または、ロボットアームの先端に接続されたハンドまたはツール等のエンドエフェクタである。制御対象の他の例は、数値制御（ＮＣ）工作機械のツールである。したがって、自動位置制御装置２は、産業用ロボットを制御するロボット制御装置またはＮＣ工作機械を制御する数値制御装置であり得る。

自動位置制御装置２は、図１に示されるように、点列として与えられる離散的なｎ個の点Ｐ_１，Ｐ_２，…，Ｐ_ｎを通過する軌道を生成する軌道生成装置１と、軌道生成装置１によって生成された軌道に沿って制御対象４ａを移動させる制御部３とを備える。制御対象４ａがロボットアームの先端またはエンドエフェクタである場合、制御部３は、ロボットアームの動作を制御するロボット制御部である。

軌道生成装置１は、中央演算処理装置のようなプロセッサ１ａと、ＲＡＭ、ＲＯＭおよびその他の任意の記憶装置を有する記憶部１ｂとを有する。
記憶部１ｂには、ｎ個の点Ｐ_ｉ（ｉ＝１，２，３，…，ｎ）の位置が記憶されている。点Ｐ_ｉは、例えば、作業者によって設定された教示点である。２次元平面内で移動する制御対象４ａの場合、点Ｐ_ｉの位置は２次元座標（ｘ_ｉ，ｙ_ｉ）で表され、３次元空間内で移動する制御対象４ａの場合、点Ｐ_ｉの位置は３次元座標（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）で表される。
また、記憶部１ｂには、軌道生成プログラムが格納されている。軌道生成装置１による後述の計算は、プロセッサ１ａが軌道生成プログラムに従って処理を実行することによって実現される。

次に、軌道生成装置１による軌道生成方法について説明する。
図２に示されるように、軌道生成装置１は、点列において隣接する２点Ｐ_ｉ，Ｐ_ｉ＋１を通過り２点Ｐ_ｉ，Ｐ_ｉ＋１間を補間する曲線Ｓ_ｉを算出し、曲線Ｓ_ｉによって表される２点Ｐ_ｉ，Ｐ_ｉ＋１間の部分軌道を算出する。点Ｐ_ｉは点列におけるｉ番目の点である。ここで、軌道生成装置１は、ｉ−１番目からｉ＋２番目の４つの点Ｐ_ｉ−１，Ｐ_ｉ，Ｐ_ｉ＋１，Ｐ_ｉ＋２に基づいて曲線Ｓｉを算出することによって、４つの点Ｐ_ｉ−１，Ｐ_ｉ，Ｐ_ｉ＋１，Ｐ_ｉ＋２によって定義される点Ｐ_ｉ，Ｐ_ｉ＋１間の部分軌道を算出する。軌道生成装置１は、隣接する２点Ｐ_ｉ，Ｐ_ｉ＋１間の区間毎に曲線Ｓ_ｉおよび部分軌道を算出することによって、ｎ−１個の曲線Ｓ_１，Ｓ_２，…，Ｓ_ｎ−１とｎ−１個の部分軌道とを得る。
次に、軌道生成装置１は、ｎ−１個の部分軌道を相互に連結することによって、ｎ個の点Ｐ_１，Ｐ_２，…，Ｐ_ｎを全て通過する軌道を生成する。

ここで、軌道生成装置１は、下記３つの条件を満たす曲線Ｓ_ｉ（ただし、１≦ｉ≦ｎ−２）を算出する。すなわち、点Ｐ_ｉ＋１において、曲線（第１の曲線）Ｓ_ｉ（ｕ_ｉ）および曲線（第２の曲線）Ｓ_ｉ＋１（ｕ_ｉ＋１）の１階の導関数の値が相互に一致する。また、点Ｐ_ｉ＋１において、曲線Ｓ_ｉ（ｕ_ｉ）および曲線Ｓ_ｉ＋１（ｕ_ｉ＋１）の２階の導関数の値が相互に一致する。また、点Ｐ_ｉ＋１において、曲線Ｓ_ｉ（ｕ_ｉ）および曲線Ｓ_ｉ＋１（ｕ_ｉ＋１）の３階の導関数の値が相互に一致する。

曲線Ｓ_ｉ（ｕ_ｉ）の１階、２階および３階の導関数の値は、部分軌道に沿って移動する制御対象４ａの速度、加速度および加加速度をそれぞれ表す。したがって、軌道生成装置１によって生成された軌道に沿って移動する制御対象４ａの位置、速度、加速度および加加速度は、部分軌道の接続点である点Ｐ_２，Ｐ_３，…，Ｐ_ｎ−１の全てにおいて連続する。

次に、曲線Ｓ_ｉ（ｕ_ｉ）の具体的な算出方法について説明する。
図２に示されるように、ｉ＝２，３，…，ｎ−１について、連続する３点Ｐ_ｉ−１，Ｐ_ｉ，Ｐ_ｉ＋１を通過する曲線の関数Ｆ_ｉ（ｕ_ｉ）を算出し、ｉ＝１について、２点Ｐ_１，Ｐ_２を通る関数Ｆ_１（ｕ_１）を算出する。ｕ_ｉは、点Ｐ_ｉと点Ｐ_ｉ＋１との間の補間の進捗を０から１の値で表す変数である。ただし、ｕ_ｉ＋１＝ｕ_ｉ−１である。すなわち、点Ｐ_ｉにおいてｕ_ｉ＝０であり、点Ｐ_ｉ−１においてｕ_ｉ＝−１であり、点Ｐ_ｉ＋１においてｕ_ｉ＝１である。関数Ｆ_ｉ（ｕ_ｉ）は、ｕ_ｉ＝−１において点Ｐ_ｉ−１を通過し、ｕ_ｉ＝０において点Ｐ_ｉを通過し、ｕ_ｉ＝１において点Ｐ_ｉ＋１を通過する。

図２には、変数ｕ_ｉを表わすｕ軸とｘ_ｉを表わすｘ軸とを用いて表現されるｕ−ｘ平面上の点列Ｐ_１，Ｐ_２，…，Ｐ_ｎが示されている。前記平面において、関数Ｆ_ｉ（ｕ_ｉ）は、円弧、惰円弧および放物線の中から選択される２次曲線の関数である。関数Ｆ_ｉ（ｕ_ｉ）は、他の２次曲線の関数、または３次以上の曲線の関数であってもよい。
このように、点列Ｐ_１，Ｐ_２，…，Ｐ_ｎをｘ次元、ｙ次元およびｚ次元に分解して考える場合、ｕ−ｘ平面上の点列Ｐ_１，Ｐ_２，…，Ｐ_ｎと同様に、ｕ−ｙ平面上の点列Ｐ_１，Ｐ_２，…，Ｐ_ｎについて関数Ｆ_ｉ（ｕ_ｉ）が算出され、ｕ−ｚ平面上の点列Ｐ_１，Ｐ_２，…，Ｐ_ｎについて関数Ｆ_ｉ（ｕ_ｉ）が算出される。

次に、下式（１）から、Ｓ_ｉ（ｕ_ｉ）を算出する。
Ｓ_ｉ（ｕ_ｉ）＝（１−Ｋ（ｕ_ｉ））×Ｆ_ｉ（ｕ_ｉ）＋Ｋ（ｕ_ｉ）×Ｆ_ｉ＋１（ｕ_ｉ＋１）…（１）
Ｋ（ｕ_ｉ）は、下記条件１および条件２を満たす関数である。
（条件１）ｕ_ｉが０から１に変化するとき、Ｋ（ｕ_ｉ）の値が０から１へ単調増加する。
（条件２）ｕ_ｉ＝０でのＫ（ｕ_ｉ）の１階および２階の導関数の値と、ｕ_ｉ＝１でのＫ（ｕ_ｉ）の１階および２階の導関数の値と、の全てが０である。

一例において、Ｋ（ｕ_ｉ）は、下式（２）によって定義される関数である。
Ｋ（ｕ_ｉ）＝ｕ^３（１０−１５ｕ＋６ｕ^２） …（２）
したがって、Ｋ（ｕ_ｉ）の１階の導関数Ｋ’（ｕ_ｉ）および２階の導関数Ｋ’’（ｕ_ｉ）は、それぞれ下記のように表される。
Ｋ’（ｕ_ｉ）＝３０ｕ^２（１−２ｕ＋ｕ^２）
Ｋ’’（ｕ_ｉ）＝６０ｕ^２（１−３ｕ＋２ｕ^２）

図３は、式（２）のＫ（ｕ_ｉ）のグラフと、比較例として３つの関数ｆ（ｕ），ｇ（ｕ），ｈ（ｕ）のグラフとを示している。関数ｆ（ｕ），ｇ（ｕ），ｈ（ｕ）は、ｕ＝０において０であり、ｕ＝１において１である。ただし、関数ｆ（ｕ），ｇ（ｕ），ｈ（ｕ）の１次または２次の導関数の値は、ｕ＝０，１において０にならない。
Ｋ（ｕ_ｉ）は、式（２）の５次の関数に限らず、６次以上の関数であってもよい。例えば、Ｋ（ｕ_ｉ）は、下式によって定義される７次の関数であってもよい。
Ｋ（ｕ_ｉ）＝ｕ^４（３５−８４ｕ＋７０ｕ^２−２０ｕ^３）

式（２）のＫ（ｕ_ｉ）およびその導関数Ｋ’（ｕ_ｉ），Ｋ’’（ｕ_ｉ）のｕ_ｉ＝０，１での値は、下記の通りである。
Ｋ（０）＝０
Ｋ（１）＝１
Ｋ’（０）＝Ｋ’（１）＝Ｋ’’（０）＝Ｋ’’（１）＝０
また、Ｆ_ｉ（ｕ_ｉ）およびＦ_ｉ−１（ｕ_ｉ−１）は共に点Ｐ_ｉを通過するので、Ｆ_ｉ（０）＝Ｆ_ｉ−１（１）である。

したがって、点Ｐ_ｉでの曲線Ｓ_ｉ（ｕ_ｉ），Ｓ_ｉ−１（ｕ_ｉ−１）およびこれらの導関数の値は、下記の通りである。
Ｓ_ｉ（０）＝Ｓ_ｉ−１（１）
Ｓ_ｉ’（０）＝Ｓ_ｉ−１’（１）
Ｓ_ｉ’’（０）＝Ｓ_ｉ−１’’（１）
Ｓ_ｉ’’’（０）＝Ｓ_ｉ−１’’’（１）

このように、本実施形態によれば、隣接する２つの部分軌道の接続点Ｐ_ｉ＋１において、曲線Ｓ_ｉ（ｕ_ｉ），Ｓ_ｉ＋１（ｕ_ｉ＋１）の１次の導関数の値が相互に一致し、曲線Ｓ_ｉ（ｕ_ｉ），Ｓ_ｉ＋１（ｕ_ｉ＋１）の２次の導関数の値が相互に一致する。さらに、接続点Ｐ_ｉにおいて、曲線Ｓ_ｉ（ｕ_ｉ），Ｓ_ｉ＋１（ｕ_ｉ＋１）の３次の導関数の値も相互に一致する。すなわち、接続点Ｐ_ｉにおいて、制御対象４ａの位置、速度および加速度のみならず加加速度も連続する。これにより、接続点Ｐ_ｉ付近での制御対象４ａの位置制御の精度を向上することができる。

なお、軌道生成装置１は、記憶部１ｂに予め記憶されているｎ個の点Ｐ_１，Ｐ_２，…，Ｐ_ｎの全てを必ずしも通過する必要は無く、記憶されている点Ｐ_ｊの近傍に新たな点Ｐ_ｊ’を必要に応じて作成し、点Ｐ_ｊに代えて点Ｐ_ｊ’を通過するように軌道を生成してもよい。
例えば、ｎ個の点Ｐ_１，Ｐ_２，…，Ｐ_ｎが教示点である場合、一部の教示点については制御対象４ａが厳密に通過する必要が無いことがある。このような場合、例えば制御対象４ａを滑らかな移動により最適な軌道を生成するために、教示点Ｐ_ｊを近傍の点Ｐ_ｊ’に変更して教示点Ｐ_ｊの近傍を通過する軌道を生成してもよい。

１軌道生成装置
２自動位置制御装置
３制御部
４ａ制御対象
Ｓ_ｉ（ｕ_ｉ）第１の曲線

Claims

ｎ個の点Ｐ_１，Ｐ_２，…，Ｐ_ｎを通過する軌道を生成する軌道生成装置であって、
第１の曲線によって表され、ｉ−１番目からｉ＋２番目の４つの点Ｐ_ｉ−１，Ｐ_ｉ，Ｐ_ｉ＋１，Ｐ_ｉ＋２によって定義される、ｉ番目の点Ｐ_ｉとｉ＋１番目の点Ｐ_ｉ＋１との間の第１の部分軌道と、第２の曲線によって表され、ｉ番目からｉ＋３番目の４つの点Ｐ_ｉ，Ｐ_ｉ＋１，Ｐ_ｉ＋２，Ｐ_ｉ＋３によって定義される、ｉ＋１番目の点Ｐ_ｉ＋１とｉ＋２番目の点Ｐ_ｉ＋２との間の第２の部分軌道とを算出し、ただし、１≦ｉ≦ｎ−２であり、
点Ｐ_ｉ＋１での前記第１の曲線の１階の導関数の値と、点Ｐ_ｉ＋１での前記第２の曲線の１階の導関数の値とが、相互に一致し、
点Ｐ_ｉ＋１での前記第１の曲線の２階の導関数の値と、点Ｐ_ｉ＋１での前記第２の曲線の２階の導関数の値とが、相互に一致し、
点Ｐ_ｉ＋１での前記第１の曲線の３階の導関数の値と、点Ｐ_ｉ＋１での前記第２の曲線の３階の導関数の値とが、相互に一致する、軌道生成装置。
連続する３点Ｐ_ｉ−１，Ｐ_ｉ，Ｐ_ｉ＋１を通過する曲線の関数Ｆ_ｉ（ｕ_ｉ）と、連続する３点Ｐ_ｉ，Ｐ_ｉ＋１，Ｐ_ｉ＋２を通過する曲線を関数Ｆ_ｉ＋１（ｕ_ｉ＋１）とを算出し、
前記第１の曲線Ｓ_ｉ（ｕ_ｉ）を下式（１）から算出する、請求項１に記載の軌道生成装置。
Ｓ_ｉ（ｕ_ｉ）＝（１−Ｋ（ｕ_ｉ））×Ｆ_ｉ（ｕ_ｉ）＋Ｋ（ｕ_ｉ）×Ｆ_ｉ＋１（ｕ_ｉ＋１）…（１）
ただし、
ｕ_ｉは、点Ｐ_ｉと点Ｐ_ｉ＋１との間の補間の進捗を０から１で表す変数であり、ただしｕ_ｉ＋１＝ｕ_ｉ−１であり、
Ｋ（ｕ_ｉ）は、ｕ_ｉが０から１に変化するときにＫ（ｕ_ｉ）の値が０から１へ増加する関数であり、かつ、ｕ_ｉ＝０でのＫ（ｕ_ｉ）の１階および２階の導関数の値ならびにｕ_ｉ＝１でのＫ（ｕ_ｉ）の１階および２階の導関数の値の全てが０である。
Ｋ（ｕ_ｉ）は、下式（２）によって定義される関数である、請求項２に記載の軌道生成装置。
Ｋ（ｕ_ｉ）＝ｕ^３（１０−１５ｕ＋６ｕ^２） …（２）
Ｆ_ｉ（ｕ_ｉ）が、円弧、惰円弧および放物線の中から選択される２次曲線の関数である、請求項２または請求項３に記載の軌道生成装置。
制御対象の位置を制御する自動位置制御装置であって、
請求項１から請求項４のいずれかに記載の軌道生成装置と、
該軌道生成装置によって生成された軌道に沿って前記制御対象を移動させる制御部と、を備える自動位置制御装置。
前記制御部が、産業用ロボットのロボットアームの動作を制御するロボット制御部であり、
前記制御対象が、ロボットアームの先端、または、該ロボットアームの先端に接続されたエンドエフェクタである、請求項５に記載の自動位置制御装置。
ｎ個の点Ｐ_１，Ｐ_２，…，Ｐ_ｎを通過する軌道を生成する軌道生成方法であって、
第１の曲線によって表され、ｉ−１番目からｉ＋２番目の４つの点Ｐ_ｉ−１，Ｐ_ｉ，Ｐ_ｉ＋１，Ｐ_ｉ＋２によって定義される、ｉ番目の点Ｐ_ｉとｉ＋１番目の点Ｐ_ｉ＋１との間の第１の部分軌道と、第２の曲線によって表され、ｉ番目からｉ＋３番目の４つの点Ｐ_ｉ，Ｐ_ｉ＋１，Ｐ_ｉ＋２，Ｐ_ｉ＋３によって定義される、ｉ＋１番目の点Ｐ_ｉ＋１とｉ＋２番目の点Ｐ_ｉ＋２との間の第２の部分軌道とを算出し、ただし、１≦ｉ≦ｎ−２であり、
点Ｐ_ｉ＋１での前記第１の曲線の１階の導関数の値と、点Ｐ_ｉ＋１での前記第２の曲線の１階の導関数の値とが、相互に一致し、
点Ｐ_ｉ＋１での前記第１の曲線の２階の導関数の値と、点Ｐ_ｉ＋１での前記第２の曲線の２階の導関数の値とが、相互に一致し、
点Ｐ_ｉ＋１での前記第１の曲線の３階の導関数の値と、点Ｐ_ｉ＋１での前記第２の曲線の３階の導関数の値とが、相互に一致する、軌道生成方法。