JP6079293B2

JP6079293B2 - ロボット制御システム、ロボット、教示装置、プログラム及びロボット制御方法

Info

Publication number: JP6079293B2
Application number: JP2013032932A
Authority: JP
Inventors: 喜士山田
Original assignee: Seiko Epson Corp
Current assignee: Seiko Epson Corp
Priority date: 2013-02-22
Filing date: 2013-02-22
Publication date: 2017-02-15
Anticipated expiration: 2033-02-22
Also published as: JP2014161930A

Description

本発明は、ロボット制御システム、ロボット、教示装置、プログラム及びロボット制御方法等に関係する。

工場の製品製造ラインでは、６自由度の産業用ロボットを用いて、組立加工作業や検査計測作業の自動化が行われている。しかし、６自由度の産業用ロボットでは、作業場に置かれた障害物等を回避して、アームを所定の場所に移動させることができない場合もある。

一方で、人間の腕の自由度は７自由度であり、腕を所定の場所に動かすために必要となる６自由度の他に冗長自由度がある。人間の腕にはこの冗長自由度があるため、同じ作業を様々な腕の向きで達成することができる。これにより、人間は楽な姿勢を取りつつ、腕を動かすことができる。

そこで、人間の腕と同じように、７自由度を有する冗長自由度ロボットを産業用ロボットとして用いることが考えられている。これらの冗長自由度ロボットの制御方式に関する発明としては特許文献１に記載される従来技術がある。

特開平６−１４３１７２号公報

冗長自由度ロボットでは、肩とアームの先端との所定の位置関係を実現する姿勢が無数にある。そのため、ロボットのアームが実際にどのような姿勢を取ればよいか特定することが困難であるという問題がある。

本発明の幾つかの態様によれば、冗長自由度を有するロボットのアームの先端を所定の位置に移動させる際に、ロボットの各関節位置を決定することができるロボット制御システム、ロボット、教示装置、プログラム及びロボット制御方法等を提供することができる。

本発明の一態様は、ロボットのアームの関節位置を決定する関節位置決定部と、前記関節位置を決定する際に用いる基準方向を取得する基準方向取得部と、決定された前記ロボットの前記アームの前記関節位置に基づいて、前記アームの関節角を求めて、前記ロボットを制御するロボット制御部と、を含み、前記関節位置決定部は、前記アーム上において第１の位置と第２の位置との間の第３の位置にある関節の前記関節位置を決定する際に、前記第１の位置と前記第２の位置とを結ぶ第１の線分を含む平面が向く方向を前記基準方向によって特定し、前記平面上において前記第１の線分から所与の距離にある位置を前記第３の位置として求めるロボット制御システムに関係する。

本発明の一態様では、まず基準方向取得部が基準方向を取得する。そして、関節位置決定部が、アーム上において第１の位置と第２の位置との間の第３の位置にある関節の関節位置を決定する際に、第１の位置と第２の位置とを結ぶ第１の線分を含む平面が向く方向を基準方向に基づいて特定する。次に、関節位置決定部は、特定した平面上において第１の線分から所与の距離にある位置を第３の位置として求める。そして、ロボット制御部が、決定されたロボットのアームの関節位置に基づいて、関節角を求めて、ロボットを制御する。

よって、冗長自由度を有するロボットのアームを所定の位置に移動させる際に、ロボットの各関節位置を決定することが可能となる。

また、本発明の一態様では、前記関節位置決定部は、前記第１の位置と前記第２の位置と前記第３の位置とを含む前記平面に直交する方向が、前記基準方向に対して平行な方向となるように、前記平面の向く方向を特定してもよい。

これにより、第１の位置と第２の位置と第３の位置とを含む平面が、特定された方向を向くような位置に、第３の位置があると特定すること等が可能になる。

また、本発明の一態様では、前記関節位置決定部は、前記第１の線分の長さと、前記第１の位置と前記第３の位置とを結ぶ第２の線分の長さと、前記第２の位置と前記第３の位置とを結ぶ第３の線分の長さとに基づいて、前記第３の位置と前記第１の線分上の第４の位置とを結ぶ線分であって、前記第１の線分に対して垂直な第４の線分の長さを求め、前記第４の線分の長さに基づいて、前記第３の位置を求めてもよい。

これにより、第１の線分から第３の位置までの距離を求めて、求めた距離に基づいて、第３の位置を求めること等が可能になる。

また、本発明の一態様では、前記関節位置決定部は、前記第４の線分の長さに基づいて、前記第１の位置と前記第４の位置とを結ぶ第５の線分の長さを求め、前記第５の線分の長さに基づいて前記第４の位置を求め、前記第４の位置と前記第４の線分の長さに基づいて、前記第３の位置を求めてもよい。

これにより、第１の線分から第３の位置への方向を特定し、求めた方向に基づいて、第３の位置を求めること等が可能になる。

また、本発明の一態様では、前記関節位置決定部は、前記第３の位置と前記第１の線分上の第４の位置とを結ぶ線分であって、前記第１の線分に対して垂直な第４の線分を表すベクトルと、前記第１の位置と前記第４の位置とを結ぶ第５の線分を表すベクトルとの外積と内積に基づいて、前記第３の位置を求めてもよい。

これにより、二つのベクトルの外積と内積を表す連立方程式を解くことにより、第３の位置を求めること等が可能になる。

また、本発明の一態様では、前記関節位置決定部は、前記第４の線分を表すベクトルと前記第５の線分を表すベクトルとの外積の方向が前記基準方向と平行であることと、前記第４の線分を表すベクトルと前記第５の線分を表すベクトルとの内積がゼロになることに基づいて、前記第３の位置を求めてもよい。

