Traitement en cours

Veuillez attendre...

Paramétrages

Paramétrages

Aller à Demande

1. WO2020161910 - DISPOSITIF DE COMMANDE, PROCÉDÉ DE COMMANDE ET SUPPORT D'ENREGISTREMENT

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  

請求の範囲

1   2   3   4   5   6   7  

図面

1   2   3   4   5   6   7   8   9   10   11   12   13   14  

明 細 書

発明の名称 : 制御装置、制御方法、記録媒体

技術分野

[0001]
 本発明は、制御装置、制御方法、記録媒体に関する。

背景技術

[0002]
 ロボットの制御装置は、ロボットに設けられた関節機構を駆動制御して、ロボットを構成する所定部位を目標位置に移動させる。ロボットの制御の例として、特許文献1には、ロボットアームの干渉を回避するための計算を高速に行う技術が開示されている。また、多関節システムにおける複数の自由度を有する体節の衝突を回避する技術が特許文献2に開示されている。

先行技術文献

特許文献

[0003]
特許文献1 : 特開2017-131990号公報
特許文献2 : 特許第5261495号公報

発明の概要

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

[0004]
 上述のようなロボットの制御装置では、ロボットを構成する移動部位の軌道を全て計算することがある。関節等の移動部位の軌道を計算することでロボットの姿勢が定まる。制御装置は、ロボットの関節などの所定部位を駆動制御している際に、その所定の一部が障害物や負荷により動かない場合、上述のようなロボットの各関節の位置などの含む姿勢状態の情報を定めるため、再度、移動部位の全体の位置や軌道の再計算を行わなければならなかった。
[0005]
 そこでこの発明は、上述の課題を解決する制御装置、制御方法、記録媒体を提供することを目的としている。

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

[0006]
 本発明の第1の態様によれば、制御装置は、複数の関節を有するロボットアームに接続され、前記ロボットアームの姿勢状態または前記ロボットアームの駆動状態を少なくとも示すシステム情報を取得するシステム情報取得部と、前記ロボットアームの制御目標を示す制御目標情報を取得する制御目標取得部と、前記制御目標情報に基づく前記ロボットアームの駆動に対する前記関節の寄与を示す指標である寄与情報を前記システム情報に基づいて算出する関節寄与情報算出部と、前記寄与情報に基づいて前記各関節の駆動の制御に用いる制御指示情報を算出する制御指示部と、を備えることを特徴とする。
[0007]
 本発明の第2の態様によれば、制御方法は、複数の関節を有するロボットアームに接続された制御装置が、前記ロボットアームの姿勢状態または前記ロボットアームの駆動状態を少なくとも示すシステム情報を取得し、前記ロボットアームの制御目標を示す制御目標情報を取得し、前記制御目標情報に基づく前記ロボットアームの駆動に対する前記関節の寄与を示す指標である寄与情報を前記システム情報に基づいて算出し、前記寄与情報に基づいて前記各関節の駆動の制御に用いる制御指示情報を算出することを特徴とする。
[0008]
 本発明の第3の態様によれば、記録媒体は、複数の関節を有するロボットアームに接続された制御装置において、前記ロボットアームの姿勢状態または前記ロボットアームの駆動状態を少なくとも示すシステム情報を取得するシステム情報取得機能と、前記ロボットアームの制御目標を示す制御目標情報を取得する制御目標取得機能と、前記制御目標情報に基づく前記ロボットアームの駆動に対する前記関節の寄与を示す指標である寄与情報を前記システム情報に基づいて算出する関節寄与情報算出機能と、前記寄与情報に基づいて前記各関節の駆動の制御に用いる制御指示情報を算出する制御指示機能と、を実現させるプログラムが記録されることを特徴とする。

発明の効果

[0009]
 本発明によれば、ロボットを制御して所定の部位を目標位置に移動させることができる。

図面の簡単な説明

[0010]
[図1] 本発明の一実施形態によるロボットシステムの概要を示す図である。
[図2] 本発明の一実施形態による制御装置のハードウェア構成を示す図である。
[図3] 本発明の一実施形態による制御装置の機能ブロック図である。
[図4] 第一の実施形態による制御装置の処理フローを示す第一の図である。
[図5] 第一の実施形態によるロボットアームの動作例を示す第一の図である。
[図6] 第一の実施形態によるロボットアームの動作例を示す第二の図である。
[図7] 第二の実施形態による制御装置の処理フローを示す第一の図である。
[図8] 第二の実施形態によるロボットアームの動作例を示す第一の図である。
[図9] 第二の実施形態によるロボットアームの動作例を示す第二の図である。
[図10] 本発明の一実施形態による寄与情報の算出例を示す第一の図である。
[図11] 本発明の一実施形態による寄与情報の算出例を示す第二の図である。
[図12] 本発明の一実施形態による寄与情報の算出例を示す第三の図である。
[図13] 本発明の一実施形態による制御装置の最小構成を示す図である。
[図14] 本発明の一実施形態による最小構成の制御装置による処理フローを示す図である。

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

[0011]
 以下、本発明の一実施形態による制御装置を図面を参照して説明する。
 図1は同実施形態による制御装置を含むロボットシステムの概要を示す図である。
 この図で示すように、ロボットシステム100は制御装置1とロボットアーム2とを含んで構成される。ロボットアーム2は、複数のリンク21と、当該複数のリンク21を連結先に回動自在に連結する関節機構22(j )とを備えている。なお関節機構22(j )の記載においてjは関節機構22を示し、iは番号を示す。つまり、関節機構22(j )n個によりロボットアームが構成される場合はi=1,2,‥‥nとなる。関節機構22(j )はロボットアーム2を構成する何れか一つまたは複数の関節機構22を示す。制御装置1は、少なくともロボットアーム2の関節機構22(j )を駆動制御する。これにより制御装置1は、関節機構22(j )や手先位置などの所定の部位を所定の目標位置に移動させる。
