JP2024035061A - 多関節ロボットの制御方法、ロボットシステム、プログラム、及び、物品の製造方法 - Google Patents

Abstract

【課題】逆運動学計算の結果がエラーとなる頻度を低減する。【解決手段】ロボット１０の制御方法は、７個以上の複数の関節を有する多関節ロボットの制御方法であり、ロボット１０を動作させる複数の関節機構ＪＥの変位量を計算する逆運動学計算を実行することにより、複数の関節機構ＪＥの各々の状態に関する関節値を算出する状態算出処理と、複数の関節機構ＪＥのうち、関節値に基づく状態が、あらかじめ定められた特定状態である関節機構ＪＥに対して実行する状態固定処理と、を含む単位処理を繰り返し実行し、状態固定処理を実行した場合、状態算出処理において算出された複数の関節機構ＪＥの関節値のうち、特定状態の関節機構ＪＥの関節値は実質的に変位することなく、特定状態でない関節機構ＪＥの関節値を変位させる。【選択図】図３

Description

特許法第３０条第２項適用申請有り １：令和４年１０月２４日 ローレルバンクマシン株式会社のウェブサイト ｈｔｔｐｓ：／／ｗｗｗ．ｌｂｍ．ｃｏ．ｊｐ／ｎｅｗｓ／２０２２／１０２４／ ２：令和４年１０月３１日 アペルザＴＶのウェブサイト ｈｔｔｐｓ：／／ｔｖ．ａｐｅｒｚａ．ｃｏｍ／ｗａｔｃｈ／８２９ ３：令和４年１０月３１日 ＰＲ ＴＩＭＥＳのウェブサイト ｈｔｔｐｓ：／／ｐｒｔｉｍｅｓ．ｊｐ／ｍａｉｎ／ｈｔｍｌ／ｒｄ／ｐ／００００００００４．０００１０５９４９．ｈｔｍｌ ４：令和４年１０月３１日 ＠Ｐｒｅｓｓのウェブサイト ｈｔｔｐｓ：／／ｗｗｗ．ａｔｐｒｅｓｓ．ｎｅ．ｊｐ／ｎｅｗｓ／３３２６３７ ５：令和４年１１月８日～１３日 ＪＩＭＴＯＦ２０２２（第３１回日本国際工作機械見本市） 東京ビッグサイト（東京都江東区有明３－１１－１） ６：令和４年１１月２５日 アペルザカタログのウェブサイト ｈｔｔｐｓ：／／ｒｄ．ｔｒ．ａｐｅｒｚａ．ｃｏｍ／ｖ１／ｔｍｃ？ａ＝ｎｔｘＯ７ｕＰｎｋｙｋ９ｈＹｎＣＵＶＱＩｎＡＣａＥ５ｍ７ＭＰＶ％２Ｆ３ｚｄＡＳＣＪｙ１ＴＡｏ６ＬｑＹｌｍｔＤｎＵＭＢＴ９ＴｅＴｂＰＵｘｈｚＺＭＬ８ＫＫ０４３ＮｍａｊＬｊｖｈ３３ＣＨＢ４ｋＱＨｋ４ＺｇＷｓＩｚ２ｍＨＵｄｗ％３Ｄ＆ｂ＝＆ｕｒｌ＿ｉｄ＝５＆ｔｅｓｔ＝ｏｎ ７：令和５年４月２７日 ローレルバンクマシン株式会社のウェブサイト ｈｔｔｐｓ：／／ｗｗｗ．ｌｂｍ．ｃｏ．ｊｐ／ｎｅｗｓ／２０２３／０４２６／ ８：令和５年４月２７日 ＰＲ ＴＩＭＥＳのウェブサイト ｈｔｔｐｓ：／／ｐｒｔｉｍｅｓ．ｊｐ／ｍａｉｎ／ｈｔｍｌ／ｒｄ／ｐ／００００００００６．０００１０５９４９．ｈｔｍｌ ９：令和５年５月１０日～１２日 未来ものつくり国際ＥＸＰＯ インデックス大阪（大阪市住之江区南港北１－５－１０２）

本発明は、多関節ロボットの制御方法、ロボットシステム、プログラム、及び、物品の製造方法に関する。

人と同様の動作を行うロボットとして、６自由度の多関節ロボットが知られている。また、近年では、６自由度に冗長自由度を加えた７自由度以上の多関節ロボットの研究が行われている。例えば、特許文献１には、７自由度の多関節ロボットの制御方式が開示されている。

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

ところで、一般的な多関節ロボットでは、複数の関節のうちの一部又は全部に可動範囲が定められる。このため、例えば、ロボットの位置及び姿勢から各関節の変位量を求める逆運動学計算では、複数の関節のいずれかが可動範囲を超えた場合、計算結果がエラーとなり、正しい解（各関節を可動範囲内で動作させる解）を求めることができない。従って、逆運動学計算の結果がエラーとなる場合、ロボットに所望の動作を実行させることができない。このため、逆運動学計算の結果がエラーとなる頻度を低減することが望まれている。

本発明の好適な態様に係る多関節ロボットの制御方法は、７個以上の複数の関節を有する多関節ロボットの制御方法であって、前記多関節ロボットを動作させる前記複数の関節の変位量を計算する逆運動学計算を実行することにより、前記複数の関節の各々の状態に関する関節値を算出する算出処理と、前記複数の関節のうち、前記関節値に基づく状態が、あらかじめ定められた特定状態である関節に対して実行する固定処理と、を含む単位処理を繰り返し実行し、前記固定処理を実行した場合、前記算出処理において算出された前記複数の関節の前記関節値のうち、前記特定状態の関節の前記関節値は実質的に変位させることなく、前記特定状態でない関節の前記関節値を変位させる。

本発明の好適な態様に係るロボットシステムは、７個以上の複数の関節を有する多関節ロボットと、前記多関節ロボットの動作を制御する制御装置と、を備え、前記制御装置は、前記多関節ロボットを動作させる前記複数の関節の変位量を計算する逆運動学計算を実行することにより、前記複数の関節の各々の状態に関する関節値を算出する算出処理と、前記複数の関節のうち、前記関節値に基づく状態が、あらかじめ定められた特定状態である関節に対して実行する固定処理と、を含む単位処理を繰り返し実行する動作制御部を有し、前記動作制御部は、前記固定処理を実行した場合、前記算出処理において算出された前記複数の関節の前記関節値のうち、前記特定状態の関節の前記関節値は実質的に変位させることなく、前記特定状態でない関節の前記関節値を変位させる。

本発明の好適な態様に係る物品の製造方法は、上述のロボットシステムにより、部品を組み付ける、又は、部品を取り除く。

本発明の好適な態様に係るプログラムは、７個以上の複数の関節を有する多関節ロボットの動作を制御するプログラムであって、プロセッサを、前記多関節ロボットを動作させる前記複数の関節の変位量を計算する逆運動学計算を実行することにより、前記複数の関節の各々の状態に関する関節値を算出する算出処理と、前記複数の関節のうち、前記関節値に基づく状態が、あらかじめ定められた特定状態である関節に対して実行する固定処理と、を含む単位処理を繰り返し実行する動作制御部として機能させ、前記動作制御部は、前記固定処理を実行した場合、前記算出処理において算出された前記複数の関節の前記関節値のうち、前記特定状態の関節の前記関節値は実質的に変位させることなく、前記特定状態でない関節の前記関節値を変位させる。

本発明によれば、逆運動学計算の結果がエラーとなる頻度を低減することができる。

実施形態に係るロボットシステムの概要を説明するための説明図である。 図１に示したロボットコントローラのハードウェア構成の一例を示す図である。 図１に示したロボットコントローラの動作の一例を示すフローチャートである。 ロボットの手先の位置及び姿勢を複数の目標の位置及び姿勢に順に変化させる場合のロボットコントローラの動作の一例を示すフローチャートである。 図４に示した関節値更新処理の一例を示すフローチャートである。 ロボットの手先の位置及び姿勢を複数の目標の位置及び姿勢に順に変化させる場合のロボットコントローラの動作の別の例を示すフローチャートである。 ロボットの手先の位置及び姿勢を複数の目標の位置及び姿勢に順に変化させる場合のロボットコントローラの動作の別の例を示すフローチャートである。 図７に示した関節値初回更新処理の一例を示すフローチャートである。 シミュレーションに用いられたモデルを説明するための説明図である。 図９に示したシミュレーションモデルの初期関節値の一例を説明するための説明図である。 シミュレーションに用いられた手先移動量を説明するための説明図である。 対比例の制御方法に用いられるヤコビ行列を説明するための説明図である。 図１２に示したヤコビ行列の擬似逆行列を説明するための説明図である。 図１３に示した擬似逆行列を用いて算出された関節移動量を示す図である。 図１４に示した関節移動量に基づいて算出された最終関節値を示す図である。 特定状態の関節機構が固定された場合のヤコビ行列Ｊを説明するための説明図である。 図１６に示したヤコビ行列の擬似逆行列を説明するための説明図である。 図１７に示した擬似逆行列を用いて算出された関節移動量を示す図である。 図１８に示した関節移動量に基づいて算出された最終関節値を示す図である。 図１０に示した初期関節値と異なる初期関節値を用いたシミュレーションの結果を説明するための説明図である。 第１変形例に係るロボットの一例を説明するための説明図である。 第２変形例に係る先端部の一例を説明するための説明図である。 旋回の一例を説明するための説明図である。

以下、本発明を実施するための形態について図面を参照して説明する。なお、各図において、各部の寸法及び縮尺は、実際のものと適宜に異ならせてある。また、以下に述べる実施の形態は、本発明の好適な具体例であるから、技術的に好ましい種々の限定が付されているが、本発明の範囲は、以下の説明において特に本発明を限定する旨の記載がない限り、これらの形態に限られるものではない。

［１．実施形態］
先ず、図１を参照しながら、実施形態に係るロボットシステム１の概要の一例について説明する。

図１は、実施形態に係るロボットシステム１の概要を説明するための説明図である。

なお、以下では、説明の便宜上、ロボット１０のベース座標系として、現実空間に固定された基準座標系Σ０を導入する。例えば、基準座標系Σ０は、後述するロボット１０の底面ＢＤＰｂｔの中心に原点を有し、互いに直交するＸ０軸、Ｙ０軸、及び、Ｚ０軸を有する３軸の直交座標系である。本実施形態では、基準座標系Σ０及び後述する図９に示す座標系Σ１からΣ８が右手座標系である場合を想定する。

ロボットシステム１は、例えば、ロボット１０と、ロボット１０に着脱可能に取り付けられるエンドエフェクタ２０と、ロボット１０及びエンドエフェクタ２０の動作を制御するロボットコントローラ３０とを有する。ロボット１０は、「多関節ロボット」の例であり、ロボットコントローラ３０は、「制御装置」の例である。

ロボット１０及びロボットコントローラ３０は、例えば、有線を用いた接続により、互いに通信可能に接続されている。なお、ロボット１０とロボットコントローラ３０との接続は、無線を用いた接続であってもよいし、有線及び無線の両方を用いた接続であってもよい。また、ロボットコントローラ３０は、ロボット１０に取り付けられたエンドエフェクタ２０と通信可能である。ロボットコントローラ３０としては、他の装置と通信可能な任意の情報処理装置を採用することができる。

ロボット１０は、例えば、農場、工場及び倉庫等での作業に用いられる多関節ロボットである。具体的には、ロボット１０は、回転関節に対応する６個の関節機構ＪＥｒ（ＪＥｒ１、ＪＥｒ２、ＪＥｒ３、ＪＥｒ４、ＪＥｒ５及びＪＥｒ６）を有する６軸多関節ロボットに、直動関節に対応する２個の関節機構ＪＥｐ（ＪＥｐ１及びＪＥｐ２）を追加した８軸多関節ロボットである。例えば、ロボット１０は、６個の関節機構ＪＥｒと、２個の関節機構ＪＥｐと、ボディ部ＢＤＰと、２個のリンクＬＫ（ＬＫ１及びＬＫ２）と、先端部ＴＰ１とを有する。なお、図１に示す例では、関節機構ＪＥｒ１は、ボディ部ＢＤＰに含まれ、関節機構ＪＥｒ５及びＪＥｒ６は、先端部ＴＰ１に含まれる。また、関節機構ＪＥｐ１は、リンクＬＫ１に設けられ、関節機構ＪＥｐ２は、リンクＬＫ２に設けられる。以下では、関節機構ＪＥｒ及びＪＥｐは、特に区別せずに、関節機構ＪＥとも称される。例えば、ロボット１０は、複数の関節機構ＪＥを駆動する複数のモータをさらに有する。図１では、図を見やすくするために、複数の関節機構ＪＥを駆動する複数のモータ、複数のモータの各々に設けられる減速機及びエンコーダ等の記載を省略している。複数の関節機構ＪＥは、「複数の関節」の例である。

ボディ部ＢＤＰは、「基部」の例である。また、リンクＬＫ１は、「第１リンク」の例であり、リンクＬＫ２は、「第２リンク」の例である。例えば、リンクＬＫ１及びＬＫ２は、ボディ部ＢＤＰと先端部ＴＰ１を接続する。

ここで、例えば、部材の接続は、２個の部材が直接的に接続される場合と、２個の部材が間接的に接続される場合との両方を含む。２個の部材が直接的に接続されるとは、２個の部材が互いに接触する状態、及び、２個の部材が互いに接触する状態と同視できる状態を含む。２個の部材が互いに接触する状態と同視できる状態とは、例えば、２個の部材の一方が他方に接着剤等により固定される状態である。また、２個の部材が間接的に接続されるとは、２個の部材の間に他の部材が配置されることを意味する。

関節機構ＪＥｒ１は、「第１駆動機構」の例であり、関節機構ＪＥｒ２は、「第２駆動機構」の例である。関節機構ＪＥｒ３は、「第３駆動機構」の例であり、関節機構ＪＥｒ４は、「第４駆動機構」の例である。また、関節機構ＪＥｒ５は、「第５駆動機構」の例であり、関節機構ＪＥｒ６は、「第６駆動機構」の例である。また、関節機構ＪＥｐ１は、「第１移動機構」の例であり、関節機構ＪＥｐ２は、「第２移動機構」の例である。

ボディ部ＢＤＰは、例えば、床等の所定の場所に固定される土台部ＢＤＰｂａと、関節機構ＪＥｒ２に接続される関節機構ＪＥｒ１とを含む。関節機構ＪＥｒ１は、ボディ部ＢＤＰの底面ＢＤＰｂｔに垂直な軸Ａｘ１を回転軸として、ボディ部ＢＤＰの一部分を回転させる。例えば、関節機構ＪＥｒ１は、関節機構ＪＥｒ１のうち、関節機構ＪＥｒ２と接続される部分を含む外壁を、軸Ａｘ１を回転軸として土台部ＢＤＰｂａに対して回転させる。すなわち、関節機構ＪＥｒ１は、軸Ａｘ１を回転軸として、関節機構ＪＥｒ２をボディ部ＢＤＰに対して回転させる。なお、軸Ａｘ１は、「第１回転軸」の例である。

ここで、「垂直」は、厳密な垂直だけではなく、実質的な垂直（例えば、誤差範囲内の垂直）も含む。同様に、後述する「平行」は、厳密な平行だけではなく、実質的な平行（例えば、誤差範囲内の平行）も含む。図１の回転方向Ｄｒ１は、ボディ部ＢＤＰの一部分が軸Ａｘ１を回転軸として回転する場合のボディ部ＢＤＰの一部分の回転方向を示す。

関節機構ＪＥｒ２は、ボディ部ＢＤＰとリンクＬＫ１を接続し、ボディ部ＢＤＰの底面ＢＤＰｂｔに平行な軸Ａｘ２を回転軸としてリンクＬＫ１をボディ部ＢＤＰに対して回転させる。図１の回転方向Ｄｒ２は、リンクＬＫ１が軸Ａｘ２を回転軸として回転する場合のリンクＬＫ１の回転方向を示す。なお、軸Ａｘ２は、「第２回転軸」の例である。

リンクＬＫ１は、例えば、中空であり、長尺に形成される。また、リンクＬＫ１は、リンクＬＫ１が延在する方向Ｄｅ１に延在する開口Ｈｌｋ１を有する。方向Ｄｅ１は、「第１リンクの延在方向」である。

開口Ｈｌｋ１は、例えば、リンクＬＫ１のうち、リンクＬＫ２に対向する部分を含む面に形成される。リンクＬＫ１の内部には、関節機構ＪＥｒ３の一部及び関節機構ＪＥｐ１が設けられる。例えば、関節機構ＪＥｒ３の一部は、リンクＬＫ１の内部に位置し、関節機構ＪＥｒ３の他の部分は、開口Ｈｌｋ１からリンクＬＫ１の外部に出ている。なお、関節機構ＪＥｒ３のうち、リンクＬＫ１の外部に出ている部分、又は、リンクＬＫ１の外部に出ている部分の一部は、後述するリンクＬＫ２の開口Ｈｌｋ２を通り、リンクＬＫ２の内部に位置する。

なお、リンクＬＫ１は、関節機構ＪＥｒ１により、軸Ａｘ１を回転軸としてボディ部ＢＤＰに対して回転し、関節機構ＪＥｒ２により、軸Ａｘ２を回転軸としてボディ部ＢＤＰに対して回転する。

関節機構ＪＥｒ３は、リンクＬＫ１とリンクＬＫ２を接続し、リンクＬＫ１が延在する方向Ｄｅ１に垂直な軸Ａｘ３を回転軸としてリンクＬＫ２をリンクＬＫ１に対して回転させる。図１の回転方向Ｄｒ３は、リンクＬＫ２が軸Ａｘ３を回転軸として回転する場合のリンクＬＫ２の回転方向を示す。なお、軸Ａｘ３は、「第３回転軸」の例である。

関節機構ＪＥｐ１は、方向Ｄｅ１に沿って、関節機構ＪＥｒ３をリンクＬＫ１に対して相対的に移動させる。関節機構ＪＥｒ３が方向Ｄｅ１に沿って移動することにより、リンクＬＫ２は、方向Ｄｅ１に沿って、リンクＬＫ１に対して相対的に移動する。関節機構ＪＥｐ１による関節機構ＪＥｒ３の移動範囲は、リンクＬＫ１の実質的な長さ（制御上の長さ）を、リンクＬＫ１の半分以下の長さから半分以上の長さとすることが可能な範囲が好ましい。