これにより、二つの連立方程式の解を求めることにより、第４の位置から第３の位置へのベクトルを求め、求めた第４の位置から第３の位置へのベクトルに基づいて、第３の位置を特定すること等が可能になる。

また、本発明の一態様では、前記関節位置決定部は、前記基準方向を前記第１の線分周りで所与の角度だけ回転させ、回転後の前記第３の位置を求めてもよい。

これにより、基準方向に基づいて求めた第３の位置を調整したい場合に、基準方向を再度指定せずに、第３の位置を調整すること等が可能になる。

また、本発明の一態様では、前記第２の位置は、前記ロボットの前記アームのエンドポイント位置であり、前記第３の位置は、双腕若しくは単腕ロボットの肘関節位置であってもよい。

これにより、ロボットのハンドの目標移動位置を第２の位置とし、冗長自由度を実現する関節の関節位置を第３の位置として設定することが可能になる。

また、本発明の一態様では、前記第１の位置は、前記ロボットの肩関節が回転しても位置が不変な位置であってもよい。

これにより、ロボットのアームの所望の位置に第１の位置を設定することが可能になる。

また、本発明の一態様では、前記ロボットの前記アームには、肘関節と、肩関節と、所定軸周りで回転する上腕節と、が設けられてもよい。

これにより、冗長自由度ロボットを実現すること等が可能になる。

また、本発明の一態様では、前記関節位置決定部は、前記ロボットの前記アームにおいて、前記第３の位置にある関節以外の関節の位置を逆運動学により求めてもよい。

これにより、全ての関節の位置を求めること等が可能になる。

また、本発明の他の態様では、前記ロボット制御システムを含むロボットに関係する。

また、本発明の他の態様では、ロボットのアームの関節位置を決定する際に用いる基準方向の入力を受け付ける入力受付部と、前記関節位置を決定する関節位置決定部と、前記ロボットを制御するロボット制御部に、前記関節位置決定部により決定された前記関節位置を出力する関節位置出力部と、を含み、前記関節位置決定部は、前記アーム上において第１の位置と第２の位置との間の第３の位置にある関節の前記関節位置を決定する際に、前記第１の位置と前記第２の位置とを結ぶ第１の線分を含む平面が向く方向を前記基準方向によって特定し、前記平面上において前記第１の線分から所与の距離にある位置を前記第３の位置として求めることを特徴とする教示装置に関係する。

これにより、教示装置により決定された関節位置に基づいて、ロボット制御部がロボットの各部を動作させることが可能になる。

また、本発明の他の態様では、上記各部としてコンピューターを機能させるプログラムに関係する。

また、本発明の他の態様では、ロボットのアームの関節位置を決定する際に用いる基準方向を取得する処理を行い、前記アーム上において第１の位置と第２の位置との間の第３の位置にある関節の前記関節位置を決定する際に、前記第１の位置と前記第２の位置とを結ぶ第１の線分を含む平面が向く方向を前記基準方向によって特定する処理を行い、前記平面上において前記第１の線分から所与の距離にある位置を前記第３の位置として求める処理を行い、前記ロボットの前記アームの前記関節位置を決定する処理を行い、決定された前記ロボットの前記アームの前記関節位置に基づいて、前記ロボットを制御する処理を行うロボット制御方法に関係する。

本実施形態のシステム構成例。図２（Ａ）、図２（Ｂ）は、ロボットの一例。本実施形態の冗長自由度ロボットのアームの一例。本実施形態の処理の流れを説明するフローチャート。各リンク長と各点間の距離の説明図。第１の線分から第３の位置までの距離と基準方向の説明図。第３の位置を調整する処理の説明図。基準方向の特定処理の説明図。図９（Ａ）、図９（Ｂ）は、教示装置の構成例。

以下、本実施形態について説明する。まず、本実施形態の概要を説明し、次にシステム構成例について説明する。そして、フローチャート等を用いて本実施形態で行う処理の詳細について説明する。最後に、本実施形態の手法についてまとめる。なお、以下に説明する本実施形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また本実施形態で説明される構成の全てが、本発明の必須構成要件であるとは限らない。

１．概要
工場の製品製造ラインでは、６自由度の産業用ロボットを用いて、組立加工作業や検査計測作業の自動化が行われている。

一方で、人間の腕の自由度は７自由度であり、腕を所定の場所に動かすために必要となる６自由度の他に冗長自由度がある。人間の腕にはこの冗長自由度があるため、例えば壁に手のひらをぴったり付けた状態で、肩を動かさずに肘を動かしたり、机の引き出しの奥や棚の奥などの狭い場所に置かれた物をとる際に、引き出しや棚に当たらないように手を伸ばして物を取ったりすることができる。つまり、人間は冗長自由度を得たがために、同じ作業を様々な腕の向きで達成することができる。

逆に、人間の腕に冗長自由度がなければ、所定の位置に腕を持っていくために、決まった姿勢しか取ることができず、楽な姿勢を取りつつ、腕を動かすことができない。このことは、産業用ロボットにおいても同様である。

そこで本実施形態では、７自由度を有する冗長自由度ロボットを産業用ロボットとして用いる。これにより、人間の腕と同じように、例えば狭い場所でも周囲の障害物を避けながら作業をすること等ができるようになる。

