JP2016016474A - ロボット制御装置 - Google Patents

Abstract

【課題】ティーチングプレイバック方式の冗長ロボットにおいて、教示後に拘束条件を用いた動作を行う場合に、教示時と異なる想定外の動作となる可能性があった。
【解決手段】ロボット制御装置１は、冗長軸を有するマニピュレータ３の操作を受け付ける操作受付部１１と、拘束条件を受け付ける拘束条件受付部１３と、受け付けられた操作と拘束条件とに応じてマニピュレータ３の各軸位置を算出する各軸位置算出部１４と、教示指示を受け付ける教示指示受付部１５と、教示指示の受け付けに応じてマニピュレータ３の教示データを作成する教示データ作成部１６とを備える。そのように、教示時に拘束条件を用いてマニピュレータ３を動作させ、再生時にもその拘束条件を用いるようにすることにより、教示時と再生時の動作を一致させることができ、再生時のマニピュレータ３の動作が想定外のものになることを回避できる。
【選択図】図１

Description

本発明は、少なくとも１つの冗長自由度を持つマニピュレータを制御するロボット制御装置に関する。

６つの自由度を持つマニピュレータを備えたロボットでは、ロボットの手先の位置と姿勢に対して、マニピュレータの姿勢が一意に決定される。そのため、狭い場所でロボットを動作させる場合には、障害物を回避し、かつ、手先軌道を目標軌道とすることができないことがある。その解決のため、７つ以上の自由度を持つマニピュレータを備えた冗長ロボットが利用されることになる。なお、冗長ロボットには冗長軸が存在し、手先の位置姿勢に対して逆変換により求められる各関節の角度が一意に決まらないため、何らかの拘束条件が必要となる。その拘束条件として、例えば、動作量が最小となること、または、ある軸の動作が最小となることなどが採用される。

拘束条件を考慮した動作を行うものとしては、例えば、負荷トルク／定格トルク比が最小となるように各軸を動作させるロボット制御方法（特許文献１）や、ロボットの形態の急激な変化を抑えるように動作させる７軸多関節ロボットの制御方法（特許文献２）が知られている。

特開２０１３−１９３１３１号公報 特開２０１１−１３１３６２号公報

しかしながら、上述の従来例では、教示後に冗長自由度が用いられるため、教示時の冗長ロボットの動作と、再生時の冗長ロボットの動作とが異なったものとなる可能性がある。そのような場合には、教示時には発生しなかった治具等との想定外の干渉が、再生時に発生する可能性がある。

本発明は、上記問題点を解決するためになされたものであり、教示時にも拘束条件を考慮した操作が可能となる、冗長軸を有するマニピュレータを制御するロボット制御装置を提供することを目的とする。

上記目的を達成するため、本発明によるロボット制御装置は、冗長軸を含む複数の関節を有するマニピュレータを制御するロボット制御装置であって、マニピュレータの操作を受け付ける操作受付部と、操作に応じた各軸位置が算出される際に用いられる拘束条件を受け付ける拘束条件受付部と、操作受付部が受け付けた操作と、拘束条件受付部が受け付けた拘束条件とに応じて、マニピュレータの各軸位置を算出する各軸位置算出部と、教示指示を受け付ける教示指示受付部と、教示指示受付部による教示指示の受け付けに応じて、マニピュレータの教示データを作成する教示データ作成部と、を備えたものである。
このような構成により、教示時に拘束条件を考慮したマニピュレータの操作が可能となるため、再生時に同じ拘束条件を用いて動作させることによって、教示時と再生時のロボットの動作が一致するようにできる。したがって、再生時にマニピュレータが想定外の動きをすることを回避できる。例えば、干渉が発生しないように教示することによって、再生時にも干渉が起こらないようにすることができる。

また、本発明によるロボット制御装置では、教示データ作成部は、教示指示受付部が教示指示を受け付けた時点の拘束条件を識別する拘束条件識別情報を含む教示データを作成してもよい。
このような構成により、教示時に用いた拘束条件を示す情報を教示データに含めることができる。その結果、再生時には、教示データに含まれる拘束条件識別情報を用いることによって、教示時と同じ拘束条件を用いた動作が可能となる。

また、本発明によるロボット制御装置では、各軸位置算出部は、
ΔΘ＝Ａ_１ ^＋ΔＸ＋（Ｉ−Ａ_１ ^＋Ａ_１）ｋ
（ただし、Ａ_１は時刻ｔ_１におけるマニピュレータのヤコビアンであり、ΔＸはマニピュレータの位置姿勢の変位を示すベクトルであり、Ｉは単位行列であり、Ａ_１ ^＋はＡ_１の擬似逆行列であり、ｋは任意のベクトルであり、ΔΘはマニピュレータの各軸の変位を示すベクトルである）において、拘束条件を満たすようにベクトルｋを決定し、決定したベクトルｋと上式とを用いて、操作受付部が受け付けた操作に応じた各軸位置を算出してもよい。
このような構成により、拘束条件を満たすようにベクトルｋを決定することによって、拘束条件を満たす各軸位置を算出することができるようになる。

また、本発明によるロボット制御装置では、各軸位置算出部は、拘束条件を満たすベクトルｋを算出してもよい。
また、本発明によるロボット制御装置では、各軸位置算出部は、複数のベクトルｋに応じたΔΘを算出し、拘束条件を最もよく満たすベクトルｋを選択してもよい。

本発明によるロボット制御装置によれば、教示時にも拘束条件を考慮した操作が可能となる。その結果、教示時のロボットの動作と再生時のロボットの動作とを一致させることができる。