リンクＬＫ２は、例えば、中空であり、長尺に形成される。また、リンクＬＫ２は、リンクＬＫ２が延在する方向Ｄｅ２に延在する開口Ｈｌｋ２を有する。方向Ｄｅ２は、「第２リンクの延在方向」である。

開口Ｈｌｋ２は、例えば、リンクＬＫ２のうち、リンクＬＫ１に対向する部分を含む面に形成される。リンクＬＫ２の内部には、関節機構ＪＥｒ３の一部及び関節機構ＪＥｐ２が設けられる。例えば、関節機構ＪＥｒ３の一部は、リンクＬＫ２の内部に位置し、関節機構ＪＥｒ３の他の部分は、開口Ｈｌｋ２からリンクＬＫ２の外部に出ている。

関節機構ＪＥｐ２は、リンクＬＫ２が延在する方向Ｄｅ２に沿って、リンクＬＫ２を関節機構ＪＥｒ３に対して相対的に移動させる。これにより、リンクＬＫ２は、方向Ｄｅ２に沿って、関節機構ＪＥｒ３に対して相対的に移動する。すなわち、リンクＬＫ２は、方向Ｄｅ２に沿って、リンクＬＫ１に対して相対的に移動する。関節機構ＪＥｐ２による関節機構ＪＥｒ３の移動範囲は、リンクＬＫ２の実質的な長さ（制御上の長さ）を、リンクＬＫ２の半分以下の長さから半分以上の長さとすることが可能な範囲が好ましい。

このように、リンクＬＫ２は、関節機構ＪＥｐ１により、方向Ｄｅ１に沿って、リンクＬＫ１に対して相対的に移動し、関節機構ＪＥｐ２により、方向Ｄｅ２に沿って、リンクＬＫ１に対して相対的に移動する。

関節機構ＪＥｒ４は、リンクＬＫ２と先端部ＴＰ１を接続し、方向Ｄｅ２に垂直な軸Ａｘ４を回転軸として、先端部ＴＰ１をリンクＬＫ２に対して回転させる。図１の回転方向Ｄｒ４は、先端部ＴＰ１が軸Ａｘ４を回転軸として回転する場合の先端部ＴＰ１の回転方向を示す。なお、軸Ａｘ４は、「第４回転軸」の例である。

先端部ＴＰ１には、例えば、物品を把持するエンドエフェクタ２０が取り付けられる。例えば、先端部ＴＰ１の端面ＴＰ１ｓｆにエンドエフェクタ２０が取り付けられる。先端部ＴＰ１は、リンクＬＫ２に接続される第１部分ＴＰ１１と、第１部分ＴＰ１１に接続される第２部分ＴＰ１２と、関節機構ＪＥｒ５と、関節機構ＪＥｒ６とを含む。第１部分ＴＰ１１は、例えば、関節機構ＪＥｒ４を介してリンクＬＫ２に接続される。従って、第１部分ＴＰ１１は、軸Ａｘ４を回転軸としてリンクＬＫ２に対して回転する。

関節機構ＪＥｒ５は、第１部分ＴＰ１１と第２部分ＴＰ１２を接続し、軸Ａｘ４に垂直な軸Ａｘ５を回転軸として、第２部分ＴＰ１２を第１部分ＴＰ１１に対して回転させる。図１の回転方向Ｄｒ５は、第２部分ＴＰ１２が軸Ａｘ５を回転軸として回転する場合の第２部分ＴＰ１２の回転方向を示す。なお、軸Ａｘ５は、「第５回転軸」の例である。

関節機構ＪＥｒ６は、軸Ａｘ５に垂直な軸Ａｘ６を回転軸として、先端部ＴＰ１の少なくとも一部分を回転させる。図１に示す例では、関節機構ＪＥｒ６は、軸Ａｘ６を回転軸として、先端部ＴＰ１の端面ＴＰ１ｓｆを回転させる。すなわち、関節機構ＪＥｒ６は、軸Ａｘ６を回転軸として、先端部ＴＰ１のうち、エンドエフェクタ２０が取り付けられる部分（端面ＴＰ１ｓｆ）を回転させる。図１の回転方向Ｄｒ６は、端面ＴＰ１ｓｆが軸Ａｘ６を回転軸として回転する場合の端面ＴＰ１ｓｆの回転方向を示す。なお、軸Ａｘ６は、「第６回転軸」の例である。

図１に示す例では、関節機構ＪＥｒ６の表面が端面ＴＰ１ｓｆに該当する。なお、関節機構ＪＥｒ６が第２部分ＴＰ１２に含まれる構成等では、第２部分ＴＰ１２の端面が端面ＴＰ１ｓｆであってもよい。

また、エンドエフェクタ２０により行われる作業は、物品の把持に限定されない。エンドエフェクタ２０としては、ロボット１０の作業目的に応じて適切な部品（例えば、ロボットハンド及びロボットフィンガー等）を適用することができる。すなわち、各種作業に適したエンドエフェクタ２０が先端部ＴＰ１に取り付けられる。

ここで、本実施形態では、特定の方向とのなす角度が所定角度より大きい軸を回転軸とした回転を、特定の方向とのなす角度が所定角度以下の軸を回転軸とした回転と区別して、「旋回」と称する場合がある。所定角度は、例えば、４５°であってもよい。なお、所定角度は、４５°に限定されない。

例えば、軸Ａｘ１及びＡｘ２の各々を回転軸とする回転では、ボディ部ＢＤＰの底面ＢＤＰｂｔに垂直な方向Ｄｖ１が特定の方向に該当する。この場合、軸Ａｘ１は、ボディ部ＢＤＰの底面ＢＤＰｂｔに垂直な方向Ｄｖ１とのなす角度が所定角度以下の軸に該当し、軸Ａｘ２は、方向Ｄｖ１とのなす角度が所定角度より大きい軸に該当する。従って、軸Ａｘ２を回転軸とするリンクＬＫ１の回転は、旋回に該当する。なお、本実施形態では、ボディ部ＢＤＰが底面ＢＤＰｂｔに垂直な方向Ｄｖ１に沿って延在しているため、ボディ部ＢＤＰが延在する方向Ｄｅｂを特定の方向としてもよい。

また、軸Ａｘ３を回転軸とする回転では、リンクＬＫ１が延在する方向Ｄｅ１が特定の方向に該当し、軸Ａｘ４を回転軸とする回転では、リンクＬＫ２が延在する方向Ｄｅ２が特定の方向に該当する。この場合、軸Ａｘ３は、リンクＬＫ１が延在する方向Ｄｅ１とのなす角度が所定角度より大きい軸に該当し、軸Ａｘ４は、リンクＬＫ２が延在する方向Ｄｅ２とのなす角度が所定角度より大きい軸に該当する。従って、軸Ａｘ３を回転軸とするリンクＬＫ２の回転、及び、軸Ａｘ４を回転軸とする第１部分ＴＰ１１の回転は、旋回に該当する。

また、軸Ａｘ５を回転軸とする回転では、方向Ｄｅ１１が特定の方向に該当し、軸Ａｘ６を回転軸とする回転では、方向Ｄｅ１２が特定の方向に該当する。方向Ｄｅ１１は、第１部分ＴＰ１１の端部のうち、関節機構ＪＥｒ５が接続される所定の端部の反対側の端部から所定の端部に向かう方向である。なお、方向Ｄｅ１１は、第１部分ＴＰ１１が延在する方向と捉えられてもよい。また、方向Ｄｅ１２は、第２部分ＴＰ１２の端部のうち、関節機構ＪＥｒ６が接続される所定の端部（端面ＴＰ１ｓｆを含む端部）の反対側の端部から所定の端部に向かう方向である。なお、方向Ｄｅ１２は、第２部分ＴＰ１２が延在する方向と捉えられてもよい。

方向Ｄｅ１１が特定の方向である場合、軸Ａｘ５は、方向Ｄｅ１１とのなす角度が所定角度以下の軸に該当する。また、方向Ｄｅ１２が特定の方向である場合、軸Ａｘ６は、方向Ｄｅ１２とのなす角度が所定角度以下の軸に該当する。なお、本実施形態では、方向Ｄｅ１１が軸Ａｘ４に垂直な方向であり、方向Ｄｅ１２が軸Ａｘ５に垂直な方向である場合を想定する。この場合、方向Ｄｅ１１とのなす角度が所定角度以下の軸Ａｘ５は、軸Ａｘ４とのなす角度が所定角度より大きい軸に該当し、方向Ｄｅ１２とのなす角度が所定角度以下の軸Ａｘ６は、軸Ａｘ５とのなす角度が所定角度より大きい軸に該当する。

このように、本実施形態では、ロボット１０の複数の部分（ボディ部ＢＤＰ、リンクＬＫ１、リンクＬＫ２及び先端部ＴＰ１等）の各々が軸Ａｘ１、Ａｘ２、Ａｘ３、Ａｘ４、Ａｘ５及びＡｘ６の各々を回転軸として回転可能である。これにより、本実施形態では、ロボット１０は、人と同様の動作を実行できる。

例えば、関節機構ＪＥｒ２と関節機構ＪＥｒ３との間のリンクＬＫ１が上腕に相当し、関節機構ＪＥｒ３と関節機構ＪＥｒ４との間のリンクＬＫ２が前腕に相当する。そして、ロボット１０は、関節機構ＪＥｒ１により、人の腰のねじりを模した動作を行うことができ、関節機構ＪＥｒ２により、肩の旋回を模した動作を行うことができる。また、ロボット１０は、関節機構ＪＥｒ３により、肘の旋回を模した動作を行うことができ、関節機構ＪＥｒ４により、手首の旋回を模した動作を行うことができる。また、ロボット１０は、関節機構ＪＥｒ５により、手首のねじりを模した動作を行うことができ、関節機構ＪＥｒ６により、指先のねじりを模した動作を行うことができる。

さらに、本実施形態では、リンクＬＫ１内に設けられた関節機構ＪＥｐ１により、リンクＬＫ１が延在する方向Ｄｅ１に沿って、リンクＬＫ２をリンクＬＫ１に対して相対的に移動させることができる。また、本実施形態では、リンクＬＫ２内に設けられた関節機構ＪＥｐ２により、リンクＬＫ２が延在する方向Ｄｅ２に沿って、リンクＬＫ２をリンクＬＫ１に対して相対的に移動させることができる。従って、本実施形態では、関節機構ＪＥｐ１及びＪＥｐ２により、ロボット１０の先端部ＴＰ１をボディ部ＢＤＰの周辺に容易に移動させることができる。また、本実施形態では、関節機構ＪＥｐ１及びＪＥｐ２により、先端部ＴＰ１（より詳細には、端面ＴＰ１ｓｆ）が到達可能な領域を広くすることができるため、ロボット１０に取り付けられるエンドエフェクタ２０が到達可能な領域を広くすることができる。

なお、ロボットシステム１の構成は、図１に示す例に限定されない。例えば、ロボットコントローラ３０は、ロボット１０に内蔵されてもよい。また、図１では、ロボット１０が床等の所定の場所に固定される場合を想定したが、ロボット１０は、所定の場所に固定されずに、ロボット１０自体が移動可能であってもよい。また、ボディ部ＢＤＰの土台部ＢＤＰｂａは、床等の所定の場所に関節機構ＪＥｒ１を介して固定されてもよい。この場合、ボディ部ＢＤＰは、関節機構ＪＥｒ１を含まずに定義されてもよい。土台部ＢＤＰｂａが所定の場所に関節機構ＪＥｒ１を介して固定される構成では、関節機構ＪＥｒ１は、軸Ａｘ１を回転軸として、土台部ＢＤＰｂａを回転させてもよい。また、土台部ＢＤＰｂａが所定の場所に関節機構ＪＥｒ１を介して固定される構成では、土台部ＢＤＰｂａが関節機構ＪＥｒ２と接続されてもよい。

次に、ロボットコントローラ３０の動作を簡単に説明する。なお、ロボットコントローラ３０の構成は、後述する図２において説明され、ロボットコントローラ３０の動作フローは、後述する図３において説明される。

ロボットコントローラ３０は、例えば、ロボット１０の位置及び姿勢を目標の位置及び姿勢にするために、各関節機構ＪＥの状態（関節の状態）に関する関節値を算出する。関節機構ＪＥの状態は、関節の運動の状態であってもよい。具体的には、関節機構ＪＥの状態は、例えば、関節機構ＪＥの位置（関節の位置）、及び、関節機構ＪＥｒによる回転の回転角度（関節の方向）等であってもよい。この場合、関節値は、例えば、関節機構ＪＥの位置（関節の位置）、及び、関節機構ＪＥｒによる回転の回転角度（関節の方向）等を示す。以下では、関節機構ＪＥの状態（関節の状態）に関する関節値は、単に、関節機構ＪＥ（関節）の関節値とも称される。

例えば、ロボット１０の手先（例えば、エンドエフェクタ２０の先端）の速度（以下、手先速度とも称する）と関節速度との関係は、式（１）で表される。

なお、手先速度ｒ（・）は、式（２）で表される。また、ｍ個（ｍは２以上の自然数）の関節を有する多関節ロボットでは、関節速度θ（・）は、式（３）で表され、ヤコビ行列Ｊは、式（４）で表される。

ヤコビ行列Ｊは、例えば、６行ｍ列の行列で表され、第ｉ列の要素が第ｉ関節に係る要素Ｊ_ｉに該当する。第ｉ関節に係る要素Ｊ_ｉは、第ｉ関節が回転関節の場合、式（５）で表され、第ｉ関節が直動関節の場合、式（６）で表される。なお、式（６）の０は、例えば、ベクトル値が０であることを示す。

なお、第ｉ関節が回転関節の場合、関節機構ＪＥｒの回転軸がＺ軸に対応し、第ｉ関節が直動関節の場合、関節機構ＪＥｐの移動方向に沿う軸、又は、リンクＬＫの伸縮方向に沿う軸がＺ軸に対応する。

また、ロボット１０の手先速度と関節速度との関係は、ヤコビ行列Ｊの擬似逆行列Ｊ^＋を用いて、式（７）で表される。

例えば、ロボットコントローラ３０は、目標の手先速度ｒ（・）に対する各関節機構ＪＥの関節速度θ_ｉ（・）を、式（７）を用いて計算し、計算結果に基づいて各関節機構ＪＥを動作させる。具体的には、例えば、ロボットコントローラ３０は、式（７）を用いて算出した各関節機構ＪＥの関節速度θ_ｉ（・）に基づいて、各関節機構ＪＥの関節値を算出する。そして、ロボットコントローラ３０は、各関節機構ＪＥの関節値に基づいて、各関節機構ＪＥを動作させる。例えば、ロボットコントローラ３０は、各関節機構ＪＥの状態が各関節機構ＪＥの関節値に基づく状態になるように、各関節機構ＪＥを動作させる。

これにより、本実施形態では、例えば、ロボット１０をジョグ動作させることができる。なお、ジョグ動作は、例えば、ロボット１０の関節及び手先等を少しずつ動かして、ロボット１０の位置及び姿勢を目標の位置及び姿勢に到達させる動作である。関節速度θ_ｉ（・）、及び、関節速度θ_ｉ（・）に基づいて算出される関節機構ＪＥの状態等を示す情報は、関節値に対応する。関節機構ＪＥの関節速度θ（・）の計算は、逆運動学計算の例である。また、ヤコビ行列Ｊから擬似逆行列Ｊ^＋が算出されるため、式（７）を用いて、関節機構ＪＥの関節速度θ（・）を計算することは、ヤコビ行列を用いて逆運動学計算を実行することに該当する。

ここで、ジョグ動作が行われる場合、複数の関節機構ＪＥの全てについて、関節速度θ_ｉ（・）が計算される。このため、工夫をせずにヤコビ行列Ｊが用いられる制御方法（以下、対比例の制御方法）では、複数の関節機構ＪＥのいずれかが、関節速度θ_ｉ（・）に基づいて算出される関節機構ＪＥの状態が関節機構ＪＥの可動範囲（可動領域）外の状態となる場合がある。この場合、所望のジョグ動作を実行することができない。また、対比例の制御方法では、例えば、８個以上の関節を有する多関節ロボットにおいて、動作させる関節の数を関節の全数よりも少ない数（例えば、６個又は７個等）に制限して、多関節ロボットを動作させることができない。

このため、本実施形態では、動作させる関節の数を関節の全数よりも少ない数に制限して、ロボット１０を動作させる場合、ヤコビ行列Ｊの複数の要素のうち、特定の第ｉ関節に係る要素Ｊ_ｉのベクトル値を０に固定する。第ｉ関節に係る要素Ｊ_ｉのベクトル値を０にした場合のヤコビ行列Ｊは、式（８）となる。この場合、上述の式（７）から得られる関節速度θ_ｉ（・）は、式（９）で表される関節速度ベクトルとなる。

式（９）に示されるように、第ｉ関節の関節速度θ_ｉ（・）は、０となる。このように、本実施形態では、ヤコビ行列Ｊの複数の要素のうち、第ｉ関節に係る要素Ｊ_ｉのベクトル値を０に固定することより、第ｉ関節の関節値は変位することがないため、ｍ個の関節のうちの第ｉ関節を固定関節と見なすことができる。すなわち、本実施形態では、ヤコビ行列Ｊの第ｉ関節に係る要素Ｊ_ｉのベクトル値を０に固定することより、第ｉ関節以外の（ｍ－１）個の関節について、関節速度θ_ｉ（・）を計算することができる。この結果、本実施形態では、動作させる関節の数を関節の全数よりも少ない数に制限して、ロボット１０を動作させることができる。なお、本実施形態ではベクトル値を０に固定しているが、第ｉ関節の関節値が、ロボット制御に影響せず実質的に変位しないのであれば、必ずしも０である必要はなく、微小な値（実質的に０）であっても構わない。ここで、「実質的に０」とは、０のほか、０と見なせる微小な値も包含する。また、「実質的に変位しない」とは、厳密に変位しない場合のほか、変位していないと見なせる場合（例えば、ロボット制御に影響しない程度の微小な変化）も包含する。

例えば、本実施形態では、複数の関節のうちの所定の関節（第ｉ関節）において、あらかじめ特定状態が定められていたとする。本発明における特定状態とは、所定の関節において関節値に関する制約（例えば可動領域など）が設定されている場合、関節値が、制約の限界値の近傍の値（例えば可動領域の領域内で、可動領域の領域外との近傍の境界領域の値）となった状態である。