また、ロボットには、構造的にアームを制御できなくなる特異点があることが知られている。具体的に、特異点とは、逆運動学計算の解が得られない状態のことを言う。特異点の例としては、ロボットのアームが伸びきった姿勢や、２つ以上の軸が一直線に並んだ姿勢などがある。

さらに、ロボットの自由度数（関節数）が多くなれば多くなる程、特異点になる姿勢の数も増える。しかし、冗長自由度ロボットは自由度に余裕があるため、この特異点となる姿勢を避けて動作することが容易である。

このように、冗長自由度ロボットは、肩の位置とアームの先端位置を固定した状態でも、その間に設けられた肘関節の位置を移動させることができる。言い換えれば、冗長自由度ロボットでは、肩とアームの先端との所定の位置関係を実現する姿勢が無数にある。そのため、逆運動学方程式の解が一意に定まらず、ロボットのアームが実際にどのような姿勢を取ればよいか特定することが困難であるという問題がある。

このような問題に対して、前述した特許文献１において示される発明では、７自由度マニピュレーターの各関節のうち、いずれか一つの関節を指定して、それ以外の６つの関節を手先運動制御用関節として割り当てる。そして、冗長自由度を決定する際には、マニピュレーター先端の目標位置姿勢と、パラメーター化した先に指定した関節角とを用いて逆運動学方程式を解き、その計算で得られた各関節角度等を用いて評価関数のポテンシャル値を求め評価する。その評価を満足したとき、パラメーター化した先に指定した関節角を決定する。

このように、前述した特許文献１において示される発明では、マニピュレーターのジョイントを制約し、パラメーター化することで擬似的な６自由度マニピュレーターを構成して逆運動学計算を行う。

しかし、この際に、どのジョイントを制約するのが最適かを決定することは困難であり、最適でないジョイントに制約をかけてしまうケースが発生する。これによって、逆運動学方程式の解が求まらないケースや、逆運動学方程式の解が求まったとしても計算量が増大してしまうケースが発生する。さらに、最適なジョイント制約をかけることができたとしても、そのパラメーター化を最適に行うことも困難であり、最適でないジョイントに制約をかけてしまうケースと同様の課題が発生する。

そこで、本実施形態では、冗長自由度を有するロボットのアームを所定の位置に移動させる際に、ロボットの各関節位置を決定することができるロボット制御システム、ロボット、教示装置、プログラム及びロボット制御方法等を提供する。

また、冗長自由度ロボットにおける逆運動学方程式の解を一意に定めるためには、制約条件を与える必要があるが、本実施形態では、ユーザーが指定しやすい方法で制約条件を与える。

さらに、本実施形態では、制約条件を与えた際に、逆運動学方程式の解が求まらないケースや、逆運動学方程式の解が求まったとしても計算量が増大してしまうケース等を回避することができる。

２．システム構成例
次に、図１に本実施形態のロボット制御システム１００の構成例を示す。ロボット制御システム１００は、基準方向取得部１１０と、関節位置決定部１３０と、ロボット制御部１５０と、を含む。なお、ロボット制御システム１００は、図１の構成に限定されず、これらの一部の構成要素を省略したり、他の構成要素を追加したりするなどの種々の変形実施が可能である。

次に各部で行われる処理について説明する。

まず、基準方向取得部１１０は、関節位置を決定する際に用いる基準方向を取得する。例えば、基準方向取得部１１０は、ユーザーからの入力を受け付けるＩ／Ｆ部や通信部であってもよいし、あらかじめ設定された基準方向を記憶する記憶部であってもよい。

次に、関節位置決定部１３０は、ロボット２００のアーム２１０の関節位置を決定する。関節位置決定部１３０の詳細な動作については、後述する。

そして、ロボット制御部１５０は、決定されたロボット２００のアーム２１０の関節位置に基づいて、関節角を求めて、ロボット２００を制御する。

なお、関節位置決定部１３０及びロボット制御部１５０の機能は、各種プロセッサー（ＣＰＵ等）、ＡＳＩＣ（ゲートアレイ等）などのハードウェアや、プログラムなどにより実現できる。

また、ロボット２００は、例えば後述する図２（Ａ）及び図２（Ｂ）に示すようなアーム２１０及びハンド２２０を有し、ロボット制御部１５０から取得されるロボット制御コマンドに従って動作する。

ここで、アーム２１０とは、ロボット２００のパーツであって、一つ以上の関節を含む可動パーツのことをいう。

また、アーム２１０のエンドポイントとは、アーム２１０の先端部分のポイントであって、ロボット２００のハンド２２０以外の他の部分と接続されていない部分のことをいう。

さらに、ハンド（把持部）２２０とは、ワークを把持したり、持ち上げたり、吊り上げたり、吸着させたり、ワークに加工を施したりするためにアーム２１０のエンドポイントに取り付ける部品のことをいう。ハンド２２０は、フックであってもよいし、吸盤等であってもよい。なお、アームのエンドポイントの位置は、ハンドの位置としてもよい。さらに、１本のアームに対して、複数のハンドを設けても良い。

