JP2019063912A - ロボット制御データ処理方法、ロボット制御データ処理装置、およびロボットシステム - Google Patents

Abstract

【課題】経路の始点・終点で、ジャーク（加加速度）が飽和しないよう適切に制約した上、速度制御を行える、最適な軌道データを生成できるようにする。【解決手段】ロボットアーム（Ａ）の特定部位を動作させる経路の始点または終点である教示点の入力を取り込む（Ｓ１００）。入力された第１および第２の教示点の間に補間点を生成し、ロボットアーム（Ａ）の特定部位を動作させる経路を生成する（Ｓ１０２、Ｓ１０４）。生成された経路の始点または終点を含む特定区間において多関節ロボットの関節の加加速度が上限値に対して飽和する場合の特定部位の飽和速度を演算する（Ｓ１０６）。飽和速度を軌道データの速度制約値として設定した上で、第１および第２の教示点の間を最短時間で特定部位を移動させる速度情報を生成する（Ｓ１１０、Ｓ１１２）。【選択図】図３

Description

本発明は、多関節ロボットの特定部位を移動させる経路と、その際の特定部位の速度情報と、を含む軌道データを生成するロボット制御データ処理方法、ロボット制御データ処理装置、およびロボットシステムに関する。

ロボットアームのプログラミングを行う場合、例えば、順次ロボットアームの特定部位を位置させる位置（あるいはさらに姿勢）に相当する教示点群を指定する手法が用いられることがある。教示点の位置姿勢を取るロボットアームの特定の基準部位には、例えばアーム先端のツールの中心位置、例えばＴＣＰ（Ｔｏｏｌ Ｃｅｎｔｅｒ Ｐｏｉｎｔ）が用いられる。この場合、教示点は、アームの作業空間（タスク空間）におけるＴＣＰの位置姿勢を示す座標値などにより表現される。あるいは、教示点は、アームの各関節の角度情報（群）として指定される場合もある。この場合には、教示点は、関節角度を座標軸とする仮想的な関節空間における座標値などとして取り扱うことができる。

ＴＣＰを位置させる複数の教示点情報が用意されている場合、ロボット制御装置によって、１つの教示点を始点、次の教示点を終点とし、その間にアームの特定部位を移動させる経路と速度を制御するための軌道データが生成される。この軌道データの具体的な記述形式は種々考えられるが、例えばロボットアームの特定時刻における各関節の角度指令値のリストなどが用いられることがある。軌道データが生成されると、この軌道データに基づき実際にロボットアームを駆動し、教示点情報によってプログラミングした動作を実行させることができる。

また、始点と終点の教示点の経路は、直線補間による他、始点と終点の間に補間点を生成して、あるいは多数の教示点の位置関係などに応じて、Ｂ−スプライン曲線などの曲線補間を行うことによって、経路情報を生成する場合もある。

一方、工業製品や部品の製造を行う生産現場用いられる産業用のロボット装置では、作業効率などの観点から、軌道データ生成においては、ロボットアームを動作させる最適な速度情報を計算できるのが望ましい。例えば、非特許文献１では、教示点情報によって与えられた経路上で、各種物理的制約を守り、動作時間を最短にするロボットの最適な動作速度を計算する最適速度の計算方法が記載されている。

教示点情報により与えられた経路で、ロボットの動作速度を調整し動作軌道を作成する場合、例えば各種物理的制約を守りつつ、ロボットの動作時間が短くなるように、速度パラメータ調整する。旧来、この作業は、例えばオペレータが速度パラメータを何度も調整することによって行われていた。しかし、オペレータがパラメータ調整をする場合、必ずしも軌道が最適にならず、あるいは動作時間が長くなったりする問題があり、また、作業は煩雑であり、多大な開発工数を要する問題があった。そこで、各種物理的制約を守り、動作時間を最短にするロボットの最適な動作速度の計算をアームに接続されたロボット制御装置、あるいは他のロボット制御データ処理装置が自動で行えるのが望ましい。

例えば、非特許文献１では、ロボットの関節トルク制約を守りつつ、与えられた経路に対してできる限り動作時間の短い軌道を自動生成する速度最適化方法が記載されている。ここで、以下に示す数式における各種パラメータの文字式を以下のように定義する。なお、各文字式において、ボールド体の文字はベクトルを示す。また、文字式上にドットを置く各数式中の表記とは必ずしも一致しないが、便宜上、本明細書のテキスト中では時間微分に（・）のような表記を用いる。例えば、位置情報の１次微分である速度ｓには、ｓ（・）のような表記を用いる。

与えられた経路に対し、ロボットの最適な軌道を生成するという問題は、与えられた経路上の速度だけを考えればよいという問題に簡略化することができる。例えば、経路上の座標値ｓを定義し、ロボットアーム（の特定部位）の運動方程式は、このｓを用いて下式（１）のように定義することができる。

ここで、Ｍは質量行列、Ｃはコリオリ項、Ｇは重量項、τは関節トルクである。次に、ロボットの関節速度および関節加速度はそれぞれ下式（２）、（３）のように記述される。

上式（２）は、関節角速度および関節角加速度を、経路の位置を示すベクトル、即ちｑ（ｓ）と、経路上の速度、即ちｓ（・）に分解したものと考えられる。つまり、この最適速度問題は、経路が決まっていれば、その経路上を動く速度だけを考えればよく、どの方向に動くかを考えない単純な問題として扱うことが可能となる。式（３）の加速度ｓ（・・）についても同様のことが言える。

ここで、式（１）に式（２）、（３）を代入すると

