(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2022059740
(43)【公開日】2022-04-14
(54)【発明の名称】ロボットおよびロボット制御プログラム
(51)【国際特許分類】
B25J 9/10 20060101AFI20220407BHJP
【ＦＩ】
B25J9/10 A
【審査請求】未請求
【請求項の数】4
【出願形態】ＯＬ
(21)【出願番号】P 2020167512
(22)【出願日】2020-10-02
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成３１年度、国立研究開発法人新エネルギー・産業技術総合開発機構「次世代人工知能・ロボットの中核となるインテグレート技術開発／人工知能技術の社会実装に向けた研究開発・実証／機械学習による生産支援ロボットの現場導入期間削減と多能化」委託研究、産業技術力強化法第１７条の適用を受ける特許出願
(71)【出願人】
【識別番号】305027401
【氏名又は名称】東京都公立大学法人
(74)【代理人】
【識別番号】100137752
【弁理士】
【氏名又は名称】亀井 岳行
(72)【発明者】
【氏名】関口 叡範
(72)【発明者】
【氏名】武居 直行
【テーマコード（参考）】
3C707
【Ｆターム（参考）】
3C707BS13
3C707DS01
3C707HS27
3C707JS07
3C707KS16
3C707KS20
3C707LS15
3C707MS08
(57)【要約】
【課題】関節空間を使用する場合に比べて、多関節のロボットの動作の経路の計算負荷を軽減すること。
【解決手段】複数のアーム部（２）と、２つのアーム部（２）どうしの連結部位に配置された関節部（３）と、を有するアーム部材（１）と、アーム部材（１）の自由端（２ｋ）を予め定められた目標（２１）に移動させる動作における関節部（３）の位置について逆運動学を解いてアーム部材（１）の動作後の姿勢を導出する第１の姿勢導出手段（Ｃ３）と、第１の姿勢導出手段（Ｃ３）で導出された各関節部（３）の動作前の位置と動作後の位置に基づいて、動作の途中で障害物（２０）に干渉する場合には、動作時に自由端（２ｋ）が通過する候補の位置である中間位置（２２）を目標の位置として逆運動学を解いて、アーム部材（１）の中間姿勢を導出する第２の姿勢導出手段（Ｃ６）と、を備えたロボット（Ｓ）。
【選択図】図５
【解決手段】複数のアーム部（２）と、２つのアーム部（２）どうしの連結部位に配置された関節部（３）と、を有するアーム部材（１）と、アーム部材（１）の自由端（２ｋ）を予め定められた目標（２１）に移動させる動作における関節部（３）の位置について逆運動学を解いてアーム部材（１）の動作後の姿勢を導出する第１の姿勢導出手段（Ｃ３）と、第１の姿勢導出手段（Ｃ３）で導出された各関節部（３）の動作前の位置と動作後の位置に基づいて、動作の途中で障害物（２０）に干渉する場合には、動作時に自由端（２ｋ）が通過する候補の位置である中間位置（２２）を目標の位置として逆運動学を解いて、アーム部材（１）の中間姿勢を導出する第２の姿勢導出手段（Ｃ６）と、を備えたロボット（Ｓ）。
【選択図】図５
【特許請求の範囲】
【請求項1】
複数のアーム部と、２つのアーム部どうしの連結部位に配置され且つ一方のアーム部に対して他方のアーム部を変位させることが可能な関節部と、を有するアーム部材と、
前記アーム部材の自由端を予め定められた目標に移動させる動作における前記関節部の位置について逆運動学を解いて前記アーム部材の動作後の姿勢を導出する第１の姿勢導出手段と、
前記第１の姿勢導出手段で導出された各関節部の動作前の位置と動作後の位置に基づいて、動作の途中で障害物に干渉する場合には、動作時に自由端が通過する候補の位置である中間位置を目標の位置として逆運動学を解いて、前記アーム部材の中間姿勢を導出する第２の姿勢導出手段と、
前記第１の姿勢導出手段で導出された動作の途中で前記アーム部材が障害物に干渉する場合には、前記第２の姿勢導出手段で導出された中間姿勢を経由して前記アーム部材の自由端を目標に移動させるように各関節部を制御するアーム制御手段と、
を備えたことを特徴とするロボット。
前記アーム部材の自由端を予め定められた目標に移動させる動作における前記関節部の位置について逆運動学を解いて前記アーム部材の動作後の姿勢を導出する第１の姿勢導出手段と、
前記第１の姿勢導出手段で導出された各関節部の動作前の位置と動作後の位置に基づいて、動作の途中で障害物に干渉する場合には、動作時に自由端が通過する候補の位置である中間位置を目標の位置として逆運動学を解いて、前記アーム部材の中間姿勢を導出する第２の姿勢導出手段と、
前記第１の姿勢導出手段で導出された動作の途中で前記アーム部材が障害物に干渉する場合には、前記第２の姿勢導出手段で導出された中間姿勢を経由して前記アーム部材の自由端を目標に移動させるように各関節部を制御するアーム制御手段と、
を備えたことを特徴とするロボット。
【請求項2】
前記第１の姿勢導出手段で導出された動作後のアーム部材の姿勢において、自由端よりも１つ基端側の関節部の位置を前記中間位置として、前記中間姿勢を導出する前記第２の姿勢導出手段、
を備えたことを特徴とする請求項１に記載のロボット。
を備えたことを特徴とする請求項１に記載のロボット。
【請求項3】
動作後のアーム部材の姿勢において自由端よりも１つ基端側の関節部の位置を前記中間位置として中間姿勢を導出した場合に、動作前の姿勢から前記中間姿勢に移動する途中で障害物に干渉する場合には、動作前のアーム部材の姿勢において、自由端よりも１つ基端側の関節部の位置を自由端の位置とする第２の中間姿勢から前記中間位置を目標とする逆運動学を解く前記第２の姿勢導出手段、
を備えたことを特徴とする請求項２に記載のロボット。
を備えたことを特徴とする請求項２に記載のロボット。
【請求項4】
複数のアーム部と、２つのアーム部どうしの連結部位に配置され且つ一方のアーム部に対して他方のアーム部を変位させることが可能な関節部と、を有するアーム部材を有するロボットを制御するロボット制御プログラムであって、
コンピュータを、
前記アーム部材の自由端を予め定められた目標に移動させる動作における前記関節部の位置について逆運動学を解いて前記アーム部材の動作後の姿勢を導出する第１の姿勢導出手段、
前記第１の姿勢導出手段で導出された各関節部の動作前の位置と動作後の位置に基づいて、動作の途中で障害物に干渉する場合には、動作時に自由端が通過する候補の位置である中間位置を目標の位置として逆運動学を解いて、前記アーム部材の中間姿勢を導出する第２の姿勢導出手段、
前記第１の姿勢導出手段で導出された動作の途中で前記アーム部材が障害物に干渉する場合には、前記第２の姿勢導出手段で導出された中間姿勢を経由して前記アーム部材の自由端を目標に移動させるように各関節部を制御するアーム制御手段、
として機能させることを特徴とするロボット制御プラグラム。
コンピュータを、
前記アーム部材の自由端を予め定められた目標に移動させる動作における前記関節部の位置について逆運動学を解いて前記アーム部材の動作後の姿勢を導出する第１の姿勢導出手段、
前記第１の姿勢導出手段で導出された各関節部の動作前の位置と動作後の位置に基づいて、動作の途中で障害物に干渉する場合には、動作時に自由端が通過する候補の位置である中間位置を目標の位置として逆運動学を解いて、前記アーム部材の中間姿勢を導出する第２の姿勢導出手段、
前記第１の姿勢導出手段で導出された動作の途中で前記アーム部材が障害物に干渉する場合には、前記第２の姿勢導出手段で導出された中間姿勢を経由して前記アーム部材の自由端を目標に移動させるように各関節部を制御するアーム制御手段、
として機能させることを特徴とするロボット制御プラグラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、アーム部と関節部とを有するロボットおよび当該ロボットを制御するロボット制御プログラムに関し、特に、ロボットの自由端を目標に移動させる制御を行う際に逆運動学を使用するロボットおよびロボット制御プログラムに関する。
【背景技術】
【0002】
ロボットの把持部のような特定の部位を、目標の位置に移動させるようにロボットの姿勢の軌跡を計算してロボットを制御することが行われている。このような技術として、以下の特許文献１～４に記載の技術が公知である。
【0003】
特許文献１（特開２０２０－９３３６４号公報）には、ロボット等の移動体（１）の把持部（２０）を移動させる軌道を導出する際に、まず、できるだけ多くの軌道候補（７１ａ～７１ｅ）を生成して、軌道候補（７１ａ～７１ｅ）の評価をして、評価の高い軌道（７１ｂ）を出力する構成が記載されている。
【0004】
特許文献２（特開２００７－３１３５９２号公報）には、ロボットが移動する際に、単位時間毎に、障害物とロボットの現在位置、速度、加速度、進行方向および形状やサイズに基づいて、障害物とロボットの相対距離、相対速度、相対加速度を算出し、算出された情報に基づいて、データベースに記憶された行動候補モデルから行動モデルを選択して動作経路を作成することが記載されている。特許文献２では、障害物との距離が近づくと大きくなる斥力ポテンシャルを計算して障害物との距離を評価し、障害物に近づきすぎない行動モデルを選択することが記載されている。
【0005】
特許文献３（特開２００７－２３７３３４号公報）には、多関節のロボットにおいて、開始位置から目標位置までの経路を探索する場合に、最初に、ランダム探索アルゴリズムで経路を探索して、探索された経路上に障害物が存在する場合には、障害物に最も近い終端の位置までの経路を導出した後、障害物が除去されたことを想定して障害物がない状態で経路を探索することが記載されている。
【0006】
特許文献４（特開２０００－２０１１７号公報）には、２自由度のロボットの動作経路計画を生成する技術において、ロボットと作業環境の幾何学的形状を幾何モデル（Ｃ空間）で表現して、幾何モデル上で、スタートからゴールまで経路を探索している。特許文献４では、経路の干渉の検査や、距離の評価を行って、距離和の最も短い経路を選択して出力する。
【先行技術文献】
【特許文献】
【0007】
【特許文献2】特開２００７－３１３５９２号公報（「００２３」－「００４２」）
【特許文献3】特開２００７－２３７３３４号公報（「００２３」－「００５０」）
【特許文献4】特開２０００－２０１１７号公報（「０００９」－「００１１」）
【発明の概要】
【発明が解決しようとする課題】
【0008】
（従来技術の問題点）
特許文献１，３に記載の技術では、軌道候補やランダム探索を行っているが、ロボットの関節の数が１～３程度の少ないものであれば、計算することは可能であるが、関節の数が多くなると計算量が膨大になって計算負荷が問題となる。計算負荷が大きくなると、リアルタイム制御が困難になる問題もある。特許文献２，４に記載の技術も、多くのパラメータを単位時間毎に計算したり、評価をその都度行っており、関節の数が多くなればなるほど計算負荷が膨大になる問題がある。
特許文献１，３に記載の技術では、軌道候補やランダム探索を行っているが、ロボットの関節の数が１～３程度の少ないものであれば、計算することは可能であるが、関節の数が多くなると計算量が膨大になって計算負荷が問題となる。計算負荷が大きくなると、リアルタイム制御が困難になる問題もある。特許文献２，４に記載の技術も、多くのパラメータを単位時間毎に計算したり、評価をその都度行っており、関節の数が多くなればなるほど計算負荷が膨大になる問題がある。
【0009】
現在、産業用ロボットアームのような多関節ロボットを制御する場合には、逆運動学を解いて多関節ロボットの制御を行うことが知られている。ロボットの自由端部（ロボットのハンド部）を目標に到達させる場合に、各関節の角度、向き（原因）から自由端部の位置（結果）を演算することが順運動学であり、自由端部の位置（結果）から各関節の角度、向き（原因）を演算（逆算）することが逆運動学である。
逆運動学において結果を得る方法として、方程式等を使用して解析的に解く解析的解法と、数値を当てはめていって近似的に解く数値的解法とが知られている。解析的解法は、多関節ロボットの構造が特別な条件を満たしている場合に限定される制約があり、数値解法はロボットの構造によらず使用できる利点がある。
逆運動学において結果を得る方法として、方程式等を使用して解析的に解く解析的解法と、数値を当てはめていって近似的に解く数値的解法とが知られている。解析的解法は、多関節ロボットの構造が特別な条件を満たしている場合に限定される制約があり、数値解法はロボットの構造によらず使用できる利点がある。
【0010】
図７は従来の逆運動学の解法の説明図であり、図７Ａはロボットのモデルの一例の説明図、図７Ｂは関節空間の説明図、図７Ｃは図７Ｂの要部拡大図である。
図７において、関節部０１が２つのアーム０２の場合、各関節部０１の変位量をｑ０，ｑ１とし、障害物や関節部０１の可動域等を考慮して、各変位量ｑ０，ｑ１がとり得る値を図７Ｂに示すような平面で表すことができる。図７Ｂにおいて、両方の変位量ｑ０，ｑ１がその値をとることができる場合は白丸で表され、一方または両方の変位量ｑ０，ｑ１がその値をとることができない場合は黒丸で表されている。したがって、関節空間を使用して初期位置０３から目的位置０４に移動する経路０６を導出する場合には、障害物等の距離を評価関数（障害物に近いほど評価が低く、距離が長いほど評価が低くなる関数）で評価して、図７Ｃの矢印で示す経路０６が選択される。
図７において、関節部０１が２つのアーム０２の場合、各関節部０１の変位量をｑ０，ｑ１とし、障害物や関節部０１の可動域等を考慮して、各変位量ｑ０，ｑ１がとり得る値を図７Ｂに示すような平面で表すことができる。図７Ｂにおいて、両方の変位量ｑ０，ｑ１がその値をとることができる場合は白丸で表され、一方または両方の変位量ｑ０，ｑ１がその値をとることができない場合は黒丸で表されている。したがって、関節空間を使用して初期位置０３から目的位置０４に移動する経路０６を導出する場合には、障害物等の距離を評価関数（障害物に近いほど評価が低く、距離が長いほど評価が低くなる関数）で評価して、図７Ｃの矢印で示す経路０６が選択される。
【0011】
図７に示す従来の関節空間や評価関数を使用する場合は、各変位量ｑ０，ｑ１が取りうる値の全範囲について計算する必要があり、計算量が多い問題がある。
そして、図７Ｂ、図７Ｃに示した一例では、関節部０１の数が２つであったため変位量が取りうる値を２次元平面で表現できたが、関節部０１の数がｎ個の場合は、ｎ次元の空間となる。このｎ次元空間は、「関節空間」や「配位空間」、「コンフィグレーション空間（Ｃ空間、特許文献４参照）」と呼ばれる。したがって、関節部０１の数が増えれば増えるほど、ｎ次元の次元が増え、関節空間の計算が指数関数的に膨大となっていき、経路０６の導出に時間がかかる問題がある。
特に、障害物が静止していれば、関節空間における計算は時間をかければまだ実用性があるかもしれないが、障害物が移動する場合は、障害物が移動するたびに関節空間を再計算する必要があり、計算負荷がさらに膨大になる問題もある。
そして、図７Ｂ、図７Ｃに示した一例では、関節部０１の数が２つであったため変位量が取りうる値を２次元平面で表現できたが、関節部０１の数がｎ個の場合は、ｎ次元の空間となる。このｎ次元空間は、「関節空間」や「配位空間」、「コンフィグレーション空間（Ｃ空間、特許文献４参照）」と呼ばれる。したがって、関節部０１の数が増えれば増えるほど、ｎ次元の次元が増え、関節空間の計算が指数関数的に膨大となっていき、経路０６の導出に時間がかかる問題がある。
特に、障害物が静止していれば、関節空間における計算は時間をかければまだ実用性があるかもしれないが、障害物が移動する場合は、障害物が移動するたびに関節空間を再計算する必要があり、計算負荷がさらに膨大になる問題もある。
【0012】
本発明は、関節空間を使用する場合に比べて、多関節のロボットの動作の経路の計算負荷を軽減することを技術的課題とする。
【課題を解決するための手段】
【0013】
前記技術的課題を解決するために、請求項１に記載の発明のロボットは、
複数のアーム部と、２つのアーム部どうしの連結部位に配置され且つ一方のアーム部に対して他方のアーム部を変位させることが可能な関節部と、を有するアーム部材と、
前記アーム部材の自由端を予め定められた目標に移動させる動作における前記関節部の位置について逆運動学を解いて前記アーム部材の動作後の姿勢を導出する第１の姿勢導出手段と、
前記第１の姿勢導出手段で導出された各関節部の動作前の位置と動作後の位置に基づいて、動作の途中で障害物に干渉する場合には、動作時に自由端が通過する候補の位置である中間位置を目標の位置として逆運動学を解いて、前記アーム部材の中間姿勢を導出する第２の姿勢導出手段と、
前記第１の姿勢導出手段で導出された動作の途中で前記アーム部材が障害物に干渉する場合には、前記第２の姿勢導出手段で導出された中間姿勢を経由して前記アーム部材の自由端を目標に移動させるように各関節部を制御するアーム制御手段と、
を備えたことを特徴とする。
複数のアーム部と、２つのアーム部どうしの連結部位に配置され且つ一方のアーム部に対して他方のアーム部を変位させることが可能な関節部と、を有するアーム部材と、
前記アーム部材の自由端を予め定められた目標に移動させる動作における前記関節部の位置について逆運動学を解いて前記アーム部材の動作後の姿勢を導出する第１の姿勢導出手段と、
前記第１の姿勢導出手段で導出された各関節部の動作前の位置と動作後の位置に基づいて、動作の途中で障害物に干渉する場合には、動作時に自由端が通過する候補の位置である中間位置を目標の位置として逆運動学を解いて、前記アーム部材の中間姿勢を導出する第２の姿勢導出手段と、
前記第１の姿勢導出手段で導出された動作の途中で前記アーム部材が障害物に干渉する場合には、前記第２の姿勢導出手段で導出された中間姿勢を経由して前記アーム部材の自由端を目標に移動させるように各関節部を制御するアーム制御手段と、
を備えたことを特徴とする。
【0014】
請求項２に記載の発明は、請求項１に記載のロボットにおいて、
前記第１の姿勢導出手段で導出された動作後のアーム部材の姿勢において、自由端よりも１つ基端側の関節部の位置を前記中間位置として、前記中間姿勢を導出する前記第２の姿勢導出手段、
を備えたことを特徴とする。
前記第１の姿勢導出手段で導出された動作後のアーム部材の姿勢において、自由端よりも１つ基端側の関節部の位置を前記中間位置として、前記中間姿勢を導出する前記第２の姿勢導出手段、
を備えたことを特徴とする。
【0015】
請求項３に記載の発明は、請求項２に記載のロボットにおいて、
動作後のアーム部材の姿勢において自由端よりも１つ基端側の関節部の位置を前記中間位置として中間姿勢を導出した場合に、動作前の姿勢から前記中間姿勢に移動する途中で障害物に干渉する場合には、動作前のアーム部材の姿勢において、自由端よりも１つ基端側の関節部の位置を自由端の位置とする第２の中間姿勢から前記中間位置を目標とする逆運動学を解く前記第２の姿勢導出手段、
を備えたことを特徴とする。
動作後のアーム部材の姿勢において自由端よりも１つ基端側の関節部の位置を前記中間位置として中間姿勢を導出した場合に、動作前の姿勢から前記中間姿勢に移動する途中で障害物に干渉する場合には、動作前のアーム部材の姿勢において、自由端よりも１つ基端側の関節部の位置を自由端の位置とする第２の中間姿勢から前記中間位置を目標とする逆運動学を解く前記第２の姿勢導出手段、
を備えたことを特徴とする。
【0016】
前記技術的課題を解決するために、請求項４に記載の発明のロボット制御プラグラムは、
複数のアーム部と、２つのアーム部どうしの連結部位に配置され且つ一方のアーム部に対して他方のアーム部を変位させることが可能な関節部と、を有するアーム部材を有するロボットを制御するロボット制御プログラムであって、
コンピュータを、
前記アーム部材の自由端を予め定められた目標に移動させる動作における前記関節部の位置について逆運動学を解いて前記アーム部材の動作後の姿勢を導出する第１の姿勢導出手段、
前記第１の姿勢導出手段で導出された各関節部の動作前の位置と動作後の位置に基づいて、動作の途中で障害物に干渉する場合には、動作時に自由端が通過する候補の位置である中間位置を目標の位置として逆運動学を解いて、前記アーム部材の中間姿勢を導出する第２の姿勢導出手段、
前記第１の姿勢導出手段で導出された動作の途中で前記アーム部材が障害物に干渉する場合には、前記第２の姿勢導出手段で導出された中間姿勢を経由して前記アーム部材の自由端を目標に移動させるように各関節部を制御するアーム制御手段、
として機能させることを特徴とする。
複数のアーム部と、２つのアーム部どうしの連結部位に配置され且つ一方のアーム部に対して他方のアーム部を変位させることが可能な関節部と、を有するアーム部材を有するロボットを制御するロボット制御プログラムであって、
コンピュータを、
前記アーム部材の自由端を予め定められた目標に移動させる動作における前記関節部の位置について逆運動学を解いて前記アーム部材の動作後の姿勢を導出する第１の姿勢導出手段、
前記第１の姿勢導出手段で導出された各関節部の動作前の位置と動作後の位置に基づいて、動作の途中で障害物に干渉する場合には、動作時に自由端が通過する候補の位置である中間位置を目標の位置として逆運動学を解いて、前記アーム部材の中間姿勢を導出する第２の姿勢導出手段、
前記第１の姿勢導出手段で導出された動作の途中で前記アーム部材が障害物に干渉する場合には、前記第２の姿勢導出手段で導出された中間姿勢を経由して前記アーム部材の自由端を目標に移動させるように各関節部を制御するアーム制御手段、
として機能させることを特徴とする。
【発明の効果】
【0017】
請求項１，４に記載の発明によれば、関節空間を使用する場合に比べて、多関節のロボットの動作の経路の計算負荷を軽減することができる。
請求項２に記載の発明によれば、自由端よりも１つ基端側の関節部の位置を中間位置とする経路を導出することで、最終的な姿勢を基準に経路を導出することができる。
請求項３に記載の発明によれば、中間姿勢だけでは干渉が解消されない場合に、第２の中間姿勢を設定して経路を導出しており、第２の中間姿勢が設定されない場合に比べて、干渉が解消されやすくなっている。
請求項２に記載の発明によれば、自由端よりも１つ基端側の関節部の位置を中間位置とする経路を導出することで、最終的な姿勢を基準に経路を導出することができる。
請求項３に記載の発明によれば、中間姿勢だけでは干渉が解消されない場合に、第２の中間姿勢を設定して経路を導出しており、第２の中間姿勢が設定されない場合に比べて、干渉が解消されやすくなっている。
【図面の簡単な説明】
【0018】
【発明を実施するための形態】
【0019】
次に図面を参照しながら、本発明の実施の形態の具体例（以下、実施例と記載する）を説明するが、本発明は以下の実施例に限定されるものではない。
なお、以下の図面を使用した説明において、理解の容易のために説明に必要な部材以外の図示は適宜省略されている。
なお、以下の図面を使用した説明において、理解の容易のために説明に必要な部材以外の図示は適宜省略されている。
【実施例0020】
図１は本発明の実施例１のロボットの概略説明図である。
図１において、本発明のロボットの一例としての実施例１のロボットアームシステムＳは、アーム部材１を有する。アーム部材１は、複数のアーム部２と、アーム部２どうしを連結する関節部３とを有する。実施例１のアーム部材１は、台座部４に一端が支持された第１のアーム部２ａを有する。第１のアーム部２ａの他端には、第１の関節部３ａが支持されている。以下同様に、第２のアーム部２ｂ、第２の関節部３ｂ、…、第７の関節部３ｇ、第８のアーム部２ｈが連結されている。第８のアーム部２ｈの他端（先端）には対象物を把持、解放可能なロボットハンド部２ｋが形成されている。
したがって、実施例１のアーム部材１では、全体としては、第１のアーム部２ａの一端部が基端部となり、ロボットハンド部２ｋが自由端部となる。
図１において、本発明のロボットの一例としての実施例１のロボットアームシステムＳは、アーム部材１を有する。アーム部材１は、複数のアーム部２と、アーム部２どうしを連結する関節部３とを有する。実施例１のアーム部材１は、台座部４に一端が支持された第１のアーム部２ａを有する。第１のアーム部２ａの他端には、第１の関節部３ａが支持されている。以下同様に、第２のアーム部２ｂ、第２の関節部３ｂ、…、第７の関節部３ｇ、第８のアーム部２ｈが連結されている。第８のアーム部２ｈの他端（先端）には対象物を把持、解放可能なロボットハンド部２ｋが形成されている。
したがって、実施例１のアーム部材１では、全体としては、第１のアーム部２ａの一端部が基端部となり、ロボットハンド部２ｋが自由端部となる。
【0021】
なお、実施例１では、７個の関節部３ａ～３ｇと、８個のアーム部２ａ～２ｈを有する構成を例示したがこれに限定されない。また、実施例１では、関節タイプを回転関節のみの構成例であるが、直動関節を含んだロボットアームでもよい。以降では、「変位量」の一例として、単に回転関節の「回転量」と表記する場合もあるが、直動関節の並進移動の「移動量」を意味する場合もあり、適宜読み替え可能である。
アーム部２や関節部３の数は、アーム部材１の用途や設計、仕様等に応じて任意の数に増減可能である。特に、逆運動学においては、Ｘ軸、Ｙ軸、Ｚ軸の直交座標系において、関節部３の位置ｘ，ｙ，ｚと、Ｘ軸、Ｙ軸、Ｚ軸周りの回転角θｘ，θｙ，θｚの６つの変数に対して、関節部３の数が６つ以下であれば、関節部毎に成立する方程式の数の方が変数より少なく、特定の条件を満たせば解析的に解くことが可能な場合もある。一方で、関節部３の数が７以上になると解析的に解くことは基本的には困難であり、数値的な解法が原則的に使用されることとなるため、関節部３の数が７以上である場合に、本願発明は好適に利用可能である。
アーム部２や関節部３の数は、アーム部材１の用途や設計、仕様等に応じて任意の数に増減可能である。特に、逆運動学においては、Ｘ軸、Ｙ軸、Ｚ軸の直交座標系において、関節部３の位置ｘ，ｙ，ｚと、Ｘ軸、Ｙ軸、Ｚ軸周りの回転角θｘ，θｙ，θｚの６つの変数に対して、関節部３の数が６つ以下であれば、関節部毎に成立する方程式の数の方が変数より少なく、特定の条件を満たせば解析的に解くことが可能な場合もある。一方で、関節部３の数が７以上になると解析的に解くことは基本的には困難であり、数値的な解法が原則的に使用されることとなるため、関節部３の数が７以上である場合に、本願発明は好適に利用可能である。
【0022】
前記関節部３（３ａ～３ｇ）は、回転軸を中心として一方のアーム部２ａ～２ｇに対して他方のアーム部２ｂ～２ｈを回転（変位）可能に構成されている。各関節部３には、図示しないモータが内蔵されており、モータの回転駆動、回転停止を制御することで回転量を制御可能に構成されている。各関節部３のモータは、実施例１では、図示しない無線通信チップで制御信号を受信して駆動する構成としているが、これに限定されない。通信ケーブル等の有線で制御信号を送受信可能な構成としてモータを制御することも可能である。なお、無線通信や有線通信の方式は、従来公知の任意の通信方式とすることが可能であり、携帯電話回線やBluetooth（登録商標）、無線ＬＡＮ等、任意の無線通信方式を採用したり、ＵＳＢ（Universal Serial Bus）等のシリアル方式やパラレル方式等任意の有線通信方式を採用することが可能である。
【0023】
前記アーム部材１は、情報処理装置の一例としてのコンピュータ装置１１で制御される。実施例１では、アーム部材１はコンピュータ装置１１との間で無線通信が可能に構成されている。コンピュータ装置１１は、コンピュータ本体１２と、表示部の一例としてのディスプレイ１３と、入力部の一例としてのキーボード１４およびマウス１５と、を有する。なお、実施例１では、コンピュータ装置１１としてデスクトップ型のコンピュータ装置を例示したがこれに限定されず、ラップトップ型（ノート型）のコンピュータ装置を使用することも可能である。また、コンピュータ装置１１として、パーソナルコンピュータ装置に限定されず、サーバー型やマイクロコンピュータ型、チップ型等の任意の形態とすることも可能である。
【0024】
（制御部の説明）
図２は実施例１の制御部を機能ブロック図で示した説明図である。
実施例１のコンピュータ本体１２の制御部（コントローラ）Ｃは、外部との信号の入出力および入出力信号レベルの調節等を行うＩ／Ｏ（入出力インターフェース）、必要な起動処理を行うためのプログラムおよびデータ等が記憶されたＲＯＭ（リードオンリーメモリ）、必要なデータ及びプログラムを一時的に記憶するためのＲＡＭ（ランダムアクセスメモリ）、ＲＯＭ等に記憶された起動プログラムに応じた処理を行うＣＰＵ（中央演算処理装置）ならびにクロック発振器等を有するコンピュータ装置により構成されており、前記ＲＯＭ及びＲＡＭ等に記憶されたプログラムを実行することにより種々の機能を実現することができる。
コンピュータ本体１２には、基本動作を制御する基本ソフト、いわゆる、オペレーティングシステムや、アプリケーションプログラムの一例としてのロボット制御プログラム、その他の図示しないソフトウェアが記憶されている。
実施例１のロボット制御プログラムは、以下の機能（手段、機能モジュール）Ｃ１～Ｃ７を有する。
図２は実施例１の制御部を機能ブロック図で示した説明図である。
実施例１のコンピュータ本体１２の制御部（コントローラ）Ｃは、外部との信号の入出力および入出力信号レベルの調節等を行うＩ／Ｏ（入出力インターフェース）、必要な起動処理を行うためのプログラムおよびデータ等が記憶されたＲＯＭ（リードオンリーメモリ）、必要なデータ及びプログラムを一時的に記憶するためのＲＡＭ（ランダムアクセスメモリ）、ＲＯＭ等に記憶された起動プログラムに応じた処理を行うＣＰＵ（中央演算処理装置）ならびにクロック発振器等を有するコンピュータ装置により構成されており、前記ＲＯＭ及びＲＡＭ等に記憶されたプログラムを実行することにより種々の機能を実現することができる。
コンピュータ本体１２には、基本動作を制御する基本ソフト、いわゆる、オペレーティングシステムや、アプリケーションプログラムの一例としてのロボット制御プログラム、その他の図示しないソフトウェアが記憶されている。
実施例１のロボット制御プログラムは、以下の機能（手段、機能モジュール）Ｃ１～Ｃ７を有する。
【0025】
図３は実施例１のアーム部材の動作前の状態の説明図である。
目標位置記憶手段Ｃ１は、アーム部材１のロボットハンド部２ｋの目標位置を記憶する。実施例１では、目標位置２１は、利用者によりコンピュータ装置１１のキーボード１４やマウス１５を通じて入力される。
初期状態記憶手段Ｃ２は、アーム部材１の初期状態、すなわち、動作前の状態を記憶する。図３において、実施例１の初期状態記憶手段Ｃ２は、一例として、アーム部材１は、基端から自由端であるロボットハンド部２ｋまで鉛直上方に向けて起立した状態を初期状態として記憶している。なお、図３では、アーム部材１に対して、障害物２０が設定されている。
目標位置記憶手段Ｃ１は、アーム部材１のロボットハンド部２ｋの目標位置を記憶する。実施例１では、目標位置２１は、利用者によりコンピュータ装置１１のキーボード１４やマウス１５を通じて入力される。
初期状態記憶手段Ｃ２は、アーム部材１の初期状態、すなわち、動作前の状態を記憶する。図３において、実施例１の初期状態記憶手段Ｃ２は、一例として、アーム部材１は、基端から自由端であるロボットハンド部２ｋまで鉛直上方に向けて起立した状態を初期状態として記憶している。なお、図３では、アーム部材１に対して、障害物２０が設定されている。
【0026】
第１の姿勢導出手段Ｃ３は、逆運動学解析手段Ｃ３ａと、優先度付き逆運動学解析手段Ｃ３ｂとを有し、アーム部材１の自由端（ロボットハンド部２ｋ）を目標位置２１に移動させる動作における関節部３ａ～３ｇの位置や変位量について逆運動学を解いてアーム部材１の動作後の姿勢を導出する。
逆運動学解析手段Ｃ３ａは、アーム部材１のロボットハンド部２ｋの目標位置２１から、各関節部３ａ～３ｇの角度（変位量）、向きを演算する逆運動学を解いて、アーム部材１を初期状態からロボットハンド部２ｋが目標位置２１に到達するまでの変位量を導出する。なお、実施例１では逆運動学の数値解法で解いているが、逆運動学の数値解法は従来公知（例えば、特許第５５８６０１５号公報等参照）であるため、詳細な説明は省略する。
逆運動学解析手段Ｃ３ａは、アーム部材１のロボットハンド部２ｋの目標位置２１から、各関節部３ａ～３ｇの角度（変位量）、向きを演算する逆運動学を解いて、アーム部材１を初期状態からロボットハンド部２ｋが目標位置２１に到達するまでの変位量を導出する。なお、実施例１では逆運動学の数値解法で解いているが、逆運動学の数値解法は従来公知（例えば、特許第５５８６０１５号公報等参照）であるため、詳細な説明は省略する。
【0027】
優先度付き逆運動学解析手段Ｃ３ｂは、逆運動学を解く際に、ロボットハンド部２ｋを目標位置２１に近づけるだけでなく、アーム部材１の途中の特定の位置を、障害物２０を避けるように目標位置２１とは別の第２の目標位置に近づけると共に、途中の特定の位置を第２の目標位置に近づけることよりもロボットハンド部２ｋを目標位置２１に近づけることを優先する「優先度付き逆運動学」を解いて、アーム部材１を初期状態からロボットハンド部２ｋが目標位置２１に到達するまでの変位量を導出する。
なお、優先度付き逆運動学は、例えば、非特許文献１「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.」や、非特許文献２「関口叡範, 武居直行: “切断法による優先度付き逆運動学の数値解法,” 日本ロボット学会誌, vol.37, no.8, pp.711-717, 2019.」、非特許文献３「M. Sekiguchi and N. Takesue: “Fast and Robust Numerical Method for Inverse Kinematics with Prioritized Multiple Targets for Redundant Robots,” Advanced Robotics, vol.34, no.16, pp.1068-1078, 2020.」等に記載されており、公知であるため、詳細な説明は省略する。
なお、優先度付き逆運動学は、例えば、非特許文献１「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.」や、非特許文献２「関口叡範, 武居直行: “切断法による優先度付き逆運動学の数値解法,” 日本ロボット学会誌, vol.37, no.8, pp.711-717, 2019.」、非特許文献３「M. Sekiguchi and N. Takesue: “Fast and Robust Numerical Method for Inverse Kinematics with Prioritized Multiple Targets for Redundant Robots,” Advanced Robotics, vol.34, no.16, pp.1068-1078, 2020.」等に記載されており、公知であるため、詳細な説明は省略する。
【0028】
干渉判別手段Ｃ４は、アーム部材１が障害物２０に干渉しているか否かを判別する。干渉判別手段Ｃ４は、アーム部材１が配置された空間において、障害物２０の領域内に、アーム部材１の一部が存在しているか（進入しているか）否かを判別することで、干渉しているか否かの判別を行う。
【0029】
図４は実施例１のアーム部材の逆運動学を解いた動作後の姿勢の一例の説明図であり、図４Ａは逆運動学の数値解法で解いた場合の一例の説明図、図４Ｂは優先度付き逆運動学で解いた場合の一例の説明図である。
実施例１の第１の姿勢導出手段Ｃ３は、アーム部材１を目標位置２１に移動させる動作計画を導出する場合には、まず、逆運動学解析手段Ｃ３ａで導出を行い、干渉判別手段Ｃ４で干渉の判別を行う。動作後の状態でアーム部材１が障害物２０に干渉しなかった場合には、逆運動学解析手段Ｃ３ａでの導出結果を動作計画の動作後の姿勢として採用する。逆運動学解析手段Ｃ３ａでの導出では、図４Ａに示すように、アーム部材１が障害物２０に干渉する場合には、優先度付き逆運動学解析手段Ｃ３ｂで導出を行う。そして、優先度付き逆運動学で導出された動作後の姿勢に対して、干渉判別手段Ｃ４で干渉の判別を行う。図４Ｂに示すように、動作後の状態でアーム部材１が障害物に干渉しなかった場合には、優先度付き逆運動学解析手段Ｃ３ｂでの導出結果を動作計画の動作後の姿勢として採用する。優先度付き逆運動学でも干渉しない結果が得られない場合には、動作計画が生成できないとしてエラー出力する。
実施例１の第１の姿勢導出手段Ｃ３は、アーム部材１を目標位置２１に移動させる動作計画を導出する場合には、まず、逆運動学解析手段Ｃ３ａで導出を行い、干渉判別手段Ｃ４で干渉の判別を行う。動作後の状態でアーム部材１が障害物２０に干渉しなかった場合には、逆運動学解析手段Ｃ３ａでの導出結果を動作計画の動作後の姿勢として採用する。逆運動学解析手段Ｃ３ａでの導出では、図４Ａに示すように、アーム部材１が障害物２０に干渉する場合には、優先度付き逆運動学解析手段Ｃ３ｂで導出を行う。そして、優先度付き逆運動学で導出された動作後の姿勢に対して、干渉判別手段Ｃ４で干渉の判別を行う。図４Ｂに示すように、動作後の状態でアーム部材１が障害物に干渉しなかった場合には、優先度付き逆運動学解析手段Ｃ３ｂでの導出結果を動作計画の動作後の姿勢として採用する。優先度付き逆運動学でも干渉しない結果が得られない場合には、動作計画が生成できないとしてエラー出力する。
【0030】
経路生成手段の一例としての軌道生成手段Ｃ５は、初期状態（始点）から、採用された動作後の姿勢（終点）までのアーム部材１の軌道（経路）を生成する。軌道生成手段Ｃ５は、始点と終点を直線補間して関節軌道を生成する。実施例１の直線補間は、具体的には、以下のように計算する。まず、始点となる関節部３（３ａ～３ｇ）のそれぞれについて変位ベクトルをqs、終点となる関節変位ベクトルをqgとする。また、始点から終点に遷移するまでに要する時間をτとし、動作中の時刻ｔ（０≦ｔ≦τ）とする。始点と終点を直線補間することによって得られる関節軌道q(t)は、以下の式（１）のように表される。
q(t)＝qs＋（ｔ／τ）・（qg－qs） …式（１）
なお、軌道生成手段Ｃ５で生成された軌道において、干渉判別手段Ｃ４で動作の途中で障害物２０に干渉しないかどうかを判別する。すなわち、式（１）で表される直線状の軌道が障害物２０の領域を通過するか否かを判別する。軌道が障害物２０に干渉しない場合は、導出された軌道を動作計画の解として採用する。
q(t)＝qs＋（ｔ／τ）・（qg－qs） …式（１）
なお、軌道生成手段Ｃ５で生成された軌道において、干渉判別手段Ｃ４で動作の途中で障害物２０に干渉しないかどうかを判別する。すなわち、式（１）で表される直線状の軌道が障害物２０の領域を通過するか否かを判別する。軌道が障害物２０に干渉しない場合は、導出された軌道を動作計画の解として採用する。
【0031】
第２の姿勢導出手段Ｃ６は、中間位置設定手段Ｃ６ａを有し、第１の姿勢導出手段Ｃ３で導出された各関節部３ａ～３ｇの動作前の位置と動作後の位置に基づいて、動作の途中で障害物２０に干渉する場合には、動作時に自由端が通過する候補の位置である中間位置２２を目標の位置として逆運動学を解いて、アーム部材１の中間姿勢を導出する。実施例１の第２の姿勢導出手段Ｃ６は、軌道生成手段Ｃ５で生成された軌道が障害物２０に干渉すると判別された場合に、中間位置２２を新たな目標位置として、第１の姿勢導出手段Ｃ３と同様に逆運動学の数値解法で解く。
【0032】
図５は中間位置を目標位置にした場合の説明図であり、図５Ａは第１の中間位置を目標位置として逆運動学を解いた場合の説明図、図５Ｂは第２の中間位置が設定された状態の説明図である。
中間位置設定手段Ｃ６ａは、中間位置２２を設定する。実施例１では、中間位置２２の一例として、動作後のアーム部材１の姿勢（図４Ｂの状態）において、自由端（ロボットハンド部２ｋ）よりも１つ基端側の関節部３ｇの位置を、中間位置（第１の中間位置）２２に設定する。いわば、動作後の姿勢において手先（ロボットハンド部２ｋ）に対する手首（関節部３ｇ）の位置を、中間位置２２とし、中間位置２２を新たな目標とする。したがって、第２の姿勢導出手段Ｃ６は、最終的な目標位置２１ではなく、その手前の状態でとして、図５Ａに示すように、中間位置２２を目標とする第１の中間姿勢が導出され、動作計画（第１の中間姿勢および軌道）を導出する。
動作後の関節部３ｇの位置を中間位置（第１の中間位置）２２として導出された軌道、すなわち、初期状態（図３）から第１の中間姿勢（図５Ａ）までの軌道で障害物２０に干渉しなくなった場合には、初期状態（始点）から第１の中間位置までの軌道と第１の中間位置から動作後の状態（終点）までの軌道を順に通過する軌道を、最終的な動作計画として出力する。
中間位置設定手段Ｃ６ａは、中間位置２２を設定する。実施例１では、中間位置２２の一例として、動作後のアーム部材１の姿勢（図４Ｂの状態）において、自由端（ロボットハンド部２ｋ）よりも１つ基端側の関節部３ｇの位置を、中間位置（第１の中間位置）２２に設定する。いわば、動作後の姿勢において手先（ロボットハンド部２ｋ）に対する手首（関節部３ｇ）の位置を、中間位置２２とし、中間位置２２を新たな目標とする。したがって、第２の姿勢導出手段Ｃ６は、最終的な目標位置２１ではなく、その手前の状態でとして、図５Ａに示すように、中間位置２２を目標とする第１の中間姿勢が導出され、動作計画（第１の中間姿勢および軌道）を導出する。
動作後の関節部３ｇの位置を中間位置（第１の中間位置）２２として導出された軌道、すなわち、初期状態（図３）から第１の中間姿勢（図５Ａ）までの軌道で障害物２０に干渉しなくなった場合には、初期状態（始点）から第１の中間位置までの軌道と第１の中間位置から動作後の状態（終点）までの軌道を順に通過する軌道を、最終的な動作計画として出力する。
【0033】
また、第２の姿勢導出手段Ｃ６は、関節部３ｇの位置を中間位置２２として設定して導出された軌道でも干渉する場合には、動作前（初期状態）の姿勢（図３参照）において、自由端（ロボットハンド部２ｋ）よりも１つ基端側の関節部３ｇの位置（第２の中間位置２３）を自由端（ロボットハンド部２ｋ）の目標位置として、第２の中間姿勢（図５Ｂ参照）を導出し、ロボットハンド部２ｋが第２の中間位置２３から第１の中間位置２２まで移動する動作計画（第２の中間位置から第１の中間位置までの軌道）を導出する。すなわち、中間位置を増やす。第２の中間姿勢（図５Ｂ）から第１の中間姿勢（図５Ａ）に移動する軌道で障害物２０に干渉しない場合には、初期状態（始点、図３）から第２の中間位置（図５Ｂ）までの軌道、第２の中間位置（図５Ｂ）から第１の中間位置（図５Ａ）までの軌道、第１の中間位置（図５Ａ）から動作後の位置（図４Ｂ）までの軌道を順に通過する軌道を、最終的な動作計画として出力する。
実施例１では、第２の中間姿勢を設定しても障害物２０に干渉する場合には、動作後の姿勢における先端から２つ基端側の関節部３ｆの位置を第３の中間位置、動作前の姿勢における先端から２つ基端側の関節部３ｆの位置を第４の中間位置、動作後の姿勢における先端から３つ基端側の関節部３ｅの位置を第５の中間位置、動作前の姿勢における先端から３つ基端側の関節部３ｅの位置を第６の中間位置、…、と順に設定していき（中間位置を増やしていき）、干渉がなくなるまで各中間位置どうしの間の軌道を導出していく。
実施例１では、第２の中間姿勢を設定しても障害物２０に干渉する場合には、動作後の姿勢における先端から２つ基端側の関節部３ｆの位置を第３の中間位置、動作前の姿勢における先端から２つ基端側の関節部３ｆの位置を第４の中間位置、動作後の姿勢における先端から３つ基端側の関節部３ｅの位置を第５の中間位置、動作前の姿勢における先端から３つ基端側の関節部３ｅの位置を第６の中間位置、…、と順に設定していき（中間位置を増やしていき）、干渉がなくなるまで各中間位置どうしの間の軌道を導出していく。
【0034】
アーム制御手段Ｃ７は、アーム部材１を制御して、自由端（ロボットハンド部２ｋ）を目標に移動させる。実施例１のアーム制御手段Ｃ７は、姿勢導出手段Ｃ３、Ｃ６のいずれかから出力された動作計画に沿ってアーム部材１の関節部３ａ～３ｇの変位量を制御して、ロボットハンド部２ｋを目標に移動させる。したがって、実施例１のアーム制御手段Ｃ７は、第１の姿勢導出手段Ｃ３で動作計画が出力されていれば、第１の姿勢導出手段Ｃ３で導出された動作計画に沿って移動させる。また、実施例１のアーム制御手段Ｃ７は、第２の姿勢導出手段Ｃ６で動作計画が出力されていれば、第２の姿勢導出手段Ｃ６で導出された中間姿勢を経由する動作計画に沿ってアーム部材１のロボットハンド部２ｋを目標に移動させる。
【0035】
（フローチャートの説明）
次に、実施例１のロボットアームシステムＳにおける制御の流れを流れ図、いわゆるフローチャートを使用して説明する。
図６は実施例１の動作計画出力処理のフローチャートの説明図である。
図６のフローチャートの各ステップＳＴの処理は、前記制御部Ｃに記憶されたプログラムに従って行われる。また、この処理はコンピュータ本体１２の他の各種処理と並行して実行される。なお、ユーザーの制御開始の入力がされた場合に、図６の動作計画出力処理で出力された各関節部３の回転量に基づいて各関節部３を制御して、ロボットハンド部２ｋを目標に移動させる制御処理のフローチャートについては図示は省略する。
図６に示すフローチャートはロボット制御プログラムの起動により開始される。
次に、実施例１のロボットアームシステムＳにおける制御の流れを流れ図、いわゆるフローチャートを使用して説明する。
図６は実施例１の動作計画出力処理のフローチャートの説明図である。
図６のフローチャートの各ステップＳＴの処理は、前記制御部Ｃに記憶されたプログラムに従って行われる。また、この処理はコンピュータ本体１２の他の各種処理と並行して実行される。なお、ユーザーの制御開始の入力がされた場合に、図６の動作計画出力処理で出力された各関節部３の回転量に基づいて各関節部３を制御して、ロボットハンド部２ｋを目標に移動させる制御処理のフローチャートについては図示は省略する。
図６に示すフローチャートはロボット制御プログラムの起動により開始される。
【0036】
図６のＳＴ１において、初期の位置・姿勢や目標の位置・姿勢を入力するための画像をディスプレイ１３に表示する。そして、ＳＴ２に進む。
ＳＴ２において、キーボード１４やマウス１５で初期位置等の入力がされて動作計画の導出開始の入力がされたか否かを判別する。イエス（Ｙ）の場合はＳＴ３に進み、ノー（Ｎ）の場合はＳＴ２を繰り返す。
ＳＴ３において、初期状態と目標位置２１から逆運動学を解いて動作後の姿勢を導出する。そして、ＳＴ４に進む。
ＳＴ２において、キーボード１４やマウス１５で初期位置等の入力がされて動作計画の導出開始の入力がされたか否かを判別する。イエス（Ｙ）の場合はＳＴ３に進み、ノー（Ｎ）の場合はＳＴ２を繰り返す。
ＳＴ３において、初期状態と目標位置２１から逆運動学を解いて動作後の姿勢を導出する。そして、ＳＴ４に進む。
【0037】
ＳＴ４において、動作後の姿勢で障害物２０に干渉するか否かを判別する。ノー（Ｎ）の場合はＳＴ５に進み、イエス（Ｙ）の場合はＳＴ７に進む。
ＳＴ５において、初期状態と導出された動作後の姿勢との間の軌道を直線補間で導出する。そして、ＳＴ６に進む。
ＳＴ６において、導出された軌道が障害物２０に干渉するか否かを判別する。イエス（Ｙ）の場合はＳＴ１１に進み、ノー（Ｎ）の場合はＳＴ２２に進む。
ＳＴ５において、初期状態と導出された動作後の姿勢との間の軌道を直線補間で導出する。そして、ＳＴ６に進む。
ＳＴ６において、導出された軌道が障害物２０に干渉するか否かを判別する。イエス（Ｙ）の場合はＳＴ１１に進み、ノー（Ｎ）の場合はＳＴ２２に進む。
【0038】
ＳＴ７において、初期状態と目標位置２１から優先度付き逆運動学を解いて、動作後の姿勢を導出する。そして、ＳＴ８に進む。
ＳＴ８において、導出された動作後の姿勢で障害物２０に干渉するか否かを判別する。ノー（Ｎ）の場合はＳＴ９に進み、イエス（Ｙ）の場合はエラー終了する。
ＳＴ９において、初期状態と導出された動作後の姿勢との間の軌道を直線補間で導出する。そして、ＳＴ１０に進む。
ＳＴ１０において、導出された軌道が障害物２０に干渉するか否かを判別する。イエス（Ｙ）の場合はＳＴ１１に進み、ノー（Ｎ）の場合はＳＴ２２に進む。
ＳＴ８において、導出された動作後の姿勢で障害物２０に干渉するか否かを判別する。ノー（Ｎ）の場合はＳＴ９に進み、イエス（Ｙ）の場合はエラー終了する。
ＳＴ９において、初期状態と導出された動作後の姿勢との間の軌道を直線補間で導出する。そして、ＳＴ１０に進む。
ＳＴ１０において、導出された軌道が障害物２０に干渉するか否かを判別する。イエス（Ｙ）の場合はＳＴ１１に進み、ノー（Ｎ）の場合はＳＴ２２に進む。
【0039】
ＳＴ１１において、第１の中間位置２２を設定する。そして、ＳＴ１２に進む。
ＳＴ１２において、初期状態と中間位置（中間位置が増えた場合は、中間位置どうし）から逆運動学を解いて動作後の姿勢を導出する。そして、ＳＴ１３に進む。
ＳＴ１３において、動作後の姿勢で障害物２０に干渉するか否かを判別する。ノー（Ｎ）の場合はＳＴ１４に進み、イエス（Ｙ）の場合はＳＴ１７に進む。
ＳＴ１４において、初期状態と導出された動作後の姿勢との間の軌道（中間位置が増えた場合は、中間位置どうしの間の軌道）を直線補間で導出する。そして、ＳＴ１５に進む。
ＳＴ１５において、導出された軌道が障害物２０に干渉するか否かを判別する。イエス（Ｙ）の場合はＳＴ１６に進み、ノー（Ｎ）の場合はＳＴ２２に進む。
ＳＴ１６において、次の中間位置を設定する（中間位置を増やす）。そして、ＳＴ１２に戻る。
ＳＴ１２において、初期状態と中間位置（中間位置が増えた場合は、中間位置どうし）から逆運動学を解いて動作後の姿勢を導出する。そして、ＳＴ１３に進む。
ＳＴ１３において、動作後の姿勢で障害物２０に干渉するか否かを判別する。ノー（Ｎ）の場合はＳＴ１４に進み、イエス（Ｙ）の場合はＳＴ１７に進む。
ＳＴ１４において、初期状態と導出された動作後の姿勢との間の軌道（中間位置が増えた場合は、中間位置どうしの間の軌道）を直線補間で導出する。そして、ＳＴ１５に進む。
ＳＴ１５において、導出された軌道が障害物２０に干渉するか否かを判別する。イエス（Ｙ）の場合はＳＴ１６に進み、ノー（Ｎ）の場合はＳＴ２２に進む。
ＳＴ１６において、次の中間位置を設定する（中間位置を増やす）。そして、ＳＴ１２に戻る。
【0040】
ＳＴ１７において、初期状態と中間位置２２（中間位置が増えた場合は、中間位置どうし）から優先度付き逆運動学を解いて、動作後の姿勢を導出する。そして、ＳＴ１８に進む。
ＳＴ１８において、導出された動作後の姿勢で障害物２０に干渉するか否かを判別する。ノー（Ｎ）の場合はＳＴ１９に進み、イエス（Ｙ）の場合はエラー終了する。
ＳＴ１９において、初期状態と導出された動作後の姿勢との間の軌道（中間位置が増えた場合は、中間位置どうしの間の軌道）を直線補間で導出する。そして、ＳＴ２０に進む。
ＳＴ２０において、導出された軌道が障害物２０に干渉するか否かを判別する。イエス（Ｙ）の場合はＳＴ２１に進み、ノー（Ｎ）の場合はＳＴ２２に進む。
ＳＴ２１において、次の中間位置を設定する（中間位置を増やす）。そして、ＳＴ１７に戻る。
ＳＴ２２において、導出結果を動作計画として出力して、動作計画出力処理を終了する。
ＳＴ１８において、導出された動作後の姿勢で障害物２０に干渉するか否かを判別する。ノー（Ｎ）の場合はＳＴ１９に進み、イエス（Ｙ）の場合はエラー終了する。
ＳＴ１９において、初期状態と導出された動作後の姿勢との間の軌道（中間位置が増えた場合は、中間位置どうしの間の軌道）を直線補間で導出する。そして、ＳＴ２０に進む。
ＳＴ２０において、導出された軌道が障害物２０に干渉するか否かを判別する。イエス（Ｙ）の場合はＳＴ２１に進み、ノー（Ｎ）の場合はＳＴ２２に進む。
ＳＴ２１において、次の中間位置を設定する（中間位置を増やす）。そして、ＳＴ１７に戻る。
ＳＴ２２において、導出結果を動作計画として出力して、動作計画出力処理を終了する。
【0041】
（実施例１の作用）
前記構成を備えた実施例１のロボットアームシステムＳでは、逆運動学の数値解法や優先度付き逆運動学を解いてアーム部材１の軌道を導出する。導出された軌道が障害物２０に干渉する場合には、障害物２０に干渉しない軌道が導出されるまで中間位置が設定されていって、軌道が導出される。したがって、実施例１では、軌道を導出する際に、関節空間や評価関数を使用する必要がない。よって、ｎ次元の関節空間（実施例１の例では７次元の関節空間）や評価関数を使用する場合に比べて、多関節のロボットの動作の経路の計算負荷を軽減することができる。
特に、実施例１では、関節部３の数が増えても、逆運動学の計算でのパラメータが１つ増えるだけで、関節空間の場合のように空間の次元が増える場合に比べて、計算負荷の増大は抑制される。また、実施例１では、障害物２０が移動する場合（動的障害物の場合）でも、移動後の障害物２０の位置に応じた逆運動学の数値解法等の計算を行うだけであり、ｎ次元の関節空間や評価関数の再計算を行う場合に比べて、計算負荷は大きく軽減される。
また、実施例１では、第１の中間位置２２だけでは障害物２０に干渉しない軌道が導出されない場合でも、第２の中間位置、第３の中間位置、…と順に設定していくことで、干渉しない軌道の探索が行われる。
前記構成を備えた実施例１のロボットアームシステムＳでは、逆運動学の数値解法や優先度付き逆運動学を解いてアーム部材１の軌道を導出する。導出された軌道が障害物２０に干渉する場合には、障害物２０に干渉しない軌道が導出されるまで中間位置が設定されていって、軌道が導出される。したがって、実施例１では、軌道を導出する際に、関節空間や評価関数を使用する必要がない。よって、ｎ次元の関節空間（実施例１の例では７次元の関節空間）や評価関数を使用する場合に比べて、多関節のロボットの動作の経路の計算負荷を軽減することができる。
特に、実施例１では、関節部３の数が増えても、逆運動学の計算でのパラメータが１つ増えるだけで、関節空間の場合のように空間の次元が増える場合に比べて、計算負荷の増大は抑制される。また、実施例１では、障害物２０が移動する場合（動的障害物の場合）でも、移動後の障害物２０の位置に応じた逆運動学の数値解法等の計算を行うだけであり、ｎ次元の関節空間や評価関数の再計算を行う場合に比べて、計算負荷は大きく軽減される。
また、実施例１では、第１の中間位置２２だけでは障害物２０に干渉しない軌道が導出されない場合でも、第２の中間位置、第３の中間位置、…と順に設定していくことで、干渉しない軌道の探索が行われる。
【0042】
（変更例）
以上、本発明の実施例を詳述したが、本発明は、前記実施例に限定されるものではなく、特許請求の範囲に記載された本発明の要旨の範囲内で、種々の変更を行うことが可能である。本発明の変更例（Ｈ01）～（Ｈ05）を下記に例示する。
（Ｈ01）前記実施例において、関節部３の数やアームの長さや数、形状等は例示した形態に限定されない。したがって、アーム部材の用途や設計、仕様等に応じて、数や長さ、形状等、任意に変更可能である。
（Ｈ02）前記実施例において、中間位置は関節部３の位置に設定していく場合を例示したがこれに限定されない。例えば、アーム部２の中間部に設定したり、アーム部２や関節部３から一定の距離だけ平行移動させた位置に設定する等、アーム部材１の任意の位置に設定することも可能である。
以上、本発明の実施例を詳述したが、本発明は、前記実施例に限定されるものではなく、特許請求の範囲に記載された本発明の要旨の範囲内で、種々の変更を行うことが可能である。本発明の変更例（Ｈ01）～（Ｈ05）を下記に例示する。
（Ｈ01）前記実施例において、関節部３の数やアームの長さや数、形状等は例示した形態に限定されない。したがって、アーム部材の用途や設計、仕様等に応じて、数や長さ、形状等、任意に変更可能である。
（Ｈ02）前記実施例において、中間位置は関節部３の位置に設定していく場合を例示したがこれに限定されない。例えば、アーム部２の中間部に設定したり、アーム部２や関節部３から一定の距離だけ平行移動させた位置に設定する等、アーム部材１の任意の位置に設定することも可能である。
【0043】
（Ｈ03）前記実施例において、関節部３の種類は回転関節のみで構成されていたが、その形態に限定されない。すなわち、直動関節を含む場合も適用可能である。
（Ｈ04）前記実施例において、中間位置は、第１の姿勢導出手段Ｃ３で導出された動作後の姿勢を基準に設定する場合を例示したが、これに限定されない。例えば、第１の中間位置を目標として導出された第１の中間姿勢を基準にして、第１の中間姿勢における手首の関節部３ｇの位置を次の中間位置とすることも可能である。
（Ｈ05）前記実施例において、中間位置は動作後、動作前、動作後、…の順に設定する場合を例示したがこれに限定されない。動作前、動作後、…と順番を入れ替えることも可能である。
（Ｈ04）前記実施例において、中間位置は、第１の姿勢導出手段Ｃ３で導出された動作後の姿勢を基準に設定する場合を例示したが、これに限定されない。例えば、第１の中間位置を目標として導出された第１の中間姿勢を基準にして、第１の中間姿勢における手首の関節部３ｇの位置を次の中間位置とすることも可能である。
（Ｈ05）前記実施例において、中間位置は動作後、動作前、動作後、…の順に設定する場合を例示したがこれに限定されない。動作前、動作後、…と順番を入れ替えることも可能である。
【符号の説明】
【0044】
１…アーム部材、
２ｋ…自由端、
２…アーム部、
３…関節部、
３ｇ…自由端よりも１つ基端側の関節部、
１１…コンピュータ、
２０…障害物、
２１…目標、
２２…中間位置、
Ｃ…制御部、
Ｃ３…第１の姿勢導出手段、
Ｃ６…第２の姿勢導出手段、
Ｃ７…アーム制御手段、
Ｓ…ロボット。
２ｋ…自由端、
２…アーム部、
３…関節部、
３ｇ…自由端よりも１つ基端側の関節部、
１１…コンピュータ、
２０…障害物、
２１…目標、
２２…中間位置、
Ｃ…制御部、
Ｃ３…第１の姿勢導出手段、
Ｃ６…第２の姿勢導出手段、
Ｃ７…アーム制御手段、
Ｓ…ロボット。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