例えば図２（Ａ）のロボットは、ロボット本体２００（ロボット）とロボット制御システム１００とが別体に構成されている。しかし、本実施形態のロボットは図２（Ａ）の構成に限定されず、図２（Ｂ）のようにロボット本体２００とロボット制御システム１００とが一体に構成されていてもよい。具体的には図２（Ｂ）に示したように、ロボット２００は、ロボット本体（アーム２１０及びハンド２２０を有する）及びロボット本体を支えるベースユニット部を有し、当該ベースユニット部にロボット制御システム１００が格納されるものであってもよい。図２（Ｂ）のロボット２００には、ベースユニット部に車輪等が設けられ、ロボット全体が移動可能な構成となっている。なお、図２（Ａ）は単腕型の例であるが、ロボット２００は図２（Ｂ）に示すように双腕型等の多腕型のロボットであってもよい。また、ロボット２００は、人手により移動させられるものであってもよいし、車輪を駆動するモーターを設け、当該モーターをロボット制御システム１００により制御することにより、移動させられるものであってもよい。また、ロボット制御システム１００は、図２（Ｂ）のようにロボット２００の下に設けられたベースユニット部に設けられるとは限られない。

さらに、本実施形態で用いる７自由度のアーム一本の一例を図３に示す。

まず、アームの付け根位置ＰＳにある接地面ＳＡに肩部ＳＨが接続されており、肩部ＳＨには第１の関節部（第１の肩関節）ＪＯ１が設けられている。

そして、第１の関節部ＪＯ１から伸びるリンクは、第１の位置Ｐ１で直角に曲がり、第２の関節部（第２の肩関節）ＪＯ２に接続されている。

さらに、第２の関節部ＪＯ２から伸びるリンクも直角に曲がり、第３の関節部（上腕節）ＪＯ３に接続されている。そして、第３の関節部ＪＯ３から伸びるリンクは第４の関節部（肘関節）ＪＯ４に接続されている。以下では、第１の位置Ｐ１から第４の関節部ＪＯ４までを上腕節ＵＡと呼ぶ。

また、リンクにより、第４の関節部ＪＯ４は第５の関節部ＪＯ５に接続されており、第５の関節部ＪＯ５は第６の関節部ＪＯ６に接続されている。以下では、第３の位置Ｐ３から第２の位置Ｐ２までを前腕部ＢＡと呼ぶ。

そして、リンクにより、第６の関節部ＪＯ６は第７の関節部ＪＯ７に接続されており、第７の関節部ＪＯ７はハンドＨＤに接続されている。また、ベクトルＴＰは、ロボットのハンドの目標位置姿勢を表している。なお、第６の関節部からハンドＨＤまでの機構と第５の関節部は、以下の説明に不要なため、図５〜図８では記載を省略する。

３．処理の詳細
まず、図４のフローチャートを用いて、図３に示すロボットアームを制御する処理の全体の流れについて説明する。そして、その後に図５〜図８を用いて、処理の詳細について説明する。

まず、ロボット制御システムは、ロボットのハンドＨＤの目標位置姿勢ＴＰを取得する（Ｓ１０１）。

次に、ロボットのアームの第２の位置Ｐ２を算出する（Ｓ１０２）。なお、第２の位置Ｐ２は、第１の関節部ＪＯ１〜第４の関節部ＪＯ４の関節角によって一意に定まり、第５の関節部ＪＯ５〜第７の関節部ＪＯ７の関節角及びハンドＨＤの動きによっては変化しない。よって、前述したように、以下では、第５の関節部ＪＯ５〜第７の関節部ＪＯ７の関節角及びハンドＨＤの動きは考慮しない。

そして、基準方向取得部１１０が基準方向を取得する（Ｓ１０３）。例えば、ユーザーにより入力された方向を基準方向として取得する。また、他にも、第１の位置Ｐ１と第２の位置Ｐ２とアームの付け根位置ＰＳとを頂点とする三角形に直交するベクトルＳＤの方向を基準方向として計算する等してもよい（Ｓ１０３）。その場合の処理については、後に図８を用いて詳述する。

次に、第１の位置Ｐ１と第２の位置Ｐ２と第３の位置Ｐ３とを頂点とする三角形が、ベクトルＳＤに直交するように、関節位置決定部１３０が第３の位置Ｐ３の位置を決定する（Ｓ１０４）。なお、本処理については、後に図５及び図６を用いて詳述する。

さらに、必要に応じて、関節位置決定部１３０が、第１の位置Ｐ１と第４の位置Ｐ４とを結ぶ線分を回転軸として、第３の位置Ｐ３を回転移動させ、第３の位置Ｐ３の微調整を行う（Ｓ１０５）。なお、本処理については、後に図７を用いて詳述する。

そして、ロボット制御部１５０が、求めたアームの付け根位置ＰＳと第１の位置Ｐ１と第２の位置Ｐ２と第３の位置ＰＳ３との位置関係を実現する時の、第１の関節部ＪＯ１〜第４の関節部ＪＯ４の目標回転角度を計算する（Ｓ１０６）。同様に、ロボット制御部１５０が、ロボットのハンドＨＤの目標位置姿勢ＴＰ及び第２の位置Ｐ２を実現する時の第５の関節部ＪＯ５〜第７の関節部ＪＯ７の目標回転角度を計算する（Ｓ１０７）。

最後に、ロボット２００が、第１の関節部ＪＯ１〜第７の関節部ＪＯ７について、現在の回転角度から目標回転角度へと変化させ、ロボットのハンドＨＤの目標位置姿勢ＴＰを実現する（Ｓ１０８）。以上で、処理を終了する。

次に、ステップＳ１０４の処理の詳細を説明する。本ステップでは、図５に太線で示すように、第１の位置Ｐ１と第２の位置Ｐ２と第３の位置Ｐ３とを頂点とする三角形を考える。この時、第１の位置Ｐ１と第２の位置Ｐ２は、確定済みであるが、第３の位置Ｐ３は未確定であり、本ステップでは最終的にこの第３の位置Ｐ３を決定する。