その場合、ｍ（ｓ）、ｃ（ｓ）、ｇ（ｓ）は

である。

この変形により、「関節トルク制約を守りつつ動作時間を最適にするため、経路上をどういう速度で動かすか」という問題は、「関節トルク制約を守りつつ動作時間を最適にするｓ（ｔ）をどのように設計するか」という問題に置き換えられる。時間ｔとｓの関係であるｓ（ｔ）をどう設計するかにより、軌道の動作時間が決まる。即ち、ロボットの動作時間が最短になるようなｓ（ｔ）を計算できれば、最適な軌道データを生成することができる。また、ｓ（・）を積分すれば特定時刻における位置ｓ（ｔ）を計算できるから、ｓ（・）を求めればよい。

ここで、ｔ＝０からスタートし、ｔ＝Ｔで終わる軌道ｓ（０）＝０≦ｓ≦１＝ｓ（Ｔ）を考える。また、時間最適軌道を生成すべく、ｔ∈［０、Ｔ］で、ｓ（・）＞０であると仮定する。

以上より、この最適速度問題は、次のような最適化問題として記述できる。

しかし、この問題は、例えばｓ（・）を変数としたとき、非線形かつ、非凸最小化問題となるため、最適解を求めるのが困難である、という問題がある。そこで、非特許文献１では、この問題に対し、ａ（ｓ）、ｂ（ｓ）という２つの変数を導入し、凸問題化を行っている（下式（１７）、（１８））。即ち

とすると

となり、ａ（ｓ）、ｂ（ｓ）の線形の関係式が書ける。また、目的関数は式（１８）より

というｂに関する凸関数で書ける。

また、トルクに関しては、式（９）に、式（１７）、（１８）を代入する。

と表現でき、これは変数ａとｂに関して線形となる。ここで、上記の最適化問題を書き直すと

となる。この場合、目的関数である式（２２）が凸関数であり、制約条件である式（２３）−（２９）が全て線形であるため、凸最小化問題として解くことが可能となる。

凸最小化問題とは最適化問題の分野の１つで、凸集合上の凸関数の最小化問題である。凸最小化問題は一般的な最適化問題よりも簡単に最適化が可能であり、局所的な最小値が大域的な最小値と一致する性質を持つ。

なお、以上では、制約は関節トルク制約のみを考え説明したが、他に、関節角速度制約、関節角加速度制約、ジャーク制約、手先速度制約、手先加速度制約などを適用することも考えられる。また、以下では、簡略化のため、ｓ（・）の２乗であるｂ（ｓ）を経路上の速度と表現し、また、ｂ（ｓ）にはｂ、ｑ（ｓ）にはｑのような表記を用いることがある。

上記のトルク制約と同様、ジャーク制約を考慮に入れて最適な軌道を生成することも可能である。ここで、ジャークとは、ロボットアームの関節角の加加速度を指す。ジャークは、関節角度ｑの時間での３階微分に相当し、ジャークが大きいと、ロボットの振動が大きくなるため、ジャーク制約を適用するのがのぞましい。

ジャークはｂ、ｂ′、ｂ′′、ｑ′、ｑ′′、ｑ′′′を用いて

と書ける。ここで、ロボットアームに複数備えられた関節軸のインデックスをｉとすれば、関節軸ｉに関する上式（３１）の一般表現は

のようになる。そして、上記の関節トルク制約の場合と同様に

というジャークの上限、下限を守るよう最適速度問題を解くことができる。

ところが、Ｂ−スプライン曲線のような曲線補間で生成した経路の場合、始点・終点付近において、ｑ′′′、ｑ′′、ｑ′の値がほぼ０になってしまうことがある。この場合、どんな速度ｂを与えても、ジャークがほぼ０の値になってしまい、ジャーク制約の計算が適切に行えない、という問題が生じる。そして、このまま軌道生成を行うと、経路の始点・終点付近において、ジャーク制約を適用できず、例えばジャークが無限大に発散してしまうという現象が起きてしまう。

一方、非特許文献２では、例えば、図４に示すように、関節角の加速度制約を始点・終点から線形に増加させることにより、経路の始点、および終点付近におけるジャークを抑制する対処方法をとっている。図４は、横軸を補間経路の媒介変数ｓ、縦軸を関節角加速度制約としたグラフである。図４において、経路の始点付近では、加速度制約を０に取り、補間経路の媒介変数ｓがｓ１の時、関節角加速度制約が最大となるように線形補間している（ｉは関節軸のインデックス）。また、終点付近では、ｓ２のときに関節角の加速度制約が最大値を取り、そこから終点で０になるよう線形補間している。

このような制御により、関節角加速度を始点および終点付近で適宜抑制でき、加速度の急激な変化が抑えられ、関節角の加速度を単に一定に制御するよりも、経路の始点・終点付近におけるジャークの飽和を抑制することができる。

また、特許文献１は、２つの経路が連続している時、それらの経路が関節補間と直線補間のように異なる補間処理によって生成される場合の速度の結合処理を行えるようにする技術を開示している。特許文献１の制御では、連続する２つの経路で、補間種別が関節補間動作から直線補間動作に移り変わるか、直線補間動作から関節補間動作に移り変わるかを判別する。その２つの経路の繋ぎ点もしくはその近傍を通過させる際に速度の結合処理が可能かどうかを判定（速度結合処理可否判定部（２））する。そして、速度の結合処理が可能であれば、異種補間動作間の速度を重ね合わせるような挙動をさせるための補間点を関節変数により決定する（速度結合演算部（３））。特許文献１の技術によれば、経路の遷移の際の速度低下を防ぎ、従って加加速度の飽和も抑制できる可能性がある。