本発明の実施の形態１によるロボットシステムの構成を示すブロック図 同実施の形態によるロボット制御装置の動作を示すフローチャート 同実施の形態における操作装置の表示の一例を示す図 同実施の形態における教示データの一例を示す図

以下、本発明によるロボット制御装置について、実施の形態を用いて説明する。なお、以下の実施の形態において、同じ符号を付した構成要素及びステップは同一または相当するものであり、再度の説明を省略することがある。本実施の形態によるロボットシステムは、ティーチング時に、拘束条件に応じた操作を行うことができるものである。

図１は、本実施の形態によるロボットシステムの構成を示すブロック図である。本実施の形態によるロボットシステムは、ロボット制御装置１と、サーボコントローラ２と、マニピュレータ３と、操作装置４とを備える。ロボット制御装置１は、モータにより駆動される関節によって連結された複数のアームを有するマニピュレータ３を、サーボコントローラ２を介して制御する。そのマニピュレータ３において、各モータとアームとは減速機を介して接続されている。また、直列に接続されたアームの先端に、手先効果器（エンドエフェクタ）が設けられていてもよい。マニピュレータ３は、例えば、垂直多関節ロボットのマニピュレータであってもよく、水平多関節ロボットのマニピュレータであってもよい。また、そのマニピュレータ３の軸数は問わないが、本実施の形態では、マニピュレータ３が冗長軸を含む複数の関節を有しているものとする。したがって、ロボット制御装置１は、冗長マニピュレータを制御するものである。垂直多関節ロボットが冗長軸を有する場合には、通常、７軸以上となる。本実施の形態では、マニピュレータ３が７軸である場合について主に説明する。その７軸のマニピュレータ３は、特に限定されないが、例えば、上記特許文献１，２に記載されている７軸のマニピュレータであってもよく、その他の７軸のマニピュレータであってもよい。また、ロボット制御装置１は、ティーチングプレイバック方式によりマニピュレータ３を制御するものである。本実施の形態によるロボットは、例えば、搬送ロボットであってもよく、溶接ロボットであってもよく、組立ロボットであってもよく、塗装ロボットであってもよく、または、その他の用途のロボットであってもよい。図１では、説明の便宜上、１個のサーボコントローラ２のみを示しているが、通常、ロボットシステムは、マニピュレータ３の軸数と同数のサーボコントローラを備えている。したがって、サーボコントローラ２は、各軸に対応するサーボコントローラの集合であると考えてもよい。

図１で示されるように、ロボット制御装置１は、操作受付部１１と、拘束条件記憶部１２と、拘束条件受付部１３と、各軸位置算出部１４と、教示指示受付部１５と、教示データ作成部１６と、教示データ記憶部１７とを備える。

操作受付部１１は、可搬式の操作装置４から、マニピュレータ３の操作を受け付ける。その操作装置４は、例えば、ティーチングペンダントであってもよい。操作受付部１１が受け付ける操作は、例えば、マニピュレータ３の手先の位置及び姿勢の操作であってもよい。その操作は、例えば、マニピュレータ３の手先の位置の変位や、その手先の姿勢の変位であってもよく、その他の操作であってもよい。

なお、本実施の形態では、操作受付部１１が操作装置４から操作を受け付ける場合について説明するが、そうでなくてもよい。操作受付部１１は、例えば、有線または無線の通信回線を介して送信された操作を受信してもよい。なお、操作受付部１１は、受け付けを行うためのデバイス（例えば、モデムやネットワークカードなど）を含んでもよく、または含まなくてもよい。また、操作受付部１１は、ハードウェアによって実現されてもよく、または所定のデバイスを駆動するドライバ等のソフトウェアによって実現されてもよい。

拘束条件記憶部１２では、拘束条件が記憶される。その記憶されている拘束条件に対応付けて、その拘束条件を識別する拘束条件識別情報が拘束条件記憶部１２で記憶されていてもよい。拘束条件は、例えば、次のようなものであってもよい。なお、拘束条件の詳細については後述する。
拘束条件Ｒ１：指定軸の変位を最小化する
拘束条件Ｒ２：指定軸の変位を他の軸よりも大きくする
拘束条件Ｒ３：全軸の運動量のノルムの和を最小にする
拘束条件Ｒ４：減速機の負担を軽減する
拘束条件Ｒ５：指定軸の位置を指定位置に近づける
なお、拘束条件Ｒ４は、例えば、管理されている各軸の減速機の動作量を用いて、その動作量が最小である軸の変位を他の軸よりも大きくする拘束条件であってもよい。

拘束条件記憶部１２に拘束条件が記憶される過程は問わない。例えば、記録媒体を介して拘束条件が拘束条件記憶部１２で記憶されるようになってもよく、通信回線等を介して送信された拘束条件が拘束条件記憶部１２で記憶されるようになってもよく、または、入力デバイスを介して入力された拘束条件が拘束条件記憶部１２で記憶されるようになってもよい。拘束条件記憶部１２での記憶は、ＲＡＭ等における一時的な記憶でもよく、または、長期的な記憶でもよい。拘束条件記憶部１２は、所定の記録媒体（例えば、半導体メモリや磁気ディスクなど）によって実現されうる。