[0012]
 関節機構22(j )は、連結するリンク21の軸を中心として当該リンク21を回転させる第一回転モータ、関節機構22(j )に連結するリンク21の軸に直交する直交方向に当該リンク21を傾ける第二回転モータなどを内蔵して構成される。ロボットアーム2の先端部には物体を把持する把持機構23などが設けられてよい。そして制御装置1は関節機構22(j )の各モータと電気的に接続されており、関節機構22(j )の駆動制御を行う。
[0013]
 図2は制御装置のハードウェア構成を示す図である。
 この図が示すように制御装置1は、制御部であるCPU(Central Processing Unit)101、ROM(Read Only Memory)102、RAM(Random Access Memory)103、SSD(Solid State Drive)104、通信モジュール105等の各ハードウェアを備えたコンピュータである。
[0014]
 図3は制御装置1の機能ブロック図である。
 制御装置1のCPU101は電源が投入されると起動し、予め記憶する制御プログラムを実行する。これにより制御装置1は、制御目標取得部10、関節寄与情報算出部11、制御指示部12、システム情報取得部13、状態算出部14、障害検知部15の各機能を発揮する。
[0015]
 制御目標取得部10は、ロボットアーム2の制御目標を示す制御目標情報を取得する。
 関節寄与情報算出部11は、各関節機構22(j )の制御目標情報に対する寄与を示す指標である寄与情報を算出する。寄与情報は、寄与の程度を表す指標であってもよいし、寄与の有無を表す情報であってもよい。
 制御指示部12は、寄与情報に基づいて各関節機構22(j )の駆動の制御に用いる制御指示情報を算出する。
 システム情報取得部13は、ロボットシステム100の状態を示す情報および環境の情報を含むシステム情報を取得する。
[0016]
 状態算出部14は、システム情報取得部13により取得された情報等を用いて各関節機構22(j )や手先位置などの現在のロボットアーム2に関する状態の情報を算出する。
 障害検知部15は、関節機構22(j )の近傍の障害物の位置、関節機構22(j )の近傍に入ると予測される障害物の位置や、関節機構22(j )への障害の発生を検知する。ここで、近傍とは、関節機構22(j )(または、把持機構23、リンク21)からの距離が所定の距離以下、または、所定の距離未満であることを表している。
[0017]
 このような各機能を発揮する制御装置1の処理により、ロボットアーム2全体の姿勢を算出することなくより柔軟にロボットアーム2を制御して先端部を目標位置に移動させることができる。
[0018]
(第一の実施形態)
 図4は第一の実施形態による制御装置の処理フローを示す第一の図である。
 次に第一の実施形態による制御装置の処理フローを順を追って説明する。
 まず制御装置の制御指示部12は制御を開始すると、制御目標取得部10に制御目標情報の取得を指示し、またシステム情報取得部13にシステム情報の取得を指示する。
[0019]
 制御目標情報は、少なくともロボットアーム2の制御目標を示す。一例としての制御目標情報はロボットアーム2の目標手先位置Pd={xd,yd,zd}であってよい。目標手先位置Pd{xd,yd,zd}は、ロボットアーム2の手先の現在位置の移動先の3次元座標を示す。制御目標情報はまたはリンク21の一部または複数部位の目標位置、目標速度、目標トルク、速度ベクトル、力ベクトル、加速度ベクトルであってよい。複数部位は、例えば全ての関節機構22の数より少ない数の関節機構22であってよい。制御目標情報はそれらの値の時系列情報であってよい。
[0020]
 システム情報は、各関節機構22の関節角度、角速度、トルク、関節間距離、関節位置(座標)、関節取り付け角度などの、いわゆるDH(Denavit-Hartenberg)のパラメータ、可動範囲などを含んでよい。システム情報には、障害検知部15により検出された障害物の情報を含んでもよい。例えば障害物の情報には、障害物の中心を示す3次元座標、障害物の表面の離散化した3次元座標、障害物の表面のベクトルデータ、障害物の表面のポリゴン近似データ、障害物の表面の関数近似データなどであってよい。また障害物の情報としてロボットアーム2の座標が含まれてもよい。ロボットアーム2の座標が障害物の情報に含まれることにより、ロボットアーム2の異なる位置が接触しないよう制御することができる。
[0021]
 ここで一例として、制御目標取得部10が、目標手先位置Pd={xd,yd,zd}を取得する(ステップS101)。またシステム情報取得部13が、システム情報として、各関節機構22の関節角度θ={θ1,θ2,…θn}、関節間距離(各リンク21の長さ)l={l1,l2,…ln}などを取得する。
[0022]
 状態算出部14は、システム情報取得部13からシステム情報を取得する(ステップS102)。そして状態算出部14は、現在の手先位置Pcを算出する(ステップS103)。現在の手先位置Pcは、システム情報から、たとえば、順運動学に従い算出すればよい。たとえば、ロボットアーム2の座標の基点と、DH(Denavit-Hartenberg)のパラメータがわかれば、手先の位置を求めることができる。また状態算出部14は、現在の手先位置Pcを目標手先位置Pdに移動させるために動作させることのできる1つまたは複数の動作対象となる各関節機構22(j )の関節位置Piを算出する(ステップS104)。
[0023]
 なお、状態算出部14は、ロボットアーム2自体を撮影する外部カメラから撮影画像を取得し、この撮影画像に基づいて現在の手先位置Pcや各関節機構22(j )の関節位置Piを算出してもよい。
[0024]
 状態算出部14は、関節寄与情報算出部11へ現在の手先位置Pcと現在の動作対象となる関節機構22(j )の各関節位置Piとを出力する。関節寄与情報算出部11は、システム情報、手先位置Pc、動作対象となる各関節位置Piを取得する。
