(57)【要約】【課題】エンドエフェクタをターゲットポジションに高速かつ高精度に移動させる。【解決手段】制御装置は、ターゲットポジションの座標位置に基づき、複数のジョイント（Ｊ１〜Ｊ６）のうち所定のジョイントの仮角度を計算し、かつ、仮角度から複数のジョイント（Ｊ１〜Ｊ６）の角度の全てを計算することにより、エンドエフェクタ（ＥＥ）がターゲットポジションにより近くなる複数のジョイント（Ｊ１〜Ｊ６）の第１の最終角度を計算する第１の処理部（Ｐ１）と、第１の最終角度から変化させた複数のジョイント（Ｊ１〜Ｊ６）の角度に基づき、エンドエフェクタ（ＥＥ）の座標位置を計算し、エンドエフェクタ（ＥＥ）の座標位置とターゲットポジションの座標位置との差が所定値に等しいか又はそれよりも小さいときの複数のジョイント（Ｊ１〜Ｊ６）の第２の最終角度を計算する第２の処理部（Ｐ２）と、を備える。【選択図】図１
2018202535.tif 000002
【特許請求の範囲】
【請求項１】
連結される複数のジョイントを含むアームと、前記アームの一端に結合されるエンドエフェクタと、を備える制御装置であって、
前記エンドエフェクタにおけるホームポジションの座標位置及び前記エンドエフェクタにおけるターゲットポジションの座標位置を記憶する記憶部と、
前記ターゲットポジションの前記座標位置に基づき、前記複数のジョイントのうち所定のジョイントの仮角度を計算し、かつ、前記仮角度から前記複数のジョイントの角度の全てを計算することにより、前記エンドエフェクタが前記ターゲットポジションにより近くなる前記複数のジョイントの第１の最終角度を計算する第１の処理部と、
前記第１の最終角度から変化させた前記複数のジョイントの前記角度に基づき、前記エンドエフェクタの座標位置を計算し、前記エンドエフェクタの前記座標位置と前記ターゲットポジションの前記座標位置との差が所定値に等しいか又はそれよりも小さいときの前記複数のジョイントの第２の最終角度を計算する第２の処理部と、
前記エンドエフェクタを前記ホームポジションから前記ターゲットポジションに移動させる駆動信号として、前記第２の最終角度を前記複数のジョイントに出力する駆動部と、
を備える制御装置。
【請求項２】
前記第１の処理部は、前記ロボットのＤＨパラメータを使用するＩＫにより、前記ターゲットポジションの前記座標位置から前記第１の最終角度を計算する、
請求項１に記載の制御装置。
【請求項３】
前記第１の処理部は、前記仮角度をサーチウィンドウ内で計算した後、前記複数のジョイントの前記角度の全てを計算し、かつ、前記サーチウィンドウを次第に狭めることにより前記第１の最終角度を計算する、
請求項１又は２に記載の制御装置。
【請求項４】
前記所定のジョイントは、前記エンドエフェクタに最も近いジョイントである、
請求項１乃至３のいずれか１項に記載の制御装置。
【請求項５】
前記第２の処理部は、前記ロボットのＤＨパラメータを使用するＦＫにより、前記複数のジョイントの前記角度から前記エンドエフェクタの前記座標位置を計算する、
請求項１乃至４のいずれか１項に記載の制御装置。
【請求項６】
前記第２の処理部は、前記複数のジョイントの前記角度のうち、選択された角度の値を第１のステップ値で変化させた後、前記エンドエフェクタの前記座標位置を計算する、
請求項１乃至５のいずれか１項に記載の制御装置。
【請求項７】
前記第２の処理部は、
前記エンドエフェクタの前記座標位置と前記ターゲットポジションの前記座標位置との前記差が前回の前記差よりも大きい場合、前記選択された角度の前記値を、前記第１のステップ値よりも小さい第２のステップ値で、かつ、前記前回に前記選択された角度の前記値を変化させた方向と逆の方向に変化させ、さらに、
所定の条件が満たされるまで、前記選択された角度の前記値を繰り返し変化させる、
請求項６に記載の制御装置。
【請求項８】
前記第２の処理部は、前記複数のジョイントの前記角度の全てが前記選択された角度として選択された後に、前記第２の最終角度を計算する、
請求項７に記載の制御装置。
【請求項９】
前記第２の処理部は、前記エンドエフェクタの前記座標位置と前記ターゲットポジションの前記座標位置との前記差を表すヤコビアンマトリックスを用いて、前記第２の最終角度を計算する、
請求項１乃至８のいずれか１項に記載の制御装置。
【請求項１０】
前記複数のジョイントの数は、６つであり、前記アームは、６自由度で動作可能である、
請求項１乃至９のいずれか１項に記載の制御装置。
【請求項１１】
ロボットと、
前記ロボットを制御する制御装置と、
を備え、
前記ロボットは、
連結される複数のジョイントを含むアームと、
前記アームの一端に結合されるエンドエフェクタと、を備え、
前記制御装置は、
前記エンドエフェクタにおけるホームポジションの座標位置及び前記エンドエフェクタにおけるターゲットポジションの座標位置を記憶する記憶部と、
前記ターゲットポジションの前記座標位置に基づき、前記複数のジョイントのうち所定のジョイントの仮角度を計算し、かつ、前記仮角度から前記複数のジョイントの角度の全てを計算することにより、前記エンドエフェクタが前記ターゲットポジションにより近くなる前記複数のジョイントの第１の最終角度を計算する第１の処理部と、
前記第１の最終角度から変化させた前記複数のジョイントの前記角度に基づき、前記エンドエフェクタの座標位置を計算し、前記エンドエフェクタの前記座標位置と前記ターゲットポジションの前記座標位置との差が所定値に等しいか又はそれよりも小さいときの前記複数のジョイントの第２の最終角度を計算する第２の処理部と、
前記エンドエフェクタを前記ホームポジションから前記ターゲットポジションに移動させる駆動信号として、前記第２の最終角度を前記複数のジョイントに出力する駆動部と、
を備える、
ロボットシステム。
【請求項１２】
前記第１の処理部は、前記ロボットのＤＨパラメータを使用するＩＫにより、前記ターゲットポジションの前記座標位置から前記第１の最終角度を計算する、
請求項１１に記載のロボットシステム。
【請求項１３】
前記第１の処理部は、前記仮角度をサーチウィンドウ内で計算した後、前記複数のジョイントの前記角度の全てを計算し、かつ、前記サーチウィンドウを次第に狭めることにより前記第１の最終角度を計算する、
請求項１１又は１２に記載のロボットシステム。
【請求項１４】
前記所定のジョイントは、前記エンドエフェクタに最も近いジョイントである、
請求項１１乃至１３のいずれか１項に記載のロボットシステム。
【請求項１５】
前記第２の処理部は、前記ロボットのＤＨパラメータを使用するＦＫにより、前記複数のジョイントの前記角度から前記エンドエフェクタの前記座標位置を計算する、
請求項１１乃至１４のいずれか１項に記載のロボットシステム。
【請求項１６】
前記第２の処理部は、前記複数のジョイントの前記角度のうち、選択された角度の値を第１のステップ値で変化させた後、前記エンドエフェクタの前記座標位置を計算する、
請求項１１乃至１５のいずれか１項に記載のロボットシステム。
【請求項１７】
前記第２の処理部は、
前記エンドエフェクタの前記座標位置と前記ターゲットポジションの前記座標位置との前記差が前回の前記差よりも大きい場合、前記選択された角度の前記値を、前記第１のステップ値よりも小さい第２のステップ値で、かつ、前記前回に前記選択された角度の前記値を変化させた方向と逆の方向に変化させ、さらに、
所定の条件が満たされるまで、前記選択された角度の前記値を繰り返し変化させる、
請求項１６に記載のロボットシステム。
【請求項１８】
前記第２の処理部は、前記複数のジョイントの前記角度の全てが前記選択された角度として選択された後に、前記第２の最終角度を計算する、
請求項１７に記載のロボットシステム。
【請求項１９】
前記第２の処理部は、前記エンドエフェクタの前記座標位置と前記ターゲットポジションの前記座標位置との前記差を表すヤコビアンマトリックスを用いて、前記第２の最終角度を計算する、
請求項１１乃至１８のいずれか１項に記載のロボットシステム。
【請求項２０】
前記複数のジョイントの数は、６つであり、前記アームは、６自由度で動作可能である、
請求項１１乃至１９のいずれか１項に記載のロボットシステム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、制御装置及びロボットシステムに関する。
【背景技術】
【０００２】
連結される複数のジョイントを含むアームと、その一端に結合されるエンドエフェクタと、を備えるロボットにおいて、エンドエフェクタの座標位置（position coordinate）と複数のジョイントの角度とは、ＩＫ（逆運動学）又はＦＫ（順運動学）により互いに関連付けられる。従って、制御装置は、ロボットの幾何学的特性（ＤＨパラメータ）を使用するＩＫ又はＦＫにより、エンドエフェクタのポジションを制御できる。
【０００３】
例えば、連結する３つのジョイント軸が所定のポイントで交差する場合、エンドエフェクタのポジションは、閉形式（解法がある式）により求められる解によって決定される。より具体的には、エンドエフェクタのポジションは、ＩＫによりターゲットポジションの座標位置から計算（computed）される複数のジョイントの角度により制御可能である。又は、エンドエフェクタのポジションは、ＦＫにより複数のジョイントの角度から計算される。
【０００４】
しかし、実際は、ロボットを使用する環境などが原因し、制御装置は、１つの解を求めることができない場合がある。
また、例えば、ＩＫにより計算される複数のジョイントの角度は、エンドエフェクタのポジションをターゲットポジションに高精度に一致させるものではない。
【０００５】
一方、例えば、ＦＫにより複数のジョイントの角度からエンドエフェクタの座標位置を計算する場合、制御装置は、エンドエフェクタのポジションをターゲットポジションに高精度に一致させることができる。
しかし、エンドエフェクタの移動距離が長くなると、エンドエフェクタの座標位置を計算する繰り返し回数（number of iterations）が多くなる。
【０００６】
従って、制御装置は、エンドエフェクタをターゲットポジションに高速かつ高精度に移動させることができない。
【０００７】
非特許文献１〜５は、エンドエフェクタを移動可能なロボットシステムにおいて、ＩＫにより計算される複数のジョイントの角度に基づき、エンドエフェクタのポジションを高速に制御する解析的手法（Analytical technique）、又は、ＦＫにより計算されるエンドエフェクタの座標位置に基づき、エンドエフェクタのポジションを高精度に制御する数値的手法（numerical technique）を開示する。
【先行技術文献】
【非特許文献】
【０００８】
【非特許文献１】D.Tolani, et al, Real-time Inverse Kinematics Techniques for Anthropomorphic Limbs (2000), Graphical Models 62, 353-388
【非特許文献２】Kulpa et al, Fast inverse kinematics and kinetics solver for human-like figure (2005), IEEE-RAS International Conference on Humanoid Robots
【非特許文献３】H.Ananthanarayanan et al, Real-time Inverse Kinematics of (2n+1) DOF hyper-redundant manipulator arm via a combined numerical and analytical approach (2015), Mechanism and Machine Theory, Vol 91, 209-226
【非特許文献４】D.L.Pieper, The Kinematics of Manipulators Under Computer Control (1968), Computer Science Department, Standford University
【非特許文献５】J.J.Uicker et al, An Iterative Method for the Displacement Analysis of Special Mechanism (1964), Journal of Applied Mechanics, Vol 32, 309-314
【発明の概要】
【発明が解決しようとする課題】
【０００９】
非特許文献１〜５の技術では、例えば、解析的手法によりエンドエフェクタのポジションを制御する場合、制御装置は、エンドエフェクタのポジションを高速に制御できるが、エンドエフェクタのポジションを高精度に制御できない。
また、例えば、数値的手法によりエンドエフェクタのポジションを制御する場合、制御装置は、エンドエフェクタのポジションを高精度に制御できるが、エンドエフェクタのポジションを高速に制御できない。
【００１０】
このように、ロボットシステムにおいて、エンドエフェクタをターゲットポジションに高速に移動させることと、エンドエフェクタをターゲットポジションに高精度に移動させることとは、トレードオフの関係にある。
【００１１】
本発明の実施形態は、エンドエフェクタをターゲットポジションに高速かつ高精度に移動させる技術を提案する。
【課題を解決するための手段】
【００１２】
本願の例示的な第１の発明に係わる制御装置は、ロボットを制御する。前記ロボットは、連結される複数のジョイントを含むアームと、前記アームの一端に結合されるエンドエフェクタと、を備える。前記制御装置は、前記エンドエフェクタにおけるホームポジションの座標位置及び前記エンドエフェクタにおけるターゲットポジションの座標位置を記憶する記憶部と、前記ターゲットポジションの前記座標位置に基づき、前記複数のジョイントのうち所定のジョイントの仮角度を計算し、かつ、前記仮角度から前記複数のジョイントの角度の全てを計算することにより、前記エンドエフェクタが前記ターゲットポジションにより近くなる前記複数のジョイントの第１の最終角度を計算する第１の処理部と、前記第１の最終角度から変化させた前記複数のジョイントの前記角度に基づき、前記エンドエフェクタの座標位置を計算し、前記エンドエフェクタの前記座標位置と前記ターゲットポジションの前記座標位置との差が所定値に等しいか又はそれよりも小さいときの前記複数のジョイントの第２の最終角度を計算する第２の処理部と、前記エンドエフェクタを前記ホームポジションから前記ターゲットポジションに移動させる駆動信号として、前記第２の最終角度を前記複数のジョイントに出力する駆動部と、を備える。
【発明の効果】
【００１３】
本願の例示的な第１の発明によれば、エンドエフェクタをターゲットポジションに高速かつ高精度に移動できる。
【図面の簡単な説明】
【００１４】
【図１】図１は、ロボットシステムの例を示す図である。
【図２Ａ】図２Ａは、モータの例を示す図である。
【図２Ｂ】図２Ｂは、ネットワークの例を示す図である。
【図３】図３は、本発明の制御手法の例を示すフローチャートである。
【図４】図４は、解析的手法（第１のステージ）の例を示すフローチャートである。
【図５】図５は、黄金比を用いた仮角度の計算の例を示す図である。
【図６】図６は、数値的手法（第２のステージ）の例を示すフローチャートである。
【図７】図７は、角度の計算とモータの駆動との関係の例を示す図である。
【図８】図８は、本発明の制御手法の効果の例を示す図である。
【図９】図９は、本発明の制御手法の効果の例を示す図である。
【図１０】図１０は、エンドエフェクタの移動経路の第１の例を示す図である。
【図１１】図１１は、エンドエフェクタの移動経路の第２の例を示す図である。
【図１２】図１２は、エンドエフェクタのポジションと制御装置の動作との関係を示す図である。
【図１３】図１３は、エンドエフェクタのポジションと制御装置の動作との関係を示す図である。
【発明を実施するための形態】
【００１５】
以下、図面を参照しながら実施形態を説明する。
なお、実施形態では、その説明を分かり易くするため、本発明の主要部以外の構造については、簡略化して説明する。また、図面において、各要素の寸法、形状、数などについても、一例であり、これに限定されるという主旨ではない。
【００１６】
＜ロボットシステム＞
図１は、ロボットシステムの例を示している。
ロボットシステムは、ロボット１０と、ロボット１０を制御する制御装置１１と、を備える。ロボット１０は、連結される複数のジョイント（リンク）Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６を含むアームＡＭと、アームＡＭの一端に結合されるエンドエフェクタＥＥと、を備える。また、複数のジョイントＪ（Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６）は、それぞれ、複数のモータＭ（Ｍ１，Ｍ２，Ｍ３，Ｍ４，Ｍ５，Ｍ６）を備える。
【００１７】
例えば、ジョイントＪ１は、回転軸ＡＸ１を有する。モータＭ１は、回転軸ＡＸ１を中心軸として、ジョイントＪ１を回転させる機能を有する。また、ジョイントＪ２は、回転軸ＡＸ２を有する。モータＭ２は、回転軸ＡＸ２を中心軸として、ジョイントＪ２を回転させる機能を有する。同様に、ジョイントＪ３〜Ｊ６は、それぞれ、回転軸ＡＸ３〜ＡＸ６を有する。モータＭ３〜Ｍ６は、それぞれ、回転軸ＡＸ３〜ＡＸ６を中心軸として、ジョイントＪ３〜Ｊ６を回転させる機能を有する。
【００１８】
エンドエフェクタＥＥは、対象物（object）と関連し合う要素のことである。例えば、エンドエフェクタＥＥは、ターゲットポジションにある対象物を動かしたり、又は、掴んだりするマニピュレータ（manipulator）としての機能に加え、その対象物を接合したり、溶接したり、又は、撮影したりなど、対象物と様々な関連を持つ要素のことである。
【００１９】
複数のモータＭは、例えば、サーボモータである。但し、モータの種類は、これに限定されることはない。
また、複数のモータＭは、図２Ａに示すように、制御部３を備える機電一体型モータ（electromechanical motor）であるのが望ましい。制御部３は、例えば、ＣＰＵ（central processing unit）、ＥＣＵ（electronic control unit）などである。
【００２０】
機電一体型モータとは、図２Ａに示すように、モータ部２及びこれを制御する制御部３が筐体(housing)１内に配置され、かつ、モータ部２の出力軸ＡＸがその筐体１から突出している構造のことである。モータ部２の出力軸ＡＸは、モータ部２のロータでもよいし、モータ部２のロータとは異なるシャフトでもよい。
機電一体型モータは、モータの小型化、ノイズ耐性の向上など、に有効である。従って、複数のモータＭとして機電一体型モータを使用すれば、ロボットシステムの低コスト化、高信頼性などに貢献できる。
【００２１】
本例では、複数のジョイントＪの数は、６つであり、アームＡＭは、６自由度（6 degrees of freedom: 6 DOF）で動作可能である。しかし、複数のジョイントＪの数は、これに限られることはなく、２つ以上であればよい。
【００２２】
制御装置１１は、ロボット１０を制御する。
ここで、ロボット１０と制御装置１１とは、ネットワークで接続されるのが望ましい。
【００２３】
例えば、図２Ｂに示すように、ロボットシステムが複数のロボット１０−１，…１０−（ｎ−１），１０−ｎを備える場合、制御装置１１は、ＣＡＮ（Controller area network）などのネットワーク１２を介して、複数のロボット１０−１，…１０−（ｎ−１），１０−ｎ、即ち、図１及び図２Ａの複数のモータＭ１，Ｍ２，Ｍ３，Ｍ４，Ｍ５，Ｍ６に接続される。但し、ｎは、２に等しいか又はそれよりも大きい自然数である。
【００２４】
この場合、複数のロボット１０−１，…１０−（ｎ−１），１０−ｎと制御装置１１との間のデータ通信は、簡易となり、かつ、データ通信時のノイズ耐性は、向上する。
【００２５】
制御装置１１は、記憶部Ｓと、第１の処理部Ｐ１と、第２の処理部Ｐ２と、駆動部Ｄと、を備える。
【００２６】
記憶部Ｓは、例えば、エンドエフェクタＥＥにおける、ホームポジションの座標位置、ターゲットポジションの座標位置、さらには、初期ポジションの座標位置を記憶する。
【００２７】
ホームポジションとは、ロボット１０がスタンバイ状態又は非動作状態にあるときのエンドエフェクタＥＥのポジションのことである。初期ポジションとは、エンドエフェクタＥＥを移動させる前のスタートポジションのことである。ターゲットポジションとは、エンドエフェクタＥＥを移動させた後のゴールポジションのことである。
【００２８】
初期ポジションは、ホームポジションの場合もあるし、又は、ホームポジションとは異なるポジションの場合もある。
例えば、制御装置１１がエンドエフェクタＥＥをホームポジションＡからターゲットポジションＢに移動させる場合、Ａは、ホームポジションであると共に初期ポジションでもある。
【００２９】
また、制御装置１１が、エンドエフェクタＥＥをホームポジションＡからターゲットポジションＢに移動させ、さらに、ターゲットポジションＢからターゲットポジションＣに移動させる場合、Ａは、ホームポジションであると共に初期ポジションでもある。また、Ｂは、ターゲットポジションであると共に、初期ポジションでもある。
【００３０】
尚、エンドエフェクタＥＥの全ての移動が終わった後、制御装置１１は、エンドエフェクタＥＥをホームポジションに戻すのが望ましい。
【００３１】
記憶部Ｓは、例えば、ＳＲＡＭ（Static Random Access Memory）、ＤＲＡＭ（Dynamic Random Access Memory）、ＭＲＡＭ（Magnetic Random Access Memory）などのＲＡＭ、ＥＥＰＲＯＭ（Electrically Erasable Programmable Read Only Memory）などのＲＯＭ、又は、フラッシュメモリなどの不揮発性メモリから選択可能である。
また、記憶部Ｓは、例えば、レジスタ、又は、ラッチ回路であってもよい。
【００３２】
第１の処理部Ｐ１は、例えば、ロボット１０のＤＨパラメータを使用するＩＫにより、ターゲットポジションの座標位置から複数のジョイントＪの第１の最終角度θ_1st（θ１_1st，θ２_1st，θ３_1st，θ４_1st，θ５_1st，θ６_1st）を計算する。即ち、第１の処理部Ｐ１は、第２の処理部Ｐ２で使用する複数のジョイントＪの初期角度として、第１の最終角度θ_1stを計算することを目的とする。
【００３３】
例えば、第１の処理部Ｐ１は、ターゲットポジションの座標位置に基づき、複数のジョイントＪのうち所定のジョイントＪｘの仮角度（provisional angle）θｘｐを計算する。但し、ｘは、１〜６のうちの１つである。
【００３４】
また、第１の処理部Ｐ１は、その仮角度θｘｐから複数のジョイントＪ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６の全ての角度θ１，θ２，θ３，θ４，θ５，θ６を計算することにより、エンドエフェクタＥＥがターゲットポジションにより近くなる複数のジョイントＪ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６の第１の最終角度θ１_1st，θ２_1st，θ３_1st，θ４_1st，θ５_1st，θ６_1stを計算する。
【００３５】
第１の処理部Ｐ１での処理は、いわゆる解析的手法による処理であり、第１のステージと称される。この処理は、ターゲットポジションから複数のジョイントＪ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６の全ての角度θ１，θ２，θ３，θ４，θ５，θ６を解析的に計算することから、解析的手法と称される。
【００３６】
解析的手法により求められる解（第１の最終角度）は、ラフである、即ち、エンドエフェクタＥＥを高精度にターゲットポジションに移動させるものではないが、第２の処理部Ｐ２で使用する初期角度として使用する分には十分である。しかも、解析的手法では、１つの解（第１の最終角度）が高速に求められる。
【００３７】
第２の処理部Ｐ２は、例えば、ロボット１０のＤＨパラメータを使用するＦＫにより、複数のジョイントＪの角度からエンドエフェクタＥＥの座標位置、即ち、数値上のポジション（numerical position）を計算する。
【００３８】
即ち、第２の処理部Ｐ２は、第１の処理部Ｐ１で計算された第１の最終角度θ_1stを複数のジョイントＪの初期角度として、エンドエフェクタＥＥの座標位置とターゲットポジションの座標位置との差（距離エラー）が所定値に等しいか又はそれよりも小さくなるまで、複数のジョイントＪの角度を変更することを目的とする。
【００３９】
例えば、第２の処理部Ｐ２は、第１の最終角度θ_1stから変化させた複数のジョイントＪの角度に基づき、エンドエフェクタＥＥの座標位置を計算する。
【００４０】
また、第２の処理部Ｐ２は、エンドエフェクタＥＥの座標位置とターゲットポジションの座標位置との差が所定値に等しいか又はそれよりも小さいときの複数のジョイントＪ（Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６）の第２の最終角度θ_2nd（θ１_2nd，θ２_2nd，θ３_2nd，θ４_2nd，θ５_2nd，θ６_2nd）を計算する。
【００４１】
第２の処理部Ｐ２での処理は、いわゆる数値的手法による処理であり、第２のステージと称される。この処理は、複数のジョイントＪの角度からエンドエフェクタＥＥの座標位置、即ち、数値上のポジションを繰り返し計算することから、数値的手法と称される。
【００４２】
数値的手法により求められる解（第２の最終角度）は、精密である、即ち、エンドエフェクタＥＥのポジションは、ターゲットポジションに高精度で一致する。
数値的手法では、複数のジョイントＪの初期角度は、第１の処理部Ｐ１で計算された第１の最終角度である。これは、エンドエフェクタＥＥの座標位置が既にターゲットポジションの座標位置の近くにあることを意味する。
【００４３】
従って、数値的手法による計算時間、即ち、エンドエフェクタＥＥの座標位置を繰り返し計算する回数は、非常に少なくて済む。
その結果、第２の処理部Ｐ２は、エンドエフェクタＥＥのポジションをターゲットポジションに高精度で一致させるためのエンドエフェクタＥＥの座標位置を高速に計算できる。
【００４４】
駆動部Ｄは、第２の最終角度θ_2ndに基づき、複数のジョイントＪを駆動する駆動信号ＤＳを出力する。例えば、駆動部Ｄは、駆動信号ＤＳとしての第２の最終角度θ_2ndを、複数のモータＭに出力する。
【００４５】
駆動信号ＤＳ（＝θ１_2nd）は、駆動部ＤからモータＭ１に転送される。モータＭ１は、駆動信号ＤＳ（＝θ１_2nd）に基づき、ジョイントＪ１を駆動する。駆動信号ＤＳ（＝θ２_2nd）は、駆動部ＤからモータＭ２に転送される。モータＭ２は、駆動信号ＤＳ（＝θ２_2nd）に基づき、ジョイントＪ２を駆動する。
同様に、駆動信号ＤＳ（＝θ３_2nd〜θ６_2nd）は、それぞれ、駆動部ＤからモータＭ３〜Ｍ６に転送される。モータＭ３〜Ｍ６は、それぞれ、駆動信号ＤＳ（＝θ３_2nd〜θ６_2nd）に基づき、ジョイントＪ３〜Ｊ６を駆動する。
【００４６】
その結果、エンドエフェクタＥＥは、実際に、ホームポジションからターゲットポジションに移動する。
【００４７】
第１の処理部Ｐ１、第２の処理部Ｐ２、及び、駆動部Ｄは、例えば、マイクロプロセッサなどの制御回路内に含まれる。複数のモータＭが機電一体型モータである場合、第１の処理部Ｐ１、第２の処理部Ｐ２、及び、駆動部Ｄの機能の一部を、例えば、図２Ａに示す複数のモータＭ内の制御部３に実行させてもよい。
【００４８】
また、第１の処理部Ｐ１、第２の処理部Ｐ２、及び、駆動部Ｄの機能は、ハードウェアにより実現してもよいし、ソフトウェアにより実現してもよいし、又は、それらの組み合わせにより実現してもよい。
これらの機能が、ハードウェアにより実現されるか、ソフトウェアにより実現されるか、又は、それらの組み合わせにより実現されるかは、ロボットシステムが使用される環境、又は、ロボットシステムに課される設計制約など、に依存する。
【００４９】
以上のロボットシステムは、産業用、介護用、コミュニケーション用などの様々なロボットに応用可能である。
【００５０】
このように、図１のロボットシステムにおいて、制御装置１１は、エンドエフェクタＥＥのポジションを、解析的手法（第１のステージ）と数値的手法（第２のステージ）との組み合わせにより制御する。
本発明の制御手法は、ロボットシステムにおいて、エンドエフェクタの高速移動と高精度移動とのトレードオフを改善する。即ち、本発明の制御手法は、エンドエフェクタをターゲットポジションに高速かつ高精度に移動させる技術である。
【００５１】
＜動作＞
・  全体フロー
図３は、本発明の制御手法の全体フローの例を示している。
本発明の制御手法は、例えば、図１の制御装置１１により実行される。
【００５２】
まず、ターゲットポジションの座標位置が図１の制御装置１１に入力される（ステップＳＴ００）。ターゲットポジションの座標位置は、例えば、パソコンなどのホストから図１の制御装置１１に入力され、かつ、図１の記憶部Ｓに記憶される。
ホームポジションの座標位置は、予め、図１の記憶部Ｓに記憶される。ホームポジションの座標位置は、パソコンなどのホストから設定可能であってもよいし、又は、制御装置１１が自ら計算可能であってもよい。また、ホームポジションの座標位置は、ロボットシステムの出荷時に、デフォルトとして、図１の記憶部Ｓに記憶されていてもよい。
【００５３】
この後、解析的手法（第１のステージ）が実行される（ステップＳＴ０１）。
本発明の制御手法での解析的手法とは、複数のパラメータ、即ち、複数のジョイントの角度のうちの少なくとも１つを仮決定し、かつ、仮決定されたパラメータ、即ち、仮角度に基づき、全てのパラメータ、即ち、全てのジョイントの角度を計算により求めるＩＫ手法のことである。
【００５４】
解析的手法（第１のステージ）では、ターゲットポジションにより近い１つの解、即ち、第１の最終角度θ_1stが高速に計算される。
【００５５】
次に、数値的手法（第２のステージ）が実行される（ステップＳＴ０２）。
本発明の制御手法での数値的手法とは、複数のパラメータ、即ち、複数のジョイントの角度を１つずつ順番に変化させた後、エンドエフェクタの座標位置、即ち、数値上のポジションを計算し、かつ、エンドエフェクタの座標位置とターゲットポジションの座標位置との差を求めるＦＫ手法のことである。
【００５６】
数値的手法（第２のステージ）では、ターゲットポジションにより近い１つの解、即ち、第２の最終角度θ_2ndが精密に計算される。また、数値的手法に使用する複数のジョイントの初期角度は、解析的手法により求められる第１の最終角度θ_1stである。このため、数値的手法での計算時間は、非常に短い。
【００５７】
尚、第２の最終角度θ_2ndは、エンドエフェクタの座標位置とターゲットポジションの座標位置との差が所定値に等しいか又はそれよりも小さいときの複数のジョイントの角度である。この所定値は、ロボットシステムにおいて許容される誤差範囲を示している。
【００５８】
この後、図１の制御装置１１は、複数のジョイントを駆動する駆動信号を出力する（ステップＳＴ０３）。
即ち、駆動信号としての第２の最終角度θ_2ndは、複数のモータに転送される。
【００５９】
その結果、複数のジョイントの角度は、それぞれ、初期角度、例えば、ホームポジションでの角度θ_home（θ１_home，θ２_home，θ３_home，θ４_home，θ５_home，θ６_home）から、第２の最終角度θ_2nd（θ１_2nd，θ２_2nd，θ３_2nd，θ４_2nd，θ５_2nd，θ６_2nd）に変化する。即ち、エンドエフェクタは、実際に、ホームポジションからターゲットポジションに移動する。
【００６０】
以上のように、図１の制御装置１１は、解析的手法と数値的手法を組み合わせることにより、駆動信号としての第２の最終角度θ_2ndを計算する。
従って、図１の制御装置１１は、この駆動信号をモータに転送することにより、エンドエフェクタをターゲットポジションに高速かつ高精度に移動させることができる。
【００６１】
・  解析的手法（第１のステージ）
図４は、解析的手法の例を示している。
まず、図１の第１の処理部Ｐ１は、記憶部Ｓから、複数のジョイントの初期角度、例えば、ホームポジションでの複数のジョイントの角度を取得する。例えば、図１のロボットシステムの場合、図１の第１の処理部Ｐ１は、ホームポジションでの角度θ_homeを取得する（ステップＳＴ１０）。
【００６２】
ホームポジションでの複数のジョイントの角度は、例えば、エンドエフェクタの全ての移動が終わった後、エンドエフェクタをホームポジションに戻すときに使用される。
【００６３】
次に、図１の第１の処理部Ｐ１は、複数のジョイントのうち所定のジョイントＪｘの見積り角度（estimated angle）θｘｅを、例えば、複数のジョイントの初期角度など、に基づき決定する。但し、ｘは、ジョイント番号であり、図１のロボットシステムの場合は１〜６のうちの１つである（ステップＳＴ１１）。
【００６４】
所定のジョイントＪｘの見積り角度θｘｅを決定するのは、例えば、６自由度のロボットアームのように、閉形の解析解を持たない場合でも、ターゲットポジションの座標位置に基づき、高速に１つの解、即ち、エンドエフェクタがターゲットポジションにより近くなる複数のジョイントの角度を計算するためである。
【００６５】
所定のジョイントＪｘは、例えば、パソコンなどのホストから指定可能であってもよいし、図１の第１の処理部Ｐ１が自ら選択してもよい。
【００６６】
所定のジョイントＪｘは、例えば、エンドエフェクタに最も近い、図１のジョイントＪ６である。エンドエフェクタに最も近いジョイントＪ６は、その他のジョイントＪ１〜Ｊ５に比べて、エンドエフェクタのポジションに与える影響が少ないからである。所定のジョイントがＪ６の場合、図１の第１の処理部Ｐ１は、アーム構成(arm configuration)の自由度を確保したうえで１つの解を高速に求めることができる。
【００６７】
次に、図１の第１の処理部Ｐ１は、見積り角度θｘｅ、例えば、所定のジョイントがＪ６の場合はθ６ｅについて、目的関数ｆ（θｘｅ）、例えば、所定のジョイントがＪ６の場合はｆ（θ６ｅ）を設定する（ステップＳＴ１２）。
【００６８】
目的関数ｆ（θｘｅ）は、例えば、以下のように定義される。
ｆ（θｘｅ）＝｜θｘｇ−θｘｅ｜          …（１）
但し、θｘｇは、サーチウィンドウ［ａ０，ｂ０］内で決定されるθｘの推測角度（guessed angle）を意味する。また、サーチウィンドウは、選択可能な角度の範囲のことであり、［ａ０，ｂ０］は、ａ０からｂ０までの範囲を意味する。
【００６９】
目的関数ｆ（θｘｅ）を設定する理由は、見積り角度θｘｅが所定のジョイントＪｘの角度θｘとして固定されると、エンドエフェクタをターゲットポジションにより近付けるために望ましくない場合があるからである。
目的関数ｆ（θｘｅ）は、見積り角度θｘｅを含むサーチウィンドウ［ａ０，ｂ０］の範囲内で、エンドエフェクタをターゲットポジションにより近付けるために適した仮角度（provisional angle）θｘｐを見つけることを目的とする。
【００７０】
そして、図１の第１の処理部Ｐ１は、仮角度θｘｐに基づき、複数のジョイントの角度、例えば、図１のロボットシステムの場合はθ１〜θ６の全ての角度を計算する。
【００７１】
但し、仮角度θｘｐは、見積り角度θｘｅにできるだけ近いのが望ましい。即ち、最終的に決定される仮角度θｘｐは、見積り角度θｘｅに一致する必要はないが、見積り角度θｘｅを含む所定のレンジε内にあるのが望ましい。
そのために、図１の第１の処理部Ｐ１は、サーチウィンドウ［ａ０，ｂ０］を次第に狭めて、仮角度θｘｐ、及び、複数のジョイントの角度を、繰り返し計算する。
【００７２】
この繰り返し計算について詳細に説明する。
【００７３】
サーチウィンドウの初期値は、例えば、ジョイントＪｘが選択可能な角度θｘの最大幅、例えば、ａ０＝−π、ｂ０＝＋πに設定される（ステップＳＴ１３）。
尚、ステップＳＴ１３において、Ｎは、繰り返し計算の回数（繰り返し回数：number of iterations）である。
【００７４】
繰り返し計算、即ち、θｘｇを繰り返し求める計算は、目的関数ｆ（θｘｅ）を次第に小さくなる方向に進行させるため、サーチウィンドウ［ａ０，ｂ０］の両端から、それぞれ、黄金比ｒを用いて計算される。
黄金比ｒは、例えば、以下により与えられる。
ｒ＝（３−√５）／２＝０．３８２０        …（２）
【００７５】
また、サーチウィンドウの一端ａ０側から黄金比ｆを用いて計算されるθｘｇは、ｇａ１と定義される。ｇａ１は、以下により与えられる。
ｇａ１＝ａ０＋ｒ×（ｂ０−ａ０）          …（３）
【００７６】
そして、（３）式のｇａ１をθｘｇとして（１）式に代入したときの目的関数ｆ（θｘｅ）をｆａとすると、（４）式が得られる。
ｆａ＝｜ｇａ１−θｘｅ｜                  …（４）
【００７７】
また、サーチウィンドウの他端ｂ０側から黄金比ｆを用いて計算されるθｘｇは、ｇｂ１と定義される。ｇｂ１は、以下により与えられる。
ｇｂ１＝ａ０＋（１−ｒ）×（ｂ０−ａ０）  …（５）
【００７８】
そして、（５）式のｇｂ１をθｘｇとして（１）式に代入したときの目的関数ｆ（θｘｅ）をｆｂとすると、（６）式が得られる。
ｆｂ＝｜ｇｂ１−θｘｅ｜                  …（６）
【００７９】
この後、図１の第１の処理部Ｐ１は、ｆａとｆｂのいずれが最小であるかを決定する（ステップＳＴ１４）。
【００８０】
また、ｆａがｆｂよりも小さい場合、図１の第１の処理部Ｐ１は、ａ０側のθｘｇ、即ち、（３）式により計算されたｇａ１を仮角度θｘｐとして決定する。これに対し、ｆｂがｆａよりも小さい場合、図１の第１の処理部Ｐ１は、ｂ０側のθｘｇ、即ち、（５）式により計算されたｇｂ１を仮角度θｘｐとして決定する（ステップＳＴ１５）。
【００８１】
仮角度θｘｐが決定されると、図１の第１の処理部Ｐ１は、続けて、仮角度θｘｐを用いて、複数のジョイントの角度、例えば、図１のロボットシステムの場合はθ１〜θ６を計算する（ステップＳＴ１６）。
これら複数のジョイントの角度は、例えば、仮角度θｘｐとＤＨパラメータを用いて計算することができる。この計算は、ＩＫによる計算を含む。
【００８２】
以上の仮角度θｘｐ及び複数のジョイントの角度の計算は、サーチウィンドウの幅｜ｂ０−ａ０｜が所定のレンジεよりも小さくなるまで、又は、繰り返し計算の回数Ｎが所定の回数Ｎmaxよりも大きくなるまで、繰り返される（ステップＳＴ１７〜ＳＴ２０）。
【００８３】
例えば、繰り返し計算の回数Ｎが所定の回数Ｎmaxに等しいか又はそれよりも小さい場合、図１の第１の処理部Ｐ１は、繰り返し計算の回数Ｎを１つインクリメントし（ステップＳＴ１８）、かつ、サーチウィンドウの幅｜ｂ０−ａ０｜が所定のレンジεよりも小さいか否かを確認する（ステップＳＴ１９）。
【００８４】
サーチウィンドウの幅｜ｂ０−ａ０｜が所定のレンジεに等しいか又はそれよりも大きい場合、図１の第１の処理部Ｐ１は、サーチウィンドウ［ａ０，ｂ０］を更新し、再び、ステップＳＴ１４〜ＳＴ１６を実行する（ステップＳＴ２０）。
例えば、ｆａが最小の場合、図１の第１の処理部Ｐ１は、サーチウィンドウの他端であるｂ０を更新し、ｆｂが最小の場合、図１の第１の処理部Ｐ１は、サーチウィンドウの一端であるａ０を更新する。
【００８５】
図５は、黄金比を用いた仮角度の決定とサーチウィンドウの更新の例を示している。
(1) ｆａ＜ｆｂの場合
この場合、ｇａ１は、ｇｂ１よりもθｘｅに近い。即ち、ｆａ＝｜ｇａ１−θｘｅ｜が最小である（図４のステップＳＴ１４）。
従って、ａ０側のθｘｇ、即ち、（３）式により計算されたｇａ１が仮角度θｘｐとして決定される。また、この仮角度θｘｐに基づき、複数のジョイントの角度が計算される。
【００８６】
また、ｂ０を更新する処理が行われる。
例えば、
1.  ｂ０の値はｇｂ１に更新される。
2.  ｇａ１の値はそのまま引き継がれる。
3.  ｆａの値はそのまま引き継がれる。
4.  ｇｂ１の新しい値は（５）式から計算される。
5.  ｆｂの値は、（６）式から計算される。
【００８７】
(2) ｆａ＞ｆｂの場合
この場合、ｇｂ１は、ｇａ１よりもθｘｅに近い。即ち、ｆｂ＝｜ｇｂ１−θｘｅ｜が最小である（図４のステップＳＴ１４）。
従って、ｂ０側のθｘｇ、即ち、（５）式により計算されたｇｂ１が仮角度θｘｐとして決定される。また、この仮角度θｘｐに基づき、複数のジョイントの角度が計算される。
【００８８】
また、ａ０を更新する処理が行われる。
例えば、
1.  ａ０の値はｇａ１に更新される。
2.  ｇｂ１の値はそのまま引き継がれる。
3.  ｆｂの値はそのまま引き継がれる。
4.  ｇａ１の新しい値は（３）式から計算される。
5.  ｆａの値は、（４）式から計算される。
【００８９】
そして、以上のプロセスは、サーチウィンドウの幅｜ｂ０−ａ０｜が所定のレンジεよりも小さくなるまで、又は、繰り返し計算の回数Ｎが所定の回数Ｎmaxよりも大きくなるまで、繰り返される。
｜ｂ０−ａ０｜＜ε  又は  Ｎ＞Ｎmax        …（７）
【００９０】
尚、所定のレンジε及び所定の回数Ｎmaxは、規定値（デフォルト）として予め決定されていてもよいし、パソコンなどのホストから設定又は変更が可能であってもよい。
【００９１】
図４のフローの説明に戻る。
図１のロボットシステムの場合、第１の最終角度θ_1st，は、以下のようにして決定される。
【００９２】
サーチウィンドウの幅｜ｂ０−ａ０｜が所定のレンジεよりも小さくなった場合、その直前に更新されたａ０又はｂ０により計算されたｇａ１又はｇｂ１の値が最終的なθｘｇ、即ち、最終的な仮角度θｘｐとして決定される（ステップＳＴ１９）。
図５の例では、最終的な仮角度θｘｐは、ｇｂ１である。
【００９３】
また、サーチウィンドウの幅｜ｂ０−ａ０｜が所定のレンジεよりも狭くなる前に、繰り返し計算の回数Ｎが所定の回数Ｎmaxよりも大きくなった場合も、その直前に更新されたａ０又はｂ０により計算されたｇａ１又はｇｂ１の値が最終的なθｘｇ、即ち、最終的な仮角度θｘｐとして決定される（ステップＳＴ１７）。
【００９４】
そして、図１の第１の処理部Ｐ１は、最終的な仮角度θｘｐ及びＤＨパラメータから計算される複数のジョイントの角度を第１の最終角度として決定する。例えば、図１のロボットシステムの場合、θ１_1st，θ２_1st，θ３_1st，θ４_1st，θ５_1st，θ６_1stを第１の最終角度として決定する(ステップＳＴ２１)。
【００９５】
以上で解析的手法（第１のステージ）は終了する。
【００９６】
このように、解析的手法（第１のステージ）では、例えば、仮角度及びＤＨパラメータを用いるＩＫにより、１つの解、即ち、複数のジョイントの第１の最終角度を高速に求めることができる。
【００９７】
例えば、図１の第１の処理部Ｐ１は、エンドエフェクタをホームポジションからターゲットポジションに移動させるに当たって１つの解を決定できない場合でも、複数のジョイントの１つの角度を仮決定することにより、１つの解、即ち、複数のジョイントの全ての角度を計算できる。
【００９８】
また、この１つの解は、エンドエフェクタの座標位置、即ち、数値上のポジションを、ＦＫにより繰り返し計算するなどの複雑な計算によらないため、図１の第１の処理部Ｐ１は、この１つの解を高速に求めることができる。
【００９９】
次に、、上述の解析的手法に続いて、図１の第２の処理部Ｐ２は、数値的手法（第２のステージ）を実行する。
数値的手法は、第１の最終角度を複数のジョイントの初期角度として、エンドエフェクタの座標位置をターゲットポジションの座標位置にさらに近付ける処理である。
【０１００】
例えば、図１の第２の処理部Ｐ２は、複数のジョイントの角度を１つずつ所定のステップ値で変更し、ＦＫによりエンドエフェクタの座標位置を計算する処理を行う。また、図１の第２の処理部Ｐ２は、エンドエフェクタの座標位置とターゲットポジションの座標位置との差が所定値に等しいか又はそれよりも小さくなるまで、各ジョイントの角度を繰り返し変更する。
【０１０１】
従って、図１の第２の処理部Ｐ２は、エンドエフェクタの座標位置をターゲットポジションの座標位置に高精度で一致させることが可能な複数のジョイントの第２の最終角度を計算できる。
【０１０２】
また、数値的手法は、複雑な計算を必要とするが、複数のジョイントの初期角度は、解析的手法による解、即ち、第１の最終角度である。これは、数値的手法を実行する前において、エンドエフェクタの座標位置が、既に、ターゲットポジションの座標位置の近くにあることを意味する。
従って、エンドエフェクタの座標位置とターゲットポジションの座標位置との差を計算する繰り返し計算の回数は、ホームポジションからターゲットポジションに数値的手法のみで移動させる場合に比べて、大幅に減らすことができる。
【０１０３】
その結果、図１の第２の処理部Ｐ２は、複数のジョイントの第２の最終角度を高速に計算できる。
以下、数値的手法の詳細を説明する
【０１０４】
・  数値的手法（第２のステージ）
図６は、数値的手法の例を示している。
本発明の制御手法での数値的手法の特徴は、複数のジョイントの初期角度として解析的手法での解を用いる点にある。
【０１０５】
まず、図１の第２の処理部Ｐ２は、図１の第１の処理部Ｐ１から、複数のジョイントの初期角度、即ち、解析的手法により計算された１つの解（複数のジョイントの角度）を取得する。例えば、図１のロボットシステムの場合、図１の第２の処理部Ｐ２は、第１の最終角度θ_1stを取得する（ステップＳＴ３０）。
【０１０６】
次に、図１の第２の処理部Ｐ２は、複数のジョイントのうち、ジョイント角度の値を変更する１つのジョイントを選択する。例えば、図１の第２の処理部Ｐ２は、エンドエフェクタからの距離が最も大きいジョイントを順次選択する。但し、既に選択済みのジョイントは、選択の対象から外される（ステップＳＴ３１〜ＳＴ３２）。
【０１０７】
具体的には、図１の第２の処理部Ｐ２は、ジョイント角度θｘを選択する。但し、ｘは、ジョイント番号であり、θｘは、ジョイントＪｘの角度を意味する。また、ジョイントＪｘは、ｘの値が大きいほど、エンドエフェクタに近い。
また、ｘの値は、最初、１である。ｘの値は、１ずつインクリメントされ、かつ、ｘmaxが最大値である（ステップＳＴ３１，ＳＴ３８，ＳＴ３９）。
図１のロボットシステムの場合、ｘmaxは、６である。
【０１０８】
次に、図１の第２の処理部Ｐ２は、ジョイントＪｘのジョイント角度θｘの値を決定する。例えば、図１の第２の処理部Ｐ２は、複数のジョイントの角度のうち、ステップＳＴ３２において選択されたジョイントＪｘのジョイント角度（選択された角度)θｘの値を第１のステップ値θｘ_stepだけ変化させる（ステップＳＴ３３）。
【０１０９】
尚、第１のステップ値θｘ_stepは、規定値（デフォルト）として予め決定されていてもよいし、パソコンなどのホストから設定又は変更が可能であってもよい。
【０１１０】
この後、図１の第２の処理部Ｐ２は、ジョイント角度θｘの値に基づき、ジョイントＪｘの変換マトリックス（transformation matrix）を計算する（ステップＳＴ３４）。続けて、図１の第２の処理部Ｐ２は、ジョイント角度θｘの値、及び、残り全てのジョイント角度の値に基づき、全てのジョイントの変換マトリックスを計算する（ステップＳＴ３５）。
【０１１１】
また、図１の第２の処理部Ｐ２は、ステップＳＴ３５で求めた変換マトリックスに基づき、エラーマトリックスを計算する（ステップＳＴ３６）。続けて、図１の第２の処理部Ｐ２は、ステップＳＴ３６で求めたエラーマトリックスに基づき、全てのジョイントのヤコビアンマトリックス（例えば、９×６のヤコビアンマトリックス）のうちの１つのカラム（例えば、９×１のヤコビアンマトリックス）を計算する（ステップＳＴ３７）。
【０１１２】
次に、図１の第２の処理部Ｐ２は、全てのジョイントのヤコビアンマトリックス（例えば、９×６のヤコビアンマトリックスの全てのカラム）が計算されたか否かを確認する（ステップＳＴ３８）。
【０１１３】
例えば、ｘがｘｍａｘに等しくない場合、図１の第２の処理部Ｐ２は、全てのジョイントのヤコビアンマトリックスが計算されていないと判断し、ステップＳＴ３２で選択されるジョイントを変更する。
即ち、図１の第２の処理部Ｐ２は、ｘの値を１つインクリメントし（ステップＳＴ３９）、再び、ステップＳＴ３２〜ＳＴ３８のルーチンを実行する。
【０１１４】
一方、ｘがｘｍａｘに等しい場合、図１の第２の処理部Ｐ２は、全てのジョイントのヤコビアンマトリックスが計算されたと判断し、このヤコビアンマトリックスに基づき、新たな関節角度の集合θ_newが計算される。この関節角度の集合θ_newは、エンドエフェクタＥＥの座標位置を決定するためにＦＫで使用される（ステップＳＴ４０）。
【０１１５】
次に、図１の第２の処理部Ｐ２は、エンドエフェクタの座標位置とターゲットポジションの座標位置との差Ｄｅｖが所定値ＥＲＲminに等しいか、又は、それよりも小さいか、を確認する（ステップＳＴ４１）。
【０１１６】
差Ｄｅｖが所定値ＥＲＲminよりも大きい場合、エンドエフェクタの座標位置は、ターゲットポジションの座標位置に対して、許容範囲、例えば、ロボットシステムにおいて許容される誤差範囲外に存在することを意味する。
【０１１７】
従って、図１の第２の処理部Ｐ２は、エンドエフェクタの座標位置とターゲットポジションの座標位置との差Ｄｅｖが所定値ＥＲＲminに等しいか、又は、それよりも小さくなるまで、ステップＳＴ３１〜ＳＴ４１のルーチンを繰り返す。
【０１１８】
この繰り返しルーチンにおいて、図１の第２の処理部Ｐ２は、エンドエフェクタの座標位置とターゲットポジションの座標位置との差Ｄｅｖが前回(previous time)の差よりも大きい場合、選択された角度θｘの値を、第１のステップ値θｘ_stepよりも小さい第２のステップ値θｘ_stepで、かつ、前回に選択された角度θｘの値を変化させた方向と逆の方向に変化させる（ステップＳＴ４２〜ＳＴ４３）。
【０１１９】
これは、以下の理由による。
選択された角度θｘの値を変えて、エンドエフェクタの座標位置とターゲットポジションの座標位置との差を繰り返し計算していく処理において、エンドエフェクタの座標位置が、ターゲットポジションの座標位置を通り過ぎてしまう場合がある。
【０１２０】
この場合、図１の第２の処理部Ｐ２は、第１のステップ値θｘ_stepよりも小さい第２のステップ値θｘ_stepを用い、かつ、前回に選択された角度θｘの値を変化させた方向と逆の方向に、選択された角度θｘの値を変化させることにより、エンドエフェクタの座標位置をターゲットポジションの座標位置に向かって収束できるからである。
尚、第２のステップ値θｘ_stepは、第１のステップ値θｘ_stepの半分又はその近傍であるのが望ましい。
【０１２１】
一方、差Ｄｅｖが所定値ＥＲＲminに等しいか、又は、それよりも小さい場合、エンドエフェクタの座標位置は、ターゲットポジションの座標位置に対して、許容範囲、例えば、ロボットシステムにおいて許容される誤差範囲内に存在することを意味する。
【０１２２】
従って、図１の第２の処理部Ｐ２は、最新のθ１〜θ６、即ち、直近にアップデートされたθ１〜θ６を第２の最終角度θ１_2nd〜θ６_2ndとして決定する（ステップＳＴ４４）。
【０１２３】
以上で数値的手法（第２のステージ）は終了する。
【０１２４】
＜角度の計算とモータの駆動との関係＞
図１の第２の処理部Ｐ２は、図１の第１の処理部Ｐ１の計算結果、即ち、第１の最終角度に基づき、第２の最終角度を計算する。従って、図１の第２の処理部Ｐ２は、トータルの計算時間を短くするため、解析的手法（第１のステージ）が終了した後、直ちに数値的手法（第２のステージ）を実行するのが望ましい。
【０１２５】
同様に、図１の駆動部Ｄは、図１の第２の処理部Ｐ２の計算結果、即ち、第２の最終角度に基づき、複数のジョイント（複数のモータ）を駆動する駆動信号を出力する。従って、図１の駆動部Ｄは、複数のジョイントの角度を第２の最終角度に高速に変更するため、数値的手法（第２のステージ）が終了した後、直ちに駆動信号を出力するのが望ましい。
【０１２６】
以下、解析的手法及び数値的手法での角度の計算と複数のジョイント（モータ）の駆動との関係の例を説明する。
【０１２７】
図７は、角度の計算とモータの駆動との関係の例を示している。
【０１２８】
例えば、時刻ｔ０において、エンドエフェクタのポジションは、初期ポジション（ホームポジション）Ｐ_homeであると仮定する。初期ポジションＰ_homeは、例えば、図１の複数のジョイントＪ１〜Ｊ６が初期角度θ１_home，θ２_home，θ３_home，θ４_home，θ５_home，θ６_homeであるときのエンドエフェクタのポジションのことである。
【０１２９】
第１の処理部Ｐ１は、時刻ｔ０から時刻ｔ１までの間、第１の最終角度を得るための計算を実行する。第２の処理部Ｐ２は、時刻ｔ１から時刻ｔ２までの間、第２の最終角度を得るための計算を実行する。
即ち、第２の処理部Ｐ２は、第１の処理部Ｐ１の計算結果（第１の最終角度）が得られた後、直ちに第２の最終角度を計算する。
【０１３０】
駆動部Ｄは、時刻ｔ２において、駆動信号を出力する。即ち、駆動部Ｄは、第２の処理部Ｐ２の計算結果（第２の最終角度）が得られた後、直ちに駆動信号を出力する。
【０１３１】
モータＭ１〜Ｍ６は、駆動部Ｄからの駆動信号を受けると、複数のジョイントを駆動する。即ち、時刻ｔ２〜ｔ３において、モータＭ１〜Ｍ６は、エンドエフェクタのポジションを、初期ポジションＰ_homeからターゲットポジションＰ_targetに移動させる。
ターゲットポジションＰ_targetは、例えば、図１の複数のジョイントＪが第２の最終角度θ１_2nd，θ２_2nd，θ３_2nd，θ４_2nd，θ５_2nd，θ６_2ndであるときのエンドエフェクタのポジションのことである。
【０１３２】
尚、第２の処理部Ｐ２は、第１の最終角度が得られてから一定期間が経過した後、第２の最終角度を得るための計算を開始してもよい。同様に、駆動部Ｄは、第２の最終角度が得られてから一定期間が経過した後、複数のジョイント（複数のモータ）を駆動する駆動信号を出力してもよい。
【０１３３】
＜本発明の制御手法の効果＞
以下、本発明の制御手法の効果を説明する。
【０１３４】
図８及び図９は、本発明の制御手法の効果の例を示している。
これらの図において、本発明の制御手法は、実施形態に係わる解析的手法と数値的手法とを組み合わせた手法である。比較例としての解析的手法は、実施形態の第１のステージのみを実行する手法である。また、比較例としての数値的手法は、実施形態の第２のステージのみを実行する手法である。
【０１３５】
また、本発明の制御手法での数値的手法は、解析的手法の解を初期角度として用いるのに対し、比較例としての数値的手法は、ホームポジションでの複数のジョイントの角度を初期角度として用いる点で異なる。
【０１３６】
図８は、計算時間（高速化）の観点から、本発明の制御手法、解析的手法（first stage alone）、及び、数値的手法（second stage alone）を比較する。
同図から明らかなように、本発明の制御手法は、解析的手法とほぼ同じ計算時間で、１つの解、即ち、エンドエフェクタがターゲットポジションにより近くなるときの複数のジョイントの最終的な角度（第２の最終角度）を計算できる。
【０１３７】
例えば、計算時間が規格化（standardized）されたときの解析的手法での複数のジョイントの最終的な角度の計算時間が１（規格値）である場合、本発明の制御手法での複数のジョイントの最終的な角度（第２の最終角度）の計算時間は、２又はそれよりも小さい値を実現できる。これに対し、数値的手法での複数のジョイントの最終的な角度の計算時間は、約１０である。
【０１３８】
仮に、解析的手法での複数のジョイントの最終的な角度の計算時間が１ｍｓｅｃであると仮定すると、本発明の制御手法での複数のジョイントの最終的な角度（第２の最終角度）の計算時間は、２ｍｓｅｃ又はそれよりも小さい値であり、数値的手法での複数のジョイントの最終的な角度の計算時間は、約１０ｍｓｅｃである。
【０１３９】
尚、数値的手法において、計算時間が大幅に増大するのは、エンドエフェクタをホームポジションからターゲットポジションに移動させるために、ＦＫによる繰り返し計算の回数が非常に多くなるからである。
【０１４０】
このように、本発明の制御手法は、解析的手法とほぼ同じ計算時間で、１つの解、即ち、エンドエフェクタがターゲットポジションにより近くなるときの複数のジョイントの最終的な角度（第２の最終角度）を計算できる。
【０１４１】
図９は、位置合わせ精度（alignment accuracy）の観点から、本発明の制御手法、解析的手法（first stage alone）、及び、数値的手法（second stage alone）を比較する。
同図から明らかなように、本発明の制御手法は、数値的手法の精度に等しいか又はそれよりも高い精度で、１つの解、即ち、エンドエフェクタがターゲットポジションにより近くなるときの複数のジョイントの最終的な角度（第２の最終角度）を計算できる。
【０１４２】
但し、位置合わせ精度とは、エンドエフェクタの座標位置とターゲットポジションの座標位置との差（距離エラー）Ｄｅｖ_finalのことである。この場合、位置合わせ精度が高い、即ち、位置合わせが高精度であるとは、差Ｄｅｖ_finalが小さいことを意味する。
【０１４３】
例えば、位置合わせ精度が規格化されたときの数値的手法での複数のジョイントの最終的な角度の距離エラーＤｅｖ_finalが１（規格値）である場合、本発明の制御手法での複数のジョイントの最終的な角度（第２の最終角度）の距離エラーＤｅｖ_finalは、１又はそれよりも小さい値を実現できる。これに対し、解析的手法での複数のジョイントの最終的な角度の距離エラーＤｅｖ_finalは、約１０である。
【０１４４】
仮に、数値的手法での複数のジョイントの最終的な角度の距離エラーＤｅｖ_finalが１ｍｍであると仮定すると、本発明の制御手法での複数のジョイントの最終的な角度（第２の最終角度）の距離エラーＤｅｖ_finalは、約０．８ｍｍであり、解析的手法での複数のジョイントの最終的な角度の距離エラーＤｅｖ_finalは、約１０ｍｍである。
【０１４５】
尚、解析的手法において、位置合わせ精度が大幅に低下するのは、エンドエフェクタをホームポジションからターゲットポジションに移動させるために、ＦＫによる繰り返し計算を行わないからである。
【０１４６】
このように、本発明の制御手法は、数値的手法の精度に等しいか又はそれよりも高い精度で、１つの解、即ち、エンドエフェクタがターゲットポジションにより近くなるときの複数のジョイントの最終的な角度（第２の最終角度）を計算できる。
【０１４７】
＜エンドエフェクタの移動経路＞
例えば、図１のロボットシステムの場合、エンドエフェクタＥＥは、ホームポジションとターゲットポジションとの間を往復する動作を行うことができると共に、複数のターゲットポジションを経由してホームポジションに戻る動作を行うこともできる。
【０１４８】
図１０は、エンドエフェクタの移動経路の第１の例を示している。
本例は、エンドエフェクタＥＥがホームポジションＡ（Ｐ_home）とターゲットポジションＢ（Ｐ_target）との間を往復する例である。
Ａ及びＢ間の経路は、同図に示すように、直線であってもよいし、これに代えて、曲線、折線、又は、これらの組み合わせであってもよい。後者は、Ａ及びＢ間の経路に障害物があり、これを避けなければならない場合に有効である。
【０１４９】
上述の本発明の制御手法は、ターゲットポジションＢ（Ｐ_target）での複数のジョイントの角度（第２の最終角度）を高速かつ高精度に計算することを目的とする。従って、Ａ及びＢ間の経路、又は、この経路を求める手法については、特に限定されない。
Ａ及びＢ間の経路については、複数の選択肢から選択可能である。
【０１５０】
図１１は、エンドエフェクタの移動経路の第２の例を示している。
本例は、エンドエフェクタＥＥが複数のターゲットポジションＢ（Ｐ_target1），Ｃ（Ｐ_target2）を経由してホームポジションＡ（Ｐ_home）に戻る例である。
図１０の例と同様に、ＡからＢへの経路、ＢからＣへの経路、及び、ＣからＡへの経路は、それぞれ、直線であってもよいし、これに代えて、曲線、折線、又は、これらの組み合わせであってもよい。
【０１５１】
ＡからＢへの経路、ＢからＣへの経路、及び、ＣからＡへの経路については、複数の選択肢から選択可能である。
【０１５２】
まず、エンドエフェクタＥＥは、ホームポジションＡ（Ｐ_home）からターゲットポジションＢ（Ｐ_target1）に移動する。この移動において、例えば、図１の制御装置１１は、本発明の制御手法を用いた計算♯１により、第２の最終角度θ１_2nd_♯１〜θ６_2nd_♯１を求める。
【０１５３】
次に、エンドエフェクタＥＥは、ターゲットポジションＢ（Ｐ_target1）からターゲットポジションＣ（Ｐ_target2）に移動する。この移動において、例えば、図１の制御装置１１は、本発明の制御手法を用いた計算♯２により、第２の最終角度θ１_2nd_♯２〜θ６_2nd_♯２を求める。
【０１５４】
最後に、エンドエフェクタＥＥは、ターゲットポジションＣ（Ｐ_target2）からホームポジションＡ（Ｐ_home）に移動する。
【０１５５】
図１２及び図１３は、それぞれ、エンドエフェクタのポジションと制御装置の動作との関係の例を示している。
この関係は、エンドエフェクタを図１１の移動経路で移動させた場合の例である。即ち、図１２及び図１３のエンドエフェクタのポジションＡ，Ｂ，Ｃは、図１１のポジションＡ，Ｂ，Ｃに対応する。
【０１５６】
図１２の例では、第２の最終角度θ１_2nd_♯１〜θ６_2nd_♯１を求める計算♯１と、第２の最終角度θ１_2nd_♯２〜θ６_2nd_♯２を求める計算♯２とは、パラレルに実行される。この場合、第２の最終角度θ１_2nd_♯１〜θ６_2nd_♯１，θ１_2nd_♯２〜θ６_2nd_♯２を求める合計の計算時間が短縮されるが、例えば、図１の制御装置（例えば、ＣＰＵ、ＥＣＵなど）１１の負担が増大する。
【０１５７】
図１３の例では、第２の最終角度θ１_2nd_♯１〜θ６_2nd_♯１を求める計算♯１と、第２の最終角度θ１_2nd_♯２〜θ６_2nd_♯２を求める計算♯２とは、シリアルに実行される。この場合、例えば、図１の制御装置１１の負担は、軽減されるが、第２の最終角度θ１_2nd_♯１〜θ６_2nd_♯１，θ１_2nd_♯２〜θ６_2nd_♯２を求める合計の計算時間が長くなる。
【０１５８】
図１２の例と図１３の例のいずれを採用するかは、ロボットシステムの仕様、例えば、図１の制御装置１１の処理能力、計算♯１，♯２に要する計算時間、ＡからＢへの移動時間、ＢからＣへの移動時間など、に基づき決定するのが望ましい。
【０１５９】
尚、複数のジョイントの角度は、駆動信号ＤＳ_♯１により、θ１_home〜θ６_homeからθ１_2nd_♯１〜θ６_2nd_♯１に変化する。この時、エンドエフェクタはＡからＢに移動する。
【０１６０】
また、複数のジョイントの角度は、駆動信号ＤＳ_♯２により、θ１_2nd_♯１〜θ６_2nd_♯１からθ１_2nd_♯２〜θ６_2nd_♯２に変化する。この時、エンドエフェクタはＢからＣに移動する。
【０１６１】
さらに、複数のジョイントの角度は、駆動信号ＤＳ_homeにより、θ１_2nd_♯２〜θ６_2nd_♯２からθ１_home〜θ６_homeに変化する。この時、エンドエフェクタはＣからＡに移動する。
【０１６２】
＜むすび＞
以上、説明したように、本発明の実施形態によれば、制御装置は、エンドエフェクタをターゲットポジションに高速かつ高精度に移動させることができる。
なお、上記説明した各構成は、相互に矛盾しない範囲内において、適宜、組み合わせることができる。
本発明のいくつかの実施形態を説明したが、これら実施形態は、一例として提示したものであり、本発明の範囲を限定することを意図しない。これら実施形態は、上述以外の様々な形態で実施することが可能であり、本発明の要旨を逸脱しない範囲で、種々の省略、置換、変更など、を行える。これら実施形態及びその変形は、本発明の範囲及び要旨に含まれると共に、特許請求の範囲に記載された発明及びその均等物についても、本発明の範囲及び要旨に含まれる。
【符号の説明】
【０１６３】
１：  筐体、  ２：  モータ部、  ３：  制御部、  １０：  ロボット、  １１：  制御装置、  １２：  ネットワーク、  Ｍ、Ｍ１〜Ｍ６：  モータ、  Ｊ、Ｊ１〜Ｊ６：  ジョイント（リンク）、  ＥＥ：  エンドエフェクタ、  ＡＭ：  アーム、  Ｓ：  記憶部、  Ｐ１：  第１の処理部、  Ｐ２：  第２の処理部、  Ｄ：  駆動部。
【図１】
2018202535.tif 000003
【図２Ａ】
2018202535.tif 000004
【図２Ｂ】
2018202535.tif 000005
【図３】
2018202535.tif 000006
【図４】
2018202535.tif 000007
【図５】
2018202535.tif 000008
【図６】
2018202535.tif 000009
【図７】
2018202535.tif 000010
【図８】
2018202535.tif 000011
【図９】
2018202535.tif 000012
【図１０】
2018202535.tif 000013
【図１１】
2018202535.tif 000014
【図１２】
2018202535.tif 000015
【図１３】
2018202535.tif 000016