拘束条件受付部１３は、可搬式の操作装置４から、操作に応じた各軸位置が算出される際に用いられる拘束条件を受け付ける。この拘束条件の受け付けは、結果として操作者の指定した拘束条件がわかるようになるのであれば、どのような受け付けであってもよい。すなわち、拘束条件の受け付けは、拘束条件の選択の受け付けであってもよい。本実施の形態では、拘束条件受付部１３による拘束条件の受け付けが、拘束条件記憶部１２で記憶されている拘束条件の選択の受け付けである場合について説明する。具体的には、拘束条件の受け付けは、その拘束条件を識別する拘束条件識別情報の受け付けであってもよい。その場合には、拘束条件受付部１３は、拘束条件識別情報と共に、その拘束条件識別情報で識別される拘束条件において用いられる情報をも受け付けてもよい。その情報は、例えば、上記拘束条件Ｒ１，Ｒ２，Ｒ５の指定軸を示す情報であってもよく、拘束条件Ｒ５の指定位置であってもよい。また、拘束条件受付部１３は、通常、教示のための操作が行われている際に、拘束条件を受け付けることになる。そのようにして、操作者は、教示の際に拘束条件を切り替えることができ、マニピュレータ３の状況に応じた拘束条件を用いた操作が可能となる。また、教示時において、拘束条件を簡単に切り替えられることが好適である。したがって、その拘束条件の受け付けは、例えば、操作装置４のタッチパネルやボタンを用いて簡単に行えるようにしてもよく、または、対話的なインターフェースを用いて行えるようにしてもよい。なお、拘束条件受付部１３は、拘束条件そのものを受け付けてもよい。その場合には、受け付けられた拘束条件は、拘束条件記憶部１２に追記されてもよく、または、そうでなくてもよい。

なお、本実施の形態では、拘束条件受付部１３が操作装置４から拘束条件を受け付ける場合について説明するが、そうでなくてもよい。拘束条件受付部１３は、例えば、有線または無線の通信回線を介して送信された拘束条件を受信してもよい。なお、拘束条件受付部１３は、受け付けを行うためのデバイス（例えば、モデムやネットワークカードなど）を含んでもよく、または含まなくてもよい。また、拘束条件受付部１３は、ハードウェアによって実現されてもよく、または所定のデバイスを駆動するドライバ等のソフトウェアによって実現されてもよい。

各軸位置算出部１４は、操作受付部１１が受け付けた操作と、拘束条件受付部１３が受け付けた拘束条件とに応じて、マニピュレータ３の各軸位置を算出する。すなわち、各軸位置算出部１４は、マニピュレータ３の逆運動学の計算を行うものである。なお、その各軸位置の算出で用いられた拘束条件は、後述するように、教示データの再生時に各軸位置を算出する際にも用いられることになる。その結果、教示時のマニピュレータ３の動きと、再生時のマニピュレータ３の動きとが一致することになる。その各軸位置の算出処理の詳細については後述する。算出された各軸位置は、サーボコントローラ２に出力され、それに応じて、マニピュレータ３の各軸の位置が変化することになる。そのような操作に応じた各軸位置の算出は、通常、教示時に行われる。一方、各軸位置算出部１４は、再生時にも各軸位置を算出してもよい。具体的には、各軸位置算出部１４は、教示データ記憶部１７で記憶されている教示データを読み出し、その教示データに応じた各軸位置を算出してサーボコントローラ２に出力してもよい。また、その教示データに拘束条件識別情報が含まれている場合には、各軸位置算出部１４は、その拘束条件識別情報で識別される拘束条件を用いて各軸位置を算出してもよい。

教示指示受付部１５は、可搬式の操作装置４から、教示指示を受け付ける。その教示指示は、教示データを作成する旨の指示である。その教示指示は、教示データを蓄積する旨の指示であると考えてもよい。なお、本実施の形態では、教示指示受付部１５が操作装置４から教示指示を受け付ける場合について説明するが、そうでなくてもよい。教示指示受付部１５は、例えば、有線または無線の通信回線を介して送信された教示指示を受信してもよい。なお、教示指示受付部１５は、受け付けを行うためのデバイス（例えば、モデムやネットワークカードなど）を含んでもよく、または含まなくてもよい。また、教示指示受付部１５は、ハードウェアによって実現されてもよく、または所定のデバイスを駆動するドライバ等のソフトウェアによって実現されてもよい。

教示データ作成部１６は、教示指示受付部１５による教示指示の受け付けに応じて、マニピュレータ３の教示データを作成する。その教示データは、例えば、教示された手先の位置や、教示された手先の姿勢を示すデータであってもよい。すなわち、その教示データは、教示点等を含む情報であってもよい。また、教示データ作成部１６は、教示指示受付部１５が教示指示を受け付けた時点の拘束条件を識別する拘束条件識別情報を含む教示データを作成してもよい。教示指示が受け付けられた時点の拘束条件とは、その時点の各軸位置の算出で用いられた拘束条件のことである。なお、各軸位置算出部１４等では、その時点の各軸位置までの移動時の算出に用いられた拘束条件を識別する拘束条件識別情報を保持していることが好適である。また、教示データは、それら以外の情報を含んでいてもよい。例えば、教示データは、教示点間の補間方法（例えば、直線補間や曲線補間等）を示す情報を含んでいてもよい。その補間方法は、操作者によって指示されたものであってもよい。教示データ作成部１６は、作成した教示データを教示データ記憶部１７に蓄積する。そのようにして教示データが蓄積されることにより、ロボットに各種の作業を行わせるための作業プログラムが作成されることになる。

教示データ記憶部１７では、教示データ作成部１６によって作成された教示データが記憶される。教示データ記憶部１７での記憶は、ＲＡＭ等における一時的な記憶でもよく、または、長期的な記憶でもよい。また、教示データ記憶部１７は、所定の記録媒体（例えば、半導体メモリや磁気ディスクなど）によって実現されうる。