[0025]
 図5は第一の実施形態によるロボットアーム2の動作例を示す第一の図である。
 図6は第一の実施形態によるロボットアーム2の動作例を示す第二の図である。
 図5は二次元の空間範囲においてロボットアーム2の動作する状況において、現在の手先位置Pcを目標手先位置Pdに二次元空間範囲内で移動させる場合の例を示す。
 図6は三次元の空間範囲においてロボットアーム2の動作する状況において、現在の手先位置Pcを目標手先位置Pdに三次空間範囲内で移動させる場合の例を示す。
[0026]
 関節寄与情報算出部11は、手先位置Pcを目標手先位置Pdに移動させる場合の、動作対象となる各関節機構22(j )の移動方向と移動量θdiとを算出する(ステップS105)。例えばある関節機構22(j )については、その関節機構22(j )の関節位置Piと手先位置Pcとをつなぐ線が、当該関節機構22(j )の関節位置Piと目標手先位置Pdとをつなぐ線に一致する際の移動方向と移動量θdiとを決定する。例えば、Pd,Pi,Pcの幾何学的関係を用いることでθdiを求めることが出来る。例えば、それぞれの座標をベクトルで表し,Pd-Pi,Pc-Piで示されるベクトルのなす角としてθdiを求める。また、たとえばベクトルの内積を用いてθdiを求めることもできる。関節寄与情報算出部11はシミュレーション処理により、各関節機構22(j )の移動方向と移動量Δθdiとを求めてよい。
[0027]
 図6は、関節機構22(j )が3次元方向に動作する場合のある関節機構22(j )の移動方向と移動量Δθdiとの算出概要を示す。図6で示すように関節寄与情報算出部11は、現在の手先位置Pcを、ある関節機構22(j )に接続されたリンク21の回転軸に直交し当該関節機構22(j )の位置Piを通る面上に射影した手先射影位置Pc’を特定する。そして関節寄与情報算出部11は、当該手先射影位置Pc’と当該関節機構22(j )の関節位置Piとを結ぶ線と、目標手先位置Pdを上記面上に射影したと射影目標手先位置Pd’と移動対象の関節Piとを結ぶ線との成す角度であって、当該関節機構22(j )の移動方向の成す角度を示す移動量θdiを算出する。関節寄与情報算出部11は、iに動作対象となる関節機構22(j )を示す数値を設定し、関節機構22(j )を特定する。そして、関節寄与情報算出部11は、移動量θdiとゲインGcを用いて、特定した関節機構22(j )の角度移動指令Δθdiを式(1)により算出する(ステップS106)。
[0028]
[数1]


[0029]
 ここで、動作対象となる各関節機構22(j )それぞれに対して特定した移動方向に同じ角度の移動を加えた場合、手先位置Pcから遠い関節機構22は、少し動くだけでも手先位置Pcが大きく変動する。そこで関節寄与情報算出部11は、ロボットアーム2の現在の姿勢をもとに、手先位置をPcから目標手先位置Pdに移動させる際の、移動量に対する特定した関節機構22(j )の寄与度を示す寄与情報kiを以下のように算出する。寄与情報kiの算出にあたり、まず関節寄与情報算出部11は、特定した関節機構22(j )のPc-Pi間距離diを、式(2)を用いて算出する。
[0030]
[数2]


[0031]
 そして関節寄与情報算出部11は特定した関節機構22(j )の寄与情報kiを、式(3)を用いて算出する(ステップS107)。
[0032]
[数3]


[0033]
 式(3)においてbは定数を示す。f(di)は、Pd-Pi間距離diの二乗に比例する関数でもあってよく、Pd-Pi間距離diを引数とする任意の関数でよい。または、f(di)は、距離diを変数とした多項式、あるいは距離diを変数とした三角関数、対数関数、指数関数あるいは上記の関数の和・積・商で表される任意の関数でも良い。また、Pd-Pi間距離diに代えて、各関節機構22(j )のロボットアーム2における位置の順番(手先からの位置の順番など)に応じて決定された値d’iを用いて、同様の式により寄与情報kiを算出してもよい。そして、関節寄与情報算出部11は各関節機構22の寄与情報kiを制御指示部12へ出力する。
[0034]
 制御指示部12は、特定した関節機構22(j )の寄与情報kiを用いて、式(4)により制御指示情報θcontrol(i)を算出する(ステップS108)。または、制御指示情報θcontrol(i)は、kiとΔθdiとにより表される任意の関数であってもよい。
[0035]
[数4]


[0036]
 制御指示部12は、特定した関節機構22(j )に対して制御指示情報θcontrol(i)を出力する(ステップS109)。制御指示部12は動作対象となる各関節機構22(j )のうち、制御指示情報を算出していない関節機構22(j )があるかを判定する(ステップS110)。制御指示部12は制御指示情報を算出していない関節機構22(j )がある場合、次の関節機構22(j )に対する処理を行うよう関節寄与情報算出部11へ指示する。関節寄与情報算出部11は動作対象となる各関節機構22(j )のうち、寄与情報kiを算出していない関節機構22(j )を示す値をiに設定し、ステップS106からの処理を微小時間ごとに繰り返す。これにより、動作対象となる各関節機構22(j )が動作し、手先位置が目標手先位置Pdへ移動する。制御指示部12は、手先位置Pcが最終目標位置に到達し終了するかを判定する(ステップS111)。制御装置1は最終手先位置に到達するように、目標手先位置Pdを徐々にずらして、上記の処理を逐次的に実行することで、手先位置が徐々に動いてき最終目標位置に到達させることが可能となる。
[0037]
(第二の実施形態)
 図7は第二の実施形態による制御装置の処理フローを示す第一の図である。
 次に第二の実施形態による制御装置の処理フローを順を追って説明する。
 まず制御装置の制御指示部12は制御を開始すると、制御目標取得部10に制御目標情報の取得を指示し、またシステム情報取得部13にシステム情報の取得を指示する。