特開２００４−２５２８１４号公報

Verscheure、 Diederik、 et al. "Time-optimal path tracking for robots: a convex optimization approach." Automatic Control、 IEEE Transactions on 54.10 (2009): 2318-2327. Reynoso-Mora, Pedro, Wenjie Chen, and Masayoshi Tomizuka. "On the time-optimal trajectory planning and control of robotic manipulators along predefined paths." 2013 American Control Conference. IEEE, 2013

非特許文献１の技術は、特定の経路情報が与えられた時、関節トルク制約、関節角速度制約、関節角の加速度（ジャーク）制約、手先速度制約、手先加速度制約などの物理的制約を守りつつ時間を最短にするという最適化問題を凸問題化する手法を用いている。しかし、非特許文献１の手法では、上記のように経路の始点および終点付近でｑ′′′、ｑ′′、ｑ′がほぼ０の値を取る経路では、どんな速度ｂを与えても、ジャークがほぼ０の値となり、ジャーク制約の計算を適切に行えない、という問題がある。

一方、非特許文献２は、関節角の加速度制約を始点・終点から、線形に増加させることにより経路の始点・終点付近におけるジャークを抑える手法を採用している。しかしながら、あらかじめ指定したジャーク制約を守る保証がなく、また、得られた軌道が最適な軌道になるとは限らないという問題がある。これは、非特許文献２の手法によると、軌道を最適化する前は、経路の媒介変数であるｓを基準に関節角加速度制約を決めているので、ジャークがどの位置でどのような値になるかは、軌道を生成してからでないと分からないためである。即ち、ある時間にどのジャークになるといった情報は、経路上の速度ｂを最適化した後、実際の軌道が生成した後でないと得られない。例えば、図４に示したように、横軸が経路の媒介変数ｓであり、時間ではない。このため、線形に関節角加速度が始点・終点で増加、減速していても、時間の３階微分であるジャークがどのように制約されるかは、経路と速度情報を含む軌道データを生成してからでないと分からない。

また、特許文献１は、関節補間と直線補間のように異なる補間処理によって生成されている場合に速度合成を行う手法に関するものであるが、複雑な処理を必要とする問題がある。また、特許文献１の手法は、連続する区間の速度合成を行う手法である。このため、経路の始点または終点である教示点において、ロボットアームの特定部位の速度を０に制御すべき条件がある場合に一般的に利用できる速度（加速度、あるいは加加速度）制御の手法とはいえない。

そこで、本発明の課題は、経路の始点・終点で、ジャーク（加加速度）が飽和しないよう適切に制約した上、速度制御を行える、最適な軌道データを生成できるようにすることにある。

上記課題を解決するため、本発明においては、制御装置が、多関節ロボットの特定部位を移動させる経路と、その際の特定部位の速度情報と、を含む軌道データを生成するロボット制御データ処理方法において、前記制御装置は、前記経路の始点または終点である教示点の入力を取り込む入力工程と、前記入力工程で入力された始点と終点に相当する教示点の間を補間し、前記特定部位を動作させる経路を生成する補間工程と、前記補間工程で生成された前記経路の前記始点または前記終点を含む特定区間において前記多関節ロボットの関節の加加速度が上限値に対して飽和する場合の前記特定部位の飽和速度を演算するジャーク演算工程と、前記飽和速度を軌道データの速度制約値として設定した上で、前記始点と終点の間を特定の条件で前記特定部位を移動させる速度情報を生成する速度情報生成工程と、を実行する構成を採用した。

上記構成により、経路の始点・終点で、ジャーク（加加速度）が飽和しないよう適切に制約した上、速度制御を行える、最適な軌道データを生成することができる。

本発明の実施形態に係るロボット制御データ処理装置の機能的な構成を示すブロック図である。 本発明の実施形態に係るロボットアームにおける２軸構成を示す説明図である。 本発明の実施形態に係る軌道データの生成処理を説明するためのフローチャート図である。 非特許文献２のジャーク制約の制御を示した説明図である。 本発明の実施形態に係るロボット制御データ処理装置またはロボット制御装置の構成例を示したブロック図である。 図２のロボットアームのより具体的な構成例を示した説明図である。

以下、添付図面を参照して本発明を実施するための形態につき説明する。なお、以下に示す構成はあくまでも一例であり、例えば細部の構成については本発明の趣旨を逸脱しない範囲において当業者が適宜変更することができる。また、本実施形態で取り上げる数値は、参考数値であって、本発明を限定するものではない。

一般に、ロボットアームは関節とリンク（リンク機構）で構成される。リンク間に配置される関節には回転関節や直動関節の他、球体関節などさまざまな種類がある。また、関節はモータなどの駆動源で能動的に動作する構成の他、動力源を持たずに受動的に動作するものも含まれる。各関節間はリンクにより結合され、関節とリンクが交互に直列されたシリアルリンク型と関節とリンクの組合せが並列となったパラレルリンク型などがある。以下では回転関節を有するシリアルリンク型のロボットアームを例に説明するが、後述の技術はリンク・関節を有するロボットアームであればその結合態様拘らず同様に実施することができる。

図１は、本発明のロボット制御データ処理装置の実施形態として、軌道生成装置の構成を示している。軌道生成装置１は、オペレータのデータ入力を受け付ける操作部２、図示しないＣＰＵ等から成る演算処理部３、種々のデータを格納する記憶部４、を備えている。操作部２にはディスプレイ２１を設ける。ディスプレイ２１は、例えばロボットアームＡ、およびその作業空間を３Ｄ（３次元）グラフィックス表示することができる。このようなユーザーインターフェースにより、オペレータに生成されたロボットの軌道を視認させることができる。