ここで、各軸位置算出部１４による各軸位置の算出について説明する。ある時刻ｔ_１におけるマニピュレータ３のヤコビアン（ヤコビ行列）をＡ_１とする。各軸位置算出部１４は、その時刻ｔ_１におけるマニピュレータ３の位置姿勢に応じたヤコビアンＡ_１を取得することができる。また、マニピュレータ３の手先の位置姿勢の変位を示すベクトルを、
ΔＸ＝（Δｘ，Δｙ，Δｚ，Δα，Δβ，Δγ）^Ｔ
とし、そのときの各軸変位を示すベクトルを、
ΔΘ＝（Δθ_１，Δθ_２，...，Δθ_ｎ−１，Δθ_ｎ）^Ｔ
とすると、次式が成り立つ。なお、Δｘ，Δｙ，Δｚは、マニピュレータ３の手先の位置の変化であり、Δα，Δβ，Δγは、マニピュレータ３の手先の姿勢の変化である。また、Δθ_ｉは、ｉ番目の軸における位置（角度）の変化である。また、マニピュレータ３は冗長軸を有するため、ｎは７以上の整数である。
ΔＸ＝Ａ_１ΔΘ

この場合には、Ａ_１は正則ではないため逆行列を持たないが、擬似逆行列を用いると、ΔΘは、次式のようになる。
ΔΘ＝Ａ_１ ^＋ΔＸ＋（Ｉ−Ａ_１ ^＋Ａ_１）ｋ ・・・（１）
ただし、Ａ_１ ^＋はＡ_１の擬似逆行列（ムーア・ペンローズの擬似逆行列）であり、Ｉは単位行列であり、ｋは任意のベクトルである。Ａ_１からＡ_１ ^＋を算出する方法はすでに公知であり、その説明を省略する。なお、マニピュレータ３が７軸である場合、すなわち、冗長自由度が１である場合には、ΔΘ、ｋは７次元のベクトルであり、Ｉは７×７の単位行列であり、Ａ_１は６行７列の行列であり、Ａ_１ ^＋は７行６列の行列である。

各軸位置算出部１４は、拘束条件受付部１３で受け付けられた拘束条件を満たすようにベクトルｋを決定する。そして、各軸位置算出部１４は、その決定したベクトルｋと、式１とを用いて、操作受付部１１が受け付けた操作に応じた各軸位置を算出する。具体的には、操作受付部１１が受け付けた操作が、通常、ΔＸとなる。また、その操作後の各軸位置は、操作前の各軸位置に、上記式１で算出したΔΘを加算したものとなる。したがって、各軸位置算出部１４は、各時点での各軸位置を一時的に図示しない記録媒体で記憶していてもよい。このようにして、マニピュレータ３が１つ以上の冗長自由度を有する場合にも、拘束条件を考慮したマニピュレータ３の手動動作が可能となる。

次に、拘束条件について簡単に説明する。拘束条件は特に限定されるものではないが、例えば、次のようなものを挙げることができる。なお、拘束条件が、以下のもの以外であってもよいことは言うまでもない。

拘束条件Ｒ１：指定軸の変位の最小化
この場合には、Δθ_ｉ＝０となるように、または、Δθ_ｉが他軸の変位よりも小さくなる（すなわち、Δθ_ｉ＜Δθ_ｊ≠ｉとなる）ようにベクトルｋが決定される。ただし、ｉ番目の軸が指定軸である。

拘束条件Ｒ２：指定軸の変位の最大化
この場合には、Δθ_ｉが他軸の変位よりも大きくなるようベクトルｋが決定される。すなわち、Δθ_ｉ＝Δθ_ｍａｘ、または、Δθ_ｉ＞Δθ_ｊ≠ｉとなる。ただし、ｉ番目の軸が指定軸であり、Δθ_ｍａｘ＝ｍａｘ｛Δθ_１，Δθ_２，...，Δθ_ｎ｝である。

拘束条件Ｒ３：全軸の動作量のノルムの和の最小化
この場合には、Σ｜Δθ_ｉ｜が最小になるようにベクトルｋが決定される。ただし、総和Σは、マニピュレータ３のすべての軸についての和である。

拘束条件Ｒ４：減速機の負担減
この場合には、動作量（使用量）の最も少ない減速機の軸の変位が最大となるようにしてもよい。すなわち、この拘束条件Ｒ４は、動作量が最小の減速機の軸を指定軸とした拘束条件Ｒ２であると考えることができる。なお、この拘束条件Ｒ４を用いる場合には、あらかじめ各軸の減速機ごとに動作量を管理していてもよい。その動作量は、減速機の動作に応じて更新されるものとする。そして、この拘束条件Ｒ４に応じたベクトルｋを決定する場合に、各軸位置算出部１４は、管理されている動作量を用いて、最も動作量の少ない減速機の軸を特定してもよい。その動作量の管理は、寿命の管理であってもよい。

拘束条件Ｒ５：指定軸の位置の指定位置への設定
この場合には、θ_ｉ−θ_ｆｉｘ＝０となるように、または、θ_ｉ−θ_ｆｉｘが最小になるようにベクトルｋが決定される。ただし、ｉ番目の軸が指定軸であり、θ_ｆｉｘが指定位置である。