[0038]
 制御目標情報は、少なくともロボットアーム2の一部または複数部位の制御目標を示す。例えば、制御目標はロボットアーム2の手先目標速度ベクトルVc={vxd,vyd,vzd}である。手先目標速度ベクトルVc{vxd,vyd,vzd}は、ロボットアーム2の手先の3次元空間における速度ベクトルを示す。制御目標は手先目標速度ベクトルVcに代えて、力ベクトル、加速度ベクトルであってもよい。
[0039]
 システム情報は、各関節機構22(j )の関節角度、角速度、トルク、関節間距離、関節位置(座標)、関節取り付け角度などの、いわゆるDH(Denavit-Hartenberg)のパラメータ、可動範囲などを含んでよい。システム情報には、障害検知部15により検出された障害物の情報を含んでもよい。例えば障害物の情報には、障害物の中心を示す3次元座標、障害物の表面の離散化した3次元座標、障害物の表面のベクトルデータ、障害物の表面のポリゴン近似データ、障害物の表面の関数近似データなどであってよい。また障害物の情報としてロボットアーム2の座標が含まれてもよい。ロボットアーム2の座標が障害物の情報に含まれることにより、ロボットアーム2の異なる位置が接触しないよう制御することができる。
[0040]
 ここで一例として、制御目標取得部10が、手先目標速度ベクトルVc={vxd,vyd,vzd}を取得する(ステップS201)。またシステム情報取得部13が、各関節機構22(j )の関節角度θ={θ1,θ2,…θn}、関節間距離(各リンク21の長さ)l={l1,l2,…ln}などのシステム情報を取得する。
[0041]
 状態算出部14は、システム情報取得部13からシステム情報を取得する(ステップS202)。そして状態算出部14は、現在の手先位置Pcを算出する(ステップS203)。現在の手先位置Pcは、システム情報から、たとえば、順運動学に従い算出すればよい。たとえば、ロボットアーム2の座標の基点と、DH(Denavit-Hartenberg)のパラメータがわかれば、手先位置Pcを求めることができる。また状態算出部14は、現在の手先位置Pcを目標手先位置Pdに移動させるために動作させることのできる1つまたは複数の動作対象となる各関節機構22(j )の関節位置Piを算出する(ステップS204)。
[0042]
 なお、状態算出部14は、第一の実施形態と同様に、ロボットアーム2自体を撮影する外部カメラから撮影画像を取得し、この撮影画像に基づいて現在の手先位置Pcや各関節機構22(j )の関節位置Piを算出してもよい。
[0043]
 状態算出部14は、関節寄与情報算出部11へ現在の手先位置Pcと現在の動作対象となる関節機構22(j )の各関節位置Piとを出力する。関節寄与情報算出部11は、システム情報、手先位置Pc、関節位置Piを取得する。
[0044]
 図8は第二の実施形態によるロボットアーム2の動作例を示す第一の図である。
 図9は第二の実施形態によるロボットアーム2の動作例を示す第二の図である。
 図8は二次元の空間範囲においてロボットアーム2の動作する状況において、手先目標速度ベクトルVcに基づいて、現在の手先位置Pcを目標手先位置Pdに二次元空間範囲内で移動させる場合の例を示す。
 図9は三次元の空間範囲においてロボットアーム2の動作する状況において、手先目標速度ベクトルVcに基づいて、現在の手先位置Pcを目標手先位置Pdに三次空間範囲内で移動させる場合の例を示す。
[0045]
 関節寄与情報算出部11は、手先位置Pcを目標手先位置Pdに移動させる場合の、動作対象となる各関節機構22(j )の移動方向と移動量θdiとを算出する(ステップS205)。例えばある関節機構22(j )については、その関節機構22(j )の関節位置Piと手先位置Pcとをつなぐ線が、当該関節機構22(j )の関節位置Piと目標手先位置Pdとをつなぐ線に一致する際の移動方向と移動量θdiとを決定する。例えば、Pd,Pi,Pcの幾何学的関係を用いることでθdiを求めることが出来る。例えば、それぞれの座標をベクトルで表し,Pd-Pi,Pc-Piで示されるベクトルのなす角としてθdiを求める。また、たとえばベクトルの内積を用いてθdiを求めることもできる。関節寄与情報算出部11はシミュレーション処理により、各関節機構22(j )の移動方向と移動量θdiとを求めてよい。
[0046]
 図9は、関節機構22(j )が3次元方向に動作する場合のある各関節機構22(j )の移動方向と移動量θdiとの算出概要を示す。図9で示すように関節寄与情報算出部11は、現在の手先位置Pcを、ある関節機構22(j )に接続されたリンク21の回転軸に直交し当該関節機構22(j )の位置Piを通る面上に射影した手先射影位置Pc’を特定する。そして関節寄与情報算出部11は、当該手先射影位置Pc’と当該関節機構22(j )の関節位置Piとを結ぶ線と、目標手先位置Pdを上記面上に射影したと射影目標手先位置Pd’と移動対象の関節Piとを結ぶ線との成す角度であって、当該関節機構22(j )の移動方向の成す角度を示す移動量θdiを算出する。
[0047]
 関節寄与情報算出部11は、ある関節機構22(j )についての寄与情報を算出するにあたり、手先目標速度ベクトルVcを用いて、そのある関節機構22(j )の関節位置Piと手先位置Pcとを結ぶ直線に直交し手先位置Pcを基点とする速度ベクトル成分Vciを導出する。関節寄与情報算出部11は、iに動作対象となる関節機構22(j )を示す数値を設定し、関節機構22(j )を特定する。そして、関節寄与情報算出部11は、速度ベクトル成分Vciと所定のゲインGcを用いて、特定した関節機構22(j )の角速度移動指令Δθ’diを式(5)により算出する(ステップS206)。