ロボットコントローラ３０は、関節値に基づく状態が特定状態である関節（第ｉ関節）を、ヤコビ行列Ｊの要素の値（ベクトル値）が実質的に０に固定される固定関節とする。以下では、関節の変位に関する制約が満たされる限界の状態は、限界状態とも称される。所定の余裕は、例えば、関節の状態（関節機構ＪＥの状態）が限界状態に到達する前に、関節の状態が限界状態に近づいたことをロボットコントローラ３０に認識させるために設定されたマージンである。

なお、関節の変位に関する制約は、例えば、関節機構ＪＥの可動に関する制約である。関節機構ＪＥの可動に関する制約としては、例えば、関節機構ＪＥｐ１による関節機構ＪＥｒ３の移動範囲、及び、関節機構ＪＥｐ２による関節機構ＪＥｒ３の移動範囲等が挙げられる。

例えば、関節機構ＪＥｐ１により、関節機構ＪＥｒ３がリンクＬＫ１に対して相対的に移動する場合、関節機構ＪＥｒ３の移動範囲は、リンクＬＫ１の長さ等に基づいて制限される。具体的には、例えば、リンクＬＫ１の２個の端部の一方の端部付近が関節機構ＪＥｐ１による関節機構ＪＥｒ３の移動範囲の上限であり、リンクＬＫ１の２個の端部の他方の端部付近が関節機構ＪＥｐ１による関節機構ＪＥｒ３の移動範囲の下限である。従って、関節機構ＪＥｒ３が関節機構ＪＥｐ１による関節機構ＪＥｒ３の移動範囲の上限又は下限に位置する場合、関節機構ＪＥｐ１の状態は、限界状態である。例えば、ロボットコントローラ３０は、関節機構ＪＥｒ３がリンクＬＫ１の端部の近傍に位置する場合、関節機構ＪＥｐ１の状態が特定状態（限界状態に所定の余裕を持たせた状態）であると特定する。

同様に、関節機構ＪＥｐ２により、関節機構ＪＥｒ３がリンクＬＫ２に対して相対的に移動する場合、関節機構ＪＥｒ３の移動範囲は、リンクＬＫ２の長さ等に基づいて制限される。具体的には、例えば、リンクＬＫ２の２個の端部の一方の端部付近が関節機構ＪＥｐ２による関節機構ＪＥｒ３の移動範囲の上限であり、リンクＬＫ２の２個の端部の他方の端部付近が関節機構ＪＥｐ２による関節機構ＪＥｒ３の移動範囲の下限である。従って、関節機構ＪＥｒ３が関節機構ＪＥｐ２による関節機構ＪＥｒ３の移動範囲の上限又は下限に位置する場合、関節機構ＪＥｐ２の状態は、限界状態である。例えば、ロボットコントローラ３０は、関節機構ＪＥｒ３がリンクＬＫ２の端部の近傍に位置する場合、関節機構ＪＥｐ２の状態が特定状態であると特定する。

そして、例えば、ロボットコントローラ３０は、複数の関節機構ＪＥの中に、関節値に基づく状態が特定状態である関節機構ＪＥが存在する場合、ヤコビ行列Ｊの複数の要素のうち、特定状態の関節機構ＪＥに係る要素の値を０に固定する。ヤコビ行列を用いて逆運動学計算が実行される場合、ヤコビ行列Ｊの複数の要素のうち、特定状態の関節機構ＪＥに係る要素の値が０に固定されることにより、特定状態の関節機構ＪＥの関節値が固定値に設定される。

ロボットコントローラ３０は、例えば、ロボット１０の位置及び姿勢が目標の位置及び姿勢になるまで、各関節機構ＪＥの関節値を算出する状態算出処理、及び、特定状態の関節機構ＪＥの関節値を変位させないようにする状態固定処理を含む単位処理を繰り返す。状態算出処理は、「算出処理」の例であり、状態固定処理は、「固定処理」の例である。

このように、ロボットコントローラ３０は、繰り返し実行する単位処理に含まれる状態算出処理において、ロボット１０を動作させる場合の関節機構ＪＥの変位量を計算する逆運動学計算を実行することにより、各関節機構ＪＥの関節値を算出する。また、ロボットコントローラ３０は、繰り返し実行する単位処理に含まれる状態固定処理において、複数の関節機構ＪＥの中に、関節値に基づく状態が特定状態である関節機構ＪＥが存在する場合、特定状態の関節機構ＪＥの関節値を固定値に設定する。なお、上述したように、ロボットコントローラ３０は、状態算出処理において、複数の関節機構ＪＥの中に特定状態の関節機構ＪＥが存在する場合、複数の関節機構ＪＥのうち、特定状態の関節機構ＪＥ以外の関節機構ＪＥを用いて、逆運動学計算を実行する。

これにより、本実施形態では、逆運動学計算の結果がエラーとなる頻度を低減することができる。この結果、本実施形態では、複数の関節機構ＪＥの各々において、関節値に基づく関節機構ＪＥの状態が関節機構ＪＥの可動範囲外の状態となることを抑止することができる。

なお、ロボット１０の構成は、図１に示す例に限定されず、例えば、後述する図９に示したシミュレーションモデルの構成であっても構わない。図９に示したシミュレーションモデルは、図１におけるロボット１０の関節機構ＪＥｐ１によりリンクＬＫ１自体が伸縮し、関節機構ＪＥｐ２によりリンクＬＫ２自体が伸縮する構成となっている。また、関節機構ＪＥの可動に関する制約は、関節機構ＪＥｐ１による関節機構ＪＥｒ３の移動範囲、及び、関節機構ＪＥｐ２による関節機構ＪＥｒ３の移動範囲に限定されない。例えば、関節機構ＪＥの可動に関する制約には、特異点を回避するための制約が含まれてもよい。なお、特異点は、例えば、ロボット１０の姿勢が、ロボット１０を制御できなくなる姿勢になることである。すなわち、関節機構ＪＥの可動に関する制約は、複数の関節機構ＪＥのうちの一部の関節機構ＪＥに対する制約であってもよいし、複数の関節機構ＪＥのうちの全部の関節機構ＪＥに対する制約であってもよい。なお、複数の関節機構ＪＥのうちの一部の関節機構ＪＥは、１個の関節機構ＪＥであってもよいし、複数の関節機構ＪＥであてもよい。

次に、図２を参照しながら、ロボットコントローラ３０のハードウェア構成について説明する。

図２は、図１に示したロボットコントローラ３０のハードウェア構成の一例を示す図である。

ロボットコントローラ３０は、ロボットコントローラ３０の各部を制御する処理装置３２と、各種情報を記憶するメモリ３５と、通信装置３６と、作業者等による操作を受け付ける操作装置３７と、表示装置３８と、ドライバ回路３９とを有する。

メモリ３５は、例えば、処理装置３２の作業領域として機能するＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）等の揮発性メモリと、制御プログラムＰＧｒ等の各種情報を記憶するＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ Ｅｒａｓａｂｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ Ｒｅａｄ－Ｏｎｌｙ Ｍｅｍｏｒｙ）等の不揮発性メモリとの、一方又は両方を含む。なお、メモリ３５は、ロボットコントローラ３０に着脱可能であってもよい。具体的には、メモリ３５は、ロボットコントローラ３０に着脱されるメモリカード等の記憶媒体であってもよい。また、メモリ３５は、例えば、ロボットコントローラ３０とネットワーク等を介して通信可能に接続された記憶装置（例えば、オンラインストレージ）であってもよい。

図２に示すメモリ３５は、制御プログラムＰＧｒを記憶している。制御プログラムＰＧｒは、「プログラム」の例である。本実施形態では、制御プログラムＰＧｒは、例えば、ロボットコントローラ３０がロボット１０の動作を制御するためのアプリケーションプログラムを含む。但し、制御プログラムＰＧｒは、例えば、処理装置３２がロボットコントローラ３０の各部を制御するためのオペレーティングロボットシステムプログラムを含んでもよい。

処理装置３２は、ロボットコントローラ３０の全体を制御するプロセッサであり、例えば、１又は複数のＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）を含んで構成される。処理装置３２は、例えば、メモリ３５に記憶された制御プログラムＰＧｒを実行し、制御プログラムＰＧｒに従って動作することで、動作制御部３３として機能する。例えば、動作制御部３３は、ロボット１０の位置及び姿勢が目標の位置及び姿勢になるまで、図１において説明した単位処理を繰り返す。そして、動作制御部３３は、各関節機構ＪＥの関節値等に基づいて、ロボット１０を、後述するドライバ回路３９を介して駆動する。なお、制御プログラムＰＧｒは、ネットワーク等を介して他の装置から送信されてもよい。

また、例えば、処理装置３２が複数のＣＰＵを含んで構成される場合、処理装置３２の機能の一部又は全部は、これら複数のＣＰＵが制御プログラムＰＧｒ等のプログラムに従って協働して動作することで実現されてもよい。また、処理装置３２は、１又は複数のＣＰＵに加え、又は、１又は複数のＣＰＵのうち一部又は全部に代えて、ＧＰＵ（Ｇｒａｐｈｉｃｓ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ＤＳＰ（Ｄｉｇｉｔａｌ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｏｒ）、又は、ＦＰＧＡ（Ｆｉｅｌｄ Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）等のハードウェアを含んで構成されるものであってもよい。この場合、処理装置３２の機能の一部又は全部は、ＤＳＰ等のハードウェアにより実現されてもよい。

通信装置３６は、ロボットコントローラ３０の外部に存在する外部装置と通信を行うためのハードウェアである。例えば、通信装置３６は、近距離無線通信によって外部装置と通信する機能を有する。なお、通信装置３６は、移動体通信網又はネットワークを介して外部装置と通信する機能をさらに有してもよい。

操作装置３７は、外部からの入力を受け付ける入力デバイス（例えば、キーボード、マウス、スイッチ、ボタン及びセンサ等）である。例えば、操作装置３７は、作業者の操作を受け付け、操作に応じた操作情報を処理装置３２に出力する。なお、例えば、表示装置３８の表示面に対する接触を検出するタッチパネルが、操作装置３７として採用されてもよい。

表示装置３８は、外部への出力を実施するディスプレイ等の出力デバイスである。表示装置３８は、例えば、処理装置３２による制御のもとで、画像を表示する。なお、操作装置３７及び表示装置３８は、一体となった構成（例えば、タッチパネル）であってもよい。

ドライバ回路３９は、処理装置３２（より詳細には、動作制御部３３）による制御のもとで、ロボット１０を駆動するための信号をロボット１０に出力するハードウェアである。例えば、ドライバ回路３９は、各関節機構ＪＥの関節値等に基づく信号を、モータＭＯｒ１、ＭＯｒ２、ＭＯｒ３、ＭＯｒ４、ＭＯｒ５、ＭＯｒ６、ＭＯｐ１及びＭＯｐ２等を駆動する信号として、ロボット１０に出力する。なお、モータＭＯｒ１、ＭＯｒ２、ＭＯｒ３、ＭＯｒ４、ＭＯｒ５及びＭＯｒ６は、関節機構ＪＥｒ１、ＪＥｒ２、ＪＥｒ３、ＪＥｒ４、ＪＥｒ５及びＪＥｒ６をそれぞれ駆動するモータである。また、モータＭＯｐ１及びＭＯｐ２は、関節機構ＪＥｐ１及びＪＥｐ２をそれぞれ駆動するモータである。以下では、モータＭＯｒ１、ＭＯｒ２、ＭＯｒ３、ＭＯｒ４、ＭＯｒ５、ＭＯｒ６、ＭＯｐ１及びＭＯｐ２をモータＭＯと総称する場合がある。

このように、ロボットコントローラ３０は、複数のモータＭＯを制御することにより、ロボット１０の動作を制御する。

次に、図３を参照しながら、ロボットコントローラ３０の動作の概要について説明する。

図３は、図１に示したロボットコントローラ３０の動作の一例を示すフローチャートである。なお、図３に示す動作では、ロボット１０の手先の位置及び姿勢を目標の位置及び姿勢にするための各関節機構ＪＥの関節値（例えば、関節機構ＪＥの位置、及び、関節機構ＪＥｒによる回転の回転角度等を示す情報）を算出する処理が行われる。例えば、図３に示す動作は、動作制御部３３として機能する処理装置３２により、実行される。すなわち、図３に示す動作（ステップＳ１００からステップＳ５２０までの一連の処理）では、処理装置３２は、動作制御部３３として機能する。

先ず、ステップＳ１００において、動作制御部３３は、ロボット１０の手先の位置及び姿勢と目標の位置及び姿勢との差分を算出する。例えば、動作制御部３３は、各関節機構ＪＥの現在の関節値に基づいて、ロボット１０の手先の位置及び姿勢を算出する。そして、動作制御部３３は、各関節機構ＪＥの現在の関節値に基づいて算出されたロボット１０の手先の位置及び姿勢とロボット１０の手先の目標の位置及び姿勢との差分を算出する。

次に、ステップＳ１２０において、動作制御部３３は、ロボット１０の手先の位置及び姿勢と目標の位置及び姿勢との差分が許容値以下であるか否かを判定する。許容値は、例えば、ロボット１０の手先の位置及び姿勢と目標の位置及び姿勢との差分が許容値以下であれば、ロボット１０の手先の位置及び姿勢が目標の位置及び姿勢と一致していると見なせる値に設定される。

ステップＳ１２０における判定の結果が肯定の場合、動作制御部３３は、図３に示す動作を終了する。この場合、ステップＳ１００においてロボット１０の手先の位置及び姿勢の算出に用いられた各関節機構ＪＥの関節値（最新の関節値）が、ロボット１０の手先の位置及び姿勢を目標の位置及び姿勢にするための各関節機構ＪＥの関節値として算出される。なお、ステップＳ１００からステップＳ５２０までの一連の処理が２回以上繰り返されている場合、今回のステップＳ１００の処理に用いられた最新の関節値は、前回のステップＳ４２０の処理において更新された関節値である。

一方、ステップＳ１２０における判定の結果が否定の場合、動作制御部３３は、処理をステップＳ２００に進める。

ステップＳ２００において、動作制御部３３は、各関節機構ＪＥの現在の関節値に基づいて、ヤコビ行列Ｊを算出する。そして、動作制御部３３は、処理をステップＳ３００に進める。

ステップＳ３００において、動作制御部３３は、複数の関節機構ＪＥの中に、特定状態の関節機構ＪＥが存在するか否かを判定する。ステップＳ３００における判定の結果が肯定の場合、動作制御部３３は、処理をステップＳ３２０に進める。一方、ステップＳ３００における判定の結果が否定の場合、動作制御部３３は、処理をステップＳ４００に進める。

ステップＳ３２０において、動作制御部３３は、ヤコビ行列Ｊの複数の要素のうち、特定状態の関節機構ＪＥに対応する要素の値を実質的に０に設定する。これにより、例えば、ヤコビ行列Ｊの特定状態の関節機構ＪＥに対応する列の要素の値が実質的に０に設定される。そして、動作制御部３３は、処理をステップＳ４００に進める。

ステップＳ４００において、動作制御部３３は、ヤコビ行列Ｊの擬似逆行列Ｊ^＋を用いて、各関節機構ＪＥの変位量（例えば、各関節機構ＪＥの関節速度θ_ｉ（・））を算出する。なお、複数の関節機構ＪＥの中に特定状態の関節機構ＪＥが存在する場合、特定状態の関節機構ＪＥの変位量は、ヤコビ行列Ｊの特定状態の関節機構ＪＥに対応する列の要素の値が実質的に０に設定されているため、０又はほぼ０となる。動作制御部３３は、ステップＳ４００の処理を実行した後、処理をステップＳ４２０に進める。なお、ほぼ０は、例えば、０と見なせる値である。

ステップＳ４２０において、動作制御部３３は、各関節機構ＪＥの変位量に基づいて、各関節機構ＪＥの関節値を更新する。例えば、動作制御部３３は、ステップＳ１００においてロボット１０の手先の位置及び姿勢の算出に用いられた各関節機構ＪＥの関節値に各関節機構ＪＥの変位量を加算することにより、各関節機構ＪＥの関節値を更新する。なお、複数の関節機構ＪＥの中に特定状態の関節機構ＪＥが存在する場合、特定状態の関節機構ＪＥの変位量が０又はほぼ０であるため、ステップＳ４２０の処理による更新後の特定状態の関節機構ＪＥの関節値は、更新前の値と同じ又はほぼ同じ値となる。なお、更新前の値とほぼ同じ値は、例えば、更新前の値と同じと見なせる値である。従って、特定状態の関節機構ＪＥでは、状態は変化せずに維持される。これにより、本実施形態では、関節機構ＪＥの状態が制約を満たさない状態（例えば、可動範囲外の状態）になることを抑制することができる。

次に、ステップＳ５００において、動作制御部３３は、ループカウントをインクリメントする。なお、ループカウントは、図３に示す動作が実行される前に、０に初期化されている。

次に、ステップＳ５２０において、動作制御部３３は、ループカウントが上限値以下か否かを判定する。上限値は、ステップＳ１００からステップＳ５２０までの一連の処理の繰り返し回数の上限値であり、図３に示す動作が収束しない場合に図３に示す動作を終了させるために設定される。

ステップＳ５２０における判定の結果が否定の場合、動作制御部３３は、図３に示す動作を終了する。この場合、ステップＳ４２０の処理において更新された各関節機構ＪＥの関節値（最新の関節値）が、各関節機構ＪＥの関節値として算出される。但し、ループカウントが上限値を超えたことにより、図３に示す動作が終了した場合、図３に示す動作により算出された各関節機構ＪＥの関節値は、ロボット１０の手先の位置及び姿勢を目標の位置及び姿勢にする関節値になるとは限らない。このため、動作制御部３３は、ロボット１０の手先の位置及び姿勢を目標の位置及び姿勢にするための各関節機構ＪＥの関節値を算出する処理が収束しなかったことを示すエラー情報を作業者等に通知してもよい。例えば、動作制御部３３は、エラー情報を表示装置３８に表示させてもよい。

一方、ステップＳ５２０における判定の結果が肯定の場合、動作制御部３３は、処理をステップＳ１００に戻す。このように、ステップＳ１００からステップＳ５２０までの一連の処理は、ロボット１０の手先の位置及び姿勢を目標の位置及び姿勢にするための各関節機構ＪＥの関節値が算出されるまで、又は、ループカウントが上限値を超えるまで、繰り返される。