また、拘束条件に応じたベクトルｋを決定する場合に、各軸位置算出部１４は、拘束条件を満たすベクトルｋを算出してもよく、または、複数のベクトルｋに応じたΔΘを算出し、その複数のベクトルｋから、拘束条件を最もよく満たすベクトルｋを選択してもよい。ここで、ベクトルｋについて少し説明する。ベクトルｋは、前述のように、マニピュレータ３の軸数の次元を有する任意のベクトルである。したがって、通常は、ｋ＝（ｋ_１，ｋ_２，...，ｋ_ｎ）^Ｔとなる。ただ、冗長自由度がｍである場合には、ベクトルｋも、その冗長自由度ｍに応じた次元の自由度があれば十分であるため、ｋ＝（ｋ_１，ｋ_２，...，ｋ_ｍ，０，...，０）^Ｔとすることができる。すなわち、ベクトルｋの１番目からｍ番目までの要素を任意の実数（ｋ_１，ｋ_２，...，ｋ_ｍ）とし、それ以外を０とすることができる。例えば、マニピュレータ３が７軸である場合には、ｋ＝（ｋ_１，０，...，０）^Ｔとなる。そして、各軸位置算出部１４は、ｋ_１，ｋ_２，...，ｋ_ｍを決定することによってベクトルｋを決定してもよい。

次に、マニピュレータ３が７軸である場合、すなわち、冗長軸が１軸である場合に、拘束条件Ｒ３を満たすベクトルｋを算出する方法について具体的に説明する。まず、ベクトルｋ＝（ｋ_１，０，...，０）^Ｔを算出する方法について説明する。ここでは、その算出に最急降下法を用いるが、他のアルゴリズムを用いてもよいことは言うまでもない。

まず、全軸の動作量のノルムの自乗の総和を、ｋの式として定義する。すなわち、関数ｆを次式のように定義する。この関数ｆが０または小さい正の値となるときに、全軸の動作量のノルムの総和も０または小さい正の値になるからである。
ｆ（ｋ）＝（Δθ_１（ｋ））^２＋（Δθ_２（ｋ））^２＋…＋（Δθ_７（ｋ））^２
なお、マニピュレータ３が７軸である場合には、ベクトルｋのゼロでない要素はｋ_１のみであるため、実質的にｆ（ｋ）＝ｆ（ｋ_１）となる。

次に、ベクトルｋの初期値、すなわちｋ_１の初期値ｋ_１ ^（０）を、例えば、ｋ_１ ^（０）＝１と設定する。
その後、各軸位置算出部１４は、次式のようにｋ_１ ^（ｊ）を更新する。なお、ｐは、１回の更新での変化の程度を決定するパラメータであり、通常は小さい正の値に設定される。ただし、ｊは０以上の整数である。
ｋ_１ ^{（ｊ＋１）}＝ｋ_１ ^（ｊ）−ｐ×∂ｆ（ｋ_１ ^（ｊ））／∂ｋ_１

各軸位置算出部１４は、その更新を、
｜ｋ_１ ^{（ｊ＋１）}−ｋ_１ ^（ｊ）｜＜ε
となるまで繰り返す。なお、εは、終了条件を決定する小さい正の値である。このようにすることによって、各軸位置算出部１４は、ベクトルｋのゼロでない要素ｋ_１を算出することができ、ある時刻ｔ_１におけるΔΘが算出されることになる。そして、各軸位置算出部１４は、時刻ｔ_１におけるΘに、算出されたΔΘを加算することによって、受け付けられた操作であるΔＸに応じた各軸位置を算出できる。

次に、マニピュレータ３が７軸である場合に、拘束条件Ｒ３を満たすベクトルｋを選択する場合について具体的に説明する。まず、複数のベクトルｋを設定する。例えば、ｋ_（１），ｋ_（２），…，ｋ_（ｑ）を設定する。なお、ｑは、設定されるベクトルｋの個数である。また、そのｋ_（１）等は、あらかじめ決められている値を読み出すことによって設定されてもよく、あらかじめ決められているルールに応じて、またはランダムに生成された値であってもよい。

その後、各軸位置算出部１４は、設定されたベクトルｋ_（１）等に応じたｆ（ｋ）をそれぞれ算出する。そして、各軸位置算出部１４は、算出したｆ（ｋ）のうち、最も小さい値の算出で用いられたベクトルｋを、各軸位置の算出で用いるベクトルｋとして選択する。その選択されたベクトルｋを用いて各軸位置が算出される方法は、上述の通りであり、その説明を省略する。なお、ベクトルｋ_（１）等に応じたｆ（ｋ）を算出する際には、ベクトルｋ_（１）等に応じたΔΘがそれぞれ算出されていることになる。したがって、ベクトルｋが選択された後に再度、ΔΘを算出するのではなく、その選択されたベクトルｋに応じたΔΘを用いて各軸位置を算出してもよい。

ここでは、拘束条件Ｒ３に応じてベクトルｋを決定する場合について説明したが、他の拘束条件の場合でも同様にしてベクトルｋを決定することができる。例えば、拘束条件Ｒ１の場合には、ｆ（ｋ）＝（Δθ_ｉ）^２を用いてベクトルｋを算出してもよく、Δθ_ｉ＜Δθ_ｊ≠ｉとなるベクトルｋを選択してもよい。また、拘束条件Ｒ２の場合には、ｆ（ｋ）＝（Δθ_ｉ−Δθ_ｍａｘ）^２を用いてベクトルｋを算出してもよく、Δθ_ｉ＞Δθ_ｊ≠ｉとなるベクトルｋを選択してもよい。また、拘束条件Ｒ５の場合には、ｆ（ｋ）＝（θ_ｉ−θ_ｆｉｘ）^２を用いてベクトルｋを算出してもよく、θ_ｉがθ_ｆｉｘに最も近いベクトルｋを選択してもよい。

また、再生時には、各軸位置算出部１４は、教示データに含まれる位置と姿勢、拘束条件識別情報を読み出し、その位置と姿勢まで、拘束条件識別情報で識別される拘束条件で移動するように各軸位置を算出してもよい。なお、教示データに補間方法（例えば、直線補間や曲線補間等）が含まれる場合には、その補間方法をも用いて各タイミングにおける各軸位置を算出してもよい。そのような各軸位置の算出は、教示データに含まれる拘束条件識別情報で識別される拘束条件を用いる以外、すでに公知であり、その説明を省略する。