演算処理部３は、図１の右側に示すように、モデル・制約情報入力手段３１と、補間方法・教示点情報入力手段３２（教示点入力装置（手段））と、経路生成手段３３と、を備える。また、演算処理部３は、ジャーク飽和軸予測手段３４と、ジャーク相当速度制約算出手段３５と、速度制約統合手段３６と、速度最適化手段３７と、を備える。これらの演算処理部３を構成する各手段は、より具体的には、後述のＣＰＵ６０１（図５）を用いたハードウェアおよびソフトウェアによって実装することができる。

図２は、本実施形態において、軌道データを生成するロボットアームＡの構成を極めて模式的に示したものである。図２において、ロボットアームＡは２つの関節（２自由度）からなる２軸Ｊ１、Ｊ２を備えている。ロボットアームＡの自由度を表すパラメータを関節角度として、ロボットアームＡの２軸Ｊ１、Ｊ２の関節角度をそれぞれθ１、θ２とすれば、ロボットアームＡの関節角度はｑ＝（θ１、θ２）Ｔと表現し、関節空間上では、１つの点とみなすことができる。また、軸Ｊ２の先端には、ＴＣＰ（Ｔｏｏｌ Ｃｅｎｔｅｒ Ｐｏｉｎｔ）がある。今後ＴＣＰをただ単に手先と呼ぶ。

なお、ロボットアームＡは、一般的にはロボットアーム自体が動作するような動作体として広く観念されるものであればよく、図２の模式的な構成は、それにより本発明が限定的に解釈されることを意図したものではない。例えば、ロボットアームＡは、６軸多関節ロボットアームや直動型のロボットアームを用いてもよい。例えば、ロボットアームＡが垂直６軸多関節の構成であれば、ロボットアームＡは後述の図６のような構成とすることができる。

ロボットアームＡの自由度を表すパラメータ（例えば、関節角度や伸縮長さ）を座標軸の値とした場合、ロボットアームＡの関節角度は関節空間上の点として表現することができる。

図６は、模式的な図２の構成よりも具体的なロボットアームＡの全体構成を示している。図６において、ロボットアームＡ（ロボット装置）は、例えば６軸（関節）の垂直多関節形式のアーム本体２０１を備える。アーム本体２０１の各関節は、各関節にそれぞれ設けられたサーボモータをサーボ制御することにより所望の位置姿勢に制御することができる。

ロボットアームＡのアーム本体２０１の動作は、ロボット制御装置２００により制御される。ロボットアームＡの動作はロボット制御装置２００に接続された操作端末２０４（例えばティーチングペンダント）によってプログラミング（教示）することができる。例えば、操作端末２０４により教示点を順次指定することによって、ロボットアームＡの特定部位（例えばＴＣＰ：アーム先端のツール装着面など）を所望の軌跡で移動させる動作をプログラミングすることができる。その場合、教示点情報はロボットアームＡの作業空間（タスク空間）における３次元座標などによって表現される。また、操作端末２０４は、ロボットアームＡの各関節の角度を指定する方式で操作できる場合もあり、その場合、教示点情報は各関節の角度情報のリストなどによって表現される。

ロボットアームＡ、特にそのロボット制御装置２００は、ネットワークＮ（図５）を介して、演算処理部３（ロボット制御データ処理装置）と接続することができる。その場合、操作端末２０４の教示操作によって作成された教示点情報を後述するステップＳ１０２（図３）で演算処理部３に入力することができる。後述する制御例では、演算処理部３によって、ステップＳ１０２で入力された教示点情報を用いて、図３のステップＳ１０４以降の工程が実行される。そして、ロボットアームＡの基準部位（例えばＴＣＰのような特定部位）を移動させる経路と、その際の基準部位の速度情報と、を含む軌道データが生成される。その場合、軌道データの速度情報は、ロボットアームＡの各関節のジャーク（加加速度）が機構の制約などによって定まる上限値に対して飽和しないよう生成される。以下では、なお、この図３のステップＳ１０４以降の工程は、演算処理部３（ロボット制御データ処理装置）で実行するものとして説明する。しかしながら、同じ処理をロボット制御装置２００が実行して、ロボットアームＡの基準部位を移動させる経路と、その際の基準部位の速度情報と、を含む軌道データを生成する構成を採用してもよい。

図６のロボットアームＡは、例えば、基台上に架装されたそれぞれ６つの回転関節を有する６自由度マニプレータとして構成される。ロボットアームＡの各関節はこれら各関節内部に配置された駆動源によってロボット制御装置２００により指定された角度に制御することができ、これによってロボットアームＡを所望の位置姿勢に制御することができる。ロボットアームＡの各関節を駆動する駆動源としては、サーボモータなどが用いられる。

演算処理部３（ロボット制御データ処理装置）で生成された基準部位（例えばＴＣＰ）の経路、および速度情報などを含む軌道データは、ネットワークＮを介してロボットアームＡに送信される。ロボットアームＡでは、送信された軌道データに基づき、アーム本体２０１の各関節を制御することにより、操作端末２０４で教示したロボット動作をロボットアームＡに実行させることができる。その場合、軌道データは、後述の制御手順（例えば図３）によって、各関節のジャーク（加加速度）が機構の制約などによって定まる上限値に対して飽和しないよう制御されている。このため、動作中にロボットアームＡに不要な振動や騒音を生じるような問題が低減される。また、ロボットアームＡの機構の破損や、耐久上問題となるような無理な動作が行なわれるのを回避することができる。