ここで、例えば、ステップＳ４００及びＳ４２０の一連の処理は、図１において説明した状態算出処理に該当し、ステップＳ３００及びＳ３２０の一連の処理は、図１において説明した状態固定処理に該当する。従って、ステップＳ１００からステップＳ５２０までの一連の処理は、状態算出処理及び状態固定処理を含む単位処理に該当する。すなわち、ステップＳ１００からステップＳ５２０までの一連の処理は、「単位処理」の例であり、ステップＳ４００及びＳ４２０の一連の処理は、「算出処理」の例であり、ステップＳ３００及びＳ３２０の一連の処理は、「固定処理」の例である。なお、状態算出処理は、ステップＳ４００及びＳ４２０の一連の処理に加えて、ステップＳ２００の処理を含んでもよい。従って、ステップＳ２００、Ｓ４００及びＳ４２０の一連の処理が「算出処理」として捉えられてもよい。

なお、ロボットコントローラ３０の動作は、図３に示した例に限定されない。例えば、ステップＳ５００及びＳ５２０の処理は、省かれてもよい。この場合、動作制御部３３は、ステップＳ４２０の処理を実行した後、処理をステップＳ１００に戻す。また、ステップＳ５００及びＳ５２０の処理が省かれる場合、割り込み処理等により、図３に示した動作を終了させてもよい。また、ステップＳ３００及びＳ３２０の処理は、ステップＳ２００の処理に含まれてもよい。

次に、図４及び図５を参照しながら、ロボット１０の状態を複数の目標状態を経由して最終的な目標状態に変化させる場合のロボットコントローラ３０の動作の概要について説明する。

図４は、ロボット１０の手先の位置及び姿勢を複数の目標の位置ＰＰ及び姿勢ＰＳに順に変化させる場合のロボットコントローラ３０の動作の一例を示すフローチャートである。図４に示す動作では、複数の目標の位置ＰＰ及び姿勢ＰＳがｎ個（ｎは２以上の自然数）である場合を想定する。以下では、ｎ個の目標の位置ＰＰ及び姿勢ＰＳをｎ個の目標状態と称する場合がある。

なお、図４に示す動作では、ステップＳ８の処理、ステップＳ６００の処理、及び、ステップＳ７００からステップＳ７６０までの一連の処理が、図３に示した動作に追加されている。また、図４に示す動作では、図３に示したステップＳ２００からステップＳ４２０までの一連の処理に対応する処理として、ステップＳ１４０の関節値更新処理が実行される。ステップＳ１４０の関節値更新処理の詳細については、後述する図５において説明される。例えば、ステップＳ１４０の関節値更新処理は、特定状態の関節機構ＪＥが複数の関節機構ＪＥの中に存在する場合に図５に示すステップＳ３１０の処理が実行されることを除いて、図３に示したステップＳ２００からステップＳ４２０までの一連の処理と同様である。すなわち、ステップＳ１４０の関節値更新処理により、各関節機構ＪＥの関節値が更新される。

本実施形態では、図４に示す動作が開始される前のロボット１０の状態（初期状態）が、特定状態の関節機構ＪＥが複数の関節機構ＪＥの中に存在しない状態である場合を想定する。また、図４に示す動作では、変数ｋ（ｋは１以上ｎ以下の自然数）の初期値が１である場合を想定する。また、以下では、ｋ番目の目標の位置ＰＰ及び姿勢ＰＳを位置ＰＰｋ及び姿勢ＰＳｋとそれぞれ称する場合がある。例えば、位置ＰＰ１及び姿勢ＰＳ１は、１番目の目標の位置ＰＰ及び姿勢ＰＳであり、位置ＰＰｎ及び姿勢ＰＳｎは、ｎ番目の目標（最終的な目標）の位置ＰＰ及び姿勢ＰＳである。

図４に示す動作では、ロボット１０の初期状態から最終的な目標状態までのロボット１０の軌道として、ｎ個の目標の位置ＰＰ及び姿勢ＰＳがロボットコントローラ３０に定義されている。例えば、ロボットコントローラ３０は、変数ｋを１からｎまで順に変化させることにより、ロボット１０の手先の目標の位置ＰＰｋ及び姿勢ＰＳｋを順に更新する。

先ず、ステップＳ８において、動作制御部３３は、ロボット１０の手先のｋ番目の目標の位置ＰＰｋ及び姿勢ＰＳｋを設定する。例えば、図４に示す動作が開始された場合、動作制御部３３は、ステップＳ８において、１番目（ｋ＝１）の目標の位置ＰＰ１及び姿勢ＰＳ１を設定する。動作制御部３３は、ステップＳ８の処理を実行した後、処理をステップＳ１００に進める。

ステップＳ１００及びＳ１２０の一連の処理は、図３に示したステップＳ１００及びＳ１２０の一連の処理と同様である。但し、ステップＳ１２０における判定の結果が肯定の場合、動作制御部３３は、処理をステップＳ７００に進める。また、ステップＳ１２０における判定の結果が否定の場合、動作制御部３３は、ステップＳ１４０の関節値更新処理を実行した後、処理をステップＳ５００に進める。

ステップＳ５００及びＳ５２０の一連の処理は、図３に示したステップＳ５００及びＳ５２０の一連の処理と同様である。なお、図４に示す動作では、ステップＳ５２０における判定の結果が否定の場合、動作制御部３３は、ステップＳ６００において、ロボット１０の動作をエラーとして停止する。この場合、図４に示す動作が終了する。一方、ステップＳ５２０における判定の結果が肯定の場合、動作制御部３３は、図３に示した動作と同様に、処理をステップＳ１００に戻す。

また、上述したように、ステップＳ１２０における判定の結果が肯定の場合、ステップＳ７００の処理が実行される。

ステップＳ７００において、動作制御部３３は、各関節機構ＪＥを、ステップＳ１４０の関節値更新処理により更新された各関節機構ＪＥの関節値に従って制御する。これにより、ロボット１０の手先の位置及び姿勢は、目標の位置ＰＰｋ及び姿勢ＰＳｋに変化する。なお、ロボット１０に動作を教示する場合、動作制御部３３は、ステップＳ７００において、ステップＳ１４０の関節値更新処理により更新された各関節機構ＪＥの関節値を目標の位置ＰＰｋ及び姿勢ＰＳｋの関節値としてメモリ３５に記憶する。

ここで、例えば、ロボット１０の初期状態における手先の位置及び姿勢と目標の位置ＰＰ１及び姿勢ＰＳ１との差分が許容値以下である場合、ステップＳ１４０の関節値更新処理が１回も実行されずに、ステップＳ１２０における判定の結果が肯定となる。この場合、上述の「ステップＳ１４０の関節値更新処理により更新された各関節機構ＪＥの関節値」は、「ロボット１０の初期状態における各関節機構ＪＥの関節値」に読み替えられる。例えば、ステップＳ１４０の関節値更新処理が１回も実行されずにステップＳ１２０における判定の結果が肯定となる場合、動作制御部３３は、ステップＳ７００において、各関節機構ＪＥの関節値をロボット１０の初期状態における各関節機構ＪＥの関節値に維持する。

動作制御部３３は、ステップＳ７００の処理を実行した後、処理をステップＳ７２０に進める。

ステップＳ７２０において、動作制御部３３は、変数ｋがｎより小さいか否かを判定する。ステップＳ７２０における判定の結果が否定の場合、すなわち、ロボット１０の手先の位置及び姿勢が最終的な目標の位置ＰＰｎ及び姿勢ＰＳｎに変化した場合、動作制御部３３は、図３に示す動作を終了する。一方、ステップＳ７２０における判定の結果が肯定の場合、動作制御部３３は、ステップＳ７４０においてループカウントを０にリセットした後、処理をステップＳ７６０に進める。

ステップＳ７６０において、動作制御部３３は、変数ｋをインクリメントする（ｋ＝ｋ＋１）。そして、動作制御部３３は、処理をステップＳ８に戻す。これにより、ステップＳ８において、ロボット１０の手先の次の目標の位置ＰＰｋ及び姿勢ＰＳｋが設定される。

このように、図４に示す動作では、ｎ個の目標状態（位置ＰＰ及び姿勢ＰＳ）の各々において、ロボット１０の手先の位置及び姿勢と目標の位置ＰＰｋ及び姿勢ＰＳｋとの差分が許容値以下になるまで、ステップＳ１００からステップＳ５２０までの一連の処理（単位処理）が繰り返される。

次に、図５を参照しながら、ステップＳ１４０の関節値更新処理について説明する。

図５は、図４に示した関節値更新処理の一例を示すフローチャートである。例えば、動作制御部３３として機能する処理装置３２は、図４に示したステップＳ１４０の関節値更新処理として、図５に示すステップＳ２００からステップＳ４２０までの一連の処理を実行する。従って、ステップＳ２００の処理は、図４に示したステップＳ１２０における判定の結果が否定の場合に実行される。また、ステップＳ４２０の処理が実行された後、図４に示したステップＳ５００の処理が実行される。

また、以下では、目標の位置ＰＰ及び姿勢ＰＳがｋ番目の目標の位置ＰＰｋ及び姿勢ＰＳｋである場合に実行される処理を、ｋ番目の目標状態において実行される処理及びｋ番目の目標状態における処理等のように、「ｋ番目の目標状態」を用いて称する場合がある。また、図５の説明では、特に断りがない場合、関節値更新処理は、ｋ番目の目標状態における関節値更新処理を意味する。

ステップＳ２００及びＳ３００の一連の処理は、図３に示したステップＳ２００及びＳ３００の一連の処理と同様である。但し、図５に示す動作では、ステップＳ３００における判定の結果が肯定の場合、動作制御部３３は、処理をステップＳ３１０に進める。また、ステップＳ３００における判定の結果が否定の場合、動作制御部３３は、図３に示した動作と同様に、処理をステップＳ４００に進める。

ステップＳ３１０において、動作制御部３３は、各関節機構ＪＥの関節値を更新前の値に戻す。すなわち、動作制御部３３は、前回の関節値更新処理のステップＳ４２０の処理による各関節機構ＪＥの関節値の更新をキャンセルする。具体的には、動作制御部３３は、各関節機構ＪＥの関節値（すなわち、前回の関節値更新処理のステップＳ４２０の処理により更新された各関節機構ＪＥの関節値）を、前回の関節値更新処理のステップＳ４２０の処理により更新される前の関節値に戻す。

ここで、目標の位置ＰＰｋ及び姿勢ＰＳｋが更新された後の最初の関節値更新処理において、ステップＳ３００における判定の結果が肯定となる場合、前回の関節値更新処理は、（ｋ－１）番目の目標状態において最後に実行された関節値更新処理である。この場合、動作制御部３３は、ステップＳ３１０において、各関節機構ＪＥの関節値を、（ｋ－１）番目の目標状態において最後に実行された関節値更新処理のステップＳ４２０の処理により更新される前の関節値に戻す。

動作制御部３３は、ステップＳ３１０の処理を実行した後、処理をステップＳ３２０に進める。ステップＳ３２０の処理は、図３に示したステップＳ３２０の処理と同様である。なお、図５に示す例では、ステップＳ３２０の処理に用いられるヤコビ行列Ｊは、ステップＳ３１０の処理が実行される前にステップＳ２００において算出されたヤコビ行列Ｊである。但し、ステップＳ３２０の処理に用いられるヤコビ行列Ｊは、ステップＳ３１０の処理が実行された後の各関節機構ＪＥの関節値に基づいて算出されたヤコビ行列Ｊ（例えば、前回の関節値更新処理のステップＳ２００において算出されたヤコビ行列Ｊ）であってもよい。

なお、ステップＳ３２０の処理（状態固定処理）の対象となる特定状態の関節機構ＪＥは、ステップＳ３１０の処理（各関節機構ＪＥの関節値の更新のキャンセル）が実行される前に、ステップＳ３００において特定される。すなわち、ステップＳ３２０の処理の対象となる特定状態の関節機構ＪＥは、各関節機構ＪＥの関節値の更新がキャンセルされる前の関節値に基づいて、特定される。

動作制御部３３は、ステップＳ３２０の処理を実行した後、処理をステップＳ４００に進める。ステップＳ４００及びＳ４２０の一連の処理（状態算出処理）は、図３に示したステップＳ４００及びＳ４２０の一連の処理（状態算出処理）と同様である。なお、図５に示す関節値更新処理では、複数の関節機構ＪＥの中に特定状態の関節機構ＪＥが含まれる場合にステップＳ３１０の処理が実行されるため、常に、特定状態でない複数の関節機構ＪＥの関節値に対して、ステップＳ４２０の処理による更新が実行される。

このように、図５に示す関節値更新処理では、複数の関節機構ＪＥの中に特定状態の関節機構ＪＥが含まれる場合、前回の関節値更新処理のステップＳ４２０の処理による各関節機構ＪＥの関節値の更新が、今回の関節値更新処理のステップＳ３２０においてキャンセルされる。すなわち、前回の関節値更新処理のステップＳ４００及びＳ４２０の一連の処理（状態算出処理）において算出された複数の関節機構ＪＥの各々の関節値への複数の関節機構ＪＥの各々の関節値の更新がキャンセルされる。なお、ステップＳ３２０の処理（状態固定処理）の対象となる特定状態の関節機構ＪＥは、上述したように、関節値の更新がキャンセルされる前の関節値に基づいて特定される。そして、複数の関節機構ＪＥの中に特定状態の関節機構ＪＥが含まれる場合、ステップＳ３１０及びＳ３２０の一連の処理（キャンセル及び状態固定処理）が実行された後に、ステップＳ４００及びＳ４２０の一連の処理が実行される。

従って、図５に示す関節値更新処理では、上述したように、常に、特定状態でない複数の関節機構ＪＥの関節値に対して、ステップＳ４２０の処理による更新が実行される。すなわち、図４及び図５に示す動作では、関節機構ＪＥが特定状態に固定された状態で、ステップＳ１００からステップＳ５２０までの一連の処理（単位処理）が繰り返されることを抑制することができる。

なお、ロボット１０の手先の位置及び姿勢を複数の目標の位置ＰＰ及び姿勢ＰＳに順に変化させる場合のロボットコントローラ３０の動作は、図４及び図５に示す例に限定されない。例えば、ステップＳ７４０の処理は、ステップＳ７６０の処理より後に実行されてもよいし、ステップＳ７６０の処理と並列に実行されてもよい。あるいは、ロボット１０の手先の位置及び姿勢を複数の目標の位置ＰＰ及び姿勢ＰＳに順に変化させる場合のロボットコントローラ３０の動作は、図６又は図７に示す動作であってもよい。

図６は、ロボット１０の手先の位置及び姿勢を複数の目標の位置ＰＰ及び姿勢ＰＳに順に変化させる場合のロボットコントローラ３０の動作の別の例を示すフローチャートである。図６に示す動作は、図４に示したステップＳ１４０の関節値更新処理の代わりにステップＳ１４０Ａの関節値更新処理が実行され、ステップＳ７８０及びＳ７８２の一連の処理が図４に示した動作に追加されていることを除いて、図４に示した動作と同様である。

なお、ステップＳ１４０Ａの関節値更新処理は、図３に示したステップＳ２００からステップＳ４２０までの一連の処理である。すなわち、図６に示すステップＳ１００からステップＳ５２０までの一連の処理は、図３に示したステップＳ１００からステップＳ５２０までの一連の処理と同様である。従って、図６に示すステップＳ１００からステップＳ５２０までの一連の処理では、図５に示したステップＳ３１０の処理（各関節機構ＪＥの関節値の更新のキャンセル）は、実行されない。図６に示す動作では、図５に示したステップＳ３１０の処理がステップＳ１４０Ａの関節値更新処理において実行されない代わりに、ステップＳ７８０及びＳ７８２の一連の処理がステップＳ７６０の後に実行される。以下では、ステップＳ７８０及びＳ７８２の一連の処理を中心に説明する。

動作制御部３３は、ステップＳ７６０の処理を実行した後、処理をステップＳ７８０に進める。

ステップＳ７８０において、動作制御部３３は、複数の関節機構ＪＥの中に、特定状態の関節機構ＪＥが存在するか否かを判定する。なお、ステップＳ７８０の処理で参照される複数の関節機構ＪＥは、ステップＳ７００において手先の位置及び姿勢が目標の位置ＰＰｋ及び姿勢ＰＳｋに変化したロボット１０の複数の関節機構ＪＥである。

ステップＳ７８０における判定の結果が否定の場合、動作制御部３３は、処理をステップＳ８に戻す。すなわち、動作制御部３３は、ステップＳ７００において手先の位置及び姿勢が目標の位置ＰＰｋ及び姿勢ＰＳｋに変化したロボット１０の複数の関節機構ＪＥの中に、特定状態の関節機構ＪＥが存在しない場合、処理をステップＳ８に戻す。一方、ステップＳ７８０における判定の結果が肯定の場合、動作制御部３３は、処理をステップＳ７８２に進める。

ステップＳ７８２において、動作制御部３３は、特定状態の関節機構ＪＥの関節値を、特定状態の関節機構ＪＥの可動範囲内で、かつ、特定状態でない状態に対応する関節値に変更する。そして、動作制御部３３は、処理をステップＳ８に戻す。ステップＳ７８２の処理により、ｎ個の目標状態（位置ＰＰ及び姿勢ＰＳ）の各々において、各関節機構ＪＥの関節値の初期値は、特定状態でない状態に対応する関節値となる。

なお、ステップＳ７８２の処理により変更される関節値の変更後の値は、特定状態の関節機構ＪＥの可動範囲内で、かつ、特定状態でない状態に対応する関節値の範囲のうち、変更前の値との差が最小となる値であることが好ましい。

このように、図６に示す動作では、ステップＳ１００からステップＳ５２０までの一連の処理（単位処理）が繰り返し実行される前の初期状態の複数の関節機構ＪＥの中に特定状態の関節機構ＪＥが含まれる場合、繰り返し実行される単位処理のうちの１回目の単位処理が実行される前に、特定状態の関節機構ＪＥの関節値を、特定状態でない状態に対応する関節値に変更される。これにより、図６に示す動作では、ｎ個の目標状態（位置ＰＰ及び姿勢ＰＳ）の各々において、各関節機構ＪＥの関節値の初期値が特定状態に対応する関節値になることを抑制することができる。この結果、図６に示す動作では、ｋ番目の目標状態において特定状態に変化した関節機構ＪＥが（ｋ＋１）番目以降の目標状態において特定状態に固定されることを抑制することができる。

なお、図６に示す動作においても、例えば、ステップＳ７４０の処理は、ステップＳ７６０の処理より後に実行されてもよいし、ステップＳ７６０の処理と並列に実行されてもよい。