まず、第１の位置Ｐ１と第２の位置Ｐ２を結ぶ第１の線分から第３の位置Ｐ３までの距離を求める。そのために、第１の位置Ｐ１と第２の位置Ｐ２と第３の位置Ｐ３とを頂点とする三角形の各辺の長さを求める。ここで、図５に示す各リンク長（ｌ_０〜ｌ_６）は既知である。よって、三平方の定理に基づいて、リンク長ｌ_２〜ｌ_６から第１の線分の長さＬ_ａが求められ、リンク長ｌ_２〜ｌ_５から、第１の位置Ｐ１と第３の位置Ｐ３を結ぶ第２の線分の長さＬ_ｂが求められる。また、第２の位置Ｐ２と第３の位置Ｐ３を結ぶ第３の線分の長さＬ_ｃは、リンク長ｌ_６と等しい。

次に、図６に示すように、第３の位置Ｐ３から第１の線分に下ろした垂線と第１の線分との交点を第４の位置Ｐ４とし、第３の位置Ｐ３と第４の位置Ｐ４を結ぶ第４の線分の長さをＬ_ｄとする。すなわち、第４の線分の長さＬ_ｄは、第１の線分から第３の位置Ｐ３までの距離である。この時、第４の線分の長さＬ_ｄは、ヘロンの公式を変形した下式（１）により求められる。なお、式（１）中の定数ｓは、下式（２）により求められる値である。さらに、第１の位置Ｐ１と第４の位置Ｐ４を結ぶ第５の線分の長さをＬ_ｅとする。

ここまでで、第１の線分から第３の位置Ｐ３までの距離Ｌ_ｄを求めることができた。従って、第３の位置Ｐ３が第４の位置Ｐ４から見てどの方向に位置するかが分かれば、第３の位置Ｐ３を一意に特定することができる。言い換えれば、第４の位置Ｐ４から第３の位置Ｐ３へのベクトル（第４の線分を表すベクトル）を求めることができればよい。

そこで、図６に示すＢＳ１〜ＢＳ３を基底とするベクトル空間上のベクトルＳＤによって表される基準方向を用いて、第４の位置Ｐ４から第３の位置Ｐ３へのベクトルを求め、第３の位置Ｐ３を特定する。この基準方向は、前述したように、ステップＳ１０３において取得される。図６に示す基準方向を表すベクトルＳＤは、基底ＢＳ１に対する成分がαで、基底ＢＳ２に対する成分がβで、基底ＢＳ３に対する成分がγであるベクトルである。

この時、基準方向を表すベクトルＳＤは、第１の位置Ｐ１と第２の位置Ｐ２と第３の位置Ｐ３とを頂点とする三角形の面と直交するため、第４の線分を表すベクトルと第５の線分を表すベクトル（第１の位置Ｐ１から第４の位置Ｐ４へのベクトル）との外積の方向は、基準方向と平行になる。そのため、下式（３）の方程式が成り立つ。

また、第４の線分を表すベクトルと第５の線分を表すベクトルは直交しているため、この二つのベクトルの内積はゼロになり、下式（４）の方程式が成り立つ。

そして、この方程式（３）と方程式（４）を解いて、下式（５）に示す第４の線分を表すベクトルと、下式（６）に示す第５の線分を表すベクトルを求める。ここで、第４の線分を表すベクトルは、基底ＢＳ１に対する成分がΔｘ_ｄで、基底ＢＳ２に対する成分がΔｙ_ｄで、基底ＢＳ３に対する成分がΔｚ_ｄであるベクトルとして求められる。同様に、第５の線分を表すベクトルは、基底ＢＳ１に対する成分がΔｘ_ｅで、基底ＢＳ２に対する成分がΔｙ_ｅで、基底ＢＳ３に対する成分がΔｚ_ｅであるベクトルとして求められる。

最後に、下式（７）により、第３の位置Ｐ３を表すベクトルを求める。また、式（７）中で用いるアームの付け根位置ＰＳから第１の位置Ｐ１へのベクトルは下式（８）により表される。このアームの付け根位置ＰＳから第１の位置Ｐ１へのベクトルは、アームの付け根位置を表すベクトルと、既知なリンク長ｌ_０及びｌ_１に基づいて求められる。

以上がステップＳ１０４で行う具体的な処理である。

４．本実施形態の手法
次に、本実施形態の手法について説明する。

以上の本実施形態のロボット制御システム１００では、ロボット２００のアーム２１０の関節位置を決定する関節位置決定部１３０と、関節位置を決定する際に用いる基準方向を取得する基準方向取得部１１０と、決定されたロボット２００のアーム２１０の関節位置に基づいて、アームの関節角を求めて、ロボット２００を制御するロボット制御部１５０と、を含む。そして、関節位置決定部１３０は、アーム２１０上において第１の位置と第２の位置との間の第３の位置にある関節の関節位置を決定する際に、第１の位置と第２の位置とを結ぶ第１の線分を含む平面が向く方向を基準方向によって特定し、平面上において第１の線分から所与の距離にある位置を第３の位置として求める。

本実施形態では、まず基準方向取得部１１０が基準方向を取得する。

ここで、基準方向とは、関節位置決定部１３０が関節位置を決定する際に用いる基準となる方向のことをいう。基準方向は、例えばユーザーにより入力されるが、必ずしもユーザーにより入力されるとは限らず、あらかじめ決められた方向であってもよいし、例えば後述する方法によって算出する等してもよい。