なお、拘束条件記憶部１２と、教示データ記憶部１７とは、同一の記録媒体によって実現されてもよく、または、別々の記録媒体によって実現されてもよい。前者の場合には、拘束条件を記憶している領域が拘束条件記憶部１２となり、教示データを記憶している領域が教示データ記憶部１７となる。

サーボコントローラ２は、ロボット制御装置１から各軸位置（位置指令値）を受け取り、マニピュレータ３の各軸が、その各軸位置となるようにコントロールする。サーボコントローラ２は、例えば、マニピュレータ３から各軸の現在位置である位置フィードバックと、各軸の現在速度である速度フィードバックと、各軸の現在電流である電流フィードバックとを受け取り、それらのフィードバックを用い、位置ループ、速度ループ、電流ループによるフィードバック制御を行ってもよい。その現在位置等は、各軸に設けられているエンコーダによって測定されてもよい。なお、サーボコントローラ２の処理についてはすでに公知であり、その説明を省略する。

なお、本実施の形態におけるマニピュレータ３は、物理的な現実のマニピュレータであってもよく、または、仮想的なシミュレーション対象としてのマニピュレータであってもよい。前者の場合におけるティーチングは、オンラインティーチングとなり、後者の場合におけるティーチングは、オフラインティーチングとなる。

また、操作装置４は、例えば、ティーチング等で用いるティーチングペンダントであり、ロボット制御装置１に、マニピュレータ３の操作や、拘束条件、教示指示を入力するために用いられる。

次に、ロボットシステムの動作について図２のフローチャートを用いて説明する。
（ステップＳ１０１）拘束条件受付部１３は、拘束条件を受け付けたかどうか判断する。そして、拘束条件を受け付けた場合には、ステップＳ１０２に進み、そうでない場合には、ステップＳ１０３に進む。なお、この拘束条件の受け付けが、拘束条件の選択であってもよいことは上述の通りである。

（ステップＳ１０２）各軸位置算出部１４は、拘束条件受付部１３が受け付けた拘束条件を、各軸位置の算出で用いる拘束条件に設定する。例えば、拘束条件受付部１３が拘束条件識別情報を受け付けた場合には、各軸位置算出部１４は、その拘束条件識別情報を、現在の拘束条件を識別する拘束条件識別情報の格納領域に蓄積してもよい。そして、ステップＳ１０１に戻る。

（ステップＳ１０３）操作受付部１１は、マニピュレータ３の操作を受け付けたかどうか判断する。そして、操作を受け付けた場合には、ステップＳ１０４に進み、そうでない場合には、ステップＳ１０６に進む。

（ステップＳ１０４）各軸位置算出部１４は、ステップＳ１０２で設定された最新の拘束条件識別情報で識別される拘束条件を拘束条件記憶部１２から読み出す。そして、各軸位置算出部１４は、その読み出した拘束条件と、その時点の各軸位置と、その時点のマニピュレータ３の位置姿勢に応じたヤコビアンと、ステップＳ１０３で受け付けられた操作とを用いて、前述のようにして各軸位置を算出する。

（ステップＳ１０５）各軸位置算出部１４は、その算出した各軸位置をサーボコントローラ２に出力する。そして、ステップＳ１０１に戻る。その各軸位置の出力の結果、マニピュレータ３が、出力された各軸位置となるように動作することになる。

（ステップＳ１０６）教示指示受付部１５は、教示指示を受け付けたかどうか判断する。そして、教示指示を受け付けた場合には、ステップＳ１０７に進み、そうでない場合には、ステップＳ１０１に戻る。

（ステップＳ１０７）教示データ作成部１６は、その時点のマニピュレータ３の位置姿勢を示す教示点等と、現在の各軸位置への移動時に用いられた拘束条件を識別する拘束条件識別情報とを各軸位置算出部１４から取得する。

（ステップＳ１０８）教示データ作成部１６は、取得した教示点等と、拘束条件識別情報とを含む教示データを、教示データ記憶部１７に蓄積する。そして、ステップＳ１０１に戻る。
なお、図２のフローチャートにおける処理の順序は一例であり、同様の結果を得られるのであれば、各ステップの順序を変更してもよい。また、図２のフローチャートにおいて、電源オフや処理終了の割り込みにより処理は終了する。

次に、本実施の形態によるロボットシステムの動作について、具体例を用いて説明する。この具体例では、マニピュレータ３が７軸であり、冗長自由度が１である場合について説明する。
操作者が、可搬式の操作装置４を用いてマニピュレータ３の位置姿勢を操作してティーチングを行っているとする。そして、図３で示されるように、回転式セレクタ２１を矢印方向に回転させることによって、拘束条件識別情報「Ｒ００２」で識別される拘束条件を選択したとする。すると、その拘束条件識別情報が拘束条件受付部１３で受け付けられ、各軸位置算出部１４に渡される（ステップＳ１０１）。各軸位置算出部１４は、拘束条件受付部１３から受け取った拘束条件識別情報「Ｒ００２」を、現在の拘束条件識別情報を格納する領域に蓄積する（ステップＳ１０２）。