なお、ロボットアームＡの先端には、作業対象のワーク２０３を把持するためのツールとして、グリッパ２０２を装着することができる。このグリッパ２０２の先端部には、ワーク２０３をハンドリングするためのフィンガー（爪）が設けられる。グリッパ２０２の内部にはフィンガーを開閉するアクチュエータが設けられる。このアクチュエータは、モータやソレノイドなどの他、空気圧や油圧方式の任意のアクチュエータによって構成される。グリッパ２０２は、ワーク２０３を特定の作業位置に搬入、搬出したり、もしくは把持したワーク２０３を別のワークに組み付けたりする目的でロボットアームの先端に装着される。なお、ロボットアームＡの先端に配置されるツールは、上記のグリッパに限定されず、他の任意の機能を有するエンドエフェクタであってよい。

図１の演算処理部３（あるいはロボット制御装置２００）には、図５のようなＣＰＵ６０１、ＲＯＭ６０２、ＲＡＭ６０３などを主たる制御手段（制御装置）として用いた構成を利用できる。

図５の制御装置は、主制御手段としてのＣＰＵ６０１、記憶装置としてのＲＯＭ６０２、およびＲＡＭ６０３を備える。ＲＯＭ６０２には、後述する制御手順を実現するためのＣＰＵ６０１の制御プログラムや定数情報などを格納しておくことができる。また、ＲＡＭ６０３は、後述する制御手順を実行する時にＣＰＵ６０１のワークエリアなどとして使用される。

図５の制御装置が図１の演算処理部３である場合、操作部２はインターフェース６０７を介して接続される。操作部２は、例えばハンディターミナルのような端末、あるいはキーボード、ディスプレイ、ポインティングデバイスなどから成る制御端末によって構成することができる。図５の制御装置がロボット制御装置２００である場合には、操作部２は、上記の操作端末２０４に置換することができる。

また、図５の制御装置には、通信手段としてネットワークインターフェース６０５が接続されている。ロボットアームＡは、例えば上記の教示点情報や軌道データはネットワークインターフェース６０５を介してロボットアームＡとの間で授受することができる。その場合、ネットワークインターフェース６０５は、例えばＩＥＥＥ ８０２．３のような有線通信、ＩＥＥＥ ８０２．１１、８０２．１５のような無線通信による通信規格で構成することが考えられる。もちろん、それ以外の任意の通信規格を採用しても構わない。例えば、図５の制御装置がロボット制御装置２００である場合には、ネットワークＮを介することなく、任意の構成の通信線によってロボットアームＡを接続することができる。

なお、後述の制御手順を実現するためのＣＰＵ６０１の制御プログラムは、ＨＤＤやＳＳＤなどから成る外部記憶装置６０４や、ＲＯＭ６０２の（例えばＥＥＰＲＯＭ領域）のような記憶部に格納しておくこともできる。その場合、後述の制御手順を実現するためのＣＰＵ６０１の制御プログラムは、ネットワークインターフェース６０５を介して、上記の各記憶部に供給し、また新しい（別の）プログラムに更新することができる。あるいは、後述の制御手順を実現するためのＣＰＵ６０１の制御プログラムは、各種の磁気ディスクや光ディスク、フラッシュメモリなどの記憶手段と、そのためのドライブ装置を経由して、上記の各記憶部に供給し、またその内容を更新することができる。上述の制御手順を実現するためのＣＰＵ６０１の制御プログラムを格納した状態における各種の記憶手段、記憶部は、本発明の制御手順を格納したコンピュータ読み取り可能な記録媒体を構成することになる。

図３は、本実施形態による軌道生成方法を実現する制御手順の流れを示している。図示の手順は、ＣＰＵ６０１（図５）の制御プログラムとして、上記の如く、例えばＲＯＭ６０２や外部記憶装置６０４などに格納しておくことができる。

図３のステップＳ１００で、モデル・制約情報入力手段３１によって、ロボットアームが作業を行う作業空間での、ロボットアームＡのモデル情報および、制約条件を読み込む。ロボットアームＡのモデル情報は、例えば、ロボットを構成する部品の設計情報であり、各リンクの慣性モーメント、質量、位置、姿勢、軸数などの情報を含む。また、制約条件とは、ロボットの物理的な制約を定義したもので、関節トルク制約、関節角速度制約、関節角加速度制約、ジャーク制約、手先速度制約、手先加速度制約などの条件を含む。これら制約条件は例えば、上限値（あるいは下限値）などの形式で記述されていてよい。

次に、ステップＳ１０２で、補間方法・教示点情報入力手段３２によって、ロボットアームＡの教示点情報の入力を受け付ける。例えば、ロボットアームＡの教示点は、手先（特にＴＣＰ）の位置姿勢、あるいは各軸の関節軸角度により表現される。教示点情報は一般に複数の教示点列（リスト）であって、他の装置、例えば図６のようなロボットシステムではロボット制御装置２００などから送信される。また、本実施形態のロボット制御データ処理装置（演算処理部３）がロボット制御装置２００によって構成される場合には、ステップＳ１０２では、例えば操作端末２０４（教示点入力装置（手段））から教示点情報を読み込むことができる。教示点が手先（ＴＣＰ）の位置姿勢の情報である場合には、手先（ＴＣＰ）の位置姿勢に基づき逆運動学計算を行うことにより、各関節軸の角度の情報に変換することができる。また、教示点が各関節軸角度の表現であれば、その教示点情報は順運動学計算によって手先の位置姿勢の情報に変換することができる。