次に、図７を参照しながら、ロボット１０の状態を複数の目標状態を経由して最終的な目標状態に変化させる場合のロボットコントローラ３０の動作の別の例について説明する。

図７は、ロボット１０の手先の位置及び姿勢を複数の目標の位置ＰＰ及び姿勢ＰＳに順に変化させる場合のロボットコントローラ３０の動作の別の例を示すフローチャートである。図７に示す動作は、図４に示したステップＳ１４０の関節値更新処理の代わりにステップＳ１４０Ａの関節値更新処理が実行され、ステップＳ１０からステップＳ５０までの一連の処理が図４に示した動作に追加されていることを除いて、図４に示した動作と同様である。以下では、ステップＳ１０からステップＳ５０までの一連の処理を中心に説明する。

ステップＳ１０の処理、ステップＳ１２の処理、及び、ステップＳ５０の処理は、ステップＳ１００の処理、ステップＳ１２０の処理、及び、ステップＳ５００の処理とそれぞれ同様である。また、ステップＳ１４の関節値初回更新処理は、ステップＳ１４０Ａの関節値更新処理からステップＳ３００及びＳ３２０の一連の処理が省かれた処理と同様である。なお、ステップＳ１４の関節値初回更新処理については、後述する図８において説明される。

動作制御部３３は、ステップＳ８の処理を実行した後、処理をステップＳ１０に進める。

ステップＳ１０において、動作制御部３３は、ステップＳ１００の処理と同様に、ロボット１０の手先の位置及び姿勢と目標の位置ＰＰｋ及び姿勢ＰＳｋとの差分を算出する。そして、ステップＳ１２において、動作制御部３３は、ステップＳ１２０の処理と同様に、ロボット１０の手先の位置及び姿勢と目標の位置ＰＰｋ及び姿勢ＰＳｋとの差分が許容値以下であるか否かを判定する。

ステップＳ１２における判定の結果が肯定の場合、動作制御部３３は、処理をステップＳ７００に進める。一方、ステップＳ１２における判定の結果が否定の場合、動作制御部３３は、処理をステップＳ１４に進める。

ステップＳ１４において、動作制御部３３は、ステップＳ２００、Ｓ４００及びＳ４２０の一連の処理と同様な関節値初回更新処理を実行する。ステップＳ１４の関節値初回更新処理により、各関節機構ＪＥの関節値の初回の更新が実行される。

動作制御部３３は、ステップＳ１４の関節値初回更新処理を実行した後、ステップＳ５０においてループカウントをインクリメントし、処理をステップＳ１００に進める。

次に、図８を参照しながら、ステップＳ１４の関節値初回更新処理について説明する。

図８は、図７に示した関節値初回更新処理の一例を示すフローチャートである。例えば、動作制御部３３として機能する処理装置３２は、図７に示したステップＳ１４の関節値初回更新処理として、図８に示すステップＳ２０からステップＳ４２までの一連の処理を実行する。従って、ステップＳ２０の処理は、図４に示したステップＳ１２における判定の結果が否定の場合に実行される。また、ステップＳ４２の処理が実行された後、図７に示したステップＳ５０の処理が実行される。

ステップＳ２０において、動作制御部３３は、ステップＳ２００の処理と同様に、各関節機構ＪＥの現在の関節値に基づいて、ヤコビ行列Ｊを算出する。そして、動作制御部３３は、処理をステップＳ４０に進める。

ステップＳ４０において、動作制御部３３は、ステップＳ４００の処理と同様に、ヤコビ行列Ｊの擬似逆行列Ｊ^＋を用いて、各関節機構ＪＥの変位量を算出する。そして、動作制御部３３は、処理をステップＳ４２に進める。

ステップＳ４２において、動作制御部３３は、ステップＳ４２０の処理と同様に、各関節機構ＪＥの変位量に基づいて、各関節機構ＪＥの関節値を更新する。そして、動作制御部３３は、処理を図７に示したステップＳ５０に進める。

このように、図８に示す関節値初回更新処理では、図３に示したステップＳ３００及びＳ３２０の一連の処理（状態固定処理）が実行されない。従って、図８に示す関節値初回更新処理では、関節機構ＪＥの状態が特定状態でありか否かに拘わらず、各関節機構ＪＥの関節値が更新される。このため、図８に示す関節値初回更新処理では、例えば、（ｋ－１）番目の目標状態において実行されたステップＳ７００の処理の後の状態が特定状態である関節機構ＪＥの関節値が、特定状態でない状態に対応する関節値に更新される場合がある。この場合、繰り返し実行される単位処理（ステップＳ１００からステップＳ５２０までの一連の処理）の１回目の単位処理が特定状態に対応する関節値から開始されることを抑制することができる。

但し、図８に示す関節値初回更新処理では、特定状態の関節機構ＪＥの関節値が、特定状態の関節機構ＪＥを可動範囲外にする方向に更新される場合がある。例えば、関節値初回更新処理は、目標の位置ＰＰｋ及び姿勢ＰＳｋが更新される度に実行されるため、特定状態の関節機構ＪＥを可動範囲外にする方向への関節値の更新が繰り返される場合がある。この場合、繰り返し実行される関節値初回更新処理において、特定状態の関節機構ＪＥの関節値が、特定状態の関節機構ＪＥを可動範囲外にする関節値に更新されるおそれがある。

このため、特定状態の範囲は、１回の更新で変化する関節値の変化量に対応する範囲よりも大きい範囲であることが好ましい。より好ましくは、特定状態の範囲は、特定状態の関節機構ＪＥを可動範囲外にする方向への関節値の更新が複数回（例えば、ｎ回）繰り返された場合の関節値の合計の変化量に対応する範囲よりも大きい範囲である。あるいは、図７に示す動作において、図６に示したステップＳ７８０及びＳ７８２の一連の処理と同様の処理が、ステップＳ１４の処理が実行された後で、ステップＳ１００の処理が実行される前までに実行されてもよい。

次に、図９から図２０を参照しながら、ロボット１０の手先の位置及び姿勢を目標の位置及び姿勢にするシミュレーションの結果の一例について説明する。

図９は、シミュレーションに用いられたモデルを説明するための説明図である。以下では、シミュレーションに用いられたモデルは、シミュレーションモデルとも称される。

以下に説明するシミュレーションでは、シミュレーション結果を分かり易くするために、図１に示した関節機構ＪＥｐ（ＪＥｐ１及びＪＥｐ２）の代わりに関節機構ＪＥｅ（ＪＥｅ１及びＪＥｅ２）を有するロボット１０がモデル化されている。なお、複数の関節機構ＪＥをそれぞれ駆動する複数のモータＭＯの制御に着目した場合、図１に示したロボット１０も図９に示すロボット１０と同様な制御値になる。例えば、図９に示すロボット１０では、リンクＬＫ１Ａ自体が伸縮することにより、関節機構ＪＥｒ３がリンクＬＫ１Ａの延在方向に沿って移動する。また、リンクＬＫ２Ａ自体が伸縮することにより、リンクＬＫ２Ａの２つの端部のうち、関節機構ＪＥｒ４が接続された端部が、リンクＬＫ２Ａの延在方向に沿って、関節機構ＪＥｒ３に対して相対的に移動する。先ず、関節機構ＪＥｅ１及びＪＥｅ２について説明する。

関節機構ＪＥｅ１は、支持部分ＬＫ１ａ、可動部分ＬＫ１ｂ及び可動部分ＬＫ１ｃを含むリンクＬＫ１Ａを軸Ａｘｅ１に沿って伸縮させる直動関節であり、「第１伸縮機構」の例である。関節機構ＪＥｅ２は、支持部分ＬＫ２ａ、可動部分ＬＫ２ｂ及び可動部分ＬＫ２ｃを含むリンクＬＫ２Ａを軸Ａｘｅ２に沿って伸縮させる直動関節であり、「第２伸縮機構」の例である。軸Ａｘｅ１は、リンクＬＫ１Ａの延在方向に沿う軸であり、軸Ａｘｅ２は、リンクＬＫ２Ａの延在方向に沿う軸である。

リンクＬＫ１Ａは、「第１リンク」の例であり、リンクＬＫ２Ａは、「第２リンク」の例である。また、支持部分ＬＫ１ａは、「第１支持部分」の例であり、関節機構ＪＥｒ２を介してボディ部ＢＤＰに接続される。可動部分ＬＫ１ｃは、「第１可動部分」の例であり、関節機構ＪＥｒ３を介してリンクＬＫ２Ａの支持部分ＬＫ２ａに接続される。支持部分ＬＫ２ａは、「第２支持部分」の例である。可動部分ＬＫ２ｃは、「第２可動部分」の例であり、関節機構ＪＥｒ４を介して先端部ＴＰ１に接続される。

例えば、可動部分ＬＫ１ｂは、可動部分ＬＫ１ｃと一体的に移動するように可動部分ＬＫ１ｃに接続され、支持部分ＬＫ１ａに対して移動可能に支持部分ＬＫ１ａに接続される。そして、関節機構ＪＥｅ１は、支持部分ＬＫ１ａに対して可動部分ＬＫ１ｂを軸Ａｘｅ１に沿って移動させることにより、支持部分ＬＫ１ａに対して可動部分ＬＫ１ｃを軸Ａｘｅ１に沿って移動させる。これにより、リンクＬＫ１Ａは、軸Ａｘｅ１に沿って伸縮する。

また、例えば、可動部分ＬＫ２ｂは、可動部分ＬＫ２ｃと一体的に移動するように可動部分ＬＫ２ｃに接続され、支持部分ＬＫ２ａに対して移動可能に支持部分ＬＫ２ａに接続される。そして、関節機構ＪＥｅ２は、支持部分ＬＫ２ａに対して可動部分ＬＫ２ｂを軸Ａｘｅ２に沿って移動させることにより、支持部分ＬＫ２ａに対して可動部分ＬＫ２ｃを軸Ａｘｅ２に沿って移動させる。これにより、リンクＬＫ２Ａは、軸Ａｘｅ２に沿って伸縮する。

図９に示すシミュレーションモデルでは、複数の関節機構ＪＥと１対１に対応する複数の座標系Σが導入されている。複数の座標系Σの各々には、対応する関節機構ＪＥがボディ部ＢＤＰに近い順に、数字の１から８のいずれかが付されている。例えば、関節機構ＪＥｒ１が１番目の関節であり、関節機構ＪＥｒ２が２番目の関節であり、関節機構ＪＥｅ１が３番目の関節である。また、関節機構ＪＥｒ３が４番目の関節であり、関節機構ＪＥｅ２が５番目の関節であり、関節機構ＪＥｒ４が６番目の関節である。そして、関節機構ＪＥｒ５が７番目の関節であり、関節機構ＪＥｒ６が８番目の関節である。

ｉ番目の関節機構ＪＥ（第ｉ関節）に対応する座標系Σｉは、互いに直交するＸｉ軸、Ｙｉ軸、及び、Ｚｉ軸を有する３軸の直交座標系である。図９に示す例では、ｉは１以上８以下の自然数である。８番目の関節機構ＪＥｒ６に対応する座標系Σ８は、ロボット１０の先端座標系に対応する。なお、図９では、図を見やすくするために、座標系Σ１から座標系Σ７までのＹ軸の記載が省略されている。

例えば、座標系ΣｉのＺｉ軸は、ｉ番目の関節機構ＪＥが回転関節である場合、ｉ番目の関節機構ＪＥによる回転の回転軸に対応し、ｉ番目の関節機構ＪＥが直動関節である場合、ｉ番目の関節機構ＪＥにより伸縮するリンクＬＫの伸縮方向に沿う軸に対応する。また、例えば、座標系ΣｉのＸｉ軸は、基本的に、Ｚｉ軸とＺｉ＋１軸との共通垂線となるように定められる。

また、ｉ番目の関節機構ＪＥは、例えば、ヤコビ行列Ｊのｉ列目の要素Ｊ_ｉに対応付けられる。例えば、ヤコビ行列Ｊの１列目の要素Ｊ_１は、関節機構ＪＥｒ１に対応し、ヤコビ行列Ｊの２列目の要素Ｊ_２は、関節機構ＪＥｒ２に対応し、ヤコビ行列Ｊの３列目の要素Ｊ_３は、関節機構ＪＥｅ１に対応する。また、ヤコビ行列Ｊの４列目の要素Ｊ_４は、関節機構ＪＥｒ３に対応し、ヤコビ行列Ｊの５列目の要素Ｊ_５は、関節機構ＪＥｅ２に対応し、ヤコビ行列Ｊの６列目の要素Ｊ_６は、関節機構ＪＥｒ４に対応する。そして、ヤコビ行列Ｊの７列目の要素Ｊ_７は、関節機構ＪＥｒ５に対応し、ヤコビ行列Ｊの８列目の要素Ｊ_８は、関節機構ＪＥｒ６に対応する。

また、シミュレーションでは、基準座標系Σ０及び座標系Σ１の原点間の距離に対応するリンク長Ｌ１は０．２４０９［ｍ］に設定され、座標系Σ５及びΣ６の原点間の距離に対応するリンク長Ｌ６は０．１［ｍ］（シミュレーションにおけるパラメータとしては－０．１［ｍ］）に設定されている。また、座標系Σ６及びΣ７の原点間の距離に対応するリンク長Ｌ７は０．１［ｍ］に設定され、座標系Σ７及びΣ８の原点間の距離に対応するリンク長Ｌ８は０．０５［ｍ］に設定されている。また、関節機構ＪＥｅ１によるリンクＬＫ１Ａの可動範囲（伸縮範囲）は、０．２５［ｍ］から０．５［ｍ］に設定され、関節機構ＪＥｅ２によるリンクＬＫ２Ａの可動範囲（伸縮範囲）は、０．３［ｍ］から０．５［ｍ］に設定されている。

また、シミュレーションでは、ロボット１０の各関節機構ＪＥの初期の関節値が図１０に示す初期関節値θｉｎｔであり、ロボット１０の手先がＸ０軸の正方向に移動する場合を想定する。

図１０は、図９に示したシミュレーションモデルの初期関節値θｉｎｔの一例を説明するための説明図である。

図１０の星印で示されるように、関節機構ＪＥｅ１の初期関節値θｉｎｔは、リンクＬＫ１Ａの可動範囲の上限である０．５［ｍ］に設定され、関節機構ＪＥｅ２の初期関節値θｉｎｔは、リンクＬＫ２Ａの可動範囲の上限である０．５［ｍ］に設定されている。すなわち、関節機構ＪＥｅ１及びＪＥｅ２は、両方とも限界状態である。このため、本実施形態では、後述の図１６に示すように、ヤコビ行列Ｊの複数の要素のうち、関節機構ＪＥｅ１及びＪＥｅ２にそれぞれ対応する要素Ｊ_３及びＪ_４の値は、０に設定される。なお、本実施形態と対比される対比例の制御方法（状態固定処理が実行されない制御方法）では、図１２に示すように、ヤコビ行列Ｊの複数の要素のうちの関節機構ＪＥｅ１及びＪＥｅ２にそれぞれ対応する要素Ｊ_３及びＪ_４の値は、特に固定されない。

図１１は、シミュレーションに用いられた手先移動量Δｒを説明するための説明図である。本シミュレーションでは、ジョグ動作によりロボット１０の手先をＸ０軸の正方向に０．００２７２［ｍ］だけ移動させる場合を想定する。従って、図１１に示すように、Ｘ０軸の正方向への移動量（０．００２７２［ｍ］）が手先移動量Δｒに該当する。

すなわち、本シミュレーションでは、関節機構ＪＥｅ１及びＪＥｅ２によりリンクＬＫ１Ａ及びＬＫ２Ａが上限まで伸びた限界状態から、ロボット１０の手先がＸ０軸の正方向に０．００２７２［ｍ］だけ移動する。本シミュレーションでは、上述の式（７）の関係“θ_ｉ（・）＝Ｊ^＋ｒ（・）”を用いて、手先移動量Δｒに対する関節移動量Δθが算出される。そして、関節移動量Δθを初期関節値θｉｎｔに加算することにより、手先が移動した後の関節値が最終関節値θｅｎｄとして算出される。

先ず、図１２から図１５を参照しながら、対比例の制御方法（状態固定処理が実行されない制御方法）によるシミュレーションについて説明する。

図１２は、対比例の制御方法に用いられるヤコビ行列Ｊを説明するための説明図である。また、図１３は、図１２に示したヤコビ行列Ｊの擬似逆行列Ｊ^＋を説明するための説明図である。

ヤコビ行列Ｊは、図９において説明したシミュレーションモデル及び図１０に示した初期関節値θｉｎｔに基づいて算出される。ヤコビ行列Ｊの算出方法については、既知であるため、説明を省略する。対比例の制御方法では、８個の関節機構ＪＥの全て（８個の軸Ａｘ）を動作させることを前提にして、ヤコビ行列Ｊが算出される。このため、図１２に示すように、限界状態の関節機構ＪＥｅ１及びＪＥｅ２にそれぞれ対応する要素Ｊ_３及びＪ_４の値は、０に固定されていない。

図１２に示すヤコビ行列Ｊから図１３に示す擬似逆行列Ｊ^＋が算出される。擬似逆行列Ｊ^＋の算出方法については、既知であるため、説明を省略する。図１３に示す擬似逆行列Ｊ^＋が上述の式（７）の擬似逆行列Ｊ^＋として用いられ、関節移動量Δθが算出される。

図１４は、図１３に示した擬似逆行列Ｊ^＋を用いて算出された関節移動量Δθを示す図である。対比例の制御方法では、状態固定処理が実行されないため、図１４の星印で示されるように、限界状態の関節機構ＪＥｅ１及びＪＥｅ２が変位することを示す値が、関節移動量Δθとして算出される。手先が移動した後の関節値である最終関節値θｅｎｄの算出に図１４に示す関節移動量Δθが用いられた場合、関節機構ＪＥｅ１及びＪＥｅ２の状態は、図１５に示すように、可動範囲外の状態になる。

図１５は、図１４に示した関節移動量Δθに基づいて算出された最終関節値θｅｎｄを示す図である。最終関節値θｅｎｄは、図１４に示した関節移動量Δθを図１０に示した初期関節値θｉｎｔに加算することにより算出される。なお、図１５に示す最終関節値θｅｎｄは、関節移動量Δθと初期関節値θｉｎｔとの加算結果を小数点第７位で四捨五入した値である。