[0048]
[数5]


[0049]
 式(5)において「*」はベクトルの内積を示す。また式(5)において「e_x」は速度ベクトル成分Vciの単位ベクトルを示す。なお関節寄与情報算出部11は、特定した関節機構22(j )についての寄与情報を算出するにあたり、力ベクトルfを用いて、特定した関節機構22(j )の関節位置Piと手先位置Pcとを結ぶ直線に直交し手先位置Pcを基点とする力ベクトル成分fciを導出する。そして、関節寄与情報算出部11は、力ベクトル成分fciと所定のゲインGcを用いて、式(6)により角速度移動指令Δθ’diを算出してもよい。式(6)においても「*」はベクトルの内積を示す。また式(6)において「e_x’」は力ベクトル成分fciの単位ベクトルを示す。
[0050]
[数6]


[0051]
 ここで、動作対象となる各関節機構22(j )それぞれに対して特定した移動方向に同じ角速度で移動するよう制御を加えた場合、手先位置Pcから遠い関節機構22は、少し動くだけでも手先位置pcが大きく変動する。そこで関節寄与情報算出部11は、ロボットアーム2の現在の姿勢をもとに、手先位置Pcから目標手先位置Pdに移動させる際の、移動量に対する特定した関節機構22(j )の寄与度を示す寄与情報kiを以下のように算出する(ステップS207)。
[0052]
[数7]


[0053]
 または、手先位置Pcに近い関節機構22(j )の移動量をより多くして、動作対象となる各関節機構22(j )の制御トルクを小さくすることができる。この場合、手先位置Pcに近い動作対象となる関節機構22(j )がより手先位置Pcの移動に寄与するため、式(8)により寄与情報kiを算出してもよい。
[0054]
[数8]


[0055]
 なお上記式(7),(8)における「√(Pd -Pi )」を当該「√(Pd -Pi )」を変数とする関数に置き換えてもよい。そして関節寄与情報算出部11は、特定した関節機構22の寄与情報kiを制御指示部12へ出力する。
[0056]
 制御指示部12は、特定した関節機構22(j )の寄与情報を用いて、式(9)により制御指示情報θ’control(i)を算出する(ステップS208)。
[0057]
[数9]


[0058]
 制御指示情報θ’control(i)は、「ki・Δθ’di」を含む任意の関数であってもよい。制御指示部12は、特定した関節機構22(j )に対して制御指示情報θ’control(i)を出力する(ステップS209)。制御指示部12は動作対象となる各関節機構22(j )のうち、制御指示情報を算出していない関節機構22(j )があるかを判定する(ステップS210)。制御指示部12は制御指示情報を算出していない関節機構22(j )がある場合、次の関節機構22(j )に対する処理を行うよう関節寄与情報算出部11へ指示する。関節寄与情報算出部11は動作対象となる各関節機構22(j )のうち、寄与情報kiを算出していない関節機構22(j )を示す値をiに設定し、ステップS206からの処理を微小時間ごとに繰り返す。これにより、各関節機構22(j )が動作し、手先位置が目標手先位置Pdへ移動する。制御指示部12は、手先位置Pcが最終目標位置に到達し終了するかを判定する(ステップS211)。制御装置1は最終手先位置に到達するように、目標手先位置Pdを除所にずらして、上記の処理を逐次的に実行することで、手先位置が徐々に動いてき最終目標位置に到達させることが可能となる。
[0059]
(第三の実施形態)
 第一の実施形態および、第二の実施形態においては、寄与情報の算出において、ある目標位置に移動させるロボットアーム2の対象部位(手先位置Pc)の目標位置までの移動に寄与する各関節機構22(j )の動きの寄与度に基づいて、寄与情報kiを算出している。しかしながら、関節寄与情報算出部11は、寄与情報の算出に、関節機構22(j )が所定量動作する際の力の大きさや、慣性トルクなどの、動作の容易さを示す指標に基づいて目標位置に移動させる対象部位の移動に対する寄与度を示す寄与情報kiを算出してもよい。
[0060]
 具体的には、関節寄与情報算出部11は、第一の実施形態において算出した制御指示情報θcontrol(i)に基づく実際の関節機構22(J)への制御に対して、実際にその関節機構22(j )が規定の制御時間内で動作した角度θactual(i)の割合を寄与情報kiとして式(10)により算出する。
[0061]
[数10]


[0062]
 上述の式(10)において、関節寄与情報算出部11は、第一の実施形態において算出した制御指示情報θcontrol(i)に代えて、第二の実施形態において算出した制御指示情報θ’control(i)を用いてもよい。また上記式(10)は第一の実施形態において算出した寄与情報kiは、制御指示情報θcontrol(i)に対する制御時間内で動作した角度θactual(i)の比により求めているが、それらの差分により算出してもよいし、当該比や差分を含む任意の関数であってもよい。関節寄与情報算出部11は、寄与情報kiの算出に、制御指示情報θcontrol(i)と、制御時間内で動作した角度θactual(i)とを含む関数を用いてもよい。そして、制御指示部12は、式(10)により得られた寄与情報kiを用いて制御指示情報θcontrol(i)を算出する。
[0063]
 なお寄与情報は、第一の実施形態や第二の実施形態で算出した寄与情報kiと、第三の実施形態で算出した寄与情報kiの和、積で得られた値や、それら寄与情報を含む関数により算出された値であってもよい。