そして、関節位置決定部１３０が、アーム２１０上において第１の位置と第２の位置との間の第３の位置にある関節の関節位置を決定する際に、第１の位置と第２の位置とを結ぶ第１の線分を含む平面が向く方向を基準方向に基づいて特定する。

ここで、第１の位置は、ロボット２００の肩関節が回転しても位置が不変な位置であってもよい。具体的に図３に示す冗長自由度ロボットのアームにおいては、第１の位置Ｐ１は、肩部ＳＨ上のいずれの位置であってもよい。また、肩部ＳＨから所定距離だけ離れた位置を第１の位置Ｐ１と設定してもよい。

また、第２の位置は、ロボット２００のアーム２１０のエンドポイント位置であってもよい。

さらに、第３の位置は、双腕若しくは単腕ロボットの肘関節位置であってもよい。具体的に図３に示す冗長自由度ロボットのアームにおいては、第３の位置Ｐ３は、上腕節ＵＡ上のいずれの位置であってもよい。

これにより、ロボットのハンドの目標移動位置を第２の位置とし、冗長自由度を実現する関節の関節位置を第３の位置として設定することが可能になる。その結果、後述する方法により、冗長自由度を実現する関節の関節位置を特定することが可能になる。

また、第１の線分とは、第１の位置と第２の位置とを結ぶ線分のことを指す。

さらに、第１の線分を含む平面とは、例えば図５では、第１の位置Ｐ１と第２の位置Ｐ２と第３の位置Ｐ３とを頂点とする三角平面などのことを指す。

次に、関節位置決定部１３０は、特定した平面上において第１の線分から所与の距離にある位置を第３の位置として求め、第３の位置以外の関節位置も求める。

そして、ロボット制御部１５０が、決定されたロボット２００のアーム２１０の関節位置に基づいて、関節角を求めて、ロボット２００を制御する。

これにより、冗長自由度を有するロボットのアームを所定の位置に移動させる際に、ロボットの各関節位置を決定することが可能となる。

また、前述したように、冗長自由度ロボットにおける逆運動学方程式の解を一意に定めるためには、何らかの制約条件を与える必要がある。しかし、例えば任意の関節部の回転角度を制約条件として指定すること等は、ユーザーにとって困難である。これに対して、本実施形態では、ユーザーが指定しやすい基準方向により制約条件を入力することも可能であるため、制約条件の入力をユーザーが容易に行えるようにすること等が可能になる。

また、関節位置決定部１３０は、第１の位置と第２の位置と第３の位置とを含む平面に直交する方向が、基準方向に対して平行な方向となるように、平面の向く方向を特定してもよい。

また、関節位置決定部１３０は、第１の線分の長さと、第１の位置と第３の位置とを結ぶ第２の線分の長さと、第２の位置と第３の位置とを結ぶ第３の線分の長さとに基づいて、第３の位置と第１の線分上の第４の位置とを結ぶ線分であって、第１の線分に対して垂直な第４の線分の長さを求め、第４の線分の長さに基づいて、第３の位置を求めてもよい。

なお、言い換えれば、第４の線分の長さは、第１の位置と第２の位置と第３の位置とを頂点とする三角形において、第１の線分を底辺とした時の高さである。

また、関節位置決定部１３０は、第４の線分の長さに基づいて、第１の位置と第４の位置とを結ぶ第５の線分の長さを求め、第５の線分の長さに基づいて第４の位置を求め、第４の位置と第４の線分の長さに基づいて、第３の位置を求めてもよい。

以上により、第１の線分から第３の位置までの距離と、第１の線分から第３の位置への方向（始点である第４の位置を含む）を特定できたため、第３の位置を特定すること等が可能になる。

また、このことはベクトルの外積と内積を用いて言い替えることができる。

すなわち、関節位置決定部１３０は、第３の位置と第１の線分上の第４の位置とを結ぶ線分であって、第１の線分に対して垂直な第４の線分を表すベクトルと、第１の位置と第４の位置とを結ぶ第５の線分を表すベクトルとの外積と内積に基づいて、第３の位置を求めてもよい。

また、関節位置決定部１３０は、第４の線分を表すベクトルと第５の線分を表すベクトルとの外積の方向が基準方向と平行であることと、第４の線分を表すベクトルと第５の線分を表すベクトルとの内積がゼロになることに基づいて、第３の位置を求めてもよい。

ここで、前述したように式（３）により、第４の線分を表すベクトルと第５の線分を表すベクトルとの外積の方向が基準方向と平行であることが表される。

同様に、式（４）により、第４の線分を表すベクトルと第５の線分を表すベクトルとの内積がゼロになることが表される。

次に、図４のフローチャートにおけるステップＳ１０５の処理の詳細を説明する。

例えば、基準方向がユーザーにより入力されるものである場合には、前述してきた処理によって第３の位置Ｐ３を特定した後に、第３の位置Ｐ３を調整（微調整）する必要が生じる場合がある。

そこで、関節位置決定部１３０は、基準方向を第１の線分周りで所与の角度だけ回転させ、回転後の第３の位置を求めてもよい。この処理の様子を図７に示す。

ここで、所与の角度φは、例えばユーザーにより入力される角度である。その場合、所与の角度φは、第３の位置Ｐ３を一度決定した後に入力するものであるため、ユーザーにとってもどのような値を入力すればよいか判断が容易である。

そして、所与の角度φを取得した後は、下式（９）〜下式（１１）に基づいて、新たな第３の位置Ｐ３を計算する。

まず、式（９）に示すように、第４の線分を表すベクトルの単位ベクトルを計算する。