図１５の星印で示されるように、関節機構ＪＥｅ１の最終関節値θｅｎｄは、リンクＬＫ１Ａの可動範囲の上限である０．５［ｍ］を超えた値になり、関節機構ＪＥｅ２の最終関節値θｅｎｄは、リンクＬＫ２Ａの可動範囲の上限である０．５［ｍ］を超えた値になる。すなわち、対比例の制御方法では、逆運動学計算の正しい解（各関節機構ＪＥを可動範囲内で動作させる解）が得られず、計算結果がエラーとなる。この場合、ロボット１０に所望の動作を実行させることができない。

次に、図１６から図１９を参照しながら、状態固定処理が実行される本実施形態によるシミュレーションについて説明する。

図１６は、特定状態の関節機構ＪＥが固定された場合のヤコビ行列Ｊを説明するための説明図である。また、図１７は、図１６に示したヤコビ行列Ｊの擬似逆行列Ｊ^＋を説明するための説明図である。

関節機構ＪＥｅ１及びＪＥｅ２の状態は、図１０において説明したように、限界状態である。すなわち、関節機構ＪＥｅ１及びＪＥｅ２の状態は、限界状態に所定の余裕を持たせた特定状態でもある。このため、本実施形態では、図１６に示すように、ヤコビ行列Ｊの複数の要素のうち、特定状態の関節機構ＪＥｅ１及びＪＥｅ２にそれぞれ対応する要素Ｊ_３及びＪ_４の値は、０に設定される。なお、図１６に示すヤコビ行列Ｊは、要素Ｊ_３及びＪ_４の値が０に設定されることを除いて、図１２に示したヤコビ行列Ｊと同じである。

このように、本実施形態では、要素Ｊ_３及びＪ_４の値が０に設定されるため、要素Ｊ_３及びＪ_４にそれぞれ対応する関節機構ＪＥｅ１及びＪＥｅ２を、状態が変位しない固定関節と見なして、逆運動学計算を実行することができる。

例えば、図１６に示すヤコビ行列Ｊから図１７に示す擬似逆行列Ｊ^＋が算出される。そして、図１７に示す擬似逆行列Ｊ^＋が上述の式（７）の擬似逆行列Ｊ^＋として用いられ、関節移動量Δθが算出される。

図１８は、図１７に示した擬似逆行列Ｊ^＋を用いて算出された関節移動量Δθを示す図である。本実施形態では、ヤコビ行列Ｊの要素Ｊ_３及びＪ_４の値が０に設定されているため、図１７の星印で示されるように、要素Ｊ_３及びＪ_４にそれぞれ対応する関節機構ＪＥｅ１及びＪＥｅ２の関節移動量Δθは、０と見なせる値になる。このように、本実施形態では、限界状態の関節機構ＪＥｅ１及びＪＥｅ２の関節移動量Δθは、０と見なせる値になる。従って、図１８に示す関節移動量Δθを用いて算出された最終関節値θｅｎｄに基づく関節機構ＪＥｅ１及びＪＥｅ２の状態は、図１９に示すように、可動範囲内の状態になる。

図１９は、図１８に示した関節移動量Δθに基づいて算出された最終関節値θｅｎｄを示す図である。最終関節値θｅｎｄは、図１８に示した関節移動量Δθを図１０に示した初期関節値θｉｎｔに加算することにより算出される。なお、図１９に示す最終関節値θｅｎｄは、図１５に示した最終関節値θｅｎｄと同様に、関節移動量Δθと初期関節値θｉｎｔとの加算結果を小数点第７位で四捨五入した値である。

図１９の星印で示されるように、関節機構ＪＥｅ１の最終関節値θｅｎｄは、関節機構ＪＥｅ１の初期関節値θｉｎｔと同じ値（リンクＬＫ１Ａの可動範囲の上限である０．５［ｍ］）である。同様に、関節機構ＪＥｅ２の最終関節値θｅｎｄは、関節機構ＪＥｅ２の初期関節値θｉｎｔと同じ値（リンクＬＫ２Ａの可動範囲の上限である０．５［ｍ］）である。このように、本実施形態では、逆運動学計算の正しい解（各関節機構ＪＥを可動範囲内で動作させる解）を得ることができる。

図２０は、図１０に示した初期関節値θｉｎｔと異なる初期関節値θｉｎｔを用いたシミュレーションの結果を説明するための説明図である。

図２０の星印で示されるように、関節機構ＪＥｅ１の初期関節値θｉｎｔは、リンクＬＫ１Ａの可動範囲の下限である０．２５［ｍ］に設定され、関節機構ＪＥｅ２の初期関節値θｉｎｔは、リンクＬＫ２Ａの可動範囲の下限である０．３［ｍ］に設定されている。すなわち、関節機構ＪＥｅ１及びＪＥｅ２は、両方とも限界状態である。本シミュレーションでは、ジョグ動作によりロボット１０の手先をＸ０軸の負方向に０．００２７２［ｍ］だけ移動させる場合（手先移動量ΔｒのＸ０軸の移動量が－０．００２７２［ｍ］、Ｘ０軸以外の軸の移動量は０［ｍ］）を想定する。

すなわち、本シミュレーションでは、関節機構ＪＥｅ１及びＪＥｅ２によりリンクＬＫ１Ａ及びＬＫ２Ａが下限まで縮んだ限界状態から、ロボット１０の手先がＸ０軸の負方向に０．００２７２［ｍ］だけ移動する。

図２０では、対比例の制御方法（状態固定処理が実行されない制御方法）のシミュレーション結果（全軸駆動）と、本実施形態の制御方法（状態固定処理が実行される制御方法）のシミュレーション結果（２軸固定）が示されている。

対比例の制御方法では、全軸駆動を前提に算出されたヤコビ行列Ｊの擬似逆行列Ｊ^＋が関節移動量Δθの算出に用いられるため、最終関節値θｅｎｄは、図２０の“全軸駆動”に示すように、関節機構ＪＥｅ１及びＪＥｅ２の状態が可動範囲外の状態になる。例えば、関節機構ＪＥｅ１の最終関節値θｅｎｄは、リンクＬＫ１Ａの可動範囲の下限である０．２５［ｍ］よりも小さい値になり、関節機構ＪＥｅ２の最終関節値θｅｎｄは、リンクＬＫ２Ａの可動範囲の下限である０．３［ｍ］よりも小さい値になる。

これに対し、本実施形態の制御方法では、限界状態の関節機構ＪＥｅ１及びＪＥｅ２にそれぞれ対応する要素Ｊ３及びＪ４の値が０に設定されたヤコビ行列Ｊの擬似逆行列Ｊ^＋が関節移動量Δθの算出に用いられる。このため、本実施形態の制御方法では、図２０の“２軸固定”に示すように、関節機構ＪＥｅ１及びＪＥｅ２の状態は、可動範囲内の状態に維持される。例えば、関節機構ＪＥｅ１の最終関節値θｅｎｄは、リンクＬＫ１Ａの可動範囲の下限である０．２５［ｍ］であり、関節機構ＪＥｅ２の最終関節値θｅｎｄは、リンクＬＫ２Ａの可動範囲の下限である０．３［ｍ］である。

このように、限界状態の関節機構ＪＥが存在する場合のジョグ動作を考えた場合、全軸（８軸）での逆運動学計算では、限界状態の関節機構ＪＥが可動範囲外の状態になる。これに対し、本実施形態では、限界状態の関節機構ＪＥを固定関節と見なして、残りの６軸で逆運動学計算が行われるため、限界状態の関節機構ＪＥは、変位しない。従って、本実施形態では、各関節機構ＪＥを可動範囲内でジョグ動作を継続させることができる。

なお、図９から図２０において説明したシミュレーションでは、限界状態の関節機構ＪＥが２個である場合を想定したが、限界状態の関節機構ＪＥの数は、２個に限定されない。例えば、限界状態の関節機構ＪＥの数は、１個でもよいし、３個以上ｍ個未満（ｍは、関節機構ＪＥの全数）でもよい。また、限界状態に所定の余裕を持たせた特定状態の関節機構ＪＥが固定関節と見なされてもよい。

ここで、逆運動学計算を実際に行う場面の例を以下に示す。１つ目は、ロボット１０をジョグ動作させる場合である。ジョグ動作では、ロボット１０に、目標位置が指示されるため、指示された目標位置に移動するための各関節の角度（移動量）が逆運動学計算により算出される。２つ目は、ロボット１０の軌道生成時である。ロボット１０に繰り返し動作を実行させる場合、事前に軌道を覚えこませておく必要がある。事前に軌道生成する場合に、軌道上の複数の点における各関節の角度が逆運動学計算により算出される。なお、軌道上の点の数が少ない場合には、ロボット１０を実際に動作させる際に、点と点の間の軌道を算出して補完することもある。３つ目は、ロボット１０にビジョンカメラ等が搭載されており、ビジョン情報等に基づいて、ロボット１０の軌道を生成（又は軌道修正）する場合である。この場合も、１つ目のジョグ動作と同様に新たな目標位置が指示される。

以上、本実施形態では、ロボットシステム１は、７個以上の複数の関節機構ＪＥを有する多関節ロボットであるロボット１０と、ロボット１０の動作を制御するロボットコントローラ３０と、を有する。

ロボット１０の制御方法は、ロボット１０を動作させる複数の関節機構ＪＥの変位量を計算する逆運動学計算を実行することにより、複数の関節機構ＪＥの各々の状態に関する関節値を算出する状態算出処理と、複数の関節機構ＪＥのうち、関節値に基づく状態が、あらかじめ定められた特定状態（例えば、関節機構ＪＥの変位に関する制約が満たされる限界の状態に所定の余裕を持たせた状態）である関節機構ＪＥに対して実行する状態固定処理と、を含む単位処理を繰り返し実行し、状態固定処理を実行した場合、状態算出処理において算出された複数の関節機構ＪＥの関節値のうち、特定状態の関節機構ＪＥの関節値は実質的に変位させることなく、特定状態でない関節機構ＪＥの関節値を変位させる。なお、特定状態は、関節値が、あらかじめ定められた各関節機構ＪＥの可動領域の領域内の、可動領域の領域外との境界領域の値となる状態であってもよい。

ロボットコントローラ３０は、上述の単位処理を繰り返し実行する動作制御部３３を有する。例えば、制御プログラムＰＧｒは、ロボットコントローラ３０に含まれる処理装置３２を上述の動作制御部３３として機能させる。

このように、本実施形態では、特定状態の関節機構ＪＥを状態が変位しない固定関節と見なして、残りの関節機構ＪＥを用いて逆運動学計算を実行する。このため、本実施形態では、特定状態の関節機構ＪＥを変位させずに、逆運動学計算を実行することができる。この結果、本実施形態では、逆運動学計算の正しい解（各関節機構ＪＥを可動範囲内で動作させる解）を得ることができる。すなわち、本実施形態では、逆運動学計算の結果がエラー（例えば、各関節機構ＪＥを可動範囲内で動作させる解が得られないエラー）となる頻度を低減することができる。この結果、本実施形態では、各関節機構ＪＥを可動範囲内でロボット１０を継続して動作させることができる。

また、本実施形態では、ロボットコントローラ３０は、複数の関節機構ＪＥの中に特定状態の関節機構ＪＥが含まれる場合、複数の関節機構ＪＥの関節値の更新をキャンセルしてもよい。この場合、単位処理では、複数の関節機構ＪＥの中に特定状態の関節機構ＪＥが含まれる場合、前回の単位処理の状態算出処理において算出された複数の関節機構ＪＥの各々の関節値への複数の関節機構ＪＥの各々の関節値の更新がキャンセルされ、関節値の更新がキャンセルされる前の関節値に基づいて、状態固定処理の対象となる特定状態の関節が特定され、状態固定処理が実行された後に状態算出処理が実行される。これにより、本実施形態では、状態算出処理において算出された関節値への更新を、常に、特定状態でない複数の関節機構ＪＥの関節値に対して実行することができる。この結果、本実施形態では、関節機構ＪＥが特定状態に固定された状態で、単位処理が繰り返されることを抑制することができる。

また、本実施形態では、ロボットコントローラ３０は、単位処理が繰り返し実行される前の初期状態の複数の関節機構ＪＥの中に特定状態の関節機構ＪＥが含まれる場合、繰り返し実行される単位処理のうちの１回目の単位処理が実行される前に、特定状態の関節機構ＪＥの関節値を、特定状態でない状態に対応する関節値に変更してもよい。これにより、本実施形態では、繰り返し実行される単位処理のうちの１回目の単位処理において、各関節機構ＪＥの関節値の初期値が特定状態に対応する関節値になることを抑制することができる。この結果、本実施形態では、単位処理が繰り返し実行される前の初期状態の複数の関節機構ＪＥの中に特定状態の関節機構ＪＥが含まれる場合においても、当該関節機構ＪＥの状態が特定状態に固定されることを抑制することができる。

また、本実施形態では、ロボットコントローラ３０は、状態算出処理において、ヤコビ行列Ｊを用いて逆運動学計算を実行し、状態固定処理において、ヤコビ行列Ｊの複数の要素のうち、特定状態の関節機構ＪＥに係る要素の値を実質的に０に設定することにより、特定状態の関節機構ＪＥの関節値を固定値に設定する。このように、本実施形態では、ヤコビ行列Ｊの複数の要素のうちの特定状態の関節機構ＪＥに係る要素の値を実質的に０に設定することにより、特定状態の関節機構ＪＥの関節値を固定値に容易に設定することができる。すなわち、特定状態の関節が有った場合でも、ヤコビ行列Ｊの行数及び列数を変更することなく、また複数の行列に分割することなく、逆運動学計算を継続して実行し各関節の関節値を算出することができる。

また、本実施形態では、複数の関節機構ＪＥが、少なくとも１個の直動関節（例えば、関節機構ＪＥｐ又はＪＥｅ等）を含む。この場合においても、本実施形態では、可動範囲の上限又は下限に近い特定状態の直動関節を固定関節と見なして、残りの関節機構ＪＥを用いて逆運動学計算を実行するため、逆運動学計算の結果がエラーとなる頻度を低減することができる。従って、本実施形態では、直動関節を含む多関節ロボット等の多種多様な多関節ロボットを継続して動作させることができる。

また、本実施形態では、ロボット１０は、ボディ部ＢＤＰと、リンクＬＫ１と、リンクＬＫ２と、先端部ＴＰ１と、ボディ部ＢＤＰの底面ＢＤＰｂｔに垂直な方向Ｄｖ１とのなす角度が所定角度以下の軸Ａｘ１を第１回転軸として、ボディ部ＢＤＰの少なくとも一部分を回転させる関節機構ＪＥｒ１と、ボディ部ＢＤＰとリンクＬＫ１を接続し、ボディ部ＢＤＰの底面ＢＤＰｂｔに垂直な方向Ｄｖ１とのなす角度が所定角度より大きい軸Ａｘ２を第２回転軸としてリンクＬＫ１を回転させる関節機構ＪＥｒ２と、リンクＬＫ１とリンクＬＫ２を接続し、リンクＬＫ１が延在する方向Ｄｅ１とのなす角度が所定角度より大きい軸Ａｘ３を第３回転軸としてリンクＬＫ２をリンクＬＫ１に対して回転させる関節機構ＪＥｒ３と、リンクＬＫ２と先端部ＴＰ１を接続し、リンクＬＫ２が延在する方向Ｄｅ２とのなす角度が所定角度より大きい軸Ａｘ４を第４回転軸として、先端部ＴＰ１をリンクＬＫ２に対して回転させる関節機構ＪＥｒ４と、リンクＬＫ１の延在方向（方向Ｄｅ１）に沿って、関節機構ＪＥｒ３をリンクＬＫ１に対して相対的に移動させる関節機構ＪＥｐ１と、リンクＬＫ２の延在方向（方向Ｄｅ２）に沿って、リンクＬＫ２を関節機構ＪＥｒ３に対して相対的に移動させる関節機構ＪＥｐ２と、有する。先端部ＴＰ１は、リンクＬＫ２に接続される第１部分ＴＰ１１と、第１部分ＴＰ１１に接続される第２部分ＴＰ１２と、第１部分ＴＰ１１と第２部分ＴＰ１２を接続し、第４回転軸とのなす角度が所定角度より大きい軸Ａｘ５を第５回転軸として、第２部分ＴＰ１２を第１部分ＴＰ１１に対して回転させる関節機構ＪＥｒ５と、第５回転軸とのなす角度が所定角度より大きい軸Ａｘ６を第６回転軸として、先端部ＴＰ１の少なくとも一部分を回転させる関節機構ＪＥｒ６と、を含む。複数の関節機構ＪＥは、関節機構ＪＥｒ１、関節機構ＪＥｒ２、関節機構ＪＥｒ３、関節機構ＪＥｒ４、関節機構ＪＥｒ５、関節機構ＪＥｒ６、関節機構ＪＥｐ１及び関節機構ＪＥｐ２である。

このように、本実施形態では、６個の回転関節及び２個の直動関節を含む多関節ロボットであるロボット１０を継続して動作させることができる。このようなロボットの場合、２個の直動関節は冗長関節として機能することが多く、関節値が頻繁に特定状態となってしまう。従って直動関節を有するロボットの場合は、図９から図２０を用いて説明したシミュレーションのように、本直動関節に固定処理を実行することにより、非常に効率的にロボットの制御を実行することができる。

また、本実施形態では、部品を組み付ける、又は、部品を取り除くことを含む物品の製造方法にロボットシステム１が用いられてもよい。この場合、部品を組み付ける、又は、部品を取り除く作業が、逆運動学計算の結果がエラーとなることにより停止することを、抑止することができる。すなわち、本実施形態では、部品を組み付ける、又は、部品を取り除く作業を効率よく実行することができる。

［２．変形例］
本発明は、以上に例示した実施形態に限定されない。具体的な変形の態様を以下に例示する。以下の例示から任意に選択された２以上の態様を併合してもよい。

［第１変形例］
上述した実施形態では、関節機構ＪＥｒ４が、リンクＬＫ２と先端部ＴＰ１を接続する場合を例示したが、本発明はこのような態様に限定されるものではない。例えば、関節機構ＪＥｒ４は、リンクＬＫ２に含まれてもよい。

図２１は、第１変形例に係るロボット１０の一例を説明するための説明図である。図１から図９において説明した要素と同様の要素については、同様の符号を付し、詳細な説明を省略する。