[0064]
(第四の実施形態)
 関節寄与情報算出部11は、ある関節機構22(j )の寄与情報k’iの算出に、上述の実施形態で算出した自関節機構22(j )の寄与情報kiと、他の関節機構の寄与情報k の値を用いてもよい。例えば他のj番目の関節機構22(J )の寄与情報をk と表記したとする。この場合、一例としては式(11)により寄与情報k’iを算出する。他の関節機構22(J )はロボットアーム2を構成する全ての関節機構22であってもよいし、寄与情報kiの算出対象の関節機構22の近傍の1つまたは複数の関節機構22であってもよい。なお式(11)においてnはロボットアームを構成する関節機構22の数を示す。
[0065]
[数11]


[0066]
(第五の実施形態)
 上述の実施形態では手先位置Pcを目標手先位置Pdに移動させる場合の例について説明しているが、制御装置1はロボットアーム2の任意の部位の現在位置Peを目標手先位置Pdに移動させる処理を行ってもよい。この場合、上記の処理において手先位置Pcを、任意の部位の現在位置Peに代えて処理を行えばよい。
[0067]
(第六の実施形態)
 上述の処理において、何れかの関節機構22(j )の動きを妨げる障害物の位置に基づいて、障害物に近い関節機構22の寄与度をより低くした寄与情報を算出してもよい。例えば障害検知部15は、ロボットアーム2の所定位置に設けられた物体検知センサからセンシング情報を取得する。物体検知センサが近接センサである場合、その近接センサの設置位置と、近接センサからのセンシング情報(物体までの距離)などに基づいて、物体の座標を算出する。障害検知部15は、物体検知センサがカメラである場合、そのカメラの設置位置と、カメラから得た撮影画像とに基づいて、物体の座標を算出する。障害検知部15は、物体検知センサが測距センサである場合、その測距センサの設置位置と、当該測距センサから得たセンシング情報(物体までの距離や、距離画像)などに基づいて、物体の座標を算出する。障害検知部15は状態算出部14から各関節機構22(j )の現在位置を取得する。障害検知部15は各関節機構22(j )の現在座標に基づいてリンク21の座標を算出する。障害検知部15は、リンク21や関節機構22(j )の位置と、物体の位置とが所定の距離以下である場合には、その物体が障害物であると判定する。障害検知部15は障害物の位置を関節寄与情報算出部11へ出力する。
[0068]
 関節寄与情報算出部11は、各関節機構22(j )の位置と、障害物の位置との距離dsiを算出する。そして、関節寄与情報算出部11は、関節機構22(j )の位置と、障害物の位置との距離dsiが近いほど、寄与度が低くなる式(12)を用いて、寄与情報kiを算出する。なお式(12)において、第一の実施形態~第五の実施形態の何れかにおいて算出した寄与情報を、大文字Kを用いてKiとする。
[0069]
[数12]


[0070]
 なお寄与情報kiは、関節機構22(j )の位置と、障害物の位置との距離dsiや、第一の実施形態~第五の実施形態の何れかにおいて算出した寄与情報Kiを用いた関数gを用いて、式(13)により算出してもよい。
[0071]
[数13]


[0072]
 以上の処理によれば、関節機構22(j )の近傍に障害物が有る場合、ロボットアーム2が移動する際に障害の干渉を受ける可能性がある。この場合、障害物の近傍に位置する関節機構22(j )の寄与度を低くして、各関節機構22(j )に対する制御指示情報を算出することで、障害物を回避して、障害物の近傍にない関節機構22(j )をより大きく移動させて、手先位置Pcなどの所定の部位を目標手先位置Pdに移動させることができる。つまりこれは、関節寄与情報算出部11が、障害物の位置情報の変化に基づいて、寄与情報を再計算する処理態様の一例である。
[0073]
 なお障害検知部15はシステム情報に基づいて障害物の関節機構22(j )やリンク21への接触を検知してもよい。例えば、上述のようにロボットアーム2を制御する間、システム情報取得部13は、各関節機構22(j )のセンサからシステム情報を取得する。センサは、トルクセンサ、モータの角度センサ、モータの角速度センサなどである。システム情報取得部13は、関節機構22(j )にトルクセンサが設けられている場合、当該トルクセンサからトルク値をセンシング情報として取得する。システム情報取得部13は、関節機構22(j )に角度センサが設けられている場合、当該角度センサから関節機構22(j )に備わる各モータの基準位置からの回転角度をセンシング情報として取得する。システム情報取得部13は、関節機構22(j )に角速度センサが設けられている場合、当該角速度センサから関節機構22(j )に備わる各モータの角速度をセンシング情報として取得する。システム情報取得部13は、取得したセンシング情報を障害検知部15へ出力する。
[0074]
 障害検知部15は、駆動対象の関節機構22(j )の駆動状態を示すシステム情報と、駆動対象の関節機構22(j )に対する制御指示情報との差に基づいて、駆動対象の関節機構22(j )の駆動に対する障害が発生しているかを判定する。例えば制御指示情報には、駆動対象の関節機構22(j )のトルクτ diが含まれている。障害検知部15は、駆動対象の関節機構22(j )に指示したトルクτ diと、その関節機構22(j )から得られたトルク値を比較して、差があるかを判定する。障害検知部15は、駆動対象の関節機構22(j )に指示したトルクτ diと、その関節機構22(j )から得られたトルク値とに所定トルク値以上の差がある場合、その関節機構22(j )に障害が発生していると判定する。障害検知部15は障害の発生している関節機構22(j )のID(識別子)等を関節寄与情報算出部11へ出力する。関節寄与情報算出部11は、当該IDの関節機構22(j )の寄与情報(k )を減少させるように更新する。寄与情報(k )を減少させる幅は、駆動対象の関節機構22(j )に指示したトルクτ diと、その関節機構22(j )から得られたトルク値とに所定トルク値以上の差の大きさに応じた値であってよい。または、寄与情報(k )を減少させる幅は、駆動対象の関節機構22(j )に指示したトルクτ diを含む任意の関数により算出してもよい。制御指示部12は各関節機構22(j )の新たな寄与情報(k )に基づいて、制御指示情報を算出する。