その後、操作者が操作装置４を用いて、マニピュレータ３の手先の位置姿勢を変更したとする。すると、その操作が操作受付部１１で受け付けられ、各軸位置算出部１４に渡される（ステップＳ１０３）。各軸位置算出部１４は、操作受付部１１からマニピュレータ３の手先の位置姿勢の変位ΔＸを受け取ると、現在の拘束条件識別情報「Ｒ００２」を読み出し、それに応じた拘束条件を、拘束条件記憶部１２から読み出す。また、各軸位置算出部１４は、現在のマニピュレータ３の位置姿勢に応じたヤコビアンＡ_１を取得し、そのヤコビアンＡ_１の擬似逆行列Ａ_１ ^＋を算出する。そして、各軸位置算出部１４は、上述のようにして、読み出した拘束条件に応じたベクトルｋを決定し、その決定したベクトルｋを用いて、操作ΔＸに応じた各軸の変位ΔΘを算出する。その後、各軸位置算出部１４は、その時点のマニピュレータ３の各軸の位置Θに、その算出した変位ΔΘを加算することによって、操作に応じた各軸位置を算出し（ステップＳ１０４）、サーボコントローラ２に出力する（ステップＳ１０５）。その結果、サーボコントローラ２は、マニピュレータ３の位置姿勢が操作後のものとなるように、各軸の位置を制御する。

そのような操作が繰り返された後に、操作者が操作装置４の教示ボタンを押下したとする。すると、その教示ボタンの押下に応じて、教示指示がロボット制御装置１に入力される。その教示指示は、教示指示受付部１５で受け付けられ、教示データ作成部１６に渡される（ステップＳ１０６）。教示データ作成部１６は、教示指示受付部１５から教示指示を受け取ると、その時点のマニピュレータ３の手先の位置を示す座標値（ｘ００３，ｙ００３，ｚ００３）と、手先の姿勢を示す角度（α００３，β００３，γ００３）とを取得し、その座標値まで移動した際に用いられた拘束条件を識別する拘束条件識別情報「Ｒ００２」を取得する（ステップＳ１０７）。その後、教示データ作成部１６は、手先の位置、姿勢を示す情報と、拘束条件識別情報とを含む教示データを、教示データ記憶部１７に蓄積する（ステップＳ１０８）。その結果、教示データ記憶部１７で記憶されている教示データは、図４で示されるようになる。なお、図４において、ステップと、教示データとが対応付けられている。ステップは、各教示データを識別する情報であり、新たな教示データが蓄積されるごとに、１だけインクリメントされた値が設定されるものとする。このようにして、教示データの作成が順次、行われることになる。なお、教示データに含まれる拘束条件識別情報は、それを含む教示データに含まれている位置や姿勢に移動する際に用いられる拘束条件を識別するものである。また、教示データには、それに含まれている位置や姿勢まで移動する際に、直線補間を行うのか、曲線補間を行うのかなどを示す情報も含まれていてもよい。なお、教示点間において拘束条件が切り替えられた場合には、厳密には、教示時の動作と再生時の動作が一致しなくなる。したがって、教示時に、教示点以外における拘束条件の切り替えを行えないようにしてもよいが、そうでなくてもよい。

次に、教示データに応じた動作を行う際の処理について簡単に説明する。教示データに応じた動作を行う際（再生時）には、各軸位置算出部１４は、教示データをステップごとに読み出し、その教示データに含まれる位置姿勢に応じた各軸位置を算出し、サーボコントローラ２に渡す。なお、その各軸位置の算出時に、各軸位置算出部１４は、教示データに含まれる拘束条件識別情報で識別される拘束条件を用いた算出を行うものとする。そのようにして各軸位置を算出することによって、マニピュレータ３が教示時と同じ動作をすることになり、想定外の干渉等が発生することを回避することができる。なお、各軸位置算出部１４は、再生時に教示点間の補間を行ってもよい。

以上のように、本実施の形態によるロボットシステムによれば、教示時にも指定した拘束条件に応じた動作が行われるようにすることができる。すなわち、操作者は、教示時にマニピュレータ３の動作が適切なものとなる拘束条件を選択することができる。そして、再生時に、その教示時で用いられた拘束条件を用いて各軸位置を算出することによって、教示時と再生時のロボットの動作を一致させることができ、再生時に想定外の動作となることを回避することができる。そのため、例えば、再生時に治具等との想定外の干渉が発生することを回避できる。また、作成する教示データに拘束条件識別情報をも含めておくことにより、再生時にどの拘束条件を選択するのかを容易に知ることができるようになる。また、教示時に拘束条件を容易に変更できるため、操作者の想定している動作となる拘束条件を、マニピュレータ３の動作を確認しながら選択することもできるようになる。例えば、操作者の想定している手先の軌道となるように、適宜、拘束条件を変更しながら教示の操作を行うことが可能となる。

なお、本実施の形態では、３次元の手先位置と３方向の手先姿勢とを教示可能な垂直多関節型のマニピュレータ３が冗長軸を有する場合、すなわち、７軸以上である場合について説明したが、マニピュレータ３は、作業自由度に対して１つ以上の冗長自由度を有しているのであれば、それ以外のものであってもよい。したがって、冗長軸を有するマニピュレータ３の軸数は問わない。冗長軸を有するマニピュレータ３の軸数は、例えば、６軸以下であってもよく、または、８軸以上であってもよい。また、その冗長自由度が１である場合について主に説明したが、冗長自由度は２以上であってもよい。

また、本実施の形態では、教示データに拘束条件識別情報も含まれる場合について説明したが、そうでなくてもよい。教示時の各操作で用いられた拘束条件を識別する拘束条件識別情報は、教示データと別途、管理されてもよい。なお、再生時には、教示データと、その教示データに対応する拘束条件識別情報とが一緒に用いられることが好適である。

また、上記実施の形態において、各処理または各機能は、単一の装置または単一のシステムによって集中処理されることによって実現されてもよく、または、複数の装置または複数のシステムによって分散処理されることによって実現されてもよい。

