(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-07-22
(45)【発行日】2024-07-30
(54)【発明の名称】ロボットおよびロボット制御プログラム
(51)【国際特許分類】
B25J 13/00 20060101AFI20240723BHJP
【ＦＩ】
B25J13/00 Z
【請求項の数】
5
(21)【出願番号】P 2020102529
(22)【出願日】2020-06-12
(65)【公開番号】P2021194734
(43)【公開日】2021-12-27
【審査請求日】2023-04-20
【新規性喪失の例外の表示】特許法第３０条第２項適用 一般社団法人日本ロボット学会，第３７回日本ロボット学会学術講演会予稿集，ＲＳＪ２０１９ＡＣ３Ｅ３－０７，２０１９年９月３日発行 一般社団法人日本ロボット学会，日本ロボット学会誌 Ｖｏｌ．３７ Ｎｏ．８，第７２６～７３４頁，２０１９年１０月１５日発行 公益社団法人計測自動制御学会，第２５回ロボティクスシンポジア予稿集，第２８２～２８５頁，２０２０年３月１５日発行
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成３１年度、国立研究開発法人新エネルギー・産業技術総合開発機構「次世代人工知能・ロボットの中核となるインテグレート技術開発／人工知能技術の社会実装に向けた研究開発・実証／機械学習による生産支援ロボットの現場導入期間削減と多能化」委託研究、産業技術力強化法第１７条の適用を受ける特許出願
(73)【特許権者】
【識別番号】305027401
【氏名又は名称】東京都公立大学法人
(74)【代理人】
【識別番号】100137752
【弁理士】
【氏名又は名称】亀井 岳行
(72)【発明者】
【氏名】武居 直行
(72)【発明者】
【氏名】関口 叡範
【審査官】神山 貴行
(56)【参考文献】
【文献】特開２０１２－１９６７１６（ＪＰ，Ａ）
【文献】特開２０１３－０６６９６５（ＪＰ，Ａ）
【文献】特表２０１５－５２９１６３（ＪＰ，Ａ）
【文献】関口叡範・武居直行，「拡張角軸ベクトルによる関節変位の可動域を考慮した逆運動学の数値解法」，第３７回日本ロボット学会学術講演会，2019年
【文献】関口叡範・武居直行，「仮想バネの弾性エネルギー最小化に基づいた特異姿勢・非可解な問題に対して安定な逆運動学の数値解法」，日本ロボット学会誌，一般社団法人日本ロボット学会，2018年，第36巻，第9号，p.645-653
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｂ２５Ｊ １／００－２１／０２
(57)【特許請求の範囲】
【請求項1】
複数のアーム部と、２つのアーム部どうしの連結部位に配置され且つ一方のアーム部に対して他方のアーム部を変位させることが可能な関節部と、を有するアーム部材と、前記関節部の変位量を逆運動学の数値解法で演算してアーム部材の自由端を第１の目標に移動させる制御を行う制御部であって、数値解法で演算を行う場合に、前記アーム部材の自由端と前記第１の目標とを結ぶ仮想的な第１のバネと、前記アーム部材の自由端と基端との間の第２の部位と予め定められた第２の目標とを結ぶ仮想的な第２のバネと、を設定して、前記第１のバネおよび前記第２のバネの弾性エネルギーが減少するように前記変位量を繰り返し演算すると共に、前記繰り返し演算中に前記自由端が前記第１の目標にそれ以上近づかなくなった場合、または、前記繰り返し演算中に前記第１のバネの弾性エネルギーの減少が停止した場合に前記第１のバネのバネ定数は変更せず且つ前記第２のバネのバネ定数を下げて次回の演算を行うことで、前記自由端が前記第１の目標に達する前記変位量を演算して、前記変位量の演算結果に基づいて前記関節部の変位量を制御する前記制御部と、
を備えたことを特徴とするロボット。
【請求項2】
前記関節部を７つ以上有する前記アーム部材、を備えたことを特徴とする請求項１に記載のロボット。
【請求項3】
前記第１のバネの長さに関する演算として、弾性エネルギーを演算することを特徴とする請求項１または２に記載のロボット。
【請求項4】
前記自由端と前記第１の目標との直線距離に対応する第１のテンションバネと、前記自由端の前記第１の目標に対する方向に対応する第１のトーションバネと、を有する前記第１のバネと、前記第２の部位と前記第２の目標との直線距離に対応する第２のテンションバネと、前記第２の部位の前記第２の目標に対する方向に対応する第２のトーションバネと、を有する前記第２のバネと、
を備えたことを特徴とする請求項１ないし３のいずれかに記載のロボット。
【請求項5】
複数のアーム部と、２つのアーム部どうしの連結部位に配置され且つ一方のアーム部に対して他方のアーム部を変位させることが可能な関節部と、を有するアーム部材を有するロボットを制御するロボット制御プログラムであって、コンピュータを、
前記アーム部材の自由端と第１の目標とを結ぶ仮想的な第１のバネと、前記アーム部材の自由端と基端との間の第２の部位と予め定められた第２の目標とを結ぶ仮想的な第２のバネと、を設定して、前記第１のバネおよび前記第２のバネの弾性エネルギーが減少するように前記関節部の変位量を繰り返し演算すると共に、前記繰り返し演算中に前記自由端が前記第１の目標にそれ以上近づかなくなった場合、または、前記繰り返し演算中に前記第１のバネの弾性エネルギーの減少が停止した場合に前記第１のバネのバネ定数は変更せず且つ前記第２のバネのバネ定数を下げて次回の演算を行うことで、前記自由端が前記第１の目標に達する前記変位量を逆運動学の数値解法で演算する変位量演算手段、
前記変位量演算手段での前記アーム部材の自由端が前記第１の目標に達する演算結果に基づいて前記関節部の変位量を制御する変位制御手段、
として機能させることを特徴とするロボット制御プラグラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、アーム部と関節部とを有するロボットおよび当該ロボットを制御するロボット制御プログラムに関し、特に、ロボットの自由端を目標に移動させる制御を行う際に逆運動学の数値解法を使用するロボットおよびロボット制御プログラムに関する。
【背景技術】
【0002】
産業用ロボットアームのような多関節ロボットを制御するために必要となる要素技術の１つに逆運動学の計算がある。ロボットの自由端部（ロボットのハンド部）を目標に到達させる場合に、各関節の角度、向き（原因）から自由端部の位置（結果）を演算することが順運動学であり、自由端部の位置（結果）から各関節の角度、向き（原因）を演算（逆算）することが逆運動学である。
逆運動学において結果を得る方法として、方程式等を使用して解析的に解く解析的解法と、数値を当てはめていって近似的に解く数値解法とが知られている。解析的解法は、多関節ロボットの構造が特別な条件を満たしている場合に限定されるが、条件を満たせば問題なく逆運動学を計算することができる。数値解法はロボットの構造によらず使用できる利点がある（例えば、特許文献１参照）。
逆運動学において結果を得る方法として、方程式等を使用して解析的に解く解析的解法と、数値を当てはめていって近似的に解く数値解法とが知られている。解析的解法は、多関節ロボットの構造が特別な条件を満たしている場合に限定されるが、条件を満たせば問題なく逆運動学を計算することができる。数値解法はロボットの構造によらず使用できる利点がある（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【0003】
【文献】特許第５５８６０１５号公報（「０００２」－「０００３」、「００３０」－「００３５」、図１）
【非特許文献】
【0004】
【文献】T. Sugihara: "Robust Solution of Prioritized Inverse Kinematics Based on Hestenes-Powell Multiplier Method", IEEE/RSJ International Conference on Intelligent Robots and Systems,pp.510-515, 2014
【発明の概要】
【発明が解決しようとする課題】
【0005】
（従来技術の問題点）
数値的解法でロボットアームの自由端部（ハンド部）を目標に到達させる関節の変位量を演算する場合、自由端部と第１の目標とを結ぶ長さが前回の演算よりも短くなる変位量を演算することを繰り返して、最終的には前記長さがゼロになるように繰り返し計算していくことが行われている。
ここで、ロボットアームを現実の現場で運用する場合には、ロボットアームの周囲に壁等障害物が存在することがある。したがって、多関節のロボットアームを自由に変位させると、障害物に接触する恐れがある。このような状況に対応するために、ロボットアームの姿勢をある程度制限する目的で、自由端部を目標に近づける繰り返し演算を行う際に、ロボットアームの中間部分が第２の目標に近づくように合わせて計算することも行われている。
数値的解法でロボットアームの自由端部（ハンド部）を目標に到達させる関節の変位量を演算する場合、自由端部と第１の目標とを結ぶ長さが前回の演算よりも短くなる変位量を演算することを繰り返して、最終的には前記長さがゼロになるように繰り返し計算していくことが行われている。
ここで、ロボットアームを現実の現場で運用する場合には、ロボットアームの周囲に壁等障害物が存在することがある。したがって、多関節のロボットアームを自由に変位させると、障害物に接触する恐れがある。このような状況に対応するために、ロボットアームの姿勢をある程度制限する目的で、自由端部を目標に近づける繰り返し演算を行う際に、ロボットアームの中間部分が第２の目標に近づくように合わせて計算することも行われている。
【0006】
しかしながら、ロボットアームの構成や、第１の目標と第２の目標の位置関係、自由端部と中間部分との関係等によっては、自由端部を目標に近づけようとしても、中間部分を第２の目標に近づけようとすることが、第１の目標に自由端部を近づけることを妨げる場合がある。よって、繰り返し計算を何回やっても自由端部が第１の目標にある一定の距離から近づくことができず、自由端部が第１の目標に到達する変位量を演算できなくなる問題があった。
これを解消するために、非特許文献１には、ラグランジュ乗数を使用して第１の目標の優先度を第２の目標の優先度よりも高くして演算する方法が記載されている。
しかしながら、非特許文献１に記載の手法では、数値解法の繰り返し計算を行う際に、ラグランジュ乗数の収束が遅く、計算の終了までに時間がかかる（繰り返し回数が多くなる）問題があった。
これを解消するために、非特許文献１には、ラグランジュ乗数を使用して第１の目標の優先度を第２の目標の優先度よりも高くして演算する方法が記載されている。
しかしながら、非特許文献１に記載の手法では、数値解法の繰り返し計算を行う際に、ラグランジュ乗数の収束が遅く、計算の終了までに時間がかかる（繰り返し回数が多くなる）問題があった。
【0007】
本発明は、ラグランジュ乗数を使用してロボットアームの逆運動学の数値解法を行う従来の構成に比べて、計算時間を短縮することを技術的課題とする。
【課題を解決するための手段】
【0008】
前記技術的課題を解決するために、請求項１に記載の発明のロボットは、
複数のアーム部と、２つのアーム部どうしの連結部位に配置され且つ一方のアーム部に対して他方のアーム部を変位させることが可能な関節部と、を有するアーム部材と、
前記関節部の変位量を逆運動学の数値解法で演算してアーム部材の自由端を第１の目標に移動させる制御を行う制御部であって、数値解法で演算を行う場合に、前記アーム部材の自由端と前記第１の目標とを結ぶ仮想的な第１のバネと、前記アーム部材の自由端と基端との間の第２の部位と予め定められた第２の目標とを結ぶ仮想的な第２のバネと、を設定して、前記第１のバネおよび前記第２のバネの弾性エネルギーが減少するように前記変位量を繰り返し演算すると共に、前記繰り返し演算中に前記自由端が前記第１の目標にそれ以上近づかなくなった場合、または、前記繰り返し演算中に前記第１のバネの弾性エネルギーの減少が停止した場合に前記第１のバネのバネ定数は変更せず且つ前記第２のバネのバネ定数を下げて次回の演算を行うことで、前記自由端が前記第１の目標に達する前記変位量を演算して、前記変位量の演算結果に基づいて前記関節部の変位量を制御する前記制御部と、
を備えたことを特徴とする。
複数のアーム部と、２つのアーム部どうしの連結部位に配置され且つ一方のアーム部に対して他方のアーム部を変位させることが可能な関節部と、を有するアーム部材と、
前記関節部の変位量を逆運動学の数値解法で演算してアーム部材の自由端を第１の目標に移動させる制御を行う制御部であって、数値解法で演算を行う場合に、前記アーム部材の自由端と前記第１の目標とを結ぶ仮想的な第１のバネと、前記アーム部材の自由端と基端との間の第２の部位と予め定められた第２の目標とを結ぶ仮想的な第２のバネと、を設定して、前記第１のバネおよび前記第２のバネの弾性エネルギーが減少するように前記変位量を繰り返し演算すると共に、前記繰り返し演算中に前記自由端が前記第１の目標にそれ以上近づかなくなった場合、または、前記繰り返し演算中に前記第１のバネの弾性エネルギーの減少が停止した場合に前記第１のバネのバネ定数は変更せず且つ前記第２のバネのバネ定数を下げて次回の演算を行うことで、前記自由端が前記第１の目標に達する前記変位量を演算して、前記変位量の演算結果に基づいて前記関節部の変位量を制御する前記制御部と、
を備えたことを特徴とする。
【0009】
請求項２に記載の発明は、請求項１に記載のロボットにおいて、
前記関節部を７つ以上有する前記アーム部材、
を備えたことを特徴とする。
前記関節部を７つ以上有する前記アーム部材、
を備えたことを特徴とする。
【0010】
請求項３に記載の発明は、請求項１または２に記載のロボットにおいて、
前記第１のバネの長さに関する演算として、弾性エネルギーを演算する
ことを特徴とする。
前記第１のバネの長さに関する演算として、弾性エネルギーを演算する
ことを特徴とする。
【0011】
請求項４に記載の発明は、請求項１ないし３のいずれかに記載のロボットにおいて、
前記自由端と前記第１の目標との直線距離に対応する第１のテンションバネと、前記自由端の前記第１の目標に対する方向に対応する第１のトーションバネと、を有する前記第１のバネと、
前記第２の部位と前記第２の目標との直線距離に対応する第２のテンションバネと、前記第２の部位の前記第２の目標に対する方向に対応する第２のトーションバネと、を有する前記第２のバネと、
を備えたことを特徴とする。
前記自由端と前記第１の目標との直線距離に対応する第１のテンションバネと、前記自由端の前記第１の目標に対する方向に対応する第１のトーションバネと、を有する前記第１のバネと、
前記第２の部位と前記第２の目標との直線距離に対応する第２のテンションバネと、前記第２の部位の前記第２の目標に対する方向に対応する第２のトーションバネと、を有する前記第２のバネと、
を備えたことを特徴とする。
【0012】
前記技術的課題を解決するために、請求項５に記載の発明のロボット制御プラグラムは、
複数のアーム部と、２つのアーム部どうしの連結部位に配置され且つ一方のアーム部に対して他方のアーム部を変位させることが可能な関節部と、を有するアーム部材を有するロボットを制御するロボット制御プログラムであって、
コンピュータを、
前記アーム部材の自由端と第１の目標とを結ぶ仮想的な第１のバネと、前記アーム部材の自由端と基端との間の第２の部位と予め定められた第２の目標とを結ぶ仮想的な第２のバネと、を設定して、前記第１のバネおよび前記第２のバネの弾性エネルギーが減少するように前記関節部の変位量を繰り返し演算すると共に、前記繰り返し演算中に前記自由端が前記第１の目標にそれ以上近づかなくなった場合、または、前記繰り返し演算中に前記第１のバネの弾性エネルギーの減少が停止した場合に前記第１のバネのバネ定数は変更せず且つ前記第２のバネのバネ定数を下げて次回の演算を行うことで、前記自由端が前記第１の目標に達する前記変位量を逆運動学の数値解法で演算する変位量演算手段、
前記変位量演算手段での前記アーム部材の自由端が前記第１の目標に達する演算結果に基づいて前記関節部の変位量を制御する変位制御手段、
として機能させることを特徴とする。
複数のアーム部と、２つのアーム部どうしの連結部位に配置され且つ一方のアーム部に対して他方のアーム部を変位させることが可能な関節部と、を有するアーム部材を有するロボットを制御するロボット制御プログラムであって、
コンピュータを、
前記アーム部材の自由端と第１の目標とを結ぶ仮想的な第１のバネと、前記アーム部材の自由端と基端との間の第２の部位と予め定められた第２の目標とを結ぶ仮想的な第２のバネと、を設定して、前記第１のバネおよび前記第２のバネの弾性エネルギーが減少するように前記関節部の変位量を繰り返し演算すると共に、前記繰り返し演算中に前記自由端が前記第１の目標にそれ以上近づかなくなった場合、または、前記繰り返し演算中に前記第１のバネの弾性エネルギーの減少が停止した場合に前記第１のバネのバネ定数は変更せず且つ前記第２のバネのバネ定数を下げて次回の演算を行うことで、前記自由端が前記第１の目標に達する前記変位量を逆運動学の数値解法で演算する変位量演算手段、
前記変位量演算手段での前記アーム部材の自由端が前記第１の目標に達する演算結果に基づいて前記関節部の変位量を制御する変位制御手段、
として機能させることを特徴とする。
【発明の効果】
【0013】
請求項１，５に記載の発明によれば、ラグランジュ乗数を使用してロボットアームの逆運動学の数値解法を行う従来の構成に比べて、計算時間を短縮することができる。また、演算に必要なパラメータの調整にかかる手間を削減することができる。
請求項２に記載の発明によれば、解析的解法で変位量の解を得ることが困難な関節部が７つ以上のアーム部材を有する構成でも、変位量の解を得ることができる。
請求項３に記載の発明によれば、計算上の数値がマイナスになる可能性のあるパラメータを使用する場合に比べて、計算を簡便に行うことができる。
請求項４に記載の発明によれば、テンションバネとトーションバネを使用して、第１の目標の位置と姿勢の両方を満足する変位量を導出できる。
請求項２に記載の発明によれば、解析的解法で変位量の解を得ることが困難な関節部が７つ以上のアーム部材を有する構成でも、変位量の解を得ることができる。
請求項３に記載の発明によれば、計算上の数値がマイナスになる可能性のあるパラメータを使用する場合に比べて、計算を簡便に行うことができる。
請求項４に記載の発明によれば、テンションバネとトーションバネを使用して、第１の目標の位置と姿勢の両方を満足する変位量を導出できる。
【図面の簡単な説明】
【0014】
【図8】図８は、図７の各実験条件に対する本シミュレーション実験（実験例）の実験結果の説明図であり、図８Ａは図７Ａの実験結果の図、図８Ｂは図７Ｂの実験結果の図、図８Ｃは図７Ｃの実験結果の図、図８Ｄは図７Ｄの実験結果の図である。
【発明を実施するための形態】
【0015】
次に図面を参照しながら、本発明の実施の形態の具体例（以下、実施例と記載する）を説明するが、本発明は以下の実施例に限定されるものではない。
なお、以下の図面を使用した説明において、理解の容易のために説明に必要な部材以外の図示は適宜省略されている。
なお、以下の図面を使用した説明において、理解の容易のために説明に必要な部材以外の図示は適宜省略されている。
【実施例1】
【0016】
図１は本発明の実施例１のロボットの概略説明図である。
図１において、本発明のロボットの一例としての実施例１のロボットアームシステムＳは、アーム部材１を有する。アーム部材１は、複数のアーム部２と、アーム部２どうしを連結する関節部３とを有する。実施例１のアーム部材１は、台座部４に一端が支持された第１のアーム部２ａを有する。第１のアーム部２ａの他端には、第１の関節部３ａが支持されている。以下同様に、第２のアーム部２ｂ、第２の関節部３ｂ、…、第９の関節部３ｉ、第１０のアーム部２ｊが連結されている。第１０のアーム部２ｊの他端（先端）には対象物を把持、解放可能なロボットハンド部２ｋが形成されている。
したがって、実施例１のアーム部材１では、全体としては、第１のアーム部２ａの一端部が基端部となり、第１０のアーム部２ｊの他端部が自由端部となる。
図１において、本発明のロボットの一例としての実施例１のロボットアームシステムＳは、アーム部材１を有する。アーム部材１は、複数のアーム部２と、アーム部２どうしを連結する関節部３とを有する。実施例１のアーム部材１は、台座部４に一端が支持された第１のアーム部２ａを有する。第１のアーム部２ａの他端には、第１の関節部３ａが支持されている。以下同様に、第２のアーム部２ｂ、第２の関節部３ｂ、…、第９の関節部３ｉ、第１０のアーム部２ｊが連結されている。第１０のアーム部２ｊの他端（先端）には対象物を把持、解放可能なロボットハンド部２ｋが形成されている。
したがって、実施例１のアーム部材１では、全体としては、第１のアーム部２ａの一端部が基端部となり、第１０のアーム部２ｊの他端部が自由端部となる。
【0017】
なお、実施例１では、９個の関節部３ａ～３ｉと、１０個のアーム部２ａ～２ｊを有する構成を例示したがこれに限定されない。また、実施例１では、関節タイプを回転関節のみの構成例であるが、直動関節を含んだロボットアームでもよい。以降では、「変位量」の一例として、単に回転関節の「回転量」と表記する場合もあるが、直動関節の並進移動の「移動量」を意味する場合もあり、適宜読み替え可能である。
アーム部２や関節部３の数は、アーム部材１の用途や設計、仕様等に応じて任意の数に増減可能である。特に、逆運動学においては、Ｘ軸、Ｙ軸、Ｚ軸の直交座標系において、関節部３の位置ｘ，ｙ，ｚと、Ｘ軸、Ｙ軸、Ｚ軸周りの回転角θｘ，θｙ，θｚの６つの変数に対して、関節部３の数が６つ以下であれば、関節部毎に成立する方程式の数の方が変数より少なく、特定の条件を満たせば解析的に解くことが可能な場合もある。一方で、関節部３の数が７以上になると解析的に解くことは基本的には困難であり、数値的な解法が原則的に使用されることとなるため、関節部３の数が７以上である場合に、本願発明は好適に利用可能である。
アーム部２や関節部３の数は、アーム部材１の用途や設計、仕様等に応じて任意の数に増減可能である。特に、逆運動学においては、Ｘ軸、Ｙ軸、Ｚ軸の直交座標系において、関節部３の位置ｘ，ｙ，ｚと、Ｘ軸、Ｙ軸、Ｚ軸周りの回転角θｘ，θｙ，θｚの６つの変数に対して、関節部３の数が６つ以下であれば、関節部毎に成立する方程式の数の方が変数より少なく、特定の条件を満たせば解析的に解くことが可能な場合もある。一方で、関節部３の数が７以上になると解析的に解くことは基本的には困難であり、数値的な解法が原則的に使用されることとなるため、関節部３の数が７以上である場合に、本願発明は好適に利用可能である。
【0018】
前記関節部３（３ａ～３ｉ）は、回転軸を中心として一方のアーム部２ａ～２ｉに対して他方のアーム部２ｂ～２ｊを回転（変位）可能に構成されている。各関節部３には、図示しないモータが内蔵されており、モータの回転駆動、回転停止を制御することで回転量を制御可能に構成されている。各関節部３のモータは、実施例１では、図示しない無線通信チップで制御信号を受信して駆動する構成としているが、これに限定されない。通信ケーブル等の有線で制御信号を送受信可能な構成としてモータを制御することも可能である。なお、無線通信や有線通信の方式は、従来公知の任意の通信方式とすることが可能であり、携帯電話回線やBluetooth（登録商標）、無線ＬＡＮ等、任意の無線通信方式を採用したり、ＵＳＢ（Universal Serial Bus）等のシリアル方式やパラレル方式等任意の有線通信方式を採用することが可能である。
【0019】
前記アーム部材１は、情報処理装置の一例としてのコンピュータ装置１１で制御される。実施例１では、アーム部材１はコンピュータ装置１１との間で無線通信が可能に構成されている。コンピュータ装置１１は、コンピュータ本体１２と、表示部の一例としてのディスプレイ１３と、入力部の一例としてのキーボード１４およびマウス１５と、を有する。なお、実施例１では、コンピュータ装置１１としてデスクトップ型のコンピュータ装置を例示したがこれに限定されず、ラップトップ型（ノート型）のコンピュータ装置を使用することも可能である。また、コンピュータ装置１１として、パーソナルコンピュータ装置に限定されず、サーバー型やマイクロコンピュータ型、チップ型等の任意の形態とすることも可能である。
【0020】
実施例１のコンピュータ本体１２は、外部との信号の入出力および入出力信号レベルの調節等を行うＩ／Ｏ（入出力インターフェース）、必要な起動処理を行うためのプログラムおよびデータ等が記憶されたＲＯＭ（リードオンリーメモリ）、必要なデータ及びプログラムを一時的に記憶するためのＲＡＭ（ランダムアクセスメモリ）、ＲＯＭ等に記憶された起動プログラムに応じた処理を行うＣＰＵ（中央演算処理装置）ならびにクロック発振器等を有するコンピュータ装置により構成されており、前記ＲＯＭ及びＲＡＭ等に記憶されたプログラムを実行することにより種々の機能を実現することができる。
コンピュータ本体１２には、基本動作を制御する基本ソフト、いわゆる、オペレーティングシステムや、アプリケーションプログラムの一例としてのロボット制御プログラム、その他の図示しないソフトウェアが記憶されている。
コンピュータ本体１２には、基本動作を制御する基本ソフト、いわゆる、オペレーティングシステムや、アプリケーションプログラムの一例としてのロボット制御プログラム、その他の図示しないソフトウェアが記憶されている。
【0021】
実施例１のロボット制御プログラムは、以下の機能（手段、機能モジュール）Ｃ１，Ｃ２を有する。
回転量演算手段Ｃ１は、アーム部材１の自由端を目標に移動させるための各関節部３の回転量を逆運動学の数値解法で演算する。なお、逆運動学の数値解法は、従来公知であり、例えば、「関口叡範，武居直行: "仮想バネの弾性エネルギー最小化に基づいた特異姿勢・非可解な問題に対して安定な逆運動学の数値解法", 日本ロボット学会誌, vol.36, no.9, pp.645-653, 2018」や「T. Sugihara: "Solvability-unconcerned Inverse Kinematics by Levenberg-Marquardt Method",IEEE Transaction on Robotics, vol.27, no.5,pp.984-991, 2011」に記載されている。よって、以下では、理解の容易のために、まずは公知の逆運動学の数値解法について概略を説明し、その後で、本発明の特徴部分である優先度を変更する演算の説明をする。なお、本発明の回転量演算手段Ｃ１は、従来公知の逆運動学の数値解法に、本発明者が見出した優先度の変更を適用した演算を行う。
回転量演算手段Ｃ１は、アーム部材１の自由端を目標に移動させるための各関節部３の回転量を逆運動学の数値解法で演算する。なお、逆運動学の数値解法は、従来公知であり、例えば、「関口叡範，武居直行: "仮想バネの弾性エネルギー最小化に基づいた特異姿勢・非可解な問題に対して安定な逆運動学の数値解法", 日本ロボット学会誌, vol.36, no.9, pp.645-653, 2018」や「T. Sugihara: "Solvability-unconcerned Inverse Kinematics by Levenberg-Marquardt Method",IEEE Transaction on Robotics, vol.27, no.5,pp.984-991, 2011」に記載されている。よって、以下では、理解の容易のために、まずは公知の逆運動学の数値解法について概略を説明し、その後で、本発明の特徴部分である優先度を変更する演算の説明をする。なお、本発明の回転量演算手段Ｃ１は、従来公知の逆運動学の数値解法に、本発明者が見出した優先度の変更を適用した演算を行う。
【0022】
図２は実施例１の数値解法におけるロボットアームの各部の変位や目標との関係の説明図である。
◎逆運動学の数値解法の説明
多関節ロボット（アーム部材１）の自由度をｎとし、各関節変位を以下の［数１］のように定める。
なお、「Ｒn」は「ｎ次元列ベクトルの全体」である。
また、手先効果器（ロボットハンド部２ｋ）の位置、姿勢をそれぞれp1(q) ∈ R3,R1(q) ∈ SO (3) とし、手先効果器２ｋに与える目標（第１の目標）T１の位置、姿勢をそれぞれdp1∈ R3, dR1 ∈SO(3) とする。なお、「SO(3)」は「行列式が１となる３次の直交行列全体の集合」である。
このとき、手先効果器と目標との差を示す誤差ベクトルe1(q) を次の［数２］のように定める。
ここで、φ(＊) は回転行列を角軸ベクトルに変換する関数である。
◎逆運動学の数値解法の説明
多関節ロボット（アーム部材１）の自由度をｎとし、各関節変位を以下の［数１］のように定める。
【数1】
また、手先効果器（ロボットハンド部２ｋ）の位置、姿勢をそれぞれp1(q) ∈ R3,R1(q) ∈ SO (3) とし、手先効果器２ｋに与える目標（第１の目標）T１の位置、姿勢をそれぞれdp1∈ R3, dR1 ∈SO(3) とする。なお、「SO(3)」は「行列式が１となる３次の直交行列全体の集合」である。
このとき、手先効果器と目標との差を示す誤差ベクトルe1(q) を次の［数２］のように定める。
【数2】
【0023】
〇角軸ベクトルの変換関数についての説明
任意の回転行列R = [rij ] ∈ SO(3) に対し、ベクトルs を次の［数３］のように定める。
このとき、角軸ベクトルφ(R) は、次の［数４］のように表すことが可能である。
なお、［数４］における||ｓ||＝０かつｔｒＲ＝－１の場合については、本願の新規性喪失の例外の適用の証明書における提出文献２「関口叡範，武居直行: "特異点における角軸ベクトルの計算法と拡張角軸ベクトルの提案", 日本ロボット学会誌, vol.37, no.8, pp.726-734, 2019」の式（３９）の導出過程やＴａｂｌｅ２に詳細に記載されており、説明の簡素化のため本願明細書においてはこれ以上の詳細な説明は省略する。
任意の回転行列R = [rij ] ∈ SO(3) に対し、ベクトルs を次の［数３］のように定める。
【数3】
【数4】
【0024】
手先効果器の速度は、次の［数５］のように表される。
ここで、J1(q)∈R6×nはヤコビ行列であり、ω1(q)は手先効果器の角速度ベクトルである。
図２のアーム部材１において、自由端（ロボットハンド部２ｋ）と基端（第１のアーム部２ａの台座部４側の端）との間の第２の部位２ｍの位置、姿勢をp2(q) ∈ R3,R2(q) ∈ SO (3) とする。また、第２の部位２ｍの目標（第２の目標）Ｔ２の位置、姿勢をそれぞれdp2∈ R3, dR2 ∈SO(3) とする。なお、第２の部位２ｍや第２の目標Ｔ２は、アーム部材１の周辺に壁等の障害物が存在する等の状況でアーム部材１の挙動を制限する必要がある場合に、ユーザーが指定可能である。
上記のように第２の部位２ｍや第２の目標Ｔ２の位置、姿勢を定めた場合に、第２の部位２ｍに関する誤差ベクトルe2(q)を、次の［数６］のように定める。
【数5】
図２のアーム部材１において、自由端（ロボットハンド部２ｋ）と基端（第１のアーム部２ａの台座部４側の端）との間の第２の部位２ｍの位置、姿勢をp2(q) ∈ R3,R2(q) ∈ SO (3) とする。また、第２の部位２ｍの目標（第２の目標）Ｔ２の位置、姿勢をそれぞれdp2∈ R3, dR2 ∈SO(3) とする。なお、第２の部位２ｍや第２の目標Ｔ２は、アーム部材１の周辺に壁等の障害物が存在する等の状況でアーム部材１の挙動を制限する必要がある場合に、ユーザーが指定可能である。
上記のように第２の部位２ｍや第２の目標Ｔ２の位置、姿勢を定めた場合に、第２の部位２ｍに関する誤差ベクトルe2(q)を、次の［数６］のように定める。
【数6】
【0025】
また、第２の部位２ｍの位置p2(q)の速度は、［数５］と同様に、以下の［数７］のように表される。
ここで, ω2(q)は位置p2(q)の角速度ベクトルであり、J2(q)∈R6×nはヤコビ行列である。なお、第２の部位２ｍの位置p2(q)より手先効果器２ｋ側にある関節は［数７］の左辺（位置の時間微分及び角速度）に影響を与えないため、J2(q)の右側の成分は零行列となる。例えば、第２の部位２ｍが第６のアーム部２ｆに設定された場合、第６の関節部３ｆ～第９の関節部３ｉは、［数７］の左辺には影響を与えないため、ヤコビ行列J2(q)の第６～第９に関する成分はゼロになる。
なお、第２の部位と第２の目標以外にも、第３の部位と第３の目標、第４の部位と第４の目標、…のように、部位と目標の数が３つ以上にする場合は、同様の方法で誤差ベクトルとヤコビ行列を定めていけばよい。
【数7】
なお、第２の部位と第２の目標以外にも、第３の部位と第３の目標、第４の部位と第４の目標、…のように、部位と目標の数が３つ以上にする場合は、同様の方法で誤差ベクトルとヤコビ行列を定めていけばよい。
【0026】
図３はアーム部材の逆運動学における数値解法での仮想的なバネの説明図である。
〇仮想的なバネと緩衝（ダンピング）効果
図３において、誤差ベクトルe1(q)に対し、仮想直進バネ（第１のテンションバネの一例）５１と仮想回転バネ（第１のトーションバネの一例）５２を設定（仮定）する。第１の直進バネ５１のバネ定数をK1f∈Ｒ、第１の回転バネ５２のバネ定数をK1m∈Ｒとし、バネ定数行列を次の［数８］のように定める。
ここで、I3は３×３の単位行列である。
〇仮想的なバネと緩衝（ダンピング）効果
図３において、誤差ベクトルe1(q)に対し、仮想直進バネ（第１のテンションバネの一例）５１と仮想回転バネ（第１のトーションバネの一例）５２を設定（仮定）する。第１の直進バネ５１のバネ定数をK1f∈Ｒ、第１の回転バネ５２のバネ定数をK1m∈Ｒとし、バネ定数行列を次の［数８］のように定める。
【数8】
【0027】
同様に、誤差ベクトルe2(q)に対し、仮想直進バネ（第２のテンションバネの一例）５３と仮想回転バネ（第２のトーションバネの一例）５４を設定（仮定）する。第２の直進バネ５３のバネ定数をK2f∈Ｒ、第２の回転バネ５４のバネ定数をK2m∈Ｒとし、バネ定数行列を次の［数９］のように定める。
【数9】
【0028】
手先効果器２ｋに取り付けられている仮想バネK1（第１の直進バネ５１および第１のトーションバネ５２）のみを考慮した場合、仮想バネK1によって各関節に生じる仮想トルクτ1は、次の［数１０］のようになる。
同様に、仮想バネK2（第２の直進バネ５３および第２のトーションバネ５４）のみを考慮した場合、仮想バネK2によって各関節に生じる仮想トルクτ2は、次の［数１１］のようになる。
【数10】
【数11】
【0029】
したがって、各関節に生じる合計の仮想トルクτ(q)は次の［数１２］のように表される。
ここで、［数１２］におけるJ(q)、Ke(q)は、それぞれ、次の［数１３］～［数１５］となる。
なお、目標の数が３つ以上の場合は、［数１３］の右辺にJ3(q) 、J4(q) 、…、が追加されたり、［数１４］の右辺の対角部分にＫ3，Ｋ4、…が追加されたり、［数１５］の右辺にe3(q) 、e4(q) 、…、が追加されていくこととなる。
【数12】
【数13】
【数14】
【数15】
【0030】
［数１２］の仮想トルクτ(q)に対し、関節ベースでのダンピング効果により、各関節に速度が発生すると見なした場合，各関節の速度は、関節ダンピング係数行列D(q)∈Rn×nを用いて次の［数１６］のように表される。
なお、［数１６］におけるD(q)は後述する。
【数16】
【0031】
次に、反復計算により、逆運動学の数値解を求めることを検討する。反復回数ｋ回目における各関節変位をｑkとし、以降では、ｅ(qk)，τ(qk)，Ｄ(qk)，Ｊ(qk)をそれぞれｅk，τk，Ｄk，Ｊkと略記する。
仮想バネ（第１のバネ５１＋５２、第２のバネ５３＋５４）と関節ダンピング効果により、各関節に速度が生じたとき、その速度によって各関節変位が１ステップ当たりに変化する量をΔqk∈Rnとする。このとき、Δqkは次の［数１７］のように表される。
仮想バネ（第１のバネ５１＋５２、第２のバネ５３＋５４）と関節ダンピング効果により、各関節に速度が生じたとき、その速度によって各関節変位が１ステップ当たりに変化する量をΔqk∈Rnとする。このとき、Δqkは次の［数１７］のように表される。
【数17】
【0032】
各関節変位qkにΔqk を加えたものを次のステップの各関節変位とすれば、関節変位の更新則は次の［数１８］ように表される。
また、qk+1は次の［数１９］のように書くこともできる。
なお、［数１９］においてq0は初期関節変位である。
【数18】
【数19】
【0033】
図４は［数１７］～［数１９］の反復計算をブロック線図に表したものである。ただし、アーム部材１の各部位に与える目標からなる集合dχと、各部位の位置、姿勢からなる集合χkをそれぞれ次の［数２０］、［数２１］のように定めた。
【数20】
【数21】
【0034】
次に、仮想バネ５１＋５２，５３＋５４の弾性エネルギー最小化に基づくことで関節ダンピング係数行列Dk（［数１６］におけるD(q)の繰り返し計算ｋ回目の値）が満たすべき十分条件を導出する。さらに、その条件を満たすようなDkの具体的な定め方を示す。仮想バネの弾性エネルギーVkは次の［数２２］のように表される。
［数２２］において、バネ定数行列Ｋは半正定値行列であるため、Vk≧０となる。
【数22】
【0035】
図５は手先効果器を第１の目標に近づけていく繰り返し計算（反復計算）を行う際のｋ回目とｋ＋１回目の関係の説明図である。
図５のように、ベクトルJ1(qk)Δqkは、反復計算１回当たりにおける手先効果器２ｋの変化量に近似できるため、次の［数２３］が得られる。
誤差ベクトルe2(qk)においても同様のことが言えるため、［数２４］の近似式が得られる。
［数２３］、［数２４］より、［数２５］が得られる。
図５のように、ベクトルJ1(qk)Δqkは、反復計算１回当たりにおける手先効果器２ｋの変化量に近似できるため、次の［数２３］が得られる。
【数23】
【数24】
【数25】
【0036】
［数２２］および［数２５］から、Vk+1は、以下の［数２６］のように変形できる。
よって、［数２６］から、以下の［数２７］が得られる。
【数26】
【数27】
【0037】
［数２７］より、JT
kKek≠０かつ関節変位がある何らかの値に収束していない（Δqk≠０）とき、仮想バネ（５１＋５２，５３＋５４）の弾性エネルギーVkが減少する（Vk+1－Vk＜０となる）ことの十分条件としては、次の二つが考えられる。
（１）行列（Dk－（１／２）JT kKＪk ）が正定値行列（positive definite matrix）であること。
（２）行列Dkが正則行列（regular matrix）であること。
上記二つの条件を同時に満たすようなDkは無数に存在するが、その選定例として、以下の［数２８］がある。
ここで、In∈Rn×nは単位行列であり、δ∈Ｒは、Vk≒０のときにDkの非正則化を防ぐための微小な正の定数である。
（１）行列（Dk－（１／２）JT kKＪk ）が正定値行列（positive definite matrix）であること。
（２）行列Dkが正則行列（regular matrix）であること。
上記二つの条件を同時に満たすようなDkは無数に存在するが、その選定例として、以下の［数２８］がある。
【数28】
【0038】
［数２８］のようにDkを定めると、運動学モデルの変化や特異姿勢に対してロバストな逆運動学の数値解法を構築できることが知られている。上記のDkと［数１８］、［数１９］から以下の［数２９］に示す関節変位の更新則が得られる。
【数29】
【0039】
行列Dkは正定値対称行列となるため、逆行列を計算する際はCholesky分解を用いることで計算量を低減することができる。以下にq0からq1を求めるまでの手順（１）～（７）を示す。
（１）初期関節変位q0取得し、目標Ｔ１，Ｔ２の位置、姿勢dχを定める。
（２）順運動学を計算し、各部位の位置、姿勢χ0とヤコビ行列J0を求める。
（３）誤差ベクトルe0を計算する。
（４）仮想トルクτ0=J0 TKeoと評価関数Vo＝（１／２）eo TKe0を計算する。
（５）関節ダンピング係数行列D0を計算する。
（６）関節変位の変化量Δq0＝D0 -1τ0を計算する。
（７）q1＝q0＋Δq0を計算する。
（１）初期関節変位q0取得し、目標Ｔ１，Ｔ２の位置、姿勢dχを定める。
（２）順運動学を計算し、各部位の位置、姿勢χ0とヤコビ行列J0を求める。
（３）誤差ベクトルe0を計算する。
（４）仮想トルクτ0=J0 TKeoと評価関数Vo＝（１／２）eo TKe0を計算する。
（５）関節ダンピング係数行列D0を計算する。
（６）関節変位の変化量Δq0＝D0 -1τ0を計算する。
（７）q1＝q0＋Δq0を計算する。
【0040】
上記の手順（１）～（７）を繰り返し計算することで，逆運動学の数値解が得られる。すなわち、繰り返し１回目の手順（７）で得られたq1を、繰り返し２回目の手順（１）の初期関節変位として手順（１）～（７）を繰り返していく。
しかし、この方法ではそれぞれの目標Ｔ１，Ｔ２に優先度が与えられていない。次に、本願発明の特徴部分の説明の前に、従来の優先度を与える手法について説明する。なお、従来手法は、非特許文献１：「T. Sugihara: "Robust Solution of Prioritized Inverse Kinematics Based on Hestenes-Powell Multiplier Method", IEEE/RSJ International Conference on Intelligent Robots and Systems,pp.510-515, 2014」に詳細に記載されているため、以下は本願発明に関連する部分を中心に説明する。
しかし、この方法ではそれぞれの目標Ｔ１，Ｔ２に優先度が与えられていない。次に、本願発明の特徴部分の説明の前に、従来の優先度を与える手法について説明する。なお、従来手法は、非特許文献１：「T. Sugihara: "Robust Solution of Prioritized Inverse Kinematics Based on Hestenes-Powell Multiplier Method", IEEE/RSJ International Conference on Intelligent Robots and Systems,pp.510-515, 2014」に詳細に記載されているため、以下は本願発明に関連する部分を中心に説明する。
【0041】
従来手法では、［数１５］の誤差ベクトルを次の［数３０］のように修正したものである。
ここで、［数３０］、［数３１］におけるλk∈Ｒ6はラグランジュ乗数である。
【数30】
【数31】
【0042】
［数３１］より、ラグランジュ乗数λkには第１の目標Ｔ１との誤差e(qk)が毎回累積されることがわかる。λkを更新する際には、e1(qk)の第４成分から第６成分は角軸ベクトル（姿勢角）であるため、ベクトル同士の加算を単純に求めることができないことに注意しなければならない。具体的には、e1(qk)およびλkの第４成分から第６成分をそれぞれ単位クオータニオンまたは回転行列に変換してから姿勢誤差同士の加算を求めた後、再度角軸ベクトルに変換する必要がある。λkが追加されたことで、第１の目標Ｔ１が高優先度となり、第２の目標Ｔ２が低優先度となる。
しかしながら、従来方法は、ラグランジュ乗数λkの収束が遅く計算時間を要すること、および、λkの更新に姿勢角の加算を含み実装コストの点で不利であることが課題である。具体的には、「角軸ベクトルを一旦単位クオータニオンまたは回転行列に変換してから姿勢誤差同士の加算を求めた後、再度角軸ベクトルに変換する」プログラムを作成する必要があって、手間がかかり、実装コストの点で不利である。
しかしながら、従来方法は、ラグランジュ乗数λkの収束が遅く計算時間を要すること、および、λkの更新に姿勢角の加算を含み実装コストの点で不利であることが課題である。具体的には、「角軸ベクトルを一旦単位クオータニオンまたは回転行列に変換してから姿勢誤差同士の加算を求めた後、再度角軸ベクトルに変換する」プログラムを作成する必要があって、手間がかかり、実装コストの点で不利である。
【0043】
これに対して、実施例１では、［数１４］におけるバネ定数行列を次の［数３２］のように修正したものである。
ここで、ζ0＝１であり、ζk＝０かつVk≧０．９９Vk-1となったときは反復計算を終了する。条件Vk≧０．９９Vk-1」が真であることは仮想バネの弾性エネルギーVkの減少が停止したこと、すなわち、バネの伸び（＝手先効果器２ｋと第１の目標Ｔ１との距離）がそれ以上短くならなくなったことを意味する。なお、係数０．９９は数値誤差の影響を考慮して設定したものであり、要求される精度等の仕様や設計等に応じて任意に変更可能である。
【数32】
【数33】
【0044】
［数３２］において、Vk＜０．９９Vk-1の状況、すなわち、計算が繰り返される度に手先効果器２ｋが第１の目標Ｔ１に近づいている状況では、ζk+1＝ζk＝…＝ζ0＝１であり、［数３２］は［数１４］と同様になる。一方で、Vk≧０．９９Vk-1の状況、すなわち、繰り返し計算中に手先効果器２ｋが第１の目標Ｔ１にそれ以上近づかなくなった状況では、ζk+1＝ζk－（１／４）・ζ0＜１となる。このとき、ζk+1（すなわち繰り返し計算における次回のζk）はバネ定数行列Ｋ2の影響を徐々に弱める効果を有する。つまり、計算上で、第２のバネ５３＋５４のバネ定数が下がることに相当し、相対的に第１のバネ５１＋５２の方が第２のバネ５３＋５４に対して優位になることに相当する。言い換えると、第２の部位２ｍを第２の目標Ｔ２に近づける第２のバネ５３＋５４が弱くなって、手先効果器２ｋを第１の目標Ｔ１に近づける第１のバネ５１＋５２が相対的に強くなり、手先効果器２ｋが第１の目標Ｔ１に近づきやすくなる。この結果、第１の目標Ｔ１が高優先度となり、第２の目標Ｔ２が低優先度となる。
なお、優先度を逆にする場合は、［数３２］を次の［数３４］のように変更すればよい。
なお、優先度を逆にする場合は、［数３２］を次の［数３４］のように変更すればよい。
【数34】
【0045】
実施例１の回転量演算手段（変位量演算手段の一例）Ｃ１では、［数３２］のバネ定数行列を使用して、前述の手順（１）～（７）の演算を、手先効果器２ｋが第１の目標Ｔ１に到達するまで、すなわち、誤差ベクトルがゼロ（実際には所定の閾値以下）になるまで、繰り返して、関節部３の変位量ｑを導出し、演算結果として出力する。
回転量制御手段（変位制御手段の一例）Ｃ２は、回転量演算手段Ｃ１で演算された回転量に基づいて、各関節部３の回転量を制御して、ロボットハンド部２ｋを目標まで移動させるように制御する。
回転量制御手段（変位制御手段の一例）Ｃ２は、回転量演算手段Ｃ１で演算された回転量に基づいて、各関節部３の回転量を制御して、ロボットハンド部２ｋを目標まで移動させるように制御する。
【0046】
（実施例１の作用）
前記構成を備えた実施例１のロボットアームシステムＳでは、従来の誤差ベクトルにラグランジュ乗数を導入する優先度付きの数値解法に比べて、バネ定数Ｋ2に係数ζkをかけるだけの簡単な計算で第２の目標Ｔ２の優先度を下げることが可能である。したがって、計算負荷を抑制することが可能であり、実装コストの点でも有利である。
なお、実施例１では、第２の目標Ｔ２を導入しない従来の構成に比べても、アーム部材１の姿勢を第２の目標Ｔ２を導入することで制限しつつ、手先効果器２ｋ（自由端）が第１の目標Ｔ１に到達する変位量ｑを導出することが可能である。
前記構成を備えた実施例１のロボットアームシステムＳでは、従来の誤差ベクトルにラグランジュ乗数を導入する優先度付きの数値解法に比べて、バネ定数Ｋ2に係数ζkをかけるだけの簡単な計算で第２の目標Ｔ２の優先度を下げることが可能である。したがって、計算負荷を抑制することが可能であり、実装コストの点でも有利である。
なお、実施例１では、第２の目標Ｔ２を導入しない従来の構成に比べても、アーム部材１の姿勢を第２の目標Ｔ２を導入することで制限しつつ、手先効果器２ｋ（自由端）が第１の目標Ｔ１に到達する変位量ｑを導出することが可能である。
【0047】
なお、［数３３］において、Vk≧０．９９Vk-1の場合に、（１／４）ζ0ずつζkが小さくなっていく場合を例示したがこれに限定されない。ζkを小さくする割合が１／４、すなわち、２５％に限定されず、２０％や３０％等、設計や仕様等に応じて任意に変更可能である。ζkを小さくする割合は第２のバネ（５３＋５４）の優先度を低下させる割合に相当するため、この数値を調整することで、優先度を低下させる割合をユーザーが任意に調整可能である。例えば、優先度を低下させる割合を大きくすれば速やかに０に収束するため、全体の計算時間を短縮しやすく、優先度を低下させる割合を小さくすれば細かく計算することになって変位量ｑの誤差が少なくなる。よって、ユーザーが時間を優先するのか精度を優先するのかに応じて優先度を低下させる割合を調整可能である。
【0048】
また、ζ0＝１の場合を例示したがこれに限定されない。ζ0＝１は、初期状態では、第１のバネ（５１＋５２）と第２のバネ（５３＋５４）の優先度が同一であることに相当するが、初期状態において、第１のバネ（５１＋５２）を優先したければ、０≦ζ0＜１に設定することが可能であるし、第２のバネ（５３＋５４）を優先したければ、ζ0＞１に設定可能である。
特に、実施例１では、解析的解法で解を得ることが困難な関節部３の数が７以上のアーム部材１においても数値解法で解を得ることが可能であり、設計の自由度が向上している。
特に、実施例１では、解析的解法で解を得ることが困難な関節部３の数が７以上のアーム部材１においても数値解法で解を得ることが可能であり、設計の自由度が向上している。
【0049】
さらに、実施例１では、評価関数としてバネの弾性エネルギーVkを使用している。手先効果器２ｋと第１の目標Ｔ１との距離や弾性力を使用すると、計算上、ベクトルの向きが逆向きになると、距離や弾性力がマイナスの値を取る可能性があり、マイナスの値の場合の取り扱いについて絶対値を取る等の処理が必要になる場合がある。すなわち、マイナスかどうかの判別やマイナスの場合の追加の処理が必要になる場合がある。これに対して、弾性エネルギーVkは値がマイナスにならないため、そのような処理が必要なく、処理を容易にすることが可能である。なお、処理速度に余裕がある等の場合は、評価関数として弾性エネルギーではなく、距離や弾性力等を使用することも可能である。
【0050】
また、実施例１では、各バネ５１＋５２，５３＋５４として、直進バネ（テンションバネ）と回転バネ（トーションバネ）とを組み合わせたものを例示したが、これに限定されない。手先効果器２ｋの位置だけを制御して、向き（方向、姿勢）を制御する必要がない場合は、直進バネのみとすることも可能である。すなわち、直進バネは、位置に対応する変化量を導出するためのものであり、回転バネは向き（方向、姿勢）を制限するためのものと言える。
他にも、各バネの成分もｘ，ｙ，ｚの３軸方向を設定する場合を例示したが、これに限定されず、各バネの成分を指定可能である。例えば、直進バネ５１，５３をｘ，ｙ軸方向成分のみとし、回転バネ５２，５４をθｘ，θｙのみとして、ｚ軸上をｚ軸回りに回転することを許容するといったことも可能である。
他にも、各バネの成分もｘ，ｙ，ｚの３軸方向を設定する場合を例示したが、これに限定されず、各バネの成分を指定可能である。例えば、直進バネ５１，５３をｘ，ｙ軸方向成分のみとし、回転バネ５２，５４をθｘ，θｙのみとして、ｚ軸上をｚ軸回りに回転することを許容するといったことも可能である。
【0051】
（シミュレーション実験）
図６はシミュレーション実験で使用するアーム部材の説明図である。
次に、実施例１の有効性をシミュレーション実験で確認した。なお、本シミュレーション実験では、前述のラグランジュ乗数を導入する従来手法を比較例として対比する形で行った。
〇計算時間とロバスト性の検証
本シミュレーション実験では、図６に示すように、逆運動学を解析的に解くことが困難である９自由度（関節部が９つ）の多関節ロボット（アーム部材１）を使用した。それぞれのリンク（アーム部２）の長さは２００ｍｍであり、アーム部材１の全長はＬ＝２０００ｍｍである。手先効果器２ｋと第１の目標Ｔ１との誤差ベクトルをe1(qk)とし、６番のリンク（アーム部）の中央と第２の目標Ｔ２との誤差ベクトルをe2(qk)とする。第１の目標Ｔ１を高優先度とし、第２の目標Ｔ２を低優先度とした。バネ定数行列の成分とδは、ロボットの全長Ｌを用いて次の［数３５］～［数３７］のように定めた。
図６はシミュレーション実験で使用するアーム部材の説明図である。
次に、実施例１の有効性をシミュレーション実験で確認した。なお、本シミュレーション実験では、前述のラグランジュ乗数を導入する従来手法を比較例として対比する形で行った。
〇計算時間とロバスト性の検証
本シミュレーション実験では、図６に示すように、逆運動学を解析的に解くことが困難である９自由度（関節部が９つ）の多関節ロボット（アーム部材１）を使用した。それぞれのリンク（アーム部２）の長さは２００ｍｍであり、アーム部材１の全長はＬ＝２０００ｍｍである。手先効果器２ｋと第１の目標Ｔ１との誤差ベクトルをe1(qk)とし、６番のリンク（アーム部）の中央と第２の目標Ｔ２との誤差ベクトルをe2(qk)とする。第１の目標Ｔ１を高優先度とし、第２の目標Ｔ２を低優先度とした。バネ定数行列の成分とδは、ロボットの全長Ｌを用いて次の［数３５］～［数３７］のように定めた。
【数35】
【数36】
【数37】
【0052】
図７は第２の目標の位置を変えながら行うシミュレーション実験の説明図であり、図７Ａは実験例１の説明図、図７Ｂは実験例２の説明図、図７Ｃは実験例３の説明図、図７Ｄは実験例４の説明図である。
なお、［数３５］、［数３６］、［数３７］は、バネ定数行列とδの物理次元および本願発明者らの試行錯誤に基づいて決定した。ただし、本シミュレーション実験では誤差ベクトルe2(qk)に対しては第２の仮想回転バネ５４を取り付けないためK2m＝０とした。本シミュレーション実験では、図７に示す４種類の試験を行う。図７において、アーム部材１の初期姿勢はいずれも真っ直ぐ上方向に伸び切った姿勢（特異姿勢）である。各目標Ｔ１，Ｔ２の位置・姿勢はそれぞれ次の［数３８］、［数３９］のように定めた。
なお、［数３５］、［数３６］、［数３７］は、バネ定数行列とδの物理次元および本願発明者らの試行錯誤に基づいて決定した。ただし、本シミュレーション実験では誤差ベクトルe2(qk)に対しては第２の仮想回転バネ５４を取り付けないためK2m＝０とした。本シミュレーション実験では、図７に示す４種類の試験を行う。図７において、アーム部材１の初期姿勢はいずれも真っ直ぐ上方向に伸び切った姿勢（特異姿勢）である。各目標Ｔ１，Ｔ２の位置・姿勢はそれぞれ次の［数３８］、［数３９］のように定めた。
【数38】
【数39】
【0053】
また、シミュレーション実験の繰り返し計算の終了条件には、以下の［数４０］の関数を用いた。
なお、本シミュレーション実験では、V1k＜０．１となったときに繰り返し計算を終了するものとする。実施例１の手法を用いる場合は、繰り返し計算の終了条件に「ζk＝０かつVk≧０．９９Vk-1」を用いることができるが、ここでは、ラグランジュ乗数を使用する従来手法と実験条件を等しくするために終了条件をV1k＜０．１としている。なお、「ζk＝０かつVk≧０．９９Vk-1」も「V1k＜０．１」も手先効果器２ｋが第１の目標Ｔ１に到達した場合に成立するものであるため、終了条件としては実質的には同様のものである。
【数40】
【0054】
図８は、図７の各実験条件に対する本シミュレーション実験（実験例）の実験結果の説明図であり、図８Ａは図７Ａの実験結果の図、図８Ｂは図７Ｂの実験結果の図、図８Ｃは図７Ｃの実験結果の図、図８Ｄは図７Ｄの実験結果の図である。
図９は、図７の各実験条件に対する従来手法（比較例）の実験結果の説明図であり、図９Ａは図７Ａの実験結果の図、図９Ｂは図７Ｂの実験結果の図、図９Ｃは図７Ｃの実験結果の図、図９Ｄは図７Ｄの実験結果の図である。
実施例１の手法と従来の手法（比較例）を用いて、図７の逆運動学を解いたときの結果を図８、図９にそれぞれ示す。ただし、ここでの実験では従来法の［数３１］に対して、α∈Ｒを次の［数４１］のように追加した。
図９は、図７の各実験条件に対する従来手法（比較例）の実験結果の説明図であり、図９Ａは図７Ａの実験結果の図、図９Ｂは図７Ｂの実験結果の図、図９Ｃは図７Ｃの実験結果の図、図９Ｄは図７Ｄの実験結果の図である。
実施例１の手法と従来の手法（比較例）を用いて、図７の逆運動学を解いたときの結果を図８、図９にそれぞれ示す。ただし、ここでの実験では従来法の［数３１］に対して、α∈Ｒを次の［数４１］のように追加した。
【数41】
【0055】
なお、従来の手法が記載された前述の論文ではα＝1となっているが、ここでの実験ではαを他の値に変更した方が従来法での計算時間の点で高い性能を発揮したため、αを追加した。なお、図９に示した結果はα＝０．４のときのものであるが，α＝０．２，０．３，…，０．７の６つの場合のいずれにおいても最終的に得られる結果は同じものとなった。
【0056】
図８と図９を比較すると、Test１（図８Ａ、図９Ａ）、Test３（図８Ｃ、図９Ｃ）、Test４（図８Ｄ、図９Ｄ）の３つにおいては同じ結果となった。また、Test２（図８Ｂ、図９Ｂ）においては関節の屈曲方向が異なっているが、この相違は偶発的に生じたものであり、有意差ではない。よって、得られる逆運動学解については、実験例（実施例１）と比較例（従来手法）に差は見られない。
【0057】
図１０は実施例と比較例における収束に必要な計算時間をまとめた図表である。
しかし、収束に必要となる計算時間（反復回数）とロバスト性においては差が見られる。収束に要した反復回数を図１０に示す。従来法（比較例）では、ラグランジュ乗数λkの影響により手先効果器２ｋが第１の目標Ｔ１の周辺を振動しながら徐々に収束するという特性がある。αが大きいほどこの振動は大きくなり、逆にαを小さくすると振動も小さくなる。αが適正値よりも大きい場合、振動の影響により収束が遅くなり多くの反復回数が必要となる。逆に、αが適正値よりも小さい場合は、振動による収束の遅延は起こらないが、V1kの減少率が低下し、閾値（０．１）以下になるまでに多くの反復回数が必要となる。図１０の平均値において、従来法（比較例）の反復回数が最も少なかったのはα＝０．４のときであるが、α＝０．４の従来法（比較例）よりも本発明の手法（実験例）の方が高速に収束している。よって、実施例１の手法は従来の手法よりも高速に収束していることが確認された。
しかし、収束に必要となる計算時間（反復回数）とロバスト性においては差が見られる。収束に要した反復回数を図１０に示す。従来法（比較例）では、ラグランジュ乗数λkの影響により手先効果器２ｋが第１の目標Ｔ１の周辺を振動しながら徐々に収束するという特性がある。αが大きいほどこの振動は大きくなり、逆にαを小さくすると振動も小さくなる。αが適正値よりも大きい場合、振動の影響により収束が遅くなり多くの反復回数が必要となる。逆に、αが適正値よりも小さい場合は、振動による収束の遅延は起こらないが、V1kの減少率が低下し、閾値（０．１）以下になるまでに多くの反復回数が必要となる。図１０の平均値において、従来法（比較例）の反復回数が最も少なかったのはα＝０．４のときであるが、α＝０．４の従来法（比較例）よりも本発明の手法（実験例）の方が高速に収束している。よって、実施例１の手法は従来の手法よりも高速に収束していることが確認された。
【0058】
また、α＞０．９として従来法（比較例）を用いた場合には、計算結果が発散するという事例が観測されている。すなわち、従来法（比較例）では計算に成功するか否かが設計定数αに依存することになる。その一方で、実施例１の手法は必要な反復回数や計算に成功するか否かが特定の設計定数に大きく依存しないという点で従来法（比較例）よりもロバストであると言える。
なお、本実験では、反復計算の途中で関節変位が可動域の限界に達することはなかった。もし反復計算の途中でｉ番目の関節が可動域の限界に達した場合は、ヤコビ行列のｉ列目を零ベクトルに置き換えることで収束性能の低下を回避することができる。
なお、本実験では、反復計算の途中で関節変位が可動域の限界に達することはなかった。もし反復計算の途中でｉ番目の関節が可動域の限界に達した場合は、ヤコビ行列のｉ列目を零ベクトルに置き換えることで収束性能の低下を回避することができる。
【0059】
〇特異姿勢に対するロバスト性の検証
次に、実施例１の手法の特異姿勢に対するロバスト性の検証（シミュレーション実験）を行った。第１の目標Ｔ１と第２の目標Ｔ２を次の［数４２］、［数４３］ように定めた。
次に、実施例１の手法の特異姿勢に対するロバスト性の検証（シミュレーション実験）を行った。第１の目標Ｔ１と第２の目標Ｔ２を次の［数４２］、［数４３］ように定めた。
【数42】
【数43】
【0060】
図１１は特異姿勢に対するロバスト性の検証の結果の説明図であり、図１１Ａは繰り返し回数ｋ＝０、ζk＝１の場合（初期状態）の説明図、図１１Ｂは繰り返し回数ｋ＝５、ζk＝０．７５の状態の説明図、図１１Ｃは繰り返し回数ｋ＝１３、ζk＝０．２５の状態の説明図、図１１Ｄは繰り返し回数ｋ＝１７、ζk＝０の状態の説明図である。
高優先度である第１の目標Ｔ１との誤差が最小化されたとき、アーム部材１は特異姿勢となる。実施例１の手法を用いて逆運動学を解いたときの結果を図１１に示す。ダンピング係数行列Dkに含まれるδInは，行列Dkの悪条件化を緩和するため、特異姿勢においても計算破綻は起こらず、V1kを最小化するような各関節変位（逆運動学解）が得られることが確認された。
高優先度である第１の目標Ｔ１との誤差が最小化されたとき、アーム部材１は特異姿勢となる。実施例１の手法を用いて逆運動学を解いたときの結果を図１１に示す。ダンピング係数行列Dkに含まれるδInは，行列Dkの悪条件化を緩和するため、特異姿勢においても計算破綻は起こらず、V1kを最小化するような各関節変位（逆運動学解）が得られることが確認された。
【0061】
（変更例）
以上、本発明の実施例を詳述したが、本発明は、前記実施例に限定されるものではなく、特許請求の範囲に記載された本発明の要旨の範囲内で、種々の変更を行うことが可能である。本発明の変更例（Ｈ01）～（Ｈ03）を下記に例示する。
（Ｈ01）前記実施例において、関節部３の数やアームの長さや数、形状等は例示した形態に限定されない。したがって、アーム部材の用途や設計、仕様等に応じて、数や長さ、形状等、任意に変更可能である。
（Ｈ02）前記実施例や実験例において、例示した具体的な数値は例示したものに限定されず、設計や仕様等に応じて任意に変更可能である。
（Ｈ03）前記実施例において、関節部３の種類は回転関節のみで構成されていたが、その形態に限定されない。すなわち、直動関節を含む場合も適用可能である。
以上、本発明の実施例を詳述したが、本発明は、前記実施例に限定されるものではなく、特許請求の範囲に記載された本発明の要旨の範囲内で、種々の変更を行うことが可能である。本発明の変更例（Ｈ01）～（Ｈ03）を下記に例示する。
（Ｈ01）前記実施例において、関節部３の数やアームの長さや数、形状等は例示した形態に限定されない。したがって、アーム部材の用途や設計、仕様等に応じて、数や長さ、形状等、任意に変更可能である。
（Ｈ02）前記実施例や実験例において、例示した具体的な数値は例示したものに限定されず、設計や仕様等に応じて任意に変更可能である。
（Ｈ03）前記実施例において、関節部３の種類は回転関節のみで構成されていたが、その形態に限定されない。すなわち、直動関節を含む場合も適用可能である。
【符号の説明】
【0062】
１…アーム部材、
２…アーム部、
２ｋ…自由端、
２ｍ…第２の部位、
３…関節部、
１１…コンピュータ、
５１…第１のテンションバネ、
５１＋５２…第１のバネ、
５２…第１のトーションバネ、
５３…第２のテンションバネ、
５３＋５４…第２のバネ、
５４…第２のトーションバネ、
Ｃ…制御部、
Ｃ１…変位量演算手段、
Ｃ２…変位量制御手段、
Ｋ2…第２のバネのバネ定数、
Ｓ…ロボット、
Ｔ１…第１の目標、
Ｔ２…第２の目標、
Vk…弾性エネルギー。
２…アーム部、
２ｋ…自由端、
２ｍ…第２の部位、
３…関節部、
１１…コンピュータ、
５１…第１のテンションバネ、
５１＋５２…第１のバネ、
５２…第１のトーションバネ、
５３…第２のテンションバネ、
５３＋５４…第２のバネ、
５４…第２のトーションバネ、
Ｃ…制御部、
Ｃ１…変位量演算手段、
Ｃ２…変位量制御手段、
Ｋ2…第２のバネのバネ定数、
Ｓ…ロボット、
Ｔ１…第１の目標、
Ｔ２…第２の目標、
Vk…弾性エネルギー。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