また、ステップＳ１０２では、教示点情報とともに、どういった補間方法で、その教示点まで移動するかの情報を読み込む。この補間方法に係る情報は操作部２や操作端末２０４から読み込むことができる。補間方法には、タスク空間補間、関節空間補間などの任意の手法が考えられ、いずれの手法を用いても本発明は特にそれによって限定されるものではない。

続いて、ステップＳ１０４では、経路生成手段３３によって、教示点情報に含まれる複数の教示点の間の区間を補間し、経路情報を生成する。ここで経路とは、関節空間におけるロボットアームの関節角度の軌跡、またはもしくはタスク空間におけるロボットアームの手先の軌跡である。なお、本実施形態で、「経路」と言った場合は、「速度」の情報を含まない。そして、本実施形態において、「軌道」ないし「軌道データ」は、ロボットの特定部位、例えば手先（ＴＣＰ）または関節の、それぞれ作業空間（タスク空間）または関節空間における「経路」と、その「経路」を移動させる時の「速度」の情報と、を含む。即ち、本実施形態では、経路に速度情報を持たせた情報を「軌道（データ）」と呼び、速度情報を持たない「経路」と区別する。

関節空間、ロボットアームＡのタスク（作業）空間のいずれで経路を生成するかは、補間方法により異なる。たとえば、関節補間の場合は、関節空間で経路を補間し、手先（ＴＣＰ）を直線や円弧上に動かす場合は、タスク空間で経路を補間することとなる。

教示点間を補間する方法としては、Ｓｐｌｉｎｅ補間、Ｂ−Ｓｐｌｉｎｅ補間、ベジェ曲線補間、直線補間、円弧補間などを利用することができ、例えば媒介変数によりその経路の位置を特定する。ただし、ジャーク（加加速度）制約を考慮に入れる場合、媒介変数に関して経路がＣ^３級である必要がある。例えば、４次のＢ−Ｓｐｌｉｎｅ曲線の場合、経路は媒介変数に対しＣ^３級になる。本実施形態では、便宜上、Ｂ−Ｓｐｌｉｎｅ補間を用いるものとする。

経路生成（Ｓ１０４）の後、経路上の関節角度（ｑ（ｓ））を使って、ｓの微分を行い、ｑ’（ｓ） ｑ’’（ｓ） ｑ’’’（ｓ）を求める。これらは、関節角の速度、加速度、加加速度（ジャーク）に相当する。

続いて、ステップＳ１０６で、ジャーク飽和軸予測手段３４によって、ジャーク（加加速度）が飽和する関節軸を推定する。ここでは、経路の始点、および終点の付近、特に始点、および終点を含む区間（特定区間）において、ジャーク制約の上限（あるいは下限）値に対してそのジャーク（加加速度）が飽和する関節軸を推定する。

例えば、式（３２）の左辺にジャークの上限（ステップＳ１００で取得済み）を代入し、経路上の始点近傍のｓを用いて、ｂで解けば、関節軸ｉが始点付近でジャークが飽和したとしたときの速度ｂ_ｉを求めることができる。そして、ロボットアームＡの各関節軸ごとの速度ｂ_ｉの中で、最も低い値をｂ_ｍｉｎとする。このｂ_ｍｉｎを取る関節軸が、始点付近でジャーク（加加速度）が上限に対して飽和する関節軸である、と特定できる。なぜなら、ｂは多関節ロボットの速度を１次元の速度に集約したスカラー値であり、速度ｂを上げていくと、ｂ_ｍｉｎにまず達し、その軸のジャーク上限に飽和するためである。以下では、このジャーク（加加速度）が上限に対して飽和する関節軸のインデックスをｉ_Ｊとする。終点においても同様の計算を行うことができ、終点付近でジャーク（加加速度）が上限に対して飽和する関節軸を特定できる。

また、本実施形態のようにＢ−Ｓｐｌｉｎｅ補間で経路を補間した場合、経路の始点、終点近傍では、｜ｑ′′′｜が｜ｑ′′｜、｜ｑ′｜に比べて大きくなるため、ｑ′′′だけを考慮に入れればよい。そこで、本実施形態では、経路生成（Ｓ１０４）でＢ−Ｓｐｌｉｎｅ補間を行っている場合、ｑ′′、ｑ′の項は無視し、式（３２）左辺にジャークの上限を代入してｂで解き、それを３乗した式を比較する。ここで、３乗する意図は、式の１／３乗を消し、簡単化するためである。したがって、以下の式をそれぞれの軸で比較し、最大の軸がジャークで飽和する関節軸と推測できる。

ただし、ｉは関節軸のインデックスとし、また、ジャークの上限と下限の値は同じとする。

以上のように、経路の始点・終点近傍でジャークに飽和する関節軸を予測することにより、後述のジャークに飽和する関節軸の速度を求める数値計算はその関節軸だけについて行うだけで良くなり、著しく計算コストを削減できる。

続くステップＳ１０８の処理は、ジャーク相当速度制約算出手段３５（ジャーク演算工程）に相当する。このステップＳ１０８では、ジャーク飽和軸予測手段３４（ステップＳ１０６）により特定した関節軸ｉＪに関し、その軸のジャークが飽和する場合の当該の始点および終点を含む特定区間における速度、即ち飽和速度を求める。この速度を新たに速度制約として採用することにより、始点および終点付近の区間において、ジャーク制約を逸脱しないようロボットアームＡの動作を制御することができる。ジャーク（加加速度）と異なり、速度は始点・終点付近でも正確に計算できる。従って、始点・終点付近で正確に計算できないジャークに対し、それに相当する飽和速度として制約を与え、ジャークを制約することができる。以下では、この関節軸がジャーク制約に飽和する場合の飽和速度に基づく速度制約をジャーク相当速度制約と呼ぶことがある。