[0075]
 制御指示情報に各関節機構22(j )に対する角速度や、回転角度が含まれる場合があってよい。この場合には、障害検知部15は、駆動対象の関節機構22(j )に指示した角速度や、回転角度と、その関節機構22(j )から得られた角速度や、回転角度を比較して、差があるかを判定する。障害検知部15は、駆動対象の関節機構22(j )に指示した角速度や、回転角度と、その関節機構22(j )から得られた角速度や、回転角度に所定値以上の差がある場合、その関節機構22(j )に障害が発生していると判定する。そして上記同様に、障害検知部15は障害の発生している関節機構22(j )のID等の識別子を関節寄与情報算出部11へ出力する。関節寄与情報算出部11は、当該IDの関節機構22(j )の寄与情報(k )を0に近づけるように更新し、制御指示部12は各関節機構22(j )の新たな寄与情報(k )に基づいて、制御指示情報を算出する。
[0076]
 図10は寄与情報の算出例を示す第一の図である。
 図10で示すロボットアーム2は、ロボットアーム2の先端に向かって順にJ ,J ,J の各関節機構22を備える。また図10で示すロボットアーム2の動作概要では、現在位置Sから目標位置Gまで手先位置を移動させる場合の理を示している。一例として、時刻t=1のタイミング(5a)では、関節機構22(J )の寄与情報(k )=95、関節機構22(J )の寄与情報(k )=80、関節機構22(J )の寄与情報(k )=60である場合を示す。また時刻t=2のタイミング(5b)では、関節機構22(J )の寄与情報(k )=60、関節機構22(J )の寄与情報(k )=75、関節機構22(J )の寄与情報(k )=55である場合を示す。また時刻t=3のタイミング(5c)では、関節機構22(J )の寄与情報(k )=55、関節機構22(J )の寄与情報(k )=80、関節機構22(J )の寄与情報(k )=55である場合を示す。このようにロボットアーム2を制御装置1が制御している間、関節寄与情報算出部11は、各関節機構22(J )の寄与情報(k )を算出する。この各関節機構22(J )の寄与情報(k )の値は動作状況に応じて変動する。ロボットアーム2の先端部の、現在に手先位置Sから目標位置Gまで移動する際の軌跡は、図10(5c)で示す軌跡R1となる。
[0077]
 図11は寄与情報の算出例を示す第二の図である。
 時刻t=1のタイミングで図10と同様に制御装置1がロボットアーム2を制御する。そして時刻t=2のタイミングで障害検知部15が、物体Mへの接触による障害発生を検知したとする。この場合、関節寄与情報算出部11は、図11(6b)のタイミングt=2で示すように、関節機構22(J )の寄与情報(k )=5、関節機構22(J )の寄与情報(k )=5、関節機構22(J )の寄与情報(k )=55と更新する。関節寄与情報算出部11は、図11(6c)のタイミングt=3においては、関節機構22(J )の寄与情報(k )=5、関節機構22(J )の寄与情報(k )=5、関節機構22(J )の寄与情報(k )=55と算出する。これにより、ロボットアーム2の手先位置の、現在位置Sから目標位置Gまで移動する際の軌跡は、図11(6c)で示す軌跡R2となる。つまり物体M(障害物)に近い関節機構22(J )の寄与情報が低く算出され、これにより関節機構22(J )の移動量が小さくなり、関節機構22(J )の移動量が大きくなる。図10(5c)で示す軌跡R1と比較して軌跡R2のように軌跡が変更しても、ロボットアーム2の先端部を目標位置Gまで移動させることができる。なおリンク21は、長さを伸縮可能にする伸縮機構を備え、制御装置1は、先端部が目標位置Gに移動できるように何れかのリンク21の伸縮機構を制御してその長さを変更することができてもよい。
[0078]
 図12は寄与情報の算出例を示す第三の図である。
 時刻t=1のタイミングで図10と同様に、制御装置1がロボットアーム2を制御する。そして時刻t=2のタイミングで障害検知部15が、カメラや測距センサから得た情報に基づいて、物体Mの座標を検知したとする。この場合、制御指示部12は、リンク21や関節機構22の座標が物体の座標に近づくほど、リンク21を動作させる関節機構22や、物体に近づく関節機構22についての寄与情報(k )の値を減じて寄与情報(k )を更新する。一例としては、関節寄与情報算出部11は、図12(7b)のタイミングt=2で示すように、関節機構22(J )の寄与情報(k )=15、関節機構22(J )の寄与情報(k )=15、関節機構22(J )の寄与情報(k )=65と更新する。関節寄与情報算出部11は、図12(7c)のタイミングt=3においては、関節機構22(J )の寄与情報(k )=15、関節機構22(J )の寄与情報(k )=15、関節機構22(J )の寄与情報(k )=55と算出し、この状態で制御指示部12は制御指示情報を算出してロボットアーム2を駆動したとする。これにより、ロボットアーム2の手先位置の、現在位置Sから目標位置Gまで移動する際の軌跡は、図12(7c)で示す軌跡R3となる。図10(5c)で示す軌跡R1や図11で(6c)示す軌跡R2と比較して、軌跡R3のように軌跡が変更しても、ロボットアーム2の先端位置を目標位置Gまで移動させることができる。上述したように、リンク21は、長さを伸縮可能にする伸縮機構を備え、制御装置1は、先端部が目標位置Gに移動できるように何れかのリンク21の伸縮機構を制御してその長さを変更することができてもよい。
[0079]
 上述の例では、制御装置1は、複数のリンク21と当該複数のリンク21それぞれを連結先に回動自在に連結する関節機構22とを備えたロボットアーム2の関節機構22を駆動制御する場合の例について説明した。しかしながら、制御装置1は、複数の部位の位置関係を移動可能に連結する複数の駆動機構を備えたロボットの当該駆動機構を、上記と同様の処理により駆動制御するものであってもよい。