次に、式（９）により計算した第４の線分を表すベクトルの単位ベクトルに基づいて、第１の線分（第４の線分）周りで所与の角度φだけ回転させた時の回転ベクトルを式（１０）により求める。

最後に、求めた回転ベクトルを用いて、式（１１）により、図７に示すような調整後の第３の位置Ｐ３’を求める。以上が、ステップＳ１０５の処理の詳細である。

次に、図４のフローチャートにおけるステップＳ１０３の処理の詳細を説明する。

本処理では、図８に太線で示すように、アームの付け根位置ＰＳと第１の位置Ｐ１と第２の位置Ｐ２とを頂点とする三角形を考える。そして、この三角形に直交するベクトルＳＤを求めて、このベクトルＳＤが表す方向を基準方向として求める。

具体的には、アームの付け根位置ＰＳから第１の位置Ｐ１へのベクトルと、第１の位置Ｐ１から第４の位置Ｐ４へのベクトルとの外積を下式（１２）により求める。

そして、求めた外積の単位ベクトルを下式（１３）により求めて、このベクトルが表す方向を基準方向とする。以上が、ステップＳ１０３の処理の詳細である。

また、ロボット２００のアーム２１０には、肘関節と、肩関節と、所定軸周りで回転する上腕節と、が設けられてもよい。

また、関節位置決定部１３０は、ロボット２００のアーム２１０において、第３の位置にある関節以外の関節の位置を逆運動学により求めてもよい。

また、本実施形態の教示装置３００は、ロボット２００のアーム２１０の関節位置を決定する際に用いる基準方向の入力を受け付ける入力受付部３１０と、関節位置を決定する関節位置決定部１３０と、ロボット２００を制御するロボット制御部１５０に、関節位置決定部１３０により決定された関節位置を出力する関節位置出力部３４０と、を含む。

教示装置３００の具体的な構成例を図９（Ａ）及び図９（Ｂ）に示す。図９（Ａ）に示すように、教示装置３００は、例えばＰＣであって、有線又は無線によりロボット制御システム１００を含むロボット２００と接続されている。

そして、図９（Ｂ）に示すように、教示装置３００は、入力受付部３１０と、関節位置決定部１３０と、記憶部３３０と、関節位置出力部３４０とを含む。なお、関節位置決定部１３０の動作及び構成は、前述した通りであるため、記載を省略する。

入力受付部３１０（基準方向取得部１１０）は、ユーザーからの入力として基準方向を受け付けるＩ／Ｆ（インターフェース）部や通信部である。より具体的には、入力受付部３１０は、キーボードやタッチパネル等である。

また、記憶部３３０は、関節位置決定部１３０のワーク領域となるものであり、その機能はＲＡＭ等のメモリーやＨＤＤ（ハードディスクドライブ）などにより実現できる。

そして、関節位置出力部３４０は、決定された関節位置をロボット制御部１５０に出力する。関節位置出力部３４０は、例えば通信部であり、有線又は無線の少なくとも一方を含むネットワークを介してロボット制御システム１００と通信を行う。

図９（Ｂ）の例では、ロボット制御システム１００は、前述したロボット制御部１５０を含む。

なお、教示装置３００は、図９（Ｂ）の構成に限定されず、これらの一部の構成要素を省略したり、他の構成要素を追加したりするなどの種々の変形実施が可能である。

これにより、教示装置３００により決定された関節位置に基づいて、ロボット制御部１５０がロボット２００の各部を動作させることが可能になる。

なお、本実施形態のロボット制御システム１００等は、その処理の一部または大部分をプログラムにより実現してもよい。この場合には、ＣＰＵ等のプロセッサーがプログラムを実行することで、本実施形態のロボット制御システム１００等が実現される。具体的には、情報記憶媒体に記憶されたプログラムが読み出され、読み出されたプログラムをＣＰＵ等のプロセッサーが実行する。ここで、情報記憶媒体（コンピューターにより読み取り可能な媒体）は、プログラムやデータなどを格納するものであり、その機能は、光ディスク（ＤＶＤ、ＣＤ等）、ＨＤＤ（ハードディスクドライブ）、或いはメモリー（カード型メモリー、ＲＯＭ等）などにより実現できる。そして、ＣＰＵ等のプロセッサーは、情報記憶媒体に格納されるプログラム（データ）に基づいて本実施形態の種々の処理を行う。即ち、情報記憶媒体には、本実施形態の各部としてコンピューター（操作部、処理部、記憶部、出力部を備える装置）を機能させるためのプログラム（各部の処理をコンピューターに実行させるためのプログラム）が記憶される。

以上のように本実施形態について詳細に説明したが、本発明の新規事項および効果から実体的に逸脱しない多くの変形が可能であることは当業者には容易に理解できるであろう。従って、このような変形例はすべて本発明の範囲に含まれるものとする。例えば、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。また、ロボット制御システム、ロボット、教示装置及びプログラムの構成、動作も本実施形態で説明したものに限定されず、種々の変形実施が可能である。

１００ロボット制御システム、１１０基準方向取得部、１３０関節位置決定部、
１５０ロボット制御部、２００ロボット（ロボット本体）、２１０アーム、
２２０ハンド、３００教示装置、３１０入力受付部、３３０記憶部、
３４０関節位置出力部

Claims