本変形例に係るロボット１０は、例えば、図１に示した関節機構ＪＥｐ（ＪＥｐ１及びＪＥｐ２）の代わりに関節機構ＪＥｅ（ＪＥｅ１及びＪＥｅ２）を有する８軸多関節ロボットである。例えば、ロボット１０は、関節機構ＪＥｒ１Ａ、ＪＥｒ２、ＪＥｒ３Ａ、ＪＥｒ４Ａ、ＪＥｒ５Ａ、ＪＥｒ６、ＪＥｅ１及びＪＥｅ２と、ボディ部ＢＤＰａと、リンクＬＫ１Ａ及びＬＫ２Ａと、先端部ＴＰ１Ａとを有する。なお、関節機構ＪＥｅ１は、リンクＬＫ１Ａに設けられ、関節機構ＪＥｅ２及びＪＥｒ４Ａは、リンクＬＫ２Ａに設けられる。

ボディ部ＢＤＰａは、「基部」の例である。関節機構ＪＥｒ１Ａは、「第１駆動機構」の例であり、関節機構ＪＥｒ３Ａは、「第３駆動機構」の例であり、関節機構ＪＥｒ５Ａは、「第５駆動機構」の例である。

ボディ部ＢＤＰａは、関節機構ＪＥｒ１Ａを介して床等の所定の場所に固定される。関節機構ＪＥｒ１Ａは、ボディ部ＢＤＰａの底面ＢＤＰｂｔに垂直な軸Ａｘ１を回転軸として、ボディ部ＢＤＰａを回転させる。

関節機構ＪＥｒ２は、ボディ部ＢＤＰａとリンクＬＫ１Ａの支持部分ＬＫ１ａを接続し、ボディ部ＢＤＰａの底面ＢＤＰｂｔに平行な軸Ａｘ２を回転軸としてリンクＬＫ１Ａをボディ部ＢＤＰａに対して回転させる。

リンクＬＫ１Ａは、例えば、リンクＬＫ１Ａが延在する方向Ｄｅ１に沿って伸縮可能に構成される。例えば、リンクＬＫ１Ａは、ボディ部ＢＤＰａに接続される支持部分ＬＫ１ａと、可動部分ＬＫ１ｂ及びＬＫ１ｃと、関節機構ＪＥｅ１とを含む。また、リンクＬＫ２Ａは、例えば、リンクＬＫ２Ａが延在する方向Ｄｅ２に沿って伸縮可能に構成される。例えば、リンクＬＫ２Ａは、リンクＬＫ１Ａの可動部分ＬＫ１ｃに接続される支持部分ＬＫ２ａと、可動部分ＬＫ２ｂ及びＬＫ２ｃと、関節機構ＪＥｅ２と、関節機構ＪＥｒ４Ａとを含む。

リンクＬＫ（ＬＫ１Ａ及びＬＫ２Ａ）と関節機構ＪＥｅ（ＪＥｅ１及びＪＥｅ２）とについては、図９において説明しているため、詳細な説明を省略する。例えば、支持部分ＬＫ１ａは、中空である。そして、リンクＬＫ１Ａが収縮した場合、可動部分ＬＫ１ｂの少なくとも一部が支持部分ＬＫ１ａの内部に格納される。また、例えば、支持部分ＬＫ２ａは、中空である。そして、リンクＬＫ２Ａが収縮した場合、可動部分ＬＫ２ｂの少なくとも一部が支持部分ＬＫ２ａの内部に格納される。

関節機構ＪＥｒ３Ａは、リンクＬＫ１Ａの可動部分ＬＫ１ｃとリンクＬＫ２Ａの支持部分ＬＫ２ａを接続し、リンクＬＫ１Ａが延在する方向Ｄｅ１に垂直な軸Ａｘ３を回転軸としてリンクＬＫ２ＡをリンクＬＫ１Ａに対して回転させる。

関節機構ＪＥｒ４Ａは、リンクＬＫ２Ａが延在する方向Ｄｅ２と平行な軸Ａｘ４Ａを回転軸として、可動部分ＬＫ２ｃを支持部分ＬＫ２ａに対して回転させる。関節機構ＪＥｒ４Ａは、「第４駆動機構」の例であり、軸Ａｘ４Ａは、「第４回転軸」の例である。

関節機構ＪＥｒ５Ａは、リンクＬＫ２Ａの可動部分ＬＫ２ｃと先端部ＴＰ１Ａを接続し、リンクＬＫ２Ａが延在する方向Ｄｅ２に垂直な軸Ａｘ５を回転軸として先端部ＴＰ１ＡをリンクＬＫ２Ａに対して回転させる。

先端部ＴＰ１Ａは、関節機構ＪＥｒ５Ａを介してリンクＬＫ２の可動部分ＬＫ２ｃに接続される第２部分ＴＰ１２Ａと、関節機構ＪＥｒ６とを含む。関節機構ＪＥｒ６は、図１に示した関節機構ＪＥｒ６と同様である。

なお、本変形例に係るロボット１０の構成は、図２１に示す例に限定されない。例えば、図２１に示す例では、関節機構ＪＥｒ４Ａは、方向Ｄｅ２に沿って、リンクＬＫ２Ａの可動部分ＬＫ２ｃと一体的に支持部分ＬＫ２ａに対して移動するが、支持部分ＬＫ２ａに固定されていてもよい。この場合、関節機構ＪＥｒ４Ａは、軸Ａｘ４Ａを回転軸として可動部分ＬＫ２ｂを支持部分ＬＫ２ａに対して回転させ、関節機構ＪＥｅ２は、可動部分ＬＫ２ｃを可動部分ＬＫ２ｂに対して移動させてもよい。可動部分ＬＫ２ｃが可動部分ＬＫ２ｂに対して移動する構成では、例えば、可動部分ＬＫ２ｂは、中空である。そして、リンクＬＫ２Ａが収縮した場合、可動部分ＬＫ２ｃの少なくとも一部が可動部分ＬＫ２ｂの内部に格納される。また、関節機構ＪＥｅ１及びＪＥｅ２の一方は、省かれてもよい。

以上、本変形例では、ロボット１０は、ボディ部ＢＤＰａと、支持部分ＬＫ１ａ及び可動部分ＬＫ１ｃを含むリンクＬＫ１Ａと、支持部分ＬＫ２ａ及び可動部分ＬＫ２ｃを含むリンクＬＫ２Ａと、先端部ＴＰ１Ａと、ボディ部ＢＤＰａの底面ＢＤＰｂｔに垂直な方向Ｄｖ１とのなす角度が所定角度以下の軸Ａｘ１を第１回転軸として、ボディ部ＢＤＰａの少なくとも一部分を回転させる関節機構ＪＥｒ１Ａと、ボディ部ＢＤＰａと支持部分ＬＫ１ａを接続し、ボディ部ＢＤＰａの底面ＢＤＰｂｔに垂直な方向Ｄｖ１とのなす角度が所定角度より大きい軸Ａｘ２を第２回転軸としてリンクＬＫ１Ａをボディ部ＢＤＰａに対して回転させる関節機構ＪＥｒ２と、可動部分ＬＫ１ｃと支持部分ＬＫ２ａを接続し、リンクＬＫ１Ａが延在する方向Ｄｅ１とのなす角度が所定角度より大きい軸Ａｘ３を第３回転軸としてリンクＬＫ２ＡをリンクＬＫ１Ａに対して回転させる関節機構ＪＥｒ３Ａと、リンクＬＫ２Ａが延在する方向Ｄｅ２とのなす角度が所定角度以下の軸Ａｘ４Ａを第４回転軸として、可動部分ＬＫ２ｃを支持部分ＬＫ２ａに対して回転させる関節機構ＪＥｒ４Ａと、可動部分ＬＫ２ｃと先端部ＴＰ１Ａを接続し、リンクＬＫ２Ａの延在方向（方向Ｄｅ２）とのなす角度が所定角度より大きい軸Ａｘ５を第５回転軸として先端部ＴＰ１ＡをリンクＬＫ２Ａに対して回転させる関節機構ＪＥｒ５Ａと、第５回転軸とのなす角度が所定角度より大きい軸Ａｘ６を第６回転軸として、先端部ＴＰ１Ａの少なくとも一部分をリンクＬＫ２Ａに対して回転させる関節機構ＪＥｒ６と、支持部分ＬＫ１ａに対して可動部分ＬＫ１ｃをリンクＬＫ１Ａの延在方向（方向Ｄｅ１）に沿って移動させることにより、リンクＬＫ１Ａを伸縮させる関節機構ＪＥｅ１と、支持部分ＬＫ２ａに対して可動部分ＬＫ２ｃをリンクＬＫ２Ａの延在方向（方向Ｄｅ２）に沿って移動させることにより、リンクＬＫ２Ａを伸縮させる関節機構ＪＥｅ２と、を含む。複数の関節機構ＪＥは、関節機構ＪＥｒ１Ａ、関節機構ＪＥｒ２、関節機構ＪＥｒ３Ａ、関節機構ＪＥｒ４Ａ、関節機構ＪＥｒ５Ａ、関節機構ＪＥｒ６、関節機構ＪＥｅ１及び関節機構ＪＥｅ２である。本変形例においても、上述した実施形態と同様の効果を得ることができる。

［第２変形例］
上述した実施形態では、関節機構ＪＥｒ４が、リンクＬＫ２が延在する方向Ｄｅ２に垂直な軸Ａｘ４を回転軸として、先端部ＴＰ１をリンクＬＫ２に対して回転させる場合を例示したが、本発明はこのような態様に限定されるものではない。例えば、関節機構ＪＥｒ４は、リンクＬＫ２が延在する方向Ｄｅ２とのなす角度が所定角度以下の軸を回転軸として、先端部ＴＰ１をリンクＬＫ２に対して回転させてもよい。

図２２は、第２変形例に係る先端部ＴＰ１Ｂの一例を説明するための説明図である。図１から図２１において説明した要素と同様の要素については、同様の符号を付し、詳細な説明を省略する。

例えば、本変形例に係るロボット１０は、図１に示したリンクＬＫ２、関節機構ＪＥｒ４及び先端部ＴＰ１の代わりにリンクＬＫ２Ｂ、関節機構ＪＥｒ４Ｂ及び先端部ＴＰ１Ｂを有することを除いて、図１に示したロボット１０と同様である。リンクＬＫ２Ｂは、関節機構ＪＥｒ４の代わりに関節機構ＪＥｒ４Ｂが接続されることを除いて、リンクＬＫ２と同様である。なお、リンクＬＫ２Ｂは、「第２リンク」の例であり、関節機構ＪＥｒ４Ｂは、「第４駆動機構」の例である。

関節機構ＪＥｒ４Ｂは、リンクＬＫ２Ｂと先端部ＴＰ１Ｂを接続し、方向Ｄｅ２に平行な軸Ａｘ４Ａを回転軸として、先端部ＴＰ１ＢをリンクＬＫ２Ｂに対して回転させる。図１１の回転方向Ｄｒ４は、軸Ａｘ４Ａを回転軸として回転する場合の先端部ＴＰ１Ｂの回転方向を示す。なお、軸Ａｘ４Ａは、「第４回転軸」の例であり、リンクＬＫ２Ｂが延在する方向Ｄｅ２とのなす角度が所定角度以下の軸に該当する。

先端部ＴＰ１Ｂにおいても、図１に示した先端部ＴＰ１と同様に、エンドエフェクタ２０が端面ＴＰ１ｓｆに取り付けられる。先端部ＴＰ１Ｂは、リンクＬＫ２Ｂに接続される第１部分ＴＰ１１Ａと、第１部分ＴＰ１１Ａに接続される第２部分ＴＰ１２Ａと、関節機構ＪＥｒ５Ａと、関節機構ＪＥｒ６とを含む。第１部分ＴＰ１１Ａは、例えば、関節機構ＪＥｒ４Ｂを介してリンクＬＫ２Ｂに接続される。従って、第１部分ＴＰ１１Ａは、軸Ａｘ４Ａを回転軸としてリンクＬＫ２Ｂに対して回転する。

関節機構ＪＥｒ５Ａは、第１部分ＴＰ１１Ａと第２部分ＴＰ１２Ａを接続し、軸Ａｘ４Ａに垂直な軸Ａｘ５を回転軸として、第２部分ＴＰ１２Ａを第１部分ＴＰ１１Ａに対して回転させる。図１の回転方向Ｄｒ５は、軸Ａｘ５を回転軸として回転する場合の第２部分ＴＰ１２Ａの回転方向を示す。

関節機構ＪＥｒ６は、図１に示した関節機構ＪＥｒ６と同様である。例えば、関節機構ＪＥｒ６は、軸Ａｘ５に垂直な軸Ａｘ６を回転軸として、先端部ＴＰ１Ｂの少なくとも一部分（例えば、端面ＴＰ１ｓｆ）を回転させる。図１１に示す例では、図１に示した関節機構ＪＥｒ６と同様に、関節機構ＪＥｒ６の表面が端面ＴＰ１ｓｆに該当する。なお、関節機構ＪＥｒ６が第２部分ＴＰ１２Ａに含まれる構成等では、第２部分ＴＰ１２Ａの端面が端面ＴＰ１ｓｆであってもよい。

以上、本変形例では、関節機構ＪＥｒ４Ｂは、方向Ｄｅ２とのなす角度が所定角度以下の軸Ａｘ４Ａを第４回転軸として、先端部ＴＰ１ＢをリンクＬＫ２Ｂに対して回転させる。先端部ＴＰ１Ｂは、リンクＬＫ２Ｂに接続される第１部分ＴＰ１１と、第１部分ＴＰ１１に接続される第２部分ＴＰ１２と、関節機構ＪＥｒ５と、関節機構ＪＥｒ６とを含む。関節機構ＪＥｒ５は、第１部分ＴＰ１１と第２部分ＴＰ１２を接続し、第４回転軸とのなす角度が所定角度より大きい軸Ａｘ５を第５回転軸として、第２部分ＴＰ１２を第１部分ＴＰ１１に対して回転させる。関節機構ＪＥｒ６は、第５回転軸とのなす角度が所定角度より大きい軸Ａｘ６を第６回転軸として、先端部ＴＰ１の少なくとも一部分を回転させる。本変形例においても、上述した実施形態と同様の効果を得ることができる。

［第３変形例］
上述した実施形態及び変形例では、６個の回転関節及び２個の直動関節を含む多関節ロボットをロボット１０として例示したが、本発明はこのような態様に限定されるものではない。例えば、ロボット１０は、７軸の多関節ロボットであってもよいし、９軸以上の多関節ロボットであってもよい。以上、本変形例においても、上述した実施形態及び変形例と同様の効果を得ることができる。

［３．応用例］
上述した実施形態及び変形例において説明したロボット１０を含むロボットシステム１は、部品を組み付ける、又は、部品を取り除くことを含む物品の製造方法に用いられてもよい。

［４．その他］
上述した実施形態において簡単に説明した「旋回」と他の回転との区別について、いくつかの例を挙げて説明する。

図２３は、旋回の一例を説明するための説明図である。図２３では、長手方向を把握可能な２個のリンクＬＫｉ及びＬＫｊの接続を例にして、旋回と他の回転との区別について説明する。図２３の延在方向Ｄｅｉは、リンクＬＫｉが延在する方向を示し、延在方向Ｄｅｊは、リンクＬＫｊが延在する方向を示す。また、図２３の関節機構ＪＥｒｉは、リンクＬＫｉとリンクＬＫｊを接続し、軸Ａｘｉを回転軸として、リンクＬＫｊをリンクＬＫｉに対して回転させる。

図２３に示す例では、リンクＬＫｉの延在方向Ｄｅｉ（特定の方向）と軸Ａｘｉとのなす角度βが所定角度より大きい場合、当該軸Ａｘｉを回転軸とした回転は、「旋回」に該当する。すなわち、リンクＬＫｉの延在方向Ｄｅｉと軸Ａｘｉとのなす角度βが所定角度以下の場合、当該軸Ａｘｉを回転軸とした回転は、旋回以外の回転（旋回と区別される他の回転）に該当する。図２３に示す「回転」は、旋回以外の回転を示す。また、所定角度は特に限定されないが、図２３では、所定角度が４５°である場合を想定する。延在方向Ｄｅｉと軸Ａｘｉとのなす角度βは、延在方向Ｄｅｉに対する軸Ａｘｉの角度として把握される複数の角度（例えば、互いに交差する２個の直線では４個の角度、又は、平行な２個の直線では０°及び１８０°）のうち、０°以上９０°以下の角度である。

第１パターンでは、リンクＬＫｉの延在方向Ｄｅｉと軸Ａｘｉとのなす角度βは、９０°であり、所定角度（４５°）よりも大きい。従って、第１パターンでは、軸Ａｘｉを回転軸としたリンクＬＫｊの回転は、旋回である。また、第１パターンでは、リンクＬＫｊの延在方向Ｄｅｊは、軸Ａｘｉに垂直である。なお、第１パターンでは、リンクＬＫｊが軸Ａｘｉを回転軸として回転（旋回）した場合、リンクＬＫｉの延在方向Ｄｅｉに対するリンクＬＫｊの延在方向Ｄｅｊの角度は、変化する。

第２パターンでは、リンクＬＫｉの延在方向Ｄｅｉと軸Ａｘｉとのなす角度βは、０°であり、所定角度（４５°）以下である。従って、第２パターンでは、軸Ａｘｉを回転軸としたリンクＬＫｊの回転は、旋回以外の回転である。また、第２パターンでは、リンクＬＫｊの延在方向Ｄｅｊは、リンクＬＫｉの延在方向Ｄｅｉ及び軸Ａｘｉに平行である。すなわち、リンクＬＫｉの延在方向Ｄｅｉに対するリンクＬＫｊの延在方向Ｄｅｊの角度は、０°である。なお、第２パターンでは、リンクＬＫｊが軸Ａｘｉを回転軸として回転しても、リンクＬＫｉの延在方向Ｄｅｉに対するリンクＬＫｊの延在方向Ｄｅｊの角度は、０°に維持され、常に一定である。

第３パターンでは、リンクＬＫｉの延在方向Ｄｅｉと軸Ａｘｉとのなす角度βは、０°であり、所定角度（４５°）以下である。従って、第３パターンでは、軸Ａｘｉを回転軸としたリンクＬＫｊの回転は、旋回以外の回転である。また、第３パターンでは、リンクＬＫｊの延在方向Ｄｅｊは、リンクＬＫｉの延在方向Ｄｅｉ及び軸Ａｘｉに垂直である。すなわち、リンクＬＫｉの延在方向Ｄｅｉに対するリンクＬＫｊの延在方向Ｄｅｊの角度は、９０°である。なお、第３パターンでは、リンクＬＫｊが軸Ａｘｉを回転軸として回転しても、リンクＬＫｉの延在方向Ｄｅｉに対するリンクＬＫｊの延在方向Ｄｅｊの角度は、９０°に維持され、常に一定である。