また、上記実施の形態において、各構成要素間で行われる情報の受け渡しは、例えば、その情報の受け渡しを行う２個の構成要素が物理的に異なるものである場合には、一方の構成要素による情報の出力と、他方の構成要素による情報の受け付けとによって行われてもよく、または、その情報の受け渡しを行う２個の構成要素が物理的に同じものである場合には、一方の構成要素に対応する処理のフェーズから、他方の構成要素に対応する処理のフェーズに移ることによって行われてもよい。

また、上記実施の形態において、各構成要素が実行する処理に関係する情報、例えば、各構成要素が受け付けたり、取得したり、選択したり、生成したり、送信したり、受信したりした情報や、各構成要素が処理で用いる閾値や数式、設定値等の情報等は、上記説明で明記していなくても、図示しない記録媒体において、一時的に、または長期にわたって保持されていてもよい。また、その図示しない記録媒体への情報の蓄積を、各構成要素、または、図示しない蓄積部が行ってもよい。また、その図示しない記録媒体からの情報の読み出しを、各構成要素、または、図示しない読み出し部が行ってもよい。

また、上記実施の形態において、各構成要素等で用いられる情報、例えば、各構成要素が処理で用いる閾値や各種の設定値等の情報がユーザによって変更されてもよい場合には、上記説明で明記していなくても、ユーザが適宜、それらの情報を変更できるようにしてもよく、または、そうでなくてもよい。それらの情報をユーザが変更可能な場合には、その変更は、例えば、ユーザからの変更指示を受け付ける図示しない受付部と、その変更指示に応じて情報を変更する図示しない変更部とによって実現されてもよい。その図示しない受付部による変更指示の受け付けは、例えば、入力デバイスからの受け付けでもよく、通信回線を介して送信された情報の受信でもよく、所定の記録媒体から読み出された情報の受け付けでもよい。

また、上記実施の形態において、各構成要素は専用のハードウェアにより構成されてもよく、または、ソフトウェアにより実現可能な構成要素については、プログラムを実行することによって実現されてもよい。例えば、ハードディスクや半導体メモリ等の記録媒体に記録されたソフトウェア・プログラムをＣＰＵ等のプログラム実行部が読み出して実行することによって、各構成要素が実現され得る。その実行時に、プログラム実行部は、記憶部や記録媒体にアクセスしながらプログラムを実行してもよい。また、そのプログラムは、サーバなどからダウンロードされることによって実行されてもよく、所定の記録媒体（例えば、光ディスクや磁気ディスク、半導体メモリなど）に記録されたプログラムが読み出されることによって実行されてもよい。また、このプログラムは、プログラムプロダクトを構成するプログラムとして用いられてもよい。また、そのプログラムを実行するコンピュータは、単数であってもよく、複数であってもよい。すなわち、集中処理を行ってもよく、または分散処理を行ってもよい。

また、本発明は、以上の実施の形態に限定されることなく、種々の変更が可能であり、それらも本発明の範囲内に包含されるものであることは言うまでもない。

以上より、本発明によるロボット制御装置によれば、教示時にも拘束条件を考慮した動作をロボットに実行させられるという効果が得られ、ティーチングプレイバック方式のロボットを制御するロボット制御装置として有用である。

１ ロボット制御装置
２ サーボコントローラ
３ マニピュレータ
４ 操作装置
１１ 操作受付部
１２ 拘束条件記憶部
１３ 拘束条件受付部
１４ 各軸位置算出部
１５ 教示指示受付部
１６ 教示データ作成部
１７ 教示データ記憶部

Claims (5)

1. 冗長軸を含む複数の関節を有するマニピュレータを制御するロボット制御装置であって、
   前記マニピュレータの操作を受け付ける操作受付部と、
   前記操作に応じた各軸位置が算出される際に用いられる拘束条件を受け付ける拘束条件受付部と、
   前記操作受付部が受け付けた操作と、前記拘束条件受付部が受け付けた拘束条件とに応じて、前記マニピュレータの各軸位置を算出する各軸位置算出部と、
   教示指示を受け付ける教示指示受付部と、
   前記教示指示受付部による教示指示の受け付けに応じて、前記マニピュレータの教示データを作成する教示データ作成部と、を備えたロボット制御装置。
2. 前記教示データ作成部は、前記教示指示受付部が教示指示を受け付けた時点の拘束条件を識別する拘束条件識別情報を含む教示データを作成する、請求項１記載のロボット制御装置。
3. 前記各軸位置算出部は、
   ΔΘ＝Ａ_１ ^＋ΔＸ＋（Ｉ−Ａ_１ ^＋Ａ_１）ｋ
   （ただし、Ａ_１は時刻ｔ_１における前記マニピュレータのヤコビアンであり、ΔＸは前記マニピュレータの位置姿勢の変位を示すベクトルであり、Ｉは単位行列であり、Ａ_１ ^＋はＡ_１の擬似逆行列であり、ｋは任意のベクトルであり、ΔΘは前記マニピュレータの各軸の変位を示すベクトルである）
   において、拘束条件を満たすようにベクトルｋを決定し、当該決定したベクトルｋと上式とを用いて、前記操作受付部が受け付けた操作に応じた各軸位置を算出する、請求項１または請求項２記載のロボット制御装置。
4. 前記各軸位置算出部は、拘束条件を満たすベクトルｋを算出する、請求項３記載のロボット制御装置。
5. 前記各軸位置算出部は、複数のベクトルｋに応じたΔΘを算出し、拘束条件を最もよく満たすベクトルｋを選択する、請求項３記載のロボット制御装置。

Images