なお、ステップＳ１０６では、ジャーク飽和軸予測手段３４で、ｑ′′（ｓ）、ｑ′（ｓ）の項は無視して、ジャークが飽和する関節軸を予測した。しかしながら、ステップＳ１０８でジャーク相当速度制約を求める場合は、正確に計算を行うため、ｑ′′（ｓ）、ｑ′（ｓ）の項を考慮して計算を行う。

ジャーク飽和軸予測手段で求めた関節軸ｉＪに対し、式（３２）の左辺にジャーク制約を代入し、その時の経路上の速度ｂ_Ｊを求める。このようにして、経路の始点および終点付近で、（最も先にジャーク上限に飽和する）関節軸ｉＪのジャーク相当速度制約を取得できる。

ここで、式（３２）の左辺にジャーク制約を代入し、その時の経路上の速度ｂ_Ｊを始点・終点付近のｓで求めるには、式が解析的に解けないため、数値計算が必要となる。たとえば、ルンゲクッタ法などの手法を用いて、ｂ_Ｊを逐次的に経路の始点および終点付近で求めることが考えられる。ここで始点および終点「付近」の定義は様々であるが、例えば、始点付近としたときは、始点からｑ′（速度）がある閾値に達するｓまでを始点付近と定義し、数値計算でそのｓまで計算すればいい。これは、ｑ′がある程度大きくなれば、ｑ′′′、ｑ′′も十分大きくなっているため、ジャーク計算が、式（３１）で正確にできるようになり、通常の最適化計算においてジャークを考慮することが可能になるためである。

なお、本実施形態では、ジャークの飽和する関節軸を予測してから、後述するジャーク相当速度制約の計算を行うが、これに限らず、全関節軸のジャーク相当速度制約を計算し、最も制約が強い速度制約を採用するという構成であっても構わない。

次に、ステップＳ１１０で、速度制約統合手段３６によって、ステップＳ１０８でジャーク相当速度制約算出手段３５を用いて求めた経路上の速度ｂ_Ｊを新たにその当該の経路上の速度ｂの制約として設定する。

なお、非特許文献１にも記載があるが、関節角速度制約および、手先速度制約は、経路上の速度ｂの制約として集約が可能である。たとえば、関節角速度は

のように記述できる。従って、関節角速度制約に相当するｂの制約ｂ_ｑを求めるには

をｂ_ｑで解けばよい。

このように、ｂ_ｑをｂの上限速度として制約すれば、目的のジャーク制約を守るよう関節角速度を制約することができる。また、手先速度制約値についても上記のｂ、ｂ_ｐに係る同様の演算により計算することができる。

以上のようにして、関節角速度制約、手先速度制約、あるいはジャーク相当速度制約のようにいくつかの経路の始点および終点を含む特定区間における複数の計算方式の異なる速度制約を求めている場合、ステップＳ１１０でこれらの速度制約を統合する。この速度制約の統合には、例えばｂ_ｑ、ｂ_ｐ、ｂ_Ｊの中で最も速度値の低いものをｂの制約として設定することが考えられる。これらはいずれもｓの関数であるので、ｓの値を取りうる範囲で、最も小さい値を新たにｂの速度制約値として設定することができる。また、もし、設定済みの速度制約値が既に存在する場合は、その設定済みの速度制約値と、前記飽和速度と、を比較し、前記飽和速度または前記速度制約値のうち、値が小さい方を速度制約値として採用する。

続いて、ステップＳ１１２で、速度最適化手段３７により、当該の経路上の速度ｂを最適化する。ここでは、モデル・制約情報入力手段３１で入力（Ｓ１００）されたロボットモデルが、入力された各種制約を守りつつ、経路生成手段３３で生成した経路上を、例えば最短時間で動作するための経路上の速度ｂを最適化する。このためには、例えば式（２２）〜式（３０）にトルク制約以外の制約を考慮して最適化問題を解けばよい。ただし、関節角速度制約、手先速度制約、ジャーク相当速度制約は、ステップＳ１１０により、既に統合された状態となっている。この最適化問題の解法としては、ログバリア法とニュートン法を用いて最適解に高速に収束させるといった方法が考えられる。

上記のステップＳ１１０、Ｓ１１２（速度制約統合手段３６、速度最適化手段３７）は、飽和速度を軌道データの速度制約値として設定した上で、前記始点と終点の間を特定の条件で前記特定部位を移動させる速度情報を生成する速度情報生成工程に相当する。

さらにステップＳ１１４において、関節角度指令値生成手段３８は、下式（３７）のように速度最適化（Ｓ１１２）により計算したｓ（・）（ｓ）に基づき、ｓ（ｔ）を計算し、ｑ（ｓ）を求め、例えば一定時間Δｔ毎の関節角度指令値を求める。ただし、ｓ（０）＝０とする。当然ながら、この一定時間Δｔは、例えばロボット制御装置が関節角度指令をロボットアームＡに送信するクロック周期とする。

このようにして、始点および終点を含む特定区間のジャーク制約を満足する速度でその経路をロボットアームＡの特定部位（ＴＣＰないし関節空間における関節）を動作させるための軌道データとして、ロボットアームＡの関節角度指令を生成することができる。この関節角度指令値（群）として生成された軌道データをロボットアームＡ（あるいはそのロボット制御装置２００）に送信することにより、ロボットアームＡを当該の軌道で動作させることができる。なお、ステップＳ１１４では、関節角度指令値の形式で軌道データを生成しているが、軌道データの形式は任意であり、ロボットアームＡ（あるいはそのロボット制御装置２００）の制御に適した他のデータ形式を用いてよいのはいうまでもない。