[0080]
 この場合、制御装置1は、複数の関節を有する当該駆動機構に接続され、駆動機構の状態や環境を示すシステム情報を取得する。制御装置1は、駆動機構の関節などの所定部位の制御目標を取得し、当該所定部位の制御目標への移動に寄与する寄与度を示す情報を算出する。そして制御装置1は、その寄与度を示す情報に基づいて制御指示情報を算出し、制御指示情報に基づいて駆動機構を制御する。
[0081]
 図13は制御装置の最小構成を示す図である。
 図14は最小構成の制御装置による処理フローを示す図である。
 複数の関節を有するロボットアーム2に接続された制御装置1は少なくとも、制御目標取得部10と、関節寄与情報算出部11と、制御指示部12と、システム情報取得部13との機能を発揮する。
 システム情報取得部13は、ロボットアーム2の姿勢状態またはロボットアーム2の駆動状態を少なくとも示すシステム情報を取得する(ステップS301)。
 制御目標取得部10は、ロボットアーム2の制御目標を示す制御目標情報を取得する(ステップS302)。
 関節寄与情報算出部11は、制御目標情報に基づくロボットアーム2の駆動に対する関節の寄与を示す指標である寄与情報をシステム情報に基づいて算出する(ステップS303)。
 制御指示部12は、寄与情報に基づいて各関節の駆動の制御に用いる制御指示情報を算出する(ステップS304)。
[0082]
 上述の制御装置1は内部に、コンピュータシステムを有している。そして、上述した各処理の過程は、プログラムの形式でコンピュータ読み取り可能な記録媒体に記憶されており、このプログラムをコンピュータが読み出して実行することによって、上記処理が行われる。ここでコンピュータ読み取り可能な記録媒体とは、磁気ディスク、光磁気ディスク、半導体メモリ等をいう。また、このコンピュータプログラムを通信回線によってコンピュータに配信し、この配信を受けたコンピュータが当該プログラムを実行するようにしても良い。
[0083]
 また、上記プログラムは、前述した機能の一部を実現するためのものであっても良い。さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル(差分プログラム)であっても良い。
[0084]
 以上、実施形態(及び実施例)を参照して本願発明を説明したが、本願発明は上記実施形態(及び実施例)に限定されものではない。本願発明の構成や詳細には、本願発明のスコープ内で当業者が理解し得る様々な変更をすることができる。

符号の説明

[0085]
1・・・制御装置
10・・・制御目標取得部
11・・・関節寄与情報算出部
12・・・制御指示部
13・・・システム情報取得部
14・・・状態算出部
15・・・障害検知部
2・・・ロボットアーム
21・・・リンク
22・・・関節機構

請求の範囲

[請求項1]
 複数の関節を有するロボットアームに接続され、
 前記ロボットアームの姿勢状態または前記ロボットアームの駆動状態を少なくとも示すシステム情報を取得するシステム情報取得部と、
 前記ロボットアームの制御目標を示す制御目標情報を取得する制御目標取得部と、
 前記制御目標情報に基づく前記ロボットアームの駆動に対する前記関節の寄与を示す指標である寄与情報を前記システム情報に基づいて算出する関節寄与情報算出部と、
 前記寄与情報に基づいて前記各関節の駆動の制御に用いる制御指示情報を算出する制御指示部と、
 を備える制御装置。
[請求項2]
 前記システム情報は、前記ロボットアームの動きを妨げる障害物の位置情報を含み、
 前記関節寄与情報算出部は、前記障害物の位置情報を用いて前記寄与情報を算出する
 請求項1に記載の制御装置。
[請求項3]
 前記関節寄与情報算出部は、前記ロボットアームの前記姿勢状態に基づいて前記寄与情報を算出する
 ことを特徴とする請求項1または請求項2に記載の制御装置。
[請求項4]
 前記関節寄与情報算出部は、前記ロボットアームの前記駆動状態に基づいて前記寄与情報を算出する
 ことを特徴とする請求項1から請求項3の何れか一項に記載の制御装置。
[請求項5]
 前記関節寄与情報算出部は、障害物の位置情報の変化に基づいて、前記寄与情報を再計算する
 請求項1から請求項4の何れか一項に記載の制御装置。
[請求項6]
 複数の関節を有するロボットアームに接続された制御装置が、
 前記ロボットアームの姿勢状態または前記ロボットアームの駆動状態を少なくとも示すシステム情報を取得し、
 前記ロボットアームの制御目標を示す制御目標情報を取得し、
 前記制御目標情報に基づく前記ロボットアームの駆動に対する前記関節の寄与を示す指標である寄与情報を前記システム情報に基づいて算出し、
 前記寄与情報に基づいて前記各関節の駆動の制御に用いる制御指示情報を算出する
 制御方法。
[請求項7]
 複数の関節を有するロボットアームに接続された制御装置において、
 前記ロボットアームの姿勢状態または前記ロボットアームの駆動状態を少なくとも示すシステム情報を取得するシステム情報取得機能と、
 前記ロボットアームの制御目標を示す制御目標情報を取得する制御目標取得機能と、
 前記制御目標情報に基づく前記ロボットアームの駆動に対する前記関節の寄与を示す指標である寄与情報を前記システム情報に基づいて算出する関節寄与情報算出機能と、
 前記寄与情報に基づいて前記各関節の駆動の制御に用いる制御指示情報を算出する制御指示機能と、
 を実現させるプログラムが記録された記録媒体。

図面

[ 図 1]

[ 図 2]

[ 図 3]

[ 図 4]

[ 図 5]

[ 図 6]

[ 図 7]

[ 図 8]

[ 図 9]

[ 図 10]

[ 図 11]

[ 図 12]

[ 図 13]

[ 図 14]