第４パターンでは、リンクＬＫｉの延在方向Ｄｅｉと軸Ａｘｉとのなす角度βは、１０°であり、所定角度（４５°）以下である。従って、第４パターンでは、軸Ａｘｉを回転軸としたリンクＬＫｊの回転は、旋回以外の回転である。また、第４パターンでは、リンクＬＫｊの延在方向Ｄｅｊは、軸Ａｘｉに平行であり、リンクＬＫｉの延在方向Ｄｅｉに対するリンクＬＫｊの延在方向Ｄｅｊの角度は、１０°である。なお、第４パターンでは、リンクＬＫｊが軸Ａｘｉを回転軸として回転しても、リンクＬＫｉの延在方向Ｄｅｉに対するリンクＬＫｊの延在方向Ｄｅｊの角度は、１０°に維持され、常に一定である。

第５パターンでは、リンクＬＫｉの延在方向Ｄｅｉと軸Ａｘｉとのなす角度βは、７０°であり、所定角度（４５°）よりも大きい。従って、第５パターンでは、軸Ａｘｉを回転軸としたリンクＬＫｊの回転は、旋回である。また、第５パターンでは、リンクＬＫｊの延在方向Ｄｅｊは、軸Ａｘｉに垂直である。なお、第５パターンでは、リンクＬＫｊが軸Ａｘｉを回転軸として回転（旋回）した場合、リンクＬＫｉの延在方向Ｄｅｉに対するリンクＬＫｊの延在方向Ｄｅｊの角度は、変化する。

第６パターンでは、リンクＬＫｉの延在方向Ｄｅｉと軸Ａｘｉとのなす角度βは、１０°であり、所定角度（４５°）以下である。従って、第６パターンでは、軸Ａｘｉを回転軸としたリンクＬＫｊの回転は、旋回以外の回転である。また、第６パターンでは、リンクＬＫｊの延在方向Ｄｅｊは、軸Ａｘｉに垂直である。なお、第６パターンでは、リンクＬＫｊが軸Ａｘｉを回転軸として回転した場合、リンクＬＫｉの延在方向Ｄｅｉに対するリンクＬＫｊの延在方向Ｄｅｊの角度は、変化する。

第７パターンでは、リンクＬＫｉの延在方向Ｄｅｉと軸Ａｘｉとのなす角度βは、７０°であり、所定角度（４５°）よりも大きい。従って、第７パターンでは、軸Ａｘｉを回転軸としたリンクＬＫｊの回転は、旋回である。また、第７パターンでは、リンクＬＫｊの延在方向Ｄｅｊは、軸Ａｘｉに平行であり、リンクＬＫｉの延在方向Ｄｅｉに対するリンクＬＫｊの延在方向Ｄｅｊの角度は、７０°である。なお、第７パターンでは、リンクＬＫｊが軸Ａｘｉを回転軸として回転しても、リンクＬＫｉの延在方向Ｄｅｉに対するリンクＬＫｊの延在方向Ｄｅｊの角度は、７０°に維持され、常に一定である。

このように、上述した実施形態及び変形例では、リンクＬＫｉに対するリンクＬＫｊの回転のうち、リンクＬＫｉの延在方向Ｄｅｉとのなす角度が所定角度より大きい軸Ａｘｉを回転軸とした回転が、旋回とも称される。但し、「旋回」の定義は、上述の例に限定されない。例えば、リンクＬＫｉの延在方向Ｄｅｉとのなす角度が所定角度より大きい軸Ａｘｉを回転軸とした回転を旋回とする上述の定義を第１定義とした場合、第１定義の代わりに、下記の第２定義又は第３定義が採用されてもよい。

第２定義では、リンクＬＫｉに対するリンクＬＫｊの回転により、リンクＬＫｉの延在方向Ｄｅｉに対するリンクＬＫｊの延在方向Ｄｅｊの角度が変化する場合、当該回転が旋回に該当する。従って、第２定義では、リンクＬＫｉの延在方向Ｄｅｉに対するリンクＬＫｊの延在方向Ｄｅｊの角度が、回転しても常に一定の場合、当該回転は、旋回以外の回転に該当する。例えば、第２定義では、図２３に示した第１パターン、第５パターン及び第６パターンは、旋回に該当し、第２パターン、第３パターン、第４パターン及び第７パターンは、旋回以外の回転に該当する。

第３定義では、回転するリンクＬＫｊの延在方向ＤｅｊとリンクＬＫｊの回転軸（軸Ａｘｉ）とのなす角度が所定角度より大きい場合、当該回転が旋回に該当する。従って、第３定義では、リンクＬＫｊの延在方向ＤｅｊとリンクＬＫｊの回転軸（軸Ａｘｉ）とのなす角度が所定角度以下の場合、当該回転は、旋回以外の回転に該当する。例えば、第３定義では、図２３に示した第１パターン、第３パターン、第５パターン及び第６パターンは、旋回に該当し、第２パターン、第４パターン及び第７パターンは、旋回以外の回転に該当する。

また、上述の第１定義、第２定義及び第３定義とは別に、互いに隣接する２個の関節機構ＪＥｒのそれぞれの回転軸の関係に着目して、２個の関節機構ＪＥｒによる２個の回転の相対関係を定義してもよい。具体的には、２個の回転軸のなす角度が所定角度以下である場合（典型的には、平行の場合）、２個の回転を同種の回転とし、２個の回転軸のなす角度が所定角度よりも大きい場合（典型的には、直交する場合）、２個の回転を異種の回転としてもよい。なお、同種の回転とは、２個の回転とも旋回、又は、２個の回転とも旋回以外の回転であり、異種の回転とは、２個の回転の一方が旋回で他方が旋回以外の回転である。２個の回転の相対関係の定義が用いられる場合、相対関係の起点となる回転は、例えば、上述の第１定義、第２定義及び第３定義のいずれかに基づいて決められてもよい。図２３に示した第１パターンは、第１定義、第２定義及び第３定義のいずれにおいても、旋回に該当し、第２パターンは、第１定義、第２定義及び第３定義のいずれにおいても、旋回以外の回転に該当する。従って、第１パターン又は第２パターンを、相対関係の起点となる回転とすることが好ましい。

また、上述の第１定義、第２定義及び第３定義の２以上の定義を組み合わせた定義が用いられてもよい。この場合、例えば、組み合わせる２以上の定義の全てで旋回に該当する回転のみを旋回としてもよいし、組み合わせる２以上の定義の少なくとも１つで旋回に該当する回転を旋回としてもよい。

１…ロボットシステム、１０…ロボット、２０…エンドエフェクタ、３０…ロボットコントローラ、３２…処理装置、３３…動作制御部、３５…メモリ、３６…通信装置、３７…操作装置、３８…表示装置、３９…ドライバ回路、Ａｘ１、Ａｘ２、Ａｘ３、Ａｘ４、Ａｘ４Ａ、Ａｘ５、Ａｘ６、Ａｘｉ…軸、ＢＤＰ、ＢＤＰａ…ボディ部、ＢＤＰｂｔ…底面、ＢＤＰｂａ…土台部、ＪＥｅ１、ＪＥｅ２、ＪＥｐ１、ＪＥｐ２、ＪＥｒ１、ＪＥｒ２、ＪＥｒ３、ＪＥｒ３Ａ、ＪＥｒ４、ＪＥｒ４Ａ、ＪＥｒ５、ＪＥｒ５Ａ、ＪＥｒ６、ＪＥｒｉ…関節機構、ＬＫ１、ＬＫ１Ａ、ＬＫ２、ＬＫ２Ａ、ＬＫｉ、ＬＫｊ…リンク、ＬＫ１ａ、ＬＫ２ａ…支持部分、ＬＫ１ｂ、ＬＫ１ｃ、ＬＫ２ｂ、ＬＫ２ｃ…可動部分、ＭＯｒ１、ＭＯｒ２、ＭＯｒ３、ＭＯｒ４、ＭＯｒ５、ＭＯｒ６、ＭＯｐ１、ＭＯｐ２…モータ。

Claims (12)

1. ７個以上の複数の関節を有する多関節ロボットの制御方法であって、
   前記多関節ロボットを動作させる前記複数の関節の変位量を計算する逆運動学計算を実行することにより、前記複数の関節の各々の状態に関する関節値を算出する算出処理と、
   前記複数の関節のうち、前記関節値に基づく状態が、あらかじめ定められた特定状態である関節に対して実行する固定処理と、
   を含む単位処理を繰り返し実行し、
   前記固定処理を実行した場合、前記算出処理において算出された前記複数の関節の前記関節値のうち、前記特定状態の関節の前記関節値は実質的に変位させることなく、前記特定状態でない関節の前記関節値を変位させる、
   ことを特徴とする多関節ロボットの制御方法。
2. 前記特定状態は、前記関節値が、あらかじめ定められた前記各関節の可動領域の領域内の、前記可動領域の領域外との境界領域の値となる状態であることを特徴とする請求項1に記載の多関節ロボットの制御方法。
3. 前記単位処理では、前記複数の関節の中に前記特定状態の関節が含まれる場合、前回の前記単位処理の前記算出処理において算出された前記複数の関節の各々の前記関節値への前記複数の関節の各々の前記関節値の更新がキャンセルされ、前記関節値の更新がキャンセルされる前の前記関節値に基づいて、前記固定処理の対象となる前記特定状態の関節が特定され、前記固定処理が実行された後に前記算出処理が実行される、
   ことを特徴とする請求項１に記載の多関節ロボットの制御方法。
4. 前記単位処理が繰り返し実行される前の初期状態の前記複数の関節の中に前記特定状態の関節が含まれる場合、繰り返し実行される前記単位処理のうちの１回目の前記単位処理が実行される前に、前記特定状態の関節の前記関節値を、前記特定状態でない状態に対応する前記関節値に変更する、
   ことを特徴とする請求項１に記載の多関節ロボットの制御方法。
5. 前記算出処理において、ヤコビ行列を用いて前記逆運動学計算を実行し、
   前記固定処理において、前記ヤコビ行列の複数の要素のうち、前記特定状態の関節に係る要素の値を実質的に０に設定することにより、前記特定状態の関節の前記関節値を変位させない、
   ことを特徴とする請求項１から４のいずれか１項に記載の多関節ロボットの制御方法。
6. 前記複数の関節が、少なくとも１個の直動関節を含む、
   ことを特徴とする請求項１に記載の多関節ロボットの制御方法。
7. ７個以上の複数の関節を有する多関節ロボットと、
   前記多関節ロボットの動作を制御する制御装置と、
   を備え、
   前記制御装置は、
   前記多関節ロボットを動作させる前記複数の関節の変位量を計算する逆運動学計算を実行することにより、前記複数の関節の各々の状態に関する関節値を算出する算出処理と、
   前記複数の関節のうち、前記関節値に基づく状態が、あらかじめ定められた特定状態である関節に対して実行する固定処理と、
   を含む単位処理を繰り返し実行する動作制御部を有し、
   前記動作制御部は、
   前記固定処理を実行した場合、前記算出処理において算出された前記複数の関節の前記関節値のうち、前記特定状態の関節の前記関節値は実質的に変位させることなく、前記特定状態でない関節の前記関節値を変位させる、
   ことを特徴とするロボットシステム。
8. 前記多関節ロボットは、
   基部と、
   第１リンクと、
   第２リンクと、
   先端部と、
   前記基部の底面に垂直な方向とのなす角度が所定角度以下の軸を第１回転軸として、前記基部の少なくとも一部分を回転させる第１駆動機構と、
   前記基部と前記第１リンクを接続し、前記基部の底面に垂直な方向とのなす角度が前記所定角度より大きい軸を第２回転軸として前記第１リンクを回転させる第２駆動機構と、
   前記第１リンクと前記第２リンクを接続し、前記第１リンクが延在する方向とのなす角度が前記所定角度より大きい軸を第３回転軸として前記第２リンクを前記第１リンクに対して回転させる第３駆動機構と、
   前記第２リンクと前記先端部を接続し、前記第２リンクが延在する方向とのなす角度が前記所定角度より大きい軸を第４回転軸として、前記先端部を前記第２リンクに対して回転させる第４駆動機構と、
   前記第１リンクの延在方向に沿って、前記第３駆動機構を前記第１リンクに対して相対的に移動させる第１移動機構と、
   前記第２リンクの延在方向に沿って、前記第２リンクを前記第３駆動機構に対して相対的に移動させる第２移動機構と、
   を備え、
   前記先端部は、
   前記第２リンクに接続される第１部分と、
   前記第１部分に接続される第２部分と、
   前記第１部分と前記第２部分を接続し、前記第４回転軸とのなす角度が前記所定角度より大きい軸を第５回転軸として、前記第２部分を前記第１部分に対して回転させる第５駆動機構と、
   前記第５回転軸とのなす角度が前記所定角度より大きい軸を第６回転軸として、前記先端部の少なくとも一部分を回転させる第６駆動機構と、
   を含み、
   前記複数の関節は、前記第１駆動機構、前記第２駆動機構、前記第３駆動機構、前記第４駆動機構、前記第５駆動機構、前記第６駆動機構、前記第１移動機構及び前記第２移動機構である、
   ことを特徴とする請求項７に記載のロボットシステム。
9. 前記多関節ロボットは、
   基部と、
   第１支持部分及び第１可動部分を含む第１リンクと、
   第２支持部分及び第２可動部分を含む第２リンクと、
   先端部と、
   前記基部の底面に垂直な方向とのなす角度が所定角度以下の軸を第１回転軸として、前記基部の少なくとも一部分を回転させる第１駆動機構と、
   前記基部と前記第１支持部分を接続し、前記基部の底面に垂直な方向とのなす角度が前記所定角度より大きい軸を第２回転軸として前記第１リンクを前記基部に対して回転させる第２駆動機構と、
   前記第１可動部分と前記第２支持部分を接続し、前記第１リンクが延在する方向とのなす角度が前記所定角度より大きい軸を第３回転軸として前記第２リンクを前記第１リンクに対して回転させる第３駆動機構と、
   前記第２リンクが延在する方向とのなす角度が前記所定角度以下の軸を第４回転軸として、前記第２可動部分を前記第２支持部分に対して回転させる第４駆動機構と、
   前記第２可動部分と前記先端部を接続し、前記第２リンクの延在方向とのなす角度が前記所定角度より大きい軸を第５回転軸として前記先端部を前記第２リンクに対して回転させる第５駆動機構と、
   前記第５回転軸とのなす角度が前記所定角度より大きい軸を第６回転軸として、前記先端部の少なくとも一部分を前記第２リンクに対して回転させる第６駆動機構と、
   前記第１支持部分に対して前記第１可動部分を前記第１リンクの延在方向に沿って移動させることにより、前記第１リンクを伸縮させる第１伸縮機構と、
   前記第２支持部分に対して前記第２可動部分を前記第２リンクの延在方向に沿って移動させることにより、前記第２リンクを伸縮させる第２伸縮機構と、
   を含み、
   前記複数の関節は、前記第１駆動機構、前記第２駆動機構、前記第３駆動機構、前記第４駆動機構、前記第５駆動機構、前記第６駆動機構、前記第１伸縮機構及び前記第２伸縮機構である、
   ことを特徴とする請求項７に記載のロボットシステム。
10. 前記多関節ロボットは、
    基部と、
    第１リンクと、
    第２リンクと、
    先端部と、
    前記基部の底面に垂直な方向とのなす角度が所定角度以下の軸を第１回転軸として、前記基部の少なくとも一部分を回転させる第１駆動機構と、
    前記基部と前記第１リンクを接続し、前記基部の底面に垂直な方向とのなす角度が前記所定角度より大きい軸を第２回転軸として前記第１リンクを回転させる第２駆動機構と、
    前記第１リンクと前記第２リンクを接続し、前記第１リンクが延在する方向とのなす角度が前記所定角度より大きい軸を第３回転軸として前記第２リンクを前記第１リンクに対して回転させる第３駆動機構と、
    前記第２リンクと前記先端部を接続し、前記第２リンクが延在する方向とのなす角度が前記所定角度以下の軸を第４回転軸として、前記先端部を前記第２リンクに対して回転させる第４駆動機構と、
    前記第１リンクの延在方向に沿って、前記第３駆動機構を前記第１リンクに対して相対的に移動させる第１移動機構と、
    前記第２リンクの延在方向に沿って、前記第２リンクを前記第３駆動機構に対して相対的に移動させる第２移動機構と、
    を備え、
    前記先端部は、
    前記第２リンクに接続される第１部分と、
    前記第１部分に接続される第２部分と、
    前記第１部分と前記第２部分を接続し、前記第４回転軸とのなす角度が前記所定角度より大きい軸を第５回転軸として、前記第２部分を前記第１部分に対して回転させる第５駆動機構と、
    前記第５回転軸とのなす角度が前記所定角度より大きい軸を第６回転軸として、前記先端部の少なくとも一部分を回転させる第６駆動機構と、
    を含み、
    前記複数の関節は、前記第１駆動機構、前記第２駆動機構、前記第３駆動機構、前記第４駆動機構、前記第５駆動機構、前記第６駆動機構、前記第１移動機構及び前記第２移動機構である、
    ことを特徴とする請求項７に記載のロボットシステム。
11. 請求項７に記載のロボットシステムにより、部品を組み付ける、又は、部品を取り除く、
    ことを特徴とする物品の製造方法。
12. ７個以上の複数の関節を有する多関節ロボットの動作を制御するプログラムであって、
    プロセッサを、
    前記多関節ロボットを動作させる前記複数の関節の変位量を計算する逆運動学計算を実行することにより、前記複数の関節の各々の状態に関する関節値を算出する算出処理と、
    前記複数の関節のうち、前記関節値に基づく状態が、あらかじめ定められた特定状態である関節に対して実行する固定処理と、
    を含む単位処理を繰り返し実行する動作制御部として機能させ、
    前記動作制御部は、
    前記固定処理を実行した場合、前記算出処理において算出された前記複数の関節の前記関節値のうち、前記特定状態の関節の前記関節値は実質的に変位させることなく、前記特定状態でない関節の前記関節値を変位させる、
    ことを特徴とするプログラム。

Images