以上のように、本実施形態によれば、始点および終点から経路の情報を作成した上で、その始点および終点を含む特定区間のジャーク制約に相当する飽和速度に基づき、ジャーク相当速度制約として作用させ、当該の経路の速度情報を生成する。その場合、経路の始点および終点を含む特定区間におけるジャーク制約に相当する飽和速度を求め、ジャーク相当速度制約として作用させ、当該の経路の速度情報とする。これにより、当該の始点および終点の教示点により指定された経路およびその経路における速度情報を含む軌道データを生成することができる。その場合、本実施形態によれば、経路の始点・終点で、ジャーク（加加速度）が飽和しないよう制約した上、速度制御を行える、最適な軌道データを生成することができる。

以上、本発明の一実施形態の構成を詳細に説明したが、上述の構成は一例に過ぎず、本発明をその構成に限定することを意図したものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれるのはいうまでもない。本発明は、上述の実施例の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステムまたは装置に供給しそのシステムまたは装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

Ａ…ロボットアーム、Ｊ１、Ｊ２…関節軸、１…軌道生成装置、２…操作部、３…演算処理部、３１…モデル・制約情報入力手段、３２…教示点情報入力手段、３３…経路生成手段、３４…ジャーク飽和軸予測手段、３５…ジャーク相当速度制約算出手段、３６…速度制約統合手段、３７…速度最適化手段、３８…関節角度指令値生成手段、４…記憶部。

Claims (10)

1. 制御装置が、多関節ロボットの特定部位を移動させる経路と、その際の特定部位の速度情報と、を含む軌道データを生成するロボット制御データ処理方法において、前記制御装置は、
   前記経路の始点または終点である教示点の入力を取り込む入力工程と、
   前記入力工程で入力された始点と終点に相当する教示点の間を補間し、前記特定部位を動作させる経路を生成する補間工程と、
   前記補間工程で生成された前記経路の前記始点または前記終点を含む特定区間において前記多関節ロボットの関節の加加速度が上限値に対して飽和する場合の前記特定部位の飽和速度を演算するジャーク演算工程と、
   前記飽和速度を軌道データの速度制約値として設定した上で、前記始点と前記終点の間を特定の条件で前記特定部位を移動させる速度情報を生成する速度情報生成工程と、を実行するロボット制御データ処理方法。
2. 請求項１に記載のロボット制御データ処理方法において、前記制御装置は、前記ジャーク演算工程において、前記補間工程で生成された前記経路の前記始点または前記終点を含む特定区間における前記多関節ロボットのいずれかの関節軸の加加速度がその関節の上限値に対して飽和する場合の前記特定部位の飽和速度を演算するロボット制御データ処理方法。
3. 請求項１または２に記載のロボット制御データ処理方法において、前記制御装置は、速度情報生成工程において、複数の計算方式の異なる速度制約値を求め、その内、最も小さい値を速度制約値として採用するロボット制御データ処理方法。
4. 請求項１から３のいずれか１項に記載のロボット制御データ処理方法の各工程を前記制御装置に実行させる制御プログラム。
5. 請求項４に記載の制御プログラムを格納したコンピュータ読み取り可能な記録媒体。
6. 多関節ロボットの特定部位を移動させる経路と、その際の特定部位の速度情報と、を含む軌道データを生成するロボット制御データ処理装置において、
   前記経路の始点または終点である教示点の入力を取り込む入力工程と、
   前記入力工程で入力された始点と終点に相当する教示点の間を補間し、前記特定部位を動作させる経路を生成する補間工程と、
   前記補間工程で生成された前記経路の前記始点または前記終点を含む特定区間において前記多関節ロボットの関節の加加速度が上限値に対して飽和する場合の前記特定部位の飽和速度を演算するジャーク演算工程と、
   前記飽和速度を軌道データの速度制約値として設定した上で、前記始点と前記終点の間を特定の条件で前記特定部位を移動させる速度情報を生成する速度情報生成工程と、を実行する制御装置と、を備えた ロボット制御データ処理装置。
7. 請求項６に記載のロボット制御データ処理装置において、前記制御装置は、前記ジャーク演算工程において、前記補間工程で生成された前記経路の前記始点または前記終点を含む特定区間における前記多関節ロボットのいずれかの関節軸の加加速度がその関節の上限値に対して飽和する場合の前記特定部位の飽和速度を演算するロボット制御データ処理装置。
8. 請求項６または７に記載のロボット制御データ処理装置において、前記制御装置は、前記速度情報生成工程において、複数の計算方式の異なる速度制約値を求め、その内、最も小さい値を速度制約値として採用するロボット制御データ処理装置。
9. 請求項６から８のいずれか１項に記載のロボット制御データ処理装置と、前記多関節ロボットと、前記ロボット制御データ処理装置により生成された経路および速度情報を含む軌道データを前記多関節ロボットに送信するネットワークと、を備えたロボットシステム。
10. 請求項６から８のいずれか１項に記載のロボット制御データ処理装置が、教示点入力装置と、前記多関節ロボットと、に接続され、前記多関節ロボットの関節を制御するロボット制御装置であり、前記ロボット制御装置は、前記教示点入力装置から入力された教示点に基づき、前記補間工程と、前記ジャーク演算工程と、前記速度情報生成工程と、を実行するロボットシステム。

Images