ロボットのアームの関節位置を決定する関節位置決定部と、
前記関節位置を決定する際に用いる基準方向を取得する基準方向取得部と、
決定された前記ロボットの前記アームの前記関節位置に基づいて、前記アームの関節角を求めて、前記ロボットを制御するロボット制御部と、
を含み、
前記関節位置決定部は、
前記アーム上において第１の位置と第２の位置との間の第３の位置にある関節の前記関節位置を決定する際に、前記第１の位置と前記第２の位置とを結ぶ第１の線分を含む平面が向く方向を前記基準方向によって特定し、前記平面上において前記第１の線分から所与の距離にある位置を前記第３の位置として求めることを特徴とするロボット制御システム。
請求項１において、
前記関節位置決定部は、
前記第１の位置と前記第２の位置と前記第３の位置とを含む前記平面に直交する方向が、前記基準方向に対して平行な方向となるように、前記平面の向く方向を特定することを特徴とするロボット制御システム。
請求項１又は２において、
前記関節位置決定部は、
前記第１の線分の長さと、前記第１の位置と前記第３の位置とを結ぶ第２の線分の長さと、前記第２の位置と前記第３の位置とを結ぶ第３の線分の長さとに基づいて、前記第３の位置と前記第１の線分上の第４の位置とを結ぶ線分であって、前記第１の線分に対して垂直な第４の線分の長さを求め、前記第４の線分の長さに基づいて、前記第３の位置を求めることを特徴とするロボット制御システム。
請求項３において、
前記関節位置決定部は、
前記第４の線分の長さに基づいて、前記第１の位置と前記第４の位置とを結ぶ第５の線分の長さを求め、前記第５の線分の長さに基づいて前記第４の位置を求め、前記第４の位置と前記第４の線分の長さに基づいて、前記第３の位置を求めることを特徴とするロボット制御システム。
請求項１又は２において、
前記関節位置決定部は、
前記第３の位置と前記第１の線分上の第４の位置とを結ぶ線分であって、前記第１の線分に対して垂直な第４の線分を表すベクトルと、前記第１の位置と前記第４の位置とを結ぶ第５の線分を表すベクトルとの外積と内積に基づいて、前記第３の位置を求めることを特徴とするロボット制御システム。
請求項５において、
前記関節位置決定部は、
前記第４の線分を表すベクトルと前記第５の線分を表すベクトルとの外積の方向が前記基準方向と平行であることと、前記第４の線分を表すベクトルと前記第５の線分を表すベクトルとの内積がゼロになることに基づいて、前記第３の位置を求めることを特徴とするロボット制御システム。
請求項１乃至６のいずれかにおいて、
前記関節位置決定部は、
前記基準方向を前記第１の線分周りで所与の角度だけ回転させ、回転後の前記第３の位置を求めることを特徴とするロボット制御システム。
請求項１乃至７のいずれかにおいて、
前記第２の位置は、
前記ロボットの前記アームのエンドポイント位置であり、
前記第３の位置は、
双腕若しくは単腕ロボットの肘関節位置であることを特徴とするロボット制御システム。
請求項１乃至８のいずれかにおいて、
前記第１の位置は、
前記ロボットの肩関節が回転しても位置が不変な位置であることを特徴とするロボット制御システム。
請求項１乃至９のいずれかにおいて、
前記ロボットの前記アームには、
肘関節と、肩関節と、所定軸周りで回転する上腕節と、が設けられることを特徴とするロボット制御システム。
請求項１乃至１０のいずれかにおいて、
前記関節位置決定部は、
前記ロボットの前記アームにおいて、前記第３の位置にある関節以外の関節の位置を逆運動学により求めることを特徴とするロボット制御システム。
請求項１乃至１１のいずれかに記載のロボット制御システムを含むことを特徴とするロボット。
ロボットのアームの関節位置を決定する際に用いる基準方向の入力を受け付ける入力受付部と、
前記関節位置を決定する関節位置決定部と、
前記ロボットを制御するロボット制御部に、前記関節位置決定部により決定された前記関節位置を出力する関節位置出力部と、
を含み、
前記関節位置決定部は、
前記アーム上において第１の位置と第２の位置との間の第３の位置にある関節の前記関節位置を決定する際に、前記第１の位置と前記第２の位置とを結ぶ第１の線分を含む平面が向く方向を前記基準方向によって特定し、前記平面上において前記第１の線分から所与の距離にある位置を前記第３の位置として求めることを特徴とする教示装置。
ロボットのアームの関節位置を決定する関節位置決定部と、
前記関節位置を決定する際に用いる基準方向を取得する基準方向取得部と、
決定された前記ロボットの前記アームの前記関節位置に基づいて、前記アームの関節角を求めて、前記ロボットを制御するロボット制御部として、
コンピューターを機能させ、
前記関節位置決定部は、
前記アーム上において第１の位置と第２の位置との間の第３の位置にある関節の前記関節位置を決定する際に、前記第１の位置と前記第２の位置とを結ぶ第１の線分を含む平面が向く方向を前記基準方向によって特定し、前記平面上において前記第１の線分から所与の距離にある位置を前記第３の位置として求めることを特徴とするプログラム。
ロボットのアームの関節位置を決定する際に用いる基準方向を取得する処理を行い、
前記アーム上において第１の位置と第２の位置との間の第３の位置にある関節の前記関節位置を決定する際に、前記第１の位置と前記第２の位置とを結ぶ第１の線分を含む平面が向く方向を前記基準方向によって特定する処理を行い、
前記平面上において前記第１の線分から所与の距離にある位置を前記第３の位置として求める処理を行い、
前記ロボットの前記アームの前記関節位置を決定する処理を行い、
決定された前記ロボットの前記アームの前記関節位置に基づいて、前記ロボットを制御する処理を行うことを特徴とするロボット制御方法。