【解決手段】 リンク30及び関節部Q1、Q2を有しており、駆動手段により関節部Q1、Q2を介してリンク30が駆動するアーム3を備えるロボット2を制御するためのロボット制御装置1であって、アーム3の先端側に設定される基準点x及び移動先である目標点x
を撮像する撮像手段4と、撮像手段4の撮像結果に基づいて、基準点と目標点との間の位置偏差を求める位置偏差取得手段81と、位置偏差に基づいて、駆動手段を駆動させる駆動電圧をパルス幅変調してPWM信号を生成するPWM信号生成手段82と、PWM信号を駆動手段に出力し、基準点を目標点へ向けて移動するように制御する駆動制御手段83とを備える。
【発明の概要】
【発明が解決しようとする課題】
【0006】
しかしながら、従来のPWM制御方式では、モータの回転角度とパルス幅を事前に対応付けておき、目標角度に適したパルス幅を入力することで、モータの回転角度を制御しているため、モータ軸への負荷等で物理条件が変化すると、初期状態でのパルス幅と回転角度の関係に誤差が生じる。また、このようなPWM制御方式では、リンクたわみや据え付け誤差による制御精度の低下を解決することはできない。また、非特許文献1の制御方法では、常に最小単位の角度変位を与え、且つ、自由度同士の干渉を防ぐために、1自由度毎に制御を行っているため、アームの手先位置が目標位置に収束するまでに比較的時間を要するという問題がある。
【0007】
本発明は、上記のような課題に鑑みてなされたものであって、エンコーダを必要とすることなく、アームを高精度且つ素早く目標位置へと移動させることが可能なロボット制御装置、ロボット制御方法、及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【0008】
上記目的を達成するために、本発明に係るロボット制御装置は、リンク及び関節部を有しており、駆動手段により前記関節部を介して前記リンクが駆動するアームを備えるロボットを制御するためのロボット制御装置であって、前記アームの先端側に設定される基準点及び該基準点の移動先である目標点を撮像する撮像手段と、前記撮像手段の撮像結果に基づいて、前記基準点と前記目標点との間の位置偏差を求める位置偏差取得手段と、前記位置偏差取得手段によって得られた前記位置偏差に基づいて、前記駆動手段を駆動させる駆動電圧をパルス幅変調してPWM信号を生成するPWM信号生成手段と、前記PWM信号生成手段によって生成された前記PWM信号を前記駆動手段に出力し、前記基準点を前記目標点へ向けて移動するように制御する駆動制御手段と、を備えることを特徴としている。
【0009】
また、本発明に係るロボット制御装置は、前記アームが、2以上の自由度を有すること特徴としている。
【0010】
また、本発明に係るロボット制御装置は、前記PWM信号生成手段が、下記式(1)によって前記駆動電圧のパルス幅を設定することを特徴としている。
【数1】
【0011】
また、本発明に係るロボット制御方法は、リンク及び関節部を有しており、駆動手段により前記関節部を介して前記リンクが駆動するアームを備えるロボットを制御するためのロボット制御方法であって、前記アームの先端側に設定される基準点及び該基準点の移動先である目標点を撮像する撮像ステップと、前記撮像ステップにより得られた撮像結果に基づいて、前記基準点と前記目標点との間の位置偏差を求める位置偏差取得ステップと、前記位置偏差取得ステップによって得られた前記位置偏差に基づいて、前記駆動手段を駆動させる駆動電圧をパルス幅変調してPWM信号を生成するPWM信号生成ステップと、前記PWM信号生成ステップによって生成された前記PWM信号を前記駆動手段に出力し、前記基準点を前記目標点へ向けて移動するように制御する駆動制御ステップと、を含むことを特徴としている。
【0012】
また、本発明に係るプログラムは、リンク及び関節部を有しており、駆動手段により前記関節部を介して前記リンクが駆動するアームを備えるロボットを制御するためのプログラムであって、前記アームの先端側に設定される基準点及び該基準点の移動先である目標点を撮像する撮像ステップと、前記撮像ステップにより得られた撮像結果に基づいて、前記基準点と前記目標点との間の位置偏差を求める位置偏差取得ステップと、前記位置偏差取得ステップによって得られた前記位置偏差に基づいて、前記駆動手段を駆動させる駆動電圧をパルス幅変調してPWM信号を生成するPWM信号生成ステップと、前記PWM信号生成ステップによって生成された前記PWM信号を前記駆動手段に出力し、前記基準点を前記目標点へ向けて移動するように制御する駆動制御ステップと、をコンピュータに実行させることを特徴としている。
【発明の効果】
【0013】
本発明によれば、アームの先端側に設定される基準点及び該基準点の移動先である目標点を撮像し、この撮像結果に基づいて、基準点と目標点との間の位置偏差を求める。そして、この位置偏差に基づいて、駆動手段を駆動させる駆動電圧をパルス幅変調してPWM信号を生成し、このPWM信号を駆動手段へと出力して基準点を目標点へ向けて移動するように制御するので、エンコーダレスで、リンクたわみや据え付け誤差に影響されることなく、且つ、駆動手段の物理条件が初期状態と異なるような場合でも、基準点を目標点へと高精度に移動させることができる。また、本発明によれば、基準点と目標点との間の位置偏差に基づいて、パルス幅を変調したPWM信号を駆動手段に出力するので、位置偏差に応じてリンクが駆動する大きさを適切に制御することができ、基準点を目標点へと素早く移動させることができる。
【発明を実施するための形態】
【0015】
以下、本発明の実施形態に係るロボット制御装置1について、図面を参照しつつ説明する。ロボット制御装置1は、
図1に示すように、アーム3を備えるロボット2の制御を行うためのものであって、カメラ(撮像手段)4と、カメラ4によって得られる撮像結果を用いて、ロボット2を制御するための各処理等を行うコンピュータ5とを備えている。尚、
図1では、ロボット2として、2個のリンク30と、関節部Q1、Q2を有するアーム3のみを模式的に示しており、関節部Q1、Q2をそれぞれ駆動させる各モータ(駆動手段)及び減速機構については省略して図示している。
【0016】
ロボット2は、
図1に示すように、XY水平面内で駆動する2自由度のアーム3を備えている。このアーム3は、土台側の基端から先端側へ向けて、回転自在な2個の関節部Q1、Q2が、順にリンク30を介して連結されている。関節部Q1、Q2は、モータ(不図示)によって、それぞれ1の自由度を持って回転駆動することができるものである。従って、関節部Q1、Q2の回転角度q1、q2を変化させることにより、これらの関節部Q1、Q2を介して、各リンク30が駆動し、アーム3をXY水平面内で駆動させることができる。
【0017】
ロボット制御装置1では、このように構成されたロボット2のアーム3の先端側に設定される基準点(手先位置)xを当該基準点xの移動先である目標点(目標位置)x
dへと移動させるための制御を行う。ここでは、基準点xは、先端側のリンク30の先端に位置するように設定されており、目標点x
dは、XY水平面内のアーム3が移動可能な範囲内に設定されている。尚、基準点x及び目標点x
dは、カメラ4での視認性を向上させるために色付きのマーカー等を取り付けておくようにしても良い。また、カメラ4は、基準点xと目標点x
dをそれぞれ視野に捉えられるように、XY水平面の上に配置されており、撮像面がXY水平面と平行になるように設けられている。
【0018】
以下、ロボット制御装置1を用いて、アーム3の先端側に設定された基準点xを目標点x
dへ移動させる際の処理の流れについて
図2のフローチャートを用いながら説明する。
【0019】
まず、ロボット制御装置1では、基準点xの移動先である目標点x
dを視野に捉えられた状態で基準点xをカメラ4により撮像する(S101)。そして、このようにカメラ4によって撮像された画像データ(撮像結果)は、コンピュータ5へと入力される。
【0020】
コンピュータ5は、カメラ4によって得られた撮像結果を用いて、ロボット2を制御するための各処理等を行うためのものであって、例えば、
図1に示すように、CPU(Central Proceessing Unit)6と、ハードディス7と、演算処理部8と、RAM(Random Access Memory:記憶部)9と、表示部10と、操作部11等を備えている。また、これら各部は、互いにシステムバス12に接続され、このシステムバス12を介して種々のデータ等が入出力されて、CPU6の制御の下、種々の処理が実行される。
【0021】
ハードディスク7は、カメラ4から入力される撮像結果を用いて、基準点xを目標点x
dへ移動させるための処理プログラム等を格納している。尚、本実施形態では、このような処理プログラムをハードディスク7に格納している例を示しているが、これに代えて、コンピュータ読み取り可能な記憶媒体(不図示)に格納しておき、この記録媒体から処理プログラムを読み出すように構成することも可能である。
【0022】
演算処理部8は、ハードディスク7に格納される処理プログラムに基づいて、CPU6の制御の下、基準点xを目標点x
dへ移動させるための処理等を行うものである。RAM9は、ハードディスク7から読み出された処理プログラムを一時的に記憶したり、CPU6の作業領域等として用いられるものである。
【0023】
RAM9では、関節部Q1、Q2を回転駆動させるために、各モータに入力される駆動電圧値(入力電圧値)が予め記憶されている。ロボット制御装置1では、
図3に示すような、基準点xと目標点x
dとの間の位置偏差Δxに応じてパルス幅(入力時間)t
wiが変調されるパルス状の駆動電圧パターン(入力電圧パターン)のPWM信号を各モータへ出力するように制御する。この際、この駆動電圧パターンの電圧値は、各関節部Q1、Q2の最大静止摩擦力に打ち勝つためのトルクを出力させることができる値に設定されている必要がある。従って、RAM9は、少なくとも各関節部Q1、Q2の最大静止摩擦力に打ち勝つためのトルクを出力させることができるように予め設定された駆動電圧値p
h、n
hを記憶している。尚、p
hは正の駆動電圧の大きさを示しており、n
hは負の駆動電圧の大きさを示している。また、駆動電圧値p
h、n
hは、少なくとも各関節部Q1、Q2の最大静止摩擦力に上回るトルクが出力できるように設定されていれば良く、モータの減速比等に応じて適宜設定されれば良い。
【0024】
また、RAM9には、後述するPWM信号生成手段82により、位置偏差Δxに応じて駆動電圧の入力時間(パルス幅)t
wiを設定する際の基準となる最小入力時間(最小パルス幅)t
wmin及び最大入力時間(最大パルス幅)t
wmaxが予め記憶されている。この最小入力時間t
wmin及び最大入力時間t
wmaxは、ロボット2の構成や要求される位置決め精度、目標位置への収束時間等に応じて、予め適宜所定の値に設定されるものである。
【0025】
表示部10は、例えば、液晶ディスプレイ等から構成されるものであって、位置偏差等の情報を表示するものである。操作部11は、マウスやキーボード等で構成されており、操作者が種々のデータ及び操作指令等の入力を行うために使用されるものである。尚、表示部10と操作部11をタッチパネルとして、一体的に構成しても良い。
【0026】
このようなコンピュータ5にカメラ4から得られた画像データが入力されると、コンピュータ5では、演算処理部8の位置偏差取得手段81によって、画像データに基づいて基準点xと目標点x
dとの間の位置偏差Δxを算出する(S102)。具体的には、位置偏差取得手段81では、例えば、カメラ4から得られた画像データから基準点xと目標点x
dを画像認識処理によってそれぞれ抽出し、基準点xから目標点x
dまでの2次元ベクトル(位置偏差)Δxを求める。尚、画像認識処理等については、従来公知の方法を用いれば良い。
【0027】
次に、演算処理部8では、基準点xが目標点x
dに収束したか否かを判定する(S103)。つまり、位置偏差取得手段81により求められた位置偏差Δxが要求される精度の所定の位置偏差内に収束しているか否かを判定する。ここで、演算処理部8は、基準点xが目標点x
dに収束したと判定すると(S103:YES)、各モータによって関節部Q1、Q2を駆動させて、基準点xを移動させる必要はないので、このまま処理を終了する。
【0028】
一方、演算処理部8は、基準点xが目標点x
dに収束していないと判定すると(S103:NO)、PWM信号生成手段82により、位置偏差取得手段81によって求められた位置偏差Δxに基づいて、駆動電圧(入力電圧)のパルス幅(入力時間)t
wiを変調したPWM信号を生成する(S104)。具体的には、PWM信号生成手段82では、位置偏差Δxの大きさに基づいて、各自由度の回転角度を変化させるため、各関節部Q1、Q2へのパルス状の入力電圧の入力時間t
wiを下記の数式(1)を用いて導出する。尚、数式(1)におけるiは、ロボット2の自由度を表わしており、本実施形態では、2自由度のロボット2を用いているので、i=1,2となる。また、数式(1)における入力時間の最小値(最小パルス幅)t
wmin及び入力時間の最大値(最大パルス幅)t
wmaxは予め設定されているものである。また、下記の数式(2)は、基準点xの並進速度(手先並進速度)に関するヤコビ行列を示しており、誤差を含む角度情報を用いて導出したものである。また、下記の数式(3)は、カメラ4による基準点と目標点との位置偏差を示しており、カメラキャリブレーション誤差を含んでいる。
【数1】
【数2】
【数3】
【0029】
PWM信号生成手段82では、下記の数式(4)に示すスカラー量が零の場合には、入力時間t
wiを数式(1)によって、入力時間t
wi=0として設定する。つまり、この場合には、入力電圧はモータに印加されない。また、数式(4)に示すスカラー量に比例ゲインkiを掛けた値が、零よりも大きく、入力時間の最小値t
wminよりも小さい場合には、入力時間t
wiを数式(1)によって、入力時間t
wi=t
wminとして設定する。つまり、この場合には、入力時間t
wiとして入力時間の最小値t
wminが適用されることになる。また、数式(4)に示すスカラー量に比例ゲインkiを掛けた値が、入力時間の最小値t
wmin以上で、入力時間の最大値t
wmaxよりも小さい場合には、入力時間t
wiとして、数式(1)によって、数式(4)に示すスカラー量に比例ゲインkiを掛けた値がそのまま設定される。また、式(4)に示すスカラー量に比例ゲインkiを掛けた値が、入力時間の最大値t
wmax以上の場合には、入力時間t
wiを数式(1)によって、入力時間t
wi=t
wmaxとして設定する。つまり、この場合には、入力時間t
wiとして入力時間の最大値t
wmaxが適用されることになる。このようにして、PWM信号生成手段82では、基準点xと目標点x
dとの間の位置偏差Δxに基づいて、入力電圧の入力時間(パルス幅)t
wiを変調させたPWM信号を生成する。尚、入力電圧の正負は、数式(4)の絶対値内の正負によって決定する。
【数4】
【0030】
そして、駆動制御手段83では、PWM信号生成手段82によって生成されたそれぞれの入力電圧パターンのPWM信号を関節部Q1、Q2を駆動させる各モータへそれぞれ出力し、基準点xを目標点x
dへ向けて移動するように制御する(S105)。その後、再び目標点x
dを視野に捉えられた状態で、S105の処理により目標点x
dへ向けて移動した基準点xをカメラ4により撮像し(S101)、位置偏差取得手段81により、この撮像結果に基づいて、移動後の基準点xと目標点x
dとの間の位置偏差Δxを算出し(S102)、基準点xが目標点x
dに収束したか否かを判定する(S103)。そして、演算処理部8が、基準点xが目標点x
dに収束したと判定した場合には(S103:YES)、そのまま処理を終了し、演算処理部8が、基準点xが目標点x
dに収束していないと判定した場合には(S103:NO)、基準点xが目標点x
dに収束したと判定されるまで、再びS104及びS105の処理が繰り返し行われる。尚、S105の駆動制御手段83によるそれぞれのPWM信号の各モータへの出力は同時に行われるものであり、入力時間の長いモータにより駆動させられる関節部の動作が終了した後、所定の間隔をおいて、再びS101〜S104の処理を経て生成された次のPWM信号が各モータへそれぞれ出力される。また、この所定の間隔は、前後の入力同士が干渉せず、且つ、カメラ4のサンプリングタイムより長い時間に設定されることが好ましい。
【0031】
このようにロボット制御装置1では、基準点xと目標点x
dとの間の位置偏差Δxを導出し、この位置偏差Δxに応じてパルス幅(入力時間)t
wiが変調されるパルス状の駆動電圧パターン(入力電圧パターン)のPWM信号を各モータへ出力するように制御しているので、エンコーダレスで、リンクたわみや据え付け誤差に影響されることなく、且つ、各モータの物理条件が初期状態と異なるような場合でも、基準点を目標点へと高精度に移動させることができる。
【0032】
尚、本実施形態に係るロボット制御装置1では、1台のカメラ4を用いて、2自由度のロボット2に対して制御を行う際の例を用いて説明しているが、ロボット2の自由度やカメラ4の数は特に限定されるものではなく、3自由度以上のロボットに対しても適用することができるものである。この場合には、アーム3を3自由度以上に構成し、ステレオカメラを用いて、基準点と目標点との位置偏差を算出し、同様の処理を行うようにすれば良い。また、本実施形態に係るロボット制御装置1は、カメラ4の視野に基準点x及び目標点x
dを捉えた状態で、制御を行うものであり、カメラ4の視野内に基準点x及び目標点x
dが入るまでは、従来公知の他の制御方法を適用するようにしても良い。
【実施例】
【0033】
以下、本発明の実施形態に係るロボット制御装置1を用いて、水平2自由度ロボットアームの制御を行った実施例について説明する。
【0034】
(実施例1)
実施例1では、
図4及び
図5に示すような水平2自由度ロボット2aに対して、ロボット制御装置1を用いて、基準点を目標点へと移動させるように制御を行っている。このロボット2aは、
図4及び
図5に示すように、関節部Q1’に回転自在に連結されるリンク31と、平行リンクを構成する駆動リンク32a及び従動リンク32bと、リンク33とを備えている。駆動リンク32aの一端は、関節部Q2’に回転自在に連結されており、他端は、リンク33に回転自在に取り付けられている。また、従動リンク32bの一端は、リンク31の先端に回転自在に取り付けられており、他端は、リンク33の一端側に回転自在に取り付けられている。また、関節部Q1は、モータ13aにより減速機構14bを介して回転駆動できるように構成されており、関節部Q2は、モータ13bにより減速機構14bを介して回転駆動できるように構成されている。また、ロボット2aでは、リンク33の先端(他端)に基準点xが設定されている。このロボット2aでは、関節部Q1’、Q2’の回転角度θ1、θ2を変化させることにより、これらの関節部Q1’、Q2’を介して、各リンクが駆動し、基準点xをXY水平面内で駆動させることができる。
【0035】
このロボット2aに用いたモータ13a、13bの減速比は共に210:1である。また、それぞれのリンク長は、リンク31のリンク長l1=45[mm]、リンク32a、32bのリンク長l2=130[mm]、リンク33のリンク32aの他端が回動自在連結されている位置から先端の基準点までの長さl3=100[mm]に設計されている。また、パルス入力電圧に関するパラメータとして、最小入力時間t
wmin=2[ms]、最大入力時間t
wmax=60[ms]、パルス入力電圧同士の最小入力間隔=120[ms]、正の入力電圧ph=4.1[V]、負の入力電圧nh=4.1[V]、比例ゲインki=4.5に設定し、ロボット2aに対して、ロボット制御装置1を用いて、基準点を目標点へと移動させるように制御を行った結果、
図6に示すような結果が得られた。
【0036】
図6は、基準点と目標点と間の位置偏差及び各モータ13a、13bへの入力電圧の時系列データを示すものである。尚、
図6中のv1は、モータ13aへ印加される入力電圧パターンを示しており、v2は、モータ13bへ印加される入力電圧パターンを示している。
図6に示すように、実施例1における最終的な位置偏差は、Δx~=(Δx~,Δy~)
T=(−0.33,0.52)
T[mm]であり、基準点の目標点への収束時間は、1.4[s]となっており、非特許文献1よりも収束時間を短縮し、基準点を目標点へ素早く移動させることが可能になっている。尚、最終的な位置偏差の精度については、カメラ4の解像度及び最小入力電圧パターンに対する関節部Q1’、Q2’の回転角度の大きさを調整することで、より向上を図ることが可能である。また、パルス入力電圧同士の間隔を短縮することにより、より収束時間も短縮することが可能である。
【0037】
尚、本発明の実施の形態は上述の形態に限るものではなく、本発明の思想の範囲を逸脱しない範囲で適宜変更することができる。